From 8cfe20f110cf5dfbeb49e75e220369f9069787e1 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 15 Jun 2022 14:38:55 -0700 Subject: [PATCH 001/131] statistical prototype --- nav2_msgs/srv/IsPathValid.srv | 1 + nav2_planner/src/planner_server.cpp | 76 ++++++++++++++++++++++------- 2 files changed, 59 insertions(+), 18 deletions(-) diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv index 8d6dc3b38d..72b5f20b55 100644 --- a/nav2_msgs/srv/IsPathValid.srv +++ b/nav2_msgs/srv/IsPathValid.srv @@ -1,6 +1,7 @@ #Determine if the current path is still valid nav_msgs/Path path +bool consider_cost_change --- bool is_valid int32[] invalid_pose_indices diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 9abc461d30..5fbeb2e74e 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "builtin_interfaces/msg/duration.hpp" #include "nav2_util/costmap.hpp" @@ -541,13 +542,15 @@ void PlannerServer::isPathValid( return; } + // Find the closest point to the robot to evaluate from + // TODO add orientation element like `truncate_path_local_action` BT node for looping paths geometry_msgs::msg::PoseStamped current_pose; unsigned int closest_point_index = 0; if (costmap_ros_->getRobotPose(current_pose)) { float current_distance = std::numeric_limits::max(); float closest_distance = current_distance; geometry_msgs::msg::Point current_point = current_pose.pose.position; - for (unsigned int i = 0; i < request->path.poses.size(); ++i) { + for (unsigned int i = 0; i != request->path.poses.size(); ++i) { geometry_msgs::msg::Point path_point = request->path.poses[i].pose.position; current_distance = nav2_util::geometry_utils::euclidean_distance( @@ -559,26 +562,63 @@ void PlannerServer::isPathValid( closest_distance = current_distance; } } + } - /** - * The lethal check starts at the closest point to avoid points that have already been passed - * and may have become occupied - */ - unsigned int mx = 0; - unsigned int my = 0; - for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { - costmap_->worldToMap( - request->path.poses[i].pose.position.x, - request->path.poses[i].pose.position.y, mx, my); - unsigned int cost = costmap_->getCost(mx, my); - - if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) - { - response->is_valid = false; - } + // Checking path for collisions starting at the closest point to avoid those already passed + std::vector current_costs; + current_costs.reserve(request->path.size() - closest_point_index - 1); + + unsigned int mx = 0; + unsigned int my = 0; + for (unsigned int i = closest_point_index; i != request->path.poses.size(); ++i) { + costmap_->worldToMap( + request->path.poses[i].pose.position.x, + request->path.poses[i].pose.position.y, mx, my); + unsigned int cost = costmap_->getCost(mx, my); + current_costs.push_back(cost); + + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + response->is_valid = false; } } + + // Check using a statistical test if the cost changes are 'significant' enough + // to warrant replanning, due to changes in cost within the environment, + // when larger than the minimum sample size for the test + if (!request->consider_cost_change || request->path.poses.size() < 30) { + return; + } + + std::vector original_costs( + request->path.costs.begin() + closest_point_index, request->path.costs.end()); + + float mean_a, mean_b; + for (unsigned int i = 0; i != original_costs.size(); ++i) { + mean_a += static_cast(original_costs[i]); + mean_b += static_cast(current_costs[i]); + } + mean_a /= static_cast(original_costs.size()); + mean_b /= static_cast(current_costs.size()); + + float var_a, var_b; + for (unsigned int i = 0; i != original_costs.size(); ++i) { + var_a += static_cast(original_costs[i]) - static_cast(mean_a); + var_b += static_cast(current_costs[i]) - static_cast(mean_b); + } + var_a /= static_cast(original_costs.size() - 1); + var_b /= static_cast(current_costs.size() - 1); + + // Conduct a two sample Z-test, with the null hypothesis is that both path cost samples + // come from the same distribution (e.g. there is not a statistically significant change) + // Thus, the difference in population mean is 0 and the sample sizes are the same + float z = (mean_b - mean_a) / std::sqrt((var_b + var_a) / current_costs.size()); + // TODO try single tail or tune strictness? Parameterize? + // 1.65 95% single tail; 2.55 for 99% dual, 2.33 99% single; 90% 1.65 dual, 90% 1.2 single. + if (z > 1.96) { // critical z score for 95% level + response->is_valid = false; + } } rcl_interfaces::msg::SetParametersResult From beeb4278140e92777ee0a8c7598cade4aef08782 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 25 Jul 2022 17:29:36 -0400 Subject: [PATCH 002/131] fixed two sample z-test --- nav2_planner/src/planner_server.cpp | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 5fbeb2e74e..7cd1975580 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -581,6 +581,7 @@ void PlannerServer::isPathValid( cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { response->is_valid = false; + return; } } @@ -594,29 +595,32 @@ void PlannerServer::isPathValid( std::vector original_costs( request->path.costs.begin() + closest_point_index, request->path.costs.end()); - float mean_a, mean_b; + float mean_orginal = 0; + float mean_current = 0; for (unsigned int i = 0; i != original_costs.size(); ++i) { - mean_a += static_cast(original_costs[i]); - mean_b += static_cast(current_costs[i]); + mean_orginal += static_cast(original_costs[i]); + mean_current += static_cast(current_costs[i]); } - mean_a /= static_cast(original_costs.size()); - mean_b /= static_cast(current_costs.size()); + mean_orginal /= static_cast(original_costs.size()); + mean_current /= static_cast(current_costs.size()); - float var_a, var_b; + float var_orginal = 0; + float var_current = 0; for (unsigned int i = 0; i != original_costs.size(); ++i) { - var_a += static_cast(original_costs[i]) - static_cast(mean_a); - var_b += static_cast(current_costs[i]) - static_cast(mean_b); + var_orginal += std::pow(static_cast(original_costs[i]) - static_cast(mean_orginal), 2); + var_current += std::pow(static_cast(current_costs[i]) - static_cast(mean_current), 2); } - var_a /= static_cast(original_costs.size() - 1); - var_b /= static_cast(current_costs.size() - 1); + var_orginal /= static_cast(original_costs.size() - 1); + var_current /= static_cast(current_costs.size() - 1); // Conduct a two sample Z-test, with the null hypothesis is that both path cost samples // come from the same distribution (e.g. there is not a statistically significant change) // Thus, the difference in population mean is 0 and the sample sizes are the same - float z = (mean_b - mean_a) / std::sqrt((var_b + var_a) / current_costs.size()); + float z = (mean_current - mean_orginal) / std::sqrt((var_current + var_orginal) / current_costs.size()); // TODO try single tail or tune strictness? Parameterize? // 1.65 95% single tail; 2.55 for 99% dual, 2.33 99% single; 90% 1.65 dual, 90% 1.2 single. - if (z > 1.96) { // critical z score for 95% level + if (z > 2.55) { // critical z score for 95% level + RCLCPP_DEBUG_STREAM(get_logger(), "Z-test triggered new global plan. The z score was: " << z << "and the threshold was" << 2.55); response->is_valid = false; } } From f69765404f13d5782e08e26b996dfad074cd05c4 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 25 Jul 2022 18:56:03 -0400 Subject: [PATCH 003/131] fixed 2 sample z test --- nav2_planner/src/planner_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 7cd1975580..dd6e4a3f8f 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -566,7 +566,7 @@ void PlannerServer::isPathValid( // Checking path for collisions starting at the closest point to avoid those already passed std::vector current_costs; - current_costs.reserve(request->path.size() - closest_point_index - 1); + current_costs.reserve(request->path.costs.size() - closest_point_index - 1); unsigned int mx = 0; unsigned int my = 0; From 7a14c46294f87afa921bf5f39b3cd345094f6dc9 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 25 Jul 2022 18:56:37 -0400 Subject: [PATCH 004/131] rolling issue --- .../include/nav2_behavior_tree/bt_service_node.hpp | 3 ++- nav2_lifecycle_manager/src/lifecycle_manager.cpp | 5 +++-- nav2_util/include/nav2_util/service_client.hpp | 3 ++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 636f24415c..c8551cee94 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -59,9 +59,10 @@ class BtServiceNode : public BT::ActionNodeBase // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); + rclcpp::QoS test(1); service_client_ = node_->create_client( service_name_, - rmw_qos_profile_services_default, + test, callback_group_); // Make a request for the service without parameter diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index f18436f21f..e36e2f6e7e 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -59,16 +59,17 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_); callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::QoS test(10); manager_srv_ = create_service( get_name() + std::string("/manage_nodes"), std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), - rmw_qos_profile_services_default, + test, callback_group_); is_active_srv_ = create_service( get_name() + std::string("/is_active"), std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3), - rmw_qos_profile_services_default, + test, callback_group_); transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE; diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 4af107abf1..9444c65b3d 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -44,9 +44,10 @@ class ServiceClient rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + rclcpp::QoS test(10); client_ = node_->create_client( service_name, - rmw_qos_profile_services_default, + test, callback_group_); } From 10445f1de4f6696c5b4e3202364766a0ba16304b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 25 Jul 2022 19:42:22 -0400 Subject: [PATCH 005/131] navfn produces costs --- nav2_navfn_planner/src/navfn_planner.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2ede7f38b8..e6214ce1a5 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -207,6 +207,7 @@ NavfnPlanner::makePlan( { // clear the plan, just in case plan.poses.clear(); + plan.costs.clear(); plan.header.stamp = clock_->now(); plan.header.frame_id = global_frame_; @@ -355,6 +356,7 @@ NavfnPlanner::smoothApproachToGoal( { // Replace the last pose of the computed path if it's actually further away // to the second to last pose than the goal pose. + const float cost = costmap_->getCost(goal.position.x, goal.position.y); if (plan.poses.size() >= 2) { auto second_to_last_pose = plan.poses.end()[-2]; auto last_pose = plan.poses.back(); @@ -363,12 +365,14 @@ NavfnPlanner::smoothApproachToGoal( squared_distance(goal, second_to_last_pose.pose)) { plan.poses.back().pose = goal; + plan.costs.back() = cost; return; } } geometry_msgs::msg::PoseStamped goal_copy; goal_copy.pose = goal; plan.poses.push_back(goal_copy); + plan.costs.push_back(cost); } bool @@ -378,6 +382,7 @@ NavfnPlanner::getPlanFromPotential( { // clear the plan, just in case plan.poses.clear(); + plan.costs.clear(); // Goal should be in global frame double wx = goal.position.x; @@ -422,6 +427,8 @@ NavfnPlanner::getPlanFromPotential( double world_x, world_y; mapToWorld(x[i], y[i], world_x, world_y); + const float cost = costmap_->getCost(x[i], y[i]); + geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = world_x; pose.pose.position.y = world_y; @@ -431,6 +438,7 @@ NavfnPlanner::getPlanFromPotential( pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; plan.poses.push_back(pose); + plan.costs.push_back(cost); } return !plan.poses.empty(); From 146ca5593276d93bf2d78a622d5f1c813a74e31f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 3 Aug 2022 16:00:22 -0400 Subject: [PATCH 006/131] complete updates --- .../plugins/condition/is_path_valid_condition.hpp | 3 ++- .../plugins/condition/is_path_valid_condition.cpp | 3 +++ nav2_bringup/params/nav2_params.yaml | 1 + .../include/nav2_planner/planner_server.hpp | 1 + nav2_planner/src/planner_server.cpp | 13 ++++++++++--- 5 files changed, 17 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index a04b1263f4..4a7a844dd9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -58,7 +58,8 @@ class IsPathValidCondition : public BT::ConditionNode { return { BT::InputPort("path", "Path to Check"), - BT::InputPort("server_timeout") + BT::InputPort("server_timeout"), + BT::InputPort("consider_cost_change", true, "Consider cost changes") }; } diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 4a02dede9a..570b80d0d4 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -36,10 +36,13 @@ BT::NodeStatus IsPathValidCondition::tick() { nav_msgs::msg::Path path; getInput("path", path); + bool consider_cost_change; + getInput("consider_cost_change", consider_cost_change); auto request = std::make_shared(); request->path = path; + request->consider_cost_change = consider_cost_change; auto result = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) == diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 51163b3775..d17b71c7f9 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -275,6 +275,7 @@ planner_server: tolerance: 0.5 use_astar: false allow_unknown: true + z_score: 2.55 smoother_server: ros__parameters: diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 54e740e528..a1f1738c29 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -235,6 +235,7 @@ class PlannerServer : public nav2_util::LifecycleNode std::vector planner_ids_; std::vector planner_types_; double max_planner_duration_; + double z_score_; std::string planner_ids_concat_; // Clock diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index dd6e4a3f8f..6a55fc06a9 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -53,6 +53,12 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); + nav2_util::declare_parameter_if_not_declared( + this, "z_score", rclcpp::ParameterValue(2.55)); + + + get_parameter("z_score", z_score_); + get_parameter("planner_plugins", planner_ids_); if (planner_ids_ == default_ids_) { for (size_t i = 0; i < default_ids_.size(); ++i) { @@ -588,7 +594,7 @@ void PlannerServer::isPathValid( // Check using a statistical test if the cost changes are 'significant' enough // to warrant replanning, due to changes in cost within the environment, // when larger than the minimum sample size for the test - if (!request->consider_cost_change || request->path.poses.size() < 30) { + if (!request->consider_cost_change || request->path.costs.size() < 30) { return; } @@ -617,10 +623,11 @@ void PlannerServer::isPathValid( // come from the same distribution (e.g. there is not a statistically significant change) // Thus, the difference in population mean is 0 and the sample sizes are the same float z = (mean_current - mean_orginal) / std::sqrt((var_current + var_orginal) / current_costs.size()); + // TODO try single tail or tune strictness? Parameterize? // 1.65 95% single tail; 2.55 for 99% dual, 2.33 99% single; 90% 1.65 dual, 90% 1.2 single. - if (z > 2.55) { // critical z score for 95% level - RCLCPP_DEBUG_STREAM(get_logger(), "Z-test triggered new global plan. The z score was: " << z << "and the threshold was" << 2.55); + if (z > z_score_) { // critical z score for 95% level + RCLCPP_DEBUG_STREAM(get_logger(), "Z-test triggered new global plan. The z score was: " << z << "and the threshold was" << z_score_); response->is_valid = false; } } From c1a499f10f36acd1b8a1837c315837a1e739a31d Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 5 Aug 2022 18:12:48 -0400 Subject: [PATCH 007/131] Revert "rolling issue" This reverts commit 7a14c46294f87afa921bf5f39b3cd345094f6dc9. --- .../include/nav2_behavior_tree/bt_service_node.hpp | 3 +-- nav2_lifecycle_manager/src/lifecycle_manager.cpp | 5 ++--- nav2_util/include/nav2_util/service_client.hpp | 3 +-- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index c8551cee94..636f24415c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -59,10 +59,9 @@ class BtServiceNode : public BT::ActionNodeBase // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); - rclcpp::QoS test(1); service_client_ = node_->create_client( service_name_, - test, + rmw_qos_profile_services_default, callback_group_); // Make a request for the service without parameter diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index e36e2f6e7e..f18436f21f 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -59,17 +59,16 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_); callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - rclcpp::QoS test(10); manager_srv_ = create_service( get_name() + std::string("/manage_nodes"), std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), - test, + rmw_qos_profile_services_default, callback_group_); is_active_srv_ = create_service( get_name() + std::string("/is_active"), std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3), - test, + rmw_qos_profile_services_default, callback_group_); transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE; diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 9444c65b3d..4af107abf1 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -44,10 +44,9 @@ class ServiceClient rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - rclcpp::QoS test(10); client_ = node_->create_client( service_name, - test, + rmw_qos_profile_services_default, callback_group_); } From 5108c97da0b05aa52c6faf4df522f0926e389383 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 5 Aug 2022 18:14:09 -0400 Subject: [PATCH 008/131] Revert "navfn produces costs" This reverts commit 10445f1de4f6696c5b4e3202364766a0ba16304b. --- nav2_navfn_planner/src/navfn_planner.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index e6214ce1a5..2ede7f38b8 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -207,7 +207,6 @@ NavfnPlanner::makePlan( { // clear the plan, just in case plan.poses.clear(); - plan.costs.clear(); plan.header.stamp = clock_->now(); plan.header.frame_id = global_frame_; @@ -356,7 +355,6 @@ NavfnPlanner::smoothApproachToGoal( { // Replace the last pose of the computed path if it's actually further away // to the second to last pose than the goal pose. - const float cost = costmap_->getCost(goal.position.x, goal.position.y); if (plan.poses.size() >= 2) { auto second_to_last_pose = plan.poses.end()[-2]; auto last_pose = plan.poses.back(); @@ -365,14 +363,12 @@ NavfnPlanner::smoothApproachToGoal( squared_distance(goal, second_to_last_pose.pose)) { plan.poses.back().pose = goal; - plan.costs.back() = cost; return; } } geometry_msgs::msg::PoseStamped goal_copy; goal_copy.pose = goal; plan.poses.push_back(goal_copy); - plan.costs.push_back(cost); } bool @@ -382,7 +378,6 @@ NavfnPlanner::getPlanFromPotential( { // clear the plan, just in case plan.poses.clear(); - plan.costs.clear(); // Goal should be in global frame double wx = goal.position.x; @@ -427,8 +422,6 @@ NavfnPlanner::getPlanFromPotential( double world_x, world_y; mapToWorld(x[i], y[i], world_x, world_y); - const float cost = costmap_->getCost(x[i], y[i]); - geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = world_x; pose.pose.position.y = world_y; @@ -438,7 +431,6 @@ NavfnPlanner::getPlanFromPotential( pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; plan.poses.push_back(pose); - plan.costs.push_back(cost); } return !plan.poses.empty(); From 827a0ba90413d08482019e0bb7c1a77b5ce84766 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 16 Jun 2022 12:57:09 -0700 Subject: [PATCH 009/131] adding plugin type for static in local + removing unused print (#3021) --- .../include/nav2_behaviors/timed_behavior.hpp | 13 ------------- nav2_bringup/params/nav2_params.yaml | 1 + 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index d696607feb..0c41b13f49 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -196,19 +196,6 @@ class TimedBehavior : public nav2_core::Behavior return; } - // Log a message every second - { - 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...", behavior_name_.c_str());}); - } - auto start_time = steady_clock_.now(); // Initialize the ActionT result diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index d17b71c7f9..1d270eb6a2 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -214,6 +214,7 @@ local_costmap: obstacle_max_range: 2.5 obstacle_min_range: 0.0 static_layer: + plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True always_send_full_costmap: True From 8ba88483fbce792019ee75f3f84b624bcb55530c Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Sun, 19 Jun 2022 18:22:16 -0400 Subject: [PATCH 010/131] removed extra includes (#3026) --- nav2_util/include/nav2_util/robot_utils.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 5281ff8813..b9712044b6 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -18,10 +18,6 @@ #define NAV2_UTIL__ROBOT_UTILS_HPP_ #include -#include -#include -#include -#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" From b7a13d354982900775b09dbde70151770e174c08 Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Sun, 19 Jun 2022 18:22:55 -0400 Subject: [PATCH 011/131] remove extra sub (#3025) --- .../include/nav2_costmap_2d/footprint_subscriber.hpp | 1 - nav2_costmap_2d/src/footprint_subscriber.cpp | 6 ++---- 2 files changed, 2 insertions(+), 5 deletions(-) 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 9993d9190a..d9e9ddb3c6 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp @@ -86,7 +86,6 @@ class FootprintSubscriber */ void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg); - std::string topic_name_; tf2_ros::Buffer & tf_; std::string robot_base_frame_; double transform_tolerance_; diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp index 45e60db65e..2c269fa3b0 100644 --- a/nav2_costmap_2d/src/footprint_subscriber.cpp +++ b/nav2_costmap_2d/src/footprint_subscriber.cpp @@ -31,8 +31,7 @@ FootprintSubscriber::FootprintSubscriber( tf2_ros::Buffer & tf, std::string robot_base_frame, double transform_tolerance) -: topic_name_(topic_name), - tf_(tf), +: tf_(tf), robot_base_frame_(robot_base_frame), transform_tolerance_(transform_tolerance) { @@ -48,8 +47,7 @@ FootprintSubscriber::FootprintSubscriber( tf2_ros::Buffer & tf, std::string robot_base_frame, double transform_tolerance) -: topic_name_(topic_name), - tf_(tf), +: tf_(tf), robot_base_frame_(robot_base_frame), transform_tolerance_(transform_tolerance) { From ef0102b0c25b95b323566dd6d15f0a4caad3e7ea Mon Sep 17 00:00:00 2001 From: hodnajit Date: Tue, 21 Jun 2022 19:26:41 +0200 Subject: [PATCH 012/131] Fix missing dependency on nav2_velocity_smoother (#3031) --- navigation2/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/navigation2/package.xml b/navigation2/package.xml index a70c869198..35e61399a3 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -36,6 +36,7 @@ nav2_smoother nav2_theta_star_planner nav2_util + nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower From 21d36bfd1321b97750a82022f6a3bf38bb28ad54 Mon Sep 17 00:00:00 2001 From: Vinny Ruia Date: Wed, 22 Jun 2022 13:31:03 -0400 Subject: [PATCH 013/131] adding timeout for action client initialization (#3029) * adding timeout for action client initialization Signed-off-by: Vinny Ruia * adding constant 1s timeout, catching exception Signed-off-by: Vinny Ruia --- .../include/nav2_behavior_tree/bt_action_node.hpp | 9 ++++++++- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 8 +++++++- .../src/navigators/navigate_through_poses.cpp | 2 +- 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 5b34469e7e..8de0b27977 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 @@ -90,7 +90,14 @@ class BtActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - action_client_->wait_for_action_server(); + static constexpr std::chrono::milliseconds wait_for_server_timeout = + std::chrono::milliseconds(1000); + if (!action_client_->wait_for_action_server(wait_for_server_timeout)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" action server not available after waiting for %li ms", + action_name.c_str(), wait_for_server_timeout.count()); + throw std::runtime_error("Action server not available"); + } } /** diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 37b2b809dd..592d1257a2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -172,7 +172,13 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena std::istreambuf_iterator()); // Create the Behavior Tree from the XML input - tree_ = bt_->createTreeFromText(xml_string, blackboard_); + try { + tree_ = bt_->createTreeFromText(xml_string, blackboard_); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); + return false; + } + topic_logger_ = std::make_unique(client_node_, tree_); current_bt_xml_filename_ = filename; diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 237f6ba8ba..34a3f429a4 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -76,7 +76,7 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { RCLCPP_ERROR( - logger_, "BT file not found: %s. Navigation canceled.", + logger_, "Error loading XML file: %s. Navigation canceled.", bt_xml_filename.c_str()); return false; } From 2672bd10ea44751e17e94691c29a59fb1d05221c Mon Sep 17 00:00:00 2001 From: stevenbrills <90438581+stevenbrills@users.noreply.github.com> Date: Thu, 23 Jun 2022 13:37:36 -0400 Subject: [PATCH 014/131] =?UTF-8?q?-=20Modified=20findVelocitySignChange?= =?UTF-8?q?=20method=20to=20transform=20cusp=20into=20robot=E2=80=A6=20(#3?= =?UTF-8?q?036)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * - Modified findVelocitySignChange method to transform cusp into robot frame before returning distance_to_cusp * - Previous commit had incorrect usage of method transformPose based on different nav2 branch. - Now a placeholder pose, input pose and desired frame id is passed. * - Previous commit had incorrect usage of method transformPose based on different nav2 branch. - Now a placeholder pose, input pose and desired frame id is passed. Signed-off-by: Steven Brills * Failed lint check due to stray blank line. Removed the blank line * - Modified findVelocitySignChange function to take the transformed plan in robot's frame - Removed need to pass pose to the findVelocitySignChange function * - Modified the test file to reflect change in new parameters that findVelocitySignChange expects. Signed-off-by: Steven Brills * - Corrected call to transformGlobalPoseWrapper method of BasicAPIRPP object. Signed-off-by: Steven Brills * - transformGlobalPlanWrapper call bug fix Signed-off-by: Steven Brills * - Updated tests now require frame_id and time_stamp for conversion since transformed plan is to be used. Signed-off-by: Steven Brills * - Missing ; in test method Signed-off-by: Steven Brills * -Modified findVelocitySignChange tests in test_regulated_pp to use transformed_plan instead * -Modified findVelocitySignChange tests in test_regulated_pp to use transformed_plan instead Signed-off-by: Steven Brills Co-authored-by: Steven Brills --- .../regulated_pure_pursuit_controller.hpp | 2 +- .../src/regulated_pure_pursuit_controller.cpp | 32 ++++++++++--------- .../test/test_regulated_pp.cpp | 16 +++++++--- 3 files changed, 29 insertions(+), 21 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 8265ff4766..9224921de2 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -249,7 +249,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param pose Pose input to determine the cusp position * @return robot distance from the cusp */ - double findVelocitySignChange(const geometry_msgs::msg::PoseStamped & pose); + double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); /** * Get the greatest extent of the costmap in meters from the center. diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 4549db222d..3428faae85 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -283,7 +283,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Check for reverse driving if (allow_reversing_) { // Cusp check - double dist_to_cusp = findVelocitySignChange(pose); + double dist_to_cusp = findVelocitySignChange(transformed_plan); // if the lookahead distance is further than the cusp, use the cusp distance instead if (dist_to_cusp < lookahead_dist) { @@ -720,27 +720,29 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( } double RegulatedPurePursuitController::findVelocitySignChange( - const geometry_msgs::msg::PoseStamped & pose) + const nav_msgs::msg::Path & transformed_plan) { - // Iterating through the global path to determine the position of the cusp - for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size() - 1; ++pose_id) { + // Iterating through the transformed global path to determine the position of the cusp + for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) { // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = global_plan_.poses[pose_id].pose.position.x - - global_plan_.poses[pose_id - 1].pose.position.x; - double oa_y = global_plan_.poses[pose_id].pose.position.y - - global_plan_.poses[pose_id - 1].pose.position.y; - double ab_x = global_plan_.poses[pose_id + 1].pose.position.x - - global_plan_.poses[pose_id].pose.position.x; - double ab_y = global_plan_.poses[pose_id + 1].pose.position.y - - global_plan_.poses[pose_id].pose.position.y; + double oa_x = transformed_plan.poses[pose_id].pose.position.x - + transformed_plan.poses[pose_id - 1].pose.position.x; + double oa_y = transformed_plan.poses[pose_id].pose.position.y - + transformed_plan.poses[pose_id - 1].pose.position.y; + double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x - + transformed_plan.poses[pose_id].pose.position.x; + double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y - + transformed_plan.poses[pose_id].pose.position.y; /* Checking for the existance of cusp, in the path, using the dot product and determine it's distance from the robot. If there is no cusp in the path, then just determine the distance to the goal location. */ if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) { - auto x = global_plan_.poses[pose_id].pose.position.x - pose.pose.position.x; - auto y = global_plan_.poses[pose_id].pose.position.y - pose.pose.position.y; - return hypot(x, y); // returning the distance if there is a cusp + // returning the distance if there is a cusp + // The transformed path is in the robots frame, so robot is at the origin + return hypot( + transformed_plan.poses[pose_id].pose.position.x, + transformed_plan.poses[pose_id].pose.position.y); } } diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 9f8f452195..9f65a8554f 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -103,9 +103,9 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure } double findVelocitySignChangeWrapper( - const geometry_msgs::msg::PoseStamped & pose) + const nav_msgs::msg::Path & transformed_plan) { - return findVelocitySignChange(pose); + return findVelocitySignChange(transformed_plan); } nav_msgs::msg::Path transformGlobalPlanWrapper( @@ -170,12 +170,18 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) TEST(RegulatedPurePursuitTest, findVelocitySignChange) { auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPPfindVelocitySignChange"); geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "smb"; + auto time = node->get_clock()->now(); + pose.header.stamp = time; pose.pose.position.x = 1.0; pose.pose.position.y = 0.0; nav_msgs::msg::Path path; path.poses.resize(3); + path.header.frame_id = "smb"; + path.header.stamp = pose.header.stamp; path.poses[0].pose.position.x = 1.0; path.poses[0].pose.position.y = 1.0; path.poses[1].pose.position.x = 2.0; @@ -183,13 +189,13 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) path.poses[2].pose.position.x = -1.0; path.poses[2].pose.position.y = -1.0; ctrl->setPlan(path); - auto rtn = ctrl->findVelocitySignChangeWrapper(pose); - EXPECT_EQ(rtn, sqrt(5.0)); + auto rtn = ctrl->findVelocitySignChangeWrapper(path); + EXPECT_EQ(rtn, sqrt(8.0)); path.poses[2].pose.position.x = 3.0; path.poses[2].pose.position.y = 3.0; ctrl->setPlan(path); - rtn = ctrl->findVelocitySignChangeWrapper(pose); + rtn = ctrl->findVelocitySignChangeWrapper(path); EXPECT_EQ(rtn, std::numeric_limits::max()); } From 9e6be2e860fa4ecf2f38ad0de27f703a9bf2531f Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Thu, 23 Jun 2022 16:41:59 -0400 Subject: [PATCH 015/131] cleanup warnings (#3028) * cleanup warnings * removed referenc --- .../include/nav2_behaviors/timed_behavior.hpp | 7 +++---- nav2_behaviors/plugins/drive_on_heading.cpp | 6 ------ nav2_behaviors/plugins/drive_on_heading.hpp | 16 ++++++--------- nav2_behaviors/plugins/spin.cpp | 20 +++++++++---------- nav2_behaviors/plugins/wait.cpp | 5 +---- nav2_behaviors/src/main.cpp | 1 - nav2_behaviors/test/test_behaviors.cpp | 7 +++---- 7 files changed, 22 insertions(+), 40 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 0c41b13f49..06528be887 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -63,13 +63,12 @@ class TimedBehavior : public nav2_core::Behavior TimedBehavior() : action_server_(nullptr), cycle_frequency_(10.0), - enabled_(false) + enabled_(false), + transform_tolerance_(0.0) { } - virtual ~TimedBehavior() - { - } + virtual ~TimedBehavior() = default; // Derived classes can override this method to catch the command and perform some checks // before getting into the main loop. The method will only be called diff --git a/nav2_behaviors/plugins/drive_on_heading.cpp b/nav2_behaviors/plugins/drive_on_heading.cpp index dae6ed83a1..53525b3bb6 100644 --- a/nav2_behaviors/plugins/drive_on_heading.cpp +++ b/nav2_behaviors/plugins/drive_on_heading.cpp @@ -13,14 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include -#include - #include "drive_on_heading.hpp" -#include "nav2_util/node_utils.hpp" -#include "nav2_msgs/action/back_up.hpp" #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior) diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp index 65ab8f1157..79c8d81c71 100644 --- a/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/plugins/drive_on_heading.hpp @@ -41,13 +41,14 @@ class DriveOnHeading : public TimedBehavior */ DriveOnHeading() : TimedBehavior(), - feedback_(std::make_shared()) + feedback_(std::make_shared()), + command_x_(0.0), + command_speed_(0.0), + simulate_ahead_time_(0.0) { } - - ~DriveOnHeading() - {} + ~DriveOnHeading() = default; /** * @brief Initialization to run behavior @@ -122,7 +123,6 @@ class DriveOnHeading : public TimedBehavior return Status::SUCCEEDED; } - // TODO(mhpanah): cmd_vel value should be passed as a parameter auto cmd_vel = std::make_unique(); cmd_vel->linear.y = 0.0; cmd_vel->angular.z = 0.0; @@ -199,9 +199,7 @@ class DriveOnHeading : public TimedBehavior node->get_parameter("simulate_ahead_time", simulate_ahead_time_); } - double min_linear_vel_; - double max_linear_vel_; - double linear_acc_lim_; + typename ActionT::Feedback::SharedPtr feedback_; geometry_msgs::msg::PoseStamped initial_pose_; double command_x_; @@ -209,8 +207,6 @@ class DriveOnHeading : public TimedBehavior rclcpp::Duration command_time_allowance_{0, 0}; rclcpp::Time end_time_; double simulate_ahead_time_; - - typename ActionT::Feedback::SharedPtr feedback_; }; } // namespace nav2_behaviors diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 641f59064e..a76fe38f52 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -13,19 +13,13 @@ // limitations under the License. #include -#include -#include #include #include #include #include #include "spin.hpp" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" #include "tf2/utils.h" -#pragma GCC diagnostic pop -#include "tf2/LinearMath/Quaternion.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_util/node_utils.hpp" @@ -37,13 +31,17 @@ namespace nav2_behaviors Spin::Spin() : TimedBehavior(), feedback_(std::make_shared()), - prev_yaw_(0.0) + min_rotational_vel_(0.0), + max_rotational_vel_(0.0), + rotational_acc_lim_(0.0), + cmd_yaw_(0.0), + prev_yaw_(0.0), + relative_yaw_(0.0), + simulate_ahead_time_(0.0) { } -Spin::~Spin() -{ -} +Spin::~Spin() = default; void Spin::onConfigure() { @@ -128,7 +126,7 @@ Status Spin::onCycleUpdate() relative_yaw_ += delta_yaw; prev_yaw_ = current_yaw; - feedback_->angular_distance_traveled = relative_yaw_; + feedback_->angular_distance_traveled = static_cast(relative_yaw_); action_server_->publish_feedback(feedback_); double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_); diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp index 7ca00e2abc..236f712201 100644 --- a/nav2_behaviors/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include @@ -27,9 +26,7 @@ Wait::Wait() { } -Wait::~Wait() -{ -} +Wait::~Wait() = default; Status Wait::onRun(const std::shared_ptr command) { diff --git a/nav2_behaviors/src/main.cpp b/nav2_behaviors/src/main.cpp index 0b06ea9f17..4af82831f3 100644 --- a/nav2_behaviors/src/main.cpp +++ b/nav2_behaviors/src/main.cpp @@ -13,7 +13,6 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include #include #include "nav2_behaviors/behavior_server.hpp" diff --git a/nav2_behaviors/test/test_behaviors.cpp b/nav2_behaviors/test/test_behaviors.cpp index de1253cdc9..4cd7527fbc 100644 --- a/nav2_behaviors/test/test_behaviors.cpp +++ b/nav2_behaviors/test/test_behaviors.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include "gtest/gtest.h" @@ -42,7 +41,7 @@ class DummyBehavior : public TimedBehavior : TimedBehavior(), initialized_(false) {} - ~DummyBehavior() {} + ~DummyBehavior() = default; Status onRun(const std::shared_ptr goal) override { @@ -96,9 +95,9 @@ class BehaviorTest : public ::testing::Test { protected: BehaviorTest() {SetUp();} - ~BehaviorTest() {} + ~BehaviorTest() = default; - void SetUp() + void SetUp() override { node_lifecycle_ = std::make_shared( From 5f2eedeeef72d42f6ae3847a7f4e105def35d169 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 24 Jun 2022 14:13:13 -0700 Subject: [PATCH 016/131] Update mergify.yml --- .github/mergify.yml | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/.github/mergify.yml b/.github/mergify.yml index 3713a4262a..491c2c1076 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -1,4 +1,13 @@ pull_request_rules: + - name: backport to humble at reviewers discretion + conditions: + - base=main + - "label=backport-humble" + actions: + backport: + branches: + - humble + - name: backport to galactic at reviewers discretion conditions: - base=main From 9c08a05665bb4bbaf37d1df46725561a96281843 Mon Sep 17 00:00:00 2001 From: relffok <57466265+relffok@users.noreply.github.com> Date: Tue, 28 Jun 2022 21:07:30 +0200 Subject: [PATCH 017/131] Draft: Added GoalUpdatedController BT plugin node (#3044) * first draft for goal updated controller Signed-off-by: relffok <57466265+relffok@users.noreply.github.com> * added goal_updated_controller to all yamls * added GoalUpdatedController API * added GoaUpdatedController to default plugins * removed first_time param * added test for GoalUpdatedController * linter fix --- nav2_behavior_tree/CMakeLists.txt | 3 + .../decorator/goal_updated_controller.hpp | 68 +++++++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 3 + .../decorator/goal_updated_controller.cpp | 88 ++++++++++++++ .../test/plugins/decorator/CMakeLists.txt | 4 + .../test_goal_updated_controller.cpp | 108 ++++++++++++++++++ .../params/nav2_multirobot_params_1.yaml | 1 + .../params/nav2_multirobot_params_2.yaml | 1 + nav2_bringup/params/nav2_params.yaml | 1 + ...e_w_replanning_only_if_goal_is_updated.xml | 14 +++ nav2_bt_navigator/src/bt_navigator.cpp | 1 + .../src/costmap_filters/keepout_params.yaml | 1 + .../costmap_filters/speed_global_params.yaml | 1 + .../costmap_filters/speed_local_params.yaml | 1 + 14 files changed, 295 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp create mode 100644 nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp create mode 100644 nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp create mode 100644 nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index e7abf148c5..0569c58bc9 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -180,6 +180,9 @@ list(APPEND plugin_libs nav2_controller_selector_bt_node) add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp) list(APPEND plugin_libs nav2_goal_checker_selector_bt_node) +add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp) +list(APPEND plugin_libs nav2_goal_updated_controller_bt_node) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp new file mode 100644 index 0000000000..4ac0ab44ee --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -0,0 +1,68 @@ +// 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. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/decorator_node.h" + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::DecoratorNode that ticks its child if the goal was updated + */ +class GoalUpdatedController : public BT::DecoratorNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::GoalUpdatedController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ + GoalUpdatedController( + const std::string & name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return {}; + } + +private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + bool goal_was_updated_; + geometry_msgs::msg::PoseStamped goal_; + std::vector goals_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 3a22a90083..3f5fa4b522 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -254,5 +254,8 @@ Length multiplication factor to check if the path is significantly longer + + + diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp new file mode 100644 index 0000000000..f8a2d8cefc --- /dev/null +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -0,0 +1,88 @@ +// 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. + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "behaviortree_cpp_v3/decorator_node.h" +#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" + + +namespace nav2_behavior_tree +{ + +GoalUpdatedController::GoalUpdatedController( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::DecoratorNode(name, conf) +{ +} + +BT::NodeStatus GoalUpdatedController::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + // Reset since we're starting a new iteration of + // the goal updated controller (moving from IDLE to RUNNING) + + config().blackboard->get>("goals", goals_); + config().blackboard->get("goal", goal_); + + goal_was_updated_ = true; + } + + setStatus(BT::NodeStatus::RUNNING); + + std::vector current_goals; + config().blackboard->get>("goals", current_goals); + geometry_msgs::msg::PoseStamped current_goal; + config().blackboard->get("goal", current_goal); + + if (goal_ != current_goal || goals_ != current_goals) { + goal_ = current_goal; + goals_ = current_goals; + goal_was_updated_ = true; + } + + // The child gets ticked the first time through and any time the goal has + // changed or preempted. In addition, once the child begins to run, it is ticked each time + // 'til completion + if ((child_node_->status() == BT::NodeStatus::RUNNING) || goal_was_updated_) { + goal_was_updated_ = false; + const BT::NodeStatus child_state = child_node_->executeTick(); + + switch (child_state) { + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + + case BT::NodeStatus::SUCCESS: + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::FAILURE: + default: + return BT::NodeStatus::FAILURE; + } + } + + return status(); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GoalUpdatedController"); +} diff --git a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt index a1da83658b..ed6504a682 100644 --- a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt @@ -18,6 +18,10 @@ ament_add_gtest(test_single_trigger_node test_single_trigger_node.cpp) target_link_libraries(test_single_trigger_node nav2_single_trigger_bt_node) ament_target_dependencies(test_single_trigger_node ${dependencies}) +ament_add_gtest(test_goal_updated_controller test_goal_updated_controller.cpp) +target_link_libraries(test_goal_updated_controller nav2_goal_updated_controller_bt_node) +ament_target_dependencies(test_goal_updated_controller ${dependencies}) + ament_add_gtest(test_decorator_path_longer_on_approach test_path_longer_on_approach.cpp) target_link_libraries(test_decorator_path_longer_on_approach nav2_path_longer_on_approach_bt_node) ament_target_dependencies(test_decorator_path_longer_on_approach ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp new file mode 100644 index 0000000000..d8e2a24fb0 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -0,0 +1,108 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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 "../../test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + // Setting fake goals on blackboard + geometry_msgs::msg::PoseStamped goal1; + goal1.header.stamp = node_->now(); + std::vector poses1; + poses1.push_back(goal1); + config_->blackboard->set("goal", goal1); + config_->blackboard->set>("goals", poses1); + + bt_node_ = std::make_shared( + "goal_updated_controller", *config_); + dummy_node_ = std::make_shared(); + bt_node_->setChild(dummy_node_.get()); + } + + void TearDown() + { + dummy_node_.reset(); + bt_node_.reset(); + } + +protected: + static std::shared_ptr bt_node_; + static std::shared_ptr dummy_node_; +}; + +std::shared_ptr +GoalUpdatedControllerTestFixture::bt_node_ = nullptr; +std::shared_ptr +GoalUpdatedControllerTestFixture::dummy_node_ = nullptr; + +TEST_F(GoalUpdatedControllerTestFixture, test_behavior) +{ + // Creating updated fake-goals + geometry_msgs::msg::PoseStamped goal2; + goal2.header.stamp = node_->now(); + std::vector poses2; + poses2.push_back(goal2); + + // starting in idle + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); + + // tick for the first time, dummy node should be ticked + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // tick again with updated goal, dummy node should be ticked + config_->blackboard->set("goal", goal2); + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // tick again without update, dummy node should not be ticked + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // tick again with updated goals, dummy node should be ticked + config_->blackboard->set>("goals", poses2); + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); +} + +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_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 88bebc3590..a35fe9a870 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -82,6 +82,7 @@ bt_navigator: - nav2_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 7ec0f1edda..618ba12bdf 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -82,6 +82,7 @@ bt_navigator: - nav2_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 1d270eb6a2..41fc36fb64 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -82,6 +82,7 @@ bt_navigator: - nav2_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml new file mode 100644 index 0000000000..46d18fe654 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 0e31d13cd0..79b50992b4 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -64,6 +64,7 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_path_expiring_timer_condition", "nav2_distance_traveled_condition_bt_node", "nav2_single_trigger_bt_node", + "nav2_goal_updated_controller_bt_node", "nav2_is_battery_low_condition_bt_node", "nav2_navigate_through_poses_action_bt_node", "nav2_navigate_to_pose_action_bt_node", diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index c068d67ff9..b4f98cedea 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -78,6 +78,7 @@ bt_navigator: - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index f4fa961a3d..1e021bb4f7 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -79,6 +79,7 @@ bt_navigator: - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index df05269447..fe03fe138d 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -79,6 +79,7 @@ bt_navigator: - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node From 6a5165bb16db7fa94c747c753f4bde8df0a21444 Mon Sep 17 00:00:00 2001 From: SrijaneeBiswas <30804865+SrijaneeBiswas@users.noreply.github.com> Date: Tue, 28 Jun 2022 16:10:00 -0400 Subject: [PATCH 018/131] installing plugins folder of nav2_behaviors package (#3051) Co-authored-by: Srijanee Biswas --- nav2_behaviors/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt index 6b0d4d1bd2..273c5fe664 100644 --- a/nav2_behaviors/CMakeLists.txt +++ b/nav2_behaviors/CMakeLists.txt @@ -125,6 +125,10 @@ install(FILES behavior_plugin.xml DESTINATION share/${PROJECT_NAME} ) +install(DIRECTORY plugins/ + DESTINATION share/${PROJECT_NAME}/plugins/ +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() From e63eae149aa8f9409dafa7e6183b0d450b9a7416 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 30 Jun 2022 11:08:28 -0700 Subject: [PATCH 019/131] Completed PR 2929 (#3038) * accepting empty yaml_filename if no initial map is available * invalid load_map-request does not invalidate existing map, added Testcase * style * finish PR 2929 * finish linting * removing change * removing change * Update test_map_server_node.cpp * Update test_map_server_node.cpp Co-authored-by: Nikolas Engelhard --- nav2_map_server/README.md | 5 ++ .../include/nav2_map_server/map_server.hpp | 3 ++ nav2_map_server/src/map_server/map_server.cpp | 39 ++++++++++------ .../test/component/test_map_server_node.cpp | 46 ++++++++++++++++++- 4 files changed, 79 insertions(+), 14 deletions(-) diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index 64ef980be9..c0e7a2cf6a 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -94,6 +94,11 @@ Then, one would invoke this process with the params file that contains the param $ process_with_multiple_map_servers __params:=combined_params.yaml ``` + +The parameter for the initial map (yaml_filename) has to be set, but an empty string can be used if no initial map should be loaded. In this case, no map is loaded during +on_configure or published during on_activate. The _load_map_-service should the be used to load and publish a map. + + #### Map Saver Like in ROS1 `map_saver` could be used as CLI-executable. It was renamed to `map_saver_cli` diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index 20ab257c4e..d2a7e63f75 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -138,6 +138,9 @@ class MapServer : public nav2_util::LifecycleNode // The message to publish on the occupancy grid topic nav_msgs::msg::OccupancyGrid msg_; + + // true if msg_ was initialized + bool map_available_; }; } // namespace nav2_map_server diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 5920e095be..6f2a6e0483 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -63,7 +63,7 @@ namespace nav2_map_server { MapServer::MapServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("map_server", "", options) +: nav2_util::LifecycleNode("map_server", "", options), map_available_(false) { RCLCPP_INFO(get_logger(), "Creating"); @@ -82,19 +82,26 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - // Get the name of the YAML file to use + // Get the name of the YAML file to use (can be empty if no initial map should be used) std::string yaml_filename = get_parameter("yaml_filename").as_string(); - std::string topic_name = get_parameter("topic_name").as_string(); frame_id_ = get_parameter("frame_id").as_string(); - // Shared pointer to LoadMap::Response is also should be initialized - // in order to avoid null-pointer dereference - std::shared_ptr rsp = - std::make_shared(); - - if (!loadMapResponseFromYaml(yaml_filename, rsp)) { - throw std::runtime_error("Failed to load map yaml file: " + yaml_filename); + // only try to load map if parameter was set + if (!yaml_filename.empty()) { + // Shared pointer to LoadMap::Response is also should be initialized + // in order to avoid null-pointer dereference + std::shared_ptr rsp = + std::make_shared(); + + if (!loadMapResponseFromYaml(yaml_filename, rsp)) { + throw std::runtime_error("Failed to load map yaml file: " + yaml_filename); + } + } else { + RCLCPP_INFO( + get_logger(), + "yaml-filename parameter is empty, set map through '%s'-service", + load_map_service_name_.c_str()); } // Make name prefix for services @@ -125,8 +132,10 @@ MapServer::on_activate(const rclcpp_lifecycle::State & /*state*/) // Publish the map using the latched topic occ_pub_->on_activate(); - auto occ_grid = std::make_unique(msg_); - occ_pub_->publish(std::move(occ_grid)); + if (map_available_) { + auto occ_grid = std::make_unique(msg_); + occ_pub_->publish(std::move(occ_grid)); + } // create bond connection createBond(); @@ -155,6 +164,8 @@ MapServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) occ_pub_.reset(); occ_service_.reset(); load_map_service_.reset(); + map_available_ = false; + msg_ = nav_msgs::msg::OccupancyGrid(); return nav2_util::CallbackReturn::SUCCESS; } @@ -192,6 +203,7 @@ void MapServer::loadMapCallback( RCLCPP_WARN( get_logger(), "Received LoadMap request but not in ACTIVE state, ignoring!"); + response->result = response->RESULT_UNDEFINED_FAILURE; return; } RCLCPP_INFO(get_logger(), "Handling LoadMap request"); @@ -217,9 +229,10 @@ bool MapServer::loadMapResponseFromYaml( response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA; return false; case LOAD_MAP_SUCCESS: - // Correcting msg_ header when it belongs to spiecific node + // Correcting msg_ header when it belongs to specific node updateMsgHeader(); + map_available_ = true; response->map = msg_; response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS; } diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp index d034ca3c68..ae3a80982f 100644 --- a/nav2_map_server/test/component/test_map_server_node.cpp +++ b/nav2_map_server/test/component/test_map_server_node.cpp @@ -24,7 +24,9 @@ #include "nav2_map_server/map_server.hpp" #include "nav2_util/lifecycle_service_client.hpp" #include "nav2_msgs/srv/load_map.hpp" - +using namespace std::chrono_literals; +using namespace rclcpp; // NOLINT + #define TEST_DIR TEST_DIRECTORY using std::experimental::filesystem::path; @@ -191,3 +193,45 @@ TEST_F(MapServerTestFixture, LoadMapInvalidImage) ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA); } + +/** + * Test behaviour of server if yaml_filename is set to an empty string. + */ +TEST_F(MapServerTestFixture, NoInitialMap) +{ + // turn off node into unconfigured state + lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE); + lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP); + + auto client = node_->create_client("/map_server/map"); + auto req = std::make_shared(); + + auto parameters_client = std::make_shared(node_, "map_server"); + ASSERT_TRUE(parameters_client->wait_for_service(3s)); + + // set yaml_filename-parameter to empty string (essentially restart the node) + RCLCPP_INFO(node_->get_logger(), "Removing yaml_filename-parameter before restarting"); + parameters_client->set_parameters({Parameter("yaml_filename", ParameterValue(""))}); + + // only configure node, to test behaviour of service while node is not active + lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, 3s); + + RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service while not being active"); + auto load_map_req = std::make_shared(); + auto load_map_cl = node_->create_client("/map_server/load_map"); + + ASSERT_TRUE(load_map_cl->wait_for_service(3s)); + auto resp = send_request(node_, load_map_cl, load_map_req); + + ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_UNDEFINED_FAILURE); + + // activate server and load map: + lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, 3s); + RCLCPP_INFO(node_->get_logger(), "active again"); + + load_map_req->map_url = path(TEST_DIR) / path(g_valid_yaml_file); + auto load_res = send_request(node_, load_map_cl, load_map_req); + + ASSERT_EQ(load_res->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS); + verifyMapMsg(load_res->map); +} From 02a8daba8a24f833d259811f7e872d1ce7e3880f Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Fri, 8 Jul 2022 12:11:15 -0600 Subject: [PATCH 020/131] zero z values in rpp transformed plan (#3066) --- .../src/regulated_pure_pursuit_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 3428faae85..33166c7af1 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -695,6 +695,7 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( stamped_pose.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose); + transformed_pose.pose.position.z = 0.0; return transformed_pose; }; From 84cd1d848ff2232db86d6f53c291a28bf43dcb90 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 11 Jul 2022 13:11:37 -0700 Subject: [PATCH 021/131] forward porting #3053 (#3064) * forward porting #3053 * adding TF warning suggestion --- nav2_costmap_2d/plugins/range_sensor_layer.cpp | 4 +++- nav2_costmap_2d/test/integration/range_tests.cpp | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp index c8020781d6..ffbe8a3ef8 100644 --- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -293,7 +293,9 @@ void RangeSensorLayer::updateCostmap( in.header.frame_id = range_message.header.frame_id; if (!tf_->canTransform( - in.header.frame_id, global_frame_, tf2_ros::fromMsg(in.header.stamp))) + in.header.frame_id, global_frame_, + tf2_ros::fromMsg(in.header.stamp), + tf2_ros::fromRclcpp(transform_tolerance_))) { RCLCPP_INFO( logger_, "Range sensor layer can't transform from %s to %s", diff --git a/nav2_costmap_2d/test/integration/range_tests.cpp b/nav2_costmap_2d/test/integration/range_tests.cpp index 196406e7e0..1ee5380b7a 100644 --- a/nav2_costmap_2d/test/integration/range_tests.cpp +++ b/nav2_costmap_2d/test/integration/range_tests.cpp @@ -115,6 +115,7 @@ class TestNode : public ::testing::Test : node_(std::make_shared("range_test_node")), tf_(node_->get_clock()) { + tf_.setUsingDedicatedThread(true); // Standard non-plugin specific parameters node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); From 1dd5d27a4f44341f516c2a9caf86e279711347fd Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 12 Jul 2022 12:07:59 -0700 Subject: [PATCH 022/131] removing get node options default for nav2 utils (#3073) --- nav2_util/include/nav2_util/node_utils.hpp | 3 --- nav2_util/src/node_utils.cpp | 9 --------- 2 files changed, 12 deletions(-) diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 484ff35dd1..d1a39d4a5b 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -77,9 +77,6 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix = ""); */ std::string time_to_string(size_t len); -rclcpp::NodeOptions -get_node_options_default(bool allow_undeclared = true, bool declare_initial_params = true); - /// Declares static ROS2 parameter and sets it to a given value if it was not already declared /* Declares static ROS2 parameter and sets it to a given value * if it was not already declared. diff --git a/nav2_util/src/node_utils.cpp b/nav2_util/src/node_utils.cpp index 683ef73e6b..e5415b81ad 100644 --- a/nav2_util/src/node_utils.cpp +++ b/nav2_util/src/node_utils.cpp @@ -89,13 +89,4 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix) return rclcpp::Node::make_shared("_", options); } -rclcpp::NodeOptions -get_node_options_default(bool allow_undeclared, bool declare_initial_params) -{ - rclcpp::NodeOptions options; - options.allow_undeclared_parameters(allow_undeclared); - options.automatically_declare_parameters_from_overrides(declare_initial_params); - return options; -} - } // namespace nav2_util From d40dcd64bbabf966e87d0de09568d2807ab37089 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 12 Jul 2022 13:24:44 -0700 Subject: [PATCH 023/131] adding looping timeout for lifecycle service clients + adding string name of action for BT action nodes (#3071) * adding looping timeout for lifecycle service clients + adding string name of action for BT action nodes * fix linting * remove inline comment * adding goal updated controller node to test --- .../include/nav2_behavior_tree/bt_action_node.hpp | 13 +++++++------ ...vigate_w_replanning_only_if_goal_is_updated.xml | 2 +- .../test/component/test_map_server_node.cpp | 2 +- .../src/behavior_tree/test_behavior_tree_node.cpp | 4 +++- nav2_util/include/nav2_util/service_client.hpp | 9 +++++++++ nav2_util/src/lifecycle_service_client.cpp | 14 ++++++++++++-- 6 files changed, 33 insertions(+), 11 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 8de0b27977..06fc6dd810 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 @@ -17,6 +17,7 @@ #include #include +#include #include "behaviortree_cpp_v3/action_node.h" #include "nav2_util/node_utils.hpp" @@ -26,6 +27,8 @@ namespace nav2_behavior_tree { +using namespace std::chrono_literals; // NOLINT + /** * @brief Abstract class representing an action based BT node * @tparam ActionT Type of action @@ -90,13 +93,11 @@ class BtActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - static constexpr std::chrono::milliseconds wait_for_server_timeout = - std::chrono::milliseconds(1000); - if (!action_client_->wait_for_action_server(wait_for_server_timeout)) { + if (!action_client_->wait_for_action_server(1s)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for %li ms", - action_name.c_str(), wait_for_server_timeout.count()); - throw std::runtime_error("Action server not available"); + node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", + action_name.c_str()); + throw std::runtime_error(std::string("Action server %s not available", action_name.c_str())); } } diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 46d18fe654..3e889440a8 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -5,7 +5,7 @@ - + diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp index ae3a80982f..f47dfbf718 100644 --- a/nav2_map_server/test/component/test_map_server_node.cpp +++ b/nav2_map_server/test/component/test_map_server_node.cpp @@ -26,7 +26,7 @@ #include "nav2_msgs/srv/load_map.hpp" using namespace std::chrono_literals; using namespace rclcpp; // NOLINT - + #define TEST_DIR TEST_DIRECTORY using std::experimental::filesystem::path; diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 2f76823ae4..b5bb56a6f1 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -94,7 +94,8 @@ class BehaviorTreeHandler "nav2_wait_cancel_bt_node", "nav2_spin_cancel_bt_node", "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node" + "nav2_drive_on_heading_cancel_bt_node", + "nav2_goal_updated_controller_bt_node" }; for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); @@ -216,6 +217,7 @@ TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) if (boost::filesystem::exists(root) && boost::filesystem::is_directory(root)) { for (auto const & entry : boost::filesystem::recursive_directory_iterator(root)) { if (boost::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") { + std::cout << entry.path().string() << std::endl; EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true); } } diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 4af107abf1..9be5912e8c 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -132,6 +132,15 @@ class ServiceClient return client_->wait_for_service(timeout); } + /** + * @brief Gets the service name + * @return string Service name + */ + std::string getServiceName() + { + return service_name_; + } + protected: std::string service_name_; rclcpp::Node::SharedPtr node_; diff --git a/nav2_util/src/lifecycle_service_client.cpp b/nav2_util/src/lifecycle_service_client.cpp index 788c44123a..70656d9f29 100644 --- a/nav2_util/src/lifecycle_service_client.cpp +++ b/nav2_util/src/lifecycle_service_client.cpp @@ -36,7 +36,12 @@ LifecycleServiceClient::LifecycleServiceClient(const string & lifecycle_node_nam get_state_(lifecycle_node_name + "/get_state", node_) { // Block until server is up - get_state_.wait_for_service(); + rclcpp::Rate r(20); + while (!get_state_.wait_for_service(2s)) { + RCLCPP_INFO( + node_->get_logger(), "Waiting for service %s...", get_state_.getServiceName().c_str()); + r.sleep(); + } } LifecycleServiceClient::LifecycleServiceClient( @@ -47,7 +52,12 @@ LifecycleServiceClient::LifecycleServiceClient( get_state_(lifecycle_node_name + "/get_state", node_) { // Block until server is up - get_state_.wait_for_service(); + rclcpp::Rate r(20); + while (!get_state_.wait_for_service(2s)) { + RCLCPP_INFO( + node_->get_logger(), "Waiting for service %s...", get_state_.getServiceName().c_str()); + r.sleep(); + } } bool LifecycleServiceClient::change_state( From b15367f624964e90710e947e79a46d6a33756bca Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 20 Jul 2022 10:47:43 -0700 Subject: [PATCH 024/131] Smac Planner 2D changes (#3083) * removing 4-connected option from smac; fixing 2D heuristic and traversal function from other planner's changes * fix name of variable since no longer a neighborhood * partial test updates * ported unit tests fully * revert to no costmap downsampling --- nav2_smac_planner/README.md | 4 +-- .../include/nav2_smac_planner/constants.hpp | 21 +++++--------- nav2_smac_planner/src/node_2d.cpp | 29 +++++++------------ nav2_smac_planner/src/smac_planner_2d.cpp | 29 ++----------------- nav2_smac_planner/test/test_a_star.cpp | 17 +++++------ nav2_smac_planner/test/test_node2d.cpp | 15 ++-------- nav2_smac_planner/test/test_smac_2d.cpp | 4 --- 7 files changed, 33 insertions(+), 86 deletions(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 086403edf0..0f58190b69 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -3,7 +3,7 @@ The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 3 distinct plugins: - `SmacPlannerHybrid`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (legged, ackermann and car models). - `SmacPlannerLattice`: a highly optimized fully reconfigurable State Lattice implementation supporting configurable minimum control sets, with provided control sets for Ackermann, Legged, Differential and Omnidirectional models. -- `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting Moore and Von Neumann models. +- `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting 8-connected neighborhood models. It also introduces the following basic building blocks: - `CostmapDownsampler`: A library to take in a costmap object and downsample it to another resolution. @@ -111,7 +111,7 @@ planner_server: max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. - motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally + motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index 8bd0db5575..a7ff03fdca 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -22,20 +22,17 @@ namespace nav2_smac_planner enum class MotionModel { UNKNOWN = 0, - VON_NEUMANN = 1, - MOORE = 2, - DUBIN = 3, - REEDS_SHEPP = 4, - STATE_LATTICE = 5, + TWOD = 1, + DUBIN = 2, + REEDS_SHEPP = 3, + STATE_LATTICE = 4, }; inline std::string toString(const MotionModel & n) { switch (n) { - case MotionModel::VON_NEUMANN: - return "Von Neumann"; - case MotionModel::MOORE: - return "Moore"; + case MotionModel::TWOD: + return "2D"; case MotionModel::DUBIN: return "Dubin"; case MotionModel::REEDS_SHEPP: @@ -49,10 +46,8 @@ inline std::string toString(const MotionModel & n) inline MotionModel fromString(const std::string & n) { - if (n == "VON_NEUMANN") { - return MotionModel::VON_NEUMANN; - } else if (n == "MOORE") { - return MotionModel::MOORE; + if (n == "2D") { + return MotionModel::TWOD; } else if (n == "DUBIN") { return MotionModel::DUBIN; } else if (n == "REEDS_SHEPP") { diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 7917d062f0..442fb62b68 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -72,9 +72,10 @@ float Node2D::getTraversalCost(const NodePtr & child) // If a diagonal move, travel cost is sqrt(2) not 1.0. if ((dx * dx + dy * dy) > 1.05) { - return sqrt_2 + cost_travel_multiplier * normalized_cost; + return sqrt_2 * (1.0 + cost_travel_multiplier * normalized_cost); } + // Length = 1.0 return 1.0 + cost_travel_multiplier * normalized_cost; } @@ -85,34 +86,24 @@ float Node2D::getHeuristicCost( { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - return fabs(goal_coordinates.x - node_coords.x) + - fabs(goal_coordinates.y - node_coords.y); + return hypotf(goal_coordinates.x - node_coords.x, goal_coordinates.y - node_coords.y); } void Node2D::initMotionModel( - const MotionModel & neighborhood, + const MotionModel & motion_model, unsigned int & x_size_uint, unsigned int & /*size_y*/, unsigned int & /*num_angle_quantization*/, SearchInfo & search_info) { + if (motion_model != MotionModel::TWOD) { + throw std::runtime_error("Invalid motion model for 2D node."); + } + int x_size = static_cast(x_size_uint); cost_travel_multiplier = search_info.cost_penalty; - switch (neighborhood) { - case MotionModel::UNKNOWN: - throw std::runtime_error("Unknown neighborhood type selected."); - case MotionModel::VON_NEUMANN: - _neighbors_grid_offsets = {-1, +1, -x_size, +x_size}; - break; - case MotionModel::MOORE: - _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1, - -x_size + 1, +x_size - 1, +x_size + 1}; - break; - default: - throw std::runtime_error( - "Invalid neighborhood type selected. " - "Von-Neumann and Moore are valid for Node2D."); - } + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1, + -x_size + 1, +x_size - 1, +x_size + 1}; } void Node2D::getNeighbors( diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index cf8657118f..606f6861ef 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -91,17 +91,7 @@ void SmacPlanner2D::configure( node, name + ".max_planning_time", rclcpp::ParameterValue(2.0)); node->get_parameter(name + ".max_planning_time", _max_planning_time); - nav2_util::declare_parameter_if_not_declared( - node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); - node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search); - _motion_model = fromString(_motion_model_for_search); - if (_motion_model == MotionModel::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get MotionModel search type. Given '%s', " - "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", - _motion_model_for_search.c_str()); - } + _motion_model = MotionModel::TWOD; if (_max_on_approach_iterations <= 0) { RCLCPP_INFO( @@ -154,10 +144,9 @@ void SmacPlanner2D::configure( RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlanner2D with " "tolerance %.2f, maximum iterations %i, " - "max on approach iterations %i, and %s. Using motion model: %s.", + "max on approach iterations %i, and %s.", _name.c_str(), _tolerance, _max_iterations, _max_on_approach_iterations, - _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(_motion_model).c_str()); + _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal"); } void SmacPlanner2D::activate() @@ -392,18 +381,6 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete _max_on_approach_iterations = std::numeric_limits::max(); } } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == _name + ".motion_model_for_search") { - reinit_a_star = true; - _motion_model = fromString(parameter.as_string()); - if (_motion_model == MotionModel::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get MotionModel search type. Given '%s', " - "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", - _motion_model_for_search.c_str()); - } - } } } diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 68af08df50..00852e5298 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -41,7 +41,7 @@ TEST(AStarTest, test_a_star_2d) { nav2_smac_planner::SearchInfo info; nav2_smac_planner::AStarAlgorithm a_star( - nav2_smac_planner::MotionModel::MOORE, info); + nav2_smac_planner::MotionModel::TWOD, info); int max_iterations = 10000; float tolerance = 0.0; float some_tolerance = 20.0; @@ -69,7 +69,7 @@ TEST(AStarTest, test_a_star_2d) a_star.setGoal(80u, 80u, 0); nav2_smac_planner::Node2D::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); - EXPECT_EQ(num_it, 102); + EXPECT_EQ(num_it, 2414); // check path is the right size and collision free EXPECT_EQ(path.size(), 81u); @@ -84,7 +84,7 @@ TEST(AStarTest, test_a_star_2d) path.clear(); // failure cases with invalid inputs nav2_smac_planner::AStarAlgorithm a_star_2( - nav2_smac_planner::MotionModel::VON_NEUMANN, info); + nav2_smac_planner::MotionModel::TWOD, info); a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1); num_it = 0; EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); @@ -104,7 +104,7 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); - EXPECT_EQ(path.size(), 42u); + EXPECT_EQ(path.size(), 20u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -286,10 +286,8 @@ TEST(AStarTest, test_constants) { nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Unknown")); - mm = nav2_smac_planner::MotionModel::VON_NEUMANN; // vonneumann - EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Von Neumann")); - mm = nav2_smac_planner::MotionModel::MOORE; // moore - EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Moore")); + mm = nav2_smac_planner::MotionModel::TWOD; // 2d + EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("2D")); mm = nav2_smac_planner::MotionModel::DUBIN; // dubin EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Dubin")); mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp @@ -297,8 +295,7 @@ TEST(AStarTest, test_constants) EXPECT_EQ( nav2_smac_planner::fromString( - "VON_NEUMANN"), nav2_smac_planner::MotionModel::VON_NEUMANN); - EXPECT_EQ(nav2_smac_planner::fromString("MOORE"), nav2_smac_planner::MotionModel::MOORE); + "2D"), nav2_smac_planner::MotionModel::TWOD); EXPECT_EQ(nav2_smac_planner::fromString("DUBIN"), nav2_smac_planner::MotionModel::DUBIN); EXPECT_EQ( nav2_smac_planner::fromString( diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index f4520bd496..f3af941c4b 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -50,7 +50,7 @@ TEST(Node2DTest, test_node_2d) info.cost_penalty = 1.0; unsigned int size = 10; nav2_smac_planner::Node2D::initMotionModel( - nav2_smac_planner::MotionModel::MOORE, size, size, size, info); + nav2_smac_planner::MotionModel::TWOD, size, size, size, info); // test reset testA.reset(); @@ -68,7 +68,7 @@ TEST(Node2DTest, test_node_2d) // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 15., 0.01); + EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 11.18, 0.02); // check operator== works on index unsigned char costC = '2'; @@ -106,18 +106,9 @@ TEST(Node2DTest, test_node_2d_neighbors) unsigned int size_y = 10u; unsigned int quant = 0u; // test neighborhood computation - nav2_smac_planner::Node2D::initMotionModel( - nav2_smac_planner::MotionModel::VON_NEUMANN, size_x, - size_y, quant, info); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -10); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 10); - size_x = 100u; nav2_smac_planner::Node2D::initMotionModel( - nav2_smac_planner::MotionModel::MOORE, size_x, size_y, + nav2_smac_planner::MotionModel::TWOD, size_x, size_y, quant, info); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1); diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index ebbac996cd..1ebe215b30 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -108,7 +108,6 @@ TEST(SmacTest, test_smac_2d_reconfigure) { rclcpp::Parameter("test.downsampling_factor", 2), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.max_on_approach_iterations", -1), - rclcpp::Parameter("test.motion_model_for_search", "UNKNOWN"), rclcpp::Parameter("test.use_final_approach_orientation", false)}); rclcpp::spin_until_future_complete( @@ -128,9 +127,6 @@ TEST(SmacTest, test_smac_2d_reconfigure) { EXPECT_EQ( node2D->get_parameter("test.max_on_approach_iterations").as_int(), -1); - EXPECT_EQ( - node2D->get_parameter("test.motion_model_for_search").as_string(), - "UNKNOWN"); results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test.downsample_costmap", true)}); From 35df8e67bd464e2e5fcf2a41b5acea433bd2b906 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Wed, 20 Jul 2022 21:16:39 +0300 Subject: [PATCH 025/131] Collision monitor (#2982) * Add Collision Monitor node * Meet review items * Fix next review items * Code cleanup * Support dynamic footprint. More optimizations. * Switch to multiple footprints. Move variables. Remove odom subscriber. Add 0-velocity optimization * Update nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp Co-authored-by: Steve Macenski * Update nav2_collision_monitor/params/collision_monitor_params.yaml Co-authored-by: Steve Macenski * Update nav2_collision_monitor/params/collision_monitor_params.yaml Co-authored-by: Steve Macenski * Update nav2_collision_monitor/params/collision_monitor_params.yaml Co-authored-by: Steve Macenski * Meet smaller review items * Add fixes found during unit test development * Fix uncrustify issues * Add unit tests * Fix number of polygons points * Move tests * Add kinematics unit test * Minor tests fixes * Remove commented line * Add edge case checking testcase and references * Update comment * Add README.md * Fixed table * Minor changes in README.md * Fix README.md for documentation pages * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Meet review items * Meet review items (part 2) * Update polygons picture for README * Change simulation_time_step to 0.1 * Fix bounding boxes to fit the demo from README.md * Terminology fixes Co-authored-by: Steve Macenski --- nav2_collision_monitor/CMakeLists.txt | 109 +++ nav2_collision_monitor/README.md | 67 ++ nav2_collision_monitor/doc/HLD.png | Bin 0 -> 65660 bytes nav2_collision_monitor/doc/polygons.png | Bin 0 -> 552407 bytes .../include/nav2_collision_monitor/circle.hpp | 85 ++ .../collision_monitor_node.hpp | 216 +++++ .../nav2_collision_monitor/kinematics.hpp | 46 ++ .../nav2_collision_monitor/pointcloud.hpp | 97 +++ .../nav2_collision_monitor/polygon.hpp | 211 +++++ .../include/nav2_collision_monitor/scan.hpp | 88 ++ .../include/nav2_collision_monitor/source.hpp | 132 +++ .../include/nav2_collision_monitor/types.hpp | 79 ++ .../launch/collision_monitor_node.launch.py | 97 +++ nav2_collision_monitor/package.xml | 32 + .../params/collision_monitor_params.yaml | 48 ++ nav2_collision_monitor/src/circle.cpp | 104 +++ .../src/collision_monitor_node.cpp | 473 +++++++++++ nav2_collision_monitor/src/kinematics.cpp | 70 ++ nav2_collision_monitor/src/main.cpp | 29 + nav2_collision_monitor/src/pointcloud.cpp | 124 +++ nav2_collision_monitor/src/polygon.cpp | 380 +++++++++ nav2_collision_monitor/src/scan.cpp | 106 +++ nav2_collision_monitor/src/source.cpp | 108 +++ nav2_collision_monitor/test/CMakeLists.txt | 35 + .../test/collision_monitor_node_test.cpp | 777 ++++++++++++++++++ .../test/kinematics_test.cpp | 98 +++ nav2_collision_monitor/test/polygons_test.cpp | 698 ++++++++++++++++ nav2_collision_monitor/test/sources_test.cpp | 439 ++++++++++ 28 files changed, 4748 insertions(+) create mode 100644 nav2_collision_monitor/CMakeLists.txt create mode 100644 nav2_collision_monitor/README.md create mode 100644 nav2_collision_monitor/doc/HLD.png create mode 100644 nav2_collision_monitor/doc/polygons.png create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/source.hpp create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/types.hpp create mode 100644 nav2_collision_monitor/launch/collision_monitor_node.launch.py create mode 100644 nav2_collision_monitor/package.xml create mode 100644 nav2_collision_monitor/params/collision_monitor_params.yaml create mode 100644 nav2_collision_monitor/src/circle.cpp create mode 100644 nav2_collision_monitor/src/collision_monitor_node.cpp create mode 100644 nav2_collision_monitor/src/kinematics.cpp create mode 100644 nav2_collision_monitor/src/main.cpp create mode 100644 nav2_collision_monitor/src/pointcloud.cpp create mode 100644 nav2_collision_monitor/src/polygon.cpp create mode 100644 nav2_collision_monitor/src/scan.cpp create mode 100644 nav2_collision_monitor/src/source.cpp create mode 100644 nav2_collision_monitor/test/CMakeLists.txt create mode 100644 nav2_collision_monitor/test/collision_monitor_node_test.cpp create mode 100644 nav2_collision_monitor/test/kinematics_test.cpp create mode 100644 nav2_collision_monitor/test/polygons_test.cpp create mode 100644 nav2_collision_monitor/test/sources_test.cpp diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt new file mode 100644 index 0000000000..e1b1c67567 --- /dev/null +++ b/nav2_collision_monitor/CMakeLists.txt @@ -0,0 +1,109 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_collision_monitor) + +### Dependencies ### + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_costmap_2d REQUIRED) + +### Header ### + +nav2_package() + +### Libraries and executables ### + +include_directories( + include +) + +set(dependencies + rclcpp + rclcpp_components + sensor_msgs + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + nav2_util + nav2_costmap_2d +) + +set(executable_name collision_monitor) +set(library_name ${executable_name}_core) + +add_library(${library_name} SHARED + src/collision_monitor_node.cpp + src/polygon.cpp + src/circle.cpp + src/source.cpp + src/scan.cpp + src/pointcloud.cpp + src/kinematics.cpp +) + +add_executable(${executable_name} + src/main.cpp +) + +ament_target_dependencies(${library_name} + ${dependencies} +) + +target_link_libraries(${executable_name} + ${library_name} +) + +ament_target_dependencies(${executable_name} + ${dependencies} +) + +rclcpp_components_register_nodes(${library_name} "nav2_collision_monitor::CollisionMonitor") + +### Install ### + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +### Testing ### + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +### Ament stuff ### + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) + +ament_package() diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md new file mode 100644 index 0000000000..bb6fac5b75 --- /dev/null +++ b/nav2_collision_monitor/README.md @@ -0,0 +1,67 @@ +# Nav2 Collision Monitor + +The Collision Monitor is a node providing an additional level of robot safety. +It performs several collision avoidance related tasks using incoming data from the sensors, bypassing the costmap and trajectory planners, to monitor for and prevent potential collisions at the emergency-stop level. + +This is analogous to safety sensor and hardware features; take in laser scans from a real-time certified safety scanner, detect if there is to be an imminent collision in a configurable bounding box, and either emergency-stop the certified robot controller or slow the robot to avoid such collision. +However, this node is done at the CPU level with any form of sensor. +As such, this does not provide hard real-time safety certifications, but uses the same types of techniques with the same types of data for users that do not have safety-rated laser sensors, safety-rated controllers, or wish to use any type of data input (e.g. pointclouds from depth or stereo or range sensors). + +This is a useful and integral part of large heavy industrial robots, or robots moving with high velocities, around people or other dynamic agents (e.g. other robots) as a safety mechanism for high-response emergency stopping. +The costmaps / trajectory planners will handle most situations, but this is to handle obstacles that virtually appear out of no where (from the robot's perspective) or approach the robot at such high speed it needs to immediately stop to prevent collision. + +![polygons.png](doc/polygons.png) + +## Features + +The Collision Monitor uses polygons relative the robot's base frame origin to define "zones". +Data that fall into these zones trigger an operation depending on the model being used. +A given instance of the Collision Monitor can have many zones with different models at the same time. +When multiple zones trigger at once, the most aggressive one is used (e.g. stop > slow 50% > slow 10%). + +The following models of safety behaviors are employed by Collision Monitor: + +* **Stop model**: Define a zone and a point threshold. If more that `N` obstacle points appear inside this area, stop the robot until the obstacles will disappear. +* **Slowdown model**: Define a zone around the robot and slow the maximum speed for a `%S` percent, if more than `N` points will appear inside the area. +* **Approach model**: Using the current robot speed, estimate the time to collision to sensor data. If the time is less than `M` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least `M` seconds to collision. The effect here would be to keep the robot always `M` seconds from any collision. + +The zones around the robot can take the following shapes: + +* Arbitrary user-defined polygon relative to the robot base frame. +* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape. +* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time. + +The data may be obtained from different data sources: + +* Laser scanners (`sensor_msgs::msg::LaserScan` messages) +* PointClouds (`sensor_msgs::msg::PointCloud2` messages) +* IR/Sonars (`sensor_msgs::msg::Range` messages, not implemented yet) + +## Design + +The Collision Monitor is designed to operate below Nav2 as an independent safety node. +This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate. + +The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model. +![HDL.png](doc/HLD.png) + +## Configuration + +Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages. + + +## Metrics + +Designed to be used in wide variety of robots (incl. moving fast) and have a high level of reliability, Collision Monitor node should operate at fast rates. +Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points). +The table below represents the details of operating times for different behavior models and shapes: + +| | Stop/Slowdown model, Polygon area | Stop/Slowdown model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint | +|-|-----------------------------------|----------------------------------|-----------------------------------|----------------------------------| +| LaserScan (360 points) processing time, ms | 4.45 | 4.45 | 4.93 | 4.86 | +| PointCloud (24K points) processing time, ms | 4.94 | 4.06 | 20.67 | 10.87 | + +The following notes could be made: + + * Due to sheer speed, circle shapes are preferred for the approach behavior models if you can approximately model your robot as circular. + * More points mean lower performance. Pointclouds could be culled or filtered before the Collision Monitor to improve performance. diff --git a/nav2_collision_monitor/doc/HLD.png b/nav2_collision_monitor/doc/HLD.png new file mode 100644 index 0000000000000000000000000000000000000000..a55702b4800c8a17a727f8eb3d3a61e90287af56 GIT binary patch literal 65660 zcmagG1yq$?^FGYsz@fWK8UzjvN=b=?goH?lv~+`n)FDKWkZvReDFNv|NQbnDw1fgu z0uoZ+K0f+B@B3T-wLaH!ox1OP?>#en=9+6}6QQZ0M1V_+i-v|qprWjxjfRE^-lC6U zqobigZc;x@0iPi5+Dh_hZ-?nOzy}O#IdwTSw5peQ=Vqu&aa@!Q+|keoJ5m23z0Pke z(a^45t0>6b_cq?T2y~(Nl=Uszo?AFwG^#zs%I?4-%!WtuCE2x4d$A;ho_PG>sl>=U`Kv_K5ZxL=KRcRM~7T=>(%AfR$0%B zJ3lxZ^7C(Vcw1U_4XvM_OC`rAp7bYhs`B6}DBd8BPaIQj# zgnN2=+L;HPo}Ru|O_}L_KXILmUi7|gFKP`gWKn5&FKdXQ407hvL=qvnnEa##ac|bK zDJ{&+O)Xedtv9WPu!!}|CBc`D5Xlu*C*H=<#O^bTJNGMc)>S5Gc!|xloFouXunS0%j;&cjonI5bht8&;A6u*7CG987cor4YDCE& zK71hU-Ixie#V%SO_e-XsrjFGSSr#LYB_~iMC4=vfExVPWCCb@{C+`2?4_Oa*5i`Zl zDn1-~hhSX!K1z#noYdbe4KBx4Ik~4#*aW4t%|$rSDjKJET+UC{Jk~c{&j}gE^~q@;2V+|A zzsD%2Az_eH|ALV+`s=aC;QO=|0Xcg`1tm=#u?}{W1vN2PUj~OvMa0D96q4HoTU%Lq z=so+omSQqBK-t81=dDp=p8uzGd3BdKvB1lgo{|u<%sX}`obNN2f;(|y)O2(b<|a}a zEYZ)1)Z-Y>{NK#H$B7w2A`v}s&)tP+714v}aH&*#N5>>inV-RAM!ajbtsi2xE$6>r zP|<6+hzzm1u75u~^muJ8*(c-vHL7ck0mml-y-!lp#(H<5HJr^K{oC&=JGh21EL0C$ zSLgB`YU|i^d^Z2|_7QC<$(_`tYU>fZvTLcOMGo?KM-Zx}hK6oZs?PWcZ5WA>JnQ16 z1^;TnUi^Irv5tD;E2YBG7H_!f(A(1$!m^}(%E|{X9+PRfbb$F}%BxqlT_YhW_mkXg z2vsb7Or+t0Q||0u$7Q&OZWcox&zw)@a3x2vXWV^a{At}IN#LQnPKhbQb&lV zM)4fF;o_2-vFY^X$B|?fLaUkg&n^3YVXVYcs19Knk({c2i-^FnWGQO7yy4MA$>D8h zcQ^j~R5{PcM!CwJn4SR}f0wh>XE>QlGsHcgCO7t>lx2c4HPQ|KK#|tLs!z9uj7+KR z>?gd1EX{+>9`17vEf{ohAE=|#O;`26E$eogccmk|Ki4~F3)<6_6Aif69hxL1n;yz? z3B9YZU2AuwJ-6bL7Mz~^QCv9etLlo=K_*p2Y@lUVVOw5%SDuhV|61JpHsx$o+ZjQri~p=qWLezs@jeq?R}9j{%_g2Y`GOnxLE zmOm_(TcW>7DD*JZ6h0PiVBSU&htu}-XUQs-no9vv$K&a`#~r2bA+wzPYD*YJ&o%Ts z{t-#&T{8U2%%pyMyZ6Norp#p0vc2*F<7D(i8hho$0v$Yqsu32!BpdKvcBL1t!K^tG z6b$7=PT4I2%qvCf-Y&t;&$b^rHRPRJPxQvRrQkRSpwD2@yR8#nD%LJEzp@n5Exqln}_XF)hOr?pg%jMhR zb9@%Evt%+Omv@pK%)ChQ@7vqm`NjdlOo*Qea5jC1fj8aS%`Fy?`26J!E}|EWz6>jYUQ zhG?d#TEzK=xzy^v#Vw(`}|t)b-2O1 z)ekp+N?r}7@XNR^V+cXwKUJ8dgv(`=-pn+m%E}f3Hhbg7gI9~=EqSZ&9uHb3<$U-^ zr%@R>eD9-;`_C}4t-J#Z<{NgE31Y(bLm@ZxoW(l$BxC}Xq~L};fd}5F`;S$YCwVa) z{*GQu>-cuTCHRp?K#Ah-K~i*zG&aa}pZ~rU^Ept4kVuEHurQ|YTGiL=s2B)u!xFh) z@RtI+vH*w0wnr8}bC$^9*^^MK!nMas2b=#&%mc2<$4D$g;2M=XMeC?&ugmE{Op}w# zslU)pLq{jPNF(F_OT;ai-(+EK&S7B~GBGis-)z}WN*>$r_6>rN1-fb*yvADnu+Cwu z2CLjev6|au2^r)#TbIVwg;8NxJD|1)WY(f%^UWo=RNhP5<2Q;RlL*JxJjc0AC|Zmn zuQ7bG&Lkw;!Fkew#pvCW&T_m2B0Tfre992DOYlX={g=^% z=7OvI6#^aZYclSb%IP?-va@@IjwW$|2sUP`z^p{3FuVdBWtN$JR-$hf#8Iy^*@aR! zb*P|cLRaG=s0>{+V-?MFLae=8$lvXqRL90>0~tO|a-VZq166NxSK!$ztRWRT03(8cWPl-67`MLg z!L3|cJgrvR#;IGu9#_WqAdX~4&%BPAZ5mXx?vN|VC1KbeWtBuC8ADPN;DNFUW{&Bl zp~XD8(mP66aS*W%xj*l4B|F;MiQ10ndNaB3v9A{EcI|4zrG8H803FA6gpf;&YELHnlLR`ng>milW}f%U!{u_1)2=#9P*kL((fBWT{m#!w-1e|C=yrX~+g>@-`)nz6W zn~V%jVVJZBJhCWI+#}B-|LNajdVL9FuxL1nN>V^TbC~ZD+F)d_>;s&Jz{#2~v_g)t3ph(Ng`uK^JCGRJ1 zWeRKVM>{#khmq=(occvP5{8S*bE=GiR}MO%OJ7FY{58Msbqw)vHn2!MqHH*dV~mmX z-eX$czxCR8w^h$=;Lcqe{}5pxv)~ZxOG<)hv5x)b_V85SU#V;#u*kudF*Y2zJ4_z?YaLryJ2VAontEEiW`>fEPK&&62!wzjt!7#Oy~U z6$IT`am1DLqVCISyv~;^DuuW1y9cQz!{zA30X#|2l?R!@MgChulgdJMj)pUtl;!7) zpKy|O6BZ}JVHwZFg&}0{xKlTQQ_sE~4856oH21LDql$80JV|lor-PviMfP73Vp)(J zw#Y3z2S_>pG{CLXiJWjd5YMbJvfi%S_;;i=#&+}oQKEBtlMH9eRDfvhDDPs$XIW{G zD$H81nvtGPwG3Tja+NeSUh@(7t&+c@`|v(9siv?6L#P9}s!T&>(-E1E3ikYB!c;=j z-W(|0z#oIc4Q)$9MyX3nX)WC$nEyV(Clba#yvj?NX)?8np{IZ{0}~hE_?EhbpGAhw ze);#i-6H{ykl*}kD z`|_|z!h&S%S8ZW?5tbZT?iY*%5r6>z_(fq(@bW?bkd^pJ5WvR!Nkj@;I8N`RSF}xl zwQ^37!JUt$Pc9$p+uN7=DFk`=@ohO_P2PEj&DG z4huXX&2cm1UG}rK> zlkrl3<@y0Yn=C9vlG7M&F3G~yGU67hM#;jzQ>9KJcL(D3^!ORJDMxR6d?yfBP z9+krUU9=b$&lZkBE=Xd=942xg>FVX%_=m+@XKT|4_Sa|xK_xA;Ogq*3d z^tYi$U%r0Gpk+-f>Ifu`gZa@ldv1?YGy5Q5na;_4kGP%h^}Q=T{~`fr=i8N2)|Nd# z`LXhq>_mTnYHMb8mWWY03cDnO=+heZw^`h1D1q2wUu1se?Ste>cyCWnWoCBvO@2zc zE@><*tTlg8ADXu+B%>b)T>sCwF&0f3nW z4jBbBStd7eh+Q?A8mjoj%-fq<{D#uYYu(yY>Dhd<0AsZIpS7G;jcs#MiY!&G%O$eR zRAD`u_g+6D!=rW4KV-T~vOOL0)=BFB43i=*=CXe8Re7xlE&v z_7W`GR5ndf7dIZJk0Ltru(V#-^j`CLl7_|TH(FJnj^&Y?^^tD=l6DMO|86uM85vn$ zB2gy`S4H?B-B$p+D3;Y7pp>-_*scFFXgbf{Ey=*GRSXOU1w+%>s_1uQLcyu_47vZkmN@*A zSC6c>S1T^2I4jRTG|OB7>LVf?t~Ym8u1mxqyIQ~NgM`a8o-!pbx^s{j(g@Cdp&Dk*CJE|7C_L5q8L00*UaN z(d~OAg5o(FvvQYt?JX3VtsMU&BxM+!)^|67wfxv$Zdn%wB=vtRkMCf#(?sYi`8ZS&9|YF5BBN2F{6>|ep?DQ|^qD@Y#YlwJtfWonEXPsrEH9E>8W z!>=iDh(Ii|W$!)SQ~KBP zdqicnxnX92M5u2;-`{an%z=E0?(bKLjx&lYW=t%Ad?K51X=@0$XzCGy{^K_|es%?1 zEkycxgki`+IdGQsbJ&`jn?IP;e}=*Bi;9W{cUMzfN?4Ux_IcmL#gWuZ%*=2HwoRn| zxBp?o=5t*e`S9UWD)G%gF}}dz81!4WZb^D>UDv&S1!;0ufvY4cJ%ZnRC1snQRA6NL z^5sj`UbZp9^jewQth%etpT~l^D9@inM%{*TM3AjmXms4H4;A<@K$VL3$GdTee(i|z zAF7%jMF@8_x_=W<6hgjz)GU&0W`j&yIzK(2Wo5$^SyGEFK3>V7Tu4cM^(q-V*0=6! zMxmLk~}Of z6(>TVU!R+w=Y0Zwp#nkZC}#Y8whEVp-WWxwT;@RNY>;e$qX+_#B^#-$^8>M`q-$HB zt7mA6(T?JKgm8d9)_Wg~oBR10NfNI9!H&Vq7|)NkLnbnf_R@I>rZrul$$EYvI6AO5 zX7d^o=;>`)Prgst4w)~u`P1^OJX9Qw;Ss2*=WX2UOKvZO^oGIcAywJK!x}!V#>roM z=PPklPccgt_A7W*3tM7%RQrv{YfENrot%>U0JdazJKbNKD7wdr3Ydb!x}^`y8bS2@ z^2JP{3!#Otni%DXtXB{Us=O%8{^%`$cO%&*2Z9yr3r)siMpx6iR>j0(#Wnb`RBu)V z-tDob5@E^r!cfg{>?Bw zXKYUj8U2+z8~;OI6js*W^u6Etc=bNe(ca03rmwGWV4w_3Dy)&7_k+^ut_-v5qeqYW zP$a97@inCAW)Kuh6M5NqpS+XQ{sSItyi6r84$!d9Bd>17JJ{ONFfuAtgS1RR`*Mn$ z#E61*;(o`w+S-rg35Ec|Y9r74GuR&69t(+zvdaYX_1D^eiiwV9es|qY_wlMZwJ8w_ zRD}1ji*!+bNdA;KXCWEP-S8s$`?pfPigbHt-||)J8Vjt!p)LfwTJ|Wt;8NYo{4a&4 z!cCrg8HUvMoG{h6#!sq)NB5FnsL)8>w?&fD7=)X)Rl%QFvP01c#H22T?=Q^0*41Cu zOjs1^?cN-Jt-6*>REC#qG0>MG5nm%GJjo<4S7wD$UR^7>YBIog6yq?m5$qAqfGNrLZ>8F zJep4?KOkhZdxIqH=<=9M7y zu~k1H*h1iJqR^e)4H{b7{Q&Z3Fb7n`Ma{(m>tz)?iI6#RCL4>k63qS2gQR*MtPlrx z5qR4u`<9>)Ov^$EBMo&ys3(kR?Iy@*KkP>FXc;8Mxey{iXq;8`=Ea2m&ttj5a2~e* zx>Ih9XHK_wRN;3g;|<||jd07T(QPfF_l7u!-E$r#ndz(gTiV`$xnmJs1?Wy7m(FYM z%XHCA*+;b+>ba=xPw}ITwKec6ptd}8V?*np^c5R^!;aLCE;3qsZyO{gmRosMUTpEm z#S{bW-ZCJBZnaBLo@wY#A;aj!-4w_yA+#RW$hz5@$@P_l8ML*)575(aFS_=skFr}_O zuvj*q)xPG6S{9(E?2!c^`mwT_)X>w=biAvsCJPpjk&(G|i}x2(iqB{)gQO=@@b_D% zFydHUKxMpVXJOISs=@3tWXk4|?=s>U;LqsC3Ms(t5s;SVdr}2|q^dbeuXAx6}XPGZ8i_#95mXEN8jKxoDbF8<tT2o?Ql({(Oxk)_$#gs1op(FBT# z7~#VhOrI9MJfx(@EQC;HEYnp^A55_nnx;^?QyT2WUitU<(&OS&NrS)!L{@3E}iH5}Zz1JF(5q)5N^ z2Ik2^U{x-9!ktl>%07GvxONy!jZ$vAC&X{;+ESA&OYp9<9D|g%CJVsFz587eWbsi! zgprB4kc2y$%|Do6@ODwNE3p-jdyp8^IiyXOl)2$kGkseh%YWRSYk8%=5O{T=Z|}yP z+sX3S25Ct$=_C~LpZoj&9k5Uc1=;ma^=4ckQG*snrWuiD2FrQ0RMQ;Qjg1Cx-^THa zTj>h?XM%-7J)0WY(!8$lZAQn$q2gjL1C4w4Sk3YEot}Pf|41G6{~Cltc)e1YhE{-C z9IR3~CKo~$Z()NBU8Sb2lo`2oOfRFDV8HjN$zy#q_xq;``=+2=Nb{(ZN=y!9v8T$S zS42_yS`H*(t{Uu`XwhpTOAl9fz6*b~mpsDy;V=b4x$BCex;}utiWc<~HG9u)#Z!ry z)%VR;#3B*ht6d1Qw>%HZu2*nD%|*AOvE))QtLY&E#sl>~aPg8o3xt+^?jxo`gxptxtb`>3q(-)EDRHtKtSJ zdVxy+IM{ms8wma9E7rYX$=!9YwD{E|J>#)U?rQgM8ocvpt2+dZA}CP)van{<_`>^p zd3}v$=ia2wy^WrPS@6ShKWFyjZ7yC#C8gTk+0*`e0eS4S`11j7k|Il*My*eEU*^}m zIc#o|x%~06Rw3la;l#b~d`uvp(nRG`N%;DDy?M4@tjX9my3fOMK5R@BeYl4!p?2?H z`;Q(vDt_adz(?%NC#q|0qlnwNAYGcAL#$z`;|o|&UyE}8{O+-ij*j_9fu%2BzT8~& zJJ_yY?9mpAhB!}G=2leP+ca))fsoYrTwNTqqQ5n1cbYM%wM#m8z~z=WeVqr=$uoH< z;WL0;<7*%$ASE9JsUQ+TLIN_w8!CeGf~H+&p=cDXp5cCpap=keBTTu=!1h~DfBff8 zEgL7N9#YES$M|c-jSy%ROSK4YRDu3h!aZp&*j6cW`bECd%exq=hy-*?8dZnF`*BlU zny;H~%3KpM1->+Q6yzfD0!v09FMkH3#|7{@6t*S%|Ng*M0BJub3JKgl%;P(v^mVH~5w5!KeJVN2vu~n2F2Cv_Ji6r%>=hrSQ*<%J|To z^O*oig9A;Iz=kjwT(8|c+dNI578*9q68zex%rE$pJ+e}NT872EUqeXrk6_;ABRxlr zzsosNGzk1V{ePDX*7EO%`_9Ph0K3(L48}W>qD9ug!K5)OuEC;pxehpkIBo*(M6%s=|+W z7JgyW?CI#?L9Rmm$^r?p%A@V|qa#mhT_1Iv8rWxY=#BD@mbGM22O@h%Ov=H;9X6#B zWw(v-*W(S;ytE>YPr;6q2kBe!I6?=TI+u`zVs!p!dzOenqCk>GCftJ)AM1Ho&uO#d!VCWbyZn;5#c=75T^vyApbL4HvAJ*Hq3E3dn{#|3{p|Z;5{el z>o9nCz;?ZK(3dFn^4YJoIwQZ~ma=X(W-%%ZlSrlXs!wkFCn?%&WsCAhAnp}6U%9t2MHUS4&t=)XsF$FE)cI8JmiynFsR zpJ(TalJ7L&%3euk=F+E;^&-P7koGj(8>|5Dud=L>qz=%w<9?EU5m?m8t>$6#)9;Kt zt)F@9vt*&e=zp9zGr`Hkz2fa>I5CjOo)`Y|K{7w|77uCnP_)hEQq@Yh_w4`zeJ65N z1-E^?KXX-v!U8XUwtK1ZcvkZIjm4Tk`AGjf?hC>661D#Lf33gkmR0&t+9H-kmzvNgO>m0EqH!*6a6p{4F`n2tc0Q9{c$5 zb+4A;(9n?0V!iuH4*_o#bw;z77>8b`z@E&dT(b(^KN0XDC@QO11gyIz;ySnfOg`G| z)w%caOY`Q>HIvIV6OWbt?NNj?K;idn%FyTM=by^5_e@Ss3Xe$TU+BUl&VPNcu+Nh3 z!Bs0-E+*8cyiaGJ8v2`*EfoIglcrDLf0~dUZbO% z(7!DYxZb%&cg*HYz5)m%fbkXt_yi5iOY9z6AVOe>Eak~Vov`22Rr+N~p)I~-OL!^= zK082fv@-{@i}{@hBv)5g@7tqgpUr(MNWIi+ zhGEl*eE0Ntq(fGb@SPB>CkLI1g4X3WF?iDLn4r1Hx1s0Pom^;0Vc^T9xh@I!ulOwg zkUO%Lb?5mX-L4wT%MUG-NqZ@T@b!lro<5zqVMT7wb@ig927ZhjEt?yaCw;%@@Lz@%UR0$ARyn73r>IV@4_A;~8>4@eKzOA*xss z2Ad-WQH(7EB^O$k|5nn8?jcZ?aV8M*&*S4xm~t!fl7EjAs}$s%U4TCkRaEB4h3F}~ zav_Ze$eYc&<=18@Eh~y`Cm;~R8X6fH{OLEb9k}>uZVt{bANK9bbH*Oh+RXOB`nY38 z6}}wEH#DE0i>8_G7D#|Z&13gDG4>x+!`E87$HuhaLE&hZ<|lQQKAB&Imukj(xxns3EWIFcxM@5H9y=Df+cEI z6>#x1I_U>56to4M`A!jSJF4=HA`ax+j$;nuD{rT!?B8x{sCe*1QouC&VwtM5v$N8& zN>L#_Bjb2`_Ida!boC1Cb#^rdSc}7Fq5ednHJDK_!O0CeRgwEvHa2~=HR)k#lDm^) z#cDeQ`f!NrDZHTtKMFhfNcD7|!1X{e*Yf=kKqWS~s`_6+`IW@yY`eZH^j0TAwKVWY zBuLZ%fq+XQXsS?PDFw1eGUi3!-Up_F8mDoDC@|*$XuG2ii$D4*LMKNNW0CNBhtI)% zO;5g8iom39-HLwfh(9e@nGVZ7kOziikRt|zJ0AdJeXm4MB3CwJkJLAJ6z9jTEm^0= zjJaD#puffa1<)C{e;jL2pP@Ls^-+WcKsMA3Uh=tsG>N1I%~G^nS-F%#B#K0H$x-BX z=1BQ$P8mB8w3wTHl`d;^-@0Kol2G86(8~lEk$s764y5Tf;oy34g_4NDR>8THwvos}SW@bi{YE28a zumND3(Cbt-EEue*x%u_4LiLOS60Q*RwHxI6k@tfjsx(a90`mQ!bp_y(m!}i%iJ=A{ zpKq$D7y+sEbwrRE1&eZzfN3KG`^~CVW`P@&u=Dl&m#AD=S^9AF_VJuM5euhAX2bWl z&BUMU9A`oD&imUM1Os%`rdZ6eosrn(j7>>U=#A3!z}GdQx#^i3WOh~Wa2?LQns9UL zU~_fEkkm&Js^eWKvJPL`x2Ps?kWqzucQZceZX4QQsGypfTAyJmMtolir-U|ax}6}> zPd~CO$tq5vt;Heq$jrIYdNmUBy-=J2hPVKJV^9=5d8at~V>Q8=u)g*6HzL7e(}1;6 z&}5iyRi<|w;1<{E5->$<+F+5)A_naP1Vu$3myOL?CYvh;Y3VzuLlckNa7p_S446G` zKYLYGO)zi394eAD-7)z@jg4PGpw&ho9Ct2Ndv%H3;Z{9++CgerPF-oa{N~vs71E{f zL~Iv)>QpOsoJcn{yfozj!%6PicW7>-DXDMfWRuOULP2GVk;I1yzooSgQy`YPTfh{% zu!ozvQi0E;%>jQ}>bCsL@M##nWx0CN%_@mqUS3Etuh+r=5uxZ|(94t+&W!X2i3#z} zoK%4G7C6~ruuzSQ$z#Yp&3;*wXYZ6msRR0bj z`&Ro94=GtD9$5YCpx(>9{k-xf-x}7uYOyTT7z1F;67v2y$Vf-INrOG6rFc~(?%>z` zckW+|MOfY?o_Fkz(>Y*Dd+lT%s?h@&r&)S8j-IA+nmM>i#pazOa$i<|;d>=3wmsF> zXF6{j2G~xPCSW|3WAGaeaW!bVR0EaXF_q*L(UU+aiMek5s%A;zYXOz7r3IX4|4fYx z@Y(_{yP@)c#ng{dS+zmYL8|1eM5p>z&bxL(~TKIW70$MTctJx&}3;^<#E{lwsIPM>*<^^c^ zA3c>nqChHeIHDp;bTUmV14%rFtsLGCLTc0x+cIw^+Y|mXYi%;1sMOBjx1F7$3X&5e%Z2#APyY`wgi zuF`fE+fN(ObV~qL4NB26z=RwiLHoDq_{FVT*h;YjRSPy%w#|iagtRWi{B0+BJ<2!4Jl11-07tdQGBiBAe^0ww zHZZWgCmfR?wLq@BccwrBgmMBKBo26xtt?ann^r&tO6wvIiaN9kLQ8i772Y+THy>c2 zK8wNT#>a(ZWcXz!qYB_kN=j?rEZA*js~Q`{6+i~d*1n+eyBY*8bbxC*=mylZ*4OLp zGhixmyyyt4!l<-H@vv&+$Wo*}AtB*=CK5r|+}K!9xFxNkq9W69cuj3>mXr@E0Me+- z5I3txgHf-rOkz+~0suo*!KogDwcc1B{S%>TW1R^3SHrm9KlX)p!mb=mP0`{$e>V8> zxeNVExeBSO;1Z|}!GjtHajUyZe*AEq{hY8l&XL&)nz|G;wcRvHzY*$S4%&Zun=41% zLW78~jJ3np_w>e>KL0{wKlBpxAj7-A5A)Mx?ZPc~;%FyGs|8_xAQkVgA<$2bCu-@9 zzK^9?AIFM|KPYn38-pu~^&$DJlt1M4$IqRDrtaEMF~NQg3+UrT^Y6qd>bF%K3LPXh z(mn5N+4X0qrNuc<3>emMsE{KG6C*h?$sSwZt)9*{U$ClRiHK3FYLSJ;giPb$>)+QR z3{K+F14%v=7Qqw?BS>byrpD$g;pXJAVOk)O4@|3TN^>ytk;?B8+i{k}y$Hcez#cHY zvmUwDSOrfZGDn76+_rpZxvOH746AZi=mAX>3WBx|3Uobb9Y}p)BwN}dqT=FQx~9l? zo$RN@_x$a^*$cif@86J(K1Kh<(%@K{Bt5jJpD!#<47SCtR_#_B?4HebKnSb|;%v^2 zfqEqnV+JXNnPgrZb{pH--g6mvO2^S1CaGJ|x~48J#HtOJUz|Iwj?goJ$_UJmtgNhG z(}Egg1tP4zU$R9N+<)tUOe(wM^gQQR^KKiDxXqNt`><|lvoyM!P6}NjJYIEZv~pg3 za!S`Z>UCeIHj>%XcDViA{21OBTjw20}T%ki8tjV{b8sW=yfSK|3 z^=;;mXSJf2O|_zG<>IcR+D(Rs`*$$nTrBwAK;>3|L$(c9wIR*8x!`?Uos({UATx1N#@J|0Xq`#d*?4_yGb3mndfWGg#d?v5V$dGoZ(@?Y z+F7MU(Z%y$<8>jr$=Sh?7`J?+!2K%SvVGdh)ttBYpLc+Kx}f&%dLN49#*vTiArchE z=5QE)u*8||ClrMaS`Ym}U7h=_VXYlFDzjfTSLaBVoHQC2CnRD@8cM64{Qvis>Ghe0 zx_T15o>)I^H3AyN?q{W<)0un-~N<*+_e(?~1u%dN2rD&elkKdT(>v-zKjfBdI<5zSu68c| z+sg|9h$bQ`T3cId`|INP;lqc(M~hZbzwtfUGgFX|kU%wGrFG=2E-mG7$RqfytgS`m z;;n6Llr0JbbI`~F*CCt)7}W2%gy78jgP<{u9aMOZ+h4GL}H)Ggw{ec!;L zEEerg*n53_;rHWe<3EEox`_#kloU!$u>xcQ_ZPBk#SnBgWq)EwvRDD(xIM-RsCo4X zD$~TNC6LFKt%_uWMjoT)P_cJ)SS^6Yee`K^a(4E6>9YPa67ynPd1dunTM}(X2MA14 zNtt1i+%P;`tYYtO^koNG5_){15a}^X%oae@%QlVG)jBM`M_Y0jclpJEmCFiZ3oI`~ zR24jCJc2Us3f;qwAizu|md7rJlAFq7GZzqQB5;^PVcTl#@tBwj1Q8+P*LtEI>=arS2nd z)G){%Uj`hqI);-*RfMh+9 zr~G{dopVd>x7_7-pdvCq;{rxjw~y~x0u^$J<4_D>1honyvd_MPM)S#Xf?mnpmV8m2 zZ{Qq&9r)+u6man2rYMgH!2e?2dpR8F24E+HBFDC}H=x;f!C4pl{kP3KF9iD>-uWC@ zW%+lj$OA4Vz@gfnzC)csI2iSHl%T-69pRtGA`H2aHa=ZQ8D_^z^5f&!*k0rY4C*Ay@01{)58Iey#^Kt{z zHL-U;A^W_bvDnlQ`#R&#^qi-8*O&{;{*IpD52;HUG6T-7r$TfSb3=&xvBG4cLKUcx zB^S0(78Vh9SV&I0-kFO)y*nlcj z5ApBYxP5`#KB7MrG+g6B7FJfEsR~MxVPIF1D2L34L(0X8`dULoOq2SAkrXgP;ZEZk zzX45zit=@2LGn7yo6%KyM37I*rWEw36?2=ybZiXlG|3s74a%CvFIzmw;2(&!oqAIz zt_-yNZnU<(vek(-i&jaq33OOXxu7g%T!0vohFfbWw=8(du;Jx-&Y+w|d0g`Gd7;fcl7^b(A6XAQB z^Apz(ygXGsl*^;42@&4z?!qj9b_0iL!l{F{c6OZX?RhA;dnF|$NjkW>tX8lEsA*{8 znhIGb%6O3rwcYut5ASR~L#^5l{k%vr@DRx=dFmkMWvPzHZOh9K3krfL7v5x?e!4+^ zvg6f`$4t&E5|6J&QI?*Sm82G;hhUyY@zyy~Ggtd5?dJ#CS4QbiFs2P2O8s4Q0TZeu z6FL%hpgM&?9bQQT;xpH&i=t?0vF${mdfyKUQ%k$tLXPUX*RSumMJcoH@F2B^OcNi1 zE`68qsv2RQa$f4cQqU$|QYUhBb=#+|eA1Lfpa{cEwFJujM~__CF0ar1FmVN#JAldX zjtOh#=Au2v2kIsImgdT)>1AnY>Efa}q3iN>h^7(CslD#O!@skfe1y0<{~nmV$k{~{ zu>BAywCMtqJp{c4APMaLbWwXYadGho)WK4zciZdhvG@FTKUZV*10KPrO;TFgnC~(` zPmd}dAfd!sz?~f#-uCi32Cem++kHCLK5w@~uD4bgHIk+&OIXlRQzPGSi>@^@7jhMU zr&t1vXuiIQ3FC57oyL^8UQXOyZoc`8_N(119+hSTYsRqu7^XkwNp;{sK|(?=NJ)8$PZ8_fEl$fz~t`Md!qY$LjocFM<_S!Wza?fG#@ph z28=eXgv}=^@<8wY_4M>rBG2ee+5T9MF6c6+Jw(XyH4OtpG&52or0y$oan=5G*E0f} zyOB?CV5Bw?n||O1{dXl*``?s;>xVtCMCzmPdpw@UYg7-54HCc!|L#nBoV|*`vcqFK znDutvmzxjfnwKw~#nalppbowyWDrTDL2~!dTY!s%cL1g_rz5W6z9=e73*lEed}AnE zBZY5hCZ|v;M^|!3=Sk3?6%Y)UG}V3?wF}7MQc7dHKk$;=<$;^q&gWB$i1U30uL0Zd zze`)a5A0X{&iSvUh>3WAyp47F^Xh=63w89y`3Y{N`Xph=Vx-lbLYk>hDidy0Of_ZXq(MpCOvU@2Y@~GTko4cu-0BN?=JYT-%aU z;DK=(7w%zs^ZLiQM49~`F$B-54jt{Rta|viL>_O>pwE5$xHfMIAB!oL*1Qzp90*5K zWhG=mE<9phaitYmA>rWQC`hFORF<+7%7yQeapOs$-##lSzxxp=c^fTy-Y&)BECp`h+=|8!1oURH;C8bMWt(37nCoS--#glA!4 zp#neqgn|Nsp|y(w?N`!5YqNZb^4YL=r2?c!wLiaq~FSUSz=e5{xc2@6$TSL+Qiq?n3^gx>;`1r}=#rn6z z(oJ70avK;8FRW2A-iIY)3mBfDPGnvE{AQx$w(&eb377SJit&nb_54?Fz{qMS&VP>w zqKU^?&ngynB^O6p3|dCJy-?uFAtybPRvh=U;!*pbVICm-i;WR0nAZg-?^nt!YaIY2h4EL4T$N}=lpuJKejCaD zq+j)SsDxlNy{9*fQo*Hhzv+ksv;JZiQQ|pc6yRekwR-n_t{KxwrCQ z!}l2t`b&Mg8{A3d7@8+g$Ex~u^Vou>FSe3<(&C&|H zsMY6bX*AJs^zTsZKXJ1F7KK?H@66M(iU*-*n!NH}#)&2Y);AuGVs0{Dv5Fb^gg&Ud z4!&ssGfTMH>joKFIYn=JuUFU!WTPPNwyGQ$coh;F8@{#Bnvq8b1;L8#`zVy-8F2F0 z{Sv^#0H_lhTb2-t=H=x@L`Z1IBJmHjPiJDI1cs+PRm_G|SGU^$9iw@LeZcG9u{1Y- z?^W)j5#h}axiKRJ4rqenTDUE)vXnkTOEHYuH3G%VwA9ue*+wG)BxEtn-*GWtgy6njO9Nu`1$Iq zTfVCdJU%&iTQ7jv!o55_S%d@xm}WS-4Ie&aH`gitjA|@wv-&{G^)+hBfM-0MU6JKp z%46ky_-C#;aNN}rP;ydIJxm{1t)pmM^J_<+aE18cYYS~p4fU{S0H}vjUK={xnMBoy zO}*v;VjLuTsVNyuS^$Y@-u)qT=e1f_ak!sP&>(q6Mn;Ij=UX%P?HZy(rsjop|YgbiWFjeIHS9zFol-7-o+f ze$POKtCfO;gh>Ad#Pu2b4lx`ld#-9-L(5Ykic1SeqCMLllSeonQ%zeaLh#iV))mL= zMOE&G768CY9RGwHr}7Q9vZ2h9LLOCw>PuMMk$}y!5+%IWPhpf{3U0W+FB%WTiHs!& z+gkn|h}7f&HZ!v|Z!NZ)1h{&F(-rA=l(kSl3K58;ZmQw}m`xn`>+B39WJNggw68L5 zI=7uHM}n*+okJI>c=ZN26X$%Tg~i;Zs+GhFQiIUB;Ccxh93p2DUDVBMaZ1<5sF~U5 z{p4C{!LQd-_#TZ@H!_@Adj$Dij#3@)G;#ptX>K%=+1#}ti8Hgw!;k+W6V9xRcTj(I zS0-WrBtxQ#d0?zijIq`|`y<%Fia6`k$mHPj7%0Z{DKV=6$+wWWc-HotOv;52aWJ*7 z2hr5b=K{@dCvlzBS>mC@$4-Q9*Swb?Y^SxWcco62LTFjXa={{lZNMLZ7-5y9j-X(2 z+y89Ve~4R&Jq!Ni$re9_g>Aizpdhn0XSqtD7uuU}#aRAZdHxfRPNoXHRv>3igc#F( zQa_%7tN^7-Viv8Vj4#Tx1POi>QR3(hHN>e6_k*tN6yE0NcQy=0;tELIp}HPH|Ihtr zlrt%pC3M{cZ%O@`~poHHMCwQv3U z_34Q!cZm5vQg|l@g+fd2cdt!XSv1uGrBV`Z&mITp=mgxRc76SOTCv?)(RZJL7R_i~ zrAbdaZ;sb!c>knXldq` zaFj49N`A~#d45#aNSmCE>iU1;=24m_j4?B_PiM8$l7ODc0UV}p%OWAUp_EDgaloZ9 z+2if5eNfj2o67f;P3HaUmG`lE$+8B!Ya!mQH}d z0?BcF$O)>YRq(yIl$68fK=R!}tz-YQBb6jT+<{}~>Ov%_6YDJGOMAI>=uxcv?H~jQ zp@#RvoE}cBfSd(fE*#IcATJS>{fY5MUSaAPY5RyQwZ7H0!ITt!6FzgCg@ChdkUBG& z<5k94GwE)rJw#O96;jUt&kRC$K*tmVyP@Fd1h;ZmZ$CPnw$%e;`keS$3H7HNUg-E; z`4mwi@{;Vm_k(z%*rW^-+d(POl9$+kfFD`N&`)%`Mz}a%zY%wMO32LkfQ@%Q8iga_JC&hHu9F;@`tWOXtu% zZ7wt?)*&~Avt%JRg3ygjP{BFI?tX4@1RDQ^Uj16l+-DL0Jo2>jS0(!=SEITtNU{*{ z*D*fPV=%=r(B1u^ewADN|FQPgQB`ei_~_o7?rtPENJ|VpoB=cRY1Z3RHRElP>?VXLA~z+&v(ujzcGIIjyuK~=MP+a&ADd0^Nr_u=JqiK znsa6cTYi=1cm_Fq;8{Fy>dsD6{*vSZ4F|(?Qe|ZY)%sRt<+sX8ckgN@eLC~Qd2h%l z{i91z`?cWk`~I-4iUI=Yg(OdENR8D>aD?aSzdT`bTfZgYSV z?o~Of46hvCjpPl;)Wm%6Okrc73)`?A?UD(<=mh^+U*8uyEcBPAfvv`P z8Sk#4iFe@|H zFH97`T)iXEpIr`0`?>ervbJmT)!G5@TrOt8FB7nASh8Rf>RGSA@>wXfhtE7F`TT<= zuzYxUm|b8)YzPvw3O>Vp)gwjv4p?y$CUnSQXNpIdR8;0v_-b{zCVjat_!tu$=td&? z-(wUbY}45lbyFB6Av;@<1qUwh);}7QQAxiTVC3N7h$(+``mt#sahY8v69a{ffGD1i z!A~no#EiQ5nSoTBxgKLwfn<%SC;wptUvj#h@p3~q1Bv{gh`y%|j``etuGKSXebQFG zNnuZ%biv8uef2)JPn^pyt$371sInwE$1m*j^r>vKGFsKfMK^WU^SycQ%dp&*6N^&e zhK##ijII!5z?h#=J)?JhjS-mp<|IPV_|b=|b`G^fyH8Jr^nJ!`inl7`i^4J^zYvFpbdiqkywY%CN#uP8-=oozW3QjB`b zHW_Q#N|{eANq52&m+6eKt z!>9*XQ%S1Hf+>mXxZ{P2{7SjR?9Y{Xq%|G%^9^EN6DvB>FLQ^rk#182RAZbT%l_LA zyo|aI;fyd0H5JJf#Lz+pYub(<(pY=ufBik2pjl)uRpoE?pTPND=$8;9Y;+IR_Lvp7 z^OvJE_Uk*1e2GI2i0OoB#w6Mo5|6h&zqcr(q-iv#yDY7koO>&?5)~J4T=;P{FTbZ3 zr^Nf=RX4T9dB0eb<(NPYn^TJpB2~xyOe;-|Rjti&aMD}djUnSLM`f>pp^-#7H1MK4 z-DO_v^*rmBJQB~shA$SFjqBo6{$uLy-e#{B)yL(+^Va=+HOvsmP$1#e z4Q&=qN|4IQ*CJ7T2O$(567F6}V693}QQTx!Tr#M_(-`}(jt z-1p@R@CHkxZQQTT5wj;pdQ2T%5q@&w&*Z3telpJ!6S1mstakhil#DMQ#HIYAgU+5` zP0)CGn|E}KG%F-FT!Yeef5he?(^eaCP+61JQ$7}COoTXRz^zy1HmX&)T*RCI2vmEU1^4a@_p0;=)RkOB5Wp#na8W*utJEY-<-80GD z!R0hQXP#3E!?qCCx7nK{Vsj@W@V+=Rmg6`LIW0 z5K4cr5cdE;1X^`0;+Bej&vngo^;KeX=;SnNl>_$%Wy)RM|L4XC(1Ia5dswbNzd zHc7X4cB1U;3WOyFr-n&%$gM2s+WGFvh~hvH}&goD;&w^1Op*DL_`rwFLZ{^ z!fo(e>;8Pn0HO%ZN*dXsw_fx*X6~p;!R&WqDL+|XLKUxYR9fs_%-y>7?Q%wD=Znth zOH}*NdiD;UE++$x(9ItnA3nOX_5>fdluf=-lzf+xj9W?@-5>2`;fvWx!x!o#SR|bkoQX`je1xT4RWsOG z3@M&y=zaM##)%&kt>W2S*3eZal%MI8zX_Ro(d+lx+mG6i%=sQX@F5_C-Uu&+y=`5G z3DETK^(SUOO*1!^!?^+T?{AyMGmt3izQ*FLrriK+F1=9<^@)K(nHWf}p3a9->f&7DVNdH$XqckZXr3+SsNmrf~YtbaYU9O+v(K-;Tp%vb~X;x0HiX_=kzka24! zd++fk=7JUXdCV7Ksrw1XhcXwAjp?}xm3o+q;^mjw?c z$GR+Sr{6FePG#kza0?Tr(DXLa+cwG@8k;f;w*73Gg;g~TN&i`TmzjWq;%je(KHo8C zyQ7=UFZBYyeA_gy8^YGTKUF5ChCXCyY^)xA;q&{K-Us+OD&1DFZ@Wdgrf<{e-8(*y zN~V&`!l;(_ZFbW5X(bY0s$|DK;JZGkl9@Y6mi5*GD@Ef}Zk>gq~LN>;|2T*Cp_|NM-PW?WS}R_X*U1@|*q5Wlg3 zm9800_RVWj7Za^8V>o@jqa;Ne7bE$ew*9$_=m!@^RGp4@%>`M#82!?Dxl<1XlP&J9 zpz;`HboCYsTSvX}FZ6SU-?KZY=f}d-9JXFc-PL+Sq9#FKE9}lNrAR0ab==6Pq&z%r zDEyWs{=^81?t6cU2FE}FFx(&(%`G}^iuv4^k&~rWf_l$W^H_*jSzU_pVqfC@w4I;N zN?-feTeJ&O^4FjLCf~5>N+S5oLZH-9hv)pxx>IKF;jOvs2iM8hk(@z?PH=*`_9inw z4=lpW?>D+NXMjC-VB(<7G!&%s^YaITdEh*=s5hD%@7TB+`6;WMe61|wX!5h|)rU^9kA{b>Z7d2Vqdps9 z0UW(wPbY2wqK?b*0g)T++n+ST?Q)Cmpd)};${|~EIc)K&KnIfMVB}@V6EWYwfwV(b zjw~u*P6KzQ;nFkkqd7|C58N&CtUcoFY;fOMS?eoclk8+k=C1T%QkBXDKWY85kB`=I zC0USY(82Pf8;>|E_Ad5)>RvKerS;l6#7;C}79WPBvS!N!m`GM77bWc)_1U+ zS-oi|X*>CZw1@bGqz9j=n5{1~cz?8VZJ2O-D>rc*Gds}Fyld#dLE<2>%NP2*)oU~pz|nEK!d3E}g0&9`*^+0@1F z4WysnYoBm=?lpMaev48%Q4qCx~GDUGt45DjHHZY z>Z;U#^x&TZ5gi?U^I%5?n`4;TUrl+1e;YZ?i*5S+?BL7AN&a%=jsF*rkpIAoXikHy z1>kSH`}_OB9|4`GlIHl#&MQSl7v2y;0EwmyGRMv3b>RJ;Ko}B8DTJ86T$!(Hdvo0Yt)rrv-t1JhVWG3bo^J<3{3 zcW{lf=_I@}c~?m>J`+3A;C;DER}lQ9Uts3EQt9!Bqk8Jl;H;hBs~o7~-y>QD?4Io4 zAYFq5yk{y_!@$4*sF=c&=gXbD|CRO(Gt8`e||y3IXMka;Jba-gxXG2iO+O2Z)#x$rm%a)bCB z(ikkHd{e$d#`75qE-0BBj?2^?Y0l#}%XbeV8Q4C&-%*Mre1sG7X9Y*eYu9}{Pw(UA z?zjn4ap^+ySOqfGTm+FY0%-Hs4M&_cX+cq{JAI!D_NFoW-d&SU7jtZQZR11Xqw9KPd+W(1@28KFje=M^!Mq>RbPo&}N*I{iT+#3?V zZdA>F4G>4~lfQEa-^eKS0WB!-MkpIgxpHR!8ImvoU9_E%7P%H~CN=q0_%s3S#wl}% z1Qq4&CLSl{`-E|Xntl%~MKfa?8MYMhH z?s)9csPDBVYR(T=QX3AnhbGNSdenPPp1s7aImcSxx|W*AOddwX`IflS8_!+ef&jjv zrju})UrHwAIUyt>I!+v4PWP|=@nNGqLel_G@tweg=3mli@s@*CNp_W-T#xr(ZL8}3 z_}3s{)CVtyy!C+SiS951J^twb*CsCJ7m@=g=@&5TzI^#o+)%gyyPukdX6f7OM3@a^ zhkDp-^KF&ix8_TjZ?(OX-bySz3h)XvsRdYK*p^p>yfp$H{JjuVY%3ViKYfN$sd z``6D=5mCz3*3ZU9v~;RyU*vNxdKnV~`xnq15rJa#h=N-Xl`Gm$k4r#n?)0(kk!R0R zjYI532)pAzzpaUyYRdc|-U{`L2Sp5sacZD2wV$RexaOEnMsk05W?in4JD%G*b%cwB z`TORBsuD!U%u-m3?CHZ75dag~d()~WYSZeaM&~K$)q{o-(AVzoEk`Rxe7Rv974-K* zYdfD`e$=~hA9ni3p#)cu@&Jq(?8xKqk18`y@6KRisKDZ~4;;^{a5O_Uzk@eokzb^6?jo@m<6V7uOe+{u zYSURTMz%tfa6+s_rIa=5QEp!J$H>HP#M-iAp&TR075ktK+`XE?* z_J&CBnMgvSF3U7>aA|6?RWJ;gw=fVr(8xv5zOSfMlOXwtBOiD1H0$6BJO|0q+ijpi zFKd+XXg<8INiA2xNW{Lkn-C{t zTD)qx|2FZ8HaCbO)ahrYpuV_t5%Fy;lEivNMRb4^a7OnfNjv|dI*+X{|LZ%-p|F}( ze@f3@&!fEOHkObnrx|rCl7t=BnngLKz@RRw-tVL72QV+<{mpBPvtgz zviExG-)kxJ6uN6lv-9qt z$|Z_z{9x(r>vL+qCkPfyrh4ZH{#oVHJg9@nQtO-mCXy00tK>g>sd0(q{wL(U>D%Rl z*M=tnLL5?AgCY_fvmPB?GT)?>miALah^v8x+SCgfxvS0Zd0KE^d?!zSqZAwRy8Wzl z{*~t68>ARTR&2+WPCBx`L;FAfN=xZaRQjJQ95yogzs5ff@Y*ZtUy9efdw)k^>1Y%rg`uBKT<&R$t{vwf~w%15eJaL$diwkH8yrfnY1@yf2Ic7XJxF1mF-uC+_@k_-rVcIrOc+EBJ0HpN$g@tF) zuJ1}lOA{3@(K}C#^|fPC9{|v)QczAj;8pwT=qz&9?+wnMc{L8iNPwI|HJlzl-U2Sd z4FHj|r0e!)+BDJR(gLQ=oyjZ$4;ryVRT%Uo_kB?$)^J6``Ssb(YP%)@NOnBhg)(;h zWzn>8zxysd{l57q99eS|6tx2;ak|9{yx(DVu3d9#36T_gY;q?F)n~LU?>!U+h@Z^} zLc4bgKv2H_ncF&&?=?IV@8;%2&yVI|XRrOrxFO0oJTY+w`>Ol86>cT; zz9F|{q5}2FL$RFNxRT1i8m_d0sGI=e;m|Ydh*Pm=@&vDZ)__h)j-o{5j5 z`Ktqh+5GE66ZaE#wx{jO&hi$`5$|Fw*JJjK-YkXm@1u%u|7&+J9LJ?F1POA{{g;^( zA*|pfK=;#`-n1{HhIpZeG)VH6Z6W?dSR3Q7|+&RZz>~D9rYN9gl!dX1j z2P`xOjQ;>PIRU)G&6lWN97(F-DHe^jG$&u`SiaT@)qZ)N= zt=oA2{*0O9p}%yS9u~t&gdM~RncShtNm`QN_;P$0VLM)*<1&M41mp``$!qy&@+Wn) zwTtr^64hvcXzXB)1uxh9~Ly;_9TNa1&HR`o!G{1>a&``BWG=G9%9Z z-lF${d?)0TJ^Obj01U*Zaiz2ig8b-9bP`m$qrSXu&%}8i+}U}eqswV^Tu!u+3@)>o zQ0aKdQD7o9Rf#!7Y?SvP-(sQSvg6?J<5fwo$9^9TLkVGH&LCydEi+Mq4YHa=J&#aO z0-XF`)_Kkrw;CPT(NkrCGf(?lSe{JL+{BC%+6%HM(L5RDX2h6h{P21Mas{n3LcI6J0jf3^2_gDl#?>3MoG7$otJpw40dlG{%>KmI zns1wwNru&sbKMyg{MtIG`~a<$p1Pt@lnHD{t?vw3Z?BrPA-DjO!&m_V`!lGe}a1c-(~3CRZEy>WPYn(A6hJr^2D; zL1(K?>|8xUhuVT$kun|P%R{ZYN%Ir$w|ZC}c0LK%0pn357~AtzOr+4?qA3ibT(bSJ zhgwl%0cJSNGI$YS(1y2S1?8&Lj%tOkjLIjuX3rm$K4cSRRv%ZfSSm_X^^L_qkRY-y#X@Fyx>$x+daqG9?Mzqu84NB?QoUB9!=l1x9Kenkz zN->#kzp7R0gJD^sX>#>L%g5@pRLYdLm@D{>2#^)nAF#&cX-l1|2GFlj#vwdMtvJ>t zqRxW*34Q1Y6l(w4bRR?jW(+WVt-X`B|H`#e=G2d=U}Ak@1+Oi*%je5;kq>A+9Bf&= zlPNaXeuRfy+ACaV_HPG?jAU40@f#a6%sv)8=u7xx)}(rg$->FoqWVefg+`Z*q`3qb z0&21}olH$Lxjn~6Sm zKf&Q2*CCi;Ix+;bq0gQ=_tW*^>5Z~qZT)a@#QIe0_A6ueuWEbN@ZY^wSP6&DzfD<7 zdiX=w&3dDZ(qH|5K9@y;*7k4buvzNkI)DG?`BLobA6JWxu|7YvOZa-#*bq zn@;Y`)%H+u{Q16r?;Qt&vjJz#MlTUT|J$zuTSx z{m%%+09TTDXk%ZmGrIBoLp58~4p!&SpY=D)3`$!}qMa!Itbsov@E;FiAD{Ii}&r$&`ht?vUpD<$LG35>Hqn~U@VmzAI^!G`RkhF z&d?3-44TxU3UDs%M=UA-4^8fh0rfI-dkGO$nr*Xzk3Frv{*W6qBBB7*55hH`P?!#xlp$!jotGfuAckhS#3%6YnK3U_i#e+k*QZJq5_bzdb2y|P}Ch% zg<}#jV2h9df|>v%fZhSV()J$f^qk|;!2u;oy7R#oeUCG2t!JFS{rA^ELI_lvc0u#U z&;Kx+80}Y;ZwQne-ulwC??|83cf-}+kF`>L4H=doZIpye@Lz2KLjB(Gd&}B0V~5~Z zuXnl-HSpD)(O*bNSTFhRB)Hc{UjNjr%b@phkUv(agWel&z;Fy%Yf_$PQ|?+~c`F3v zaa*`V@cPvV{nJNSnWuMWd0=X)rfzX(ZvU}?dnPkskU0w$6vT|&SxNK2tF?bFT3w8K zdJ3quoPtdNilOPaRnva6Zjmqp89B=D`5&7e{ya1B*c$k^?;74ZzG~E+xiPrO@@;jh zMkY`N87h$dcW?~|2bw1o7%8W}fdG(Q#<5>v@^s&PkYsYU#Hub^0%Uc{Ko#8b;?v>b zD_y&g4A%h2Af{y%61c83FYF3%LquC%H{2H4)TL`&jC#*$>;wVwpg)iYv|O=tYt&br z;5IB@qy~W0G&s|}96JS;G|(@kqJE08Q%ZYkhOCTM$l$|82uc_Bu>Zwt%b9EfBs3BAMNXP z^Mk~|=xFcTblu$lcn&J^0%FJ|hf31YYp2hDJDvaLbvZwo=w+cXCXUBWL( z1D#-GM06c$0-hu0#COG1aRZNu=nmuXP_WQ4^`ZcIF!u6#(0k0__Uv%OC%{%B?~wyQ zTd9z~sJ^fgBp>E;g?j3PX&DwvO=EXXAh@$@KUUMxA)7^_mS-a0 z`Zef!Y*ShHq$NRy+n@iBi5xVlm54GR%|EEPhl*rVbcSuZ#h2 zQ&Fi4wIl$*8(q0vs~H+c!p>*btFdEPDJ9~m$*dVo(f7JDv)&%YJXGHUyIz@0S0C+i4T?1NL*E?~}~<2Y1PQ`3_-A6Hk|zUG6P`Q7H`k?-M%w)qI8VzB(i-C1<EFP&oZ7^KDm?jrrw=JUqO-mtDSj6g5CBd;=pH1$pMoX3)6c z=mIsR+96SiG6z^wvIZ-c$oLp?a_$F&m<-MtR8Hi75jds$V|St|p>Aayfkd>0iK^HL#u{ReeXTlv@|iBA!je%c4pyTgdsWLy;MA1z zIz#aZpCA-S-Ov7x4-;@)OD9cVufuMDyq1|rAk$i0k8H~Qxk4>~Xfr)O^ul+V4tSKT zI|;e4v7YJ_Y?ul?(SK5zqV6}o&dpGKi%2>NH^ZY zO$Z$#7D2*($}wU|XlnZ5i;0StCUZPZo!o~(3tEqSY-_uaY*N^J1nl3ZiFG#z|0yVz39 z;>iGKP5vAN!mvJpYx6YQARV-1g7|ML&2B}Us+iIYyfNd^kb7YYC$I%>3T*t82w2d2 zKls;z28RECF5G|pspar}TqT7lJu*xoBM;9XdmW09N`%W*_bk>YDN!-_w^(*g&JU}r zQ?21=w~mVN(~zbAW^;;?ncLLXMHpz`rGcIMIejbe2tP6Bv(xAX=uBcCXg0`elQ$QE zrGk~G#Z;~gkB)aCw}i>4C-TBIbrLrN5h_fkUH_&KmKsbM7><0t0N945do|m;?8q&o z>`8!MqerSYoPzQ3(*Wh;6hu5B(^+>8BYJgpLQT9Sa91)Jb<(*9*gd7cKXtDIsjye< zNdU9VWP~&jryvj9oC7)8w;4l2mRl}yN#PIM$1@sg`<(OODiyY|zv$NB}!h(;}t4gpYVO@zS`2dg3 zhMRsaa|;r5IYo0VN{8@q_Ya~NoXy$ZXfiUiv`AOyO{S48woEcI|&R17i!f-%~2&nA{KF{#E*;qxrvmENIjmGU*HZQpdyv`m2RGMR$Irdn}K zBQ{4reV|((L(P(dSDowEEpAq!(WKI#tTktjk(;>S^ZX0#gqEI3!0G~@FY;8F> zqx5*$y^d1wiz4Uv{||qPXLBBgg6QG6jjeg$HJU{=7GF) z`>!7#f!_n^M&$Oz=YsL&2;`D6g>ntta*KxK&hOv9LwVEJO;Yje!}Hc5_dfUD@jK_x z6rULntKAfH+A(blJ!SRvX5y~bwsM=~BJHP0xIXTj##$DA(%s_s>=V>@AmXd*6JPV4 z40}hzaw`_|+fktblnD~f^8Wt5ZDgcGi#-WjEQefi1t@oxr*7WgnI=2dUtP^fSqq*} zEYgy|bp1Byf8R1>xr_+23|Z2U3`4{Ylzj?O2LI7cvD~UTz$^?|2X}brG&RE8NkP2< zJRI4l$o11V1x-~N7^eX-q%iRU{vSNyqAG9V$dtl>5Q#`9Mi;4L+Rkjz9Q+-$pJezL z@^#sq2Ne{o@cU8#g7D!1JtT#x5ng3xOiT>OPZ@=gPoEQ;`WEy zi$&}cON<3(Q*n^AD^-jY4exKjV zr;E5J#E>C2brnMZw~4*#X4zR?h%=TY=I!|hkEHbwUM09)@l+eAF7A{6{CD`ViFD-~$S~q@z(m|5os~atn z1XyDDr0h-GQ6$z)hlqihfexsRVvT{CC-hX? z(;scjJ26Uc&Nw|pH*i)5oVuj?&npJ?0(Y(vTb59x9zxMQRu5jOV+mz z8cBv8`g-oykS*ZG8NPhID0|JzEB`BV?UTI9O10zexj+~RMK)qF3gSq$3q6lFM(#wf?tYvtnV3eb;>n@ zD{{xOR&f?%40XyUPP8`1J0T*G1NNs;xw+&7F%KnG;d+i#9+6X=S*!!OcZ764jKqv-kabJoFXaYYSvM7zVBEiiZ-^(H<&oiyF zEaMCZ)Xhj$`O;9X)q*`;be9aP#^4(2M}A%l-R*V{4(O-i?l4?#$3!_%j4(UL4|W}r zpAoapx&P(qDpjfXVdtrab!7VG9-xjJ8_)7NHV0NH*@OZZpX{`$UMb3wA4G?_^>jO- z4zkBw{zwn<^LTuSmIwbVy|gAoynJ1{`gPz(Bn_K;v5F<$UuP~FQ1;g1tn1L7d+7zV zc;8#5o-Fl?{=1%EZoY%+S`y2?c86c%#3VV9&hds_xM_X1E>V(6g=u8v>144p!zb zUC@?`&5;QdSITrNYw5QYyhX0sd*zZ*9KCb4C?>T#@3-Ymfnpn*kMR;zwV$!<@3ST^ zBwBet9}PoRHm5Ca{&mK^Z+QnR#81zuk|QOsM_Uf%x|F(lqI}91vs*OFaZRN+*odv= zC`dYbK7UP(E+_&)KSei+G{v$D-$%T&C$)Z|12U+Qyxph!l9eFaU zxV{SC8FFdWJbnC$s*#P}&6#IvJ*{OdZq=U6cn=Cw0H`NZHOo%k+x&rg-YeZtYkR2r zq%_EH<5Qn{R-3XgJwoYM^f@1V?kL|@FyRO=aH<=>?p^yyU8K)wK5CF+nuZ!v4V{1T z0xIT#xUJNC;TNe?HH&!l8QpS|Tq)3pjQT*UrrO~Gd51zWAqB!tp95=_kSGE;m!m*!%g15pOdtyH63uji7 z(>u3ZIjAF?M2_cA+D{lJTGVAag@}kfU>_v9bRYL1)6lN~r37H$uej%{%IwPz5Oh^B z9`%$KDzQ8*^Ub5@yUm|u?n58C?fGD3sb<{msQtP5o7IJa8D}&j+Wve+5c-g&vdz;c z;(m2iZ#?)fHEy$06k_BWRpqO-N(X}ttHh7?D*TuOO3)o6~~s-){>ecH8lM0HHDe}nZ) z0u7fN3&c3@dqXAw9i#!?*Bygm6#VygAH={paNMQDz;YKMH2E`&Tm9RKjKf4x>C^?X zh!BD=0{@GGviAS-!T0r%fXxBMRgxKQ-|n0HnDnPS8vKWb6ylku^YZdMw&UZ$S=Obf zK?#znCe)b&8}q?DKY1-054Ra4e)BOxhehEV_24avU?S; zUx7R0d%{mc3~qfBXq89o)p7e=#4SImdttbX?=OgRFYG7Jay^>*X0-ev4Rtxza^lCi z=jwHKPU#NcPs~?mOviuMwPdFYgXFHz!NmIGx=+Bf9Pl@m>37zx>8R5OQfhZsLP>p% zL}#zVjXuf6G)woUuO#9f^V^m{liATfmYKM9puR^Yz|5q*@YK+cgteDA{wHuzB1Djk zX}DJhNQPr}hfgft+sLTG>6LxIgJ3rK!l7Ux^rhqGFKpWH#@q4wI-uU99p_$X`mx2x zsz=VFj0Hp-I9fo6oe>#YdBkcN14x@i~9c<5}3uaDLAM-OW zxnzZlb9y{#{-Vz5vF(>MZ%XO^?2zRh49OKNG2K2!ntxvkhK#WnX_H9*-IWxkc%*Sr zGY)YG|MokxLY-(9ICmI#FhdWBVyT+81Bd6L5V({8^(<>z0$`9p(^ z2k#xAbHUykAT@A7$hjHhAMlXm`SaF|1B9`bW52tt+7qi6e~OjQF2sz}v zK|y?c9*aH`=-1zU%k)OK2Y!h|B9wpj?ydyEwMJs<_ZO#=@yD0L=71PGCsYQf^~Zki zbe%V{>>xgI)@LiQ`^@hI8phi5#1oY2UXYU46r6U8w*VS5KqQDOqUTN8BQEs3d$QSmz@u$DEs|w>S&$ktCYxZ*u{@bjF2||^TEN)(=)%@IUf}8=dF93%%6F^`s>rT zny6Et#zUr|d+6`CwxgVW&)n5%-{=Ho(tTStH!jonwV81m@ z*5o&0`X)o09{=|`Vq}>AD-m9-e>x>KtDOGQ|vh z1Pu)hF|*G3uWy~+R;N8)TU#5_QQ(bm|5@?&=q$Ncw?F8Mhkmqr81`-1=1ko)L?h-> zIH19PL`H=C{_1q?x5Do(h5`1OL7k&_fn$9*_(M*kvF=d_TRn(DG?ACrszOWzxvauJ--qh0;LQRgqKIRZ;M$lJet4Yd-_m!6)Lhh zXo=A+*u!HH*w_S<#<(a!vpxn>M17Jo4h9jTB3R<#z?h z%|O_v=J${of`riz7%?&X3)AzLonA~or4l8C$2hfP+re(h6>j1%VNgTkd3FqP{dqsd z?I`^n$-^>_a{{g?iTYa4DTt^YrwtjLphelk1#ZN~ng>87c>xAMPAw5~n;kK{_g`Xw zJ~G7KBi(8&fYdw{*ZgGmG|Y7^l|q)UbnQ~jWWv9`vD&55n9_+5I4*uLfNW6qcPO)6 zxPC<$O1BdC56u^yz6;`6rjjt1a3lHbtpTLa#t904iXb5ixr?prL@ekzs&Bw%%e04s z1zQ7YBSdodfBvZ#6!$@=w}=1#@bCY8%d7ILs+;#td3$@ipSR9`O$90M@C$naGiM=^ zo@1f(HwO$I(LRSdmkY=>5l{pPT^^2k_T(a+;Pr|Ml64R^7xjm@I2#i3P<4dLkkHXR zEv|03{mPshsv{PF0D{c4u44&G2Vl1938tL}j9SObeu?tQj< z`T~pHawKmq9{|`Kk_Bv)1-VEWMobNHucSwk>ux2jO_>rbs%aX&QeN)DW772u(Lc|s zqtoMS$hi3kjbsuE*+QR4e#wlFLk`OG(HH|HND-zu48J1d0!M9Y@`XZYG{oWH#B1l? z{z~FK-y@TmKKE7JoSEehs~mY&tldw|04T!*T|a_df9%QuqR*^fAm|o?br1Rr6g}eb z(m*H)WVWiKjq?C@tk^(1`Lrz_(PTviiwW-yE6`?Iz{3aKv%a9ydDuK0NvG zwMfpP+cst5J2$q?j=-!ngZG2401>5gN?O01HJIL*wEfzmtlhfFfZS%}X3fO&;Q#@W z4v@*81Ll+->w6~kjUJ4qp}UpU{iwGpDmzTszt(H{J`_(cMlN39uQj|IBB1g@xa+%ziIFLu?+WW68 z&V>?Kf%EyI5ewwCk6tsbY@WkgmD3){ucUTFUt)dc6av|m=f1rhxGmOWFYG5BYkA!y zzgTFCJpcCPi!RI3D6rJk&@afZ+|7}vfSN?)`gcU_o#Bf1YZcQaKc5)bdL1%%iNgzCqoN@p1j+36g>y)g2~-h2{OXQy(2jWo^ZzVh;JA%*v=V)VrsIP|~OP^`yQL zP_v~U-HD!eMQrK;q#`S`Z=V`1f3pf^uC|^kvCFwsNR&s7b}hXb2LZdibp*rHrAd)QGzx z#M|A~AXo2=|1rEAeaZ!)0u|e?1VFd~DZGe1k2 z-?{Wej$E1tQv1q99=-No3uN6<-F0s!n%J(N@~j=~QbvgM)4Hk`u^5LA{`exlAaeaU zAz>|Ao@d3+Ql2I13B)FSv+Q7YJ!B-qr-R|lDUb8JCz85!4sKoT(;A0tOXmfxhk?W{ z?153qZlfTx)_WsUumKm8MoxQ-`N>UCUuX0_mC{;`wh;<26>556ACD!I;1F=@!xvKd zWSZ$^pK#E8oBpnKn!L82FMzMe%LU!yxxip#_dYH&(>@1v`#24#Cm+BL+B=-}>l?CbX z+$Y$qYYOFwv32nmQhxS{8z)Yu;u529Yz#_t>K`=6=Awl>0U2K?%PpVFRPkBQ@r#H; z*!(9aUGyQNt)oPwisFxS6!r^GIZBhBy_&nCey>tx+v~~5emER-Fe;WjO?{7WMARzw z#M)m-vqg?`hb#~CKP{*+{bAXW<(!P}u)1wM!|gyiz!=#x(96& z4LLGECQnh~ z{&-UelU0m*JOZ>54R7Pmu$6l!z4MTJAWqhZlvy9?2qRL|nVim3=SdYcJa0z{7%yJh z2L|=NGh>1x&5RBbs0GVJ)~$ATQ-%IY?#`kv_|@!yAKG7lG^bp|zm zS1fV*{>$8pDvG6iwgb41B*2;(?>9yH4T*gE7yGt|9<*i$$_ zc5w$)-8#p9n50!`g>Jp1cYQT7vUE7`^HINhPTQ$ztuqqcT=yjnk%qw_;*S&k@ zvQ&K`Q|dW+)PyT8X=5{*kFel(E~=qX7(+erW;j+m{}_3dB-nFGY7)iG?pt^aexrx{ zktkm*W!2ixbGOY*EFWFhB$iaIw`FU;wV#ArOkSLU@5POSwrP^Lb05yBIpHaqOa(SB zr!bO<_+(fPC2s{vD*K+bU0-s(XsFdfuCAZ$kpH``w1wmRM(7S9Ou1XdnMd}z zroUu6mRUJ+VfVU$VLC)->%q!D()Hd@KajXw-FO7`-dp0iDA!G##R*Oi+??tmzWwxK zCMtUR#J02MnwrF8%uLm?gY(m4%g>%s6L;KV&o5V#vd?{Zb4zF{6pbI;mIZ7ZH1|6!0Vj_A%Fh_57Msrvj@KJ-VvLWb zOYSB*OS{+1&XTtE@qtyZ091r__4vGdyrnixz=%k&O#+PfStQ!?sEz;HMo`B4RE>n6g+$G5L?X{ji~8ko9#E8VJn4 z`!FY^9s3HrkY9uvPns)7NSB8)zf`-t&8<;(mA+dyyDe~F1#T~ASK;2uZ6w(mykLHL z0yFn94zIcTjl3-pabtC@8L8-zu@POLaZ* zWT4UVXN7(0qZ^)ItF^S^oAn)|zrJ~(67}-f?;W_;Q7Vx)^W=n4{C7B@k5ol3vV0Kd z;TMVYZX&O#Y~Yl%*9*JoAvSqA&}3h*yh*UP%#9?}TgYAK0SU{3YxXIyU4u^lq1RK4 zJH6pBjVV$I{qW+COhPspZ`29UbU?M}hO+LdI$Sw{$VM7s5U7s}<8j<&NxLDpI?yt3 zm?5K#kLvjd>Tqv{F~CT?d?0>u($OIciQ15h7Z|gLF;QC^r;Kv4A^3e8Pey*49ewrc zRc$>Z2^4yl*U9=(gdbcph`sr-!ca`_wgk z@>>v>@aSC<&&@1{ldtG-z-)Yuf^Xf@kM1l%JW-*e3zUM<;1cr7fF9vYGw0JUphuA~ zs&;C=pbV}`(RSBk>@%(1^4`D+ zlReWKdT&6~+~($3yHudhK7KBvHaL$H0ZBo*QYOlQDRrX#v4k>)sWX zLg+?=(Njm^_G^IyiFQ7|`a~YpR;|y61pplIuTRo?kG&dC2AeHYcJ`}&`aU)YL@(O5 z(;#<*?9GsA8;VG`xZADbG*W2J9R6?(U64L4*Jt+r;daYI4au`qwvHR>GB(~3O28j( zTF-o!h|2~b3Y!b2(Qg&tUC>cpnBw3NY_oj%3Eo7H1*?XU4BvD58%-%xw zc#bpbx~}`~zMtp!$M28lkE_>(^Zb54|aZs95-~{%Ji4asW9BsHG`oPWK z4?Kr_@q5~D5M1CcVZ!!!1G{JixM_#qAUnRC@cRpSjL7d|%3cNtpUsBa6u zImW}!Gxv_m5i=jiL$+L4$YddWfeW+7c_yoXNx5>~;~Cor-!vW)Ldc(<|1}@Yi0%Q! za4ucMYB|n_+`)(A5;@F-1ax$C`_0w6@^x2hE5BBv`>`rvJ$)|he;e-A!vQ8aqg9#j zQRcjd3R^z|zmhGHq%GS^-_9feL%V%c*UvgyNp;p+(%nz}T3T7H$2b__0?>P&^MuCJHA9 zz$ZWpa`nW5%ZGEi=9i&3c6c@b!Fc9@7Ha`vSc4*%{DvGK zft;hI?S_S2HX`x)OEhXe(;yn~P<|4JoCVMW`tPCsKkdeU+P-nXk*LHnkfRF8V~==A z&H_YA34SL4?dBHQkC;n<;PO^F1D&&riyc47Hz;8wRw4%#4+WP*q^Ksj@qqDgAZ8#( zO$Ud;4z-yK-koTm3;xka=imPbpY{&&@gzij@X!Cxqz1ijAok_^aV?^^%r0YJ4-NHD zVLGRA5|Sw0#(>MVfNOC8a-KVkk~rt3uwiUaFs~07;c8%Q1;nkNzrPVD>N`YdSy)Di z#G%2ir62TQNB}Yr=_l~&Bt9rhtDO;rNKXoiLS)`*f7j_PIfFkwEu5x10FPd+r_iD~ zb*h~Q9utBM@9Q2O{_K#1Ys51RGyazEaj=h~E_w`eXrc}c4TU&J74(pTr(N}h7cdb5 z;Tx*S+ga3fMQnh74RjRX#f}r8-k=viBWMlLDEI{q!*8FG@JGLUYN8YaFbj?4X)N2tRpC4 zHGUlAdhx=AHs}BZchA6p2mp>tPXgE$nuLUPsWbuU#+O2j?Nbm)AiaapU7VPR(bAQp z5Dh*!&rnyFG3GwnW2PBL&ZwVo*6wM!{ zvzhA{7^J@Bw{>t}9G=tf_dd@kFjQXCNees)?~eIVS4^M989M1H%9o9S>h3nK9dScZ zQ64-_*l_*B22DqA0@fEs*GZ|B04vXWbuy37_70VVIoV#bZV7Dl^HW(mbm1QnkDGC- z;z~E1vX{t4f})}FZ_v@gWL;X8M%c#H^?nzr5l@?{`@IgX9mrn`=pC5}V8@MxMb zY^(}&0*htn+6D0gu<?(Qka)+)jr z25mBQzJUg+_X!iYO^ltwU>X1Q+-@|i1|uMajgE}8+LahvVXy(76|H^frE;*hbg(-m z?m!ohe*Reug6l-axY@Yz{cAicfO>OS=qoh5#vN$pR!*3Y8%(3^G|P4;2!H84SqNU5 zgZBx_pXuE5=c9K|gbl=T z3TbsQM39v(8)v==xKF-G8=HvQmGibb-Lgy*(?TMyLOjK{dv-It08fB0=cd1|q72rX zArd9+p8{HohYoV@kN6P~gpNACq$f3r;UpMg{$u>WZ_OxC$(V#h$-eg!3QSNKr_g3; z8IJj&P8fT|>C@iC?Kn4=aE`=t^cX0LAZgFP4;hXh5BfhA9$1A)UjJ_orK$E<4X`Qz zoEgQu6`|$6)hNFIT^+g=w+b)qK2B-{TP|mH*m2CiThk>I5ZLAAI}j2FbX`UI>PrIP zjfi)er1Dz;ixbeVwVpN5DaMW(havqEoS*Pb=nFo^Qw7x&4Zgz=Y(Z z!2;WQw6`Jq$Iy1E#2TB=1Jw!y&C9LEd|DsYf_1$K?q=}10#1`4(5-xqb6Z#AU2>Md zesv@R;V-#xM#NBH-E1Ej!be2Vm?W$=e*L>NE<^noI8rwsIq#Wq!|^#fL+ur(VQHM9 z@_S9_>3BFaa@a$a*vcySpd#P3Pw@Ka8ImNB*Z}KMQC6gYQ*m@OlAJCh4V>$>jg8-C zWXQAdqP50}Hj9(6Bq33VMF9v~pK^Wv*LpCw;GwT2M~E{`F*)~$06*34?46HLr2{l= zMvsHMOQ8b+(3^CQD0~043$~uJ_@ttyK!$qiBcO|@%QuExqFE8*mz$X}?hgZV*_%W! zY$9WouTae5wzV<=)*h-E;LA|VX=ZIj^PZf`&B6FLDYRbx+apg+30!EM0{#?Nw|%(m zbQS=ChiVX}R+z#~xD3s=DuM4sZpYZsQD8$7VTPyp?Nz1bZMbvk>XTQDZN>-MX z5&C(8VIdzkSeYJIOz>6kqG9z&X_+INscC(&uN8`}tm%Zrmj8D(v6;dZ=Sl999q z(g!k@8zRuH*_sP!p1O@DgV%aiSk~xcM3kv^R{9IBdl5Hq8D>CQiAlqGxS%~lt`X+p zDh8)Mt;v>n;bjZ#+wU)v7Qf{t%&|}=WY~MpP0$ypX>t*v_GD&t@qLtq_V=`Ze^6uY zd68#yeEv2ul#S^j2R3Wdx84=*og0n_QV^c3?_;9TCeCxP;2NSCPVwxpb3C}Y+b<`U zijO$r4XL3gm)Jh=lK$dRRZA6ftzq_w*_6G(u zcn8krPh_1?)+O-yd41)Te294uJ;K1gE?d&u7)akOi6>*Cgm}$sK{diLY0KJ=?m$a2 zLo#@0MVn0!iFJNt(0(b=ddKQJ66u+B7isV1H5YRK_c?b|xKH_OJS95+GM-nBVasb! zsc$HbxqkKjgO|g=AmC_jcsY)|zbawxv6Z4?;7f08Hn(1L0|y6wYsebLcFkW*;dSi% zwgyQYg&3W3Wh>8x2f(nE^=>6E4FkU}y1s|${6bgT@=YZgT+y61Yw^S%Cn>glZ51)EcK@jQwlY@%XzbmytH+BTYUM&=Zcf4dWP zgPCf+<5aBx^2ScptIut$`cq-{#VVGHN$7d?*z?(d<9K-QJ90tJaA|s2rh3|I>PfE84SatcreOR8-I@meLo4BxVZW;BcntEU)>Aa{KEi!~T6f?KqV%yH{ zV3w#5=A2j-duP8M#y>1PvTP^2{vYH_?S;?hwf$@Yt)ZG>f-K#UB`p54Msk5~IAa?c zHnm}k&HD8A4Nq=O*1TI_ey;xX`ex)L_3g>axmM$nLdX<{a^BU{+R3(z;ttPhw5)ct ztb);t)>L5yfg$S~l@)}`5IALGan%TM`@{KyskPp*+$0j-AKlMhw7*EzPP98Xg2kjT za{)wtHnW2r7?s-ONSJ6!#B_NFpjzqeEEnD1w|32Jy7J0`3sMo7vU zA>2TQb9wxWA(Jy<`jvlP&Lh0$e~FdKG2gTast`O8L7r33;-pw_{Zs2Y`m6tS z0`Jr?7(4llqbm~nOCM^5zgrjRnq$zq2&J`PxaNNtu3iw7vaaGz^IX{b$dW>_*EVSj z?)0@Sj2=#aS+|o4{Mxzy=)22`uswhH$S_P^Kc z*jFx|&`y&z9JX_Yl4%_lkI}jE>Xm6-RIeSoD%m*MH5#R7Wwvs84Rv*OzT&$&JKb^Z zHH-Yhgv+3BmSn~o(`K!sPxJj+MTFZ-AyJdyPfbsD+txL8gt)K**Ro-MwoMccUtN=d z49jlg0`|`SqOi~$Q~c55`YS-wlr6ylj(Pf-AA7m?$FiW}(*YQevaSN7=!vIoC0jGv zMy`;MRXRJ)HIZ=brR(OXZ!+)b^62|kf%kr292_NPu`EtWpyel?@n zH%7_Vc=E>fk5~blG&sGv?H2497WtEoIFt(sjXGw(57e}Lwwp|=?2)SY9$f34HbFA7 zwjaR}xHr>EFSvz&-gjLiwY+M56XnS)$_~jKuFR>x($Z3ZvkUN9$^$d)FmP&X+>)*! znVN|apf1YLy?^2~!Sl~#10@q?wEF z6NtZsSh;7o*R>}l9Tu-xkm3hhpWE61I|8-#{%fV*vec9zuQ3i0R5yEH9jv{rn6{rUs=7B?)%h74>WxBZRHIr=!p{T8`J^JJP~ zk~k43Qj$hH+lKo_9Z7gD9SH$%m71#vGpWLghY{F!?_YW620YbJbBdy>ww)F5udzv0 zEB0!MiHfo=;zZJhU1)Aix0Uf6Y2z#tH^KX*v?a8h7o@CjfR#NzhDlCKe_r4UM_|s( z8xE5=kvYAKoovf5L$pXmeQzg#f|I%*0EJMiPc5viN#nd09-pa#s@*Xe0{70A9#%yMN(R~plKI&O^RhXQ0l{zg<(RB7W=(Qf(OAVDbQn0| zZ-GMFGd%fj@-3$w0!z<_TOf=c>;%00 zEpmv|4nGW4RE+=I6_o8mxgq75uOp6y%H3>kmb|;y`Tc39Kn0_~`-ZXSQ}3_1b)6EC z95Z|b#!J=8N49h41JAMDdAT0j_$pYMT!d*v_^pNO81;~Qy~Wxw&D_C&wZ~S^U%V*O z@fmf|b}VkS1g`;_)%BV6?dOb`#tQbJOH)^fGpG9ea(B=*GyhUZgi4@4=@_KLWORP0 zVtdR59&~UGTczH=zT1xNJ^@@n=0Apa6P4MY<*eZC0LP0N5F&+#(hCpd&&@+yOJcrJ zR2$R`WAX>|tVxU7i+&v@`Ow+mO`XKfU9pV;n>{L2URcqI>G(=(W;fcUeI1PtlWC_2 zeqN{}k0o@Qy#9Hm{vHtbW!I~Y74qV1!VVVGLunSmAUj1U()x72xR`DcKT>yh{J5>xpT=+c&RKAtRJY z_fO3*b{eJaNgmu*<9X*ijmQcY`{LS2FDf?wE=0RRxe>c)P!J4a36k^|&l{pO3LJzW7xgLj)Gj*I z@&bx%*_SsKF$7-QlfSgOap7NDJqV@MaW1^Y^mPo>tk1v7AyE9yet&fR;QV6Rf@Rc$ zk;H}g2<&Fd`^ne(C)R0wz#Up!+0hbK(e#;(CFI6MfcV9q`Ik%ZPx8tvP`6Bq;i_+? zHX89fn=S5bv6V$C8ez_=p^)nmL<$hEgbz(8CNGGBHgEVVH>^=F4>^9|Z(wv&m<1J0 zzc+nt;qy&-yM-0|Pa9>Vd)nmOS_OPov^D5Z##_)>aqhn!7#M&fI(tC!5?e3zLKc4>^`wsVyM-D17rfaA?} z6w!3jbs212(S-5$MB?n3xS%HN8>sZXtKMTM@jG@Pj=!QMH@aQJfgO0fXV1sh6XR?x zCl&W#D=R*7=8WgE*Y}g_la3k947Z!=!!-2PH7S4DUufAXcv46D(QL`Ejkr)m(vMZ= z@m5A3^TGD8>$mJGBbrt@tw($lqh_4j{K#OMk-)tjPUTPCXwo>n!nBY8o4i3Fc6GMh zNM!sw#cl?mA_<9zqK8Mw>mn>gu)p&&DSum+5;wY85j@hc5E5nQ;x*Lp9;5o|D`jKk*{i6ii{;J2rm38K9Y#eV z{YGQ4!E#=go9%U6GRHCJ>6c`ZazwfQa-AyfaiF~C+(0lJXp+EeKuu{BqvA`LR!_He zH~q(l#GV_J%fd*@z=hhOkWpG~%r)~EGpEk`O=#gchW`-6uPDuymBm?@d$CCPjao5T z19foBNJFLK5EP)*v3%zv$Lhz%$i3zWrv8-}gVfenY-UlmIu`djZ{0Cgb1juiTv?w} zA7XGk1Bnv@tfJ|h*cRIXckKKwbEbw{xrTO)h=NzYQ6QQ({a5HpAknP!u05?{&&SI1 zGV)4~6ai(?IAy#_7rK!Xko18U89{UVLbWbC#wwqZ5qI)7-m$voure8;ua!I3-ZuAL zb&xqskf7|~KdFGfUs))f!NXT1g^p_B4&hi0RV>0+dpxM54e;sYeYDc35DY%$yM$TX zDBz9~8X}eo4E82Zk_V0p3k!M2H3tBb^f;v9$4ch81z{ovMt@dE8KlGQ;mrnqn1QY5 z<>Y6USSPtnnSe7@Py0qB5SqKsD#wR3)IcwD&if^&O&%5ZdL~LD34`~%62F7DjF$#| za^fD(6KKUyW7quG+l$<1TykFONhd96TG!{D$;wLVyyQkz>MdCU1=FK2UrxC&_p1>&^e!5dIcm{Ns)L zZ)5(yeId;AwD<|8rRF=5?!g~8|g%sA9Wxp zaow9A`!}+2zrBdk`lir`)_UuI`hb&mo*qDo=&=)U@YwY1xt_)X$ecfl!aRA2NjFN>2(%I&$T2)V6k#8p-lD5^L&Dis5E8=ku?G?-8y z872@4Px@!fdHIj`E~eEf`Jq(bpBi_0qpQ$uBB$6DA6uY@WN*QRE=Ltogrjbz7<{8B zTD0bV+$TN0$J2#>U6$TKw!dwR-?}d}v)@ImW2c&i{`~3Jj4TFi#EEhe#PsbOxH%Rd zN-#{(%gX1(%`Hmjh~(WZBfZ7a>y_o)fB9&C+)SwK-yi5cX#F0HjxJ3;rY3DB+J(?=@}av_mc_J_w4Vc#-Fm(27U@`GAD>Sv?M*@ij(mO$xe*=1%|ZgX*rZ_{|K9 zv)gYbuV5&i-(qgFkzxL4EaKOdZQO_}WPLaS{H9}ZN-OeziUS;)-JJnsw&Mb~U-p95m!Dlgr&)+GF3zSi)nCSqE1Ktj*cA7Imtfe%HE(hre8~wPMYq>dFJY00gSCD zgqBtRDs$Eii)xfj8YYlN_QcTm2x0wg_#s-; zh$*~(Q6`})R;2q5UL@RnEWU37{=VDekk8us?XdpZt~mkR)$$tT_Zk=qH?5EplZE!; z;b{!nmBbd%`d78ePR~LaJ?3?x^TOZc;Q#-R0?7s{jp*``QoVS?HD$v?(410Aqc)1= zv%68Vs_g@rA=pI-a*nwEzNjEm2bD)_E575A5I~xW_t^L6ANmq^u>p#SrmFTG0$3Wr z>qiA384s2Ng)T6uLZgPIfl_5K0;hqj;tN_U#V;Ft1n_|n{_%LLD#27-vDzvOUb0X+ zt|eF!01a0XZl)989?te~@26{|(v=b1O{L|+-wpKma-P9S7ifk6I0Ck~B=)N-kN+-j z0KS1jm^QZ4A?jd+LCP{CGzuRwv*Np>M1|G!PzhAH@GO?d)MLLx(O)2dN($nTONoI$ z^wyc3^3GXQd~aC&ES{Yb#|a(>m|eeQ&GmASRih8PoH0dqcJ=;tGgtMyO8qd0A*U%| z-<#{*>WnCIJ(hAM1Veah1o;M!f^#~HWeao4BhB-a)9i>uJ0%0d>tAx72MNi@NGgS- zT1EGjP~dv6Jawk@8!#rJ%C2wdt&DtdolQ^8(yV+tNG56B+}U|sOKTYFW1<{HVMESb z{w1LkfW5hXV37;S@GaoKL6UW`F@`vS<+8kB;o$=L>QwHl(1fSGzh4+~+)@nQ#)!W`!}S1vX{LjKs)Dx{6e z0K3zw7K=ncGR3lBclw%m6l5&ElSt-$gr33-27%CyB#o*&3@T(Rp({z$X(mit&(&k7 zkPBwJ2VNguRuF8-4m9zAKwQMZx4=&StgI}9I43NM7l5wpo@t`A^Z`thl}@G2)D2o5 z1||#ud=aEtZQ@f|ITHvNq0lRK)xp#*n1 ztzWzLU}eDOvp)c)06ruP>MGB@47?{4SRwrAaJ(D)YNzrl_+mX^nX_0f1?pL>*A3YyY#41T$g-iC%en zx{;;j&5jwU&4RpB-yxVv4WMB{M-#(gU{e2rMu>}&h?Bq87KfqtOdN*mNfyBY!~@MIKe?c{SM5D)j+&>9;>_Ugs{_!p1k~vaf`l!rX8;fP=qaYHT60^_T9HY z)oASe2vAw$#f62DS%(>~>(CJD9bhmb?ns(q-`%dIBo1Cz&AM-NswNIv9+;JWChduY z-Ygec=PQByL{BK1o%$gWon!G+VkMcQa{>68qNlS6#p~CfQgnB8U})jp=mVa@U}&K; z1B8n&NwDu;oRDnUv_`IbCnqi3c^?1ly5qBNkE1xfAG@ULy)LVVF4f?yYa<9--XAr{07~bF& zfhM{qWDV*1HMXzyED<%){%;F(Azt)Ls2RNVUW~^f8ZA95Wu-o3xm^&0*2R=GGA{U| z(V3YT^KR{Io0HA^bvR~pH+?p6hFit^uG(^`m$CyTY46V-t!xdb< z8T7iocA%k}Qbd@eU5##L zpJ?IKWb3@q!sLB|`p9ZPJ>r`n5&%`R=zlvPPPU#CO$L;t+s!>?M4rU$Ie(i>4GVlc zgz;~(gEEB}1bViKQ8usAL`!u|g$$VDZd|9aGEU%=`&y9u9p8`O zg1zabv!b@#7q5L`GPLwMZhdaM@iMo^n)}8Ur)q?WX!U3JxXSp-VXl+baR%9iaX)qi zT(Sc{i@7ZI{Rqo;t>0Oj8M-7QGViux|IoH0-Q!g31Xse3x!8;Dxytb|YRIdIEDCgP z1Y$6m9zCZne|av#8FtXX9B5_(TC%roP`%Q0^MNIn`O!!APapHH`s?U_otM+d*RDY~J3}2Z7%%Qo zB`jGP8u=X~#>QWTHG0Biz~Z}tSW%PN#I15XCn9ah&*`b^CDBE-hzhqIdkbWHpjnz$ zKKr0E!C1q|IJE`rX(!APX9ffY;){m$F>QY-&7LnMY0u8(J=Q1JNEd8uxhl!lgU=q> zs313#XzyMlbWvQ8GKKw>WWL9EZ~o$OtYFJg95RE;TtlcH)8 zM5htmcxaRe&&3cngo!{f=0y~r?l?qCqr?ZVFx|3ubsfN>Y`Ue?I3)A(%b6d(K0XDH zdDNgC%sVSKz0Vd@#fBYrJ<;tywz~|uR+yW``k=`bZCGV!uPLA1@L?(AY0W-UES_wIt_Icg)D4iJ@W@)J@JpkF8>xH zq_4ItjvBPVN;-Xkn>z=RE3)So%_P!ChVrOsXizl}PNw1Nm(l&abv6bwOIM<^WQV?? z3O$hRSC%52Yp(Z}JPglo;=Q-!G}o0G#qeZ&L8IK>h&RmAl(FAxf==4s z8;)-A2n%6*{MXN@Q`3p_|MHV&ey{79kI-2AIsrmL^~}*eW)bUaw{szE#ebcvljc}w z_s}Ich&|_?An*|q6m9s|w~s*|*q>jJn2Hs$lhYId9APg+U!ivQL&np0B#`Rk#ZhPBnL{-7Bx#equ!+9`HoBC_n~ypd|RS z%(Y^M!xwx7fHWT;pFQ*f%ZJzN17!xOR}=L#-QiQx0=zY!Rb6zr(2PKwu>LXo6`t^& z?u;$Q;rMTRi+@8$6sYF+0>-aW4I$)xxW;!Vsi}p5Z%_?^c&z}v&I_Oow-^rz?UkP* z8XcI`Iw2CopYh$Mk7)q9C06y)N=xd{cg@k zxVpJof#I#q+}RL!dO-GP-kcbkv43casclH``4sXe~k@s3#PYykyq-x*N7<;5JCM* zvOm`QVfK=jKS_I}a8Fjhh5{4dzjT%kbu9kK)jcJ?mUH~<; zCok7IsQBTa7E2p9O=&peoiOtl?nc&v?&`Ye%G$$$w8fX|9E*d45;U6TU(O%xFvTTO zJqrDz+qjXvSXccm;Zj);+{!Z53+=2iWXIp&X(q8OZ;v;_Bq#0TO?1`>yHM1Lh;d6S zdlj?$NUvhxwesOvzsaf3^nsIstG?~_NvfC%-`d^qsA+Hax1sNBp;3^rDVcfbyjNQg zRTsYRiIs%Jp_DGh6r!2WnyL@|ICv2T!)e9J5@`SOIA%__(s|*~iK+Pw%J-<}oiU;n zmMoTCuPL~IFj37D5EbPxTNtU#0*~7m(OPxOg5;d%KUdJn2-qPCv}+!N_UgSd?j97f z)1+8bRhP=QPmn`kQKYVOaH1EB3gqBK1VUTHQ^8h= znA)Fcx%j6~90}bED%|J6Ja(JQqrmSe$Eg$&L0W}h{{RXY9Ba2#|epDrE;;#NBtO8&58l$M6yq4{dk)lX_lo_~)b%p2wH1y>1-og3?#P z2m~v#nLy~t;W3_gAdQYlL;Q0rh}Y=<`w!_8U<8iTAyA)y0tP=jeivZadwcK1UrPnh zi*q(=-KawqmihVu&25-|zR?YG#O(lt-|v}BiZKFA!f9&)5WUS>niY<1dR(>{T6?pq z9tRs9;-aEvH|P+zpZxtB7a2>Pf6PMKq1J4u4_Zhoy00bDL(1wES?@#ps@$#VfbxCE zq{}lh49TyV?%(^pJsHb>Nxi0WPrqeZxcLVL0%AI{qgMe38Ox#ZO3B`Q$aO8Ne80!= zrjSyfoh-3u+pnXbnoHFL8wz+3g&;|@%C&ZRKu7bj6q*ow3ajHFPh=B&-f}oPHmJ|S z$X42}(MO(m(kEo~t?mU^mEjHJ?$Zc|B4W?;nhc1<#iO&4_25zn;7{Vk-1?Hvb*k~F z(4t#j!$W)ZjU1gD%Cvr7f)lKKjKL~r#S6ba!J{yTW|h(j)aE6l9g5<-1bwrBEg%wE zMnX_PC%j(=JkyJlzVz-r>{GWI93^QZL!dfEu};0(+_rf;Td|_(sgktd+GhusXWeWt zWz@_U%9zTQ+a==K8T&uWBwP z+*XE8bm|=k&Y{Ka9JHlOO8bg>wzkdP)nd7ZB*}i((~ISmb#r0%8l<$P1&BktWjh#4 zTlO*(AL-w@Qw0rB!NtC!bj|6}rHJDV%BIq1GOxQEXP~1w8XH46CoV)P35CiALWeIR zlQLN;8%7$Zgo)@x&WWS=)C@ky?LpTH*VPvOIp_qwomNbq3BqK&bvc9Me#y=C+H#ED z?~U}$i8{VYP7p1)cdgDp6(1I|=pq4HiBz&MhUHwy-GUFz8ZV7MMp?L-Y^Ow5gqa(4 zGq(lH;}QQ9f<$_99At~W&DXbg&a&~Gf@?txWmkO)zOS&z9{888Iw-bYAU}Ne^k#$_rzT5Q2!;vteqliTu=^ZGlP$SUqQ(yj=|~ z`sFG<-BmbAhMp50%8^~l>EjVkL*@CN@csC3Jet5KUxrMotw(nkrKmER9!MgJ&UYu@ z>f#H`pn}hQ_ZGf^NO^ht@KCm`{yb;W;ve0y@0v)vpAO%-*^( zX{-@Tkdy%`HDy8T-Ap+YYUPK=mlt!m6 zWw3u8x9w4U-u1y)ccC}mAl_wB&`rh%I6#HCri(-oFDykKo->l`o8^;Hd1)-?fHk60 zd^rz}=~?~i#SiL6;L?Z&{!~P3iTBa&%F<|**``Y$BXJkY|EQ#JF?BD=(ElA@mu8hK zwD2n24#Ky1??a&Z#moh9a`dlJb$okZ#&!PpYmOgkxNVxBGnAOBukRbWd3ca#jW9=Q z?+s!#Jy%jV643ZV0i6axk%1|CN3s2f2O&P*Ieds@OBfHcnH@D#1~lWYY}h^Mk|~-; ztG;90WUDIZ`24R^WO2r#3pUg4mtb)<@L}qUHrrs1|(ainKUs-?1H$6@1bCbiTWR zBL8mYyS;b%sCMk>w=(4A#d*t;*pdz<>g6z8J8Lesj2v231D(z3E@C+!cV-hg512BK zWSCr)6n%MX&%Zm)OoOsE<>Tl<|2=#QoH^$m{h1QIIo?hcK0Ub<@=@(I&VmoZ_|ztq zG#PUX%e8HCZC^MCuEgHML7K}FBJcUAHKMu4|> zR;t%`l%tPy{k2!F-Y~yN*pkLDYK%>!ovLfvtPB2Pz=!WjQb6<>#5+>UgQfTdvd%euqT# z2cMMGR0x7E1#olLU;W5?FWode+iK~>S^EC*qLsA4Cl%P-Z>BmsI0h69fGCo}Q(d{b zoo}&|DUhRC<QiR2|dO|uoDbbn4jZ)f*yV)KnXt)RCTbwQzq>Kg%diCE^2Qpzc*0dQ!tF5 zywC`kcm;noXU6Wat~QSSuUdOy3kr=?PS^s?NblPLG|}oD=H-*4F@K$zbCmE$b{l-8 z5>w;s#~eumj2w=Oe%%K}xog$Me5uxMSd?fz9K9Hpo1BKYSuE59TW-`wJd3fJK)sN| z(PYNP30H$4Tnz=65OeSPqNPmES}FUgUQT?R)hUzsU7DW!UX2e~GV^!GrBw6g6Tc>LgfS~O&9D9=h*a$HR}d*PP`Vv0NfWWyGybMTIdsA@ z*}Pu#Y_NiN=WdII`=yH)t(PkGw@16IxH#tWKZ}Jj8b$YPm$trOcmDCjY^8j7k8?Cn zQ9xkmqy70q8*PH3>ex11)vjx8*Bq9H%!~zAu(U@f?h0Nb5?u?@pyxYL6wnf>`IKCT zb}*qk>?uC-jFa}6fwbrI+ymwpek%sU6Q3wOldn{5^yqhc7fY1SC6`u}_BEGTSv)E# zHZa@sd;fvbbL?$>rs6qF?cx)Tcm-}8G2Mpj<~&v1hPw%Eo9z9xBYyu_!*Zj#_TD0D zPlNabH=#BMw(=s2Ve_eN&9r5uN5yCDUMLMOaL+3=AqsBjCCLVv6yyPxstIT1H-nM*VhZC%ZJ0Bfx9$rO-X`A+q{;vkY1}pB z6~=dv>64TGapc4UtS2;ac0F{X!tqDgVlC7l=wUxpXD~|m-qY{4QBBmBb*OK646M_D z7-G3oS+RE~a^cre?|V=$PUKK4)q5{dn4v&nz{3a z(E7pJX`jm2!cYZ50tVnUVX{mjbp)=J<0sUIr;_x z^2`?cQ|e3yW7~(dJS|-u6((6sPF5Ia$m9xx0LU2=(|D9%&T``8 z*R8D{j7W-Wv^VjD~%``_pX8r9Ryll^W5&M@In)? zOH9~6xBQVTF@D3{!B&+VJx84f!Gcqbb^hCu&iKUZHb=8{?2Xv=OqMwlo&kLe^@rfe zd?xXnOWvb@{Lo-sQh&<%lAN+{$nnL6I%w}5Tcd7%EHXn@&T7~xnxEzAI0x{{*GrRB zYRwiEnx5ck?j1jsxeErcivJl&$wA1%6-3Di6a||V+H8!x0IlbA+e+&BfNeN}os`ql zvfvEpJQ_|no*k7l#V#P7;E|PGYiN02TUnr2DNHBoXU(bC5f!u=N0G_3Rk!%$e$Qgp zAnl2vhf89tJLU%m^&$f*_4-WhAIDQ;cxW2xMO-L4tJv-`KDaABe@*wz^8Q-PvuxtNZODm62XFpwug07#FN@+ZOW>mgy1>J_9?MQ1F zcxffUE$bW~_z)w>L{IOE5H5!0fPq5kmRf$6)2Gqo2Q9gD{G^S?+CCs+9%@UnUJGd1 zvrj8oIH}zJvf!&(&^sZ`c~vTcy``6ms^N9zom=>CcNWd(w=c_uE}sAUuvTeDxY`$W zx&nsmL*1yXtA_7Pw|v4h`+QHs(NtJCsYE%^c}Zyv+*yhvW%N%oOnzq=od5w|E2+`L z6*A+jqXT#A?cZMlvHx@61wSjexP{BBdNjauUr!v_`zWOjd5V`u7VCdA=fXrFH1O~w z4Qv@p=Y3h__9NTWSpebDW=Lbs6F!@I?bE$4hWLD^SRO~6D1J%e30;;IV}%}`%sW5R zj;;jK39;JO_nR(}Mh?py`#zul86vYi-0UXT-6dO^(mQSXG>`1?H9bW5bpQ6A$&;hnL%}lX)47#+flP#HLN4(_b=#|M!|R&W zRlSD0Gdo?>{`EDL-!Az#aQHW%jJdgPU)os&lWsH7_AxBd`h`SaYJyeKm3;nW2MzV5 zleB(YD_&_eJwoA~PAqB4uRn<*veojX_kVnlTU847M~yHn8W6jR($mfmNS_FydbV}` zc_jur6Fv%g(A}KSq?jfs80LJVKLaH{=f1uis={D@W4pW^jSRjTni=%zo#5(jq3KLR zA1KovLh<;$ZgWFhzL;i#9{~mZsq1HbvW_{bw+z-T&qqDH9Bb+5BwxOWcmeHy`nW&Q zY9k(=Mv1|S-#Aph^R1O7g~1C;|MGfZwTmXke{6tKmuZQ=*O)R9#uA|z*lJ@R2?v9Leq)D_+; z=^=q1bRVfbD>t-#zt$$$k(Q`C49l4jXjZqOCnq6dRAnPhdeTWDux|cIrYk8t7Y5tB z155kF)OC92KTiyzt)pA6Lq@`m^!cKy-mA~3RFkncO27^k%Ri-bg)qlIm{iU;Yi}RLXgHpelsV?rwyZk0k8Jqa$Tf4cKuM=gv{J`QKq^VEMo#|U0X|JB$bVt(_Dd*TwmI4V)=m}^K*@0BDfPzoTJ#Em{hhH|*~ zeN5ahPRQS=rBg2xt61NNh!S@kDL)0Q+T!exHY*#a73+5BMuAK1YY#1vrN>p^As0 z2I=LpM48gHWG;mvtH#A_kG&^v-WaI93(_k;$Se+^6YVqQYssA>XBO>8Okenez*uX? zpLkYI7Jbl&_nD|NXTA3s#t!tU&?c1gFoB1!&lO*E>rn7rta90NsJfJAJyG=iJCIVW zeVw?~o=@;+{YJHJ9JxC~8|}WC89b%3cu@cOX}Q(Ch%3Z6F6Ey-MLUdVXoZ!OPevN7 zS8nMq4Ysvp{KxZhb3XFSGl%&n7S%8`crQ@wzH*aJ=wc?hQb)(u!9Q4Vz;$5VLZ)cy z{Jp-Zsma|X!sTs9oEc(`@()5pJO^+8b-EM@ims7t8)*A7DPFx$>}yD zkG)i>O11=cbnl(Ib2GhI5`llvRK$!Xw^I&&qzsN}(QoZ5mk;maE*YD@fI(q?=nKn$ zF0q^t~ zbedqgnH{8O)6>&|0m1)(o3*>A2XA<&dWI`tVSiP=dOIXSL|ExYrCt9#!4ONM`{pii zheM2@r39{ceT5QIo7_o@ef^GN=*D8$mfml^Dygp;)&WdSRrN1^XtWHBnQ_6wvL9(g z>(SaL^uQ!@_UfCWhQ`K>gZEcU9hp?;U3K&Ib!FecS&MLVosM4$zu(v3?Yh0drMOV4 z;T*oN&amtId8vPO5;8k^yChs_C8m(|5OZ7}3D3<94r9}zJk__g{2rDuwpSryzYDN4O|?gME~zGLc2QtXodWnPccUzDo-o!K9Iy_o2w|cbtPfRV}HscD%6(} zQ~Px#WXX9hUVLy8aheXL5bPHRoPZ@IpPL^B4}ApPfn9zWx=`6nwMKq;5)eZ6eNOAZ z8FJeYd(+tQnc}>H!0MX=7%E^mdGHX#fhbYg%@!d(gTPqe<>PCh@I(74Kej-Z-3#5I z)esW!joFbJFMu8L;~|2c!Z(iVX#+M0t8W76&473868suw76w9_oGqyfQBe%BuW&-tYb7ixy35+M2YXdb>{JN_YBX8VHpswjWM;8-s&QN!_l1Q z6mW#_Hh(=rB}@UOprD|U3YvuHUr%g(fxthF_vu6U-{}t=0d@(Qo_Z1c79qM*X2h%Y9PXD zRnqwdwE4I3iq8$09f{=O2w7=7c?ZgZS0Ih}<+aIJLf7}a1ph+w+l2kbdOyzOe)(bE z;#a%VFm$$7RB6chNa$f@01}H$M8|_C@$vzhfG&=o-^;%Bqnw@^Z*wIT^`wq!Zn1<* zzyJN0=3|b$xJX0@ycm(zSH})(JQI&#UklGRJu@%v@{ajXr5&#AolTA}QIeIGbIe1v zXm<_o5@e9O;}wKt%ESXGyxohRn^s~VI&0ytq4*>GU^v9={C8TJ5ij)f&+{6%?2+Ln zu&KNYqJMr&d$4SM?3f0F8vlw^4gj976Tl=)e{L`8n>H{oPl2BjAdQe_{l_-nnM0T! z&Z0+M$iQ9%(g~LZ~WHI=%<9AE`nnIeMdGWukf8S z3xg)2Hj~%KH;hXjK}$L1@DC@7QQf=ie+*pkCK$Xmli0^PPAH{+DcEZxg)4YR0D7Wt zjs-RoK`3>!<{3@c{JV07C7kK|te@VGKKfY2rRMXdQg)SDKK#|GfaO!%L;{m5^pI9d zn$1A4D~{nR@>X7spA%Z4&kK%jjn;iJ<9q2vAIjbobt5Ojklie!mR8_$=*==!I({kc zc(e&&V2n=5+?)uE0+PdsfK2hur5VRF9VfVfIg6h^LYkw6L2qJYYZUQUfAT0%MBVTFe ze1MzgM;An>%MH=N#T1Si<$!#RC6426U4?r+$1j>@>x}mlRArr@QapJR4Wv$!IOT-f z#QKV?o>;+2_PfNb==s>%@|r&sfg53Yq$W5x7!o#Gn>i;gk66cNH1>w_ADLo5~XxkrZBPk1yS+#bo!iJOuAeiRw+U7W8ouc-WKU+_0&wPx z7kyIU9DQ2S<#^`aHf-K6n(_keot^Kw?=9pkNC}0E>t-m@TABU0wS;3p6|xro#bza_ zAg|a+zQ1g+EIWF8+HGUdATVF#t)id#`|-+?rL8A!q7EG0=p>b!Y!Mey<$mYOh*jXP zbA$d~D`|Ol$LpBXfW+t9lG~C?^=3Vn*v|Z=*jPs!l470lG}~n1jGS7!GriAl2`rHa z?LBw>&`$6$rku%-RUUEjB9jJ{%7-ehUf=)I*p-Jvxwqk04$`JX$Qgv>AWDgB71O%?GFdW~tOp0*^HwLPKfdew{N*y2ci!K2 z|DOAK?&oVMX>X1tWhm2~4tGUZ9nKtS-^cFUW+En#Q za>hN~b}opiM-87pLO{a$p6aD=wTe=;R5szuq&35IJ45JvR@Ae#aQ@CduqvZfPHz@B zlWCl9?C9^#nbLc0S!0IuPs^Gg@PX?q{SwkC*=9bXdrO~+{5p(nSQEQbacbfA;(2}E z=Tx~gESFQTrL*oz544is*3?&NRiydSp&imEI0RKot?D`b)U;I!q$wHgk%vo~?1F+t zsfmzRd|z2qB_YyaJ~F|DBWFP<%kaEbVvLbN#B^93>B|DwC@TP%ibk~+6&laAN6QIh z6@^~PgcghsnYz2YlO7X1`h`w(hu- zC{j_!5t%Ej7aZ>}$$4C&$J9%Ga2J`8v@q;0G2^Q-sTX5u>pa*W^!0RudLX^(7W1!O zwY?F0O!v*ldr8t8ozpPABL+*LH@jHhH0|mZdTd5VVcu9h{Kge5Ujao4^r2NRqJp`c zdhaQ{??2Ul%F`R zw;oP<>EBZVLeY0)th%U})PtqWbfTZjttX*+A=f-`wJ^4qh803Yj0cfNzFL`K41Mjv zVa1iaod+hDlF8D_1!Z2v0XgVRhp`zD8S!)0)#l7v;ky7KjRC-qs05ExNUH(yWs4MY zuGgE57qR$yE&fkK!mtJwuhF1v{C5u3x)sxB6W8Q1OFT{>#`!L(O?xap>8`3&MPQx< zXd3Vt!N6pD7Uj+1Y__Z?VKxjDBK6M72Zz<8@_4$@QJTI3vY{L&CY{%Hu9|TY z<)n2zyioGf4$*%qhSc&L5Vq&LdDH9z<`#~~%gY;Km%YfsdfpNgD3?zO*Fb;3(nHU< z=Z0K@yaSPW0=!+mSIpL%zi%7~#id{iudHIR?lt82V z$^}x`?1!BK`yY@+bO~`2g|$1zV1B{sCwM+5{C2N&L?gT#%<#Dij?>=uj_S@>HGzCe z)^lu+(Qz)mtj{|)CZ$;i-|Ae^Q|q4*qK}bv@RHQt{kXhDzR^Q-(wJ=Io*{JZ)W|Y` zW2n_Ovvz?T#}-W)fJTzr1~gVBU<&-SwW53-<^HP8@6aj}d1S5KidAc>S8sjWpCaNu z1PdsaYA1;?r?ULXpFeb0NiGf7ki%`(zGYcZ*hw{7@tAq~^ugvys7P7&RKUs=vi29d zKSwZ3wN+&b%0;oX7nH5bwsdRfMQLS)$sNORG9Deo7HH3TW?Zf4S7t@xj%+&AtP}a= z_39SFs_ExIkIPdgc%8o^?w0$nC-F-Jz8}6JG2NN$bnbHr=1v)VsC4}H^UkY20DML< zpZfFlR0`N{&Y!DE=msH!eg;t{&G=!o4e#UFR_>EG=-rC40T_EXnZ0 zcdJ4~B&)pDtULF*LE*mrjq^5Hl$FUyDI)U6FW)51gk8jdv#m>I^1+?rPje>ncgLWsB@QgZs&Y6_G;~?DqCJMW&nbVFNck9!|@a6KeY~|1? zRB?E#%Nv!b@6{Gr)c#XouvE@%+kENhvzWfSQpLfFR4->MUceuiVmiZXJQGo+pUP?x>02&rU`S8X5y19m1UfLR zhxmA*EIZkg?qWyn(1^w017?Mp3vQhwIdar1@v1IE;Ie)%A=`Sy+JbY($1`Wn#63Zf z2_QwqVQ2G)oHRSMW2Q~A0Z%Y|=U*GO9f^GkF&#D_LUxc@nS6DAYJF`Ak|)vPvi_^% zSHWqp%yBY0_j;2*M2ArYGl&25@62pF;$E^LGw2v&TqxueD!-Q#zP0(mtFn6Po}Z#J z9tOysr~v3^@J_8_nR07|GSNp#@MWfdX#naEtqG1@GtGd# zvIt#mEHe)vb$#Tqt2ej9*G2P(Sz$5cu4s3lv^?s!KIgx3%AEg@1X@R;3Ar?0IKLVgT0(x8Xw3&YLMmbG zpB+ggyq_beP%cwxGoYz-VBs#35tgG+0A5crq0GQA{~j~>uI*vbnqr+9s5>9?U@*hX zY<;>KMAK-LUY#J)mD$v7$v?1ZV!MufwDQ>rBiSWrTad+lT0wOCXZ$-8>M}}4zt6?^ z2aE-~Ct4XlPt4L8Owr=jd+!3?j_WeJSkc`bY^Dx5BVnb?e+#aE5j0SlkM`hP>UZp^ zq0{M2x7KD+W9sTkZ&z|Wxw2dHrYGdJXJRSuCq(7EwRWT_!wa*Z^z~$iA}{e^napR6 zwqiDmPqgs-dXqb@P};SjavgGdCC>GfF*Bkf(PT?Blv}{rYg|?C@yo`Toi@L!5>uCV z*DS~Nq_iC}zUkA^nO7-`3x=RpKk`wc;jd41-S)0*Z?S?!GHT(P^cy2H@Wua(F$$5& zyWJS@_Ktd2sj}ehPc)In)}&_dv?~7G7@NyRGWWXN|6<3gRi&46Q^i~e;>!IkAyl4h zXslrL3<9G63_@b-t5KAmeIL{2S8+-kD~*b%f4neI{^2liWFTKRCEU60U=e)ZF~A0k z;Gl@3oE%B-3}W(jbDIIlXU^5>Bam941#P~eGS5+t*s*W2*pr{T)@@P;?$iDGjS5rP z{xz&b#IeH23pNzg$ybFw1DQ|2+pA2nR2Y=>8JUDj8r5q!1|g4le!N(|H2W*y)6sOF z>h1Er5FfVbn0Hj1mnHLu*RC!OOwlCQfT%f_FAWo*u?V9l7Yb~LPlQ^B_N255u91Ca zZOokQwsi*7Ykp$8A-VkqUY~Yz0U??73!gFVG}w zuNKYQ?&V#`xh%_;vNNKLYEIT!+A_HCl4vcMTSuH!^pH-uRu>R{Dk}5UqxXI0N6psl zKRq+7sGt z1BfXoiF=8;!M9MN0T2pMG7ekoXqxWp639qLTax3)>#!UBL2_}(j)Xg zA&>dc|J3My;Jq?I@qA9R{?`){5t3+-fP%}1N;bi0a;m7Lm@fyp21jwb1Xlc>98dm& zc@uXx#u?hp7=%=65U`+{d>Mv@`kevrE>l=6+IRg}-~WH(X(F$w8!&){ircjzJo;fw zTTnmrx4jAKnsRWm-NzCC_Fw56SZh16dcP7)21oLg)0U3v(?X-_$Q;dJ3R?&TR^HJuRCTd(>Z1 zMo zc?mdZY-k`LAUG*WQDq%#pD(HBU6`+Le;B_X)NQTZygq)?x|6&$m9L)p zc7fgX+wV5APd)RW-aD`4-JkQkxTgoPAA;{^VG8LohoZy>x{Bc&yC09h!XL*B@2k(8 z$vvM2*xnPf0EU{+22#L%+tJq@+h`y7sh!aGLD%iv!VkAG+g->%X(Rq8ViN$*>m zfaF!49)sVGz|Yg@(HzC3-_L;8+2@p_?~m8btJjN}OA|Iy%WmX*V9s|ttetLfw|AfE z=h^4lYpKgt1>waLvf~j=2^*y&$h)wINR=@x6FsxBd-~mITE|Z zWa*W>M@I^yGET0ow@_twuAevBv#y)h>$W7PlO%U5d0kqcv!A|CsGmB5Z-wvRmsa6q z)amb}-?=nZwH~l0@_rBW$ca04JDV7L0^gsd0^cR|s+B2iRMWEnGhKl+O}ek5*2%P! z!4GVGKW)koUtb2n@OHNYLuDe61!ao~YQ#uWWglcV^r==Zb!hw-!zcY&Vr|{Ql}z?Y8e%EDu4;^JG;WpV!wLj@@@g%Wn1eRNjx! z>Z0B1eHv9y-A@La9$e3lLj)mG&`FmLrP1 z@~EU?4^>;*_w=>^^ULm}j`{b$-}jFRJ*CSJP6kZzwf@7iU85`Lz{k|O0tR(y!@Q5p zvyu*>=#&JhY*#7tqkHECyuZEt?^%Yc8qri;i8pL7*`@}C-YBqTrjCyYlIk8)xu8#2 z?#i2FsOt0z)~vTb+f6kn5n>NGIY7}`RZEg?J2I15NV9LkSt)|NBr{$L!Wq&9Y-T$7 z`W-tOnRT%!YjRK1+b|k_*ALHa+P!QkVu5__j|-K3E$Tf2Qsm?{Np1X5F>7;&3EliU z71?T~SBc;y<|ZzP`8mqm9JQst$4N@79x%2A76X+uod_aD_<tI5MsCX7ROa0~Ke zD}#8KmUc&NCE6^_bp_|6C7a^P=_aG#uBv~eh9Pxpa?c{3=0=KLb{8XO2?MRE*?ryd zQr)aK?lMXxIlt%zz}him#^>4rl7>T5(lPNMu64ZmdB`D@(O}nowd^ZT8}jFwU4{6T zjo;beG=>-X2KZN-2laNhmjd`}^Kj8vh89l`(^d&1O=H87um&O1K8Ob86vC%Ps9f?u z9rS~H?w6v@u3d9GvcGK1P7Q$lL8L>G#1&kQ zl`TU^7xOz^bWN`m8si0+k?`Eg#*3UVr=-M*VxGxvs|Zh<>gKtLHdAjGXtE|vqJNC$ z1AotdkSV{=?A>gJ6S$F)`q#$muG-n~dGXiriYXQE2Wy|nOVY0jPr~dOm(!eUPv@e5 zF)XiNIz(k6^Kw^EJ9eEGXv$Hvxmi2rNCuNuN`f|ZswpMYbZ#}z`B?NKC$@}R5yd^H zpZMG2Xdo6Q4^jS|_9&bn$zuhu>@>SqgVc)2N1*Hhm4YMto&)2g7gQPh6OjHxkz+g_ zrBt+}je>is_2V4v4mCR|;r1Nwz7mcSX5C2LHjlyQRt|k`l`2DN=bk5r_#K)=bpU_W z(;gC1fglM$tt)?6Ij@037%v}Nf9x?|EucI&i-S)ZQ@(IkVRjigT9~p*r!>`ik)JjQ zqVG?@4L$}yP$rId(FQKJ0S5FWL*g(aj1lJjsH{l{{{6SG02otA_bQ2X3t+Qymw@?lbj!tqRuBlp@fQBg9=kw4rW^1Xl+w z7WjO%FjXW-q+Bl`plKS`tP-9^<@g)wxdm`7V?~*P!lx6QB^rce!QA#!PQKz7hjn23 zt-NRX*lxWu?l;7R+K%h+qz{^h{&HbtNZOx#;_38k*L%FkKqG2WJf;_ zAOIfNtpnY|?5(7&M*0N}HRmx`dN;)jg@^+=eJJQxK#1a5@nLTv{uuPj>uT%Llwq9;r z>a-scU$HhJ7P^h~7Jcnc^;ztq!so6SZZCv`3HR-I2 z7+V;c#aKtlOAqx;6!##^smWua%%4v#4+B3?fr*-DO)wD!Z+;Ak=fKeXuC`cl6-j)a z!`o^<3j#opPG%-S2=|X#TyN$e(+^E%=9`lV<)ScJC`P1QQE0VwM0nYj5>lQ6;TM0# znkF*|irxFJ9|c5xDnm2k_JR&1klJ>@fQpU@8Q^7zlELGWDsgy_45MF$nssPz(p zph6agFjD>;=Qao9%UOg8;vJ@HH(v3rp zxk9w#X(wTWYTZwOnC!YR>sl}7Bizo2BD$@B?=&P~s2~G6TH%2!MhPql8lE)lPN`jH ze(5v@$Ff6wC`d11@lZ+CE@a1^vlrI}*^*$||LFIfr1nimokzZ<$7l_V?H^9p%-#9b zV2wp)aeOF;DQ-IQxVA*PB>cM*03>>z$$E%hDI}01MiMaYal|&L0tGq;vQ_?TkIa`9 z$X3S~YicN`ou+d@9j1MhoFtqK$)P96Y`$*AaU9db$zH-7D$syU2wLd4mN^4c#h?*e zvQgM{Fj0f_b<=V`w6 z_ihQ{5)*D5LyCF_D;KZ#%?MhAK*J$^>*Vo-uF?|_2+}U8R$jzSPv}0da(*qm-=YT8i{z1DWJFg8LiV_@ZDUVp6bKg*gx=tkY$90V3I6P1b+q%$h82x_-f^`30v6R6(F z9v``^fP66PoW8;U0#PQjZc(Y*#=oi9&EW%R_C@VnDmJ4(`ZxO6K2yRzVHzoe)dw{w zHzJc(Vk%fC(hura*3GzQh!~O#j$tICZq?vV8L{1&IDOM>MpiS9;8da#3lKd_UFKFS z3{GlLv4kyX=W>}*Lmmm*Qo-0bJ~u(1W<(7oM_?@yMOYL$1}pYkAU=`V9cZ%s@6>9n zV*Jz4A;Wd?wtysOb`DSNk<=)jOtR!~0itMWBM7equ+3S2WKRfjNWtWN!6-s>Li$e* z8BuUwn~zh#)~g&UOpC&=$??C*cR|t!@1Pc$Lq)Bd{blnF;UG(FQjoOPn#EcbAC(1Q zZGHZo;t)_6a<2E4l9Us?k4KR0$d$sLtKoH@vqWL17ZMYM3XlxFji!WuN=!#eH|ePm zQRXOUBTf^rb+PM=w2D0k?9${&jFly@wfwO$tEd>jq;{*Ag&!T7a=|8& zvn(;VL_qy>(EM@DJmlt~=;>{^F2%7l5D`^|c${HGIkyXm6qiTGY@KOtwxDcFYUW8g z*gm-m(KAVz{T2G4q|+_c($GRoFGbGNpEz0)2%(3M{701Fnz3sH1Cfti$J!P|_9z$E zSfvvIFZhtwtIi6SNF0uMu?3^IdW%nB7~`I7fB-dO#=vii-=x5eTgvO$VnB2O!eA0^ z*xk!;^j2=f&ImF6b;)iVYlZ7VNhJ}H*`SE6!`v96!>r-2dW2JwPSnZGBxa6bQsybv zqLaqBC_TeYoWeJgwuMA$&CR3v*oHp{n^5d_J=kC7yf{Z4{+{lZwz{!qT&1?4dB9O1Mr`(59c>`UkwQM?xnHcYcvT=~8xPtL9J7 zcc`A#HeN?o~ujb`9sV= z!s7#NM}uwNjdSJ6-pvMr9%fieLoJ^66Qvk%(y7-C{2jI9=evj7BVDydDwQ}#k!hc} zjVt3~jxT48IC`$hF5NU4fY=a<3kb`|ht3y!gCIQ}ncG&cPnWbb2WEr-)hPk0?@6iN zk;12NpB+;69_Ivo2`p0C-VN=)1di?>>=t*+_fjd{WI|aq(7Y=iFhO4IfJ2?p+`~mh zW#Xo@Y%)Ax3b2u5 z%S4s~65^*iu#E#oHp%W-_RWi-_KicjLDInIgKmMz-1AIXetSP6v5` zz^|z>#9v$``cOmER%8ho6Eju zcXF-=4%G29uf{}x`!Zy@gQ?Yh5Ws}Ib@TbaCHA^ZR)B0=&L(t$Wms!)O3~ZHe=M6u zBpSytmKNeVz-9eQ{$?lSO={4ntjbaTM_5EQwSib0FC`2t1KMnb9k!lx4`}a1$O4E& zFiK_rBX!*9Ym~_xoI3!s0Y6)Y051aObM-<5Np3U7{61X00BxoN9U9cj+eGR3+}PFo zfV>t=74#9ls#lm)rPyIxu8UO)Eh_fAx;lA*7Rn=jfoz;C^Csu_0AJkuZKNEKC=q6Lg%FOkbrYOo=ak7A(A>p0;q? zhn%mfMyT?ej>l84kroAsk{i*7L213gj_qO{RJ_`V-v~N{Qe*K5}_4_bikM@_WNnWBwvG8=1_qN(e5;eW8Djt z!E(Slj8w8(T&5$ov#=*6^L z#O4Lzuv%4_IEExF^F`_1hFCXgoD!fOc^^b6>u^XD1d<lzd^LEIKMN z!6?>j_ao&&E02Pj?7+Q&-TzfKg@yNC_(d-ed$%==j8drNf1WYUg-I9^ae4q=Z|an35o&=@`GZCSB@~HL ztCG}6x4po5F#KZ)X|GNmLWw1uWD;eHqj<+&a!!+}!4L4WK$hhuSU!Xl!T5_+$Q$mi z3d<=tT@s|G50;tTL3ny(OGsc zoM8XU%(0uS#8#*9cQaL?6Kb7>J~GGL6!wwqAL88j#1No2G0_mhwnu;T%-1M|^R}WO z5Ipz&hOnsJ)X?HzM))z48lPNA8lo+kENSGUAqaYVj!x!yyrW|9H6NNh&B;{hTC5ve zVmQpZw1|;OYr?%sW{}t-MTM}QK7aWW{mOHR;q-2K!UykWelr!;9nbv$Cx$gHs0#4K z_>fT^zx)zQj4d{bhOv-~Q5H5pZ6izHq!@KtL`29u*-q8o=2_P7*( zp7fOt689?vuE#todbGHOtXeTU&|w+*UfYS|mBmW;5)y~HZD1)8u!K}pTBO(?jHDS1 zj)ow)l&#@k-1EV}0eK+AfzXY^$AHxm!GT||3ahSX2wNLJT>!7cn*g7>XZIFHXa!_0 zNkd`;hdaw0ztW3o;P&~r4n03GbhTrn=J)Vj>{v{B~*@CbY{Zp#7SBV zNGL5582d{JOY|8MG)~M7<@|^ii2~p6F!Y(Q*1;R#RUN1-RJ*80TM3>VW}#bkhv_M| zJ{y^9bvd4-=%?ulN8fnnYa}s;bz9E2{oCJmecpCis|5CGwUzZ8K*sWB?g@(NRgwtk0BiLVwX5?h$hUOBR^4Ule>_1maa7mskaPD7Foz> zpvYrKFB?%rFU#96QHPYy6Tl@K>58S~1=0&}Gn9#EmjEUzDLF%riy)Y@+%H;+N&}~| zEXH=11bte`6-x9szZp2O;TyJDn7$adK3_+de~g#Xoh>O?_)wG3g$RWoJ4-i0qpt3# zpz|I3Sg@tZN$<#cDZrCGya-oZ9o4eM?tZ{zzU9RsTHp*ePzgj@AxO~JyiGjqXr?F) z5CghDI$*~X%f?~5c*FKY zf(I64sh%ut*kQu|GMu<=)(k2k;oF3o@2&Lim`ZGaj-F4sJ^aU@8_U zuk<0&24;fB$KWhpvAg)iq1h@$$6(P1+2QVnt3%q_%rTnP&$%GbUd=>jvCuKWSnSQl zswb1xmZ3ZF^k_#4ic^rY{iEbqy=Q?0g8k2T$VARV(r~(IU_Y|BYg4 zxjB*pq5uVX8^1j3mP)A|h}Vn&iTXK51Z1lfkAeLq>*<+~O& z8Slt~`-?~eGT8hfAKBUim4-9Ebg5p|wm8ut=m9OL*1Hxw^YhtkK2}3vV5FR6eiKU;J~H+tR5Fnyr2#+ZI&wXQ?(SHB*&6L)-3n6;eDGErKP zyA%=zAM1Nw$xca!`Bs1G5vXLm$v5n29z`dZ$xzDHmz?s;?*s2u1d($qt5$0p$mWMy#H>ms9f&79 z?#-Jwf$6|=S^0q68@zVEHX;Zh5?uW1!18W-(xD?@*a#yMV?%~srFTh^J77>mDF&zs zKC5DArk9vkaincd4I#c=xp?0R{>E|~pq{d+VEYaV;lhxz?c5C2nkmQ9Su(K>&Ps~c zUU=s4J^&Cm?{6x| z{mcb1l3eKe#ZC#8Xzq6UccYL!yDXmklyrIl9pfuhO~x5(U$j5hze+A+fjZ)rE;-4V zO-;WSbN{w*Ht9^obVP(S7H;fug7mjY_2!~IEy`L+6CxUIgv=xY)-qg(uI+P>o@k+k zikM{n5E`aH&YQv}+u_>)KoAODdNWyMIwrzW=xtvy(jy{%>(#!&)EF#LUau)^0Oa%& z^z4L*6-Ef-x5RDt>Ao9j2!6JSii9WJ;Jhrhgj(EL@PpeXzZ$f%j?l)<;ZK2Vru^1A z_Z{pt5$9DHv`g{v6y7~BHWgutbXxCR=wUlLYa;Ra2AM@7JXgMWU6yIQ6wXu1@gS!8 zj)}Bsh|bZ5^gkg_26Q>+6x^Z@)c+mx3;l3I?jPqn*`3ZS@ScCv_ywY+p2 zac~n%-)MdbTeLyaTmc17Hf+{9%Nfz}H|dN%{jxfXi>DItX+v=kaVZFCqCB^iF5&_k zVfUV+wj#xv2jrD{;=V|)B2c;qT<(@*VGE_lB^ARw1viW%8*cIV-Xk*WXfYd`X*OdR zDM^$rcHuao(Z!sg)clLc17^+(#eFP!)+q3l!o zXmYjgNR&A`Fe9^6P7n{*M-qWkw)yl|#AS6{E9i`pVI2$_`klYb$6_IS*(#IHc^Hm- zTxB@}EB*Qu1?Sb>C-T73Ne(f9Wu=Vv799$6B}@yvxw;1FR+4|&=Y9`6)h`K8FoXBJ z2oAmpL>h~Qwxf7bZg*Tg1W5fH<)cjQO%A?yKBkSD&0A!l{&4lRnY; zOEJu8%Q%pj9&ygcojTY_;=Cp#&QwTi#VL0 zqkmn&v`$jgvzM;d6BOiNmfx^HVEzqTn516ov_L?>!4@JSic%sX|DRLyk4cm5lfWlA zB#1kruhK${7UqiRFs=wT5Lg~wq+S4tCR{UL88lHsAeOhb#o02_N_6V7q5*n@6xbEyL6$8aUH7>&#eq`dEEL&Cm>hWX+9}~*=jM=mjWGZ+*Jx^i6*i+#WOyjW9m$#-x}VDFvGk?L?0h-8eHdY-V$_g3I_yGAV? z^CfW*D*s``fj`uLo_@ZZYxbq#fK}dqkHx;E?K6d&@k`3Hreq}6zt?7Ozq5qOtN#do z{>ON{^io@F6j3k` z6qElL2rQ=GYBWFX0~YTnOEer{nAty=-H1QvEm)A5c?hxzQwQ8QAQ%*Ln6M~B%p63! z#x`bo`~2uhr;?dve}~2IPx$nBb+zY*vf9a-zH`rHeZHN(f6fOXPcIx#9M zH7m6w>oTY;<3T@>|E2vdHS^a-4#}0(mx3Y`u4*D#_E$h8fDspdTdff_z@FIQ(~Mm=zb(FuK@Q+6(WuSuYQF5d5f~+^aW*Va{ ze_y%Kq+e6sxG?LTU*U;Bu4^C8-Fy>L;I9{2tCwdnVFwdK5lMaUi6fIheSV`592|1+ zCwy>3f2;{H4l8>lxbDQczoarRBS+pP&x!9mY;ri7Cu$Oq1(K1H5gMKpbbTK#K1*K z2>gI`U&Q$)If6>KfMxPx@Vegw!JebV1{YZm!`0^Kj@>$slv`tJB#ezxba;F@@Vf64 z?YvwI9KiJMV`*uu#R|Et+T)IGj4>bBGKT|!KpK~YsfZ1H(TI=>VOeq7ZL3 z(z-pr$Zb?FNe2TmLD}5J{jJ#Tj1P((uN=e_X`#BzbKNxAoq#4sF;~T}m@{3IWH5H^ zdRSa7y5x(md^IFpQVm7OG(pDN@T(7I1GM=k`zd5yh|&CtTLi*DR9T_MHgG6fw0T0(>CE# zNt?A>!G&WObn~E?WGem!B{aV_dh=>(D6`7`qW;%NkLes+cwQisR*D-%JuI&whExh^ zq?{iQNP%^GBt$F4F+(CN4G;AP9ht6QrhfTV==%Jg>Lm0=MV)LSMG2rPb;4KVC(WMX z4#;x?_4kLKh<-q6!aX@hdPNV}FlOfyHP#rziQ9Ah=bziqM;(3;J&s>(9M8quFwSd@ zm~0&wIlJ6q%4YdAk%J#qP60;ZLJ-SANq_E5;!dvP{G!de_n;I9yngn2J_i|kLU`75<5EMgFfVNU_~;K8!R7Gngl$iZI)II#?Rsx$RkOt& zz@P&i9U$CwFnn*Z{+uc0zP1-3{@u`jwDvvQwwtiMN0PL^QP>r^bNK7-5@^Q%^RVvz z*HDwliV;K+7_JHw)(lngqNm@M)rSaM6lHc5NujU0KgYh8R%6|F?9 zT;2O8xA4p5M}qNUYXM!ZyOi-ayK%+V2gCF1YBjuaT;Ep;w-%PALyG+sr#HKi$6b|6gZal}THw}M!J#-)^TqN-!Tad5euDzUS}pY%!O!z^d^m+iU{#C<=J z@Lu8M0pskf#vX7&h{tAs(*%miLR&VowUbuIkV+?}#X9MAbwbuw3~EXe*QAj2k*6Y_ z!BPn?&chrd)%zpTPZKS;cD;}_mdyko;+m~A+pzYZI{QTlH_!OIvOcw9K?f(#FXvZe ztcspT>r~iOEN^o}6thW@l+J*qF*a;x0Z0r$S)odV5>T1MBsc2UOYV7YVp9e8mm8W1 zv{+YC^c+1rzO^QnPtCXzt@E(j&|iLip`YHgLCYwvB}9mVYZsF9Ssf4j86Vermaw3^ zq3$%eOboKXbbgMw|NRk2vgssO??j-$kDFbkzt!lo;G7N80q*`Uai8P*s);gAwp!f} zy2duOuf?kyk}F~jgVGG*&3U2kzSS@sjeT#=4IRV^AvyXMs9}a>QjU&`XYZp@vW^5R zs(RcogNxtl@7$`}Cg`}Ip9tW+b;q;0HF8q^%g5P1G>ENi>D)o$tBd5D)G;SDndkfs zzk0NLJhI5`wO=GRr}f`nSTX)CMdfxHc&N2bWiR#7CWw_OnTBeThDoNSo6iBY204;4 z{GrT2Dzk#h;xTPwi?Pd$0?Nwl7p0H%w9hKA7TQq{GH4>iZ?HaXj9QuNylESeiJ#is zHo4BU{<+Y{cv0{9N{yr(2o69qw}db~M@SEHnkE-G+m~_UuPLF5ja0N&uhOW!MRMxhrsSzN9M2$>T zN6b}{Yce$UW*(iAI~PoM8oRYt4}_YYVneEQGdHa|c~a9_scwvx#LM2$lHGcVvM7%PTvD zsM=3&_jN=*?=Pp9@xCOx+oNMD_x~wpiv5=(<8Vnz3L1G*gn;wEK9h`0n;bnfB@Mj5 zgPn@`z#W1&qWA&sHdry{R*hSg>7`X0EGOL0o7_)E%K~=T`OxRa%@@{z7rS92sqNGY z%fDqm(l@vZG_8^wJDeR)s+g{&Jsy`9$wl!?{MRyWC;_S7|Ztm;ByWr)>e%tqT>Ew#u0tP@e1VEY5gk0kjo@K*yP?gOjA zd)PTL1nASQ`=XMQ^nOn9CjuB9uj+Rf{bzBE-jSpsE>9KU&A1s{lx)d00Z3%Q{+2?_ zo=KAfOpXx*9|!eL-Z?Bh8Jx3fJ(8I-i}wF%V;0Pg`=t$@FIaGi^}9ajK}~KNR(0D~ z3j~!+)ct+U7o_fEhrnN92f}4qT6%)26n2U#4>W%w_8884|Cis)5Q;s@?ibhrK}`THT>~(=8Ib zyi6$$WdWINbDMfR-ZYE2BH)?VOfi6G@9HaxydnKkX}%=CTk}{YOAw6;pF0gF)KydZ@)Q{wXDVoz^1pR{H%7cq4hkprdO^!zq4shv zcx?!FCvLuPIgG%>>0}BlfBzH3prwfp$Jc{h0qw#U$Xo!89b#;wRojZM<+iiWVh-vz@;XHcowwX_#2i}zSb?ul56m4<0xW_iU8M?#Ya4L|U zv9gpy@{~kh{#?n%nrVnkj`B6*?sff>rTS-g=syeNo=jd&nvNXQIDNsRDg12O?NRi8 ze0bb!?Mz75pfZ(lp0jEZeAo2tr4AgldmLbb&_RM14>PJJt2bI!ea_bPbkTjRs7VJL z&dU849ehnPrh>ZS9{s)qt2%a>XxW_aR8131FHj|u;Pgr%p;p2zJNLBZA`2H+HnbgM z`0^4>M%y3uab;?p?Izjg|Lip6Wn6@CK~E4oTfbh>2fLYRS&9hO%$#OiPNQ7z>s!A7 z3*_L=Is)lWYRhWykcUc5iV>&@fe-_WND(_poY>5V9Bv(^0KH zSX^~%r@{<6$mWBTl8{7Hme85u$Ytu;vkLW+;tBcc;hl8bm%X~cSF$vrbQ?QTx#-n6 zkZE@rdT7E!CKq%6Ox;`HihDwQ^MN~dw{XO%F@>0zu~Z{H`P824NTcwbd2)LSfp4rF zO&JA^+fDbH@!H80gVFn_1(DOM4>aMrPi@Q2D#H)D5^iwA$r zSt~{T)~*}x9DS{W>DXMG%tsG~G<*VDA!Smbv1+Py3R+oI;F8#$b450Fa*Z%1m1xS_ zhs%V)iNyfU{~|N4*_ut6gx9|`WX+=uk{0jCSNqhDr~mEDk0mf$%HLnV4!S3ycQ+J0 z1vTRmfl@`m)#4ht9zlUaXUDUR1KMPTCHIP zT5p%W4YtYA);i7Ct0VlnDWgGSE4)|)8krPvFm7m;a^W)20hA5`L7D7ZM}`RnQhcqcuYwd;ccFj@;V1C4H&KHv7lIbPXFG}TsV1n;mB{TF%6 z_%=+h*CCQVYhQ@KGm{b{YL($^4V~^OO-Ip1#H|n69)z>nSkO1GOJYqY9@=Fi@7dup zo;Hof{L#(&3U)ww6kwNrj{i{+G!?V(x2&+=G`Z?pPKbSTiPaHH^6_CY;lQ)8ldaXs zbi@YKaaP!%!4441#UyRQ%i28_{{@x}yNl{hgZ=;Iv=$iv6B#QdtYy2+c$M$<*y|}l zH=Z+$Fo+nj?Y};Dl|6N~Ftni+V<5C9$q((8T-jjQ?w!*2SKJRTZ?b+>QNnbN>VHNZ z+fVSD#%eSE**PI52be&OJpFNB<6wr*8#BFY;~Gh7zpAYRi*+n;*)hqK*Gx@YGUWfY zbEK4XgFJAg^A|+uuU!3>wP8mt%3|sM>?*AlM0h%qs!&KXCiywYY4^!7%(iYZDP=%# zVq_pDAO9j2)8me}y5~Kfu@(^OD*CNQPt^hhFIM&;Bj@PYivbFmeIAQ93?E<0#!+ zFUlh;M={zwoOx&c@q+~04NXjNWFjpwzQBR5VK>CJKjfimA&g6#sHw*QT6Nm!d=rn} zuVTH)ar#brC{n0&RZ{3^|0g)@CB#*elm%Qf{aIJ*m%Y31Yt9du(z8X$_Qu)&3@Gw( z=gN_0RhG<%X@8oLH`kN*3hMnhM5q`F03@3bvcZAC4R_{lElL51|2*V=yv}fZf#Z~| zxu59xW*zTj99A`dcw+?pS4s6mnu4fWhiYkZkzdBV9R-_s50oLhz4&6okR`@M`ns(C0e8$ zR-6xy5By!(;~2Ira;bpX1T~y@Bb0+QTa$gXqeIw~z(LdJGvJ*4CE9oxv-jUU#Hw)a zAf--c%LlFuWOyAkyW33jW%=X9KpG?`DJMr2%5Jptvm=nZRfZCPq*?}zug{lzvB;*I z1pm;g{|#qOxH9GH=@|Mym{40Pwj6J)BIcLKu8<;~Rad$6#p$K5?TD_GSa`das*xjR zKqgDNw$3cFki8=qBRaYB%Z6;THz{o-JaTb3Jb16?LU*(GtHNyj!1H;I*(9hKD^oBi zk=8Jg!bxoeot?hJb!~cmMM17&bh?@-;~7T~N7(>yzO^m?z07!9sdgnmhM zAEy7s?hO#k@IxRt+2ic`ljp$PWg`Mfv6J_wzpzGQN6SG$aMpqX zD7KQ6VDb<5qfJq$L?l+kt-P)cGk9U@A*T!mIcx2F#vBupD`RUWmPElMWVvWnbVyW5 z*mB5BSQN;Dg5i0{iU@y1<$OtD)bLFY9>@@(e@ypcn_Vlf`@ff#Rre#;(DbTr>9UrZ zILa84!-Ud=WMaZnN~y~YMCihrSovyFsEuXvW_`+9?*j1rl0u zU!EYX3?~E{YMc-jUud34$Lv`@KACRb->&qaYGh<_@GkUm&VRo8AijIes*Hue*9} z^;jJ3eSH6O1img)Xw8$xKI0`b_t93LX8lC=16>_n=VMeqK>~-rTVrH&1~#~5!gx57 zLKDVLfvJt&fU|p-gAwyKxXxB7*A}KIzy_nqA!KFo$p!*gA~tt5rSbqb+K4C>RL$Kq zVnCrkYh-nBpcJO!STNauXoTh4DgV?v$q?e!mbmml*V&3Ja*6JiKgk7a3L-6G!7$`7 zqzg^iFp!*@pdvQxZ(%s%3Od5}*FQQ!6eX_HQ02;&zvGDu=uoLYy31QHXtINP&Kz*_ z`f^~(EU*L3s_f}6&e@#$nL!qSg8GyCp8fXO7~Y|KPjz{X_QK=016vBFJ4;^cFN-7fsVL=L0t<}I5l0|ubKm%qG)y%C>ZF|_wRnT)U z>Y0F-TFYInV)zYmc(jPiY0m#ESq{%Lw7C{_0ey!y+mn}KN{B^eY_9YVqm5g2Fo>JG zAl`Ef-@D$20=+3^ye~J7oG}sqkEeHxuIzcfhbNqHCUz#aZDV5F){QfO)AnmTe3aPC-M-d2kETyt+29yM?;&~;Fp!-hgu8p?bWLUOQHyyys3=! z_+Ff8rX^M`lEsTQiW=t5VWot77`zT#JyR^6i2x5rtESA{1761Fh^iI-?|*wg3i`(s3k7xoJO5%kW*fBz%SXNYb!yVWODfXh=E4wI2!|N$;PukFdK@m0t#uE zvSQhLnD&k(ft7?nuz!VUE(c2}&!>{cNzzn^oE3L^irUvHCyEz=q_d2nrC1597`xxI zlH@|qbGk{_(UJV5?HVXr?#yg|w@;#p3F)qEBTXmaK5WU!Kv;zHzHoK!l6{0JL&CvB zvYz=7zdrfCIoay)49otEty8NOW)_qzM)(!%ytTV}df$1FvJGA0$?ZQ}?AZE0E^x^& zjL^ocWaxnKAUVwxyn(cQDZqj6_|KT^kEh=hkLo)&j>`jZG(tz^MlZ^>u~F~ z*PNi}F%<%$R3T=P;9gNzp2To!XXMgIqZ6h%zrp|^-CAy{RV++AoFijnom$1OY7|fd z?O$lHf&&~Vv4VR!(PSEPmjkpQ6cZPV^D_BBfe)3s&cEDS&dtiAZB5eY7fd<`rfowX z0vfCxBjx~au&)M5igM3M@3BaRJy(-UOgXx^s6+22%Q~9|vzQ%9eLpZvW=uQsVK6``Pq`_)B zPJau%-ys3LT!7qr)TP8zESqa1-!rRgpG&K8)7#UE%`xLo9O4|}ZPpkW2hyRD9~T32 z{3Uyb!&kglXs~UQgT!HyYPtBMCHrarKDW?Fy~q-Eu!T={UO9@YfTIx8Hi^#AX*D8a-`pBzr2v~{JHy*aNs9RG<>cGc;Zg}aGKHjZq|~OgdYWA4G-RjRFRNy;=lmV zd!GL4Q;>_0{|M^oRS>_gHug)Ga})-J&s1x^GjhDNO{tRh8Yq12}_VJRz=z)N4)H6Ir>dwE%N{50|ZF(iUY zP1vQiH;;g|$DEp@?ENmh_5-A-7Ps33f*D-jGf*zra4)PIsHH2K$JQ4m$9vZ>`);4% zkjwQgs}h~62XXu_3+ku{?6y3Qj^KCoi4{I`{({VOadBY#beoAk@f$rQSoky8YpA=%a{QQwlCVuIWN9*!f29 z_2Ri4>F+KA)n0auMITf5vCYs)zv>jAtwOZky)QfQW9To^Q;#gracW^DM1YGtgk?x% z1Z4o@UZt2pDQZzvCEW0${EK_h#8rBhwh7$jV_Gs?Qb|hKx>sc=VsT|iaxuvQxcr3< zSwSTdNq!lObx92MDEQS{t`}=uexCYOmAFDeMHH%VEg^uP5;zlhF`5!`VJ-rh7)%f` zffl6jXiZ8UsfExiN|7MtY%l~oTAcx>nl0Y<)_b?Ux&x>c-u%(h-PXiRT6NAJ(7bAu z*BW)>;kEr^jHGHbv*H4OMRHs4^}Y^$&U#y>QbpRzfO94UwW@h^hRnk$p8v-Cd&ASA zx9_)yt*P~?pSNcfy@m$(mo)@N%+^@Juz17-XT4G{*k5A%a6g7{vEHU&P$1{l^kxd$ z{vm@{yfs%pJ2oFH2b`g$76|7BU!i4RDZuYE{IpfFOTb7w$Lb|x5B!m)DRYD_XbGCOl7um}+AD9Y8sIYmh*3bEu z)j#ymyZ?ChM2D&|EI%}dNTH!x-oI{n>w9pdO8Qk98cg7*KuU|(>zT1~wYp>(xpQ!? z!bwTDVW?n(Q;CJa4q}a@BY~Iv;F@ax_E_!v_NF36sFhzSrKH;;ThiuoVau|t+5@O% znFLPQN_eo`{LRo|f0E0BDNa7-^(42X_~tietV`&4wV4%OORWXu4tNgd-1#o*>r@LP1^hAP)KyRpDg$P`}F*7GTZ(dMe)q z=6+4e48sWHwJ_0x`0eK@7Oc>QyG-1o3B(^08J4tMpv%SH_RBvsPUGLK`1F~17)_c( ze+n({rRm>V>)rRfis-#Jw}^20!JvVYEfUQKNi3!|5vkgl<1}FYdHcBFx3#g|Kk$82 zynNMmZPH5U1~f`{e&(JRQ{=jLBjzQ%bsfFp>QbqPm}<4>=Akf6QpwW_GwWj4MixCq zPra+I$TFmocZ^6dl}Lb!wmLH*ygPJs5s|zA{il?mLV3IO>}knu4&B^kR?W zwSF9}v#d>Ogn0=tz+j|{qqG2~{q&oqrjM$ptX-dlB!@*7A2%EUuedA3BNz)E(e+E>$gTGI9xIgeV(h^3-$Ns-S`N`>5CSQYiNB_>+%o~sUhYMbbn1}Fn z^vEMf;8kk-VfkYa^EJ>865B6nL;mVLel)-irF3sf;N7Wn_Zf6oa`pTr=cTiRxwT6e zUK^>wdjRx_N*?B*Ncw&$at=5)Xk-x8e9L2l77H#;(PNBL;0dkQxG?_;A^XHaJN1XP zgB#y9q!EWSPNWx_`^|_&+wH?qWJ}I-!l_KZn+YX`&uX(Le!@Uzm_!(vzQACXMIsBJ zH;j)fRGi~gD=VdrX}hb%jeYpX`JjqPjFJck62`{A#{itlNLG~eI`HGUNdmF3`J1}- zfSEouO0z7wFba{B9BQgoa0e~b@T&aN`bXED@b4&{KyrJxLJ2fdSR!mFaYab-gb*P` zAUP^DvnYjssg#PyT$$k%t-~*AEOsdgE)^D1@AXuU>9D?$viBf+u-i*|bo>BX|VFg8jw z+fB#JE}EUN$MVB}^vnWVOd73DqBZXe{9d=`>hpns*^%bV*-}YdTch$qv<*1U<0_9p zzhYO&0N#;?d(=bEh5Tc_oiDvcs)tn|P5r+hJG5TMm{akqJwZl5(8}eK`AzyIMNQtq zW>cgG&QlK6?RcqR!Vz8cHz4Hz7$UVYQh;{iXpGU>u*= z=Os8Z|2n~S2ZEgSAWY4F_9RUNf-<31Ottd*H0M=vxNmhHrwLs4r3Sn929Exk9pc1DkmL6oa9I*T0Hf=PZ$&E6)?*TxA_D@eq|JVFF$ zR_CmRU1SM%GYv{vD;P@I2sEwRTY?v4(S_S}Z}>nj?lZc#jUqg3psdteVkOU^#@0=} zXU}nk>Ua{HhCT^W1M}wk?>LspbYXVN=!|r9xN?}DSL4q^hd;Fj(qP48DuaQJDo!Sw z3PpmZ{zA%(5>Ra7=UYK2$oo2g^ws9)aIqj_xIl3(=6XxH&BbDdC@WDmaZ}L=^mO8S zwkT2@plE<5yM0rnf&`qBf&@e>ibKv6f;kqRxwMa6gv^$ErSk()uhKYlp_;;JY;Lun zd3y14qUBrb6Smm&<6Fawwxi3L|2$%LSroZ>3<^)q-}7S<2mf9Ndf4pz*-!k$<(J{1 zf!#k>(D~~85h3s~AaFRXZR_7oY~f$T-XNq;W%|kSzZ2Q=czgwTgc;TiTQKi`N~(_+ z`aWOv@Bh>Y2P#;K$;9UxV!ChO_kQ|M^>`O@+hbsQ9+s*VrE;Ja{RtAOKU*qQ2oe+! zs}IF$*i1nsi1?V9vMV^0UYJweo(X}(YV4*dL`bQTCbZ#~CwapeYDO_PFH;yV9{KNF zlSNj%aI(PY;;A4u>G0Ov zN!vebbY)3~xQ7IiYiP#ORNt*OlFoEf?NUfMtoW5$3{O#Z(wmUkz~^Af1ZnF%zpn-w z=c^y=?=E#{m@RUBuqy=47fzj8+ibP1?GQe&v34XJqm;DeoNa<}jFMO~Xf&0AtL zy+tQjDLNL3LBV+R3{@Xfe=>spb}%$M0J?vd-FxNg;C)uTI=Cd{>3-JxO=DBQAs7WD z!DYX=&i>?|DEN4b<$7-EhK$sFRGSM%Ff_dbIwe2%OO8O|9Vs(@@S5((lJGRHG1-Cb>nSWD(QJNyn~WF%+eFwa|FA zk`|p!v?GKQDqEPsfcVEP<0P9-TNvMKeqQ1Zv2QK8c<0I7r`il>yYRKWq>tt?@QCa9 z(*+x$me0w=mhlCqX`YSd>=t|e7!e_O1gLQk1XhWnlUome6_V4Ry)`j&g3>(J?X&rW z?wCvzhH1m;ff^hS&Iq*4b`-tsT{A8D%4LdJ;admwvs|znFEPf62-qa z7mryEagLZ~NlJ~8BLsR+zxz1JoXi!3BNu|aPEyh_fsBpmYcQG7i1EQZ!9>HZPje06j>fYR_;Pxu;iw;vmayE{h+79}z$#cCp7`^muU?)iGMTeKXhOi{gvOk5Z2e_L*pUDG zkz=MbcWegy_B6jn$$uVF-4LYF5v#7SccTA9%zq28JNoTmq!5es$8^?3|Iy#|jmgga zzUKPXn8Ab+VjRgLJs=OU&GHD#&TZ(-{SDp+Is_PP9(-Qw`p@mEF_!&tIa*dJiGL2O zy69mRNX1i&v!60CwC-U+=88^|G@Hus872Q0ds1a}1h(ev-&2LF{`}OLb zkHP&!dgjqN;^fshOUJBmVKuaZnG zUT)y+&^}$~{5i4KPj1Eywnn>7Cg0J(Ew<+0k0fpLV_S4SEY(QTF-Vg+-wYxrn4ZrF zcdm)ie1KIh1Cft z8DhzPKHPk{LHk3IQid@&o}5~hwKn}4CX^cCK6HJt*#jByahaEXwr;v^lg*YF=6WR!EdcB}eX*N<>i@iPavjIx^ z!TD2{sa*#z`p%7n!HC0pm%&SpMMfe|BJWXT_C&1jg23^~GeX1|OkoLJTJPSk)QxXD z!}b+MrksUk6qNnqP1A}z{Rb1;kB>Y8lL_5_%c^uTF@VTp>|?9{(FC|T7wbW|{&sCQ z(8O!@OK-Xx1%$ZG?K{zUdrhTtCqC6+6zfhn&A7eXhyWq>xM=?wJttsj1sKH&_4AFk zQ+Xj&lhVW437lMtrghT2eC{Q;rKJnJ}2 zw9;^jsjSw~!(liN0=I+$FLwG)Vb(X&?;Dre zVnP;>=zTy&7znj9^ccjL8|9j3DxX_`-gh!(k`?O|j)V-?_`!b^gfuO~U$OqCzo{!) znU)u;Pp`1{rE;ubR#}$8XhL)==MjRYZF&nLP@8Tq(5`mb$K8h08OA~I?LV6?LK5jC zRY-{jfIYDNZ!MeS%J=MQH3ZC^AXNJ+CJ8bWN4ki+@+I1a-}R=)c-o6&=~+x8Guy^Q zH1V)^&qDeiHITNu>9}qUa-~YM)?(wrc{+QG$5;)ccioTfn~n>pj|&361UbGdEKkRn zzAOaKv-;OX`gg?5axvjdM#&W$=PGYy@T^isq>>g^FziAJrUzgt;}A_XRx{#MjI-w= zv*-$qC)o8B-?ik{QflgD4ym>h;n@~Tp-7=?!7E3RRoLy4Q5KeSleK^0bT(acqMB<4 zM(Q0gF_BVYjEU1q)dq+XM@&wqK;oENlF6=b*5~O`9vcS_VIby2O4~7jo98TDuU9Mx z{T*i_FEgYaFGNTr4{E%FNwS1dzJw=CT=F9)1NP+^MK}FKtg}8p_4JN_1tj#IVzG<& z6I#k4TjY{kXp_-sUIxu~(@PIVT!bT=P9N~484w3S(v%q-sO(x)!m0{Q1qA^`VWFY= zlWmxQz#FL{%QC76;GVq%%=(KsSrc?$);OSAhP?3W4~x*@CB>k--KC^Y*?ld;bIb>_ z`?JzkPO7A=GjjHesREYD^;65;8ZPuEYbo|n!V?{=+5-2{m~+gQBP!qkY@RexkQqiy z5(^#gTc~C%?(5Npz~Dmn=}9h^Eb1UF2S}Gt7fSn|2A@_XqhKLv!(-CfE?8ORyu@|z zxwlf~a?l;~CTXpsCQ8zjcP3(MUFG6CIL}mjpsN2at?&zjUsGv&grv2a^b?~5lrvL; zn-VeF5{iGu8PKLX=Fao2?-+%^tApN0gTN53&wj~M3B#M(`v|5lpZ@IL(54 zbhFMPV{ZnIvaX+QnM~jIYaq@?ml~j)=&a_6Y#Dh0O$$4Lp!$~R5n^DqP}(4eYzqUb zVVKtYt_uTbW)}KQoKK7Ut({wJVL}=n_Pss)7fz|!1a1ommX{@%EaFqUr;G#x^kKyW zEh#VxQGJoEll5iV=u~0)d9awzidIW!89cS5hT2fnv>ZSd!4p5q=CYV(Z#?=H(5IF3(!x(fw1=jO?%5nJYI7Qo~gt@FE zR#drntpNkm#8#?e)2F0qgtDF;We>`3*uC4vAVNglldXAlko$(d02@T^iD4-ej$FC7 z*OYGO@mUU!AQ-w9#&)^m$?;b#?4rH<90yufgeqXP2>G1eBF4JFBBAa0pMEoA%_25E z1F^(a)9jMTm*-{WD_3N)lv#I3>2>t+7qRoh#WiMnt=8`GDOG?BY6KTl zf-j7az5tnQhZBe5+`0*Dl{d!!JhTbG7wTMQiG#}`{ixiVMT$u@oi3w{qt$JdSkllgpfMmVt6*{ehbz6261$vZ;vVP9-;1VI|ysb z??J+$?-}KJ)g2n0QEg7Tc8%d1fGiXFFDr;5#459&O)=(sp!HEV+5J*$A^Z_(e6U=54=!VdR z%z~_iF@=d{WIt=-*n80n{^|K{UCs0~{!VQg+S&*2FIS)tT-)nm`)aKtWeRtzYTgX| zF?i0un;DPkGjAQQQ+6NUcyH)0wVuGYAD45A8=ch-{NfU0Uqip=Jz!)~$KEAk@ zS$Pk8=GjZ{sa^dpYv&f<>$vZ0>)-XWR0~2hFu-&tax7JT(QHtN?3R(z@?y!zYC$+| zO~)P(YAozmlS2@>7%5sRbUr0HcLl+cRk}?dVS$;AR)4b(R=9*vzQH}3b(j(Y4g1K! zU2A*hw!JZn5)u_y1w~^joQ=wTA9H8UM>z$!O+oTGc2}26=H6BFEB@On z_j2s66^m}sav6UXNKBS38Xl8MH#kwxjPc@+7S)-ba>a1_Y^DE-zAs-+HGdX7@L~`2 z`<&sE1%s3&D#2>Z47eI%aEFa16D~kd4n7HDc)n!-J?y;OcApOE&)n(%0f(*tL=BTv zN+PH)Tcppu$_!YCBs5Q}>e@14M?DSxUf}JwwC%XO(P7~oBxS2q(5-)ALC%LUhJ+>C zn}xPC4Hgd;N8hhZUiLB(VV@G%c+{A@{_QVh)IpBml^7)y?arirQuvE&|IYniMHxI=bmD6-lzna(xb zu#%fho;0QU7EK11?F1LCsV72rucUeDvVE@iml7U|UpQdifacgYVSU{hm>d)|qqHem zW9K}>7%w3sVHlK_KS}(?Esv+kXac8zrA#C-dLmf$;OT1S%Ef&lJ8TqO9fr(?TOm?& ztPcXDBcXuV?5w=S6lLZv?9;+o%Q?@+w6QE-1{x_CI>5dyD~LY>~sI)U!5q5 zBs3GZkzx*Lw^j~WchkG38tJlil679r&F@O=o=h*bc;Mt)oyi5ie_>&z8x83~AQCk| zu={9pb#!Zd7@|b{zgmE4)T?%gam{ou_43c^oAaY2_0=q%-r;QEAk ztG|3(?w{3v^_9LdKt`rRaAi;_Wx`mH338IXGrvg2$H}?xr@JH&ZX&v&%b<)!nE@t; z4YDLr8cW@_;q`l;)oYBv^D40lMXjvFGqg)XG^5ENNRfxa`c})uGZ#^L(|;T{OhIhE zFH$s6uk?3?3kMrGVvpk;J?l%tV^!8_Vz8Dk=I44ttZcHvX#nmyWQZk6@~AW#fIQ!J zCfB!ercRd6U(per$&T}Xbg=$ z071jhXI&?XiF0ktR|yy2D_i)JY)P=j!*8ydYi3iX3lfEz*e$krC3x{o1Lo_UYptF9~kUH*f0Y8LStQ*#+25U zff~c6g!pVTQbIHsgP8}ITA&ain*{h+KXYo|s2?d3ih+tSM7oumvYGf`7wG)`Ad^Sb zM6}!MskmGnRN+i>XDYhsd9cZHdF8bOZMmWFyT)K0nV0pA9#^u4VP(}c-y&7Bf~;fz z(1a~?(-Bc%a*e!10 z%Pm@rG^A@S*6Zj$bK9TXbn3~9k9U~mb#&G$g%j%w%y};ByshX7v2))7#9c74I^f$9 z6*3kgI0}3>JB4@q@O`Fqo!H6*CU{*7ysI5Wl~f0q zaK~;W-c3C@Q*r)a>fZX5oz3L(GvDpEP8_g4$+OiZpOhpZAds&4X`@0p6N5ERZlmC# zr!2axDMliN^J2;K^aZb$IU8YC2NE~EVje$W*FK7z#423Ta)@bbot$$>CE>S-0(>bq z_&3yxRqrN0nLQon_)xjdoEF;;_InVF_UA$Kt_!ZAufYkCbj?$Wx|YQ0^j6)D2263d zy=e*-{)BG1au2l0)?=jWbx%biH~El?;3quA55|VI34PVMA}FjXt!D7BB(!vNr(%dO z-8X^N(#v?EUj1g$BS_y2gP4WOIu+c1Dq^Ck2{OXbT(e+FiiPKH+Y4Pcjm-XELUvPB9|zAAvtY_q*A89i~h)L zV*5E;-97m7SM+T1JuD$%T@%RY?}<0H%YEVL-JYFxb&dd= zjPYC-73dsOE$yvPbEL(Oqe@?158{(Et%|6-s3}CG)$sKrBrfY=%Z&SF-fTv(bPT(u zB+oVq;o{CG`M>GTX)*__MMXtPS0O6dGtdXl>nu=h`U>n^XVvU~e?Q1DbU{AlQgKk$ z??JYa?XB_GfA7u#P42T&{EfLe^?hN<{Ad^f>L$1VmOlrHio!)lT=&vBd4l~ReeZzkYPg1WuJdED&7BrhzY zv^JaFzeHCcRq**AImItGeiG}-)W6Iea+{Rk4fiBFfOK7thaD0~P<1jgtX&bD&QgO! zTSqoog`RtxX}eq*zhVpkb`5{*WnpgjZ7Z8^K5Lr06?#Q&t!0M39}>x3+SIAWQdLPj z6<2S+)wn2L@L8SO?>~f*jxv_o;xc1INR}LG6g3`F*B%;0grG3t2MuYT@ojuqSKl(7 z$Ds?!&h2um=AgKenjg zRjoi8r3Eff@St{M`f;5^@dM=cP5;n^N@vl-YdbTDv#I|k=-(s5qQ8fEO%B^uOepB$ zD}TJf@X*eiZt2-H#3t$S!$Ru;n>TdnUV+^kOi6lzHA68Th|^W}@{ms8q4@B#k!HH7 z4)&*UT$R@Q8ehz=6#E{K=VXyYo6L><%vF3wyb&QCA*%U>dyp|}AV>s?e+qS&Dv+@F z>w6vdu#D_dkHP_5;z>$cMb^1m%KXx8|9E6T3LdTJ!+_ox8QSYU6@iB@=J) zo;#3PbiQZ{e|N+021ji1=w3WIkytSvc?(Cmce`K8(vJK_jc0j+3tSjNa%w(jY~COr zTGd8^(d*2Fs}>c3$eOdIn(J>5|6<-RX~xK>oCv|6#V7V0gK-i7Y4K!9L{dM5o+VQk z4C1-&3p2n5#)P`8f>XF#Vzq4~l^8K=qzes#?T6vFyxhisItEH%B~dMv<8Ut4!MoPW zry^LGs-s;-T`d7B4Hv7J8RaP>?^>{MviQRYZd!pw1LynR`Iop<-3i73dIB5I<-S8Q z_A7fz;@`QhQ8=c66@La!cv^Ae)49|ki%BD1GkzQv-00Nkk@M)E=d#1nBVOEloiO(% zQhYd0d4I9QR1$v-FK8`OnA*GyRYLxkFFqfwmg@Ig?GyG2s`e`vydPo=`#Z09DSc}V zjDXekY^BgI)TM*3x`=OsZ>AqF6PzquBIv_tN)Bd@U#}kTU1xb4G0kV!*x5)(NDjz< zNG=z>X+%r^Kr86%KkGmfzzO=h-l~SA0RxGcN{59Brg`qD{lsYcgqe8DkChdC+`oI5 z0>bBvf<-hw_;ex^uCFUg=@j36n%i0epgSR_EQ9))nG9i<$DrSMvK%HY5+W z+Sz206%KJ$|2PtDS#^N8(Fjy%x>8Vp<6h~a;#qn%OMGMz<~X@!_ByYg8K<(2Ld+E` zJ!Se~j<~$-h41kji_UZcy6X{b=3J{_fp|g4is3dRv`ow|E0G#S{MBszKw|PJT*J*c zue^h$^`{sd8NgmLz9DtY*d~5=oI0cBF2(!(_uHkNKh8R-c(JMr`%#r-rX+1!sM8f` zW8!9|)IN40EPcmp730(Y|9Xy+u0DWjJ)`fwAMuS4@Ve7>+fMN%d)V>jxO#buMdicR z$(bfWd26-H zPm!GS8J-y(^G@Gqq95&VB%8ihEL;!Q1J)l)?4a!j#vI4fr%mn-^cyg{5M-q2i0Ct$ zG%{kvtUbK$o7Q~DAZMcxLDDp>`k#oTSOl-Lp^uQt;zKZi>O-^q2x4*sd2&m3i5RRm z;&$eq^;wf0PEvdw>XZ<%6zK}G%z$3-*ivF5MrvvLbz+;6)>ocIB+3B;dtm9c|dA-~5#H#4jBo!=dNZgsgmkmnVU|jpDcTxWRz1?7@JMy`iE_$MrR5 zcDWmz3rcBcwC=+{a@cpNnspArI*^pIRA$)(@7Y)8=h2D~^qQHkd%|gMLroDq$(=`KR8++_8QLL-ZxTd zFGq-W>5i+bg!mK&Tzw8)Z(Oh5WEwFD{#NkjODB{dEcoPpJ*%mN=Wp70HE=VNQs3 z5Gl?(LMf)Q0CB}hu5L~ekXSZ=;letQf zYLt%8PyKXbb?M5ffu!uD^jsle4n00{?bW+1rHnE<5-Tej#(YDAbhPT%tRO;yq^p`r8m5k>QKf!hZMqrn{O>flX_04UP!o=1P=XUu(vo0buK<9w-TdUSee zc6th_c1dGX=;0#ssT99l#=jC|X;xjoFhfgip6P5s>! zr88#K`~?#t!uJ;7JIuGx(dO)x4rH#zMHW{1}1N0kBM38>yn+QHuXvpn8iS>r&@H3WdYAx=x(CXPR75pF?2D40y zBV&1az!qz!@V7pbO?#7ov)a3zKyEZt;ad8%b2%=MQPTv$^40DmXh%2|Hui0KgtMVF zy5}UjuP(v3*;|b7VX+za&LvzNzK&2*vtH8Pqiou2H_Jl;jGv1qg$@V^kT$a8R4Rzru(mpq# z>M)oECuWbPSEgE;#b_I4yl=2V33{t-`sHFL^P|jt!1Yx^DFffz;-#0aela`1PixgY)OEGbNkT)T6-+#R;Il7E zHZSjPWK)p-yo9h+i7L;kvjWW`vx@!DErY6q9G5PdwJuB)XxzkS)|r?12M8WcpSUmX zP;tMorlP*gr&7a<{%R~7y9uc;z`U+OqrO2jpCm%Eg6Uj9(+Wf^wdVnw(%y_gMqwE* zajhvq-S_Bm0yjC5zSV|R-F6iS`@Fs1DC@IMjY>vEF+`sf&}8R+q1{yQSNKOIGc)_2 zJ?b?l;{Tk*_<&mkahN3g$uegs_U(`SjX-uAzJQh6$BRI3+A}-M_14FJlGn3tPRL}i zBXjFJj_ZC?xlM=VpTh{FnfCFzHPX&&MJ868TA;Wg zhEQwrcW4r%0Pt&qu+)eBllz0`IXx$W6yQpk;WykmFv~aM#XD9VqCS4mPh$!j06gJ+ zn~5L^I0Vf@Xb!|%CUhXx*ZT&|@PrAHws9|RN<6RjEE&9T2yFTLb3Ud+-C|y}GvQk7 zy?mk=wR4LGz>W%y*fRT6bxnR_2;Jw(-KmUrM)`YcSf~wHL}qyKWP8M;#EkQ|qdOn*mO@ozPG1{{ zQFNmJlKR=lF-~4vMX@T7w$+y^mF4x>6QO+ZA=_C128UEJh%2_)o>m#sXtcL1Mbt`p zIP|7=3gZoB(Mckc4GRagozdM`DR#mf<59;bHVrk)QG(!OW{!G0Tup`Yj8D1a6}=@H zyh9CbtJpug%ugB`sBlhRVN5F6MZrars+{t#<8xLenPMesC%h^L4&&Ae#Z)yv3Mb0C zprSR+%BFNuEIZR8XP#wbe)&qhXd}3o(ab%np>88mVA7`4d_-kt!5tFRTB(BC?sMyL zSvEfEmN}*F*#GiBC^I!7PNapg?fjMDH50SX(}il-iGJEOH$cE`VT04*9BZktFpVQ< z9wyp9bQd8*xC}ih=CNtzgIoV*H>IX4Rd>(vA0{O^7#8y9nP9qlKL>so z$*rWuflxN>wh+`I9UCfw%=7c2jO;=eMbkxBE1Q!1vJKrFcLmmC%XC&KW&LaT znn$8b&NHQ-#k?D_rLRQXYEF&G*5FY%Md6yBF{?!*FFQ04pk~7wKh3(^Q#g0SmD~sG zyU*6wgY!4o*6Q|h7}gNf+%N?Xkcvc776`Ixu|I#*b}B+!1pEpo(zW>7AXH71@Yzv4 z%n#BlPvSsP5e~f;x>AU4K6bzyE$k`g*)dr<&SJ%_KWe+1gH<4X(VmMLR!SaQ)5VRG6B2<|YDav=85q?v1X%2UI)^ap{YN#bz zOQx!fau}!(`L^m``UGpZP?x^mkKKfD$Fl~I_mKC}r?aZej+%93U5)@jdng3uGxpu# zb`o6c!$7UZ#53HPxNYHVy)+2j9rIgjn4Z*#<1Jdd9XBj>@V*z4gbaM zsl|up!DeUV@>i&Kvz=iviv}e#pjAQdaPQiI9u0F&8LGy60Pr+f^C)xK{xDsnYap)U zYR&#U{SQ*P@fLu`KvaMG-72FUZ$sw#*c8hVcE4|o#jQ6aJ4GH4{>cJP2Pepbx-or7 zC}LGJY2`#Xp{<3a3W$KW6*b{Zmp^l;O?`o3KB7~|DN$$9|Bz1wIPw*)xT@F|?H8=G zFnEHt-Zp_pP6p1KF>5+o!$zzFGH6F4XnoC-S0i7euW|4W`&*^E@TO{%PAO=3?5U{- zC}U&*?A%ny*@H@xbNHAk=}UeP^i|8iR4XdcvL<-C4pbmxo}zJ0P!1KoiGcQ?9p0umZ@wb8L*IEMQsnP!5@m(_G@TNDUpE z)JIL0`yvrBED3#UDSd3%YNNSmE3fLYGJ}7$-e;k($i&(>yjrh|0{oyS%gVfvN_hmEcCv;u@fbay|0=G7)2EP!!e-*vIRH`x=KAV$RzX1DJo#TU(By zk#>|RyvuIhN>57U)z%Pt3{STA*BS4zK!e>Dh|9))CgKd!lg#-J`v07YmuWAIR6WDR z@b|dW)~#LB8ILw94Y0Ltoz^xk=PcD}DAmb>VAaN8sryy;<9=q;hLHN8EN-BxIV{*4Mvkb9E^$sSIV18(rDL9SMHRJGF8m5RDN$ zoxcDGgP;r(;tm#l>}*r1tZrTLd8MP101iuyjKK1h%H|I7xA^=tARHZDxdxC1jZNT*XX95 zKns_CaGA>7B1t&>?;QjOb=Q}%SHX75U-d53bFx!;)M4nbnfcQol=~Px%h%W1RKd`R9eFP zcrOKAD#AS4pT9eYpG>yID-`PGs#rT+o1}d)k;6$PTARQ3LyHtt(wKGpfmJbnV(S*l zDy~ysPW(j*q_%?$)+%iHs*)gq5W#4VjYqn}+nP;VQDtJP9$Wdg4IHiF)=Y{BvOu9! zTtaszBulMThjN(^oHbFEsOM#}$C!VprK}Z&A@Z3{5+@Y&n{}&n2~##xa;l@HWK{^; z&45Oy%oJgiVrVVSL&L0k{|1wqVPjisr2w8V%*k%ouzDipOA1k>o}o|E9H}^Kk!KZ< zT8An{pJQ;I z1Ri-6ND{xP7HyYnwsI+9HEH)^Oe7<~P42DuyheMmdL{b)4fv|mNoc+|{x9@$a(`P_ z-g)_c&o$j8h_qr8&=cWW&zyZ^Aw7(k*?vUa+Ag~h26orH5Z-1XfzW|ejiSFDuYJ)( zS5D{nVh2{h^Of?{;)GGj9b~-sT{QeEG z+XdZoNd57M$eRNOpSP@ss#+XTA4OHG#Xv7TtFwrz>LXH;l$hq<5Kkk?H51BVR!Fnh zY-6ueR?o%6YEsA(T23|?D5zjqCdqBXaf$X9TbpkO3-Qx68qg|wo&x@YRgA8+AYNyv zXq;#S3wb(5MHg$i3tlwFGCJ>?R_zk%#5JQ3!>Dp548q4$Z`o79UXd{OO={%h=vrkq zECtBCd7v2nb)PbN2t>?OaDuEPAH0fCEU&YJ5Efhi4ZhoW>6a_xX`Y7CQRb6rxs?- zs+yG<0rdGz7h{Z48MY5LbYH``zghlCn;(|#^?n*U;IEoEBd;D68k62cbhQ_e`sR#< zdTsa0vEJ=-X@!v)BH2h)JfW3tg{g|61*{`W7~_El`T?Ip`a2B#wYC zv-+pcLb{=`Ku)cZ`q@m>Xmom82KGE*nOlsgA639*N6_5Vh3G8Y*ZQ5E6|6Hk-l^SXJIxWJieo*afiP-_saI zOg))pt36U08F#XCcfqQ!P^Ekav2W;=08RDdDCM?Me*)UD2G-5G3V{~Mt?nEH?0f2; z=GG%Qz79X5pGfFSgA8uKP`?#?0Tc~4E*De@#4=Q?8^Xbof(*BmrA|ORyuCPa|zyU~I z=$?I3n1|e4jEa|A)T=lE9dmZwdPRTL!HPZq9e+|Bk)qpKK^2s`w707N`G8vMsC2Ju z8Z6E}f&OeR+?8byAnS>={h6dYIKKVtp~Vq}b1X_0zX3vg)>Kbu_AS=;^z%7$J|_w% z<^A~UqoDJ2UKh&LVfNoO%00^%S;DPj1is{fkxnznYk)oCEAhi#Hr&)Pwa0iv>z`kR z5sT1^VoQO;5=D;Nv%B-)ICE739m5!`HWvc>@|3Ya23$~)iK&!caWz85cUjK|3UbL0 zFg)gtp2ewfv?c3Z+^72ktJc`N<{4-cyA+TJ99zo#U}o40mKL?trPL#88HNRODWs;( z0`DSVTJ0t5fF9z0m_aq@g9^pkFAj|Gt9f0(a6IgTv=ue1Dk}9&R73o9 zqRUL@kUT=}YU+;d1ohHK-I5O`Lc=|28Ya)y^;oVE^DmYp?sSqh`A(W%Z{tmXl|!eK zXYVq`Bih0SVaYjCqwTYp+uyslVjFlfYG2qVuiwC~Sm&JV{Qp`2u*b3V)sZ}828&LW za~WJm?j#CDw8Ny58=zBwyU}9xwO)NUq(2P2CAO%iTs!a`5WOVf8d16Pe1pu%Qg#RA z1xNlV?V))@b0Fx+-F%ZbbuPNxU1*RQhkNX*nF`>42pN}MCfRt@i+4IG&fby!Oe2wO z!yaP$eoXbYdATe3$mc{#fXe03rNG~Ea2LQp3h8qj?CS7#%ocrOPtM|XyNPhC+VjGD z>3uTq0n`8A#`<}gIik>y@>(Q>Ah0>p-gPUgBw0g+5#J8mV-Nz!8AQ z0_`mGs`oda7Z93ECf9K0SUtSB6SQ=Ja#l1V*wdUjxzf6Q3z(!?q! zv^3b=*$F+4lb5@HVq9F@4SRY(NabYLB>)5|-Q`JIbVyo#^n`pGzz;LLbe9wI*87&) zb%B8SH95UAVO`?Z_=34FKNx*Z=hN+X^_qOON9MCXhzii}7^37Fl>M_UdmBFe^~nAr z;}Q&Lr@Z@YGlie{m#W~d8;ULxF#4ErSS{Hc;JP3gH+Uz(q+SmcxE7wy)Q-ig^~V&z z){j)IG@2Y~a6#1AyzX)bar=tbYTI2T8opHakY_vkHQE{$3sp@*cdEy6Y->{5=;5cr zs;2~Sbq{C{W{n-Lbb*rIhuTMiX~FG`S(ZdMQkcoianxiruAq}Ek$NI-U0{4z3J_uv z&fJ0ejX}1EZ#Aj8IZW;#m3;Kl2y|o?!R|WL$$-y8)cL7hb5w`jXXN;mM?@I{>i^i5 z7>)>)Of}&?c4JT$VnTfFEj797 zac#*&uEf%m-2J$sHzc21D(mc37CpG#xj-Xn#RRSQMa#TQQUh z=8vFM5U+=yr&eBL!cb1`ZA?Jg9=#kfVB+~DlM6h-4YTN!6aK5eaOk%>Xn_EtW9W%O zhxuLJgX|_}koGZw%(iRlt`M9WEG8*fsv18OqwZ=qXVXS_2;g^JHVX!7!E^(fA|Zvi z0O9B==>S)QKu-Pux9OBVk(E`pwraLv*}rUd49+v61Cf@g(Qd$zG`;;QVUp0H+-xR0u);|7k zGCWzXAFmzmk4JnB4cgapwhB1aZ5PlseE+di*i!+aIF)SEA<2~JQjR4vE|(rb%lP=f z=-|=(7ji~jLa`8u97!1hAH1~)K*PCdeUIKNmEmoB>%JkC9j804b>D*OWw`KH=q{|; zkxur!3KaAaNohEMTBJ!YGTyrN2TocPs&JM2EhyE3U5PtQIhvZ zw!x*uEl!5DktR$q2$*M|o-;8H?6C**x$cagR2pIFfR^dPw)u>+B zAfaShYotqxoj2ni$A|6e$>@*D#K?r9no`IgNL0EbgAdGJvpMG&RCItyz)?#k+ugs^ zdKM+@D0nhEKi>~#p7 z@98h@)>bLy=-JxiTbnSb8jly(8}ky~ft8_BcADOsQ+At@9kH*)nA;Ef=ViO2-NUrx zqr!ii7%K(00o-vviIMcvQzn&XOz7)^b)%0f%@KE>?a~9lCyYfVvfRFMEvaQmS?h@P zHyO_MUSj~+di$e#erDAVikc~VQH6@j}y`7JTOqFdr%UBX|D>ZFB-ei z-kl%`{;u3Y$xCvW0TnV{Md-W;*_-n7?U$GIv$T?ks$UZu6PVmK;AU6zg{DKcE7}{_ z|5ca|Em1UgfZLCVq=ahg=b%LU6e0yU?iOl9Jy*r`mn+@N@I~FJCau~TW2PoO_yrvY zOV|jSBHA&KC_D;A;lJy@E2z07hLfe?aC^)3=WKyG z1{1dg1+-ZD*J7Q2!sw?qK5a^(kI*7BaooK%nx-{vaw}kttYTP&Y?-u=f^<(L;gtdr zDzCKM;sl@+^7KN3aL)28-uGg&g|mzoMMsh=iQP`xDCL386Pel|_otr*T7mP2_>qc- z93_Z+TTlpbRs&{i5R;iN`W#QQ1+_TG$eioq7QRL4VObj&4INC!*v^x#i#Xm0y%rQ+z+&6J-AvD1Xyezsk zd!H0IPr=6n2F-Fc{Q~iI6&z}so4;Li<4e&7^R<)JqmQ}1#gragpBe~#> z!g=i2@)pjZ>Z8mi0Y}EkBdy%>|B)8oyhW?~^8E*R8~l`jy2Dq>AEfVrR=U#-Hg8r# z35+^N_6`@j(YG>}tAC(}&oN1G@+csGm!OmnLtpaQ6ZG6!eg#dxj!o}0n{J4<1Sklm zIKD|H*Z+6UWgl;-FJ7br#OHgBIKj{G9s5%hz5$E%`JlL!9*Pa;H{la$tdA zc)$z8LPxZ1H~LASxmLItbeE-W)Bf>b$wpR6o1H09)zrPjiz60|t-WG^N(Gzn;uPFY zCjF5;M4z9O>n1)%3K5Ce2>*L9Z_5sSd7#SDlv*dAHLAI$Ii5zWXndR%VnSl9_K;D6 zOb`Fc(t;q@H;C1(k5@$Zod*%p4ayA;^agZq_6yxZbL4)NEkv*O`AO>F(`wWnNd$QI zj{+*aBuTVr61ny`jUK=b+i}f3C{59&JUpx1prw4Jg6eQF^bVX^wO$eZJW|r7Jbts> zsD@^h7TP6CA%+TF>cjs34z=jeB)9;lSS&;4BaE7QA7=MsNOwq9*d7K~=fR{c@Xqws z^>;IcF<;-36;2flxlOnmfZhUjdLfDfz_#E1H>&5lqV|{`@!I^-$5`rLt!8B8&!Z7% zIff#2$Q4lC&B@``kh_gx5l`rSt{fN-m|`%U%IVyMxEoL*6+52nc9OPEox;gMN6ci0 zQC}N=LTA(zwTYAlhtFM4c+M6Czj|L2pwoP8V(>%NZOW|43^i)dlZIPi8M?}z5x6Te z>&YJaDpsyJXK=~HrmNPs&NZKE8!OQ{} zi0E*di=<>}F)B$ao)Zx1ju@*lE<|%wSKy?v29cI|mHJoj@I!&=Kgua%)H@3X4Uk5w zB+JP7YmLavyl+Ui`AInVY}C@(=;YF8)p`*iP5a|T^5m&fYZ9um4Io7z;~GJ(;mG{t ziIm#QwG9iJ>YSupPC8SCsBS2uFNPqcrIjT~$GVE&mHReej>zGn0@Ul$LYoJCEknzP z7rEpVq3uyUS=WmLhpw*P=;EySNs$v0c2;-^T1~Z(2L}H{iZ`2cN}`~%8OZ7}GBw-q zNTUdDL#Q{hI4?q0Sh6ex`WGz^;sikl@?V?k%O3$z|Bo{x**DQnsmVw#b`FFHhjuCP zyPLmba7X3x!G|r6BbHfhzr(~Z?U)#H9Mb@SZ^?4drusAr3ahBYgY z=72m9b;@_at3mOQm>NszB3&j8(A8iYc3PeAeh7CcL!qL&gx-Xq2Pn{~JGjO2v*J7I zv-MN#Jp{;MCIT?&18;z%Pf(xC0>lM27ea-{Ws7oYTG&6-WR)m(65@X%;{YAu1%)7y z;pYuOPAP)rK}#a`hXmtQsavT&e~-VByol5gQs9P%xz&r9c1KDL_7x9>E-VUEl8`so zXK)OqVuFId3FjRAm0fR#N<_2jHC9B<7JzKyi3GupAX_DA0~DTb06Vd9JKT@?=Y*#R z%W-BjUTRliQTn8nm=X1Hd|wO>mHD~;$O|Z;!A}qkDy3SNg1dHhXpb`o*y3fRp)_m%Xb`3*Og;p0Tf2INkAgA)nE$zqnt= zy54xh5ecpCfJ-hHjxPvP%I2gPzFWwgPu3hj!B6~yH0}gNSw4{f*^Wt6e;b{v_B9DX*k@pD2nS9qIXGTI%CSarxfMxtC1=X4p?%}R)4dgK z{A^T&ChqX9^KbNr_S-e&C24?vO;{;K+{l#LWJ~G_bv?z_G_|1Ix{~pi%!Db)hiHwl zW&Qorl3A^EJPvr8`5wofkFosRf^fcw79}1$ix4{BY8p?|d8bi^(RqjDWXCHPQuOW^p6|<$ zPpAnp5{KwpA&-TJW!p_DP}S$`iJ07MENg*cfsR+UNI(!w#lD@AKd_!c-7LzbZ6>nb zDbRi=<1;Q{B(Zub$j-TI9gj9N`)ig7=-$IL*1LA*b*nXh5RRSPSD z4j1i-qJ=(#G>D+3J3C$`-46l?mti;cjLzr7gczE%B1yIQhis$!J!Coj z6>tRN9yfiQt`1@Dj9VspqDUBPM1k|q=1>LQ znM*@lzZq=qqcnuRDsw?P2zsCY%fP~^v4ui3r73Cw#c(^IpDN1P1gsB1^rH!us_uFA zgI-jAZt0L7o_|k49~9ps#Dcf>vUhb2IqQ;8^Ie1LAUjXKF+IqGZ7bi1{N9;J84}Az zg9iMI5K7R{szvAY(&x~E8Ky(K8Q@5H6x63fuB5U@gg^a;AQmL*`7|#RD;oqI*>R8_ zegZKL7UkMt^jcKS33t3}1T9BvK_h$es-m(03@sKWaZ5mwbyRO9;Xa$hCAx%j(^ydt zcl-RqAg&_Q$Yfht=WkT1(9m@J)%XP`UDOc|nTuPea(CzaUM(5UYzLW7+8Vzy_tLJk zgvV)t(Za<919luP!rF^EAx42E>v<7ed1fSd6*johd3{gyB!N+tncc zebl@zXUW&rwuK6kXWnbY-!JJoP~P|s;s*-k?5J3}!C_6u6ay3rfyOF!M81P3LF9Ek z`Ip7RNQrMYZ*@N%&Z)646+KpB$J>Zp~j5I(NWIkJ(~e zC1nc9u>dGYs_jdGkPwmuT{&Blqmx8XA^+6x0O_ZK^u9eRsI9jThukSx#I*AQd$S1x z6j`^q9PPDV_S%|HD-Qf{H(=h{4){62Qn1 zUZPOK2$2%vS?=jJUDFu>(Y?1SSHG3b;EDbpGj8SEBp;3%8g9dCUf~5oOz9fJp$3Wz zz9nZ3;&3VVt~k+?4Nkn+D(@Y{0Bx-#e>^8{WoxV79P$KYf7zZirbD(s_uWxN@RiE< z=LNyR0Q~1+TlarkSNTMk1z9bB0!6nlrEhYFS+X{kXUmW9_x1(Tb5+YYAOP zPlr(gT71@ao%*2D@Jcnz0CuJsM(D+#%CPF1Ipr-Zm@4}gn;SDnzs*f;@&bG4PO9Kx zqaG8phl%~!)UHi!BdSP0lUc&0T1uAfFGWy-Z8kFM5HAq?G82XO9Z4T4zPcy6kEK* zF}*x!0VIP(qNa&NvJ`HkksQiY3}{1q5ETZDG*utlQv%ZsI6CHX_qv>!*f*o>;!mHr zM%iSMA~n_YSJ;mGcEnvKg!Zx<5iw#Rn>0u_U@y!?lM4O@T~|3zKJQ-C{Nkf!SvgU> zlubBDzA#yV+QxB4q8AIb7!=9Nvfu=VjtwTAHZ^Kzuhja3N$vzL^;iYSoSIe+7-gGK zsdWK}8YJJ-QLN#`HbZ;|gdnj}nxG|Sqh9BTvQ_*vU)z0@|7FwuOt_wxN<+0$G{r)& zU8h!{b%!u2AGUi=a@$}WG;W4kTw6}k_PBQXZF$@#i9IV|61M*VSuJir{#w!=Bbo`j+&+1-WmK+G`~zsPX4%2QG#w zWpFB0i4$&3Dy1yziD*SIuJ(3TauFR%Fd};Q>LMb!7f$1G|E|F;npl%d6}Z&Og+W&X z7rRd%Nk@Fw6*WyZhEw%MF#NifS*^g;G5qnT41uA*9J{<+uq9?baU42FZ)hOV0_l11 z>Y4G9o9hyi76aUXs`r}_+;=IXpxycXD;4PR=~SV28D&vZKu-m;f$Dm#M&dDpG>D zB{DnHZ2XPp`k10t6>}2pJduw#W;hz0+zl7rbarmKWjUe$Puwm9{1m_#A{tTtC#ee> z^m067w)qB6eZAZK4m;!45O!E|cyVZ(a6V+TuL~RlvMPKnq7P?8I{h?xx>2fCJQ^`Y zc_(Q9Z-$c8LFTApk;ZQ&f)KTUf_!U!`IXv-Dx%iP$(q5UpYR#zjkpfqVm@wHvHZ;k zuY?lm_-9#8DkUcNe9>kKCIDnz^t1T;$1-ZUxW)`|&ZcCkUr4{--u-cepvOg{rFnGtboz>0hl@DvpbDwPpbfHFb`u~WA6>eTQ#aZEybhbaoG&WdQ5NOgg zavfn?Y@&=y+9l)5WM`rhmSSQI`U{Z576*-yt6LcjiUQ2-!Dl+hbI(q1(<#krM*N=r9F zccS$AH~Lf7e%hC9$1rJ;hYJ`U0eb;eU^EI#xYuJNz8C`IW)Xy&6(}H}~ldy-p zb}x0!t4R7Z;Z|l)jK<}-e04Z~F1Z|sq{n^#x%$xK|1cI>{}0CSCDFM{_I~Q_547*@ z+nyv(omaA4yVq#Vmx{drB@wh=W!Rf^FSV(6pW8lsH80)EHxj1!f8E%p=4H2rb9zpA zxc40iLO*xXcK-8yt6*cb1l!+2VFP?998m)mKIfV17G8*ydwEMjWUYCg=U#%YkQ3%B z!bGqRya$(y?khW#dlSC;$PB3Th@IP`x~d|qMt$8u2tDrIe8=-dGK_Jg?;qD+W zhw4BN8VPzeA*sPcysbHD6)vwZalxd-UxmLdix|)M^IpB793h>;>rIcYC=x*X09H;; zQF#A)0U%naSoYPAn%>}03WS($*X|Ad@TnEjux|eM=Ttek0@6E?=JYJa()lY^peP_FqEhA zKDDvntX2pA|5|_t{}--ps0eJ}^UHH$xJa+G;wNAjp>d?k{M-9cMUKlByY9cgopJcy z%D*5^ZPq$@r&51L{ucqhg%lR)3x?TmrS;zbwJ)T4!w9}=>>t)UU~+NCk#j#V(q`K9 zOPCBbh|5tPY7$Vf)0TglJ&H$G^rvVNtP(6Z;|krvkCKr_gq71r=homjlqt;;tx-5T z0$eC}Y-lsdp^hlDuOIlVf4bY%7##axs84Cn9~G)+tGd2`RyFA{`wLsvd?qxXDs(G& z-Eb1e$Z{6LabGDWo=YX7G$>cSP6xB1TZUAtFM%&xznJKeImY6at&JU~k77@b108XO z>T|p*!kwx!kdT_{h|MnklyGkhPEScbJU;D;gZ%sXFl!cwkgd)$EWuPFRXFj-Q&d*Y zaMn#P5J?UjGMG&sKtcUv*M_h=cXD7=B(oB?RF`tsru>lL{6vLHsn{!w+fb~pHIP*j zRkN>%^*5#7=;kNRY5JWcWpvv@W=&&9GBY5kw(b*BC`TYqiHHR*??&m=z^2NHfX$5A zV)m&m)3X_4Rxl|@m$xA~5<~JDtwE$;4OF}C4=s-wUh1zgcUt<6=%)-~?jXfN_{3ss zgg%FIu-z~jCDZ?2`tKgQ1#z5a&x(=q>uB5U7b_t#>EEAfE|6;A|)>a)H)Vc3D zt9noDnK`|yR16Q2tg?nHq68b`p)mM%FF#rdBQ1>=1`1+cv|{{|apbPCenF@0a0-&( zvBjneiP{Uy=9S;bNzagftOq zr}So12a|;U;mXleOS<3aEiCxdFlkBRFJwn<7E4wnfArc+FK|g1X%oX2f5yKj5NwRp z(?P_7{cDZ&6cRP13B+JqgOdM1z#G=HWz&b6Qm2ITbew~?V_XVlVMz@SRw(2n(*|E6 zCUl5)rOL&8179xROsu0iyNRko9G?EL?wm$l4}PnZc)@uRAI=$yB==qvQFbO)DPk_c zXU>;JT)nmwrxKAtR=$@psDV{HkDUfNu3n4SWmKV|5x?1tuYHYI_(w|*cscH5x!l2w zd#BL>Z;0J@k>aqOK7iyQlbl8AgmZwl6l5JNq+x(514865dMbpck>w9&y4@0NzevayL!+t1L6@=8wUDebrF8{X-zBc-gdN-g5<(DyC3ZCPLqP2nl7 zLL2>B68&TD7`oS+Wm6z6#7+wRY%z-nIkesvXbey;3bi;~44SFs^fiw=6T!vh$U%Xf z5f{=jCg*(=RipumE-FJ)QwRc3MweHO&{)rdls}Ic&&fWY-kDmQIc)eisq;u&1#lZD z{652mnK%TNj+@3YNI!|bO~0nh#=}fnT)Y()A&S$U0+eukXN9)&bodkPsq5)u<2A_D zK(NiL)ZeF{7?2Vm-o`KcIbZi9Y3Ohp)t8kb8I^x9Qibf znt8Zrzc>qxuk{C0}NO+hU zFGDsC)$>90in@GG)*Z^yT5M%y6-|ZGT2G2vaGB|rYzcx2gF-788IZBE6y+}|auRai zE~S>hxUyQjKE4tQRB%Dd$!6NbG>a(PpSluOE-W&z0Gjc87$;1&VRJ0zggR~gbe-I9 zEGTNET^C}bp-Q!TKwYYodx-NIh^023BlE;AcRr0t-ijLElq&S%^w%bxlb`F?K}VSv z4ZR-7J@s)?ZBoBUx6fBCdOfff5dELWWh6FDBQGup!iVrYXq_kKp1y*YP%WlHtEZY$ zcF!rtoJqr<`1{?{TLeFKFOn^3PpMS~^}L5vntuVB#@jTCOX^gTWK4q`))g-zM;Jnb z%fhxkp{1oQW01e?a8S1m(b-v0P_XZ%2(>snr7B2E?axvJ$LEc# zec&DLb;9@l11_XlZQ8ieM>osA$y*vBUO9@TqnqxyuB>EzICqtV)s3E=bXWmj!|Y!n zeSDlK#DAX4DN}ytCZR&zrL+72B?^UHHO4Oo1*(Z$h+RSdmu5VJReGES8aZfUVBLV_ zPe}7#0#;6lA7a#sRkFMinkWF#a-i{-#c~n?ZAHt*L1hYwaBs|8$Wq}XJ|}rrTf8(P zOI#q7@Ka4SPe%53q5Z*FR_XY=C;I7I07j&|*R|Twh*Rj^)a&Q85MsYLVXE5r`2G#} zZ*39D!Gb-@aW;{`1?WO?67POq?4t%-QOg zLnkH`|AdZ72&YJ=%oBl>&NoSCx{oa`Km(+?9L%g1#6WX`rp@hNV@^oT3M3-5Lsj0b z!XT3@Yq21jha3gdQr{~|FNc-ah+kNE55nF$g2Q($6HGVOtX*YvkBp(p7MQZobm;Lt z;5{F2-s6rUy1~5Py@Zdb)yTv*?geD}+3{#a;&DUf>1SJJm0_&o1CC&bxZYD5qNXEQ zIz!6m{X@^;ou~lk^%!kP)p_+eixUS4OxiDT>zp>zk(m>z zs;VRu0o)?&P?wlZCw%4Q1jjXl8I3p>f2H*RalZN zNc+5)KtmKoF`3(f1VmrIXpsQO7%@4)=$Y_urAu8!cI4!Nl5vk^@BjmxN{@?lJkbH? zWSA^#qRB^`&jmZMisdzPDps8RV~6H8cu~)FDL!1X_B`D$d_Bn_$?%LU!Fdg=vl`T7 zBKx=!zav)#Zba)IOkD`vrC-61J%xH$Xa}|NWZ@D|)iD7EWao+CT=gA-t)oPLccz@c z*7n6c1(YmrgbQ?7s=zSzlbOUsavU)JZ5Y#RS!|N$0b8=~$G5ud?wPT703xCwJ$&!++d zGR9_6oBqFhIw?oQoXj%{oQZM6<(UuxR_wk554uZyyJ6a{Ftz>uw-+v z+~LE(`D9~G_F2~gr59r$`As=SOENAG_kXPJRA+dA9`}}0q34~2F=F>&wsriUe z4k$O$QO6djVcEyl-D2-VvZ`ItyCKhAA1mRzUZ(LLOt6d4Ua}6GDi=THCG^yA5{>99 z5KzN2xrZ4S+`UFt){;*nR*QNV_9!9~BOJ$4I%vJj$aO@c**}tuV!D<~SRuh+-}DoJ z@0Zao80R-n9D@ptU3^kD+H$T`?fzW_y^LkCr?u9kb0+V?W)y9xjuIE_J4w6aerwiS zhl=Rt9w~L_M!Ha0U)>P`wL%?l-IhdHKM8Gv7=I)>?gx2~MzMaw%w0(W6zrgsJzpKX9x-FEOXR{mWx|1=4#2?LzGM*QuVLA1 z`()4xEpiR=sFY=tYI^^QEa60P6EMF%Y-mFZFbPFXiz^*OVT;ewRq*!sS$ZZLnjqh* zER|`53LznM%yqJ6p=jVDs7S4yr>!p!-er(^s^$5W>@0bBEPmS3fB@=RQPax}ubwtf zMc4AW^Rpi?EC$OBo8!umn;(4?MmqMu%F6LaFYZ^}r8TB?xtW(#T!7r_HUl7eA-WlQ zJ)=Y~ov7L_3(BcZThz#qi581yEY0PCdhyy)sK|06{RaL;1-GJ!3H&FeQ-IVVLMPY|J(OTXN{L6rgYqS~<%AS|2KhamOeklQcVtlWvo%dY2eH!ku{grhZw2oIf zTe8ZiR;$w4&z+j?uNb^hrWvM;rx$va{Fmf9+oS0W|6E-zxcC=>ML6lINU)+gaseh# z1cZf(E{&2m@0J~Zz?eolVUB7U?Aygs#v9FDn39C-5ge_eNnE{4Y!>am1o*YQYw}qE z#>XjoRJ$lX*F?N;OFg}y5yvkmT0ZjF9z{e?2^xGTMOdhGwT9F(&O5V75))?BJyWPD zkw8_z(Y@Hw2P)Gk-jZLFLCF0zxqoki>JUkRLfKDE7ds5Evx=So&#fDH8&AOa^Dy6YSHfN+$&L;C zr>W1>B!rE>CW6Dgr;RVH&6$^g7Dsvi^D49quSa@QhRPX~ar@N2<)9o7hgo$G&Nv^t za6Q010C9PG6%rSriL;WeqIPH~T1Tf&tFM+>0SE&gTmX&c2G~VWcUJ3zX3sY$0GR~? zkORFuplO!@U_kG?qd+X6@BCzMiC0Kf5s7PcH2LE8o58s%J?vnyvtoflxw7my@CW&eN;mZDguLxJjdUGrx$a|*Ey za6M7TmbmJI{K`8!0=#W5vHLJ!PY5C0Uh_y}Ny?&2{$r^-1TE5m6W{gl3Tgn_C(35P zOS)Y8I|RNH#!ee@P<~dm0fFQlQEot9QHOW>4tlhw0p&$br;u^K9-Vpp zo%b1C?tHH>ORM?coes95opW~cUY&~C2<-l&1rCO5X1z1RrV099z+t9cEhOE$9;d;! z2~FL@+hpJ`G_NJkT~77lMtp~dQ2)zv8rIs|8vRC%JVT>%OHm{o(3%45pb z1GeDyg9aEZWD+gOW-Lgns0<4xsCYLbkRpaQZNrjWuWzXJ!JQEdn?{_A4^6FMT+fUm z$K>Tc!eo1DqZz_(QEjP+UpH1vo~3Gp{k*iClkqnw>dwYKb8NC9!|Drd|6FaQAH!!( zULEquf_rNBS+0BFgJ4H(+%v2g?>z3hBAN$262m>CyKUPd1~EK!DF#Ie!^g3bcB4%4 z%Yxmy?QXffko6$aj(TW4Bv3lb$()R(JHi__ia{#Z1nePlCJ{YFOG3+l3)x{6F*FaM zB@#*w5B}<}EwnW3h}jn;R{k%X{f=m>oqn(CxO#BEWax~=c&)eGq42%k+1*XX4x4Mo znx)YJc7LgQ=BDKyrvhD| z-e`mRN@8rTWS6kL7o5dsB9aZe0ZRj-{@W;9@lt4J29jZI98iPt+oEE!3o50;dQt}5 zWG$;1LrJ&5ZrQ?dy+$8uc1oHwUL!6yzHD^28%;DOkG!isx5eoQ@+zahpY z6UiF+Dk@6Z3-73hsqS0a*=`=I-Y}mxZ9$T0f=gv{zg-40Rof?Ve5-_B`|r%AH8FBX zl%NQG?FU;FwO*{oWQntM{wqADMUZkOp6Qg`O9`sReyi4%?eDEdnjaZGlkn? z+vpauxX`RWb% zy0+%NRqTY`%I=i&c}k=_c06c&)ny zg*9~Ck>9=T-LHV##h zK|6JrOo6%1*Gf@%%e~Ff$1{NMHv9ecc@0OC+BC#2=L-Ca_~J4qq9w}+V5R_%sGn5& z7CzFR5ejS(k}N5zumH%Z0SML`bSeQBlGr>q*kHKLVzChEQT7XX`}~%^fV}=UnJgnA z8)EUZ)5|<1MKDS8Y&bs%#w#7 zqIraL&6x&O#x*LmssKH^lOByzzoW1Z5gUW~TxUz6NQ;@YI)hu!VX?u#S0CfLMzL=V(W{eZG`$J^z@lgcc0Tz0 z%+MsT4r0u&kU-S6ib<_D2%DHQ6^ojJrAeWb?s5N^jBm3uE}>!3$dBMB-$AqKj9brn z5ZQP-@)@d{_CznX6>{2l$W!-@Sn^SKVT@C-xzs$piWj++QD|F24W--vVu3$p!uUl@-QsfTw!9 zVJS!)a7X-;pdFAbZZYNIHq;&e+g#OsyqE(&lUvob_9M<7x1pfiEC}l>oeVzYn9*GM zw#CiLrqTQx4m^#}g0>}AS{Ww8(l z1^yF%_py1*LP@KT#^@q5_01;O)lRZmdyS|dvC&GoRnYc-h9{KTrPG}zsf+O{sXzpy zoz!wrrHXwFt74VYQI%ptRZ>w8l2ad$yIRxPT|IJwB@^KRg9`;4`@uo;+P$*Ch}j(K zWZZ3q5FG4YBIjI5cXF-)d1rU3C=*eT;jahD(?7OEBdidAvuQs#A>HLN9NtS)w<2P2E|VHFP~tcN5YE39|aD*w3~*V z7Y{xI%kLFElk5+*SM$360hNQWkGF2Gjoe;4rPwDLn|;vXW?6f^BldBt9qTYT#$>mP zHHYkQeGj9!GHk8oB7t30dlvNUNcyxXvKBf~A-Oc(=+UQX#p#pkiyf+xR+l98xw`R` z4i*2QbK%S1-SLw5gUv+MsrAeQOIlx&X^l7)4b%lJR2rm+f7*Z zrxX_+nw}3(%!V&mmJX~g4)$CH3DZ$kut6H`AcD#vB@-N{wkcR$Jr~}lze=FDh`>YP4^;qXGU&{RllH1^S9>Rt}bnUV41-y5Gw364yXuat8h;^+Md+a?>~Qv?9yt+ zTtmGoVKyT%wJWjnh99AnNd5Vk-k$n@G`&-FBy7_)9NU?2f{AV0wr$(i#I|kQw(X8> zTNC}cpZEW+qdw_A=vAv~SJke)Q>#p(aH6Q)L3@^KPM%@Y)dw`fR~DdfbfZ`a5CUs! zqlObg5di!OD_e}!tT^X>Gpv5F&ZBxs$Ch5K2HmfLuU+JetCfs~9vBXCBq`bK^xM?RunHL;)qD|C@8h*-MZOGd>({Rv9`^VDP=P ze+*6bCgXbyb{fhY<*9FIMw$HG&9D1dm-{s9dEHIrFE=J7qjKDSiFvutQsWL?rejA^ z+i^N(g#WLuK&N8G9uR3HzGTA@VQGSWoKqs!9OLMoD#8KQ?sTg71o^0QPB@?lmbT3p z8OXY}Me)%H{)CMrFwURZ6qErpRh;j7)u2%xvTV#MB`9Ux?!XUFL>v67Ybp(0RdUms zvNA`G)P0oJw$2qgEt^0lL;7tY@1`EV!|WOk%A?K!xvUddK!-1 z%U;-TSbv~lwbtf@W;}G+i|Qk@FI?GUl4Ce^NL#{h03PPK_&$7#h9{(x1vUNGsq5dBifI!GTXg#(? zPCprePM34&fzOStCg5lWB$LX`{^lP-CZ?xrI1*(5<&W`U)`S!udxpae@CbU=m1{S? zf3<@HaSxPpR(@~$VD;lzxa)I{xk6g7dr#VHPr6Otz$y}L4+ejbyb5Nj&Jtk1dsNfw zirI&!^V*&_Ui2J52L=_w0y@eH%j#%AgdtksPSjb@GDXdsph|76wV1=fVee3w8CI@k z5>W*m`(jK_(*57Qc~x#E{5g)SCo#Tp&Y||MuBJ}g_i=m z@n+xZ@ei*3o)b6Q9n$)LlnHRX@GjY-+T-lXL)qJVZYuOgj$%eUNY>svv~dC?$Pl!p zOR$lGxf$FvQtcV)r=pu|0nPT4`vK4YK*Ij0H3vxjE>>(R8W<`$RDdxekKSmd2*WHw zVRU$G2_EjSMY7ZypV^XeP-M0rPbEQyVzKe!_0v zVw7)zC_Bbud9eescA=cuWMEtbJ@$Q>ZT+PUKhENe4XwFT#ZtCxyuMTwF{Pz~Rv(kX zn&RI%*jYS~mjosWN(=Tc31&%N;3JoS3A`UlK$V z9+1m{9T}?KX9f{Vc9B+(J!!@5N0O5|lpvj^bJUnYX|6C?jD23ukIa@1uB^gBTGI96 zelz~S{rrMd2T)W&EBpbotz2K5=FEm>ZG(b-{)-o-Sa1+HJf2BM#YdH$pXcdjwoW!m zGyPH|+fu{JBG3&sM>S%y5Ar2r^+Z*b`Hr~4zv9cRmC=ScC2?ldhu%9NwuYEA=NL4+ zDOCS}FFO&pqM*#UbrdO~3djgs{IBW^kofdik)}}s+kw>N4*sYIcpuB$*2~$`lk*07 z+N#STIt^BS*ZNII-h7avZ-P+bE%UFqu;*2JKhogu-S`f!B>EXZyxFEEKXU&slzgID zB(HdX@t4a>@-{aO-EHIyNPSIToPDgsIxYgD>pVcs$#fbY_^F=?(vY^+GN$s&3|T6) zW*)nI2rgu4fCc#{Q4TjbnCX*y@u6{Uf30|zH8L(-~@yTEif#h!-gCcL~9qY zwTtWBt;i}6fT)tT5ANYQ+oH;$YbMm3yn3b`JBo)9Kzg4Sd#BD@M3}#n)-re3kC-U+ zoAzTiVjR3~AfN(}H>M0qTa*>X;Ozd*{q}H1WI0WWhB~mrM@o2-t!dp%g-9b%3LQa;c1F&5ZZJ zr$Qj9z~#Y){sAJ_I4{letyT(xW#=h7gs>_C$@)`mc`3lmOC3x`bGy*njosZM3NiEm z#d1YBDRfWRFsRX3d`NurjwO=LXL3#ps-hY(*gD<8P%|6o^Q60GpGq5OZ{Qksu^E%6s8S;Flpe;PCmUie zt;~VLvtwk?HiqBzCG*k!{eClyBZqzRUt?ag37qG9urF$v{$4C2yHVyyc;V!25#HGu z;3g+>>g2Sx=A!MD%P;s_G@mTS8dDv6_|n{b0uW0-2wBaxR1y9VV9-T|(XV{8T)B(Q4jGCdx0V79 z%pzz4AJNY?WjuBQ;V&Ssy*-?gvmLWn5)Fq+EoyU>+|JftOmmb>?hYGyQMY-fA1KnK z>ag5zO^ECQXEUx3jF(Qdl-8iJIa)xkbP9t*thvUrf$3W5zCOJSlnjxz6s_m1R@g$J ztK&NM>Ps=d?DBkLIv^=o#%EMAI+qR-z}d1M$tbJUnns^8BfA~|A29rLY*{{zU(0gF z{FXI!_B(o_K0jGUo|2Ij{$qZ8R*8lOI90@}w>4Y)1Wc^#RKM=v?_mI;f+V_%noD!r z$ORLa9+gcH(yzq@mDECHU-i8xp~?uuI*Ap|7I0CM@D3bgc87g1TBopPYipbO#>T;D zu~^1UJbFr4=@f?bwc~m*{DYzK1MZKp!Jpl%LJ~L^mrx3NN}1sBC*pQ&bIQ??3GYWZ zV&l)!HoJ9eoCk4x(42iATFxEBMCE>j&x^3FCHq)oCY#WVq)y{@gzi&|+skVxe;PLnHtS!m-EK%~cUEYJLUt)w z0C|6a(O3bx^{56Smy)Ytz6$DMYD z%h7QNRurzCT4)n_!->9s~kX$I=SlJft!IQHs%kP~Ld?YfrlZIS%g z%jrQnki;OmK@&e62BX~_cItE`NPg@;?*<+0KQmagP(F+)kp6|g6_kx^bm;(g)G#jB z1xKFnU*I~=e<0{WqA882CgZ0Rf-JQIX~K5FrAq@ZT<%yqdKF+{u*X3N)o^_%4C&7u z3XDzkoegm(0(5mOCv;l+=yT0&ySra#YNPty964q~jAtHs!`6p7;tTq E~I4)QvNvBBCOIeOk_#2#th8#4v-R z#HN9_n>85par+x`s_S$#s~Cci=9d3HKumKVmFZj6h2v=(UMx{8363#B1md=^$t~*X z7Er^4Y=C-3Mv_m=9HVPpl&-Qnz!2e9?4f7I`XID64~`;M-@QTJ97wco_b=?DxbeBO zX=!Oy;c^abA@0EUU9oM-&PDXLpCsaN+lNWQ^1jWHwVyolz)i>bK~%gLz<9v5bOGKtPz9x(K-d{RU_4dyc!+(e%^Wz-US zFpEP+HY}0Z>P+&&E1-xD;e!=Q)x`S3qsI)_YDgjr6OB~U$ug6@CYg0xv{LRT--L-s zM+=OS7H6SBlah;q(wY;;wzDUm2US+}T1-BZ6YOPJA5i?*ClYJJ_gAR261x|foo&0o za!KXt_YInx0g-He=EynP8$^SK0x;oR?LXmgkuZ$f|GMFg=czrr|A-~{``rrdM7xqj z;+cFrk}=IjC*-XDm#Ak*H5ih8S4&F*uA5;6hVv{-QUfoBIY`$TdY*)e$2C?=+>xZa zMRgq#yYgfgVA49`K5M$92dJ%qa6NnvN`2FnBGE?Jz0;udjh{_fs!McWzd&kk#wdi! zUw6KSUmQFg2vTzMNjT_9kAGUJCI~utmJt;t^kBzjj<#}2QTPHhQYeAs8kZnSNov~I z04qe7g`g>3Sg#T!D8swv84)B#RM_*fTAHe77(5V-6al0zVbYDIBIVWAeio^E%jzUB z?1nz8;L!t;p+jS;uwkVJcU1Clmf+_E$#Ln+3^%kPumCz}_6bil%|>k_r|NY0-vzVM z3I*Scbkx~*(`hAv5OGq=Pn#`bp%`&C1H`&Q*W1i06E|eNe%uKs#1Tv6B^G z!inNQ@hW)}#$=(~1D=97C3d*;a1RZ1y+=VpW92^oK3l8z)F~BuCZDf5;I_vf7k<_!h_{C(++cKV@W!Tt@6<)@v$XkXOFi?1JZet7Q zzUrKbnxa8^vyyG=xxYyJXDgHoVY}ad8T7I9Q);8BW{I?ytXYog8se0l6%p5%3F;yc zIhV^muGVX9vW*Tk#U%_Z5-f6_9}0YLjxe_bBr#BZq96|k&H0@t!cn;P_ig%7yvUf4 zhzj~)x#076pz5`W>YaUiJ)8zuSQDB^!74H!&VsC{ zzUh^WX!q{5-;Dz^;^_TqXs$+m)0uXj{HtaAcYKau-50;#cV1I&zD>vW^m_hp#z3pOyBGV?|cX%Y}JQ|oNJCpYe_XSs$GO8 zjvHz#=Ke<0yfGm8jiD!vP#W*qn$8izF@3d_Q25r{USAjk0 zZyshsP-BRR#)xRe6%%aeCM!B~#G3=@%4kB?Xq-Q2I@)Bb@r!s#F+_rctokiLN6hWX z#Mcm86Dcu&7%H$722U&Z0?kZ;9X%pzugDmO*)TBT$;&Vlk8Jm)+v08{GF}z5@^{en zsL}Z=kVyx{GGj06@Aa%i0Ch)Hd69ica>+aG6rzE(&dQvZ8GaKguHP~b*0hDcz+b!zOTOs#HP3&J7JdxbIATFLGWE1gWqsmx@ zHiN|e#8ym|6za*l{KKII3BVvH3ah)m#Zbr-&Q@FY%SI@BGyRaw5!?gvMmKPzL9hac zeqCMos8JvZ6%!>TD`%+&vlLvhzJANwGT)npv|qd!;O2neH!b)J2A}ImprS#)+7C3h z{3|Ou_;j&3!k9s^rACQZs*X>tg4NTI%=kRFxLh`$R(*N@RuO zSWjxev`i1woyFGmLL={54{SZTxXe)|$<9+o-ehQ!FSXLI50rgf)awE4W09gxGK*Pk9|D|X_lEHuml%l_j8-u8^t=xyde`k?lus!T--*GjsAno>l18=^hyfZ|`2#q2(^ z-H@i^p*`27$o;oNS!J@HMK+yU|J71>mId?N=?FgUEiv;DR}v#)c~bAXEmvHlqvsW~ zY=te%0?=hdQJb$B&ZnQ69{ssX6v)x~4^aWEpc`_QeaQ9Z_e&krkDo6Lug1DqSa zkpk_*$L5g+EQaHB3iSKD8fKGPjMcAkebLr`{$YI)TGyqiJ-Vj26Zayjxq1xo2B8Df z47$s^fy`kE(gHU`1pf=`_9LDN(;nZw=jW}N%;xYSAitj}eqWcHVjmXN1$ zmNf*GRMl559}pK&9qxo?PB-)0p}KVIK?vN>hz;|@m7gmlUeMMEV;yC7rmm09x$w1S z)P1}fs2dI)U7v?agLl?4Sde%HE&x}MwF<*1tz;(v`3F6Sv(J^PYyKfKGe@&KU#2|K z$dqD<*IaSImwL8=Uh(`b*9=ZKO=Qw7sUbYp5H)YkjAn*1pB!gT+qeT{Nx(E$xjUIF z{W^^}nA?S)^IaS#cV1+Dn0YxC=f7rPTp?~t(?Z$_0~h?x>x{ouFgs7j6;%uCCt+k* zJRPdU_mfD)(=rp&s0CKZmLnky%!E<2vfnWm%Z2q02pYSO`p?-(ZqI!ySnf zssTAUk`+kqJ|vjuGaGk!YO3sr+fd2E*iuVTKRHGQ+NP{dV3g(9kW1-4Pu$jhcISg3 zR4r8=bhw^iL(e^W=(3V3KQ!VVK~>-Y z=cvZZs6ZYy1ZuHLBcVbbOkvwXoyDBLxcU4)Z56lDs;{k;+_Rxi_)k;sUR+4+BK zwCbhOEqBtWs?%oa>EoxoxtUvjLUkM^G&dKyQXNC6opOf~Y*H)IU~3ifNtnf} z-Lsz=^&G*{#V^jkzY)(FXdxt3BXNzD8?+%5bn|_V>OB|!2qppWA`TB zto1};3|i2bO@9}1c7mK;n^T_R4lKoha9`B^QfuwiUm*v_*jqvamQRS3-q)#Er?g01 zORoN#dDfT)3D;;ls#}f1)C2_u!KQ&j_Q9BsEA`$_zKz>`vHHHU`GMMYf;S*wVww;! ztDREvzBCI~ng~GEf!e)(`{sVx{{YrYH9{Vy#J94=%7g}ppB@m}EVD2zt!JDzq0&FS z++v9o6B{iV*jhd+(LPGr>fP940m_jBxjGiS(&4U2(s@_1EdYvkIVD&z(lBFkBDimNNMbX-xvk*G3(x|* ziGgjonYQ;Z2^K9WlcgwMA4V1F#q@0uds%q3=i-d_O?O$TVLm9K`B@#8q?i1-ZFHK$e z7rm*{m?I6%(I$djYXA0jAtr>6G_Lbt9!g(|rq`9mY=94dy>v*52bf zpN(ZVoe=hlit|h6U_v9fWUa5msXmZwh!8kin}aFsF$b-i;^dN1Xq59Q zgsj3=YZ)Gl7)a`cS1vxboav0AcV!q4zktiS5{@|11BM023w}=Ix~@t`(y{pVBRrXy zeE-L%Gzt$ZQtWy(a+M)xov0S3Cne6Z=`$QbfkA>@1KD(s^83fcHy!%@8tZXfpGgpi z)aL@17Extn*h2uBC7t8uy6a@;l}6U4#olHg7rOmK&(?b@I%e>TvdTe$9w3=mgqQg< zNvF?rn_K@!ZCV`4ITRs8?=Gk;EHTy1dSokjly|EoHv_1Z7$f4v3@Dg88od@x_P&O9Snp@l(Ih^hn+ zag_uw@_FXjJ6+qOPgEhhj3Y~xK*b0^Pm(T;nd_xXK5K^8k(1NZ4|3A*7V-fKo-H& zgtl2~D*+ydOeX-61U;_1ht?m78hUgUDYSbh2@8!R9= zD%HS^YTOWF601AVR+MCY_-45A>LwVaG;YyQs~rpkrqf=MY3oyzq*bOyD*c~Yt z-@8I(nmb~_%4p1W zDwz@*@*)dpc#1~fgq`mOjJhf=YUK^@!(Djqmv_D(jm!ON5-_F|S+3whF0Rcchm0Nf zBfkCe;m@e+2JZW)gyGJA)?3_v^mGbNZA1yl>{zDyt}8Na8YX8ysxFqR~2C|w8A8K_~X zmV0ODLtsV*1)1XP@w>2eDLk2&03Pgx>tn^tBSOs!-{=AqOA;(EH446s@(%G#lTIjT zEE%PZp8_ZK{XtK5Si~9l6BBCiMGn+;&0PE!z@xx``=}GXRKNFoyAze(2d=Sdg>e>~ zt6jkn#TIS3zb;*j&)aT0Vzrlz@-%gZDeVWlHeJ`LUsSd3%Wg=V23_V-7jOC#zy4#V z)xHIN=9V1jO#lVgXF-TUWj-}}8BHkx#kM2F4zM?NV)QQ;ji>z01b7{i5me563p3(> z^X%b~?|9-{y-_xx#pyFx8v2liAGt?SrgOwBi+S8$eaDyD`bOxg%vVcZzbDGre(HT; zwGrtf$Rf08WYNAGg9cn*bfnzA_Cw@B(PPHHi`z;_F}U*}-lhicW~jYVWB;S5Ni39- zN-U^wic3xDYWnym*PF8J?@zc}2c{4p48EcZre;nJs^U<*Kx9-sb}n3$bu}-i(xvFH zA6zY~KNn0HIv|Vbl+3Sz_E_iK<%r4wITMGpsm50lVA0hmQc&DqBcD-Hy)ycsV3f>} zZ4U5^4zt*%6odPIzi2bE%1m~QT(-~#OCh-pZ~X!ez47@= z=V6?^fq%W&-Fo=@dwKSb%OG2h=f0G?!=5kq`Wu>Ba9gLKF7p8#&qj1+>di~bQI(;4 zbGT9KXqmje>1q4YZ9EH0I_+UQ`1myV?Wsdw6CT1S;VO({K)@}IXp4ybpr(=4>nk_? zNX9}2+oj9AfB`LSYL=S*^rY*)xwntb_ks#@jDWYNVc`4gqIV<^GG**OD>_{AdvR~^^oN;n>(0w}iH4RKaEh`O>YR;=LaQijHH}EZ z>lfab2VvXgul9Ku`>^8oik|ZvVHa2-rIJj&3SKcy$thSh=I*~6`+fU8$hLV0?>vEn zHLy2hu>J7t_TWOtF?_y=AfH5ZFUw1o_#Z#Yp2m=KhN9ka2z_yx`FVZew>sqZ=NykU zhruMYDvc_Eg-}P^a^jF+(&J zI0v73!BQ2ZN>To4O~8}E%i^G9G_4PzH12xVHxnB4|Ggy?V zk~POEk9J_6P=np}=s1}4`rLdU)#pS8EPYv9&g2VBf`9SsHITWV#}qLQ|1qgDfWF%T zge_h@pB~{Fw*^0-KQ$+jPulaF!c$6;)N{aB0*g|npraF5=V#i#gGzB9g`C`;?y1;* zek{7PdxsPsK)yvzv-LBgUI>l~n=-|EWlxAN^O;>C7!<@ys|Ds`!{lw1e5MOu8q+^n;bq_@Z&84g-sbn z*}}p2LsYE_XF(H^X_iRi45vR)w73!>C#QANhtq|ZG+(~q90XS-NNfA1Rwq8Brp6z! zvty_VdF82CZ%IsXLQN|$=8f}+g>U|5b3xvC_7Sga`)Lzs*R;(Iud2sAss1~Nh%~6) z$q=z4oV)0I=nb>q*}C<28=P1{V&0|qKJ~%hr}#dkxNAi7ew4vKwqWB6X3-&`!jYwx zG6*K4;mL0OjBR~bAnQ%yzKOE>E9H6oGsb9BonOr#Gd?qa*f{ZD;roY~324Y}_bF@y zNk*G!9&|^hM8#-@L4l@4mQ4j0GGowiM$ViP((*6_SBO+_7X@bXwGIEh()m-m@W3zv zAPCd~DUt(!V^MM8m7-G#-ol8E4gri6H$$_5b>Fk9Kwxcy<)7}3fv9o9V7He8n@^bI z(HRanjZYE~B-`x4RH876!J1t6B@6$VDA=FbRwn@@Jcz)MDH(p5+M!D5VcbqqP2SJ# z4+)2VspA!#Y!*9vTbl{kHYYw-C#xA2Xi{B^q@`(JXctyA`q<~<`z@^=u=7I@YO+lij;91vr-}xkS?gKp+vSHL3=xT zga;S;qxy`7`;I83tgt(unx>#5*dG9{j9*;c53jx4Py0sq$m`U_&>@}ENUM@Q(Uclp z>emhO&ej>t_~7?gtb*AMTlXiCXns>YlkOZN+hg1vUj^afxnBbFzK%h1)O!YWUbjUK z2a6!tfAe|S;gPMWYn@I(C6_Vkibd^s=P z+~>CLrj12b*Nrs1OT?&+tF1r`xDlqD*saAmMw%&FP!Q-UaAKrNd5N;;iKiz}^4OR$vX%O5C`z_k)CaH)4j$B+WfK2)rT){;Z=V-(E?O_@Qx%~`s zxKI^=Lre$C5c?2Rhf%n+r~-`~1Y@SC&T#=u5n~KPGQI0aJze=*yU1q8PO3LA+Js@t zlXSo0#7*x3?nmwSGnqqteqlqSogRmlt`z)QII6DVC^#zXpFg|zGr--KTlxS9Ffi?A zBaLsM<1C?lsFD(o<4N6-OuYxr?MHmuKlKPU;77{sy5qYx-|ri*Jr_N(DOM;_Q8COi z77Qk(Z*yLJeQsZ?k?FqcExW)_P*CoYM#%0B^ZU*y40@5X{c;H_m9w9)W=M)OWt$d9 z5SI!^k~K0o!VoPqfK`u@k(|Y5C#Pk0aq81w=an&XMB**lmuJOqbpKJWB9Hy&V zAEw`9>kWT;!7nXcXbbp#blPgDz;t+ATAzj>0n!h~{yhm=L0wn>Q^)P7JvV;Vr{%`d zI@&sc^9kUo{bFQT4vJJnO5Z<;+Zf)Xz2eif^euAzBJptCsh@>e(_l#fOMNvOJttqC z{Y7~}nJOUD=vX;7XV|css2noX_CKOrp8b&?%aA#<(xbQ0L}Kk0roUsQQ)LzV$Q$z33f7zh#M=K z>moe$wm$36|Ig&6J z%{U``fm&8bSPx{lLUYpW!e!Zqw$kdjzZvX>Zm+Vz3;gr6{Pdk(mA%*6Q`tV}b01=I zzd>y?Uu6w1D!b5U;^=&Ck)K-;!!(}Rx{znYhIq^3}y*;34hsnwPL^$*oI)S zv9iNh{{sIDc=tD$CkBkEByklb-9ZPhJ`L^fev(26%+_|g&gqn?jxksBT)$-xu~LV2 z&dkH3ko$<$d}IjiS66bO9VHKV!alZvjAsI@Xz0ELnNDH{+*JSY57K%UVhF;M?C0#F znJ&kybo2Wl0Jkmtff4FbVo=6iK~6Y8yYs~2GAHNYj!4h!C@0bTbah4lcaHWsNadS2 z>H&0ZSV9`-3atW3E&x@YHR*6Y&qPUj8+=f$`cq|TMv|f!S;r(d5Ns79a;bl|f%>5S zks*d97CAerj7Q<%;O3KkHp2sbw*8u8N@kXUiDU5bkQvno)jD%cv~i6%#;^&Hzh!qD zGP_@~YBs`{kE*D>%_qrNM$ccoaWr*KDFd0#&jc!)b^OwEmsQ(3`g+E7Q-mev zZBiv_iL3g{OW2&2Wk5GmpC24+ zJFf-5T7s4$G-!Q6oh0OU3R9PQm6jhj6hp@>AT}K#xfvK0i(D2yA3Q?*sG#MGPVT{8pOe?qJ zT)h2Mey7ROo7@@<-a>6ZKh>uMdd>KupIf^pc?^`=Wx7p^hRvXcreI2FN5} zD~f$0Hn}rq5x<((v)Hp5!j#C#{zS0Xyh})Um^A~T&eXRh#Si5zB(bM*qMFT3Fal&G zQfB8CY6#=gFNRx7(lQj7bMENcpKqJFz4vopU-$iju_ijSU3!rUbG~(uX zg&}Fx5u#HStI(Hmh#MoMYXxB#ikZP82<22PgvENh_Va?5!KqS?>P^f8)%TVLC~_{j z$4hyCG_Hn2SC==3g}M7@%Cj%Cw#WT_AXhuB=!~uyG^^3$W9BsNI0mFbHbde?E3CT^ zf#xZ5Z>eFLSs!FSrd1|dZ4RiZvyLCRzLU&w{1SvlV;Q!!ckOgPP;7f2WO}@&(jFBp{hf$mZfpmM!xhHtWv^Td;95 zuus-ja1P_e><&0WbH_1cw?|Vi5FeiI#6Du+a;PKjiWKALi>bIAQ|&*>T>W@%a}Gz( z?u}^VlR2rO>Xs+mXmWrq=IBP%N%|tDT9&>;B5~HFN5(ISOBWY$rehQ@mdtBd6~umV zUCG*}_4r%0=nT5N*BIHpbUaOFpSdo+zv4vy-8lXg2b9L?N{>09ms1=8+pN1^9 zzM5lL%=a-`dJW;h9q&k^EN#Du#mb9hQ0mq+wNH2JIvNA zIVN1af+nsA-g2Qx>7s7;SR+Yfp=dH2{|#v8b9FL(qqVTN#^JyC=@WUeXls15i26}n zT~_QO7w3H~&e@qhmd?aZL#%q4H*p>FPw}u&%iXbneH*vF9fuQYasZ8cI19k&_L%U% zGF#3UojKQ1#9~ScUiH_Uo~xvwK_ddVx0W>j=_!-T!diFGR|TbY9j)U%F9@5nf1RQ+ z8FzdWWSJ|d)*{ItK~U&MQ3k&;_D54BB_4L>oNChK3GVNCQwSTq3FiH9wFj#FV?G-ec;BsH&lycPSsMF<3JS6bi&vH)o`0W>wcxo7$D2 zReGbI-1!ap3}nwm{_Va#lIT%Q-n!$aHCQZSwu- zOPZNDB|FW?#4?zZ?X}wL#y4$>TFoq3Ub%G3?#K5R8*?6VvMl`rqC+>4>T90J&LO5r zflOr}eQ7Q`EUlKL&L^Zp;w&znLdvIk@ENrQ#PxL+446i$CtJVeb5DxT4Tc)Um)}MV= z?ZLr*z-HUsk)?~{&j(!cvp=!YOWFTb<$^$SjFJ(l&VBJKfH` z=?l8miJNiui-_E)O-3Y|Cg~=e6_JC4fH&-E|2>!=$k|qB)S}$bMhW{-FchQB*xHeV z7cA%_(Xc7U?(U8Jx%BrXf}P7V#eQaGmlBYnHtS7zlE#Z+&!VpcmWup++vA8YW4od; z7G+D#i%bvz23l&s8Rl+i*mnk+uY%+eV&Z>n#~;6W%) zyAqNP7H?*G94)n_3rSLQ2kTBX-7z?>Y?}%e^ckCB*u7K3qnVq_!-Ey*#)A_dDD_Eg z>Rq}{W&2P$KiT=5g=^aEaX1%)o_H_+XE^7^>WDh$2ts$@R$}KGTW746T}QG`W2gDj z?|lUxx$u)XEl>u{{>nGhe;W}C%Hfp}QjaZ2 zUa%_$!^D4{x&`Vn%w(jb>Q}T(_#CQMO4Or^&axup4!cow&onvaEq^Cz^fIMLDVbYV zoOFC)anMiRGvu}?-)|x2wRje&cS=@UYmDPO2A5Q$7G$*5;t8&LdlAxRkx;81`fW|=0}DVeGW$(0SE|43uXmv1}xW5yCbr;`1cXtX-J&*@NXa9 zP^Z^+-`o-YEpW0zNY|5l{nwh*KfnB$K{fM8DSHWc6!NJbsZBX{tw{p>mF;TPOSSymnE94=7u3Dnuy+Ad84mC5$^HsR|uMuwLafw47jK zC=jO?ru29E9lfmeuIetn!gepLLe(R;lp@3Q|F}QiK-0BcQjSVqzGjAQ&n7N@8V!14 z`6b||PLCqmbW88;(eXtA$%IhpqYLqKC|6+!;8#ulb-FqgS*ek3@Q7Rrw2`_O)CRq> zxvdpuvp0<=uOK+hmylcb7YV1|#yc1Zlr!5n{Ke*XMXHI|^m=h{bLZ0D0=(#6P^#bA z`DQ@oF44#7yI0)S5LVOQyTy3}uc?Tg>y5gHQ5fDzmNtfwMfK*QKBGtX4sNAzLZw1s zkjY{hlH85t3+89-$KOHgx}4y8 z@8RDFqR-CBCw85_(Ve{Hyhxr;qD#Ot1h`;9fD)?{W?d8{fmR|BI^T%HrP!vAl?{pA zaE6{k|2p5ktNrop>ngYn(k$s4&JgFq7N+J1WDj)$pvn<`281v0)xN-FbA>7*6K>@w z=PM7P6pQ6pz#v9kVEAADb`^DHYTv}=`LBlQA7W;kC8}H115iX1#eYjS`mn>Jq*chnawzSYK{U+QnmIj)!oA^rgq8UenJxKxZMi4 z?V3mLS46^_p_mi@QGyzO#yDHZ#YZOz1`GA%S-!uRS(ANlE z85nvsdkHj8IyC!hhLlcY3vcn-`PPF+Ld%`XhvaCV6xbYplyyrak>%tF zBoC4VErts~6iA{Es;)k-`o!#BT!fe1_-8b7$5py&y+hQ&qB1$TFbI<-FSL$s(~a_~ z-N&0By2w=pDsUfMUIFfR5)kD02EE(hF8ad;!^j$8)YN1ev6^8*ov>|$9o#08_q&@~ zK7N5{QGy3mRhd)FZEVN}mW~ni^$eQL=$tCrUav@Q*9N?_OL`Flzl21QY>IFM@kSm=;T1=_AjL2#TphPq2 zSVjlQvYC{04#E9+Hpc6iQ5^Y@yb+0pQO9AOI|QBeStiACo^pmxo|x3r zlC=kkkA{thZQk1cAmJY@>>@-FtoZ4|SKTZ};6?ancY%gTn}wVzLVty}KyzxLlvm)Q zeKwdFI9VS|`}(RrxJ7wS#u)@R?fcZiwbZNa+i!EW@tJ#4>=+$ycefT63To97)I68$ zuN3D@`{v+ooNfbztWMmMy^Ag8eWB17I-MLQM_s7L<>F1Bz5=D`4oz+7^RFu7Kk(ep zKmJgOFp;}m5z%Cw_p{lBJ3{Kyn+Xw+qK+iw?Wbzn5-!J&PMZZSn_o(N{iQqi@A*H{ zkDncCH4!q#_Q_x|e#S7`1!v|WYE;xPiKcO4*UMPlsEebsQiRjW_^GZeFGRxzOsYmj7ZfQX>lZ#aIlr+myggAccET)Fj@@cbyj1^* zWNc6!N!%hM2(&ZVA=HdE&$gUr-`qV0Oke&Wtx#9I{kd%+=@_{NA5$*W1JtHWoAs+_ zr4}l6vz3X9j&twhT%){nY(iwXkfAqL+bR)}FDyn*qYN_X)bg_{96pZ>aT8+r;TogZ zdn44wrUkV1Es^Uj-f6vT?ta1QnQ>20dt(hzt{8D^VegX3wgq-4*XS;K&*pmqDIT21c@RC*eNT3}&o4#3o_FgN zHoIk6>A3)f4x8pBBZzAL2i)7&3xAQgOUjO;fvL$&-wwc%)bOP$W73`d%|5C3+Lp;g zR4fc>96-&zsJjqx%4;>S0VR1P^KRoN+)%+KUjEgtpe6*NO$pB6>R>Z`{z&$=1Hf7k z9SUbQ;ef$X*Fu{owWgGFT#Kq9=PUqf4p~j`2agUDr08FlOi69@Fbx%y=Lt24|7Q-S z2>yM1;v2%{cZc|XzVq}n`-3tjnJhM%C3`aka7KT|DI>41?28ABQDg_&-_;jymv^)G ztvjz;atqS?jqU4iV@3DF>JelvoR3l%x)LrOtX6k$c%{GVkRj-*Ch!?@e*G@3(n{H{ z-Jqn;?X3a4M(7XM^kHU=&*U)(2$hQWRRX_j(J*qqs887kubW1kkn`QH#pX%lktpA>e*&b5v7*lo)sTD=tyjw$b>RLV?R}Gl(D+7RQ1mL^RxGm0rkqoS@8qnr; zI>b;*S(q^R{j;(&3jeh+*L@ZB`Y%=CSe#ML*5&8!?5o>!w1G)!$vV12iQ0{`txS~7 z|A?6F&jTnbWVj3S2=;0*xbo=AfupT6D&h|+hic=@Z#H#ss28)gs2&u7hHCpe8Dfwu z1GC(*ZhE`qQ3dikw!p6bw^H-D2fHG&Fr}q=j_sFC- zh?)7HBG@wIPxrtLK4Ob?B;7_mSoNd}L-Gep7;6=knM1U|Q1ucfCUA}ew82LBvTY)r zONyYcCCAS^$^JlHtlo;?*qEui%{SX}t$rdZg91M%nI|a#QROa^K@tGSFj})MZnUinv?QM9uX?LALWaQ!4%u!=@p{&dG7r z(84RsEPZ%swtNN9Mfo|0Wa01rf_kDEiN510Md_gHzbRo7^KCViX6mKG<31Wpkar)8q44I^w! zP!ek!w0#arf;~y&xDuTP^H)Q`Ym!N+)3A3^9(e>WRr&a&Z=7)3KMDU5NA{Xq|(y zGkynfYo1rQgohh%2OK62hO@>%xJ&b!-Tpzu`+PFqA)m+>hvP>qIxTj8vcQZaFjWBv zv>}=JwPblyXEY5twG8L{!_l&b1MI>nS*n@xve{%+)q2YzeeGl>>s_XulKW4z6;o(A zD0G&t``vTfN~zX)6ACR5NU{#HSH%}8Zu?8%7C$R4FKn+>-JO}uAsnYgC+p1hk(xCf zX*pRY78a4tWl^iEqmvdO;UHoO@y@f5R(TcLZ+fGeUGh$ak#tS40{eF6X5zZ5;n@RJ zCnafWi#!WoKkHq5qGjZ{V)10hXOfGO=yjwlSI5 zwr$(i#I|)}+qP}nnBbl7-goy8IBT7~dsSCgS9d$2oy7np0c+g0BJ;11Rp@vXKBb0D zU3*y6QoOX{O6Xq+7(;lQ<1wv(B|crcu@qn%Ky9sHD^!8(KPjJidylL}9{QZk))RQ# z-ipDSsplQ=;&j{V8{0v_ls@D+Q|z{>zE>B=FX>b+)SxR4+gJd;U>PakenD{itoEHd zJ9@=(7U%oFP{SD1qTQhn8I{OyVz;amo3Z_yy`& zmogd=&;5U1fOz)r>H=`JTqPTQ`BvoAdhYqO$zF7acW^LmX;y4F0FOQ1MoCMZnJoee zf-K`(lSDW7w+j9R|8ROSOHNt(U?K+-S*3h11N}kEol_IVfaM#n?kL7O&0(<0kpiqS zQ%)B3<27_arF&7blS{G_mN9JPT<55@mD=;#keM^xUNLOcWF0&o*67kOb!ChS`Xor) z+K9XRlfga0VfKozc}Y zPK(yQ;7KH|2%Q*?KU5{p^{W1s7S}KBl|AG~A^^}pGps0H7OKrsyGcqcNvLou1n;47 zYU?IxT@F?DktJv9*h7uKi(-=$@ATXBaRDAzl%sm97yJQ+l9LfQYc(WchhIu#FbVLw zS)IRN7aX^P9oSb_!SYj78XCs?oy3ByiiE9wjH(IQ?mU9=UXtOdd=*ogx*)M5! zrYcXD%1XGZuyj_$^`~GiPB6@eDR;{Ov183}88bVOObJ1KiYszQ!LA#m@j}m zL%btSkw?Smd+gKt-#$CMEP=7O7X%PJE;s8E)5ow(-6om2_$3Dy5JmtsU_&Ycu$AyXyrooaj?wZ^- z{3nTz03I*He@(>H5m?v*JHg&!ZEj_o$!#xox8G&jU#2Ir%3je7J1!WN6P2U~Y_bf~ zUK<4Nx1f5vTq|3Wg&RfXIKZfnl0^Gg`trbI_m1~v%mpQuFR z{OCNd_EN!-qYE8KqBq+v^od5b+3|AUW4PzBSQZE|@TzR?>YTWSx zWs*ew$3GmWp4#nb=j~4F+K3Y4?DIs$cfgBgJP@Yh5l-7aOqMH%i|X@viM-C_n7*8p z1g_%*l)oBy%xzhyi9?JSNtL!-(3md%s^&5O?`C|7JS8hMO*j0?>CjB?4!~S{dyZ`` z8!b&Y6mvj<3za6(3AV$@FCw7^ej9kJIgG*)B&rsZaEhH(!+;iwX6Gu> zOjSP<$*rYQU+FG;H<*N1%Tv*i3foB!5F;{CH;zD43>8aJ%h@|pJb_}cI$drLN0BWq z4VRhZE4?w;Uj=K}Nh^+LYqdS*hM&aW1y)a&J6*YBcM{~}pc4RPpxL*0eLz|<_r(Ny>KGPQ#;{rdvjve-xkcUI+@64 zn9+y_5ljSmirwcSfv8C8vWRNtn&=POO>3<#*hlN+R>cBNQf-d>qP^--_4IL!;7B!C z=QTI)ilto{y8HJQoBM-DHzy!b^LB$kpTz$>)?}~~hvyo6;~Spi8ne(~Id|hX`z?N# z7#{7u^u$aApR+ofuL~SP7~9VcJwp#}UH(P71IBH9p^QsehtAwH3&S-B4>#W+9G|-} z-L?$mEqE#(Kd@n^8G1Z(>h;5b61jFj&`F-A?om^`)E>AaSC6UBey|edkb$uG!gi&2#czSrmc zZ{vK|N5)*?iovXaI&7LoCeh{nEH`qEy95bCVsbS@ZCUHKtiyHZ?@B~%&b=AGgDdH3 z*oD>m)$X#joFNh!auULrU>^Q|y0Otglmw$iMT!*GK^F!OQN-hQHosS87f(>Om4gZ% ztHu2rH(2*EeU?e=f!Y^Wjy5NcXs~AWZ`_eh=PjAbwDmrcID!7j0hTKM=5Z1;{5#P} z-keivd^kx*8a?lyCMik~xkF1ur0$P@0d938F9qg_Sx2>qM>{k{5K+-hfv41m5O8Ez z$M-W&;XkW)xe~_RDw;Y}DI@w#JQXA8otz8c@m37#+D4(A0_|LTBF0e>rUgb{=NxR` zhh;8pua0-8gC=EXn1Su}|EQF#G&2{IMB%oeC}216@w&4|cl6h6Va6tQ&gZsp2>jP}F#EXBVJPCJ4Ij8$DU70^KnDHaGi`#!G~Yp{&sGNM zdpy-MzVm&ISGBr83irLH_O&;0nX^9dxC(1XY_A{lB$~o$ZCb8y;oL)=(;q9s zbFW|X^Yg5a1j?ly_-lZYfCh|L9`(bI{g2mLv=?DG*&FG!>#W>kwI))3G{=3@-c+Kb zzPzP=WwCfStXFIOECS_5hvw)i0Cat;_g1%R!Ds_*?9bxNs7p7khJVBdRe4*@{snJ@ zbxRymsK&5_Mi1g|K5b^l7lk~Zw^lk^HXIOZHliDPPJ z96lVAa2OCuG_uf>clrIKQGYFI0nWVrUgnJw*}^ zF4_dj&XLthn1%bls$mnL>$ji!?~1~F95)HAsIq6OG(E6gHNPhrUaM~#%%gA|ww{sL z!b;}(3?i0L&92{B&B**A4 z;@00|>`@O#V1h3x)#(nrTx8aMLzr`(4^wbBBe(HGanl?P-dhPTyfhegSC#lxaaD^1peTD@pDTJ`x@{`7 zofe5WVd516B@DSG_8EdueGO6*)%k<0EV)cO3o&oXcy)BLlZjN3h-sk;;#U@9svJP{ zlMAIJDFTkbMHNlrb*`1Q2{eC}7t5A!yU_EWo--n9K#&iZ2F5y{y8P6F?&hB>hVq!z z1Qt_6bR3(3Y$=p6t!`hs*BL#Pr0w1CxS+0}D6cPxL{6SU&egjtNtW&fm4E4?A8vFe zqXE=hLmKeR7jTV3Z_D>KFjxbDr{{-p{XKlr^RR!?5--Q0Dqta$BtJD&`7>tIY7FOQJtduX0%-aaXc=UMY6bxoZ; zg6XPzbGqROeX(BbK?_=R66_G6HriA;#Ax;C`o{Lzb4p?ePmyk;$GfKX47?~N8m6kj zWRF@5FjIwnP8-8^x)>|AJoJ;?wrxYf-;-tYQMLDRo~D1|atHTWI$Fm`V(B}mFwal^ ze*)zb{j1H=SLyc7e7^VGk zC1)l~?mmyZCwTQE41?fcl_R$HbC;I5&Dhz12la_p8+eNpSX{DiBC0{S``PhLgf9*~ zEqMk;jjt0VaZWD=j|>yqj=kmP9x}lfP*3L&sR=Hxjg`O~**M%+kVj)FELg8P6D{q) zbIv@|CG!w<4dyF=?GMTCwnyW~bjp=~wSWrje7v!^h+rV;X54-+57Pxws-aeqMVy~Dem0C6 z-@GQfz15Cog0?_G6;WDF?jg&JKg_jo$_d{Ken#NK`|CWSXa9E9z%HK_L=Uw2=f>Sr zTg6lJ<=@y0Gs3zAEiN&gir|E!6c|q6H7W*}?u;!JUW_%JAjMET$@@ym0oJrR72pZ! znSCfx5xM$Wac8udw|~p()`nx!%OoUnHCe|IOB9?RuLjKo+tWGk_{~erOg!vT9m}TQ z)1Ut_ur@p=>)bV2y7GLInB{e;MIABOH&VY%POjf>!TDYIk>snEtc2sQ@ue~kmo>c7 zE9A6dw}IaWE9%~#IHQ(us%S;vmNKJ?r&WU5plBj3Fb+vG=^##}du(!kbabFG0IrjV zjXKObgzql|3AUnk0V_@aV$#9|MBSH(?+l9Jj-qLyLxdu;)7W!8kCYTtICa`}Xjx^T zWg(8uv5%S`gJf~de(S$<2<+jKvhw@S^aF0Xav%hpAUM8zY224)zH0R_j)d&CoRL0F zWJ>1@98R#{J0E{QU_-oH{W^ihtguX5C%TA3WI+KoOo|Jcb=d;ylHp;fhcy`T{d5Na ziXK0X&DD{V6^zzBxHP0(-&R7EeT1y|(25NWkn%KrOGN*6{+?Zo#Y(}~YAsq4Snk>a zlwlo@H`S=mAcYGj)zG=}f03PN14oK0&(vX|eV{TMDH^LDVt zH9fHoL2Uo=b0Bq&;NW7KiWZ&bFP9W4e;RKE&|rC7<4PhKQdr5l~)o{}2F(zphOkL^J zo^L*UG^SHDWK+2^@Q-M=YW_MSCxir-TSGhb?7bg9@!q_j)VbL*{BylB@^s8FEd)|2 z{VKlSm22(P>y?Kg>Q?pa`L*BRF}CzSigd#1nu-fAcUhz8iLS8Coz*dC)B#-RN%~a? zs91?fOp2mB&IRhr8AqBie}pXa+d#@)F<~xB{?xG4fHo?M@u=jGBr_wHnD0NZab}wg zvKw--^LJ>L>ymD~uWfk1Dk_Yk_k{~pm!sp_H~)4Y-Tco@0+cC)G1Hd2F#7Y(d;26N z&D74m7D^>-JC+zAvDw1)$ux+NWH_$t)%C`$P}QIro!R2NKVWdVd;)LWr(pUH(?O{9 zMBC&eiGV8CIVQe|e)R_OYjc}U=gr~R19=OWd;!uVq@Ew!e7kd%7L_Ep_9V-8nXVyQ zht=S_XZA0zJ{U&hkZ#+o){a*)xgz2%n)VMeh21QK`zCg|TP`{C2%s&z9MkHG%zCH$ zS$T>&Lnr?NRW^7vpn0A~A^-h@A#yN+0WEn=Tlir!pT5wpFY?8Y3a75huwH$xkI|G3 zNtQ^m+9nB*z{Xa@18Y@VQ7_hHuMscJxOy&*7NaMpKYNrd!*#aBNGcaY8##CKB*bow zDH{%5(v+%6Wld^-IyJIC{2YhS7lK-gwwG17X6^WRIw$qDK`fj=3S`HQ8v-3MX1rHe zn@-<+A4B5bfiLARZ`jHfU#Ghb?WU9$PE^a|w<=~kfrlDT7XsMsij-ryY$vjgT=e{H zO?#J0lf6lYNn^QD)>Zp$gkOc;`rM*%`{Q3wL@3*;iFOtkpR|fm0jF0_QZR=L;lD9(5L`|wC20J&| ze-Brit+4bCZB|!cSHZ60tIf2mNww=pjZe4{6D+MCeB%@HP>aVro&Q?C{X=EKxz$SL zga%RERRACsmvAo}Ke;%V27GU=<-{c7Y5QL)678<(D z2s9smKgxORAaunZl}!^P2>`E)Js5Yz#t=sfZeRiDUXHt;IB(wf2!@Tpk%n+~UIg5Y zo5jePEWSE4u}~`vPhm&ja(=~YC6iUK6^5nHDJHHIb@Z}EHHv7hnPP3?@3r~#N- zAWnEV2bLZeEO3Rv&By*HfdQTLrnY*(tG0S;u)sE2iNKrD^}p4TRufxzWjMp0Q!M98 z2?eh$D823nq-&3vE@`{Wy=~wSO80@E2U3ODOl;y5hAk8j;sOgNDPKLu=2K4vi|ZZF zB3r?*OG#3Akvu-?nNFJi^%oPrX)f)7Q*X9r4LbT*dAt1_qdW6rqFw5T^C%zR*CAnP z`MsGb`ID}%U?{IP8KquXL^H~2|6Tjf7l?J1q9cCg`)19z&CF+Wq+|5U?4%1Ci&SV! z_e}`zcOBA+8@exlB9?8>=d;v24%v-S0*^IoWS#WgwBGlOGo!KQQxe|Fnt`J9P z2Wo~JUC-YpC>RBjU>4P9rc{)A6H+$&y$7TQ9@i+WxOroU3s*@fnW*}NRybXe@I37}-UT7CFW9iY8Z;HnmeFVT$2P`|2v zXzZGBcjsWaVbw^nPU+(Er(iUkeEFaKpk|*+hF%pL&D)3+$M$H;&NtUF2G8J=!}U_+ zfkmXO<*t4~B6CM=FOTn`-d*+%XJqBlDdir{jgP>=_xZ@#qW+qH><{L_A#>vB2R4$J zsXw#Zw+_oVowCcKMhZ*;p~*Hh+m&X*)*)`4Pj2pOT7J++u&UPdew}qhhb#@hG9i}# z+ID!1rsU~tUg4_2HGYRy-XjxwU>q3#;y!*OM*^J&);_II;@RE`r}ulHNYM?H zcS0`PL^W?AZrdv_O)Wil$4{yg7`FbnlahbW+n+yFMHDp<-E1Pm>HWwl!aKjcd#$!{ z__sWAa}IfvT~hu$ViSnyc~tA7Ybh&W)?0lb4m}gkQ*=-UVd*90YYdqp&9r`jC-?~4 zs076Hp5<_7pzN?0d_cIyx~;OQAEIHH0 zmRVUyEK;iTZV8jM`0Xf!$;z5d;y6o%Y5>%F+b1~tH%1C}ahl#03EP|~A^xbnhL z>+L}GO9INS@3tq}zvY9WIO}NcuPC6gZ zZc2=&gLb9J-q6S0S3}(cR{3@j1}58tGmA|Up@nH>VJ?P&2ugj$c#L`wh4&1=R*gP7 zwUqQ(s?EYmQKh9;b?(r&$4n$D? z?`ctu?Te}XSrJ|S2rJt%hC06Mh~+4Acn{0;zqtA{bqsn5I_r2Cqw8gSXlJ!*%GqcyyVBON%lVG{^WV+~UIv+;I2?weDVFa^ zg_K3B-*3BJRlf9v3{@ie1z>0tlvXtNeoNeNv22eL@rp;&4=J{-3UY~n&2F_Q%Rkfe zWI6Mp+UYU|zoaP8vs5vU9$GguF5r`cg=dGZ)Hu!5eFtd}XwzM3ULa=#G;mXBPb5Y6{Gis{c3|_dq&E_(502$V-pPaR1V;c67 z$8nkJ??)#1=ojbIV40Ndvo&Pyym79bix%F~Yc_}74%7OptvU@zQME@0R0%js)=#zRi@OqO1*&Q&a!F{$ z>3GbHMQ=CjAHYQxN(nu&RAYd?s#+;3i`JYZgZK%(#VhL3YN38Bb<>qv=^9QyWh*zA zvzo1?yBY*ji1h3nlM243qZJt74A;@aJTIb{FdfK&U=$h3Nc|mqMJqk~e3dAZMd4Tk zps$HyG{dHT7}$cXK#TWhLtI%;OD1cl4N@A6pNSoQU34KqRDlE#UyoIDoz5W-p$jE} zBufz$)QXutWh@ZX=Sog7rp}C|j~}S9uVG{*t?yb{pIW-UC{a=mX}AfYI{~!_YaE0t z9v}+l8Qz%BvaPm7<-SjS4~n??CEz??Qj94QOxaAd+QU$Sj!9;pt)oUbh*mOIm%Lt% zTpy)u{W9u~3V(IQc&`gs2#z!A7uv48Tt@4NjqtVJ&)Zy0yk zTH8Zv=7Ptt5OW#tHO&JbS+MGB&}w?;&5kBfRhhURK5AJd)pgT*1Ycc%I>&f~-6mS3 zHxySw-O_WClMuPHlRTrA;FV`DRk6KCGrbt1bif2-T=YcAAV7#lH5}loNd;3ZFN&8> z$AnUvvf3mTq+D?K%z5MaFIO>_L224cGv=*0OT z!Pr~WY>W(~nr4B%(%7jKRQx8cua0jJi^Ym0l)LR2=AirC<-gI++bEO3MGttf)%pdwsJAd3Jtls zF)doU#uBy;oMeKe!uSW;;Ed&$#+^1<=l-~e0-6gwm>WZGh*ouj%HlfpXt@y8!2TL6 z*RjE&TC%9soLrX6o~aMZKewAw4LjWgrS47U@a5!Yy(GPql3vO6;_Sw;)BP-B6gkZy z1vDXt?f>%v2)C~r=!$;~=6+(`_&iySW++vW9(;^S^+e5Z;1mrjv5xZl(28XMAj=g8hgAYKv_lB*;fR{Q9)u-p{kG{2A%jOLHOue&==tg;5lM>naSR)!WytTdi+LE*v#Pe%5hn`);E5sW$EadjriNqQ7W(|I52m z`$JHy*o(FuaQ~J@b_2r9HS(=!H8P%z6Ht+gZaaGJy*jOTqAX0=EEQ0aEy+z1;y$lN zUUU5ZbskWn8Jn0wL=JeWo@y!+nr-Bpq{HQ zB-Jn8b~=<&&#O+&4%kveq=1&%T(EX(CTxAjIPzH$7t|^D*7P z9GCFi5v7T##g66Z5)h$YldX*ROXK}=+d5lW1|97~@sbFRC*01NVr)#?so3G0`GdO30R8YL-2D2KGP} zBPv)EvIjPIQ|9rFIFL4_(AUa`QH-!-vWjorxB^Nox=;-m9;oep@PjT@OWD7DO0B(^ z*dz!6!%xLHI%3wGb+yBcp~LOU^ajvH>PF7{d}@@H&UrAFNUUWQQvbEUn{x)4GKU_P>O#r#-Xv(qOpYjhjMxEf&aF8& z2c8_*42`#v<`!v(*BPqjdrVNref+r3EH^D3PhXQ@E|Y>qoT|SF;&}$<6|j7W-|hI0 zs^>-(w}K;D&4k{870+URl1eM7T#V%Ek#M$yH|ixH&fR4m?~L2(YBt1i*$>~&H0c;YhW(Q6Xd-@QTJE0x`uufwZWaYsj_ z!q!59y~|qh=v&-UN(G$5*iq(NC(yOgDTKI#8$0eW=&x?cSKk9XHwRaCon+eAFfnW) z^-k1^SbtG3sh#CS=E1Wk5Zl5VUi@*uqFT!oX}tanhtphWYFJ6YN|$6DQhI9{T^ZO_ zgyakj?@M+ID(72LjMBf}Hz(|Z5SV{dgLQA}7?MS*FI#^4CJ>Bl+I=>rem6YR+Afeo z0wRfJ9AZkEZUw>8k%9Z4?T~hP=27jxz)F>dRvjmqOjdiuIs}Wd3UVxm)t+X-6_P0w zlfh5f@o~K5_`Xf(*&4C?4C@uZp3N7dNdSwGXBVi+lT`|Q+p3M6mEG22W$D<6_G}_n zG0PcN2)Yv!c0OE6W2kQYjudWmTT60=T;myXA*9=+#h3bv01ev zQ`8${y+1U0BW>D~)}$j7F3U~R#9$c$ow*`v0*e-a6-%Ubm8Gd#ldW3(9-HK_ynfI6 z44e_iY@}9d)vL*pg4^2)Mr_Hl`Ic#x>kYQ^!!dPa7CnfLlzngfm;*B522&77v(}#^ zyZLJL&8M-K2f)e~;f&WkC5&Z!W=;A*@%KvySGOBP6m*I*)>+{wOQw$B+}ax2>sO|3 zfUEH4LAkbGro80BDdyLEb&t3>Zn-OGsl9BFdysq4Pg4?NjKU$sIJU$XZdF5W&AK#9 zy(%{y3QL73rGr`XI?4-{zNE6<9|(}E74+$Z3ho-#0K;wP>0f~~gu9Z$c3pqH#*ba^ zo$Ned{2nq14#Xh*z>JusQ#>~4LiGNveUHB2yZ@3|5#)+d7egItES~y0X7hgS@l8t= z_&oYy45DdMbcYWViIX*ze~8w(@s@4QN z;rf6vQIy?u!nKn3T$@SRw`ph0do2w`m_cts3MhQFC`PtgS?A@p2J4TV^DeRekesc0P&U7pG$) zsg#Q}r=MwfVl7@|o9!i@3DCTwI7h;*P!SRBVs*w2o6OE>we*OIWz&@xk)jXnHqqdk zPxqXFXLegxAbFb3-4xQYGMvm=klm<$f@!9sF-nRwk`BE?Rs-2JV>i&u_L-KJw)<{FaKbvkRtf z2TcpiFs6}SW87BL)}nH?bCyogUE8W(0;#$cLzJ??6FPdv(l{r!<`b2kYMX*cPoVJ{ zDl;w8Zx7!^&G4$iCHBekgG;_o!g6f0Bjc#=3kuy4E8r2Jl!I`iC(Ro|&x$O-B&`!X z_B0Q>{5XT^;1g{7rZ*T&4E)FUWtT@1shZEpzqc(Mq_)N7C7rJFrOKj|*>%YL-{D)U z{!`a=Q~owkJ!adT2QQZ%L4ErF+%b);K@E9lU`E(y@EO^4f8fMSt@^_;2WdqABofT4 zUZQLAtN{S{SV;E2wh1_hS(s_8&Qx@&Uvm;msVkX{x*TroK7E4*l`vlmh-ya23?m(D zwpIi?1FUZsL#y3W&VQfn4=$5wb!reMYyigli4y2Fwv-D*Baq2d;f-J|GE&BdkjdyJ zIT`dbCS-#_W;&8fePk?nvjhpj6n;=`ez!ra@wUdZ?IFf1%Qux#Tg>NGAc>6q`2G;M zHkjC+Xnjwyf*k3`56gqiOQNh>pY35R{s(M0w0wWakx z3fP`VYML`m=fjvpu6C@ZI48+OP1sy`)UUFuGPD4Hr2e zGQEA~WV6U`c+Zy}BDA2EdXd=_U30=$Id!-SZx<8|{AI;e>AMys?ALm42L+>y;5hMn zMJ@^ey#5MlEs_i1q;ig4jtUsGcBqaaVTJA<4;w2_Z~Z$?@4{y70$%0Bh{<9!8BLg> z^ST2j@juYE9))!63PrHW>K9woyeH14UZnKsYmd68u;dEr`+Jfp752x3x^z#*X#mCp z?{K(WuEF^Xu<^bOM+y&=z+)b+$a1@hbQHJi+AM2Uusd@u+-Rr5;R8cIUiCz1X9yQ;5T1@k(k?vYHIG4r3S*TB1sVcc)B15j1 zDptl>oS>n`|7vQ*aZ2NY~> z=NNpl0EE)O(9Ib%%VK$s??93MMVEW|lI&{L^%BdMZTP2SF#b`#wd*Koi#6mZ$uTx8 zROt^3QSMz>`90*)u0#K$12F((EO?RZNBVuubE>$|ifOyHX&+nyZOH*lR{d%aSazOJ zZa&s(FGgmCku4?fMYUEd}TG6MS0s~WeQDj{x zsSH#~la)z)E{)S<>V@b9+#VW=myV_}U%zow$f5I+Ru_#3;XIVi5k?86@p1jc1>7|JM@$iPQC$6(Q-sau92leniXy9YIp196%HCX^s0Vl@Qod-YU2aI|e16I#bf5l(4{3on%g zmrv>t93aF<3L?Ac=9=g^1vIQ^R1RC|w|jSCXfHd^Q9COpOCj{C7E0)YAR|{nA+lkX zg3)iZ&<-(OjX^1WoM;c%rKSBqS^AUEcx#^xc&dBf+{$58i$G4r9fYjNaIwg^59Gy~ zHb&&7($S`e8JAhG@N790ZGxG~+uSq<896bN5c@M!wf`#!o)%!I_c^oXaJ%P~mtnm<^`F2u+Aw?d0H7|*Inf%kJ$gBKAk^Q1RXQlq zF>tO=VK_GC?r6q4-mL3Iqjp51#58zkO6A>77}j2wyep5Vc`Q`MdPYCZn`Z z8BIVsTG8>(5RcB<7oV*58=(FsO$?%C`z1iw4yO&L-BnXs4$aQky3DI80!huEuZoJs zuzHkG*G2|jWf~va85E65Ml-9@(aKdw!|8SP7D$a5wECkIDsUw(NjNOg7*Emgle2Nz z^jCY-MsxOl`@&6xCCfAI-8bN?V2BdANs0IJ)U<8^5Vc|lGxfG>%0l^H~DmO(%1=r3mMpP#GfUywn*pn;fiJ%koz=??l#;JDb%{g*PFtQ^BgGQ_5;j!O@{!&YAAzM{67b}K#& zrHuXZtYElAdZpTH&-XuwwrFb0q`_Vi5)x@}@F`f*pTx8dMzIx4!R)<}2HRw;u#`XP zVaR#Vs?vmKyfZ~=`P9i8c8f~AtjM@?h!YgR&D9K69AWBQ9TdDW$DT5n!Y=(s)5^fxV7^+NKSTW}c;cWOZp&_!MnZ?Fw)G&B@o=I3~A&E*c zcqC6b=Qfrq>`VdkS2X2nrU$)bZ#%4e^kRVE=-jILPOIme-*U0*``+etSiEN_gH43@ zag?OxX{GZAi21z!^z?CG_9Wh2h{A8$*!ji(@|zPbPi(@YLnbd-zK|#*dtR}A z%vk}PHuP4zX14L#_W%_lPyI* zB30t*E;T`Pf(PUY*XvM^zM#NN9%XB=kKV>8V8eDgkd^-8@SGo91lmGcACy`MrUXHl zGF-yvFHLn&FlBS)Ez!zV|Is4<(DuKDaI)&dhWTUK%f^jIa}zrG_rJ-P>gZ6@Ssvz> zj~~yrNdI(o0L48X7O9=!ilgsnI7iB03N{EW%IGkRE_!bTvT784#bwT=BgW}48%yP@ zH$D6BlB-q5inJ7yz=YyircA9&tvhq(BJj%njK4XdM({@6^SyfGHC4^_YAofzoc(vd9_{JHn|Z$9c(*qpYLEpM64Y%v8-b6mU`Z(W5%#H zqj1uypJb68LzZvOi>ecB2b}8?Cm$rlBzFV5tz;c2G40`*h#HcGOgRu77bX6!ON$c` zXONA1yHdK2dzMz&OvYmq32|wZ&qY@ZGLc#YDoIu7FX?AFQ~u1isq&OCn8KcP4v7h# zho4jjD}*oPe<&Z`8*~eyGvVPpD{2PQ!}^1ncm8K;Z7Y4r8aXHS)drYyCABZ@HNs8k zRW#9}P(&uE$}zLjCgYZx$pcUbqTT#O6})hMoU879q;amWYe^-Ynl-(uMA7%~eNfXdopZvMXTwA>yQ)K2k6?9OF#RqEm$G=c9vY?bZol;xaR4y>Tzg{WrwtxfJ-YmRRlqs3qr z!s@btz}(F#Q>D|9vSHVgeIctOep~Kuz%u!@_W7~2*K;3HWw|%tVKxWGzs43(%Ag%2 z_j}FI8O#;v`k?eg>R7(oH-DmhiQSoywhA@!jQ-u!r_CA`gf``yt~Y(a>LtH1Kn6YOpdXH|$DpuV$~9OIW8&1@rV7^IU#ej`1tTl}Eu%Lzpt z%J{@km}A(9jA@^eSyVxh%DlLyYA0IW+&iF2X@6I}nyfVC1DH!l&x z3TI);WmoEc#s{7@*M+jmFi{WEi#f)G%;(cOP!-TI?!#n88}o}@>W`Rip5Q6%ykie| z**$@~sv@TTi4sK)ANea`g0+7r z25}CKvuAasnC{pAm@~CE2%xWZU+~$pXyB6LRHl%?LnG&WY!ZNgXB?Cu&vnn5u=`xQ z6s*WLn#S{_*p<#7pmuva3ar8_cD^Z&+9kPZjoNG7#6u{E1GXm9TPQ%&t1W>0YIF>353SSm)5?AQ zxs<8FD0E;1zTswdnY|6K z@4^*sVeU5abQbYhNtr>B@;FVHH@`9}rWLG45q_+tP$# zQHga~^P@Xly~acr*edqn32BUhe{sbz{}s(rbb};C)M>TC87}Nr=%f-X*YC~00|sg| z;6dt0(?zUQ4H^fMkTQ^IJpMGEq$72bOVJpt(CGHfbRRE$JVShAHsAPLrfDg5Uoa`i z)pe7{Jy0eG-gQ)G4Ws(RD4KxFp-JTJEoAx54P5I090H=82jd<|KGs)ld02Oj|r{!@`` z4Hae!YriuFSM?Dq1}kS&`jQWeR6ZS&GBmefU9{)QXmPXj7ALctj z=-#FTlXo5@7Y7fPwL&S%SpKRLz&K7nC`dB>Jp=_e1c`=|SLvVimondn z3`aa~%#yEJt{)Vf>CeLwOE4ZODUo;AjP-{x;?rw4Lu+hHZ+N#<5GX&2^^c3=&Kesj z73m06`J^dV;YOSJ{AE17ta#yScQ1K&Uq`7>FnC57Nz87uJ{YBYOKjV7o-er^7)ce9qe+5W@)eRSwbrxdY>lxol$>Fs&K&vR`-y!FN28FRTLOM8#f7 zSN;IW-{p9sQWu`?&>4%IGm!ehaRce!6u?{SCrml4NrcHRY;8)DwJD31rdGHq3AmP% zsB5+zOe7IB%)(d5pH`|*6;B2buOhpbWe}Ox>2)4b`D&Goy09eKmSmdE_r822y)-s; zvwFimO=Lny;$@poSU6fg-x~q_-*6}vl3QepIv@twi;xWu`XZ~GKLuPDUKA`eEgjO< z*2UOgr8AXea>M_WKHCpe9VzwP@?a$f(6j6|(z$Ch5#bmWTZ%L;^@je80TZU43|-TB zk+4_M-h>aQum05@3CkbPT=zfm#{KtT5G_FsT!``9t8G5Eazpoqd#m+mQ+*$KNHI`7 zvc?j-jOlh#3-kNgE$u6nf%!4;M1;)H)HODL4k@Aa7n?o#P|>z7&+XZ#wJAzagG&YZ zAx-WV@=W_Pdy%5z@~qBqV^=PWr_NXsZfVxyh2siJ_mjgGwcsezzRi@Z9_?mO7!D(4c?dwcf4OptNwH ztA4IU$z2miwQ=IEnan{hn#K2MUS97%vnzfS2huyrxhO_hsDZz%G9K4XITMeUtP9l^ z*S;UN&(&)uR3KKg)@Sni&}m`7J!zOYIE5u_wAmpVqL6|9gpj}jBpNoZ)_2RZS+6_0 z{EW)B`^$8gVwlTwpAtn02=WnKb%0{)IRjge%)z5ETV-;xJSW@jDj_Eqr6$x2K{T&k41dHysrx|%S z8S=QxljTZR_k(fj@T+yRjb=aURpQcvAE+LIFfH?(-Y~AJ^2Nkg{<&j#C2Rwodx?G{ z0(>`Id4W#jao@llHjVlQ52^#8Q)zEPWLcPId0gU&LgXla#78*s!C;4H>tlOg`%XfP zSQIo}_;=Gx^vOtYQkpZDu%v_N4Du|{)v4Okj)>mu`a6e%If;e)33ms*K?_q%bIPwp z0%jE<#cJErWD058HHL`uo)6+TZlt!`9+G5~a$i}JWJUg?a81Puskh(U)!H^rrH2YM zcZ)vi+_fb(2)15#FL%d0H-|n1?}W;3D5X#eWW=>Uv%s)a8NoGgd3v5VIt?#q$~i97 z(YoJ@YaBjl?DTIVg;Esq{sR!nm}ip}ih~X-e-1x$6N&1bRwo1^#12)SaxtO(Vm&Ma z1xwQ-+0(OwD}v6{j_c04&ZiSX{@MbDC&fx*qwn8IMVMM~aVUcCC5#Fke`T?V=Q z{{R?4=e~jU&Do&WgBr4Gc!BG?A$CTPM+YweGjt#@P6)l>^|;4c|Tz-G|#`TtFy>H0KQ$lJM{pWOLV z%zwAVjW4`~z{QO3KfrhY=A%6AwLb*4D!r)&$@S3!IwGa#+Mc=4i;SG%M`ZC&NYQAu z-P9Ad_wk*AB1_5t4jx~+nntgS{)8c0mK*vq}t|F zPGd({i7?K1k@YUIy24h(pqap#ah*rQ6|`}e9@HH(E3c-^OXM^&)|clN*KtXqpnA+CX`b+#ud?ebO$vH+GYwBfU7 zz&{3>S#SLW12Qfe340%)#jGVdnOZ#!C;H+Yi=wjRk%|@=X%vhYRFzZ#F>S`Z;`Q&M z_g*bSpTh7|9}-+N7*;g?4U46d7U`%eYQ*)mzRzXorbLMnCC(k{^*T>~`qLR%GeYg} zYW&N`|HZGvn}AV#(kNdavpD&d+7{N<)-q5TIIVqIYpK<0)Tl8$G>odUW9trXy!J*8 zA34nP4nL1$$vvX>j-QO)Uj^O zFvC&Em+$@r&wBMMU|@hOLqV#JAT^9XJi)rFM~SsX7?d0-ORej@OXxy7E~|v9*P*^p z_$*HgRpXcnj$i+gf6q$~9_LuS%4dG({k-wBpJpErSP->@X+o%{sADh=Qk0Ywhi&Uz znVt^pGhgWo-*Uwss!TgVrSpCX-!iA?(4J>O`~AJgz$%`_$~(T!^C*z4YD-JKnG#Q# z>8`|6=(bDla7uT#ECm--+tfUlTv)3d|0T|BUTjCR4Oqj_GoH@j?|chRI&9KJla?P~ zTBo27jfBJP^cIC@4X^6$Y2GT+9{q#54{T=^2Wdo55YD$R+*1~&!Vh?Zqa3@I@*s=xm>?{H8^S3N9)W15^f6{^E z!UgFBgtH{$k1#iPh~X`JK`y65<&$q+P=r(kCx(PNu||YyGkdJNut?ED0bN>P5$egX zKu;^uVhJhjR4h5|n;7o?e(&qjUZ-+}6Q_l%BF3{&MuylzY`jK$;JPnq9uizNQ>J#O zKLev_yRJ>TAA>I8-n7`prp1BP+YZ>t{%hBRB<(6_SI$BuN|dzSMIu!9ou#=Ju}UGW1fjq z6P%hl#eqWy_{>*6!^0;ZW^Q_p@o=29mNFO2F<+TyKA88%7vQ_Q8o&&l&aQ|~hgS_4 zpqt9j^ep`G(q z``F#w_}bS%y$Z%B2fzD;@A9R;_yRxm&YvdOSSGj5IMC7cA-kx}tdhyHB{3#xvUz@< zYo?|dOAL6()Fb|Sc`0gj~=bEu=sigx9)CMuB?{@-8zZ@mA-zXt>0?4_piQKH1TfBy20d&$@p`sIdOvE%K3wz8zwU3c!u+z>r| z_wUI#|DFST^9S$cy?5^MA%JeSx)SGW0r2!5K5(Bl!F`wgUw%I|P7-m9N1xcs&fDJ% zZVYs+9Scx`OtL8Np3eeEl2IF)s4ex_q36%Fzp})FGufZg>`+YCpjfn4LMaT6a+O&u zn&Z2+{Pj|S#HBQW8BIwm0nGqNJ!4It*eZtXRwGTJ0D)0$i}2^*3y`IZ7trVit~D0IE1XwM)G2tj>e}V zIJbO)>9kI9xr%0yED_dCkx2K^EfOixMK{ZnoM%$ii=KY2M2Qk5N|Y#Z;Y>bI9EoSI z-h3H9|BiQX&-*@$v4W~=^ZoqB+kcbWH$M;Cun<5H2B;>isjgveW|pl(TX^uE2iX0R ztHFBRc3TD?OO#j?PNb`*)U!xPrrry`utEyLyYAdmkWR{Jw795a|BJC{$0<}SIOi~? zj2fSF^kyzxmP<@pNY$bTzsnjP!|*tDbAoxd6=V)1?qT}r1ao}hsNkHT$Qh@uxWk;< zEz1?;oiDa6!j(}c7O4Ka{MJ64yV_ep4Q0Y>p;TE`UyHGp#H8pzg@1SFkMrO=e;1<= zN>dYDsdn*%`8f4Xg?pCs;z}NO0CNR}&_K1ZN!1IOd`Rs|mH#5$G}rE^-0%}n ze0DHE2&1z7v_y##s{_UuMn^{(7#QHiFMKhFjvnIZ_))&~$hX+Hc^_^_aWN=`Rv2|lS{raafesY}YnQ2Z=on(AsoF|Sy!Pg)8I!ES?aAM{J z$AV+bSLR7=N-Jv7id$%)XrM5c*(VeDB-8+%&aS-6t4L)i^iOyd^}CvMl4&_jVy|vj zzP-SI=61QA%j7cFvep|s#cexau!_Cr3U*oL+joD0>tFd=zWMpD@R|Sqd7eIcD}V8U zzu;}pc`G*GZi(|Z76Gk>%xKE@YE{-;zKOXgVm6AncGCu$nO<8M#)Nu;4fDb}=7^?_ zGBY-mCAIRZFK?VW5|pHEtg2{RSXM>a&d`~ft&+7^i5Nue5=N32sucxwia4;&+vHAI zlqtbepG(%Ir3I}~Z70dR*?)EKWC06OvoBhma z8G-=9Dm-J5*;+^(85{{X_}VAG#>Sz`*jV4hh>TFR5osJFIHDk+Y65B+ao4ZBhd+G5 zA0ZVV#w-hKN|floZl(%q1n0cCw=p5g%5}AV^Nu|jDbS&}og}j=XhMy6YP|Q}xretD z&iNNVd@t|2a}OpclUwI))|c))l>lM`h+8N^LPCWAhs6geiaj)xT#Z@?C>Yz!}Ge5!3f1ml|@84jrKs zhHshPDp8`uYC{x7+hQJ0FZUGY^ z3M0aikdfgL;y5NwVp?&F>FH@+@XQzR$bm=LcW@uar;l@R@*vacG>0b-bDZPMRpw|! z4H}gOsmZvIb1K8I@EX7;x(4ac8%A^@_QlD)A7!%3eCH+$=?n||xw>*SS4CG5s{WalY?|U&Q{;Kgy1m>|Dg0lO^7l=(da& zN6t){ijbI)|NNI9Va?Qxzohj69ewx9sMJs4AgEDVY=56Zh zxKvT&Ks-&M6N9L)U$j|*$*S)LKU;^gk{E^?i#ttG8jwgtY&>~&ijcU>7RqhsC5&iQ zui#}AtW&Hy0y63a)r_w2?2}x|!XdqA z#~`V<`=1Dpvukh?iH?C&e*K-l!A&E3AX3Ca!_0d-2ZL0>#VI4zVd@hD{M%>$jay!Q zOYW?c7rI1=f*&FxhzNmr*Vw7>%OC#EioXBuJ3M`#k&c_4^JaF^u{nx{LtgQa;&s9Df*uP);Sk#fwy||=Yxj4%3=ahpz~~x=pMDcpi{asq z{ux)k^o<17DQLkbe)ki+(!QEqFW%W_na>uqMMX#pMs;cIF8o_&g9?ypiVZA*SWXTN zGFJ^z6OxLhRjDu%H^3_MqZJ;VP1rYDXMJU!`Pn(fH`O^fKS{$H8rEQ(Vw^=XQjXX< zTERrY0^`^T%i zrsq3gQmXu3XjCc{;y5MEYgFQhnP{G=HFKn5c;W3oz;_;ckn5W_6Gdjx0CtHI7yGlZ z(!jSa<4)-!Uurn`r4O&HTkG9->}k^vD$2slPscA&Q3Sc@pS?Wy?G)a3=N{f%>|B$w z8|S>b`k5wEE+yp0dq#mHPh-n7|1+2tm=^2Z2_RCJMJ^i}x@gZkdj+S4m4*?T);oquN@zn|;WpP*+eP=GE)1+J_23jc zxbkFeY3!6JQDQZtUazzJs@<#~ThGoNJBgDRdyxn~=C}T?^_(2ImF?K!%PR$OxY=B!Wm{NGqh~WcWY(#3IQ;Vry8u|6I?@d zl3x)W{jFX~WXr-1uf+S()6Hr24(`R|LZwnLcLY+GV1uA5={5B98>u$uc*pBr#SOcj z$xG`4?A*AW|MAgJ@E>3F|FDcu(xNbBwdvBq5P^zVHVzKc1Of&~3RkxVtOsljKK6xA zu`_P5W@?fH|Md54h-bL$VXG0Zrq(vy98MI)RpjW&lwFl@!*^!pm=XIqEX{ zzy$(uc}h$A+~ZCyl2s*2l|TNq|H@1yUGx;k2U`62FaHd)5zJOHs%6ugxs^pnlHf?< zCSt9pD-`4D4xx`~>k@ih{VxAG7@D#|18m9ko1CYyb&d_))7sz#(nl+kT<7uH7P8Ju zbrW64B8IjDehHT$TWd+vlpqMoL0F>1Dz@)@(OKrp)LUHhs=d7Thkuy2{Lx?XflvH# zr%-JY9{Q6h?)BXA@@IPAATNkBnT?;@|7GsG#HxEBR%)Fu42^4_k~;-b zC%k=yNv-$Zu?LqX-D_(h$S;c=TP7vEI+|0my3727jn4}>htbprYb0FYGlfBjN{n-k!DtGm4)O5U{+=*BOjCBS``NDpT@N~jVsKee zg>*3yN|qy#f=mJ4ZqC?Ta>)XpU?@;tib;*NDbA*Qca+mKw==V z#fu~Ke|KpO0d2=lw8xP;55b4?cqaKUkb>_YIMYIHfD`LG0kFiHGw}G9!vz(Qy0VUQ zQMxClOWd;`7#QHlkt1x}xUrl(OO&|85k(OjH*O?~B9bIwXmIGFJWgvZ)> zUwIpM|L%twxaM;Bm#^^LXFZ$g!_y31HU#2nQ%oR*MO#=*z%&Q~&xL+z6!DxlnC#X^ zi@1LROaYBG0gq^a8Hrf;rZ+M^Gs#S4ka1GNfq=OLH1X8kVc>1l*!m<_nEGK=ap1(E zV%rI>+~D-KNk@!XdIpvj47D(E3<=bnWwdD)7Hef_g((D<)GCQt+!>HXY#C=1se~}s zB5=-EMP&xOs3|1k7P)r1@DXvckVpH1`#yz(JRiqV>_C%qaqOpgoabu)%Q+Ky1zm~r z5+?ztRHBGhtF`$2CCdb&sK%o{RJo1us#e#6>-6YR~U&!#jY{QGWJa zzs5Jdat}}6{3B4Q`5>r+*~%kaCowT`ZC%fG(!_4~Q*)@!@X?$W+4anVD& zjePBSXk|@9& z3n^ksoRjHadm5K|s7X`|sen<*^ta4{+mNOS#${i0`*`B(f53=4O2koSbszh_@({aj z{W)MgvyC~TTHR|~W58uxZ{moJB~Zb*(C65U%W5LDaD4vr>a0H=_1{h6lpu_<&$&XR zB98+|=7R&OR1?G6xXRIKSkr)+2xjc!{^V**a>+9P=#DdVl7XUnx?6mbXAVD(O%^Co z$>v-iIPxQ0)l=_uQ9^Y!2cE$>mX3Uh5+yESjE;`toLd#tJR;(C!Yo8##L(an8^$)U zYx^!{W@nh3nq+!*nxn^$^1@>;)OSeHX7zW{iFWk@7d#}X=mW55%oj_dvQ$&0vx^tx+y@VI#$!xs2*Dq)3e|IQ`W~fY> zfca|3VI$aREjb~Mv&y`QsfQ5*0vj62z{GLFdPq|e4mrd8$U2e`5=p>>C=MYgPy}Z8 zPZUupjGu)aR$dd!(;hBm%2ZP0$r7{neVnI4T&cO|=oAQ znrtOj{WP^q0&2MquCr8_LpaGj|eN4k1xN z1b%siiLLkDxu?q?J(ov)@f-G@Qt`ca?%^$k>y>69gM}wk&K>8%pI>k}dx{-8zGzQ^ zvEcZNN>;?BKogf7!!SuC4O+~Qno|%o(fJ00^%OXg(QAqi7-2SL0HK;w=9UwDnfHjFNf1CW}*Ig?R?6B4SDomk1}-qMq&fAQU5~H zOKr62BD00A(k`T&l9l|G#asd~h??#Im68p%PVgw-NG`HoPAw%$lqiuc{i|4aA`C;q zFl2CGkWCvl(Q37buZo$Pp5hrtp1})`zK};AeT2gkhnZ+haNyJdPRI$KkSAzXn@rcH zQL8>_6=$yoFhM8bRiiuMRmonFUrfe=F*a5=u4qwiceDWDur-Ul5HUfby;TR>&E5S2=WR;yJOm@Z)y=ZTw6Dyg>;$Y0^ao0UG{W5nk| zt;MRMaZJTlXo+K38$`1$u6pz!fBz$Ippnc#nDUyt|0n+%R-sY%x^SA3;0OuYE{}>s zai}<~`Zhstq8a+Bh3X1?KJ^(?UlEiQj>!UQ=DaAV&m~fCQ;QbIYe6S$v1X1!^>i^E zs|CkHyvz(dciJV`<`k!xtzPHN%O`6J?rf(cUzVWKFL!rNt`DP%Vb`O z5|_}Nn$r~;;ig;n@~f|V9l!FaPeG-gS>Fkg*<8i0*jQu2R>%|o>ll~0E!^{g&$H`U zg#?%Q&U=>?JC(CWiHp+NDcEd%HEuJutCXBa!h1gk_=VFH@BCa`cUj<>WHWO1)_OUt&>MM~Q)RktoTuJ)ok)O=kg$Up z#+lbaVm2VU6`B!Zo8X=Rb12jL3V|&smwBo~=L=)oc?Or2tcnAdWYmcWNFaFSEr$!R}6t5XA5 zR>x$~?|MrfV{xP=$=H4zNJ?N4lAO0tiql3>s!O}xmD5Uz5+zDpV&;60cGp0CfQ{=n zvipkN+-^_i_J;`a7`m_b1-K>mGZYmd=yV;17TDCwboAeTphI5FvFfhOj6po)pKo(u>K1 z+PSS#V3pS1fK7Mj-bze9lPAr3L}(YIGRp}Q+q5B~8LNYMgIQ#GrjRTl=Cr4WQ9IO~ zi#;524({zrG*-`lSmErB|4ffl&f|IY9*&gla_7Z!kD};|G`KuXd#+z0%dVAuVFY7k z!DoV`yxAp6lvrgjnbygMjr{aazlpi~zeoM%TOiGgh@98@DF|SM5<(09=J)@Gr)}BI z2!kB_>>)0H?Pb0@CG=Kei4v>E-_ia~p(9Ul32*=4H&?X1ci*{(07%kS>0x$VfkJ|1 zNgupWaacO%=f_FOHPS`N#IZX6$h)2Gw=!y!9tZPaL2%KTf5-3A1(wm?3CQ z@X*)(jF7JrvIZDn%WeNBv1}n!MKoVaOx8UnS!UTl9Qy+_0;vO?fW`q1e*12!<_YHA z7Pj8-YN%fYNdIy>I^J-%H8BL&I#|u$il`yqdZBIi{wk z7(Y4AqmMtzp<{=5_~64Fo;u9b+!Xsy?q|L>&r~o)v(jWXobB)oB-vUzll`o~>({@Y zt-H6bD8ZGPIvFLiuhdYnq;X8JZIs)e{|xSW(;eLL@9*Sa4?e{A9()Kk53>Hc%L?wa z%eT^{8KSDhaf}fuZRRs~15pgQ`i)5vg1``3i=<%H4~3RM#RfQ_mKv(fgf&UZXk(6s zws4_PnVDnQr9??etC2vgnF;L?1_3k4Jdvwp)mcg^DP)ueQgD@WDnp#Cplc>iF7=EP zDF(*1Z`XnmFO-YZEcBxoYjJ5Sqef^yT#N2HuOREPt~1ItV@>976ne=rI+LYNON@{v zsi!Ah?k?(CYGCyo+v(0ZS~%u28ij@9@3;SzRLAy?k+bbg(X1m-3fq$vUc-LJAqC3J ze#f?y##HZhJ)LtCk@mht#Ltz5?VVBOu@t|5W}VQjR;zu^1N8w~trpgpMP0Tf$E7Oj z^o%PpU78XnX||2dzpgL=001BWNkl>Ji4r9)A{Yae3Y&lQM|tb3 zU(0)b{{t|x6O+%Q;scWi-oOKn+;fc0x{=Xj6c;AE|2@CQum9k?kU?qNV`cYNiHjON zNiB^X5}3|75Bx`dafRJlc_)%nA8WNXDswb_@EdQqiMRh*Ct3KGtcyzlbqrEjmc5DMo$gRVtQ-ZlYGX9J}RaYP6uOl{EwDI0Nisg!@PsC1EYd zDOw2DDOL;^Fjurl5Bt>h)|lXCXocBnib{bB4gr1G=aJ<_RVrGDN&+CiKiBVOz3tb9ARX5gpC_E za^8Tlc!EHacY7?M-MSRInEO&p5XC`$2l~0h)3c_n61t-6-+@D;xO*$aLvH8 zcRri7)wQSRbZ93~DU;-RuS21#Z39{*_a8dqsbZw7LrX+fObeEO zHaEPN(rRe;eNeh)=tUcrBu}GI>j6uYl-nM%Dyx#Ag?s2u}DeeCLnIyQ1 z_im|cyVx-nH3!e+{y25*#E)}I;?8b+`RC7;;@3IXK4%+Pn$0GyBsp`3aaj$oC6ZsF z9c@4@$!<%OD6z_+KoEc`@A-p2CsC5%)cOzPg$@Z2!+-lPzr_!3`9Y#) zNE!>*Ty`}N-tz#vUwAbPcssG25lgI^@efGF_{5fhH~-05P!LkaQw!I>-=5I4bEoDt9v)25XZUt}T1|4bYNjyWc%NQ+o!aLnSE0%<`MWreQdv&qKx zFr+NJ6xwbGls40h7Ieppm$o-4t?GRSW6c5|=t6~+M1&+wFvhM@n#40a@9bs_vb~M2Qkn7!idLg9C$X-nf}& zvq_x9OwUX+F*(8b$#M1{+|R)i2f26uy*zg6F_JLhxH-;Tb&knslH1qZ&X$oatQ}f= z21g*I2tjT@MG7OzTi^XlocQuTp+g&C9{&Er|C1k@d5Xug_j>U= zP&BJMZd)LpiLw%gv^4S7vJHH3uY+2Y*~kXQolsrBk^k`5ALmbA^-2;O&_ehRANwfZ z92;ful+bKVu`xNt4~~Y^T|zCH=U6n#7bgyLm{DeAltfI{g|3LC1>>_%l1r9IB4|pK zdP=b{@FLNg~Vx{ydwi>h5lwGuuku`F1u2uVnWmO?_pX zbLnaAzpar~bmZLWA5(8PTRV}-(n*r#5=G1Xf7K=H({s??l+lkQ&Jm|6Ns^rLKt8LG zhjWy*5+yEalx+u3vHPu=7MFp|n`tF6eEmBNH)7^uPr*EVtR@ICw3x^63VTf?Rn zD**$|0Y38XKjj_IcqcLtl-IPx>Tqy63~ao4g`HaOy>m}Sv9^eh*=uB!b7>c>z;FKK z9^TfIMD+fRc9I_DI#e_~J9L5z3}fvt7`@!^4?^EP)k{%WG~KfVl^XThM`Q(sVm!-^ zWv42~-qdZ*)ip7HWrNUo(xj18WA z`d?jyZuGoN(+V4A0zUJ$_mc!l)1<`FB*B*IVE$;dMc6vDKAu9T9{J>!Dd&m_+>e#qhMsHs&7elF;qjsJ=f59;1QxQ;LztEW;7b)Z~yXce&#tp<2Ao} z;xuKv=FI%Qg;z}4&Rdcd^ak@yDn`*HhSXpQFzORIPK8tr5*lV&^E@5|{BM8urz98> zvFrz?W~MlmCak-56WgaO>rNeEs4>gP{2Z-{aOL*Rgrl2j4UUmGgCjtULnXyXreWoL zvM7*%T2!O%EcH~n5*fu2uzfILb5dgjXk;{O-rzQf+Cm3Tija1Et(-T|1Fsj%%!6Ye~rrywY+ScgGjp*el`J8iI zRAQ-HmnXm=BM$AA1)UVhaNVpD@jEaEKd!ZB)7bw2W6Kf>#O^YsP3@e(C2mT97+ z(cI2!3B2XQE2JR2^iWNmh?*3 z$D$u;Gd;6}(Inho$-QX$y{O6_XsFuHR#SE)?;z&gceo^O|#8yZ{MU4M{7iGETo~$5A09xa!Ti*bB8@NHRqY#Wxml;fY z5GM999mCMJD?vAcu3h+?ia^|g>PB`v^K}p(p>^onY}k1t#C89j)IlZdJagIlrLL18 z%jB3+QPKbs>)&^Vk}SOX0+OwRPV!qRUBhqht$UFtVNGi`VyrcM;~U=~6-SZ%WqJEY z+U2yxajT2Och1E3ElNA|dmQB)%pL7?)cV zF4pm~(qkK_4^Xew8C^5V_O08QpPy&GG0)`GBqvXuoC&oi&Posp3WSKRt6rjJiEv}UM73ny*4095=~vBuz1ciQWh zs?ut;NRosg2z>XzX@1{YOPZ!Mo6XbyelKQbX3oxJ5a~LWu6r(vMZA1;In9)^oCw%} z;ek3s#$?4=zC%ee;aI1FN{TZHX)EDKSZi-b635!0{PRz|k+1>l ztYv1%a{V1I=cPaPBiuJMfM^IV>QIDbhMsj$kE-Y#tgTo0xtF|=%cm`BAkE0ORWzd| zfM($M%I|%Q)Ob$r_T1nsM!hBU_~VZw8Es*AqRcxvMq#{~aLX0XU}^}C+03IccxNFO zwC!d0(}iYd=n?{#;HIXjH=0;ukR==+Sz<0RVz7Z_zA?Y}oTN{#`KEG~{FZ2Bmy_EG=t?g$)E*!Guy(_WQoV0@ZK>B83SoQTNGyWiTYx~jUMyJ^;@Si760v7r%U5ou765hzr|(GeV3MduY? z)DfCS5p+I^NYfxX4#KD?D4ROSyntY{(+%{#cXe%d%gl&#p7)Ovk=xB%b+_8j@AJ8} z+{}!OIC0{fXZ`)OQ!Hl_!CR%N1;!>g3z`^Ipl}YG1dq}a5;TbqiVZ5gg_hFiut-OP zI=n|rkgkHjY~p+=Ym_LdA^7tH4_fDmGHK|h@(%#n0yYhdC_0PbCT{xbA27{VDcHd5 z9UItw=^H>E-*c>xDnm4AXt{Cld%&JUcJ4*sGO%@UNjTBGjP8^tE#-w&=T{g{ZX9_6 z%>)VwetWOkV6}G+DBk#xFDvI!S1P+FcIsELgvr|0z(zgAmt9J#idYlk5asOdWV(h_ zj!LOrcMHYXl=9{*b$(OZ~WRu?Fd^-T08b^tqB`(GiJ= zIBT(zM2`2KTCKL`YWATEWY0RZ+T_-->shO|$MLw<({a76?vJX{YPE6BuP}fDwX8;( zri?a5*)+Y0ZJV~?)gvn0d+)uA+>rjtkAd@W9_I^MdAsZy>5l7o8%CRsOP~4#W^TNn z>8UYx?cR=?%emuH=t5* zn1mbV4>4!pTbrlZT+f(mJHEYfoL9X31>CZ6jD^V&tY%o3ls262IzAp2JS1()ffn3; zU_a(ghqr`kr;1vqbVi2NE&&*C5FcLICbl$irO%nAJcgq{OoI=-`x>6{rdQCer6qlX z4^D;|?!3uhfDXvKka;Z=A#%)$%*FiR{aRheq}r2{8=hZ}4w|)C&3YWKODL0WXKjns z=`)Wa&TN||Vr@^6uv91asd2VvX`cDGpMh!)xNz)Pc+I3=7lWwPeaXNmt0>~Vm}V- zFNRX)TTG|(&(Li$>#*&W!T6+6N=a=R!H-$$xusroov%QVXFGoMQkUu|JBkdrngua< z3{Hf?gq%iUz$O|f14Zx%-Up^XExVbP-bkzNu0bv0l2uR)9!Fv&XiRs;TL%tSjnHKI zQ|&6Vz+oEP!l?ZQh1*8KcHm%7V1UIHE4~ie2*<59Cwc-8>id$P-9%)6d8mr^b>R7d z@#q$@hVQR5va1Brn&?JYOnbPRCC?Qwn#1&ANF^h|APPpMbKXlmFe7bGE6JFxIV^&~ zhfXyiXr#&Fm_*;#_rwbZDfvN5O)#Zxm8MllYvk(tr54s;s&!*9)f6ZziL(Zg2zfoX zR*lwl#OXU#0~nJ!>#NF}bMoF!M_4;es9K^8NqyDXzQWMlN~k72N*)JNU=H{UksCyZ?o; ziP>%^kuj(VfA2qd2=U7Hty_=kwDg|anHbz#O9g%(Ls)MCogWB3Rd>9nVa`;)GhNMc z3nVhmEt@UR_}h;#f6E<=Ews7vVHfh9u{!f(HCi~*(#RFbfi{qY$|Udb>M(W$5^#mb zQgVwM=}@{4IuVN#bY(53^jLP~Rmgzou2-;5N$1*R8dSBa<9j)I_tfK;)iHFsTQYhY zo!4r#y&{e=&M|-#eV>SRSgpD>xiVNOuY&DFCgc=6b506f(3v<378!T~q;QWrOv5)A z^$874`T9S6gPo(>*-_ifgdGj$l`k;jNv*;7ggqPg@Xvqs8J_d}XEFAWaa`e%B;Kfq zH8>l6PklFqAxqF~iPw(p-*)h;A6kB5t8!z!Y99smnd%FkN_gEh*YS>hdlB^@Nv9O? zXp9#9iIJX|1|i*&c(8soHe+z;f=069rR%>cg9RYcmsC@)v@+pNSMZcews6Ow2cRAIy<*kbob?l1w}nrF$jV;vWS6XXhDG)zAs4W zB!^}?4at=Jm7&r?oc9DdRPXqr8H6sn)F|9C)rgMdP-kxK`Oer&Q0)T$5xe; zl78F88$^Z7o6a1l5?oFO;uu4ynT+5yn6Gn0Uv8EjYfkcNmiWX5)s<%Hc65&Jw3hRE}F$gkfm0ub&{7W zO;D{T<s$-&(+=Dd#`)Qa=CHFM;)Z{_j7@w()J;@YQegpyxcae5a#JR3Kzo z4PC~)8rEHQM){4apw;aCoGj^R^hL}1tQzTQQ0lA3B2XXPSFhuz#$akA^H`eogjows zpChJ5W(^Go<9VC$VwSe4kxNE15!$r`THxE2lVSRK$GiSYe;j9-w|LM$u##Y$!!+|i z1#ybpSMPkF;9O@zJ0jv#fyOx;5wU)@z~F!M6x%G8ki1zOXpC5oS>Q@r%?Mohgp2sr zU;i6*IgGmP{Pu6Yg^M;ml6GyLJgL)4npkH^eagIDU}0nd8c?fceC%T%<)shahcy!D zua+v~jyEr2%{D>*Br3e{rfZj%)Vg}#-aeb63L6t+rzdvrseBmiO6DB9xpS~{rMtjb zAO+I0Aa;FprYRjdz{Bt&?AHLTEH zFs2?lxRwfddha_``|BMrTbE61Eh6Rv0o0YA-%j{$68C4Wms#EdB3ao z@87?lG)*~j4`_$r86Gh%s1ogjO$IWQL2fNN?;|30WCPZEMZ2hq1&j^;x1LMlgo3gY%y7CeR}wDVPlWrmmfFG=J6H2lj4g-3KNIJqL6b_y%ab)G{mMG(GI_ z#u(Ijwzo!^xD9qTpj8Wrl3cp;YU3P?+z$u8e1z?DhCFX$M6hLIWvwMN+M{)~s?V2g zToWzeo6}^Z*ur!YZ{6hH@G+dr7C4xt+&602KV}g12%&?^898Q=@3FeCOgRr1QkYK{ zKVM;e)>W+!s$~vSLrlq(6H&`~om4k^=!qG#=0N;lU~o_mVztLvs}r@_3iYL~sp@c9 zEgjif15VC4@;qOc=N#>JdsXM+z3(LJ)L(v!CFhJ)#^*2 zEC-do&c=-!`{@2wi$1y`B5&que1WulmH_apS~jr|LN9 zVD-B&LYFJO?|0e_@aPg2)?k&NsceFQZ#gE{`MSA;!w#IzeJp}J9zhR z0WYQ$sh7S0vYf=!gHjf{h@lv=6Gp7xOz0a|1B?RO5?=YXSM$y9-Nf@=xi9=qfEhS) z{~@?vaBYv>oba$8JwLQfloW^b*n(2J)YkUvek{L}BH}FFeaiVMR91KO<6N-o-@MFj zt*c&oQAxSxI>!>J`e$^gr5$l%cA$RPWxG!O>qq#rYU-#17jN6l*XA9CS^T-9C(r^i z=SiIFbdneeNdd1uXf=s+DC~_XDe$zLT*B74ufBjdOQ9C)4Mq)JVJQo`biA=jFcb!6 zvL@bITuDKQ3WZg2qqG`^`AqTJDn+ifOkU`!D7~e_e&R8rWQoVSd1@mmBMZKNB&ECq zL%wL!ykX<)IJJD5dXeDqIQ67hL`qNpXd8eW;I=RP8A)=4c5x?Z<4Nqe=w(6R%+x!! zi9tf=77}#G3Qv}%?3k-@&%Z5j_n`wMBlSRC*%O1P&DWTkZ}5=0F}BVMt)`)trsepu zD2jFjG_ZH@dNpN~`Tyr*|yIjQ$QQ zdJ==(;aiRU*tJZx)67>Ty{h?{EGIC$8cM691-e$#71n#6b8~ZRJ|FKr3kwUY?#NM9 z=I7@-p9g5S+Z;G>pmY3{U~X=1P1n=gjkDJK)o3(UoWL2xwr$(i>@cr~-p^Vc@8cD` zUdwr{=Q*uZOGs6RJ}ZOQvtAvMD;bl;?Xhbc+nBgGechd`)M5jmUguT6{Tux8`>x>` zm;WRjhA)2Tb4*>hfv5lMb792b)38GtQCuXrI*xAW01bCv^-nOyypc}pIaLLk7({GH zKoXo*>w~eRHdMhi@+NZs-JE-1KYMSzi+kz~c8rZPHM@Y>WKn_K6!@x*9Up4@2B`@L z#7}$7s8EC?&CbwU8LV3cH80m+EfTNjll?t6bZ)_^BHfop&7MZEnPS3qqGOl43&URRVh zbxCN-JQIzvaFCQ>v=wk@|6!)KPnKEAva`TSv|u9o7Zh1Ccu>AEu(wpU^&3~xt)(H6 z)m>4v?2758wxstbE-)t)SMS@)YkTgem+pGBnw26kp%@~i?BwbRx~~)yd7DkG7RG}q zlSW3t;n7l`s+>{XZXzn}E+}L(w{DqZe>>sk++w`JdIM4>xYQy?NpMx2I?ljcYXN5y z3R8`YFjH?cThG~W*-q}529rXei5C_ zk1`%4Wk>D&)f<=ITwdc=g7;m( zEs%u;CL1+I6(*cAn>*&)Ift(Q9_h~ASf69HAk%s3DB=r*g1IDPE=dEu1BT2AHuof$ zJ|nxT4fQ&lbGt*cSiaAcPDG;+CYuH?LDaXh>K2*BWw%)1_LkBhm*7AM%(#rXRI#SF zjCd)8001BWNkljdyEJp)rtxHvva7mqI ztR?*gzuNV{`!tz!v6Ejp##Y1LvJwr(lL@P7&^f5V%;KwB^YIG;ebc>jvou|Sw}zq& zrBqeoA}3E}GS!^%)Uu;CPJqQm=w1;1sNciS^-keVC_R@l-DSp$mgPjg>`fgTx-LvvXrv&hAiBy#akm zkfY%pFW-wu=DRZ$%fDSV@|KX<~pt}X(U&Bf4mQzs1qWbGdIIR z0?nQ7>zBCK*J%1Djw$(lk14+qP}nwtL#PZQHhO z+xE0=+s5tP@7?FtKd4hDD=RW1BRE-+a)6B;YJz28^~sbnT#6ERIa6Ss{DkZTK#8i) zJxGWjJ($>}aD@Ndv1<}17+ND30M>vzO`P|uN8N~GLhDty)sYv<2plIvWd2hwE0*9m z?m5gv?pv^7lW2t2MOM#z+)!)4wcO(5CQELw{N-M}*>W7GikmhBpOG5(U)R&g#Wu4@ z`Kqa$NE=rxkgVX z9Un0}nP6oSbSVP~CQy=M!%)-X{PH#lKU=BJXn;(V$c}CdNK9%8ZdqdVP0Ym3@flwz zLlfZVzMCgn?s?pHW~|;(-8U=E>3uskZ8zmE#X{Bm9)peDk&pvpF;g9ss^*I6j*mzT zo6>T)Srs0RA!9&m=plW&J&^_HWCXp1ZLu-vO6mLS+LtoDn<#s~ffZ}Se=Hyw+@!&N zI}5hvU`F-xo3>;PnxuU^#79#xoP=HUtG6^wt_~9y$_~dDn>$q zok74l{H*{xn_b6G%m2o9CS6MLc!9)%DJ4HJ2`kMy75V!p=gNNo6g2(~8Iq#>wOFjz ztolV}GL1Mcy~?W0sSFgS&QOB@WbZT1MjxR&JkGx{Nkn5>Vf_-H1qoRsK*m+jo2m^$ z9LX#>+hHcrD4&a}8dMF1EI@_^1k^Ghq%>?WX}qou4#Hd&%B4WYv+d~}LrFE2JjvN7 zs$(%|>#atLR(<^=zy(ah6G_REoc_$4yLhKb`;3v^=(eadv&OST{4_ z^e=Sh`R9cg->9*gP7Dhw!vph1HM_~EFACOut$-mJx(Wq7Kv6TOmL(C6$^FEkBm}?= zbeUYtLq+T-%ja9OcG}Xo&*NL&1NO5~3dOHk%tO1JPg3t!eX5{m&ba6Z7w@gWwfCG5 z`#!aZ%&Hx{&!@sH!9P83HBde9MT@su0=1&#cFZY*1MX>|Mz2$A-!THMI5NQv)ojy_ zp)~LXsFY}?oQ(4L2;-8mbcyw%o)K-$JmUbV3SzbNUCHi2bb>qk=$NoDc7=M1{4;D< zoSePwD%on)LC;V!r)p3Sbe;*^Jj(?7F-W2L5L4az!Khgi|BB*8(M4H;AeW&ou*!(& z5R0WnAw}}ci~;nNA*uA;0fB+F-lbVj($n zd%~)g?GwC3@Db0MkNnBE*OYA{wrlx|n++LLE?ijtT{O-*V>6ek@&fEB=PvUh!CU4Y#uov)v~ zm&as6@d6!xu5JRS`yeE|AwR{_znJoaSCcGM<_9YjW3pX*ob+}*XuIepJGQwlEUxZs z24tMt+0;Uyu*K6E=bMWkpWLeRfk0F_?O-I96j)YK9GERy$;rEd3X_eSHm=y=f8jp@ z**L>_b=P^Ah#Qv|3z@@fl?&I2Z6{hXDM!5ZkCqFM9nLuyaZ=uK}raB+{`Tcg?! zGu`K-DF?IW!LIm7S>UGNInFlMB9`#{y(iV6LJGGn2{;s%N=XAk#si15A*V88clA#s z4r;##1TwgaaaMc`Q^9D~Cu_$pf+MO!=nuFt#Ft4z9RIs6{x( z200UW9U~fH83csM`jMFD4t(M`$S9fnM!qk}*ZtJJe|%ADav4VIh*5HYv8wKKoj-51 z&rvy(W|cGQ^SkSOZg@Xe9K8S%bIWle`A2EzoJ21d1Rt$o)Rg{8dAGlHz1Xd4=ck@- zcHMvOcptzXt#}#-5AgP0s`>ry_aO--b{x^@cRASuvp)$Stn2L;9fuQp{?Yt6Yjs#! zN_Nh65U*HfTkmj9>yoNig+A8{0<&Oz0E-j+0eTGbZV8ln%H}yL>q>;m-k+nkIn{w* zB}Z=MH(8N2uX7%XAfU$47eci<1}I#B>JIVf%k>DI>>-2bj@pmm|Ah`Iwvdj|ecz@P>5AJ!?=IWd+T2R66kRd9SA*eCtlJ6nY z4F(;)E%rtJQUdG$!%Ehr6y@+gM{ zi6tVaT3b>@sF4Z3=R-gcYhCJ6k~YwH2ng-jmE8UwlskdB@y3?OG4u|I&w;J!D;U@P zzSF%OiYHT?wKTh4V`|R4@qXvhzMTr?@AmS|CBU;My&*`Va#Gvl;PX}xqnc-@XGsLUfG+p+(U zKycvg!7CGZ!H<}za>3qK5nn+6dxHDOOfdjurF%CIY<6pM+5>Ps4;VUc7ksm9KLydHCgIyNK6Y9m!Zo zlb2Hnik5BmRtZGW_Uq|&e-4O7Xu;K@yr0AK`p5%ht1_kP!R09vMrzPAUH6ztl;(84so>&$fcK#D0wRP{$;%Z zpq!&{G=rAP;~#>n7GGjr^bmpDytvw#i>YbBTb1|zqtTvwQh@kPB?tNyz=Av=G_OZH zUpJfg!F2TLD`qGh=&=tZJB|P=KMSqs&V}L&3CoNP%*OM!yIZ}_VxRp=J21`!u95~M z8GEMeSt|}d%ly|@D7HatG;HNh4c+K+8W1Z=R-`JeddyjJwq$X#hT|JAJPB;t56v@D zwcWq3_VfI_D9d3DQ}+OH9qsg_3V$BxYu?438SyXLE~Uk0b2KzR4e;=4E9^F?Pqy|w zDLi9FuCe9NE@aUhXwHBPWb81aCV;m>Y1+S)$E8&Zh6Zk*2Xsf1y6^WAZ{JD?5?_E)EGW0u>27|J9F8_HB zvz5wHLqL$?^;H>QZ#GKh&Iz{l4XO3{hSGlQo2&!ZOd9S5>AHHdJxM>|$^7SpeE{Qz zZkh!a#p`$DeGaC58}J}*-Q+X_N-S!I06Cnc8Q7U*;{7|lIsC-z z3e0sDC1Q7Ub6{bgul%|XvaYn}TIk-$9Tv%D;|RP5=?JoD1lLdr1R=^PtwDwxPY z;@pD1sq)LcL&2F$DH%eQ>Y~mdj<|YLHL@ zeQp3v()%ai%lAd8+v+#gcoqSLp35NZHM0R zCfIgu7Wso)e7hB~%dl{k&s`jW#{j~p=iYN!5Dpu0+C(1Ex&-XmzcLlI^zPbyc+gmd z9gRdGQ;h7P^yjdamy^yI(qr5cS>YiE+<}=ygTSw+-ATHWNxUz(2vvV;R4dFKa9Er7 zsp-qDH+73)@RAfKt88YuRgQ*4*PbJs&sft>ICLNfu<+>EJ@Xc%Dx;?S>CtqW_q zoec@?r|Lt-Pwb*rUINl~*^h7%LhWW-y4ss`I*G1ovL}ue9hxQFUf1@>mMVooOaedgSDb^(XeVe|mYCy`xFzIHS^kP_gDQgy9%=y5L}UU>6R) z9Kau76W0t!DN@ox8*&11WEM~eNZWn`E2TAlh+km{gCTyhA!{1TZAoiqO84)FLQbbX z7S;uem3M-cO?`tno^srd`#afh;z|I zl6~O~3qn*YNG?X)@1)UzEy44>u`jT+iPl>!TQ4fP&3j?d1kmNxd53`Tci;z`oUKHz zx7$v>DJh*V#4?q|Mtxe>mDrq#SN2i4IFO!CmD<@X?E8#||Gy=fcV|**&@dmY<;2N( z>pe+L6ZvrB#KiJ`+X0XB1)0fUruC-Ov>s?C)mFatEg}P!eLXzhMKHv)h314} zdBsU~Vtgbtr2vFlN^TH9MM!Ay2306t^e2MdS}*0@*h1d|Ip>xh*LfqFc^<>(F}3z^ zMhlj)#XhUE*Nv=M2Bw5AjTWxpsEdxvAtwGZns?z{Lr1Zii>LP=<8NE18-c?7Rc1w* zIh&>O!{aXv@@_*AN0K0lZLl`9#rb91J|r)fH;Ymz8ao*SazXi`&!i%2;aPnl*Ekabb&9e;HZ7b+_D{DjR$|13t+(r7Ig@rl28-qujeI1RHG%y7Oe0s>+pKFB5d*38darSEb3 ztS9RMTsANtHA77#(>cQE+syCSERb!yK9#BYXv%XdzwXQQKX`I*sg0#)7bCnacXWC0 zOTGnmIa#b;+f(jG5Xx~z*nFH>seNm{8j)~Hz>!^$b@xo=&PadSi-!xd(y*Ix%4k7L zs-lhjrJ!|XGc=VxBl%80cUt}aS^uejtZ*PL{EQ;;P~{fnq)(5n0Rss*h29@|v=taQ z?;0A{{4n}0XXb@kE#0mAhpJB)94X0O=l>Vy(FNq<32j!wS>Lpo>i%~Kv~Bn8pWCtA zxhtPDH+u_FA?f>c--o`9Ck$}A7@a=Aq8%mvw1ak$yo+}){_g?BjGM<7jxNK5#r;Zj z+~o*eDvAo4HT+9@UUT7TGD_RNlQP_A{&4#vF8zGlK3r_Y8+Xy5j!mnqD+aNSQU8n* zvcmsmd)Z#=z_x0srw^37aH6JA((`68tm9=VSA|YN{5qM>5TWXlwjkp{dkE_7xJ?2= zh)@zMCF;p&e4;rJ=C5wX&X3JYUSZ!h<*aZ z?L06rNz@Q+U@(a!GHMY$R??ncuwc#ymB_g7<-JGH`94lHOf(W7RLxo=TfXN7z1!t? zsC=i;CTIVZl1ureBb*zfC2xqY#n&4wr?GR*oQjAb6HP3AJm0V92$x?-SCxa)}WXxHPN63u!?O4rQtUv&;#%Ze)wR7 z4Pdcrsi!Dten}td0}c}57Xh~zPIA>!AvT14^zM8|0;j5Srm(z&@_sUDNUy#=!u=PX zXz3RVjOy$%E3au-+RqFFgtJ%gfn&;=ffi!?Jk8eZ@$^2)>fG<=9&7EqLSjve+PsfU zU-q8;$(-5v(_EvEZ1`20cH=#zD>1rS4&+HHG}3S$b8jA!Qu{W=*kB5F({W|8UK6f4 zibU$_FlUM{Upe^pbBDwG^ND9#(cU*SfZgbp)8GvT_cLo7BF5crMZi=BuKRP``s(ZR zwPJV9Fv*P6Ew6|}Ss*!0Q38VZ5xrx&ZBZIZX%^{D-(TP6TCqF2^C{UxJ7rX?lkQh7 z*cm)Z&W;dKC2Esr&3w9;griLN@>yW_#XdRKRiN6-!lM1;RB z^N8dnVWP|uedzB4FyF~-Ah;7UU3-#)GgQ%OQdTsq_N&NN#$jgazMM!rzvhtXW)9Xv zE%qUS>#hSAmz+*4-IO@B6gjeR8w2o<*Xq-5wgp;qUDuVb8o+82`B56Mo7L9V$wwA= z$FQ)|ba*LEMBDd1XYFoVTWPcGA!h@H-j8tTB=NJZimTi|-I0>E^V+~^f67A%6>j>p z%uwRCK@vmtO#mOU=Md;yAkm@I^>85&NaO{E4`i_ZVeX?LHPcFxZVe{%` zf!^HAQqDTmLW6i6wKVk&2U-xQ$cf?)+FBfLliwTWCcBO)?ev%!;yUfA)lxLs66!SF zH&A9|XId8DR7r|qPm1F!_+jfdf5*%CE>95zVTc&-c>yoNw*Ka3FsJmUxmq1mc;GU)18f}gyvRG%!gy~EC6fKD=DgEok79MkS68ZLGLSz%0xd?j z%xi;c_}Y9;w!}!rGQY5K;rRrici?@+*{TKC{FDFQh-k{fOI-15@hL$6N>9{?I)VdS z9mwiewXoLwSisfxPTM;zSrEG|AZ_>Ct)aHXqKKCJ1<_Gq3z!-P0R-|0B&8wmgD>>X zgKXtD>qF1h2U(2=+N;U;Z?V21WM4xI_+aJE&Dr}(cAV<#e-Bom8&Nz#imh&Q9r(o% zA#c>pe&?YU1D^P)$fgyKi}ZkLSbC}3sl*qE)!Rn?kg!Q%QKN<$06SO%flAF+dtsF+ zqg3y61=Ny*{zc(GJuzHNh!<#DBtW#&Ge^cbX8D>DLhwlMwNz+Sf@v|I-ifwGt_ua~JL5a(wf z$pb0c73)ww(7V2Z7iy8w8^|vRtr!u4tlf$75p>EQf-IAU0o*hdMn8?95I4Wa*mi_h zZ6B%%56Sb@9E7@Ue>X(B4L0lEbX`QRXpe5ZT)}+D4FRuc^q4*UR7bK=on`9cCSbKc zN%;p{nNUlf$R!CLpD>)R)z`^AfP)1CDA86U#{pQvTZk8e^JU0Kw9$FIaA zOHDfdC-NbmmTFvd_3d!-bw`V>aW1W-%{Pb0B*MT8Oxp#79j-i>I=G!9E%QiwOHMXY z1IIXoQ-@kMmWPjvaC|*^@>;RqOk=hRXWbs<3aRbNDayRH&;Y%Asz9@zk^Tq_(RP$I z`!{g@vt|elJ9hZv;@VC>S0JOJ?e3Ivrk-*?u+e&6`9P?opo3{9UMPbp1V>fJL5!yGn^sr9};)wJz!5xd% zh!vD`N~rcxR} z7Qvr4r|;^3q=>%-{;2eiSRvBMWd;?%sgJHYD!B;B4lb$VG%!UWBcUrXoLZikSf0T> z-AR{%>qOqaclUpmy=noWWJ@=|k&X-rf;?Cz>w+28gYGvaQLHz%Ti_k8!a{TZO)zv`5yrH#2r&|afI7s~V~CW> z_X4^7_M4;Bz9(xR!e_lR!1(U+vt^&Bf))i-UJBCKqe_EIn^yNQMD3OS1dEqS$wkSx zQdrk6R@v>WvNO2^h4TCiOM6}MMij^<$O>?YL7}}@MI9jZK0@NU9-2re7(^ZYOFVH{ z&@T>`PjVFHx>DH^P8I;AH0A2nE?!ucv2mwZF;S^Pvj{fkg_+SUwXA$_1qZ>1hpo<6 zpg_`D@*H>x3!n&QMi!7h#n9p|IZ~3xW%{ihR&9xNPGkSmjtXPfqN&Pp&S!?Sf?Tfu zxG%kb&;7=zd4JsziVrcsiJ!kF-?YuUtF+>V5F>Ig#iMHYY}Q7f2w@ zp3|eIy=XSlI50CamG;EEcl%uQ*b`|sxs>&B2;C-^DK2FgEf|SKI~a~ zxsGrhv;Q9#fKA5{$JX;rC*`;zjQ!eG+C%h1*kKH}@k%`8M^`!z!f7GvAqfa00wOuenSE*#lLpj*@8{WD>kWCs7lzPg0GbC{VNt?{hiC4CcI-C4sG_UFq-It6UwM>$-OGJM26 zIK>w?4cSCNqyHqDkg&8AgH9D%jOCSuYPzeXJgv6%+?2pVu_atkaH|N`5LZ z*N~G6y_sfxJH9i@Ng3Wj5%aLfSaK2ww%@g*E{YOWL~&Akn;1YC`5!dFu#!A_ER#HX zosIcVG>CV|mXQAOssyrYZ5dGLg8>Cz@5x%YQ&i88XRsmr&55il>aFN*%z4~l;y52Fa<6?k#L3u+y9nBgzho;+3I+LVA|S!~FG z`+#XNNEG8NEi?wrb?|#2`py$OJxtbg{j*;Atg*fiti{Q}6*#h<-5wWOw#ys$uhWMC zNFgoZ14+&&c6JS-7CwoGz#Og)W z$2L}(2ajYDW~fyD41~r|*P^4`=KLGDN4xQ&Q~O?@Ww4PUYcZM#6Ui`YE6^K)tf6PG zM>e2Xk1&vk8ZIk6re--wU&3TsetONoEhk4n6!#WT^++&3!T2NziBPZ+- zz6y8`hJ_u3ZO0_`W1v;nBhkNh2UZK|DD`31v5eq$rv94RS@=B6|FYGek#`mWbyzb}8ucQmtJQgEG(&#N1Ev~aL(Nfed+E1qX{!a<9q$Vbyu zkgv^fG<5YSP0jCNJ+b=MqhO2%G~4U9J37s_wz{GE!=eA4O4L;w*bHD?#n5U68-`Ns z3|I?Lx{53M=0#sMm>ImpHP@z@*JNbQv8}d-0Z$q(_D=u{%R)%$%FCERZWAxv7op5| zet?AG;%z*s(`fXlU7GyXZtPD|Ym4WQ6Jr`482E%byOTR$zBw_SC_lg!l|XZst+XN` z3qmyG>9?gZ%#AKxX{zdxx50AI|M7;TF2FGKqx?4;Ml(Y+=ix%J`a8PbE7|%&;r*6^B8bXha_O7 z(RrN?D9#GlmPbcFRPntF{f;;LrdSaSPLXW(*zwKC?q-dO!@3Cv5QMwQ3n_|*Sf>~?4vRKT_*_1MC38+n- zNSY``y{MynpI-IOrWtX+z4{vknOP6u7@$>Al@jdF) z{7jvssjp6X=>rHAs>0QGg1p|;dQ#2u-mDc_sy}aUag!DSNk*`uYU}y98;ItBF5*!Vo^3iU=+W;+c_PFa)gj-#!Af zy(CV4OuiWpk(>`ST+f;6^Up4p_SI$KfdC#{ElDHG!^z#96u z`iw9TEJKXrTuG+5qBTARGaQ@Sp6)lw=628;=d1)-J}Zi3x1Bt5zF8_ZAD|+b5>Svy zqe!=nA8HeK8YZ3Bp4^72w z0-62S_2dqukF> zA)W_F8@2)vP1~L0N~7zGNKrcpj>N4_xp|-j)3E3Ez0LP-#0x%!O1oFquZeet7V`Yc z_Z?-{@1&xW1qaOKM&!#n`}gmg?@N97q9-@=s4Bfwfa-0ru3xSa?zf|u+-YjpIrKJOcoT5MiLuO3Sua95Vu@7L5!(yEJz`<{AQ;ALRA?8MY#~wn% z!mcO!ABH%jU78t4=3lu<|ekGp`kthe_ju=L{g?kPD$ocA@AGA78SiSrn0+1qOj!zgO|8BUf6%I~;|0rV0hWfX6n92sHz>F(w zEc836pG&WrkHj{m(^~jfY9X5i()N7yDzfd*V*ov$%QK& zhGYUQ*u{$)4IP6EoR4Rh7)9@!gWs9IXGN@Qt3tVzL_OcPoETXirK~=kZ9_4=Ck9lxfCms&Bg$D-HvmbuAlCkeU#o$6WE_z ztTm2~NV=a%HMfBoDM3+Um)flD5Gy!nc_q9tosdl>g?tNmbFvtapf_fLCaa+v z^68v!7L_X}R?ah)xKQy-OE;k@N0BUPVaR$IC+=HP5+eAwCYdfb<< zQGUfhotaFzBR&L#&A+|M+_Dq;HQQ}niStOttdJ{6Y-9@0+z}B9~ovzbr zs}4O#0*;Ft2XQ9z@*0xYggi|GCmjIW9+!0FAi6Cm@{y1xzZ9{g?LQ`7WrQL4z26?1 zct7mJAKPdp%lp|s+wk!@`TE}L8~EUJ+HbYRfs&|Q zO69bQOg?DT0K69u?gUORsNSE1y(U>!1DUOor!F7q7ReTY)bA1ca}6Usy*)c3)T(d6 zf+)&=&nUTK@>9w?vMkhEZ}|*6(pWt;6H70dn373G(!XDC>|9eZOr>7WL!&H(6{90q zKzSSKy0cd8MBR+7T+b6)UY1w&A`m+XTrn)muGNXsEhz0ElC`_EA-q-nTcfgtf)nr- z39&svAln*Ah%pg&S0fe%jQt%y#CBdO*kkg!>HWe@y=)8Wh>DYps1Q=F3olkBj>=RR zF~2?g$?ax?OT4JM{^UFHzNxO*E?TDD(4?kYy(65g>;f^Q*ZLD^7_tiiKtd4Ihh%bm zk_J2`&3GsQ8kJ3uZiL7<4h-%e1QDq*Wnuu$Is$Bf)um@;HOab~LQ_uC+|gNa{gJbS zchjxXT2ZOFr6{$0@8cfSSy^ebb@X?JW5==QMAzo<>F11ZK;CQMYFo?tV-mhX-6Zg0 zyNQ6CXo15r;*FRxA5MC7C}Kud*l(jDxZL-hEW`I}CA;w6A8cU9whh2_5a#u2qg=-Y zWBh#Sf$NbZY1K*1-q=ai1>3?ttUFy-k)Fb{ng6B}Zp*$m60t<%YK#*ONS{t^hLFPq za~$2rQPhsKsz#tzSjgCZ8w3$-L|`F6ZeTVXF4cxO7A7efM8qG!ENdX5u_oAqiMr>R zyn#Z_phaot+ZOunj}+BQV|9IY=#6K{!Hh51tFe0|kbVWPY&EMm!3=gpIW!^Z4wTv3 z{=A_yyM#TsY9@3WXSiz^vC*Fr#GK+Fp98Gf4fJOeAlikVo)t$B%l;$l`QDFgF3no8 z{|BZC3qXV-U^wC-i2y^j4Il|BgfZFeZU`2mFg1AYHo z7{Id7QtfGx=kp_S^JC)Si0xVwU4w&66cpwLS@>ZbH~T=4)7l$IHPg24OV&wkX%D#+mC}ekXTco)=3GggCaOVNck{ z&6vY=>|6~L)vTTnq*?Bajqepc$CbZa=s22w5rsL@0yK}$N`=h!Z33g$@4$hP7G8S~ zY_|z3J!!*a)RWEeowl!gf_IDPevBVyUYiE?Ve{J~E7MIBi=L1*-EzHN7i3|254q>!}D1m|h>h?{{FE zfzAk;;6F(Q91bbIiPOGhgNP$ILa(dGza&1o$;eCiLEjk`SrHwXD<&q4BZ_Cz?+Mk? z5iNpNyro)hfQYdh4q_CQPbVT0%ke+)j70V8uQuofl!lIKxNJM%s_P~wPuV6ggIg{JVq+S- z(>RTSk(8U5m|XC9@JuT)z}5gMl1e8jL1v?3KB1=OAVMv+KM#1m3jK303xp2zSDT4| z8Z+yUY15H1$_ueLdRog4#jey$=s$;Kb?7+Zse|{wkTzusQQ#$qb`^M!2v@Nj^dj|k%YUwojNnzR#GDulq+rIa# zri;c;@_-$z4m5_yN89z}_~x}c;!a*6Az zN?RUafGUq6QhIv9Zh*i(BO)(l2f{_E_72yW$=+jAa|eTF_TW!^R*0gt9#lV+>yf@b zy|Df=k#5odU69sHnP(w!V5KqriK>nF=@dO*ceVBJNl33Jz3*J8IIU5+pBIDJzH9dU z8Kz0YA8z2nI?wM9nhvy(FGcXQ)L7&Ghj;7;;$09bs-*ts;I-Y)bONM#3UI1Bw+Y-B zY{7fR+V7*ju${R<=Yn}7yqoR0?sx#?ZjfYn9MXzX{JY(xV}uy5Zm@!zI*v%t@Z=Sy}-$OwhusU2XeNiX8quq zcw18;3JJD9W9&p$dxv=* z$NU{%w>lYwgake+ce--2iri1KaSl`3UkxfUS@l|#cKc1&E?{DTLtM47&?ti_ zq7h?%ac`y)awy7Y@h?z56)8*o?TFL)aOCm#D3C6WDHn!^`KrpMAig_`o49H2oth1_ z_6?_#_=>D|!(*kX&5%M3DZ)HRo;t)%Kn>64q~TEgF!~eh2)@C3qt+J#I(_{c2hcU%i?f?N5yfQ+|s(x`MuaA?p90iWuHJKrIIS zgA*`h4inuk4bBgLyUrUPUiTJaA65@y?LTV{?m^du?{=A*Z*hjIxfb z1{*UWW2~X;lL1CBKpW|yS5%L+Vc4tM(b8LwIG(ZP>IyZ+V=w7zi_pd0quF{+1G}{gZzxUyE2f%WDajX8KU7irgUno?%QSCKT zx+9~N1%59QR9fp0p!QUjF~d2?6U=$guQ;6HR{y(IDl88}@wq8%tX%GRjyBbkF)AT6 zVFE|V&KS5^5B7XF-#nf!x>!l9(qK9m-9+nw_Zc$=UznsTb$DJAZ(euF-nmCDNukbv zOZCO@itM~8`7W1J=)M(9gQM`JNltUZxqjjGLc8%i0QI76cBylLHB&Jd*M2U{5+6t6 zj*x0BQ6`bnNF^piB8Eq=jxfBcc+Yt_*BSdT$7do|=9ylug#9+!CX?jeozpg$NfJj~ z)Aeji$}~Fa6qf)~uv3Q!&&KV?>JJA7B$rzzzs@J$_1=)T_`EeL>(*{ z&2pj*)R0ZW32g(uNv=aJ%7eO6en&>KC5&`vL87K}My*1%_4xTxA*X(d7?SG`h1Ms` zbGVvU*JSIc*h# z*MJ>y8|)JpZ=`E+pB_`80dzF=V-X`MSsT4FQ>5Fda&dnOg48)Q%FQj}H?C1PC2_g_ z%C$s7KC&T?5|R>h3@jlnL9##=8#-6tGbD9d1GOF+8Z=y;l^Hr|%iZNIq>Z-&S!_H5 zN&dI4!&-liZ21FIXhE0!By}Vib>+N^q`K4+Pb!VH{bf=TMIm!k0>L6Bi#mOfvi0O! zRa?|Mlh$2+2^59m)_?c(>^@5QZjsiNJ>oU;FxlUlQ=cygE1RQ5J1X93;$AcF*Ae^T zn{fmmUR}e*ECkNNAdj;TKhD+u zf#C&t;QSoXEqqSz5c8ZBeODj{}j&N1p4w+?xJV92vV3JnE+%XSA7?mh3>b$R! zeK0$fAZZ_ESBRfdhjnJF=&LQ^V zmJ#gw6D88(&nQs>HRePRLkYB~8EBVhsk_n@L@O@x+#wk%E}C}~WWkfqY0Tkk6?fng zba6!r{NnQBlwSCL3Z8DV+3G?cviEzuChc%x^xWK2iE_N-4aSrEFyZW<@nDpihpGy4 z-j|Zwj#r-Umz_HW4F)LE1Eb&fg;v}rUNgF$1e*V1+>z|rgDtrOOo1mUga&a+?9(~t zAr~ttv=k@PB(yR%b6R-RnEzeTV>!o}V%6F8|Iug^orbc({(h$V8LHR{taD^~>3MSq z8EI+kyU9yXeH(231RaOcm0@*2R_i*K)IR-Yp_wNobi2`LGE1V#%pF1(=t6d%Eu#dQ z4q|THyv^w~D%h7)Gl6GuJFwe#a}XH0__@XTO8T17fo>^$m(z=+X`+tSU-80jo(ed53S|$N_;>kMdjJ4HQ_!~ z2gRn#aBqcTk+}P~xaQTdU*`+dUY;;O8l5{JHc3MqFQL^4bnHQw^!)IwzEc;W*n!4E zg)>y&2SF1-jPxI8;+jV54!-dI*lNE%NA^P!X9Gxbu#2#4n3hQ{P>p1C4G3y3FIP!a zv)O>}O`U!!dAzD|5@jwf1rqC?`jmRc9maas2M1w>f21N9OF{$D z_LgH^R9b6qu+CP3Z^m3J7@&uT4$zfihHd>~njte)u)5I!L3o33a!0EC+w>^^(0>rr z@28{P3zy@b9GAmvfu_hr<2nzVa3PgQggiGv@A>HJJ386gdXSIorYyxI5x?f^8e_VB z5Hk$LujFT1>2Km-O1}=+^H9T^!I*J=W2fE%+2C!CjC76mn>{0EiEE!Lt0?PWf^85i zTnWzE%C8mprk|LPBaIVdOf<=-UxlxbnPiaTojN?(iH)yTrs`bX=1yj!^}teIX52yM0c6WWJ{I!9y{esSME;VRLrLW`0Ur0FG%-;MY$X(%oF zZu5Rw1fBat`S4q;jBRA?+m=sXzjy+!*O$$-`<|MTY*~IOC13rUV?XW%9AJ3AZoN`h zi=I4_lvq=y1Y;Ju37C*P34CI3-0Y)G6sCaQ1Y3q_SbnR{%;-;Ihh5)0vg}~JAdl-B z^8dI1#VG)pzfQ07k`e)Ru6CpDY}0eJFN>001pV@vLBolHrDGy-x=tf?m6UFZ=gW0m zG?(CNK9K{0hox+UC_LuVhj|r{p9X@Uv+(DTjdEio^#$J~Kp5p2tSUCUKn29qG!=0GG zQLlO$$xCooHG)5@es6}Q&{?Z89S4Y~`;6PS z*+N@SEJpgeoxjh4?RhPL&@wXlv;y7%{lJ2dsZ=^?s&qzMMExq_m&x-Anm!upu8KG1 z?6LN^m2pK7(bN}S+Krmb5jT&&ZVb?*q@?JQL6Ed#q?v*Pa__u2G8Nq2)cPM45~z#8 zVrVQHj|0g@M0v>yE-Qf%SKsTsA285onCz79z7+~>t_;!|X^ic{4*FQ`W!EmyY@81; zCXD^6u^Ot@p@|`xSB`gY1Jxyk0X;(J^3dWs*(yd8EfwGQC%Kkf6=q;(8a;LggM6Q| zvg=>y5ORsP%!btY91S5i?N>Zgrw4$G0GE*~dVqr}|tK!7} znfv%%#|^m_@bx<0Yam{izMmMquL&!5ceA;}vJz=_NXlg(h4Tx8bz%P>P45^T*}`>= zc5HX-q+;8)ZQD-Aw%M_5+qP{d9oxP+&v)PTzed%ly_e^li+zwskaIoXB~irv*X%L8 zglR5e010#aBfn@IRxOD|S)4%$4af?yxT1l);zu(nJ33<{C5SbqZogZ@u)0&w$bw1c zZGf6T?(h;!N{KSX(veo_n{3r{@}mw>slA9U#wDz~g~@wbCxjYD=^DkY&h&z+wXBOo z<@S$Pt;$v0#kV#CbW089wh{|5)z2NuY`_r4WmbwXn|p`UY@!d{R}cQW`9$>}9Pj#_~rK zeTDVi*a6?%PfBrrm|0+{GsrAp(9&;4X%HNpZNOma!79bo?BCJ52WEO6k&XFlm;XVr zwq@y>j{a*GJlXaxOtDzCi<04Ro8g_Q8}YAt`P%z`nV{?mVbh|LP$vS z{&o@Jz9+!gdvptd|9>48Pe*>^*srmw0g^!NuXeSDOEJ0HtH#HV$~1;{4+g#0lN7g8 zUoZDCw-*JU>(ENCD&@gyMwxO28sY>2gTx z#mqMv-W$Ur*{c1WhP=kFF+`1px{|-2^?O?e=qZO7npkpwJ?)QrPMr^Y*FPz`h1cPN zMBX_J{tJoCdKRI7WxkIr+CelY?IA9vVE_1`2<_7dJDlVD3_KRG^W)XvQbxS3}cG`Mz0#0<`UMa9{-9{(z8R+@tg;*hQu*XIyqh@ zs6k*O{jBI;=eO7rG3AtS44G5hEwA;Mbau9h=1BL?s3jrPRk7j23G<>V*vE){f;Br# zadY1L(cLEOx+tSG7{tZg;^fDg%#pD5UIcv}nldW0#;6eM#p?n`=p6}Y> z)Z&?`BP$|XkF}m}?rYjXLj3bz< zb!7fI4*Zl}FTE$gDiY$SR?QhAq0i_$m0bL)ZCPgerjT6=8rP!qk7J8v)pyA)p$80tx zzan7vP-Rb4hN{d|?n&|W>w2zO^)ih*A~jjta4!!%NM*N})L+LvcMPQIMvjsI>IAk( zQS?q43P@v+eI${7JdPtoq9@s8Bolrt;7c)pQAk{>Lw6R>$ck6IdYM<9`!6d4&i}ob z92Z42(QRNL)a>8Z!}wkiWO%+&@hs;rYbn0{FE*A}J>Jh!vc5klY@t zx$0d?=24It3Y{2HspwpQP8IMr6=}^s#lKK6C@SimE~!~c<3yOGguyu*7~MO{y{f&;fcn!*Dw=pb? zuzF3-6I^UG)r!Rfaqkr#xE2X6y{|F2V6%QwxZ!{sb1>sadLyQ0Oalk2(G&jTy?6 zT(-ygTK(eg5yOo?E3#Bvjqn4G;3MY~x(|@+LG)g5 zpTNg-78A0zsd1sQ&hNO-3p3?p>eKhM!`6fK9_y_LR;-gjP)7DvIV<1imllcZ_-$*} zT8s4r2fB4|?v#5&jM9IsDH}8Q%8&_R5_O^&|BBSi@+|g$_VgTWCQe=Oa9#Ot0Bm!g zEkm$t)TqDf9E~dCUU*KY$#Tk2J6v`l!m%j<(IOppGeP$hq0Yvn@78jxK~oZ)#RKWH zHJljpyeBlSb%Mb>MyPQ#`gmYALB5s1F=KMvV+|ELwC<%@Y3SXJ4yO*49LG9~Bh9mw zu??ncGXj5Tq+8{ac`M$@nABD0lf5&CymRg$7dJx)9wfw!!2TaNW^hyfi2#ND4ea>7 zW#x-P?eh-%HG}V+Ynt=R(Dr-WM!WF}8DL?zPkAYs7AC}P*$_%uX6snOHA_HZgGuw* zgCU?pOPv?N6jwxVkefh>q7l3YxOAQdAnaR)V(?2L^LN0?(=b@$VqUb1y;UE;H^LWu z2GSf=q_AHR6a7Pt$W5rv;Fmp$&qy9DZSDu4cO|y$=ev8U&7D3M$@0uS7%Q(_ep9a26*c2~CA)o> z%p|lyGlkvQ_`q~eU&fH>a(dr(RsKC@Qc+#V%M!jS-xeJB{QcF`={QAI_6oG~1Ga%Pwdymw&1^{(Wl3lxOkF3}rmsYuvV3F+Y> z=^C6NWxw|6?OD(d(OMt9R&pMz^^p(+Holt)?L-&@IsZ2H8$WSS{v{2UE>Q|ig%U>e zPS5mgc0twhsJV1s_y14T4gYz&lNR>YMZOlv)Er%ZAI83Px#~`!f9|xI>NknQnsDB8#q5=8*Y7;GPjsTQkL`EyE~{;tM;Zw#WC5YsfHwB z_Y#qwOs33;O$?VwPk2xRpaUJ$#bI2;?1~%IgFk7!GZ_`qTDs|e((gh%p)?^kqk<~0E;1e{w}k=!-sN)4+qXSUey z#X8%pDUgunNKe&T@EDCDI^9S7wYNtd@_?%4-aZ|+?Wys;0!)Sh<)RS6A-B*P7^;xi zz4M$8HC^w=qde)ngSrbDI3=}D<(gG(pZ%j;BP7{OsUo!Ug@DF(HWeuE`hx;V(?RK4 zsbu~H{wC;0&bP#z-4>exyg$m{dC4ZD`68F-&DH{yA{jD(OQpN`;#y~m4VTwx*SMCh zOTWtDVCg*ZmBZ+;)X=)+tP2f9og;wSrAdoLs)w$tKm!yf@{L$TaKR*8){5&5h%>VlZxJsg#Wjc1Er-d6s*+%XWsS`Qf)bO{gY)XY|r% zVI^9xl$v5nLST$30s@6?bTQKt zpO~KNv(Pn!FFF+s7Q}x-n$*d{Kxh$`vK^gI{~D{L!wzBT|$pTZJwI7z3zFY4jc1Q|-K; zv^pQgQxxCZEMrv^pDxENuLqJUJ2)f|b$cv8&iJ#MJsvE>Pbg7zkul4|KHHt4o(>bo z&K{SgqP6UPnBa?F_!7v@hQP^mp)^_nGz`4FMn@?+ofvrTg_j+bny7_{=(L$FQVb6% z!qa}H1p-Y8!%LioU$8gxzFOIKw;i%-jub|KiU#Xr_P0gXR4+&LWjq;^A&)qbQtI(o z6v#}$Y=$6&e)Nm%%=|n8<+wrpdP#!DD8iqWNy-sGWrdV9nAzZD4mE-O?#k|w6eF_m zhqU{bV>_ztOXleo=)@Tak`P(@o_E>Pc>jOUOq>sMW-LOGO|GcA2b5CV*OsHfC(V9_ z8NS%(o}(UzuR1zEikHFf7xq9au~z#7c9c zuK?TDE)``S^$$%@lzZ19=!?MuS}9O+!1zM4xZljtWdiF*YPytiBNyiCQHYjjYHH@k%aQei_P(DoZYV*V9OwB9&jjB<3>+{=|z0P zefqR(p)f{~!OIVdbB^OLlt3`!L zQ8_-RC6!pZ1NHH!u*?twJdeZd#fGK&btwJi>kM8`#5qTgz7JA)5mJ3zL#thY*$#0U z8xO#kXFj_oQsYh*NZAr+b;uKWLXB^E{Ah&Rl|0A0^;ywQhVPTvNzE6fV%fuIlumo2 zh%Hd7=XLN(w@b*>x{)j|=JR7sD2u^Z0%mNF+7tijF@p8_tN{DZarEo`bs21V?dnD( zq+1-#t9%@uw%{g&5A2QWPfx6$`Rrf4aBdgsi5Yh)B5Y4mEvdeJN?2T9BqJl&L=(6{ z_cI)bG2~RqKnt5Y8A%SX)4wfxa1F*LMzrOXNnnzjcDFzW#T>-y=OKvrK%>k z>jSv(pNSYdC*%pzP^Bbr z+Og8Npkj*zSnPzYenGCuo~8=ay(zryednY(WmOcU|`$w^3am%TG)1U9F z)>o_S?K}~A$@Bs)^}Y67q;1W8-o4l(&M1{~{aT+m(F{D6zL%b69z4o}N_vBh|$) zStcN;B_2`Sak^m%YtvAqGOlnpB*;?9!RGZ^8?8=Rn6ROhKN>j*2?=7t5ye=+d;6Sn z(vBQSwcbo^Lz;NB7$*7Y%Lgh+lYIK6mexY+i@s#d!|U{;5m~C0l|lD29j;OX-8degrx&3@O1L^-kW#&W3N}w)%DTBCN`4 zrxj0Yv`Hj@XCIOOvAiOc8c91^&L`n;)kq?Sw|!(2vEK6<4lbykx|5l4K4lY_9tMs6 zCZi`@qmdj@3&L;dvU=YUzLLdVad-RSUu0M+pEPN5xJbbw6^mDLE;T@A`KYTkf!lNG zF-;!WhXN2dS{pdVnHszcE6UifRTp(dw24Y(F zZ3}4qhiQ>M4E;aetI!<83{~rgHa0gpS2+mt!O2R`-=4OQ6?NaIU~ab=p-i`-p^v5O zZ@TT<>I#FZ2ZEVKCTPsd%K>0C4CuOa`PP+zmmli7lJDRDmB}rtWS{jxnvQ6X1^1Mvxf9=tQugh-IbgSHe3FJKO7wM~IVVrd zM?4!s@%p{*GiV&CTyviNzlz7j_z{FpJyXemASh8fM@VB_?vbUFZcEgBddo1wn(jLOu!+z()xI;GkBvp6jmV z@gdeQd1E1Z=s(UMf%43MiI%`BluWv&Xi_ktGA3KZVzOWy|#%2SalD( zRlH{WKUmE^BI^=&gZcjKgr2P59%$zt0^f)0$NjbLh&y`sU)=9E>JfOH8CTv;<40rjBWw|if)4ksd78Q!q0Rz+!Y7x&GdH_XKe*yt7HyyCdU};oTLLkO#2My7L zAM)*9bde=&Qla3|8QzSBsdERI6?N03!p4%ZJ^@hrt$`R0&Uwq5s$6zI1l9Xmp4!YF z#wvN?ulsSTr1_cl5s~uJC(ADGiBiF~m6X!I{fjo+0dz9YtIium%-NYoKlIBICd zDB|_oPpuEQ?r%RCt5&=k_r{;|9_XWj(K{ixz4mh)msIcz>ggL%C=#W_OPvwcCvPgg z4`SU;!gK8VBd4v3Ok>G9H1tJN%UM@B>4$em?a9+-Y|e1CTx;!z`W2ttUj!zs5<{hV zmeajRtP-_W{+s^3|16h}DkLm^hoyqB!SAbc4f4ode(QVQ)vvd$icbKWOXLyHI_ul4 ze6cCTjQ9RZ()$p>>TucXdFJ_rh5td|`pFs<>(&k@|pMq&M{yg*Y7g9v%(|%KtM(mX+_`msqiu3iSlq$ zxv`o?dr(3Q!||2yJ$^?5{(*P`Yz{j^ouEkM4*VS#B%@@aX}S`nGI__G3gLbjgWYFs zvZY}M&XE|GykT3E+a0csKpQw1ASW2C@$5wTq`^Y+9Tz?z%0X@i~0J4xWhG?0uXKcHIYTY(SYymab~Rn&-2$V9xHd9|GGVDf?{l3 znoVw)kXPy`j&|}@Zsw`cy+z({^L0_%g0(>?z_LDaBodGngQ0o(2pdznMq++n)xVf9 zXZJZzN$Ba$g8iUa@0%L;qw5|pa4wq}46zuu>fOgt9B;~X%-!@OQAVzLG1HfABK|Dj zl6mOgAP_;D5$CePI|daH=9>~`-|dhdSfhsgiZ59^ued(;WZE}g@>b-+GLe$BQw1TU zP4;tbA31uR3D11r+U#%Y;wX$8gOLduV#F4LV2#l8pg7zO?o=>h=pls-ii&a?Zm1v> zA=JJ+6T4dte+fEm7eYGwb)`oBw#D|bs}h+q*m1J9UhUep>A@5Fi*?3@P_UF_&m=WW z3%XtvW}|5TS_|bBe#%2I(EGI9q-p70U2U96o1h(_)$3MMJN@Yn&;KMWRlIlh?YUm| zQ>)Kb%cea}(`o;77-}hy=Yevm^6&+>oX_WpF@H2Ty~px@>mf9G(s!6~nox5k0J)6T zvFn7D>Mu4&J1ya%j=o-RrIl_qH#ZuAeA<1fHgDb{s6P{+e#0VcDI5gd$HD-xlM-^| zA+mto(Xh075%_H=TEPgU`UQ#CjTWeUTV`?^)7*`n&7dT5>Xw?AU3!R-|C1+a&}wE0 zK_J>!-zoSk)*yeztFL@bns~XU^W^cp>Fe1m;cL$^n)ao^0f`>@*|aK;$}1+O?5YnP zP1lF(UL!>-ZYgFLdDXx)cbqk&(9NE zZ5Niv(8%5PQ`7DCm!kDyezmV;I_YKV?QmIsd^N-_e)n; zh^aBTDX8HatuEpdfmpxx10@8uF)9wDtYM!Y7L0ekc~95OlsL{S zdC#SM?Ib&do3>x~I$nNu^n+3JVM2r$v#h60PJdEU@EW*!Kf{8#g@(9g7I;8R$zCX5 zkTfq^bNt@WvRm(klwV2571%oh3Z`FF8XX0))y?|fhjP<_;D?9|1-L2U=K!wA+=8OB zL{|yQi_>LgOoM>JYb=8Q?YvWmusm1C--LywM|j63BYKHnNW*b6%0{K6mZnRY_J1}try9o$T2OlJ5#84SyJ?sK#16X3lX#+AD4y` zPNf5;z+rn^C*(770qTkRlp%%t8~!J0I_KS<{shW{^ts8(CZZBQ?AovabjsaA8S4>U z;h$%VizZOvDh4^JMpbJH*Li8vGhDT^IYyE~J6hD{>CDss3<45W%@=Lv`(Whuq`~e% zLi#KgEpCf(bZytKm|VYEvx|-25lp4l6XGwjw5&7&5Eio^#0Ee(oKN zM?pej&>|L4VIZZ&Mnd}3@dzY?Q|JK%7pT?mPcF2*cI*@+!`_A?e4dDiNQO@5F?iU}%v7u+&weJSqd+Vs|kWOp2Z}J8jU9Kb@&YY&M3&@X6BFWfJ01 z3<4O63lnHBvwyc$rp>QqwvUMVl?wi=gIpHc59~XgnjF;lAyMx&@A$X$XY9P8)hk|) zi#v)4j^mm9V^o$4w9&cTwVW-G5%%=2V@)SX&yU4u<4aBO zog47-6`jyZJp`%b#u@8J%ZsBFCHLnJ%$J$(wDGoM7uNUmr6Z`Piw3PjwBAL&FXG2* zpOmp!-^{U!nqr?R7MdSO*^U>})tZ@^_fEvn@G60l`tY^01e&O)$&Nu36chwfXxwdg zkn}QBwOL&tjKkBhqkBK4zGP=2405n>7-crNZb>fh92!6kh!5)2_9m9!k*^kWk~TL! zQGb^_JwlEDz7gqmk1JxUiJ7A4xY9KCP`C0wyhC6#{ZY+=`&eJ$C(YNI;szeUynF3@lTGpagwp6Sigc+N0J}O z*FmYo%h&~}=}Jf(ak=Tur&RO3BeurX9AlCZD#j#bDeO2S&>YvF>=6G8fPP+N6nx9H za7O;=$pcglemzm&xc{NhQTZ=`pYCXV0*FP#h8RnK}GJOKSZsArHmsgCK1C4{R6r`Kd-Rf6fb!2e42|*@O~&@@+tm`wxfSpUzs+ov%nN6 z7eK5^nc4R;KSwot?shBlf56AVIXpEz2)mt($K$asQYwDmZ3#|sTm<%m^#smwHV($-v%*UwhfxxdMae=r2+5!-N4JL(`gG3=P}$u*vv4IS6% zBVq6zHEMWYo*?)@N{XqhtfrYUdF*)Wl&GDsPkJS|j3HknkQ90fRsPvY@WAbHGK(Yd zx?sS-4X&%>@tM}P2RgOV72-4t?~m&=^ZOaD^|BO=(-4AQ8yTKqB4 z@ZEeyKoP#%g@lHYUNJwwzm$GrRQpPl}Oa zS^8K~KjG$Br9wd1{enTgK~|IS*&;IP5d2O-Y{5Bu!>%LxxW=1DRB_%Ik(%0^wotPs zK0&ccg(18)OeFP3V35=KhvUVZ)l7XAFh9?&zd9bNi!EZt-~{fouMY&`CzQ$Tixn=aC1``cXqnsJ#Cb z|5K^q*u(lC24T(4sL1j4r_Sj{De9s?Ph)6;j-2L!_&zWzwH&Ul;ThNLh!E?3N6G%q zhhaDm>HYUMn3@KcFYbu@M3^TiIQSQbxgeqxYN}cqgCv|(xw4if>o8A5W%Eh|w(V~g z2yqkw4QL1AK}D%%rk`ctBB}TnpXPU#)-N%mAyMH$C-{t+Wy+8U=s(AV5^t515Vp;d zb}P(3DnH^);hyA3rT%KV4!k*E7wGQi8+!gSzZhsTgq@6p5E;$HVR7E;Q|@YV-g?+` z_7Jg@ccES)cfNPDTkn7^C9o0#K|{)FtJ|Z75O@bwzCWAvdoxIXl_gnJsN7c3WQd+5D$T;>uCj-Fz z7tgR=6EJy7;lo`ozc-CliOwDHo&zYhrY3Rmpl%wjNN8*lDsDoXe!p4Z${}pe9SGCY zmJXy^&TGatTHy-uW3j|F<@qKn(^~}4(Qbyou&8&cP5$jWhdk}&ya8z)BsgQR}W1%HmG<6^$hww3vjAPGi(t5w{l~M9O7LGi22FRiDnzk zzfk=51g3i)4Dos{AvV3AabZb7v}^Zp@V;&fGHc(IOGB*FR*z`&H(;oKI~F1?j5QrB zVP`50Q-XBf6Mz45 z_TDh{IRcv*;j460v3-4GW5}_z-@K`VHCBnTK`{5`69(VCG~%SUeYvbNFw-n_6^m9`@>r^d`1kj!csjl&6zmY z;WJAVS?DP{J@QE|g~po)M4j+sqJQ~>-jir4Yt1(o6T+(Nt$3uA4|@c zvQa0M;f#-M+N|X#cgn%2v%&n`>;j0rjp1EtfOv(7X&qVl-#|p@bSVrpDveu4JfvXL zfJ*IH>sJO8UHi<@_60pK7bp#o^5NZdOc#4}xt%8l#_XZgEa|ibb=P4!q567fJJ>Ah zjA?_bk^eV44GVh;sd*nL994P-y=X_RT&>T1GtNqk+4?0$7pS6Q``K@!{ZSs7JKx3> zns9DzE%uvO!d}Fxnh0eA_uD*Fo?lkcIG_!%ANUSl!YB^&FtfqeO8lX%n1zW6Pp&nb=ccrW&sjW*K(k&~ocq@)nXRl?K7{fGg@g|N z>vAHG-r|TdSW=zU^avxlLd_hAjXh|#wUI*Hb|Zd!Ibia&eRZ6d$)KM|R_OVczC01_ z`(*~M`+4?R|KO~5+QH|Dj-N`Zq~1;E=vP@fV3Wt!c&(<;$VF^Gaye&M_^CUG*R^1# z&Zl{|JwMQroQ5*`&qVj+!Rf3Up=zqV)!w0$Q`km62`M&0Xi~A1+W2*CM5t&QSq}<1 zYznB{B7&8B86(vV=V%Ah0O!Gilo4S}DzEuBX(}y{0wqbt<)8nfTpRilPY}ouWEk#V zB$|JmjBvj%ep2f+WR~=N1D>}W=*)5)@IcNL0jIQmF)eiY#>(iy{_c@%Dy3THQR>;t z&_AV(8v^w73PYL5(7ZLJERLuN_~*Az%_FOy*>mX6L;9@8JDkn|`B_+&Xg}R&F0z0A zx8m;0M)F^>>cv1QZ(a6laaPuJKA4u0HGXWt+k0?YzR&CV`cYB9(ce3POy^)cKt%l! zr6w{?cUYWH+Po{$dI@IhOwUUc6&iN8T8aR78Qd0ndRghuUf!ft8(yy2IdBl`aV1km zsSV&1v2;J+JpJ1KGNb>wRUMV3Sq^K(8gI$zd+)kJTThmq>%O96%(lhM1=U|so$t72 z0+{jCjU!+VdHdO!Yqf+JZT~@E`!@x!TEUAmQz18U{-a>nE~+&sO^*vYYQWg?T+1V# z!xPORj*JFSGN_P3c<1b}$@b*ZQr-{ZT`j1uxNa1k&@`*}2KzWAS?M{bm}mV-LUfIdY+WZ8|n8ZxkL- z=}jpgkC?wTcf`+Y2FKGIy|UV{Qw5O;E?1C8XXF8*0(sd~lTxHl8!A5pq&OFq4k7*# z9wa>SJEp;d;gc6GmRh+sbGt@TNf^wB!^fBWDZKD5QW4p1vCtS8ZptUv?oQ=)MrY9Q zyGCa8zVTUmhqpFe?=QJ;SF*%Y z;4wPe<7DuV>Qd-mdmOjq@UDj3u#e{IZ{P3c7(-lQduDxDpt2Y{f^5Q(H&!;4un~&XM5jr@k znKpI~yIaxbeMqBlDPEWJ=b6?ffy4a=)hp{R8Gh#d50~%@Rk7{(SKhAm+V9WWxW$j6 zFzJmc*eJBqCTLoVhS>gPv+r_~iqenD!{-QwH-J;ldDId=?BXTSiGS;ZN2Vd(If49G z<;{@{7*N&jf0%=*C&UxnwEojLg+~QQ1eEV*4tT~o-7kVwB+V&HP9cFP2BIdT>%2Nv zzt{LqUiX~OwB7!ra~QuR14E|1?>n2TK5$-Rox9?lw29!Qvz$f0pzrL0#=A-28j@K_ zV4{ecuY{U1Z8++bkor)Rw0*^+9{X|8z{zKFF!by|-7*P73=BMjp@^i5)emJa0wjnT zA54eYJoOT3yw3Z-QXiI03k1zW>Ldeo=>RYK z7qG9`JoW6+v$9Dh^Axc;KPFM!{ls$Sdjlb#lXq}@cU0JONAU9ZJPuh(}|HB zD);q+WOJ=3M@6X{GQRkS{Te)~)IWkMbpY8pi*Jx3P`v?KwgZXJ{T4>^K)DnFwg@(w z#9h%8nw+*-cF5~J=1wyVvEV=AzNQ?Ut{~9@q@Vuf$B_uZjbJU(o5N_Ca@MkFq{#X_ zLourrJB)d1HXbb14d=>isJX1bGJJ0kiOFd7Z^w`NFkZgbxbE3G${nwLXUfX2vb!3l z;>Kiw{VPHI5rGY&$-|!O!%mV&t~}7YB6jcI%3ERfuXHwT`8&@lv}q}c2!KGrG{|5K zpa}U#@e(8$wOrM*?!OdNwp6+%AFMTIX5>&hiYv}8jyYzoH44cOiRdFTiJ|iY!fGbS zqk`a(2PIFnBso-d^B?}=Q)yZH|LuQ1p-jBi8H||+2L}&D_8PkNccW{~j%=i9aKymL z?VIuI2ZqaGMkURqBFjsXuqA~5wk<6-0kfCECxk3`Ije8(9H*;!lLGMfmmSlXehT>( z?M0pLik}47%v|(VsLj&ki6r8dVR@6 z5}Qu(GQ$tN`E|;gdj){UDgKTw0JcbyLnQ$nzpFXfwd4ggNP9lS8C~~@@R#oWM4S2B zedm{_3}8_WW<4V|bw zU#AFg@DM;S`)5XSyReq(+Mg4w1lfbO0zqUifb3tNW1n$2^a1lLaTWC;Y7a!Gr(?CZ zw@V9Q{1}el4L8C@vjk}(Im;luRDP@yA23Crl8a7&(F|Agwe}NjF|9TzRO_W#&*36W zvi%u$M&DBrE=7nOu93aC#N(?kPVTUZdC^T{@FEr!BZGoYejr?-&VD1Ur&fIhW|kcnPqLXjvanHiIN_urNi<^}wEi@^Qei34O}5 z(NM!04uw>%y-{&?$OR+DYh8^Lu*)e3^E>o4tx`h%1Fgzs2?!$NW7t?Ih`5J>vWU)} z#L9MOeL34sVVTpE7WBk|r)x?10D?p~G{o)c444kX8@I?qWE=}HpuxiNu2 z-@q_P{Y`MJ_6*zCz38{0BoZYc`p{4w>9r3%%B;$}2KAD#$@DtYU4@6V>&~L~f&ex} zMAnl%`LaV_98(M6>}+~I|7Z%C!OED1b+caJ=`p(LLG_lTX1H=BLAIyuK`|WwW+H^B z8aT-J0n&vOK;`s)Ni1kjDEXRTmC}OVDI_H=3HKHVTDV-OjgU5hOyc%NR1%cP4%QsbX1ALBJW8o2%z$8kNI&W?7Jwtt2h2P1Yb!28yTyk)& zm!qVqce`E?Ld+QV`c9Au%?ZGm0)Vx^PuGnUk>8`dTQnNV{9wldx?*3I(gzAVppE&z z7l#v4XhJhtgf#;<^I5eBj)N4};%G67&Wue@cc2bJwXvWh^Z9pU%)(e(s?ouXILZXc%$Lri z>R4)}{H6-=Y-ivk82U7laZNe`0s|ky^D13Y8 zdqLKzi!<@O1c(Tli|~jGD=V-Z;%cPq+)pf)yu{oj%-@7(RNODOe?_q3d1I)ye4`!( z)boo|>G2^oXMo=DOa9ApqV;Y2mdkuvo)B2c{j)qHBn})6Ej=Ary$X?WBp>C@9mALjArHJSTf zu=>Hs``|JGRQRC;*&H^{!ln!MS-Mr+>BCLUx=FcqE*i1H@2Q6>%RddXfTZ1%H*iss z3fIWXUr)c}axLmjYJW5@ac0$hri&@%3IqA95UgL}-3iM+c~%Dae9df5ngbAvfNqyo z?mHA~Yrr{`W|KfCKqjK?Zqvsbo2&ET6@O2Qa_-lm`lKK;cHu#d^bdS@$ossC^txCO z%Eym~J}LngMq4@KP zbeoX%zzSAon}v_xPACn0nny8_PmlW(En)KD!X%q{xrY}{;!qj=HumelFt$F16#jE@@uxjRPQ;GFzhWwY~9nUSs0J@vFY z$}v~L(?Km+CK%DPH&uZy#Pt;&35OA{(Fv!?1maO#UhePq4!$PKkv^-RsMXXA>7HL) zGSK@W+*P&or!uIQWQ9Dxo&SWa;PdIGc$RUtDX6>epeeIrpFanJfkiA7Mdld2-nKUq z?zAyaM`AnVGDl{)%k^>rX2Me~!Y25pc>^BrAR`O8g`^Pbg8JbBdlEx$Qr`y>(PW=> zC6$J+`F5`&&koI}lH{CO*1R z5GG3fWYH=o>)g#^BPs&mcnf{|inSuw=q|mKd{|pc&n$;$j@8 zG?^1E+pj>pZ%KCXC|T?>a32?M`1Kb|Y6+R~QBsWmcD(`cdHDryUi728C)BsU$esvr zyJXxTF}t`PUg+uBxpSKcwGajaiW(U&^=orRvYx_KE>T&h!)}MeYFPT!&Vqc<0u&L7 z^M@#IsBy~;Y4Kng>=nnUqTZ;5mgHi2+d8#>2!p}nFLKHf)?1C0H1rROkiC7t5LjY; zCA^QmZWl=E^?!S*nEyIPY}0}jCQxg8VO3mv`?c9|p!IW$YQ6#0+`;9!`3yNl)r%!g z{;2b8-uFD;4r?3}Bgb=7nyTV*)C;nBS2LT7{1h-MJWQX?T#)F-x!o&_IXTDtI7E)N zF+nOZqxHR0uX(%`HZ{|-gMTSr5rdb<$cVAALaIi`Cg{W(n+<2OBA4a^SWG9wp1gnO zsBcfhskInb9C=nk`tP-h`h-D$+=M7n{Y4}P5SixB%Z-jA!6Fb)>LREU=~xaT|C_59 z+%Jhe-oU~^zyX7r(X&+M$k+Odz>9>NbCho2DHH-{#PqN}YlZG;8w{#uvs@g`8MB^l zU|9o769<%+8r$s$Kdqdv45py{xeJRZNm!&3ezDgNLL+7!f*CN;`=9Ca;TB5rCiAQJ z{#E6DYofd1>-pk9*YjcFk+U&&oC3Y-zKs3Y`h$+rpn`l?3)5sGe*}r#u6&0EHdm+r zs;bM07OyVj9n4GrV;Hm0BvJS!eP3c2qGavy5`_DN0ixu?@uOVqrvde}UOI~dpul4L(DOdph$o4>tJEyZhj=1!_$6jgY5wu z;(VEW3jJZcp94)d+e!>=LiVsq-_Cs<9FW?)aZ%bLeK_e-So3+?!Ss&P{D!Q-q(gx~ z@&ulFVZi`4WxgYgx(`$pl4jtt^W=HXswCedb5P!zp>!FDe}A`~UQE0Y<7p_Ym3mjQ zw-YfTsuQjHaxb2$7EFPt0Ka|NAo{lM4 zsVP2=C6z^xd@fB8qPnIL5Y!6Eo(gHJ^X~1RUzV_tmQLxM2-*~*<}#u7r;?D=Bz1ou zh>OC){Ir8Yw2`GCS=lZR?36&OlV3s61kY1}q+Tt-4aTTOx9Or!2|tHT8Ci|k^QNmN`G% zNu!0B=Dm6MiO+QYQdH?>i@UL6SK_Yeq%W>@kzk}RUnGuq7?3E4MTh>sj|+LqEdW=6 znV*t{pcBOVz(av$KMa7Q0;&_pU=@<*#mf}PxdZ6?>#CV={tpWf#EJ9WR;{S6_-_DI zqpk(M1c!|1IwlAa>u%x0&USjJ-kR@sd`a7r*;2eF8?6;pTQTH<{mb&cB>KbJe86qh z&8=K#WC~mFBIYh-EseY+u}P35>;bJrxc0(ZT37c3;nq`ttAyF^sXlRbK`4^dzSpH6@3yHjI)V&MZtP5!WpJ?+}eqx;tlp+U|8)Oxt|D!~z z5wm3`if8~*y(7B0LMMC~FlM<}GA}D=ivNGs=Yv=()Psp6oQ5XwL@dSL+PWGIWlGH( zj$Pw{zv6SgviaDx@p^pzg{qn7>V#gl+}=B|fzt{HZXYIJgr_4I^y(1AG5M1LaNw!3 z*05t+pPr$RwInQmA2N0)kWV7zQQzE+A4As_gaTHexYZJG0MMg~+ca(TzB6v%iI1i8}bLg!NHnvvE>H-EdAtdOP}1V^PVx2!E=#c$sp+3>=_+j zZgkgtZ*Z&gYNnDDcb8Wk+E9(EkQ*V%rWeX^zTUeA>oI&VQqb0z+_QnCuD=Va7yyn? z)_c2<0-DUXE1vj!0Ta_DwzfJ12xaPgm^j1dam!!3vjH({4dv}5TVKq81mh-PLKwRR z&V)O$5N-oK|Fy=NXix72iNXWDx~b4gY=xcsZ75nKep zIV!00-5V!R8>QZc@oHEv{&8@HD=9@nXVd-ZlNWhBLR=C%4BGEGB~WWY9TgXRpUY}t z6*P(#6C#CVDE+LB7d-jURUhLfcg|i>nQ7*yrEoEct&%Vl7fc`izk=>vCYKfs^v<1= zW`Yl2#&T~!?~SkWd!vh5gAseU@2bbS_9>}^6-#G>w;w`HYSdEM-z;pZ zKl;YYGk12uJ&GFYHf2uEe)NFfbV!#3g@XrWv5pN^UKpfO;N}ox{hd10FIVDn(13x+ zdD-J>K3`a4s6vN04Sxu_O1J2Y9v_&JNUo$r?Z7{%kfjY4XJ-OS!3fph%kti@7dBwIXjY2 z5y8UY)mHFXc3uG#UM>qDXLtK$A7Aan4PPC?p=I`7 zj(_8&DWR2Q`w%*aDxxR^$Bo6D?020|Wdr$Chl_hzTMtG4JQHadySr4KjIe1igdwAQ zurc21u@SsI>to*|y~xFTK-%wZ(bUUhIRNJ#S-dYE7EAN`WKozb**ThU_+y1iPOUBGwXH0f)f>9*QiZ}Elz18zu(SNB{#Em`TLe-UW&nPw1! zl{cLiom*ZWDTJ*J7oIGo)-Kq6WjY%|6wtf|!HiR($LoA~$cP3mhZsSVV6I(IR6P7z zoD)TmgO!#9??a62Oqc*#-yOGASuIbEQ*p#yFx6o5Z2pPnOQvPT!@}EJJ*yS}?vXd1 zppB$hKejRqMF8SXttsj8wGTTBi0EwYgV^!ITJY5Q$vNH#c6jule&`9BCd$?R zH@x+CsI06k=0XJlZISGjQOUUW_-ETw3%&hgj|`RV5leN1>G~1S$0cKcxi+D%5>fZb zimd@fpA$JSR4gz7n8M39G)aYSk8rEW$cREEaNr+-$c^yDvW-_GzY+*->U*>OK4U#S zxlaK`VQp<3ZEfv^#YGWRbsks=QaD)neGaAF!rfuf=Ye(cUz&jrsrSW9?z=VHB=0K? zg^H*;is|oHo}p|~?X`MqphgauWWRaGTf7-vO2nS>dgkghED^}`QT8kfV#S^4-=IS{ z_J@u2=s%d|Rfo#;s!(6H5`-iOasiP21g~SjvX+I^6Z!;mCGZ=;xpp8>$4Y?@~v|NO-i|;f8@PE)aCzf7zsx& zV;bdPxwgHN(z&5Y+DH{x`m_(lpUF zx?8L0wIR_EYp97d$UGCJ!TIk9=E)yaf>t-4*#9^E0-AwQT7+Tz1q%|wKGdim+NQn> zAli!YNfbQd(qH}xZN`{3FJa2M`{-2p!OkF#OU$En;~p&41D6SjI=}}p27lY?qOKj* zgPUs zD(?rWeBW_)pBU+ z|J__ey&TTtc&zj|_HdRIFz4~ceQO|VB23jk6Iall0$nDd&&>Crz-xX9?i zuZMc#r$iawL8H8=a$s7rxBK};sl)cG%sv-Wr4%&v4)%PNm&t~W~KZxfGx8YfSJ~NcW07) zn1W5g;r#^_d+$Z$PH^`M@&xf`CT&LRb2Vo6y!By?Rsja^#ADcR(1P25;YRFoJxkUT z(LOlMSa}<4Q$sd zY3bqdl#djjy}>&dWey+{Un~?SMmlJWPyj9i!-cjMP&7>M@O7n=q|+W-Sv3T{<_`=M z%wGJQ)YN81_qrSZjii;rv*=-2r5&LFU5GuiRzOv0Zy?U(>9#yvtIcn?YQJbZ9H1!P z!UrD>0_n^1JAc}l%YDn;JwS3IE%55Jted+Hn>^_3tl#^JVu zCM%rDo6}(eU6_$0-`Z)QzFEyi08X6XE*Nsi&(qgJB)@)EKx@k`8djjSj#-Tyo4_9b z<|EmjCwfLGqCqS3tUOXIwa4o%y;Lr}^eIUJ^2`F%Mn&GVC)&w1u%Z{KmwcvRV2073 zA%L1FB`j55qi?SyM{uOKf9f2pK2{2U13USG;eYG0Kk6AY?t1@rt+mADZNh5D7mAxT zj*G{ihu$i_)-+e&9c_A&FsZ>qJmbz5*mCaJ^O0Z7`fjkWqPXYn&vTZV590m>eez!d z3UC7MgaJ?+8)*mitH<$oR!AJ`a+ML#&|jT^Y-~;o>yHJ3+Wd z`vLQGx>7C!GMu_AQXVGA*Ij#o)MncJvXnw47X;>u1)HGJ$Fb6sTI+*#)(5n$_Xs`v zXRE42PPrz`0ki&i`}(bt%A*PF%)PMy-Z`9x3OIq*A=Oq3Z9C@D#u4oNb0Ksb;WNKj zI=0flF-vZ4g8Q}s%1f!IM)MWCH^ik#3^m1Ky(paX5Z{Xk>;+8QWm*TzY%XY$szbM-4WfcT)#_3yr*Sx zlCD0zHR;9UD{Rqf=-E`ggq9brrvz?`e${lOXjG&WdIZp1@`-wKAVnKpY`6wCpesQ; zg`=;Nb_!~qihqI=&V*63;~MDzS#ps>B$65%!~R!!f5Hy`H<~6f75j15u@F*XMO-r^cb`j01m1I%TElawE4v~IO zG)+tFy~e1A;mSHw9M1dD{ap1ecWYUasz*G8kyp<17PV>C{N%*`Gi)hBNTm|x8}Fwz zEt<|^UtNPMgUiNwp~cfqn}kO@)=FDJn5k;a-ZP)|UK3Z9kdO3ioV#3{&O`^>Yxstx zZFTfY`)N6AY~FBL&k~?7Qn@x5Hsq>B_Pc%OmGA3T_BH2QeKdrR8&4AzxGb!!s%+#Idgk9gJXIiSp3Sxa|9z;Y1bJgU?1PY@|`BE#TqLOg=&OGH+rkJ?hVsw zh5t%(oA7xbPx1cqIw6&(qx>1JF>S2M5ZaChVK~_YioR4Kgvl+&jzF+YJx1-6QU>U^ zt}O(xq~D<#_Tp%JiOn(jBXRNrV)i&p%(CN!IgD493Nf~$9tT7|$jUylvXMZ{EgvBe zr~Kf0L+tuMJDHF4-x|;}xxyJmRIL98H16zhBtYH-^v)xtvIj@-zR|LFwr6M2T^HvS zC$AOHI@z2TcUPmDM|eSc6;WGhDB75+70q`k0lo5(7ivmzu_z_4z!}C@*r$^7{kSIvxne$ z7pHB;Km(qv7n)SatN7i;t7d>5@&rnsz6&Uh%MN#UALsuWf6*p>`}^fmSM6-eZj5!H zeMw%dvEjWDAY7p@)_nE-PJf=z8eSd#2u{3r3c76!0l+;i0$S|yR@1P1T-{p(UtJvLGfNcWsU#;hzqYN_O46 zt!^ye-x0AH+;XDUG=wN}_{^4k)&>)g(G4*e5u^>w(#`cElVkplG%!_-AZUgCpMa-K z65+R@0AeIC<2(dq<@SUi)4dU8tyUi|WXn0wT(SW(WtOwlnEb)icl31Ei*6iyUKp-I zo-*ULTgz4AqB8Kxld93*cK0r{IYl0K6V=rJm$mysypw zGHoyH8?pmw)e%B+%qz1#O5^qJc<=te6}??dxaXY5(-`~pg>eKt-%dEXWZLjF%cF0m z$5U%__WWFc6GNB%u#C9Pv5LgDlQ>lEv|}Im5?%Rwm5Q<>8h0zf8sv;X{GDSGs4e4|KM95vPA@lrvT$wM~ zZ)_sC*j3U$ugA4as>i@Y$G$DWIIStLQ6aKKdN`u4RQhrLofT(X5ob1+lbCVIT@RO_ z#dlF0_poZ-3C%Kk6wZYyt?9<+NGUYdcgG;EwhbuY`QOn4n|lrV+h`Z+yEm)R54$nT z9emq&ovZBx?;A|#)cAa9d9~gcf~>P(($$>KHGVcph7(+@(+-mo5Ol&Mob2D1=l+7U zgQI192M|`o#C1|@j=~Q~#y$`tv3vTyJOo*8Qq*j5bbHU2O5=U(RYA>p{g<1wI@20u z*>i`^XQtKbtxE|vN|vAFr2}B3q^60}P?<8VqVUX-`|=LoRsHh4TIhffvAMMMs=qWx z#rsSmc-r0n(A<~U_zqIS1D4zlY;Tsh4YRE_`~WwOW|#ts}rji zM>mEntdX@65(v~(#IL*!)i-C?MZ|1=LhuQw0(1Wnk6OabV6)B>i5{N! z7n4PB>RJ(b4|bVqy>lA*MbVIhOHK|ANC0t~NLOC;K2@dyAsxTd?nS(1cl!NQzVVe9 zlk1Ew{4eyl5(bVJzsrOM#}WE+m7yf+fvG}r?g_T_O(Q|GEB)%cMvU*>Ds4)_Fw32E zDU)MlNu_d0v8k!FnijC1%Y@Mxg*Ce2PPVn_TEJ#IwVnMsX^Z&y$Ucn2#vN54 zG$-ruGi|KAnI|6ntk zv!;k!U3$jASN@V1DLQ#fmtaKhL)XHTV3A^TT>ft_*SnL8gee?%sE1^RGsx%rrQzed zJs_3ImBHov`i;(GLfODJMQAUTZ)ZUvgN{kk0Xyn!B%bMg0>T4@A-O-AFHZ=`bbyJD z9yzk`(AE4vFY6lDGO^@M`zvXS%5T5g<#mQE2VYt7ENs5cle;1(y>~u_`R42%9xJE5 z)N+X#?~>vEA8s+;^58{VI>Y3pF)D#}{i~Mucy|4P3azT-`~L9l$!CGuvr_=hIkBRw z3x~Y~TS;bH)Tp9v=qIo8d>=%O_9%=j?M@q6_~#(UgYEu@#aa6KkBIfi?ouTt^zK2h%V!pP==g ztq2VReP}2wA$iTOZihE2fof9>AkpI1lQ^WDISh-s8?Ps)rF9b*WM;7_2&bL zPL~pDB<1q)y(j*x(DQ7bY%)9?$@c-4&gL~j`4aZ0MC^!^EB#rF={Rwxp{t&q z_{ERgk3p;Mam+WP1)^p*e30w-m7PW4JQ|H#Y8aLQ_P1XAe$OWv>F&9)&>lX{xXct^P>!`0{d>&dbv;=X_$vin0nlZol0m z8kc-}MJ@?$c$g9WAkOjBrX_uKVk{O)!SGEV7;fR-R@KLzZ6rq6v_A%8+NctT1Z!=t zHp>6T`5aL+*2<}wU3#MD284|F=ly+9RO;(Js$wX7$9Q^)%_5EU6 z4MPqI*`Yjp-doe^=*+@OX-dlaHbJ2;#XnTcl#qOGkUL>)&xt?vh=pe@63~xxq_KDa zEv9`n-R4|J$t8OuR&uhAA#GVOJlR9n5{)Ye(jg73Cc?|E-S*&$#T;l%*b48k|1$&N zu}95_R~NE_V*?<%Wt(F5|9AKig}uFbG+S(pH(Ko(=u$r0V|Fy7)#befY3#NrCZ?_e zv#>ah7ZTdOXOCCA%^{G%P%Dzx$<>D^kWH9gFA~^~>%~S*!khNMl8)?E z+dB`fsU*khtjC$V@7oO3KZ(d(yeBC`SoqBMP}9|(8cb+?h7HB3GQK^vKV^7b21LJ{ zri8aWrQa#R^F;K#YlsT@&ABY|U~n0dlBMOpcu5{}o0qzMe9sP)Z=X*{eI6fMkyn;lizb8DCbM@Od`l{hU;0W<_xrCsFilJ^94>Sl2cL z=P8NUdq**E0$+pST*7Mp#WuoF#M2`L@Ix6_Reo%&p|gw#B81!48$Y3j6UT$bc)xQ> zDRsDkuYY{A0f+0?9Y(Rc6*0YCUtfRi^<3|T!W=@X3t4SeR&%j}K+T#+q3s6LeBOk} zxiZfwx);j@M+i-QdmA=q*J&G$w03zN@+{w{dGa2QZ{t@^py%NXf_ambIap(gh z(B^@*{M)K+UKF6E<`X*^y?S5a;o0t8^i59@kViQ(&m{N{N?o4$h>|^Qh+S*;+kyk| zo43DSw2kVPAw#DjJ(}Y;FvGb%$E^j0o7W@1RJ3rbblJ@b(Ng??b5;cJ8l zOejVgN=*74k%(mwQtk|S)o1CikQsX|OJ9Ly{vjNps`4;utR`e`sWA7#iIknwEVsvczZuBf#&O`6mYGvI$#!m)){D+1%-XEeoG z+$l9Ww>rF!UAl8cSZVC0z%{&2mEn}&uQSQW!S-V9oQv#s^%;A)24eD#YTn2L?H1wM zn_ML$tzpSw5n`yG$%K^Z4q7?tY1IF3HKD=HCC^4G^V{yqa|v5_%g-ti3i@T>$mFZ` zbu!SShs@f8nhU=)pf*KheSpiPhSu5FRmQKi*36&K9K*;+E4OcI_p{8l7_vYGYzAvzqJ&ShMu3*IamQe?-((5jx4KUEw@-2Rp_vsJ<%m4_FW~NvvW}dC zxNk3j$dsUwG|@0C&H&YOrRmt=JVp4=Xa1!=p=j^ze?lgNr~pf`&ZlM-j4exRO=fgX z0!oKatQ@_ZC;p4)JA8&=lwa82(!--aEH(9gEO5Adowg*Z(M>nQi*&8d)PwhJS%=;J zzQz7#`=jaMuN?^c?|r_Hh|NtSJfGC`2s+&4eV3I#z^&%$Sj)moYII{sqE|mKr$AdD zv)ZlBGmH0QU&p@_{;LIGErnK23-0+sO1NKp0<&{3GO*!|gu(P;5O%@D&Ev2ZQOMh<5~lkArB=x7q<^{aWX$LkXI3 zCbUzW=vP~ma;#7D=*R|S7+G0h^L<|V_Thf>?B!Y)f>P(T`p8RSjKhCCYMm9m$_o)S6CS=D%~+q@V52Xk*HnrF$X9KkW+@JE zHVMkw%<~LvtZEpp7puelH!lZ2U*z!w*-=}dW{*^x`p8ONl6z!tjHVhPyMElU@_HOO z_l+_%T5!Z8*np#iB+-NXl>J|{ZyUVnqSimxwcebQuia~*w&(nMs|8~CG+JA;!vYn- zn}sF!_~T?8`Q@o5J0gfOlgE<>u0rbxyFVj~1|Ar0E!D*7a-}mqKb5nU`x7?Wi`Hd4 z-Fx|#f1i7*>r0VXTY{15V~hqpz9s<<8OcZacvzm_%oi@3@tN zj{fOE;=)7x0A+8nlvih)Ax+miSD$}T>vI9U7+d>=wrD&6hH2wl$)bnV75z>IQohu| z&Uuh#(cL1;+ZgNg%I1i50X&+DR?ok-Tm>8Fp?#gc+~4n1Y@kcyeoZu$9t<>IO|7(3 zQgmn4a6R{rI1CQf0rqA;ypPk^o7dBF9W&e~~r1Q|-2go$j zJseXX#Tk3S&DFizu4`Z@bMjLGm|0Hc23L=-ZPmjWi=M=`CKaqD+lZDr_@O7=|BOB+ zD75a?+m6ZDGkVX>R(j>ti@k1kQNK3y z^x6Jn0@9<$KOjmN6Ba%ooGUo2&5`6j$KP_MI^Nx8Hok!J(V4>YwUtml=f+J0*w~4W z9w_8CV!(idi!mN$wzeFs1wKom$GJ|jS3hX`c6;t{Ftcu$+|IRQ)LAAc885wPhdAk4 zjdLD!x^mp0IEET%(BpIku4o)eBXR!oLGd$KLZaB&w!8S85)>Y5yYp?CT zGQiVCHoMzbPPW&vo@>T;%GIpIH?L;Y^J{R&R_zaFrJz0P2aDLA3c&CUgauLlUDUy? z$j0Q`@u>DJE2W)(;dIsSsOM7A-wy_K1H`RPBXUkeC3)#?g)kis2TL?+kQgh{W zDC5tJbQ(YI?Azg*5s<9ufg#XJ_70+DP1}AQ*N{)!N`BK((%u=o;>r3M3rWHxqjEbh zLo3+KFlqz5qNkt__jo^Tf6@oHx$QR2+7R8~?2OspYf$jN zOgUuxW#$`RX>lG+78Kw)P<2_boGMp%$x_4h#|s>k=Ql)#ep%9&!c$`o<54xWTyhL$UcdA&*Hy8pRXvTApcVF{3Kr*kIC;DV6Op=1W%WaOp3v*NyB*-Wf#{gP#{NF6%H*F&*#yVZ zag`I%iNp|JlV{G)Y;J@1l6NQB8LeJ~?c zH*4jaMO&WxWG)0vJ&F{+$7s9_@5ez}Ft@wfls@CiaWWlf&p>~3riF$ovz6OPgEFT5 zdEscXvAIs!(C|>kI@VbnT2*Z(z|JwW=i!S={DUd^(ybBXgQG)YR zs{P*(Hr6*R_qPb+QBXgR>}tEe^|kB>qa+V9tfpP>^`?)oo_8o zv6s94>%POOvv41eUp)?6g)QJ5ARl%tgrq~M-eZ~OpK%0;N3kY)cA)u9lR}|t)S50< zco^4J1t!Vy5H;7OSP%CqTWNk+d&>%8)k58}U(ZdqnasNb6KOY_E6$QO2BP;dsk5+X zK$~eIR$ELVDLOg*it8HgPOAI0Q0VOeYsT#p$y)2DGNNX`9q}nan~>^0gF^#^&01Y$ zk#(s&bgZke+8Q{~0<<&0Z7R;6l64Y<7+)7jIxOo}Q1|>bf82}brH4(A0_$}IvTa2V(GE_XVaonT@V_cNB%zFV1#DL@B884{tP=Zzbw94B66a#?RwM8wWlGNR%2<@#h>O z#eog#-H^9D1#N{dMjB2F{Rt{*D3%iFxT=lQx@vbWHH9JY9E2jnx3CCy_b>Z+Oh3Hz z+CO7;^r-eDnhS)9O(w3$6Xka#9EGO?0p`v#{MT_}I>@tA(UU8zQ$fB9>S>?&LKy}3 zM{6?&nUoqWcn330z@Sqb7Jh)?l1yNU0ZEl2C~*$40~~yyOX8VI8O($c88dB-H4bh4 zdY2|8r|;ayaP3&32|CqGyLh4In?v$JQ?eJ|_=6IiMq1JBf`FB3IxoU=2 zBF1D4V*$n#7w*$j=D?VAM4R=dedk63cE8tc_(5Pmc$-Lay&z9J={HuMGjDD#WD~xz zTCyBkm%O^01$Z(wz(}f?v9uRL;;3u-Jy~tz!oMXTI?P&-h?#tvt^+aty?mq$toGrI z)t4_hw^1~F21~NQ`6L9d2p=pN(FoLOm&zF&=|S;<$_cl(86^ozYu~fh`(hu@@REt% znR4W0t|*Y2BaWaH3=#rW+lle%$j0Tzj2G7uLaZs$^r4lx%aiN$_DNm{Sl)2HtbJ5X zox3DusGYOh`ID_pe{b2G*9E)3`F_it6M|4(4$8B9F% zFPxpx_=+GTCKlGJ+=0Gp;RKH6i-05Xj$3ZgP(#3GyX@ylo;R;NFgKI}ijo4vpc7*7 z4&zVHH5A@(lTb$sgk(FqmcL!3)JAtw-`4k5ut-8AIL3?## zCVu+ZfrTMpysiU2!e`XHPc?@#Oi|~H7H!fi-caMqk4E?EK6@w~g*vS+KxE$JCM9KS z^m%Dw2upaSy0b2wHi->WxVRBPIesRF{D=gXpano)Pzm3C&}q*QKovJ3PhEvAjuJ~w z=PuG6;TKKyat_eP#A|g{R=#FGK6|bEREL0#M$9j7_rz>Ww3>XHOB9DQv_=>bGV`~a z?KQUL%1V&^P-X4>u;$xLrv9>*Z=x>c^MH-C&Uf6Luhe`8M(cUx9$Icus5w*j-IiR? zglnakjIPegbUk#xkhna8J(l8pz_l6{RZE^WXq)-;2wsyP?M>GPn?-`gjSK;3DGQk6 zQC15%M9lOqPU=fU{7~B~JiI)tnLo0yXG)Pn)?awgr{nX`w0?8SWXiU8Wp&rD{wrQ$ z7|takICE&(hELIEr7pVAt_#?JvroDF7s3k~_AgW>606!~zdZwH2nttEgvW6rmI+b{ z$cSDMu;dS+x`UuwgiU%Pw!$vXBG2%xq1{*wSR#k`^*>oJ{hEPcj}{_L$jj?PFh=&U z|7ja8Aj}Dih5HeN%wTxG3w-w>R6m()-*?DXS`CPP44zF%rBi!PulZoopa<5(k;AU& zL(2ONNnk1T)KRmK!SdUF#0znNDW0Nx-ZTZGyXxw$qU*oJA*fSB_8tb?jy9Y6i+sN8 zizC~(92s>}NK+P@L-vMj48{+fLrdE+h@N9DVUL8SIF39C^Vdhn2t8pN{1Vl?l~K>9 z3dRP4CVOj9D^K&jvE0LJzu&Vb*|D(~RPw;{zB|G%a==8QgK}C1MPOR@Q!bVHJWm&` z#ev%Nrkewrr>A@KLovMj9=ZQeUNQXv%yMlQb;sJ(fibiEcnf?WB7C?r9cRyK26Y%dE+m;3WPUz04)Odh;e)qtn_FtUd}lt7I5~N8x&BGk(z4(ELiiMk z5lE*^D)eOQOZ2|!Hlo8|jXBln9{PKU2c?MWsQssM7_N5eMKKu8O5sT-?#%rFWz`D@ zH=}Y?IU=t9@m*NXPb_Pa);b0}?;=lxfw0H~+^E5=ewp zV@vs%`*$%~D*k43{qJ^?6kPUBc~+6(mgcWCs{#F0SuRJ`9(QBlPDUWL5w&9n)Rqe) zmwU?zE1z(u%Px{YG>8oFg5+(Z<6FgF`iFh~^=2th@nBaba3vSvHEq zoaKoj8Pguz+_6&k#v|y z>+^hH+LpDQQC8L&Lhv%WUX#AB7#jK9AwN*n{EO)_%?jeNUK^>r&+qA!n2Qt-Y?VVi zGNNXa=syTDhLqPPK8BPZL8CwnLOk~l6%smBzxP!Bcl!5F7n9T{u4n^F9KN86TTJw_ z7G%`YVIg6zta@R-;W@R-3n3j z6YluxLdieiRjgaVg%K^~CHUgh)PSZ73aoXMVk5%aKV?u2xz8sS2PcjNetMl0TBYTdLRhs!&c?z+lJSnI%*7c|Zl4eqEv(Brf& z7HEeldZ)<6)hccyz1xyREJ4_rDC{Q6N8bpgCu`dkR#452wL$-qk+A$1imvAbSPGx+ zZQ6jfa8}gM8J&@5Kixo0OzQ;XBm3s`)IrtnlO2e&4D$eWmh)6Vi587-QA%vM=;u+Nr$+CnV&CxG@%{~VDoa9I!w!6leaY+@9#*A z#ofFs04J*WFggR>``j&-Uva%>S&I)NU{tG2gsNXLZAT=c@C^$zxDdnoX`Tk{J z(NW>)VWJ^u&iwr9C7ND#^_MNnQPxcc8@2VmnTzAP*YNeWq(<+%3_>pkB<=X!SvLs$ z%})z4|950ll7{AIqF`RXHXWhb_2b+}sI^wS?%;C)#ro%jm{e!WcHMhQi!w^aR}v1mVj$)B zbt}7NECxv|`EKkrb4r!-Zd-(#Ln6})J4R-a^#%)Pkzss;njym^rk3^5xBf}!$l0+y zgTa^&!4f&HCvfrf#dV6s%hj(SRCwN7;;WB!KK4D5CMk5`R$O&r?Q>S=CGNrlbd3j#ij?`8O%t6 zi6!*$8K24r6M30VOX5fQM?AAv zMH&BW-28F6nz0f(@351*@!6l6mCc98<49_F_A#y_ z%IMxP^4vJIA2f~i&M>G9v|`PgW-d9#~d zrc@)R_B9J}-a8Lc8RxFc;+V1JC0d?y5#Bp=_e;Nw68+^3L#T(_Ph2oTI>Bo&hX%(R zzfq9c4}bC*j5et<`5rmtX9F~CU$};+Il=FIhTG@}>tfN+m)8VcwYL1^u}+gdxhxM> z(%`Gx&y6q5jCr;#|KfIR80|ma7T4}g$wYhUT-G7W=nlqUG-)rdwmVbp55wXrf6jOp zr0NcPIQPTk9Iib}ZC86r^O5O40U<0```Sk*RSZX-3?5!ZeZE(cEO&swxb=)Gz5d9h zfUwsT-fvHmn;41g~D+qSz)B# zR=*&U3d#-cO?O)|94$6)i=X8eNcQNDwx?R5s89{dS%J!GB%8-kyM`udAA7DPfoKdM z@6roFO|!h2dpeGOjQH5{&$Rw zo;o}`o=fakHB*Re2>zt9M9)w0WG_mlIKH$E|Ey>;ICe^d{>ci_!;Kg(CTpXqU({8+ zcZF+}OEvG1!^5IhjD_beP);lvRF#Q(8AniF=PLKJdMrpW)2Y+i*uLJZB#~lT&T{e# zE5N%&koRHo-JIQaAhbAMRTR&#cqkvP_$cBX?d8ZEb$r*j8^$vfE3Di0dU`h`Filw! z!B#Z2!0WWe8*VYg_4tx*KXiTVgUA***Bz~dAKZH5{Vx{)<-MT@ou2@s&YOb(?~!Xs z(PGtxL$NFjegPTPr{x)c3a2SmE?Fs>#_SXH_&EBw!q_sA>p$fOG z?WsovySH5vEMic+nf!eRzPj?eca%qy!h=>_KzI>F4^vCbFR**PQ;|vvn?%NPY&A)Z zV5Tr`=}HDVXi%xF&{Y{Vh54pHoGE>vc6kM8h9i|>0LR?|<$?_gI**BNO^K&i9?c96 z6x)05cH@1R&*6rk1_BuwUEr;pSz4i6@pt!g-gSMgC4qYu5t+La(j00F6H{iNkZ=1B zUp7Y-j?i*vqn_3NUg}}SFh56`$?&Xkb)xsdZ73aGQwZPx{U32?*lOcWZLvm^|H|+%h#6PRFxNQ8WV)~iazKtj+o7^ppxlvwmDwH%!t;NiYo%2d%2_?WTZi^SJlvX z2iOu2=M_N?%&DZg5LnsBqbNM>>J$OCMQEg<#z=O}{45Qn1O=+lb6Zt+PCjO?1_MXj z*y4l1STT^`*z3rp-gs+Ow~zrGrQE+dfiOWNG(97u6BD*Fbn1vMacb630kW;WCMrZw z|LK#vo0}W-;290W$kp*21#sTV&FNC2qlTxxqLthjdg|XY@ULebw8#0+-PupCG|tNm zFOAjuO~}?8kIfop>C~nspH!E3kI^Q!G9F;5= z`-Rx| z*f!8iI)AgmZgER)efU7f6-}~xWZwx{C|1SI)`&)JHd)H2<>VO>-#9~}Bz4V84j~tqMZ>qxvf?ryL$^)^6GjO^jR&(W>EpoL`g79?Z)q##J zE+PS>VbaH0F`5hpqaj`wlS5bGdFIB?x@>&qmq|9oXbUX|yV8`M%nnd#1ZC3%?Vqgv z;(fb0%F-KxJ2~YZPyVPdl^kGDb46pesMZc0(P;Oq`P9~A>tAQjkELJvvaSZOrii$D z6{3ljpsb;yM3nE!nU|nhot@I!$61^HYNB~1qY02C(}#_Q!2~QWDUilRCHR>R&;Z<{ zY~Yyk3)|zDOBqxU5 zo8U+n6k@DHSE{{fMEo4V`CwNQrmXQpa@2@}``52)X*ogd?*9P#Kn1_YTRCysY3L5p5E?mw)|W;BD#aez1x2U%jN?3ihuR2Xo~& z^PKVV;oT3RG8{hra~wKlf9`qeJpS_Nf&61bb5xP%O0nh#H%!hc@!T$@?W#HX=1UG-Q1wl}0 zQT3|K%KkpIvLYG!w6bAHWgK^b4lxop-6G2`{Qz;PM5qGVEr_WWXU{saY+VfuE4$D5 zxvOhk?6t6hXpM;=%){a%$McF;e2@!Yb2*)(h8X4*o+AypfCEl_G2eXE=Q+0HBmx78 zC|BDAPzLCO|9SiWa`J?ekt9F}5o=JU#Hf55+vsBY=vgfL!wMFhdI)sl_?MKb8sHM4 z_-HTr=`A41(DNfznMkVb-i6xjB^^K>!awj2v?e>c9h$)>hQ4 zS%e22L@;B<0MD=|Hd~FK<6T!Q<Yt?P;rHiF9?v>S0~=LGnizxxTaH+6FyETl z$15PO6)K4dOhQ*;Xzwc$nuNf(`7A0#2OX>$w~b9Ln$=^%OrVpi{nek@rPje^GY+m2 zj(Rap6(rlx!IhFpD?_2Jg-vk}rO?v${7F1H&QTsFgG5PW*?zOSo`@5ZGq&yT9b7wA zP^5-Xtu;xK_y+Ot@bDOh7~bo*g+d5@MVK*-3op5tWYt!_|LtFK#Gxl~*YbNgMJ$rrjS$8}KTB^TVrai>gS z*_~6k`|lk*wlZYZU7EIumWgu{4mhlrIR|cI{-IAWexj?@^28&@4S28nuMGIz3l{M5 zi*BhozTeb``PjFf;jVw5$KP-4;=#LeYW!}em}44y1C}~)KTB#8^_u`>DEpjj{+98 z^zi(#&v4?j+nG7~tNpIGweK|E^S(}SrPuB7|m`IXPQ4>>fg$7Z*CNCeZT6I^&9 zwyzI!@x>5Dm<=0H9rJLg_M`nOJ|%s7(c zPX1@j@i4cQqo02#M?b&0>-6=G=IftYz^ZE%j5qsazl`trE#o`j=vmu$-ukUC;qrTq z+-1-8j@|>A9{iY-+FxCBJg4vB`7`d}`AyPA_D09?mH+!sZeMyOM`K(ha9wp1spJfR zG;m}J2BiWF2C=T@QW%7k;u0bXL^$|6A8P7(?zs3pXK;CSKAv(0wvqP!8O3cCkhgsmRGQ_IS*E+nfFr7yl>QQ(rDEZ<7%Et>>XS? zZ*BEju7`(*hsR!^F;y%t28`jr(~jl7>u;k=je{pjJk`B{N1lCz6V5#r6^ggf?1P)r ztWs%ZFr{$E4H=CU-}LTc>jtE3_dArMmc4Ps!(-Ru#t#)(_w-S`{o^YcGw#Va|t5bpE12<^F+SzKMPs%l@RY`sKwLWui#^UJDc^S>1Ij9TY{bIve{8#+r}y!1GM4d}*xjX`BFEOry?(W}JobpLQ!{!M>^B82yX-AI z{us=eGxTVWt#kGmz~o~#7B(@ukVo5RK!F&;+9#gjm9Kp{W6T65=cY0`8p{KJdx)@Q zG`iQa|50;kQLRkP6*+v&^9Uxm3kG3YV=E17|J66H;`lMgv-Hlp>3Luqtp~OZ{BCJr zbh##$#X6&1QbLQO``|;HY9p6*;~J|%*;yXPY~ z>V4g!xCaM@-L{S7*=I>sts;5&Vd5oAFgM&l=dxu;u#-&OGWhXhXs##-0=~p$MDx8BHh1WT6fk|M6KdY!FO zhEPw$Zr4y7Iwy)+x&5yP^X>OfVB_tweT&v!j%%Kp zL}b3o(NiB}?u5Tm2=Csm^SAaL&&?0LnD0M0hh&EfGL>tNG}o?g*t(e4^*k zmj7+850iuW>r-#!e;+!G$9siV)k9B9!lu!hh=fF(p+_6Rh;^AOsUanyzcX};GOWo* z=)iK~mp;+dGu;2JkHD+y-&|phd+#zujy?bpA`|GV^jcU)^G%-N5ZUiNBNI!*!I z@N97{eXIHi$oH=L) zmvQWf2$lc^r6S#n4r{7yx|ZJK_?xV$6ey|j*~-9rpSb35&Fc3BBMMpjdX@jnq5H6f z&S7fy+@8^vN|+5BNY<<&dHiwWd+s6m%U@vq`l^gsv4Y&9ot#;f&XdnwxQMs?U}^m} z4Qq|d7#^V$#@oMlY)D0sAqZ8qc4&6ZmHy*z`vhZEKe6K6O*)&VEA_V!5)>MXvUcFw zp0-xLmGz^6SrP%G1r3BrbMM|S zYF@Ul@%%9M@Yrz_9}&Lw^3nY0fW^Gv4LvM8dNb2!b<@_~Lrd#cf}oeE)JAV_JI}7| z;Hk&kSo+twJayYl?z#1D3Q_Yj@b=H;x&3oQC-_;Y288?^wHNh zik@w)tX8WaTt&eCQgs zh5?)A&gRk4od}kE)=k}RDyz(wRA67*wpKHtz3>p zyS=)l>Y&I33N7V*i2IIzwEwsu}neu%rhiUJw^Plf05jB3wFhd#yZR| zf4OrUT=fBV4Fc}o;b{szo&&9aaBwydp=HF>VtT;)id`iR;7lN}uKZAI^~GJr^s zTH{3d>bC6MlB_*yXgh*CR~)@yn)fAXBS<|JO+!P3)MRprcn6n_?yA z;o-3t>_^Ib5Htx=t!2>#FMQLv+<*6jocHSUx%>88IP8_jQ(6^~n;JS&X0H+KaST>B ztL!)$j8sw~1O_7IltpcwqG-}`vt=F88DTU;wi2OkQ%%~Y^aOT*9bdL03J6xjab;(s zx}3slDQV~!KbpQZYsguP5*gBBTK};boaLbQbf59%;jvTD`+#uGJKDI0He8i}{xHgr zh+*Q)7RFEC@p~=#LY^gG7!Qp14UY?uzioXdx2^944jj}tAXb>XNz?xDMp{dnmSS3z zyor?A?t&tp*R+il)&voP#TtXCs*`M}WH6;PCN6Td9?hL|E~TidEXgKmU>MQWKZ{s; zl9=^jo~OsuWkzL;I~ItOG`m1RX}IR6%{jI1eB%eu#$|;S3~6+*25U)TQ<1oupc2Ga zsr`_#)JW#fa2ut1>;G}ovfq0;XAft#RllBB_8ux9mYCPkAW%shxm++bJm#4V38r(-s% zS78!Et|!NwU?$i7_)om}LuXayyp+d$lF+@dNvGEG6`24vskTGCb7CMTpA6EF-myEv z=a}qe9v)4|^wgPUtwn2%Qp#s>d3fxtJAO-Q zph_)CaSUNVZoy1~haY719XAphfg=3)Yd_?oH~bqi*E_2B6;)XvrCO?Hk&C#DEy)<@ zl2VMIG92AQo9imhEnAMP>d(=wG7<1bgh&en8XG4ds!U)wkh^DXo-Z* z9?ht}kd9uBP3stxBX8v1`sDi<43>HvYC|PIRixm`ussm^! z#L5z3=>aws%2zZ9%E=>J8!UX&51VspEqT)i=qNg4NvBnH8PScc`TdUC2C~(b4KKA4 z(PPi5yxG5uF$AGz=1fo$@Wi7qWd?|rx}Hw0bE>S>q*d(|(4HfI)FS5O5|;h)Di*x* zl`f@FR3(Z`tOAp`pq#rFVgKbpcL=(>ApB_L9EvMZ+_1>xOy zliYAa4FzFCw6>Ca%Uj5waRz$Agk8DC;&T@*;%!&d(G-fpT8kAIL@{EP;JsmE#+V{(nR`;Rom=srS=aQNWRNZT7aV_bOk!RgUiFiV>_UfA_xMC#bR}X z8pi~IUrIbY_L|*(y0@xq-Y6qYJ>(!Lv|-aKCfaB)hR{kic)XHzm@0EIEc?q!W{=&UmbgHxEq%TdpJ9@5sfQ|CoRW~F}aEZ&*Y6dI8cIe*Fl?_n5XjPa9ZSSgU@T8lA8LdTdQ zwV77~sdKV3CYK%Z@rJ%r|0)aK!R6uMv3*cV5k*nOvE`jx9v*w?T&+lY@Tt$$O<1lEbBK=9v&VZNoKKR4Cg|@_HCELW1S9z>S{2c}=;_0n%~)_I?0>g?2aOSZtt09Qz+X2@#tay_x8B z$C|0Zd*!usUHoAV`tAoBK1J`pQaRqeV1XN#&voYRAO)e!nS<@_hV|=_{r1B?{WSK@J1O0H zCjiCy^J#g{dk79Z5Vf-$T|*!Ok|ZIgM^-`W>{=rt-oZ88Cb)$>FsfLh7=#dpmA!b` z7+_`a`jyZ9A$HQ~8Bd(IOnZ2EY?lDb<#I$(RO8qxQy6-9ccPE5_tILQn z*u;QMz{D73=#6@5ZE2%h5=5m1Y?U;W8DcFqsTw4e1u;(14ub&tdMWsk;^E=pQ4txv zIfb6u#pXx0Fj|Kdiok4!*WL+>SWAc_RKB6fOsyBZb`d6hM+3wWNCcuRiH%Wc6kt7lA6<>}*Yaw~C&e>L4Pi8!QY!S&lmA zd>;A5FVV53_3#B?I$hSIay3G2M%k_y1LK(c{(K)Z3)7g~GLepA8`|hJ1I*G^XhrpE zrjMG%>J1SG)xBBQ@?}nOw^TE>tZguv#Sd_gT{gXxrOE=*Z|-gXz^hOYu3qgZ2$wEx zih@vP%_4mH%h7Y^LJ%PNJbJY}i0_(@mILZ-t&7?8=q&zW2R^uX`OW7hHf8 zcK)uRo~H2KS1jcd7cIh3>@t8?U<3>qYrRcy7)j4Wpba>0!Z?1paSLnnHr1-C&aJXs z8&2?MEDsM44-b!h1=4u`VFgeip#h0G@U&xi>UTFHDuM{4$MEML{E5>qJi}`Sdx5#e z`Ln7UBm=~NQHtI{&d&o=d!Vb>L)+GFdUFA#LXNFTjwIKLw$QyTB1jxfk2Mx$T>*Bh zTy|KwrbIoLdbaL(TUa0|_VUxuf1SsR8yPct481+QC{b8z&;rUDih&`?3+KP{_4JO* zlOzeo+J0uK$~~;DiMSWzC{naiU@Rs{AnNl2#lvH75;=RwgCfIlnAl`BbZy|pO{!riC?!V6x2q?swZUpkKg(H~7} zH|PZNN#e>2P)lDs`h+FSi!rIN7 z+4Q`PbS@n2+i)-H!Z?YEbifl^*Re(A=+P|=3i-O)DRVc(?6>kMe*Km=F#nlH+1lRD zwvGk1`H|4LX=-nal-YfLGjT|$Z*i_8jJ z(9cF-LZVWMFbFC%Ks`BOTVL+|ppF0lAOJ~3K~zFP6)5H-qBzFb!AZBX@2)MxX^jgp zE;6vi4-XHIy=I487As=+j5%jSn^Gh-&Zl0+avvxWmx;pOYac)Mhs|Yco%otXgsI~$ z0Z^BuuApO=bmCmDD$7rViLk+sX$G-a=zViQG$y+X8L|acU%H}ZXx#g2J0ugi{v5@KQ zN0JMQ&{kqoZVMp=WV6s)=%cr-NYA!DqP!v48uGJOT*f5{Bsz7F8AtmpE zmd^R3opz%v)rtjn0F}sNmnod7ViM?p2anyWIxV)Y%0vW?TB1_nOB!y^ED$9zf{@sVAcBGkNYZ2N zgh-s8ml|Ud45i)@ea2$7#-iN48gaF&+IwjO2?9xiR*GC0KwR{L#ADz!&R9hGHEMUQ zRo3ok1*V<1QI6^Xd>-g48z0NA z3Sr<};qr4TEDA>}Rh4)aJ5-sb9q6-NZSa(uxAiNa^StX2&N}Bs{PuTCxb)47!HQe6 zRBeCBKpmM_=ioxadd?i_7ZS%IpIZ{L?5yL`VfumtU{9@i@_c^OYH`5Wh7{9+#IEi(J) zncR2%eH{3L10k1naFvN%lpi3wYqOd8Ljf%)lu1pEHmN&Pg9T#^YwlZt&KnZxA(kA* zD(H=%B|s$sIpb>1cj_TATX1CjP z8alKr!^bdO&Y-RHqzqOEv3{Iqoj6w9dTUUbY!7LW@tfY>TGxLTtQC|3D>25Vj-Ql* zDj|kIA#seh2C)jPpp+spiMwy=I8ai~wsH}L6b}u%DSdBCXSGGaxJXUEdXkf}Tp)V@(DpFt zn8p_J1(-93OF#8-e)Q2Ru~_nG)JIRJ`=kAz?zNBo1-|nay1xTm*RNzv?=vu7D47U4 zfN@YMKv#bBxcp`Y2^BI_XL?cRu@mKI|If?mgNCkqm(qRkqUsl-1aj$Gx?uy!+O;IB zwvU2PX3fGr`ly@1C!IvgWtR~iaKMhgQ=vfqxZ}tYnJ2&c70FxPf*wD9m$#{*R-AiP z)MKyK-_v=%E!8K>nc(H}k4XP9Y^yG+%HQQukb+J1uR14~RP|b?qdUD|*UcI0Mo4Z891i;;(ihsTIT9LM{#fz$#~$NDi$7k|d#ArNW9}t<*flKt}Dd zx@yFxnFh|OWt%YaDO-JSD^6jp5Q2Mn3^iZ8RbjWSOC~WSalGd;xXRK=svB}UoEos^ zX>G`dsl*mzCZnm__n`cZoYYF{r-$I$X{>)!JIkt{^&=zb-g$(x zXFqqb+SW#J$tA?!_(pYabk|+z7rt<(wvM7dO`$a|E<}7VsK*G9y46*}X8s>ohmwAp zQ?14#+1l|a@1+#Lg^%I&x=VXZgFczAet3>O4JfVFm?GLqT>_Q}!` zGWYb;`R*q#=i*n+=WAcQf+>^c@x(0;GW|t!EAj_dak>kqVC|3lgqM0rGL|#~%wozOG_Ol}G=Xf5H?M#1 zTDWLW;n{WTn7d%!?!SL#^)aarsJ>Lq-Yu$3Mxd$+wVyQUr4#EMTS}#MgtV{~Lo}|7 zCn*6fX%p&nb^3%wZ2ar9Y>+;B9(#hPUN)V7%#KK+h(IZ4B$4z7TX9~x=aPz?NJKDN z5tLkD#+(nmjfuShMua4d2p%j0tEta!GmVZTlRvt~eF zUoWekhH29v2uw;*S=BwV3r5*&X0Rx05v}N+KfkH_buC}%^p!HJTMTqSQAcPSU<2zc zaOI@41SrC9zV=&2$6dT^%*)6h+|t-e=XGz7-MnvfDmKbIHB-wT^ml z(j*X}=chj<{@(Wh2;cs8@+Y2{7FT?(V$~2HbyU^CmCqC3cOUFz2bXCe7vA~9rF{0H zMSk#jjF>gTR@__CcvC;b*)G#wcB*(Au@0`vQe7_0+poc8Xr00h1?ODE{r9co(UlJ{{j|9*tU63L;(Its z*(23O1}ZdTA=a=hXkm0$C#^4jJ*^TtuTs(r(H4I5)=OBdHjuM%N9|`- zqg`t)lyd_01p%Q+2uDTqsXU_&o{o&l*VHF#)IaUT9Qn=g^i2;iW5XIk8zb5}!r7E& z#)u*`0YRiNfgw>z+Qyb*7*Px&dSBF1p;j9Vi|(JpCSxhx(Zc~7TG-f@qq{xk2?J|# zAu1P^%YQkYMQZyv+X`YGji3c=2r7ZcN4K)MRgs9xQkNlWXieA_GqIr9l23zOCBWF) zI+%7CsXSgg%c?^eJ;l0mLB}5Re17-4WxVN43kb9tP@@$CLgJiMHlcgLVNKn)>yEo1 z7gnDlWpQ@O6ZDpP$(tOips6GarVg$rZhV}Hb|Pcru`IcE3CACFVzqvThsPe9M;YOY z(6wS2-G@}`S#=+JFkQ=*lRx!TN}u{N!uIt+R~K}5L$Qby3Ye{1QSI$Jt$mb$wWgt7;@4sikL}RqcXA-nh;(pe zH$LUsA||mYEop$TZ5;7c=0z~JL3m*G@AkHKdU|?j&n%*zF2%#c!((_L2=-0Um~{*o zjElJmK$NBBh#73|d4!46^0bVa#i(-*foF;^Enn5xE8(WT^7ZaU#ndUb;fAW3(=TY7 zXKO69OHl2y;#Qu%;Tra9QH0$aX_ro-Vo07Ci$Uvv*ajG*P=rL3uu(6OEm9yvuo$aQ zXbcvu0*tYhF=aFEENvxa>~}{L0fFRD)1PWzA4>I&8ZVs6c=a{gim17_4z$%NuRMugO5ax17WT0d) zIic6~5rlbu`~Pi9%?N|*OBeoSUaB)F5>XgY6k?$e2|a~AN3EEEPRksXUs|Mp(ylFk zmidk-_r0LYz5jkkL3rLdgbNp;Iz}~Kd!Km*bK7n1kJeU#88de3u4@8v)L{e4h` zpM3r&y!=03NpL_*rOtNRFlms-$SP;Pr(n`3UJIzikTXexZ%JkIS<;rRIU!DKYAF)h zTNTCj9GgPnSuvEf#@G-;j7AeD307PdSA=mcEnA|MJ_>9?(@7rzMZlsk#xyAB?&!*@ z8)Az#LYoO`-;^*}YiGPw)eW*5V1AWry9lPrWpAO^=IIFs_HczJN!!O;B}eYMZEE!I zK|QMb7)IhsoY;gGt7z{rL^;joN-ELb-x`T1N=g%jIo7unoP&$1gR5Lqtm5r&uudje za%dz>*aUQkj-D@3g0hZYZ6LH1H61}fOB6D-I|mgCS;uT*sAk2X6kmJi#`Mg-LEr1* zzwA$IPmVAVLaVVNm3mp&>iQlS1&wlY2ViIvTU)lQ+$z?Vn@LE-qO4R5i-XnRvgtXt z-%*#L=B3E+U;kW;kEw*<{P$ZgDP*9_;IL#j$q99*xEekHzIgD8EJa7(p_4Zy4?)l%mj97GALs zi&NEC7h^WqxT-~G7q4`KphVa|-@>?|I@s1K;Ef8P_&T8;dsJER@bDPHN{CZKG@7^s zVJ-)=Cv)!WUWM)HgWH~^bK@B9xp_H9%v|h(_EhTSl5$#*cW~_vl>I>omNunHN(~%b zRs(_+*j(r((3)H!bdE@+=ryf$op~yc-0~}W?FLM#jX*|G+U6>dD!|x;7%*sZlxQVl z0$Z8DX4cvw-E<+evbiEoOzP6giWtGUm)!U$#E{#TaP@~Tr?=HnUda$C73Hi8_cfOb z))zuC6wZ71>v?wUzz(j~Vjrwola5MTbIl#mx4ePVW7utPM&P*ZEDBeBxP#42WD2quQSg!t|qe&#K zt@ZT}tZ({#-u#=npugxLjXJH4i6m#Dc=qFs4W*cE&a;G@cEIPv6~^RS{EO z_1%w?Gj`AvELStqxEhg_8W6UUmpl^Z(s0vc>!o#OXgD3f8bk>(j%WG2^QZHI(_X>7 zD}PRla+9EY!NR8Q)^*QvcZYH&x&>6u-Ly5Nb^amDOA_97;e~wS!yiE{UJOB@<}u4b zb2+ayRb6?1)!=Izk-!^O}uey6|$J8pbc1W4a zwdSkc3m4I~as}b+In5cIk_6S-TK(pR4M?G|(;huuwTk4{TWgMmC!8<z`WB*coFp?1ZL4-AX^7cNI)xF4FNhe^4OXzNoy^l1Y%-p)!UD$t;Sovcx2mf|&2UcT-bu@TSjf zz$RnK1^Y2-Y8!pIlma<*tl8Q^F11DxYilU5oP(`?Ob@&+%}SjeZYxrY0G`3oirj6V4U7e$>wXiW9#jfDj6y8ccM-~R|&O&d9K2tu1}XHLf) zuK&#qoWJN?>X(Cl4PghE;8whW$zvzYx*-$_WBYG#2l&1<22(0kPyO?s;eY-Ic0zqD zvuP83Kl>T>@yDxAe$YYaNt1R_J4wIFH309tVkw`yXwi_2>K-1$9W}L4>(`G##g0^E zRF)IkZm%s#`aQ+4a&XyxORlZ_NX-(WhG>{_M3;+3*9bXaN_#8YqF&-S#-d4-i)<5X zG1j21C8uqf90X9faGE7IL?3LIk2^h zXtg3dd^&eMct1g?>73jNI!V_EMW!;8$1dDgDkYWH9+i%gfv^SG+~!J3Bq*!t_ldw{08y=f{HSfoC7&=F2W-YR_7p9Xp%nedb&4Zh6hL;(F$8UB$Z6Ms9!G zxjgS{zs9z9R_rIg{oPf(`0SSuP^iS_o78p#%0)RVgEfLAAfi*ds!8=C=j5u4O(Q5J z*s_x=wfkyOkQkJxMub+?*(ne!$2#PkTvd5y`x%2|yXH=_^3B6zPwb2Og&B;9!Rj31 z3GOHB7?EM&&6bK*P{|*%WE@ZxCL>6hu`F5+kwWA;0VD1SKXHjSrqCX_U2t58QqrnQ z7H}p5YXF0XsiP{acbM5Grh&Cof-00q+BT-*IP^}gy<>f=xAN4EBB}CJR^}9GeRY=t zuMKn`Iv;?uhq!OoiWTk}S$obP_8BQ#(?SJ057u(vYhF!!$DMrfu*F>ViO;}M$I=o< z7%Wk#5e-n zyy!*G;gK!L>eclAIQ{b|id;n`>J^cr6^d$a)pNlf~$P370VSn?o_A z6lzCWS_alT!27g3_R<~20OPpKfxn~F9gTd$)@Oit3@0d|h+$Rl79yiCDu)pv5h}&m z2O7tf;_YIvV(2VJ%pKLWTir#{Z&If*uM$95f}`f~{ujQU7an>B#~<}?Oe;*`zJJ`$ z(PtkGIzWj07}x{ra(RpdGVp$Mvx43LBJCVKWj#ix5&cFKap!2dI&+kYd2-!Pa_f8l zi)nppm}AyqHdy}pp0{wq2S3H8TkoV$+D2Qlk-j8mvnm1Gkht50&c`v@SVpmtd^nNK zk#R=MRA;Kt3|6S4$~vqQBGN#Jh^i_^HRIG~455QHlR%O_gHGuXRVrcn7t5CI5P%*Y zd-pa|Wq#}k8O_ydNp8>PHpF&N8NAo^oRvzri5)Z|vcH~OpSdTMfX^W^m@57NLtFRb ztUedMbnY{QKIx0T5-_dr876LdlHTHK#;J$dCOLZ7-9tEOHi6DjioxWn)QQyP)%5Qb zsR>xHVlx%MZ1ZElx~u@KsZiiHb9SVJ-as&|d3tn-&{%Snr}Y_Jd&?ZRLrLkfB}~AY z$QjKNP@#r6rh7s2wY3JEcFhb0l~xT^8p43!&_lWG!yn-Y_(Q_Qi_v4o?9fvt8#d5)-F3v*TvPj$GiH!G z{&-g>Y!|muJxyWRz9lt*dHYr-W!AX`V2saM8$qi=jUJv2C>1g?xlU-;+R3C=iM4je zL|9$4Uoc!7Gke7StLl_!8TvXE&YJ3e!a25_p|a9p)-*?#@8yt% zhtoDIgdiBe?BM6^-Z4Z?ZFkft7D7}kK`|m1xs1V(YhoE|o+jyj zl$`0q8jT71*j#*)F{;O9U!`ljWM0RXqxo&4=SW?%u2hmEKUQhbn`HLsgS5#|RGfo- z))xRRit{eLm}T$(5@W1}o`@_%jMr=S@EVsfh*fMgm7tY6`WyqIa!@SNeb`}5UB7F2 z>eSjv_HU&!p=xbyaPo<~|K!uy@Xvd={+4SwW5&4zBxs}g!pFbB{QVB5Rq}+A8s$Wz ztR)`{v&YQjrk~%$ndiQQ_W2#Ptc&$>pl-}dO$wvb*vVY8B@E4Y;a|RzZEoTT!!(0SnplMGkPfc8jYLf&+M4BC*{lj+`zh!R>5nrLyAqMy-~<`G zFFibV&(0yU|BQfWkWtth@kcakU^6Gk(w~7lcTe$~MzhOUv(OH9RA;l!BH~Fa-`cayGN~TflF=`&$+t zbu1g#bkjC{e40)!&YD(H*!@O%?^@WiU7ewj6-|&233LE`0zn&5+jv&@ZAA*ghHxBP zbO-1TP;G2zpTr}23y(`DCx7yrpkn^;w*O$_mTo5ZY+-%Ajgv?J|Lnba{9ILi|NnfS zbMMT2=CfuGWFdsGg#^MPBDk=qxZo~o)hge%KdDvJ3f5Lzgt{xX)}`%Nt3~TxwN|Yn zEeI~yA0l8_R5rsFLK4C!$!D3Fd(U})|2X%~^4T)kNCNkr$2{_xJ2Q7Z?{nVk>otcT zJ^BdSe9S<+5>o&GAOJ~3K~yX(CB_+3OFb!*(1W8IproX&uV4zu0?FXUn8E!E@lM_G zwF?;=m^OziSu&uTm8yd{cUK_63n^ru`$tc-X-_);)B_~BHI+fGJGT$f$$^8ZK70Dw z?vh&P{#}K6qy3-}qLxTgVxMBLq*5kjkc5M2=zPk(hv20OqRm2rcAp~esG90oSZ^q$ zma>C#3W+tO^4L{?rv~a}%UBGJo`~8=N_E1y7T(Fe*AW8~xNLHkt*qiZ1yW)7#ACYJ z=g=*;GoHf#{ml1x$wM%IE|dvDT9MtGP`&pUV*)&4Y@uYKrp!L+cwY0?L2mnxwX8T{ zC4cgkH?V5)8oYNjv_V?(h)anyMkB%ZKpaObnX{PhfBXBqa>Xl=l9_S>Y1%8EfZOSP zco`dP`utlmakeR?OvSrgR^`oPt>(-I3T}yi7jkurxd{S~t4nLgo8{fElu!no#j!iC;XoC)Y z9H^eE;+!LjBFg15-g}&LSZj+U*Apq!Fb&7`bluleGCg$xJt5PcNKT&CiiD`R-ok- zXOew=$~mBz7Yf;%tMG&&?0^*I&TU&6E>))esa1HeR%fJECl$pDIMrs=rpnA2d4mB} zN*>Cp9M;=If1ATnNFn?8dLH;89FQ7A-Fcje+p0{3nE-NB0|ag>@MKznsQtiDMNNR@ zHFcIh^GH7Yf8NSFKmH{c+YYPd@!MM2w{= zNDQ{0Q{MlN)P8v#cIzghcz|QidLhI;;NV&R@_iWJ%<^(?sD9YKj(OWRGq~+x=K3*| zLCXCLS;(WrR5@j6fQ9uD9<2`YzmKfrUS_eK!!V*4kziw~A`-YwCoN4Ps7?-LSK6oaUc{l}Ec;=5v@ zfs&{v3;XWKPu$~j!nvEd_`j~`*!HoD&*GZ*mWv0q7oWeh`IUkHP@A6_5uVXyr`D@K zRwZ^8n+l~gCiXVWfj-5HCyfkS8+}B5D;XWzM(Qo8IfB`YooC^JQOYayA)L^6>;B}iWu(Y>*?fJP^fm9t{pynMat-ge=sg5!^|3jl3HBUII zZQCz+h(}mZ4Ak9lRehM!LV~NUIN0PJelB77xHVnvZ)okUXvPtwbvWh8PvcwPx{0^F z`S>RP+N1%plfy3W;4(JHYfx-Sz(U_Mm-FPK=5gasf5N9e`+ol6eV?N;uZQhplLMQD0+hc!ckL_dA^Ns%NlZ`N2(FMA{>Gi9<5jXi^9Dak%SFet7kd zIrU}dGPraZluSo-Q87iRsgT`N&}l}`1iH8Br1aFMlD^;txc~T%uwiVB^k4s#(W|c} zdcg~bPC5xQI0%gfGBkwWxRKe$}feopQ=!K3`pS-Wo1y zgDHIH=i&)L`|-%K zy)->ZWaMGgT1MlTMxNjjFkUl`mzfp!3=K zf|G6mTFsr$78TN#tJHApNmRJayhaDP+Wy5rgHi@P#b4%+#9wb5E*DVC>@NdAY}vN zo6%AqX|#=+OK8MZhFwGj$1@hpW6{`p7S-3YrR*rpn#~oDdLAXk5Mfj^9TBuy2-G?L zbc)p@1hWJqAs{+Xo8EISrG_L8*C!}O6tOMHl#m>oj=v`SpQZ^G!5W(diCgC}6)II`BLWn2XmvGOK82j|dQpCGx_lYd3cHJ3F=|?U*%Gh=y$GoXiH0C5Gq@H`AE(t`L z@5kS8LrZV&y_e)upK7u_%bGRl%{TA(WaiH&dfxLWJ@0vxR;}7+Hpx^Qoq{zso4_dI zzJ=^JHuRnxh6aNX--Hl0ZIl5mKRX;T_DbZHM1{nJ$biA4n2^hv0_i2d!DzGSHSD-8 z!6k<1Ku&O}Do#SL)fgc#pI_dNT!p+!*H4&VBA_}J1-P`d^9N!L&V%!os23)py!tjn zd3zKS*^nfgYO*sxgOolZj6jJgk!%YoT7nCW}D3WK1K{Xko!3CR~mt!8bwt-#t$d=a_J8-6SJCb*yM592$x zcH$DK#hi8uWB>3CNc*|=^WS9XpaHJ<;*~t-|9dt##gS%}+l68tNaI%|Bg66e!A8uYZS59Ur=&J!i>*N3AtpgTeMjmnqrz>6 zlFmMi)IkKLC`KIkdYx)1#`w^^?AZZkcp-&6*1;6-Nw6dp3k{1OePYeW?OjYc9Nz(T z{>{a|b@Ef$15-ia=d+s6U|7$&eoI%!{KZjo5NlX8Qf0x`2+C!=7>pRa&pEJ^R5jzo zvWT~+R2feUz477H{bIcKpwfeC-1KCHywlw_xf!Nwj)_yHI;vDg9ldoJtAO#+1r+U` zA4A5s{Q36qY?0jR!hw|C_S+U6R(H=~5(^S?X3Yqnfs)~@v!BV@A6)^#tyJAnkNA z+6Kj@M~8`xW!PAh0L#;EMOz> zS_T&?-5+-{2f;Ku&>de}_lMq53cQdfw*KS`s7$H`mo^?DlXWa%o26)y#|0B3@NGkl(xv%C(`e>AU*)~2(m6%3lh)qd}5gLft%(zvI*d|Q06^~+D z+;iD?{Dcw*qk`4YJ^Z=%{XHX5+6o9ykdbno^Z)8ijFvoOC5M+RK$~?NcWU6|n;B_} zua99(gs(NwZHj81m~Jo0*p;=Vn}=8!f`#myl;W6%g)9F5D|qRu=XQ3|Jm!YyzO#9E zC1Cr9huzD|j=QL1+k3D6Jii+5f6Pu?@}e2(oL$-rW>luOog)Y5Uez50;SER4BaIEU zER&xe%bL!FOYayf5);WHU5uVd49zQuL_^l9cNb%sVqS>C8K3)oz1Fm7T-$`Oy& z#`&MGeT8Qpb1|r*HfkI3h237rUhqlkHMFNh%lnw&$JSCl=_KrBmr=X?a?&q-Y3Gh# zE>n8@+bNxK2GQch`^StpVeWY6$VDdVwHvLzmlJNwur!(8^x(Wv&} z6h=!W8n`A7dT!^GoF_IGPw3w2=oT+JR0`P>F@`Au-P1Z0dEsbo&rji?wEwn+sT%6y zg^a6&&fiZP=+}0DDBp2Tmdv^BMujJC>Kfv(8%MNI!Y8u+7_lU&r<^abzH?}p{N>}U zidtGB`Dr>BTRaq!RW%Eu0jjNIMD7ZywPq;-%`9sxyFPcI7$e|ZxX4}uhYj$g7oNcU zW0r8-U3C_pb3FImavw_hBAg@vceB7EqbS-gz7rl+`2HR)MdNjQ2!DReT+~PgsqEVl{jjZ)Qje5V&pd_)?_Ec_ z-LpVTt!LZxgFQ%!sGpt(Q3I=wg@EuNotS^E`dXhA!dVQ0x=_TBb9YVcJV@u%sGaw) zd8c&Zn45X@YsTZ`CA``TWVNjnNq6W^bb%eC)6M_7A5NrQ*djW?pK3zQx1~;W>RspF zUp|Vw_gIq$)Ep!!!^fZ4)#(oXdTq-n&%{bAaF-v`wmeM83X`o77A=B-!CvmV8xB1* z*#9zd^4y)Gv+TD%XT1`@#2oXqv-t7P)^f(8X8~}_y>R)@##npfjXa8HyAf2Lj+X(- zeiJ1g=E%h-^56rTc=>Us^6WY5IXoHTC|6@Z9X&}M>m1fYe?|o`;5~zBjn91egFN$+ zcY`s_)Ug!)ZXvrfP2@!n3PZoXjp5^t=@|GHpV7f|5pOYu=-`8?zWL3hFMJ`%b=TqV zyc74MA3h%{dvV?>Ji zWq)>>l7@$}p+79dSR5)bKI6=`>ZjG)41l*IePP;tgEkeHDV7>1%BTl5+QQWn38-L5 z22?n)A3Y80yyIvG~mJpE=2;S#UnwI-40gD3sHPY9pe* zVO!YAc?PqrcnO)zhMkb$l6>NdXqbpdmAbXm37JbP(sa0$R(qY<6M?Nl3fXyEd(v2& z%0+7ih}v*agZrUPt}Ru~+~CPw*E($+cCs_LTSLFln?m+nybh0rxI`+UiK%Scbwo(9 zRDizGn2Ir^CWE-q=}mcQ(<3DWbBsdS3sj*-;F5IAT<_N2`$pD zir6?jI@8LL>ZCe@yx1qRUDRUKa;ORe)`N}dD-Uq`B_D%iE5}B&A?}052)KmhY^KgQ zqiH{mK3bKXtte8iUt&yF2+=B~k5njoFfJT>+F`4~!g4i7IvA4}!KvgpV1PxL>Gw7y zvqXsSl*BYaNmaCf7CBISgBj{%2dx@J&0&&;~CzMOcK1-SOI)z?!a8cB*`XPAJIO)p^593qH54uWk?O&;bAnr!zT? zU0j|{-^9ffP?fLh7{l=&F?DQ~k300Zebz2d3(78`sCDBCQm0KleBwzA-F{Q%jri`@!+`-xt5#83xe`A*ioWe__^n&3XiRV)U-3WPK@k4vY4cztp^_RB6Ox`LqgvcOh3c;3 z$dH8M98Cd(O2)sCwd!bFP{U;NdV7AW87OIcO+2jRH4mBbliO2b1LcyZ`v#;!_JE%` zvEph{MsT8oC{2hW0mM6~k1~AXDP0|M==R$K_mt+agS&D1S%lepSat}cX`TD-hb2pB zYd_n~f-zCh(8TixwTwA-G&F{6c^!w6raVh4K}$9}Lu(kl;GR zMvRS&AzBBGLYa^5z)j9$3OhG@WHzHCLP?Fs871}*JE$h`l*LQvjYAm0Ort98aNf4f z+UV^ce7q#M9#>07G2<1o=U3Lum~Cgu(YEcL5ZHG>M4~L))t%$|@x5(sGdnnYyw?CIgPN*Oo?rE%L*K?5232?meU1w^3_E zh>Z}jc=3V92LmSJU~lLjjp$D-{dLnE+qphWHw4=gT%tjr&VI_X`O3{#vNeuF0B42- z*Tm&Q1+1|c=dp=r@t&GL+Lt10AW>F6`wag5L!aT)BkP=Y!g2h5%T~sIKSuTFq9XKw zS|Usv-;3-Fh?yYTN^WV63}6I_%6JhZts|uY@G0O(ulg>BBpWb|1{E=E8Cbzx+Y%g6 z#)uJ8$CAPS$yCgY*@7Thds#P4RL(o<^#&%6c261fvF?W{WY3QE!Wh`=Yo_;L!TEY| zGj>v}f)T@}M;;{}=p{`Y21~^YFaB|NiLD=;KA(mtiG?_kz?YaoCr>~E-wlJH8$EK_ zESVi!CgeRN6dtG#-Mcwlv!BLynW`YK1##Z_ec5eZ-a_`A5ZaKG>4}W{ux)a@6 z`M$hQL)X?!4}E8KFv|d+`|_vo?QdVno8J6vFnenE3gSR48`cl=?Qh-9rI&n%Bs~mc zj$yp95+{d(9|x%unB@b|lB&z@SqV%TTt5x1kut<~=CFaeY>!gL>xLdxs*>~5haUAv zpsY%@k1{i3;IUisqjG*2Zy{LukTS*~}VKkcrGPAiOxfN$qMm#1g zY=hLCG;;}zCRJt+3#C$96=s(+TwWOGtzo>8K#yglZMu^#jD_~(Tz-nx>{Uo1ySv^K z2_x!gRC)uHtyolCAizZM>X>H?F%i}aqNXho+=8H$>HOWcW5wQ38cVY7l#o>RnmG?L zbGS(e$udV!)>5>aOS-g0i#i%t6k>sevpDy4FJkkJzhNw~ob;?`@SA&o$MQKVDGz2A zk}1Z}0Xgn7JRWW13!M^THcQ?rtqxrEkknK9d=FZR826T{t3#tkWG%Ra$Tf(3gWjqV zE^*$PvT{PSfIM z?5u;Y;2J5CdSZkZUA48V-(B&vd8h==wR)sPDp;@0!Wpv@bLE~ACrU#h6^ru$x~gPs za~6*y!KdJTL}Y?l5oHKgB7*LMtDIkJvtUTfWYh_YSmJIQ#WKx%kNF;mY=Bb2K z(pSH61(#fW6{1havjlasQTo8U)^$h(+Gz4*g-#ELcOIck>|#n-RxCJyyC1%T4K-nm z5>;&kHeWGKn3Y(BUWA6G%to2(8gTDAIA$f7xYOG%6r_+{Rh|syGwfKZe(Rd-V#wYy2Kb`-HZ;T02L zcEhSTRl#^0KW$DCmD?rOH<{rJnRL^3Piz#C)>@T}#+st=w~&2nfLLe%>qkduWXYuv z7*`a7Vu-yUb-}wuuDrqMY{hr*i$zuR{}ujUt@y-h~uZj1k79K_hZp zaq~57uQos&ibK?9c&an2sAh0cMjf#z%PWIagsN~MUJOQ@+QD#fH85Zes; zdooy@HZt?P>uN`ko$VDTf?~6lZ0p;QUZ(0g(5c_&#j!?Y@2oTV0R-qP!Ri$>Zu>Pi z-f}%lPCS`8J+t`b6~E-EfAAzQ;8KT;><%I%h3xBv!K=@pltY`YG?TL0OmI#I31G4h za~7=#2|S~sV1%etfrf=7A*~FuwKR*0)NyHpQB$G;IBie@@P<|h)KH><*D)fh)bJQ8 zxD5KNRc|p<*I7#^JG!rO9xm+ylt&Hh@zmJPbV3xeTcg_-26rL*(B9q&H5XNm7+cN} zW5E3*Gk*7~Q|B>aJ&m$y9?qFiXka{8uWeAtq2N*;ft;iFIa_=gusTMW(ID;+i7bR@Ge7eFb1Eb2+MA#KDUfh9{dXVTRG>x>{0is(#J3xcJk8IV+oA?|tjVPY0@x7)yR zrb<OXMwxnbg50J2*)Z7>Q%X%NFA;+v6BD1{Jfz1Xqio zGfZW%7fR|G1NybI`DDf!y^unt86D-wLah<>#%C~`85VKg)tGb0xeRhJ2j4U2 zH0LEVc>RSGvj51u(>>?>$u`!UyqQ^Zw$s-?j4>&-@c}k(n#G;B&*uAI9OCY8?7awS zdAV}t8^$@|)U7N%beP`Wae8{UV{L=+u|dYhs;s|nh+A(OnF)p!_V9;Mbel3INs8@3$9UF%nH!@4DmB`x>-hfkl^ z>${EqXgxjA{glmbNOT#sbSc{pdJ>IP>&(bL(&4zYDw~Q{6WSmX(1ckLcsC-u1^^ddby@RuSf6=)sX@ z=MjZMIsj5UH3BY)G;NkTRlJ!)-49Y`Dcg9M&1|MWGHg$3#8!yXG^=1Yl#OM?B@Fm7 zb5h4Q{^_%m}kO zZhlX8_#|2>C(T-G@h~(Fl~k}^CIuWJRM?lCFi?Yq+b5iphL^R_sK?k*n77?vvr@Dy z{+QL_Sd4n41S69klj&$%%^yKRat6o5KkoZ%#8tqX6G$kUA=Vih35C9?E?d>Sjyo?EoY&NIprlUX8j`x8`eF* z!0MHlj^Kng|Fy4Q-L2Dw>|FN&C~vUnFSU`gl2WBBXeXM=>UbazT8x`iWiRUhy z%N8o!xM2euQR-SHukH%G))sIf$(47LWmQIzhl(@I8n-Ok7G*WinpKDe<NT^(JmW5AEOH125tAj(%*->3Xxsci^SolOJ0$Hbv|`VBOd-A~JLWPhq(lDb%fmeH zWos#yw@v!2w{H`@eVaJsP&ni4GQT)~HJ^CnJQ}~>bH|V7!|Ogi%(Kq9gTDTaQ+}tf ze;^0&@o8m_y;&C!NJrBNuRK{RB2jgm=El z-6>iAJ!_VoalR<4^5=7Y!*dsWk)hI;rv8rHPq_qx3%?8tfzu&<>+2c$@hchq*lP6t z)?7FDimy>!`Sxk=$&!y9)BOAB?Eizf4fra(Krhhv-t~-q;z|7Grdx2M3wBM{0;dz+2f_J&`Z_i|42za#N8HWsE$=n0peR8rWEba+ETKC|oNH|tc|wIT1#rI8CV9u@H*?u}Yl<;b$bn%C?8T)( zrdZi!7x$rUK0NH_39eD#uJP?O#F7}>3JMs(t5QxamBjL*RcEnadl}TC*k;ghN5M^P zlAnhEDycA#2$7!%f)j35ISmjHuQ>9#jK?}{b}N+43Z{Uf-CCE z51IZ3$)xF~BgthwM8e6heIcJd`=z|;(vNfSAqVq-tFh&t&CFgh7cJ$-3W){Rmdump zOd*A??E~?^m`^CFFgq0z?@$x8LX2HM`2STh_CQxtu-F%W=bZu_Yd0+1y$D z=jJgsG{ZKEp&V8oO)#Wj3@+oWE5#)mjWGti;XnT7?-`3d7=zb5iL9BPN>)g!I2H1= zu*ti;RH;QN_1N)>_gujGfh61;30xUoWm+A|xy%IL?8I~~#cth7Z>>h;!Ooaq*P_8| zx4ov|BZv`PTHMTTrMz5u|M%8$DBfRI(Pcy&rshz&AXx0}Pdg&J+ zy@Kr*t=^;e?4kNC-2W4ZoTKDifVKsC5~!r%^|I0!A3FI9Ea>~Uof`x3HN5bMYdB%S z8~F3jhNkN=Bav+Th4Dak9KGass1QtdKFvAwcN5%D3h zXs|9UNC5Y|yUe;1h#s1ps(mhssvl$w~>mEF7eD2LR}8l_(HiX-{vx3A!W zw>&5Clk94)G|PM&u_zufuzUsBXfxgoBW4WoHPCuyG2{p)J-MX@BnU3MEf$dq7DaTB z#S3Rs+i)W{>4PkZ1|ZoO3gq%>FwGn=Rne()5IxTLs#SGTF_{#6AqUjZm7Pp#?FsK{V!6t6dGhISjhZnn$ydIB zqqN_5aE-;5nnW~A+^S$VVY%;9!8)imN-W$~q6Iy(tLWQVAb!f{D_B*GgOalh)}*sp zrA_9IT4s+4^G7V@q=d&|CCVyr3?pU3Xju?XfM#o}j?WWZAVl7vk)cuOU?J0J`joUg zQ*r*9ymH?($5#{bN%L+{?b5c?DPA&pvS}ODNC==BZHDlg(1)$d2}uKG2i>VMSvT7u z-L$LnpkmQ|qKb6;rMyc^I!C@_0rGa|%&6=%09m_!mTier0{sajK5G)0!AQ&#T%l4^ zzz)BB!ka$$Zocxl&-3a(ypaA`TE;Jc6R*tR?5t&Rym}%+R-6DaaN!1_c;-)$?UXIW`Rmb7O8|{ZiQ3>KT2=Z z&u9MoM=aQGh{iXu{kvb}f$x5vIiu_8Ee}yH_j1vv{u#Ew+K*nyocdPEV$j6%zR&+B zZs2HaDS}ja$tzxo%aTM^Q(~VIk&;rT(Qs77;vJO74MXFC$`~a_Q%U+pb%-Tjy}fg>~Ngo?lPfwbhL*nDaH>clrh{zUGfvH(uGh zBP?&J&es1HpMh}ltPw6a?Hlw~ez40&SX}xjpPl~z=Wn`%Ykjrr;KI4R8+qHZOX!Wh zzu%2BgAzAd5zbeWZv zSzKyBVuUIxWwtVh`&ht3$wSmkiBVstN@gGB`i4>wD7%EY1{NFO_xHf+Re(i|?eO6d zgL4ViMxc&RI-2o;g-R&Igre;z0*LT`|T=ja(Z1ijSPPhQDZoDYW+SXP_3ztjC8g@9JaRy*_7Xr?AM+V`t_FX^Y8; zP;hWfW$4Tt+Ue#o<{=>2vxBa9K!r@<6g2TlMW%Ip^J*C{1{IkErNV?nQ>f(1zt@u-FWda0~=oAp6Ef9AnkXR=05y^~~?j;W+W@jNvLMz~}U1(?_PvCieh9TA- z7&XmKjkf0Jta}T{Dwujh(OiFM~9=lD*B0$xk&Y(^) z1F(IDqgM#1{?o2dGP9mE??i7eJmpE8e2?SbKmH~94n2g2)v@)-qx7xn$@(C z+bTM@_N`dJHd0DSN^iCV)Up(=d!TpeIUk8WE9# z@uXg<#6ygDQ1Q^)3;hSv@D>!3hEnP61!DsxM=2G6X!hTas}syk<=Afx`pg_^2UGJ%+afFnY4ZT zwgud9-7>EGp8@W?#M`cY z5l;7(t_7%8qk_>WgL?HGHgW`WduzP)sqdku+zG9hx;5N!{~vJYgNJj&$OsRQ$BbfG z*pqP7L3K`EzKzwr|A3Je-s7PCDj%AC7|-1N21X}TxlSy;QGwIr5iUIRomA{qlg{|j z#_PCw___RY+Y0V#3~*=C!=UjjwsoGjIpJ{qC5w)}p6dL6+RN^XN3kdo+6%(=hWF7Ipv_+ICkN`VB~?;eXD$r z51+J*vwr?+M!FaS=Rf&YR;|8yit#_68?HOFi}C+EFo!)Zq9C0cLJz)Z*4dN5@CiGH z*BZLz*Pv`87Dh?t7v#k+&prDrzWbfKxZo`-S}i8ByLZ+i244I6 zvv}VJzDlF9p2+s&T^*zx?k|$TLmR9_pq}8}X4acAcomQm4{qE_4-449LLN+q*@0UtlUDau?--08? zQh}*BfOQ6wN?OPiQ}VeLazKQl@{Ui@+_s*n{X+3s%*Z~G?$dI?afjZfMIiV|a2mqqMk# zM4MeClaAbslxhcIpl|<{2?2gGUIEFV;daFA+k${HA=oOdpqA0w<&OVf<+m!;>jn5M>gTX3iNTxRE-~Kk;m~ zl{{jE7AsP29OtB1(vjc_#|&5E3}6l4x#5S53?|bIer2GZa`@m-ryl6e%`K@yq#n~- zX2t2Ja>ozXvUT$#oN~t5Y`kv+gR2h1BP68Fgh+d0yime@=Zp$qydx6D`+z+Yi*#+V zgiiY4q^7yPL_Y94npT3#i-a+1vWhAaFcwTq{~6EYkstn$do*U7Rkj*sxfy^`AKQi& zvA*Qkl$02aX0cQvFbQnk3L^CSQR=pb+0-z?~QG9o~7w6X2@V6@#&m{YH-Yql7b_Gr4o+x3{k1BNr@X z(>0w~I-7r5;`)!zW$;OJ_=|tOpJPu5zg>3t|LoH7O+Q@t(R(H(w|;ZWDn9w{CEWkL z$)T`2ZjZR*EB$9_B^?kwOe0RHf> zdwJP$7j=C8-m5>)uZH_VP(Opj&#Sg3&I(2ihmNdh&5r`Fea5#ZCAaQ+;8H$)<4PW= zS-f$mRKhwhaOY@+A8(k)C;AWP?2$A1%ekLqZuHsaZ!FYEkxgqDnE(Mk>@4@DB@VuO79*F(Y<~1~4n6Y%`v35!_(t6M+@JF* z;FW;V`+?su=e5u3_uJ4`}TsqQBFVhpQaf9$MDe$ma_30+tv6#;>f!Xd<#J#%8}($zJ^pHmt~_qyqiVBe70~u8?+2qW_u``j7L}nt58NLp$v&- zR$AxZKJjtvkssde>-qT?W_P?l?e$v;sf*0Rk}_zyb#rx+Gv9sfcp>|-XF)4FtisSO zw=lfA1#dfi!ifyswzm0g1CtO^9?vor8e!@oGR2@Tq>#NKMS|EytI%y@Ya_*qqUkW=9xa}TCstHEGx=_XMMVsyG)9VsV2stIRvG~-t@%~JhYP7SrKbw2Q?f5XOK{vNzG1K)}UI6@VA62m}Z zvLeK0>L8DLO*w9*38$I!Q9bRYxO+dKBOeADW@~ zvYh%U!9^#|CDW~AB0?-6k%mD1;xenAb2!e-X3?^PDIHeHZh?@BC(4~&wMCW2K6XL# z5Us-@7K{-Lg3l`Gy9q>vO71WY@@2?N;F5Df3rZSr2u^~Yi49>*Oh5EgIQ?T^4bVqJ zeK0->E`m}&XMX(4(AdVorCx9rB85jD0GmL4D~lOsgYzt63uh1ZvCuzCb?bxlR_D{- z+s6+^w=>GTa9*k%zPHJ_#k_Cokr37{A=70vyB~YVu@wo4J<{;r7&J{=_p?}w3faf< z+6x{cj>kH--Tk|xdDnB7kvuT{@3#I@_?y$0@PVIsjyd+$T|WM6KCzW$hyAW&+yDOb zMBe}OgSz_tFF#S@oevzt-+jK!z~DX2?N42PC70iEICZ~kI_7hpR}#}mu6MtM?|=V)dE@z~AaRR#YIhC+Ihn>dB~u3( z2UA07&|)RB&6na(QwN`i>rA%2Oudm(rOIJLM{@u0b=>Q#%*ZG4>mtTD@=;Dw5f z=)*HoOPB@Bkz}vj#-D#-GEC=budjuBCn0&KUJ+}W6|dUCo?SQz2Ua*LaItq#Mu?fZ zJxg<&ctXMT*L{vA7=ji`Gs;6 zvQOm+l;A2B%|ae$qq!}~AEv zp+xC)2MwvC)Otpx#-mk7(kG;89b;_<)e?f8DNs$IClO)?v6l|$H!q4ssHml98ypl7 zs?xH+{*E}fnjJT4h`pht5q;HwRGuVs2=uV)( zxQ6|TLSqJ2>@*A3?HCMnDnng>R;{ox#p zgwLu&fsC$Nf^83S_1pi1vLB&0s`J!EOZdTq+o&=^WgN=7jeb9l^L6HxdKn~Y)5jJY zT!6qesI);cihy@nHEC89qB=GCTn^VOGSv;89L1U;!c`=-3fW&|{+XVmR^8dLtzH}C z@7_6caw`|V9WHzGVm|eQ_4M{_*`;$h{kivdykGyoVSMoU2kq28?)ZV_YyWlvZ+LSn z!PQ^6mKQF03IBfYq8&|$(<-Ydc&~T`RpF@7BW660Q;zy!=d}#Kf)C$xJkGdG_if8U zon`_4sqf+P;WzQtIjscOfc*w<>G?A*9$zu#ajPh24t#ym_U}IQ-knNr?fx1gcnm33 zL?Tozh8QC$Uh5$c*0fsEj1$=MPJBg>f>V_H5_|!jm1fmLJ0ta9~om77Q#qKkGKmS4}58i37Z-8&)gCVo2 zgw&;26J?$Eg^hF|NJl~Ogg=IEzn$TgD?6QEBb`g%0s5GAYz((?BRV=tyz-c??$cD_ zv_LE`q>#NKPmlyxduY9wNeX#<7!|Yy5;ck#A;n{HZDw+_6+HXDZ!7{U^hqn|D4CT} zO&}T*s!2pg7Ab=S$%E>(*t5Zo@WQ|SGdA`HJ}oa<8%*8>poYa;g>PQ^5A-EMRHvB$ zbAXtLO5(9~&#PCR%~%x9SX7%1?@TU&VnAGA^HNVwxx|B|ZG8W>YpF$!dWMLgnZu?U zIwy@WtSI+T)RsK4b;oKjEb}cg;Tz4>LOT;nSzBUj97_IIn*l!^#HL8wcczcAHAC{! znaFOw*U8tD!#!x1Aah;UWKR)*YbD0AO4CSS=|WCA`DuLpAHUAhWruLx6~ExjKRyF* zA(9Aa;Bj~{ZIFe9?2~naK%ic)*W0$q&YMU9y{p;zXnb(g0{pCGK};d`5Icx`_)f<$ zOhyJ932EXnF2yy*iDLm%g7F$$a|O%m8!0umQuSl#{8iK6k2 zZNK=*k=*y6-7{ot{;A>WD-PoYFW>n9|8qVt#{32AI=21Cmkz;i-}!6#&(92S?(5EB z*1-3h+m4=hKTMI}3PjY?wJL{%w$1qYR%~D1)$vyxegks`Ki{$KJGY-s+BUtmhi;9L ztchXrobloMQeKn3fw|FVn%j;o-3ikqxZc+DdxoNa>)7_-*gJUdrehzovE-$OnsFdS z#fI58Lt0QZXclsi+)EJ;3Ip%o%$x=P-m$N*{Pwxjci|j9_|U<;cJvRJ+xykleVe-u zrb%%9@zT4A%x@-)|D(9?KP{%qPTtKwHC%PY3SRi~>-XB4J^jk3BUD-d03ZNKL_t)t zyW+s7-9r%Ga!Y3mMzdu*UvFiCy}ZANT_zen;e>4jhaCp*`s=rI&+lRR;hE0vY&5!~ z=1rUS*76nbR~NsW_q^vS@P{G{01cp&eJAVGk|fg;pZz}Xmx;kC;|!6ofP*=j^^&kz z$EXvyQ5)6}tAO_ybyO@=oTFcaxe9B4dLtKoV~tPkWt) z4*V?IFfoZWll=S(oe&eJy}nN38bmhEth8y2(H)JYqK}*nq$HUXrzIabNig1HbrR%)r17=K)3d$a!n{i_2SFk5V|pg%q+^ zWWP>uO&R!;X^g(ih-k0bZ5iE)ic+_p#OBV8%ozTY3Fn{^f-^=#H%jbgs@2yiX82ac zwDnMO(A$8aahgzB0bAU*IYNQ9Q=(w@gg~a5w0=ai(g9Me@J}p*A~P>*zysU1v0YQt z#Hfiw7nhi3XG-5_#5qrTR;a*orssBDU_1=lnu#QJc3F`+e zPBNg8Oc6y5qDo)G(6EY2+Y<#DT%q?Eph85oVHv2oCiO`(fi>-=XCeKjaSe?v*r2bo~^2>|9f1dX*J+ zkmS;=s8%LY%1`YxH+3fOCfji&M7s%==**s@VnXuGhIjvuy*H1StSaxlzt7r3)tS%f zxtkuKp&1%z8W2TWML`)92VPB#F$@>=&GaT}G%*@yjd2J`G$!}vCNVbNt1%&oXoB+) zw2cu^5NH`2nx5y=XRKlG^}K(qy?5%!oq z!FrB&3?bpcXwN^X30z~^u_oen8b%%!@u*7@Hr1l7m?o)>D&A3qNbE>Qrju1?9c)=8 zzW$}5S?}lH{=T1=T{D2^9h}wgE_mKZ;r-a?0>1vP?!#KwP}s8boI-+Y`J9J=Be(S0!dEa$v&e?~n7Z(y-D@#8Gu4?<; z^Ggr4Z2RusE1%S{6ceFIFy3lfsOI#2j7_+=Xso`8Ej0AJO@;l8RdV?>oPc;95C&FYuza-#u&Yrk2t4wJ!~3&nFfk5|646H7{oyrt5cNINtmZL%jT& z0M9Ezzg2+oLv$=D4pkyZ87!NNn6zjzYE#)&`la+cCa;e&EuS-RW7n}Yy!N~qJ=eke z?#m+5HV02kj+?HLZ@POm;ZSKzkbR(GGK(1 z5@4p*Z01iil7800ul&jjc>nwV5hJ4rgQ(W4aOj0k?RHxGwk(yg$h)Wx2-&Nu(>EnNP`7iPzZt)douFUKd` z1f$xxCPCFh#N*;?V?1LDqk6;onMLn6tS_}}b6j2)l9^E~)or#}+jP~pY|?4sWBKhJ z-{jaTi3rTosJB8Dd+;Ffu3gw|+lYSg3!)7h(7PK+s@hNfoAOIuIwL!!c}D!<=kDgF z8`e%U9-l`3We1s}BTpfDO0F`^)IyqrzG*;{6$5oe^^`&=CF`kCY)RM`*4W=2d)hIK9*XE;#6tMljE$ z$*$OO4#|u|tqNit92<&Ca=l`R)bS?7;^m;^EHO6gAm~)goLKYWaC~9K1)(F6gcqy* zFVmz~I+g?nff*au1?E|@L~{Q4Du;$CY!mNl3P_}#lbXfCr73uz;yO|kLoAI1mmC5o zt%bf%D71WL`ZPJPN^({z8B>#r5Rnj)i@*XH-gy@<`lV-c`Eyy+ zIzG-v?k|IJoCwMIk~Gjdqsb@P*ozapBh*I1Sk>{=X*zDif^V@hkx(pzt}7~m)Kt|Z zFyi)MO-O`L3Sx+3{FhwKW8e51UG*wGU3GSL&tZ7~R`%di$LF!JaY`uO-hG7ha@pJx zXH<6)`Q210@zvoiJi;QX^e`y_u^1xt+ypl2ptIsQ3Al=@kXDOlp$m_JQOio-r{*a& zqgkI#+;zDrLvn@819neal)H1v%~HI6WQW>5e=}vSewo z>uUerrQHA7*@HN5{f=Sx_BAYC_JdjdZpEnw3h#GrpEt{OEwxphW*QkQa=`SLA1NdR zOac)$oz*aRas{W14NQC7$9o=6d_=b(2!<6y#bn9OgJ(7T(wyO$@-RDY=BY#E`=U98 z_XFNNQ=ZQ$;a!FIR!`yc!>2u|W67#T)KBbxlOWy+aRL$>OUifXya21Nehn&$8hInaPzUG{RY1#j9xB1*IIGj5>w*r53r&*Qf}&6H zd(?7;i9eUVItj0i9G4X-8Fx?FGU2t4cd-(NAj{-T#N=~*T{#i%Rm zQBi93DpC&6vd`Ck_DzP%lR3*ONo9$}ie;cyXI1}T;=Fa_M3i<(u5^y1=40LXRJ?r8 zhl4W6!>pAekDi)Gchw}0O7mWGTq9FR3Q3g)ITgifj7gGInz(Iw*F8ntFIGRgpnx3W zTBlnf8po3?!0Hmx4i2#qxNSI$9hH0H&4<| z8`#AB@$)}X5yuabRDuiBcPQt}EvCAOg#=eSl$qurivZnl&5f_*$3NM?$&)clMpiKA zf|H=vEgF)+p?12AkH1UW_;+}dFRMi+1iI^y<3fxD5if%WRDqHRVj`k`VRu~1ipWzP zV4I34>PCo+A+(k-j_L0%arxhT1|k=nyTJ5t^Q&IR1R*9EVyxap$@I}j9nTK4prouE z-N9(3Ok5h|g-cK4@vXxsF?FdqUP>{5M-bIIgR~>ZT!t=I z28Om5w(Z(6c-Z^fvwbd$m(Be9!BSYbxJYz#+8Mv#EB3)@HNJ?>*lG;@Ur;v6q&2zK z&;3NiB;Ca`lO0`KiH#*T7OR>;6axgJEbRYb@mdyqj(@x6pPKd(HC;ZtFd%5`pQ_|6BUgAMNrJ^B|9eal$Rz$rU34w} zdSO3j%=;8OU-sE11Ebb-->OZ|zcTfG^M1;UHkYWx#rrlGMhtfSYhl~=9rJl;Zs6sF4rRNw^X8ro zww|>3!8wF}Uh(R6eEBQi;LX2&1%XF12xHsp)wu48d(zvdF&l2~X*u!--uGSJ`~GiX zq#tEIwP+MW+R@}C2`)d$aMqLk2c$_ZB|s_BHFOGFhBxvk0b6Y!Ln2gd9Zy{!m|8_V zArPSpk6sH44J--*qDLOaKmE*{Vb)yf)+YFHNbf7XJB{(P?)u^nH>{ue*x0bXuVvf1 z>&Nr6JR#ur+7@fcl+V5SB0K)ckY0A^@t z%2qz&#Bxs!^P6A;h9oJ!PZ>yL-TR zHmjg(x6kGPp%uV4lq;6*P#G)5EIguHCy08K>w+Ice^8Y;# zGVXg=QDV_Tuv36Aa(Z3pFpYKOI2Z-0?c$%=d?HcndWgB4s{f93WY%MFW_EQ!jDoL4 zq_7O)MH>MdlUrC*ci*=~Xc)mJP-KS(bD?bzHqshS7sQZ50%#a@O(}B7{MziCk#^^r zcJBCjg<~V;z3Rop4}OWc)j53S=C5+YdA|WRD?lX$b?J0*J*C~!L;om=fdoJxa7N!i zw#GNxAQl)4OYC0iv3-Gn7h-cGpgGyv=z#JtlnCQ)N;bw=#Zy=?8t}#Me4DuDQ}RJ4 zAyf=%aIJ=v`x%dg7hHBZm%R4ZUD7BpP!GWC0rh#stFA#L2{I_-)M*s^ppDwgp;@k~Mocg*8rrMWs5P+1m8WIwG@yY&>*g`*f+YON-CwY*RrfcH-@> zp8C|7he^R%pfR3S@QE?Rgls3Y+tCY$5bv?3UsVvP9M$u<{0C1k`s3mlnDdg>Wa6;A ziN&8h&lMgd-A}QFpGZs``lu(UX%r5sDD^mklFzp4qL%$t!jPRoX&QBE@eGvf9(4>s zvqhVS5kL}D3dztkK(V5Z12ao-c`!D?@Ux#YqkA*>^Lr9PO&^UT*OenOMF+t%V_lV8 z{AbC@aMNG>4OhMV3RWx!&ot7}1qh+U_>SulKmI~EqvLdycMx=KraC!~&|5_mS5T_B z(jniyY$<`{uO^&!DP*6ah7a^{-(%Y_EM-6Q*<9Pt$vq*X6C)VWEL|p6@kQxED917V z;?ZxFuYKW*yyD$|1ZA&j>>b;CoF%+828n&Jn>jboK2j;9Py zy%C4YZPN@ZLkn~Y9XaxuGQ7=q#d|Xz*dP2QqnCS8_o)8mH z`5X6l0}I;ps@im4^FB%({G7GQ`T$Pi6BR_cAoU zn+JZnku#sa8Vx$&A5ZmpH0vLync*bS<+e-H=P;Qd-b{qbNVi8UF;E1-2xh)- z0=1cq>jdJ~Wl3G_=a3PX461=|a9sn|2l3e#46uLB8H`3gb-$P8ms|?NJJ|c9@8MNP zVPFYE&wU9HGLHiUI!UOLROzNhH!&g?JC}0) zviM24PG1`Hcjd^;c(pcnKSC@C2$}dak__#w)OqD6XU+y~Dp(eq)+F6z{8UPlYcA+8 zOvrA^JVu$vs4rXaq@2Np+D&b9F)`RPCu;fLsO{mAxnaVksIC(?gHyLjb6OE5IBoU_S1ajMo4@ImVCOTAOszdqBXt>mm>@ z9r$dxc@|w>Uu^A=TX+3va=$_voo~%h70~fcj-jW~)S@(e&Up;pcaO7skC#T81q5Ni z&%W$3!q>cp@ci>J0|T?xf9d)?6UVGGB0JKNBe46QjwhXBds-U&SXLT=^OSQa4RoqF zz_eI3fl#nmjoKKRxkV)xL`>m)HHt83LP*}cd?_XYQB~`R%T*OILTEywC~jH7Gzo;b z^L5C;9v(K?zmHHEcU5dru99D+(Mgb!w0D-ojy$F2Xj3Ol3iZ~U+)6R94V~_-j?BxSAsvvf4}q1bdBz2uzHYv!4UU+@$Y!iM?c95HgV~K zCG<}Wv&$In8rjEI=CRE#z!x8)l(WYqww5+Jl%?IwO*=kQ?#YDt!8V``Q-lD?+nClO z>XLp@J?f0DBORK#HKxM_Ftv2|wpv})%Hac@S0`up+kv_ENn5H;8(BHy@9*z=6s`2* zsZVT_SfWN%zqa?Y2EyZ)lpc`QV^?kYc+bM*-Yb66^BqybV;eo(K^I#ovxVB+MNi7v z#3*sIm(kmr7_7z~qEQ-Ock6fJldEZ<#$>}l9UQt)tSuB+Wh4hTQ(iuOf_NOa-eoVy(F@tt81M zlu&A_7ns(La^br&{EIO8;JLA2SpTX%guvkcSjmkl@1yvw*5+R*LK%f`8_ z)iJ*B;A#^)!D@Q#9gw)A^N`!z!&)6v4xZH0$RG%J?7%+p#Ec;bW!W-<8*e0Bw+{cL zlV-+O+~gM&bfOq;x?wGE|6HR>?Cqbsn@?Q7){V?$0Cq-RM>=x&^0e*XisP7C40Z7u zoCj%(elGrCo1oG%4xnOLe!(PnJPb``#d$K4HsX06aU5sx6=DPtL2VnO+PO(tP`25a z%52RfE1=TU5u-UW*Mv-_nTM<+&n%geQ)gP&bM~LjCZLecx0(kU7FB%4fq;Y9bQSVa z;1=shN2VO?O@q`+HdL`}9~)=bn_9IEO_aIfyh{kI>xxo}FC#TBSi72F)CHyEICf*R ziN9^K1lPasZB+c2Y7l4KkHrMn;f!R-I)|+Yo;7&Zuz1w)g%5rT6Fc{;Ie})>CFe@9 z$DmFfXdElWL>MX_j0YqEJd1~7)FrwM7GGVWB5c0-DpN!5_XnK3?gH-q=WlTFs#hV; zUEz`hX_2rdxWayiE%JmBpm1_rQMBFKF$F#Y)kFuP z@Q^Yw*18Ut%7>qH@=6o4`hI1yb;NJ@-bH-u(uK3pmI%VZQcUk@i#g}FuI9mo^@e|x z%+uKb_G!jYi}R*RjZivJ;ptg6@wRgW8%{DVfUgo`DTtJDZChMk;u+hd_5w(yiFZR$Fby_ddhs%btZ$cZ<7v(>|*i zuXRngFCJ@!l`8qQoRQfmy-WS@X7f{U2`Lo1yxAu?GfIR)XTD@SSc(v<;){o12@-Bf zUzKytzhFkk7~HrKP$Dc=jZ0%`o}O4*o?GG1XL?Yaf)wlvY^ke{L0Ju$cJuYBHD^i85l%0a_bHzDFKySgeW45uP%n$$NUwQuOGr%u{Jei4yG->D7hgRDq zP z(4%G`OgKndY_lY$W(pOw#h^@n`B8BOg~et;31^?1Mu2Y$M=S6{RBhJHm0d&AlCR`W zMDwcNVZ0-PHA-ZXppa-&m6gd9eok!Y{84h?6SDmZLN;qaMqtjG|&9TBjC7@gv%2C=$(MWJv z&r;vIpOY7@Vz+L^Bybs$4@e121j@c{%UEa`4dcQBoWU8&>?f6|gCw*FDh6ZSoGM2` z#RuOiVVepvlO0YD6}?OAA`(^B00~tLD&p|#)#5d~A2Q8?qK`8pWZHOk`n0hEdn&B~u%YhZC*?Wru&u}L@+OY=3?yH!mX!TqwM;B?L3ZF#hpnx) zT50Z1&2wty!S3JJ}Jh5>qeFg0kNQV+L2-w;258& zYpJ{W8`cZw9RiVT-F5q%wIg9-V7thSWH%ecGZ_ewB&zz9m4)KM=(B+sU0V9xrSvor-z^X@;ZeWo84B>MUyV%M;?PXrcFcH=8O(Q;Lu#}9UY-8r) zv8W#Lg-?BkbFO^@gQuL9MFx*~elzP3w276NtL84iLue5{y|A!*371zIIdHEH>u1IX zx#9EuZJwhx;&dJ2kE5e|z2ZF_KJNksH{Oq`bx`GP^y`t_^}~fAlx52(z2hB(7hjC& z>pMDeip-J7t*5Hoblo|;{pS0!ulqn`!8e^&g^qOONaZ*sxU^vU<-kM|g<@L1UPwx% zYliPC!@s-Xo7`~5Rd_0i;h=|h|FuyO=im;VGc4MGpi-5rO;i$+E4L6&z3|kc8hiHa zVcA*B+Ai{q_@lPD7w#felgj6o3gMiyP{7G)sw?ayxlRnDI(di@4uPrR_!MouG(6T* z^YJHJM@u?F1|}hl44#!~BV7}rWjwIN#_@-=d|rxJQnD}zx zu2b`s-+cR9vIJKO<5Z4>QY3uggMUt6O*&m$9hsHx72bp&001BWNklqfAB`?8nL^) zkmtVllVHXf@p=#<(3sI+fHCdoAmgOHm{v>{;!L%zW@nOid8I+lhT|%x(eatWX}fbz zzLjHN{${5IO);FKwVJhoR_6E*XCw((@7l<&7?lsO3dvuz7X zj~*xcEDcCEMWmAg&6pu3S&65P-)!QZ)9B;=!d}V?Ha8$453zzBiMty=*qY#4(7QVy zDWI7#MRN{VTE`*cI>{FI?we9iYTL6-lNXKt|I|heBc&9(j(UwF)?9SqjIPOU+{m1S zIu7cj7}0sI!gI{FH60vMpKQ*7-#n;wR1>f_D9pEIAKZxWL zq`CW_JBirBul(wTy!ZWohh-^36(MrP&Pn;Qh=&lO5`or9+t;(gJbZ+1Y7C+*W-Hx1 z5f3s?EOUXHbsbwKNwNk&3FxuPLh+arDc|_~7kJ$t{t=%4WQLO3VGlL`ty7zv>bUWY zpB8=xSS3k%r4f=E8`jSP*0729Yu)wZxDGGBn1h?bE0v>`ra4i3gegW+RybPxT$`Ty zOz&U;f^hR@2SNCq?;H|>uyD4go{HVIi`t!c5?**AroaE_*&xjj5KZ#-&)v-@ZdluX zkavQ*9qGudq9$1SJM`|J>7=bvBu<8@V5n`NSoOWlWa43SITB~Bjv=m0=Ila>8 zj;OX6nVM6XMqAA?$(#O#z+^|Co)}UD!k7Ra9v`P}Jn9M7mx?COUMNLAVNH1TIhU|- zGyn`%rBD?E&{a3|)q@PibE;tFo}N0d`SAb8L;#Zkh4Bp#XoVi%GJJ3^T_Y9L7-DU} zkthaL*z1*f<%i!)?3Z|4Dvs+Y9rq|bHKC_wmp~L#`TCG@n|)pXZ|4=k)+Fv*5GX3>DCj|Sj?j) zbh@~vmIFT)4jx#-z)+)$Y2o5wV8!elNfS_~#)8HBX7#&?jl%wYOPMowYqo9v!Y5e% zsFCY|1-Pl3r;*gt+1b9%nk zxR$*(=Wf3>g%v=0#Hw?t8l?|Vr{iGWVB;pVA> z+v~2^LcwVQVvJ3aT&WLY(j;jT_-RgVsXDZ_rOUw&_Y+n3B1|Ck;F4IbMP1b-&$9|r zMYQB_hQ!%NSV0L;x(FC#ghkv{-_FUQ60y_yGoFFSou{vckQhI%F;wcNcVdDse)QwK z?j7&S+!ZUw_(@7SSY;YpM5jLiAxUmE#54_NRX^e0YIJ!ub8}568`k$Up8Iufu6s<^ z)!P=l`k5QfO0IwK{*4Tuy*m47`0R5Sy!YN50)r1rSiKqv!`WZB6rqK)IB|*s(Z&y|Yh(PjMt3{5Y)3jW zW548hMM-Spbk<${Y+@jo3~M|=oS{@Rl>LypZPkAgyY<75a&=bH#%Y(akjsEZOL$1rUI4Kl4t%%$qI%~owx zHn-6u9XWv{0w#Q)n&(q9UV_uBSto{C*Ztj73s^iFpm^9u9a&q&J6*rjL}=O58!;L} z3Q=!!0i(WpjEV&!dh0dR7;J+$&N*x<91IM@U3H?sqnewCS#7Ks1f_0lHu~)$9NCU^ zq+LN+(a|{cBB(T=32Aysn+~WvH=HC+t83;>#&Q$PLi1T^Q{jacoSxQ_aLs~5Id~57 zvbozX{+-tb)7=5j3PBCLFoBK>{3MH2?CeT|dOFi*lKpLA!65Jd$ou*7$Nnd;{_PL2 zW%C4>AZs_xQ-pOO3r`$Bww)W6+CH9D!q^oI#WHiAA1y%sxp`0v6l*OtU-7Ja#YVBB zm;|ai7KlZWFmyJU1fYFbTf)X6<%;qM^O-zYqZeg-=Lmc0r-Wb_W-RJrBI>0FpMaPl z6w7{M?V^GQ(#53bQ?FH%5N4WKijl;YjQ}G)G0;_021YD#mq!rAc*bB`sZ6LdOhLESL+Rmh$fo$J8r;y;9GjA(r-?*ClKRY`fmZg_j z7M`?m*2jPBk@1w!$&Yi{4aaCPque08kP6(NYWw}Hf& zXz5CNaZq7t|JSEBS#x~gX-A*}zO|UzB>k017WDPdr|_(E*3RhM|LeYcv0{itD91vd zC%K{|X|h}oF+XNKp`LC|!_@JYD6;szm(1CS4eQHO?&-PLDxd!JPdRA|oV2Xr(`J4_ z>Rn5|H$|q@*(t<2BE0K8H}c{4ej7N5(hc;Ib|tx)5-0n$C=o*Bj+aqrgjjl5uwXUA zyEn3hC2Xr~<{SyAqC~OlqEAPLhwI{+s8w0$`z%+5$De@JtHHE~kUl2QCeN{V46@QB0^ic&mMipEA&fk{ zxKRb)U{GjpWLuLI)h7m^SjD$W$tt0Zk;!p%ViG1N9iNsMtO(-@uYTwCh?%H`(Yx=~PdaAcb2>XKA36;8*h#ahk2>gm*zR}R5hQHr3e z0@ZFfs4*D?3NG+sg(K^tG(MDiVa>Umf8`}06_%a01WxO5+4@dG>lvZ8Qs34~s$I(# zG>RR+iL~P;ZICOcpQSK+nhQ9`KJ4p@G1et>9JiNE#;q2qL`a;{fv+B}AwbA}nM#7nVcrt8C){UF?f`sWUg}TDGniOQCE6 zVrz*-+{;FkY{;B(`1cR|B^47>@obi;juU&d;zKnMUj42c*t@Xzu)>48no&S>N&m)%@q$yA#T?!_w8E21; z)Q@qu;$NgDBj%bQ7L=tgSXQ>WVMY?jBE_Zcu@qsLniv zPrT?QGdkamKfjN08)H4w0w+B)k8if$pRwoo6!i<5pPJ1IsZ)F2&vgJzo7}oC##%V{ zytDY~SN@Y<|BXwC6#`$5sJ|_W!1LjaZ@PkyeDI&qXfH;(LHm=gt&n_qT}n)Ei9mgP zd@KQ$7)vjY?>$J!LiVzkz4ibH{Vu|~E9B{dq^#U^iUcZ@RG4c#7RPlqeCbQ9d;bSr zXVjBb2O~52@K~RS?~+zp0V+VihMT9h!#1p!EXgIA-&KSf?FgCuZt%du;Lq=8c+J`Q zKoRX&;iqGvs^5Dr(WXsAGlU?NWy=VzyN>XMFT`800Q>|u%xmbD(){_LO6l3prgqm| zRR8?X(aoFDEnBGl#a|#{NcpnM4r5TyZruLi=kDecH>_!iJv^1y-*$Yr9pr?L9B(-; z2`&p%fo)@B3}e9<;xve16tO-=Jq9NOu71{~_%%Zi1xe=$xe%l41RI((3`QQlo%w{) zLe;)gN~TsP8XH4v6-seLP_I+Aib>9|?)!Lw&uFR6NO#b7k^_|LhPp3ACV^8lfYm0- z-RT!bQ8_$Ypd%eQQhV}f9>;V>Ye$}JgZgO9j%H)~a5&;L`)z(x9zLvnqHJ19b&Qy7 z9|dDl6^l*!pQn}}7Os5ui}}ZUzk`;oBfXX*Lq%)3eIC#qu;PkK`0V@tH-oa2Gq1RS z)z@AG4xP9oC$L^=j*A4q(!L(*IU7)BF^dWnu_f3Qhp4(jD4BwPT%|ahZ34NIa+=E+ z-2BxqGcPpw6Cjllh(`IwJKoH^$(UXKTweCE&+yJ0e+e*LHg_IND%+Sl(9cfO#hnK% z6AkN;6hO$@M)jo%1mfdGN-qY99JiholR1DXbZ^=*-DSEN^ADw0r=H_Bs?p;cm>5GR z74ZeD?I5zB0pqb_e;iW2^0xzAbxj%HZ}4Y5_k|C#{8e*j3==r_`IgJBJUq^)TR+sx zt6qN+Lvyxe+xiE0@V524_{3!kkE!9ibaXifYx`TiwzPi)cF%2SW0~2c`O20>yz=ZT z80`K=<664D%Ujkwk3YTVykq(tKUF!IUx}_~$Y0;Mj_yzJNAoV_kN2K)^!@m~t-M6x zDF=Z0(?5c=Wm_97{DBWGXUXedOKHw6+4o9=Ut!tjE@jJQ7a!C6w)byOW9c<#VEhNO zZ7)3e|FCY;8h%jeI;3tA70wP~t~m8eQ+~^k7BX4->M?z_h%mPNujkI}+J5r$`>~!& zh^bY_b$U!az7uENl(ybMz69DKh)l=PwC?(e#ELOkfz_+KxbC`-^XtEH3H}l0Umazr zvl#mO;JxqpEk69g+ff}vsp5L9;>5@?jt7b$xv~gjeAK1@&LymLQ>8|U`3MWysgJO) zG)UQwFejcQG=gU}Llz1V0+bP4Dtvf=a3XK>@jRy#j`6Pq4?ylorR zcfIS7M>Q{a0m0Q*6JC5VX71c+?dC)|Vdl=IeEH?zc~t-SkI|hw(Jfo3-*OB7s#W;Q zmmk$}D^qBU;^!SZwj&*xtsIwH>J*S{T!e{oiHUNViBcC6r7p(9G818mi7>=dkM2;uK+ihF5it?I~A-5jxss$#7c;0b4})^pVx zGw$T-NJkEPyf&1Xc}ij4xoEm47EfXVVvLO(EKSD4lQ>s7&aHOh`yQTgf$`?#R{CC1 zJmXCZZxCAOu0yvNyu{!?s{NCxXX3@sWw`kHm$9IA1(Q32n$dQanb%i@e zb8Vig;?l>}A9vvZcY2cldx5^gcv;{;H|*_!-Mz50AGY_yj(+!d`r6g&KHt#?+xnGV zeU^Pa5qtU~_6|l&^t#DlBanIo(Lrwi-8ZmeWHT$rx3YR{3wQnY>p6|hykxMC`IVgv z*7tFenP8brQep&%9AB)2C?ODF@G%&C6~Y(-P1(5|e36H!z~}o3tYx0oeI1!vpshyL?LJgS_qIoAGDR#AW*69iQGx zPw)Q2I)1bpzI)p#E!$sk>787+{@`I>*RzI3`Lh>%jiDf(_Ivjo7(lhy`gGosUlgG1 zXR04#6~6m$YbVb0PyIN*wsP}fU&|YU1Kd7$J!i_)(4`Xy-x>MkmhG=z@;L&#LZ0)z`4Zq#~3RUop!Hbo0q%5A8KWQ# zZMzKLy6Y!dcl~h7?>2nChYjl!MlP$#b82{hof#*YKWe22PoIeeNo-&{gATahqKgiF z+_h^dU3)F1OD@5iH}BBNttKfw_qhZ&-q_IX|M{O(zwwP@5+k_jh9V^GJ3n`KHVk9? z^m3T3wGQ>HBOQ4Ptm(%kDu{HHMn!Q8ylS@!kK%wf&bOkFJ_WU$yw^t$Fm; z481i&xo+?rP{54L45NZk$yKa7jt8Tb&{~4n!4(?SjQ^pTsYR`}h1h8u>!Sm}){$qP zbP`(~IUc&@xSxX*A=&~P6Vz&yYE{a$NqXuvdZQ}6^*a4gOiyeHqZnV+LEeZdRzaxS z7DbjQiizSl*A1NS40cRFv9(z2ly}2e2xIquv$C>BqX0uJm>7J8g>zVV@=4St4&tNS z{+-)r-mKP0rwQcTr{*eJ1$An+lMFtthI$x)CA&X=bf^Lk3R2vy!*v(W99tMy6%~rZ0g-qcs~+(>>0hPux;i1zvm+t z-_A=G?qhMOPB4r1=6`&21yAh1wrOjf*RFjB@4nzp7KZg1@9Qn4ZG2j zV%5+8gR^gX1O2ORI%K3{>tDWrC|Xt6cE!v-IZ*APjf8V!}AK$-`itiJdWCWX>CP^|PDlQxx`2OLo zeZ z<*pE3^~wtf{N0E-fbr^}6EwKDJY308(!nJWN|s?22|-{)826=SVFL1O47|IxedX0HL%u&nt|M^Q$t$}7+_`u!ds$<{f(6K_rzXnF$9w(j>HUkp zp!EFbWBU6Ki%d?syD43Mc~Oi1`JbcJ>QTQ#kw$Ho=E&CZpJvG-&-0!!N%4+6wdJ?~ z1*Is$;JTjD%GK00mt6A6=9G;z8Du6GP?_!Vy@b;%7DpVclE4Y(0yHY1Mw0V!FO&O! z%HUBF&}lY$j5dWiIx&Tgoam;FbSgPIa-w=dQ^yQqah)5!LOHV35A4UY7Gop4qm;uDKIPtCw7-XHV2C0`@R|ZZ1~7c{o8O{m68b7BOL+qJW>Y5&Uir#b;OgLHnoh1( zyz_ju`pm9$+B<+61KnljzF-xPe(W!}`V}{__*EA=eBv&OXfYjKcw8KjG|6>T5?#6H zDbv8u%Tbwun(-baEM0D_PBDH{d(H8Gxj>QYxHqT>L>7W>Mh6!YbOr31*u`k~BKFDv z5ALY&z`k+T&Rf7iy3inCB8sWgO~p=v>2iIVVyN2$i(V=4F}T8Dcg7wTpBTYl)g3=d zl3iYb>+kR+O;YC}8B@-_G;mPl{m#$wPEh8VDEIw+!1u2?kC(js$A#@@oqHdjx^)kC zY&e-8eY>AcKMl|rPB}l~;+Kr@oEL0nU~otF{Y?+Okd>$3KI`Mxx5CHXaw;GD{0RL6 zI}4vZd)-~E{c1n=-hCm%nFl%T^amLj{JU8l z`=pWO{B+B6xbo~<3fs>)=~J9@($v5EkNf_FzkBes)@xDWlmB%kAH8e~edX^LujS;A zvv%p(+_(Ko?%%zf2M!Ffvr=L#_BgLxVM(~3v%2*ao4ebUujGXHvhcHk=R7(I=LtNlD2l^HaaWnPu@PTkL{58lDjv+kgK z;a|IJ+4S{kk2n5bFCo0C-K*njtH8Tr{MjNdtz`lBW6c8~D98o}}k-D~#JcjgoHt-6EK(7(;< zzE%Fdj~~D0O+5dqcNWLMq7U&eFTRr>ZMmB7>{!X&<2_h}^SZ~mbkXBHf7#a{!=U1 z*kX)FbQt>jgtfmCT(p=NgGsum<3l92DoJR}xExX%ZBh#OzNelHwho&gk63=PKkHkV zLAC*vN{y~^h*dacCA{y0H}Kx~eiPz7$+_5EX46CPQEDJD*q8?Bv%!@Q^)bm@wo{|d zGWIaa0c#mmrKC!qc&@8lK&>SZL#Rrh80H~-=}&Lsve&*A&OMjd+6?J|6L^?3W|~DL ziA-wVHcA7LCDvUx!iJmsTlSN70=hAjcKu1m$H_=#;#?nOLx#^;&EUq3&VJ2}W^bB% z7QW^+)cX4=zwUKNPY;uS^;g7?K8h?@K|If%D6qTbk&aZfB7|)o(xvtm=$JHlr_zb_b>wK9 zY_t3{UsgWFV6+Mtlk07qYMv)8`%-K>7j0ijex$*@=BeJCM3znfuOl;>Ij%E9r<^a+ z7){cC0IIscdtfc5T6djXD4sfaLN9?7^F$aQ8^?5)pp>d))2L%JCabu#5yX*VZORIk zB(YKhGfbQVX%1qyo_n(%YV?D(`_}C4pC+9wDICA#rAgF0ijn-RLG(#52N&L&N}|3xt>Se2yU zNegRCewGE+gRt{(W39)#nc*Nr2X@maUnk3OC?K;I~XG1*AO`wi6$zR<#kEP!| zhcnMER#*4+@8Z&{c5&%d)BpbA2Ttc_KUl`f(+}_XTfXXX)4SL3)(?9HJ}!W;G{S|? z`56~J=O~PWHeF2r{O~F+SoT^Lk=&Sw001BWNkl4(?z*5{Q9 zd|Uvbe?J#2zn=@1AMF*=6~cV4-p8Lk_6gp4@_Xs_zgGCxP`I8;7p~{hg-5pjrZWB~ z=h5@rw-7G+)QtCKI7kv2t%Tw-Q-L(h7^dts^X&UqmuJ+W{));>IH5YU1zkiE2|JF;9 z#EeL-=$Ufos6tmcY&>rRuX)|``QV5CiKu!2=m)7G1fVwAx5@lj%@UnH4FcQ}5q(s= z8pH8?9;25%aW4}=HxsoAlNwVtUIK`2F+}tjL!}-ww`^EwEw_E)|MIerd0s^qO0AVDqVarrRP2ufU!UM6XIKMb=%+Zj-!@<(l{n~@r$XZ39i9Gq6Z#0 zstK<21~oTIzVmZ;^YI(jJgvrwNhlOL9b6rGN{q+jd2q?2vnBy>S7BT1+-e7VlbQ5N zuA#M2F`KXPO&xkojKf%j$hhNVI!kkAl9AlLlk-%`{-kEf)i1u7il1DicrI8f zpi*>-(*0(4L#5O8(vicG3of`I7vh?Flv*n+pa?rL@?Gz)Cz?@dQKGTj_V#II$21lOlL@`5_3*TXimX{^tYi`Qa`$e|t+~r4#v^a!773p_MnJ#mStr29;a``?JTN^Nm3lI`v@lki&^M=xQ)H_j%Dje=i> z(p>0SiuZ~)L+>)^Tfjm37^lnuhS<(R##q1s0xq35$nW-!@vD6iXRwX=jN%)U00_A* z&5EsHRT1$Z>D&US;ZaA35?)pp8A}lY40+Cv*@_iIY(i?*Bhr9KJYs>!q%T4J05u*) zjB5g&67ulrDRRPH(Xp3kH@xSSD|q-9t7rY&CmuVU_ujbTh|l4te;x9UUpkLRA3E!Z z_qBKQm3;0epJspEFX9FRk2S#*4i2y2rXReH$M(Ew7K1EJVz;-n?>^YeAAIKq9^LcC zBfgg1(Qoos2X5zn^;=fdyyi}m_+J`d#2-HLXY8x~zejEdX%WRF|6+&7zPJ4jl}A1@ z<9&u~WiDT5KL5lb{*FQZlM>r0v6YfMj`^D%jQw&qwTDhR;`=ta`&u6U%q_$}@TXlX z3<8nI2X4ED2Y39@tdH~f-WyqT-Pd@-J?}r{cdx(WPF{2O-Ms4O_u_jdGlOcLVN6V0 zic*#4m8}s$K?Svr=Q|2Ks#t0{)9S=u{~B?hIDMyCuQDVcga0*Ma_jmEm~OI;o3=%3 zNg-KsBNbRMAO7r|LoACJWf>1g%E6$YO3+0e#dwZm-XsaG5K9lx7e@@%DtzuwKMrxl zW%6-$Bk5{o zQYSU**h*#i{ByFPv?(MRr`o4`F!hHXBEH>qjhp8^5C5EVjxms6M#2z1svSxx zPH9^&?XclbkK3ab!P*Ydc1Mn*MUcdQ{o_6vn)m`zIG1#?8$G=G(&pu{`EM)I!i^we z3JIvJYfKx##_VjSS#u(zRD8vHZQ7e@E{2i9u?~G@9qGu?aS5jVd>V+U1`fOw$8w<) zE17{h75uRj9JG9LOON{aZ-hoW0j`eBY`ipht#YF|6H83Ddh|?M;?QS|xY*65`Iwq! zJVZuwfeDHbVv#n7@yQaip&#Ol=YUi4&6U{%bOjqG0GdE$zeB^A8pm8|)k~tq9ZFLt z*X#`pckTuQ2D(mJN@b$LNkPcJtp7)T@0Gvr4i)4gF>SPCM~+M4b{5V?e39mx+lgqFy#5EkZ1&VebmWaUIVO@vPFT0ezEo4!48LQxDGyeS?db z+_ekqS=87Zhl^mX5*p*ky*=7j)OO?2RoC20SQ?r3`*Bp_zivO5 zzx=HQ9C$o}|8eB!u=~4)x1Mn-SATL5FaPB&EM5M{EY`Jvt-G$}_HF0zua7UqwuJVR zV0fE(wQaJ*AAIlSTr}@uUU2dzmJDoTPS0ja{vHBvDYExCY~ zpYcOZ8v5ca*11J&s{SV5op=EssjQ^dvam{vJ)i&o|7Y*b!{oTid;iaSPE{|nYnQx8 z#zjhFF zU^(OWUCG{WU&h04ISU=1hYlbwvKvWaMt6B${nZ!pswbYqubumG%F&PJp8x(V((j~< zyz@qMboMWK-W_)oP+9^^Zj*+gaV`-uV{l#kG@U1c(xg;Ca7J)*1$RzTRo`aSilA8? z5y#fg^v`-uR}|57Ph&1#e&wfp@?AHT>aw z{}m&rhohsSCJ zsUbqh>UGoy!fR1a1gONg0?fPQ%$9jy*Fo-udXW3vd}4!#xF%x%^bSU9RetXSzmGpq z$1E*}3Q9xg`Pot!uzpDcqX;Y(mYxyg<>QM?{^*{ouLZ_uR>-tBw}4Y1to`1 zX}}P9AJ%Q*ctgtizGcMe9snwCnw4?F08OStc!Y@mO%%76Ub61M*3 z|I0nM-QDCXK2-VF-|y#NfB!rl_qwxr{8J~`aKQnVE*q!XH%+B7hGM8q4>CE~$FA)o z+Us7BP>IwrV)9q9$!m9FQj^|QFa^)=BH z4n-!^@CSP@<|F$)&I!uhFWN>V50d(Gm~v+^@$loA`spzB8!lwz zn$z0!kjIi zLH3TdOug+)%xm7n@KYaT=z@DFFME)vZx2!B=O{65`b^TvHB4>az{JniGWwM>NdBpM z5Wb8DEUWejQ~$At>HlX9umAbET>aojxa!Pp{L<V*2e7&4&F#w_js|Y7@0jo6X$ z;@{fDNB-=yOi%6yQqUA7%e!DC58vfvwvq&FLmCA-7Vje*mTfyHv8-h`OWBqUu)Z2m ztM8@mJ$2ss&7oSQ)#w!_l^|EP;c0@ zdAiBNRg|sQ88%&+HlIyf+U%5CuI;I-kvs2X^nweDn+2f{bAbS#BBVF9cth=m4ft9O z)8BtYuGa0{i@&!}U=u~i5$BhZS#{9cuDO*DU$wDkkoKe}hal0SysE`qBGM~#?MY91 z%b+Kx^xQXCO3pKq8p_!`4M^H8E+HY<;kCf#rHwcmMjJdi0VCfc#N+!|lk9`31c<>z z5LJ+TzDB&91NrD5w1L^M09a-+BO6!>&8FbNXfLp%OD+?@dp7Qj`)f zkfd|;D9$Zb1#zI1I8d&W5K%Kxv%*#zWG5(29Xp58yn`!WpObL=)Gp5IH&n_Id&`!G z;*69D&Sr>DnM)%#>bJ|oC2ZL83(nYIn)iv%I<#W) z9@Wu$&5l-rK@O>&Dsu^uErEqqYv6Tn zcsaXv!x?9_c;OEQYQM$NePHctSRBJjp3CUf8L=tYlVkdeW1Fmc^OiYFKp^TqO8ZPB^IAVoRp) z!N2sRCq3!ON##m=Rn&z5v`UM!u}Nv8G|}86u|y(7A|M{;GsJ@x!HMF_8V+U8E9?~fnU;&T&{?{0*ob0F~ zod47-*ml!irf(eO|9<@juDWnN#I^}{&#biQJ?TkH27ybt_?#b3Pig&cU;v!guB8?XK>&w21+{_}sniDk9jtT?a_ zv+5jv`J?{;@epJY#4*4An=izu#i=1fAm?C-h)@S53`3Jptp!d$ud{%2%Y19k^7Fn7 zjVsjSl*u?}me9PbGqyOV#MKy?G?efmv5vMutM;TP3)&OC<_!^gLi;;`zYm-?I*v53 z>GbL@=9DwHpV9pLjKcdj$<2jee*i&CIh#=*Q<{#h49t3B(g0iW3h%` zf8lz*^p$V$x;H!3+#HIWymvWAY|rm$Nd|(gniW?aWa$U9qxTY9J?TkLdU7I)p*sXj z@BqPo_gpPNBS~`a^%V2edX>)+{y(#Zj?CASNzM~&6h4Gd)4utKDyrACNJCum+=o74R|;cnLKIDmePc6bv5G97=93BSX7g|H0g~ z102?kv$D@qOD4Ge9ak-Aq|W}6>oBU#UYgK-Z!zBFMG}$+F13k%TO7qxvy0;hgoqkY zc_Ja%VsUanGJ9ET?mKwv&tF zG9^6InqfUedDD7q3>AbvBb*jROr)Og{KMyY%4048ug&_i4$9t>R^%Y{b$D_X?-gq* zYhsJmGcXW*%P0!Jr>01M@)Jsz zUV7*kyQPdElBQBeE&KeEM5MZs7X4zv+q( z{3R1M!o@Kq3A=((z&VUUgoOP>!I+M;qZAk+3*3&qEq>uTT{^nmwr-id`(@K+2kK_5 zoxlp$nk8mIYr6QLT1VwHnmp&(&)`ef|D4yq;Zc~VQ1&)&7E^PW&0<{2Wp-S2p-vwuE2*=GGG{TtL+sIg48{>t=>f_b*zk;o30a8@ zHp>{wl;u9-pFj0!F8v>`VPO5*=Gh&2@bCmM7Zz?x%Zz8Tpc~5{TX86?i2}TBRGJ;K za7(~_eA1N(BJruasBQb|8-3j47`gK&EvOVT_hYadHe3|$iot^#M55+-qb$Ot zmh=|~(8)Tnjq_qjkBX=4W5^tndptf`&O=FzAE+`>UBO2`cO4IY?yKyzYslOL1J>p! z{>Qr(tTjj?LRza6$7LS<OHOEhJ(YbDr{GOl8V5jDcmRv8TF%*!3}OEc+=z+{ZvN$@1E6`Xpv@ za+=34zm%Wt3x2!RjsT2mL3iYuMMJXNXp1Avqiwu@(qWlOTb{Us<*&gEuX)g1B|l!9 z_bn&Mb}T*UuVb^8a_7ut8OE5lV9jEmO4|mvarStti_3^1QpL$ZSf+VVrcsEUP;#aK z^_hc@<;yGf;ipl^*+{I{jA5cKhzhQau*uLZk(C=HA$g4usUS|U#V_vA5wkoz0P2xr ztelWU4cN0e9NQx#tMxBfU3mQi`C+@gl|!&;n_1DWv*+fW%F_mg;DJaIxQZl0$k_1c z-(S$RUhu)cfjADmKh~noflD!B@Ca6d-I(tf7|9C@^1;_`&qR8j&+NOi%y_MNx03fM zH+`l&>(QHD=1`Hq0w<=)r8nQVEF`8g3pUiZ2`Mv3ygmOtb*W5i=H z{Pv5Vzz5#{kH89y)=^3EG&&ni4q4dXvQ23b--hu@h!6%Sv5J@)Kj#)6@Ix#w*BD73 zpv#A(?=qL|h2IQ&E9dVpl z{@BQA8gLodJ~nMupx#Vs73F)^me{m833|R*SSg*p)JYmW>}I2RWuZ8jrAsfRR(S2M zzn=Qz9!L2Z&%g{0F6R8Cz5^Z$c`RIa)TyL|c49g2o6(Q(gjzUEf` z{Hl$;*l|yKauBj86I?2gD%2tvFGEc{qLyyz{?LP%+WfAz4N6+Zb1_I60+=2Zmsiiu zEm@Wk#ZjoXolzavlb)Qw_DCa8k4NzsWAK@41<2H(J~$TY!N#G&tk~&79;_2pR6X>P zTs>J7Zcqp$KlI>E&O2*EUOe3a%_Bt+pY{$9rG*~SI4Us_F1F1M4ADaJSE0{Gl(Fq?yidz!v>sz2L(#~PX7KRfS}yteQrjWhj^}a0#C1 z;+>xr-K-}^Ufp>Oa9yUOYi_Y8xjL_H)#PdHuIxZjG7H8ST-Q5jFMf$Ul$@}nZW)<2 zI0-)nLDUdAA@cC)4}F-CdiXo?L0ve3+t-xY_}(`#RJOFZ+d0x@+bMRKg#TQwPdK8_;_>>p=HEBV%>|uU ztnzm>%lpL#TSKFQ&z?UsAC8k8jei-T1ToXRVH*ebU(q?8Z zczc-V`9u+99^UlEm-4;u?&c{^UET3aiwRcga&jq9u@wzKW&K$~N%r9M5h7}&Itfw% zZGbh01Qyg#Go81gm-4%d@^ZHf!ytyGOmP~w`TJQPmpCgap+?A3M}#1%ST78km;x$w=?1B&xh84re{0L6D>F-n7QUARDpcFl4Tch% zu2i;OJ9EsoZYi;Ufa1DU{;8m`J4O3ihNY^T^^%aOFajRRtEV$mojDaB-7(Na_h z!$p9_YKi`-C^sQvT)w7_Dn^z5MCeZp16j+&)Lx>iC&$~~hGy`g=zeNfS4P_V6pIBD z`P=>HnWZF#*jWbZmc$9pgzr~Xj5pw6GK-mFE#KQWjbIsL7)OL_f&og? zWuFI>=cte148WtQI$}#O)X&qN^-Q*Y`#UUKvxFPI{O>&bjn6%GCe}fD<2^arV$~zY zp~fJ-h=tM!EYs}fY}lnO4y=Zy7=ggUT4q^gsFmTaC$MCQ-TiBb(nla3qOK*jKJp73 z=K^!(GG~UC$#>Xd5N1ZGtZG3pRTp%R)IgP2T+_#xU8Zoc)CLjPJNV=kw`2L&MfNHa7ZY9sFH4 z$q#1UskTs3q(ENay+?3>$CwN;b=GfKSW@dfFZ~k#^q7Zfbk2o@zqW7VIzxLlZ9ZSn zy{(2|X_QQK=<6ONb2l@ollc;{K+4TqX6Gw<(&kB|nb_%Ka&;)9BR%ZKz^}gGBEIsk z-{i?p4hb$XiZN#1JC->NMI-R;cm6IP`rr-V4&X2#{b5Xej9^e=lp2T&Nngzqzj=q3 zNQFd8tXp{=V|#wY&soVYruVQxg-YhAiZr`6MU{wB(#ook$le zN62aNd7!-!WNeaq5*!LGRJ&yN3Vux z8pfu1-OI1!0H?KS6rEvf>I`ipDuoUJDJ00lT=Rr$wYh!k8*5)%m7dKK63IahB?EE! z-?b$PdMhMjCdGOw?^un`D|BL_;#=$d|PMVb((GwZ~Q z$^B!yFFm1yzw7&QBVb!laGjm@_5j#X&R+ka1)cv}yFbSMGy7)Ree>9G+GW5SsYvbE zZ?uCe(bFsEo2j=O4GOL2ITz2?6E`0qut>$s%|~?uhWaQa#>#`53;aMt?VFj!z* zYgRGvk}IFVhd%UAByKxasyJT3%q!>;1&!FWxz5%tGdmHtZZRRrrA3HDbJFIw6v$K=`N@wN zz3?&2=@`u-(Yke%U;JWfx7>p6-kmRj487|vvb*jg`@6pj&*$88(Yx--4~n3`-PQIS z&pnsudC#N#tY;CeTem32>225E%7?Goi1Q9Ha+DQXPkM6V%L$itIT96tisB`7k%+Vc z22Midyls%iHaUjEUXzL7#o!kha55rz9ObA?mbu<+>&ZzWUJPnWBqdq{lPzc6BFS*r|MPP9sjL|4_|L+i{b8trkw21RA zclwo@b7Z9o2a=2ki_L*As--H^NiF1$77@glbaB)X;IuN2*|d?Hes%}1c>n8Q355bh zEjXDcr*utjbzqxFi`K9wM@cLYdrt{aC!uS>Xb2Li#j0R@o-i_+!<6Nf_|mHBcLJF} zT^-JruxTD&DjA>x@o9mJVc4*P)x?x3&9UD#Ld%W}M-~#?+Dcp3BS$TEWX6-)0E^`$ z@Og#(S5YHOmNP6F6_?@DwAG20b7hviC(azIO+dzzt}3*b99}HS|HQiB<KuO;V;0h_YPJ`oY@bz)ETCk7W9yc_SwH)v%~=6D^Z3q5vEk?WFL*j%`9DA4 zRj+wGCX&N8Wkul3weYU@yoUF__usJgbX*pU3=x;F&nR$lX(OZ;IJz3?b+4dUoW?j- zkZ=Z%@DLBtPi!4ECJ_;A4tcEVC>cXpVHm?uR^u}t|2R+i!}kV`ptv-1k}qC|uFyr* z1Zghx<`xSUTQ*o5TI3KMJ*_rwNrWdm3pXoPc+!a#N;%YZ9%RzDzxg}W6LGHnZINu@Ct#!lI8qF zy_V9RoD34`zq7DDq(RDUKmy^l3S{c>VsWYkXT4=DN)8|r5IVFM$n8l_8U}U_1!ORo zob}Owjlrx`x zcKDz5IULOiZD0gykoLD`^M2c3H+&ZraIS^L-!mPK%)D$-tYR#Tl&a|DB;`1C?|box z`Y`tSpeI%WgrTvxe1(P>g>U9!O$1ZM;0FwL45$NBML#sa7%pKM-B`DZiLMmIQelcQ zQnDOyNoiDSAE?%OsDFz8@$TQ_k+L>9d{ljNAB`e|xH{8g4}sR{D|Vid^Jr=)HnAFN zo0}XZJ?Y7*;|vyftY-R&79sQ6%1m_mm^z2WA}V+fq6X_*T>u(Z7Y1odn>5y=3aD3> zOv8Wt>ECkcyI#f5m&!4-i32i;fcMxq?dN$u9}bS_+(&*R$-}0V`JrJHuFyh#XSEh2-6T z8!uT8i~*BC0wjoW;3HHGLX2t|NW!@XP*O6X=b)w7M*6#XF@~&2JUd9ig<-Q#s`X5 z7ZL|e`$$>?uQG?>e(+Zl{G8Ulv*~3CNXpi0`)3@r^;%)m=C&Q9Wf(BO{AJG4khEZsaZ3wqOe1 zy5&~>XcH0tN>_R<;3(Z>-I^%GV5Kn*_25R}MT zGUu=&cx%vdc>zX=>Gh)Y9 zrS%qGPfo<;-3TzNN4(G7QiGa+tFJB#PMR?xf$Up##uC{pshrP6CS5 z8CYej3=dTiLd~>Vz46msuPaMaN~ID>lJNKca2@4=e*SaE4b-URF_8wWpkcD)*!;dXu)8ng z;l4%(S)M5KAy{jTVPrBP_I1iWCCaiUmrQ%2D|CGM#Z8vflb#$6ZBR&F$PAdm(diKK$V%g;ha zFpP^XmiGA*%vgktY;D?HLlGQNUhbjHgzfB>G&}|mMRE*oiMZs_$M7#- z{uZx(^)o`fmN`5ezbJxpHVhJ%_mO4$K-K~kyiYKc^82%dLK;V?un8P=n%gR945otf z18m#7hcj5peLBqA%&|tG<}xA#BcT(#BnGESRXr=D#88rQ!{YBmLJUd? zAWcFtBh6sv&XAFeVpz2T)1N1oEUnhsB4VK)u7RX;7^5*r%%&^rY`wM{WTCKW^K^K& zSX2w$WhYZyOJL~EI~lv+f`Y^7z%IPtQKE=w-8%YS`&zQ+KcDnJ{}aDs2kvWML(Vt@ z-M$^_b<7o4AS+jfqK=C%rgZt`L}#6KLJXI0^Zyp7Gni#rFR|5=<1WWF!6n*J0CE(J z`EDj^6YIRgTz6JTjYZ;!x&z`URJszpkt~NW^xk)`HkieK>b%eS6~1&RFbY*9y(q*f z;dn1-CLgb>%WI;H;&RR`)f~zxH;yAOScSZ#HJI5FLW0_y&N)n|wnKo-zfPomObYAe znwjEu1Nv@upqPk$#X|EyGEt(^KV+KQ}b#A;e~Xtn8&CXM6BP&p*Gy@BGf6^6FPUBNT2Ofv-5{(eSqSyoe9H z@BhQt^YE?$+Mm;G1inC;c5}m;(VQjJfW!g#*bXu3MyRlg2RXnaQpbrfoh0-V<;i$Q zl-ochl(ITjnV2tq_!B(sH-8(y@i8r%bg@g8LRN!&?qk~*{}0#y^{45ds_}3=pp=;fjey`(n@UF?0{?rH`mrJ)qD-Oj^iB*flxhB z{3g?J8>1Ir%*Y*g9Q1lNLqn7v^B77OU4$PSgEzeiw|6g^B$%a3(JaH1%a|1_kT_nn zc8LHV-n@}FZ@IPLPte7=TwKpTilQEWbWe`|JUsquxWH@Bp}n+OV5e<_k)3fK-xd6? z9SK{S3ripDk1@P zM70Uu)v1xg_9Ter9lJ%3D34C{Cx*Vn68Vt$4P157?3}13mTF?@uSZy?0XEEMf$I*B z;;2tf(jUi+3=T6nSqII7*!ea2JlSP6CsWSv&(I3EDIH6uutBNJzCY<9JeoO=u|NXV zp)$3xhxyG{yd1x}AL^hLKW}nN=|bX8t1;@hC%Fa2_fLfRmJh5c82X7-q(2V#kVCjS z@AIF)i6jDPy+%1Mv7~>1$izL}c7(+i!VcmCWtJAg5RFM@^EhZ7*k^Ao zPdSPhs2cX~hldAXg$yv145AJ)=MbL;a57UuY^iIBH`0d|*Dj$6R^{9IoBNe=+cum`iRq~3L{JlH@B8_6Xm`wV> zE@#rM;VyP@X;Nj(mWZea<}TG{cbp}3x(`JWE2D^&Ny<0=@pC-;;tTV8oo})oo0-&# zvlRwWH9*|e>-^UzKh8Hl`IoF5o8Yvm8iPs70LoO7kyPPrFT0XIzV2Gi|JCOOcC6I>b~cybssBy@B%Q&IU| z?ZxN8^Fn=80pcNWk2C^@Ehj??39LrfiKw69^frUB zM@)N#cBc)a1!L4`t8%Ydb5rct(w3vq48=BYur($4#f$fpg}!RWT|H&%|APX=_8ugI-r|L|(lsGP->haTc2I3M!eO1c#`0DJzrL$Ke_sPuUT-TzP{^3@T zvpAMVU~_NqL8$7fS!tKwZru3O6e_`sRY{TJ2NX;i2(-aWlF3`t?3RDlN zLyRRhHo#w}f|01RGOJ||2GcXokNb||rA9e%_-r+W!3ocFCE_7!MAm?Bm3e8wT+ohC zVk2N%9EOH%X->8Tm$uLviu$ZAVIYUL|A?9Wz-~5(YR@oPjCMrRmTs)Gj>o!cd=>i! zA7aYYXasbNC6*1YOYLYH@gg1EEpr3=HRj}+)oW(X;l^birI>wl91M(4j$?9OTr|Ly zbzqJI^Fs}OUw+uM)O`NiA!No7`v65}ND=Dv>kfwf4tB`P>E{9Jtij2doaX=ds6YMo z_(1Dt#z@ONXDUpc;&U^{U=HVb*RFF-7vlfPv0itr0<`aKncYy^w0SJI7-Iw>GI3Lp zi!9(Cj#(m4W?X_;c-{-1#uu-@mDj&+WB6y9JzWH>}hc#@0;={}--C4i31lbQqZHBsp)q zC>0gzS&ab$!&!~L{nJ0<*?;&xh|8@qLOH&AJoK&=isY4Mg6jwW`y<@&XMe^TuPpNc z_FcqK)xcXi;_D2JAK;ZQ`gQ*Ezy!o*md;C=_qop!<;+8^Qt0(-KjRFW!B{qJo^Ez- z6=mzSAzOO+OH-m8x?DRtx#Wb61S%tU+{)-h8=LQpUU~^5ciwqiugZ^&1#Y^*LHzi5 zGii2AMx-n6XK1=;It7bN;yA`R*E2GEa$M!OB)D3tw-u&tzEWwcK+=x>^JZU@w#WXp zLm@O?n;GV8xAeavZJ{J-+ZztH5ma-iwq}q)i($l$^J#nSiHgiDy)6cCrB(4q!?1LVVYJKPj2Rs0#iUODpSW$twDbRLsCZT zG?QC{Q!toRz!)CEqc+A1q~b}8$Ep$$vDZ2#hDuCkg7pSdDv@|kY9mtdOvNx|J-g!2 z)e#a>txjR`RP#hsVM1e8B~{KGHw+R}K}fOGF+>PaaXxE;lHk>$-ZkfR#KS~xUr6G4 z%g-pWcW4c2DtHmBhX^4vkQrgPo-sU~h0d{zF3?G{YLXcNk|PC%V2TF%iUFSdyes(G z4{zn>J9lu|)Z;nt+2;n|e4a0{z+hjQ3N0v)xLP3&F8pFv*KOl)q|duxhs}HX`udoj zo@QWR;FR4;&FnGH=BAqW8i&NH#CU87fP_p(>*J=qpt<=qUJ#8!ag_B1YtFQS*ZE>I z5{kYW`)JcPlvZa!*$LPD*(Z4Z`(K9{uvlyGsqY%h>sC43fKxNwXd$$_Lw#a}NLW}$ z!(k*0obz)m$C*WNJ*bvNss|K0iv+uU%&3Gyt}d8WDmgDk19E$|s1$Q29hzN-I#bm* zlHflv&9;Q$4qK|v%;~otA@#p=B(F@LR zJ7*`a){%=uX!BQ#HBIMzX0r?>GQ09UDp%h&S7iRenMT~;_$@mRBy8O>H`i8?4YPQ; z=J26Pj?Tl7_OV^FCNNp-+773mmWSK?G$dKbfNkHw*S~l@XKcKfi=O+OHeS}wdk|~D z1l#s~?|lOw_`o;uemO9Op^D&|Jl8b%w$R)wD&|&VZ zc9OcQQFAHP28Dox^kPMoe&<+*usV*>d+$XyY=9_67J2zVNY6aNcB% zArY#WPz(n|IX73o%UEgPfWX*&_c8S7a|rwjGS7sPgSa}Gy+}}JIYujNx>A7}Td$p& z==%O=VxDm2bcl+IZ5BbCaF#~rmfB4@9joenRWwb(IoyE*WVhW$cKhv6D#6qg?v`6H z8#iKxhp`u2K(u~6cI8T>QaP&1~tY_gV$p>0X46kPao;xji+`*K~GeHgoatYgmi$N!q5vL$_KhxRM7m zu()RT*33rZMs=1K!RKA!?bWv0g2yP3=l0}ObJ!1ew+n{YKw?-u88e+I)3Fb8t=So= z`P@@V$cyFg{|)45T&>+lUDqKkC`DI^vvnznUlCRblD% z_pzcLGcqnjQpIW+97M)qyuo6~tm17rR=mfUF@#Zuqwg_jw=q%a=RjO$_kaH#u3xu- zv{Yphi}4U)NJ-HMHtd?_2Y>!|3{IsCJAYit9%cj2@K&)g)Jw{3|9&fH4P3<6Kl3%t zd)B%0Dh5lS_N$n7dV`!Xql1LEC*8ypNY8s%>0P(M<_klB1Ppqngp(-PVfkm%ya+ut z4P9jLV#AK;8v!bd&yu`TtkIDbffh_~pMki1e>H^wA8k;g& zp)&*1&7gTP(IYJo*GA!#FWAcd1fC9!F>umCQh@HAHitAdM4BPjt|2g0dXa|{$XqV) zRw{7c;5OEepWXc2vhh{ytZv6QS4j7XAt**Y)(30Y6XxE0W_epCxMxWsYk* zv`fjt2uO}3p&|Jf<*g{009ryQ(;q7?-Hx0;&J?S2OKzq#5?=S*^-&)0o_&NZl%5=I z8?dqFwq84X*lE+|1B42vQdfA-MY%0$D>>d^!4ehX7=G;q8~N%NzsW0K^{lWY#`eIj zhnV{ESGe}?|CZXs7}pugr*Ho;eP^DDS~Gihnh%93FL~Kh`QRU2hf7Wa>L@irBFp9$ zaAW^TP^kl@knqY479|2R!2Yqlz!D}{&i%%&BuQi(!WxKW6WP;yZ9u+o%R zrpmWJ{{^1@#y4UH2O&DXKv0Gd4{JcfHcRyCDR2I*mvJGk3DBhKP&~13 zh#G;<9RL6z07*naR1vX!fWUxjj zbWo)oaZ4Dv^A1KYxTs(o7oa_l)-^bX-?NA0w%f@5{oiq4{c6|GxjXK_=ar5|;3mY|e6`bZWbUoK39P{HqRng7cIKDD=e3y-Q_&{BOrz2c6eoEatKppG zM@vuRol;(d0-1rtwtjNI@{p~qzQppG4}OfqYT%2Z1^o%3OXc*nXRu=Va&EchM|onp z1r>-fcsxs{h2bf{nS!&@b0~Xq7`}iAlfc(@Zx8cc6sO=-aMoZdW$OEs0l$QweBo9e z_lq)>atU<}5DUxlN#Vb(dnUK6PB72Ah=~DsIIib>@)96)vW$V@0eAi4tT+$qd-#gN*lCQkw_4eSnxXsu=G{MA*A}0DX8leRiA?EwR!;+4m9qYHquS z4lIk1pg1eUKE-N+l>}>JM$80Dd$15Y=uEx@56+mD zQ`>0+nBErHlOq^tXzYKM07Gbfz8~go6@oOAOE!1b6M>3RgamX9cm$)|BTGP*L7Jq3yC?( zEWeMjJ8>{-ah@A$m3V3sj3gwvkQ60BOrRfFan3ahI^}(Le;V%s`1;M}y9J+K=7mh` z$?;!ZTer+Ut2b?)40|J`kcf~IB)QdPg-D~4u5LJU0RHuV{Q<8yYaPjb-{V_<|3{QY z$LOD&W|~$YOXDe|>WM@U)g0JRDXU}1SVp|(x{rK>r@!G1 zb8^uiQ{|=YFy3Ar>Cd-eox?^32W094JUa^BV{`P3stN;M84~5{kNj_*`{EZeaN&82 zXnt8Rc_Itgx~08asPTH!mA;)!dBMBSq6lTpi5orom0kaOWZA;6{lN!GKlw@0Pkxf>)mIZ=bP=Y%{}_*6&*AM!PrAu*gD4b0jG`_hwk0y3 z<()_&n^Vg1&8-PZp$y-u&<4~54hds~G|jMLTLG>DHo9-TqZXqvMg{d*8|ix&FZQsM zhLK?3N5&$)om0#=cmaSc-<2(dU?{CluY@{cV#?JLPFqloA$1^a+%v`YXLG!1)p~MF zod%VV)XIRb|KOXvY{P$}nhBLu4*9J83B(SZ5xi)jqBKn2)F{iwgwy)V>>7`-UXV_> zT`|NiRDKyB){}=q2Uu@g?#ZFa1hBi(Pd$Q~C3L?Uix(jS2C8Lb+_E}d$%fHBtP0lW zoLt5-RQK$EWEfpuV^^h*Jp&oLhqTGXXw?v785k@xUM`1(ld(AuB&3m0H;Rjd%qTXt z)QlmMP?_9FzKAMfgv1N=SV&Z`^kY&(DHTi}-1d3Ff^YOSOIR&dCn(DV#vH&fj`1nU zv_e!uvoc04;vLgdlW27?T%Xu>E+H&NeGBA8=jV_-n$ik;8ITmtUe5Dg@?0ML)&pE| z`IDLS2Oxz^gOd@pRfo2^KpTPjVmwhB`=oN%_jtIRISr00fGo?f)*kfn$HW#p#O|$w z9iREW<4B+Ha5vUsN4ye&%jhSfFH4Cl71W3BpbSrxN#M`}Oa__ONy`qa`zg77Y` z!`Xk~s8T0p)#bm-bpIvbW7NlJo_NP7Q6#tlAt50nqedOEiWuMIlgeyJtcwWN`JiV| zRHPNa5yd*Cq(ZDh3DaC3TI8c|+K{x8Ze?(?{1c6%n?NvykqNib98N8IY}e2B{K+}+PQ@muQOiI_|8L?!aX~A zz75`{;C&Jb&qgSx*zL3#(8!nDJG5?NK!^7CSYm`2KsRTnm}d0nZri5pro|9 zFPz08Mu(UNAF={ zI?hxaBWc1&WD%!C3gR6RN*Rb;!f8>utZerbp}N#X_ixc*cQEr9MAhQfJZe#1U7A+>I(0<^;*NG%?g>% zBfHv02ea5?S*;2!fo`@^mO!Q2br(OR2jm>tgAY>w<~K<{`q4wbfhdA3qxJ`XfP4AN zss6@qAV;99u{fozz@J~Wk+)oPYxDo1i|gXvM$?m?%(gU-$Cp|{K)&LYehc;5G-WG! zf-4WGJBYnOQ%J36vJy5UtB@H_O;adEIAf@$hDxGDUWk1|vA4RKtP?7kVac@R^f7G} ze$A;`4t+!DOC9~{>GKA$7Uz94G-t$wV1z35k}zG)!+sqYi>g9Wb?lkmPo|#1#G{gd zWOyRHD3Nzq^^~0tNwUHQ-;_Y>+fAe5 zXOjp95@Db&4Au?S3&s`CD@`tsW}$ln0w#aY^kO|dIYdc;si+({uwuxNkbpu0M2L+U z6ey(?My3X^-eB|UMG>KzLSJnV^)MAjj8;5*`=v<^9;pzkC9N7JGD{65vjx~!6QC`e z4P9J`HKeA&yQnn50P~M$#*i6Ha1(>34qcC?`ME~0)v3qoB$1z@!VW6zL^Q!j0F zW0~;7?AJlYSRee)bq{(KHKk#w|@ImIO0u^ykpJzwXz z97;k`MDX5|BuOAdmPXPdtV^A$E+N2}Nk|AZAS5K!B;C4GW=6z0`~7huA|vzWt-4dGrhBwnU0-r=b$_zbeQV&~Lq0!@qEJ<7_|xAom%84t$Vrjlf2^6Ja_lp1=fc_CKl=S8 zYVedKNyySJtwsWg4FL2RYv+2r2V-(Vb%`U29hYA6%4L1}J9qp6N`oAOlL(`ZqQsRx z>WV(kB>X)VxD|k~jdr|Sp669^(o*2o?mcrrs~Wo?GpSIgnLC9TKPrBpT=}bazMEU# z^#?q@qg>Ewb77tlOBYK@UYxP>K<+)8jpdX7-+$*vU%Usdxq1NH8aRD4REuAF=~j$+ z1a*f|9YbXdkw$Hu*zl;hlyia(mliS~F@_O3`3(0>dm8NGet(!lCdP`R?Hu9_iC8?2 z7)XrLP|qd|XF1QmeBv+o;m2GJQ4%!8PNKMpLm9d7e17rQe~r()^=)*roF+oohkJ3V zC?0DJu|n!HCXHdz<^0XZ{*<41%1$oB0p zGlRMITFhBz;lA@7Xt#0y@DI@KRux}*@4cjNcmvS|7mz&R31IEI?JIiRz4DE=(N4d1 zLIAE)y?5l2?I(W4p+!H12GdC+&V5&H0RZmtjmT6Qt5I z@=T8^AN%^>lQ*GV#81oZI?o_!1dnm&gs=;P)P_rI!xuHe|Cx>b?(}FnY1s7L2j)bnlkj~H3XlPfoiKF6sBcOB9%T@9mFy|XjC{2S?)$39fHE8188*t zzKsQP$_LQ^yoHww>B>`i*fx1^bd>wdCA24nb|(u-6ZjAarwE9XW7UO#QxAGx0gt&J z7jwD_1lW+>%UNR_aWmZi`LA+8=R9ut^cQ&MCC{bNXoU0BKa$a-k%2g=QvzEGCyFBCIL4Slb-@UzDJl$eRC02O*(5QY>1i&SohP45$*=`=phG3X zizSK@@^R0%cRa#;Q#gBWhFzV*%%7E#?(Z-^5_4D_TQA?rfl-Hb9_u_wu8d%C?ErA6 zdCvUdc@7;s#6#JGIG-c72UyQFl=g^aq1`4M^`yBERi7#t9GQ4BJV~cZ+HKQyIf+a0 zrOuXAoC45GDO>Y+oHXY{ap{rwNNj0m>0;BWsJ>d!ZI<9*MFpu#?wj=rt}{j7uJq50 zR0QjALhFTBHeZgt2QH?7ug-Y&g~*SI9Cf!r}>6GpR?KmNUU^R%CTFjY$~(_6z?=Vx#KCGz!%!hhvzJq0xlYD$P2=BOcTscp?w63XP61(FvLJ zC2iW$m$*12ugly%+DXpDyvKP1d5$=E`wo9IMWGGj02P-J*WEDLs$ zEPsTiwOFqqK}0?HA}Ay|-8f~z=gevrvOf_P5=SR?hy({)i@$Yz*J3WI;jgvH*wf&{$%nU5=(#K7^8P4aZx;4$Q zg@uJR&(m(VOPbb#>nn(+mw=M@o;=SN0YWQnqXWF~)>;l8JV=rx%iC{NvDVURwW^A= z70m~cimGwbrcK0gJm7P#OrOXYLmbCMQPh_$zT8&-Oj!-4LryCtM==--yMPOJY$YC; z$0@J?+y_hpcO^{@b{iz^QS9slU;SK*uG!4Y#5fNnG1olndLFvPG2(MZG9k~DJPUSe zr_F7E9)Vu-v&@wi$gpi?F9I>ZPgQ{%ks)vx3;ul~bO4St$=BFCa9 z414niS$tla7z*FiKO-TG+p)Hgh%_a|JIsf$M zf6fbE_6l@lbj`NUn)jGROg5J`2DhRJ`{O@Oyn8p%WtU;kJ{xKFSIJ2$P$Zhs zQOv~`6TkfBbf5Gjx*z!n{-6Ij*aZ9bkzRDs3IQ%XdbCSDNVG^_`jsF28gF~ab;~q- zHcq;Yc4oB`b1D_*tN;_Wrv{g(1KJANxaUx$l?F=ndc~(tA!=LDP^l6X4u3B|Ltp|d z0-?RlUKTBX>4O1uAc)o6AWNSzR;pm@ih~r3|Q4CLrvJZ_H~wi0J(hK4<3IyU@P{mg0d3w@#5ulefIWl<*FqQDY9WMvWb_q=MK)`ds`C1FX%`sxh)L<-R$s>sA66KCjZ)h$k&B!3RqY)W=o?^SubREQ4OlHs`z+!z)GqNnC z8BuH)>ttA;g$fDLN|+fuS=wd9CyZ;C$aYBMIl6I7mNe*)?hKvcQ2>oAzYF7)IoLnbJ6}`_MKA^NIEgqgzd*}cOqTbbdKz?Xy!UjwU9v1&e&|pWR1=J( zSh|dNmGhr^0S6yGLVmc-3x49K+5i0qIe72@ZQmvDI7SOj9hzNB zD~+%&!gw2!uDl^hM~M)|7-v*RNz(-y1ucsTSOjAfH71yq66iiFXIKQUzNls`&P0M9 z;BW;SDFF7R=};a`fNDL%u63=_`u%y;<(glM`%8~~#+@m4xfXQmfgV&;0VCw9UtFdMh`) z`ZXcTycUXBw5`gcFb<1idBWz4FJaHGzmiYAJ8E8IsH$6&NDT(uSh>x+{s zSnHY%>mXGimtKIR4nmc)dU1e4Lg-yX zl}ad4_f>xcLm>RWF;Y0cJ!23lb#V;q!Hj%t(ebstz8=@JT3wp;xW?6jW98?Zb5-?< zY6(_`4S#lQ67O>)7a|iV1D)oGCP;3GTnH|SI9%lDW+_o(5F>b}jBtd)I88EM4Cu0tbk&@eGxB0Mn`#i}DR3Em0~2a$_#0!{)gZiHZR zuzAAfPRx$>I2XzUmt1%`J7yDh&Nj%c!x@h?9gNP?^oJNFCrTp{x1ERX*ulK>v>7Ll zCOPl2A7XO+TB0Q7&Ztci#Uv(1#U>DTXeI^O)SR<^28Jf8Tw4 z@2&_p#$4!k51gG?m#ESBM^S(6sw$+0CHJ%c(KeT zo(E!&i-nkR4#zoX|Hy8#R*QuwVze{LSthuvku!*oh)e_y;y5DG28~>J()K5D@W6f` zCL^!tSHn6k&cs4*LTc4Y;RZ~#7L@+P*fYuJ6;?%JvuxVBN8?O9bqv)|AuNR-R%PWq zO1G``ha8_V3>0H#AgQeMKp{8b;bdj{ZYO731QW06+rD%Ahfy*dK_jr&o;DDY zxm0Tmgu0yXJ%#*qBaB{n9xr~y%lOx~yp3&HmkA(MPprjUS4fPZiKnHWZE=ICEaP83 z_A#FGTd!T#Pl~|ryy<1U<_-Uh^G8r^f{X?$MWL70YJe^w%ess(l~n3Y&=H6xnekis zKHJ!zIl9KuRVCHD$EKtqUe`b|-8S3fi1S1%8}Xw*{WHA( zcW+`W%h(o0WO-ioN*D_z!O+0dP|qZmv#dq${XTN}6<};H9><9&-!4;>YbnJA6HQ2L z4ZhG*Ulim}_U&Eli`~38VfUUkxlamz@8n7fkfmq&;-_NZ&wu`!<2qU+=@pc>zvMc8 z{kvwc=wgbb zEXy!Ph?551`$BWb_w8mzATmJ%tkla0*)JiuFA@Th>(%PiTB`+zI-Sm%Dacf{(zday z5H~F;(K?+DS(dFCC|E7UTlu@j7>qGQQ2=kGU5tYF5$a?7f*~0CUB!}%0zBAoMd4jd-;#pzV&;N*FSi9BH}ix@rF;Nn9qF5W-NBVW9e zxE;Zq;>CiENaXvN>@K#%-=fLaak7ODTj-|e(zZWJ+X?NLk8t+Ib-5-m~(XyXW@z z;A!D$6$xo!B%E=mcrqWDOjLbQcfQ1IG4r4k1>K3MMF&m|oww8Zq%3(fOmz-*7rpcP zJ@Yk-vi|V`>C#hwmGs4DqJ$1YI?|%!Jl=Q~a>qzPE>P-MCnh5Co;X*|iemoi_uj*E ze&&Vfb&st@on(#l24P=oPMY<$I4)Tx z#RtK#-Fp_;w|B9w?!LV(cJFcY2OBpUu6Ye6*o}chtWV!Nb$N35PhI#W1l)x0yM<|ZV(#IVTzzyk^UHfUNym?*FgQy zjU7=zHRdG*1ya}Q~ zZ!gdm1}zo!%6?H6+H2F;an9kKtMqXOX|k3RTr0t-Mx(JRP^xbuk0Kh42Bub7vyxWV z$64#Oz+%mxwFW6GfUB5SUpR0#=Wd$BmDNZn-WptFu}zO#nB|`F8RDG=??AmE6e`PPIX+zk(5q%X9K5VS$)d6qDzQvvsGns9UHnLKPwmu?bnb z5fOP`T88u`r-&G;GUbg>@5VYlfh$H-F=cjn-JNQcaJrA}ll7@beaG}wR|0VN?SEWol`ar52OLD0Ewq7}WHN*Ion0)HDqHH!xF# z0nR=TUj1MH6L0$M&w@?@TTn*ni4PR|#Kkp*TJzG5HUaF?1o9YRGs+GQavu-TV#Hb| z@{|TbA_k{EfN}y2@0k+ASeo&n-+wR9c>U`^;*)1qmbB9%;61mJMGZMQAaBS(pG+~>J74Wh zUNsbiOR4iI6-}Y|p7U2#!W0@P)(EP;cd2Do&MKU>r8Ih0JND}_=Jjh@$#v9!+k3w* z`Xu#hDTDnh?GNugS(f3w@2l)tMdva%H(zOPEkp_F%v?o(5}ZA$0Gu*ob-JB!;ghez zI>qLmq}9SEp(HXB-4I7JVVh3X4w6#r||jP|DB6>UAdV0 zMKz@88#$xw_2Z$hx6xM6|FYOev}LV9eJXpw6czbt6pZUgA~JlXG$TI8_!y%E%`GP= zE@)s8v`MUl%CIQZiBRg(Fkp3V%!{M1nO-n3&>L^G)4Mfun7#9U_P+H!{McJw%6tL~ zYlyj)e%U?acXHwEC4H|MwWtP)Y~5Ib0DSk3-$MQQRoFCQf!G|Psb6C1iqBQ=d+^Se zllvzI$pIBaqk?lH#gTLH_FrS$mG7zEf8_S(Vwgl|p;A`Kxw3|~peghl18|~j^wNA( z&%O^0^P5LNiv%mP)Td9Eo+vkn2zKq_$6xUZK7Z3qTxcK?(71uiQk*L4J(01D7(>T9 znrX@=a+>Wq$T|?kh>aPB&}+f*Xd<}&X_o_kg0b81u8WYNl7rf6ji^S&8tKrV+GaqS zU?xc16qin1&4V*v;(I#ACcD69jxdVQ5F4u1Q4+6=2^?8yvke%{Q@VHELF4krfC<=v zjZUIrjT4@C6#m8szs5UWa$Sf`c!x1s<19b< z{N?pxNEsI2U)GE&^uxXPYtqR+nOl%#kyZJK!+gsVi4_dq*RkTRB7yNM0)|!~oT%VU z-~ZPh7O5aDML>E^zX*smK@hSC^m%cM0Isq*3BkQVo22T%umS)jZpEZ&iY-JxgHuxg zZA%Pz8s((Tpc6D_yVywa&ecM+4lzn<9Famp3lVQEt6mf}ipg?fV=zJZp{BD$fs`$O zf-N=4Ry+CT=jYdD{7Wjc3Vd9~eU;-{UVFRUCeQOVFPbz>D{yP2i#abqq&SX=qG(OP zhl(&ZK2gp@VyB!lK1q%il~1Mf9RMy@aQ-*ONBFIG|2Dbzm^dPCG!ScWF6ehO)e>c% z-@Nfx7=Jh)>Rgy2vMmJ4K<_;V4<2N0ZjQ;xNdPkI*?i7cW*(m9t6%(g$RXBVCZ!-W zk_Ja+XP6wH3K~kj5Xw}~%)$(6k(fVI1Jo7zQA405ESvXpw*Q#O=d1Wp9>$DAfN8wXLoV#^!YHq9o!g>CRxJ~ zcMaW#MoI5;jCIF>E-GEDNkQ9SBAf#?2`UY62GIyxWFjju5aA_IX^KYpVvl#lNuy+> zD#T1N&n}Fc#p!l3w$Wh|kOVwO9F9PZ=A~C187iuDkwz8_$@Lm2@F)Slqe@dQGybsr z`65F_0Ank#YZBk^bhDHlQ#%Lznwr6`XBNP<0b-n>jYfL*V7`PKE%a>3D2I_4Mo2Ik zG%F$>Dy+J$i+ioy z-pl;$9}IR(A3B&~SsdZ1Br#Q-=BW1I>0zz)vSCI>`42bV$S2?Nb{@_%rs9~nZkNf(^ln)_iLu~1 zOxlD^PWj6Zyq~B4>Ps;fT!h+qNqwSFxb3;%f>CVa5Sl$qY*R8f2aFTw2g_=$I8;YU zj;e#2mNy!KZ;~_4OqOCfi@VZk&TA;=A#|LhDYo!4JTXE8XnD^Tgso!u?8iRNbARJf zbfiHDCDtbXiJt$)u=$e9_`hHNGCuRRn^+JS!_ak(Shb#(ORSLKX?jn~Id1;+-|*vC zUK6_V7QE_(mIy@sc}I3uc(ljWpcQd76x#`INJRZmS{ zbrn;$ezOdA9GlTYg#7DY?|UkK_Omf#W9vH4^x=nl_UBi=f~>>$OCQy9sMN}I_*BfD zC$Vu63E`ymmDfW;U@)$|((f!~FiyAFUD+3_xwm0};b6S|MBJBijx5W_^L$136{+gl zs!i7UW=dMXJa=dus@WD2>6zQ)&qFqoR<)Sd zGn=}sMhQ@E&tNGD`6BuyM_uul0~Vqw+5%k>Qd62O3F?Y!jjr;YyT8YUkBl6}5f8L) zWtIa9V~pXB1kl(;Ii@Sbd4zO?NDanOIWh)A-pv?e6gQjq#;W*{Njm|)sMIO~^j)YP zI%G|XG5E?D)u1ON^yqLDoEqYSUM(u})T;Gw1Weqc314Mq!$v#tM!W#s+!I?vLa)#$ zq0~DpKusLE38WyRLFYz+Z*1AYC2xKY3tznlIRcx$x&J`Be z71a@o{4F|x8jS`>jz~_Vb0qpOlUM$azW25+|0ECJelebeUY))vY%)Em=XQUH;q=u) z(ILqMyDQzTVu|#1hMdM*QW28&p6JrcSg?laj*_+vZIqVxG^Jn+)UZw_iAYtM^e~#H zeELuJ^8DAlF2tj}5*g|1yQ(YIaKT0J+SmO*yzzDafX@#jGJX|?|OCvt^_V@7YpSl6_ z*vs(_jMaVzL{9K3%sv0t4L9(y-+LPmXI(Z~OUF4H2-aGhI*bUh2u;A*h{@FRM{oRH z{?DKJS>mg%IJ&)f0`JfC?KT1QxAjZXgxz~G@EQ9)+#mM|;Fe#!@1HWjykDL_sEXOV zx&OIFW8JE^!a~jQ-~Ddy-`y^{u&^GxYY_*OYP7AuTlQSXYxaI^QJhRG#)ngFEFV3) zzK&0u;|SK;0C1`5sw;BpH@;SiHs>7OZWr&1>WVl)ZNx)I9$}%?K(*99a2OnlMX~%~ zA?Au_Jf22wXyhR~rrBx^SquS$=)~}Izw`?%#b=rurHf;Hd<<2OHT~d;6U7@X0uZ`L z%ScVe^vod=+hn9M#>m(tP6MB^ND*9%<11UX~52+mG(q1$JHlwA-ti52n zbZpFmwLKrgg>zgycn4x*Vt1?!J%r-bm$aV6TG8+wv5%2lXhuyW*P6_8AVuhLhZpxy zP#=ahT!XMg@J=y>{;);?U>Mvzl46e3Vc@ssbn%6jg_8LkhvM=cb*G^H^6FAwt4qv(p%8m_i~O^}(@?2V6|QG6 zgkEBq6h2De?**b1ub#WUb_Z8H?NYed634bjqqxyF+M^n|gIwVLg*oOD%UopeSbR{? zKq(~J9UPd=NZK*m?sYspvcwVOSqSDv9HN;t$U0pvzjr6e!3CzW38FSM)!?1)>C9$9 zFLH3cAFaT0fWT-4>qRQf+1{c*JtB`o`%5{&lyb)>+Ta)cM!gq~sYtwTeXX_7iq!i2 zc>jL?7fOE+_*k0D%V>9x-^oQannKZVGD}4&-+ywJTCy@^Cf3)h^sQK3GL z)@6N4t3ZMdSlEj2WPS2cKV9l?+1R5l2QfHF$QWDln(EeyS4B0)(_J!*Ys4ZF(g{?l z0(r|BRZ(5CAF5uLfq&Ph(^1xKeY@1!71b-a^r*Yi;(L%~tdXH`nJ#@!ENA?e{?yT4 zcijA;{(#=@J#B`UPoAD7Z8GecqybsROaAk(^RG9(mBV??CZOY$Q6p8FSuBPGPnJYmx}%^^ZU)j&wjiACmB0Du zM|j>sl%^CK$`X=h8zL-pJ4{*2PKo&2KmRz- zf9-Eo2iZv(I?!6=Lu4_qt|2XY_lt7&eYh9<*|%4?{zV<6t|_f@l=V8W4|rW)kZ5SU zFO197*T2d1W3TDGI6nuYqlMhX(FaAHLmG|#6Xo1Wt-bHR{(7X< zTIcqPe^u_>qsYQ%I zOcW0PA#=q^sP?J?esu#iB$8uA3!Rl>4=?ny^4vqq&=D;tXtdBms-H889%+$!7@yos zy9liIM3;l1GM;8^A5Vx#7F9q?pJ^YeZ&oS4b$krEbvor&`aQ$mzaH0eGG+Rf$5Y+K zU2H@LuH`Zy6&3Grl97wx#Z(TRF`$l0-vc$vaC%GgdoX~mh(s8Tuo97HDHe+rQwwD1 zfb?)r`%cSLJ`KXRf`)Km@iX2qG%%29|I9A~Rj!KBxxO&LpnWDM=~$oa>+@uGBE|HN zuK;6_gseOOXRy8(u&Ffd5@1x>Kh|W&wibr{sC9_3!S9QT3dTl@JWf#4A*e*m5pjeO zzQcs>W5G8`^ei-iBUwU&+i7tJ!Yr0~X1S77A4e;`2i$FVES3h-JcSvq$1p-lOp_R) zgTtXj#DM1r2_xj>1*mPYq>Kr~8nVSj!?{qc9l%+@3R#w8#8ed~qEd_f zva|7YMzhD!%nVUC1gLT`fofG9*FW_seEF6y*9ysc5Of(dE&mz}>O;bYFEw1HcJ4}{ zT}O?}b!domIU&_k>#QjE$ic1SnA^y4z4f|rvN z*X{45OHu?bJ*WX1_y3^n-~ae)xu;<4@o9UN(2zoNJ)rK`~e>kh@S{B|=LL zBP#5O4F^&Oci%1(+8;F$saH1Pndr{&fp`5b&wKN0Ky0BdTsw$Y z2+`LA#95ZDsv@F3CSJgvcRs)Ho3H0nzw>5h(+&+WI4!&@6{JY4kF237o^dg>vNrE} z(;ImHt6v3?#Te;NMmR>cOu6lrdN}Lln-2?GS8+_$;_Rx~1?3qt%vqTY^Wyp>l=tB>M?>uy9 zNpP#)zsngfou4QF%2)cIJ8*z((X0t6mJS`9#$_3i4IEEp ztXX|19&yfLt*wBVmCU!2(W%?*9+L%ZdU~2ehYrzfHhWA^wN^5Bt!4g~tXnIUgeh6; zO7LZ5WMpmo3slubC%bgXnbxAqFTZ?U2Bp+4TJ7dt=X*TnY8O)+)}j3R5Rgt-G7}z( zIkq_`wM_Dwvtfiq(1HPsF9Yd;BC1>ciuPWwooaK5MZD5Tz{@~>pM;I4>9hhfj=8)} zFPm_qEjy4ei~jV4LA7$If(dI2Az571XmT(rO3W*c@bkcSA6X3&Va z_J^Oq%xyC~CccWP7oOX{1)MC5CmZdIY97d};RvvQWR!N?WX?u-QJg5TH^k1s5!kA5 z!NVgw@s36)>xwhJ_rR&kX4P&)tCJy-~R6J zA+*RfK_x~R<$Jfkhm3@r1eH=DR||?Z6TJj`JqM)%#a9N1>f-;a?bm}zv`1gT^nAO4Ij4Ot*^tRb4Om%^9sej3 zN1YFv=PV9%?tXE%7tvUB0cVosRvbVVUdRjn^RM%To8C&t3L`!z^&U|PiF2x0RT^Sx zsInQ$j$HWHKmPN>u2-3yRS|8anDS&NhY}*3kLx9W*by z49bjsS-n(A84Fx%@Y!pz5Tl4^zu*Sm|He14U7%?#O&=;n{%ZgLAOJ~3K~$;@tOfwc zc}H$2{Gvd6-?D~<2kvL&+;hQ1f%S&cg z$;b#}b92?d+aLJ|4__RfsK=S0$C$?9VRDZFy4I0e&)8T*E6K?99$NkY#uPKmH_3SnbBxho z7r8r^lnGkV6df0lXjG~_(jW(NtfZ)xM4~f9@f_oP2b+EstA`n5jzj(`a(Mzaxr8J6 zHgX@rEOEn#PXsl=IWS4glb*gCOiD9Wwph>Qz*br^{N2h zH$5KR+3D8af0|XGa!HRCQfWePz0xAp@SNZa-O>ia6m#k2w7>IfAs!y|Z` z2iX+`DJxHwH5X(_-=xwURj>nKxKReGUxt6Z*Pf5HDhgE5LdQybfy}B3~$9!a2$dm~a(e*hI zLJX+)ScHZZ5(k@!*qM4h`p)-o-3wp9*kxDrC|#DAf2wf9jZfvxZ+$;L+lJ~asx3l# zTdCS@D2UvPM^afYT160IJS~KAx@_YCcaV#toCP3OB|)$fs>a1QtazHva!%Z0o6Gs! zNB@-P{?}Jy#=^Wv)08N(IA5gD>2L;~dW91NP^+X4YqeU-1_;XU=ytm_8V%;>=NTDk zg%0vCao)Mih;YzGBsRu4Pd745jEsfkcJFy)ZU&RYnmYyqHwvcb%4h6_%ZoQRl3k%2v7c6?~MHgMPtn2o3b96uVIl6!H zC;h#C)m5DJV?RcG;f0UF2Gaw$-tn2cdHbI05FM$`BSXJ=(pn{LV)?n{R<~tYc3i-% zYF=os@_aRTL0#TPHY7_<%NDISkhIbXJ#2!VNK{o-2ehypoAQ(`X_s;DX{9NT#0{q7 z2Gb40!%qAgii$_^i|({i9V+c)4W;y^aGE3UH&)z6V>>Wi{Oy){g?V0R zUga1yG?NC682mUqJe70rc875u<1M*$WF}+VXvX#{Cb<93yJ+fn=xR)Zt!%sYhspNO z&~y{DE_giTvF52&H56x-rKWjz9Ko%6h-MLUjS;YdktWwY{d)fQEq}qc zzW8rkz3Yk4*f>g$o2|3a);e>mGpTRAI3SKdEUCtH-6(2e90?JVy?|2+cL&t1`$jcwCyLmp#7vaXSj#)S|scF=FuQSKK2Tg;I|LX@ zAKddK|3gRp?k}0oa@iFxt$p7-)=U8wUr)JYjJh6#kINR}?4 z(@3Eu!npUG9VKi^yZqgUKEzMI@wed^0D>5};39b4>;7N<+iU+CBlAF~NU8vdpbaPvbA>+a;ha3=_23#(uUl8>MtJ=y+%m=#YbUt|v0#xirri^Ibmr?sxIr zm;N$yc}DJ)G< z-DVVFTr8p<@xGv*R%YWSkBH^I$nf2N`x4*%<~NAWJ_lnBdG0VO7&YO{J?cd%#z-G6 z+FGm9cI?=(f=(&5s3jyo7xl9{fBHeb{PCUD+h)K1M6TcC!d~Phh_;I_HkdGvgH}s{ z=v!SB>s+l*QGyLi#g4PgRrI;jwdcFf`cbC7`AyWA3Mf5V{CMGp8%W>smfr9F(?8L= z@kZ>nZ7aTJckm$T%{P<&@gKv&Lf>oAbDl$V&N(NfS}0dYOiylLy?5UF7#(fAU9GBb z@O^6ql)U%FzOpjCswl~#-PvY(7P&Y3^LIr|^r zf86jQE}vfD3saljXW4$k!n#|T{xT4 z4{(J5pyG@#wC_xjQKU4zR>Y+n?aXZr0=RYV{45b2j4$SiQA89k1{39U6polt9^7(- zrbN|z5<2Ah92dzvn;Qqnns+fie=8H59oCoMPJI4P5)&u~iJF1GULMamPkjq$UO18C zpyODbUIfpfwP>kL(Bd*+YUd?Zk5yN(;3uI!^IE>Z21?99DzJqc}=9>3V(AQLpVw_6t3& zR+b1vO90+-$&1`46>Uf&Ip!uH`rgJUF%;~`Rtj*1MOi+Pq|#bS*? zY)^~Ws4aks|DV10j=!X;@Bg3gb7tn2PuIt1q8vWo-{BTz8A`DXM9(A zo|C32&bfJ?C-ynX=m;BL`ciJc{3=Ea47-d<5%lrigDNpl^PVb>ArY4NjOp8NBfaHj z?i(1M=j|A>%(H!>#=3PCD)BUVIu0yB$v|u|4hy1NnniVa-Z5|4Fixe4P@@tpVYqfY zH;@04Lw9N6khwoMTyJS>iJuy6%6OI`&vbMwVj4VyrC!^3nn{o;CYyK#p?KNx3cl( zn_2z16Bs$-$k3l!e$H--aaYFmtiLT@dmLFh*f)A#IusC)3thkRtY?vZ=}YJxcZ6p6 z{O4)ZYSdo!D$D}BP^~_g=VU+qDUC0F5&x}k&Fs6s_G?sM`cfoGx}I%Uf`9YL2hTf& zw|)M)*5^OH>^eSt-YGL^A3Kqob`{=lN8k7En(sR+wmRqR9j)-}%Yh;LjW2k|JKnCK zx|fdgQi&|Fz$L&Uh`|_ePO&H}noT}`&bb`_z(bt6b%L!#-1i?>Q-9ubk$Md=;P6CZ zJD?eS2b^19`m%dfmD&8f7N~n6cMfk-ya-MN)L_I?MX*kpj8h&QvO9j=96^a3h$GE_!6}t!(9}I|mInx}^v_1m*2$S)VtX_8<8<)oXY%Sb30{4w( zs75WL3W0j&sHKjBNjOYHMkU1&qOi2tU~RQV6{r@h6^mILcbp1(C!K|}e2xoaK%$6) z4n3Ib^(UP6geTL~2nv{0^QbZ?`*NU4YnhZJY`wMjiBx2^cYGfp!-5!9ES^Yn!~>G!BoEp9ivawdW@eSewAlhWfb&&U z6GD|%6OoieHj%goSbN;(TE}m{M`Jt<`9_42waX3zg=bwuezXo;+VTs5l-^ zP*(HJR!t>hFgQd#%}P$Rv_h^8V$E6!uM0_lg;%@}i9@;bRGlmIzwlklB|ghds3;~j zL}^CN3bh0(&8*c6x)4lN3P(JMI>b40XNVE3dLpmHSq{|-xrfQ=Cf?`sCQZnkflO)L z-_-O}D*?=s%{cOy=iSGqO=BogVZ{ZGslZVuH1n*BV`oPLN@WFvbB;7k$+B#h#@0ZO zGkZtBO_o6s@-7gqB<;2#mv6D|`>D5=0fA+KZjT>j4yiy^0 z*~_S&cOG`>(plHyy{PK8=JqmGrCpW03)foYGwi9v$+@oI4_x(SHhGj5J1l^SjTXxJ z+=E10F7m*uVnh({QLku}kQakA76Y3nShh7|py`RNq3S)=scEWOa+(wlIEBc88b!$a zp18j3rQRR2M4ablkiS|8m3k$ZWJz*KIFgaOa(O^P0onJG(zX=63+b zG33P%+@cu+#8ehBA@ycPcHWz%&F#~d16L}9#DqZ%(>;zNLkMb30^1bs7kFqzlN(K? z)vpqT$miU3PlNSCLp;3dFphdC#!il~Ob!9Nq+S2t8h#~zhVAO-ZP*wkPAo1@v5CR? z3>zK8iYLA&RC~~5#n3R@;|iH^n8M$k7leAEf>p3C@IMN^$dvU+C$TWnB9E4iXD=bt zwZ%ozcHi^{jQ6;$H47y}r{g0zbd*z1dpwhA$_YB0{U@G?kE5t7W zI|^wQ%Cn{c5yXIx!g!|0a7Ii>w$grI-yJTU14R;RaS3cu_>Z4`n^zqC>(qcK-^;Bo z{;|{Yi{wT#5*1p-u4?r?VFqpUKp6>KqNWSC7lz))h@*4&JZgeZ5YDv1$t zJQl}bDy)qxSAG0%IPIL@X6Wc+x^(`%>LZbMo2t4{{VD-I)7Y4^Uj7QM`oM>n@SeKD zG&u%KWCY_qu}JYAV9>)751;+3kMXqgUdz%WkH#7hGrMnOVeiSl@ECO)UZLPrituE& z{Kxy>&nMpV0oElEE7bEOsZvRm|M~0}c+MZaAwX~2z5HCAeJGm$eJ^xhm1jR28V#~f zeX2cnKJ_WGzxzAP8E0T0|9I@Wb@*-DFe4-Q2OlK++SlNwo92hAJ^pwqFM1Ktj=)Fm zCh4u>YPXlC4{qL<163Zy0j`#67M5l2TE;+WWX;S=Y7+#t02ri%giwqM;sb7P(?i85 zHg_cTA;xW$iTV)t=NV&>WlB8tyvY&|&E_=q7_7HgXHb*l(-if>V3y8GLaCxuUEZSp z>`PyE%oOfjvI1dkiRoRGA{LHo!2oHY&Q4WQ9;uogR>oQebK%UtzdFUhki%L_?z<0M z(9BSeK|nn#oo_)4R_1Y2%{*r}jdB5C*k8~OaIq-1@B&p4npsX9$3_2DniA;Ft64^p z#B7VExPQnH@|Xu(9Ej8$v@X9}VFGu~u&l%h{0Ap2Pgf32k&P->q!9m?g&|Q*U zp@_O-m*U(I)-DI{N%NdMH|T)S$ToEZa%a!FQqj)5yudToTJLnh-fznDY8wS-UyquW zkz!z4GVZ!_Bgfx!DjYT28rctb?H0SSUuC}g+f>T+*WgkxQbQalDc+}8n?be(q>eC9 z%ndPgjr!SF`tb474?Bc!&d;MO9UvgE>)wm$9fU<5Bk@TSEl8-8ms zh<8{cExyez2fdWl3Nr&-bf_S+!4ruHmYgJlq13QsxXPi2u4UciN^-k!_13K?-^9U3 zg?6XW{oP?ltwD4I)fkZk>jmp!g>GlI0-Pg`TFYp33~6jhBa3lh3hX^27AJWKaFw>d zE*!l*?$nc8{co;*9yvqg4I4*ts7(<+VGVa5xwX~)^K}sEz&|XxiE`tK>q%FDaUm8( z8%4;JuykaY!v=>qb;a@)D#dJ8PnnF=f&=trpsbq**_3{5S*Y{RX25kI+!V)Med$c( zkI@xtL$!{~@5#vXe9pks-j*&XJ9aGKH5&)b{J$?>h~ZV|zm*?daxr7hQMZQFc@m@u zK$mQFFlZwNoMR~W{QXBh%(E`M5R3&e!DrQTy*$r}qY%J3`|Kxi(M6XbekIBj$WQ@E zSo9XP9r{KJMdg-=K~(}wp$y|>1*zN4UDUWsMi@1QVKy^t1TRH`ohpfmr~;$LuoA;E z=lSlHSMkEPy#wmi-J+)NY(Ua`Mb%?1WG=(n1dJuER+%uC?RZwyt7KDCWa>#YBp7L+ z1Sa;Ln#);Y4D~$Y8(+GLSH0~WNF{j`Ye8VLCPWF6d7gaw|8mW>r}CpqPvP$Wy`ClM z2YIqYEKPG(Czb*A3@0_N+cE~X-3dn>197!&{_gEZF~aB#*D-d&DP8`gcBPzQzF9Xq z=pcCUi^(p#48}XEubWN$zyCY_-~T<-s16Nve?QmlJoTx>FL?=-XFLN_ukVvL+nc{| z{-xLPq4Q2@C06t;<-Qz<^5_e2bylGk6Ko6&W+}O`coQKw#5c-04Y=Rh(dT=|jP{HdRd5>E@SJJ!Unw~VM3Las;2I&* zFfV)sk=l0o72piG0t&|gPAsNyB)05mTq_`?MR2=xtj)-8lXmqZdt>Cx>jx;*@femI zwU&+7UB?YSxPcQ6K7pv3qeKMF)U zHr`eQ#4}(OkyatH;iDGVQ#$7~(sE+afBtwjlMmfIf$eKCUM&w?LdgHmStu21nHEY+*vMiW6 z?ym?27Z7l{RdS;oT2q!zdxo5lxlRt4x!bt+^tp4yQH&A6GW;gpcBcI8Ml2sLohS^25Zo$$}fqbGQE-hYq~9^<6qP0qF=xit02% z0-9j8%FxivbKQC4Hpb|*ua_IR_Jl`O5og9 zge(zK6Vs@`wj~ABbh`$A!GUTtY=TlvlD1y9nDK*RhX-k@l7daaN2pIwEdyW?P7}NtL1i_Ybsay>@8qBw9E^~6l>s1a#YmKjK-D{z#+Jcm$|o=M>wQ1{pI1S_nJV+LbSMJ+W|sy?OSn;c`7!9V{i{J~*R*RDk7 zJr+Wl4Rvr9|J2wCCo+1&^}((wd{HF;=L}RIYl+sbrT({nOY>`A!(Dmh?AM+zxK*zs z#~zD${NqVBY#=`2gaw0JZu4f$$Oy6r=E2!oKlczD*(c$xzJ1x3{XwEf@2$57P7EA2 zv@|&UXglmYp<<gK6%m|?XxNxZ3)}1pMofZAr5#*0 z5-d&C06nIQ=!UH*lj%Cf7=w5)dEhzlE#pC2j<7O2*NfuCcR!tUW~^yPcuyAP@7S+K zjRCC*1A_xx|C67x{@F)jM#2{*)KGtv_ho+-EsSWia41L?Z23*cwb1hnSnZ7)+pw(`FSHc={q`~!-$aQIcb)m zzKIdv3IuGKjydG?V@RAKcD8W5cc0g*;#>$oRclquE;?Nlp;Ajw@eHn5#WRMVjo;Rw znpxBs{5;hiQkJxdFfln^R1)|Z`r_)1`V{toDh088i-j3mTfh`kg5ixE4TycW4iM+3|qeKCdlW>QbBRt+dF zVv7DmQU0_TE-nr5fvSiBOIf+M7?o8EW-DRZT2k8Z@pT8Y2wI>N;6aFDi#9gZ=SO%vK%Y|x8IIA`4rSCVt1``?6RX=!bmDP z;XLsCv(M(T4}6#jATxsVit{c6{5=>^5`+q#tl4Cl6;>hScic&I;z>JPfW6n09|^$) z&9g$U4C6D(bDFo`!FR5@lFz*F&si5&S)qpGvz&Dv290Hqh-xFH>RqTHH9|#|K`S&S z$GQ4%KFx;bzW_!?XGuifgAE~znCH|ZMKo2rtpC|1Iq4+q+O?$X*VDZ4!iDdlQb7(m z1crx+9``unGtVSizaBHVu$Nb}jKT{pF`%PwnOU^rmw|Br4S_A!r@4g(! z^5_h3iF6r#B{bU%g}!H!(;lU<1|cFgF$K*OBL?aNR4SG@>X}QEC}i$O29mhnbNaIH zy8nX42#a7nH4C}u3JZL;bn+r|b~Y<(VV=*9#K3TV6>aeV8MAwq)`w54^Ow(=;M*H$0Anyp(m@rQ3Y9}Wf2Btnkua&+}ad0+NFF>0|c z39vv$!AwH7i93Jtw=Cnom;lg0;F+LD1f5dLl&mmMm^p0veGReqXi<>n3I^`$wo89` zKdQp2p%J`*m$@O(q~oS%^aWKS8<8Y2k*O2m@M7^;(t(&i`OJF>VIMoIzNhM2l}XiV z4HMa&ekPRwwN*4X8BqtCYk zTxDKip+_3XQPfHc8h+jY*BsD~sv$ijE4U%Mm+S6dG*=wUBR77Qp=l+}AyY#n5$Y72 zBeqfNIx_=YA)#hGH>8;%C1%P*0cgY&_c3o7gw7cN(~dTr>)wq-2+E{J449bdNsmcm zenmIT``&+K|C5>t6&!e-HJD>bsn`VHgv^7AC-C-6lTi`OD) zvM=+vtORu)iG^t^Ovf>eTu~x&Jg(63(UA5~5ffnlY|Kh$`09r~!82ceHk|Z0TEOK! zWYi+%Tmt93=GXc2_x=;G8DS|xMwk^{Q3V;&z(s9wn!Er??C=k$1(T4ooGA`tbDlF1 zk5I|R7%HkWth$0DPe>4A?^snG;E_D%dtd%CXFmRvMe*3}F;Uv~T1WD&5liKW!x__@ zt!c`^k-?!j4Ya9JAl_NAg@4gAf@Ph=eB;V5^MZ>`fXH@pobTc1&^cT^GaR0xD508~ zJSV^74(_}DXWaWgKjq7x{A*UBoasCxuEENII)iE%%9YqFiO;B^3}7%)ggRBJdS#hc z{KPoic^4diLaRghd?(Y@6b*&peYO zHb266tB+w-`b$W8_k*R7iYIq&Y~0V2HdY z_Ms0t~lh=W5-y&_`p1zx-Fx&Bd9Sc^@ zi|ncd5(Wu&&1M4+*FBlBWKyvF>g*wc92l2fYgI3T7{hd~RE!{G#7PC`K%HQXC3il+ zzt8hxj5kCI*4P;wIkQ%9>Fn)OqV<&&O{Fe6U&olQ2|PL@C^Q?~+Otl7Iye32Pl)lv zxe^&0xRJ!-eF5211g~32+*;h9R}!0mdK{sGs7>87e{gNS0M}gozVmn~^zj@+l+>ss z1H`g0$4ulS@SW%j!W}n#jw9E%VG0vA6Z@FR8k%acSVRZ$jFKXxC_VtEgF0e75g963 zfFxGaTK7Be_jG;IZXK7_S`w!WHx0M{=st$mA4C=kSq_lNaqbd{-r!c~KM6)+oCdC< zzAR$(DU#cs_2L)twU2&;EqTr)LWHLdRK;SIRwY#pp_%5ah-$3N9RGIt6`Xa+`-8`; zH+x&Ut>1O)gbOcxBNtuxZIJB<4LmVW3G)pWAwqrO&mu3}fidj>R|Hh?jFPdItvt*Y zJH!xCN|FMs=;oNgrdd#t?XOBnUI|Ex~@}g%ZY$;wt z>6vDTPvNe6_{QZ|@#Rl_f|0br3I`|VuqLvssSjaYhHXq@oHB?o5Lc)mXx5}6rIMl{ zrO-o?=ZqQ)H{S%mdKM%$AQkMT>8Z_UNZ|bI;b0s)`7}m<{=cZzE?a+5^Z%MP)L-!m zwAn<@I}dF(@!Pj!moEnwd{lOHbf=zI^PK4~d;z`gx&WKFaU*u^+TE_#&JI(kM5b>m z_vOI#^&KFFZJ4l=azO@|&Up9>U*ho1TR8ENhd6QDR*u=S1+pAlItUAc#1u|U-^$*f zU#lCa*V(>(d!NL;FZ+eq7TshpZfD=L*yz}#V+(<*;CnCyluS9cO3p9a+TX&r*HWo0 zkCDQpRSXfPCp)#|*xA{w?-}gNyp9<>H5}^)hBy{jk~gX8Gy^mkmXr~3ER7A@bC&at(hOR+#d z<@7VS>6#ni9xz%(iODdyegL~K`?dHE#WR8?&@fa6ROSY@FprSuqtiX(m74l zzU5Rox4(W**kE~{bIWgE(6#S` zw?6fH^GnX)vho(51b!K6~#$9qRK!hxJ{XYTmVZpUv2MkLlc)W$V6%w%RFo z`3|+26u-`kFjeo`e8dqSAn~^m`5UR~tqkZLB>D(Kjwi$6aYo5(s5Pv4rS7H~%Ezd? z260hW;_-MKh@TaXu~){Px8v`;k{bq_5yNTBU=u3&0W(`NvnswEWHY(lHk{i)ipZap zzOA*e|EbOIi-I}sIDX@g-@^T>Y!jhLgvW+pkxEgZqEw9~R;8Y&47(|6jp<-aJ8hS1 zbiTI2^c5fxc*V<42LAx+9|1FkNE0Ou!EFt2wnDWCkY!!fU4jKoz}FBNX62IO_$3*) zH-z!1LIcId3D!i2DiKOmgdw2PXs`kp&YR@7-wZC@w_TeK#Eq`9XyLi%oWu6mvQ?BR z6X;3Q*mjeu$9g4JuvyBWC{a4i19#jCX>+dxcY6x;@a1}2?9aRlv#-zLp1b+>`~QMh z4lm&?k9#~nyX3<>rI~TMS01OzF-BRHH>gjKhoF}#Yph|ZF$}nzN|sRrjJhzhi{g!t z7YHLs?)ds&e;V$%6S9Kg%I{grvgOg~!V>ngmyK#j*6<5bU(C^6DizGg2zKR4;-ikj zjE-WKEyFHbwsXNe?`eGVo4Bj4YVB%%_q)5_T$T`I{?K`k>-z2;m;H>qKLz*Yfc5n~ zHZ+JCM&#_1WTz%T$a>5(DgreTCYnuZwE<{Mg9eOQgD;E6!(>n4eD4RY`ts;=xdo?+ z^CaHQzEsY`q16f^6~U{D^Hh8Q{`93HsX-MB4h}mT2HmNa#?0v z#LU6|SSBTP2UT{A)JJzg1>9Y0<((%UPBRt~50dK)y}IPaEYaUehj%MTSweQJpP>uy z>ib!^@j@V2c0m2^LQ0TeDbIGJE01bB_qN{pOqym0=cNVLNTD;uGDck=VafUrw~oK} zrVo))#kE+9_8i%h^+J*xVrNJSblC&P1d;9z5nu|q`m*jyzNeg^SY4`Er!J+hf2aU29pWhqfH zuN#y|Awj6BW4W~~Q{_8XT)|lvymy~gLYAviw`U{#J$l%oy!Op+;@>`eF;l8E6*AQV ze6yW;5sM-5p1Q!W2sd8+V@}xcbf{GJ$}mh{Y>3RB2`oGu>e0 zKmRLhfA4ppnt+Y=$~&BmS#|iv>)C$%$*sL(C!NaZ4L`>eYuUc02F&lhm+Y%w?b_#l z{_}1K*xl4XbDJ9@0U`~VZg&Et2yiN zi14S@H7d1Ov<)ysC%a0i&N~)H9^e0EY}00ztr*2=Mp6mf9N@6zNT5INcjvzBQ1IMZlRSEO9MsIs`gH`Wn3v?xV?*=g}9!v7^XP&`L-}+&T!^O~+hBv^Iaakdp;(e5j$#|02Q_DvjTp}$7O2}OaCK=wU8!Oj z07ikKM$Wgt@FmWA*Pp>aGVfHNc#b&2@Tc!Oj|<-QZO{f{azc$|Sx;tY;p>U(aiSI9 zX-sa0ner>x!fNh|$JlIBn$|Gw9Ro%X*RCQ-jG^K@b%iAkzV?aFaMs!9z)2_X)24SF zwofd~tKm5JdfsMMTDE5Zs-q}KC zw#BVzOWhAVLe1}e5C7vIx9{WVqp7{&4ZA(K6*x={9_8ZmPT?J&zph(<=u2M?KzZzX zi8}0~87f%$Arh#@y!!j!g)dyec7frue-o0pWsvkT9O@k0DqZL8>wJCLj~ptc5t|hd zoofe%SiKA9E9)qx2ZuPURwo0J#oN;@DOKuvQg97*H~{(ss4u(fmSR>Yn|9_xy;%J! zzzE%F9|jn2Op>M3Y-A~Su9#vxi5XAg7Nk_kp;EfBO^*RJ*D{a;88hLhY6^9QQ@2i0 zr@}BW(vf@E>sI$TGF7ZZ9oNyv#H2NP=rZ2)XK&^IUHAoF^0Mcl_*On;e`e^*-qa&Q zwS{(6TC_iRZp}#8R)uXf&v*@z94a0H_*RVgRp3{G&B0|Lb#e!h?U+Dz>(VzXpj}_~ z_4Ky8vv+6hG-0L`4F-_kk)Q5@xmBJ|ThW`vmZo)Akf>-29=DjqA0}Qvm z_5zOi+@H5TkN(D4WZ$_OZ;IM9uU##q9nQqww-wYD>D{xSr%rcp*)QP}!{sFKg8`{1 zX)H7&UGzIIpGve0Xle_-Ztl0Poxq3y1a_`%b7*o|aH1BUPuHi+d;afMe#EKI+_AnZ zoe<8Q5SX$fXVK#QYBx0Wt^_Cdrmv)9$JLk4gyY(9Zn_`d{G&{rkRsVLPI%=T-^hJwq6l zd-&%ouj2U^{3%9@5?k5D@z=GEwIMA<1#$=Kn7ZQ*zW=2!@ppgmm#mGeELGugK4WRx zWJOY85FweKVz797NfhXdD+<BK@?U;3TDrNAxRIZ7_^s5C6`7(5Y9&wTR9&#*Qhq+nctCwq73yQ#FFhpYMX1BAC{t!_4KnY}; z!KP5vX|Q+_`qf)~*(=s`F$fK`NDmZ6)nO6v9`Qc>i!zpY?jDBa&tA!$OJHmuCXDm~ zhRs+ofw(=aq82edilXdz@IwkGb+^N$)JuDH)LtWZuI*9&3cEPCWWViVyIr3tR41$L z;8qDiIOn!YPof>?(H1mf>#Vx)!)cFo=0)&E3-@c-Ou4?!j)FHUH`l(B$3OK+OdG{n z!J$ZG@r4apR(qH4Bc>@1QS!va~i7woUw1WfJBjiTNfyri*)H`yc zG^>VfRT!(d#lL+ip{+{v1&69|>n(pj=WiVKVrp_AS)upGsLsBwJ2ktg(P+=VGdqtK zv;(24A#AcpJHAz!*U<^vONbW1-g&mgj6hk0-($Y{V~3{t*?6}yTGc-E?X4Z3ltYE4 zgH{mxIj=m2dn*Gxgdr11Rms)2<~QS&3c>(T)09zimgpqheLG~jz&kL+5gc+bcy~W= zAF7XFOp1_qqzXhWzZK#Msdps`M2VD;;C+?MtYsTVb9=MGMy)ZO4B;fg*d!dUiuFoO z1nY8!vL?&ZCV%t3_d%K#SZQ+%%{{!!z=M1}yk6(0AcrI&Z)u_6b!TFw70Owl}^x7&x3Z#g9ZzaBcuJmef_EGaEKi#(O`9wSmH>+eq6U0o$&-ji zWEcm=)bWr@+2oZ?DvTu+?vEpWnl`w`HTds(!u9nkXTI%xw#5Qudz|C6-Nkz+wjv1I z!^Jgr%E>LgLAsKr=aD=I=XTpYy9XX1yW)y)-a_n|XA+-w+HP;3cExeI4$l|1&4^gx+k8SI=AgCPM}gGLr=RdjFw>NR||0#VeR)WS@n=eadK z*<0&&U;47cgFWPu7dXs2u_eV81Br7a1zz>ep08S{(9){S*j0GKzVwA1@)Y(aKa|RJ zCxoDv5Y95{Y3E^=?m1n+gV4PRA+Tl2e@ll7;7zYm&x}m&?x*SvXFUeSPic1pqr*$N z;U_9jesGw)POkm#JNY(4C_3`1+EgOp2^s;sj56wgDoS5$pM3_ z#`tPP9t*Alc^ps)6@zw{W3r$lc1PzvUF360$NVIH&2?{^D|=++)4gejw1ZDGtG1-W z&D6VfuB0>9N9l3Il+K*8va7cOy7xSLYrEyO7j(78NvE7lGYT-oVhp*qo6jNBWrUPAMI@y3grM7Uw%zvRuH)~$>CbS~V5ie|bk2FbbJHRyWr{X6g+8`e z?lbA=pYvUPf1i}RhpR8`<`vp-?k3uSkiP6(FYIx%E6B0O@{%{ck&TTe4Y8ymWZt*X zgA7W9Z`s|;BEuSiAO7P%v|&`{TvWn`IEHt>`;Rc@0bmQNQ;3v7ENxpq8Wo>2+V#F+ zhE+{)TA`^UjIo?wlCV*R*lubxz0&l)sr=@k%D6ge`f_v_KQcyvG#tjunV0E9#UHVAKfByY7NCo9E&>M-ZV{*VLoVg$v{v zWEqmCFg=O;#V`2g``*v%pZZkJKVt*``O%NDeteSCL^v&uIYE@e^Cs(D#u_866k(Y$ zjEZ51@D38EL|%!F4M{&Dd_LwZC%SdD!L4_m~D&-m?G~_f%|k#UORhuTRDp+-?jQ_N6a7 zBahuwXjz;wS!Q>Z{~9TY6lTr>(wTNy;u>@^diI>NFMa7tU;6TB^DwrRXD`7DO(4k)CmrBf)JOO!4+9@&XC?XenRd66~3N@9oPT zc1N`avOpE#=vs{_V0Bby%m_&mlbf>QveiSk&>vztHaJ(KhSxKA|PqP>xyP2ftp*nn9mnyI^L5f1aIm#=CD*){a}M? zT8DAVs`_%KH*H_=aj%tYdhSXJShM;2z@0b$4o;6pWEj;1A;pm6tRX{~mcz;NVYc1$ zG=z+tC8#XNX<1wE*b|I$x|`3XkJd>E`0?elb+DoSq`jN^gf0IQ1=rhnWN0KY+nY^> zj3HB3RDAm)=&i)o5;@1P3d|V1IyBHd1W z0S1j_7{fnbb_FlJ=sm3f`ON0mjajn*|=i^KdQUp3L+5KmKiwjkc2?`7v1kUa*z${?s5+z&y&{ou_(tj^20!W5=J+ z-sOEkh`qQQGs~8dfAyj!MTQt{9SAASh8*c*BA%U(>@NU@sqbBO!0*ZZL%K<`!o>pVC`qPRt@?uv1p z7vs8VAdPa;2pX|zMF1+kiOt4BlC&B`hfrT#$a?QfUv_Q{+skH??!vFd&{}K5xdM=q zZ`E^Ln^%k&#_GcM!HCoI z8tWgbQOSi$Ik_OMYJi!qdj8fgE0Vl{90ebsI}hFB+5Uq&sbzx<>ImuheGKZ(xCCUz z{}ko+n?A!)>wmBHU7j3Dp0C1agledWu#(}CqS9>`&nP(_CkkpX5`{Vv4Q~I_7U18y z##~e2J@;kDMz0hjs#`X^+66rr?I-T;VheX-E2vk*T7LEH-{iZOe2`7eCZpDpdQYaI zYAZnN2}T2Vb}e_Ti!0pOXz=YXUd~J2`SwLY@E&?7y!+k1&jtVI8=&LBAX2aZ2M#Vn z2-~E^7SS#bvmjB(+>rsiA7z4-+~YQ}$$7?$&~{#Mhn2|(0o6qr62pipSAOynJn!x2 z7chu)FblKq>3{@?BL3)|@8F*-Bp}w*tx)qhmwxP{yx`nd zlN@{WJg1i)*xmwAm-0(k9=%>dSoTlYhg?W`idh%X0OsjAN?aQN=QlITD{y zMX5!C^%_Ey-eWO{hPL&z!)J~H5?6#KFeQOgYsy;ENNA#Lw+S1kn{1B_TVuo4IO4b7 z^v9g>(ibB~AI{bPxP&Fevt0e@8$8wH1xdN=RSJE4PGcAS=2+3oekNZ^;SwgBjq627o)5YOkndMuAR zb4VxF$wk|Hh;NT#4+;PW#%Tu&Zy-|e$Sc0#Vl_X73G>OS< zdrX!b4@Qfyw}6)nZxnAVs0A-1Jj6hBKcrE>DKv*+q#80{O<(Wq%K<#(ydp+WuQc(9 zi92BWLWOflJLQUC$JUzgK*f<)Vj7X`vlUT+Ti^9Q&VApzTKiVM^m*KJ&(C<_8-ANf z5gKtkm!{C-Fj*#ow}!#{hN)h82P-BlD<^G>hik5smln^{Tt>bRn=?f)zKEF(r7+Z7 zgJ^`PC2|R|m)+7H;jX%23XeJBLTzhabUf-uhO)|GxLLB8ph5o&hnqEF&tk z-B^WT8#C!MYIsJ0+7N90*Xk|Z<7V4>4#k!<&yFMkpj{Mi?PF~p33Z-UqYny?IT zWgsQwTuxaFBx*>K8qL&Fjg~T0e*(YQejg`h6PyrNsAnl^1o2w%+j|U3RfO2(ER8Iq zQxkC4?QqiZC^1^tJ^MVkRZ5ofqF20<&t3dM9&R*QZY*gYO76W+F(Rn?z}IXPaSduB z4CbD@Z@Q7Ak3ANEJj;mUsMY>byZ{S0??Rwn9b_5Y{!701rLXYCk9~w?E@LRod7_C} z9b1O1P*+bao2I6Q8WLz7OoHGr&S6xDO|TMGLs-j*#S3I8xq+t7ne^bCG6iH-m@+X_ z67!H|+@EJ`w+W974)c;f_+6g!>a%G^2?K{68SpKmnd{comk+YxT-S|gwXX-)Z19)f z;jQT^e4fwFNzOQfX7A2Tn{Z$G3hpakAqC9SpN>8CRHBnk!meG5UAq>s#X9F4={48j zzyJO4x!~W5pZ|R9iWPgd3AFoN7=aI-cM5O&{B^Bk&j0*%T=JSz!m7N#jJv+{We-X} zz_l~V;GVDW>`Pzz(wDyMLy1x9UKs+GH8Yx4X}SW&wpa*eQE zLDaN>DfOktoeo~tj^jmgwBS65Xc9@zfZoGxKls16sS1Wcj(N%(P`89GE@so$E&KB5 zA2+@0lrCsk2~*yYTVdNkm9eV9c)_Y^jlL4*DIhv2VP3k?_*mZ41G@s zaFy?MrTvy;&f#T89 zIjlLV+Mg8n$S5tV12e9^tTKx(<^r^Czp-{nW(2We|IMAS@Bxi)EmVNBTbU(6luB@)ujk2UXubw zqMTPcQ|j^#&Bl=BAk=B*Irlt$V{l{*wCzl6+qP}nwr$(CZD)dsZ6_ULV%xTpm+#*9 zUR76}>i*MJr*>_uz4lu6tqYt%@Au)D(bOuV7H?gmx!OTkY9LfwjUu#vc|VT7-=WNL z{85lZ-Deq2Cr_+JFe{dq1~G&)iaLg|NUw80%^ zyj~9(SQnoNyDn{23uC>H~o<7kZXibXJGz!7QkqJ z|4lXREYUn|wbN2z$%LGk$&!ISGTAQYB81{bruG-b0TDO9kbvcRjmyolaTO7VyDk7q zT6hQ@>#hGU+~G~C_+7F6SB81d5l#7QE!gT>kra;`Ezxco7XX@B`!BR^PxPX$`jn2;nj3xHm+BJH;pAQwu{$Tu8@=%#* zu1+oGugyhW`X|O}=ESj^!k#;bdtO<9rZ`{8(E!!*62n+k=g>W;kZJOyE9La+!9GsX z%f>ppcAXZN^ls@i8T7oA31bfhSHrYJsvF3id;fMHdCoCy0sf;}62FQ>=UJvL(8-dP ziD)vvuZRHBc$fy762hndKR@cF&! zea|sbd3N>Q`NVg;Q5aT}hkaodW(FpRX%kK{l3}rxfT4(B$<)P|{6;_s7K_EWH$*@b zf6M48Osxna>ti%&uz(>O3^7i?98oYUJFonx;Cjt_e9^td|J2@kRb5$A6IPkW!Oxdo z!=2w5-n^`5{p~lQ^heW-prmk9uEH&6b}UJodaEOMLwkYZ&w)L!Iq+N@ueJMq>-#|j z@v05Xy6R{Zp?h4FN8m3bklVN{L@PZ_l;Mw}xxMwjJ$;(#`h4|E334vxaU5L(-dp!< zUuVGEBvJYTX@B~uT-H%p`_Tm`(dxPr8$A2z zP5_~7=9#`6lEO6hiW1s02-K7gRHMZr-c*M?dU`8;&}>{Q8^}xwx+sbJ92MMHoU9OS zD4R&+yh)t%yByf&5LvdPF8n(IBH$$yV+tFk?q>|+0r`ICxABy9wOB?_ z_1ek?Bh$Zw|5ACmxawZU$pH-m#Z=# zn!n<>e_8n5XOBjb7d>yx-*@?Lo|k{U)e$KmA(CM+d>7tGD4*l7% zN?G|>-<+r96%vM+E(7RHePq#?m>7k>s?b>jdD$Ge>jZr( zyS8%cRxe&iXVHeHQIB>PYrMM&;gn})n7G%17dy-@PIp-@uEOsB9GUVK9~K~|NR}Lu zxJrhLlYLEO^NNVpF?r5@u1L~pzER@&ZaiTSmmt${{`Qmu!04CKWjsw=$Kc0p-)rJF zB#)+|QndrHTFL{DyU3YJIUZuT+JMfWoG}4ik4e%RzKf_ZojEjtVy(+P?=i@jnC-5~GVKd+DBUj@a|3P1AjUAG80G4)iiWZ^qOyf*KPNmxx6f zqocH`CowozDFPmC1e1UB^D{hd?1N*Dr&S?M>17VxjI368%;QUMB#Fw*KK41sw>!vj z??tbd;en_x`{Xr_fai}vv|a(4L@h5gIg5l6tMI5RQVHM-A!LN{m|?f?h;*j+0#`8} zRd#^$dvgAI#P69*|9)PiNy>Spoq=}Fu!o6$2TKqO_R2%D#^amOI%5-ROu?wbI;(xH z2~LEHXy$+y338Si#9%Y0#St>2i6t=;PGh>@)#kBz%7b0x>isP9AC=y|Bg4Y5nuz~x zTxfu1tuAZHpfS(uS2Fl=I&!9ps@L%EdBuMHO;*G;nK7M_E!7Iu(~xBs`8DXxT~?a7 zbDrGR*_CULP4#}42TB~xdNWs8&w>ig-)(~27|l-j>g~?7*>BGm@o)Rle#9BDCXvJSFzwE=ZVU#K_P0o z6|E{4QC|*$m`ov;hM^i8ATJ+Z=rh5+2z8yhq*|Xlt0fc4`=kuBs}p9R_CYLQtb^l9 zuss&j!g`f?u+x3`WSpKZ2_ac4z5bAwFU0UbR{6wI`d^$#<$*0xUvbeSX`o{Q*uynt zdMlw2kBkYkPOJtIt~7?OPax7%dBIa!U5Hh`UGY>I&MLGEubh>LVVa$KTwcXFkN2bp z#nD-RPvPOK+fu?3+OocGy6a9Z^S?1-1q>#0$qx${!PKiW5wwJjdcVaO63o18gwySB zw0y$=}+1$3?DeE z)Tro4QIf5Mkzpu9)sCMrM7@>O@CH2=a-3r(iaVBjEp3|SCGUO%I|#s=nC*U+e=*7( zah2?rQ>jI!)eh_VI6OI_SRI7l&-aSD$p3=;Zp?p_!i?J<_yYJdbT|?>xKQ3s=*MX` zguG<@C@P$hpqXz>qPx-ve|C>qQ+xAYb;(*TNMDpu#2=>oZzkW*(bq?TkYlN90}Buo z$}{a~?XS^(-WS$wy{)Z9*Oud+w!Nlh?#(A}UCVY}Cz7Pv_=4vLfuFWkylF4#NXgK} zo<$>45z=4>-sAYABw06N)B)sgYtD5icqP3cfOj_PsV`atnsbx2Z4515zlk3@LM~+d z%LaBh>~@2@Upi%_^(claLQrH>S1!%8{Fz(A4%JxJQV_9YghB4E<;4%J=SNJ~iNrrrf3N zj@T^+muzd?BrU-p&kKD872GA;v)7!!!LR5j82O$DX@sFhfoJ_6K}SuFjgSL13@g{0 zIgsAQhZZyNz)edDIKGVg?OVA#$2?h2Ik1xH$A~tpP>q5bh8dAb<->LTkAE817o+YJ zd2C=`cisL0jM;%x1jGcEiDHbo+AmcJLvWU?v>ZH$VNxrF8SOTvjPdmu&7SzM$+I_M zla}cyl}LZ#BXQE}N#ZomK=J%s=&=w~^K-l*iJap@o8&^32atU1;O3FS1C%qM4r2dwiD!>UaC zAY2($5QuEod4th~>4c<1SfWV~<)5SKh>!YIsqn+KAc`K|=plta9eBHpc3^*nEf zb-dr>U(V8Ob3Xeg%5lAE7S0gVft;;%ei&qql9jhp#=i!mpuJ3gsn;gunMissda;EI z=FjtFosH3WKV94Ql1VZM3#-U+$DY-Ga{>Fmw?6n@IjhvP0dXE>;M$vBw*fyl*UtGY zcm9VGY0q$mk1Y3you_HvWV07f9Zj|bOQ?T#8z9tR09G1oqf8@_-Z`bfmhIMFQeB0m zegfABf5j!v8RPjOb@bqU@v{?HL^$=V5bE?O2e8aH21(DIc}6J-v5Bi|d)cA*LMJRej7L6Z80rPKmArm7Dg)&AcUcE~0M*OLtp4-`@480CQ z&$5e`#;|)*LO}%lBh)z$=WW&}78ZPW*xH@F8{YeHa6SRLbd{9OtY&HX)sqo#agNE% z5}2E-J$J=xu?=1U0I4(c@TwfgJ%L~opZwXn&r#aDtwp+9x9 zl+44v@c)1o>BMY7lmtt~MufsVf1hiwuNeJZkd$0hWM?eWEGM-H!qykmFfdL#h_m1J zApT)f@RVmtwYxq~BTPjULA7%g1;ZNx*ZnH35OtMACPWpU@NB67sQ-a;zw?bfkEKD54gV81N5nGP(qFyw$o&+3c&n}uKkOyrtM}(2&tRpiu6f81+oT- z^nZdBEO+PglIlr70?euHI*xnPSFBoVzqj|D*Y9vaJsr4l$S&6r&cn)t`~2f791%?Hg=seV3oaG4GmX>JY{VawIGD(hJ9~*g}Ngh|YSIbIHqT2w9l5{ity-uqF z;l0U#EmJbtnM)K);g4ah3%VZnYw7goEKP~HFH~d;$;HZew z!rrf?HKWO%wZmKMd!PL};<{YV=uGn)456X#kTQt~flvhzEcURE!$_BUfBgqL>-8C! zGp@=RgWlW4!ExjG81DQIfcHJgE`SasvxsF(RE|a{wpDbK6qMaf1VKxmhggIF2x;LA zVEHCBE0vKkNCcO7w4w-C5P_ZZIP<`?r_lQzaZSI?z6%l!UEmUq(m1cIgm+Yo+yk!L zQ@&q@Qf({`4TxH}kHft>^WQaY>~IHD?cD_FB&T$Y*(M(6B{W?{GD zHEn8hfgvwldVn$!wLG*aU=2hH6szlGaR=$lq3Z}IR!~HFjCt)}^{)?7kGoUH4Z($* zg;-|@r3TEkmFwJ5=XJ|2S5GdYcLGtV#WlMtw>-TbD>Rmva9jO<+-`JnS^t0i3SuaPn z>pxGZ$L;{M+3);s$%i2Hi?mxjH_}pD`g;H36p9?u6(M6)DbCXWVsnMuw(YscCStR9 zAD(=I2FLfG)3nEwC64EjVb|M*dMc6~soM<1!XDT`AKU&YBvYx!=x|arDROi=L9r?` z>KkccmN2Wkin2Iwuoax+p9~r>{s@Ee-=7ewQxPb$5QRZW1Vg6aJj-9qhSvMi+up4O z-2ln?6cR|Jyy-mW#kIaE|d&E7RdOn7kF8pMa^;R)+y$ynpHB{j!!r>&N zMdNORE;iZbF) zpP%-$WFYE3pU&!zBSL3(Q58SvmRqrxno*bm430Zy+hbXjCzG?q*kUc?Pjd;#Q>sNB z;Uk669ht9XGUyc8?UAftSamLk{c%Vq7wvN%1)T0%+P&P!s8|Rs#=Hn7?NM{sP?K25 znL+$$=*?%g{eI#|?1nT?at&{6gVT42K zqIJS*6{PX7Hshbee+}pmAH+59qE)KeUsa}GPZBC*AIXV{N1Ek}O!RHkbo9%MNR1Ge zpXkFy9w zelA>7W*fL602r&)ps#%`1^%Zr=R-kYs5Cq8)ePB*c>5pVERx=HSo9NCb(#~uiL6+! zm+Qz&Pu{(p2Z8G-(hM*${=zQ-M0qHzSGYk|qR|IYW#HVz*B#|Oqk!~q|35AOqkOLv zzkW&l^&9|MWmO5{*I`6S=!SaaG1ARRi26_edX>#K!!)+&r>zZJO9vBBccK3M$M-cypoTcosTqzWyFR&PZQJu zCoML@({ff{<$Ln%&F9>2nBUJVGfjc%S?6U1O{ODf=aj{^FUnof>#9vCuQC0Q8$*yR z=V3EEj6Ro~6lh4&ZVZ=Z zb){i~d^VM0@3V_f;BEku)t=XkFyfI8gwuwy^{kFw(2%!bEBZ+>OVLBAVMGXtrn#;e zoKOxiPzX6nV1YY9z!|MMLlk!JC*~|1tzbPH9T1W%SkFpA0Sty_^m>Q7yyN$VF@o0I zW488t1+`M?omNw9KQa7e8D4iJ`PbPC)>4&Q%=0q&;eY&W0D;M*m(Z9d<{C^%%5sFh z@jnqCrRht*v9}3&PKb(Hdr-r|4A$BH<$svCt%;>lPxE^sbt0R&{){$<4O(pdHm}rR z$2y)~O+Vl&>60Qh-p73ROc+1CTI$NbwJy-96kB=tdv5FBvDY`#YQl!j2iijCLujE`$yKb!7QNZgiA6E zkei0?j1E_?wF4`%<#qy-5DBQ@)@z?;LQ}Mq4{*(!gFsk;WaYJl|K!J!@XRBcHDfOw zb0^74_&k^eM;4Gh0RhbY*{a34W*Cx@i>Jzjr(fVbN{ zqQ!n)oS-At@?%k{^r$b7z40K48r8wG;&Q{_YIH^r8mSf)G;3wrb^WUjDb|FLc6k|I z@osr^xjoCvf91Op&v|)=!!~~k?;n9rPOkCVy*-HM(AUI2IGro)a#Hy4Iek6*9Opj! zW4NtS+l|(1I9SzAu|Kn@9nU&+Z)+M=(ZFLmToM({{ z1xy?Riyo!F)Q==b)ApNF(E6@Q*KyqZGolp!1Ir@wP4jB#fb}t3oASsz_E0zuKWI>6u)t-NR z27Kfb&{2RDgJBP~Gk8?m4Dr`NI8QR?E~|gKWif^82>CX+sp$_Hd_MTOd&^x{v{%cd zFcC2+bqSFGVI5pioneCLEE!qi+)s{Dd^{j} z>_!IcI1Ou^=Hly;pn$@Q%6lzo7KHkF1VtunlYaG8G4Wh$k7-qNo;KLOPydFba)eA%j$h zEhKUzc7$dFOg_uY>IJP0WiaO0KQ>28v%}wiYkp$&2B3m`2ln{ zflR-8>dcDhlH^(D?&g6odA_**_iMn-MQi-)Abj%I)V9FiMNgWZ10<>+=4WH?Ll0lk z&!<#+uq~&VcS__%2QMGUL6a zBL$}O7z-x!wA(ORGeYBSys}DwN%$KIhM>k z<~uEuGMX}(>NS3V_qy7qZADW0Y}pBUFuG-2zFh(KhddZ=6h#sp+-90u%GBeYu1r0( znu#?2?Pl)u_tUrVQLQ!$V?d|=AFBER`Jv({iGTk=^M1!_z}o$1=VSOa_!roU73hz? zv-6fKVyo^KWR}tB!mLwaFHE3d-Y-=cRUT(VIEA)HYW>5>{j}iU*rdp0p)MB!39^3Q z;9n{6elPrj;fTv36-8}Ecwk?p7TmW79S2aQ24&ltaHi9HS!IX)=LN9OUCm1iES)hh|=m(0lr} z{U3^HB9YOeb|b`+g98LZ+l|VPI8l%51WXF36dt8$3c;+o-G9z0&J+=OgyIOvN12Zj z4YMa!jtE=B{%DAatnBe-cbNyPFGbzS5b4~+x!;Ym>kE9A4+0e`jjZ})u5DfPNScxP zl?_Ct!u}KvS3KQrDzoA7l)R7=n{2&Y58Y(k52QTb#@A|X%+Cw+x%@AeDp9>VBr5Y< zR_ET~HBB3mw!4Ak_LsL#G8SFSBI%2C$C3ufpI-3RMsbOps$SoYePWH)n?VJI?O)+i zTKI;-skFxn`Lol4A|QQWoYc(o*O>-`_?o#X?rv9yC<#fIN(uxakj-&@U70$t25c#- z55)}bYp~|sfV9WKbmwpB5dz9j0oIs%I!k8S)*NP|URU&B3{t5{vPz?|U-Ui76Eof`*3g4#w6z@{iBI%+#< zu|>ot$TZuILyU-+r(yVOkSMb4t5V+S?DaSar#WYXyC?{?RCjyc>~xqY$9s$VeqIA$#vb?F+hc#XU)Dzv!-HiBqJEQ3Fzu22Xp2`#8Ru`_ zhHbKLf{?)zqlb5PUCaq<3O{cFZf*gwx;4rRhY3TSODbE*TYlVM{$FLsy9YVWK=)Sk z+luZ>i;n00q|4jGspxq!l1;Sr{6#RRsT7KPtF+mNV<$gJXtKx~>0oo`BsuN~;Qp0y zlx%q#n!e)uE~IEwkaxV0TMVb7QV&rN#i(nTuizHV6I2D{S#$t-0LemJ(1CE2Gm4fr ze_)~nH^L}FrtNffVMJcIaG_xD_TAlaqiVQAEz1f*ozj*8JKJiSm z*lT?91&-129XM^w_S+gqt?6%&rljhos|#~^P|oJr%yY|b6f_$UB3>{)ARc>rk5|=+ zC8P>BE+s|mRzDdmE~Xwtq?Uo0*ELs4X3^c)uJ%xU#us@o(G+*Q&Pz)mid-DnUngY+ zLe;qZFO#}KAja2z7`;%Iktq!&32#1quE;?@c>nwMH(ShRNQR+7N`i%(DA0US?t!S@ z#@eSQLs$8)&lL+qSiL@aj(}LVh-v|8pyILU0ax>*? zh}1ZhqqcE{$_VGe8Z~EXo;!wbuf@f#$AaLO{dv7fcK$m>IlH5r#08Gw5uCi^pY~E? z!KPk=COXD6uX(bWkrlC$BJy5Dj~jBQZvjauBn=8S#I}!FPxIKRMP%SeYG@4t@8+g> z(iwf=GnK3MN29>=xbpnk}@^gO)C1$qf z(L}X*HZ-T?W&R3Q>2U*>ZCw274;Y-rr)I3n+r7EAU#;4q23`O%U2U!ssmEEJ-?HG(*@9noQjoomhna$rU zrO+I&W_8ubPJpF)s8?6 zvNz=d?w{)%nvzOfCK`+wwmOLxhG~{TCJRFN$cd-?am?)+H2#$%MvjJQFWRSdCXPRt zz3qw^zrU&^Yk^^bNjR@SE4_&LJMvjYXF7o6EgLpWOk9{h>a?IrXeO=V_*i0wFTfRHk42?ID}SP zIHCu#0IT9VE@X{?fg1?G8}zuu^hKVBw-qaDTEw}f!#7qZoVA+sIx+mZ6vp3^1$Op$ zAEQ5bn&2O(&=%*M*Vdz91b7-s7WV_C*F$7LPYRwsbNZfDaEf2xignKS2EE;Q^CHT^ zz-HG$u4rtusKI0#8OA>Iq#SLW@6>SY`R|1&e_zbBodH@M-@NG$`?fiiO}Jk%>tu>j}vlrBuZ5C&2PHwJ4SH z2wW`8JD@9Fnj`y6#_Wk~M4!hUe6jeuaEBJDizp7f*Xg>^Vn}l2L{%Nx;H@C)3>{CLBt1Fj1!yug176oG6NoS& zZTE>CqevugY$`T0m3su0b?QH51Xf%v7vguqYeFtSIZSe94lNFq{+=_R_~V!FS8H5` zsH?c!Y)`R&dl?wpn~h8p0-OH4$0VF;QFff0WAfzbICnj=U>uea$KPCk-`CnO6Z<@_ zf3J8hoBF$80_b@Hh0ns#3hne6^v2KV=G|D=NQ>9TD33i}vA6nU^V@>TvWRgk3QMK_ z6vCxcqBXaLU`grXI)i@QGUp$fqU-zu;G>r(Cc_N0068NCzrn14*!tKIxA}=2CeKyq?Eg}N{?Grrd;ZI+LhTlt?STia_v>*57=8o2mAO=u?zT(N z)2H=6V%LB_hBnzf4?T9t&o^f1+M%U^pbTQQ9HGY59Z+dBaMV;J<{*kFhnkWh*=|M^ zA~XZfOCUz$pUZ(%#A{@?|V+0hCCpKy)#6IEjYZ;agP)i{6*)i<7|AN1783w=GnwQ%b$A8D5w>Ili zfhp6PaJuF!ST!(g><{EX%gSjVVO;h&@+kK%tqBv60eoQvHRxTXqK&)3A|+bSqsd#3 zGkYvyPt3#~mmO@BVZxVDT0>e6D31K|&+oaKFR}Lf-C{8B3CmY#UUgFh*8GdUAzorL z7GHFD3cCC-=H83cQ{BHuO5)u$`>$h*;UCu$z`8c`+?UHmzTAwI>efwI{qqmtujqM5 zooIo75RNi`wr|hrX--cWYASv4ejEFLTWp2eWlI*UhsVd%*#GENna%^zv5}05jvFh9 zL#lS)Tbo@kv&rLZS&^3&5T1F+qNUgzpMshwxXX-$vbBQZVu|&^mAS~PTxfJlITBhb zCa{d|2ASdJd~Da`Owxd}bQfHGJM}pWz^}C=R&MS&>HsZatNu0xIK6S?N75&v^?M(~ z?{`P#D^qOV?|R@?DJdakC&ydU6Zk3AmJT#|A|{b)|z^pNaAssC2~rUr*5W>DP#>@ zuY1w$hj+bZ#x*2hZ#w0crX6oIh=rATQjNm~X}6*0d{dhEKKvq*qeT-3`Xz=KHzVwM z)28?xc*TR|YyxK#oPpG8C}^nz;we=glf)P(!ASuNlBZ=A&K^lz@+Sz<#KO_CC3uxY z7^)%!cX^cpw9=YZe|eI)dmc-%U#q3RVTv%IA}1IUS$xuG`n<*Pyi=~XJdpjpU&T2H zDjO0a2D7v^u(Z*#Li#5ar?ga{AOT#n=7dkS+hguY*uU_?l?l`9Uz4#d3@r+5z_Wm2 zIf@`-4&wfBg*lgsa#)l1<#3#hPLnB$0LgKB{VO(}w z54C(ZyY1;2Jy!Cm>FhwMrMivz%Nuc+CiS{MWZ^A$P4y&CXY^AKlIL+WkL8dpvT+`w zU^Do^HczNaa!N={$zonP-oLCarVAdcq>MSlJ^H7Wit$@D~0%c*)^PjC_Z z*M8+nAchcEPS6Mq6ua|GJJ#~#y>%&-IsEyN8%~ufwTYl~kNDS!&X)UoP0o%osvp_| zc+11bkx2uz3snVFHQtc;^GH&u7j=SymNRO!>_B56^WsKUZ;)*sS?(on@r-i7fLj77 z`To`> z4da0^Uqlxu{Kof2)z7Ap;g?q?<$c@mBV!z}$knJaQ!;qcVaK6py$_-^yK0$Qy41vD zv#l<0R^ve0sX{KORc0Q)b@j|*euxVk#{y3A{97yo#+*U}JL5~+e4<-^!=5jbXZbt3 z>>~nqJYyO;*Z4)sho3PZ=ZV5>>f@hEQ@{^yAl{rWF}Z?0iD4|{#Rmun8zR84TmKNI zd)phf!PA&o^57`x844*;d*?;-N!e{6WB=Z!cRu1YH{yc9VJ2+Rx0izY?>e6e z#${p>^AKVr!X?sqiP%zUyr6CrT^|SAC9i5DFnv5ub`;(ScpJc<6^d#+2cSXAzauP{ zM3&V6oI^)Z_Ay|MLHopD@cloU(&BV0yeSu{=I}p$GqH#?xt#V~{j~)DBdosc#I^*JZAEB#@#h3^waag6>*U#`l`k#- zx)s5WH`-?(IzSaK9D_@#fI_jt4J`~zfc$vH7 zWq%Z1Q8vFGfqtA~Tl;-`yWPB3Et+KCC73d{#qWmGx9{=i$5bilktON!Ao09Pm`YS+ zivmMJHhQwu=M`K3=-L0&7N_yV!Y1)3OT(p%WmK77MU>kU)HuU?Ww-0_@_MIbf`1$h z-Z@~7hWvCzp{`N|;S3K=6csIEpPSu3bh-QbGk~1QtqAsN+#NIUS?8_O=cPfL(T_Wh z%ObhnsFkd)EhYq{)k8s@L`C*SpX7ISz@%X(-5w#BXG481GKt~Bp$enMqsVZbWB}Hz zL)@$40F!ngAU~kr>c;hs>?=|H<+;`;6r1nX5zT~>o&l2lu#Tc)HosX&wz^5OL;EyhQo6DzZfhrtnCeF7L;K;=xNYlo_I6Qk^X}nT+`AvoA)Z zhGUQMBdl>af_g2IT9u6AWJ+WwE>rl3=~%HNJr>{YkfrY0EU)`h(|1jQ+g1C?D8$7? z6uK>?9JIQg2NSx!|8fRygpe>cqb^WM=`daWb&-t254iH*jRENsUJNcNI5Ey!(!Rv9 zjp^$K@(wDn>3hn`-{*vXaN5=ecCLAf(tn~Y9gy<4gz#y3f6ed^h?9t`Ai#*h3Wbmk zwji5ywyiG+O)HFFT19R$K1D}tdT+hsrDeUs*u>YRwy_JCW@IGH-R`S#vv0F{m0=xw z+h$X=*%KwRZd@khGILwkfXf!PBTi}&!EoL?kTfkY z3{N z;!>X)kG8okavX)Maw40=2+U2%K5ck^%S-4YU+WS`G(|6A;5*@S8W3h78^04_S!->) z*y|sCQg3UM*tp8LwOX6fFsjNoy-yXfq`H$u!Vjh{_TXlejrW{f&-Jz_E z7EYQVDY1(%gfkHCNxf9W9c!J77ae8XV}yvLfhDhLNuBH>b=LGWki! zc-AOZF%?V(#9%D@*7_ZZjr=Y`2K?hIF((*v#>lfnbL_Z$P7bNj zU}p>+F-ejb6aC_0)c}0%YH6H~=Nbk@8s65Vi3Kf4=yFYOL>g5B$tx?s0&%ccJFw`mv?25aX^?Gn8`Q{j)A&YGBD%! zNCNlS+Q}WyE+$r=>PMAGiVGM(LbNftr2{M&V>~vpazenwUddsBYR;U{t0~uh=7@qn z1#{xL{02@nr8(h8b@`Plh0uKO8ncAB_FDiQLoW$MSC3`-rDVbOy_Po%<2tvSBs~HF zakrWCwfRMFWI82S1C~fiNN7uJ)!I)RE@Ny}P5XTa*LB9qSCG8 znrD@#1YU`8#x_!kL>NJ}hdReY(()@D?0Ev@eP7g~D2A#@M53NWw!D@VN7bV;wSEet z1!Kkw*uLw;Nbhlkz0WXQNGDthRg2R9CGh$F8vmW6nx_o3DPWXykIr*U8<4py%|N4dY8G(-Om`PG5{kXzDl zHj2a)|92hseBq8F%5gkCvVxpg_*5Za3%;{cUMxqr#QE{T9iMi|GHBdA1jDrmg&34b z08e2gOrU|Bb6rdt-k7PpIyq)@i+#E|N9hx=<25|zllO$z*r9A{l~cM*4xOej56mB48Awa&z(f$c6CMfp|RNv zT=Q$YnAZ$-twwukT`0G=e>M%i53S&h$5+1kCszaH-JDO=_7OAvGiQ5acTntC0g70n)Rz1Nvq zUo%D@V~Y7hdlxff*~2Xe6=c`x6uH5q*_{tB5-Xx*Koi1z4Fx(lrqC}X0(eMZ5b+=G zba`_2tBo;Ye9}z@3{%bnv(V2b*NC`94N2?>rh$#^g9Lv!aJLKt(=UkFF=uYlGZJ}P zWpGYTGvSbC5(m7}DF6{|J%Z4<+)Ms~*D>)SLXqeM2V^{EHVQ`8dgUOMb-HnyQd?!k zhPtYrF}~cvJIy)zc|+@bKh)n55`F7Il<#wY-E&d@0~E&{XW<+H6)8S7V!8KF)~0d% zjfPm7NGpK7MY{y+oL5!#EoVP~q$th&<37C?;tVKzo^s?`3%0y_`}fIKS2KEWB)Ga- zEt80v*RwZLS`3`_-+g5Z!&RIFW}gj6Zdp6n&jY)#tM=r={0&gYP`5$>N+c^mOhGb; zGCqCBVNmkt4gcpychtLBtrodRUw?W%w*Cv-m+>8o|AEPap^F)R;#U~Td*Vp4kTGjl z`$qoz#eae=Gjdikb0v$ZhGh}YKK#+A=~4sRrc){uL@2#-*cueT!vsfkD3C=nBn?Xf ztYf}^saVCFqr%-&ZqB4;Y^pvO^5wZTLn`e!lW~zZ)-VsM&XxGIdO??Yz^0wg{l!g| z9j*wr=Rz}g1g%}qeYx=hmyDKb!Hvph^r^gHM7YQ`YBVcMm>+x}ILAo&E*rJR6KH^x zX1`iLg@e_FYd|yi*ig1JITBa<>auBWGkw+ZZ{rYzPjaSYZTNK!uX-2cyYNwX^0`NGk&B7a@HhhEHIPAx}h{o9ff5og+0g>nARWB$E0RXd&tkrbN9ELm$igBJ#8+SngM;tJgN2k7)C7sv&6JcOG8BX9hY@Q!=i{y6wy+Y!?i_lC!76IW}#n%n%aN2?0mw|2TWU?^j-ZY#(oB zzNVG^m2S~7T}>lx4}feLR7#pff|zRi%~oRLmrW|oXtJDQqFfhz`zJhWXygxOPRNIl zWP?Be1_QeCez(|6>!ka5uJcUyvyYslLJp)v zaROwBV(o=YV?>p0U|qA$p7|n_d*w0s^u4fwGsew;dslA!<`b^o!`8FIzuF$4&L@?V zb8ze(Sph3sJeR5%oYt(?9|4nv7>3eJZKa&n0QKR&fE`N6lm+F-JVKaYIYPN#!5X`n z{rDPmZ9JKW_ajxv7kTPu2_%Lto0ypO&0kfXGb~-Nc44hHgseeeh*pDL$m( z-r~nbs0+|#-OgD+pmAEal2UR@%rRTCeDCbc9BRPtp_+GNSpQUWX?bfWAPbG}OihJR)>dk<3pT zw`_3?S&ZnPv3xGN6<^Edvvr)KJ0=zJZxis=26FTF=O51=pD7C3;nK9Pp=%hoKZbn; zJA+asKmo(t#Ibh;D=8Hz#xpMM`9wj6ri82jJOb;G2m+Ddz)65%{|{5&*qvF}EFDcI zwr$=qC$??dwr$(CZDV5F_QbZ{Jm)!Qt@jV?Pqn+NtLp0J`7F+Kvl|Iy4ktwD@_@nt zipux88;jB$P{ESY7(kII$Pg|xLnAa>TA-7Y#*LKfBeH#I&E)xQ@kCXs)5z%8`~BNi zWYWg2Icjr0tqMppr3VS}&VFf5Jne!XM}%fsm3}=`AMpTSKs{1NgL7;T+A1sU;K`)# z`gKmYi-ITvFDTQPa%3;%+c+F1t8NfHw9E)d;el-M(B3vV?7h}%7d59^lwnF3x5!%NuF-aiPr8$w=2?)pcr7+j(*~BCRtOu zbcPB~a~iQ6*5X1_5aeB?P*!fV4fV{zg#t1o{^x?3S>p=2EaWHcyJky6vTjcA#3$R1 zvFfY9di1X7;M;8(yQ|`;GH=pI&JGMrh0K6C09h#QW3P6Op3Gd{`r1!fH>do-U zSc?;_v(xT!=~V!E`;aosu0)*|sI-iJgek+ZjQ&uEsz4n~Y7@~^(}}w?YwFUZ6u+r2tVG^iRpIjB+gZ|genxh`T1#k%l7*}Yo*5BH!F#}9EY=V zvYf9$rd@q{7nmVP!6`R=&CR02g+HbAj3e!)(SmYovhG0GY5?HW?{U=8G}5hfKEd#V zG!k*U?E{o7V8~yZEra@rPJH6;+|de&%(0$`hS0?x1oYqmVQgKGy-bKI7|jv`P_Q`R zC9hdT#QGIP5AMdZo^P4bJnyEyaN;1BBp!{_vc!6TuB*oxQ6o0`h&OSiMB1&*d1QhF z#Qk+e)Wrg!G8K7n`II}b6wwjO3o&FAK@=tWqeQcmoB^dB=(V|Thn5WVI&wF}sqt4c zb5Jrh{-Tsw=y)jB>nA+YC95-hYGzam3Ss;=@Hw`=+y2LDt2d@BuTjly8=DIK_Lueq}8xKq3#G35QUSyL6)ds!~+Ov zVrF@Tv5HccMip>~Y*Qc%hZx_l#Ztyn^7n^Wth3}gy%cUFgcTE2Qsv?0YD257j&3~9 zm#a>UuiIlQtMGAzAa4Dg615t9gs+pwOellOz6B8epHd08tTE4AtVD$x{Xakz#hhoR z!P+YiqNlM=I1U!~Lnuk0yn5@`w&J=}U(@JJxIRCnQPWx+rLn6!q-R6X7$ch$VY;JY z5_yJF-{7MGCy&D#iX>Xz?IuRO@gEow?<`3<3|=Q?L!&eju16d^TDuWmmuDdB1&e0OMEQhEER*Rg0w#+mo*imGsm?S@J^tEWp?6ElAQ&Ex^UEc6Tje^*It+~GYbk1pn zZYlt77*t$)?Q-(!V^ZdMX3Fw$OP;pVb24$&agzkVbgbAi*D5C#Uv+*Po7NvwXRdp) zSSqF|P%Z#xL_1Hj*YS(6H!o1(LLr(7TN$4yJBML|EZg0^`a+yyi~^bQ4ti^EZv`k3 zl#}w|EfMn65~q=yDkpMlq?dZ%*+uR3cVgn|?!1o260gIHLv~HX?%2ciQDX$AroXLt z5U6AsL2bMTFXoTOTnOJ7h~3=@Cj`I}F1`9v7bNW`5z-|Y$}WnjYeYj}DXI~tnLi?T zhCj3BQp4H748z)Z-stFf&1=BKl}I2=&VjDOSYO}LwRNRDmwpDi<|NS6o7|3d9jR{r z)%E=z2#O?{_Xo>D!jz;^C;BUdQvz5d!vd2eD#%bz&(E_JzO-_BgI)rQ1GXAnW5bh zfiu)u{VBT|Ow-OIE+e5ZPaW`=TCiH;a*Kni5Cn^oGf3m90FdS(!5l!kPi7CS z?n=7RtN(=`p;0bjZ@k7>R>n6JzNeK5vn}QQ0v3QFVONNPfa4F0Q)-OQPZ?&uSp+L7 zi1RoLS9TkufPYUL8)d}$Ykd8c`|%;1s=&SXQbkr&uimCnxg|``I^Nv;WM;FFJS;Pa zfB~AnH}BdTNvG}263%C#H(vU%;n#P>#6G4#SyG`iBrlQiG1aVk-|^fFKdvo9aQBdS zAcgJR36DYBruEY@r>C~x#-Px(bWEML{x;ad?6jxnAecH77jR6;7d{u$TWh~KSv?5? zJ@(j1QP?dO60|{~P{{qKmWxhvlyeRxiya^0_L*bL9>mb_8V?lC2*^yQ^u|lr4AQ31 z{DQmO?|$^f!=a}hTT0nTVVd~9B@a;1r7JK{HuzX$^M z?kXUlh%-0EH4o1kf>mi>BzrWa&xkq40xx>md+V=M`S{YZ71+bSn$)xky?Gsx<@*ZZPmL%k$@mD&S(o4Kf~` z==6c{n2qZp)pt~jmHgug)nFgsg@PI%$g>5%P$*6^2`fVlsxGwOj5Vp3G?^vWQ;}Oiz+O}a%T?~v zCvUUr_Ll(Vu=MmiB8fxHfkRx!xf3g{)h6WR_w>{lf9svc?+4103|9w)y;Fe8<&#HF z6RX`oP|Q4JiOuJM3;?tc<>GxceRYCK*;8pHELP3HwchWXufdYEA&Ige*!)WArOy}K z1r?&vm=lt89bq$GyA~zPkr|QlNGBA{M4!r#LkQ)iRWXGA@dN}u|MUxxWrGx6nvxH$ zy1=V%Wx2I(N`f?b)3=^6eH}OoeI2H|M6r4tnZTZQ*{RVu%yhhNBADEN#=n*wY(+WH zUok!?3SMPs;ldV4Aiw^DZM|M}l5}9eH^Be9>ry2pt105S`;+SH-!VKwh;|6u7_9w} zK}lbKLVu09mWq6ci$vZl+B8*qJh7bwNYOBIk-$ILWl2thJjVnTlt^+W>*-7Gaz3gE zm!7Ui1db)HxWjGjCS{mQFnzTX7a-B+cqyI9e8;f zf0;rtK0Ns|reXt72$&xE0gOy;ExJ53yD*Y6yylix>bR7}$*P5i zT?SaZ+GR1ZGB~C7Qs$x2dsPMM0i)I8f=;3*#sKK7U=dWTqo~+SS>6_fv6cq&5`JFt znh^dxp&+FCI4Xssj}KG=iM=!+w5j~D6QVY$9(MwZDQ_oRG@T<(HmMn;#@vv-n$HDh zQV0-K%Vhz4>L z@^NgsKe4<%-=cP>4nide)M*U(y z7`gU%qWNDqMH5ZR{pf3V>;tr#ju(ZCvx9uCu{RkEJg4S%w<#=(?Z$;tm~}tE6JzB3 z)O-e2K7$a1F*$(sFD+`1+d;I}i)ZgjB}9&mwje4sZne!_CJwhDv8mmgjxUebO7@yY z&FJ6Tq{(*=;zy>g`!{JV*CBObVpR4Dc?7Nhf$UE?CWcvn0!rQ6JmF5=VB6`(_ud6n zHzCc%elD)`8>=cQ1o`zO3Fe6wL=#~7M#P}IQkZdhz|8c8WlGOOh$5?wt<)?&h!Mgl z;dl7;S-p+{9iDT#T`JbAx~r$xG5J~p&*@LVXo`P2eS9iC!Ly`%Kh#7N?4gL$LWCgf zSvj8GCD)ZuuUtY6Bh-AJflBl{KU5*Zy80ZFJE$hIj#J9of*TBv3Nq>5O1I+*AkykI zk5aRRiAWYg@6W=SuN6x{uAlNIIduUu<)zPee_6GTy|v&2mmwMzpS@6;=HN#L88Ai6 z4kMOOi~v(o#|S)yu@4rq{C5#Z28NaDdW-K5jr7XRY%B)ux!YrH_h&NKQ$LLxbSKTC z2za|Gn1yi~gd?LE3o#gJERLL*HCjKaDr_^7TWy;AdTk~Gs-ArzzjfF2YalT|KK4FB zo3De6TD_xf)N}&^k=W=or;4HRu}=M6*_31yY2{2`?snTVfPe`;o68L;wfg~y$7f?_ zUleyQA+9I~A(*u``cRxSSZsFGFeJ#tH?X(;w)XWvl9x}2Pe_eo4OE392VK_#Qn&MJ zW_8W!K~wOTEL8}Bti2UUrQ2dGsfwDp#KY3L*JWew5sPXMv@Wfzj}3;Tn1yYcP3#IgSte$;e5<|^WxHf2GsRcas^`!Ew6+<_3uB6IKHUx zk`G>5tyyw*!CdN*hV&WSAj{4FqDbBXOdtz4Q=}XWx`Gq6g+N0^QZgtM-G&3=Mz9zR z_50uq+b+hb-M>iDk#n55gxr(TqxraXV9E@PcGeQql`S|iR&F>|wTF1MykV#M655i$ z#BPEhMEWa`!sUS9mgvjIrF|dKyY^vI<#k-w7phe3O1(*Qxgc`FVWFk?+fGFh!1O@8 zcxK>$b2}#c4c-zdj=O)ryb28!Q2C!p@uw8q4c260*#|aIHeRLX47B>ZbWe7W>O~3P zw%)3?)NG2tC*Ob3u1>V_TL@7=O z`l#~oi>YbUIb{re(pln!ohdMO#&VXdN9l@s`uy`Yd=9gT$cPmsAt19C?Pvm&Ny&X- zU-)Lbp0X+iup%=_11gn6%d$o4j5{3=$WjREz-UJy!fK*&{7+$EAz;|_%D}0~^yh5C zh;GbPd?%bbDS3~g(>_lv1)705zYWnVM?k>{>wsIk>Ke}vV|^waZ8Zn$4L@OG)l-&! zmk$%Nj#4>2Ql^tKe5DN5at$PbLf~ zl>Cu3TRBL`!Qu`8NX2*M~Gm>t9G_c)<(|HFNX-e(r;frS>nDG_B z)$cAMDvu8~gTrq00gxmjB{w@Cby#b(bSV}=v%xEg^o4fDO6|rrcKTa+Z^)yPBw)%F zR3V0lcry9HwioC^7Rv+@V79H$?0g% z#T8&E1NiZRG4DvI+uOgGw!7}Ez*~GHHQ0x2S`*0=f;4>bI|Gn@I7#xyyroVw_hplm ze=vU!nVEEm0z%@-$oX>IyNtuBlQY$tZ?D%Wsn>@fzCpbsfFY&B6-ErGAk0R&VbThI>VYIv5g5N?A5hcPbKrD8ea>cod1Pwh$kqFeX$y_Qu0;V+^Iyi2 z*N50taP6%bBe6!r;V02N-o5k6U>VCB$bOd;uqk0Gr6S)mlm53^b*NfMafjA;kd z%LXm^HqS7)&}JcBlC;l z@bHY;wLG>NJ%gM{rggDRmOQ^%2u>?eJ(`wOO=)=JVr_jrw=$CR_Rb`ag;V11#;fdB z?`Pv&3idu3-F5Ux6-c8w&AoPN6UXa};l2z}H zENk*>a)8YC5g(K8va9(h1jLhh9$_j=lStf4pZ`!&xK}I_TQ74;O6X1Dw0A(>L;>AO zRq0}sKOXFSil0oKq}ul%u`~$PlyIZ}vwks9@hm&>b$m)*=Cb7-T+rAc6ZkaIN(P8(1P?Swe)UwcshZ<_6s>63|&qBB%OS}e`D^)iBCF0L%!ID7I?-|<+yqxjG zMbDtVRXFP@l~HFUQ1*oR_D*O+_5zeOH4o!lN@3NTav|j(Sx37&Nv^j$vBB;`Wn@nS z!t#&HeZ62&ZaFS*O>_;CM<%?=dS&~prr()eubWUwvOQnTwjP8>p66i?R_pcK+UELs zPL*re)0QK<0nCi47P=X}B|nERI0s%+SL_;y+8T~~qUijzP;7T1&;Oo~D z&YRET3(OYUPF(w9ZbVrcbu{Z@daYGRRO?VgD2~X;sPx9`rtgS6qB5g3iR&99aP$dF$Di;E#>DDj>MW`Ej z80Dd860Z+x{Jp=bJt2Ouw&!vV7*xwpH>I!Xzkx z1n)~O6Lr5UX#)1!`T zhCZUmO7+HhedzTxabVOQNJ-mc$lf;7yWe))U)pZXBzf=gobOyMAGBW2fs!0~xOp{ZK;8-w$j z=v6;-Or=%(>4n4Yy@ouDh6G%s&_`tAQ;xQfqtQjqf&ejW5qT9TSD<|d5hh4V&T^)KTpa_hXz^?8KlHJEN_~1#XY8#oz!Xe?O*c2E`!1QB@S)omBaAf0PvNAG& zr<$CiN$YiBI+32|vLi!_SBqS82$DuCQ zz$*NiV>D(9Yf44!(qEr)Zx77t^`H?kvM&o`n6Dxy$2LPZ&tGl3^LwAw_)qi3tNRJS zw7x1;?G}1{BaZE&PC?O4%lU>{GA4x>A^lN69N5X-mgk*r1 z=I%4?bzx%jJwV&Z1u@ z#;fBdP0R`y&`(oWEKePcW^jq%ecfoa--g}+H_ns5yG{BQW{?lMuF$mT7^X*LqPY@X z#ee+c%|UTVQsBoRWVxaM4I;7-1Z+s*E-DOmf@;WAN*<}Vgy^2X13|oe*F58!1MT~) zEeGXBC0d$ZGk42sTf^?xlP&n&9nQP*!M> zvM3^Xay-qkurphI5eG6sjsaHS2mEI4Kpe+*zX0^!>J$RHy79zxD~rsi=X1k9je(qa~1$~)AC?bJOpz8799 zs=`BsXJU(c0d_Je{SQv9R+MM=^FsN^RT5~*Lexl-CtdKTF-jJqxAU5&r*^Twc@B0o zYJqFsN5QvWHM1#dUN8jj=fKioOvRU&pAD00IJ~&ywIp&_C!M2$iTFYsuO8@)%{&OF?n1UaXt0u5yT z4C0fZ7M3v;#=t`=Lo#>?5)w0)y<@UJ? zw)IZZRAVHyoI!%m)_BGOo6(?3I9z+OB0bGYp-qF35uS59x{uIt_OE%K)+SNHu@TIH z+V1nMkNc$+dfIV5pv3Y*I%YRzu87^!R$xWiVyeG>k*}-xUwG(W%eH+Dq`0P0f?Si& zNrw}tsV>Ma)%8i!GH|)dqW3v-kEV40oCY$l#_>WfAFLytNVS_AC!Vb&`+l8d&^(Oz zSAlBEyPKpiBk1%;82T!FYg$Hv)7q`dk&FQTM}q$;;Qf{#R)~iQ7%X-Np}aqj!d_V~ z_vy8VvkSmg{R2%<|9PZh34(@IkDXPVl&`Hj>!VbmgTUu2g4DjQd!;4jcw%}cpf0LM z&&L`^_>rN79>!~_7YNRPOxCMSh1K+#=1jb+3Uu@N>*OOga5y&S4JyeQFj6cq1H%nP z9wKGfCRj_@%!%>=GEU+ax^m)=MS9XRU5IQnWcF!vj6CF|WwoTPOSZ0;OpAh?6s8}o z#`QN#yy+#IZbfH0Q%eRPFg~4P$kKRtp12e*t<{z`xive92s84K_3(CTwUTM3fc-&HV@Ft2Ifp7+rouRC3-&pvzJUB{ob zdrZ$>8SAGk8Q9z&DSt(b&)OidDofKa)&vdRP}Et%bvr=W*kh^PL0LnPGogS+~37{llh;CB^wJN-)i;zX$qk%4` zE#-}9Nlr-WvBD;)eTey>IcHPauuZJ(hT!}uSJUP>fy^bH$rVGI1TI(I)7PzPHkirr zLv%Pz*?bGcF5lWs_nFOEXQi{qOJH#hBK5 zB_~xGn=!FS-y)%RLN)ITL^Jspd}6F`61mR{f7oyz-o#mSs-pVixe}W8pt)%eZh#`w z6BCNozC5`aWFMR#oL~8&$W#{wp^rYy0y861_7Ii4J!Jl~wE&7qX&#zLKJH)B^7mw` z8=!~hhx=7pLua-zPdz&xQUU@6XlHmuxqJ{<(()noqGBiveo#hXZr3LB61YWz40%X2!a+GyLEkfVM2gmrBWJJ2 z-?_)g&%R0A=dQ#v!T$HZPj}=GOnzFKK}SK2L}|@lLF*@LCyOzovl}rpu3x{~SeW}Yc!GDc4zrb)B{|_YM$8IHGL;Aij!`~2wn*B)*cWZ z-N;GZ{lr?37$S=2jH|Xh|MOANhXRF;1rbz}itcE8{0}u-?!;cVB~U40%6jLrmvpMK zX6KD`BRUVsPN|R*;o{O+Q?Z2ie+Q*X?Xhy#!P&8VZ75Ww^=6NBObz?NePgo#K_H$n zi_J-9(p8&Z?4e(qou>-^ozovPIWu2xL#7JSiEz9t#t*qX#%99GxOg;Fp>k>^1NP^z z3%P+q?%5}7@2)16AY3^t)H28Os#+5C@oWLI?D9r+K`Fng`5JNV<28At zva#~YBu`>R0Q8Y+&Y^TzcH@zBJz?aa#+|ZhNcq4dQ0Cj6G??uQB4t#7Mqp}`!3%ve z@8B~`QWrX0V^`y(bvfL|O#~d`pa~ca<$3xcaby;_)D;~RFiRvWIda_g)a37O{FKcQ z{1-}%H7I%50`ain*)!so{;4Qf-^5Ev+r9iRSp|+~)|_X{zJywVo#%f{yv9NAYU6=` zBO4i5e4~L1ae;dR7fS1%str9Kw~-{S<(^LFKt!x$q$vub7GPLZ1A<3&Mqy&`#8NXD z{|MC-aMG{449iuI8QI;qWy%cVj6c4%4VnJT#!g!v!z)3WcAK=}F zDK1NNRqYH3`17E8KBj#$T(E#|t{9&Vef=te&z&YHh7(( zIFZq#a*u*N$r^_C*+ahvV8A7F&6k)}gM{J|rn7YuoY5NtuZ6ol&zWA8?rieoKP#GW zA}XrEq=&XlM0%qC_=Wv)!ksOJkjpd7t5>uvWPzL5L$rZa3(1NTp&#|u_%e2hRh-t2%mxn5%6(GOgEpOoJj^*NFrEb zs!P0!-3C)IVY-dnLlIAoF!(Or7(sGB*30-+XcH90thJ^Oc0R>5&gSm!!g3nY9xOPEc_Z_Km5qK$cWUi3cTUQBU<9QT=vV4IPaO?{eS3QfonasU3`3oOnl6D@pRfhfLYUg^(oo{RlkQP@{*RmUb3{jqN^u*L6h zyNcLm4!-X2QPjjWKCD55gZdTY1}2RBzhQ@8F&LAr$Bj3Ry>7J2;G18~`1V%?j(Xt) z1(#CSKQv6rjjwzEvZrY0U&Kqr3IT#pqleI$_DVo+%My!4V$$x@aHQ|;eVifA!q+AC63A>pRZKDc1TZfIhuQ;Y%F5KQF^OP;B6eqv_ z=4CwdgKk5xGG}?N{TxE$?atUrq_=`q6c<3ve2nz^_5(yz(meuD-5xp_Ds^!4N8lLS zH}|EZ*u-ZV=`jQh>v{Uv_`3NMliViftYq}O#I!uq2v&s+*^OQvOK08g?2Wo&-?&}t z_q!a~T|NnH?83@#@LIq#(>8e|J{rl04JE^JaP-C&)6+y-9kZ0mEW}7LNssl(V5&T? z%GLSGn#{V1*CJ?PYPE)Jdk%2Dj@B(P>j~`_63h0Nj1Z^XGycj2G0Fjhqkt=<3fl=k zA;jcIj2x9Cp&I^VAETk)9&cjVOh*tFN=i)XuoxT9^sZ5wA17*N4}Km?Aa>oPsB2%K zh-D+GOLo%o)uU0`f&Kp}nW-w{xpMNGM>?<69yiL5R7X7$ph7URVnyg5*Y;EQa7wFv ztf}{&aLh=K$zPcz3|r)r%HZt=Kz7WI1t2TV_#OtVCJ}2?QLCGAQi^tkU)Twpd^B~! z|I@#Sy!WEsqJq$0% zmO|od$}-GxMP+s|SvvHFicYSc4F^z6#D#_&mBS4g!eGjmxIaX*uM;k=PCuRs6x9k| zwYD)lN_b~D?`zj`9k;MN&ABy=35 z;VkH^aIO7-eNJF|r$T)n{wxtFUU;j3LzEN{2qcT;enswi*wS1z(VG;+!ORFR%pnUJHeo?f zh6I33&t20{O8}2ZGna2MMC7yucus+lp=h1+AEsEBMuhjT7-%i*mNdNS024Ao`b_S* z#2mOpHxhq>lk9+;*=A}|=#Ij!d>|%+fkzQOWeib5Vi02x zC=r5;_vNA6v>kKke04;Y+i3ZU1M{9gWh+Wb{}ZSEkI;wG z*LTAp$)JXo$#3&H&(^MYd2e3Y`G*~?7-YuiAF!I#FM0-qW9a@(gbQ@Xzm2qe@tnP) zG`PgaZU$(3>k4| zt=cWokb;FX1??Mufm}X583VhzJ@h4mQ9@?4G@zW}z+N#0gmdeOHa5vq&w?U$cf;p ze!S~BC!G3r=c1&fgeV@P>DmONE7MP-as$j@-}8(#7bpoDR*ge3L?xt<*}>eSl(U?de11DAsxr z83au~m4C2|9R$67XvP_giYFc_7_?D43DEid(+iRDEsALY%PYTPKv2L|@)aZ6BiDRx}yJdc;xfS!Z|nm$w-<554-E(2ItB!PGzi~n!Ehj%F%jJH@$YZ4TwpPPMI z(508_=cY_)QZc>r-mwewh@cyqTWoai%No`^j(A)xOK?eSRdP!jHznM+(YAMV7`=4` z`^+U}4kA*u`bTU&F-*#X5xg!S-)X`vmUF9#J_WohM4Fm}4(d7BykA9pz8iLS&L6qE zo$j>)txvLDd44*&V-h4~z^)jkVU%ad)!zNi2UqS_Z|*Pvl}rLTXa{*pVodsV35ro4cja@gN60!^$2Zf0R&al=B1xbW5^*H1XDra>&Rw0mtvkb#^6c!{CvUdKR| z=(QwTwrihL@)B5tI^y$5eOTiL49!Uqmj@f)57$+Hu3^x+?TFQ;h$Ic zW+GC-gtlK>&o?u^AHNs73ZoQ=i;alwVpP=o`!I-3Fri>P_c?Ogt~7L>J+DC@%}{o` z4emRfm?Usp4u`0Rp$cA$r0)^B`VQkYph=opZKn&ItEUzpnYkA#;FmKy^=xB+8|SoJ zE&;?8ZAK}&WPIjNw9Itox^*|Ii6pRVu@tfGv2Jp@eaeVKL)4bt{&~X)6=hKFGpU`aA&bJ z&q8oM5`>39brHqXcsm%#NU^eAh)$w5sL--N@F@r$V#Wc!ho-JI_d_+OtLjh7%b1~C zu~>0=lQp=*j*h2M_>+6y}x^518k6TYCdwSoBFmI7_#lM>FAWvoC*F6&>A>2G`iCF zo+;B6WDLhml+0&_IE9!iqm$(dghnoTqwlT7xIhfaFzBz|kNvWB_;Xs;*nH97i3S{* zFBKfARA~ z8QJJiB@mpYq>Nz<7rFJmCv#`u)Rrq6`g>R4%V*({nQi!ABkH}?jVV>4B^FnUMM6(1 z1pIIM6cQ4m!Rh?hw$f=tqgek2)szvt4~i(?6T%QY)p5?edKR*^v$81Vf0qB*^el12Vs#L6GbI4Hj2%9I>NKKp^w3@q3Nn1#DPUwN*~;+d^VrbB5gG^CrVS z*X*ywW{*<;o-r-Nn{d!IUMm2O;q@U6Qiq+F1C;;^Dpz;}o){N|83f+A_9B>QGw;}t zHra~700XA$K18z6A1s1^WhjCucM-!cM9533&M%-8?DKnO#`nUIVvFA{w@jYWZj$s3 ztY2^dJ(kxjJYS~yuP(PoquHkh5m=Rd0y&V;%~}eu)0us$`nQ?u4^5k!#p9SWb*-QC zrKF_Bz4$ZaG@2xbV{-i1bi@e4KPA1PKmG5x%73}UMdqss9a-R}ox@*B^1GK6{G$@y zcd%NIksqPr=a8lFJ1Tox!s;bYFa5n96@ni^~RO6j)!m zSw@Z4)>7*Tfnd-uboo<8)|1mDfzrVO+3-fb$W@2e9ddB}HM-ck14kCC3G+S72#qqZ zc)l~otvTg^BAQ*uRj}x1gkJ|?GJeAty~vOm2yAkS!rlB3h%6AKaX^pNcN!(ggyf2P zEeCD6{{MWWI+7Mw%(z@QkCMsvGr<+{Mi+dqgpZwgYCW^b2A~tS8YCU_>#SpDT+ZdP zi(N-InW6y!avbKrG>xb)6O8YXjTaswBuZs*>p&L%ymeezACrWtH43m?46p*o;OHq| zR`;}q(=8?t@YR^5?DGczXu~h|>qpGH8x03` zB9{u}K-si1z+33qW;{!hy7&x z8YELx-kgrGdQEMUHhi_B*$k%ed|GX`7|wQkQHQVGF)VGm=wBSTsB>b{8;qFb+e139 z>Y%e~RpIG1z`QWL=K5_b+8pR%_LPNL={&~c6 z^|4F_!5LWw@+&6+bq9b!*FQ~~5}shoS?gh-$F^FXWW?!zMHUC1AgaaJ@M)d9vq%?YZ^SHr%WZ~@VrhE#axOK3Qpf=tkTbnm* z0|Txr^Xg3J<&%Z-%B8$9FvSb~D12#~{R&?%y56@h)w?xMo`Iduf5?k6*+lFs^S(N~ zCf&vkYb1qpcxT?y0Y|_~%A{11)KavH5_D=qfyS$3kkJ-mpf?76mYrzEd2%8cV!?*6 zS@ghOo&ovfAp4;P7Mdk#9&R8mKzJ3@l~tQ<5z3|b1933)kB#iVtY| zOx~nS!R&aevUWBhrUBKrkaJv>V{B?Lmk|XV~tfMEMgNV{Y!QLxLIYA}- zZ$_0J!?GA~@_{6{uuK_>`22apa~8XJdnUHuW>yFi3cK;!51r2RJ#WI8g)k0GV&g}H z7BqoY?v72c89^d0Dl?+o`-!{uDR!`)s+1e$+yT#`3qGK{VN>BvmM%OWnfk9g#sQE3b zQAk!Vp4lBhDmOo8@VaE%+3svu6knxi@p{Kxog!D9UDQ3*JZhe|ntU+1`w18jpg=>uOb0Fhc@7cP=s$a1@0d>b ziM0ny78{$t-SQ%;K`{Lyw*+?m5eXp6!M*1jbzdU?KFzRfe%pW@=h-_YF3D<(a?I^+ zZ(JK5hhT2PJDja7mHU?VYsP%g(7GMz&b`FwUxeBXPV!gfrW-YIikvA_!3!l*a~U9I zY(Xwxtnxi;a=(bmo`;|zKg-T%S<5{`{-kVQAc)bFV;NFLi(a&Vp^3yO(>(hSwHw?> zAKUK=QhPJBrb>51(OYTX+GL2 z_PbCev7GUz$TCK6MTyUD>WRExh>5)IJy8br1c(MH@o!rvRK5}TS#C!HW63iQef{q# zqbNNGf6pX|MIHd9$Bs4f4(6kWyjY`FJ_Dpp&05T#aeN(%IqfDX2Z*)0Ynv#h_Jr&)ZtLrU1=p#;!OCv!V z`#07Tr?1jsn>^+Kx7U5(ra8~IIaEZ9-vR-*|82n+xrpFjz75z;L2h-hM*<4{&vlQL z%`l&nf45FqhPhuxNQN>8Xh&s>F=j}ItM&Na@XB@hO znhJM8T%dl8@qJ}MbA|xu>3=!62OJV1kZQF>5Y7blP&KQCLirdJFlz8B{cl;To&^rH zsMFDtxuP6?&d&zc)K5c&me?YlqZxb#IIQj@Th`jE6#O-H=4LZZKZ*Rvk~8@OLf}*m zC#kSyHQJ6hfv=N4zN1v?k6`uL9h#Dq635W=`QQsUOq(s|XteEbj7`REhuEp}n%N7S zHG#S(44DQ_3hH4SD9d}Wvwlv1VfEj`V3}s#5MhXcX>o*2+-I{~M|w6jJCkqj>`H)k z48{}5bRpp`a*;A%=^$9#qit-xg*h4%S@Za;w1jk+lc0Cot(ZwZ%Y1Qf(`ePKSEy;9 z?MiovdovY=Y1iHt>*dxJJ=34pjU>VWkKhM1BHFL#fnYjXMT4{j{*B4=S#H1q0<|dyaV{_z#ydtsUgPXN50?YGd~v86LD0@ zvd`}f``z2TU`uwr3;%4g@%;wPA_*k<6$|tKm^!B@O@b{-cb9G3)?c=5+cvsv+qR7^ z+qSE^Y}@A4y?4#5dCSLKDViO(L)Sv8~F zBhf(T@GLF4YS;@gXd&qy2Y~~o5Ld-A21uoj1nlNIeiQ*0VPc2vXxGSmiRhNBcQoF+ zYp&-MVKK$0q*oXrJTs_KfGo=C#Y2!FC3kmQ7gd93|EgUcu3^| z0tiURfB|MLG~1nm8lA!8?stv|2@|@Uk!<uEHhTI!hPSo~KE@-K>V`k`xxV@fQFIiQ6LtplqO$ydv7+p2syV3cuf&ImB5( zLO3q%SfU;QPHTgJA16nS2V4-52jN2=B(|;`}#0SttY9@C?;f-(fdNe)7d$Z zubU2pmY<;I18Jc7#O%apF$LLpPK363Y*|}=KWe!(0>JUhVT(iV5EuQ^t@TH;_s5`ieI zt;OsHh?8{~eA%*fS{7(sX2XZmS00RKSq|E(jc;UX#efu#@E{Q-kqHI5L~KrsaA|oD z0~@4FM6Mi%z)}T{D(O7BupjYfqfxGt*tfEONu16_TIz$;2_;A&RAKC)SYw_Nc`hUW zigGtlAvPqoce9*jUM-B>1QdY>R`T`%v-H02?68q}I1NM9zw0-#BH-40rsQ(B6FbkP z!}G_Gwa|R; z1^}sjZSS@@KmCjdzERP=+`SrGc{r&$&lLt68^KBhJ8_##PZu=l+uH~+urAU}SOXTD zFYc$$5R+@w5F$AP&)v0WnAjFfwCZ|?(WB}<$2m&4FlDgkD3S)j#q{h&sl!9++LoZP zJI0@T0MXS-lhOjNX0dFYdoo#{AIlEP=k{1dBu!YNQ?n^9$+@2|rD5n%P3%Ukb_U=j z2H{mYB%f{8LMhH?afo!L#s*_VGuBy5V#8+?SqoM6%#>D~ms@P@7ZgiFM^6HrI;w7Z zE}lb`1bFp8SL;xzy06GS?X}^5t&L*>(~tFuh_IMjb?gjbpXU$YuqKf*vu-o~%nB#g z_}n7->?Z4Z&E|8o9f}dPdV{# zMAC2ac71-FOP=;e>{i(C6cDxkr<$03{;E*7n!s-TPjTVqlubF&_iVREBM!`7)wed4 zDSn-4F!Ag&Ncny0HZN3C0aM*oyG_);vV!rv>56~8^tTerWbE(Pyu*t82$nv~PxgMt+9kn0KK896YSr&v5S*M1lGh;upp3IG_6A$6}J`OW@8*- zQAw+ZG2j_I3o=UNKjQ-sl2L@9UUS3|mh6p5*_vCdk>doeeGQ(UQ36HRP^o{B&d5;@ zo6Hao0LFFs$PHn6S{Y*tYK(Wp>B8~w&ZdSidq88wFFYV7W510Y-@e_x#pL#EzGs{R zQ;*o>5;t7gPB)kb#rS;?(jcx0W?2k+uU?+M3HN-kF7z`XO|Q6u zgmP}RoF+lG23tiinfsa{!NMR9ieZ+w^Jh`YX4Nu0%mH@(lF7x;O@e0J`Al?9!}(06 z3TCGjcW3w?K~i!&27~T^*xa-tivBk5SB9WNJ}m7|q4QmNAt_AqGQ8`*(01i=OZ|Q; zSlP(%lif?A#PGXODuV^N-p}l~q(woc~PbU12Y4F*poHD#Q zEsC%a8jR5UzPg;4V&hA^sedd-EzWHhk*T+hYTUSe$hcvLJ7R6ZdaO18>C-2ZTr9|X zPacmAW=~>tZEwO?;Xak^_JnD-)T^J1#oyAl{9x8R+?m8xnp$aRV3uGoH);&4fWJ_p z)Y5f#8)(p5$l=9Ysrk@w9{L`LO|pYARlPWCMGIL*=bsF^N}9p55i}(fV9jlKKkS@2 zm)f;~KCYc1InLCd@kJKtYEGm+q(mg6aE3@o$lp~rW6$16rl#VSfGudI>d;9Zej63( z2+}|Li~7!kwo$@BlmQia>L6iJgsyOPwG&T2`R?Dp#~NZGDC12)`@c9Du@DGp-2DyG zW{Ik$CN4Y>t6~r=1=^g`!ih)~ss<+O^;h10avAzPr7a$W^M2WO1KV~waPfWY+H^ki zpvym~8oB+}QQb`+1cZ>2lJADzbFCx z4p|TM#=uy0MT14a_SCw(USVG=1@FL!VH z(KLu@A02Y5CA9MSPbPS4w6P!;BkQokZwd;$oZsE(oc1l z5+v)EJzkN@TI733zqOJdg;YI=XCh#%V7HDI8*R>~wCO^%*c0+IzO z2e=EQo}FUvaSxTu-><=sW@lwQZDS(MR^%d4Jq}CQhB1kI3k`x*=yq%ICpug`M=?0EYwFYwxp? z5e9OjYs)App+Ujod9#j}xNW*8-|>BIv5I?Dd$AWoR*TX~v|m~Mx$UlUi=M}RY2>*nQyEqKl&!N)$M?2lg_!)Le zl>KV?U4>T2>sU78IOO`@iv0Jx8OzU+7y){lzv%*OVW1QCEoY+@J2gP7Ws&SmZ6}(T zBd}sPb1?Xb{uBcPawN-Y`?ug$cEebpX#ESZr~0g75KOw~+3}-+pL=l_V67v9IJlR1 zTkpM`24h%=J?aRhf^7fFZPLlv_a#Wv{rJr_fHW*e=M}HUYrKwjP_Wy%vi*{GwJ0%E)(vWYR-;FMubuc$vk!S=ts&G8 zBf~k|lV({=`0B(&cvcB6hexQg>~C(3`{DIrNm$StF*Do>8+L2*_){ZsBa-#)5;dy! zyXbUBI(8q#M1rv6#e?JB^1X_x1g)YAUfqQPvs+zyANX# z-$UK!Y&GNHjS4Nh3bgc3JB+G{NV7qk;-bXQ-b)f9odpA_I@hLRvi0{BeQdhpAH34L zgJwAio&?S*x`9>UCQLN6Yzh@_5sVo|K@owW0*4!rUC%RtZn}$2EMY`q7qRvq6Y{Ak zPvnI#P)37DtV#1Q?c2vr-iD(4zzn~;IHZMIr>?N{C8w(rSB(;Zb}J^xgM{zZA{^O& zs&!9g>b+r2s%Q6PZI7Ap`f`+h&pXzJV^#%%A(s(Bj?ioS|9tKrEb>kRx^FU{_cD3i z9|Ym^UOsFiG6xN4>dIc(9SMcM!EKa)&#d`N7u@YTXzu6YQlprj@a#ucwQ*S}y&bRU z9=DmA+iLXe(1acF)%62WcZ@`hr~G1UR65;L!$X0kUZ{6FSWQkC-|v8Px9>Lc-7e^+ zZ7OBt?pO)$=IQ+uL`Bgr}7)~t>i=m z92zupVM3%8_&^p!`e^aCdlQY$y1Pn!;7aUuP(FV9~Jdr_5dqzW>T#@*@S0 z;#+p@&D!|=vw z9*)8nFlaHB^e`EMiERu^*uUJ5|17ZmM9kBA?Nf5U9_v9i$(Yl*!CQL0zItzpPQ+

LYMI*h+=O7i%sY(0cHgS;xpLZlY{92V<(wdEr_LBM$n@^(XaL*sZG>RLfXmtL zC7X2x^t@e_#%aJ_NPzL;CLI8yg!hT$IMEcWpWNO4pri0RZ=WsZHoCgE3t(bKnr+af zn$r8!AKbV(VxsUczj+;r)x2)0BHI7F{=8kyV`HpFubsRl_|OaGj5MT*IXIGHJ!se;U>!Nu&A`iEFLNWT+S{^6 zX&g3DMRhXoKJPJF>+4=1`FjQv*mx>Yo{*gvtMFOMB$jd$othmhHSOv97+=PfBI!&V zOb=6YrK{FYmwMS$b_+x1#q=3%S=#bIwuh`z3Xg;71zMaY3L-=)W0+wGOAtFr1uf)y z=9Xu)mHJEDuN~y;U}9{9%dN|SNzNe{RVwvv5H*4>5U_YLF1wBK)S5S-)UQKwc7ro{ z)53uC?1ilNisiemyPizHE_iYUJ~?-kJ8gpu zQHhXWI^h;?OTsOoG!rxpg*+(Hq=jds>!w`V95;cr`QiZ!XSIb}q?SU&vFkfx=ba!SZ@9(AX%p3W^_dJ8?R&YG=5RZ&`(x& z5$bujubl1$5rAtH7`RzREGpnQOSiuf`SPxTyDgX{;>McT02vzM>GAm4YCu&{*)P;T z9~Jn;WDq2%lOfThzGT6Sjo51t*BY}PS+~O1KDR4mKoH$323a)kx=mgt+?%$ejLSYU z6$Z7^LIA4xqyJLJ|eu^Ih_?D414L->Lb zR`n?JV&u0iA`;DU)*}|(SO4q5^D#*!vGx^Eopo^%(!V%TB{me1RO940JnNTdTrGB+ zyVF5dD2-GR0i2U z31$ITh*cfjh~~mY;UF~T*l0{_E2MWUTiv$nC-9r~z14!rkKTV8Rc^jRW=cC!O>^>%HNi0r2&4fW%!}et%ue4 zMzJg6;Px&tVPV0itgJDuCywa%(`-G+H*VLpWIlp#F)3i8dX55Ngc;4*{K<>g% zWE7bs!E_FarjUf#N%20>slE{ww$}wSD2>N>7C~}}yey+B=#kE~28v4a@65Y!uYj2; z9LNy!ZsRuy2ilmwPQ|~%AF=w*55#i)gc(C!2F66v*-Q1*gOes6V7%}8) zalGccU$7vH*SUQs=cQqY*w+^2+s3Z*Axaq8wEhGP6QDA%`*&cp`Dyl1-y=?Ih}k5r zoEQk{H{?T+m}7-3Z}RYQ5DVz|9X#I)F znbr#_wY$?wJ_)DVS_Teya`vC!x-e|yQhuV#D1|-|g~+hPNkW;agD52lzAPQSvRsrV zMVYn0@c;D!^t*fSIy7Jo;p_xwSm-lFW>TNnp2x`@6Uum@*TSX+sf^SVG_}im+Qyu6 zl}fBTdpx5TIkL2CF-^N&O}Nk1xe|pd9#kmS{ZbRmZ_pWtKBJE4_k2?4m*8IyWhG;)P1`9?ia|^ z+HgSE_F83oS#b45xh7Xw4Rb8jSBI|Z8-0nJO)x&N(>Aq9kB6irMr)QPRr;-V{B^xK z^kRWM>k(Se3Vvy9yucF$IFOMGg+m0hDj=_t(egP%GuNWxPqXx%WZ{i=gs1>&?t;KkwHLE+#H~ch= zZgeKm)$=t>G@$uA0CplG+A@jEx-A*&Ju%~|Vtrr$I59{$5q$)Dq$TO2*6v&{#Unt{ z=<JL2u#F~z?C;87i_&D(9iZU z8gw_r-eoox#O!t3l<8M|Dsg-!j5_PxqGG_O$CJ7CxN2ZOf1{Oq!L+$#v1Z*d5-Rx0PlYP?Qsbs(dV3OOq3rdA4pjbCIdsFuR26yZRdE3UcEzW)}1Va z&1NXw$sTxaaJebU`1K@cAnYFykVVIr*&PpE-HpMPukA2(oQ{1Rm%SyRhR(e7Pkg!Y zWQ7%XtT&*sH^%?m3>Q<#I0z{AOq`Yrc8;L{?3~DJB!0$hHgH9(_Ll;l5H?!#R4`GP z#_J|l(MYMZOcjvbO#|rjw-eVK-GH(k?QL~hD&IM3=SML8{>?CA716K@4u11@boPO9 zQ&4a)93tU4)j=5~nmFPx%97SZMb5ES?k5ht+X$B+&^!?}ePG>F;I*C<4yiC}|BL*3 zWl4xSC~*K*pa*_EoQl2MAe2q+++5a1P_rjL*T$jQ>)j81@QPQo9b*<7O zXNrey8FE84$Iuy=wUzA=7c>*f5C%E#BWma8^Y&D3j_YseO_i`$_gW=FEh(~FO`#%x zM}dghIHbW!F||nudp8{awM#A9*m=-Xe|kZ%HSO^vzWAIcb2GNMsV6~EO{JH?W$2 zttK~Uq4Y`FE4r>j3lAf{g2u6+R1~iZiJrlaK>-p_@#@2Gx*y^t%8&`(>$BdzaBIA^ z$Qe09Oa>vUtrg!jBJ&p|!e*C$Pc@;_Qbd{{!g>m%C2b@SBWS_dL+J`pEtTcEty-lH z;l$Fo2B3EIL?y)yz1nKd2MkSb|^VRNz0j zn`g?`4TsT;EzR@L)xi%TrH3XHqWkFQrr|)rpA-Di8~@D`H-$2qGAD#J@rN_M7&%?U zW&J{AS)ylF)_uv|hmF8@m3$p+Gcr7x-s3Q8*_pqOYe@LrY7=7FWA8rj3tSf7ju+zeUP`xi7wnT+_*=Da7FT&lCuzt z9#C5RAX}*{!5=pfRet6{9+B7XAA+(qi9%rIv4HfxhoE53_Jaix1|Qr!xG5Ru5>=T3 zU5dK^9k%NNw#~v-oA|4$`|}wuz;WkJQ)9bKOeT9uSXdg#(?Ieg}jMk zUTIt{pW0>1i1_}2T0KwRTOLoo29Ay_a^*J0^Z(4JG={Ac zS^iRLI!IQ5otk8IUWs@6u1`62oY#)^eaz*4@aTLS(}e0zVo<>@CI>GnD#jXfqKxP0e8#`&T)Vxr`e_hcc80=K!uR9rb@}$oQBHG-Rn26P>OxK#MD2fGidq;K z`T#A19z%{9Ef5k=o#WO+t>rqazsHm_FC3a;CA(d7*pFu^<^4jf>-f&o&w&}_exkeb z{xpn%8L;&vzynfo)Mpu3C2cWn<~-{S(d?UH`v_=hMFgU^#W=CG;BsEUp>x58FjV?g zL!RJg16CDsqxjrENi7|)W*SdYCRrc)2dMnAVM|S0JL6Fm#UyH{^Ek7JLJ86iqIFOf zbuS&%YY#qM$zN@fIzoYBpimkd2_16w8Y)$9q#hws#|vS|s3JNs!eGuKacn4oBYULA zNuj(~p#;qzsz zF_7^NQ)_x+eA#_Ylsi6>klm-?eZ;vWf8%$Pqd^~t2%v~biC*t%=qukKt@TF~h@fL& z$A5TLv9Yv=y6ePzl7gU{gcPQIO4!2w6%0BIQxUVjqVMsK^?hC_sn!sK=I|AjP^ z$so6HD#*J`tclLl;~X;Mb-K`bpT*Z72Y0U`I{H@uQW@9aRwy=Ir1>vCyI~4uT0~sp zzJqp;JrBC~Ys1GsbiUx~=TXZ*J_&Q{Dgz{FBT3!sc^Qvcpv1vJizv-dCOQO@y}*xx z6pgL;yf!4{K=cS(0bVWs8i-X*1>uco16d2Z%l_#<+VsEH{VC0HmkU__5#`Rbd7iVD za3ZI98vOAII9)e&|CE|O++fR{NF-xjDDszqog*Nhm`@#eFIMXbnc#bqU`6ABi-ys- znm|kJYvxEMP%^)dfqW(?E6G`6 zs6b=b>FNYyf|J~9cZP5DUhj#D{muNywmp?z?*a7dDiPhm@KYX(mux)mUjuh}Q;eu` zAsIl^OwYkEh;Jpz3uy|57V^536nt&HOczeDw%=ybQO5j6}~cv zYVSJOjY#I#|2m-T_4_oX6i`%za7G#O5OG|`DL1}a*y`KcLUMIFf|cO7Ft0xt+O;dD zT!O`zC&O+BF_4Z=g@wpR%zdKZ&UQdTb#00E)=KO7b#a0O(FE9_t;!>|c68J_B{DG26_PQfpL6 zbgjnl8GHUAA@v_)g6kIUK2pi_a&rvoc4-eetHG;lI~355b2IXk#&v&%d@_hSN-!zb zg?-Es?m(#?2h@h=OQM_zigwV&qAbpc2%>m?AWe2-s-uD$-O-6@4-USTHw*FHLdvm8 z5sos($@fH5Q^nEZ)9Y7m9S?OP)AaqN)AiPavq*zU$1y$~$!5+{S8eznT?w{D{r*~c zErQ5;`vMemjj~Yw@QomA9Mn+6zA#wh3h-RdC=6+X8y&gsog16lwtUsav2{$ul&7w2 z0)Z96QH0^ztDDEh98NIZF%L{GpLSrr%0(`jf^2{p-@QubS?bJD*#iZVA{H|Pzo@9R za&tJQ@m2v)D1P6>)|=|rq1<;+^UtWfSoj3!Y#CCeBGW8m{~L*E2ldMvY3j`epC>$u ze;y-76kx~Tee>DyPq6`#HQ%dB2nVh|IlWj3g)DBmD@`jNCa7*OxChfk5}n@r77Kgj z3a`L96z_Xoto@Yd_Q*`-?oOL~&R)>sVbK`_YXQN5u(M8|@VtISt=D@&&+gM2hML!n zAY8K*$3~Tjp@4Z>(rwYu+7f#-QGdHWRaZ!c6=aBq=6=u;;RZXO>=9i5gh3w5B)`Wj zfg-R>h+q#VqrDXZK~&cZ*F*V1+cMcKNqA zNw^@dc+JL-ClHl6xoaqyQfc}iF@Kjh5n%x)WFaXf8hP!CA%a)@S#@aiz1@_Y7G;pL znha^hWoM(q1#&oy~sVDyIurqv2)?(6!KVZSkM&k_WFD9ht}Mw@F>H7p^7jc ze&%{H+IcR_faRuRas>y%dvgk(-(`kP2UoftM*Qc5IPA_{jo`X94OeGOdFzu0)Y+e@ZVaQ<{J3@QePN3;}T+{{b zcUoU!*;1PrV?Tn4{i=k-4EHorc+ZtENBC_79u+8D=fY9p;j6b9r#^jKgi$g~&ha)& zj@(2@i{#OzyAzygQYWc5txPP;5@{M&C6m70+&iS^o1&!ai-iWO;S>l1(2@*+yB9Wb zqD!?|^ez3iTy_R5)dps7Y<2(0nI4e7j#J-qfMljb+5On(C>uN@Yp<*--JSV>NjiSm z0>G^JED6f?gJzy$^$gbZwGL%S8Z;T(3K5ak=v>YKilFx33LeR2(kmA_1zJ)RH_Ry5 zU}&lI-wYBeqWaY=9WB@S^%TGouF;sAmqC9_;7%}q? z5#m9JBEZ7lm?ev8htBX{LRUOFb!1ws`c}Uc>Po1YVW8@d6EXJ3jb$c>1B0|V4_7GH zZ3pzdWRh@zC4*sDlz_G#++yqs%1~wLF%u(>JJH3-RJ~NY*TdbthWB9KA=-GpZhU{@ z15T6dbQ;z$1Bys!I|dC{C$XT!KGrbg;~EAHiuUan^z7|aRtlcvJR%vhX_Ezmg4JAv zrFb<4d_0uHsY9$yR5WrJ?UZ&J0a@WtXEoiiigGnF8HoHtA)OP3be=&l^>UnUEpx_i z+#pt>g|cb=10QW4dS<&mJkFU4WK<4gblW>PkA0i=Q@US2h{y1eNOG*2LY5g)z1Ao` z-skMdi-kheANR1ngUpZ>ow@z+xsyD$qbp-ge|O6rF*fTa&&lC49fb*0rB*5wIKuzy z3aZ=sM-gQ?{v3>99k#f)c7tH7n6h5eV_wf|*MRQ^Pjg}9v~Z8GFg@ki62V}xh+UZ+ z8ux!v_Gj=+(lDTihQziXPi*P-&p^*c>N4Y|K@y#^$}(?ho9XcHCH;6A5~&|r0qXe( zpamv_2WR5kH5V>XHxc*_3eS8yskf@2p_SLvv;GyeG=j7-nFa2$s10_mYS>FyZb z#4JSS>X(vcBXPP`>bf*)|Hgx6*h$14==tvS+-3MioE}0OZ{jIr8Stp1wj&zX^|2Xx z)XDD%m88(*Mw3#J3S9r1!Byh0@^OIG0P1qW*IZz{9mFz+u;xWu(g#@Z#3@_U#sd&a zSyD777hOTifkPvzqRx$R0059~lAHiclPzF!%@tXCX$k27Ob9@;nwPZ*Ib9 zq%6vt_>sr}N;GT?(8>^T+Owpp%w4V>Q|Ksk#oUCKv^HGm2h%@k{x0;PI=!s=s8t+3 zw5P&>MAR$J?{M?75m$;2Kdisi{fma_BN;nyWHC7$p=iz3ksm7t?kHQex{bZDZ&l~b zznZu<;Aaz@M{W*yN1h&napKn&w5dGb*@J4sDuPfWe;J4M~Er*FIhT;nq2@~!}J z*pjLljBv^!#K6nDV&?o6#{DTSkM7=~ey2Hu(CfQ!YWjmBJr3Y3Xml#HI=qCS?}q}t zL$2el)YoA5J9cAh36#P%a`3Dsnxe#uNf_5jQ<76oI4Su4R+S=X#C?avx;2odGKnNq zar&g@2vox$VHnXM4{f2rgh!|xkJ-eRC!2K(;){Zq42Wx?O6caQ{6jyYIE}NDg?9gq zvtPIj_ZX0|MC+m_54dhE(XOAjnd!JaKmWQf^jepcwz0p)&T@>p0~YX6eW)=M;iJ*S zmKX{D0J-gFN}X?NdQ)SSa+)si#Bx=*qqg*Fp3T6gK4y(pPt|5cXFuBPg0C8j!2d3h z^1BYM^vq|y6@7#=e8JbBCnJp;9$&9g%QutTZmJLpv^~`0{`nlryO7ZIahNxT080T# zXY36ct3sQyj$qD?(bmK$EkEKhXO+|8g|m|GeqOfn!k9UJYJUJ|w|cKIEn^Lak-dn3 zwGAfI>~{%fnn7WHj>7kE!gT$(NPuC_zvpv)P|B(sEb~(w6)3N{f#FA>%bNv$FlJP#xDE|gdz@W`YnvZ2@2!hdLK zsn|Hdi*P19-@bSUubgLrrm8;M+VSb-$3J+b#Gk8_jOS}AZK&PTvY}b5<$!j2jh!5n!z%t7K3x97?wh%Rj3I! z=b#NiRphLRnAH?hchGRUUST{4WX;7R4LQlDkxW~rz##ka0)_LI#+)@D$?Ak(Fb%Gb zELJF`r{&_LOFQ4%YL15NydX!U@uZ?i`wanUJ{^`l6uDYSJk*iKRAQLFfhD5Hh+_{f zw%w_hKUUqVqG^yZQI__y;fq)UhGrQ@J&AbG{ zPDoqjCU(i8v`TMaL7N9OI}!P)yOCjqD#?+Hb3s}aI1F3SYC0skFBofLG0O-9g?36! zP?bkZ6ta)DqTxA54Ar;c-DcG_eOzjD&1!Vh`)O1MRqPsg_)pGfA-v3qoa}zmNMyx# ztjl+xnuJiKAHjbg+udY1<_sW)hOEBrzr*d$`@SF_nYj?1qd0>GPcS&7<5M4UTMo0+ z#;G8#YEKXOzH7+(42-dVi2ti0SFFwXIrX@-9REe~dww zANkU|>(f0k&)r`nDP-7c-f9NE`9!TxufvW18$c+#DZA|9Asn5j*-?2dt?VA0y3OW( zRFgINm^F1yRW`_l8@UmdbZ4#=*3XL_ZC@te2|k|js$I?}*o4&#h;-%f z0b5>w^6vpTF#H~u#8M^^4PbE&W1kcu^dpNr=6bpoR`OPxO_C0p$w8M_I&{cOT}=W7 zmrC+hdYgC(=Ewlil*Gi@8_=+!NSdZ+h#oU@2B3^7bHyhnL-KVWSw>0>LZ zvfGDLta}fFEFB}BPX z=iy*(vE0bH&U;Rvwa<`H0XLu11(_u!zWthYcQUmnVCMV!zO{7SpbN3YlgE-wzCcz; zT?VBJr<`6LSxo!w>-k=)h8sHu5-HCR1nOl4oNv+;mmE!q#y=>LF-|~52rSiq!-VEQ zhY@F5)H*kFKW_leXkx0xJ|T-W;r$A+fB+B^%D%U;{fXQCj*f3w8f{MM+-xPyX8DH? zw%Ntd)+EAf6Y=7V(qX37=h9$NJMxF*T~$-3b7sBD8hNfWQwhf#Zn|Nr$n_v)n(p(Y zwQ9aE5?d!G#wSzMuq8>e}e>wItb_S+y@VXS}FMZd-wus@e*eB>&nEnNjQ^vI zEbk@A)z`R9miy5{>+=;C=JpQ(ZBpT9hxw_5d7rwKMA=@UG@9^)a$jF=JWNfDHE^Y~ zjQ}b`_cemm0!6^bf|DsMgGaF1`?n#?fv|N3`>UfU6c?#aKbW0&CSk48NLqg_{HdbD zt8uoaJ6ioGoo-WNg4e(2NW*h&UesI(?&vcR3Adm7FGRY4$C#i`err1Q+#m3v_q4^W zTT{{b{-tYmxtlYcmPH~B2`NOt55X7~plVY|(KO^Q5#(p?ZRu^PmHfur{5047Pc?Xn zQ$>rG2((W2a(yI!DPI)_PeB0_#F#87=wR}>HneH!edLE)8eE6dWuyWf;`a3Z72x|hz;+)s#yG-rr0%(7 zg76rRikxw%?vz1(IUG+X@=L!?9l#FD`t3Gz#L z@`3`0rQy5<$9o)e<9=T&j@6OUbB74k)a`KXH&}Xv17K2_>Dy0_q^XPRXVP{sQc$U6 z{JZlm`s4GNPnLNu06}`9fZ9FKGWnlB6ic}sge9S~Z$|9r`iR>Es4q;LX7Q%LUNFyJ z1BRjv4rg#S?@LkMuYbNoJScmbkvmQ@CMK{zV(zt)3%RCir1Geez=S?SrRjs}*^nGc zcS#=ATu7k7QlP2o+MD`Bfj05Q1kAjQ#W!aer*mLYN5I9ut$|a7wnKa>KD~v1!j<58 zhV>iZeM?KOZ+~OKv#xk;C2K#etNl6XMNFd12P#vX9{qN)kON$Dl_HY{I{gqYb0PiR z??{0wV~t@9#{D6^5-7-qk;2BW*9+>B8vEHOE1i1JNPhTRJ|V|nuUvf-Y%5HX#>UDa}+b?g8@l?qz%C~-&f%^~-gF9Vli|NW+?O+Q|ukC`Rbm+{@S>9?z zbC6OV#7UY#V}?vw-HAkxIb_w$xVP3eSv}WetUQm^& zgKL0e3}b{JqL1z0lIZ$%dVL#l?;BVcb7>qpxkQbf?~ID#_M-h!uXejzX3@wrjO6z(?#?TxCx>5S{xCWA`=H@@Emz3K2xkse0YBViw~5Z{@2`p1wHR2wB}G zW_77j*8bUq?7`HsbMYaRIIUYn7zM5U%@0-U-Em({{TS%KX#@J(*PUj;k!^zn-Wf(Y zj#xc!cYkr52eCHfNugyfABRz}uYukjw~zU?I4Rfc5kP*&YlV;=q#bvrcy>REXjTy# zfQWI#j*`hKs_W+xB$WR7l}Cx6tc6Ec;08-t|QGp+wK z@&A%;(hSNe(YDMdxj=yH5-ZR_R80g*>@6IiZH8B|&=5ysSf#6>{YNH3RbGG-Xv;1M zc*!v?nxdf_|2-Vggoa2c+u)>79FLfD|K+Ry@*_T7(&H9Y^7rRNx->*=UNjY-@X)x> zS6p7B)~=aAwdy3{-(_gLO4OyWBpTs#*gy;ICq_vxx4qXMmX!?|b%$1{JiB#^UnyN9 z#C4G@cN=$PO=cxld&0_^l8T;52%Wk#1c6&fE~qtB4ARbiba_Ige=3*c#0R#UiJN^m zFCCT3e$!B~65kj#7M zc4Fx@G)LSQ9D~4I?p~5!U66e$tqKjtHJ#rfgQY_JSQJ7MFf_l4Vl=E2C{SI@ z!x3&QP>@^kS;lCGYw|#~D;*h{=?IJTxi^fVttk~wW-N}`RO$LEm=;dsu|*$mSnYtH z_Br)R0rSe0`aYoQe&2cqz`baiV}&l|k(wFeBz4i$8uXUHkO6YPcEH`Pa$aJrp%eNPfdV)nrB{G3^#s$Y7_d2n0 zm92g(VvciKUrF>j_W))Vys}Vf@KEGr(G|`^2R$lgjwGy51{N={o{go0obc=mV-78h zz9W!-tcrM!#4zn34zaFOg3Z)ilD5l0*bP)pIM<*X>9z&|yhvd3%!p?cEa65t6Cl$v z1eQ3LGB?dFJt2{F9MLSHCN|`{Y8>Q1)0j!qXRMmkc?O%KnioYwvH>s_RjK-h(0CNZ zjHozZf`k!7oqc|d)~&Y_Yxe6s44Wg`cR0A7+U#73>;K3S;U1{M#wse3}RwC z$y`&RP*o!LroMF=unKoyU}F}$K$lF=I5EsFD*v$H%-FPF)%EdQ&7|7ygOC~y1M?dV zKP&d6I=Zf!yeWD;_`TvYjFAh?78-yp?#fb!&eMI@w9w^IC3D4z1NQ-G(Kq`Joflm z?*;7Z3EVf(wQTr}lC>TrU(Omi$uEq!%wy0&*rZ6(N-X=l$BCy@+YJgXFm!|jc?0z` zA+!5cqxw76cNmTXD;ez#g#vd^2^%6z5|Py$>75_KhxQje|EnNTiu#Z~t&c2GYfiUw z-H;;|b}0!hFf6brgl0y>-}o5pk`@*wLF(~qrl6Zvzpp+j&L?~4k)_aPL)W6k0)r6I z(AJk_`W@66j@XQ!Xd-)dlkt}qh>R#hgOX;J+FZ9MO3Y=_Mq>h~ITl!+;5p&Q^iXe9!V~KD=3Or)^+mQeh5( zyEUyf6E25R=3z`l@F^1F<}-&x0Mn3rP1moO)9WddRYy-OBV5TxxPrKRnVJ2ZvljQ( ze=|?>2=Gmk!VY|ZPmGXoOv4PM+;%a0ngM$ZTFasbqg#hPrb|`Fd!q~?)xZ7B_@fUd zJS=3S=)68F5x5oh#Q*66I?Yusj!p7DCi#VoHn%D?6rJxi_9 zIAMQmY~IdMvj%>C^zW(+%@-QcoP)?nid%`!yS-C){a2V`|IS0YvQS5FO3`+NLQ6Rd zom>gUO=Gh5maK!I&^1onUX2hs?W=r+Qg`3u8#7q_{5nb4#-%HA^t$=Ou#ieKc|LCW z*2q&$$*DT+n??xvhmdAqy0POZ?g8h0Yq*@6a5HBs?G6s}|>7NtSKu+SKuc?+ozb!l$i?DT32eHepnWH>ZRKmC~AW_NPvN_{Cqq>EuLR|WU4j@>|7TRWh}2R z4z=xG08Tx3GzZ^XbuGJ8z8s8(C{IDXy*B|XZLzh>g@N{fIm47OwI7IsmV7_NgWO6>DsVAUZc#N_3Sj2yV|r|Fs(N|?wwjem}&nK;7Li(<~|9vK6-OY zS%4bB?;==1PRt2AtCx{*EJ;jjP}NM~77(2g^$b!oEEE`O(7&gXsi6IRnE4z+v0W=t z^KXt89Zk;IH(Z%o<%fEhAyF^v;4}y|2j%HdQxXVGvbtr_tmXVDvb@-@g$9jV=jD8T3OUwJJ!f2@Z zKf)<-k8K66y;yTcvm=iljSur=y=T10enQHZ!pP5*m19h)q2Prl+LCbt6aX?I^{&Y# z1A9gp{vEFa+4pyFyTu1{m_9|bRLS!Zmf0#?@XI*HX6Y>S^NWet6P{%~&^a9SY+;FG z**xPcX$d-*(IU=YzK%0`2k*OY?zh`L?T8k+(WA)PVRiH2_3BFODmue!EUCzYl%zsV zYq=Bi^4jPg1XGQJRA$W*s%$PeGdz5W@7nba66jK}G^N&r$Ise>^k^+AKWO^cR{X9+ zJO2)y&+-H6A?6nglh0C7HnsrDrYejUYqk5D+m4^tcYzM?_+ktu;Hc5hI74MA{kW1! zcai5Pdxjaq(-C-|4-MJvFO9b5kwg1Dk3i~c%m%KEm#bbSWZme#dcnH6hh`gx!|x;B z-wsV_SSeglCezglDhmVAr~Xa$>yl{GrHCStDN&ij{;%rq+G8BId-jXh>pqRLAvySm zkWTP=oQKzp-%)t{%2A7!cf%`DiR6ll4qF{LaYeU*kAhTh2~P9|=tj~!CDtdVV>IH+ zUAm}-w*D3}TFWctq;s~D`iUVaxwI6W;P4gm8qEpxFCW*_hrzI{bQ)wgk_ikU*l$my zT)Ka`|LCYS+W6`s<^LnlglGN7SH~KYw(W7!LZ$s6uI$8eo0xr3j;LxdVT$)Mmhnr^Z4auDr z*+K#-IH;v_A1m}MN^{Iryqw%*X0TjUiwU0dUaokRcw{-u-3bR6L8qr&!yv6vyB(xg zHnLq$E%Z>`&-%>x@{ypNyVB`jCTf0`Uu5$6FuYvK8p3ODXop0`a~5dj!js1L|3k`( z@ihrL*(R$F{Ai{g+%TCBCrPAdw$?pyK-j&8w%R68?jbMm)PaBc@dSo8>%R{YZp#xiZ%C*^ zu-3QOC6C)O2*1-5bHhvg6X3_a^X&hb^z3W9w zIDO-L+i*iP?*}c3KQ*y^he^PmTDQo8Ag37&3t9^~Ba#k_!4}A#ypXQFUOXR_pKz1V z(;bE^zr)B#VANE;v?gt~JWF?l;75;Fy93>?nB_!=jSEBfB0Mar!8R5DnKUcALA|TVZfT{V9wV?NE!-50i zpq|Y)$BTDM&Cd?Fz|Xig$ZT*`O09c4l~?z`$&qLo{m3r;$>Rz7bMNo&Q#z??2-$QZ zh=wq1hceLYY-PjGiI!5JXk^Zu{@Z6>zi1=tfW{g>K62mJ*4eGeIaEcLTd;{QM+MGB z$CYhr6NC$%%B!t|S{jbG<}K4p^Gg64shnsav(YrdmjB>#Jf8*@2Ar>tCOE0mF*(At zX;c?;!SudeJNH2vFHD0pi04EN`T#^av^a(3WmGKH&oR^1QN6(b17spRj0p(OVR(MY z3q(tmsR$Cha-MT$JetSSo2={O@evuX4kCd=9Jfwi&&}O1Tk6+3f9*bN<4pkFuOP0%MtQyn7Z`6~eA5e4kB|YZV-3QTGizqe z-@zZ0PAimpew7If-keg-wtwQWeW1GbAZ;=|?c_P_e8k^(U2lzfZ{I5xk3lcni)n@) zR}nG$?pkyGgb*2x6uDt(!W7=t*vfP!(L&(qGnR)_gjpUo9J#qfSI<x@t`olR>;Z_B`nm2$#wKBQ&ZPz4FAXUgic=a*+ zvB8*R_XkRG0nzALUt7KD4k?GXCz0mPye$W{I#^~21q_Ut%vN<;*(eJaWpbbSOHdUr zmuE2C1+xm)lwMzB9$FKo9{42&7xe$$+5U%lnm7dCFG1$43s{ycYD&qf{z7l+8+h4n z=^4?o(HGJ*Ri&>LoH_SIV4#EyodP&6i|WiG4rjoOEkpu~;;aE0I2 z7y9$m@c$iblHHy)AKcKJm!4~q3RB#0qmV&XY&9`XRtFnM4&c|S$7{GU^I4F1y>11w zL6Bi&|0&rFy7^wN6(?-d0I-@Ssvn)FpU5`o74G?Wn=1R2{`57nR13z%c0EgztdV?M zK_F2!Xe4sRo?ock#6kmJhCm^G)9JI@{>bNsO&Sn}?!(5xLKYiRoRM!#%tV07`z{fA z3U$t;a3op-#!iKQz|6)M5gXc?P=C;U**ao8US%Y4M$3JD;q3Vk%|I3-Q%T4Z2Le%S zDWakxbo_UD(mQo4Ar7kOvjR)RSji^7w1C|NTjP{T)jLz}!}6|OGgl>IU)}>tFIebZFH}B^Mt_o%c&2J zd?cP!HmcP!R4tCXojK~RD)hop%lj67D5Dm1(s`z!rBxsvt%JAp4EH73KvW7XXlZj5 z^1G=(59htfq)<@JSlr6un;YbTrcIjsu~?vt{@ZQ3g7gfo>-BxIk0?1=nw`-7T{CVS zqa15PXt_%_(ZP}bL4g}Z%o#S3s#9A=TgJ0V&q2c?p`@5GGz3M$S07r+HTDJ``^Zq_ zcXui9W%QGYDdSU&q3e*J2>$7x9YwCvh`xNfBC<_2lAGP2uYyvkZoLuFcb&H3>b}Mv zwF@0#|J@F6f#!=)F0nVAvS(YS7lJuQm1|065hr3cp-&b4KdBKr3#erG;8uo z%CjX&M+<*%z(#AD)8s4`6hjI#XDg zcFc|ff<0T4FRKHqMKU$&edA4|=#PDRXV%$&f;69!h4v-vOIe}m&Vokv$`_R{jIfCg zW-CQ=yL!eRx<&PwX2POH;E*b3V)y+)F9XCDi)R-vCPfUZiLlzCCe6x(^1%3MN=Qdb zG)N1wn6!_f<*au}teBM>bJ7V0_Iae^AH)}t2>aAV4#25dxLXVbLeZdi{)lg6+#B}q zNJ!$$Ta|YBPzz|9gb`_dQDvWA*S!!=8@~c_L_VZ(ZuQ~i3PzN{Wft`IIe+H~{%=6$ zx(-KzLTn+KbQGkPh-I0!3OUzZ8(!V!iU8pJxdNS% z;dMT~2WIlbuqK%=Tgpfn)+dsbqTxu3z=%+LY=HOa!T{S8^Wqeb9Pa}nVqakuNhBJm zk54DT)X$?&Q94L zHm!7)4&DY!)qSJ^Qz`@8Ewm*{pZWiMg3e#T^dw!$;UIxC3RRRGvRbJqEm7N7ZuZ^h zc^R*&y6y~(_?h;BFSUv`e21sY$>$uh4lE7&up5(i4g{|T?l-;BocqZXr&4_CNRd>N z{*){Yc!*bqleWo^b@L_2n2S4YeAz;~9W6V3Ym~MpeC*w~rjQPA5DH`6*A>2Nr#XSC zo-PnL|?he*@!Tj^n*j`s%=Ui+@N8fxruq5ei?mKz5V2^~xgq*3ZaAxh?v zp|`r`&7P2!iyk1tCK8tdCI+zG=V9x@T}s?uC@5TnAAC?uOoA`roBUhiv-Xp@z>OJa z(+IgOmqx9V<0xaMsjx^t3mKG+bb&R?3)4JH+<$sXxWb0I{ncEd_E?XH&ss0911{f7 zsDESrc{g$0KS(ddZpAMSMNUV(Y}Tv|0n583@&LgmBQM~z74G?T+L)Y%~=o3+xJdel$8Q{E*mkSA{t(LE-;s1i?AubylaIx*-T?Z(Qm#z2nyoWWck( zp?5D`{neBy5;dGU>-JozKsep1i@vGr0ehwDE&|stL7hJKot4)*;s@eo{_!T96~<_9 zaJNBQ)}IHVqb)J32D{t)(9|iICzZil6xGL+q@>C7I1Q`^EKB>Ggbg3rps?~ny$Kb| zDqmV?{w47(^OB%&fdf>*7{N`5KuDBW)G?{hZ;T08k(4&I8xMuu5Hj5Q3%G| zpi+40xGIW9fsu>Ol?)!c;cCPkt09KYNsy_I~AvO!W zTRSEVyp<=rG2`ys;H^~VO@(>?9v(;05|a!KowPA#Q_fULaf3e(T0Inp5_$S7!`n%F1 zx}_vQXbCMowp=hX)pyfGCDah~JZTx*g2PfvETxsU{{BO=Gf{k%M;XgSf~CDvxnR%L z_2#0tQ;n91Hjz44qDlK}M>tSIlPP_F4V|_nY2{WZeN;}Qb6VDUv@56KtK8!CyrDnq zlW{IRfAkC#v)4nXlNhr7*r@7ZX1 zb#-r@O`c$@5W{nQ^3x}^tV?Ey_TAMS=dRX3-7EW%yNe1;A3lz%jffZP$z5u&n3NWa zVhs!XKb`F=rE<-Y#K|BlAs#$oT##iqwgGZ*axh~D^qIU#7rd1Bc>+`pl}eX2338IQ z{vL22Q^Nlc^FDLf&yW;=Yi(&aubl4;6;Lb4$lvfh0)542DaN)3wbfrP6rhH^3v#qm zjh2Z_WKOA8CeYv_W%aMdpn-Q(I3xL78i_0lnNcz(spm=-3AzgZ~Bo z+tXFBj8rUL{Uah-a234Z38KKc#PZTT(uPeuoB#r&G{SODxK1&923OC|b=KC&wgdx|CSv@~-&JP~K8 zDWQ3{j}6r7qxk02T@T}=8h#9xF`9>(-ppOw9`jcqk^d6!w#V?0DXay23!MMuL-0|h zO|ap{780joC#R(aEyGGcV&`p6J5Tcj8+`l|=BN?`Bk7%iVK`mOc`8MvRx)IqosL`1 z;-ObX*NA+8hC!&bGBzb2g(gYaoLEuUg9#EbI!G_ZC_+}7$!~OyON4M+BlN5l3H;#P zHli3SN$x)iBrY^{hhKPij-b5K+01&P`_qg*izyRHB1bI6E+N_({^Nr7^G$)^^#MUD z-@I{b9`EBW#H%Z5a$cB+cHRHIkrli1viaQvi*V~$$x}1neBr`Zg}67mj8Xj;|_O1wBuKsNR*>orH-^#JX`8RE4NP^D@~efO&3kE-v6K!k-2vJouQuELz2`_J&B*)O%w zXcMkR;gn^39}u8pRh!cW->B2wa9 zZ+QBxCRKIvS?%=K*h!-8GRdU7x0vf52Uj*lsB9%iK(!^*hd5f7#^$-oTiDqLCLtNa1$k>!WEIiA0}pIMe92wTk`_%US{9k(CDpV=;bY?+$KdqGAZA|d{>;y01~!F&pS%ms z(eyOiLq`oAX}6bQ0uX~m@sMxxe{9>y$Kmkg9-Zh7e)l2c*{6u45Glg`R-FOM+9X-ia--}#n~(5`U<;_&Ezw&`}>e_Q0$-Q5jvlmDACHO`&1N)@@Tod)hGGkgzMNf|7}s` z*t_(4Gwuwk^IO5zv_EQ8cl)T$DM28kpl?8=MV`=C=~?9~N(V!_PJVCo^l<~LqvjYO&DfRF*njkyBQw;n)g>m?6)d3ixLu7SzED7 z;f6y-w6wP9oB)h)*VMeP_hZ}5)udA7l$A!**% zNIHnw=9QVHF&Pnx7kM`vFX_2=()XQCu?6VP5czT?O;zsk4bu7eBBUD>&|c9Spe1^{EEdlRRpa0f>R*37vk{ae%P9)aip^t$6p_nx zW>rR3nH}Trs|CHm@}9q+xHP#|<7?$Vi)*(;%CHg9f` z?mA^>By(yD8oV<*FsHU?wJy3<#mD*;tMGJ3QffTt2|t!ch)~q>ndNFed}d%WAk~4JARK=ORf-Y)LfwC zKDw9=)k}{^2eWZQZ2R2cRjqyc;d-@KmyVYrL?7spzJ z?#Tn=jN9~$4}?~TEAxVWS9`)HeS%(sQX$q7HO1eJ`O0T6iJlBoYi*fmhnsRLKGYc7 z-k=;bvQSxd0t$2t?!^^a*Mx#E;E)bIV3lLmU5yzeT7bvew7;28*1G!0QuN{*zqZt2 z2=fH=z0MqNgD(XOr^z&g$dLzeGZC#sY%D+zq$P&MguwpQk!#ok`mBO;+`hv}@vP-E zx3vRXSa(w_40?xp`~^nTR!E*h)L=%FMT63AX|(T4Ak!v-NHfJEMroC6Iy8kZp@>|NthL6gMcdiw z%llmW+U<|_&na3CpuCYh{^Cu`@ulFgXDXx{kRF7k1ShjZE4Y=ClW5Zl{6Y7j94vYD zN?yTf8?Y^O|8n8$4LY4LF_FY^3CCZetos->@!ausEjq}X^;^h92lCLn?QYXtbq6a3 zQm2_LCFH#OAGQ8dAuYH0Qk`XgNmquOn|{gWR2~Cx`6#4cz02h>5LDOqg<^ZNXg{bV zMWdxx6)bd#3puGfed+%lqJG?AA9h$NuLU+0pZGsQ+CM)VIDOL296aW2j|?xB9mpQt zo|Hn)qmQS7dh<=*f#oFGuj7o(T__X9# zUHj^H$e~#cC0dTDuC_rO*OEF%MFo2i#b_8d8e>=Bf`SEV?Iz~g(g<;?aun zen`WpNLqFCIRilpc8J?Bqwz2FIC;K3Bopl~UQ=3W?VcLBC4q}%9@?ZDt^1rcyOIXa z%wQ8xj!VgnxwGr`^^3u&7X}D>cLCkC!*P-l%nS4qgsajvg~H@#Wo&wfW)>faX-7yP!8H$|ApK0r>onN{?gJiDKs zAYC7AUaxz(Oe9~<7;gh zK2ZkgQUXHHpJS#4-e0Ge&u*w?uZJD!6(vyYX zjAF>jt-2aNFGik5Jg(<*8#DB8@6-K)>uM?I(J1w96@-pf*_M@;am_CKcXl-BxRKMi z!K)bM%BZ_72kBb1UGBj^H*Dg{>mARyNjE#1hVH0`MjLxJxO-|0$(@lXabu7;0!H&* z5SF>c4qdhREZZ|u%HrO~b8NdT(JYCX+Zy~)HYQ%Y%u<%KLExiH1s6MXZk^pFBJS#E zDmQ^pXm}hh`mBEAWANKRsOYJeij(C7Ae~|9&|Hac!w%L|4jmO8D&M714p_RzAAhpmqN+t?p&0#^|!XDVgfv z$=l;MosgKBt}>-fRu=DSePa)+FYA@`j?cztQ+V2Gbw?L?k2AK-y|$vEV+Pvyi@i~k zo7o?n`vDeJ@%6YrV+q>9WnEgFG}wS~tz`y1fyb1cf+-S%<@91A?`!NAS?MQiY){rR{f z7=YhEMQHo?ros>(n-LE*ncZqYsVXJouB`Pa?CCZPY*KB}gR$SgV7*sgD34N2cdV^RC@j7Th%1cazN2nuN zBd{(W(dck8@L(5*l;w?I)!d}E@P(7wbUXftmlcGHfX2Eea(islq`cI0e@On#H#2ep z34<&M8qNgc^wzBwVh$XJG!$VfnCvovP!OieU@)HE+YU$z(y0FxU7u4?T#uJj9Uel0SC4? z`y;}nMIDnRU!G~&XNn5W$V$);;gl1VU}ZhPe%yl5cROEluC0E-zg~iX4pNXdEZQk5X7C`9>TYQOObD}a2U>b>nsoNY^v2F7kG}8P8o*U8 zd;=_vU2*#4}El#R(<^^`O0@VT&`1*084V6lWOKtmKkM+j}c4DM-aYSP=X_>%ry2zC&=Vso+ z{a2(V8+OHwP|jx=NLw3i3??>BIUr#9+N6K2@J#wI38{w+1hbGHj@wrZPZ2~S7=k`S zq|OzG&Iv?%z;*V#A*{VmNE;~{We=6h4J~?htq&!iwh>qm-`x>jMs$AIikY*Q4JCw# zpo&zovj2Q5N#y-kb>6MdJA}mPu%D54w%mpNlBQ#2zb$5Jt1&oyX+F)kE41@oUn{m1YJ zb5Q+2O2e6aq%3u)85QMfYKen*iignc5Tjq9Dra79S#F9RZByDLP^;d3w4%iorsSnVEyMi-cl<{pFeriByVdfqGXkcbFO z6lQt?TmD|k?nh~Xw-5im6ts(RYz5T*C9`p(P%3+;Cw}=bnF;Kl7*gziT^o^8EJn_& zfK)|5wj z&?mWcdx1B!39XKz2&i%&?o(Cqw@3+F`jkVkF8PU(^(LT`o zxN3r9shV?1>6XvbUrS#oPy|8J_zI!WEmfOdgp_jh^$1$%wYk?=G1hUfJPiFG>{oQ{XQ^YPP=Od^#Qqan5Ty7gb~OA zV4q$W%0j@v;!p4Iiyaq#^XIJaU8nEZX;j`?9#!i$0zog{``=?u;RU9jG-f=+_e9>? zxp<8wwrTed>)O$ok`J7I`;qY5e%|3i+KtOfn194sc@Hn<>yHTt@HFgp3!NGF2|up| zcS)ekEKBI+OqGpF+ZE-b5ZNQ6>WqX?2Yvp%_8ebk?FfsnxN)cDo}|jV@nbysAr`TG zLkP=5HQ;=ZGF?~Og2!s!c|To(@dV$MOHl_3R2O_cUe7(}I<`~p)Ac}0@ly-U4eSIg z5U9hF)!<>Mk&2OS#{A#NIKMxK?b5t1#vBhr|04XJl#sT323>p+Ah~hB^|&b=&q)nCfEDg9YI?%F7y-ukjAOMtD z2$bvxDkzS_l}8wVWz)9cqrd__3)B_DTiT3*!t|PVp6~KaWmE2sm!F~_Qv?Q>WnIRM z-w{~ngWYlc-}Kj5`SqW6xK&FR6%-uuP_34yE6V{z9xo8rf4#o}8qGwe-M`eTh*M$O zNUJ13y4BDFXK}sYC!sW35ZnH>GV=mTL5J*^B@U#9$g%YL*}v}9_nktD$bhy?b#@-s zp&)y(#uz5f1i1Ori{nxz}d zbF5JoaS*4Pq%^>x-0SV^QfmbUh;4<6C)Yi~;BiW~dCJw#H#8eVl(eXDk8;`Lf19Ka zQO=kwUl6>nMbR?$1i2KMns8ne#I$cQpu;9ln8l1?;zNPP{SbuwDO;StC9yvUiWE&r z%9ImwYux`#CE|=;cd9_XOc&QgC8S-q(}#|!)SY%wyB!P?2aoeuo)bxCU8jyShWevj z_;r~SYN}XrA?y4{+rOJeaV_3k_Hxv+rdTXPJJvrcjo*K9IKH#o4_3_?6w<-L8Fhcf z(l+Z5`Lo^WXhrJ|#c82F&)ds@w2s8an8bIfj%Yfcy<^6MCblZ}q}jFw3zbqOkLAPu z6}^bf0_<(BhzfEN4v)CtM(MxI8l-^eacNK-5(}#A0sLin{$=fqNcPnkW8^pQy;Zfe z*f3}{$pRMGcs_W8XA<}BeRbRS$({PgKa7r(aei^t8eH?%_AldU*lV=}wwsvPsk1^n zr#H+2*58k~YE<{5v!a`wXA%Mr4r#ZUJ)Z0a22ZZ9iU7lZi@5vbu-9UQ4$^Vjc@$UQ zFSLMIQYX-o(eWf& zn5>pVXun#Lv75}N<}0Zb8jC7Z-C-V4r-jz-#hw2QST?jp{h&D`P4w-13r%J~M7;K$ z7JUAUd zBTk)VD&N9wM8n$tzNowLmR6-H&G(>V5+-U!pNk>{l?|1i_Of5tm9wQ)4a4geDZ%Os}s`%%n-++W{8*>=yDi} zn3cgp75&btBP*n~Ntr||peClGXl?$aBz7U&)pisY!JLo|`mPkytKL!Nx>DZOqVu|Z z2)UvxK>J)wY@3mURo@-0vE}I0%Pm#{vEcnJ#Xh;YTFmz)S@QNCt4Ny}_@3y%2tJAMP zF5a>q<5+TG8fRhn~T#UTg+L?RuXvijz?FR-Z4l2t1-%-eJ@3&Z=CkU*eKNB?ICtnu0wpHsX<$O8&>*~y`y(kfNyTU#)Z5LE z)c}U>3l=}`C3UrAk}^_Iyn|@6mHbF{hFv&e#1po#wcaX&Hy#V0laJ&$zCT0vZAira zP$d>*b}uJulHV-#H0MkF6=O-^XH+~Kr9zYk)+V#|^d$SVjp z?dGjz}d1#>SMY_=L;R84>z^u5F`*b>_gE)Lnn5M4F@Q_eHgyc zsilQr1zN^~&wp=tmI)pf9cNE4e(uS{Z8itLkdpq>phXV`L6Yr*^>Q4unxac7LJr#m zPP7QGKDLs;kzjz+shS-24*0z82o5M;^Jf0^c}&s0A6z*RGz%CnS6|d(*rpa};#QaF zkf3sp>!ky02q1`N7+eLlRIRMg_&;6%MZ*#ySn*{4ILB$Q*?WMJ1W!A8H}feO4??5U z?4%iMnO8i}#fMN5h$PzYo@jAnt6Kfx14i6xY@)xO83D)V$_hVW9sEiH9a zqT2o(ZXpeW=$odYX%1PceTnHe4(%J`)r6{KRN7)N=y<^6o9jUojp6uy!eu7kUwHKB zqd5D3p6=$-36MVrwruY)>vnbe2FF^TudQ7*z(XP7^raAshWSbWM#^3OD|sw(AFjzM z$fXrmZ~|^Hir>@&i|SYJX`VwhfjbLAMS;cQi`7q4#ys=rM7o{TA2g%wK}!4jwf#iN zW|rqB|Hdq+2jf*xX~%Ba7zU_B#C*l6)b5)w4jy;ne3yQ)Wagh$g3t@KB6>~6ZDUEZ zvms?A=t8wr>SU`WEe}_pS;?KFPrbp~xQjxr0c|yMRBZkw=_QED-K;imL#SfyX=2*Z z6*Y$s%$Sxicdusl)mYJW(64y6hC0sB~sBa^Ej2OKd32elb{yOK??a~ zy)D={MxhFYKFCz(#$7+GL)qy(7T`DM9d7s4T^eE)AgR}3_1r=T-0B0GvIj-8IZY_T ze_olR#Mh=fAcND-nW~O}4r-fJ8A;DDdmI7^>6Gih zh>>4X7+NOeU(Z~h>a!70Hu940Oq_M=J`{!33O3xZ>BLE}wf)9M zsn9NLV%q$(%M$H@{rw%BboBRr#aYN98qtj19cPf)m^L^m0w@C0SZ6W|1pN zEJqj!-cKkEPJtcDL|IK2-!5AdCR{6e3YOns#QQ7LKPXv+XED>}JE=%OkZ6 zN~=TM^1zvLmgDvB)n9N>J30b^kth6-AqUMk-V0({MC^zGY+A8-3Uus|eN6N~9nv1A z_^U<@3qCpoNR zQ*k@{SSMnlD~{*zm*;!mY%Rn>S=1NiVP-=*73kt7E(&5lKoxQV34Ayp<(hP(7^484{2^GcoPYJJ5zYBaWgjHLiKGxJ`M?BSauN7B<HEpp%V_EVOo}BiAAAD%e8gO-PT|`Z}T$B_w`TGjn z>wNA&Y`AZyj_Yna1o!&hIB(nIirT*0_x-I|S`gTBH1fRN=3u>rlj1`CX#Ery)U<)} zjA%$7QG)&thhfm>5C;B!atDMc8U|6zS7a)ocq=25FU<1bqAlWC+ZFHBg~BzsOjI$s zS;bO+@)+rwgs~4opI{w#+d9|wO*oeFZ+Eb`)>f+lHF@dMCvIS*$x0#>g9A;6kSQ;-QVoX*N`2d$ zu$*X}2ziqB!!5V(nAN7oaZAJmib5V4AWFGA>8Pn*6J~uWozJpDnV(^&?Q%o+x!wK7 zyQ{1P`Qpu@YjB&bTzWVYT8RP$$Nbiq;r+Q}^COgY4F>z%P#sZG8Zx%zNY*;eun1vo zUy~ylUU5aD?cnWeRPyQzYY?7r?p!&8TgF8sVC8hr{Y9xh=C5Sm!Y?cj2+lR7U1(!U zNb*??&Ii5NAq-eKnlDT8JsV$!W)T`7h*l(LrV*F2!Or&pKxL z`363$L|$5H*I!d|+%ED9E48=Rs13j?GUdPJ!&<}h$)$u!e7Kl_`3*_fy{YCj7)|>+ zsDOWp-+ztbiegF-_JX;kP436d5(7do7jBS*8Es8B0hT&KNUqRF>6=@+W;i~pl3Z{~ z-x`_f_I@%5W+>`@DQOVkmw-rTEAOjaewk5xc`&$GaZ7} zqIR#(m5$GundI2GNmweQI!HY$p7cKLn@6vl0PYJN!)CdMI*jSW3iFrGSfmoaSGW*c z7`adhL4nW+|raMcwR)w6Pn-@=h)nC-X!Tlsz?#g$Vn8Wo1}xYR2Ix26y- zE^Zu!i;wMj+OpV?>0F>T%*%$AC1o1KEd~lvrQn4S?S%S6yYrYY!)%-@$r{=?_nOO= zAu9A?wq6qhlFEbgLzKHQ(0_PEH`^HMV3D=+rAi7z#XE>#6S?M}eYt6LBwJs09R5}w zmZ9jxl5YC8s5jb%;10N5B)%hG>MgCSkqM1LTKQi=U;!5}42l(xs4VjcqOcEHP{U$i zCxigEa!9s$Wu6=c5`=TI{?|;05X5x`qAnfiM{mD#0Vv<;YyIf(f9cWD?dQa^phpt0 zjhNHJ_;Q-Yz-UNsl?Us{e~Tdp(SsF;4<(8ui^30#meSk$x`Dxkn%>Tuzh;AySDj!o zZyiSX@CiW;ESi3MOGK<_;VJmQLPP;&`2vMeK1zRe9#w(30vMz-miUN)<@kia~zf zEz1gy`+hk>6s;@$3KG)fBcBC2UKxFnPfA-h(AB}Y)u|se`K-otQgipo8m_D zHXTp8iYS1%*No2V14v{GVCYZs^m-C$(->_=Q7mXr!Gh_}uS`GmjvKq&k2V&kLiuPfGXS(}FhqEu9{s{%^q@wG(EN~Za!Fd<(_22v} zb{HJTMHvSp0jtsVTzHPeN{#FiRQ(Fq-uy3IvHb5);sCUeNz&7`(d+ed%*2ge#L2nU zO+R&?vf0ZDpcnR+@o^j ziw=T>T1&{n7~IeyXNuSX z&FHhE*v%DY4tFy5X^)q69mnvx;x{R#{)PZy;APIC#2jF9ww{%|O zO;muA2gPSS?v_gO>I>e<&p&@9JB_6vpaU>MCGyb5qJ+Rr0-bP!fSCwpDv5deDdeIf zU@GJfZw;SKTb)Gmk>!{0jXS=HC<~-7u_0L3B9us^(uGAj6608G&O%A>ueV&sg>Sx? zrM*k{!y;$Ma9}af?5P$R3T`$p&v5XN5nRvXWG+L73>h*^ebnu>5CW|;xr9Sv9B!TX zD4FQ=D+M_Xj^ya`PUWW`|3}Vx{dp`}x-?0A@A-{g^&+FUGVDn^6JTWGq<3Nu2SHG+ z61gQ@X933V#RM@9LwI5nlB3bqq>ktI96%ZNXU;7##wIyJRbtDnHAtb-Oeh%+7F^fe zZIQ<4Jd0H#8Z9T zulX)(XH?i)h}qB+C6Z8(%EQ&qlNhU$z4DU~a3bZu8WCJFi5#eTg20n>y?Q3=d)l41 zQh*W#PFMuTp|BJupR3S*ZIWxtux$A?&_u9dX=>ut+tK1o`g-4R^- zDp4pVXb+Zz*z)$vQ{F1^I$5GLJ&mNi#uU;tOkYy(BZ)0e{31E>y!*WOaNW;uAlMn= zcKB3>s}yGB32YT1BxsB0`-D!Fs;nYrL2jqd+Wxg1-FGa`{`y$PH9`y4B2-Fo84Ixi zsUU>fv)eF{=Bc&oS@Yl{5NK|^=3gmV2TywV!Xxka5NYOmhaqH0wO}asx@?>|$g^{5 zJm>9Xbf!kn6-09_vQuI^9O!^*u8NaaI5~+^l#o-&S_BQ!V5~{X#isc_B@id}NX0Iu zY>7u|AOw|~#R(yAazuj>;bzUqmLW>j4!U=AvtXseYr+%xJ%0&{%e|}?qYSa0USTMy zhzNluA&Z1fi282jGpwuRT!kCexb?b!#hpK!Q_lY*)X{xO?on$HY2@_&@GPwYlx^0) z_3#%R#r7a(2a>sdfv_@46oYR;i6pE}NkPeVC`2){eUF*e@{4cXzd>Mb%Qa=Bbq7#T8T$S_S7;z9@tg#tkkWCe1Fp?xQ)O}qp1yVw?P=ZsPp-}u`b zIqU6baQy3+AY8D-qyjWq;bqvfHfBWUl=zerHgC`u{{8&dIR1$SEMRE3fhPms8&PjjWbh2!% zPR#mi7ms7QkHf2YjP58?n%_w^tPxdXD5M0F5*Ta17icfWpW#3^-1O6%_=}4^MNSkB zkS(e!>Ln$+?Wt#QV!_5Ip5}pje~XL5cfN50ZX|H6B=7k6M&ZSRqf8$~qlk49Z6+ND zop+Ek3+#|X40QxVvkYsB18iGVrawPQJPVvT9`p!=r_n0Ls2I^9kiJ0ribzEmN2juf z$(%wWl3Z*EgptYEuO&Lr1Vc5Pnun`%2rVf^Ic$H#=rbd@BM!O#9HpR}x4-;+E_lT| zIp*;K_x<$`m@|}P#<0sVhn>RhYoDe&3h5M3!w`!^uhk);AsnNVs3@c95*~%9!q0EG znXg~>eg4nqKgF?cdO6aQgeJxnYX9448qwPpO_xTKz4y{f`Q4Yl!cJr8Q$B$X=oGTi z?^SA`3j&J7^a{xj{^c58b-_DPCoDxwi8;M(v*#X)#odcp{^pDM>M#F=xT=$At-v7i zvJtjyg#||v*%+B~h>J1zJaRAZIQ?yj<3DK~ zpC;QJQzyYPWH`{s2(E(+*LAZ9fD9QjWSEA?BDfBrx*YR5Nw^JGC@co)!C7y63-{jh z5c8rwe*N8FaQrKmfCECr>2$nKa%GFG43o4on>bYj0xKYv5E?@%mq$i<$Sq>DRwbm1 z$aWElo+O&EtyS%9OrP0wvmfUB&~lY@W1EGZX=L(W!vjS zRSgJ$HP&N9IJmTDDiV|Spak`}vwmQ0pCOtl#}QoPuB{V1w*7|n=c*G>f~Gv7O1#Ak zIy55C+AbH(*rUmETa8yn+;tq49qT0CT_n(+njuTO1A0Hl=>u!S#;}ns7(D4zx>r5e zR{v8kEy&mhi9tB&C(}P0s6tVs$b0|jJ$&WPFC#`{+`J^JggAKzOQP**OoS>(%%~>U zU1UU!@XeomlTTdq$pbT1v{ri*gHW7x>S@dfi=5DXBrXaO8y3~N$Xf?jI54X5QFJ&y zzI5m+mKgLa#C?jbxiULDL$=P}#$$zbR68^yg@9Du5P2h%TH%8MeIn78xCr+bU zg!Kd(IN|hjdHUBoi0upvGcdLrk>=iNy0rJMttCn2Thz8PE@zi^J;M~MmofEYu#~Kz zD8>^WayK?T7ElF7!-#Eej1UUD%c}&f0*EG+t zgZZxRk|w&D=*BHO$fu^0Z32P|hcT4Qw1q;VjpqDUzm2c``?b`zR&lx|QO%&k7^zay zTNs2Z5FXTORb&qg4Gi+=(4(B))rUkKn36~%gD?LIlyYLAy5Oi>`<_I;-Lt=J*K>+iXY{01{~t2W6po= z`MmFq?`3J%aezafQnReI!FlPiyy3df!Ec`dJ<1&)x{KP5fT{@au>?f;fP+S+(Xu{J zuu!rwB_f8Z{cL{n36`9E0@wcKmHdgkoZ07{lIWYN$vXC4wBHtztDaL+TTn+V;txM^ z3BS4epBOfRJfMX@S&M>3e6S}3zJYFO=^QPy_MUq={Ee>%H;JO(ciVH|d%Lj~M@WvF zc^vsTPdF4Hy9LHG5Q~J=lF5K%C5VzFj6(cApI~c^TWd#DsQS zB_Y#xdaB93cox&G^X!%dcHtE4gC%vW9}i-p&&;WY`t5*uaC`?XArs05W9A zkRh|*8K%r~N+fv^vzYrQnz{ zYw?U&YqA2F%42R?Ut$s|Ju~L8tryY^($QLAaX>g}kD2^v^s4krRXwiE>O?@)jwiQ_ zwMi7kBuD*ojm5Ma_1*bG2m+g?`wOG)%_?;ZpTo zAGZCox{+amV^^Q;=`rS6#AJ^*X^WZcXe~|)($M|rs&;$5^yKkZJ>LdY?BYj{3)72} zs3vi!%tHi9Ivm%1Jn#QsABKKOJR0Jl5L)0$w^@}6{Gx+(H2y3XI~%_9^Y0K5VOqoU z_97`<>uHT*PkphM{pJ6qM#PTt4vv^Phhy@yIks~?M|I7k*DX+R728YwtXs5+|LFb= zzdGd!ez@db{{7?!c=#2YS@-+fDW9crPH>P%K-^a&DhAL4a)!d0AS<9Ff2KN*dcQEf$jq(k5es za(U&27c=DL*h+Hc+o(# zESQA=M>)xx9g-8zIDuGH#Dsk?0VK!`n`>hjk)nK(e$Hsbf4_0Pc1fo=Fr=pSs$Y1?o69 zL}Js#_3ZX&|6b9ur+Bh(LXseMFNWtmEqC%i69}atx8v z&^DdmU<`S+D?FV1vUqq7FY7#$KY!h&-1bjDO-$8MRDr;lv`!OAN5?|WLWeQTbzEi% z!GGQS9f;#8vLX=;18#L9Hz{#=+iTy3SN4boLzIySEil?P%FYUlRR-w`v=bq^1jPjo zYX%;hMv{K*GKEbPnJDrn!*nt1Htn;bF#=yXWSCFmuX~Awz}?88T!z zL@|+TsJ`nSfTOzUDiv7$!(Vdujd%0#f31e-&|ha|1i%4mUp8qf$^j#j=#hLbhp2Ag zvD?2uZ|z?C^zX>=Fdmz5#R#Gv^&$2wzne(^s{4nM2#yRHrnwDzk=sPu)~ENzJ2^I; zdyC}am@_|l{mO=;IN=sdo4jFLZHOk3&Otp6W}C7Ny0V`3zI*kf?I-Evr<&G=_(C_v zM$xF<%wncF$t99#NEeq2&%XdsgK#iLg%Sdgu|yaLY$`SCDab+OYjQI^s;bHjKez#n zLAYwKi(|r4OnID_oPG)wC#GM8JXl`K#u)?Lzwl{pKWPouExer@mi?NaowAy>=ZvuV z%qqcIz*0dtvrMgPl(4IUGuuJTl87Ejn2QLV0PE@`QKw8M3s!=*mPlI&1*!}Tf!-Ps zY^`AiHKox!#eSb)m0|Gi0Cj)Fj8#zj4J=-l=k*__GBFCX>3|h#POGR`_A>EE z1hK@%0%D7_0*pnVu}Wjz2;;`My$anex%tPpV5UQqrxi&DKydepv9>PDcYJ89xK;Y^ z_&CW=T)(nqBT`*qVQgHj$l5`My+%fGJs*WEvtrJWAwz}?8K&$syzWp8;rN#xPgidV zCo(+t&}tsPdjod((BinVd_)ItIcIgUG1v%!blgNv#@6T%BXl~0I6RBbP6We9!j|BO zJ(JIC6Ygk{Hxn7lwd=Z(S(~4#I2b}S)5*q-?bGZ>d5D(0QWKRxn+r+o%94qe$cgDk zZjCR!gt-_?Tn?UeeCqulbLy0U!`bhy?Agzou09ea5eSZ)v50rQ@m;7vg&hVFfP;h4 z1|gGZ$kj(v(JPTR&)XAUsDS$))Ns= z@!r4du|UUJih$|AA{sl(8Qa|Jk)dOHo}F@oafWFGCo6)?iIgG3KFz_>UFTUF&yXR* zA%gMDw9h6>7E`aYE7VF{2zV2n=l`Xn2|tw$wZY!=K=m~HR zbIEm=ARnULh)~4HBIG(fglEw*MoS2&C2h*q=URXjI8tGBx_+)mq9H?Ei}8g-1P%pL zK=((~wp56=ROqTg&rUdJ$$VaN&I!!$is;cC1DgV7_Z-DVfBGrr&RLM;9K-=@k-|+{ zb6tB=B-wC8JxaVq9k4DFCSAdnwNH}|9ft6E7E33V5u?dtiHxD-`j{xfLE-_PHTW3( zSkr;z6g8jHtdl5)C2~QYb*oo%?2;3Zk;Ce1ZsVul_&)#m;}ytbJD~s=ldy05V?Bg| zBadW#7|^F9j&>c~Frr(y2{Ec8log;MFD0XKK%eh3#~5z^_RYNVgp-m+phO$dWqASZ zK%zB^hs*ol@_w%S$u}u)tK#SK#FYr?C{U>coHXDn@HLf64e}C`)2w;=Q5MZxIK}-) zb$QtKHOSc)4}b;bw7aQmqdOVezLwGcC#epsr8e?7aj*fSx8vrP;^Ysf*nJG0b55dr z_A%s2eFydVt0UX#-|#yIxBQOEz;B6bkK*J`z%Ly~X~t4!EIfwX&Xq*f$I;Qf0F)Mfom%x+a`}AQHCHZQ#wV^%<|Dlfvf}#dIP-`hf{~{P zE9=nVQ@Dj?c!foj`i`f2{wZ|LT#R!41MsdS95^GmGGxfGZ_`R4P)ZR+QC1ikGGsV3;KXob579j#Tb_B8IY*w&rvG`C zjr~va(zl!q9*0CImkY2g4HN%(sawaCb+aQ6eHuP_ZHy4WU~mjo8>U=MadTL~DcBDTq2lzW?*@ zv+VrSkk;Y~Z%n<9FR}W*%ot4|LKKP!&11o1eEYU<>4}OHsh4W!d2~LH6rf^t1b1@&d-~(L! zpI4$sHNsa&E0ah-VQB1d@{)Gwce+%aDtAA2H?Mr@D{*nBu5Fg}S{xIpWfVv-T2rg+ zq<_PGY+L(1YQrlgy;oR%g0Kvg9k6{}^4^TaU*+(ne?*~k)?Po`mUVZs{;qS{-aGSM zHI(ZUhgF__;5%%6;$vV(+Rlk8PZCvr%;+}QvKoqgf6CD>y^Lb-{6psZbri7i;eTiI zqo2f@rzTmeG-ZzKvj>47j17HafTjEgy<82z&Zpb24Mx@wjKIiN*t{B)dnSjS@;T-$ zITxw+p1P=vY=1%8(D*{J?AY`$8z1}^ssq& z8LoWar&_JSER)Hr*ffGm5u%g@3BzUd^%*0$8;cxfz~=BEm%^2oqLTyyuA`P6@W zg4ciUwR~;uwG5uv&&bK6xMz4c$2r)Any_btuvEcx8pI3-cUGP#Z?Gka$qQn~V5Dt{ z9aLal9Fd2}B_6RvJ7es2pdT25!e|crtR`AjL*8qc^*|@btvZ}D9+=Ndf7Q*YtKd%- zAIG_~ma+5cVOBrTPbq&oANt@GoN@YFfD*1>KpWHSOHGD$Os`F(Alv2&^`eQAX0__R z2gx~k28Js{_zVy+0MuNE5D1ONL1NRG8zlu2iyPON;RzH~wrt;kqhm_3Vo}E|esBJX zxI-cH{4S1AT`a8l{PRaY%`-o_4{E!gaf#iXk-1Q z3P4d?dJS}-c=GYJpeBo9ePJPyc(}at58p{i76=BbI9d{iI?ab>QxjmomEeKR8RCwZ zYk&MT9^3Kw6z}&IV^Yn>lh)zRud?=6 zXVCx5s=fYR9o1O<%PZJ$-)qK^TN5Fw{*-mUI+G2nemt#b6fY`*%cRr7h$I1J{iu=1 z0+)eNU<4T6gQJ?7WHWyLoFT)sgWaWDZDKf%LmbC@{XLef(`1-F3-77fTPHg*>|b?@ zWTwzlVA)HKVZ*QgoB6pSvkIO3@)!S;Y%NL<(XlFi13l zC{e#jmT=nQhTw(Zg&LjB zu2AB~A{$DloB7T#u8IykIkNZTp|I_ecA|w4nd~?yzdwESefj+su*zXaHNePb30!^g zvbOU+b^S_ep>8+rBaUJp9Lkx3-%RI;af%$WR}6%KXh@1bT1 zTs$s#?FAIWBDH5C+?oevkj6pSq_t`ZxXBUwF;1T%@B`KltW9JAZ9V?3*=h}jn3x(> zhQ$zrVu1UH@8`2W`7Cew^qcvI+x~&;AHI(2u@%HA(EEy+sFM^Uvqq_Ogy?RAoGtL? zd$_YbC|Dv9q8&{jB4Qgi66i@IaI}jP`S_7fZBv&4v8h^a{q`*gEihJ61}b=rq8N$=0g9^DX)epeW1y^| zgdm5ZgP0r<1Mzbx86)DDf^_j?#q44aCF^jw*Ubrqxh#r0`TC_-@YBzIy_uKG?EP}4 zF2zdDr)%+1EWh+p`V|aWs9=c^=*A4oqO7IE@yO|zE^8>1N7-=qT~G}$MyDlbr_|IT zj$uZACf=}%sz^-LAYuWrZAP~V+lb=QZcJfrfsy2TMI!UXFu zc|cSpOuUN6Za<0sO{@0b7RNQ#-2PWo2fw~|^gr2-@=uB?Cowq*+`n;-?FMi8OmNXHkZ!*frXGKP`)RO*5Q-&n8}Guft{QL|WVT zjOnkLPb$OI=qwYsbtcMNzN=2g-Zg5w2WN=go|W~W{c*BbETSlw{nDfP{2zakPk!;+ zP~8d3<{fg|g2v`ilv2K^b1P2Py(_@_>HtsVyV+?1szOmSF|KroL=t(WfLsh-VCkv} z3L}E9@+i6fEj)4WKhdY|!^lp0-6ECv9IAFU*eIDNTASw3NSsHu(a*;oiA_qTmZ^QR zqLF(mqq&+D*C;h{=*oH>0@ZZTta$yfd-EK2d;rA3>`{|(6zxQt-QyC}Qc$&qvWbcC z)2uu?k?|UXqNmVIl&7lww_Bz%!6o+k)XBC43@$yH?p3QmIf=U5Wcj+D=U?YcI2N~x zsU0dtK;YtXOwX|_|6j|w=2zF?%*|2DhlG_FSjN$dXTWv_^i8PrNl?#ZSJ$ z-@f~A6UV9$iPN7X3Pu7osys5Xl5gMsEpGVvO_V!osCkm;v>30;!|if09h$0Jrs`KA zr%(k4kprXwY0xUd2#YYFtU!y@zE~1DLM^d1jV^~U8WI~&QsM+IvZ|=AtuVMVWae<5 zp6Bx9)(I92_pu-<(mNn1^lxRZ4p>y^q%bnb<_<|*gSqZuyzY-a0oKEkM~ZZQpj2C1 zwtL$y>PbTlgqydbw56j^z#=iY4B;?JH-k~gETBR_gvE0dHjYu&P*94J(NuLr7cK(~ zGhE(|uR3w8K;mE{gChjR$YCxW2nn5|!#r{Go!s)(>$v>-FLLDT&PcqHit)++#Q3)w zc1b37RBOXoZ+|EMcGVZ?3k>s9Lasx|bi+U*Pv~hw&Kf$g93dsYxZ>}4^Z9SUIq5{& zq#U2GzAtt?A9Wn)V+J37(?_}bXI~%+G$Itlfkr5Wbxe}eRa%6v&^1k%i_rO)E5CXL zpS$R9@bJLOR1?rlopquWm}sI+(m4{3RI@Eh=Ty26dpF(lP9@(tmwd+zl$%2-A7eE- ztWp`;%FyPA+4A^Th=V)Y?zryGk5PKl5Ah4#`~0|1KlokBJHOiaf6s!?GyCY*($Tj7 z$1h>xklJWJ{m=Z4=N|n#Ow>*&t^4&GDE6$R*gbcee*bqU_kXGJ{~1Spg&B*^q11Oc zu2;mGm|A&&;qB|#zU3K`sL?cXYtt$={_eHo?x{J6XL&DgeB=4#O1&7z#RL_~J2tXy z{cY^r@aJvkN0lG5=8g_ddE+*z1gx9}`VJ%{7?1*|ayl|crd zdzfcd{@u9W`{X^JqG#?8re#j9LzCdL<6laVn8^z8Y`M&^Y--YK0oGMig}~%;NaYeL zXBWmlana|DXzYu~kYT!Y5wV93zUOm3mpP#_Oe^P!*uM@m8KG)rb0UfSuR4+lL1BW%4kWcb;?Lu0qX$8i|ax^%v zjO1#RMOG63pS|}Ev+FAF{Xg$oyPP&v8gY-o{Qh4edazw0YpdD$y@_MB%ElTatZVAwFUfuB9}Q`T>On6Llnc4~9W46Pi8 zg-}>pz;!w5UY(j(!}=DApmG`&DiRl`xnbqfL`qi8?1{p2X@b}|P>GOKVv=zKb?|CJ zJ}FR4a+skA^K^{bX6V~qpnJ5$K)FPIn_}UvJPUW`85pjxq-I#)mzXCIR_er2nYa?t zTZLP0yot;H{4?Mbq8laE2L~_PT8l#Q=(>l=dm+lg_WIM5@VIEz#Y9g(PQCgRH8JFz zP;(Y>4nMsuIA_Tz$XO_2=qBcg&Fkn|wwSz8c*esaSPPzok_0UF`zT5dHN2HGyv2O- zE$`zUU-$qgz2w<6s>W>$Dr+H~tPiI{VAQ!hEMLj4e32csQR<>8yNJjs0!g71l>;9~ z0f?+)Kv@$R+T!AB|fE}As?pwK;!LiY=q zd)&D^@E?EB)*TpEzs1g{?qS}F^Jbkjam+vz8aL53QnrS%Kr`-Z%D$%NY3icD3lQaD zoJF~`YHr%CrSV;M6LOerJanWZM~~a@uuW`v*Th3wKCFK+iG z*6x7jVFzIFXce1!UC=j&<6rSA{{5~W5yT~a^)J7CMsaX;Wbboy^6I64k{@8A80`n3 zR0~po!#bN)QG+;z$_Tk)ERFzeVhUy#J?=*g=r7T)zDqaj3D}i!D#!taY&+7Cj?h_E z+u+F5w0Mf|x%r+M{mL_!*H7rZ|K3T%#$rWq&g~ZL?QoA{tz;U{?3p{hEtV_(_Z0+5 zNHP+oFuZl}tZgQ2MI71_2!*ng@dkXV#R^~j)@=-#?HFS2tKQ3d|K*Q(@kd_5AARpn z_`+jfWbm{dZ~^pOR3dkBp87z9=`xsJL%1Lyzc5613%VfS8ALf6E4&4q5mX|$NMY)N zDl3vLG0{^I3_>{UQyMLh-(H})uFBY5!}R~Kz)Al-kC)wZCYRrH7MK3~6i)r=0#4kx zfTf##!l$2RRXoBu!4Stq+gV^Mbj4Nj2*ud4NMNpL;`I-w3E1XiKR7c}DFqfhJn}&f z?G!~EFC93G6H80*YdInukuTJ6l${}IN&h+&q4LN%=uws)Dh%@!JCp5LFF|FovsM+9 z(>`;8BF7i!F+VDDg6-pP-u3~W_?MrhaD{Fnxz#w(q=qiEYe|DNrh(SH{9W&4NI}hm zL@CNx66J7`CWSqvD0&{DgDw;$XSwOd8`Da7=aAhSfJa2qo-4<)`O6sFIgT5%s92$$ zhlvd0Qtm+K97-$1*BA^zR{$l=-~8jPBtU{oukonT8*Z8*+>Rr3+9iL++@)viHZd`s zctM`k7rdFdtG>{-fBX86kwn!4KI;=M{)Z{arOA>j_HptHKR~YM+P3}UgCAqZllRWr z<7@vsPJPLjrY5)c5Hs1d?%zpj-)`Gi8n})hx;HRN34rTvxhG>Tw6xaw&0Ts1C%y3N zAYEY*xJ}opg6+96wX$ zPY0&`D6#r?v(&j`+2)?`_Zu?2_RIzoV!Ly4r1hAMC+#%Ppf#egTBgYb56$ZS)x7eO zE0J-a;!p-80b4U@t58-D9G<69D2!*Q1XV_R#`xer{$G~9elah5?`3@Hp1+~IW*mE_ z#hoe0shVAL#u@1zr`B7;^;^98g6@SRtP^_?DimT85@+x|g$A@0w9$BphZ%LmLkab5 z5!I*b6h=MzM~d`5naBUR!TZmMz90H5`AHY^zVEaA7rmVMW{D=Gig&T@}ARPR_;@m7-t`i^7okO7ma0-;vs+%?W#OLU@Tuz`#=v zY@np_C?l-pM9!MKlHOXLlNX(W!Do9^C(=Gupk{4W>y>eUStugpY(l?Qk0G z@k*-20^5^_s;8*}k+WEro`X1tFG2{EoMQmZT&1Z#v<{4Ek5L>2Q!Ggg0Uka+@A#c} z;8%RyxI+@bRt*?w^3GYY;DOZ^FR!q9!!6&qnRSEfu(*^f?kJPHZ<)LT9URt25n;tS zSEK!j?h|X)vFqsv4(N9WR{uZdEGo%%mazIc@1aIJB1&YZfBJn3*Rbf6o2LBs*7bKC^0jxr$B=V8{lJ%|>|S~P zr|Ft=++mwvtDf^Af_(RkK3nWvME`LgXxlfo?KZ4E`t-)$&7JA?JUiNRvH*`&E(c&wF>vu#<{0n;eVpqZnt06Yur<>@vErDui_f_ zL^bZM*SNo0V_mgQrE}YluzqY&QCuUB#2MQ5YE&OJJo#Rqx;9a}u#&(=*z z^@5OA|D#e)O9c^Kz(SUt_j}Aa=VO%4dMh;+V_YxF2Hgatxao zZu`|&`P{~v7&>!=%GqOt=PC3WOMPJlv%nDdMCg8vH{j#X4NzTR1DEn_1r9^Hkrg{z zBZyJ>u@9pVZHr+D*a=~!M0dHHa4xcuI8x%#2!^M;X2xnSc8 z1~%qlyCrfFo4sv3F?=7!Ji_8qg@ud|R7Y`1%EhSxjX_(3Z!N_*VZd1a{u7^sM;>Y3 zH4sd4U_P@2@H8ZF`=|ewuqM<;D=aH4CRg<+)blLLFJUgr*o9`qrUiMi5=47xN02oH zPAK7#1M(U)5k3)qC>U+A;+mXjSn#Z(N9QTpfF4(1X)uqawvQ9)eLV30zQt?Lx|9e1 z`NvZT0EHA5aks>PN|QU`B%T~Ev&mU1S`!1Y1>X;{xQ0PFM@}H0B+M7ZA`$-k@4f=l zAub)krAz=%r#*xkpKD%zE#YWLvNIuYAs9iU(JJk2RGz|l26B!_*HPV;xKKuN2EpP> zM!({Y7W`(}!~O$bnf$Ow|8dv0?Hk*%{(ydS+3By?r_cAyUq$cI54G(Z+5CQL<2z^F z@%{Wun791dGqwW(c0PSSCc3-rz4<47m_qk_K0e7FdGD?Fkn(m-<=b0&+T|#Ha@*$z z*MA&i;zR56?dKR8-u5V!U0-Z}7E3V^};=`fJZl zcFrXwwr_^&(tVZyD1a;@k~2V^$vCG|=2WWgB&sZ<%3Kl(O{kqtYT!tC1syq95(EL& zY6{^p`x9O65O0TR4#@OuQt!L%o@0BkiqN}mUE8kV(@t+DxEg`uM$z998-9~0)9=2e z{~4^YH6A`|`c5WSFTfDw>jiWL^q3}B%@I@rq8$;#>xZ$!z+EXK*6X7lN2o~f~-5LsDg9PhYoX%`L0pHWO2t4bf6CWxF70 z8JRxP~7IDs|mwq8ym+S zV&QrHy!lUGPbD`>)$BkeqX>1Zwaxh{E`8#8AZHAH)-sTow8IDx<8TJFK5(B|k&=vd z*^q9P!#6^Iu7{in2$KL+Q7l@pk~$?uoh8u_VTp`sA|;9gA0dw-ND}fSl-bJmp{Gfd z#i~|bJ3(6^aEhGs@S^nCi-I|<^5=70rNnav*6`6c-N3_NzZ+^yRH@sEVKQy%SqV7H z@BG1ajOPlBh!AHSs)?OA2kjgoLJ^@`peHfh`I*nbqZ^O9S)oL-dSGEi_X^(hiZ`I^ znz)jHQ6#a!8jA=Rmw*R;-Xjr*DhaiGmD|5@I}S*&99<<1?Fq#TuO2$+=U1A0Lfh-H z!C&t8Z*?zvKe^Jr>)v@QUp!^s*pA0$-SOwHx)!DOT;(}=og5upH}!m0T(pN(K$<$R zbrg6(fjKMQ)joQX`=|_WKIEfxf5ysZ+`8YbTW{~jx^?ZD~y+R z5M+OWb`15bN~&3@>?Ex=wt!P8zXaYIhIt{oxPqLjfMC_A^EWavQ-%Q`3R%m8^PY2q+eMMvKqoOLJFrAU?8T}U8U9& zqvt8S`JnnVrsPP%1REqco!}&EU(7&6k~-KjFfJt9BFv^b$zXySf>OCeerJi=y>)hd zf0Xd2j(I=wS@Ex3Tyoc`yyAiLx$MEUJZsBpdNu`=A0K6Cc!Y5hc6c>5$VTS8Amp_l zyqvRtYb8(U&E!vC$tLR<#Sse_XHYE^8NPy|7JQ&vDY~uYhqvDev;0}Kc`#gDd3?aLr<;?Z9G&WgvzJ8G(eq@8iJM7432RmxU@(dgAxcu=t9%)dl&|J zYUd-w%3)h=qO3#1_dIf5Ku$C{G+ipkyr75Ua`Tv94mjIi#24Q6ejfk!4`3`|vJ=)c zty=7M#DOwg_S#po$x}QX)u>BWxN9xWrX66~Sn{IDDIsu{ZU+NSv-#mirq1`HKxv8t zmGPuqb@^4ONQj4PDeP1|!P=C6%i0u%(eqWBAuTCPk8sOB+{PoDAHyf0$w_fEECwKkp~f|+Yj40h3aF!+V9`$Tk@R!`d;sX6Q}GO-}&gQJAU`1M9)lQbnB1XUVGui zZ4iAQy1E^1aWeRB>rm}qI<4HbneQe4;Y%9m~9nu0_Z}C@x&19A)w~L z1aWG|-{{C87E4+X_4N1{W6olgB5Adhk*1NoTi;J9v{KY+HMWn9V@j1~2jpH25@}CP zYnuiOyOqbHe3%9gJCCU&M~P@pW1T~{c5*QR5i+661=ptiXXfua$lyB;qCe&KM6A(S zWoG2{HzPCWvv3^t~!zK#u3=e=B_`c2z5G&c{MN_W{>wpFkp)bOKnbKM_zOJLJVnja ziNZQggDMEBpwI<{RW>Uqv*4K~1iv`YP9avXvBejkq@Li)4mWBE$f077pz0x`j_Rfw zYS^Q=Jx@xTfN^DYLoME~|QNyB?fN?07 zVp)Wu@NG<>6nPRx86@#yB=OMbCd8mp8F!Z`ia{Q2V=75R!TET)n0 z;uwEUj%26hj(__afB44tP~hl6p}6~&+*IGS=~p{s^4jPeW4oSYbnp?%yEaf6T1Qgf zN)kOi1qN?-l0?5ub-;co^H8o>llTL!`uqpcq#n*03IWqXD28WO|A`Ype8&5V3-88O+lRYkFjpH^P zsd{$Yc#=`4fGA(2>Ky)VY*q^tNZZg=$NBEaVT#;opd!LpE)OEuhLwy;=a=l+AXbuA zrq{)zssk!s(1iJHR#VN$v(M(Sbr2;iAna7GcjOR9J@;XMW~^0fRSNk+)0mMK!z^hy zS&}Y3)>uSM^J8rpEY}j6NHGAEKx@DM)A=)bRolZP-s7}#6O(n?&v#auJ;pmHzhU6p z-~9m>^gWyVe(@`w_0+Q{&hLR7m?%aE{*1>{M`p)3n9k>Rj*N9rV@W15-7+ zL^p^nA*n**aCjI>I4NMAk8%c;1qx+~W8abed4>+s?hBVf;D{28%kJxb1~)n~GimKA zIV|^i8lIMdO^_%i!N-WdM;mlvt#n9+OO?$Peu%IB z=r&{pxB_Wns|WlgdAz*FdX~hm6X_W85On1r0OMJlSR%=~NEMJch?6Gmv^E;$d~_s4 zqY>4y2n2GGPoY*oDn28d#xO%6U1LT1cNBT$b6&yu&pV4Zz4Ti8Y9ZzFDBWjtLB+9s ze3&Rnh*V59h#3lpd35|?&Un#@JpaX)BKac!v;J|GAAcHed;7Z>n6sGSO@oZ70z=fO zc|PT&iib1p%?@!WO?he8;bP%EmtVS6q=JEudhf54f z1Z%-d@~oV@hWocZ%wVD@c^)|;Lco_SX@$ji(1m6wiRnk!L782Zt;`MQk=NbCl^TH` z;;h3ehZY6K5#nVrFhQ(gAe@6wNWDDB@^AsZ&U5uyFXN+M{URs+#|j z2Iceo-+m?E`HN4ntsc|o2aHA$f#)NMO?gjU%K7F2z0R^sEO&nTFF5a-*W#}_5wuR} z!Ie9Lep7?5JUzCL$BA=K;w``Z7C!f{pTm?aYO#;4#^_vX<2b-7hYdjji#Va`SNO+m>L8Yg=aB@x36wPwSV7rzS@7J8*JNG=sV%CPbck%`(waQ^DHg1O=e|+s}(4)HN6#)R%oR5RRjJh zYr&Q@80mmQ6yPWOuhk{9cRlB03Tc9CQ+1rkIgALNPV4483&W#h7#w4{){LmmNXQLV z3_VfIs;;ypu){#qk;65I5>eE(o*)Eow1rQqK%Xvnq(o2_Vx^l~E0>*{=~-m$mQEb8 z>VOk=8=^zk(Nq6H{oxz8FOw=KG39KxJ>sOGhc=Fa)GUv(aE;# z)B2v>GTV6GB2a)o`snq1^8^3JyKa0plqMGIG`ZEe$IoPaF%dhL<}|i0<2u5Sjr8CxDD$2t<`5WBf4T#uXi?C!|E`w9D|TRSo1PawbQY@rj4))Hc_tZHXzmE{elN%8ymJ;y175 zwQs$WbNkOE#PO%my9mmPf(aPeHp-TfI#pdK$y+Ki#$b6fkBx8SZ6ACq`9%R6hc~eK zi6?pKZ@iWZ&%X@l0~Ec>`nmcYzsG-n)!+U93ls}wpk#C>)-kNoGVN5okBZs#$c_(IYnX|$`9zvEuf1n0vVlNuK`UHlx&gd0{ZXk9kjA8?JUOPttC|bjO z-={l@_|MyK=f&4uk5wg(o;sLxo+}gzxawtB@i$-lYsN<_c=I%oim{19ysWs#S@1wY zMZI1}ibA+BV8hr3&g?pK5(?JQ;lEsX%}zjP9M{s+Z?*9ue*L}oQ{MH3gLW)y&3+*T zX)oOGV|spjH%(mI^o&1#5?7}=k7CP%;gqpC6QtfN6Z}W))^_g3B_scdH6KS{os;V3o#-U?g)_tPmzb zE)ib7_7eJr{Jn*^c0a*Mji$k#D*yQ4cPV?0dRR!SL$#tNV4cIWj`7gL;Ihd|)A+N} zWM9D-H_-*q32=0zBOU3;p$=zEvg=#$HCqxwfZ{vycq63L4ib~%)63qv#AIqf zZKg0td;3%~DB@ZIAe{we-(xjdqTgfzKhK9+Fb2DT0VbCvPmMaxhD&vwu>Xvlgtkt9 znFigg5t?pljSK?Gp!f53)U!*Q zV>-9-87-D})C>g#&Jl_tN*OU{l;CocyuU7>+gOaY)LJT9?Li+Y^{_@u=MHZvMz=At*eOphwtaB|8WP^e4VfV;11&c z8r9QksD6*a;v8KAy_8**iW*BZxJ95S_&H5tqLe3B2?m=LYtmHy$|-!S(MDry7F#zE zgRU!d(0Yi>_o@X&C`NVVWKT7x8<4@)kb*r=KCN zXm%x|6mxSJ^(*Xj+Zl`>W9?-paP{Rkuz7Sl_ul^)$DOd2cfRWtESSFnhYv0*YS97D zz2XXP|InYaII=855sSr0nto8u;o-jB)o~5w zoCVJ@O&ysj3`ZiP{OLYY_1LA<-5txU(F}3y^jC$ zS2wck((@pcG_N*A$qD$1E8g}Ne)NftGVCn#5$eWJ6ipiXb@&##L@{D562)|h;;%pa zFT)_)60!ikJ2+Wqjv2QBRDHe3Y-TkwI%kgPcuk1->HD357XDsv|XS z{)byR<(kv*8;}IzA4QSJpzSmqih6k$4}AMAMB`sSD%J{h0MN_YFSNZj(Id0&`1?9W zY!XjBkI6~0X@2q#Gke5ZgHrA5uMQTsCAdtq=LA=+JjDIq{#~N+I}e{tVd_(#oqv>d zU_ZlURwlSwjqld5f(0t#h<%Sl2iUBO3lz=@N(;V)TurlJZ2uBmlI24cj?tVZ5sySe zU8g8(jqpT*#luk(Kzk&$ENzoMo8Mp<+5H+mj;Dw%RJ0rd%zM8t^;#F z1-7EuhM}8;C5ZHqOyU9B17K!H_VqalQ>s8a^32lHlm7~7g?@v}#@oYenqd2wnAegy z$1bXaI9yADOC9V)g19MX7AJ-zF_`Q;g1!8N0%GIPI)xK!4EH^)_I8@AUqpL1*s-I_ zryn50r>v!S-GfKLAWxs9uiHa5ci$3l;Wh3EcbPQc1sjm_sx8sgDAcexERL9%I(3{l zHVkj%&b#mA*6-Xx(r2g?YQ!hhD6Y)W+ucjeiy37cau!0t>&l_>g7YoL#w5fb5V<%t zaYdn=##uo-L5#z*LUpJPHG`@{&Ib5NfE{*}w^SJ)8YdhN$c^TC-Nn~%<%_Q5ME^vJ zqEOlX%JVpNpR7_{pR9 zv-r4Gy!GAhW5L4HunvsVO#s4XgUX7P3~JbEpz8VLY>ZJEzb7eIn1>@I?c(Z3Scvf7 zcizc!*Q|yxk45(1s&_EFVG}tW&=Zuh#*>1uh#Ls5R4@sge+fp0>9Yu2*ak5 znZQMMNf%?mag3;Cl*8o=n)!?*6=L5}jh_UShB;ak= zyp=Ef=NB;*hxi!>OOUN-SyEX%NlDI2f}X4S+dKc3H(l~Z)+}6|@k$>}Hm1g9h5wDS z+W6C(rcG{j&3`ZbOP@!v|9HY;FG1LaXb+`4WD>;M_9y;__5XGDK{-R4B;fDgcTHSr zzuI8+tULaUR)m%XNqJLG)+F_|(LEgxS7V${PHq91=;=w<(Ywd<+3?f9oHn_2D94b} zQ#;-4G~H|+Idn2>6I{A&a-@L;RN7q?i^52T=IaC}3M-_YU0KDE-lGY>w*@UV)S7%l zv?RB(eV)>I0$OL>M79;aCOr@3vc3S@2{?9SmN|tsHNaiQb=Q%O%${gii+*=$GO0Sr zooI|`3sk}!w+(efe>S9Upn`$uE@8+>zKZ^Q6z*(1`(=u2b%rZ`&hVFLGwQ=TjWIuzW zZNbRKRLkzGa#&ZxQAUW#i=yl*c{+TQlw42t1#L;^VwZ-UqP6VN-k2JIflpq$ zmg`zzf!=%bJ$&ezwdvJ2xTuc-Q=vgyD$~(SL18K|w)Yq4ORD z7OPTjEu<`|DEfC6n>QX{E6SyhLLv7E_CHpyO1)2914|H8Qp%<)*h@Em^r(|g$% zKgrhmlf3>dzsZVIma=inP9C~@3s+wC1}?bZ5+ILrVUyaTi7D8aSDw%7uKz=R^06Bk z5{+jK1(Oi!26v}Li6az2B@z9eW`Rk#?Nfij+1I?5;_4F)*!Fz~y zJP(V-YK0byIE#%Edcz+2S;{yY7}p^+mN4e6;L+Zfa_iQ9#%g_x(Vr$XYtYLk)^r(E z`B7?~;4svZ3YB^d*QF@v9NMKteq^wD%2BM~v?^U?bF82>qe;X-E@V-h zaMxG9%!}531dIU{9%=2==C^7DjppkA{W@;?mz${XtPmD+RO(6U^!aI5oW-I&4+4mi zh(y;ZlybPxVz6lZ7LKFCr|9E2Mr+NFzpN)+smc<-X$RWq;0A`D`lGhb1f?rD@wp$R zdtmvDk7rE%pq+b~oax%H?Um=}1QodCVYFwl!c=5>cttQOe8k zeH3)|PCHAkcf?{S2ReTJ!zkwWrg&Zr!ATcI1qb3$NM5-M92lGG_G= zoqV3sIGf#@3@XqlOG3gpWgZ)Sj4%JgZRl=~zx%;$ROig}Hnji%AOJ~3K~&WkUp0(c z=#yWZ$8;woK}6yupb{v76&^Nnxb z!lFe!C!COHPM=Sp>nxZ%mv_AP5`O*AjW$b9@xrTK&9^`E1vZUW>B2|ewAuaXRJ^`P8(p;{f`pi;lh)sasmIkbs_g~ zF%?3pfoEvQJs|>C0*y@K-W-SL7?fwiV zn;f>KtIA~<{K{pdy9Y^!J8_uoDB8JZ1)-bPy0B>lV^$iLo|kGi^OMF>n@fyzNVq!E zk&bla(5@jWbrOt&&E*=yzF_ltEGq4wXoQ`*yQvJ1uoFj!Ln%q)(4Lo8g*!!_^vCAt zZWfIAm?$Ci{ON-fPMTR0$;yTu4ktOPvBmcR<1y;Fy{v${e_ttsrz|<^nt?jCXMd?9 z(-;_ye1j-33Fs4Yti9+Ib_R9MdCd#b7P^2q7ausYEN74GJGF^{s?K7Q?5-qX^> zrH?^7q3sfP8z?wZ-S0XzSY;~vIDoS;qWmLx@7%;Xs35@*5mTqi!_{B&)gRo>ZQuJl zY{^pDT87gcrR7ED6#J-4jkp+7*EJ{zs@q3-3egEx99H5K=2s+r?XvmeG#Iefp%YCq z7GvuPB$}WWP>hP$@r3cgF{)43=r7M<-q=DeKmRv*&0DTwWqt)EirHB=I?o(F3QHA{ zm@S*{;X@yIH!D`n;e?ew^XB=Kx=u#eI)-cZx z=ySriZ@YsRzyE!cbl5(QPYdgRc|U&a=*|~W2DGQLyP_z2b!uV4rk)X=?B2m(aT`Ou zL(ExpD(k+#5i{o^e(==!>|_yR_3%vs&TEX?&W9#_#- zkN#XQ+Gc)=1IdzHB8r?2FgR*)9cvu@L4krFAY%?MuAsv{N_n4;zwvs0>#eWlH~!>p zfB~(tA53`Rm9OGEpZ+{gMrDdYKs}0y5ehB3K2(~bF_f&M$65+W!jlg^#A(Y{9fdB6 z!$YH`z!jIjg3o;OGt@?Fs9qlv)uAX83xOEC5E9Se6$4y1eD01f@|%~woU`YgMQjp$ zeR%bUh~Q1)lDhDk5x}L%>WuSjbn8#rKGV14kM}t-0Z`es@t~c1dH5;%7M{4z-;3*I zqVnIj?JLbaW!4>kUnG~y5ygf=|68c+`eJkc_~3VdKWtxCRf^5_(QDUoLyJ8!c{wf* zKRx9*rM}&FVhtamF(gV+n)e3Awtcwey1$OK?@J4*J95Zn7To1_gVIgw*r!#psdJM- z6E+sgX+G`tLf|bkI4757qEvRQ2DHq?5n8MVQ4;~vHh-kSX5aGp>~h_1?t*k8lO5UH zLY8d}cW4@W7Jx$J<0U#R^s((@&b0(M8;RE@rgJv0y5c(i_%J(m4znm%r0f+K_rhkP zd0*U0=hAp_l26ZXI;XxtqV2Qa5X2ohhNU$;VG?j&I!>$3QBxkqO%zDj%xTvMk=E2j zAzC*rt~AV$2Bd=|{Z2u0JYt!eBxrPSwIsL%u;S33Lb(Jbj$#h1@6X_BcIU4q z2tp*amQaipFw=t39g8v(8jnxOUPYXjwVx%?pj{9LXjSu z&m*I(nc@d@44-;Ni&2%dU{3EYI&nBw89PO<&x~Hs&0v(c8H}chD{e$l3^9fXo7u&W z*Pq}^-@KVG|Ld*LZ>bDasjjY)pO>f5lOrj{4ErPKAa&@x-W>QoXoK}*oN`%=W#R%Q zI3GfNq zJV;A|#knd-!q%<7=DXkf27^!E%kwT;Mpv&#p({PbATQLaijh&vuAwTMw+yj<-56KD z`UakT!DXO&+PrKVMW`SL4S;i*=DA4Py|nc7(-M*#)e7D94lABeruU2|fl7+h)NyVsgaf*unrK(I|ed zL~m^eo(@p92&O_YERj}$!pRQG_xbm%U;m04lkzr@)Ls7>PF_F;M&JqFz=a{24 z{Yk=>pZ|i>Uicy?6sNhV!r`k&mnk-8%J~XGoFiAsQz_T*jF&>##SZ*5!RMUC*tE(& zk%*wLNPVczSN{1coca1Q@U&*{tj-71qsrjXO*H8}4{KdB_%A?QZtr+0&Rw(bM|76q z&EGm`S1`2chb&q1qJ93}@YaW?>?`)KoOQ?F7x8@`Yc1XL&u>d`CH4PgeCJb?<}95u z#3nCiH?-O#z|f|jPdQF$&hfjChn62cVq>Uh;j1iMg#tYBQZi4MlEXJSuT%pU-t1AK@7-w4z`CuZ{3&*VgR9|brI@xB zB*nM2t_3T;LD1q-j9laFgxM++i3HkArA9@f%K}jySfP$gFBM>Yxk93ZShZJtJA_=K z2qWP)PJICb<6$#N+aL=DhY}&N3CaT>4@n#e4l9BZP|mWl@)Q@(fv`~E$K^48xI=id zundE=k#bK=n=C~uj>*6*BEn!-op0RtJ*vK;9#|wJfo;q;&w}sZgnWUX!)1?kWY)>F z;9gmRw!v25=@go<4kXH9M5T!g(#ki5;EV+)XrZ7l$@eYnmE=it-%h#FacNJ=A?D% zCwBA9()%(jF*~*n^W(4ofb-w>+zBhn&faSi5v;WsW2RJ9c4Y5MjL5{MoMhz0s!bau z#^9?0*a3V!pTu+nF+vT`Y^C5f63a4T*M-Y$JC}8Hq=7!$oSX{5W)Ln7ebUC;)!HF? zEI*@$z3ozaMv0`sH%BLl`wX0razN(agsmxA!DKIR#oPgIA0B2fU)Tq$V_(lb1-;i! zP*!j%<=nAY5+{3KT4euCVCPyA2`!Q&7OfP=f}1(D%YAA(eE8JU>0S2#INKH^IBWrK zY4w=Qq&I7%l{UKzVzkadIy-lalbAlXY>s(k{Sc>|{2V_2g?EDO z0sIV4YeJM4fOXlLE)zkX=Hi@dnxs}^ykMxz6V{iu@U!Sa zewjRsSrbwlh#4-8!yHT3aYY{3Whvw@;qkvzNWFn*(duVRlnOe88aWc_*Mc;y?on6*ZR5`S-k))0?=~!8qA0V`&Y^m-58?e$} zK$y$%$G`nH{`||IW6jH+la3*iaM3HT=@ux1edpWgTPd`Xp`Nlze8l#*4jN14+$(8!|>3f44zCC4M*W4Ae?)dxW zH>X9D-SbXn>qG7DZF}s8l;&L3jCZR{Gr4KtcCAU{_eor3$Hvc2(y?bz=-qus)|4@{ zAKlLTJBE4}oXVz2`?fsvPn`tUVUk%9&2WH`Gzgzeawi%A@)(5%{F-E8`mFK=!D%o8 zVGKp9aJ4uMvb*WE<4jb!IPi?3kU*juCzeJzoXZk&4s0YOIJ)vWt-^7Qs@}9UUeNd= zR7tyTJel4cb)@*ff$hKtb!5*H0~OCF(n72=L&ym(OAwPKe)Cbl{ILLyWF1=4VuU-C z(pVA#Tcv;{K@+1$5FlVBF{kaQ5Edhb$H&-Wi^StT2{P>>HPM6U8YM^*2`(j+14jbX zv}P=5%$El9v&FNHz@^u>SB24;BFD72X@P}g`rwA7>&tXwHHAY-w=4tJbTArq3X;+e zAPu5qN7FgmlcUlkoo1rv*U1b+WwN(770#Jyc^{_(x^Wq~b4R8ZEkFplzHWRkWaE90 zaPAY&!aF5*7^7T=$bIhE69@5TP96-YLhwC5#Y43P=AV5rwV!<+hXOF@#V#kAErK89)crYn?hyG0#2HlF*8J|RFybi9tOpSP1{khCk!%%+WvG^0l2s>X)OuetTxH}mzozlJS3qFh34Rh@8U zj(kB#3MqG!^9Zh|Vu`Riy(B?UN~1($V~4MNNEEJasFkZ}=DKR zdPt~kD#Lb5*SJqz0}3bzGse*|jR|!Oh!PZmhVCYG zu6WYkd#!cDkuhl!KUHz4}Fklz589@ z9LBgD5^?5fa+sEU4Cs8jF)25%xrO>*!q#GgTTAQNTCOrRnDSe78=F=(xOYy<dL!rA41vc5EGU;^~Et)d}5rU3s_*L2;)KOwS=jp zMyQKKqOi`iQgH2#Y=uxz5iStO5}XrMpt0IwW5Z}#CA0?OAtZMEpO;<4zh3hx4m`tvF z1caRPoOAiee|&`6XpN{GU{Y|YMYPJpoE|=u08E_0AjHElw{O0K1Lqzziv-v1y$_q2 z?RiEfzxhr4NK@g8GR*8TpIyZuhq{_$E4dg9;CdjIhacQCf@&C|B^ zFZ^eUeZ!B)^FL4-T5=>hIvJDvti5f+pWe!%gP%^JZ=?&s%pt-yFomr`n{_vR1DpME z+Og*y{08(+AU4C|(Yqc)JCTPnhWh92Pigoy)V6%Ay>0BCx3Fo=a~N56OwVB73%2b( zFD8c{oC3Dw|BnNsz}EcdTCR42O$o!*fO%D6WJ;Jj3BwaGzY3L2j=@P73Mzi@2kDpy zf+nDBBgzvnGy(I*x<2Eus0s_F6!Yr^=2au+PQb{x|8(y=G6q2sa%au(r$)jb(*`#s zDYrB&Hzk%oq?TKo4eo4AFg0_w_oOF#%oOp#`p63N$XLuq7BR{QV-%@%05hCzn=a}h zEGN#n-b3%nZVClBY#TtO^N}`yEgeGReDqrJAD;y71e2we#a3T!cbn|vreQ&L`Y`DDxecIhwl~>yA za38lF`$OnhyLmRLwL^Q1nf5*|=YVv1F{10TS)Ee+dHqJ-{`t!|xcxD)7vgy_8XbjkZBuX=);5 zskGog(LG<}u9I?|+x3KaJjH*@!jjr{DWJNfhx zKjrg>|CB2Z|0%aTZ6jNsk`g_&LVS2kV}6r~(m3frMluL)Nr_~?0t8Ke4^E~rfO!we z+bx14Gfixk^5=Wt>Ysd_$x@U1&1NQ~Mnf_t(i&CQBvXFWS!Z$1^WA4H56cO$4vCd7 zfOXbUBw~I%z);vv!9|pkfFNSt(jEq|Ia&1o$S96fEnt!q9>vo<#Cn9}Os ze2MDT%Xa&%Y`XpL*u3r!v)VUpR=MMsmu$0r{=v_G#Ge0wiVh+c9P+vuYu{>azy5=m zESbqFxlL&PjhElZ=!QRY-z|T$&9N68__Up#yEL$B`WU)ZzEqe)7pZ9Bl=;R*$(G7dZ9U_=6LzU9yD4lbO|7jxg4vav7a z8#n)$rqVREX>+^y&H{*>B6iyEtu(##R!{b{yB}zcF%60mutdQVhvurxQVco=4sC^`0;>s$&?Gxp z3wlk<Ff~GAhR z>)2MxDKw!+yJ=czf*rRMvXC$hP)S5KB{c4tA{d9_Ce1*zkHTFOl-F72-v|3o!B7T$ zbqECl320bL%~>iID#q}>cl;;DCgG`{cV=ch5~MSz2J_0YwIfLxQkD96^#~ zG#ilA6?d#2mD@mM;2c$m5jXC3N64g^tvQ<$WQj$@cRpZn5`&A^aS z^kpeU5#k(SLFjNrMQO%@FlN{aKfC5SPJ7eEsA4?Lj_*!QXBf|whQ%qv9ktb5{>3lx z*{@wj|KTO3hEv7|8`yqBVPQ-#7?Q?gG^Oe1Lczb+pf7}2(~uey=@uv#A*_QfB3YE5 zvxQ;N;;6Soae`7CIqnn)3d21AuV2iMK6NdXW|3kzkI8yM!DtF$NFWM@!?~OdSrj^l zju#+AQ_x^ZQC1O2%y6tRPLVY;w17kv3JVVA8<$@`}X(vvjI?8Q7xrD~}7p85m zjJ$!l`>lFJpZ|f%;zOUs#@nAmTJLaKO>THMxBaBd0Z)7_aj~+)>2v_M^XDJF6Wnw* z)cJ>eo>KpUot}5QW9TRE=CG&joiQ}B>~U;d^8XmW=WXq6CixjRefI_I|HMx+vi!JN zp{O*fqpbhS59u2@nBgTy_DuF!$iAE4a-E@W^WbDvU@DGLqVmL5C&8s04A(S6X^}#z zDQ3a+Wm-U6kLMd&K^_!*OD8GjjHQSW7(jw{a?4lJdeb-4in;X&MZmnN5M^5xxf*dk z!6jIlfiGw>3R0_c&M;pkrt7xI?3pCI} z-nzE4q{KUoE$CQ<=r zkb%K$3tSW{e}5!x%;G#=0^YLu3>Hhrxh2l=A_dDASZE z(xB0MkUe?ijCuD6f>UoRzE;B65g^3CyfkC!#KbQALU~9h*9wL*%TwkrB*OQ1wz>l< z)2?q1*h~5U@4a9Z7k9w|z4=Qw^8WKzJqG!n-3(5y8BZ&J7Y`q2X}H~_+^j*lTTt85 z6&yihtmoHCt>>uCEZlV`eD$lpU&=sC1gm>ytqq=|G7>Il(lbHj%)TLz|e;gcO<~K{a4v-8jKkC{6WK+7xll zQ_ke2uX+jnNtx#!csz?NjHFP`Aa*`?B?y@@i1J;~VP+W+m&57VQMmEYAR*(M#uX5QWh*YhS$9M`WETs}g*vz)OIu#CWQ_2!D;w)dg z^irOE`twn%j$$T&v9T$k3aQaxj7e5E|IAmvdnI4}`PZ1JG)SJ7(Hxl~o*y$Xw@gzt zsktUesfm;XRZvjUDD6<FzsJr1oBh z25*PLGa!bio&7v6|IFuzngN4xiQ!_Asb-xBMWHl6QcH;?#Hj%1+K~kWDCod*2|LJ~ zADjIkKBLD`=-I+%{%zB zY`pEScgrn|9Pmjt-SNM0b~S(b@#EOE^it*@_-y)yml4EeY?jcR+RB!7zvbRrE}3@i z03!1^;KYB(;VvI}=YP;zt59M8lP=}YKlskH?PK@6l^ehQ0~Q|s28I_OOVL zwUW~UxB^1SnwKYsgT=<7GJ_{gp?r1s1h5>XuV#Mg1- zH-AN1|8^&?kln!TKYcv+-trpe9q>X1<{wN{8XyRZSZ6Rvo%+N^s-tVzvgSujtbZ2( z2R-F?J?CPUvTr81+_ol~12Uy)1R9OUfsKd zZEjS_o3LB1k-x4qaFh~_MpJ2aIJmUco;0;JQ8R^ZAztL?aMID7Ry>G8DxH?)KzsP#Lzh{G^2qx1IUahQ@mM$|8I6s2RwB z^Tu1IR1Uz`fzGgy^Tg%krm16&R_z3rcg`GQ9ihb3Q$;CJ6apc2jEe8zmf#AVqTm9G6R;rK)eXu{|LlO!#cj??qnO6@p|$S_Hb7B# z(sA-gCzRULc1azsyz&Z8Sa~A%-gGaEhZpyfTaURxz$}c&oz0&L4Hi6zt(>bn|ELKq$m5la~T9=Sw;}-`#>Qbdd-fVpz-1CXY99>WA?(T zG6?kJ`$NKM#_?WI$8HD62?C-Ydn-=sQ4HLAGh>Gx(cV6G_|Xj9d{Za3EqnWlT7xs( z<#>{vs4r(6qNS~}EDD_mz}KyXtH1hPOxn-8-uX$gY$@W7MavLk2QbYlL@A=ju;meo zLr@Q^NB~)sdCqeTt}BVAL?Je}J&ndR49!{wO&|rGMilBr)VQL)ev)isMxQNG+FE4d z&N?qY{T00G_2+X$oYVtOiJO!Hj z8M{O!mz}*R52`LGbYGdLzU0Mx*nk%UZ-p(~n&k8d(M|0ghU5R4y)ZG<3wc}7!EdIDjbIKGRY{p>;{ z@b?g}oX?9cJfHvm=vUd8OwiX?Ak8cjX`QeUGEgYv5)b`M&L8gb7=jWZ5{-3`m>j7o zbCh(53(FuG8J!RVVwEi9L{>^u0 zb&O@leV4d0!p7Tn&n+x}+}YH&{;>^PH@fysjIMpttiQkilfF)QaM2_7{0~Kj7azg0 zjiKY-%j#clXAJ#%?~I|S z*vH||{0z7L;O|MQ*G>CdZOg~FyNjc%C%e%Hd065D^1$K;E#&A{7(m+PQ?gB{&*hfq z_7YjLi-6#+y1HCf3aj13@4BI$BoFdS8_H^%u%z=?%9rkZq=FH)ByNIcvMfbXx)a?bR60#If&nZ}sG%N(NZv zVQpLN0^Jnat!=Fe=_m;)Co^F(g+PQX0Xr2j5X1x-J}3ZS1GE7Wc)_txXSfzJn1+OC zG=e4FiH+asB(wQ-1ZXf|Gsihco<=n=yHJvD2adF0y^>*5CFPrc{4t3(Bs#;2?=TiT zq)K2dp$N+gWd0Z}a6x%xH=hZh-MIMjnFe5^z~XUx40 ztHlgrb+?oG;{+uk;sOvwBz16`s56C-(c&U-IrprxnxH|uh^lA$JqWkMHu({!Ita5=V3O9%N})4(u3>9i-$Z-62`v1P5nC^ZeHc#o>%`~ir-r5CK?Og2abpA;(6^gbTZeaWu#L)=*YI&QM!I|c}dr{ z`Y592qhtsrPr+n;XyyTBgtAYfB#td>Z)NbLBWVPGz*N??kvVKTe$l!hO(-!oGo-a9 zSu?{;*jA+(VJ)Q4D3aXWL~T$ERQo7b3!M3s7jyQh=d!f2oB~Ax6d@XCLBg1e{`VWX z>fP@mMdPf&08SL*EINN}_2x!(b3nOlti6Kd$3RtB%CDgjC?>(OdcAcdp>$U;h~4@&JZ} z=E^2Ar0HKWhj5Og5jAM)X8w>s^(zXc0;$O`l97_O(Ih(YD?xOGGgXXB7$5p?`Y%3= z@lW05Uua<^jLiWO90{WCpb+25HD5W816I%(BoRFOyfgUbr>|z?#1sQbiBeFZYEo=6 zMHI!9l|r2Jd|Nm&@$oafi$hT~N?)Zu91B+um$s>t4 z^Eof!l2Ykx?j`;>l4u2M#*4(h$9=8q(9D3>{-17Y<^|8-B zfcx)b<%!>B{>l>`(dU1tvgEL562#ZD`WI*6>^%?o`-Op5a`>t5rBE4qU}I?F;T=Qe zfrT9P+|Tmo|6az_rVsQSguNj9mgnJiY=i1txwg8aWQHl%f<oRGSO`ApX=WXAaIWp{oATEB!#`*`^)UQYeaDvMrpfX9;REi#WC9o)?qIc?VDMDVU-r?qWK zn_H|-`M{2Zh$~IrYNN_=8>6D z%&bM{5I!yXnLTN*q}C8C?QIY7UFX*AfG2Y=${W6LBbQ#V$``*pn%A72+}glRH!^l) z7hj2Wh@PGslf8PG4m$&{>H5nxC!KG)Vh`IUZM#7-to6U&wHmJd_v?A<+dhQ21q5ml zT0H?{=aNxI%OHU&BNQ<DO2}XVay>@j?&g5XFcU5yyVm~I5a+lGJTXNw>c^OoN!2vxAuZ_ zU&<#hy_7ADg#JLtGDp_MrzH4}r%<4gS;`8kP0(pdr4(Zjs?~(aN!YY8;m$kObI<{g z=b{VGVZp+q^Y8zoxu&ibF1XI_9^Q z+rRm9bhbZ%YoclLk8e7am%QWvl|X90SPni2$o%z7kFazwj)UpY}wb+zO$Y zro=0jbH-~g;#cqgFq76&!jdULY^VEA1EC0ka||dLsMWah*Z;$Q&wma?{>exlH`ymZ zXT;C@KB*8iL#Qc&G$3g-kOHWp#yX!Gx7tAn8J2J;rg>k&HUD`v|NFv!Afm8mV1?Ti z|CX6uVkez9xy!Hbf9yH*4X)s>-@cu!@%>%yU+BMpl}~sRg9{JcBab^S4RF+RKFZxU zoWzFPUb)NtOT(|>z$gA6Dnm;i73Y7bvS9xw(m(I_+e5~?4NkOo3Zwu6oB3@o6W zhnWGLCjlIzW1}oxuxv(`ieU4eG;N)4tM9txb}Kf{bIG*M)s+~?GD{FDUtOd8SF_Al zEIFHZ(f8z$9ZcKKNcQ#Jz5~$h0NC4!#yRKV1YD;p%fkx@VG89clw6E+#v^)*v|A3v zwdrg<&NX>g2)@%qZRa|(yxui&8A=L>^T9gb_@4q6Cyor86AS0oVLq;v2&F{RKMH*n zsMV+6zZL}nG*eJT7~jE?;t6Q|!WM3473hTp9~<*V0aXzu3bE&z${k~I3Pi9tN{OPV z6!DZ|(L~@YHSzEx4vU0>70vJzG}C}&)KOF=tjRpSy8|I2G_7%L^Weh+>qE3%K?#M! zT2HKePoL)Gw_M0rAz(DpId?%`@F0SROEDa=DdYNgew;Z?OV~7s^A+gIb##+a9nB@q zLvMsAlnUAoU2lTSbQrf|Eg2ZxZy~??`nOng@=6%$Ww#zn>xvQbp5~6N+0I9~HlKGV z>?sZ<%1=U<&$5OflxeUZ6LuBW&B4+~)6LBXS*wJ_mzP+C)cT-GW>S>M4t0M|b}ULs zi#p|z#Shh!z2cI}w6W4jZpmIaf7I#>ar6VuP2Pv4ENlda@$`< zdD*!aa?**<#iz65yni}HYn{jB1yOqYPUk~@x6;u^fo9PmhcK=b<3K&sG}Dw!9L9lG zzL>5W!Vb^A=E3Bmh-X~#EXE5p(gBM)u0Xi3$l%Zbbv;R4Rmrr4q9Bz3702kJ z#)`$b%*U>*CCfujItaizlm%r3lUR}|3sVgsMB5n29C~Vut?PbC{ocD74pwsLNeemi zxC5z#eZ1hbC$ZlW2y&Ev^rd|FBbPB| z66T0z$|UqF)vg=VK%^C_4Uh0)p-<7=+8~HS8pRs#__u%I)fZk!)Gp=d#@3OCN-^24 zqBbZrwHeo+&FV1o_CJB)W#3}+y5BQ)-wjM{{w-6$1+n zn#LVDvg~n;oWFZ*<{&C^zzMHl@!`*7)0*EfasO|rjsBXf`6q(-VB*SQ^o^`ycBBpQL=+>-k7MrAFEO?C62{iw!o-FfX;$wc zt^Ea)-c1l6Ojz2V^57Aa=N`o1!ow-enYY*8m)0RCo&U)9F(3X8F1PFL?TT6FrdLb2 z>Be~{z`yHJFSJ_ew{>!M)V;Mmo5r;&cJr*8o54TnaCsoabvj&Jr-s4J2&TEN?@Df= zwUQI!Iw~g!{lsfQ!KFYesokr=e{==aaJ~!BC~|CVpLE*MOBSHN9_Ac%9Yf!W573QN z;jfN;9{o+wN~4s*8arc*d;SlLBg0{^+n_E<*L({LF{eK987yCPEP=#`3-T&Khf)S1 z<*F}Vm8Z<|fWGTopLK?)W_ZD|&n4B4MBAKaFHZ+6p@|_?@V%RV#AqSeMrpL9Wa=!h z^r6{{P$E6q=Oj}?BhV;(LQ7np;K~zTk@Dj&$~1GDtul%La%fI0q+%&F9CO1GRSHDS z5M$|LP~4s+Kf6#$Hxizcu0(V`KPFJ|HAxAvLS96mgVE7-~a1wTqsn6;kzP>4ScouE$*4liBIznf~BjJ-2l1 z^}U*Z?W(by2~+rwU$~L?UvTuJeEpFfY5=#r7twBqlD&U<`KeEO**Mo<(cjr@EU3JI z%LzfQGMX7^1#8#9)nEBJZ++`WLFS`a0`75Sb^w?MM2N&7CEropDiw6fSIoMcz{gq0 z4Ak~x;)*}AMrh3{VX z7Y;b-Xtp*|{<H4m;w>T=vm7AXIWgt`&e$v@0UIU&QW6Zq0nRxi9QvK8iUjSMbt{ zF5;K(eIJ`lMp1)_9bplwk)eb|FnTy_S)8$DI3XE;cyWlGW0@>YVkPxN zWFbVQ7*z_$bcPW}Dj7I~C~%o2bOI^_t+1&jZ6v5BxH?D*K^72CMMP5(*=Q3tk&uVv7>+_3 zCZ%8*yhcGX|U4ygM-Og;WkA!GtNnK!nT}ZrgMR$B!Jh%XRU=+tYqQy8iNk8GmuJK8p?_ z=C61H^H)4!pV1t}z7dulaT*+P+GFzk5B>UwDfXSrf&)+9?%$6D-q>Z2=+@%y9^Ba6 zSH_SiMc>@z^vzukM?AM@gzo`)coSS*&cE$;DrOK3_b*s6EjqBn1{#My_PN+A>COxhuABZ3vb zZEmKBj34_ZGoU(LS*=ELZ{9l+QlCgw+g1olu4n35LL3E19O5z%W3bARIZGi_xXcn* zo2L(?-KZ|@P8dy}6N)u3A{f= ziU$!C_1#oini55kcc`4}V$gbUF$$UPmrR4?Z0o-n`cU$=G}A%8?aAIYSDeTC-*D%B zG)5Et@Qa%`^hrk$E$wBu9%XNO`uxu4oJNR@BMcoHn=``tI9RKA$!Z@9w9W^@lqj57 zj5g?234LP9d%TatfFn-|u%K%5{6lD#nVY%+|U-QEAU&-rV^E?*qcK{IQ{OvP4dviX_ zJxojMqziRi2@vz-7oWj(?|lz_0_6bK4=QT?!rBqb{Rj*pn;04jTXncQ zn}qjUMy84sHW!G~0p4`sKl6+eo`pjZ(?`^bsYw@)#*Ut!f}>75ndjYb7C-pJHLR^| zrcx{rSqN-OElY@^fTGW2wRygk2u8p;g>#B#DWlq7CNbH-(f>-5u9l4 z%PtQaK8$2Tf~go(rolK!eFwW!;%!jCX@?6P;c$eS)U3Y$E{+{JE{|#MMhCMM&~*we z+goSL^rR=VwZ|_0!i~J|f}`?~XwN_D$&O{8;NUX&4y4v#7V$d1MM>B)ttw4LnEIVO zKDk0U)oxqwuR~;1n_Fl{QVlJ&$dYPER2N@Q4l&c|y4ul%^M5NP{u5Y1JGI@;FPUMW zN}J=wwzn5k#ZV1O0W_L(pEZ#t?@g%L?UQg6q+= zz>8qB3<&}hP;-vSFxuh%^DXo@LMlz6lK7Np3)akb863V!RIn~X=SzX>3h24^{It`; zex0;#SI3D~Y}~S$Y@tOtODLY^H;6;(^%{{D5S>GAb~(&lQzmdi>@=A%h-mMqh{KA* zWCjTo%J~Wo8wG9Or{f3Zzt|o==VRR<(yjG;Mwr0O5^X_ph%oRDQf4x;G(%i~7I_Hv zzngaKt}AOplC=&V8Yrd^#%&aecH+4w563zTVHjY=(6DvRI_GRc74KGGuqO|{?5OKE zz;jm?tfizZh0GGsDg+S^sFWAE8!TC)nOp%N-DT_#% z+hHbf(~GEjNv_>zNA;v9kBG6lmz8`wRGY(vzxRSwT-;SPy=^FW7J=(Wk=PlKuwzFY z$-qrFbzwX2@hjL?7X&kWK&LP%+`kDv_r>pk9p)YHxD02Ppo0_8GD4R2;V2UZ16Ug% zL|7N$T#R!n*Jfd!+-gxR*xb>}y!|RooJpWzp)jAdKfjYX{Y#003iWD6wz0|NhH)x( zkZ65Ox~9RIFFcd;{_1=VtQj)!GwCKFa!_)p zK1CF1(1xbXAo6~-&uQg1nUANy3ZW5P(^8*mVros44Nwh5!G-jV706cCU`tA+QDks^ zg%yp3$cB_?tB|=8li%Aw|M7D{8=TXa)S?50H3k*zBFvcEOwT;~bgudA6_o2$w%94= z#ud`0^$D&Z@-VKp<0TnEy-NK}*i%Ig_CafILB|3i-HTL=>1TmYiloW}y7G-N4H zebE_w?E@cXbDFUTXgEiLBNj#AEZRX4MTDa$!U7$#$O+uB8jd==^XB#eyP`c$acDHJ zJMZ;;;-?>n)Zh|>i5)iZ(4wMzp{5QLSzwVsV-$$)rk_x>QfIOou5P==HE`0)>UoD~ zPv`gKfoMbZfSOu*^4OHUpWxaKly^d&w3(ssTb}|DIym!)Qz(pw2m#UtAhV!6@L3^M z^rgWr6I`1r883S0Yp8`#5A&ns25h@eW+w?QrjEhIY$r#D6O_d zEO|hx&*h1#4kUw>>DsR4Qds9GWH7fOYzYih!rqI07^R!DXX(i+sNFTnuWtDTr=0k7 zs7ynH+>vY7lSj(CrjtnSAPWj1u%gn>ATR{n5ER&IAVa*}mO}uifg{zHdLdw{B-B!v zKU{(I7XI?v_fU50>0=|Y%q7#q82eb)XSLmq$fRjI2Jz(PNQbH7Z-owM*j7@ zRfMXg3v=j(M>927uJ?UY&310kTAztjd-~=(9T07}8?Bj{UAyBhxa!K^@y<&=gLQL3 zhH&l_gh7m%gLM^z3XX^@E8#)g0vxf2Cd}{Gzf+rAs(82&l3=m?Vf1Exdm!925B_I`!45&K1le2& z76;gY2vasp=}9PAh!m+A*vU! z<3e0hWE*RgR)@^LbCBUJF;5scoYMUv3u{B9R=D^64Qz=FU;FS^c*VsRL(!+`bmaNr zl@4Z=1a}JJgGM;>wdeD_k9?WC#_mJ)EksiS2c|ZOjvR4N#96^P@rAI;v8AwqiJ`3= zbk0HCIk=h|*8Gw5*99Jb;^_zj+uGtzKo{V^L%BDzEXWK=6wydBGBlxp$vYMmj*`}l zX9@G8BKu_-KmGETdGb;31m`>&6Aynut?h!%4$!>f^b7dn_dd_q`YlAu3M5I5pf4g- zrX4#8f)E`-K^4IYl0s%Jfl}=(?`~P%UFQhf^rC8+QP9hVdEW)AygIbD&6(Pho^;E@*TL0|C*Kb*@B0wMHVAMLx)U;((XEI6Jv@+_Yz!>1@*zMh>=_K z!HRTYn|C1F+T(o25v9%b*6AMf+oh8rYZ>TRj5X80*GgFBMvWHVZs}}mEPKOA+RkGa znW=b%IP+Z2d4u*L!0z_sF*Z9KFj6kDDNTsNki=yV@ECreDapyLcNuh^U9FqzGWCr( zjB;ekk!e3StT>`fQzRfx9an$glbrna*Rt;bVm!7D_SOJPRl=YCbSuZ*ej?>V%D7U_ zmAUVr^7mv97|R7bOGgukjU#dpQu^yv0 zk;pJrbqLNjacvI3-3$@alb-aXC$kZ?WB*x{NB<6#Y+`@wI$oB#O6^#tl76iaaKaac0~G6zQ=LJ=t7 z2yr4T?NA!1d~TaEJy+>16xCA0FyYXh#4njg0!08epjO`5obU5uQ~S_ zFJk{_31tS*X-r^40u{{Q)0%yJs5Q7md9n&&lIwr*4X(cWGb~(O=9Fh2%@4o!bLttS zw&k0Gb}dHB9Kub!zAlMS$PE8})m5DJPj6vw;v%%hQh0T_DGNK-X zsyfJ~xTiWwdglhhBj(ZRY?{Tw(1MW9Au67B#&h`chdx7JJcsqQtqk>-F)5^pp_(>` zQ53=$EYu4b^jyRtetq~FDAbJD9ur*xqvCBB-Z(l{<(q%ZM_Jsy6rh{r~4qbRK z=fCJYKK8v2lZ>ZOf;367alU_`QE5PE0}9QU^UpY+r>}Y%0jdqdzgvCI+@$@*7iGrg zxQDm@D3&x$3B%n`oVy(kT?ZJGdVap1^kfG<`@lB$%WGxsp=Z)&r zfeB))(HJbE@&LS(He{g7gZaKp-p%R~Y`(kL9cYW4C=Fw zYr#}mHG>0eJ3;*Jz43>5OTcI}izm)cWcSE#k4xoo^p@5+K&%nuY*_#t^t>KG6cK6|6ui z4rO)TCDwIaT|vG!d}wb7^kkN{w2)I|YfUptb7oSR#kU@;GJ7DA9>2d)j%MJPiX2n1 zBhmgdTy@MDuMk{&^2_i44^KbjX>Ep*M=GFPtFpKU z<2G{+cQmbPttqr1P;+}u*8YtqR<%h_B;?JSYtbW(w>Nu zARwJin6;}NU%C1R7&ni9ddI8L+G4ZklF*0HkE0(L09ODh;#}x8m$Z;S6oU3N4=n=8 zJfVdcgallIPjqEC>L4j16NniHCJ3S`uohU$n_qu6M?I;;p@*$t>Ck)>mO|dErNh8; zg-t_Plxja=VuXb9cwVIHTNF{S#mL;a9l& z2VbFHYQm7vJUpSiqQqc-KaH?KGj5Wq6k>?`1ENr2v|wC{jZh1nd2YB7Z@rf7<6 znVKYVjzEM$7^AXD77mW!QrqQ8*{*lJ1FN`;bKV$-j)cm*LC$*B%lYodzD!XxW46YC zl!#SC-KLD$CVff~Dp=n)$@qQ{u9#xW+zqI>NfcWK1}zJ+jBCF7K^nCtCqMa2gu$)= z8=i+NgJ+z34&Q#?dl+>Y13Dzh99Co+7EI)X3YwaQK@|O>SbOVj9NIU48kxI~#D%)X z7D9^Q1A1DE|(gjEM!>E7y zt>LbVv#6`vtF262mw`9a2Aq+w6DPeQ-$%F)IdhOsPw>o8{n7%B$e{<-jtv7PKG~_J z1;*SoC#?JQo!QXcMsB;ds`j@eC#SXI$ehiIeH{*y0|;6ifrkxs4ka)rsWOyJa)8@J z=Eg}GWRfCP7g1L+-E3YyVckjNN&DK}PG@hoWFjPa5Hg(2gYU_sZ+f)~1)PUMNq0JC zWX5dhz;!zCnB~(zbEl6Zqaag`TIi@nyBeMO*cL}MRMbMn-~Hb=G8H+Jz;}8qnh3b& zpFc`X6l8`<74{pC=$^?g4^oJWC@0Vi6q8ZQ-EoMuY%Ms(W2lAD5YG`5x^2PqdTQi> z%Xh!`U6vnoAa~sQ7mi^N3$ndN2>2nbBcw=$duTgVCsV4V7yro@b`Ftw;_n z?b@8GiJr^i>#rFf4*=d@4sLb#u{HQ^cXUwwbCHMz^%73cGzK^Kcof~>P}dAH$0Orm(9mF zoKGy?vlg!U(tq)ecYcP{&O_7^gyTum5wHV58ApiHbFePXd9;Gu3(_+Vm|LRG&?(JG zz@?y?C}|>+U~L^yqab4_8N;y!%NR+rmH&6iMZD;YV>s{tPxhE04mGoQ0P-kL{r$n`d3&2iV&1S5;9N@(F$>bu_;&xB|y3S zJ~B_67G#>{Sb{c!ZbICMQImoj&5$ue-F+Q+8Ev3<*kZDL|L&^nJz48)vv&KZ35 z->zb;nXsWbPNh=zsa9i1rOs5Cp)3ENz4s22?5fVae{1i3&Z*R4dU8}6jRGhkvgAv~ zd?sALAR#1yFuoXL%!dgUAVf}#1RNvvA)$oUIf?jeb(9Cs|w`-_+JanWCaTloV54I>U~higY^4C_hJ6nagO88m=Sh8d$6&zc~B; zjxN>tZ7kZ&R21K6B$QbaHyGLX>Xm;Hzqs#Z?*q=7d9VA>D})Oo1U%jH@x2CjAslN1 zSnd3cc4;!9=xmwIXG<3L2vo@t^6rTAKt>`KR1h%;9wRwzog)|}^9q%S+y;mO!Q`lT ztP?V?s8wHn`26@C>|zz$L!D683EHwFvPb}gl5f4IP1H>~ z2>YcFO|50BoNsZ-DIK~i#eJMpWnf);SW`}(PWUb*=q&pU5VV`t)lw@lXh5Y{=e1j< z&>KfBS0uKc@urt@+h~R{rjHYSvFWK~c;;#i#*O1!MhUi{=FN?=D-wRW%3<0}S=_Ty`@>UX^B9RR1Z()RG%wqAZS)_oY-1NFWVW${me&_$Z^Y8zC zfF5-HXX#Lh4S2i1>phBQXY>5^{ zP>3m$Tye4JsRwdIv!Ym00zq?-fRQFJM+j2{+K5b{x`)`_LCAlD*gahGrk8N;b05dL z4G>2KF+%79)LKT5+TA~8(KxM0_Y|69J`{W329-*2EXq*NP%@hMA}jAFFS#^bBJ`K>SG^_N`9+6^ZaJkqxOuO8xv zM?U8{eDU%RFx1XiX(FaWMqP@LY(xgGoH1L1l^B#Wxna_k%Pl1m9xE|y+ z-c4)PFvC3Ul&3TNvZZ86;OATZgMa+?KXUF<{*V*aZ)Dxr1~w$e(4a;rl+;V?wZ};C z+hAIYos16wE9rvQL-$w%yKkB?hAhhthx5KTmU6F9z#G`T)$W;0$fBiA(oJ$LbToRk zApKdfzSd{iA6&T)!WrpQ>h}Dp#hjP@&N-Q1=}#X-s(qSVi{ApB52P&EEV{`n`kkxN zd6N0(&~?NHYK=z_+OoIvQx;u;HMHp^UIQinefek3Tk5TT;A;V3nH7mG)0fcd1zbhP zl@B?=Ir79Xn`Job`sQCT##}Bm^EOQn-WqbN5JR(N*ggU)#&c#hrD9@IQf6w(wjocj z85xR{C1k$cB%0aE>M+f+Y=U;w;H=dr@TEH^0NbT1jJ0rzyF0F=o>D2Cf1a*AoeHK}CnzgL$yU!`mV#l^$^Q~|HUv9hYM?Cxy>lj&LXbdUV=2&aV zQepR=7P}`>c25eoPs}kF!DI$?Q^ddyu-cK5@F)qPnEUteYv4 zT27FvA*x5@`OJn!QxOXF%ijLq9*b?;90e%OeoGf&>%H2utW1p7WyMHz%B;1KWyWka~SW! zu+#-zFZ-wz=^VvSbaO21loBm;hv)+=4h1iezJ{tmue(IF(=!md4nc>}A#~r@zTY|E zy$EGh@PL`{z7;b`A8?iebLCi_rPtx76buC!g~)xSWvK1BbH=COy`ymH+a^wp2n8?N-_hP1($k0bLA&{ z1fO?7cS}{fy=Z%{2!k#0{t4F{%qRB;X6%c8u#e+DHlvH-!_ z?lT;?AKq+lJai2sS8tYE_mE z$JpHC@;Oq#t8~dX26h())fm)lja$C-3yyiw1LiL+=8y;9J$pGKWLY+#C|rnV9&nEd zgi?oVud40+-tb}4_p537crM1kYG+5a~dJ>n=aiL5JeN26mXX zzfRemnJ=w$pWYC(jLcy4XE!i;+R6QkOZQV^I{hv4Teoz9APCAhH5$Rv?^Bo{yl6A=HZhQYB~wRa?-(t3~M05xOEvR*>13TUiFQP|XXyDcY#E z2+V@cftf_iBsLmHWH-nTMDE}XZ+aOQT<}=dt?8rQ?KY|xg&T{z47y-EEcB@b)Xsa! z2ak0z_>`@8{3l=f@+aAR+rP7JLzPXN>I^jk)w;zxrJV+5rad!L%H~^U*}e;&`Ro^S z;o~pn4^DYBP045zFbc62i7qL-w>sLqSx_oQh|-*^ue_2My!p-DXV&eAmifgjiz#%? zlbKQwv3OGUWK-OkZ{s7^eUuMh_d%-1B*;>sxv7O)8k4N3VMhb4N|W4XFjR~ITd}yr zqSg~Mkc;9?=%aTjaVz5pc_3stAr<^g@t92>;hGki4lrS8?wF-9ImGamVWPQ+=REm5 zUhvrS+5V%gT=C+!aB_Po>sSgtbf2Xd=?2cyk3V%VFX`MP!C6Pe#@MVyBd#)e;~k71 zySArke^8Rd3u1CpT<2vUc^_|i(JQFeV&G2Bec=l@{V9)y<44MbVv5&*wN=i3#8Zi_ z^0lvilEh`KSWz$<4KJyo5^>{=Kj72Xe1LP#c{NCF{_!HmIj?#J-+as4X{x6V%xPfA zSR}aOT0@|2B3hx)>T2XoKKkx=^Vqk(shDr~vSdwv5 z!T$<;?-RodUUc{RjbqiuvstxK@3+Ul_l#z<=)in+Nt--ipc&X==8n|yuDX}YR3?nH z=w9ig5zGwaC|-(c`_u)DdLo62?{#5yTH3o*gZp!Mr4TS}IggDC;aHrz8!|u>IiuoOGE(5z)BQITAgB;jCH%(KO%-BMYs;^Vgc!Tl6e}DMnsqR^@ z@OQj$48v_aTeANEWoo_a{BU?6zrs=~HVkIk5fP4d4D5xzD-f#+y0j)??w6$`S2d4-x=(9RfYr$L|^D_(z@3*00=* z50*^ZSm!Ze4xwG3JA)#Ofemb6_X4}@qSN`yzN+cK{K8IizgVgI0EY#E#pD^MF>=EV z-TQGc8-$iDbGO_C*IxI1UVF)VadtK63amT~VHtT|M;Qi&5Qb4AAR?5BtS~TS%KCv6Uh+i;)?WNmM3^gcdtcCb;b4yMncABThSgIZKxc)_N)xhgX=MNoh6{wrx+j z={GyswDF-l^PFe1Zq2FSEr0&HH*)3M-b7O~qVg_i_puc;g$~e=Q3;N!hs*!!uXw?m z-c-^l?w!KwQk>6A+;!BU1731+GBU|&ld&bgohv_gC0Bj@YVs9<JW{_ge0IEPzmG`N(%mYX;MrMMlB{d(w0ZF08P-A66Fy?tpq#a37eZ# zcR@54VP;~U^zf%~;h(*PW9!FKqeeZiaMgEy#L?LrmQKdhlbA{zA)!cKnFTb93&*Z4 zr85egt4Nr{SQ<%{t(&(ndi2^p?&L#^)%pOvlb>=HfB0w5(6kJV`v!S*ifzIY&^cfEjRw0fBW9&c=%aQp_@^} zfOY)dv!2BlE_)9|=3xKQeM8R_a} zaR3c2FoaM@hECT5)3!~NgwjFMZel_tr#1w$HZOIun8&x)@uy@jPhhav-`C@$US6u_ zJ!ZhPC~fCRjiXZpM0umMY+!fn**c<#v{B7b zO!2=IF^WC7Oj#FaRK#PP&}w;d0)f2H;nIL}4r47z><~QGL>N_61s@DsR<38=g)iie zUv8tm(=%tQ?64K)Y=T$8sv+^EAn?Ejc5j=q2RSYeB}pKr)ZB7H?lYi|$GvO`DS}N( zA@G1h5e;SaYh2P5derpykoEK*!QjM@=K%@Dg5w9;Gl#|3KIs9QXl|Qg38S>Er_zbc z#UGK7-$1A7?z4PT8wZ z=f3Lz_M{L3i?d2dpj-56fIJj=CkJt?XBOoSimcUOzDKQC}>c`3TO^8%l12e!?(Wk zS#G}dhn(@?A(k(T8EOb|q;7h zh>VOJ&n(mYa_$B`_0?;*_M6u+SDhmpPKh5L<5osAM(Q*pm~k`sIACfKrsjy^3MNv# z&G6FKGNoWlk+^impqUohQB6-iilcs9ieDfT^ zXI%QqKVhVPI<^yIcphu{4^CqL?8T^IP^Iy`$V@Is&aoOzyb(K9*bzn#j1AN_k}mNkHYLkN_V zhFv1XQJ#DLE4b>DZ{;^PUe7VdEMdiph|!S>MrJtn*alzu>fd1_;UN!wCLu%wBAkhc zk2{eqb8R*N%dKU|<+OaDZWDs$MGU48i4f;0E2EgDn)9`*uH+Bi_-2UjolvT3@!UGZ z5*=2XaJQq4^N(`Y^P6RH_x+;>cwmQb7`^+V(|J{2!Od$wQFL()Y+wt`9kFL#`f>ea zOLLJj8$%&+8OW@X8)Yhi7ry4t$(<0KWtIshrnVQ3R)_iz@MV`*;V|wStTa+oeoTdg zb6@$#BypYKrSIKU zf-*W6Dgh#IX|&wI_>1$3ue$Wv0$zlcQ>GI|CG=?q^|%uguSDW$J4GT}YW6506cy$L z7GEadLPu0qu>wYmxuC$VN|ia&VCk^Owg9zgFnEcJgjy&#(xr5s2Qy({_sC{)z|S1Q zH->=hhw@DkR0_>I>C>L;6g(JAQ6xbhB?uma$B89q0BwWG5NRXi^un038njC*(x=te ziC71Am)3AuTE#_fdB zV9K9D(FrBwboI8FBM;Lkxmor`+vTg4}bPU3~y@C8fi1L zVFpGZIjTat+GegYhmSo}fe4~njIBgu;)}GKEm(h~L#j}!T14vbS-|Hx+ANqPZCj}1 z5pLFErooK2vEwOAcEQqJ(6|lGh#QPE^n6mI_?=p%3x@J@*3S0jJ(O zSFTbBt<&e$>yo>B83OjIt5liat~aoO9l8@Tuz~H@j>rU;==>A!Y^J*_$~kZT3ucp) znIt0=gu&znHd~JuX!{lo?~%E*x2bnn#mi2v10}dL6hLCr!OZ9olai5PFsfKnbim{x z(D2X*!fez5GEE8O_d3M-aLobkc)tO><^$>iU4Xmeu=UdmYL61TOATNS0##e6H#cv> zXs^parJxBk+S7-VlFq>pFshJ;qBBejiZm&|BLst}EyuQCwCo@=pm@|2x?WjH$(9;} z^ARRQAQllrD3iPfDV=PApo2YaV0Zh>+*g2bVA|Re_OXWCXZuZVbpaDK#k>iE)gm<# zI%_~@PK9FM5FC)A(n7RN0(e^F%n=c=G>dto5vpy3g7UD_k!-b?Pb$7xU34(J?vjl_ zY52mfF z$yi6qGJ+1FefkKU`lo$O*TK^|#IZeavIjP>L$>!k?{r@IiC)$8D?fHUe|^#E-B{s1 zzRc50@05Pkq6>*faAXR(LY~9c+u@pPe#RxQ`=3~|9ApgP3BU+h-T;OX8W?H_6_kW- zQ6n)Bm%gIVqptNRd3i3O_+30`3#AFPK$`?6fm!1CPJH$oY&6a#Z+tQ5JnL~By`k(Z zvz>jWABpJSSIVxQd6gQAh@Hz~>uu+6iY^W5a--!yn{R&WANYsQeuR@w7~???ZZJG* zNGe(~7g%=fo*>Va@m&cw{%VF}k2`~BzvvuRt~(xR6zA4q%`rl&l~NfV;+2=afvey4 z4(6oDD}}zIu#Q)5T@j}yVyI!b;WiP^ z>Tmr6&1E^+*`8fv6ExP=7#gk7j&s_s1+GZaNX=k}9faa8=O!=YEPznn6u}qGNgkYc zgt?5o1r#-GBv}pH24+0rmKMo2*f0glw!zs`F(==WQk{aac8W=bd?qji+^QL0{p6>3 z+*?jX;uwG%zWRNZCBsy(R3k?w1y~^no+2UVdugoV-00}qm%U}$p^-06bp5(hxcJo{csL;oZH_&Dn9qFfgKRi@Jx8s5IEVowJn8)Nxb8!jvpmaKr-llUYu-(78vv3_BK>)r(DX^$yBc%J)CMARZP|GLe{p6Faa& z8fzl(p68#=D?fhyytl%F`1Kyh;|}azW=Cd%s~ou62SNr_TCp%*g^5~VqLz0Z(Qd1? zfU89$vw_$c91w+)WjQFS#UwEYNN|BcL!cQeSw(1uKwCOI&w(7p#u3^Ih!@&M%NmMe zQP+-Bij-Y3(hU;22@oC3%lqKkyx*vx(-EbIah~X+ughLx42zOY^r{8hS(~cQi3M`! zO5jZhz1pB&r-+t8QQx&lsSg$?fcr>=;cS|E-UI?7o~BJmw5J7D%yYd<_j)jg?t9h= z>laHoEu2+JZgJiab$n_H`VkF6J%w5d&dXlor6+qV5Cx|~B#;HA3RK!{S~uT9 zvgufii^zdsGCUbA+Wcbjmt6PtYx(RquVs&$#4quL2MCp8>)5(uE}mn?%uq5)A`OR& z9M%~!n~|HeCm~>58P*yk7_1gQeJjV$D889vQiyVc$qdzIgPB`rX^&^rGsmhsVZ|0$ zyB!|bhBJ4hoN*^qGD9s0O$)no(8w~Ed9MA?hk4}xcrn#uHx{gLxl&O}j3K0H*_}j> zEut@}CrA-blqpwHaK_GKmN2EvKKL1biO^_7Xxe6I$gpl*4UFZ@Z+R(ie&gSIdBj-;wAhhGu1iXQ!Xkmdx>OTc{j;6q$`ljqB2Vsstns>SilL;RgtJ z66r3+<~Fi6oWywUVXlIy7;-U09{&AbzsG3HGu94V@Rs*+`j$PIvU4(x0_`~Px&Qnc zJBC3dfOM!0dv`Jl9_KTTuMIOg7=FdW%;CN7e?L!L{}_J!qo4ACJ0DDKX@adtN%P|d z_vt=91Y}+ZSF!%&6!gE{*&~G(Y~hQ}lD6;Q=U@K_$F#pgj={M)d%{ZQ!X^R^Z(~fb zh}yp69L%|t-JjAeg5IwW+n@aDbCG`szRo@ToU|{Y*8%SY22zB-dm9-Uq1kLwuh$2i zp+~wY{Pjhr^XfkB3lBI^l(3(a{1JAMi$qO=5`2y^amjp9YBViU6mr>L{hZfb`axg? z=qOemfv^+}qj-jJ4B@c^Uz7_8391eeM^IDH?MX*dyeRxbGek0ira+D%!$`Z(P7ZS@ z6Ce{{c4F*K60-x}x`nsD?KM2(oF{PXQAJX#vo~}V%T15)P{ed&CbJmjpd29Hf_)~`yZbGLmQ9hk1n}{Yu@%&M$tmAsjqn1A=n^zlt_h|Q7WOuKYsdC zoO|h|D4vXrpN(J7mH+YyKJ@ty6CaZhkHyR!HA}SIQe9KUG?ckyidK@LH4m{sY>`O7 zfj6FDL-9PzIyO-)r~I8VRijY#77`A>gFJxZ{d zGE}KUP(sPzV&=iaox-~E8HZ5B4jM2buRgG!jy zg?dFPg9PgR*N$inuKqC_)KgJQtr=)n43ql{sC40Xbhgcd3(>Xo0>uc;YW|)8%z2oB(Bqm6YQ(^m^Iz z5Qr)XL5eM-LtEHMC-~Blk#b8B?H#+zmaPV>WdcDBR#Qwq!_2YcR;X*sZYc8Xjqc^}9SOoyt)qF!jsBs}@dC*o425^O=$ z&>52=Bsg5IRNLAw*WCj~6|yvCGHvpN7e1R)nzOX5=?2!_IVl-R%2c|CFg{D}bBr@R zuoinzD;OV;5J-GZ9CCg$*I=$@a5f^(JToq*ohY|gVQUN# zhI$$JM1Aup@+pZ4WP_ns>R=Rm{M-KKulerfU*e^gz8pp<3ybr?&V7H`vbC@BSv$o{ zi|d74Q%S{Bkf##e43G__R)rizF3;|U{s;E{_st5)1gJ;y0T5Oy25aMgjqYwRAmYx zFbU8wNIk|_Q$EK~M#d#7qz3U)96t{jELqDFnjWS-nnzU9geZ^jv!3>CDcPMl_0Ygi z1}=Q`b9uqbFXV*EF)X3M*M8ODyYF}p%dKH;o-rK2m2w**0i#0Q2y;PcX0XHsK5^M) zJpGjygEYAO-S6e>(G#!=P6~yxeuel#f}^L|G(Qv3NsMZDa}^9GSR!w!WR9Qx;NLmn zsb^6H{pPofwmVdWVII4Y>~l~q7=b|$+u)%Odop6C`Rc!Zl&Df+`Esxph8ls@t0HdQ z{0lyQ<@>nk!Z&~cv*BoFl7uOrGGh!aJed~Wn3OzBt{jndRaQqrquu6TufCe6zUFmE zrBW0$$-RuYRqk#trQ-~nvg5S>El22f3=a0yfgR#FE*FA#UvxUJ>Myuah{X(w*9O+L zBbK6YFO33)p*CFm)(`NMx4ftzU%rb#);`}eCA@D80mZ8l1iS>)2o6*5H-}JYpNKC6 zVAWtthfKQ;#oi#k3;?qG(Q4g4k`KJwA1*=pZhCdW7fK&hMdyH(I4+!L6r<(;GQkz3 zTQ^nszP%Jdc~xb-V3)heAmk)b27VgNrVveu1;X4EH7qTvOb!Oe!1iG|FejGOSlZSW z+EYLfOAudlRyo7SjAJZ~iDx}Sndv56wJbPrqGYW$HHomQ#ab?f9#({qtB|WQlegI8 za;AnIlX3AqMb~0sQ3%)&n3~+fn6qFM&9Zq3E`nmM;8jp-Xoe!;YeGSLVO9I869sGI zLPFdK;v7MQKtvRe633&Ak7pPH&MT+3mvYPJZl@kwMiY-nM4FZTlcJ0_n(V60asG>5 z#ZTAlA&DyZ)HCWlTD4r&^6c^{YuZiDUOq+*a60H9-tXdRfmJZ4)7*ajuUP-I6AC9A zFx-8B-#|C(p2G^H)V4EP0B1o8c+?p~zF^{{X`mto$t+R4gtS?wLKEX>iP%DoU!rUx z)1_q@NpI~?SsYN*KA%APf6~{T#oeE9SxC`%kK;KCYI>dVQUb`osaLEXtbto^hHF0kO+vbqcf9p~q2YYQuB7Q}cpX8^ zC>n+_QY|#K$__0^2%40BVHqE3;Wa3EnP4iU5)6uJK}CqpVC@{i&ycf+*vt^Z4vgK6 zHQRa3r7z&Q7d)J!HWZy&KlnuN+P?R3!&%R7Lx}sjHTriDIc$4GkHRsOI$IfAxBeI3 z|NiIMy!pqRe#RI}ms#p{L$xYs@HCsA$?252wq?hi8LL;G!HZw?L{_cZgtgV8>q-KO z-FsbMupbt5iwYXpc*;gH2kp$@og+^(QVqSQEQ&Xf8W%KKr)!veE}O zn3%SDyeLv8V+AE~4wq(B1(q7edgI{6o8Z*b@L8MK#ApB;IgA)=D7u?Fw1Y(xp?2}9 zZueg&D`9#ll?lRdwZR<9EP=oI>-TZaTmHOsNOF+qE{v$&PdUg>1Bfl=tTC2{J?szo z*-wAQZMXdjHG!qesu-IyGU{086K=ct2LAoW|ICBVd^{vWJmT}Fq#_@|GPhKpa7P%9|$x;E&)-& zArQ(oYc0f3`>6+eq=>qBP(HBx;mUouKYbX^PI^wSMKG$wK2Xm*i4VA-Me<9fq*3hX z*Y$CTE(n2w7`$~%SC_DDbORxqBV=<-)t50dHHAnr7#0KDuNe%%7;;yVYN!;Qw9*3( z3=p-HMsBI6rc4HyuE4VqC00YMJ%SzW&&MBXHruptiw86bOe;2jYA*~ zN?eO@t&BL1$nw0{Csez@a~=u-?RJ_XE}6%BkIpCF2q6FtXJfJ~M-1e7sd!gg3ho%D z^C^Y}mdwmyr+2f~9L*$+LdQXEcfZ-ZM7VV%aEsKqX>}lE0izLAgjQ%#Z&>D{D)o?& z)9dKa!G^TRdMNXXyr-ro`Ij$zkrzJx1W1(N?wO>*;kd5O|L%1T>;Qger$W1+_pzlz zZ7C9+(+S)$(q^oku)Z1-h^SPSaN;8#%lJ3GM-mFkDcFwXCPbE(bk$0bo)hbh$D2UF z6YvGNq@CiD&N*mN4Xb@Xquw)#7U|PQvvX67RyN)}UIRm?|hA0bZ3PZ<#IpQyI zepzYgUbZc|esZ{D3taulukq%${4IIDoXD(0!kM&nguqfns-OwR3=yP)4;CRRDbobS zfy7W7@LX(=b3I83buN#b#lCCfeSreShi z*tR3**4xK<>XZM7v(I`qokU>q<(`A#@#sfY5LkWG2L9|-ui#T}e;2bxi5)a0fP-K` zE$pd5dnrt;gQ;U+ZjG?Jz8jW9b$v`SnlLTXw8)F@{0Q7|1*tf4o03bBl(ezKXTIoW zD)p%(DBcGM9-BsFlPM+_Tw92n6_~Mv$sFAi@Vh+KT~+*!z=co0kP9Ar0jCT-kc1eE zL-7OxmIyF#)UoU^hKbCRI!79mObV=tGBKsz7Yc7;SY{(u_`vtB`wS=7>oi8|OjUD& zcpVQkcpuQ7c7^B-ky zdWy3jb3Tth|Ji)*gCAsPJLPBtd4NoWNPFu-BtpX(g0(F3%Icu->)*hsC*zFk1O|7y z001BWNklms?oRJj~Vm!9it{|#(lzpaHurdFGdMqt0N@ZetEU+suYaP?QVmej`B z0!>7{-eYH_2$WpbTEeht%Tr5g>`YUfxB{S{LvU4q6hK+QFN3)_uzSV)GDZrjLSSNi zg1Qz?yG5AZzJqA#N@VvWaTaj7VlnKvb35h$x>>4$R1@Zis6Xh*?8=*r@7%)JNJ4G& zD0W}+VuP^%??@G+o6yYydo6?W&@>Q;VVmQ*>QeV~g&p zg(X7b@Fw6x5vn+2apEx6qM`&s7f?_M1gpe}gES@0(sHcRrK;#^!IZCczN5?0(4~&) z&~kW;3L!H^HIn8?$Z^In+Z<)t`lqwwmR*Rs1sir!=htK$jnxPx1=B8QN7Z08kQj$i z3i?D-fHooIMb)V(G$FOzodzxb2WcLnJYspWl3)Mpe{s|!j=`)n2NlvDj?ODB$MWGw zM)iNsZ}XjO-d>we;(9DLeHNQ_vB064fQykEQ6!ykkhHK6xd# ze0nFxc|%T46T|9cn9S7hGJ|?hrm$u^8qJYfA>>)#{wMe-C$||ng;>2JU@U4CabTS( zlG)lNPMwFV$ZrWl95e)FdM#UBE!so*`@S)ASI1BP-}eLW&KPW*U=ecfNn*Fx-xx9T ze*f*yMUe&aJkB~ynh(~@fgQ2&_F{BWul`t3HGQOY5*NU?A3=GbG4W}zg8P}0*C4=U%Z=Qa86 zCwde%-~IR!&U$`Ydd{VnTn+@e#cI&!5H0mB#?J0v+kHA9S?AbY8*4!m;MP z;%q-rujA?Gp2rpM{3~ML0)h4j%+!m*ll5m-nOl``lT32QQW#$c^|e*Th8xV;S(-Mb zSxK=qi%AT&W>F`o1FyaqFTPXcrbuMLE5-wD#d(9D&7cV;K+>uaXBL@NnzyFJlQGrl zI$=lPg2!IK1uwdgqehOxVo8XJi272Iwk(G(*e8sE7rg#rzVo(s(h_AhG&nCfmcQ&QI( zW6HzFJpOU#^452JkGQgjjTbb;TggpgzG zd`IY!7tYe971cqKYhZW9*Wa6_@O2-%p35#ey~I$=V1V7v?TGE*>edS_)+ll?ljE>W zvQE{rb|pDSvcPaNW6f;N*zRd2cW+0TpsC6X4XUn2o)?NH9l9*lK5%wm1G^W@FZ$dv zQ~I$!&G9p{)U%w#dz@B4js?RtPI9RNuPT4SbJM`&OaBAG*VJ62~1niq68<$bXo)kif&r-=xJKnTUR zC(e?mDXt!&v169I%SFB6W5_h^<8q8j6M zSURLs6N3;VL|{kA+z9Q~ZmNLTI!&ozM@ASL9wVe}B2&`(BE*%1q*7tEu7*)hqcMWD z2~#$wS%oNa#I67=i{xBCm?Ff%<0>_x5J*Z8x1b{`c?68<7FU^)YVlB?ni!d<<8`oK z&>0`0!-Q}c#Lg7j-SdS7(N9pbr2-`XC6pP#rhg3AY- zg#$Y>tqarY;<3V!*72L?InFt(wRkTWTdao~V9RZA^{2l`mJajIw_icX)}uTKC(BSB zrJYq#8VCtOglYvyx|D9JF?3ic=5cU&L_DePu~N;S(c$FGE_(&cq*9KWO(9u~+HsxAKWC%4nIx1;L~ zQ6-`6Gj7j4JL54$azaUK3}qHVve&u3AutXX{muYcX; zyzc`q!$k=vY+OkQDUG3+b~?xLCoJI~|M?2eSht=(y7({n@w?v3ZtqxbV$zV41#qSB zWCO&eNXHHZ<^Qqw-eGo~<+=CodEa-fwcE^SMjCZlk~`RrO*6)#1P(Ph@DVVM3Am7) zlfntfb&`;T9ujg$fPCb;lAMs_%eh>r=7a#rNl0kH#9&ImrWo7UmTg&dB!jO0`-mOEL|{bs5TUhMxk71At#U% zLJpDvEeK7JeJC|mptYhYT)qRPCVA((ewAx}ZVMYXKqcxY^-AuSUc*y=5tAaNBc5kn zpYrva`c&_?-&8G8TqSH`Q3j{QqBdFdb?-e|8LZ2Bbo+Pt%2#jY{vUsvO`Ar!_|hSU zYL+C{C@kJOrdlb}(~8HoTOQlh;idoiI-YgKiwguCHKN2*(Dl-z5}&6Pb_l9FRV=h9 z6qXK-oV%y)=CKQ4YCY`UsEC#eooJ3{G&N6SH9z{t?ZnefWEie|(P|zZO&NO52JRi3 zu4NcWMR>lz{x#PB5Kf_PH^>^t(#BBg0{h-Pe1no`j$Rkgl2t*z$o>0|v@Q|32 z@^WBUIEdo}8yt}~z1rlg^$KHz72}OU^Da2v z`9I#we|!Bq`P?5ncI$*{@C^B~$S)f41$&1HLd`@}2@K(>ILEfT@8iO=FPNvNeER1t zx>2<5tt-kvF(DS3Ua*oZsce7V;!4Oc+GLoeX?Ve!u)IKiW_Au?ZLOV_vBPql$T}f|+DtVRr#86?eDSAjT zgYr`nmS*>99$@wpCr8&^Jm`>gQ6!;V1XQfZb4RQdxpin0ktj2##tX)S@rx(o zmUM#-oQOPKbai?pBw7w*e`_VoT3ABeh0vEYeaAzo}sjRvnWa-H|D1u)4F zO|=5?XOS{W&KPZ}(V|sqK7bO9!jqdEt1}civCRq{veaV4<1R>i8F~QK#5K~iAy#(NqTDL1ciXx0THrG;XO_C&Ntx3~#zXISPAB}SuV;CD7 zJN#=Ipatv}xJ0C&mlH)tX{OVR3V}{FVR*+*DnmnvNvT8$PKrWNlxC>8Hp}aV(GX*& z_Tu+-=pn46$$htdfiZ7^2z-qP?w_J5Rc44#SR~{ny$`hFHm#)1H+FrKiRxphkbsMr zOFXS4a8CYwGV72aJ?*VQTAbWk@^%O79oF04TrLV+fgt??S9e`J8U$B3fKUBIY!HJ* zu5NLw_j9qeOUZFTk1c3qKIB|pvL!oJd@jO)xBjqbj3Fn467{HkU`n4R0Ak28*!rO3 zQ-6OO?|A2*BViR<)?m~#aDD~3Zy<~yR0+}`gczX$5)sN~CWS_!2|@Q__<~N(7O1R{ z0+t|_LLsDsNDGlRfk_CHh}uaAk77)dcf8}*xccg+vT-AzO2igr5d~kOwYm*kYTi2E zLr;|Is|)S+n_{+HXAkz!4v`({T0yS}O}_IVxAEEk^C4ER3tW8Jaz>YFVv|v+X!4FF zZ9B3&Vb?DB@qIfv|GdljFE4*38#g}{9}G%Wm`94;+kkrB^1N$X?3{9PGIG2)EIHfk zBYfnuxA5UFf0Ska&vKgeS@w*3HrAH0CM2ByNQ12(yPFM@u(qk#Gv#UCx|6v&eD^c= z@zf7)W=A7os-7|1$omw5zF>%moFQ*!1!AiMD)Z>Q|BX&&NOG^m8lPklQ7INV^*Y?F0B9*Q&X!lAjpB=6x)H4_tXW-!kTE>0S-mt}ka)_fxTrC@T&j(*fFTy1{)#yk0w8!lZMS4(!XBDli*AXLg;S}D5f4^J>! z4)NOseFOsu`9po$hl1ZzoJYALOBsw{yr&X8Hf`aQLv)6gHr^|&Pp|~E7y>B@N1|&? zJ4Z!yufu*}25}*MRUdg47G=;bPzOtPs@uF#AdcMSC|yDF0xcC5DCO+0-ZsAwtV^dk z_yVsIv6kfO@253DS>KI!CA*Yw3^(>YAQZLsRUY(ILK5X z7~CKGN5?0BRL|!y643M3j3Y!@oQR~@i70XnYVUM4feveRKInNI{2aR|UFVA;tr97b z2&%U##>#+zF%c@K-|Z>`rKhrR6-4kM6b0WP)S2qAG(GT7gWQU=g zjd@C}LXpJwp+}!v&yNRMGF!ff}@r@be2E$-OjStRIbOmc5FY!_!d9zwOyexi^IjoGDRsmJoe)Crb7`6$ z1*&Ht7aoct zG#U-G*2i>zr>3S@y?QkpH*P#(YsIOu=N@P^%7ATY#%x?;TERpdu}A07Fie*^6QYDF z2~`qo2iiI4yv}eoO7IyHDVW|#-91W`79doEqu!h*Fa}Bx9pfYy@<$yKE<`}dN%kNE^n9HIX+IqE zcn?S4fV5;wcG9OWB|vZ}41aw6W&F-f{jO>2Kr05c1ADml6TTiCXr^U(P7+1)JQ5FV zeaQ3iPkaHNt>%yZ@MfG}fw-3vsNrZ37$z``P(`Q$5urqEK~${GCG~T0+%Ed4(9%-D z62u{7C~Xl*@u5WsO}w8)!X!#h5@ZMH9lYzEZ{V7%uVmxKl73M2`VD)vor0l2F`x2m zekNd^kAmV^*XMlwrfA_JN3W8>ffxOv_+sXbfj+XzvTOTY-2SzH2j|Vf$P|43H@0&5UvK8&bxmfIy+k_3TZhXWe$J6jJB-nUwkPp1 zYARyx;TBVSr)hK=EZaNGYqz`>TEaE2cr9m-Y@$J}EQRVKcSJ$GsJcuLCCX^OElB#g zDqr3b4d6U$&wn!8gJHEvuwrRjj}=WEx-1e!oDvC0p0TPv#LBeIzkcKvUi=5|DN$Pn zjOq=I?w~N*kXw(4M-aSE3naZCXdz$dG{}M#Lk2o!!cm8N$DHfyUYHnNu1^iFc;?Tb zl;MtV{R2rPtQ-qOk+6Jug-RT8&kw)Bw{QOewgG$E85_{Fydwb;RXi{)0p%UBcML1X zDzEs=2mXW?z4|KDrI(xxp3vjBLj>I;qmlr(-@I5uTGDHISSmf70i!~sm-{-Ae#z2U zTCxM#NlQ_9ux`+WKH17= z1fDuF#04|=Lk2cjo;q|Pch9DHugKG!mptzU+<5O7aRT`P?%Ofl|A8XSfs$TFnhZNu z3bErhwvMrIw(z>=>5TOB2#>>g97aH-0%%lUzgtwP6(L2pD+=#yA@nU}xF-V?u+pa? zT(@rBzz)(y#5zO=_KleV^j1JCK@$)yC{aB`p;nqm97+Uj3@9meT#9brR0Tvsh+dVW1G0SeXK|-*aR|P z#WGAasiSqmBh@+LY7@P3ocd6XQ85xq%^p$2k)=5~!(85B)}?IEJ)McYh*s!0A`TXn zcNnV8k+t()-r+D-u$~PM4p65hSM5R(WHf^Xb^~x+f zwC!H7frox@KbO7eDJz|hKlnZtjJ}7PGp#i6iuKN zX;M?5m|Z9mp3Zr^FB(14P)bnP1gs!F0q3u;eHr>7WihKvgZ7A6%h!8rUs&W?7qDC%kJ5~sZ1tVNqTb1PeoMAw zr^M=57I7AAaBsP#PgA(qdMb~@nJu(#amxpA9v;~SAN%N+c*ooS3>B?Mg|iU%bgUmn zSPmuSV4z0X=0kyNSA@{r)Ky8BS0G-yg@m5&DHbIufet>*k-HYK50Pm^rcq`FXLmxF z;+^mMWnTMp&t&6SkVMCp)(eZgFCbiTz2}ab`p@e2n>1Ul%li(BEPf}A)}@Z1Ld2EV z*<#xx_jB8Af6v6uAF_O0bHN2w8Y7xY9hA~^(u`&bdv^yWb~f3)Gv|dbzMkhk?`KQ4 zdCh!{u!SYiu;`_FfJm^UII*5QZ_&zghbKx1Q-te%n8 zT14WBBp~}@estp`=S;&;yMOq|mx-_z#BF%e6oh}?%f_qL^GGxc2wB@g77(jQS`ksx z5M~2&yIQE-j_On$GpTv~%U;KIFT9S6hb|%_A;uhV>=y+-IjnecUY#gV6ahDG=4apZ zYkcQVKEO<#5fey*!;}mTh2F3y7DW}skaG-Y9d7*75Axzad=EH_HBz9iJPUw0gKaOq z(2YaQqisHT>gnsV`G{)4#Y`^b1oQRZTb^D znBU9ZJ9aTA0Taqqp_GWz0@{0OB?xYPrNZ9a@r93lj2AxnNd^2;pCJXVf%$v;&HD#G zS6*l77q&c0cA9$%WjtD~R!P(JgwOi--*6dko<~!-R772}Q(c2=VJBu7bR16Lavu6F znpJ(8v`ib?!SG-;Fdavncg;2I+&;ms?>)-*zVL0PWCdwxQ7O`^Xd|?2hlqeuV1vSn zLF17w0;asKMYn{mwt{qv9bJuwB|Fvqg>Ijer_dVmrS6OM(5`ftX$(GhsMl-E&9$(u z=pOXS_NK(p$S^a_SpY^xM%a^0GF@>@C;M-IpwHc=2 zX?sgPHo@N7!)*6;+KdwGa|zVht2U5%aNRx~nLj7HBK9Uo5j3Lkrh*CqEe>sGsgW|G z0!irL2vl`U!vqX%jE@Pa!ln5k^T*y7S2MqRqzKLj8_(n*pVwNGWs4&*qUgX8X_q>z zdmLj7#u%)%hkW#1WJ#q`IiS;UG^Uj5>Cr?{bXe!I@P0&5M4sn#I-UK_htvMQodEG7)K}^Zm5ng+d}hN_m~A#8)9fA(JX)J!Vse;W zYjX0mLoKQGuD9ha<6~pQSZYRCUa7HZY?Km+M&xrU;#jXLEccWY9f$Hdk8OM$Z0sjV za#Y7#IH!)eh@)Zk`Zb6MKl7hoi8gV8k_my}!(kW4kF~gUJgi!+lIJXLhv0%b zqPD98sizQ%HP5Mn+IPv8Y{{1FC^zBW&&8y5dTXW+Ck->#!wk{f1F@ya6c45k@{!Mdlv{86JF-zn{v5~T=ss%e8#I}J3L zz9HRvJPKmN%YN<}KKIA(Wx@yQ2svO04B{4Jfr<+>Two=NQLSm;cMsJIFDxkq7fY;_ z>=wZ<^7;`Bxxz9SV10&^1cuVuFy2DX+`clLq1T?Q~u@yAK-=Weph+%dVW3kG|pM!J>KsxG`i*b z)_|1yQfJYbWJ)QVb0>aUFRD70z&bf(l&6xa&#jY|o=Z7~FirK?ig= zURNLxRA1A@iNYJroD%X7BP%t{N}UTfJdLeiya$n(I23vgK?3{i6b-$+*KN~6dl=kusSC-8PU@;b23wo^TGku!wQ8WCdMg?SJJBy zv}ogVkBK5|)iG8ZB~lsR^j~COv+%x@2tf+^LN5UoX%ZAs1fA7eIq&uBxOeAcv;}t6 z?q<&Kq&Dh=#+V?+PJcttHQMthGm+#9ny(W2JZ#kscM=(Il|Dj*3#Mr;Rgs>_Mu+ z!?6M}b)cJfaA97n6m92N-KbL=8R~s!A@L0chYlHMO;-8$uYQ$br>SRM6oX>kt|5cZ zvR~jTv~nC#qM4bhvhu}0K}tqjJM=&&ni9}HpsYu=J5=)=B>`|f8p@Oe(A zBQgT1ccej)mfv4c(+Hi+vMe{aU}!gUI@5Wnks}Zsk+-eBB)XS~5nQLk zUV9$1_B;}5#6;w?ZK^{?jo^}mjs@p(X3La~4}mWJ%!MbG`u1qX z*x?@UaOU5BLp1k@R#Y|&bHx=`ux;x$Ha+>Gg2GZA8p+tzt2&zVKAQ7A+~a!h&sg-< z;YCvm(}CrhQM$BUim4K0RWzoE^b)>aK!s(jf9AE!eB*Alxp##?UbcTK;!Oe#6=(IO?S-V>8s>c&i;y7lwC z@4f$oG0z0Q9OssyWtdnE;eAX9#UGIhlyP^F7K!NdO>`Rx9R+7ZS2Mf&@1n2;Gr%-~ zX(~xdZg&#c!<&Em^;~lM!iTWr%ZOWs&^498ZW5jVPa&_*W7Y{MEJZ5;5H5$9(D^Z{<^8`QJ>%Q`k|9 zzX+;lS25KF9n)skG@%lZ3M4g66l;hzQ4S$@_&i|p;u++!kl7@lMTiNEwP9lmzVKJO zx#)t;Om^CIW~XUSwy4;c+MXI$U-=qF!!oXV)2n#W&?O|)F$+)weje`X7#q9>klB6c zS%liWi3w-f4U8+zoVO$rla#qIg%BacA&}%bE0kd@1ipOh$N8zZy}jhVR3~y2*+z*U9wZj9{&QD%yU8e4yjfsQKeRpEcjjNFob~C8f)`j_fc1yt3bKr z#fGiqko#%TWnGH5mNZrKHp6gm@7Xh77s~_??|bxxnTOeWjShj)nS_pM5QhZi4OR&q z6{z?I$~l674WJ|-E-xITvU5q5`eQPX@bH6QJOCvR!J$obTmZnwo678@*oKe8HFXn! z$BanmuDXNzY2|ms0aWgB-jXB1W@y_ETu-f-2 z0x2{y1eg-Rl?k{>y_?i1M9NMxaRp6U$;8p}f(nXur^Rk>acTvrw>WJ;1$N$dE}#E! zlgiX8Drys#*M`{?r;JoG(lCuG3s4m!#4a#0TBG7LMBub5bh;FXGKi|ebjGF6ejZ6{ zlHFSstxAR6*5b~+l&4>`np8ukg@;Bn9=Z2^ItJQie(uo==wPWvie2pvw$&L(+LpRw z-2&G^>flinkt7LG6z!)ibvVyHgg_j}G#ZVgqWoG|fHKCQwLasK6%?U2iU_R&R`usV zv5HI4Kr9LuK#9Q+VVo}-5K1p+fQWEl_I0VhKztx*A#dkIW*I?goEV(eg`QIgWw9(2 zi3thBv0*ODs2PPgi!TeVO2GTjqerwNbO>}5wD9ZjklP%IG)j$t+lXa3 zAt>4bl&;}iLQZsCbtH?%E2u4BMz(V&l`~LVZON8w$&NLFxv&-5V;0#fyf5g039is2 z^%YHGy<3_irYTtcxQ7R`QzbqiV@AVtUv#tEI1 z+DTvvm<5?Yo4tgvi#+V%ZSVXQUVrWL*>G0jT6T3@yI`eIZY)Zq*UxYwl5hdx%IiCP z?WR70{q~zArvuD@mI>u?RWEqFZ><5_xyktJbyhSV-Z8c8f`R5wL+Ta z?49f|GpX41Sjw)QZGQTvevvD-ybx#<=_REpA}%%HchS?37KGpeqD@IFZfMiN;rZU) zyZG4WKE{VW{Z|Z~H$*;?(>%M0j0i*PhL90Uv$~H=JE$mBRikPKWgK4HV*Ck+Q{V$T zC-xAfFdB+II+CI(vMX6(DDpUUt5^{{XEj0Jky5pakyaf)UE}9o{yMJtrE9og=zJ<9 z#6-Q8$xsqw`SRLANtQk+7I3t4VZQtZ{U+}?=D)t>t=#s$_tS|gWUV<;@2Mzy4W=r1 z8lssCfssfU%QHUv!4L9NSG@)Rz8{1Xp(yEz|}5%>fjbl+7b2YL8vAlhYpm z0#~2MfWab{cOI__4FO*YWA}e2<)94YHV1s^92Ut;1Ufz?Xietw!f6Z#5BiVGWdzVp zh&;%y@x)r;ohQR#bwDRzLskfe`!Z!)dcImI zGL@l9CoFa)5nS-So!U4Ld55T8Kk5Y6$z9;18T7XCcgVxMIl zoG&}*LV+Z5-ZRyx^FW>=Mv*y7h%Ht+cs$Bz=JZ}{eU{GVS*BM{&O3fn&tPqFpL`@Z zJzvL7D^2hYXC2=8E~31cLuFy8OX3#9oaA`%1!^p4d{C^jbF?4) zKI>L2r#d~we@srYn>yQf-@#*g4UQbIEL+Die)+H7O~*iIUif2PLA7=7S+zAbW2xo*$?Owh5>5NZf7bbda%%c>uM-0 zVEpek3WR|I7Xj}qr)$v4nsp&~<6uR(6{;t|&ed-V16x6*X|sWxl^|>JVGK=|P^U}77q~FIqcoLmvyVwa@)Urp68zXqN1X3 z3UcqCC^6tuJvXwDHK}X4&1vT?bW|;nT2Ru~(DO@+&cOf%A_cjf0n>zh7fN|L-q2#K zSMchm%8tth0tB22IHZ6GoZw({$(C%%PWu%R041J3x#3cN^TxaSzg30Ipi)xik{=(3 zLH18DU<|zKRa^MIKllcn<}wtc2&1K*mM-ZCb@^{oHXRoF*kH@{pe|C&0&PUK!O!8s zEY41$m_cMW$ZmX?;9c)}6R*DN>1^5zahdP6&ZDI$cXa>Tk0SjTZ_~j)st%Cqv4jwn zRMY_@UHI5OzV(fN;&Y$-I4i~tn>JM#8}(Fb5z2TntC*OW#k(3i9JaZHL#2IDl(;C)%DT~F(a>w_sr5HCFQ=k`?&R=Kgnlq|1{}Phv;&{%!(=M z>l-vihw0E_CZ2`Fp&Ag^3=&5;*Xak^i%a6-^7nG=&vv0O zy$IebUU>D@-1@=)m&dY{^UNq44mlN~k!?xDqC_ytoOX20EtCgk@{s zf%k@Gg*9s~;-xQnJ^%W}n=mMn^$~GRXe^IlW1WpJxr7I{eVtWx*gNNFh(SU?QTTgN z2-JZT%L=9W)L(vx7hQ7=>f%dI^Ws*SSMmExIc&MUjYF}hLf}$92;D6yo$*~OiO#l z&s|4Wb=atb_J+u5dK}1Qa(zWLg>(*;c_Kr(!Isx39%DVBlQErJvb1xAxscG)32L=k z$GkXs?`gN&SZj|;^QU`0aU36)uJYmG;bWU9#;|hbNTX+A6hqwQjnYre|Sfz-=0{ipdcUjUreY*J4 zFX_Esm!EH!uNlaPb4Vb2XdOKCV&*>m4P@<8dBxQgOyl3U|9g+HVa-`gt=o=G_px@< zOPQ->q-lncVrePnmd5CqosaHeXqO@c2jXxFvQUWN#wc*xxN3Bi&2E-azmKTCiZ9R2 z6@($7LR=zFA{B62um{p5HZH%yodT24tdfMg%{`#e{J3qHDpgI39|MEGWz48hY6En4A zf@SMhFWscmARn@L1BCB6&L&bMQB23>pdv!Kw&(z+$dQuU5)F_$V3w$w!tA%1y6bN+ z>>)BMz+S>M&%v@5-BWgvV=r=*UETr6B_OKYgU;aOLhwuL*pe;T5p8z81ySgW>#`pM z1q^Cm$RYH=zyB+ocV3;1XAiM*JfT)mG^zy>qtnTmnG4L! zC?0xXmW}JKz8BzsZSLliXlCKmDiFYa=WdMXl-xW19L!<2>v2&*q-L`FE5B zYeD%UF4W}_YN}A)8&#Uo%(3I%AG7XBPXcW?T}6P3T!-cM56^F^-*SBlMCJHfDrPO& ziJ#&|YuyFr|GREMX zBZ?x%$H$MksHC;-6{k)Y(fUJO$H5%IE`qB&xI7Lu$bH?F=@20!W0&2@|Nk$4!DM|a zf+P5hka)bwXqoArMsJVjUV}sm6GcREgh`@a5dew=b+@BTPzVMSJUV#1DvGqqX_PlO z<#5U&rBLp=_RF~Q51TOkT*3pZF&p5?Prir?mn}!B2G4)trTp8i-{$OmAA9z;5v>a* z^>Y5!GNG1w8f`}{l-0HXnG=lG#KB{NW0)4}a>sH%OM@BejB@_U&1{`$;e3}^(BrJb z5z0Qx1#WC8!d2@megW;gNVk}9gaPD&xpcU5>u5i>WJed`&#zN;wGsPiY6*fYaJYNR zzAU!x%YYu3l=@fY5mfp6GSKwim)hzDO3DwU$QSNHN~EuOpaw+Ddt7KGjf?c>uXOeH z#F3}lFx-QogAXk-){^@5AT^*(%yxuF$z>I(tSVzyDUvK=QK6t7b!93dl${$v`hojm zOndYB>(BJjR%SCeHLrUHXRU+S zlu`IVjM%+Z-n;$k2Md>?6X#Eb0snaabkEmrsj%hx33l$-$>+cHQFiY7K3kr%krgW( zaikflM;M97I+ji&Z#6q})G! zKOgzjhxyR|`CIhH1iL(^y}3ykh05A0X4EjHCvjq-0ji=fvBuOiRuu|e9?%hV@*Ghm z#j-E=YNyf8p!0~%jAb(GFcHCu4y+QvhEP(7ODQc;JQq{OkfT=9p3$rcBP2A+gqa+K z%0O5o!Z`k{2SP#X*d0VBV*S&e#-vtEo4~LHvH+Ckl7!P_4O=KdGI)_eK*d0Vi!S;}zWI$Wv;DEHXyehwVk!x0JR+Pu#HOFQ zj9s6;lj$Z5Wl+zHgKC5Xs1jJ_10(GgKfdD|tb6%S7ct%dHU6R2tdss$4iu{e&+Ru= z_j{WSh0>siLif6>kMpU7Kx=9TQ@ieAX6JY5%soop+KTObA03}VRNYK{>{1#lE@X7= zMbw5@J)yTQYtOLv(L0%X>^rpf-AUfQ7nk1wz{aQj6YDR2*(ra$A$Xem9%b^e?=w4b z7g_63+SA|S>p#edYL}AKHZZjEDU7VSn33^wQQ92y7?9BQQ}%C3?9mi1*%@hO>(;G@ zMNM<86mKGeR*J|(DBbPyDGFSBsxgx`#g+xG$SJfHtO{5aa5-dIVAZ-ctZoeBTt>Uy z>?a;eQkTfVsH$++NB)`4K@_!j4rg5Q>gUmk1~q)bJW97ic^Co3hH2r(x4w^B7E|q* zGNCUh(RdfgeFvv3*FXI-wteMuylnLtn>!Qih(`I!?g<`ZoZZPL_PII2aKxK0`F$Q< zUa};VJX|us+dXD8&aTHr_wWG{GSa)NCy$*jx*mtRj)N6227l)X6puKd26(xJgr~X$X~DMrsi&zxqQqEw7TQ zoauZPYZNVKaUI85Yc{ee7ZTzVwKFhB z`#kpj*A#t9Mp?KK9!Ed&P+e_Z#MV+#>%^roq%7JlEFgt4nW+KgJt}AhOPlktS$!a* zgnl`yhtL{G<`ny6p;P69rHi~WN)^op0eldA5Q5Sr@~bQsDJeAP5X5`LI)X2#!>n@z zt$Q8C)`J&CP=eQipoBDYm}*Qbg(w6yu%kQ32cFaS9?)1XrYb)L_WIpb%d)rk=TDyH86gMTLD?LZza(=d*Wk z;cKse+EQ`rR9#c{d(xq#(w0)>-R49(qM|j+fNJI-GGw}_VJS6${u0P?u)b`?&>6IL z5$hCDa|E9f2S=XYMLo3WM9*a|Ur{z{3kjdS)4(1SR@J@Ea6;L@Ia z;;kD~x+O}?Q+ubA{mAqdI=cIHO`kM%Y?(&sj3i0OT*lOFPTDr?*^_hs{WF|@ z-j%%lcm9wyYtJD#K^7E87F}OGcpb&lH!l5=|ZPn3pTidf{?<;*T&#%j!~x2&b0=cI(*aN=Gx#Ll{~?Wj$R)Y1ur=8h7Z7DPZs0CPt;~zf!5w3dIJBxN=ZF*Gx`(p5@h-$uibd5{7Nc=We-?SHJ7` z=bdMl`Q%WHM#tz}J>Jpc0nekLpAQHzaWcj){mQ%fUw`{v9(?47SRGitOh{snJ*z@U z*0A1t?!WbWoUILU?kP#|Nkoz4mho8FK;V;q@#kFr3)fOR?|ht&24bp{?gkxM;L|%@ z;r5&NFHmf`zD>znRU%ha*=TyeWXX{xU)b~L_j%;Ie@T1tLyNw}b{@obpgjqD9)O2{ z`q&#;d+`md*!UEbTD-6_J@EiP_~&!yy>{6vcTgWWI4j4_`~RH>Zhy|a*RJ@v4mwJ3 zdCDW-|13Lx_`3xE-~n!}e?qSJI3~o{v+YhE`i~Ei&fK!-&`D>$M>+#DJ7N3xAa1;d z4Nv*+tlD%rqK+x>bYRWr3^3I!-lmlU}P@m-mia~bH~?k+1YDoPfQc;-%TWzgp6EZ zF3ZR=%Pc$CQeR8e2(2OEf!UqxnrUIE(#&Q_#G^=<$vt^A%&e=it1`}aCnlMrMz`D$ zdIU}fxP1#6PES-K{&0@Dlt??V;mROxYaY^R&<`(*zw;`g3n(jOkx4nIf}=f{+*5Hd zqy--wYJg%?1rII|qv>F<67a$I&b1pf+ENEBv5MuvurE`1t5KDt9NC_LCst9v3s#DK z&BPT%IwWz86|2uBe{3b*?V}1$#BaK+@8+avj zbXM$DO3+aeV;E70^HeCVx0~Qs-lD^RP;fYt$95PPK_!x}qS)8m$76Tx_lWnP!4}NBB;}G5MMVO%pej<}HJApetI}bV#7q;*3_9#Z zEaW^1kHw=v%z`BI(IB|OJPOu%dV^=`>|JU)c{#y|8vpR^vX8gD{aUVj-BUShQ-KJZN8QXxQqT(ry9t3oB;Ju&U67?#6eD;;RLaTreRbpUGhk z$N(qASwfP>m>EO!kvX<)+s#TJVF`(ldaT(tfT2X-nr~w`4Y2=oh z-~Rz#{nocb6we2ng`zO)ojnaG3gwVf#}AaB6?K087v91zz40aJ$|y+;Qq9oAH9A44 zU$P4Q;A(c=xs$O-6Ra=v+k#LBTF$d3ju}_N7e4lJUh$TYd%byQmV3VPN6hT}A4hg?nxEu>uYQufTi?S4&-o2ZTs`^`Dt*43 z58QAWzctWsBLkyw{+M3!OHX$*^?LnrLD+OVLi!}aeX$@9IPdVK1C#=}(s(ISenCT2 zRebqdU*fgrz6|XQiPI&nUwT?p{YB#7E``P&bfO&cTEc+W77Uauyxp=|ao!n@9OX;& zS`rg7MJ*m9K~r-ULr#RycA@MH0*|9hK6nVvb;(XqQv*L`KSv8pDPw!p`EO+9=24UcDJ^Z4C0-=>ijCBs$p2!;fpuU_)(d9tdEh3(&(ZFke?{}F| zmbRr7T~-Buxqw817$IIWPBi9-&X0)v{cQJhOi4tX1z7bH=slMBo*k?raF$P~hasXA zOaXPl* z8hT(qd^UadRJ&@`TF-jM-*3cuT1n+dAraHcy+4vCTuXZv-LZ-_iVPU`rV}I#@y)M) zi)U{B<^I|{PVer#kVwH>r3BcmT2f_Svw=o&`&+6VSWlGz(d8uYGdV00E?24z2vNw9mcb|3gL zM8|mhJ6^{tUilQ(tt}AT1aeQ&!CxHP9Jz5kV%P2$fNPQnBnRJQ&(3Y^s{Bv3-}3`D zZZ?cAA0bJEfm(!(JN=#Ve$b39BsPXywaC%xpweC`A9W3|F2F-)tcf+d!s2;2*0BC06$Oj#Di ztQNul{8r3kAJbJ9E^Ik<3%C62=3xKGiLd3%oCqNd@b=d*WpIE@wyy}{p_>&%(!UqTW-k0OZ z4{1SZExva?xCzkn{R24K)>ynTp=v}7S=Z;B34w;9FbD5F;!S&+b{|$3H8#4Yx6=n| zE4Qyhw0mouqr_Da6-rTpF`l{@?$|NLx@AkaKh4>n3^8SE2u&g)%CRsjS+VYHf6YAt z+{SittIIcNK}#ARlb8xm-0~bsQX+F18atNSGIq+5ea&#v?vpyRP9csXTmcJH))FRN z*q{;wtm4U#G-t`4lc9RBBZ=jtr6C zzMpS=<-42|kFqkavXq1+J|=O7I9Hsp7BzRL|qLPt% zWxCX=&lC#q&X7RzmVw+*Nol{|15Jof$_@YdkN?W|e|!t6SeiC;+KQTxc@mHF8O

>5E3Cc^^TQ}^#u+G|x zD~v6N?L(bzM-?zJ3=7nvh!w!*a@5{eTh3n@z!kW;eLkB`hpdIiKIKA6%W9l);d%Z2 z`-HpE9XQhp1rNsp?oo>aQj0i1?m}SHJ8(?}BCIJx=@d3?{dIPJ>uS{Bf#9&rV00SG zG&vPQt@V+ar2Q@N$oMjS>5ugzbeB8mC*FVMdAy~oYWhvr+{6ctg)S~a5^D3waMc@M z#Jk_~L-NdH#Ui>F*b8PiS$-cTn&S7~`f4tF z8K+;&?|kTY!L9?J7oEkefOoOE9g;2R+fd+7dF)g8+~jAOD2>w?N=Zf%w5$Pk(13~$ z4-I=)i`L617%xOVBAw3gO`rij^;DY`k~~JI9kchQn0`4dJ1C*JDp>)V477AecX*qC$g$-aDT^**TxHb=IXnWTD#V!B(YO>ic z&U2I^_luKB$@(hydFi}#$dxy&!)F*c_wQoE7?)usT>VkSOvi-k4&CK5a-G098 zk|&^>=7af4b*&n>>+~r@fYuHRmue zyqcf>{C3Vg_dN2n&dm4@_TBqE#(w@ssQX3xJN5C8aoe{}KO0I1% zA8OZsUyeiR!fWrbth{245IKlrONwH}25vbU3@%NfY%H;hDZ5?+!#4uPx>q2wWVrUB z%^dTsJ?Xt-jU{a~k#f5q4Aqe2G7@->LPv{7F_@@?dN87hdW=!_l^kx=V5`HF&U`j^ zZy)E2pZX`Z4sT@Bv}F|)R@ri(@JeD%dE9D#?SkjAzpc2pTnx$V0>_My$0T?&>g1izT;lbKlNg6n-1L> zV2Doomhc)Y}UJBLYZ6_**b zO}-e2$(N@w!&8p(w86NPn4OgQIf?#=3O7-bX{;(45ltClCO?l^xro#?aO9+Z1!;AN z*<5iIn2jQfn^A# zNyIeP?;~O_PL`AFQ2&M;ai5}ZRQJcdFAt6GobO0GAj06hBQi&CUo;Z(Fs0Hi(&o)1 zafHt^j2$DgW4!6jui~;-J&BE*0-Pa5GRxYo@3;J{V|4tz)4&PZj`8`>edAL^ruqK& zzsPm}=g-)*d6-8%W{9APlV?oFx94zO(Ba$bDV%c$mMUiR!4vtf86Wy)9taV37` zrLW|pfAlUY21W^mD1o8HAo%b(ln5nJWyD6TG%$19HU`c(y~w&9V>(SNL9ygEs&>D-45|YXg!z)i=-5F1(G4m#N z-1=`A8$T?gxBIqN0}xl9%NbAoD1*y3%?V^gREJklox|^Syj<_?-}#$)$*q-VT*IbE zzZ4sl+jgIr2r-tpTw`R_sf?^Tl{Ke7n_F*qH`BX6)c)?iyWYso?N4CkNf$oobG!P= z^SEkG!Oif?%@aj_B~E6En!kC z4A*LC+ThlkwzH!-&W0^3*nILv&RnqtldR(YFMXAyK1HeQsFoVUOk;);H1|woIdQ2* z)-+@ulEjjipjp7l_2rSZghZh%fsfEaaCnQ3sn%@R158vrsX%5!VT2D2j99N!QlXN; zZ0v(^E}gEXBGrp1#9pX6OOgg-?N?v(+uSx1e_R{#C)jT&p!0mL+;vpI&^AM7|hX} zzAX`%pZ6^mmUJ_VO9-S{2EeYsF^H#(kQTt)(?m>@GZIL-O2Mkoqng}m>E0-U7r$gPfB24{g64=cdHdU6!^>ZB0UI}TQyzwb zRWm1mc&zWTCKi0Am`DN0!zR9NFzQ}GqN@mOoa z@d{3LJj7VE;mI>a(i~TCYsI-CnJpm`g4SVX&n%_=Wh%R?MEfkSeAX+u>=~DH=Fp?C zUa6T%+j4CYu~b*CV%k817DSVQ6?183dnr%(^OB;#vO8(-`~Wtqe}Wk{KluWk3zX zsx0%KkALLPIrqQ+1|uhL2#h9k3yzOv{n9GPmH@>c+4&WYTd$Zbs9IPc@q1_pUI&uh zcm05g`~Fw&ev@qFtY`io21hp@@^Q)oOE~E^a!hs~w+xIn<}x8j*nwi!o{BN)YEtYx^dMUM(DnO)Uz-!`&{h+#CC3q4Lk>u# z#-vp6YG{%N9x#c>H6;hUiNHbR0wkjv@LuW1uTH$cnY3r9^uS&SkoW-fUd{_cTRTu^ zvCFVtsF!kf)H?Ah1zZ6cVWXHI{o9v#)?=PSe|X%(t6U{QUNN zx%2LQ%A9FlEDFbcb8>kNzxQWwkqwWFky zmi6OH*|2vP%k~>KGzKs#c<*uQka&>DIZT!_SkH)XS}~!KA+!ADUZm6tqcq=U#6Uqe@sa()9sviv+TTW-|c&v_Id1 zaIsZ9B^p?wf*DiEG19<_!xrcQN0$FWcqDsreOsU}eR-(0WJi9lfGPAG1C8GTn>NE2 zzI+v%H*DavlWk}~(jvL#9o{R(M5y-!w6;*`Tk@F{H>-gtD}~6FNE#6`4Q_je9kUEG zM0IbOOP}}(UiF(-vT<-dHEKkZh_Dy~xpzd}q}Kq|3UB!RxAEzBy_0~dJiIq_i}0zb1zvF;_|!mXEZ z>o0!H?YG^+#*GzLtN#x{_L=lJx5D6UHHMn zN^Tu5*L(lXpXzx&vhLmO?fPXU!=;T%xz+%A#>kY;m%DKge1(mMPD_%WUsAiZV zpplZ{HPi=(t{cznEVwKn+4R@ki5tM{B)NKQDiQ;0tEC=RHz~{^M`#K#z(0TcdP+9x zz$i(;i_aZIF-*@uGjd37i2U4iZ_#p&P)#k>g2PDEchC|AoJi5BrJ$jf&7*h;Yt)F6 zO9|gpR?Wnyfr)W2d4?fwm##NN$yNDjHqIS;_p)JXDR-wc2+M#v5uTs?a36zAGR`;y z(GYvcIq7ldGNFdE&pDfOFT9Y3Gwj<@XZO8R+%R@8&52t{;w4lE2e96w5~Elk3Q4*4 zb^7-8%YzE=^>@N!Ik?jLL*~9G9+eKa9L1iZ-N9p7sAOSI(S%oGG2~A9tH1th&VJ8T z)FZ=eWRW6CHkHIAaZGN6+(-`x7qtNz#^O!Ho(R%PgpJD#*CW<8Dy-X|bS0e>5rdH! z?;Vx~o=IRIunSw%kLE0;!FuYP%q*v%3=}XcHmp$vizPCKw1Aaqb*Q!Wy#OQBRm!x$ ze%L)cow{9r2qCUxSQpRjeri>m$pMUBojXf98PQv}--?Y47oPt(?%s9>8%{dy0Qm$t zjNRhl`~Kl|x#$BtuLB?RP&iZ{F1^ybPlq)Z0f~2%eNJqmqRZK1x(r%X#Tg+JNDbtX z!&^`0GHh&t{gB;9$xh&1j@s2sx-~RdN3MhTf)%7^tfdPa2{s+;cGfH4txFs2pQ)`!szc%b`f(Pyj*k})~4 zn_GVRO|H4_PdR1F3eGxnh*c|Ns-+pK)fnetW;$nLQrLBW%H7-dbIRs(ICuD??IYfJ z-7>ab)+jLQ4csj2k`xnf(2AOfVMuwYBarF$FT1*`x z7ZFV*%--7|-=9*=1{l~g$V(poVg_Y^7e4b6&Z?Y4l^V94jf1=>+KP;A(*mgvP}od2r>BFX9Uop-;=?lg1sELQ%&kx0 zpFjG)+2@rt5>eNjGLVQVie|?Kf-h~)yG`S6E$<_muXbIzF^Tev9T3eb@)sx^Ot z+YW`*Cnm)4#2~~##N}69#b-YGXWVno4~gQKp`i$0hVkVQ)yHojxn~a((_=)9jFK4Q zEM?e63^eQ9^}X+K-ZP(qD_L+JYivQoa;!zC7P1)?yyM1C%!j2g(NM4NbcE-|_|Dsz z-S_|Wyf(bdoV6dR~JL%=~iPCSu!M4Ey%cpDR`!=zKJWR_tjXt139QH|7F8oqE1 z#E_JOVPC1^AbE)Yi)CRU+ihz*?OpeySY4LYj_3Vr1g}~a!Dd&({C2?TA zVAQnNNyk79Nm#thOZ-Tya;qJ3G{y0YPZ|T_iA)j_&Nx@ilipS_7S8liQ&%XhL8-_4wz@_KM0+Sahg+6L)9B9nUWd6 zgtg(FqCNr_7e0Y+7YO;{q*CY6JPUh>K(pB_rkBBVZN(8D(Ht;{u^2BH@rW&>43Xz` zEGbRaFwJ90*@UBlA#QUSD53MxN=$4btb}r9U&H`Rk>K+JPP?2|oge;I>5Nm+uS5WM z>9G7UPlp_yEA?;%1X_u$dEFcZY=O&jY!of>-Od9Iovc#S#-8zv$Km#6Y#u!YE*J_y zkXE}-4+;B$t6#kjaK#5}Hkq4?>P^_`BBN~RfFH}e?;m=CYyR=W?{{JFKcM`ac7gxM z(N-<(n$#9TKr7U-G!~l8S!$IbOqya%DR6gafJ!wIprMZ>X3Q2QIG?6pOHMF$2wm#JPF~!zZ=_9*?t1o@& z%R>|#Eqw90+!t3}a})2o{9Noq-Si;`e{*i(;QYNY6hC$L(Jua&hkE=LELT|g!rEIx zs~VjQOwQPjoB8@TzQDcr-j!^UG5`P|07*naRK(Vcm$7_#g=$5K=!P7?&ZAf)i2}p!7ao^O3M%x zz|kEf#qRiJyS&j9f0>x!&8PC(xBVgi@7v!&O$-B)2N=J?-_ybeP81T9GKK+!fBpPF z@QSzo{=Cc82Qxj7!0ho9et70taq{Wxj|SP*m|-dsY$gnu2&Z6+qV5Po4Nsoutc(q- z>UF;Qsq1+5Iqw4rw$9A-bU1fuCAb7@I#B_c8&#GQK#zs8_hPO~#~$?pW+_{@zK9RJ z|9WcGQ4(9mnvC-D8q<{qE1q-)cm8aQ(Y>%XR}!ZTT3F>$zVyNO@PbQUh&}Txu%^S& z3daB{#v=YgH{Xk$|Nhp?XS-}lJpy`qzD(@AW!`IR&bZ{bm`%%0xiS<@oM6|xf5*)| z&r3tEqBgqz0RHHKWn|rxdJ`1KiLu6#`;xI*bbGPVjkx)As2 zba>7mXg$O`A%}3!9&CTop*d{}=-dU2ZeMiW7$cY{ng^BFN(mX$`&!kY9m%cm{ zq6bq%$AUILSlL7iLQjtJdFuktWZs#&++(5)F&W4VfBjb<ZAv2vBLX;Z?oQA;^d zO0lJ$I`-|YlchDbZy)Er`)4`z^vCm&kGv6B0xSZj+>@1j@LW3D-t#R6%X-#Qy}L@Y zT&Fyc5W4}~bWZc047+mtSHyS9~38nzCk~#Lle2mp=Y?{PKI>8xU@~?YLty&Zk{L$c!7W zE%&^;_42eUGRwlj6J+tvz7E zt2D5z=Xqn|`wu!I6bc=b4_tXJZ@%V7op-yqy_8P9dOwnPV#-lTaCI9YrZBIf1;c9{ zjv@n008`~AE*6?aDxj6%vOq&2Zop)ev69lFlfTv!N}05iTl3)v^&mTT=mrSTiJ|hp zg;^}#f)SikJdQMYj9-cP1V@dk3XSa8qfekXhiYtE(_N?SG22YSX)T| zgOdmf$g{agHW3q`1Jz-1l-SRJyN|L?gzn(BTW!z)K7u92h{Z@qP3M|{ScV3MiVDo& z+;;DCMvFFnU$mV_r^Mu7p(5!q9n2yGN_s#8bn%W!j)2#^_`w76igqR91uYB#uaG-O z9Cy(2oeQ6htm%an5qcnwH>^IL>6vM6`ua_rciLmfQ_xbdO`3#%jdT)=i*ZgVaQ|3? zPU^Ae%mHxTv0=>sW4jtek!|bY5S*Ex;Bww$O*sEuyLOQ~*Q3|&zkdhonneIa49?|P z8(~D)F}8y&TP*d}d2UQZnq}N|*FEjCU8p*>#ZBjPM=2_iH>Y^rYhF=QW1C}G--NLm zG9d{%0@MibMyW04f(!hqnKd{I)2ZSj!CS#b5s?*|yHk=lg3KeNcp5;8YKA8d#Zs+8 zlzGsCed)__7B)PLYTmvakA_%prN;ZNJdd|_b#cX}KSYOavmDeY3PHnIKvKT;_0RFy zPyYp*Pa5KsQ!1=lQzDKW17(9XF;kPXOxF#&_IZAO%RQX2<*EG6YhS?XHK!B6sReKd zmaSLRx$)Xc`!{a9HfHM;dMKxJNQS!hM1DL=S;DK%emU3O_z8A2$EfdVGT5xKY|jX< zc)}~F_%hFb_4C<0d=h0!#Kd72ba^P0j6zq|qBeuh{rxT*+ZZHCn07fe3Fs)Xf@>AW zwa|fz&M2#%nna8^`0{5y!^{8RZJnx~W55rx-6v8j^VCF|#O`GNKsUvWg$Xd^qOUpB9C@U1i7y zT&mcp#OjUbbIOLNa{u<58L8D69*ptqMb-~;Z#>J%FMB-qef-;$fYLP7)G_GcZ$5f8 z&wKCN!*57Y9CxfHx7rUsFS*qYygtNG*XljfyL$Pk23K5q*j-UaORB@GPPpq;RVK$i z)AQQyZEs@F9q*d+!L(~OWqj)gS+6gsQ;Ysa-uxQseL#wO5OE>10F3)p1x#U!ThsTw zsuNm{+RB+*X*0i8$QM(zV?0|RD+x{OX&BobUPBbJOesZ*RASh_`1CJ1C_AAWu#Lk= zaA|k7P!UXORRx9f4V_`eM8sMqagJnqoT0`9%kl}l43Q;<>XfG(RhhLOM>7OQ3Wvbw zZ5pG#^yL`$Z%w-$xI3B9?u^u8C2LqUo{Un#>J{OLfHZSpGGdofGJ+Ao5F;cg35E)q zd4|drtPC*YmeVZ3(ArUkS59%#=_yG*!Q|LBe0?wC>O{*j?!4np2C7T>>_1$?$`u<~ zGP0Wbbdw7o|D-PMV+$=3(dnUE5crNhhsp6WDZ`yi@{p_oACj$fl3S6t7=+w9W)p)c zNXNZ#cyS@YrH~s>Y85B*)0{>+^!EuqqHz*!rT&3 ztTst#lCYbY?Wo^{(!gPf7$i4?)U`}QEOk6+fOsKRvdkmaV2mOLBClAlBnq*I*!y-T zr&WhwNYqow-9d{hbWY-Bq2;0n@OtGCoML^DUb8L&?W08!%XuNoJ$aUe;$AUic@~m@ zt#FL=8un?LF*7qumIY`@TNDvMgf(UI`}aXJ=ZD|_A@TVmWESq(wWEN6R3c0V8nWy9 zTESm2hCA=vPN!FB?ijRS1hq&+3G3Ib4T-3t+S;`E=ClKF8xkHAKML0s>luv(7rZIL8Cy5*ZUAB0S@n7h)Ib_-nyZw;T(n@Tre~sf*j~ zm{%(WuS{XLI>lO#xYqnqa_4CpaK;i>1t*HPf*MPfX9$XSIf;16#!~T?De}M<*yfcr zU0st4kBs$@)VWd=}TXZhGJY~Tkiyw`f_N}ffDY;vS~pgH`y_^jjw<6 zAG!O!o4D}e^{iYTF;Ih2Im1dq+H}-sGNvXByY}Y%&|bQ!nG(!E+Im zNTasz*p3BR19nd9(81u9rPl@#gxaQ!yzcjZpHIH$T{QBPSTt~VAzh&SU_~L%NUV^! zl&jzQ$GrS)Zv~ZhM}Qs+mP^5zh0BnQ>)CIFUBD`YL6iZMfGOmOjm4-^Ho}0yQZWo? zIsg8dPxIX0dtG;`r*qc~sOBtg*&WZJV(pDYiuP2!D@9V-1Ni`FpZ#mR_I0n|d5u#z z*ICwYv`i%#`Kl(>(?&SyW#@D2=fBTL6XM7*3cUO!&*JW}n^?c0UA{kGJbo=T#}b^~Q!j?5Z7R;^`eRX21`dcdooUV3OPTGjD3kgniz zchKW^*i|}q!6BQUMa5d10Hi=$zgWD@RMe_XDnNEL#(T(k)k+Be2W}ULPnWGFa zk*DN|K{dx}sM;4b7$f}O3HG$aifHs?5yJFkBnz zY5(o4v(^WNI1)5#Vo@TLDk%eGG#KE9rY9L$iO*))7cJ#pQ)3^qWGF_Yoj7hZrbw$z zqy;H~pk{yuHF8^{yk-TFW-Q&*Vjl2+L28k4_0qg#rJGX}*xB@=F zf|k{SG(oB$hFmke$q*kE^De_`O2P~krbrkg<}PB{hf1AUKa4Y@%nq*P><{OaOujjb@Y(hN6T455o~mS*1leW)Lhn)huVVMho^nw_*q?VIR&tOil$% zYhQ49X@R?^J?o=j?NVjm-n}7#q*_#m`UT9m397xFc)80dmn)164^t{77#q@1M#8)@ zror+RRmQS?Tzv6ks5fRP57&77;~qoqT^MHttAX>=&*@V0-cu=;dB#&NAhC-ldvuY1 z=Z@mxs`jy+`!OOcUq14nC;<-Tce@$fynt&=140XA;3EL&QoS`ng1 z$TDSeDkV!3?%X!P&asqVdBLypo3DOm;op>jsJ+L>CTT}O#6)bp!g1rZc7YxH!OV>g zB)x4F$|#J;QqHcN!-p>Y5X(0%XT!(_N|Z=Q3K&CE6p(enpvfGl-gXkgA7or*p1o*{ zxnR4;KuH}#81~{B-+l)pYgZ$cYF85IC@;U3-5o_> z4Zr%vH}lO8T+KveS&|78c}m$@A|g;F79r1-gq+b*iQ#6G|L?^(TonXm|aWvXTb3=v9jxaiL3j4owJN1S#B=F^b`4>ii_bIGiwT_s9BBE)J z0oC0q^$!>df`W4_M4WihWm5zzUFZe&Kl1lIPaE@Ii<8=MGp}q^I^nK2&l(Tgnmkgw z=?AYok2iL8l>fn}e$3TZJc_=xa^lM|k>FYo^;+i}&Mg~f-gmn=SIm68!3BS-oB3pZ zzt%yb|2n^yL)!oE^mpZ6iQ+1|bHn)lSxzsnz@*Ba$_l=_XAcvsq9j$Ebws9FRQS8l z9^HrZ=*t5XCy*zGtf^$=kQ;0fI`l_)TB&+m?l23s6Su0-0X6tMLufKW#t3!JUA~6l zw80N2_OX{TH;nJ$f|2!nYkZ7dG#R7HR4D|JOG!+9W|kxpd`VCjx@ByZ115B3P{LC} zy~7y65_A4zpM@dij4h8w8Rtho`UaIs#P+*yXJT@KdVQ8Gw_JPOUvTo~Gg+~0J%d9d ztX{LefDteR_r?TcRN@}J)R$vdO2FE|!O;0T#~pLUUG+7%{(CoI6`BG~OB=ejB?Bqk zd&}Ki|DQh~l?c_CC9@@#)FX5}BJu{C3%OCaXFpDS8{%+tHb8_BX@?h2F<=ZQ?MfJI z#8eC3x`+%*lZc%p?54s5CH4`qjfgQs#z9hYyM#P>6dTTeE{)0vGf|nzM7Xa!!|uT> zaN8+ZFC$DUF!Sil@GdL|1MJ#c=W}29 zC)TfDhcTw8>~uZ5tIjz>WFuCtTG7@Qk&TLtv}>Q$PN1MvD)E@dJi6V9;;?& z8&Ru4WEbzh?(P)VIbn}>aY_r?a*>Mk?&G#P0b9ugmcb9M`wv#HTgC8&Yl818osfVj zzS9j)*#6gR0Rz)*)6Bh+BGE7N@6TK@3!d@f;}>a$h_p`mFheT`KmOcsSwetq6avI& z$+XT{jtk}54LjN%g%;fP0`_MqPVkbGHyV)Ucp|(?mmR5LkCXu6+Y6Js@QiMC5SCo% z;%eIdw$zut^yTPDOaIIbD)r@Xq}9QtMRKbNI?D|=e39!u@lj4aZJ0C9t}!}Vrd&~s zWEhEP%);c9GPa}1?b~*6=IL9xErgKVn?n&pt^#3tCHS|kD~ir6{{#-pua4)ygzm>FV- zY~WG$9*i-C&2G5;_Bx;c{Pm>S0PlbAbu?#}V&&0zT|qe>B6SgeckMS=w)`wkG50XK zdKdc>kF2RO{7akA@(ka;aR*rfo1cFwU)l9F9>afQNpa3{j9?dZ!-q<6sJQXkUT(gv zS4`o2xg9o^huGTmt~p;b#+BnpkIM;nz3yR3a6R;Q!P1LG)AyQAL^-w+T-{9Ot%!m2 z2B(ZFQgDgP%|WT}@SGfYqIcAJ2Mpi zhDET(;O6oVX{S@tYer1QDEITJYIghS)pG9!wHrhzd1VMyPP!xi11zPTi7WfbW0z zI}8ma{KvgtWqkjAjE~<%QWC!UwYyolVm&8q+``E48dk128BB$cHc3FD1KQL_f_Fk6 zw8`|ylot4GjBN+{V|w9mTWnKJpjlNWM3^bTxCyVPU1PmOVb5rVZKEaPNGT^vIc?7p zR_}p{Z~mO}Gz{elY7Cjn+mqjTAt`Y0@nJvk0*b}jgpqwaSv4C`$}Fl8!XzbT804o^ z<-dq!KUVjGP9jWWWd>2m{YqxDb3r!K)FF1#Wbcq?_ay^-^QUULu^xoE1RMaAWENa90ZW9@5h{o&zJTQBCOpWe)qpS&hGEvA#VldixC zbLkt*LXeM(KmvB%G&!V^Jd^>~2N)=j2cX5Y=y8{jcs7QHhjNkRDv%M6JLjo~TUL8{ z{?dAbScF&%lm_r5?Ug3&(4s{(LCNrvcik6d;Uw2V__MKK31OdY#-{(^7Z@Oie~^#GY~z(M)0A-dQFmBKGW3e({Sb z-uR{uvVQ&9K(&DD0Fh`8_o>c_hqQuyi+3Kh{Te-VGa$rGZ0EPT4KRTyws_YnWC~Fk z>1q&ZK{^N9y}Pu>6}+(~5MM63JBRb_wkdooP?N-5^?Se1r{DF*q^jiJ{WE{hl4os2(wxYa+OXFL_e$kqy2^ztFYSRfmj^I#VuHDq=5WV$ z_}t(BkpJ}uA3<0SECc@p60-ty5GyrY7L#L`tv`|Xec&$M^0rM>2Y29V8FplrTl@~D zUR);o&C~da8)r%~^6FlG&K@4mmLA(pj!j7pUUrUJmM;1os|nYC=PNw>f@iWP@ks8lUT{Wn5(rd{3YAQ#e^3GP84SD;P(IVB8HmNjQS=!{4gJW`-kBC zi`g#;oNSmF4L_7CWI5P4%=;{Zaut>iwOpL2XM55@sYQe?fwujs29Zj^lj4eQ5as*} zo{i7O*>d{1K%JX@bR$U#w%@glX0yp1cixV0FT2LR%0(AFk=c66=;%t;tv|WQK)1NN zT3ueI?J~|cTMvKbK47E$ksKz_P=6;B0p;QjNw>x01B|B!8j>uAq%R(M~2w zj~#TNu+Y{k(w-o9;NmK}^!4!X>by^NazaINLGVs7i&Q6Rj~AqeIV0ykvqeFX^Ki+_ zU(VmY?>#ickm1QXRw7_9qF4#Q^|^Np*oa}T-1)PgbIuv3gY#_;)NUVo@n3&v)}3~b zln4wAu2(w3(+BmKe)unsM!iKguO*Yb5Fe1iANJQ|CbV z2uI((?Df0nWKtLTdMk#weH(o8lmE&)-uahgektfG#6J#WmSbc&o+|2VKnbLTu?aXx z^D3To{OCvH3=TeukxeBU$v(at?d0w?_pp6vFZV2|Q>`zhGCRXKjdp>T9_8d#m&Mxk ze14bC)+dm^B(dp#J(KmHFdPq=^^ir-Nj6_D&RYtS*z7uOc$KeB8#W9-E|U2^8t3Grz30L zX||F~Eynt;-|}tT&~W60l*rAnEQ#4uGR&&OkdPY_TE$f{n$t)#w4^j<9Xgy7C5GIE z0El=(SBwZ*=8(#RM#S#RapWxaELMd`;IzTP9>}$M-z$Mx0)Kz=KQn7gJK@xsw_)0@ zn}C;`{Y)xpgiR>ph(7=TAOJ~3K~!yzaeg2MIBi$i`J%<%<3;htkgIJ6*E^L-N|vUS zt3$z~Ai`y#9Xt1mH!+-X9;7MJvN971>`gM#IL4L1XCY|s3tF5J4auukI%#hGh0HP8 zr10&uuw;95bO6wEZ-Ngbf!eXoJCHC zb15PgF}C2tPTKCSXwc^#rDP(?ZYP7>Ly4a-=x-%s8L*nfjA48or2#%KYF{mQ$c!Nu zPadVjm9S>>td2;IB_Z}9Sfa%o^Ef;KTd+G6?$sCk2ECI>5UjNeyRZ`Uj}t>?67s@; zh)k!#cF}pOibyNjBL}^IA?Ra@^a_&MaBfP zrj}376~T@#ZDU|y5ZkMK?W0n->N>E21o%ckN{6CcH=vn<|wf*`;-cV;SeyRxACl_WW~YWh_zU0jQovFB9wigu17MM-=@ zXe^$jS)`yY>pm~>^{@UbAOGb0S-C3Db&p)a$VdRDMi43129(P&lSdOK4ms}quW2s7 z?8!Xy<|ni2{EJDwqgi0XCn%h$msB*X>yPb-+Pukf(=FPbW71{}=6Lf$W>-HQvw3z% zZndEI*pAn3TRgy%2`V+k8tA$3B3|^mH*)7&-$K7Afhsv;2@s6W7`D6;8X;CC=VOWr zKfU{htiSo@6uY~{u9MkDfIPIuS^KSR!N3;7(sfrbVIvO2HHPyADwP=`Rr1ng4l|ne zllRuDToe`9ZAyG{)BAbsv)7{U7DVa&~q6FU1Pv3&ldnto-ldNU)rk=LfH*73X=iRCoEVnmt zo+wFo0y5Y+J{*JvL`G3P&^T5)a^E?-Pb`M<2GOwzt_5+8bNRt?eC=Vo-CO6hEc{4< z%nB+9X~#&1%&Te(59x8%L7RPcikEb}&-XIJ0MiWNShsX3rRiPd+#%vZl>^p&?6!m};qn2`%CjE7BmP@6W1)ET;zott*R5OpCFfKhFDVq&0xk`6}bN zmR55$Gy&Y8uLd>iiHy#3$7nTJn&+weHa^4pGA01$Th8E!eHBX6nWv(}QIW%{Oon|N zDU1Li6!OI0uk)toZi@Bu#HZwwk_&-ji1MOS*;8_;W0xO5-z{& zCT!q%@&8!MuD#nhba*fQy_UOnZ{_DdzYj4HTej@xQ=j}7p8C|^U~0O`mFw4o7z_mx zVk{95WISbaK}tynr?~SI>8|>d**NsXHn*hkc~+nh7C9n@Ni2bYjVxvL1i2j3J$Wi> zsn#5;D**$wfF2*>@gyXv@h?KG384h2rf`S#Z!(RZ!-P;E^n2-N4(r=y6{YJa%qq z-wqCavubO8vsV@kK5rJ$NMQA2E}(b1m+Ht2D;|A*+ScYZ>$*$3=^iet_e4pqW^je9 z>)oG7eJ`l7)niDr`TFYU+J4p=P!C=OOhCTSLq+ObUD8s^>l=k7^$K56FkaA_s z*mr0g!Q-hNgEZCM7rL@wY*k}$F6CzJ`YI2@=YK+&;DynT zkAME>Ojl+IjZpRp1yW#pt4ajqyb`q}=05Oe@8nl+*~pR$FIqSTSF1~lFnYmSe(NPK z<$LdXH)Fs6Chbs@lYJ^BWT_s{rwKNc0 zL{JSdrbsouh=2K)!_%Q} z{{Ttt^&OL63rXH}>(#uvrQpVrg-=Y^Ha%Bm5#zvUmdKLULutTdo?E2O^Y|QJKc_5b zKhGQKqo;g?p@}_w&yMhck9?G5%^Ln|pr0g-hc&J>^iVJ`&=14pF;QCCY{GOA)=BEa zD1BgEYOaT?oSE?7iCj;WW^&7kNjp|NL?*x_9?4+XTvnNG#S=*YQ4MDH!q4yFiue8v zm>$RvagpnTd>_svXgp2qd#OkaLC}C_YsF&s;+b93_aJ=FGF`z-o%> z?7c^Q)|)3DjG`n!jL}Jorb5o`r5GJRYZDl%7$ykVPa>-+%Oyl=3FI(U>BreYrv0O- z*vB#BA}ZQLO^X@+zu*&#Sa8-fc_p*pxLEL_sQ5NtxGSweo%!ltMYB|;5t_lCOdaW< z!}JJ5rd~$c;3=D%@bh7=2o_n(QB!Yp4)fIaDW!$$(s;bhQ6)#T9!eroTIJU(jZSJbaQ| znI@;M#UoRPH81hNj-T?MU-~dxw||F~D|0;J${u=q^W-8yOhl3-Oi#xg8lPeRK4tH& znCCqEfA9w{dm`#0#2S1DsJsU&8#a12Z)%?N8@C%aY|JXRq(SdB|E*`PO8YaVp67rj zkbI6QbsP~GEaahFCeW03D@V#rE7`DzfTh5pIN{&_=?-4_+SfxqKif&}PEzomdH6Gbqpy4E}$QasFKS?B(BzG1D_o% zk=Yz|tz2nJYd2q=-~%M%b%R|Gz=!|o8@%nUA0(-bU>HI5F@!RN(_W19p%n412c!qD z5n9KJsaMyw;loIS{MY*gOn_2=vVs}l@Et$pyc@3~zb4^Gw2QGLOL^70mor2^vY?`P zibwfohve3VTPmE*F!8|hM>5rdfal%4OzgXl;T4yile@%X81K-^$FwE5R1a`u&revo z=K6(e|ND#?p+!<&(5pJX*p+j`2RYYbBSz-8+3HCiKN;zx#Ikak{?a%DlvyS@{`L)j zz_ahXor8n@)Vwxekt~q+Ff~(U6*+n|g*?GoWJUFxM~uZX8+zTj33<1 z-u*kM4iB?+%l!mlj&0jM!j}7Y^OPt5Cezb3Zn*wNjO1v7EY^F`Oa`|=3*BoMmhiOu zNYWX-Jwzw~VJ(4NlyWN8^{#qQM}Q&Fgg|TLOofOTOBFRS)>ny$nZZ)i9G)eJjN(-g zZwSm0B6E~k;fJP}aE zZI{iPgbgp$HWUPTko%6DRF>@Y+i-*DpT{4({&oDrTmFP{tww*wQ#FjC55u4hF`=O1 zsH%_z0X0?b_>;Ht!dJbfJzv{Wc@j6Dp5YUIy9+Uz;Ei=NHBHrgvYzjZAvv4&

BaS{&wJ}L295C3>Ey*fFs{L z%DUnheerqx-o-EGkzCI(!%eOTIs1uz$?o24*>Fpl6z)_`gopBw$>7)(?3#6K?@vE{ z&L+4P!*~alU%{>)wIADg&j(qu`nrW>U(Zq(ovtk9ee5Q;&Q%Gzx8R{zS0*LnlN=bR zVJWNPqlhvY!D+UiIFzbaqI}3C(9(QR9vWn9?0lvstK4wI6MzxG5~~nNj`q&fW`&7&ZWa_tH()n^ z#VoCEN*hm`m-I+L3fW;j7?Zx!h~linXpNlrh?gB$Z1^-0OUe z>eE0&2V?@9?ap1q_)}E<-43oZe!oSe%fa>Vc7!{MBZH-V*vyNje`N?zt$>u+yYGI! zzWG0RVB5F3@lkyYFDX#yRl=NNgCez>FfmbL@+cfS7<2!96a3LD-^oT9i9^(Q8 zL2w$^Z(2;d;g)GO-`?B)x~YG4<~2UZqQeJOc=cf2pllvK<9W~LeQ$mXRdp1A z-^Ed9$*HC697Qo$@tJ)AJtm;+6Z);8x9TzFD&jRO&^oJD6-|<}I#-m-=6}izm9)I0 z7Q>G1@Uf5H%{$)uA!>0CB10JS7>pT48NlcuiRM7^pgHh4&;TUBtI7QBI;on;d`Fid zX2c*dkYufLRiI1|#CU&{&>kg@_dtA*v57t|-aX8-u6{Guat(tlAtKi@Lv#UHb;ovZ zZMdbH-ruYf@oeD(42-U&IP&|H4!y5&Y+~0Rb8yE~Si1U}b8e?t4C5Ufy@=wHmr)x3 z>&CJ2kq@xvXU}2fBc8BuZ4ia`Y`l_JZu)WK(5vsbr@;%_m2*!PeS)hM%H^!?!S#X+ zlxh0dP9MddfPN{lqd3UR-usvA>**tQsnHmkl=sD>sAx;zv8Ba8Xg$((6uYt@M6!x; zLmH49s+L$;62@zL$YYqHk4cIM0WlUb2seyDO-jhCvB$@*#SKW4y@`=*O-mL;PG`Ti zEeY*x66!;>u#cyJ3`j~k+jb$UjE?rv+p~fzF25S6@q@eXqNmp&vV;Bm_fo0EIA{3e zr|)EV$x4QYmvMMvnn&Nb0gFYXklq^!sS_$28`8u%>*|A3n!S3AFWT5d1n}zdc#N79 zVoV*md`Ka0DNRfhN`&!83H>&r z+lko2;m|N%Tun7DQzTCh6BMa26UOW;mdWRr;M^t5*bBJsanC1omr(GRW}HsKK8XqP zj_E$A797P8R%Qt_8Jj6qj+>T1|^lmQ-u>nV3EQ z?>|_v{+bMgL*^LQS)Ak6^2^VDHvjU@_p-ZEp+AbK)nY;s!j{6B04N|7oM%uWui0yQ z?3|D@yn;Frh$x_55Ch)B{afJUAOAdWd+P^@<6*?F0Kc9f7{U8~)E7~Ts1$)>+MPu} z%BK}&akn7zxwD9zdg@HdNP>_cQUOXRF{)LNqeNkexHgVq3a_J}fzh1!1;LmvZ z)1F5kC5E%)mQkAxN6!A-fHnxy<^v8FhF%t7{L|4Y5Ms%bT(*YGhL@sIfxtmht71cgH{gw6 zREa#;2IH2bxX`q7aG9=?-j%byE(L=@umn`FR9Rgb=Zan1xu!Q^AQ0}FIK+1;Q=y-j z0Ag?LB<`^)JPg%k+q$+P5rKL1G#}?#`s#dZ81%q)>p(6c`N{r`EM_HqkAP zr@z!ev1brSP-3pV`Zw?-TzZk<)$zUWeUm~VVE_KD92wujl+Tu5?1xQ+{dPvdOCDWgeZ@T{$*_=`k&kvt8**SI&Tf@6i15n%nN?J-4o>GlSu| zc_Ts-5;MWKzWaIp`D1^@s#O8&uUX2_P>x6{gt-(UVsffXsgh^+o+{h7mU+abkL7vK zd^*dQuLTOJy(n4(sjr*ZAj8e2q#x#9QC; ze$Zv8ZU7xb86t{CNn9?ots)>meT0zD&S|rxR@gFkJv4g4vcDw;t)WyAO2AB-+@glC z6US}>J5D~B=C!YX5zl|YFEe%_RCyvHF%iAlIsuU^sG-t}e#*aUDGiz`9JC?4weqR1;W#$O`?)xHrOD{d;Busa_sf?^z&+wY} zaCrMGTNLROKl|c(&cF6wS+VYk*f2M*b4?%G$-XUL=D^SY81=ij=*AbHX7$km5i5p2 zzx8VVXcjN%!;KO2;UWvTgX`ENm(D((8PIx)XV(s2cf5A&)PBo4N)U`Fs6)hLhYeno zN$sJ;05wyk8kF%;U|$%~Yr!}O^Etp}@##AEf@ZeStQz}nK&&gLy12b3pE$hKA)kF( zORfg53Sy`R4yy)LY1=f9x2Ea|rh+QsVg^ZATRsBi!wiPX;h_up#gTx$R4EpRs8(i} zb_pyQ<&tz&#inf<{5`uZI;?+_bOw#%5tQj2@(?H%KnE?7fYiXf^%^$^u~ zCvXT)Kx_ut*)ziWSHFgGGRc>I_*F({s+^w$d}*|Y8(#EOcAEhv`y&#~61)yVNed5M zHh`RkoQFOGS6=~FGnfH)(8BVN4t)v3^v!eAmkxio-@Y^D;-2d6etN%EI@ltnt+F)T zx@(mRTX$_`j_oD-2Q(R+yHB zAU@qAl{CRs{~Z{p)}}FThLBR0s>)^OGIQLimUX7xb@;Rla=rQD(nkiAMd1j_V_U(^Z zy7VfZ`jV%!YSj&y9T9LkdhZ{M2Hmi+#O6)K#?j5U7um3}LZjTUW9(#c>{p>p+kl>R z>n(im-R}kG(b#1?n9aUv$>4*%f=iOrW^qY7_IC@~h+PP4Owy$2nyas-)Z5FX^Heb8 zRY`y#gTJ?`6vR+a=(A}dT;f~vmWoRfPCkiwJSe^s8@-xY+V#_@rVxbx{E@qO=Q}z33SXF1wVdI7ko{@ak~2GUdqw z%pBRlp&egha^E`uxa3#9zraIB3xB*K!o@eegnK6Lr9APWw$~o`!PD7u?-Loj^d$_8 zU52lf@oot99v4@LYb9nT_AoPkKZkdHfzsi>J)?8p%$mioyzOq@H3u(fH~hZ1Wf4FW zrcyyr zv9&}FM3XeYs1b#HFM$nk4#av~W+0K_kSj_eb&AaP4X0iNZq9xTtSgHwpi@X3cqK8A zn0k;$5EZNma88*n#FRqC=KQ=|HW7npf;E<6WttI*33_uRJCD#O;3{QoG2(q6{S-t+ zd@hfwL&UMKw}M~26c^O+VU4onsM#FerU5{4A)zR%1}v*-Pe7zvzyJ{eQCkPs$u=|1 z`zD^&lN?(UB?O4+1@b^Y*IoM@z;nfA&j4b+``vFbFxbnNcl|rn_z*jGA7aPuZ*ySp zZl3V?Utw~x#*(G07#n*8&<{i;iRpmDulP8|L=m%N^7CSkm6j**pps-~as|wBT$&DO zD=e%CL?{u$6GoOY({P1gxVK1oGGernV_7-EjC%Gw{Stoovk?v*UO`gm=Wwh%-_IiLq*8R+b7NYTu*Q<5arT_6rC!Ky3VL4j_W(7ku-xT zCJhLAuNm~TNkb+^Au`lr=p7p6pn#>8S+!M!ag2V{u$T5Mt-I=j3h zJum`@ zZ3<855j7}+28NQx#DM`TZ3eQd2iH7jrb9<&`8cfsvaEAx#ii@mgJ;Zp&a)Abiy4Rl za31j+iHP-lacM z!)rNBYLVczM%tk@4Kjrdrm0o;AhHLM9lY_)zmNBax#fkgXXOfr!nDxIB`KPdO5L}V zV+~6=3o5SVp{T+)ZtsvVH)M(Gdi!lmYscK#il;<{J}&>Y4|4ApUPf)^6K$`R4!w`< zhu(Ki?U{>Vyg^vtvM0QYpWan$|Y{TtZBOH6cDQ)H2v7FLihsZT#Ql_)5>) zo5Vy(iKQg;F-4h(iZ3v3i;PDRJwXVS7-tn{g}^FS9a5_jMQJ;Ymke$VAykiehpjIx z&3G;8OykjAY+X5#IuC*K`unpQ!qmK!rn#HiGI{vi5APyz7BQyHkJEW9(1F;~AD5`! zekVKM{TE;iQMtfj4WQWHoXZ)6#bh214j5|rJk^}Vt4%u}O}dWrwU}P7D3+e%7-@@Z z<2l|H);q2O(j-^A+;GEFz*o5B;zs~we)M18rhlNozCF9S`<@>Wg+1)ty_fy_zQ|2C z{wn2i!jh%SS-Em$W|U^g>fU3*TPp+yQtO*wkRY-7z zCQM0;^XoWNIlvSnnEV=c%L=v?dpJ5I9PI63WcdgOW{$FBFkwoF;E-*5*1_ zcbYcrl4jD{S3SGFx1FWSN15oKX7e|`&9AS1TH3v(873yS&90eWJJ*99m!5Dfoa`&J zpY!bcPC1E7YW)`P0CT1yLMM6uyrM=L;5GFX6@->92_InBF49ip&%W4qFoGpj>8E;(Zs%jM6#AGH;H z`79Ao#<{5sj_`wAdT}q;T{BEipCMmx1d&0-a`Z@rnQ0iGsB-Tw4zcdy8+g|Ld^W3B zTnY4^7L(@m_$l?LXOH3HN?)!5g+-}YRh#i86Gaf^xbC^n<-`B5iDKgE!Qj-TK64{3 zB&Y;TM25rp94~**J0XyUV|{k-!fb@27VGSS3whQ{U&0Um`fr#3@?xks*XZCfS+81X znwTK?X7!b-CkTVnZmo(Kf2K|S363+3aV{2bLJ$2#Hc01{k#;+FPJir7n7Y#L@PFg>W0TbzHco&hY4pg^ z#MxdTk;k6F<*a|&hxpl-S2KO^z32R{xfsTq?-^qKlitn!-+lz+_rLT(ovUxzYgv8G zi%viPRtK;3J-1%XD{s5|IKFJVUhBr(7pE-91XriY)>O}Rm^Bd5tcq6KnF-_s39bwh zMX%x^9q<@|{wOoRhW*ofPY?qPrL zD8|9yU>}pyQ-o1O7{=tRQnW%(7QFD;vJnxiq!?6 zvUTfsvDUMF`y~JNuX}jXlYWg-$#L1`*Ml7d`hX}smv*=7_Qthzzi=oj z9*bbIB7~I33LG(r!zxUN!YZ1OxcA*J$aT)LqVZt0>Zu?@Cih#8N23X z;GfSD+XLkIE+tISFk>83fo0klipz%C<*UTGev-^_a@qQgNIAh&d`4TR%}}`Nth9j6 zW0IX}z~gl6Pn}zSWERZJanoJ;(5%0X=@n^*v#hfcC^k(~=M4j^26^N&H?VVh59=Ov zMKg}q!eL+UO1se-t4Z^A+sv08OLy%Niv1}djj=Kn%L72g!1X^c(=<4Q& zI-n`iy~kbY%9%1{>-)rP?u$CFw5m^`UKg~r*a_;Od0YYY4)s+IjNi-GzV=BT*mXD8 zUf09Y;ee4rLl8zN!c?isOgUltXvD$&p0VYZ@rvL3b{44&0>e{Y@B%*nSO15@K4HiPRFjxuXfq0f=Cy~Cri8>R z>KqcLduOpwIE9PIVD#I%x3Xf(tmsQsk*x5tmcX{{@QHuEhd00B{{tBUhETnU(2N4T zczV)qcPXNp1M+xZFW(J-0Kv4x6jBJnH1ZZ>sG*JW-x8P-$~3|ZB9o|_0J9gz0m8yQ z-u$NDX?eWtcPlv9N}mNTn~wZ+pll#Y4FhK;3q;HRalHY<>A(|c!kedh`T ztbfY87~k=HcKqNks7`%&-k&Xw{5~r$zlD+W)-M{}v>3)~gNREvyo42(K82n4e3T3u-XGyNWgVpr!*fjC`$%6Bj9h~5! zzx`YEZ}~Cn+&B;TGQ(p9KK%KA=iZA};}Zxiga%v;6Vnq64&)dtMr~A?&ArazZAwGw zGN_wwoTDqJV*%A`?d)vdjMPZ0<)CUEwZOEmkG2`woT-A~T0(!3iP#}Ee&dhd#_+%h zR2_tacx#*HQ5|AUQV7tLthY|x+w5#O8Jxj$etFcptek+&x5u0ftXz2|7o4Xk6)t-5 zTK4U`j|2N3pucYoGt-mY_w%3Qz2z78ZDG&uBRuYLPs2Ik^7YrzTGf^sv@Mlx9ZT^t zFHO#oMbR>w4JqtMfX{#`n)BR{WQK$)2tigv76B?8B^pl9eoBBGGCa{KNP$=4-)H)V% z@}cAF(bIi~GXdYCr{^@Sfi0y4Z!m#@)x%JP`@X%Eo+Y`|V2M2#hcOQ01=MDhk$&!5 zqH`s&>Pf*?;?cQ~!&`nDx}%-2v=tp{uvSgsTB@~F_sEGWHjSx!`f|z`v5TaJ9~*>? zJA1SoGZk3&6b$3#8K$E=C2J4~@Ic@c6FW>Lq*uU}9k~Q-9AN3eE?vv?jy{BhkQpqF zTo!Oi7HN>MD_!YISLWRDJ8)&c`nJ1y&#hPCyd$uSVJB!I@oVg*0>lWOeFuKVzkT+j z?AY-gF21CnE3X_O3}-0j1H!;lsX9t!;pn7g*X|h(9jtKkZ~hKfU;Qf>GYD8hV;5*x zrWLHY161MdAsaSUQ?4*+8e9tfec%NAZuwmfLuUW&=HP^G` z{5zOF{B|bx{e;Q=cT=6&MO?ZM7jGxXT}oJ7OV7|1^enlAq2-rT>>oYhxd%qqvSH)N zk9F*#M>BSjK5WL@N&1$o;j+iShRV!~nLKbm6Z?Kd<>>vCj{X#lcOa%fP`H>}&l-A1 zuB31D5(Y*uBFy!i(YVj?-XX>|-a0$B(v`(03o^mgI-y#bsfxhmy?t%rn^y6;bUXX( zy`x}SB&sq#0C7#R zCM~-c<1v*grsnDIDNqNdY2_qs?O3a04|{zGc!j$1@NgzGtF$d3>wOESh-sL9E^|Kd z?crN%^i`+=E5hjXXgV<9zDbZJGU}CY}mfz`{Z(lpMCt_*|Ylq zPk!>#IeN6p^^bfUDI7{4N|a6&M+(d&L%Nvuc3U~mLjd^LmFw-#@}1u==WBq4Yo68b{A?z~F_;PqU90~Z^Nf?|oq zA!>-oPiR?KHI|OW*oK!sZ-BuTUwUl$a`x}v&yvAa zjflb7=(IUejcSrza}Cdou_>@kXsKC4Ra0mkk+e9<#3@H!K1qLaH85&Ga*!WF!y#(% z6d{Iym^{-sVya#9r7Kyg(Ny11JR6A%ixm)6J*4wntAb3;_Kh| z6d(J<-*V9_XZu=kye{eUS zdH>(hJ3S2oHn8lCBL3H3{vUEr_!Y>75ad#5VmiZGKa|v}CuiMQ2m)UAhX2i7@BA|+ zjip3F-a86LvDyUn%Q=`ha0ufE5rrg)QB_=$;F1b9IR2P?-F|D-WTjo2E^$a;F$jsm z1KZ)B?)(DCFzT*YDxeM{ERw`2`m@w9V)KV2l_&G!tEzu*Q&FSr5kJ=?c$ zXJllAfq{Xu9`|XT!|#0ImAvMTA2zR98; zIGS{po#RR2+p5M;u_3jfKooFn=GFj`fci$`f^S)QZK*Zs!gHS3maM;+bzsd_;|eRz z8)3zHRn}c`12D;fefLqWOd#7FNdnupZy^kO`1r?eV|3Yt3=OYha;nUYH$DbyO^Zg# zg0gDp*u}L7NdcRUa48331Qfgy<4JJf!D3JpS91`9C!uB?(~+YR8e*e(D`_`kmOpUf z&`?MssA#%25Qvi)3A2u@jE~v~IBTf+v{TpxV0>ED=rc#tsigG2P6)iv-)Mj-}rI|{puaN+lhL61eA1JOUa{k&8hL-jc6ohhlnjjGJCJ#PRSZ)(C6zUsDnc-O60J%s({r^{fp;1y$|EYgsGiP^dHKEA&BQ|#G$ z57%6~l#!*Lz8*&ydO{nLR4oS%RG29R9N1T3Z1f6V@$zRfy7UULQD)Ah`>M1-7cZRD zP}_FxeHQieYKe0)>u%CTu?^}e;n~uHGUwT0e?@XG+6FE+#v}=)eD~Ginj1$3S zWTXh}EcS9}nefEVeSwJ|{+LIvTgP;vz>jklm-sYDhvL1W3OTf$ud!t)^_Y-ki)*1# ztaoK`x|EGMG%E?*NPI~>ahuLnm39uU_I1)e8(ZhH%YySYjj4K60UzD|E%k!p1td-iN+a%wkG z2%rAsR~cQtioualjvSiiF*p7y&vY{89nCvG4icX2ulPDM`*sSx#D})Np zBuwWL4oSj9K4iuiDp3mW329QwSxXY6ez1h#qZnU^NnBdn>cr!$AwUuDspMj+0VIKv z7@tAI2|nl~z99+VYefv>8amGYM*s)_r7HrEn6*_!Q6g1;enmjVWPriRQ z@l+}8%<*Xg$&2ER!&}ArfRKn#M_JXomU5}Yw(tLdT?L`)O9WwnN}4R~?dc~;swA$K z6}F^M%NJgJQN{}jswVBSZA~yChzYR3^^g5!;)cV}p?Y;zg@Gbmb;}EAw(xdn51*Ol z@PUI2*O{~Rn48!9t~h~ejQ`T-ASeM)x8e zU-f9p`LjSRMW1Alyb4}Q?ArSizVM|_vSr&3xp2(@mt2yk&=b=)0O|#oC=*9296eHF z{E%Viu9&Ai#dO&+nLR7=73^AZt!dK_+GTF1M+gRC;RPmG%Ng)WW zOd=d5@#DY*VQ>Is2d{bEOR&K-&wcJ6v*tn}v?(w`Lsx0Dd#h92ZUH^uIrVOXL$<@K zH@#pQXTvOu7pM1CTd_8~QOK^GqQ()yIfpTZD2hmuB>W;_l;`S&CojY^n`1& zLDK?w3svJt#8D5-iB0*8Tn5=>F&H&O2FlWELS>GvLg42}1b5{GPoo2;k=7mKdZ0Yt za$`Ne^r$Bz#9Vgy^}rtJkq?>3IwfFJ;-X z71SYBz-FFZaGb4VE`W}?tOlhsz{ly+jIDYWRU#42FAk(hsOHF8q4}u|_miWKm;z=5 z+z|znkTdBSy;q!AtQxGb*a95xuW|42BsM65jc{2~L~s~Urbi*@gM2~gjZ6y1Yam1k zapV&v=L~qCLKzx|35D1t*dTyfjl2Y!G`&19GSy+yR)$Crm^U!(o)pFJq4l zR; zD~gz@#Z;}Kns`F3`{GKV5LtuCIDczN45mo2*h3sAbSiMuS!T5W+qQZ>_KDAt_yTYH zlMmr)%RsNhGlpdt!ywKTfFkNsJ}n~!RHKaJc6QpkS&E(}wiKa?NEM-mQbo)(!cj!Z zsFVm~9QAuJ{xHVQ@W*d>5ifY*O{_dG%TrSC19Rju9Kt%c)?6H0i$fpRZAs6!X2FSW z*f^El<9b~5M1FQR%HNffh_Q%Zt!?<q!zy|1aM>XVEQHi(hTppF~_3FI?Qq^_LobC%=l zImtL}TGEO_kZGF;$Z`Gk&jdV|Uy;H*eE)l!DHJT5ciqLr;oZzk?P2=^2?xjbFf_1~ zi`HJo$jB;2M=t;dP+tHEW(Bv~1L<`f_Bf6SL)&Iue)|-^)ZHqA{Uz zVQP!it9}8(DE)krWPh8cCv8ZxB(uAy8gPtt;^$7_;4PJxjYN}atVz{+pz+v%K^+ni zAhahi3+vGf@!*q|@@1VjyLxNV(X?LQLTKP^f#9v@tFq$#^qBE@liGAfJ)ub(**p*i zU}KPMKP?suz*K$VlNJXExESY?l)J+?yq2@|ZxHQTe_iQHR~D0}q4U^MwIdYa>o?ua zyKlWRd*#7f7Y{z>(8^QksCS0!-}h6#eAma>z2}E)xN#{11H#}?Kw#4j-D<@#IaOk2 zx`+Myl=D|@;Hgh~3d>hs2;?)zC*7A*9k1EmY38VIKij3QdTcaA0ppS|~vlk2SS{omi` zIcH|JOImfwYL;Xh7i5faSr;Ifm`*UJ#kNdB5>oH|{RrX0g^*wpQ^I}amREkcA>mr4 zn-+2dp@eGI24jM-!A-V$@7g-$oag)d<2h&M%Vh zwlVeBl)Ak)))kQn2KCJBTCJC?msasCAC8y@*R?4A(>9-k@MV>aP_VR$t4>F<(Hv6= z#+Wmd+-jY$^#vxj<;sl!F+RyJO$n}i{!cdq)8&me*AVN3jSJ(b*-qrOWTN-rOH(9S zAhAGfnp0ruYuDpXb?O0=AslGks+ljH9H+uPnMfuy06H947gKbpnWu?{iyOC)M6(@U zzT3(ugf?U-80An~6BO48v~=4U_q0i|}>wQDn=JoC)cY}>wz?K{54 zjo1G=BSSHB=PzL1yhU}l!Oy)?@>)T{!>)v3NLGbyQiuAu+=yenPnu!7lg1&~@}W*3zuD}^#%J2J&=;9_7Nu#oZ8 zwVoq-yxpeiS{8OZ+j9v(B}oyUAW5|(rGwVNXkefaG&m!t>`ADZ^E_*4_#lWGf*47#@i;yG`1G7qRa@ z?x%?Ff{WuAN9^N$YV*S{M~)n4ogY&7p_$0{9b4QaXXxF6i(^a>fg54x&ZqhMzkQMI zJJ+*xSqGO~GK(-6q`hP)1QE5^aA=^$;9$g_J)>;jKE!KY{dTT;!A%&`i!bPrdLc*i z@hdIt^C=h+P?#v;)^~f2{!flCP4wMdUW+^(&c>*^Mgh*0`H0P>pJ|%Qh-tF!(fOI zB1EbvWsHn~3?n*-FhtGmMHoWZi?RFo^Y{M|)*j)O*I&)5RY6^?%hU^)wC;`nwsKgRvdub?>?n#Low9 znqo>Oxa6n@L`GO%$8Yw+$;ouLX4%oE0%em1_?q5T^4ikq4`{@q66&2?TOr38bK9EN zQaf;AM|VAdp31aNWab!}lgQ~;)CQa8S<2^kvRMow#iT>SUng{yAhw7zEb2}u6KM0- zvm{cnN(jWxQ8Iae?_~DNld`G>(9j23O%Ycc!Gt|ramDL^n2RrZ1)!{7{|h=gEgLs| zjrHrlM^9J4u04)@`}T14HPwKJ2Nw{?g?iNZ+VvP zmmJ5c$tD*RF+O=Udy>CO$lKUCW0XU4+6amexwNtlYz%D{+J|Z^r$BphcT#s_R;|jM z(Ena1dtcIAHtN@#PFQ95EX=CRTbN9c_-tQk={-+&%`}-*hOgc8H5SaA%eU5jgO|SL zW(Y!ziEv3%RZ4>#n1D*0FclhzB1a)G%@(%En0cS22huZpAH<*;US-V`_Z*F)P&0IW zQVe;ZX#?VDKvF3DXP0kw*=f)i+Kx6QVy0H%zxn@}o*3Xv43X#i?l>Vf2V z7H4kK&74=0<;Zb*{SV3b=6Bz@hIeOzN-nD#Jg#Hl)N8^tzplds7UDxZ^zgU%*RQW* z(ULZvb4eS$y&=UgA`A-P9K*w928M(^`{C)Q4sqcHSMi_!_)W}Tu%^jA5;4<|a1PuqH)9d9sJrsCpD-k4_*^48k* zZ+IP_|8E~+HkKt4P_EU85S&pT6G*xY17|2kLLrLK!NZtp#kV;G@YK^4zIfmL)M~xl z^_L$i%i%S^kM5G9Wpark5Rgan_?u5D|p_Sgn7@XbI<;hXjeQq@6n8yD41doqBd){@=dOmXJWk^2%r#*t{`o_nyb!V&F z1Lr8l?xc^4#>E@D2$Hh%X!FBOk0rmiDB@z&#Ta#Cf;l;-XO7cu-;P`SL1rGiw7p%% zXx=3-iQb5i>UMby8AG*D<4Ajrp`xub7ieNs78HV5=_yA9SVFQrL(PFsSFZ`namMD0 zCPb`GF=1!o8EJ-+0@kc~5ir7v^H;HF_mdnxxP`8+cD8Qa%;QfyMi`dZ@YF_z2EWZU z*S?71A;;xwuB6eXBQz3T=~p8V(g>2DD&t}THRuUNrL^u81?sSq(1@uKMuCAsl|4Oe zRJ0KQ2M85HRpz^xGCtAVomp*Nc;b$1e~64Zo1|sul+?pWlAkT7Dd^Kc^?>8Gum271 z`|f91v20mlT~!fl8+)+&rBtiI?p=PGn+S?FV*Z>0n5q}^49%D%m!UO@@)^1X;uAP^ z$*}B{3`&im?({|8p9+RdvK#BUVFfEwP0o<(OJ%h{E*z1hGX1 zXveCdEi6#2#fg2XNeD1YR-Ge9jvUk9|7dvb;{y-xyyp=&RX70S_@<)I8>nvP! z32_tv1!5O@+03)l-f29=Kc{bP4T$Jp*Vgp#1NR2Wclf}EKTfSO z8yC+bFjpcvACWHNxE-WKxfWne2e<;N&HkS6&~lB=IdO?5ohHo_5u#NPudQWd7#PHu z!&IZ)zH`eABC?ae_`qAa^$mS2TMoqls^rcVZQC|;{`o~7{@DXOwc+R3pvIT}`Frfyvx^tr z@Dk#h;nK^l!FP9c18oU?Ygl)~R3VPt8Q^Yiv2I)=!KLC-L8F#1SWmU7;F8>+ODX%4 zByv^YaKZ5X!4aO1AqNfaGj{@Vy66@yWa6)e(Tmt!9C#> z?Jv1B;ZkXk%xX)c{MiYgIp=(5%dz?lR=DY9H^aWYuz2xtzn;Y=)x^|0Y6_+?6&jWA zQ)eEe4Qn$>ZH$f%(_ScGqKGh5uQTtQFKQB~8K?wMEn0>OaHN!cHi2&DvGV2bWXr$3 zpWb*A7Dqr0DB~DGXv;hOPstXejSj9HrzN=2(b2*1@Gu=69r;Z;cV|QWtWvyauWL~t z6yu8k)WsO18An{6b#tIjljJnf*;24Ic5Q!@?|%P2cJFzV1q*w)@S;vSyM?e=p;#<1 zJT$mh+R7>?AcM}6|a0V*Is`MDHnwef`ke#NTfR*=B##PG94ro=v!N<>u_a} zQWk2xa3?QhJZ&Q+4ZmAbKf#3j_IuvPPw)OPqqUf>pulJp5vunxoH`VtCIxCi2P2hv zL{T??_rHF{pZ?kZOPQHeGm{(thvnu(3{r8SfZuPCL_<)S&@s%8>$JU-|K3r?-MEUTYm+`K99;yHC zo%gKgqj#?HtQ`4pPFqa>1XuFih~R)ChQ%Enb;_rcz5ZCFGU3I{acwgn8iNOf=v1s4P?C3`CQi_yWWZO_EhA5E&&FhnwWRNp5mYp_$^6 zolTMoyH*ML08Ms2q0KjW3H0`MGpBb25V2y#1?<|hfjxVk;n1Od)S@bzw`?M`Z9M&z z&#-mNE^d11t2sPS;o7TjpiWtVV5CG4STc&nxL7gv^v@A7;sX_lo`MJy21@Hnb)v*3 z(;1me@;G_>m4ac=cuK>18Y4-D)vOY?ShI~gac0+voU6;CzF8$B=KMIT0LsdOh;uV=@eHgEuN6dQ)`=u0YNS)|e<d#B2=Pf=&I$x78QM-oCH7P#YJF#yIIzvc zhpi|^^A#~Q3}#7Zhzf|bCKJ)|Hi9!?B)~bv7>i*xi{{N>(Y(t6A<|KH@7}@Q-8+eO z8#CvO^33Lk>Ff;o@;^Vw+`03aIeP&ELp82^-t|~IP};z?`|Ft&??H|-N9DxtL;aw% zhB%Ihh%pJpgC6B&W8;VbXA)A7RD-L|kS@eYVTSMRHN;6WIGQ9GJQV^~@GK;xr=roQ zEk5knEMpUeoI~)H+Qjggzx@nLE}jirFNI|#utUjC+n#7obzlMqQpJhG86k)RX2hM$ z8;)5ra)_CwHioxtqhrMqFoJUqW9)>}S)16x^|Va@J;~uSKomn5hSZ!;twLKtrqCpp zaeA%-bchcjv;*y8sHylArh0?giSORRdNvqiO+Bcqk{I1iiOOXl#t5hPaJpGa* zM~<`0qRd+P+H|HhJ#t+M#uSA=L|{97|IQA9U1!W7y*gs zU)MeEPaeG2(zjM~BAYXbLXpl3R`aGm`4b-a$cHJ}c5ECnz#MA9ECy>Y=1X6R_`+Ae zOJtU#(R`FwATkd}FR=zV7Xk&K4Us~^PgEeA+)C|IP2*(LfEr3Q`MC;O!Qm+gtr;Ya z_9ONHvE7fgJNSz~|1EC4^(8D_kuc2|nz|xYVBhwVqGQrida5$kTG(hYep*iu!!lQ+VoQn|@ zr%GUxz*CE^7n8h)APA^bs`ckslQ%u)I6Z6RSZ4P)#U44n#bN|Pl|+xk9|kIo6tIj* z8RvDq(_{uU1sG(~!HZ%DGCqwl2}Od2btwhK5l9aU7tCbwf(ucG5k~pR!{4F1+Y(h{ zwr}0Wz+jo0GkoC-pJd*=Ma-YKl!N<+=2%9>y*R&q${lT1jpssjLyihZA_Z(V^L!Mrc{NNz=S;hzjN^8tNQsQp3E}u?>_~zHSZ?Mkz)$@BZ;tk5}p3ooojeU zCa7eknU?ps&hI9wyynLn|4+~F8asA8!?(Wuuk79T6Ry8;8MAsr+B+P@q7Vd@T4Wd* zj5xT@uzz=r9ozTuikII>-;J+E<2Jm?cG5*MuG;Hd-*6pJKwUuJ9Ygfr+d1wj{p*Cj zJ1S%~c*otudSE3#6OV1aw??*yFug+Tzy=|Uo_{ktN?)bQ3>J3GVfUb+YI^y`eH(ei zE=GA3E?SCeJC1f#+i;W+u~ZvZ?b>_@8|#Z5fqOgep)$IE-=#7(iqY zV-8}A2ly}V`5lZsz+2vQBdgB$)PrI&DpN`9VWMPtt2ra5e1AK&<32tgtcj26=wCMx zt#vTQZXa6+m#?iH=QcbuPxJ5IbTddRP0~G73 zE3G;Za!kD$Bn=w2rr%9)kOY%YIU!9IX`-{`(>&10RnNZ_h`4CAqH)Z_KYNI-t~R!B zf11HVdl?zp1ybQNpZhM07A<1l>?QQhT*%z{EAeEpT}+tzY3jgHUz}iqiQj6AoK+~$ zO+Z)4(o<~rL2ujQ@(B(A2Z5ghqqQm(YpG<;Cxc>$n4vMV;)pfCsvyjy>Dqzzro%ZK zTo;bUYkg|VVg*=v{t6Bqtg>KMq2(GrwpUKlo-WF0nw@Gf`Rtoa9Pcr$NG^^sfo-uQ z6kG%{3{ru(LVWNL`w#AC$L^i%+`NxVufKqe+xPP3Ti-^tFi2r>8wYmprc$j@t<`ws z7Z35|6BUFRh+1$Vf}s$G#8HIA4ii-gTm>{n2=PP-j(~82yOVYV`hK$0lLVE7a?>>5 zzhlv)ALE|ez*38*NZ*n+&ctyxW_BGW!4)L-x&8eN(S14S5}0WY{=2HQ5q%o!Hu z45xOY^`a3G1QHOb;1D8XP?y_?r(|d|x&S$HqbDRRD21ZIIpyG? zVO-S1j_nbfo+)$TsvG%(-+vj4m#zkiNv9bTp2tl&h-2_^WLgh#E4w?nKS=>v-yIQ9 z9+&9qzt?MZ^{v%b8+Jk`Yf+KuegBTfE^YOp>Sk(lf=h_&8cdU2*NxH4j*=mI`WeS} ze_7@6wi|h9u*9KqfTKXsb#st~)R;>gcO%Tim-!WN6o8^nXsOLyH{~`;$<%!!*%!UW zKm|h$kr9HhMy)!4P{y$l%T~l3206%k-}`&K@r^HL=~C}k+Qi8+W6Vsc7gL(XeUXW0 z#FS2M>Cye9lxfE#YeVV|YfS&T);?|D+FIgk>m)Cf$0c***2_dIAom%jFM{blDNnbd z)8OKiaSK3C?DWc!OvNyhSWqx|-gZ3z$1^K4QF+v{A3ddFhWCNvjPYMd-o8C<N;fJ-mA zlr>9AnBg{t+!HXrM8TGTh~5P&!BxQsOBOHXfgc~?i(lD{s37VS9=mJ%2-^aRE+E8l zaOYNL%_t%BXM;LK9Y`BE@qq+Ml3gO>R@Uj-AJT5Iu{vQVvcELVCO0OUafaeeGUkm5 zV6-t+{5wq3iphD0?@UvHasVUNcW)gj#dKFpa$Z(p^Z=rjo775}4WtT`K|KwjFoZII z=n<;b0|aIiVF)8zc>nv~%In{770Xumj!2tK0ZqELj=tvP?4A`eW~UQDqY4jvx-{XR z^xZZB1UMIGl7KmK*EGRp=n{j$QmfS{ z1VKH)g+L{)(P_iFNt`FQro)UleIw(O9D+K<*pd%$$5pWHTye!MfMfOfuK*Dqe)vbU zcNW;P{okoZN7%UWL3S2|?cdzN3t#vm4jma`&fG;Tn!g&6PDX~o*e;5tLW>^T)(&L7 zIIdoSs~7}aFu5|eLZ^6E`nN1vf>+rYFZ-MQ^Ep5%`Gph4xUD+oeaRFVKRuSn(}XYy z6_E`rm1<=|f%elHIZ(jWfI~#pJ=7}7&aJyScz8EYZFrPb=U+-_yI6hkrBH0+%Ij}Q zM%MbQhLqU<_%s zAl}WR|Nc0m)f(4ccRfp%T!amK5IZ{wBBW6Of8Ru0(j{v+7TTI;@7|qVj`QR*(f0z> zKe1@j+|?uj=2C{&`V(D!Ln$yw`M|HR(c$Fr5Jwxavz}_3oWlCqhI4svCr6H{h`aAx z!@Dyv*$^!xo!?~ zW*Z76M_Zc`77UegL}e5X?vFXJPk8c)1H9>tzsJ>muL4Sq^R!jV{;}Fh=O`3VN&AeH zzB?-Q&aEu^*A?ko>ri#R^Gp1A<~#Rf^FN@KA+9K~dV9neUo*7v8MyDW5Awkeew;X( z4Kjn+T~E=>VZ_Y@=|<^bR6|B;0nkph;`OxRx=m>s1%4{oS3{(NGV1lVG=_wN5Hk!q zM67#&{S*rOK)3SlcmFOyc!bx#;lHqCg%Ac1MgFpJl8`f*T<3T#+;b#=kQ7dl$grT$ zfA7Q}x^L|O&_;s}){-Z;a^yJ5GL!Lz6z@oB3UjTjvQyB`ywh7jbTA& zHxb^pO6$3NuZqPLyC~AA`N}aJoTt-|*f~!R=Kv`rhDEW-hkO3>Uka3Y&V|nfMtSsC z576Cd*}QEFzj$Ol?d{#{*s+6u+xbmi@{(6FI8dRtcOeTGEx@OU3i#|p>a?Y^QpSJs zvHwsxY8)d_f3UX8F*yE!B(d*orEe=5PnlBVcXAF>DyuMK{yd&|_+ggoY2UV*@_^2P zu?lX4^5`Kx{kczY$wim5e93ASFP+PkH(Y~HTXy0DUL{7gN+4}Glu!)Dz-K=F&vY&5 zV%slwaNY|RHOfyl1_{7{5lf@$RdEPy^iZyCB#I+kWVqn6HE=;Mt4c2bVC#lw_|(1Y zc+rb)X2}Wx+n%h8;u!HyW%@a@I<0oaqf51^Phg7inh|wzuCymp7%f*&^(Uc0u~e~C zu$1!zSB@Mx&KXR$+f8h4Y0DEYeR>c$U=(GTU;N_V`Qn%UH;d2fV&%#r-93h|P^GQC z9Tz(eAF2^YMYe4|!uG9$oWJ5~-u2GkWWl2Ilk>ju4mEaldcqvXe!iz=T7G;A^!=tn zP^EuebD}Hl?&@0`)6^xNyJJVO>l>|(zGF+U2A~~Ezn;^AQb3Ke4r22)8y)g`ZwV8^dYMRXslg=Bj;RPjxZZ5s}Re=lr5KC(w77;LLf;NXpB>bn4n3x zSY5_VHbZoZM9KmKzzZG47~`2}V!=|;k& z65Rq3bd-t&8WTxOB#Ki*PdJo&<<~K=;alKJ z2o>7dPl^2$IhdT8ZTaDlBS((29tQaEoojek6E%>Tq*P=4BiiJZRDp<{yMN9HKKLiB zSk}p97q`xt6*aZ( z^5oWu8C}_4A4zQ?Li*O$=wBC(`-}c{mcF&+q+2>wr#)xAq;WGm?Ih2-9jeKfgH4;@ z3!nQbANuQmq+Fdz=*JP(1`3LUj*f_o1?jyYBj3Uh~=)v-$#WtXh-N(4zT!-L_;*?bf*nr#Ou; zfzW?%IN{U!)($3X#3oZEZtPW>I+{6hoY^>!b8gDT{d5dp&PVQC!=Ghrk$0_2y0~&o zJLCzjGql+A;C_zj?zA+lYj0Uw=dfzPil-RJ@aHqeJkRHUj>+eNPV)SQhL_eIJRcUG zKW_#L=C1~-Ec>k$?A*11efzd>@x?3o@dH0*%jT`rG~lU?pJLaJ{ruXmy`0hE8dqF? z4YAg+#tSKX&hi3L41p~qRAl&et4Yh^T1@;_A1O_=(1@Q%lev}S)J-}~xFBO};D8E8 z4({Z!#~x$x!bMzj-Ahqy@)crC0j3TfAFb?>t{j+%uM6@&FX0Y_a7Z5-8 zlY~}s7|SpLBSduJ@>QB6M~-vElNfvQyyKo<@X=!qDmheRjCc*8UAvy({vUpw&D$U1 z$`{OMZf}8^U5YIP*uW5Lfdl(1967A)**(gJCwK6sTmO*ju6q?|PeQ@g;w(F-J4l|L zk*;eWN@_&~^sODCe_i{yXZNr3`a*qcD@l@TT1S02Ijd#@3PR^;`~=vz5kC9b@9|e3 z{Cnbf2E=nPdI`!LMCMYfwIQ_Ov>nwFs@~V}MSg`wGPj;;N;}4yBs-A5`{v6fq{XfXn1NCh#ZaaG+##36M2FwM~)mP>9_cf zn|ir=QZbZPn3?>9$Z?#Gu&5pH_-JFkdB>~aj5(*+BoW|)^(BOM7B1*!;eyK%47a@g zCU)-F$*#Q{(Suu=*;`@Lmh~uA?)#VTvt#FWUi#9TICL+^4k~v(N8yQLRQ`Lo^H-Dv$c)R=qS;2#P*fqh+uK zaET^Gpr9BAs8Mwei{MlfowrnET~Q)ZwZrpPU&ZaKE@#uzkMiJ;zRJbVxt#79bHH@s zX$w<5s1}7Z6WkYTEm0I5^ZFcLO?>=;iL>E2&`gV#i**u8rN-Xi2$8S{7OtTBcn77R zg4RZHI7DM4kD}(tk>i|!woK?Zn|>3EUKq zbm8bm=?3wX*6K<~qAMo}O?%_+rU@;BQUfYLu#RK!&|BO9f;bPPYL2D3(Tf z*E{dzw%dM<6)O@AEt9d8Bt{5)5tEG9I`($b*<>Lew?Wz1cm(>_O`^4R`v?$VtQ}*I zOF}y$CtB~8BgeCbLZLvlTAk7fuI!FufREj|hIeGTxY7<;Bj=X<-*iWw;5rkFD7Tz* zoNacQC{A5sY$bfrX^Kmyf|HSB`fZd)a~P%m-AKc53W5^#DmLx%DF`vB!h-p;ShVPT zUZtLA`aRcX=TQO5AQeXU?&QfQe@Qhi^QJq0 z2bc+z(iqo-n^t4?2_~6mfwfSET|0L2)FZ#*>dP)hmUKdS1VVv=@sut>K&9sQ%d94o zr9@aPA@P`!aWP4vDoCDhz1b%1EMN6p;%JEPfARCY`q~>{*+o#8;WKD993NDfWFIaT zi=Ha-SYd37Ie9X}CDY438k2Wcz*xzK80tE6k{p8B&siC*&~{)yL0F(%9iSi%RFi~h zp5V%nBgeU7=6OSCEHzNV-b|x_VyrC`0})W>{vZAupZ@IsVA+ZuROXJ zSaS$CfRiC$1Z#Hi=kHs~t#5b%%a=o;08!1Gh%WYPxE9q23qg}VBS*boXXmQXmTN%%PTaBB_nSK*SkQQOtPOLVjNzI4h&iSJQd&ZrIM%gVjW7(StUKq|RP*ESyk|Wh zS-WP+pEfy~kteuL=fRd!6XrPU+!c@Z&Q2nYDPR-Fw2^$sPl6oJZbTrhXq;Gm*cZZy z|4KUv1+aooPIUvFoHu_K=gn8R&;iQBKm9RXJt3QGkF$68W=4m0(b3Vxcfb1}g;IeP zE6?Z9p)xo2y$tY_A2fAzX~uDsq&v&h`SFpYIcJ=wt2vnT>NL(_?}FiYfJCZ zIaGW?$;7eezl)C$Z+M7r|LCV&+V^6XUvfDx10OIOYq|$h0R z5_9TQa37BHvZ?)+>Yj;Hj4?QYnko?xOFE(fs*-7J7Muu1V>&`dZRjATHcZKYXd`Tw zAD20DH$BAlee>y=W$5lU z*q}_Q)J{|t_8+b?FdVXN%P?Cu4)FTl_+4(i@m74GKLiDniG;LHZl%RB$GgS6h?~Id~Lp{ zjkteuL;{wl-{Lb%Go9y=1 z=3IUl)(oorc*b4KJSkLNi0R56I8mEIA1 zesn(ve)VIvKJ;DAyZ$DYUv@cY8|soI)r5hG>^*n&s&nv3RE;CV1pM~xZ)WF{Pjdd; z+5Gp{-^xY75I-qe{-3YhN9S{%Llik|5RBO;8?L^pCJdZ$VmCO9SWjCh4Hb*nK|lbe zot^8SVDQ0*Invg}L*M!#&wJTTaOLwnm8J{8v`zd`CUHcLwgwk1Ud;aO+nK#+5hq)D zt1Tl_QzIZT(c@h}Ay%{&YwU*avZe$i)PvjAMY0fBd8D*!C+H%!8FHL%MpP)TRUl#bG&kV3cYl zn1w7EZeqxjU$J)GjMnlVW@oPdpnso?>v^AcRoEc7qW2vDtwn% z;#`Ua;-p$N3Fl;e+@=mN$y5i8slq#34Jvt-c7<`eXTIx6Tm^P+<~#rELsTDof;mS9 z>At8h$$Ld!of&AWP!`o@x)+zjY&> zt1qBXEK;pNteWtBG<^eOWoxi(Y}>YN+qRvFvt!$u2`09UiEZ2V#C9^#+vnbQ|H4}R zHL9zssiPUIu@SUc$Swo_*;p|f=em8)cBkBfTw^daOh0Jb|YXe4n92=`3XOqDPg zc?Ep^wcF62ERkiwpc75x48Ti;QPHy@{@UI$JB`i;B3Z;~Wd5cc${Gh&O-sR?(K3kZvY7EwqaF~Tz0 z@~#P-GZ!||b@Xx5SiR>*g`Is#k4*+iy}?)4 zk%^S9f4^%GD$(08c$nl`mpeImktLVIPS|=!@{Qm7rh)r%EDm9CyX#54$v#;ookq!C zuN}O#abtp96jB*WP_Kva2NtC%CKA$tAe5gjAXE88yDm9_WvjGTcHf9gv#3KFj5 zkS(xzSf`C@qa^$VE2+x+h)_iDy zErZJOC=?b^uJ?;YHi#pp7B3~f$90R^y$Z%srn$K(WWw|pIgK<6TUdH1Mhvv~i|gd)U-(yNgI;;n1)=o_f&f zdL{d%H0SZnSG!L#!ZZTGl}wWwA*$(VKb6s~OeO&^;D#v6Zl{JrU~78;1iYuinEHM?OCraRt5v zWP@`I3~P#I6@wry6{ei%MgTKbkFC^F9<>}$XKWo1e=Is4vr}UXOX(kHg6f&k+ zN={B8j$&Uga_M2UP$gfkAMyV6rDuQ*SR2f2bt>-|=oktn5a@8l3V`r%!~;i}^(^4= zVPh5@HmtYD0rBQ->ED#O_(vddZ*0wGz{o&2DYoHSf3+@_U6@&e2nKINq`6dN_%uH9 zAG@?aK2J{{C*N=UIE&G)Q08b#%$zmNO)jlR3P=G}CbHY5a>&~~{8DGVtqxo2#IF|7 zJ)6z#(Ldt)7SFWgD%2k-J(M=WiH9-c7|nuZOfRf}H!*rcCm623_Jq+itw@jY!i-q9 zqs|6ws0(wVFjV1sUX{dV*+lo?&D_-&+p~6GnVs$m#o9&f~@}^Ap-*ZXNTF_r;>X z{b>WV#V>>9~3 zr;L~Q>k%^1@u+cko+M!^35tOMDGUbo@-GS6t&%jiTBXS{5qwDvOzwgK2QG|!7&!vT zwR#yHnh-pR{_+s4sM9{V-$#?oChyPj_S=s<@-w{%L#Cn5akzm{4v8AB{$Q2~>`muC zZImLtlJW~fMbzU39n6TPBS@>I!uaIM2O;>Pe#m4`kt*Y+co1yRHjKFb<0TPOZcjy(6aL3xmu7|-R!VsXZg&_oqv@1rvBW0sacK?O5|=r+dP;`=tK<# z3uaWS=wHO&mw$P3eKGdWb}2!>B$$(Du@V}YqDbHul`qE-jm2T~&g8Q<)UOg1O;O3Nyl zgw0AyUO%!+>ye|Bd@rK2OFh=qP;mRjNx6MW!Fsy@A-Kg(k472@8))sFOups+y8N`a zRO%)mSv1icuTNdff_z!4Q0mDEeb)Q(sTqwhhy>=AY+dr0IeYjcJlFNabN3m|!+v*S zXS*{)uq^b(@5N5w>8~;kmdT;^cg__**WGvtFMJtGD4MOZAMi z(EyIA9HEWkOA#E~0CM0fxufqU|6R!aRUZsaR1r*(iMit6CWCDQ!Ihs&CNQ!i5|ail zpOywL(+}Reirf!>Rs&P&QPlBkP<6ybKw3na@#fc2Nhrn{{08h>azqJ++d^*kl3=SZ zG#=z6>I;;^5)$IbV6+f|2VHl5gvo%IBFCoH<$pter{(4{`S@T{xYA=1W|7VOQL4!; zDVh#%id+B-e%R2rEr`J7wx`Nqjz|xpi-3`29l{|fxD;PfvtqT3aBeDSI@)wjIJ+@{ z0tGB&WRj1DJ*QG&gag7@$wAy4zEKEEOqsepxEfDzH)7?aVF&V)@14W@`{4%3Bf8&H zM)L>`;_JS}Yp;BBnDtn%me>i6_R1j%-4ZHbr*U*MreY38=UV1lJyO2h4`;`K6_T^{ zjOYE;?WdpM4mnCyUeadkfVJ=wxsda!uv*p3v!UKKo&B8rWk;V|(S|5SGr3zKEg~sY z8SGU)jJx5k#`kZWk_eg0WswpDJt+lT8gTLa&?o{bmvIVCh&zo=Z0q3CA?R&LU z=2i&Mq2@4VvCk{zR7yJP^sR}^G#^J>$HZRv z9DrE|&F^uBvx|LkczF$lkS-Gg_b5ybj){{z zRw%ZyZ2@>@3VOituc<9fGS;q-#Iwg8_x;7eYW7<3bSG+Oz5C<5VH8D9pwR|jS+F}? zyX(Lm0ZF6%e!>n_wkm?47=zN?`jjFGd^CG^ZfYCVpyr$sM9<)0EyoyA6YQkxPfKEI z9d`wGnJy_cG#WT&i*R#pPY=EbzlhvZzDS84W&O%(PeOFw7(gkpnohe$ixgp_;Ih8Z zgb{DKK3KUNa}ebqorK|!g^kBfQ02affJ8xeyxuf)sG`-5rE!I+GDYuCGoOx5T-7^S zj5}BEU)4KWeXe=$0$*>F#>XwKJ(#<`8YWw(gw(&&T|&^X#SVdBJ2zViAL!2=@zr#8 zDMGIC-&}RWBWti?mtbTY6)efB$Es!+lOPeGaYGOt$dic_7McqG9^=ZezZg>)_%7m8t?S3Zcy}B<7HH6^R%o1FUQRiU=*AyjO^*o} zo6q4`Vkl%Ne=LsDIJ{Ffp6I4BpXD#@|G0U?umLoW6bxb>l*c0~UzzTHMQL|iF**gl zPm6H>(Ek*;Tbj*S_>KM>YKAg=wr@X64|{!~sHvVFMDw2`ap8CJ;mdDmVamtt{gLdx zQ9j{O?F5_SmxTo|w&~d?D@HK;jdH)Q$#1FYW&qIV484{Q`H?MqhWZz-2sYF8qemr{ zN(_$8V0-6e$`=?FralY2wT+e(4l!F4P?{5FgM?^tq-Z2 z|8#n$0+%=hE`aw3OC7c3HwdisIt8dP7X|H-*nO7#XZC>54Byg5tB3F$lpcYr>(7?& zzkp$25rx%OjOu9KAr~C^Dl{mPssZFK*~>CdSPQmOMP4ktE(!qZ zEFX}bqQ=PW&KDNnnJ!j!9{uj*`4Kc_`6Q1`4&ru05&XuJM>gLXJSL^=H{%oJHF@Y#svB-raoCpQJ5rIX2w^+XaKWuEN9 zDBFk9K$zRmH@|J=wD}0Bu|G_rs+5QnVjSphg**Z+NkKjQ?1K{r9ae@P=a^sajm~C@R15`W* zx2RHDC@us~8oW#1^qj0bVexRT-fp%G?eZ@?;f`;4zkjZQlYdIs+wZ*}GUn0MU@9@> zNR+RPmcaX?rW~SlxzEIV+9G&#!^55MJ{rD8ilpW*nJIAk`)c?Y-t^0q*)^9FRX3FB_nY1h8uk^o6 zty$0&0(pa~-Bv{?ds|cL+HiZ+>8@zxNEuw_ha~$jUbiwY1A)=6*TwG>00jC`VEG7hz z6_C&mzt5kir!XL+*M|H!L_{UP_c81Lg`VB&#o68cu-5y{vTFEZC!17t``!3*3~(_x z>VA3QEvrn+n2+PYi}%>n>tar7KwLY7L|YLNWAW`v9Y_kJc%f=X(uavg7Y{<&`MbmY zcDvI>p~HpKB?1G{CSo5ty#@-N+Q7QD(vy&yXvLxLslDudO{}?WxZNo8$3p+l`H6C1 zb;fl?EuZ^&w)N*{l)K-@Ye~>uW@mN+X?jtH5?571su9rRmZ9>XLjDiGGqZN%6-e6m8ETr8B8>G2%r?h zkntDJvSMhA^_NqwYy9=mjJ@#|ZEhzkJ73z$mT*A34jV09c0j%5R6!9QEvSk)j z-g5pe7_hem$AeuZuljNrVttw@eb(5P0SRoPB4OrSq=p$~Ouy-clkkghjS8g9JQ?E@Xd`jVB2_CkTnV=N{<^iz@;47=FmG|}^C4hx4syK z{>{&#g$qXMEBw_modV0AfqYW=Xw#2v8G@GKYrbW9r6G!+X|8TbC6gdwDkZTf{0bzG zR3QYDirxZ^yCh!_{1}e>2GPekP;}}_V%OHwKzCqVtahbt-wpV{>i#E&J=a^ya+LQ} zg9~wGf%YyX^a$)qej)!WTd6(p=N4XALlnpx8)p5rH6;iDw!FEJIOg|29pLN@qlhcg zPXgWq$d;Y%B~XkuuP3K`rh{sB%w;^`x4WOGKbX-K>`i8O6yxF@$Pq@{u3ByYBY!L( zrwnC0U%-5PwAkE4GPb)(p4*0~l>E+FHhpgOZ5%_uRwXRClIKsB2+i}POJh%I1mqOE zMH8h8Wyzh*FMZBQ|JAmIEY5ULz>+P^Vo5lw-&-n|f@Ra5k}3A5QGo1&QMFtiDm%TJ z_dM0*9sa|2+>3R3M@n>+P{pBVZ--*djuB4^OQSjmUIfFo;VUE8^8?K6>&jz9)SL4^ z&SVi(+{ILPyt4JUtQt<`t;)8aAf&K`K;!7i`X^9)S>$pln_h5cQ~Y*bkLdr$T$Bp1 zL6GZW=F$#3gj#r&u$9yRZ(Zb6|84U7 z%`&#@#!-sl?05g*3HnN}ahKa~vW_%6 zc_H6?4yKu;i)U8Xa}l4Z3k-nK_}h(8ySsos1{S~v^>8*Hx0f%uZ{Xpnm+I2T_>j9L zYjPPO(qaqdZk)C~%=ngv=X^{ne#>FZ3}#kE8&FFf@4X2wg|Ma97OzdAQMbfI*QP3B zWGW0m@1gk)pnBRu%iJfZNjArv;;@9UA;paAZ`^~{?Yqn5ZUULtL&JZ|9wM?#9Gk}0T9)?K4c~831uGxTq-l3$#kFstXXsa| zb|rK?^Rw)26n7w%hm%~h#pxgA=gaP-{;oT?3KT;yosm_qI~yWlF2NBjw<;u^SL%IX z_#CM4_ghide%&{oIRcrOuReZmvup_cn=63Tl8kn~KvEWr$Sir{fLxgJ&|9LCwb@j# z9w-rOD_B~V-_`UNcw3s)S#E^jP#D6xoS%oVLN}QGg$}rHkz<4Zh!7 z)i-fjj?uw@rfbF~IgWb&3nJuhnHD0D$zf))3WMLO!Sj%9SDy0;#nk@DfG`{;HA6W% z1+MU>0A4b5WOoqoTG3$*$`)3*Pv){CvTf^dYG6 zX};@ycghGLJl9v3{ZkxHr4{kP^mhz!u*t&afI!B4DVzw572u=|FxbnUOa|_S2*~3a z7;`jXC8NEf)dldQ+AjZagHODjbR$Xs&inXFYUSQC-}}zLRLQ^I;7`5TvWkO)v!@+! zq>T$?78%W=`C#Wly9Mh>d`KPHF2rL%a8^IDj-W}Q<~|{tp9^vymlo$1eBF<#kMNw+>=q~|mu_nfDN#z)i z8*tQ-yi<>>TQ7zh zOVjJyc;efbqxWc$z}JKn z_4UoRy(TdoPe0JMiu*?PFjAf#eLg-l#o0`Q_Z`)@6dqY_N1 zE}LDjJzkriTaUmZ7V)5RCDjHdjh((8W{OL5IH07lc}S6}lMhkL6K=VbJ`6CciUHa( z;YpD#Evr<%szI`Yx%?v83SGWRiRpZsiwr;}kMx>!>W&GQ&)11N@*sz_Yl@B)J?EIX zyK$AjI-719nY8@D%d@_&VfX^|n`Y zadFf8ef;F-G7b4QEg1a&!|SrtSV=XVd=$V1*C?D3Xa1M6)=sYw_r=q|W1L>qHrw^s zyAS4PO<+*%y2IsGLvo(rJq%=TPR=Iu+GpVPQB0Ll|5yPl)|ByB;jsKToX>lYws5U= zxAiEh(8QqidrJOD3pLTfd0JPPB>gP)_0B@r^)N?>!;15>_euRCUI+`wY$Q!cAu`^} zD!~qlvMm#PN&>57K`086z%7h?S}v6EC#@+48Du}+U_@E28BvDC?~b8;MS3s99^){Q z6*;euyHW4?Dx+|W7zN#vb(F+%Pbf<76s*QBKq{2HK|6uV8)w}?i+gLskN&eJcjk~{ z;nOHPxg7KH1Imw&og4Pq^in5ZAD}JjjxdBfoD`cIJDe>Fr|d1m*&@1Sx!#C}(NKoJ zpn&%a;j4m*6?Q{_5ex?G7DF~x@`+H1%)&pqTQ@0gfy0f!DJ+>C60e!4mC{hp_fIz9U2 z7EXN=3SD~XGWh$6@xYNGcd}w>-^z2QD}x3v_uX;4Q26Bz_*3De7|sJhg@aBV2~uqx zji&zNf$AuMzgH3`_4V)Qs%p;ZNV?`87O7jI8`#wz-+@Ic&eCsmd=78ECFLLa>RB90 zKbFtI~2L08i^c(`hSXhWIIo}+JDS1 zpduok*{h1CQm>3pPJZr2-VC-79wKb}|Ej9%h?#G|!v-El340s*rzvNkyFnK5fe^-! z@SMz~$_O&J77R*s`mMavaqKU`tU|uRbc@!?T2bo;mrHuZEq6u?OU|+Hnux1huFI}) zaj=VaK`-IFiG6(#0x7ul{+szwqhi@MOms~`l_$Cf36x<*Rxlh@YW^`eN@}DifMAt` zM*-)-LXAb7b^K;Cdg!FN_f9I;%TEy7m{%CKt&O}3|L#tqx@qQ4Mrj$Gr4sP#wAdvq zb5=e14x8F$-tpcw>?CZa)^Wrz9w{eupX1{IX~4F6a337qlo_-AR}f@Fhc+5Y16(*o zjxPUJGFft&yiv%1TIZsj>?4*ekHTzw6L;;xrz2EDG3CVBc;;cjoiJR@JzuCxk;W{49nBjvFQ z$2JR}74$A)j8kK9=o-#>XhW~ENQYkn=UVcT#1{JQ zQt9iQd3R8M=U)|rE?h1)bpwaGT?t-@Ps4AfrqcnKWZF?zg68sJ^EBv-6xyX}fhDCi9MfJwNBv*o z=zldJ@$-!;k|t{}CRu=NuTV#Yua-!$oEa_F?(QH~$E5=dGr)RSuI1*HQhdr2u5(UtSK@8wICMFR%BQbNEwAJV-!t@C}dVo zte_<|#4-B-3w(Xy1{~TM&>DN9*@IDn~id{+>XQHH?%+iL<)@oe4#9z>1PzqoY8^ zw17dAfku^O@2Y4{+#3`LElmjvn1jsjI#0;isZ$$!aUSm;`djs|2lqRjm5C=b~uT+Vn z$pS|q9NPFrb035xrYk~IhH8UN2DMzwuJF^(jVi~d)*=@ei{^&Vl39%>icTFCPw>G( zIi1{qUH@InhthYi{^5ty+>`m8v)dSxY8Ch}@xg>U)WNP)l;!K&Hf((H2e1I*f?ck@8+KsdlkIL>0uxAk1v@Im1jP zB2|%0r(i}d%TzWdBZVG`hKTmAoIWwNYJ*tflJ(CtD?=0?IuGWD;snOClLj}q)O-7B z?}HQvMM-Kd;HYf%}~0LziyV$d|eCeZfjfyPp_@ztCY{*+AqeT9=-c8mo z1#{A9{5c077Z1X?`%HlnE7d$JiwGT5UGkM|jduD5`HbspE@Z^|orW+x*7F~WBisBq`I%vr zF#;;y^7yB?0U8P5jbpzkv0DXK?iIJU=2TmNY)6d-d7SmZt0aF4AA-v8J`9;OhSHXP2`=25zNF`-KCAD-UE)ynB1ErMY8fJLh8c(whyg1y*ARmjSG%dyix|lwu z9D2k+dSo5C3O~%z=nvQ3LJEEwpCXOI^`&&__;v*z;y7&TW<15vFdML|AI#EyjYO&% z!#-srLOC8ktVaJcGnBT8v9NJ!ZV2k68CZ)TDn(JPNnm5Sh1mUkCcMP+O7VU`OzX*y zbYaccX;~{|4y|yc5A=WEv(&&;hd8&0*1M4WOg&@&ZWHfR@xP%z4smlTDd=d^}e0* zr9i@Hn{@H=tVYXVpIebm4eU`qyGFOA@oiT8_SacZszITU%f*G(K&ddX0NbrXg@_D- z&2}6y$2W^mb^V3>{pRa6sc$@hTlVh7RiLz_L(E_&j60Y(WLcKGTf=Ls_(kdiCqyys zZa3LXTlmnFzCExCLg}pb6I?}K0kYCVD7$T9fVUW zaj#AxiRqEq>6O1`?W{r#J0ze(|8JrlMX+Nu#W@BCl8PF#BBk$g6%H3$sKyp3B4t}X z&D7a!&JJRAJK83~PkLc+%-(CId7Vv=-(O%Iqw6gns07rHpEsRKJoj}qiaa7BkX2Zi zsh03^U!&ETanwQ3@f*+e2*=}eM7TcPI67AGhO*g8<-U=7M)h~zgz~{DH^VLU5muKJ z93Ew(tvWOHiqFej9y#T4*W0jAW8v)pF9w)O!sfULL_;{lyXt<2MawAmpk)ax`Q!O3 zVL~dL1uUxl6yuB>o2$!h@oJA~gmpX%s1<|C%KG>TZlUK379+tD7TYHF{MI?HM8@A6 zj0S*dCpK1Axr!y#vYbr1`3(V%QF>QrFl*8U5?z8ng=UOJxuZA>r+acc+IAI?30&s& zi3gE;&|;)$aEZe!6dApszBir8NC64UfWmqy5p4A8Vyk2Fro-&CIQOI!4AOx@f^DHH z*c??x1eO&PE3idiAsf4MGdV|n!4pF)zdy|Li{y9wmQH100_?msp5{2!tM#*ooTRPw zIz`#m#QVRKx7hQJ;aXQ8E3Wp$FQyvEk>lk#WJJ9blpl3EwY{MVb$hrQjHuJ=T*C>fD|HMU7;AZ~Ecof+>ocG@R(i3Crz^c>}T8}qLR^JFSfHoJQHliX$ zle{26PlWVSYSmu?{gZ_Ci3Cj;T?XgojVSJ9{<~n(aBGqxQscr zv*~=(weV`)7H@pM4N3Yj>MtgVVrWWSMm*(DAo48ka?zoh&obyRRsjQCwy&>7q%TF($t3Bw=iD2RC_5hk8|-vdy5HqmRc>x>e?x&W*Laa2*ew!lL1>-K3JuJs zs>Jfx%i@q9x4qY8ja8vz4pysuT6}!|KF4occQ@RuPL9a%zSz#Ea}dm3k9a0na|yPf z#lLt12Ithr-LRI!W5_hR2948r=IHbT8lthgUpX7Tf?tDdG?Zp6lq*e^OqPP!qdh`` zsc5gqk$RgbP8K@0+SC@op~;1PW4r~+8s2w^)m&BTDLJBU4IzHxkx5Qi!o*MdW%Zi^ z7GSc|{|YSLu-oq_u*(DluIsgjdakuuDxFiifie?J3U6e^8Mhr*wfG7|}r=5<*q!l(HwXJj|K=PMm%mrIan z5|CqD;n1NbqK=$e%FI&D5lQ&RZBV$CmA4fOZ4aUnto)t`@HJ4cO_7&0K;ddS`EQwj z7=;)=^;I@>g{UPeg^S~rP5k%`z3Cw^L76sf+Al_I2OdKit5VE%y%uPa5)GRt*h$#^ zzyx%`d}G667L06Lqmt9>-u_#seQVM2OSLuta86PcypiwjxaGZQkwyz}nWUteNVsy| zJR#mI%@PvleDybB(((y*)Hoq2%^AatE<~HlplF8U=_>Y;OG+YTx9J%cG}k0RkCG^d z8WP;a9Et-s|G@!*U>57E(sEyb+zmZTd_3*+4`p)pww?$=_9|wP=30bZT^)WRvB8@? zl$YPz;0PmEi_uPoZ_rA?z6O`_`gjs{0=QhhLtRW}4bC-0Zekjro6x@R$yyy9=2BSA zDE&)kIE#4yKO)ow+@2eBNmU^fAT-Po90H{&WgoO5@i3}fexBq=89Zt9Y8$6IBlMz{ zA}-MMWfXAtI;+#>Of)zMjY~{8QgPd^|BX$bYAEA0%hu89dv;}*4FFk`P%G3W0l3>z zBn?p=%t^R9RxXEP=Z`#76I;FXk)#=ay8J?mu?gdA8kWx)u*Y=Z(rEnDM^s;;IM`#1 zwBx0OjN@bhRkrs_YB;Ak7>$l@h=yxvBqWArD%^W#FhH1*So6W5B3(rdl@Fn|8W{^; z>3l?hQW7q542b2*+GCSckpPJ$jS{P-kz$meHJE-5MwLP)%~7Dop4U# z5n5~_puzXfVQ4#zs0UV+u0hJzgUz?ugkVM?uue<|4KB8~$2>jl)KHU8}^QpDCjTS!Jm~QlQ9Ne9bFbyHDgKkQ(NS z2VF)q(g`Q@6n8C+&eHXqXR@cOLf^ms2gdu4C8<94qbkcZx>U-*og!nTw)~nMJ|tbt z?DS0}!_vAd4E&wn`wi$IRWk82;Gh4*RYZ>O^?u)+qf~@P{krQVF@cHWi(hIE0BgGu zn=NqPB|mwrc~o=V%+YxViTi2~zCAya z3r^M3gD{$tmLZO3h&LEsK^qTbLDJC10YdN`3ZIJR0L^t58aYjiZgwX`TF>XQ-&<<$ zi97L|_y9g#d2>_{CS*~$1P{ZxZPW>hT~uKd9x3^6Rx#vi>qF;wfQc9ZRlYSLm0pbb!8%sO7=FCSBIvH0=I|oh>Hw( z^>PleU9R`l{vEGOc+ll)JarN9qL<1b$SrG6 zDapa+PeQYUo-}PeVPg?x5XR|^J^X-eBSw3*A?IGpU=yq&GdNWNS?^B2^;RPNnw2Fa z&-ep+G5&X&*_A;Gk_lENC4psroEnt|zUOF_YzM49f3q2$SRx3i2q*B~hXyK`fIewl zL_b1N2X+~7e=Ek(M=kuk>$UAHvS9xA66W?I*q{?cNqac2>OnBdpPDh&CAJ4F9_s118W) z!?&r~8v((d9LCmu;}K)CoAB~&#Q8d6Lp6_I;!2QC8=f6(%B>=7^l=s*LcQyTK59ma@kPm2zQ&QL^Q* zc;=Q}k68qyd-Yy4nuJC6h$#i{*>5TYmO~ne&q@ZQ5WP_0tRpB=v4(T35-12_ED9DF zhZ9RQst;D^P^3q{c+k+GaIh)v2#|~5AW&BNn8k2JOF~vEUrRhj>_9c_WJ8l81f>sjLp8Hp6h+|1u#YG;~N^F zW>UB2zKm^nOj9hJL$Cry7e^4CS93gF7Ztx1aNNi{P`1LoJctrKfa=~R!bC6?2muZM z6f`;7jI)OLXsIXM)D77rs@7sK1rW|TI+i##8I*)zr2Q$->ein>w_bhM21Ix6|5fR> zi=8uMPbcYRZkJL!Z57jYjo|#a-(AX^0X!~F2Gf1ED)lNGekB0nqC4Nt^}7CpV~(B+ z(}7swnVk&=HZzCc1xNqyI_z0Umswz5{2yTO_W`70E+Ar%spJqlBej&NRNx_tW(n-% zR#CrD)1>Y7zz+22ve6ikhsfJP@Q7YJ>S8dCb`oIE2q!_qbe7=u8-u#1~LC%Gl_ zL#HAI2RFM2#6u&&F(3ofJ*kKW(W*|CY=&A14F zie%xM(Tc*WL>3IHSSd#$EY_-Fb(uWLcp5^t54eZrKxoKS?yxb_74ga)QCtV&V^>7GFQUjjn3FYn_BTOz> z!jMB0+g5ribfW7pbSKjGsCq4NLnAvARnzb9^6gy^Elu5cm2&=b#bP#SgwRO zBOlIGU=cao3#A=ZvhJ?uEZwsks^b)&P0RLGT^FV|I~^7jFp7J%Y@XmAd@r~63yNtl zK`;|o8X2d%&@E&+s3^--sMZ!u!iK7cxE{cvd;h))*L<_MI?8#RHW4y%c;)E zE9h!XZ5)+nk1139q~}sU9s~_g4p6F_3Noim`ZX6pRW+ty52}8wGBotc3c_Day)4FioC|>w>d!bGB~LfGoOUJj*<>7 z{;>btV=DcpO{-r*XTJ^R8F1$7cklF-5uKkUw-)NeG(092wl1@jpjNEL?zI4_7gbS^ zED>&-O%aQ3c@}_2S%+828OM{o!lDLFg@P;Yj$s!eiku&pAGg1*_;GwObJE*=Y(5US zHhFn5<(_M`LZCsR^D?`O>$kC&=YTLQ^Q9`0eEl6qD6qV%Xc;yAXdsyUynt~bw|Mdw z$9kGo;0Y}_f&}&RVx#PH?P!&7AYGTEdAAC@`7I&dvBzSy@d5c6LYcDS7St zqF|5b$ArdRoG;hMB8@?qCo@9&*%x!!%3`}oNu=YE4jm=Tw5@j?^2wnbRoP~(iYl2J zy@C^;ANcB0J^n#+A|~QkwFZ3&RRRN=zM|YBZE5!rM*;yC zVcfHVKOy%8r;xc1un*47RQrF3DU<6dNBm&SpML3+geWMg!JEI@Y@=5^>|tL{*%(2^ z5g(>ivn?uZkno|5qQMp%8Gxmeh`ON$B@vf0&hatL1kGN+YD<@E$Q6+VXJMc2R)C|K zpGHA@Kb2s4<8%$cjpvm2`xLE#yDBLa23T;kE%30$lx`rcHzoO`#Jb{ET$kvF#psD$ z4b7*9)vA@zT+hjOLXo2`(W5CAcBL0~@S9sz$|S8#1Tb|A2~LT|kwXG=59)n<$g=p|!`YLx9B;qToSfeo`<|2t@IVw8{@m?M4mP}Ke}&9>oME14 zoiuZhC{QtboWmji9Ryx9u#=Mz-ko;e+4~}g#VmwgfPf*hc+Q%n2Ev;u@-y~{4IRjh zs5YF2)~72&#uuJ2mzn*yl;tPQ{!n8hWE&CUb8pNGJ<@kPeLKwTm-o~Eu`$dPxvd>W z8EL_|MY0z{!2=5TH)IPbUS86wr_; z`vE%i*G@KpV3vN&!k6{|em;e4-9lSSPJIcNPuid>pTeHSCZmw(jrT8)BT#-iyCOoZ zL6}7Ed-9)zQWnyD>LJ@!7U^9pxu}9b%1pQPwUx5=SgEpb00m_OUcT-89(!QHv6lbA z@A-Orpl?R;vMi*nUEpm9-6YoUs3L2<*`I!R!M-cx{(ycX7!SkVeqwFtF%BY*_^OQ~ue z{Oc-$GJi2EB1m(oKnKF!jUnSO3rYGKk$Tk%#0evQ!LeweBYAPP`$d`^XeYH*;123w z+|OLBn#d9I*j02yefn|%Zc}Up$m2DyMDiW>7{OK{XQ&QQ z(9oszP_VyiSY=4WF{G}WNf&V}k8_i$J9S{AQZcnk^ZuD@=|EBlu zZ$zH+%7kvVuOsyEpAsFHC;)`&(AXYC=zG;A@7@|0%Z#0Idummk7dTWNPs8t8`RVAU z3>z`a2t+dajh()CMV~^QF4CwhplQk z>9L&5BHMoX@T2wwS9=?(cgZu=YWH%qFpao7#i1%m6igHvp_(al6A&y31#&~A0{ANM zp5h!Zjwt5H&WG_gM-EQjq?6hsR==n@P13*IACUN!J})v)9CtJ2*y!$hh^0_RHd5xi zG7)CndKYkWNqB>;;|QKIDy1^QX!a@_08mkFxcrS)V!N3zA$o?sTA4F+vneNIcU=jA zWBZ<*X=?=Pwem?%;|Mh+dAzxX|0R0ZVY5UMnEI9D!pZ+&WdK_Li+|o0*h6gz=RaZUvl6R%GgO6V(dq3t5j$N)Un z1_uP`u`TcIwT4k1{~d@EUa=U5magh_3U%FYvCrjj48^5TudNaRj^$WEoE9=QZO`K&|oN-SnpJ z$HHXa2M1H7liL$+7C^Ewt;4JrVI zTrTV2g&V;O!D>*np{XJpY!5^7Mitx13uX~ZHr$-c%W%=LnRMk^D0EQ-BJRXw z?@>6#Dg=Av#NOCWzMFORZZ?G#zyIn9iYu(vm$^ zZ|LhgLRH}U63tpjt+*6$@|%`|_zWca)NoDyX5b+xcCBqc;JWJq-rzlA6=mhQ-h32k zWQ6Y3D7x-*6Z`DiNJJUP%Zi}#y2I2)(Y_x^wMxWucppvwz9pSu)+1sJacdogjr;?7 zVirLa)}l*D6C?ZBn;i=?7?rP8#A0MJ?PQ?L+JqtYa}1^cCoF2eKr{ln_5Q%}r2cI` zuul^n10M1i{g%J(6P?LWFW*8i;5nK2VbA}iu}MQDlyu2H*kQ7*tu?Z|Xy8J)p1^Nz zKzF);j8uKOYEKNNGVH2xE&*xa4p2Cwba~0knBZNtjjBDMJ^0J49X`nL$^oP1NM#+m zBE*Io_2-z%p2ej{+1gTW;@KdE^tE2L7ieaOS~^FF;*N$Z?_0eUdJ&&DqO7pd2>BeG zmUs#(1N7~>v~CALPlA@*eYq~(jNLKIjkkMeu{({4xb?(rcM1>^;ds6ti8lf9{_OM@ zXiE&W=_xb@TmqC?J7$Qn7OKFYT4$t3;o#P=a_u*|YKMozYu)X<7)=&Ro}j1?3>zZP zEVD#-T_qi&;^Y!od~G&Yc?VMMxea!7#a=)(-&0Mur>H5TN|?(Jfo`@8rrmr91%91; zqn{jyFhMZ&ryC)-0%~()tU)1D!1jvHlX2w6u`{LK-L_|Bo0}_PI3>*D854ot;ubJv z(4tZRzL>;rj96&zUFe-?uTX=R=d=bt+$CF9^cD*yrkxu`)gQNh(X^G-z~(`W6HIjo z@^RP40=ABLM=kTQvxgHCZ=7%O|1tFrjFm>)vS`P)ZQHhO+qP}nwr!`Aj-3^&qmGS^ zI=*Y~bMCwEC#?0&Ip!EuHLAp4*S%?V`Qnej*{ayfF7pbj>Fdiqe6jg!o!C;rLsa4{*rKBJoisJb zGpTTdka^%w@P?-Nd|!2M42?Wa7=C-jSKn1ZLvXR)W{32A->b8EY(<>-r-Cif`&hrV znN>vtW|lD8UXMGgu+5+bzgxaR0__VVQcr#mxK!zllV+8-&%zx1(hBncHL1MOGEsGv zTiWA^EhNXa2-UDqT(|FZ-2MfpS)gPxf5qKX;Q=`ed9F^ojvAg8%Af=lhs9*MezWs9 zucJBGo`xdjzN=V$a5g+aHv9q!Jwjx>nbb5Q@61_N|7)z)XetCF1KFzfsaPqoOmA=u znsRk$P5@x_kH`89;j};>U~*PT_<*UKrq6QiFgjl#bSiY)6PAz{G#Sg*$9=ku+QuvD ze@TrAt8Obyvw|kUVoez~n06AJ79*S%LWmSPDuji!3y1DE3SW2znEw&kAW)(G zmo7gra;$Q89TC>B4q?^1o!=<6XOB5?=|*~V!7Cub_phYDn-dM;y>_Rk&(lw`W9N9j z5rsE>o@Hz8v`mhD9VKrWQfafxsuD)gdKt_xc9X0gqteGFzJdFe0gfnEHQ(Ftys>+`0^ zm*rMFY=!mzsjMblxU)JO3q9sK?RPp6x+x+&Me~eUKdTPDieJrJH~jrbl9HNOu9X-nYV z%_{W01?CUbGrM9waPM4J(xQcr@M#CL?i!Y~`BQh!TH?y39f1W9@iQU1Tj%=c$tnl@ z-QcW19ouK#5XGCxWD*;Z#rJl0F%_<4U*v$Xxp2udqJo=c|BJnBD5j?IicdOEZc0Un z0UdRZT`nF1O;VagU2-2zZBk(m9#(m~&&%P_;hMiO6pjy=wpMDZrmdC5miSkw6jsXs zj&?UW^11y4j2JR*IW7e%*>?yIa*|7%qTAvLU~zC>Hu#`_cfz+^J;wQrT<#4YP}E)e zRmk@M+RrZV?Z!moB+HWijC<9ej02hgPMWS39uu;aeKiU;w>z>9x!Lg-nC()QZ; zB*ZY71CH4eQDr5>7N#8ff(%`mU%g-4imsA)#W$5)A)OpO9Cb{{4q6ptTm&%$v4mB! zgERg~QUvi&j>E^X(Lfs4&sWz>1}Hl`bv0Rtl1FyNfeJmEYkIlBy%$6U^Dg}G!^a?# zH58)W?}xc|e-|#*C5ChOa*?%mG8#OGfX+lMTm^)?_-$;e4auh5%?N5S7G|af=sdgZ|J^`FVAaR`LI{PmYYVZO|km&6rTcdry)k&E>tugr8$o3Wr&Pz|8W4n_a`7oFh8(3V{x zAFdf25iV8^LmGHb;?C>&mvU^(tXMNQ0XNhM*NCmkrea_|REI^%1u3rZ(GZABoDwZe zmM@7;jOc9cq+N%!mzysW2<;G3%fRfkNE-M$dRU+9$2;FXG0MHQPs*j>)kEUMsPN6b zgq)r4egS#z-Tub*B-uA5h#`p!g1JcX*!X1zxfYb@2|69W@A6}YTdxX%t5RR(E` z>@R*;yt@*Rmg=MUcV9jczY4Lk#4Cec+YY!G{HYSfJiRNRPM+<;81J zmS!g6>RQ7-9N+RpD5R(TC@qh)nc(ccUW4jBuLPii3;fboHwy3A5#Iu-1e4-pv<2fe zeG_@gJ8{jYZ)us+9$a)`m4j9p&Nj@6$tDm66-PAT7DvB^d_KU6R3_LD3wn0s4~pg* zJO=>xdJp_s%4?AG(n7_`H%$Lo7Io3IA6v$|^hVCnEmcE5Y@O?=g;|qwG86>clm+di zM4SSxA#Wfr`*vO0K~@iInd}prBsD*K*VJ{_`qbSNa0j#OL$uspR^2nx+CS=CsuTv& zfkRTNwn)Qe4oc3FtgKetO9NjkXwKk_yKRkJ=GhT3OIZxN$~xx+gS=xhztsO&Q~|*O zsYJ3!Y-GEOROPfCVFA!A)(6#`^l%xW+!bAr>C9`rp2yrx7vwQOHj|_>!zc($f~f!l z9+VDa&p!)n(Rm>+4E`s$ujB7m{**Z7ke;7yn|)4LevzIm-2V(3ul#tRq9(Ej&)6{; zb<^?tv$1PrOlK(Ye@k9=)`_ubdLP_KX?J?#;067oZnHx{Nql~09lM8d7F6 zr)Ah*+f?SxgTU&~y^9g49b!TOO{W#)IFPrYcXA|$X|X=kS;P@#?`d^OM7z+L;^;0M za#pl64ya|UtuIuFxaQ+>2JCpXIsurvA0yzhmm6ja4S=3!;wI7!7ung$kx{T$TP)N2 z1Hne0t6gr#98Pknuod$A}}$Dd>s~6WYa4C2X!cVr29RGN?GHdR;V* z1P&xy2y3L%bJ?O}{?(B8i2uKl!3n;`zEldiL(&rR!Ew z%VwXppwkQVu-fr0Pq8A<;VANQ8z<#xI(p{aw>tEpf+ zd}tOcmvU;ZptEpZ}_Y9So9D28Gr?o!g;<9g%qhW)7Fdc7|=H)1$^}l9n zme^+1tG2Jgz+(=ZozGhalgm4l6YMW1Dsg|nNI_Sx?Fkvi*U8r9hKX*)B`0Y!;?M8y z99}`k2%?raQ@z5z*{`~e(`+nn8Z|!OBF5Yv=82**>WhMyMA!7{Q5x}({rmUaj#p6F zMBwX>K$Xq*L&C_YsGhfrI&6Xn4J<=6DCo23FmPZ@s;E_Vi&>Y$Ml^49{V`luKnxab zcn7hfwgR_c>&c*7E>~Wv?(+uBb7o!tS1;FHF4th>uuav_1~xN_RP+a`ZFz|o0sAt< z_&p)WJqiZD*?j|44dWX5MaJ51@~3HQ09aTQmBf}6;H0LC@EC&0M?e#8#_&*i=ID#W z0mh+1Ab{;oQT7CONsrQ3wFk6vw*{IOcNwzMjoGcGk;!W2{w zx_Z?Vf;$RJh=`lKfm|sOE5>|)hpl|v6u!Dng+8NZb=bcIxc&Evd6;sAxVL7IxxjW}098ihIp)09>eOAV#BJ-PZkuyRX5##BOI-b*T?o47pk zf_s|#;@M<2P#h6xV&9rEIB?$ObBJd&*J^<*$pTc$jCjtt>r_3!37gY?s^*;Nm|6PS z)efQ!c^fXfkhkdIb&BVoUFxZorRcj#WvE{marK>_4sh^Z*HlKR<(@ZFxf_!K8&>@F z^VKfPe8~fP1?#Pi&LVa0RsY(;sm@ge#cHDQw}R)LwL!bnDmUw{kLhE)1|&o1 zN(bqgO2gc#f+JG7D{udDSFvq6&FyWsoY~=q)FYx?{~>TpxQP9a`z#Akn(e;_fC3sE zowk*1nF`y+>G9Kq44FTlS9+y_;cA>k6L?|)eZ11_e+Rrrp55>;lu5CxG1JLqXTv6k zsSekWz*91(H@CAmt!5}Q{{20p)oS-{hR@v{w%G>Q5AnOTIxWL%HiX*qe#9j7+eg{` z%=A)q77RQy$$VDU?!jgmBZpQ#00smfw=_Mwtx;qxJaq1vE}wp3V)<@V|L_u$c4~pL z6L{`}@tmS2dYj#I{UgT&)ihpsyE{V=^wjlX^gPce-&DtcH~5n8mlDd6{eQIpgPam< zWjHn~aSbQ5FR=;=qM(rQoYeWt7wQ&} zLs)6nO#0ugv8x;sYJ;w~JXYV`FL}3llnK4#yh1`|bN84|-hs0E^o| zCe))RpFsjeJ+J-ui7U)8FZhdm}G1*$xOLUQQH9gn9QxSjhQw!Rb0 zhidXp1-GDO-Ixt$*?tbylT+r%S3@Is{a3Ii2f0MpLLhUpFNw7ySG}r#lZ*uB5#_ zv3sJ@&YO!X0o|tI))+#NKUvY+9gzr_Ohy*B`O6!FJ3e%a7|Y(I+SN=?g3<&h%)q$3HahV?EEBq0wxtX4u*ie-xZ#FGujY&qLw8p z8&O>@%P5(ir)CK>OJlxeZ;y&!o(kr`{pokw<&6t2TT^`iz0e___{u-O&r;^BD$yVoSB!TEttyq<##L|!Ydlo=jYl5k$~MEHt7*^P`djRq`wosX^~dS7f> zh7W!Fx!5#Qd^Vo3_y@sB2`s0^YB`0*SfsYHuk3|nMBWU_HNV!d=#5^tkX)9gscv5R zvw5%?rOGiFuFc({Fw!K-aY%mQ6in;kr1GEH(rR>oyL)KV?XIqRFrN=y=)-I?Gl!QE zVT-RQI`cI*;)N34^qgYu-al&rZFbN6#Dlp#!9()wiwp|8Lb^riCKnPeY>7a})9dUJ@0-tB_O3vvF zJnlYZCeSl8P?_kQzHiBh;#ZsC>*gl}sHDwV;$UOn31IOPDk3cm%Jq&g+vU9=7NT z@mu8_yQZFaBBT$f98ol(&vUa`8=<`T+_}Neiy|Ml8;G^<NNQ*3@Kz(A(Edh}X3m`s?EGd%j{b!W{uKhrN{N>rXm)!`2#J8jhFk=83fA*WK)WT>Nt6 z7~c#`yTbpdQt28*Yq~AgqL}^VUKbxbfhiAmGuheF@W`jEfZCB3OS4s?f|@Ul4w;q? zlBEda<_cc=Da@4#M*KII#piDbM7by{E8RoZ0j9_Q3Tx`ItEr)+y;&{T_I$`Bq|SKiX62R znOsjTJsz1{^>PS#2-%C-@|TXV5R#2&((uhiji-VLszUS=fRp)N-~z3SBaZR=S~-Q7 zmm^lja{zi?r?TxfXen}q3%4oWVm6G<`(qjZlOHC>ps*Gv;AJf+9!@Vvgkm{XGqjC` zmuw?3`r7HrQmmZ-#(R}Mzoyvg-Iz+>vE#qB>%$*M4N#FkiPtA#}cemn^AAAO+nCUimV_KXOqDOIAv zg?(S9N(b2PB%B=7?b)ae{kc1Tcy%ouiy5rW{6GG%)6u#(>wuMLif6#SKVOyT(~BB( zK~WW_R|17R^C$2;e6!y7F4!r=|#MP#btB(RUcKbZ5KD`R= z8azUT{jPg3gd0#Ux%Z;UYgq&#LdgQ7Zif^nTYy-lX;kOSAZFeQ>f%Yxr~g{qM|OyO zIk`lkb2IRe=6`jF^aIP(W>|)7{>X(92^|@B17%|EpR{Mhuu01Svc}LRi8BiC zDt&_qP+#7LckF3)ttt2wgM|8<(b3Qi$?a66BO?Q(J)7N0h#|i|@kO{``_?W4c7bdx z;Sz!+N;FtL_m4$w+33g%7Gm#l-L6Kh8~>WsCJlJOJce26y1s#V-iBEL;-7o(N*s-M zW701CasPcuKyvCuc1l=0`4GKY$pHADYS>Q?*HP`wVYbhEM8>2EU^l=&A*s{^TS1zB z!214duG8{?7c~Awd@rovend#;7JD#wo`W3*F{33PT0~-ylLWVc_+RtRGEA z+==M^W8U;2+Itt0(eKvP{2VfpuTWULEKI&O2~HN@Hj*rGM89wnQN;zbtd-xKXe1pE zM!YN^Icr*a-`Yom&u0}pgv`@I88>V=W@saud6Fdz8H1Z1B~&toQqN`--kkYo+-8Pd zSCl?r*u7zUJdILf51LTeRhk8GqUC2PSj*L<%{i?Y5tFzgXvs*sJL0GNhyvlrg$ ziROusJIkrT%Ub(NeBEGoG7<@{V&hP@D=L#mTBMt&yiW5730y{W1eU%OV5`{?A(^>9nG&d5zE!C5lY(1jGQCS7i12 zLKGPB(;yGX8?a^Xz=UufwTe=rp-N<$%ADP_UhM$0XDUDYugCSe|JHJWAWVH2?l}*$ zI9R%x_+w@fM@It=cong;z;O(MJ}dmM3Z;U_xU&@4I$Ec2r7)#BZ*sdw@3VQeS0)(< zLfE+Zel#Lclc)~$2C5^YjQRcYBzr9OU?Lhk-Pv|uKBm_P7n6|Ugvwb-%{ap21()lv zT8+^`QaT9GbZSswd%olghfO6{R#5I`J3q|e9nWv%OJa9(OBdGiC{EbykuL?K{=V|z zFDxt4ENJL>_WUJvQ~I0l|VX$K~{h>YjHxp3uPdrGA;>4L5%60IRT ze>~MkSt`qpN^w(9p*4>NZ<*8Pmwh}2t3$<}VunPaoJry$9%AgUg^wS%pKSuKz-b8? zNyn$vLQb2~rH4$GUJ5Ih`}VrJpcI`>iJFlci!VtUTPdBS1E6%nk((ar|B78#jjLVd zFdLKB)C#B(4Ivgj#j6P?>B;dyOz)fOD1eL00x?k)bWIP7EAR0kfs(z@XIdVmc2DQc z(vFjzA=V&6)i6lztxIgnSeZXu`W7Y!i_l^2kB=S5b=9YI*IPiRK|VZkS>U7Iio;0+ z3YDV`_+axgN!p->Xf=leme=#jEH z>QEZY+EGipMU9Lg&>*gMbxePEqRM6Ij}s6L#PB>|f+z%!pJ``3aM(Ko?VC5Fi{&u`X3$om#ap#39 zQ?+!C8;_NSCF@I4XZgW^7GU4iON^pUV6ch=$B>G{@WNAPkcq3O794|5?_BMeLvI^K z`a-rq9!#1hfSe*{ME@Mujc4iJNKqlt40ojBbBbtEgy#X#8eUwL#f|XZ%9T!3cMeq^|k((@8#b~}*%znP4X^1`Ead!y* z=BTVSX*ev;W<<4PeSrk|OpUY~Zi=HYrI)kM=^9C6_S1cCp_+eBMi_l7S&oK;akN2S z(cz7squJPB$$5S@xfyu{w>U{CmNk(U_%JYK-0Ay|$^d#@XOLT;mxS3HeVF!@*G6{Q z4sddcaQrG=#zuV8vv|J#TW>_GJA>;zcb7IB4&FuQbpxV(CBeV!YbU|?Hj&mXcL-Z86VF+XsA1>=B4tZ8W`EwT1W=$}=Kbhr_ zX^XW9DU0S%@WhHoV5<0;@bU@&Z*_xOhC}M|e0&uAO+(#T+}_C6G1Gku#XH3cE7twc z2vjF*JcUBg*#wV}oypxjqbh6PJ>f6dE3Sg89(lC@Ttr@8;UfUC8pOYMI$ov!DPvG>VwI~Hro{uX5MFfSeB*HgYvbcOh%^tdtbcxT^L-!m=UBL7&$nO) zIwWLP?PywPVs5e-K)5fU6qwO4Xc!k`21>9`7?l4yvt*XJ(oU`5L32&8x!_%kR zKm(Gl>S1M%V~`|^Y%zqJOzQVyZ+h(0L>@~QF5+n#WW^yTgBrHBLO_3b+q7BGru{NA z!SkI@u`K%PZ0d5*>_KMWMCy7T;bEi3N>Q92#tz3ajB+7}+a>^h%!&=si;_?Ai==Y4 z*^PUuTwqCl3y$nOMlB&Gi#<#}=0DP$m3p1P)0Lqg-_ceGN%lDe& zQVOb3rRib`C}}dVq2ZqkL;8Z_^K3ZH&EiGp_eHzuB92(ENGyp;&``EYoxN%*|`-D)EQ8mMf$ zb89sG#8@|iD(ZH_q9{YQL{pKaNG9joQMo?U6sIObpNhnwRWUlaa@AY754EBrjm76jEC)4U;)dQ0d$J!Etpa- zH07Db`6YBn2$0DzLVWe^b!jEhPRHf5p_*?%+T(h%Zy>K6s$?r7E1CmAJ~Lk090Fih zLH%z%1c)fhR*2?s<*VzoTcf=9%F;UmT$-_IDuD8XCZ29)eTHjF50DY?jw>u!{^tiH zx0}BeM^@s}w4;etJg+WRiT$V(9gs(A7}FX={&yW+_l;cJlf`t4?dFX2pBT?KOl)N& zQau}hoOfO5t66curWbqcV)&x(GlFe&$fIFPP{d}3>n`obNH~x<>3m6?naP*G84T{4 zhQ+9y%G2y;ZV!Er!?y74oyPN3T*hZ?dl+1;S_V>)=l}uyyfsO)L zh7Z2US0wGAe>TLg6q~AzGBPvjIn|nSz4djMCb&t;J(>7gxUGIcM5}_!idBkN^ylD% zVZ)n>;zQIUIE!R%bed?Ob>rg}Wa97|FnCWu3ON;Vx35_@u#Ep3;=Y2R{HbKNfFs@7 z3HjPL_>Rux7?k?@&_=I|zOV&rVr*?csnW=e_?S*3p<}qQG}mY#q1n(M3kbO3EI#?? zIlhLff59q5sW(WVrrWEu%VzL%ZybLz>efie1_=?65`QO1vIDSwOqgHp(Hrxy2$3w7H;_7Qe z$8!~>PWif@!kv{EC9~P|n-EX~$o8*cLA<%>i!qZ+>NjQBevYWIveOu}VVRHzd+L9@ z``1g`w52VwycE~0Q-pF*ujQ2^7k?Wt;ro1H(-n@Uw zq5I4p7ElCnd_Autw-8VYCT|1?jEPg$5iN+v`l&9Qo?$FkzGqppcD7Oo9w@q#ICEHn z?_S%liQlj@e9O*9wwM~KjLM=4<_MT_G_(`}aO4h;N2!5|^A& z6c+GtKYt$jQO4V{7V_#QmRP1dO63t!3**GGLN;I=&_kweJFTLM$S!Ge%IJuya3V-i zAtZP1BPv_6RSUMG<Y|ry=$_rDLsg#}(VV7#d zZw?9pt-@j5ERCk~zRo=8wflQ3PX#Yn*#9H#UQYZU`;yXCJ{j`@)=05 z*;$z5)x@y-u+BuSooZbWgY2+*PtM$FdDkVw>^I_;wilLBxL^f876z ziUH?09&i(E)f)9lbJjrSA1a}4i5sFP$gSnMq5V2WDSG43HM%zSb;ei7%Al=L>Agwa z2w~+RBOY0sx?!kMu9#S3)|VH$K62OK_O9MR&+|x&JPT>&yXy2lFlXtOf{%K)M}tlu zCaC?D8e9E4A#C;91EGur`|7%ulxUUH!N&ISnk1!Zw4pL<*&Ej@uD302ieP!AwAt!R>j6Z!9b7689fHjq%kC z4dzb#6vnr`sk^oLQXy8hH;{B z=ac{2pUWv=hC$zF4guNDIl?_x>7~S6|7utc1Y#8O>*8efV=CM7aa=p}S66|Mqgm5_ zJR3--=@orAEGY&%1v|w8RP=Tr{QE+0lF;Wy(80@|55xnOu;jX^C4Hj2^5uPnIHlbF zUvD68NyD((-MfdOh^~nmwQ{ZIVgJ*Pqk0kIX{0z4p|Y-|BQW7kzO>Fy@yFcU96SQT z;=;l{)Ch1Yuf#FIMwLkMa<2GW-iCN;-vBi%(p;9CAg_rS;ZlT4(@mPVmdIK7Iq%ec zl)HDlFrU2mTQI*hHF2dx?Q~F8Ry8WsxK!3Ow!QP@ABs%*9cwmJ;lZ0vR$AJR)xN_1 zX*2r>IbO{LMmU}C^Cp*FDW_n1?Bj**Q6Lo)4RqBe?S4TbW?7IBM?eiNAAV|e`maG(J zTY#pgW?zmQt~t8IxMZG2wZI4AA$a>dVxrBB;iw_Rz2K90GeT59p&PSiB zVWf4$46FaOUmZ=EicRO3esGqj+y5S$qzN{Q zM8)cSTbB~wMV`&WQV2O}MEF#vTp#ZVKkUfs7~0z$Zb>?*SL7-T7nX4tZaWo?q_l{H z?=LjMk2)q-iT+^X-y$hE>2$bJrOL26`3{Hcd;ZZ6jvo%%i}CX|(X<_chBdOq_zSBD zaG1or*dD@CPG$8>6e1t7>cd(1IufHb_>1hcpE%`05XCUeVPfGu zI8zM!KPc3l(>uyaBMK=umS8J~RHvXrMy1)|OGunYjwGt)`b6=4=s{9*Kj~#~2Jl;{LhnKJ>PIIt={+Wngy$q{XPoOkIM_##?A0Ze2`EP z27QqmZ^Ix!^8ysEk9&pL0yoTsY)iX>N97PS+BkKXk^5V`OXz-*hkqrgRO@)V+;P8w zM*N@{Xw|G}F?&-a*&hbe?6W1CbS(_BoF< zN1rTW98w*-9K@3K0K?85gh3%8d&6`DG^{-hsaTbe(us{-HZx)qc(LD-w7)_wNyrtZ zVnu#Q8zlUN@r6uC6cy6e8LZ)a)9es5QUza~C{y}~&TH&LR1e_p7erkAo|w|}Y{%Lq zcq7wza8YGm07ayla^n2Sc(5HEvuVvdZORdiLw?!2@ifV0vu-eY@&h;8x@^`oJ};z& z0e(i5wlSD23^H9o*4&qB9c9;HAm~75=xx_3>es$}jDcU#@u|=^qF$Hav<6e2gq~fY zsN$qKZ8dQ@D$g4fDI}EJ)s}(DGMiZcr$d+PYDSarB*-QULX{$!S+bldIR0H0(34}4 zcVvseNPFU^o#y-REAFK4oEehI#={#vq0!%t{tR(gR4+xawq9oTr0wAz*J78`d~tc^ z6;HsapUmt#gf!^k=c&}!JVfkkn?FT^S!HQYEn%(XbkH7)#KanqPaw2FX*OM67+|kn ztq?Lh!nr;DD-ZGiE>*eVkHYCwO8*H3QW-6X?UvL|OUnuVHNlRQiWvaqJ>u%L(HJ&- zBrAq&`hkQyfgzCWpaTqe}$wKqmn*<9N@$IxAqVbCjL zExfgSwRqd{1GYIU2j>kx%f1W+`-J7p){th^k1%W0_^Arw z(|SJE-m6Z4d0caKQnhb~sk=Ff_=s}FX`&r5(X1kaf7F;Mm)tH62=syX4){7E#;M$Q zsJ>V2Dv+z~4TImV!gYWsm$dvWI4$zatuu0gkbx>6mL~hl$^t;(2MpGK^S)bMDbamV zgt}#~29NMa)zgWqo?Py|Q%ca9=dr3&qtQZl5bKhZkg_5!f-ri5%cdnk<#>2a@~Vl_ zKb{VZ|56&1LI#FL@v@p5)%i3lv88chHoO5ojuaK-SOD;hM{{8;g92qmmDD3S6`F!C zB|@iSbl;IzwXewGF<~>6KK5%AI0mu{E2Ya)N%JE4dal@7iy*pWl8Zm9k}!bMs2xOb zo7)z5E>_Ej8|&Nr#e|XC#UPK9rnJg+h2zB46?}u*Euy*Aq%<)KhE->ggJ21mW9U$7 zRKuj#6lpQ)g3Ts{qI>E8iR$#i)87uKRS|K~!G)THUBM znbcUjx7RVIff1qY7p!{zx`>QAT^;7Z5N14-a18X=k`h#~5fZ=W3Hz<)qBhO`(3d?| zfi~XE(sJQeU2;VRlrizT(ey+kM8Q)9D3fyEeAs_0xvOTSlXU{*KRvFzODTH_LW!|^ zxFirQnKRD)kbsY(TCA)2#;RKV92qEE9_tb7?qh&+NO7Zhv0BZns^6wgob_0P+U!d2 zaV{q>@0N`%ZM+{7bgFA)s3O|J@jlw15j?-zsL=8MeEq~2YkrTT{o7S7X8czQ~CjH4~Ix{e_2-XnN z^Hq}KcF~$2W(2wr+;qlx(drPV@TK@T1yw`fF>a#Zw2{i_(ieahfTLFi%jOTm^NGp|2c5bVF+(1*I$uL_ zS{*qpMHKel?!NVeDi{J>=WIpyBTfsc(5h>P(`nmQ_lt5Z-6lJ&15NJmA^(RBtzj9- zssbu;-bn8@GGWa4YzYo?EX?ajK@xvVqE@oD!KSK)!HV&4O$wV~X`*X_J3u&K(7Kvu zNRHv8!@wj^$|YKJ5BKUScpeMuz(KZ}U{b+Sf#jG-RC)*3yGrYqqhn&HLT(GoYp=2Y z4%VMSv5_c`Xb$4T`k0q3d|yQKe^e42e`gBfFdBFbYrl9kF9UTuz+4M_MbRt>B5ZT) z26rey)tWqlqqaY=mQ(7xI7F*iXTNwTsC48vZGTI(UtZM(OS+A4+189p0TZ-Xi>JX3 zuS-@QO*`#{Uk8IjC43Jjsr#3dT|3dPOKk@nClhtVr{+BB?Abkc(|7S7~ zOeI{i?NW>HY^z1+x`e{IE+&!=oTdNuWj8vu4Lua6d!rP8R3+XmhSmF%sez% zV@4A-T!%KL#LYXn$^S3FnkoFqKF8>q;6R|rTgb0)d$`kxe0Q`aGKpx$I=L2&mj zaj*2cL2FsvpU6R>i;neC?j*muIH#`H;@~L$W6M{ zh+J78W$6 z4H0dR2o_{#VPQzmQBdh%k;k*(A|gpbUdt#|M|-3L4rW8XF1*@I=6|!op{eg5qz2}+ z2L2A_2r-THoOUg2Ws(>*TW9d^Wwe$J?kz0C4f^=9SvC0MaOr<@p3Lik%wnhYpWAAZ z?77!EuF7p|>+Un{b>t=mnp_VJ{z2>c4;YYcskM%rn{jr#YOO!Z=AdL zeFj?(ZIn;1l^vr$%&tD)lxpFc!TCMlva&s4@auE+%=nSlzFvYZf!MrFN45erK|vw# z=TkoO|0=k2*K_sW=^2ww*^%QV5|v(QxJ1l!Du#IE?I$CZ33m}{!#xtn6z1T}dDIGq z!ny=Ue~gfk`k4x%uSkP;K>`PWxeC$%4lpX|Abni4#wGZg&PcIN5n}iy8g?8 z+Ytxt**CtyF+10p>;fk;qNx62qy?dciP9`6`raR~s|idI1K!cQe*{l@;T2GslOQv# z_3+#ISrAlu7 zc0F>-w4W~qBWV;4GD>l+N!jV6$1BD_K^5VSum|WpE zE*1F-`tRx97_{NqG1P9cFH*Vs{09b*vZqJ-wnP%zTM+{|^Z$L@`?iK_;_U+tG+j%( zsYQ?PYdV2n_JHbj&AFmKP@bR}6BWJAPq)7-H}o!>*6S4?FWHBiG)+cJB$Dizt9=K@ zYis^Jt-F2v7u|DPQ72Kj!GE5W1k5&MG51CR4{g1a-S%3)DcHQ3QT6<|*@NOBH6Uf^Dk1#1D)TmSid z)5)zua0WzJDAJ zwyEgI>Hsa|@=A6C|2DAU(EQ6oca=GS zA*|hSVLl==Jq0C{m z!L2AbT}6_dW;P_2m_T_E#aqG4?T1Kg>|@$QbE2d9x!(peLL z?fzV(^(Vxh&##W{zLO94nnwy)0bnszH|(3yP*!uocSlOcNwi(X$ZgHtbco3M=4UKu?DwAE3&n?HftfY*7b~*t{X<`DVxOkWLDQ?G5-dEN6W!B zo2y>(o?=n=i|kH`Z(F2|=PNZ)93TK0;s5%F)_-u!1XY(!&!9xvqbOxZrXFl9Fzw)V zQJc__wFS;-YiD7mPN|xxjasGow=+I{5FG?1!<;7yqW3_jJ<39VC9Wh79;ek^)tC_j z-6D6{=?$I{@R$i#^@@Xk&{M}-K(B0hT6pSnl;YI#8X?tlR_e)XX<zWzfHKu! zsow2&8;GM8s$n)@6gVJ!eAUexzm5m+zv&|}5mG1Vq6W3LM9l$zuDuUl9l)Ln&z988 z$5wBz+QFd3^n^YCaR-HAgK;%X!rc9G{_&;8(k{Sq&Nh|wi$yefB?DQbN_c&S{HC}y z%@1aVs89{QASm;bmxxDQl35yk5_TOUp=riqL5EelU)pT(xP2|E3a#ep9??XxK)MA_{(FL z+GA~9 zQ%*3N;x@aR*1Z^hI$UOm0N71=C&O=wZ^`R^4_S3b7w{GSc}Mb(VfY<1)!&0Nuai+$ zBQt41mX6pg$;NGdeooV3r8L^OW6>D29P}qF=z~v~(V#{hcVu7k%_pIV|Ebz1Xd3NS z&U+Vooy8^o>TZgo?p3-#gvrHOW~qE8maF+c)z-z_$SLKk1U5onxCK9L#+q06Z+J+q z>MBu1_{@Cn+!Bw%$HgmQ;9q&K(x3|aONO@VmYx3Yplmz9WjQL&n_S0$0h?=X=J+go z9W#3lCplV>d%Uhhf!hqI7rv!YL+&mKbgnJi53QFs>rc1Pzpox3WsdFB7x(yH|Lv6i z1KYEUEHAJW)iw5~b|mpnUeFmSHKgxp(|k|jWU!@uEgCVFShiEhW?>?aIIVr7a{zhM z)|nEP&6cBF5ixuCkjAxYx8h$A#?9annuXu@i0$n^B#2oeE!%hwT=lDo}48w&jfFe(A^ygjutSIU6L^-v85yuHpAt% zTjwi;NV(Z1a%5EC;26IIAc=fYuSrJ{keiEw9iRzn+<@ivgso!@e5j_$vNx>$$CD(1 z6&h6fQtIrcXL1zLm!Q$-g;2AzOraZ~K;(Tk*uGMnGFYEd>^r!kX|KiVg&8O=Z}xx1 z=synNVySY2U=C|ptC>o07L0q)t{3z2|3hTk>i=ApOejz!0CPp z*4swTK&KZm?yI93qw$}s%B&+_&S$eH9}Ts+m0qv83!3ZwJ6}=F)-3D=+@4}wryi#TN7IyO>El}+a22z+nLz5&B?@e^7Zqc z_nhxn|LpF&_PuM>wb!m1Vqk$B+aDz+(CO&D4T41urInQ#0CTVN^Bv)GBlZ>{HNKH^QNmX0* z8~5l8@hhu?{~ducAb1%U=9N;CFP9BZ&tzDRnw?qcItlB$RcU5}Ohb0^;e)ma?Jw-n z8BtT{abnV5V-4Z@u84HA^n+v^uXXkhxnl(u3CL=*#9rpQMf-b`~4Z!$_KQ*All+?u&orto>vZs}G&6#u$dnAE9*w{boN zIGvlj%@kK|K~mn?G*i~}fd7Z3U2)G=yFGr<myvxbWvNMftujc=ID%!-9WkSef2}su>RB1@{~H__xUb4Wo3A9+T+|V^O>4h zC(T=B%jBmneQ=Zg|2z7uenGebAecdgkHO?h_f74jVOv7}Y~T;~CJuiI=!+YjpBe|} zB;8kks=iKY(SZ|?>%Y`J_it!cg%>9X3|kphZjGant*yl|heC|LA2TwFrOGk;QJpOd zO_nMsv6Wd#u5ZX={9QlEY2-;X`Tdgz6#=pF<7LEaZGxF*gDdNIal{lU`M=_F_DkTO zlZr1zJH3j3EzV2MxunaeeA0iTOoPcWVfs=~Bm3~Gw5z~RAT9ogc}-41?MvUA#<-zE z0w>HG*3O$cpdXfOlk?bQ@gC2;!yNqX5YAVdb9r6Zv;C&!@ex=>hm)odhRE+9hY|1j z-LIn7+D%xvx33NCy>=Xe#{wtv)xlu7;#6dn*6(Ir?rDe9=&JI{l(wx;K=gzK$=GDA ztvW5w?yoUu!KV6XpeWnf!gh58~xTyN4JJeyBTUb+&2cOPO( zi|JZ#jf$;q3+=_qzh#L&0bws|Gd)Ax-8Zd_0QH{_q3o7+8m2+%-(g|d#2h@hOPm8g z-k7quA6$;-D?PuM9130f;2m`mtfgw9PLu&&SL06RRV3awVl+K|-dYt4AX z)Yuo1OsbBoFVG&fI|+UzX`KM3TW zMtL~qkT$F_3JMCA2VKx$gAhvw^PrQwP+plf)#h99e)0(JsbDO44{D@OiF69zo^1pu;U(Xfu|0p^U zgB+QsvS_eaNq%4W`jZE(MI9vXe zHOkTUHcVWSwL41nCthzooB^z%G|SPFX6Vuo9h-Z9oS^RhKs~HsN~JJ|TrR@u2Ib|Z z!@FBzkIsci5lis*CvAlkH`7llDI+JudYci1_w$pl@o4@Pr#{J?rWPFl0Q{CWoIO(( z57{;4^lagLi+}Q5$Ko#c>y`R=vWM2|HAk_tDqhE=EAcc$&oIN38kAphPsev1Lx;tV zi%CX~b3O##ZLiu^SPT{?Q!4rXX6ul~G0DOW^R--_o~-3S2)w@ss^sl7Ii%;nxI5}f zQ-tb`s!N6(hasj_r4RM%C~UsvSV&`C)*-jt16OaZqxCj82mh7WOq`k0|LGKIusiRk0~RaSB@WB__2%%EE2qWh{$%cHcVZl3tAOAH zbGD+spbiXaYGoDj%Vs4?5Sod|?> zgy)5d;>=n9P~IfFe50ITI9=7exJmM5#t{*1xtlC9G!FAgG;fT>exwrgD~=p(R!$tk zykIj1d^GK6d@>A_@Af(qu6L3l_wD}!ofR18Pu^|h9T(*JW-!eN_m4Lo*Zs>TG#Z{= z1xnf!;7TGsjv(IaK3;KxjKYVpE7g1Za{l}}!M5r6>5S^FEXcl3Wf(pLxFi2_hldsI zY8@pOw3wwZCs;v;-?y`2ALr+l*J)QgX||rX<_%1pZ|zqW8`8Wf#~c=(7f?BjhO=V0 zlz3_nBc9Vq8H`yg{gb7L!rDD~vH-wrd$H->LDr+o&8>ST@_<(vP&v{VG$3_r;HK?~ z;^28M%s$t153H8AH5jA^{r2CS+XvJO7M`ktH$dKVwKGuH^7zwjT^2|$n6#7oKHUpN z)M4EI@An`<4~n9d!qgafI8`l4j(GxKoN3i8ae~#wA(BH!F49cZK~g0)d%4+yf|*iK z*56>ozy%&BZLIq9Dz40pTgJ|7d`zT8#MFQJWeM;9k~Fmo0~7Grk68nB7|yatms!V8 zC0=BOYmsfThc6lF;;VqeLz3eI!2-9V$(x>c;lHqL&y_p=0r|b6;@i^38;$00(Hq^J zkK_?$Wig#47iSHe8(_i*l=Dq*c>p4}v|>$=xM@Ivdpzk}t1p3UTj`Zib!|g)*L^oR z1Q9gc{d$^!QJn7Q9_{+7#AxQ+F_0N&R3c(o`-B!r6r|?LCT=!MKNy7%*tR_;`2S`% z66!&qE?*2UO5#1AS7Z32{JSyn+wik#UrD0_L6n6M9QTL2R$*Twb+4Z4&bKnX`73R; zIK&?RZkUt#ZDr)!g8h^e*E-|9zKL(H>+s(MhdhDXD_U&t6Bs_LbNc)9N9-BGl}@sPJ*A}{3U2xyCikm18f1)) zZu#(@PW%$8p_zZWW(YzIP|4VTN&BM5cPdop)Vkov(^@OoySP1w_Gk5Ay zi$_|7lPbt}i!Y~M1+5dnK^O0U2W4L71JIheZZh66-jTXkMD&F+OdqOoxj zUVDdM?lwiGr&vzPg!~I`19?#cCk8rry zY&64$htu^$O{%_YH(|sS&ib;Zy9 zn%VY|mT}(n$jytGWZc-!${=HMkQafEEqP*+o!eF z1Jv0V)6ILmtaBk3k#4+8KT-)u8wo}yOKt>*ZVui1v+FANvBlR+YLg^GBLy@#T}?3k z@-#OR#h?f@Q)fX+>fQe)cIV3D0wT^GtDp6wyzl2~+aNi1>l3G=E3L-%<41BbxBPBq z#*EA1yzd$f}=-@+(QNnD@dDR*(A+X$)1*e+3@^ zPpCL$Mf#Rai|Q;+>h$sR^qs|-pwTIo#i-|6Rx7e?ZuWDxo)*OcU zb3=m1hQgNCa3**ZST;Ld6NP)dZ0M-EheRy>xayNmf4{`wqCUX=zg_^2q!FW#Huqne z`75q$E1gprQ1H<2{>_^=p70Fd3Fe;9NZlXRJ;(1uCu{8#ebYp6NwJv`MG-?Lhqsxo zQmnisxcJlGS!=vzeEb7pE`5Z5V<&(gb&O_W_#mA9j95R|()gTrjP+}{&Kt8pLAkvW zOAe3Epk)s3s}r@fiE5~3(4lf9YR@)i*3YMFe{61JjxUqJqOS}s{w4m#e}>%(W~`FM z@80BOYZ;epvUd9@r{<-IGR4-`SXA>7qhK}~Hce$M*RipGe(qo{=Rm=do4!jRJyO9Y zDQ}_w`T3WpOlqO)uEyYX9YbPUy<I=t$*x*}YR_*oHNfy+ zD7WMx4Qxz2hJMF0BS_G1{4eYbYzItqD!B$A@yQql6vWVQM;zyDkf4MWO*-Fb%sLn` zC@rd{;8B4ozCR*x!g4W8i<7L|RD5H&04pZ9V3+3_P*kuCS=NV#dLeuY_nDqc<({iRu{Cm4VT5(4t7ho#eey zlf13O2Sks1zIkcd)j5@3nJh}t7m3C#Et`*bq}wn4dV%BuiR*!IG*T@^6Ha)q>Mf&TJylDFGRw~H@fVq!*`9+oj< ztOIj1gQUa`#8!BzU4RY0Zu;fWf1xEvtJ5vk+Z48)b9Gd5rC8^t8-y{3Gq!sS;-ky40-)Zn!(2D?QifR26#`VWr~zS-2_74)w!_%W6%G zV?MGhUnbsh&+wB3FeLET%C^vxgWO30#^k%+6>{l23isavgc!uU{g;!^RCbHJgPK;? z`DHRo+onj#4@r!SDrvr>q|E5=T$u&6SBm}*JKgb@-JRcHn$!eI!)&~818uhO7~un; z-pj;Q^h%{7L0dGfYs=v3RSIB_$(0q zR6y(*Tuc00*q^s{bpDZyL0w*9GCZH=94)0^W$%8n`n;OYgogfJ>GRh7)fc_@s+I*R zV!KwNNjk?;ug}k0KIw{|thL#^eZdjCknd-jg^B@EzfQbYTc9RgPMQBVDe+r2X`UH>i?g%4_Yf~awIoZ(-9gOe@r)yNfMbkWt$;Po4#DlmR?A1ACFHR4 zl=z>BD{WxLp0ux~LQFcD{iRg$f1Ba;_=K_DSRcG-r9$Jz#FUAwJy18ianD>Z?Ib?0|*eH)KWw@=a|6C3a~$LA?8mGF8mK_exRvfkoy z@O+ttoHIpqczd~jw(dPRuBULqdD(NRt~3d|8C;+~ck`xQPD-Icj>_rB?&A)a%-y&` zS~$Z7LN^Tu{c0BIIe@Vdt2IZ5o^Jc0O@vLv9>6DQjMBu!#MRwB6KA!~-39I#8|3Rd zo6lH%e5GTy|BkB}T4EetLzDYBL|wgiE9rT$q+{UsK&dK4hb>s#1Q*T)XB3FsouEBW zo)UGphZ2CVq#Ni-D!L9EYsgXw9e1$ngsim$)aYCD=RTC~p}en`t9lCu zevhp!Nts)P6VoOp7u;)3ar;ob)Tcqst*qkZKd)Gmav`4iA4!8}NMAUn6hivV~ zcQi{Y+(9Bi4|L>N)Kls+IkmKo@bG6aI0T$dQeNqhI%4S!%`IRExcDYTvtzMVP};wk z6n}lZtH<4`@p9#4?BxV+o$su-oO=~FNOFPnyqrd18}%{5V9|x{kvsUA|j~mNb}M{bECA2)xvoV zGK=o(hYJ)-g{s;4t+x1jK0`uk9a`b0!IVBnb~+iO3Q}->CGAkq!F%4QkrK3+@DfGl zP#t}a>}oZ}4YH3dRQbQ(N2_c7>|(wycKpH%v%uyE3AOT@!%ed1T{%XFDoUV8dd+N*)y z`i`kr@qIv!ML<#oZu=#<=ZCn-R{iY2h*TcrA~C$bX`0B>)R zy+_>ShDYZ3#uZ3jZMr&%=rcN}s9`Y9r*9j)mr7Ht4rGIe)dQxPd!F;m2)!Xq=LJMU zcyP{c5Kv|Oia0RRVQuh=e<=rD?1n2XC3p4Mnl9?Ow+HEdlQDC3s`C_rl<_6UiyUV< znL4m;N6&lZgArai7<;PHH@SP3+WrM$)0(!L~9Qfju;K~yFDK0{=Q9m|C1R6x3_oE)m zzNaA1gIh~lJ!CJ#Yk7I%0~dey_RYcz2!y~N72;$@tzs6ZIu7un=%WtZN{7;d@e;o^ z8%l6ZCJQ*GE(h~lMIj|fACQhGDyY`2u9egOJO_s=rD1V-9dzQ`;9u={1C!+<4GGp# z*LJ_?x=@wv;{4Ym8OwPmrd4BB)Na2g@D6~Pf>9PDD@L3*ex*a0<0VX?rne&%9aKzU zq0RpxaqM*CVwy_+jlrxEN=#Su*bhyj*a zjUI1bYxhH%9-g3-g9Gr-n{LF&?Vw3e1O;73Q2Cm`-CZVwmNO!Xnb(fb7Ys6yHx7W4 z)O6B;siR3*?B$#8O8vJCZjXy|>_8GS;$yV}j?fe?N_o??zMtA29F*kutu#M)7EGhM zskE;-PQp%BJ4>*Bxo90*N9kq(3O9LYA@Re?8s&?l^fDNBL+6obOJ2g$q&;<)k7h5n zIA_Sv8gb0@BBU9m%Ghi#yIU9`bLSE5t&Yc5sK+;?PT(h){U>nrYUJNe;pD8&TbFf& zDhdP#q5%@izB|P!}6#vrj zJ-Tpo!d$i1+-wUJER5|NVN^hW5?oVUQD}I-Bv2JSN&sL{e-57aGG#d6Bmf)_Y(ZNW zRTlB$Rm}Ea*mC`t_Qb~Tx1q+RJq>zXpgNT%~ ziCy-mA5_TVa*;oYIb*QK`KT!?>s)W-+9bW#AOlx?fA;{F$2wS~U*;Y0jmT-BIO=u8TJsnzhc76x;11n;Ddp zq3Q@K&!5r=DCcK|MO2pvYWR$D-v#py=Cv~&i13ca^{9deJS#?5{5?YhFDd;lWK3g? zCpLWWJ@!*tn8A%>(6Z6y(@cn=fuBZ*{0jk8iW_C!hmR;;2@)J8uv8HRddh z+GRA~3zKG!=t7#=R4VCd#I2CbVv~r;fe?kqy(Z8U!3EvClr9K}(Ek;SRf|yuOoGKg zNLd@td#%g;Iq9nkTDZUu7FW_1D2#PBT04#0@Wt+DHFw=ZQkreg92`T>)Yd-_LR@w` zQ0|b1&h-o~zdHb)74W0zZi=>16 z&H2&vJV#%dr;?(hrAnMrrGuDZ?LwtTKQ`;4p|1Im+-t7#iwv22iTePd==gPJC>|A?ut39MWvp^e5)0m2#Kq7^4lf5R*ZDdHyixCu0AujHBrCfQhBcaSYXT$5*!Rg z-yQAz;Q|)2US3c8U?b>7rVngiLXJqYAKBs1G{h)atj)XFsp!Qzb;|yPNX4uQ`DKz; zapqum5tAIl&(OeZ&FfIu)P@*{?T^3V{jAhS4j0%XFO?zD3O#_-2dnpg;s-?*g4Sle znXIwK0=_{HV?%~xYCFrQctL8@juyye${YA>_7XI+?B@Y$X5#yC7BD3E^g+x2v=uMh zjz>q7s2U|U3#*&INv=Aoy1K|ltad#i5};fo24Ki$MF`J^#p+<#jV6P)>YP;4I~=Sh zDcsZ%y6%fdbvT6}g1{=5pO-UsI%a5I?v z6`O0n+QKd?mn=@Xj~2__Bst3sx0^d;|XT*s< z=gIqw1IeM=&uX@BJULWF4=h(zTMVc+l}Y399e0_6Xo}U{g9cA&3fSs*_ZK$@sk?W9 zu(i!k*JkT&V{xgMpAbaxrcMM6_=G3^`0(RMs_3XX{!G zht;zQG1H+n)7->IgdUI@fYpsg6RUakQweM&9WQ^Cy92g87y5y`Ct%px2v>5B$Vb_6#`;s6 zSe)7|v6`=?x&kEF*-guBxpL|Z4fez{VOl^?FTmN7)w`CEX9MF- zeG$*PrlKpH7Vf$-dus3Z9`54{UpuuRCrnJAVZZho^Gturv2S=#|Fw*t-=vI?EX?O< zFEkf|Du2N(Q<|!?m?0y6%kxi~u9?zq2vs%E4MZNSR_#IQ{-x=18e(*y?Z7C{MH?av zV7?zEd2>!ERj#}(n@Z-!MLbJ9HU$PwL(XKE!|JUS?(e%2{}Siluq;npcc5dW`)j3i zyIX^uD=4U@`1w7N+4*piG9K}f>AB0)!jK)!CPYlxmg|Z>PCwUVrIgxxBblKiH>k4h zdv5i5OC^83X~)1=J0V56Y-~-nor?EHlGek4+)fGbXs!?bCHq0j#!u*XwONXEnyf^7 ztR!tfFrExz7$=By*+`g~<^K=&Y3F;5)%*#(owS+SkLUf9ts}iLp`U%x_(HF*# zD_NQ1m_V$OfVT^Q!eP%>pRaz1nyKE;&Tl9{2#MgAnOUr5Eb_bK5aHO{1x)xzAbOP7 z4xmxsy5W7d{3_oq&ZGNoi^KICa!WZ55%8HU%Zss+)<^~A+T`(_ms7?;dS{BvIZ~DY zwZM|^NXN1ewk%P8nwkuQ7K#}Plf7+nFY#NZ?~`^~ZqEG>iyfiaz-wt!x{|C62mYV1 zAwLArY%hWU~v_J01z&l2G&@K+9vBQS^}QY^G?gX23=#<)x1%&j~LGJ~QH`#!S`k{C*i za=;XlO02r`sEvs$sPh3|+ltIF7VR9#EDmoc+|*vrA|wd<1Z*Lje+6WE?@J^bSAG8q8!9a+aBwS z>Le1J#}kfS@c0!vYrmUoQnorJJpoE=K-y;izah+&l2~&@R@`NL3dia-5mOaL?5S7~ zQw>83Th5dq?^|6>Z^sM#v%Q6O)ITKG8>oTZCOaqJrxF6MhaOp~YFo)io3B?*k6Epr zQH4p4Iv4_$`FH2#z}NO9{=4r!=jT1j7rTSsh`8;BJBlZIlLmcnbFYRDbKbTrMGHOu zve+IRN@o-E>{Otu=;4^6-l4`P6#zF`22@sot;x$)KQQLfu^j$5+DW4`R!FUcdx+c}?`DnHi3S0y z`)V8Mj31lLUgBb>z7&bDHD_Vef&WgCT$Mliaax-Y`jO{{0Smrg?GD!-U)N>#_akc! zOj5eZk^)9Xyy!}z-a_%ZmZs0DVaXQeYd#wTzV{SYWwik;sFi^G{AT#+N?`gZGTl6N zn1u;}{oV*J@tqu3&~bD)Or_?zu4Y^Oiq`2J5{Op}&utT$NNYJK_NKUC>H5@GBWo(@ zCfVuBNF5_!29H*$Zy)rJ66*S7VaGl^U0nP|Z)*xWnVxP4_!*_21#mpCWYL+x(lhA% z9&J$9a>UfRo_4+-u%GKM^y(N==YjLMR!e;;s9E;=p?x6uC=+00HQ7TSimlNu7W8Ga z=$s)0+IK6AA!|6v@ne%Fjwb&%(7@7v&td{1l}j1lu{m2KB#{-L7xvyyV2RKN`|}X= z5-RkEzQMT0b?bXv2gWIG^St2C^Y^Fbw0=|Z6|Hu~WO|xM=*Y*5nEJ`Vo@`Uv& z`T`ETwi$o!4?_?u`a`~Uk2vI;sYYW6k#iDE@<}_FYTq-R8d0QPBBi6C&jd=!uKtE$ z*{UUdfDr>DP$4u{zywYY%Z#ZC)QyOhR1drea_qb1hzp8vt{-#x*ZK7W#{%^QQKfvY zzcvr+hSAn`v!SUn@wYNxwYORap7kSB>5sL{9g`Bw+_E|x1=mw5ZwRb^{DL7RzX%7! zN1la+-WY7RQh%OYQs(HRWrE(EmT=S{y=fDnYMBzo={%yMvNL2*Tj}^Zt6e7A+Qk(J zKtYO>wtvOe(Ad9J;z&dSIQ$fcn;Hz4Q`K-mLUaA%_KXYp-%eO`b&l+IF5*xO_20s(@n4odoe{ev#r?i zF@2&EwDbr*8O=_{b8*;FM$upM;Ak*t1qDoFf-u}pH}`VyLnHx#7aNcBTg`YAsjjX~ z=nm1~ytCE2%`fFOdf(=fD9Ycz1nQVNef47d@1j1r&?4+|5gNeAVGaWYK%Mf2rAzUM zNcIL#4{X;uqJMM+ZoNTUS)DU`fNnNwu&);Wu)ql| zaFeQDq)pT}y2i=^Kh+CK9&!cAXU8U5_Y;`tj5Uw!W-9yYmcS@dhKDCO(b{u|qu2FL z{zUYzpmBm4tua!XN{n7lpaCy_Yz&;DDC)pL9&}9hMu{TyEcz}9i`}3OCB#ERCRYyY z_WLRk;guAluQ_n%5sFJ(VWjI(_1oPj2&F<7G*V61TtZ4SmyK^^7Gj@v`AofSE@l){ zU8=+B;ZQdmMjK241E0=MNww}hKhJHwHfC)Ddwloc(}vl*OzPspLV~HFa_6-JcipTQ zhZbt%Z}x}UD?YZCvktfYh&V4cLy4Mx8|hd=5~rBO=GB1Es2FN>v*RMI&L*Eaf;w1FS!0Jc0jixsKZ-z zp@Hap9i22!PfwSG=kdOW#bG8MS+HtB-jocuy1;~&e0_bB8O3&d#|lkKx<1^OBkyK6 z)x}PD(2DcPTs$WPVDjGnLrFYrcip3e_~6I=yU}O&_ULq7{S<^qK55yf))G+4fX%j? zWz+1W|A+iLFmD74JGTzkxp1=6tHF{@;wHfpoDix7yb?CPB9RF*| zfVbG%`2~wc3x}lTW!$;n(vy+-Q6P^ec#RD!ThhU1yH$%rXPQ#L>rP{Nfbi^LZNK)S zVHDp87CV)jOWnK6`zENQ#mEHnJD;e%)(uBVFZ)kA3s%Rfw75}KLe{KhpVSuIaV&Ed zOIXTES=u?G9;Aq9k^Qvhl6H`Go;&FT9CUq%pwuEyB~NP=w&^L|-JjwakjOf1ltLS~ zNLt7N*IW}xvU7|Cn%i8Xsi49^PKR~e%7yTv@f`*;mFFow$!ZoN;wqc>1UXw4Gi123 zTJ9wRq%eYLw6_0)Cl=$iuj6|&4Rcbg_Z2SOh61QeQHl2L92x$v7hnm}JxQX|G`|^4 zY`*90+vNa+J|!pT$l)_bi~jDajgr&lId6;IhLnXC&U}N&;aXQCNim6NLSBZrSJ2v; z6|-Si?^+|yYKtFEO$#1j7tE0rvf%q(Z)^iN&QvXDDR`j#5B($ANR3Xou#)RvRhXXr zU*q3&%12=(6`?iunuKc&7V`z_1R_xTR8+135r|zxyv{M|{y`y`qW{n^tq60#;W5#E zi|oz=c8w0;P?Wq6?|3DycgRl+Y~)c_*pf*A8CiA5!4g!Qwnw5NLTaz+nEpGJ?4)|b zQU-fFU*U%4Z*j=(mr)B#LN==MRzw znogrmT9EEWSd!T69N~!tvfXh8_pp}<<-t=W7zs;Kfcoc|A9u2Tzk>L{REC{=ZIym0!OBOD*1&%geL+sN}TP zSk-z7u3)FqHBh1d2-bD2EO!vS!>Zq4c%H8Z4oj&sveWzTI4Ft+1U@EP>9DbUp3V;@ zPvvYkTCzU3iJ@Vxj&}!y(TfRUpbXK3y{mvhsIjWds_TK9WN4$xs)z%zB6|~YC9<~6 zq_zk~8?#L&UF6^8xFvN|70q6yf}WJKoBi$ zXaZV*p=fS^mCo^LPx;EmW@qz1sk0IB2=W9A?_0(l?_1D>d#})Nl5It`XoUvRpZr0V zlVo{VlheKTz>(QmM4V~!msM$@F$f1goQUI}PGWVX04!9t{iiaqMKWhRd#<b>SC`kz)-NsN>MLo+R!^fM?)Teuh&zgKCOTSv2lRYu zghskLtpNaFJ}`h&+sHY0SD?l8q_TtN-{Idw`Vb16jb*`c$InTEo;(+Z&pUZKfxoe) z*XoTv)Ip{dMv#F(ysQKGwZpUef0&-*wEV)l2oVg>!TDGoB8@% zflSh*KcSQ)z&$@*Qd^{GRhVa~fMTBLa>2brjqsK%L9-XV<`GAuH-u=MA=NGtH;%YN zd*D!njNY>qi}-$ljqV-X!A3+6`2~Cf00zT+4XiminDM}`Mgrfdo42p4ud$;7oV70A z@sLNE&aaDz6k4Fh!yl+UkDYmtNzctKvdqM0(~71tI29B<`*DM6nQav99zF;#9v-uE=!WM}^`zK(&lzas{Yq52 zC?y?$BR|N`lzRB(usTD$79^ywrNRHDJ-9XB{4flXwLf16`*$R+L+L_AUd&Tql_I<( zl0ltfy49#IX3;iholNVl@XJ$NV;2BWBn8@xnbdwvOj2q!DJ3@~B~*PaR4w;Nb|dw5 zwvoNiRDISEwJ2ImW?LF&N1wbQR3Ez>3zuMetL&LlE_tSgB^eEH%;?fkn$K zpa1LtTbVG?#UH)hWLLVTG5qLR?QYE2@*z0g!KA`Z)Se$|C^B`CJU*eM(2CN-BQ{t2 z(tR#R@k`JB+~`{n4XM^Y#R)Anx0VPd+q{Y#cTN;@a{9-l z)c_D;KpF)cGq^1;zlK75N-{S57_{zC2@AdWAq!+X?L)GOuHXD$HjeAWu1IRF4LC$M ztNp#snxrlfGOrKU?+G>$mG(qo>e+)Z#Hhf;Duxi>JbH1-6KA!^U`fp(a+Ao`q_|t z@m{t+yDgG9Mu)T`ZW#7k$2~bD?U9i#F;2MR_2z=%!R-)E+ex>(_p9+B2DYMv5kIzU zyS(B_de(v--Yt)lZ)V=hDvjsg>MI0n#KB(p{sHV}k9r9bo9~S0V^DEaFZ-a3j}--v zDpsf+tF8$GokcgV?K0gSwn2@_Sl`Mu;k#W$Z!T77ye{SiNCHN<_;94CtI_QgSlwM| zK#JgGdh&YrLcfW7NXr%nOHjkjZ>_Mfgsi2|Pl70VY75B{sgtYq`LV97zN#`${&ZHh z_rNU*U??D9`FiFScEEFZ_wrMo(>>kD?mIZ>knO+X8@~pMk&{(B$ys8lCCg4B8+P&6 zZby^${;e#*Q4s&Wv^mxv@>JjtmGSo)SOnvxwL{6Jt||ivw0a;HId*j7%t@qFkhHuOU?SuU$NhQe6_5eVv<>(v9}zlur^i^`d%KY=LZN{s$+}vE(uVIH2-S=iJ(GRG!PX|8rpc}ExTOU$f4>a^#Z>E}ao`1Sc`E z%p}F(YdNN4R9Bya?1xNu4RYXJ3-zFRGTi`6NXiyW3(!m(0uWJWV5%IhfLd( z`d`nHrLC>0zdy)WVXkQf@|mSVasI~cfGh~0pV7T8&mygC*R~FB8#mK1jToQt_hTxn z`0L8!w~YQ&`Olq&F`QBJ;jyEpvjRNLVA0_HYa>;%$!1^eSmWs~i<(!(n2!Bpd^3BfOt`_MRj=Q%(hu^5dysz+n-mfr8>#O^%l8y{o zzhM8BFsyvMSqG=w)Yy@_Z$8P0_I7;@kVOkR%nIhboT)=d<@ziJ*)-UAN8%{<`pI{f z){%{s41Ek~YDad$JpAG&{UGCzzT1_BEB(MtqWV(HBXk*nva+nFiuL%_I6NGPAAk=~ z_k!#(D64yT`FRtFWjcXcklav=NYuTa&u!j|)PSk*!8CyX|3nu9H>s^x^dwxQ&2#qe zyQ_aMn^Q-aj4{;-BDy~O(uTsZBj#0ZzL&@ewBFs~)l~Jg5*(P#r4b_534A}4={UyG zO#`dk$Go6S#nCn98K^9sdE&ygnn2}=oUm1Mc%khRm2v^vIzp2EYhlXoPB#sqBbmqu zDU~((B@9#qNoFSdOD%0i`{1xMR~MX6DEbgu)L3sle7=5{8KJ&*7Z%5zkB|Gup@^Y7 z33A?I)I%5M(->*`gYSHoH|ujwLXUHX$95?GV6L4cs{orIQzPpBthyeKs;%eGJYR^+r{RRJH{7&$fG&l8kOaj>uJfeF3L8?`DHdFq9%@Kgg)6b z3)KvAi`vPXlrmf9o zWj$!v--KKlb;ekm#s*AltHF_j4HK7646wnLGnL*$0y_B$dSt|)FzJ32o z51EP$ixRb7+gi$)4=3S6@sf&5Pn(`2BEl8Vpr?n)tOB&BoY8C`%+JT4E&2UjMZzjl z${_iZk_kb;VQjRE-fyBSJuIb}xhXF=S@p#|phkw3pFi|W;Z8c>n%}hHYSt*um^lv&wFRPiY6%0@~tcIY0sa`VMZ<4kOnH8Ka>y;BjdF7#b&Y^1)w zxz>zE(@;N`9VBa5;XJug{k{6Z%zpXrU#&Bf$>|sFIFbIi?9FOg{IZ+E*Uj}$FS6`> z^);T=9)oe%w#)#tn6T!68PJwybn*X^3zmrvcJ4_*eN3mLLV7233SuBA zQ(UUvl*!lt8MX}aQ?K~?IROi$_&S>rP$vL6VR5CeZ=}%c74+LixdnYOqac*^x(==E zD7cR9p3?i!9|;E_ATLIUN@sd0e|u}U)7{W`@>rR6zm*(Hu2lfG{&6;n6U@rg(%%LD zYZ+Nz$v^h@U4H2i!vt?yQEy-HRDl0A^RKmnGOf4vd@IU~e#Sw$$pK+!Qq61Uar%%_ zS4sSF*sEIF8C0k;!-V0#+Uf^gVQ$#R{TP&zJSNi|qL$Moq%d713P-&E(L~xAY7sr; z6F)s0Jqb=t@#XL8tl#OFDi`iyI$Js z14_>6q7gd9Kfs-uuxXk6H00Yz7df*Vq-sZD>6+d;G0{xycKI6iK!Qu zYX6BL#KLBY)n()j=fWg_vJpBGK4RGV|Glp= zq3ySi3lvuF4=13d+fba}NHrW6$5S@acys|L-B8XsK3302aR&u9t#E98DGtB7h<>!U z!Pjo$!@r!=JV0*+6RZaz>c`k;p-F44(jxy5|NOck$#sO|fS*Deph6K4jMiE&XNoWWg*7W+mlf39b37utS>BFVvF8LJIwEMxTs& z-nqwZVy3o?dZdyuR!QD5h)(bo)tfo6lw!KDR9dqi>R)nd@KNoY{5U3eh6kvc2i+<2 z28Ge2yBTGlQD2W*Ul~kUS?y<+?tn~+!9wC1i#<9~=E~7S7eo}FY@uZ1t4nd;`{Xa8 zKZ3fyNUh}nK=_#~*4a9DKkt%1Z;7Y=TWLF0EQK}X0Sf> z-Pb}cCMO5ZfJjlp&OvDv^qjSP-`^fxLEN=$n2j)dYHlIdw0SC_r4*OHBUuAnYg(i0 zYFokgWE9q7m`&ppSFW@gS^X0fg|+|(nygNOR3*D=xzrK~Si{{{*PM^z6BHEV&@e9z zMN@5nwQ}chD(fByrZ2TgLNWK;F}f8X+;xHuwLx~0Ha_$I)w+GI#$NopP@HyA#=0Ri zLf&UL5LEyfzB#tF2lF--`RI-)=VN9L5L#pxP_-R>6FXZWQK|92$wFUc>ntj^Q*t| zF3^o2WcaNQwN9~q206nmIWFm?CQ@Amwp>qUn89FJ0;M(kdy~9)alX9m(s-I3{mnH$ zAB)&@x5nv6I!vUC>Frab%6`~x?;P! z6TEz>a{b?WWt2SwtqgQGGSJ<~J$t(;x{5;4BN=z7PHnA0OFvC^$RN;4AuP#bbw+A^ z>Enat%nYKYV;f3C817ZYb@r_d2E#4GEvmHmlPVsK1pl3N@uWpgNvtF&StMfR zh`N)A{3KDCLuwDLHJ)%NiUdW~fJ-xq*n(J=g|Riu-BUrW8eFBa-^)jF-JCIArdHs} z{F?AkqqK*g*{J~Jc;@ME@b$m>BRaY*4&KvETdPMbu8|f95hs%=kebUeF;?W<#R8q( zgM8wbeuaU}w*e61%u%Zqx;Hr1nbh!qDp-ygD zflC2y;I8T5`@-pgT8^Kt;RGgthC2FT%d&7?*R(o=!C)}dMlTpg^nwA%82N^qu+a;7 zb2MxrD#2EoA|R0hqzetYNF+OorzysAKyh}mQdUb(iE(jX=Ku3aSLju=i z#wC?0(Ag4S@%eVN#M#~+bKY3R-`?s{vQRDpe z5>}uHlr4=w33$BD6og+4(zsTZSzX0xHESge2E!_FONSB_fuVGktkx7PE7Y~?+75nc zasUxb5Gb@zH0Kq2ni`S7)v8EHDN#xxr8KS2U@#aq7;BoAPcI&W*4_OuW0~PPHFmuM zA!l7)K`FOYD5Hzk3MnjXKxapI{ri7S=HknAIpZXhB3H;`Ned}FY>`8F#l`QKUfF$F zYD&TAyhJ2E$q~mo9Mh>?GfP zW0I+iOGnb-L-+NuXQ1_}f7{&>t(wleG?86W+ybyINmpx(ZS676g+!^>FV3>9ulbtJ z|IpqJ&OURw{PT&CGUW}sz@z>t;>enl zA6jc8ZW#;)!v>_jzWX{%(fb9is=bbNu-H8u0igifw$VzNhGBS<_E_K+E4|?-54HBt z0u&O3<1ZOTA;~c|@**wSSLu?k(jZ)HD~qKxN=u{ykVtJN1Pp77>uM}1{jyk;#H|95 z=Y<#kmaqQVZ?kz*BX=BVrK2r^gf2 z5J$)ynpB|Dx}oLTCVI=TPRAozviJOmginRi6h8jtBYg4Wcla&WgdP|StA^5=QTaCi^_zU`_(h)TK|Sm;^kNa@TO3a9#hc*_d)^u=Gg; zQk_tNeQ2SC##1g?H&4=YQMd?P(glSiiA@AcMsQURuI@$Ayx3kPyt>xdk%sGwRkd&< z52P6#Im1&=eUl3pU*Nub`)FykX=$}_Y(FB=bzyEk&&-Tsd|Ywn)GWX8so$ZiXAfF8 zA}opaT!dWkMr#!k1~gaCdtor#pe}uhxd6X@Pq=3`7!21NDFoS^OGCsWRjBr=j*Vn^ z?${_la#ugLz4V-0!S!g2TFmGDzi*7&%%t+mrW&#`w) z>x!=<5wp2(TQlE3H(TCzW;D(2P5B!xaDl(r|M3-wT2|0GDioq>Nt&+NhZ=5egb*mD z%pAC3O}RBAxO64OfG4UaRO&TXeGx1u*exIfG~ws?MVJ%@27|$HE0V__58lk690`37 z4^HdJJ{0)TDyk}i>lRy;ua~$nh84G^?v4g7Uc3YxT!&|OwRWw_GoeBYBsM|>jvX2y z9hB2TOW$4S#^a2R&ylbbpaco&V7riK8%1_dq-VjT!NMeR3*R#n31fJ3eVFz%(h+Et z=ko9?Jn`h8a^d0;`ZqVQYfqfUWRYmx!jAYED$?m3>3NS!7v`Cquz2r#{tb_Q;zvO@ zf{Y+^rEMxtBZLLWpg5cgnyU)FOxrcA(?Lo5-@bHskjEBH0BbYo8w{&VG-8o+YcG5H z?ultObtKupwROqK^H{{L`uo|e8wk|P)7cVZS66}~L#gt%@1LB!CV>m!-ren00@tar z4EN0D=xtA2*NOj?^h%G_DQbQ0@E{hIl-o*cq^ww}y2ga35Ey~WP{y(>Q=Z4LrmSOu zYZYgXM5x48jVw^hol7cmhWAS)$Q6Nn__4Hs~bBN?)&9hE#a9qYizk(C78l zUr+F-L$HJMs=Tgd#iU_9yeygml0v9`mwq!3oG#{Il`JS%I9ncyj1U=ndOixYG*U@)<$LNhQIBu>wCLz-+sU4obx^Bd*-?Hmw!P>0tgO43Gf8;(xhbIx$CAXF5@9^ zLf{4K!5Ph}$a=kG`#-f!s9 zBL|Ef^3)ACjplBN5<6HV+=i0;k9_<&KK8_^a{QJkah-A7-a5$!!_Y`-D|QmklV6`> zvJr9gz}WEf_tlAA|NM)~U5wd&-cF5IIdPydKyW?x`Wnk?9VW*ryZ#=>4vaHVwXC$$ z#Uysht8-0`9~$36X7TFcpqe^qh)8bz6<^zFwbrjcaXPa(6NCT{STO}3K{t92yLVR0 zCG?g+6h(BqT`HAIS;X0H?RdL;Ip8)Q7}j$aS}hJE zF0@sjB-Nbh*K70_xRzJg^3fO}aY;_248N{s6xG$;eerC`2%=ut+i!E_H)ZVPo8pG= zmrZUfP%)M^f>p(djxT8P0#<|D?eJRhEDyggFx4ONV2G zgax}XWe&8LSydnh0xwh(IDX(T-|$^xAhsD9Cb}UIk?x_xdd%)@guJzEoOC*B78H{- z;8O^e$=l%D>z1#i-8EbSf$e|cekQF4nq0p00{`iAzt6c>pXOU0-^SP z_wvbSXY*rcE;V`Z_Vx&ZYg8O`L>y5z*aQV~%LBi#%f4ED3sVqm7I`Tqr}b00WR%}n zu8k!!L{UVNBvdMu@?n&?A-HKPxON*pc|&3WdiCd}WSJo`W@O)$a*Z!>GeL?LQICj{ zREIgC-v?;BDdRWTRc>;UrMU&h4S7$x*cz_1+N?W=7r|u?p>8qLLob9XoEWQ8HN)@U z?{a-8QQ{g-9za<^_~eCGSv5j44Cn=vSA&3M8{k#*zKu$s6X}>LE~v9wuhWVQ^=^eR zm#_>>+cfB>K*Q{u5WbFIbjx>aq$6&M0yDji&p~g7C;!H;Kk94LYc)EZ4vde#!TXb4 ze_titi{$2>_=Cft7tdc?N5|@zN<{2EUQ9L@G$rv6RKXL{p~{M@FkRb2q-z9zj)(RY6#y*C4mSmL@bdHt}H z4bmzUBOnHhNr|nCufN2ffBOIB?8{%|jyorK@WF8^Rj5{#FtB*9%+GgNT~@AKam>uD z@VCD2r}&n~zaRAx-c>Mm#N5a-$cGz~Qd(;z-ZHaG5PtQE(}R@{rPNZ15<5cTJWrpQ z=TDxy%))va9k^{G;^g7Ukv;2saLzMX3v+_26MKH-{YQE3wQtbLqz9kBxW@A@FL3`I zQ&gfsHrjh-q836%a8&}K6Ys1`^7Letdk!|3U0ceJeeT71-gW(f zE_JxF)aJNF!j6M zh?}ninm%TevniW?DN*8P@Ygc)M+66~0cV>{I@Zz&(mn|?G!3_q1)c@(y(mTjue4k1 zti>@)T}Q_ygi%B{Nx+8qAcCx4U4L2(&rqs42hOJ?R#qZrdNZt4C@gW^!x^LZjjR>` z6q*7H^&0b0$YM3hPizGN%PvX7$OVF{U-@WBe1b+H2?{nq5|@9<8>{`f!F#Fto9|vl zx45OBg>~!@kKXcTQJ%d9Ixwd*$*PCvF>O|DM{8d=(w>yX4w7va4z>dzC&zi4rAB z+{}0j5?q?)wK5PK*-ZtP0Aw<VapRhnU&)xgpXQnSfc zLcD?wE4doch17V5#iy^B;q&=gb%QBUV%XT)QXn^b(E}?+SdBuKqm;l!g^mboMzT5y z8Ibhs)nSAf;w&(t&gUcAmyX1>`y+1W8D6V_A=|yNCk3_}Y!xu`17GQ?#>i_@Tp`rob;;{huLyBrv;k+w1F0>Jn1_Q?BSDf%sPry!D7-a)}=DL216xD z?afL8y1GvLwF`tU$&GMFgM|G!UrVc$RWriRqxg|y$Y@G(MOdWkkZ`;4Q{q-x0;RBG z80i8X<`>TK=|BA)UVZ(mJoeZD4(y9))Er^O6GWEPCagBQG@Btyi;jI0_wqA8{ev7j zd>@#JUV{%nC|fC%xRqR{VQ}6Nlt~38N{ocH!griHi1W%nu^cdutqFZXb4-C{AlmKlh^ zR13cQ;ST_U86v)IR502PV0OZoj_vZh&;13n6KQbXipQxz#bDGE;92c-IbhOu;$az? zFY%^DKKkZS-z~EkZ?R&s{vo=>>55V9_utaqoJu>t79|dQ?JkBVI1UeIs)59$8Sk44 zAPaxG6BM9`Gg&&l~M!%gd(P7S%ycKoFcNfu@yUHt#Oi?~-$?ONgbt z&dB=2Y>y;K2!deizuu9b1M-P2H8uD{u;-S5X_ zHoSlP`m4<)T!MFs4O5-;q1PyU@9jD`>4@XSs}_4Q_GD*+z})mB-;+x4zS3G}HVjz~ zLKc84D=W12O%Y^W^u$5X6^s+C7sl6Nys3<>!$ch7+UXN8SUd*L1nLiBsZqs|AjF!I zqaK$_9`*%*``Q7TIIKb_eOtD~EwfreQNl9Yb5woSlf+?2IRENb`Lj>|dtSZp94Alg z;okcy)ax;0bxUL|?M}k%Y?HNSh1V~(xpF1pd%x$$dH1`&2bctGYS78XRiF%%m3aHw zfV~)l75MchPVctP*cJx``vS=DMo}TrCbv)%lK7hXx3)ODkRe z_}MGmcl#d3>(TJ<6+4dXtMT-!>3K6NU91rr)sT1Idw|b>bCJ1bj4I5mcKOPSvpjOw zKE`TkqE?)Ej_#}RmGdjP89Teu+3|hvo2+uz!Nvf=_2ijFzU$E=o8%C8f_skc0bb+b z!wtUm{{7r_bP_Y_Z>Gcl?LAnVu0K1$ujdkkey~MFi4rAByiM_zlqigWb8GEf{>d5e zoBH~U_VTT!O@iv{mhXA|JF#6$g^&5B|@ zGy{tfi!~|10whv)S(M-}_cwf_sEg2R1 zDC8ZDC}T`3!vR%0ADcii(BSJlb z-EjX`1MeZQLaYHnR3X`Ly@iEUHWPot_4h@EnM;>Oe38B&p%a5Mf~aNY;uT^M?z-a$ z;Y~g^eP)AptYJL}>Gr;n2nnV?20R!i1c?xN(h4qT2xEbcVh^2W`NDk!EMS?#P@%?U z3^fdv4twxAl~bhseRuR|(sE!qV9nV~-ZCXa*ZAD$KFM$Y?r(7Nu5lhZHO17F zp;k2n)*=dv%Pl&ckSkX#FTFg=efPhgAO1T(z@bAA0ufM2%kYcrxb#B*xqx(u5^q;m z>6+OpB}!Z?v^ohdoLl6Np1I6p$0zx~qlbw?d)+^?8U<-mm9pbrR3gIR=>~uEq5b^T znFUrmXvYTH|zW3`aO`|8xgMb_gqdKWw8?4=d% zzhi3G&#zt$_!mF+@b1Vl9Sqj|{BP~Bx7JTQk~ZEP>ZOz@QKH1#7jH>|s|&0KLEhKI zNk(#I4!&$haCw6b0>;*1s#_&ohk6_$2%@P(VPtHUMeftel%heak{y#x35^52y|UM& zzygRFkvUQBHzjVU%O?F8g#fs`V}s~+DpaW^p{9y#x$UK*sQ96hp|T+rNky}u>~H}> zuRedZ7_9bXD5b>pz{X{H!?0A9Y+aXpP^#3! zkh**3!V2$s@W6E;xR%#CoOx}T*XP@Wfv~(Dv(`?y_wYEC(D2a7z0{&$M`YJyckd%{ zj!%DmAwTwomzEf>g}m?KLz{Ou5uzX^sQ_xBVP>JlfvGwG_Z-_pvz_oCzLX|aU0I9y z)U#K(=hz-5$0~?G6c`fUw?$Xh69T*QS-XEvjk^!ldG@un{MZ*?oZCIYmEA%mv^y(+ zE-2yr+;8rT)cUn2PH%{j6bGeQt((0*RaL{M}-U-2F#3C;1 zh0<0UamZN5%L5kw5# zBxZ48nI!RfmEGp|np`?{X66#=H7CfB{>r0(WDdKCprq%WJAV%EeHQRC18ZE$>3Gz8 zRFxnIIDY*2t_UsxjvhI><$J;~%{;Zn^56px<@cEnv~51%yFU2XP#5^}Y>bF-a5@-n z*X+O_56JpX$@X@fC!JTlHQsUaXI;sN@ml;FJ0K`jaP}%ZRRoocB&ZyO?g83-o`@9; zO{$zBHZe)yNH|VA>(AG>H}j2mQ{OTztT82S9i*0)EfCL5F1+?N{_M{`!8gx5#RK>5 z;lT$csMZ6b3Iw608z*#HhNVTgJd<$Y{5;?HpZ%A-@BKdlRKZ8->@>RBpW1;w^uStC z`Zp!sLiXV3*JWt##m{~G89w&JBiA6ZQKH1oZIJN|)kky9n3?%?#_JK*yz#M?IM2(k zuke>&yTUgvuVW-lAS)6@-?+REz#l$+neTphKMx+CV(&zC%j@o)sPLW#4)D1%OSF?T zk!r2$`1A|296vb5(F0?f&N;fj-|xzKP5YB(iMW-}a_ZzhUVMF(=Uz__Q?uUr&m1ARFT`nQi>@ zJCq>&<)1u7D5)HA*4-BGJ;ns37-5MLC2mRFq_Kpp!}Dn%&*3tOz_i??FZr~sT#uxE zex%p9MBg)3W3>&tkVl_)Iu;xYaA{d3lIebRG1*NT9;*C0E>Ole= zN{|O+6~zG-)$RJQXs-|pwP`@zkxFPP+eOJV`=4Z=qxdXH?A0Thmbc{dM`TczGn*vE zs*C)-j9^qTn&zylbBMJY+{`{r+z_nACZ)0WR}H zLTjMY<+UUxX?N)c0cQilY?9CkJnP~!vO@?H_{wL$jP;6DpOd>@Oh)c%K5j)}PscIY ziuuWZ{=e|(tIL=mA_3+aT|W7>KWD81>or&;CDnR&UnM274gs}nGp_;%xIP3|aY=9< zjDh|8_VS6}`$I0gI+M*ogNov%-xaDyz9?|v!UbH`U(07zLLz-xw^TMz(=`O;#EIkS zxdrlISdXDzZ!9r}L(^0A)ITw%_9QUX`#Z`umcST{k;SHouH|RQeTN zHnNz^993Tk@@JL(CS#x!aNc2^1=k^Dk*b`>>U=utdJ2EUcItc~;46#OMGU}ErVJpJ^i_<#P*uW{Gi2YLJ*H4aW2(3omH zAdDjB7TPph!u)K)nHLwi=kE9LkN%q<3@FNnn)??2np1RCmzq-P^?%vCz_v{}%14Nh_ukyX` zILKeTFvnaoO<-GTIX?aD4B!8rV;N~w_`U*-$k6P1x^A$~H>9$EA3S}C#pVTGT}q`2 zzxeVpQ;mrCpSle}7_Zn|7O@^0W|vyrwy&|HXHtnQ2d8Qr7z??w)=#*5;oKtM`q054 z>xT^W4K&lUS9PGCRNXWfSQiuq`uuH$U&(rF*?fI6g)w3a@you|ujQ&#qC|;X05@^6 z>~>3T-|^2Ws6yJHZgXiZ$jA@$Ng43rM&E;q=7F|>?AalS)C$-jAXdc~lTi>QN?iTi z@R@ZNBP8@lACu8ehOMjbvy5NqhbStE#fl{~JDr=7k+`X3Cn|J11{Yh#W0O^yWCoQ7 zv*CukD+InhdB7MT)DV)GF3_H;0$@!3a7owLY2%RVNg{oi?oS3w8lJ0HeuOmn9>Nif#(!4E!;VYIy8fHPPyP=<+Y zY;yWZEAVQWGrpBYA(Hl;>JbvfYm7$y+L$gk&8Q!+rVxaH4uW~ZhN14@@eD}e%3J5r z$%gTocNNS2Z&QL*!4B-(gaVR)w6QE;^>u{htSYksZVEKQ;M09w2S06Ok2x*(zPG8# z*Z*s0a=G4tv5NXOVmudLd!8ph_eoxPLr?8Pdo8^_UrSUI5dtJn5+dk!4+CVyX$uKT&5?h+43n~w$=YJTAsk+|7&;8#!C$$3dPPUm+h-{NQQm#cMN|d;@aMKc8 zx`WsHnXO>6o+v)of6}cDR_Pn8xr8A79xB5MU_=W>u5NxIzi#2$lHH0GA+Q0lcQ|8* z5ulU2Q>wPW6U_9UK@jWG0np`zBsqQ zOeI7_ut7kr(<1RPW^s=7AmsSMB=VWpxcySZy;tg3Z!zA|=vt;eeUZ>Z=rvEGa3VMZ zLRvwjV)0lO>Rpx^2|w{){X9<{x`GWVs2HNC#lKx`v1T3JijY*RU?MPzC&41%Jc~HS z$LrKuEvB5Zc3q#&;O`bVxPL5n@CMzxJ`7<#H$7|$&~GHTdh3_VND$d(O>^1nJ(t*S zXFp^Tdb*+C;70k=q35e+&g7^SQ{LoN=#2dMS6-P}Won{AV0T~1{q&hx{@v#<=KZdUc>MST58bhc&>GfSG2O)RmtUV} zt{HDiDEou2T;gv$lx$haHC79G?4D_!Ik(KKOP&1qlP@l?Z@j{(JNMQPUk*Q&!)UuU!z=wN|Y#Z zE8wOixVCfN^IitMkhg3s+1k2pNvVWxLz3UMQA zDzxK-u^^(eVZ$kIQ=T0lxgjr?4g!Nz;PmoA$*#W`Q$+>vqO zIF%o5R)oFdRl4Hw)}(}z3UQKv2~s(-K`(X!7Xl`}r4hPR$e6=vz zCtqt(B_&N6tm^MQ4p9iQ@#%~bm=4%AV409*3^8DFOwr*8o-wi=7L$n&dn8NZDsk0+ zx3?I1GuQ{u1CB6Mpv@P)_!&O+$=~3>{tm~F*Vwa9h{A+=J*3-FnyU%zZpft>$4h5s zx&Pkx@&n)hgB-j4(X3%{H1J%!_a(ViqQuCpOg99Wizr$W1xbmr5nYMvgo#FlM{XbI z$us?`qNiV36MOmV zR~C5k8w<>=cJuQtt#tXsS1xh;!EvT1NB86!uSb0N(L?;s7cb^gg-fl3Pdz=u(fy6Q zpI4(6GTjK7S?!_<)zH%FB#dq7LsqW_y!XL_{Pn9VyuO^?&(|(CIlO0S6KOZU+~)9f zW5>_O8sWBmb$;S~N4fLx1Y`9q#!;xKFUf5j48gA<9CMDan=a3R9)IBu1sZ5p40w78%hqM?+tam)E+F)zRVkJ!sjM2-f zEKy=JdKC~&5!NdeFczn&Y-TzhrBAF5J6}AS5nRSwrdq;yJER_4YVpnpuC$6V4ZV5e zaX6+Y0`^Zv+}5gKlB*ZtY(_Cn7ZY`X@w6h%ZU~60dPMqpQ%JERM2Qk_Z2vKU8X++@ z+b_s_Y^iz@7qb`!yfz6(DvI|8r>Rt?DtMRRNc-VfdVRORX7rY3A`>DGt4c*f#4GJ? z3nWC<;d-SYxxzZc0wxGBT9D{9mITE)5H&Ys{OcA{Xwu(A2aOGrev6`7scbqi`=`lv z#9au+_j-73SHUd|R>1h(e|t)Tt6U#kw@Xk7^L?JMaa^iWAs0yhpI zFkP%!2eV4xdu7ixI@D=00aSBIUP#itR!lka-iYm_{w;BzNsn+w+4693qa~GDm>+bjSGe7eIjvYCbk6C+Si;B_*x{Y8A`9MT(|Un_Y|f@{4o4`~LLY^>)G? z)3uyLyR_7y8V!#zG-@FqJavFiKR?TBS!LLhXBKJ2{k=c5!jYW-SPtu^Sv^s2&og84n)Marb!5+zEM7=oLU;OYV`+1lKkY=wno zKRe!QbY%%)MpTrp2fk7~M3gqlI){f7r zAC0w`EMOKnOEtEPb+#v_WP`||m$VO9Bev9>rQWrKNib9nXeYG~Dh_Xiz-IycUcp%J z{DQxkXH#a!%J58yYc;>pib&1&6G^LnG^@W7p&i0<1@;An0hmodYXYo#3<*{&stzSc zNv|x6Pqm8@XL5O2wx628ek<7}r6<9Q&yzHCU`9&*-y0GDmkkeW=o7rkhkq^HwUNY_ z7H+?E3rp-CSy~4TQShm~iiKol89TKHkAW=lN(B-FHh?4mX9X7uE`&rpMm?S`LK~$@ zOh`;bLPXkKG9s{26OUj`2-@j6Gvx#;asBoQYuGy5A5yM3jE>@Nv)6m_#;$oLHbm6!|Kypd&XkiGWvX3EE4Q@Y#c1b&0gJ~FI>o5x z?>zK9bOOf-A7Y#$k~Lr!=p;(G4k{6APFb^>XI@w!s?~7an2xm^F2`kwZAM{g2=()M zdVPLGd%Zq(I1jQaki;;w@7&OHdVX84ibDm1;JxO3@Y2M^te=Ap&Yqo+Tw>CB(+d(Z zP%+}cI7D0??$Z%7v_y%kK_(=IkYsCkkkwtKvVz)kVx;eOm|TP~Bvv8B6KESz#Udos zNU+#E>&~O5jT%FOB@99$DmXF;QBuiUXTay3e|%OoSqx{Vcbm_>>-!& zsut^dje1pVyO0SYWerw|tAoK2l5l){j5aU@w1B4?4X#86FG0EzSa24cho*zI8mu;i z2{pK`AyGp}1YwBp8oU~OCQxk!oQ5P+0EZeuG{C6EWL287$uiwZF7K_qL#ir9%uRof zB9i~0U7*e9KmYsu$)Elk4&PSc?z=)JCo2RtAqWK|WO2St99w2CJHGkNnEUR1oS*)w z@8`&|yMaoU@ow||sx+EPl-Rk?)n8w?nm7dk!vTHT@)?%6J{YTqoVsIzSC?k$Qf?a0(Nv16SN%D}FA^yil@cXNl-LT_ zk>J{{hH3+RzcEJ>SdGGwXCL&irxh^X5ym@(K3qeBY`IyggUR8>7}TW&#J$j=>?jOS z)YMHs(t=W7C5T1InO8ssvdAQvg()BT$D|gNh{nPghOQVNl6;>0pA#(VuiM0=#wb`oQ$EFl@mrABZl4u{ggs>5iQk;eUw zH=FaEMolpT1gHU*&9k)UP@l+7r9X>W*PETuB$GG{iYg&x;?vt6I&@zctZeL8I;P_> z0LI!%gkda(nuY0Zl}lZV^XYlE3snsXd`J>o>JF+2RJ)D|FIkXU2)M`|{Sp<^CNgaj zJxan+j5|l&USh?K5i(7O!+3uLV?w&#qs9UrW757pk#X2qsYIZO%PK<*6$gg{NFQoNVqR3FIg|wPo zlGrmd@3}PVc=7AA{MZlw4DWy6_X3kp93>ljYwfD+eYKjm4NSj zXg|O8qcv)XYy^UC731Q)xfSWAg1k+SXL25~yCby*w%e?|a<5u@^Lk@OuN~!jQsOO$ zemuM!z$I?bn0s?RR);FeDk;IWqa<$KEErI9P<4cD*$aAV_K#d5TX*8L=|)-&Q60#V z?_E$8k~m3&i9NYCzH6xUzAfc8;jW)>*j`|Q&JS*3@)(!J>)b|xq z8`06ruPTkhTW5Y0TnS0{ly0`SB)#zrS*sd%dq(8=eYB^{RGD2U7Nk}m)Os9h+o6_CdvwGJP}``;a;Z8 z1j<3gcCl2MbdKXz8DF$KIyc9PG-!pBv~XPhEri~46x}^ZHgN7XUth0#1($BH zVA8X^?;zq3v&NUc@TdIIpZpfn2bAL{0;czRg3wc~Ms&K$++34x$8u$6m6y&pIeyoB z_{CrNAGqVlX&{1~s6fcuX}v2^;zsP3o8R}#Pn;f*BvTlaK`p0FiED_}^)7)mM4`od zB@7IWYQO`>CI<+vubf}uJ5F_}L;+jHlv|yI%d6e|_s|Np=<1W49=KzQ@h@FswVRfl zJFi?`?y$7hVb9psRbgZGh;M!HAYXiGiTU*yRcLlSUpTwW(pt<|WLe9udvSi9!_#A1 zUM~y`_usLX$8KNbs~1+8tXlrYgVUTkxsQoPw9P!%k#qPT|JHLmdKMpj;xrMX>p6?T z{vByclxs}8!I13i5r49h2TmB#?mlAUe8hJ9i-~F&a&Lr`;@#t zP%i-Ciamph@6?^|BD%>X&9ZZ~pb$e^edZ_-Uy=nVj9>yYK$GRy*0Li=i5&!Kvi}A^ zV1!B#5+@F8&0znl9lB~ZyQmk%i_f}xZ9NtZ9;=EIO9EJLFe>X2nYB@H^ZvugyffiNra_MR!434RG{z}fXx+JU8G4aQqU z?O^4DdX6&hKdd8bUd~z)EgC&u2PSo9z!jLT9Q_B8Db0xV>(Zb zZz8N=XwpRqu~axmm2*tUI8nGnS3QaC!rDb{4?1xE9!@68rN&*Hm4LXik5xa04J^(( zoPZib#e2deNq@IOGjg0do_=#{t6Z9>@~PPltuUb#CaKJ!S0W8B(ixjpc4;|3ZZY&6?MkJ>{QP|RKz4zi@<3uBwrTqW10>lVB!x_@0Kq1nx>&l4 z=g{^yuq@z2Y3nqJJV0CTC*}^621zGlnenY*1_V>fVf4mh3 z>7dN`PAz6Jpgo!6%tmtN4Y(IO2#d)`-@+G!A`6y_I+A3Oh57S*^rJt^$vbPDJW*x; z{wnnvRI3n#5o^sh&8A^>)o}Ku6>dNBDF5|O{UC>rJb>4Lp8kUpgBnD(tJf=0qQo0q zmIm;1AAg3AJ@GJLGk>L=fhFD;R@b|H?UhBod2uz}!xR>pF%KS{VE<%=hfnV1$aI}O zwSc8e-p_gE+{_w#C#nQ?^oG}{h9m{aq}VBwjVM(lx{84tg_cvdPw=^y7V~54UB`vX zYugfBkV+kX_#FrN?AK3)2LUjO8B1t@XrqF23DqQngycQ@0`3xIa{o;Dm>1)RZ>eZQRG3J5*8dip%#AfLng9>#(x zlEfp%(A7P3nZ#&HDEAz%Zbhjr-$vpc4$t{JR&YObf`v+jg_=)$Z#iYjS|S{iL6?19 zPp2?vRD0FTqw1nd+zPPPmK9uA^V|RtgAt2ZL4B{TDkU?tP{D-=aqJkI*h8X*#7?m8 z*6DCNvD}B6!x$GJ1bs0=u~`)uRe(yI~SUAf&9zDR(V*yd9jE_ZFW9W1`EH@pC zOUnFQ%+t@Dypq{z~AO>3#6Bbl%d6295K*gP(sab_&or|fCEeL;YUBfWV;vemMnNbdfXZH zePJUJ>Rro3H_R&>rLS;=ZkUH@aK_-((@;+p1)9`>iz{n%Ivt!Qh0o9Ck*2!Rf-Q8; z8jeaLn3my(@BI+%pjXLN6i;)AQLc=|{N8h)r5(m}qlCd+Nku|SB?@x#E3F*$JLv*e z;;jzp_o_3b1eWoZAj_&%>C1_pnzQD9{PUR#It0H|h_A4c+Z#M%(=I_WIT zwBQAkR&&beUAx2$vXin1#xSk&tF83jsqXo(o&eY4^RM~z-f)j^^uDfE7rxLf0oUqK zX^fGm#~L99Dy01>RRkxH1WMNkNnnA)tcUl5Tg1yz*7XE|zk>uvg^MVMfPgNZw)aFP zB)|}8NR@~-g0+HkidUc3tcC{Xx`@+!FZbCTw^+m}p$)ctenq8&Vb66Z&uXKIu7wQo zt0^KA*-Xrtolhv#D}fCNtf68oRU2-3@4CaWB1ZDxQ?-UP!^o=}yjL;U*cG(Xd%~8E zr@r)i{NW${UmQ4C<;0y8ZacIWbcL~cgKnoov+d|~Ei;!qXJ1<9_=!jPXaDr096EFt zFrIrNX&vE4PsLHzkd!EK{a}D!`SH{I!f!n@aJ00XO1u%Ad3A|TKYxYu^X+~mwhoJj z!E>*#@#5umrfP#By)&~d=9k;lt3kgf+y*qN;lOo8V0O8KR~>oIb~rdyqZS%kaq7P( zUP+v%-A$-fwhuejDgmeOn&wMq7C1ZK9{RnnEO&N9Zf&%4dNywFzrAQgPn6NK^J=YM z$$D$qeCO>)YyxaSjJQlRDN*{hB}&{#yeSo29+Ph>J%USu)D=^}I-%a)ph#QzHF8@8 z9F)X>6D78g1d>;(^d#K!4A+q}k-nx$@!A_Cd3)D09zszziRSc9)og! z!=*Gu!ILmShcV`OeBYgnyDopUbcrdGP`8RYzag;FqkGc;&}MV5f5dvhsG*XKsP+;A zqO3)bh|n=1>w)yYfA;>+JB-+TUFo@sB#CYF#jBG%||khUJcuz+!ESY`>CL)n9}2f-5A z4zat)*y>z$cH?L{stTL?u5W9zCN@G_8Yi|Vb|M@DGyyfNhn~9*Kfq@%J;`g;CP83PRl0RW<8>yghS2$e zT4@ysyeCq@8iO@<4`de?64Ji7#uzG< z(NalU5nQ=ka#7;Qrxmsv%$NZ>v5f zXZf{Es;oqb-M~4|(=W~O+h4jw*KOX3DuLm#J0_@CEw9eD=p>$V3vD`yXQt3gtA`g~ zTjsWXb)s;1U#)$U)d7h@)l?F2VY$upp4!!Y2d_MMc#N-{U&)WXzR(_d4?}~K*6^Ws z+{T|gJ;Rx~)~45Axw3v$wsp}S@v3+c9PslyB(;9+iPJfKo!~IoLSnc!b8xFNMZcP1uk9D%E<&`SMUIdUBr5hw{zg5*EHMz9wEs#2kcD;j+XjJH@S1UC8#Yt**mB&J*aBT( zEeP@oivfa5(jHhz++m+U(CQK;L0WO5f|J2HF)9F+6F%A#nKnlnsFmt`y)yI!SLkv5YrB=5tw(J0(ViuL+d_>5fW2X8jMFIxc$ zM3-cyXxdw-q_*BR*l9br-rL22NVbn;WUGRe{?3se_0*lA>fm?&_5aR2|KrclY8YBp z)7o4SR4h&d5)+V!#YtLmtSTrL&oqv4IwU}hP{9+?3u z5_JTtXF;176i)|?BM^i2P~#HHix4Z32Bd+^Dpox{1{0=ze;f%3{R*}8`xk@CJ+aPo zT`?4!NRnizCc5|Ox*Ue#mW0o>&cwS~QYuLjk|e=7_lECjeSLk4 zd(jbgqD3@dH4Is`hSf`RBvVsVB8$8b`K1D+Mk0a-A+w z;=0@+XJ3ikVSc5<=U-gdL~cEBsKNUloaW?hlNck!iKiM_=9k-?y|luYUs~kl zAwRW+B0{*K@7MW-HqI&5?0iPterTNc zK9F$cFJImC+MNd)v^xpatRhUejX-Y3&;8~OB?zB5o%hr#q<>|J@;>*kie#1)l$=VG zC{d!sjl~V9;OZ~$F@5>93#-c{Mo5f7)PNVP2T45N_wEnVSc!%NLDHXkS7z-JSOWukUG|bWmv<63b*T1VANo|kXR))%2-#Zt~>tYum2yc&$X%9 zDC@}}*(RrjYs%J!a*10W4v3Ax<;0d`)q?35HtH~y3amoaSgZ~Pv-$;s!-I9bm+~<# ziWfyxGQz9(&S8~cf=3AnxocvY>CQR!CQZiUS@yUI#!Z6p3BC5J7R`EgDGp0qDB&+? zSP#+w03ZNKL_t(`sq4iReedS7xvfIFKHxK10U1c!mOOK+a5a_udP5c0N{jO?9!=Q7 zjmam#U?0T(*dOe|Cc2?$Nj%0mqOMR&l(8hus5C)#@$CAVlVp-dG7JH#g7XbLjY5zk zodYpPP$qFQhBFf+au*#=l4zA!t0dzTdg`X6D;!ukM~+W~QfS!C;-9 z0SE#F1X3^;IV?E<%d#b#CPoqBC;EdeZle6b5sr{df<}bRu)Zn)(hi413X3E@D7c9b z2oeN|WxyF=_U@jZ-n*)6dG)r;y!ZG+=G&^Ps;k%O>B;{@#dN)Txx9S$+;i{wpYuO~ z-1M*^!UTuX1?dyfB{C_oUL(_U^mLiPF#;*V0B=3P48S?mDq=iRCUrOhQc5mHBE;&6 zLMUZQCnR)bx96)9TRGkL#<9X38ttlu?5p>;=Tp2Vgg~C>BO+@O)>^#x^m;wk*VlKf zaPi(V7z}ni{${bYwRKy^hY)bikt7M$-hRqZv)P5MXzf97-!faSa=@Aj5`C(4I`2vIF z03EZy;_=7Gn+?{kZLmJrWc{g_)6E_}_egAL^?R_sLSh8**|xg}pXG>gctX*ubK5vW z$NeLw*WDBE+3yT3?67{n^=0nVwYOD(?IoiWCMvFqK7%?+g4t`!a1+g{C7BZ%<=e>b>`;5xj-BXUJdK(J<>sqGiQ3db$W{reBj6V-+$|$g3f@t zLh53gx@4T4uydE-3Kc5ceSGQplle)PE4=^Fc|Lh!lW(5dU?#DA z_5;g2^Y|ijt>n6m{p1?0@Qwi~(`duQJNH|y^&3C;4AxM#RWK82wPl6-h~f&{yl-Xm z6)IGy@J`~MN)&d0H+oz2jUlbqN};r(r;lK$rBDw-vSm2Zj(6U90xoFzv$Hb^0=ZQ- zYM%3R8I;v6*QZ+TQl+Jnu5zbUNCfH##uJQZwr`npmaW*(vNgmSG|!9dN>z*sCKUA} zLCkc?az$!Y*m0B>SwY^wSX6LqFQ@}8g6~1fzHh?{FP)5cY^%PIiB4Vx8#D^v$>c(D zI6QTGUbn#`cw_129dcvHjbnfzw~ka5Bm(LVs;sB;P-6p~w`p*{7D7mAjc(w<~A(uHxq8Dol}5hUO-Mdn?} z6o;~hj)tO-O~?zGE3ypd1*3t0M?;}mU&#JVqUc!{Bq0|^omV=_)=w%c5H$VuhgVm}nKT9`75Z>aj_eE#JW>J*Zi7$41iFw2UDd3}}d8%LPF2#k}Fn=&6kDPSet!$A7yU2qBD!9Ov9k6FT1e5lNG#>1`82 zw@bi?2zj0}7z}o;nA!_@o)bk8)>@({!WeT~RZ1d4k|e}&JY`oy$(9fT#u$t-Ts_N06d&D^`` zMGeS^9PaNrS^g!#TKS!E*W@^*KSU(Lk>-*oA4kHuR3P1VWVLtKxWsV+b zGNslsvaad0l1VaaNN)YJeUe&VEUUGy_o~A?8Y@_gm7t{DaZW2-Y}do6?7u>V3Kj0r zzT;kAECagE6$F8FpAMPHce7%9nl=XeYiP08}O%;iT7* z9nP;e*pd$`Nj%`OL5S3nJAnW_-=|?pM=qrRoP?=ru_CZ4yKa&zytgr2IH^oBI5LWD zphD2G+)pUUFWL8i5~j1#C+(-ur5+8`(U>iBp+xA1oKxLCkIgKzI#}mGv&rRnnSRou z6ABAz(Pxo0o>@G^TxR)RdzJGcr;ouk=2-9cQDX^VfEg_ww{H%|y}U+(gpXTPU=Ols|PZ72wW;qOCoYd-<;o$mX72tMtnw~neWB=(FYD>sDWw~gO# zfLiXbYKVfFR^kg|UM%(yLWVe3jw4TvK;knJZ-{(`$%QyFSaO`PIOEBqjCR~BnY3b8 zy4{C_5QyW5W@{En=80Uud4tOZWCj(F;Q(Sz6cn=N)b$+c<%E83gN6m8ZR+kCl3bus zJBLZ?cbyy z*ji#^@p+D*BtEBB_Wc1fAyqcsP!9cfyS=r|ZuO?ea--3}IY*x7H(ix8B$S+U)a&)z z9)E`p9lCAXwAPM@oSP1ol|>Bs6g2z|=kX2K4r9^M>!agCAn@ zdhdIBiAJqQ6!)<%BF~krc3^!|xOAbi4o>;sgxi!SXOpTv<_Au+6vz%QYj9&lF**1UsnUq>=cbt3U$ndcf3!MMn z<BfOo}KwZgrEY#XAJV|ZAtRmc_= z0cg?T0&G&kW_sl(*Nw7*3^J+{y}pWnZ$I9s7LrV&sPUW&bNt(OkC9$=%^RoR23KFBDI}EKX=x`^ma1c?;wW3>+O{pw>0kV!2P$|?b~xqCDAdgfSDu- zj4DR8l(-cJD4DY(5;zEWRT2{u(m=Q8X|HX87Gi-`6&r^0xfC{J3>)ph-}>YyiM%25 z23?9+Y9=I+QjefvEHmQqv85-<>|9I@%R?VI!eVn7i7c*GE4Al$@*#pM*buO)xLmL~ zqpCBb2e9-pDu^0d`47psMtfjmV-7Xp%55NfZ04H z&I7S8Lp75Xpc7J}k3Zh1ChQ&Ngj znD|P2-5$~65^2bH4PpgVJjx&hax>3*e1bTSiM^rE5(Ad$@(8{BI62GYER%(3Vj&Q? z7g6Iv38zoBxpXPx$tOO}|M6SDT+Y1B@;-|( zGqTegB;MWKeb?>HKet;|TcN`J>l_N3J2*ex+wfPu{32g`{v*|%{ymDfF17jQD_8mJ zGn>5sP=o*SZzasPk{#chW}a6rw8tSR;KYGCj~tr0scRltZt}4w7FhZI6}nlG7~z9* z`eK{KnHtVrKN=%v`M~i7es6-{+Dbj&KDEY&A6sB)u69%Ru(Hu-vp+rGKmJhbruuKm zb%M!zRfdlfU={e){Ynr%e-gXz`?=BU)Bd-z`3e;(RCwp$-krC7i>^cWe-_#8cmWyp zFQJeh)Ibs$;=vXz?IAQ6pad*IErE7l_hM^2M2T@pGbA{aw3rU>SQVehR_+FEhH z!6;tB=+oFRZ1h^Z$0|ktlTa4cOiB9OISggh_ET}R7rNc<^i*?$;M?}|4}0~bX?hEL z^YOXzJRixt-40r<)@>ahLcm%}qtV#00>*ofjcfFM&PuPtmX%QzV-P{CgS1CX#FDi5 zpML)5iM%E9MPD4NMU`SGn*#?Cnz<17b0TrrAgpz29NFm6N(`oE@!d8_J>uxnJaaQO zX8Vq)k@4!%Rdi6;PB1dO3$~M7K?Mzh%L9>D;#`PwsAUmJ7IA3PaAeCN;y?xOmAID> zQpXHntobmSb@{u;A0u+mnCRCx-bUV8TiT5nx|Q?X zZUFmsuD{!{cjKHlgPo)jw-a5veJ7KDuc|M0M!iul$DY}94W|ea8$1v3+)w;tUj5U5 zM}vnLutcg$WE>*bLs%OXa*<*QVn70vi60ei&wh*;yxx1lC8^cf+*&8?8!lf=IeX?B zkDvHBfB*CUh(pJo0W#fC+>#Otc^xo($V<;h8p~8c_ZwouVZmMdI_aMIi#tOgv zjdSDmSI{!~ZI?MRDDB>~JoNE{4I+2*GR{vOo#Wg}m%lo*IeP6or`Gx4@da8g-oZb@ z@?4FNJvz&`-x?otaWmyVzIciM^hb{EyNaxz=KR&GE97Ck3R-SOJbbWq%XYO>7*DtT z``2!Q)GAVfhrS7{y;F%Vm6cbhP@%&8fct{r8kS=lDSqxm07W$ryvOG`rgZ50E#TU% z#$zZAH_9z3i7Kl(((iIeJ3NhL70)U)1|$sBP>lR&!--XBaq^aBeN5V)Uq>4 zTJ(_@M9kC(&!WJ$atioO8r+JgSzuodEsJ%*<`sdy*v7>-8yl!VtnukAG|JsNc?g9mp=_IUB%QXC0F` zkfL{%k)l$|d4mat+NQ8|YL!N2v8h4TkcX)@I)KtqrLSOvV6k*I5@yzvM$56$$|xfBxiD zzn@V4)eKc7s`3Axz~RMdRfE^B_V)bl*V+TN2IHiQ#2K2kTZ<*k&m=tk@az=9_2x>C z)y+PQdbDeW`DZ?SkXJ8vXirpM{rMZ~EVg2P?1P7hqrKxI-G0WmUcSP2&bFuGd(S>P z%WU%&BnoxfEe>;3efg)G?W;9B$7J&ZWAEI)p+bcU6)M~(`;G_ogBerwzlfqhn&pU% zwy#IGHPW6|&mI45rvl_sYgdb!fyF_WM><`OK6a32Ub)KV;yizN=`A){B(uwOvn?X$ z5QW${@_xWZrXUQYIG+(ga}NQ6vwLP#-;M*G?A3E8Gs^E(fM7rOa;un>L(8)4rl0HZ z+4lSWn<5vkTj(Y=&R<<2H$rBHWK#GxJ)E1W=urw;#2`veC?q>I!-`_n^XO+j ziub~a{u+nYFY?2G|HoWBbDoPw4s-nP{};G^#1FpyJs#FAj`lWqIPG$zw~p62H1Q~P z+VLF0IK(FSJi{f9tT*7y>C>1vCROD^C#TVB;o?9N1*CjdhQhXN@YJ1QDXO!S1&U>H z4gztK;H=xb+58R0T$-jBW2Pj%Chg#6#=vfs6+7Ags&g&#`~XohTr->|3}aTAewlGp9&1#+H6{(LWO(l zSSR8F+wiMjet|DOf0E($tHOH&L6zlZ#7d`#CCnt2D{EbjAD-Ru{d29DwTTL@K^~ZI zCbx9$$QceVHh5$q;q2PD7uPFqZ*X|2xhqk1bh*XfdTNocy}UY-Hw5_V_pjgsJoES> z%e%-G_6IpHoxa9jyuLQ|dfgeGd2A79%`Mqg8EPy1VbSKEU)d+I^`$R-Wc$WsatnFg zcBWeAR;W;+!h?Yamf&)3Oj3xsE~R#gteQyUANkbXTSRI>Flf;_+-lBa?Q6X6JbZSbd?8?1!{;|zIhu+AW=B-Stp` z%QEsjza!OG{eGV;%ZQ>21JWh zVi3ifz}fkn+E0C$PNZ~VEqh=U!Fkny_c`%I_En&$knsk*2oZ`?h`ll!*SXZ*z&B$q z)@sbpJdF9-pQVLm%LHDlJ7zBnuYUJ64t6i{u?5Rgw#7n!1LJ#ycAK}ha;_#1voSDq zNl0pje%`_OK!lVMhHz!k(`vNXN+64e5{7~|iW)^iAgB^MLnAf_j-tQC)R%Y5UsD{% zyN-q7n7Y%%@9_LPRSE9&+Rcu`chdr2C&r@}Q%J&MFlrDjX7Jn$Lxv8tAh(9J)}+Gw zu)xAdk#yh?n!Sw2#KEO5s)41t<&hNXy*?x!!5{<_Eqm8(>!f&{3z4z?DTq1@787hS z_XmRs4igk>3|Tlprw46*ngKro3=mRgdXt*;H zsJDZK)`$7rKlvFRdF;c*_a|mzYzgep5yVvAbA<};hFNALM_A#(MF@)7M=zl1#ydiA4SNd>2_B)y z&=~d|0>vA@>jqx*`WS@D`1K%x`y>x;*T>P>R7q%Fhx#TB5aei>%^E1=de z$aB=^Tv(prV!kzX&D`e%AIN=2LPgndnrB5E5@N&eCty z7$lZdYFy|JklA^>b<~{DUR&Y0<_vY$Ao8BM0nFwDT6vdQJHvdaW68(|0ZT$mjnMB? zCjtVISVGWZJO>?(_S_V`l@vzzWsFQoL*0q*=VtAus+hPdafEGC+_?YNFz{OzDxyH@ zU@KCRjH$m5Nv>g?m1yB7RF#aJC^alaH3n%H@HAqF-RO|`Q1oFeV6(M}JDJwWjf_(Ulj84mq7Ba!I?IDjho?-gQR%5WjeGc7a;@EBYjuV}{PA?n2Tn&(-!=L*sSB^YP)=x>I8m+GI`(OFDy#3Of zG;@a`!)S&#h72LKO5X~UYcZyj-O_;5z!Qxo^VJx>KQ}tJBmYRz zLrzhQwPbldWdn-ffsjGu4K{;VZBe zEYBwhFGd8glzg$E1Qf4|ALSKc(@O``qlf_k zi=~dk0iId93N<}LL>)tlQSjvCrU$Z0N7MpZ~^^`9$^e@So?_cP6%CD?D|m!KdE8 z%o9iFi|XkeJ(hyxUEjPuD>|*lSX!nJa2At2ys`2|k!UA)mTh~Bx?zQf=(rZ7ustn_rLoAol$Dr43Og#6xCX@*RcJ0| zgBiZD)#YQYMY=N)>!!)KSAT$Gjw9M9qe)JSPJo4Q>`q{FJ;VuX)_5h9~%V;MQ0 zg7B5{-=Pc=n@Lr_3IC;`lnt~Hj7PefNLb2 zp~e}M6vZQ~W9=1;KgMwDCu&C$jWW^aMa%g>r+zQ(T6U)jDpYv)PORfWJ+ey?a798$ z3iO?!Z!802xkSXDt)8c=j?~tO@L0{rjm2Bbe7`|$!;pXDEFZWu2OgYp)cZLz|KUxH zcSvp-tkpQYQ6upX=YhJ3kYEWUWM&qpU1I5?I)~RqQaMOp576gfa&2KXCQ_l188C)r zEWOPLDW!0?l7JGC8p~D`Fr)b^5CV8{SUk>{%BtVd)!n4?k$nu-3Kc5c?`zRXRe+tg z9XoVNs0gn67~Ou(U%q^GirfNtbESv>{$(CNGLPFM-^mxeVC_@ZSbcr zU7eCn+zoRz%V(Zm;+YeREX>x*7}|J$VYZF`pn{cxXxlHT^&8KhEcZg?Nz7!N&{PAr zLWTRyKUS4n72W~36DLmG`Lot)(_XAZQ=kF#1iUp!7=|Vbr(`3rwkU;;LpmitbiqRih4h`XWa>wO@8SE5fH!6KZYgOt-7aM;c%{RGr3fTA0!|ESgXxeq zBbI*tAM=AKVk5Skvan{0yUUCL<4~vIJNRA?=VyjuQG^h%CJ+ZhmU~Q?CI*X3S4-gc z3L@Y{Qs;CYNVOQVp&y$2(ha4QHQkZVR_YXl!|;|>hIOv*4i^j^~yTrig64W^JilZi2F z&@$kmC2Emi|2iV9VSx!!jCm|3M=&{B}X!9@!qrb%FJ&G zK3)wrU8cNwb5vqFUj+{Gw@ z@V`EPl3$w$+W*p*U*Jp6mlY?~JaFG5j?9h(kPph6D}BE6>J>in_|mSGP;q3*y_Wmt z;<;s=nK~%VBaFb0QZ*>zaB5Xg;b)D;V zB74J>n{-LU0u8^Z=Wbw3t6B^9{m8Z!qe8G$?m)L6o(nX`^1Sd5^i2*ywM zP#LE8mX$0rel}tR)j-gUkOzVf^i7N|EOKRjhRtS;LJ}r`F^DmQUY|xH)H_`s_B}Q= zCn1!io5O2LY+?pkA2DU-zcqU=qANVOMoC$rlzUtf(GMQ6QudA8EoyS7c1%U&Pjq^y zsp@d=R>-bNA!{}awyAoU?5k&Fhp3JsqKGM~y#_X+?Q5*!XhuuavNL@8*uyMuyvgz2 z1=ea^YBEn9LFJ*+sEgydAwMA_X|>_coqS3a-uXDf&B8iMWMV=Hx7SNb0X2mbO(_d% zR7;yFtGg(A#6=zq0V(@E8r7om%R}&#Hlz$9VWpXIYB6Pyd!j^%gD@M_8O+Ym@RlM> z?wHM$q?h4yPwZk^s`M|sL_#0S7NRktGl(RN`|nJrglw;X-mh^~1XqOz0 zy}MN5^rbBxKGS;A%Y9o?QCu+8aedeU>idBsE0Q}nj^@Dv~_cxzExj%cWP@%&8>m#d-jtcLD z4JFTPZ$L$ zhQ|~fQoxr^*p&T)hO$%BH?C6`(V_g*@Bx}4i#N@40)iJqkRl7;A~-QDn2fnzn@85y z2v;t!+FN5lLLUeuSit6xYfh+H>h-;e#wru{!QU|1@_WuyF{VfXvpO8Rcb?;%>_<@q z>}7)3E$l03VYS`|taDgvup)#!4D|*eD5jKfyB-BWWz-VV0fG?dQ8y7j^B5rA+9GzA zppd0Gt{!6;5IfHx9Z;_&XqxY9nX3Z*T)EKivCwLao@<5oCV~$W zs)7<5hYb)!2qL(kgnpj{L*hNn{(!m6W7K2tcro;Sz|PFlcaEs8^zpoYxR0iVY=}xm z>;;#%sVyn-R!$vD?3ITvTb>xSa5lkeg6aafJdD!~8s-X7IKo!;B!R_Jm%cDy5sCx~ zsl;t7TtEMd2+d}bcDwx^^S3gwcAW6SL!C|;R`M%FJ3GPabPtuG!x04a3VGoUbF}Dn zsX{^utPpdanw%%qqeL9TX^p@k;0!{*at$Y^5FV!=A}os0992WnDD&n?Dw*iu9;)?Y zg$fla+&LdI+1{Y^BdUd*+OKYtUUROB;ucRF>D&^* z1zbmN0lc}=XJxZTqwaQWeX#Qyq8Q&Zjj>$fY;!UmF%)u_vWxLp+rm)vq1z9Bvy>nl zA`e9jWSCqR%01X9X-{uyNmQXig$flOV7STP#ytt7+<`all)tJveqAOs-A4U}#1w8< z6_Fw!FCx>QTee+Tw~I0jd$AOO%z{%hh>40WLuDVLAVLU=6ENO$U}J-)I-9Khf4|R< zT?nVBZj%l(TqPX^m}Xluq<#yZQKiaPm?E;w&Dzf$*|h0Ms#CpU5sV125sc=@ zAm9*01EM)DR~k`{_c?LUU9C4*LwgPeaYj#-e9*-iLw}&Gt-sCSk%L^d3t$eB=5xec z;6VBYHELx2oDOqznA!DdOqxxYkSVNC;f~m4C`e$d0T%-K#s*79%CROi@)XzYQ1gK} z%UOTpO?*Ek;1Tg;Vo?{-Uzn#|OXy{mj%#uJGsoH7nj?xVxzDKKFd5{XKr6L0213$P zuKnH{Xvd;K@mWUdQ*!l%uTx4X!+DfCV794wX6#u0sN3fp-Iw33P~pLV>@wPAm#Qhb zZmDX-l>KBWtBs3Zt-~XoS;xu+nsSPIc$2=a5hTND3N%W>tcQ^{&RoTWO&lI2U@)cC z>J8+1_n5adRFR+x6)IG?Yc6E$7pxRX4wLxBFTcQV{Orl1ca!enek$Bi%45 z(iOG_o}c~9qZ~Wfx`Dq0YYdSUGOwt@$Ddqa@!}TWKeq+I+v@|q@#1Cvljn|HCsjB+ zK5>RYUU=_TV!5{2XK}W+<27Sv_|Wl%DT3?jR>lv{Zt&mP7L?cUIkC0S@A1UyD*X0u^J5z+A6!d$)dW7Zbc}DTZX(PhVo628 zdukrsu;J2B6K;pkTH(RPP+D*M_Z-Up8%FQ02DQrAo`#ZYv32Bw4Do@w3&dn-cR=3m zLC6q1E`X^wNwO3P`3@&Gh*++g8h_`1`6qxuuq08j3Fxj~A@LcP1_QLc#qa;-@35G* zd5nx-m~9XwBMUiSZC~Yww771Es5{7vR(A6qv6y>%J!1uj66Gm&Wrg(em63$z{Gd;C zZ55a1)D#W{j}5+*!qu{7*wBj|u?b}282F5HhYqrJC@v*D1I`;vCL}#it><~T)8f!t z%%Nr>ou<}fj3>%B$+?JOi(CVq49JUW5?nEx$R7T5TWcaFYEvc&_KM)D@Sx5f!#)|q z72vRVD@YNn!B})i4(V8mceo}D0vLlqQ1Rp@MJ2{!5%nk;R?ks)-=m?YXqh*0dWnpS zD2E6fKp5aj5$RyGfzx#oR`4_kG|RA&FA}sWRH#s)!u^3Ma%-FrYj#+hTDzMQsvhIr zz#Hc`_?`cFmaWu}Ui;%0F7aP|=J=kKTp~r^-bQ5D@Wrv-D}U#sM>)On2AlojwLgDj zjfWQMJomokscNV~^+ajLV}i>Fq}d*8aS;wJHu%8dCO^E`8NL2D=h_@uTIEw8dT8ID zd#ji6%K0`vD04N-v4w;mT`id(XI$#|^A7QA`y{r0Xq|Pc-s8FH8e&F7Ti|*D&XsDu6^zXUdprwrZ4TzStTsoFTEJAo?zoAjjAuR%d zx^pORC}mT;Q_3Ghz<`*dvMUsFP|im9;BjJTiKVGRz0;#U$cb%=i*uBe$XL3;6V#z8 z)NP$lp8N@Z=dG{d4R{$pH65*;t5E%ejA5WzrRdKy{MwaiGWYIXzlh+84*w4!P0_6` z+U=t@kog|5hi2Jh$VegC)>KavJZZi&!3Cbxx}iH0$fW3FB4lJ~A9tvMv6gmMxY*8l zy}rl?Q+Qu1;gPM^$U?xy2|s@HIbJ(o_A)9@J?x>k-857xa;w6fZR#yX3b{_NM9vX< zT@HvvHAe_6_j*L_Hi-&xfXt_5ug9X_BUltmoaK1$88}1FKrildso5nLMV%#5i^+wk zmQ!yi!T^^>%tWQOy$t(82CT`^kP!e&o0#)tEFzS>CnqF-RVaF0N}1qM2J`jMg zCK@|e!>Gc$%j%2qKH)WYtJ50(QdlR%sSsSD1dt@)GK0flWSAIL*pU^80>mC7FGZ5a zaD}KK7_4}lDzUE7(6iL^9I;*`5CIK5bA)m!ZY_w-A@5*m<5;8cur0>qR@m1k-wCEN z;tCZiRJe;iXZqJKK7VrG@@y6EJRVx8^J7oW^M^keO9j4oVT-3vt@7-X%OracJO>vW zYzzt_%Nk*$n{jZV&QE^m0Dtu2RoVkj>XkoxWrYLt36DNBJ9Yel`5M={%E+_m$#U0#MtY4zE5QDOvJr*Tv(LBiC_Hk_xJUEY?mMmJ|NcIF_WuMp+bcU z72G>@Vcq5&wT!_@S=Jx+@LD&j;41Ry$Nw3M>aAe~SP4sJ5{*;y|6_x6p})UmeNN%h3D?(7HwR?Aqq|`((lns`y@`l7lf8A ziD9#Cbne(EPK&^t*FZ3(y#`NEFs_c6fDunm4Y5t|!4s1*8-Wcy6hq6-BE6IvjwtA; z*H@6~{7IR(_bHOAS`0tHR&#g1+$kyFa{l*}2@$F#2{az}K6rd-AEMw&$=lpmauGE1 zL<5D~V~nzX;S!BUYot*`-lg}Z=OBj%$|F7w*O4ri|QN3Vb5 zl~tNG$A?cW?Rsn@am>V)_Q2zVk~mA22R?FQk#;ZRk4n<)-1>k&{N5%0+rM3>Q5(w$ zHfj-z4L2gWRAH^%zhOVE6G!Iw3%2U-!$T7)q>ky{h-CN-m8q}xSPe(@&yY7Mdd zYUW1dmfq0BZu#)a{3=waP~pMEJteqwpWdVhMDPBl?Xh#62wIGG_fyVGN3RFb2^TKo z#P%s3OZFxcgl$>DwUT;s5n9h zg|rQZ;31ZX*fhOkfSg^^HJS{Pv2EM7ZQHhO+vu@vWo(-n+sxQLeLwGa{`d8(YuB!Y zT}v3N#b2(b>(<-Kn&oTML_+GSU>L5dorS`Ll65*!Wwc|pXkzr!hax8vGK!!$e*`7S zs!#(3-oyyVZA4f`H>zcgW^nQ1GuE>b>^SbP&S%>j6=?72+B3ROBa!(-V<{MqM51!J zX*L>vS2k{|yDpMs(2O4UoZ?%CuKpIfdD-ogn!kt+u*xVU6m3?UmuA{^X24=(en(KR zok3I_{K3iwkv-r9SrASrw3Hgv$%ICb(ohnS-WXIxQYCD#Fv&=0%<+%_1f6vXfpBw) zg%_JYxTQ=c0vp?UJ!GT4-N`>jG6V@^m4~=|xQ`>P2h`vyDCH2eUncFp^9kCxVHd0i zGIqR!?Q=3jZ}RbywX0jf-b6Hf`10+3UOR#mvZiqIIC~tAb4>N+5}-;`#l>KP)G9z= zsNP(}ex_f2g&P{XRb(56dPnfgMi!R;SA)(MO6~I|pL76SXhBX_$K>F9SF@~JU++91 zYV~_NB>cLz?~*Npsn*$)c|C8Xzk5ksqg1j&`eZqoFTtYMr8oqsvw4WjCUj!Vd)p=K zarQD60XD|=$>$xNqeJA$B0rr^!5QLc)>>C+r3y@JHMU>b98STC#5cEPfHPR09nKBB z?yu$b0RQ`GXC8N-Wb-_Y{TroZmS+saYDOL1Yv5W9y`x!uePY_khLW&@mjX<)OhkMy zm)w$!bfAb$Ann|Y{jY>)yCo2|K0DMFz%td3)o5g zoHBT{4P4%t?AsjjVa5U=ErMgpFqk8&LU3|Tx_AQ&&n=bvUi5po?=9pelPD2M5c4>Y zhC^gkn_b;l0$o9jha3vB>e_6OECvS4vQO!HM(@N|0a}BNQZ3%;)nDu?b!1uCo{9Vz>9n5kMED%GH z2x4jeY0kDe0kBQJy4RrwPrr{dI{%jiC{N12t?SdT%Rrm=Y4RjDlFQ?2;(Nm^NAtqq zdq8toh8WPz;+#zwtx;pi~=chmtgdu1QUX>s5frq7%%b$uAn@o-p{ z$31YNKJwY~W7&(-|L%@ek88!O0keSxSvj7TpJ(OxL(FUI^t~b;=WxskIlw$m=Xfn= zADZh*nCgg2cmg#_sT}w7|Ev;|^w}a(l;-ZO&#|g@U>^LyTScCQv9aa> z*sc+;c_lDODxy$E)!8-fw7&Dr7F9m!y&9|MjQT#BCd*c4JtC(0+UGc*FIFgbKCNG9 zU%Z3K++fU8TFzQ6SA4cvZNjXA(EY)-Y3{6+39D?b=ArSk+am?CFGcm^)>BORf@V0O87bCK zMFmLW5sNr0HQ5ZiIVCSug%GO4?;wcIpE){}5x+7idUlGIE*WmXC3EK)gyQyvNZNwD zx)AS%7$j2spn_FS$>$j0s$hlE%j^NCr!e4XRXR`E9h92yPRh=S~P)RK?*Yo|!a- z3L+&Ih!~D;fz*jf{W|bsET)$p_0t>!$zmPM5%#XDPXfORob9j}jEhG$%|0>Nkj1N; zineePU1YyfvLH_HhdCgTAv$Vb5em3?lbH%!1?jg1Z=LQ6t$M>q%Aw(d(jvoNg~q`M z9gA*?QJvyJx7$vImWkix-?Eny|F6Zx3CRQtMFTrLqWlalK8KY7pmSN>ob9Dzns9ot z=g9lqjQ_;DuI5ft(F*SW_X0GZ&^w#Cwp}>W*@~%8IA36Xf4!@>KRZPC-W!v1u&ivR z@o=$h93sc#9o5Oz#glx{*&luMGoyQ+&Um7kVJVN|2RZFD(9!E&fig@DF2ZbD0`;3O z)33XC>TKOvh0w>q6YQPNvG8kcquPz#D`MPSZgxI(di~XX<7@txSC8Ep@Z%239iJ~I z|F8WeP*y^EcVxf)FJYgX?GoL{UY=P;y3%i^x(L=1miAE%Zv)5y|Bn|+nwYNo*k3C9 zR^w6KYp5L7i{*+%BhwvM9_T`R+jc=>EtgBb95e-hmculf&)xA$ZgJ??4-J`WH~;}c zh*3InRM3?{Z1UkIr+>Dz`mA$cimTapG+&f(@b6pX-!|^AL#$R^%!8_tV2%{YzFc?v z^B0AQFuUQHvS~)_&`2;N)S3k-kqpV&W~>FB%`@cY@Xp$+yXn|W+3nHnYPCxF9*n8l zbEg8$Tw*o2L3XY(r4vFo_LE(W{`&SZF;CvzN@kV}Ua>gk7w#MRNW&CH^2|Y>?JKX#g0|_83xT0tziD7Scn~@+0yLE#jDAT9ciF5Y!~v1{Bm*7tuM~ zANX=}FXwxk#OqAk7w{U_A5AD}Y#t?1lnxSjjF;KZqDEnz0gVDp*}gokvktLY;7o2} zutDYhry)D%Lm#Ycb~x_96|Eaq5i7ojjiU#hH*=CocRrx=Xll4>zd4pI8YW;aSYF{^ zKhb1CgaXYWO)LJ*xqB#yWO^tCNsbE#&55Z(TC5FL7+2U9KO9t=xBk~KUJCLe#YUG_81{N50=yW7uWkj**V%BPjc_wLmA*1zu7U;)QkBv)@IWzp*IKA*4AKJQDe zY|Q?T1Pp`53zM=8_e};Zv>#nO)-QEaA_uifY3*`f~_NmB!nE~GCaSht%-`*h2 zO!J5}v})#=hrYR5ILXFS4#!{_pGyqC-!TZiZrnvs-8X?-=;xvFsD&MIx{~=Rd~~Ax zilBo{1Mp|-`!7b(=JIv7-Hq0hX-d(s<``%{%|q~e&2!`V>ZcNgFL|5nll>xQAVocS z@xXfTGQ=H2nB<_w4!L{^xmx0sto~PiouTe5jY;Fh0~_7fk-DXLCgmR^-*56S*xxU` zWNqm$c#^2w2?EP03xTQZQWT~R`&n;>g4!X%6^88^dnehADYlFokUSghu{#|ei7|sV z6Ev2_ZO@zwTrTCeGjUqrF&TK>RQCPldW5c!(**^Iko(ftw2??cmo%i8kfV)#hMf~a zJ)-10yAJe6=H_UxpRuZR7gHROnsAWzM)`K4^{#ARfN;_d+2P^!M{i7sjk+U9*$vy- z$*@7i1tGFZFt`>Y{+VL0LRlhm>zJc29_e3K27 zx>`L@X`&PeAkg|H1e*KGMIFFcj~dV(A04+C-#9F($lM&4{%aSoq44$CXGF)Pe-O`G zkma^G2h;L8nbx0%Ce=M3YK4JS>GPz=+d9+xdW_xW=K5S)!+nfB^#{jXJ^YZbK2?_f zZl=ckW70lH7^sN%?fG(t<8^x{Rx9*8O6EuZ&DIigos1!Ku zNyQ5{O;>z&U%%ksPx8?FKDz#>eSq@2&WPr5dnU?&XUQ>C2vM|9eqrwwa9zGh{0(M$K#T(RiM=^;Z9! zL1VgqL?>_qUJ-AiZZ`rclUl)URL4H=yRD@%;`&GRXMIYp&JnhzIb(@NoN73U6yIub z7Ka?28>i)C9>vP@Gs)g^?&C$T6H+eRWoyY+*=_f6iyK1`z@qxc~Yx|p!Qn#)Im zm&}rx#_Iur2{8yGiGqj~3(t73f{~IjiK;q$f;RlT5=Nw~XwL(Z3L@1S9KcEw8 zgNvd&Z~#INCKNUT)2N!bOj~@hB*LM>)YEVKZVNfgKKtKaZpdyl7G4(Leo36x z)aWhD6A5q58kW6;U^majILBwTZnYXmM9Hlgqn%Nw3t zTBj4pVVDURPFx;b9yEL!-z%c$doV5txUorjwKrQ!3UjufJe6cfc3i%7iIB-_`KjzS z=@P85?={pFSI-ZqWz8&xz;SL{FkOP{aiw<-|PGX&dIvP^7REYn#XH}5G#_D8l-|byLaK>mD&1Z{9e5@H_!V> z-|qtFekrRPpDBp{a)Ggc;`C5-ww(khB!>pVJ(YmFw`R%F4gHfaV|DQwxI0W$$D}x z|Cy>Rm4!O#EgZMCU5DqemA!gjquPz zCVE)@q@!q84T_=HE8#6=g8nISI%iL|0aR=6%&kz-x?Cn2Vu97}2e%8Hqe!}>F3w4It{O&Y8{gq_oT zLl(>bo_~K}fTo*;ylGcDX(}9K*bstbW`uVmz}eA_l5D1q*1xVLzN77rp6oJ*gp^sY zwD5DU!ylY98}jN=uz0||N;yZtouNH-AqNt-sXuQaoVpq_HjElA8w8(+5RnVci>d>j z7UA5oK$AzP%RP9l&w;z5n&Ps?Nb+ic2W50dia>t9szK~-$P}m36)81d=`5ocPCo?& z_ykkfAQ%@1Opq|c%PDX}Lt4ZrE+jQFPib*BE!^;K6$Zx;9KE%`fP+o|Q4qk{!w!sH z@MtTzOU@FcdN|4HCivGk!|aA?G%kp|0uqZ$K5?* zJE@ChKcHOAKKA-)-!pTM?xpx<6wH2 z2(ADIjCX49^I<=_5%PZK5Ft{0sDQ^gqMp^=|EGo4ix-{${CHUi@taQ6S#ZiK}c_Xe=3v|48^sxmCe5~_s;eR)UWAgqlB%KsqRHa`uriCa0uD+oPGLRVnFuuDuK#~>Hn96cxEYR-f3R%iss4|igGUV@`J+h~pMuY$A3TT|s zwE^QNN)spfc@Xt(UBO2^4JPO{m@Ji^Yn^C*_uRrk+wQlvs|>;NuqX|oTmERp)e1|R%kTx)bn zxW6WYgTAi2tQ^h`zCF}XpoQf70}FzJGb*_cRe!DlPCUDgxEbBwYH1d0C1Z4hSgdNK z;-AEn%#8ETQju2|qD!X7%$&+X6r!Wqmp57H=)+t(fQy9$OOmE%Js9XEVLeo9g}I@1 zs5tw(jn6l{$H6QA0iB-MDnUJ>`!Vq=cKo1H0tn^y6jKTt=(@?VVav-8Y#hTJ((fpN2q;N9)1=1{u4ba8wc*W zcewc1pElcR<9xrF_#Xo|EQ$e!SDbulaT4{D7lXKmYN5u_xO`8$NcPparp)&ydaqaQ z+l$UC2pD6kRJ3&J#p~0qtf5} zU$y_;4@1yN#e8znSCL7d)(D?w>X=cCtO#IO3~Fh5*-P+acJOOibtO}0oq{!FqX6sI zORE475_ljEQ7qUnr{J*Sp~ia2Ge}a3m$NuYPjUm}mOq(~ zXU)NI^l;fyNU|Y9pQkvE%KSgXU+BVL`Pp&Mf{g|2oh9Nq!&nqxH{V$Qux>BBB)LB! zDSBpssMjbRH3Sb`tdg+v)e;jQXM+d}24_FF+8xwCaq^BiQoz!9QXY^IQ?@E$cg0Lt zgBL!_)#fMlG?#Skw8dI^eMM%uxCE-?s1$vw=BReqR%kRTbRrpcl1PYBM$?bH-!aLe ztH(zIP*Oz}C?&GZovplbzLSi}|W58?h(fg@D&@myOlRMa zj9gfCiosi6E}^pEX|ocD5jtH!qrN}Z4}kniq+O*(=kIyw_xZN6AfXLxcA*=T_u;^p&FM3;BMm(PP z@dH;YeRKQw&_HgTJv%a=$2F6DPuB!B^+DrQT~FIY;wu_{hku40b;jL0mv>3;HRG1M zQ6Mm5@wK~*{_5>z$&Zng(&G|!=F4%hoWXk^wfBRpqq&k8l2C*+l{bp-oYeJyv|XE~ zLoiocsp(=!dWHiU962qCC1OhnN02K;%Rn9snfg;|J?kMfIc;2kP!HrP5^6G%@wyy`07n)$RLRpJ5HIA zsbAYHg@dg2_kQ6CTKR#XNYLH)s!iBqpOae;y>m)vO%wH9$S7$&!hqC`eLczLNy1Xa z?uLeGuCskz=8QcA{RE(KHq!uLLr@$7TeM)|$qQg@DMp7%V!@pxm-7LfTUa|)jchQO zi2IL^aWf)ma?YWjj<3GkLS*0K(f`uNERjw>9mB8U++dnl4xt!se+WB}IKLUvn4)l5 zfw|AB|3WvHoE5ghHVMYD7ij$1w6Isf?;W(Oe^@(h-gbY72^ansg3^i$CU{WdcU_Z* z*(AeyQX1o?Ks@syM<|`b7-6GBN|5L;Nx@#-1fPF+Ue}WcW$40*UTjzVf`-me+WAIC zG;J$1RG=ciJ`6s=!^|SYZ!??z@!C9E>|3I|4&9r&@GC7*OqN~Ogs5J&s}_mH;)k(r z)HsnNAp)mzyiVgV`);nQKCidbs!it&noH0G4r*)A?KZBO=i9Fh^ETV|_tLndFXVe0 zI4Hm8aYypy;n)~+eeI33T@YXg&imX|^t;wR;b_9*?F$p#-Qn{MJdm~poM?OS$LXwB z7+2-}wIuDlqHhSp_jSm?*I7M$z3;}2hmRJx+9}2hlFNNQbCrF37nH2g_xxZKfzBp5 zz?I-tUj`y_u~_Zj>>;~JTS$h#l-%I223qo}i*(jb#yaqvjOz`r4jqYRoV1dd6i@~Ew7w66Xw2RT8^_dkpHyXO z@|UM0SGJn69BTjfJ^35$oo}c#E7DY!3sk{F&6#%>2y5o_9 zZhma^5t^Vayn}vN2?jGWEH_+u#HgO|n^C;hLXO8C)Gd3al zmC#u@7O$~K0Qx^~olQ;$p=^%-{PVE{Tr(5h ze?6OyQ5)1vra6O!+SZsXZQ*q~`EbP@1(JrqN7PJpKHnc`=(P_VPUYR#7d!4Qad{pD z2dzDhZY;WbXQ)2+p_4u@J)(H+-IFW&dZ77v@78{Xmu3T__*_#2+_P2cZctCXZid0$ z_IaI8zyEIe*GbgQ;wCJvU)_`KyV3MjU0acu>i~-v%O1I7i;lkgLCM#@;x=v%NVIf@ z+k{CtsWvss6mtg&(KQ6Iw+2pkm!x5GP{=$CWV)1lF(>Xz{3=VKf>A=$3PKN#h&%+u z{8LX@2qu-NZbZ-VsJ}C-NV=#*zLGe&q@$=IA~uZ}{wWkU0%mh|NyOT(dVm(z7(J}> zm@X92(q;k%wNZ$#@<8PgYllN6HEyjf3~EX+YVUO#xCgLzw4?-J~y~u&$1xF@Hv8>&X>A0E!ivxSGJt zy5>)27uN>ud#924$`7Db&iY{+p)ZZZ2?fY@icby@KSojS#!WJBdN%iuFQJWuOCUW% zVTG1Z#$q^Bf?^7+2AKu;jfnY&rbPcS&8dZq;0kI95M{bD3=XH{5Sl>*0Th0a6rj-j zNnh0@mC_rMtJ#DAEvxh;g){`x03IDxJ-;sf*l#joTFpTtUb%YQV8#+#QB0|x>aKH z>!QAiTAPCy#S}3E(W3OJBd%o2#pfGj>}<@=C%;M2DJ1N` zEMCIu+v6+`2-DMfY-IH(vZ?D`JFqrctv3HTuv?98lu6{wK`h_(zH~7$BKB>JulZby zwngp6UOoIu+R|(OTVGzd1j8j2q4pY3dDr}=?cznRsvZDPQf`r6sn#2q^NR(uy4NVCNKMqq{aN~WjU@Dj!@x{lmDf=_pnO;`)xb=sXeCmo&SUV z!8Sh+*vV#~`iDYVl|_U1N%86^wb7eqXf)k(H?g{!5O@kGIQ-vW2h0SGr{}erbzZkG zbUwEO2Ed?hF?^GX-oRkHMYPpV!%Xo$cFV@%Y36$`G5R?cIlY#c)M<>_i_Tfj7ZV)0 zo_3i`%}sOA{OzYWfoabJ+G%rZ7m?k%&{FC#5;944h=XCNdu52KX> zsi7dCG8jglxKdbDNC8Ke`f&h3822~;5en&{osY$@8bb*iMcesu^n;u`^LY)yo7eUR zgB1e?iwDO7|C+Yk3Ri6f$bW4`YaeBv#a4vdg}pDyaPKWfYJQ!zGd4U7Nmuc>tq?_S z^g{DIa}l-vh?^?baQ@7DBYvj6OFdB+J7IvIIGf~!!mmB^Ub5og`}!Kp);x23vr3`b z#PG`Xu+1r%Y0IzK3z4=4jnfNek^3V7z@p{s$~&6Gpv`cT-IOj{lV&QCo^~NfGMmQG_<4In zD#gL*ngK6O&ghWE4?iCbMY8)vf>$FLk|FCduv&G9NT&sGWzj#z6~Fw=y=2nV^U3y>p1Kln@G9as&!V4B8#61Yl{k z?+$=v@I1z>nkt0^WuxtAa1Ii-XcW(mtfQJ)C=r-=A~!?HNhFm_8gAfj};jgV&!0c5tAa5G7KGnn&I+ER5sy^4F98m#aRr_ zVeh~vRp@FO8rLNtJUb77Cb=UppX~|4(d^R{CydiWgP7(hIusR5w*2m#pk^Xlb&tbt zA5WrVt;HWn2g2x%OZBx!uX$a(HTqumj;9H@*GlS1xl3_@Nh^wZODEYX4ceY%BjY}e z7V9trNlFQfp$W4hgc;|jjb6|$vEU{Myiv00;&^Cu!s@i`@;CuddGuKyP9>N^!jTZ8 ziC1;8EzQz@6YOF0T`156L{)h3`lIvZ>+_uJ$RJdNN}(lDfhmH8L>SoPw^+z!QxNAD z2Km5#cKtL>As3+U@qyCYR)lPRNZ|8#NHccParQO?I2BBtCq!5Ia@Cl~3z37B{LPLl znyoQ+*;t{E^#NirVrI${_QHP!RMk&Tbb6Q0J{8W>EA%tXFE7HAK4%rsI@?(8#j;=9 zlC<4|J(E$oZerYpK5u1LWR7O>C)ZAfDLeiFl$evBH&yX@RHB)N36F>ug{^iKApH@C z>nDxX+^s;0(6Okj_l-~a0CymkxXpyi0-JWt6kXGh(ivd`iFie8R~}g^6Zo!mo$Ts z{c@y;uvlQ%hE=)@H&M1>iPb6WfgC6A=>NYLAUm53QdTl!E8x#{dU1fv5qi+v6z;t? z|4t8pjr%a}hazL3L zQ<#XPazGkKV7eDf|9X%Qi6yg!*k-F^1|EyhjZZ$lKTfBQavT=sf6M=6|MBsCx8p>3 z9mrbFc(h3l1|(NRq~&Q@Ie6%70$XlM!TEND`@H2V-Bj8T5u6xF&LtIMelg(1lo6E8 z@${%8XpJ=N^^=7ea#q?V8OR`JsYh( zz;K(qjmo<8$nbL(6VQGtBUUVBlNb|e38C1;aVIveSPwK&r?+u4-853ZxzReGe`30&uV2v#O;P|+@ zKx(f8hMI5+j%l=#G0%~w1zW;?vD(f0)R@Xo6`_Sd`j5Fj=&ezjRY4+9IhYkKHQNBT z|6~OtT?>BWe;u@Jf6-Ple(S0??-R&Rf1%^`W3(0x-Q?&@ns~;@{Ut%-oWWY~C3&qz zgqvn-zjlbe*8dmZ!td@CyOGw(@|1wXA~x1tmV@E->cQx7YZc!<^O&rI-|Zgg|9A;q z)XV-W_p#=m05FgBll*uM=kVhy8QsnIW7A74Q^N)2M1`)<&1+tP571^iPMb1do)}R0 z5UI{EBNnf3?MBGwK3bRB2uNO0Cbmp*+-M`n^j#LeegLlT7mIx+i(!0}EC0DThT9W+U! z1TIW_?BDa>c*xgC;ydLNHfd-SMX4kPqE})Av@D?RydQk)ES`HCv-19>itg!#4ZorN zGBGglQ^g>aP*H>fnJ!Y=AE}EA1S}RzA}~e`IPxthmKBrCnfA_=ly$CC`BWGglY6g&`dQTTupON@Y=IhnJ|r*sLis+y z!bx0`ry+<0!Uh(jUQ9vuc3=fn_1`q>M}h9)eMk_~fYNUq<10R1(*@Dx<(C6w;u28< z1<^cSmEzX`#ftHXTzi}A^)$8$9H5q@N$S$Uw&B?4u2+T`(dU9xh7(6OU;#E;ujC+$TOKMF<0^j`4VtyWZvbJi$ z&8It{s)laXw$^b&m$@x>4g-e6+a*{14%Q3OMq{?aE`ZkJ2TDU{AGSg)AFL$=%aE+1 zBNwXN(%#f`4A?MHc6J8oKPCz4AZut1TNsE~HeOU4(pk!qO^u(P_+BFiq?njh78dqsK@Uj}ODYjyFg{_!{-djuo53z$Aqwa@oHnTzmZ`7$Vc<){HaAzl_Nai&(FgC^VY?G|8^7p*`#AoCIs< z#_Ku!Y0WK4<<4XgR7N;jvj4z$X(Bgr=MC~SPG&oC5ZUR;Hf3^#U&Vy7O?(wb>5$mj z9_iZ-L&JqQli<&Hc`;IYTBkssKXD1~g)|5rf;&CGzc|bhmgeleESEWrE#snemG`4g4(CeF)8X;HPpr-$ z0hQ(Ng|s{-HO>{8&SrK3h7R^!Uu1CJ{1B$!aYNC^SrfX)*`2}YfqnIZ$BS>c*v?vc zy%O1C6hr{MxgR&umP*gX@L`ET+ZWrbToX#rPE5tTl&mEkP^Gl;S4Y^yNDzqae?TmZelvkt%`yZKn7c}2yh@*pRWPVA@4)x!#x}1E{`c^2-tl(m)f&1{y|Z~% zT`bXLo_n4bQ3?dI2xIFZ>NVFFnz}eX+$}kOQla3_3~zO1QB~Y37K0HU){9CAnRx&itLNVr3geRMF<(lb2;OK@ck=@l1HWiMK1K zAnxIWk$Q2*SERKOFAO*I|1&`WY-%YEp0Cjag(^Mb{SJeHxf62CN)4gpP zXb}|Kn5l!+(8PAwaC+u4PRP#rO9kjwA z(`j|cItz+5%$fV&XMo}kqVs+F1wn+WBT;jA?(dNNk21%)_TVeI+OE8%+btz&bAlcO zkYZCA!EA>&thvef6JP+?VgCyXgaiU`WZB8s%m%?{y4TEZ_84~TRK$K%lHH))AO{X- z4tvZpJEIeTpaM^5upd>n=4m5<=I97263WK2({kDIvC1`{(<3x#)!_4)jOI^?9>GQ6CwEY2Kw2~?rP}%PM z9aJfGC{tklN+CGLNI~j86mD_2H6Wk1)`^YxHk*GRF1TFrl(RI0p9Gotpgxscqt4iW|z)@N$qx=)!5&TT%fKoeye3*u1pa(sA4a|LdJ zc02lu0F*hTI6{6wVZw)NEPPG^CSFf>Z}z+FJMP=YA#$UxXQrTlA&W9n8WDynF1F4$ z51dZ;rs#DbHODygX9I&lV$D979$exD2>o;)v^egUeRrPy7MkyM^>dNmM1#3cdx=SL zH-uGa(erjc17)Fju9=RGXCks2XjRmb05$bk?;ENP1PAN}kD+^h3LjO92?5JCnyhyEhnqk*ru0M=9Miy1G@i zD&-ZI$n&@n+*}i5-|LY&_z?Zr#v5nUhN*i`G z*nLV4S;yYy?r#=mP1wfPtnyy4JFN_&PFWocN9RJPU68e5DdSRi3DhVW)=U1o)VG~D zaWl_dkLZ=ZAGmc$Ja5}jxBJv}Q@j)_MTgY{@CA?*q~BHa4(HsvAJDaeyyHP5!(6gc zE@N6t=G*eW!x@fUpq5sC3%9RJ-sVQK4u6xmIgr-#b^Y=x?hhzDDe89{|46AE2`EUW zt}!Zf5|brqgD`zo&4^C8v<&%7J}6AJ7s+778OP0x3{aV)?VTSZSM$}P? zB|vs(Mm4_-);IR_`Op1RUdOtmWl@Do&P6I_!3Iru??_8hoEB%nEs+O*)!7h zTgfc{Rt$>N)eu9a2M_>W&62-L$t982W63I55B(J4rq3AZ4<5Y!6{}3h$rsc$Ae-eU zgi<6SfkELzgbFDjT`-_RUJ{R+c8T+}0cv`0uztgA6D2AmaM!o!bUk_XrOkAYfR9UJ z%l>DtdUyvlOM`dx{G^JVp&9&v2^i4Gl$08C!sA#e`7VJb5e|L;N2Y<5mGI7$Gds0? zl+=+z5XQ^54lAZB^FE`08Be2gP*KywurpXP2nwu+Br{+gwumZFkIT~JiAKp(NZ4IM z7;3D2p8I%hJ zUN%Ey_IAGi1Dm(viLIWZ{%2NsJMYIAJl=NJE2Gys7B} z`P;~b(K`BS{i1b+`#iYwo&k*BUSo&J)X@r8!W5jiz9V?a`@UCZ(fy}_@7)!@wbWW? zX5IVvA1p3cG`8=m0$b5*-e1YAXNC?&Gm+~XrbDL=s!Mt@&8Y{l_A?qpct4t|>5*yD zmdHr`a}?+_ICL%97%u%tX%l#mge-?70G}~ONCAnSx1d0*E_B;Zj+W!Bi-Dkx^~p>@ z9o>*$BtcXY=WT^>d*?HySu&y8cQ%P+?s+@h?aU(jtMV*gAFE%c?41&IvlYCFg{aM? zn~ISJ3UsRsD;OX$O=Ftn%EAam6GkN^9W;y({_3I-ZN@|Bw?HZ>c#wrT#$ttILYYKM zwpJx(oRFDTM`01-K@N zQbC^XT665lfS94L5%MGYZ{7X6O}JmuoI9NV3kJ4k-sZdFXbj7G_HEP@u91R(17x}c zT}IM}uLCSz8^hpTSbZ1assW)omUeE&Cc#30g49-&U~~}h?{}`6kfNyPL-2tz3nQeA zq$GGiVCBGR=!I!%3y0+kF;DD6O2|LQ7-P~y6TjbIYB|{-Pgyb(J8ypIo15oAVM&@y z?y1&-Esz4VW=2xQ(_zM*jPE1VREj@+LK%&!B{&AVyXvRk0c!pel(E={Q@n_YYc_xP z+t>1ql?niBef4ZtsEwrC;LEJ{V2B zL71-A6=9JizB(_XCP7z8Un@Bs(>m0wwcNrmktvxnq${r328#k?jS^NBk+}LuphW@# z48;4gwT>{28#`K~yw{4d#q_K@T z>yx!q3750H^-HrH_q6d0%73YUgJ*?vJ(y+9TT2_z#Y=u=R1-Xg)v%mQ>7bRkiH^hK zaO=(37go~=rvKasGgD?DGzeaRbBC_`uR`5ndf(2JKkoSb&-*TV*`w?D zoOAho_a->RSO>!8rfwR-;eZ4+bJ9-S^T>sNp4fTY`wNT=f)4PRi;m9aUt3b;Yi2K5 zqu2iiT!`9Ky%~J}6Qua9ZV_hQw!*$;f|E0K^~ttw87ECc%N9>x>(EF}?^b%#Y77#e zr~Kk&bGbl)MD!AJdr>O2E8PwHobeP7Emh%QX1TSCVks*&PSXkB)b;MM1wx!edX zexhs7YcVTDs50pbAOTU38Y*LBFS+RJ!aTEBuszS8R_)c;QM0ZqYLI^GejeWr9TP<9 zXhL5&N4(!I8kjJa87TNXfDj=R#9&A?SiES_j~5nCFSX7(Q3zXa&elIb2sG3{aK~Q#Mqz$$<9;WzexP4#pY?Bij2aKsY^hRw(eD zroFdceg<@`p@FI47`vQZVM&T6Kcvl;FR4^;)LnOp>cR*}sFzlISt|OJ;Y)m)87Cj( zdvizAp9$iom@^~81w3zd#uV#G|K)=VGko%%e*tNZzduZJ%?~+E#Z6&rGT=fhCdg2T z!*IZ}Vn|t0yCREMDWIUEz@Z>LK>k0PzQI4v?`b!p&va-i1bD)^QJAUcjT^ajwTp6Vh$UN*?1~Do~oR~Dk9PUsE ziovI8coj@Vu|hL9O4voHw5PFZX*K@l)9msx59iKf$dz(yf9?WOD>wlo0k7+0*@wkc z7-6v{p8lGDAi0K3@RHdaqt{(d-yP0`Ztmvt!57Wp#~Hi$8`uiRN3|GbA@9@yB)iS& zwLI?rUE7quXf0kQ{aR;j^OfuM>&5j*Iy6vp2~&=~pAXJ;H8+0ffN>drR&IUw-)Sra z;m*i?{=CAn8j&j4{jK-G`^uYg78z6#Fl7d1Wh#c$iM-oTErE6c2XP4@X&@oR5TEC`|pBgyK8e=2ng;|rmOeqCY z_l$R0eabkg=L<5L423^AtY~E!lvx+s!36Hd&h?tks1I`(MJ>2fsa2a#BTrcM@p0azQ%Ak*b@cdmdTes_ zVwGu@=nDk3>=$y~LOU)nvyYu3%48-)c*n~*cJ?79ALTQl4BzX1Vry?gv?Cuee~ov1A$@s;E@4;N3*%daiWX+2aHICi6r7d@>~qP0fvB2aiu1U)OJA$}ZZ0 zEfoznc_~MNTQ?u=2KFxujL~?IHW)biCszh0)+F#%xMHMSvY~`R;gTep!orK`dm>6n zg>jqCHQWdoE;sqB0G~`0+La@_Si~;z!(!31jOc=gmWsd^bQL~L_<;AdkDP4SW+s_0 zWmmy!*mQ}m9Pfok@9MM%0IY37cj6Qy{-w!|d3icMe-YU{>@Pj-G^nKYEf))g=Z5lY zTNj%5xlaCm$rdq_wpBxCN8)M&(~j`Di-%(ZKp82O+)FAgxL`$)Pg6xeLGmW@Z5aY( zOf4pBurTm2Z;;S9=OyMNQ}BkJ|yF*b*9}r?y)-0L)?WUPK1(Q^uBm zh82Gby6BSf(=(m&x){1c--ChY+wGQ+zVTU4rz@B>=r$aC=0%MeB}qw!`l$885l!a{ z|9<*BMuWi`Kwx#8%}^m!xNIn0gPVo3@z7Dv@Rt__-8=`mWn6Da2Q~FG0BHV6`nQ&l zBB7;qDrxMw0}7GximtR*pEIwOw;uoAHwC&PrM5be?SPl*7B(v$UdYuU|1Kz4)c1k> zXEge)&GL2o!!M0`#T&QP$$Dz){M^L{Wi|m-Xo`^wSAspaf)uxY9Kms9x4BzS3H1Z!h{;rc6 zke_Fi4!;R;WM_<&f=G*HfV}G(76WWA2GHvKR981Q(RRnv$%*lxF8BU1+nuGFf6i+NQbY#MQ#l@SH*4e^eL1O^Pd-9zZdQ-S0cS)<5mywQ) zB*t@IJBo08+0ZL=--+pY1v*CTHln{K4p2wf%#)DX1?xr3b}ZdbuP)UPM^;RiJ>pNqni+& zmZgnS1MtU}43#!u!n4W&|5=lV50cxyY`uJv<&vFgZ5z8ytw?1`^@zj#V$CAi`0*=u zTY4QgWf@u@X*CCdB=>?^xlHG$sXjzkX*Pej$+M!)2v7>9`K~et*9_ak(Wp+p$u%Qz zOAcL)7(s<{w~jHh;&eHtPfkrv*+(c(%--|sL<3Npi(SFs{`s#l1C0N`d4r4!H7!{>EYwf4$ zL83I5mg@?D!P%mZO7Bi+jGXoTSRd4+;Dhzczi$H5e^Bu>L?6@C`1Dt?I-JZNbO-8r%o2(4TO`-LPD#U;m!w}8Im z@1hjtqn`+pG-i$md9Zo};m`}4LUnjFQ)M34#22Ay!JmkZUPsyEmQoB=b#n0b1i1_c zgm^A;R1tMkgN16EbABZit2YJH2E|FMu7+~4lbT3W1sO?z$d57^gSr;qMNogX_4Cal zh&J|@ca>#&#ogGpB!iutu6VM<>3~miY_ENYACmn$0`KON*4=?$EWC7vQ;x-mD4-89 z4@#8=sBZ8vfBH9dalJxtT2*eXqyR6Ip^`kCLOHE|>Kk=@0fPz=HTI(m=EFozRc11R znIV6DSCk@&LEsoZ11)m~Ot6ef`)TGjJ7IF;Azm5&K3M;fl&hXSW)0>0s68y3az&eB z9omR$Y!CIj_;-E-JhqJ0ySn8U&p23#b*$0ezI7?}A5GUz!n7K8Hw@HX?B5BDbt7mp zg~S>?3!=@-p#;3A#I+sq44cFI8f;QFieEe!5|J5>Jsp%PU2!Vst@E}5(`lB%FZVR) zgC^0FnX3<;B{Xf`mWpHIM9XO9Ib-Q3X`-bmlSVN%wWNX3qr!*X(5+dq8!%{XP`Cpt z#pdxFyZ5M1AwBOsa$IJwx}0ab=!6+$`@IOtj{sS^t4q=i9Vy`MEWHbC3yN9cJA|K+q?MPqGL)ZbDf*R)HY99_lcst88h zhQQ>u{>3r8n3k1}ed(t$$H|-TGBfjL>C!8WqXp@>b_qBXa5sP&bOc}@>J_a9su5ht z7fo0GuG}4KnnR9#BEITieXi(y+51eyDutcQPrN}2+vShO<}%vZ-tE(kRcq81_`$kw z41%m53`18ezR!$591>d}>l?zBX^3t1tJn{)^N`cXedtf&O(U#QdbyhN4$a)ITR76{SLu3F4TehVvLj~;1YONy(Q@%t-gb|nXT{4tzOS4u+nT^(@$_)bPU6( zr=yZ5@ujKed_=)NHd87b<|==<;eq2K86>7Fut{_2OFQa$#@u?kk6Bj~tnugFDV;`s zdQUUC{Y}>WDpDxd2eQ=U>s~c?$=u7C8)IJP8SeA}#bRXQ=JD3X;k&YgC0GyCAa7_?3Zc*L7d(cY;wS~Jr)cRxJG zTYp)&{@nAl?55Xh5!_+Ls0x1p{O;Po z*jjIkKPP>&;|v(i?$8Nkrod~jKGxT{+8fdL?MDL5WNSB8SorvHB^|a?>@x3kw9(>P z&fvoe#UBnFb6E^xbLHI3YoKEP-=SW+m1E?JFpIW%t=`@A@HNgg%N*9u6aKDly;bgw zc;;fJ-cvrO>%Y@96S}VW!y>F}H9-}NGXNNOe4ZHLXVX*J zFc}f`{TPcDd0``c5~zkh9kVv$$1)oBd!o6hqkr=wFhD-f(zy)es9&E%q77x%&kh~> zmEFF{j=2K*Q#PEeN^xDlnf9qUD0`865faN7x)TlA>Yk!2{pRPVj0l?^mfk}OxxOcz zy5@I%^$ig>toLo+fCMv*Ij3;6o6qigt!j+CIY+Jj32D26kpXBEa7Fuf^lY*0vDdLG zZk__z4bl{q`>QaD>{&Za&)BYC}IS7!f zX$^V9ElfBB#u$9W+LUKFUnk+b*IpVqkp6wJ;ca^wbeh}W?$*v-xCzS_ z9+}5mdYWe9efsrRf@in@&N9~rq*#W{+UacHG{E|?ASdEu5@(-&&hge{U9)KWjcH}% z$bqU1*kk!yMz;vV-o4s#H7~m@*OGV}eB7`0oR$7jxk_}ft(1?k2~+egQbCIIDhma( zNqT?HeG%89(4@l)oCRcWZ@a_I%&Awb;KdJyLo&x5ll3yc-~NK8kiAg}R;N)m1~YXE z`5+yBmZ#Q7sgxAh(g^liZ#0HWKXelDN5>5Li=L}u=xBxEbMcK?4mj1QIpHc)2Z%io zTQyW_Gp}P&MPktI<=o%j3;VfH{HAdksevsi1`pjWheVp*|D87?zX^T)bLFh5!*6l% z^PiSfJ{ii6&$&?4r^oG3N09YEl88R~ILQ^R`&dU8oCIBhuhzjrYxHihY7t)#e9DYi zeL9K;1ZLB%KTW=;^{er}8{lkLkgZLos?IddJ1L}8c&nz)g;K|m&z#7)0_dEO0}mRg zf#@tJtv}jOeesXDZlZLgaNjH3DpVb3&LKXI(poZCQ_JNDXwsdm+rBAhmV_V3MI3a- ze4H`48p)kd6YPdfV>ehO4lDT9%@i%xvR~7b3n396V2g;Noh(zT3UqWkr*1~4;smCpruTxU^ z1KxkbJ4W(_m+##b3Z_ca#}#PdA}^TGQIT%0QY8IkR9!oojNa~?yN{2jC$TxXIG;VE z_PmrERC{rZyLMar`BSR~|Mx#fT?i)YPg5;j-OJ;l87R7Hi}=~cSC#DfhdmHn&Cq6bF zQQbDXU(RNrNH0r^NjKISRwrWXKS)U<|wouzo_bW29P{TV_)iPbs+G`m%;su&V&S@}3<3kfs z<4F=utz$-DtnuCD!bWucVw};(u0_44@YzZJ@T9 zKHCI%fL)2Ps9swG$Oh)Ux6M?W5Yu<>DTTwv8U=<*#7#NGfUArE_}r`CgCRvNnR_e< zysU{C-$@(^UN3p2v2FZv5ahwblm|1ki8Y07{0FT#cU*|~>J6hInHj7~CBms9o4*Yh zfOZM*zM#H^e7LjlstCJR#?%^X>l#}4a8c+PO2b3eFy%rDS`8~@CF@ZrGwUs>>Bj66 zR5Dg|F}&U!a4+B!j~kofD~JP=J<*DdJFB4@^H8&0S9Y32pY8G-SxsV)D@hRJQ(R&d zw#XYDX788B?r4vT7$)4TuZRox@g zr_iB7HuK{z@M__C70GHidts7|dc*mW!qO7JbfyvoT9EfrA|4EGF@3awJqMR>v#3T? z{pP{#zfAR+&+gRLG)v$+kCE>+jbZB@vWGeXHB`?7cn9bOc&I6^w;su$SXT|8_OhyVWq_R(TX~D+oEP>~2yDej08h7a#d?x;2 zpSg-)c_2HjJ5<6t(S=^1{KZYv97oWXkc;}4@on~q&BKWER{#`1(J`TNOD3z@J)hNm zpu&NI!_Kyjz8F06{0Ikz>RySvsjvW>e=<;#kye80Hb%0v2KAj^l_z}n zeXgkea6*y}P6m_28sGiZQnuF9uEPgjb+_B(MuvOfL}>ITk??5Hk1WCICts#F>#DY@w6a?7Sn$7BpTpE|@;DJ44YYByO_YgK}yaXJ$T zC%gbHI~dn6HhYh`*lK1F=L9{|$(l*lj)J&Jub?)mWzN>|b_IJk7Z0-*BI>9v4L3r znL#6R_0dy+e>v2`q7H^Hh?DX*NW`up3}xroH|<7q&}7 zdqkfLD!!gd(&3Os*nmb@2AND z`6?8(;Wkilz6rFUpF=j80fCbpm;8md4QM=Jb91*~?~}I=7w)BpdR|){2j2F}!&&yL zo^t+=lzUV^>D7rMO6n*(;m;j@0*i08ot~|LYCJ{+@yj>;+SL`EO;!~exYvUpG$i22 z7}F<2^5WAYP4B%4id)+D>p)1gu=|fb)q=kr5Ef$9+v};)RTa}V8Pcb=Jyf-eX?|-kGN~0*AMAFGWL&qx&+s@C= zzq^jT`0>z5(8?pM=@_DXttNi@^U7^>Fy=!Y)bC^Kp-{`adkUzZfk)l!dQ2~INni>l z+`6MtRh8Be(-PWEE4qv1)0h|^!-vurwmFJ%`;mH-G}6LT(!}Ur0+2zTKeZfreMyAK z?$zWSDMkomSM08W*aXU98+ujp{%D*-v>)4qqPc(BwsU-vfKMzx)+ap{VZzop!va2} z49%s4_V~Q=&HNOx6u$Rub~uJ0NOaDD)~;R3Rmv)8waDYe=p4n+CpbO(c*hMdyGTkP zt{Gz#D;m*kRsh*BfT0}=@Wmc?FAN-E{rmzYh&9%;Ehx(;j6e2C5MYLtAtTsmOd>=s zjm?S92wZ!)X+ZJ(wq-rRf^GT6+NMfe!#^$>;C`7%wr z^)lVj=>(eVJUXzUs>3ghL;jiGnh%>l3x9czUrr;d>Lv@k>!Xm zzFSjU>X!EqM1o9iGQQqW1}@CN& z<;fQ(F>@X!o|wU+FE_+rqQW28A*l0p{#NA>j<@feaXfjBKe~aML9NI&^9S{m8grV0 z6#h`;jF?9+svH4mp@7jwzG-V)FbRI zqx;#QTNOw!)wPTIC>&xxJ9^=fku{<@cKByn^J+|kXK&9^>p3E9ujg%#H~Xl@cHb5y zrfXR|Xi;pq@hIbYxz>&+q37;JQN+dnz87NI-w->uQO@G=pEJ|#@j9XWfy(5rTyUXn zhYmI-Nuwc(H($s)6*17tQRb}s@JxDT=Ot`dKZ5awFgV<#g7?2yi~#3sn}~`wBH+tn zm8@P+7l$*lwz-^I6N}EzqN`U4ekg8NUsPg7`MQSA)@a@+2yZt|$WMj$a)a7{Rt?Yr zPpkPH%;HT^ypAhhO!_><>tf%EYX_k%HwQgyPOy7i;UerTaFXDw_W9Fef4UW{R;mq) zBil<^fW+~_C2&NowOCf&xodtNNj{ky!9i6(S~5m(r`_uM$*(G)f;S;FFe5_^Zv!0! zyQ!VyA&1JLsg)HKP z*n0^H1D5J-O@9_`3t@GAnZ}Cs#6Ez=N~b5-rgz#b>^3$Q+!?hC##Bc~nak6yVOlw? zvYRh$v(O7#X*#eJ%9uHi`-{MK=p6hU?aoY#SemGzG$2e-zjZ*=0+BB&(_5$+5f@rR zcruvIz>Z(HY;yOBr~9n~Uetozfyz^eArCU18z3?iDnoEfPm^yt4m0SUxt*y!Fxbll zjGo+{Ady5-f~HHWE6db;mu_-%~5sVEhkv<3fPJ~T@4dwd6E0bJapEfz&{vMuwe@`DIRH}YFE)7@Rc@6_FW4X=epB@;2i{LR^&?<3h zl6k$M_@u|G@0}jv?fM4@ffu6pFO-fF`Yn_QSyOQsZN7JH9weJ+PnbIX%|8O0{N(&u zcdK1)aqrH<=v@MkJ;j{Afhg<(S$lZ@leiexyZ2l1%V3Ej`81Fhsnb5JhRH+>l^pZA zt{mlF;I=afv#JtZ&nMEck`eC_WA%=*u{CI#QET%3Z*rTWlsX30s>T-{<%SMU1wGh$ zKdy#4_NcX_(Y#M8nMkkr4;T6K7mMZ(7<3sg@wi5Ax z>p>=C!j%DZ?MAP7U>}hqN14$?{#!N4!O%(5$3fb%o@h#o5sVd$Dfd)t>W4KT1kcPW<{rQ0&>OPuB@YBlTEub=P zILD*BX~h)fJ(NhAmE)Z13{Jq(v1R%G1hd6BHcY#rn)8Z?$Du8(>_c zpe^yy)UVc6CKz#4g+Qx&Pt?xpcoruhO%=l1NkzV-35ahPFCJ?m8p}jx`_6};)K$|% zPvB|V=oJD{qJHO_f{*_D#`1Gz&mWV<)P5fU^$RIkZO@(n5WarB`V*2aqYnRU)0z&B+wiIAvLl(Z{MG$HM z)xyy06q&J(dUa_LVwtCstK#~nVJ{QddeDn7WH8Yx!`kXBOQxEHZJ)RO7-UVJhIgbB}POTDm$ zt>Rc)&ul|Ww1{8i2%TeWDrZs&r9UU&e@zsv>#t=o(f(6X4S6P`ePZmIZcYeg9DIw4 zQLuH%Mrg;0&&-{sZHPl<@tC>kA=9;mh!d>Uo&E-`hE}dg(iDveIwSQlv2fF9b9RZD zuVez}fMPMZmE-k3IT@pLK2YZnmypYWTV$vNc=1jDja8TEf;Y)ks%f^PZ-U;>^7 zQ5U-Wepc*a;+bLKF&6g|;&{81FI1-1=_0ddgBmF5a&cXAshH%=TKi6aer3$&G)gYp z>ex+w@IKCR161sn3b5f z?`xXw>-D1nA2kB83|M>_>ktCry;a2E2kmchAVRCb+I+_XJ zG?F;?Wy{xX7NPwh!&;fVBR2fOUwCb-Yh`VatKPJ7wu_yfB0q}Vq!j)TZ&kuqCOM>h zb1j`@vYK@bS~>cX)l$Lv>U+lfvw|y2a>11`_)QgxJxA6xCh&E9|GM3+mW9|L7gYbQ3pWBe3h2eo>Eo z<^tuBw^ksR00%0+tp3?ngA*R%%XM@s-);i&mnpN=W(x6+QMlL zq&|Aj_ryhAqrdkteUDUJM|~}7$x^0-X8$<>nQ!~_{wKQD)h8oBel`n)FAcNQ<_U4m zY}Z+2!JP?h#hkxr)4}D*(HP&V!;s2m1ZlUWD4Tk%q-Gu_Bpm%ekC-r(JHl)J{*70p zF|2vzgp9!sJgf!IOySLxWGn{TV|@(c)*?T#u#p=b&j3q1W&u(DOW(*I$##8h@iM=K zeT1nbExrryZzIZd+3^DQ#fB)$ygPfFL~Jd_N9kb;nA19$*>Z~j(uvZhI+7tPAm~zn z6>;%3U&^)=eWzM9#XaCuh=dteemGLhnMA!CpuAZ$g5s@DMMq~D-4NRlY>C5Kd3Wdc zV#{fcFd{&eXBJ}A-xv#u66h@kvdKx-?K0fvlruRN!(9vUZS1?|>)^LJ1SB)1`pGfb=d_=~p`Gxuo(3!&Kir^~kgW;onf zigwFgxYrt#W~lq`jA6M7x~D&|mx4(fVu&jyx(}6of(hCact%3L#hSU2wq_o?k#Ph6 z^Tx76Nyv@}Kr#v|xl)7U6#9E^6tB|djHC(0$vcr+pIfoBpIXFrgXuG@o?+U}(vQbq34Zb3aw%+tBb-F|x$ z$tGEBO5fSGD*-Oq@H%rQY6n6Q+h2hO)WPhSFH>y*C4z({iyz@W(l~|R&4Y$!cWC9F zu>@WdYu+PCmu1@kP_8I^TYZhOEqJ5%$Ry#Bo`bb16MG8x?BV*ontpplI%yReG3^e8 z7=yaQDHuK(7y;{e>;K|XN&5k<`m5i!tlfi1Gs40>aUF``dpTs?ZDaW#Pp}&cj_=wT zXM!-_T`?!p;t-Z)eCT#bFX>*-K{l^lrAmzp%Gr{s272;e7qd=s%pNk{|6Y=u^7ULR z=MTlG2+%Y&l#UVoS^4jeDDC3S0+C?$dTCiC6oPSo;?PIuQ2&|iDRSOziQ@~=#9=Ln zYQQ%@*F^f+_z_o+ErkJ&c-}8_bk{JDTysG{92t1$Kb&sv<+VDe$F<3=#JmJ9lXyAD ztiMdUccD@9XYxvR+*n%tksYN(a_YC#iiE#@+0dBpI6Ai(jigdGg?kc+ht>%G9FH3C!9=$8V`j zy+v)uf-q?eun8`_^$k#|#+W7E@pf-kv^jkQA=f|$X@C%TwJmj7FfaJmD8Bj!y<6@- zt;^qLS9)+s|ZeI_5bm zs}U3UyI*%FB3@!0^xOF!{bH>bE!6cCDeRB*{|6%D2N177Xsyv(6ALKG!KkHt{~TJh z`t7+wz;ck_UfUPmkf5(3M%+3H&K}PkKBy?dTR6Txz}{DoZL!#RXEW!YdSFyZ>r#?evU!wDss{i6^B{TAQmtpOy+NM;TVFC{A^wePN@SnEC~4CH`snJ1MqSoNI`d7<<_u zklZ=LCh#=xkqR6^?KOw@T&+vxeOTyC&upeuTgEy1Hcjl&d3So-bv_=AEAYH8IFE~Z zo>D7|uA*j!ey7(BiHPaBi63JJC=gbv*B;0mJ| zgoIO!V=%8=2;X9KUJIhijkz@(`9HO}+;L^*T5+{ek{#9e)Nyjqmm`uOcwHm7Dc5Xd zM@u10dsC$VA7#A8erlw{ul#M!vWY7?vXcy4iy#E&eP}L-=Y(e7y{O*T_qvNpC$FFm z5i$E~X^+6oU4AlfQ9fI^Cf{ca#N(t_%LmL!(;;i|tLfsvp^vGItnahAtKRY(p)1mutDtlsVndDi>&s0%Jt+|D2j9^Bca|1#e=5&n%yV}k9KC^1pP-#Bvn zI|*~2w}9W!P!&AbxTzzxBdaDT`VFsWrc-S3-c>N8B&9E`fmSJXJ z4NoNpC)*^rfIH2GYD{X=Md&fgh;0n#?_9W+;wc{T2G#cU-Gn^A^ybK9fdX}WP3d$j zP>c(~z@VcEky3mqvU~X54NJ}YRL?f_7L8b?&<)!XAf3UiH}ux_3i+7PP-_f7xn3fx zq|BZJAgCZ9Z_92>WNs!bxR1}R?Z6OQ`ZKzqH(5QpbWpWQc@%2~PF|hI2TPKcI9o|P zMB~dsAfbj(B-&dCw%VnfUn^@nA6lsT9)?K%tP{;QrV~#YHGwOU>D+(%x4uv@Mu&|#b>&b$7H9i-&y9&|k2OMmok*H3hvK=Qi&kY?$8NSdX z9SNlcX>}N(26uH^;8h+dtEYgnV5VMtbf(l$#C2l&APlzT*eG2@6RO|VQ@b#2hIP;A@LVVxZb6UzCV<$2_e!rBx$h(MIOtt zd5nv;H@0$NDL((IvQ|jKVbvd z$x850YWq0MoaQ%e&=I5^f{5?H<`y_XAN818h*8M9Wonh2Qb7lvPD=ryJ?|DD?;@6^ zic=ZwseCHT=*Gi8HTkb!FS{?Xr+Jw5=)2bz<6BRo{&aw6WX<^fVl)+9TkoQ-J(cBm z&7a(KIBvK3Wdov~0obN)U6N8O5uZm@B4qKeN6Za%*BwY__iR=cPC#%lla%mvP1oQu zBR&21=4Q$A*-#4R^C>&gr+-KRAMhX>Rd)H^Lq>;JG z(GXrdd(cQ_1nD~2;gF%jbB&YL^S-(!j;4DVAff(rb@se8zR6K45-$#b3n ztY82|BysQo_QF}Y@?3nY9eG>nj zGRyxJ$$wGs&uxk5@+499IDt2?n2777JGDzwlD!z$7X98;;G2s@=&SnJsh)T><1YjW z7A3RYq?52>!%yS;KR8FIf82bE|3ghoR+1V<)ku>5er(sCQibS<1N?%}PT4vZiFNiV zo@|2e?{Mn6Xz{;~$# zI*L`-HU;a76fLcDw7O}*kxcFU&uy_Jdm@5+2oXV%-z%u)I;+6(^DpL@((OofD6H)e zrG6h9rof@Egh^^>J|f z@hrIb-D8XLPwzUx!nUD2(10pMi%eSN4M<$z@V^Q?6g*3AK4k5AchkK+lY z;Tc~Jn4&dYgl|ie;dk~a^|wCLKNbi03clP)g0U%JUPl|Jm1- zqjmsIH8ZNZ{3igst=F|DcOOMb8gl`o99^L+Y9R%LS)Ar&GH}^&6cfopA>C;2_dcxj zRzX5CJ2WytTqz<+Xna&?d_h=z4CI-L4$zg->1&h@8ld81r-0LU>GV{SgADnklCV9y zjY)+^Uu7(;t?U>Z75s8~_(fg_)j=q7PC6=zP2?9t`U+Yh3uq&^jpW&OITBNAJd(Y9yTs_g9{lJf>yTj=((#9`V}*>m8b#herY&-s|aR-t^0S(8)Mrar`+=Hayb4cLm8esb3<$1 z9SzL&OE*Ifq)Ne8XN!N4LII9g@l?dom6$fj9fi(r5X={l;F$Jb7(iqic$SC7nf*_? zpmyu65@-eL8&zF&(%A}co6j|SZcDQUy;@>5jz~dd5=!HdpB?HMx14SPtO3eikLmA>YCwerlsH4#Y!1lqT$s zDKo}VL)-NQuHZnKm(>=*6{`wy02ecA8%B!nFDYZ3L)2tCQTck33Y~vxbJ@S&1_|Bw72RHCyMwy?V7rvB*p_^hmP*De1pJt6$x^DLA=cHG!g z_LM z)`o_2%&8c&HK%@3if-d~LHuBoZUG}T;iK%TTm0XV%TC~&G$bh@&%b8|Eb2dBjuEw9 zBS(vPijLX+vM$vi61aiU9ABc>y<(B0#!w)Fqh6c`Y8r zh{j{2Mu3Zr=cpz7{~|Yxf1e#xJ=DgL7YWt_FLb;qYOCPZ;mm{+)nU6gI-F0gf%ZDi z`Z&4qzd8pk4SY|~k!a=UnLC$B)Lo$sh>UZSy*)t&I%8@}H|qMVdD}ZhMWSu-=8?xd zZB-2{rK6Jqo*@0Qjs(F>dkCKKMt%wo_=!p^uE;q-dB1K_vi z<`OQS*JP=5^Nhq`D75K`j4HbRWdf>@v>w*%17hLS6%b$vHx6|v9Hm(C<@bfqjlf}3 zrWUEC47KJymsV&eVN+gGgGNd=eyhFwXtf{99M8xd1&IKS^fS$ z<0XPS2(c_&GJoQ@Hyr0l`A_p93;rN0&ZnNZj^Erk?_obZ!M0Q8bIqMTsHn275GEHJ z=wIMx^z%(*rqk^CM(dDWy_DxX!u2az!t7z6rCu8R;T%Qj$`+lldgMyz`b#Au*ftrc zb$m@oBqaWRRqJ%H5i62VZzA6;kf)_oeN_m8tu;oz%M<8ZLNLFQSH2m(PL@=2giS$& zfhh}p*CJfE-%OR=^?H2Ekp1ic6C`$jCol8f$IWp$mLoI7t0wOwH(I=zk{TbxdQ6zW zBuv?SvP@IY$K;H(Fd_z4e$t|tB?VNKz*@t0KBF`o4PU2meYa&X-+#m%x1MIuV~Sne zPR0M+IT{puuNxW$Mly8)GYCcttQgJR{b8IK!FTe}4g*G3{<&HQ3%hOD758zw_|3XD~l6S4XIgc|CH zs0K)4y{iN?OKCXPOU{IH8GC^vDQnZs?qot3Vk+1P^{_uT0;t<}|1*J6^&&@?(Vt~+ z`fnqUt^W~4;8FWbS6G&anDqW<=Y&QFA^r*X&+UCO{pl98*NZ6DFbhEju@(xk9EyJR zGA1_)gNPNZr@3=5ftBt&Gr(k^WJ{fMlDqBV9M1B_wK2^~WjXCQg@*q^k{%Sr*DQGT zE;rM{8hb^VN-y|~vZR~?>VOlE#v784SFJNt`2_&)PzGYEE_h*rFyzpQBguEm&&aw$ z&+l~fZ4Pa-`8S0sNLk&E6~y$9p9%sq4W@g*IH3S5xH77w(E>|UpUBI8J8B)j>SXe& z1Ftg?gYNtI`}NpcuKx)Ua>4XpX9p~k?UwcI4mM$iHDdmRUF{r%kSn&BFaas~o0z^-Yi3BK-tB$~1dq^I~s#tBfUMlt+uKxlU;FloCM*2>X2h58U#0 z=eMVngnP@3*hb*+5~1{dgUO}OiGQ*q5(LD$8MHnG1O)FFgfQAT6-~xWfkj05wDxil z=!WBvQfrXVQ{u$j{weGp4>x^=;jB)4l&vl`S2Y?GDKXh}3M~!8NcE!2Qyfl9(1CAMg{py0l zicr(f8Rp{+u@MJ;$!VJHGg?o<+DuF4NGObym zLM``ygMl!7)~Ir5b@TuNiz8JmIE>jL^cUS*&}@daZRP^?gMY~N1cItkth1#j1nzs> z3J>0Nm5-`5f%KfN?(S+8&Dd(q99m@L<|hEQiT_v9xd$@6|9|`w6{nJ~3Pq(txlYHT ztfE5~mP@$|yD6Ew5!R4fb+m8!(g~9hogL<~vE?#0_jSrnZpEy{#2l9}Ou1#w_-*I+ z`TzBNy-BiO|M4{jVUz)#n!P5aqMRt zWZHh|?%~(dP41_{`}6OiLLyxS#2KgGCC<*Z0$!2sgUzL*|^7%t$(!PFOP)aq_L=V{^M2dbB^^m zTYKyKTu^Gky-HOP`~0GIJE`Gm*VU_jJUD}Y8e z5H}Y`H+>~r6O3iCA1ir@26x%KaY}lm@NN2Gt=!X{G1EpLVkI2!->o+5INw0_+x#**{mgDd`(k3vz*;ofV);R?^hTh{sOTc$!8|2idkA3# zK~}}G;32mQD{c7y{P2l-qZhByJ$-nudB#ND?Se`#@?$>-~z+n&b;r2Tg0;-IqDunK(4%`kS%M zt~Rw);XS0IfUj^u-?dFUwa^lL`wl@S9`HfoL zC;5KXNjzI>jgt?bfCTxXjD@aK#AdoCObMi?%PP~v01I#K9+$6U;bTSE)YYbAQG7)= z9Z);O%1{YXUfZYj`sH}W&;qpP%!&=n7p9rrN&f@K3z?S+2$E^L{DzfYj($?g?S#a8 zj zJL6FDCeGvy)nhtD8P5-8QIP#+_Fm!WpS68M84$1)uF4A~uA~pd&2*f;7ng>*8vX9* z$hqgnrvlh80)YUru{l1hBv*_wl&>3z%Ov0K{lY)fb1%0yD9ps9kPpX(h0IU6u>&kiWNs6hcvm|3t~d8&<+3 zo(nlXEt5(bZ3co6K<>w3!5&jAfPv<*&=Dl#z)`gf={TnhNBQvNZ9 z!IAE-4r94FlxS1utTgJJ=k@QWQV2lAr4tB8}eeVibgGOKjoUkF*@cnYeN zsZO|D$^@zA@JU39BMbehL2O>$1_)zMw<)|dhbXK7>;tlxLn>h#NTB~U)NM3yj;FSf zavTV0rK;XjlbVQM74scP4JrZ$c6LuJ#Cl|eIkd~0Hwxiulp{{4RIS)*ah51sklbx| z`m@)Sbj>VY?opVur{$O4^z!OfMW*Nb0wU^1b2R|@%5#+n7aM=_4pvjXOL|<*a;_?X zXZPEe!gvQw4?X&~$rkrfsqX)>vRov8jVwyIxNKhx@mRS2o?3E~y+Rv6k8n5-a=Q%W zBk5u4?)K|l-5ItxeR~ZuU0gn*lo}QY#~ZWQ)u6Rf=P_g7^3JPXzs@2BcoNCd?1yx|N6r8F*9=5nJRc2pp7@pbI&(Ec?Zk( z)%!1H0-=fe`Z6ai2mp2jJ6-<86TLQRr=buM?S1(y`ndsGSqTI%>c(~GKlJGRc^|$a zDcb<=`mw^zTL?VKOy=u0eQm;$!iE$W_vhSHV#qHW5TpqizX=(!eFVr>eDwDsg&F`f z!N9wLW)pl_=x%_{Kf}s85u@*D=`?k+VV}PjF#z_4A*<*AQrq7|D@$OkSB&&0nJ4-& z1y2**9L{JSf^$6)kp&0|u{6W6%LQwj^s9ktOUpn*f3OA{=Uldy- zDmAMl<0%N5z3moZZZP{gHV3+!(j56#0v%6iGnd~$HY4+yc~s^%lint~o|O2VJQ}Cucq7z z8&Y6kc2Lf5-C6a?%_CN)7;)S7G_m~uEzKyagO9|GZuD#(^7XJXfDR2aiFoTMMi@zz zo1pa8j!q==16Rc^F3wMd0A@z~RmTXlda-;drj5~PmFQf(>V8%!`V-<7&UpAB51FTeEQvc z^3)t)P0iB{BnV#F@oZz_pb6OP14Z3VdZUZp7ai&62im(5FGD5SZbT%ZWVyZHH$s;X zG9R>Ii;f98n#M^sy7_+d*-75xZ!D=sgVdo%exSQux3+)o^OdfK80V6A{^ZGMW4!DC z(-rMT|dAF%gr=N|kX+>IpXq|z) zOWtWwGh712+<}G;!D<^v2CWlS^CQGdpO0*9ot8H!_~?E)3q;t%$_umSbmKW9)YtXE%)Wj@8fVdSLK-Y6m~7XCRd2_BX(oo?d5&p(mp)P%rqc=Ouve2er#B4 zmSV!ZiHo2b_V5m9Kq^C-p-cvREuVPQTwJ{`tgm}HnAvY;S{2HBaer-P>Ha7`1U6gK zpDjC;Z_zt&)Qkx^$A6z(Tui-*iJVD}DJ=~%JR9#d!{!^o(1Cj)37VYs;i-(fp%xF;O!^`;FJk9#l%|1>NK~eFm5u z?N#dbHeuH!Y7oC?l=sEv^6?lm zbK^PzO!|G=)Cq*4kZ=j#MPNSIyWuOOzyh;DU#%kc4&6!m*^ZYjBkrZDNJrj3N&cbI z#wbUcy2<`ta?^CvP18x*nD*(~lDf@Txo@~Y57kUh(5~(#z40g{m{CY1>kbX^P=>@i zL#(Mb_F0zTgD(t?xdOe}`4hf=;hyXV;?4_l7Im6i-Bf%^8i8nZ#Qb9}M^aPFg(A|O i-b+nC(K}-Yr73U0VNI{r8!svt;dI6IGTr{x!~X*T7q15Z literal 0 HcmV?d00001 diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp new file mode 100644 index 0000000000..ec04974ee5 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -0,0 +1,85 @@ +// Copyright (c) 2022 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_COLLISION_MONITOR__CIRCLE_HPP_ +#define NAV2_COLLISION_MONITOR__CIRCLE_HPP_ + +#include "nav2_collision_monitor/polygon.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Circle shape implementaiton. + * For STOP/SLOWDOWN model it represents zone around the robot + * while for APPROACH model it represents robot footprint. + */ +class Circle : public Polygon +{ +public: + /** + * @brief Circle class constructor + * @param node Collision Monitor node pointer + * @param polygon_name Name of circle + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID + * @param transform_tolerance Transform tolerance + */ + Circle( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & polygon_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance); + /** + * @brief Circle class destructor + */ + ~Circle(); + + /** + * @brief Gets polygon points, approximated to the circle. + * To be used in visualization purposes. + * @param poly Output polygon points (vertices) + */ + void getPolygon(std::vector & poly) const override; + + /** + * @brief Gets number of points inside circle + * @param points Input array of points to be checked + * @return Number of points inside circle. If there are no points, + * returns zero value. + */ + int getPointsInside(const std::vector & points) const override; + +protected: + /** + * @brief Supporting routine obtaining polygon-specific ROS-parameters + * @param polygon_pub_topic Output name of polygon publishing topic + * @param footprint_topic Output name of footprint topic. For Circle returns empty string, + * there is no footprint subscription in this class. + * @return True if all parameters were obtained or false in failure case + */ + bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) override; + + // ----- Variables ----- + + /// @brief Radius of the circle + double radius_; + /// @brief (radius * radius) value. Stored for optimization. + double radius_squared_; +}; // class Circle + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__CIRCLE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp new file mode 100644 index 0000000000..8d4836e980 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -0,0 +1,216 @@ +// Copyright (c) 2022 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_COLLISION_MONITOR__COLLISION_MONITOR_NODE_HPP_ +#define NAV2_COLLISION_MONITOR__COLLISION_MONITOR_NODE_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/twist.hpp" + +#include "tf2/time.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "nav2_util/lifecycle_node.hpp" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/polygon.hpp" +#include "nav2_collision_monitor/circle.hpp" +#include "nav2_collision_monitor/source.hpp" +#include "nav2_collision_monitor/scan.hpp" +#include "nav2_collision_monitor/pointcloud.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Collision Monitor ROS2 node + */ +class CollisionMonitor : public nav2_util::LifecycleNode +{ +public: + /** + * @brief Constructor for the nav2_collision_safery::CollisionMonitor + * @param options Additional options to control creation of the node. + */ + CollisionMonitor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** + * @brief Destructor for the nav2_collision_safery::CollisionMonitor + */ + ~CollisionMonitor(); + +protected: + /** + * @brief: Initializes and obtains ROS-parameters, creates main subscribers and publishers, + * creates polygons and data sources objects + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Activates LifecyclePublishers, polygons and main processor, creates bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Resets all subscribers/publishers, polygons/data sources arrays + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called in shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + +protected: + /** + * @brief Callback for input cmd_vel + * @param msg Input cmd_vel message + */ + void cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg); + /** + * @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds, + * quit to publish 0-velocity. + * @param robot_action Robot action to publish + */ + void publishVelocity(const Action & robot_action); + + /** + * @brief Supporting routine obtaining all ROS-parameters + * @param cmd_vel_in_topic Output name of cmd_vel_in topic + * @param cmd_vel_out_topic Output name of cmd_vel_out topic + * is required. + * @return True if all parameters were obtained or false in failure case + */ + bool getParameters( + std::string & cmd_vel_in_topic, + std::string & cmd_vel_out_topic); + /** + * @brief Supporting routine creating and configuring all polygons + * @param base_frame_id Robot base frame ID + * @param transform_tolerance Transform tolerance + * @return True if all polygons were configured successfully or false in failure case + */ + bool configurePolygons( + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance); + /** + * @brief Supporting routine creating and configuring all data sources + * @param base_frame_id Robot base frame ID + * @param odom_frame_id Odometry frame ID. Used as global frame to get + * source->base time inerpolated transform. + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + * @return True if all sources were configured successfully or false in failure case + */ + bool configureSources( + const std::string & base_frame_id, + const std::string & odom_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout); + + /** + * @brief Main processing routine + * @param cmd_vel_in Input desired robot velocity + */ + void process(const Velocity & cmd_vel_in); + + /** + * @brief Processes the polygon of STOP and SLOWDOWN action type + * @param polygon Polygon to process + * @param collision_points Array of 2D obstacle points + * @param velocity Desired robot velocity + * @param robot_action Output processed robot action + * @return True if returned action is caused by current polygon, otherwise false + */ + bool processStopSlowdown( + const std::shared_ptr polygon, + const std::vector & collision_points, + const Velocity & velocity, + Action & robot_action) const; + + /** + * @brief Processes APPROACH action type + * @param polygon Polygon to process + * @param collision_points Array of 2D obstacle points + * @param velocity Desired robot velocity + * @param robot_action Output processed robot action + * @return True if returned action is caused by current polygon, otherwise false + */ + bool processApproach( + const std::shared_ptr polygon, + const std::vector & collision_points, + const Velocity & velocity, + Action & robot_action) const; + + /** + * @brief Prints robot action and polygon caused it (if it was) + * @param robot_action Robot action to print + * @param action_polygon Pointer to a polygon causing a selected action + */ + void printAction( + const Action & robot_action, const std::shared_ptr action_polygon) const; + + /** + * @brief Polygons publishing routine. Made for visualization. + */ + void publishPolygons() const; + + // ----- Variables ----- + + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief TF listener + std::shared_ptr tf_listener_; + + /// @brief Polygons array + std::vector> polygons_; + + /// @brief Data sources array + std::vector> sources_; + + // Input/output speed controls + /// @beirf Input cmd_vel subscriber + rclcpp::Subscription::SharedPtr cmd_vel_in_sub_; + /// @brief Output cmd_vel publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr cmd_vel_out_pub_; + + /// @brief Whether main routine is active + bool process_active_; + + /// @brief Previous robot action + Action robot_action_prev_; + /// @brief Latest timestamp when robot has 0-velocity + rclcpp::Time stop_stamp_; + /// @brief Timeout after which 0-velocity ceases to be published + rclcpp::Duration stop_pub_timeout_; +}; // class CollisionMonitor + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__COLLISION_MONITOR_NODE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp new file mode 100644 index 0000000000..8dc418bd0e --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 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_COLLISION_MONITOR__KINEMATICS_HPP_ +#define NAV2_COLLISION_MONITOR__KINEMATICS_HPP_ + +#include + +#include "nav2_collision_monitor/types.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Do a transformation of points' coordinates from the frame coinciding with the (0,0) + * origin to the non-existing in ROS frame, which origin is equal to pose + * @param pose Origin of the new frame + * @param points Array of points whose coordinates will be transformed + */ +void transformPoints(const Pose & pose, std::vector & points); + +/** + * @brief Linearly projects pose towards to velocity direction on dt time interval. + * Turns the velocity on twist angle for dt time interval. + * @param dt Time step (in seconds). Should be relatively small + * to consider all movements to be linear. + * @param pose Pose to be projected + * @param velocity Velocity at which the pose to be moved. It is also being rotated + * on according twist angle. + */ +void projectState(const double & dt, Pose & pose, Velocity & velocity); + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__KINEMATICS_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp new file mode 100644 index 0000000000..8ccdd310be --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2022 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_COLLISION_MONITOR__POINTCLOUD_HPP_ +#define NAV2_COLLISION_MONITOR__POINTCLOUD_HPP_ + +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include "nav2_collision_monitor/source.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Implementation for pointcloud source + */ +class PointCloud : public Source +{ +public: + /** + * @brief PointCloud constructor + * @param node Collision Monitor node pointer + * @param polygon_name Name of data source + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. + * @param global_frame_id Global frame ID for correct transform calculation + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + */ + PointCloud( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout); + /** + * @brief PointCloud destructor + */ + ~PointCloud(); + + /** + * @brief Data source configuration routine. Obtains pointcloud related ROS-parameters + * and creates pointcloud subscriber. + */ + void configure(); + + /** + * @brief Adds latest data from pointcloud source to the data array. + * @param curr_time Current node time for data interpolation + * @param data Array where the data from source to be added. + * Added data is transformed to base_frame_id_ coordinate system at curr_time. + */ + void getData( + const rclcpp::Time & curr_time, + std::vector & data) const; + +protected: + /** + * @brief Getting sensor-specific ROS-parameters + * @param source_topic Output name of source subscription topic + */ + void getParameters(std::string & source_topic); + + /** + * @brief PointCloud data callback + * @param msg Shared pointer to PointCloud message + */ + void dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + + // ----- Variables ----- + + /// @brief PointCloud data subscriber + rclcpp::Subscription::SharedPtr data_sub_; + + // Minimum and maximum height of PointCloud projected to 2D space + double min_height_, max_height_; + + /// @brief Latest data obtained from pointcloud + sensor_msgs::msg::PointCloud2::ConstSharedPtr data_; +}; // class PointCloud + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__POINTCLOUD_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp new file mode 100644 index 0000000000..91a95a1251 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -0,0 +1,211 @@ +// Copyright (c) 2022 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_COLLISION_MONITOR__POLYGON_HPP_ +#define NAV2_COLLISION_MONITOR__POLYGON_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" +#include "geometry_msgs/msg/polygon.hpp" + +#include "tf2/time.h" +#include "tf2_ros/buffer.h" + +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/footprint_subscriber.hpp" + +#include "nav2_collision_monitor/types.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Basic polygon shape class. + * For STOP/SLOWDOWN model it represents zone around the robot + * while for APPROACH model it represents robot footprint. + */ +class Polygon +{ +public: + /** + * @brief Polygon constructor + * @param node Collision Monitor node pointer + * @param polygon_name Name of polygon + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID + * @param transform_tolerance Transform tolerance + */ + Polygon( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & polygon_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance); + /** + * @brief Polygon destructor + */ + virtual ~Polygon(); + + /** + * @brief Shape configuration routine. Obtains ROS-parameters related to shape object + * and creates polygon lifecycle publisher. + * @return True in case of everything is configured correctly, or false otherwise + */ + bool configure(); + /** + * @brief Activates polygon lifecycle publisher + */ + void activate(); + /** + * @brief Deactivates polygon lifecycle publisher + */ + void deactivate(); + + /** + * @brief Returns the name of polygon + * @return Polygon name + */ + std::string getName() const; + /** + * @brief Obtains polygon action type + * @return Action type for current polygon + */ + ActionType getActionType() const; + /** + * @brief Obtains polygon maximum points to enter inside polygon causing no action + * @return Maximum points to enter to current polygon and take no action + */ + int getMaxPoints() const; + /** + * @brief Obtains speed slowdown ratio for current polygon. + * Applicable for SLOWDOWN model. + * @return Speed slowdown ratio + */ + double getSlowdownRatio() const; + /** + * @brief Obtains required time before collision for current polygon. + * Applicable for APPROACH model. + * @return Time before collision in seconds + */ + double getTimeBeforeCollision() const; + + /** + * @brief Gets polygon points + * @param poly Output polygon points (vertices) + */ + virtual void getPolygon(std::vector & poly) const; + + /** + * @brief Updates polygon from footprint subscriber (if any) + */ + void updatePolygon(); + + /** + * @brief Gets number of points inside given polygon + * @param points Input array of points to be checked + * @return Number of points inside polygon. If there are no points, + * returns zero value. + */ + virtual int getPointsInside(const std::vector & points) const; + + /** + * @brief Obtains estimated (simulated) time before a collision. + * Applicable for APPROACH model. + * @param collision_points Array of 2D obstacle points + * @param velocity Simulated robot velocity + * @return Estimated time before a collision. If there is no collision, + * return value will be negative. + */ + double getCollisionTime( + const std::vector & collision_points, + const Velocity & velocity) const; + + /** + * @brief Publishes polygon message into a its own topic + */ + void publish() const; + +protected: + /** + * @brief Supporting routine obtaining ROS-parameters common for all shapes + * @param polygon_pub_topic Output name of polygon publishing topic + * @return True if all parameters were obtained or false in failure case + */ + bool getCommonParameters(std::string & polygon_pub_topic); + + /** + * @brief Supporting routine obtaining polygon-specific ROS-parameters + * @param polygon_pub_topic Output name of polygon publishing topic + * @param footprint_topic Output name of footprint topic. Empty, if no footprint subscription + * @return True if all parameters were obtained or false in failure case + */ + virtual bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic); + + /** + * @brief Checks if point is inside polygon + * @param point Given point to check + * @return True if given point is inside polygon, otherwise false + */ + bool isPointInside(const Point & point) const; + + // ----- Variables ----- + + /// @brief Collision Monitor node + nav2_util::LifecycleNode::WeakPtr node_; + /// @brief Collision monitor node logger stored for further usage + rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + + // Basic parameters + /// @brief Name of polygon + std::string polygon_name_; + /// @brief Action type for the polygon + ActionType action_type_; + /// @brief Maximum number of data readings within a zone to not trigger the action + int max_points_; + /// @brief Robot slowdown (share of its actual speed) + double slowdown_ratio_; + /// @brief Time before collision in seconds + double time_before_collision_; + /// @brief Time step for robot movement simulation + double simulation_time_step_; + /// @brief Footprint subscriber + std::unique_ptr footprint_sub_; + + // Global variables + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief Base frame ID + std::string base_frame_id_; + /// @brief Transform tolerance + tf2::Duration transform_tolerance_; + + // Visualization + /// @brief Whether to publish the polygon + bool visualize_; + /// @brief Polygon points stored for later publishing + geometry_msgs::msg::Polygon polygon_; + /// @brief Polygon publisher for visualization purposes + rclcpp_lifecycle::LifecyclePublisher::SharedPtr polygon_pub_; + + /// @brief Polygon points (vertices) + std::vector poly_; +}; // class Polygon + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__POLYGON_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp new file mode 100644 index 0000000000..f47f1bebfc --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -0,0 +1,88 @@ +// Copyright (c) 2022 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_COLLISION_MONITOR__SCAN_HPP_ +#define NAV2_COLLISION_MONITOR__SCAN_HPP_ + +#include "sensor_msgs/msg/laser_scan.hpp" + +#include "nav2_collision_monitor/source.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Implementation for laser scanner source + */ +class Scan : public Source +{ +public: + /** + * @brief Scan constructor + * @param node Collision Monitor node pointer + * @param polygon_name Name of data source + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. + * @param global_frame_id Global frame ID for correct transform calculation + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + */ + Scan( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout); + /** + * @brief Scan destructor + */ + ~Scan(); + + /** + * @brief Data source configuration routine. Obtains ROS-parameters + * and creates laser scanner subscriber. + */ + void configure(); + + /** + * @brief Adds latest data from laser scanner to the data array. + * @param curr_time Current node time for data interpolation + * @param data Array where the data from source to be added. + * Added data is transformed to base_frame_id_ coordinate system at curr_time. + */ + void getData( + const rclcpp::Time & curr_time, + std::vector & data) const; + +protected: + /** + * @brief Laser scanner data callback + * @param msg Shared pointer to LaserScan message + */ + void dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg); + + // ----- Variables ----- + + /// @brief Laser scanner data subscriber + rclcpp::Subscription::SharedPtr data_sub_; + + /// @brief Latest data obtained from laser scanner + sensor_msgs::msg::LaserScan::ConstSharedPtr data_; +}; // class Scan + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__SCAN_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp new file mode 100644 index 0000000000..32227e1df3 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2022 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_COLLISION_MONITOR__SOURCE_HPP_ +#define NAV2_COLLISION_MONITOR__SOURCE_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "tf2/time.h" +#include "tf2_ros/buffer.h" + +#include "nav2_util/lifecycle_node.hpp" + +#include "nav2_collision_monitor/types.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Basic data source class + */ +class Source +{ +public: + /** + * @brief Source constructor + * @param node Collision Monitor node pointer + * @param polygon_name Name of data source + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. + * @param global_frame_id Global frame ID for correct transform calculation + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + */ + Source( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout); + /** + * @brief Source destructor + */ + virtual ~Source(); + + /** + * @brief Adds latest data from source to the data array. + * Empty virtual method intended to be used in child implementations. + * @param curr_time Current node time for data interpolation + * @param data Array where the data from source to be added. + * Added data is transformed to base_frame_id_ coordinate system at curr_time. + */ + virtual void getData( + const rclcpp::Time & curr_time, + std::vector & data) const = 0; + +protected: + /** + * @brief Supporting routine obtaining ROS-parameters common for all data sources + * @param source_topic Output name of source subscription topic + */ + void getCommonParameters(std::string & source_topic); + + /** + * @brief Checks whether the source data might be considered as valid + * @param source_time Timestamp of latest obtained data + * @param curr_time Current node time for source verification + * @return True if data source is valid, otherwise false + */ + bool sourceValid( + const rclcpp::Time & source_time, + const rclcpp::Time & curr_time) const; + + /** + * @brief Obtains a transform from source_frame_id at source_time -> + * to base_frame_id_ at curr_time time + * @param source_frame_id Source frame ID to convert data from + * @param source_time Source timestamp to convert data from + * @param curr_time Current node time to interpolate data to + * @param tf_transform Output source->base transform + * @return True if got correct transform, otherwise false + */ + bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const rclcpp::Time & curr_time, + tf2::Transform & tf_transform) const; + + // ----- Variables ----- + + /// @brief Collision Monitor node + nav2_util::LifecycleNode::WeakPtr node_; + /// @brief Collision monitor node logger stored for further usage + rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + + // Basic parameters + /// @brief Name of data source + std::string source_name_; + + // Global variables + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief Robot base frame ID + std::string base_frame_id_; + /// @brief Global frame ID for correct transform calculation + std::string global_frame_id_; + /// @brief Transform tolerance + tf2::Duration transform_tolerance_; + /// @brief Maximum time interval in which data is considered valid + rclcpp::Duration source_timeout_; +}; // class Source + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__SOURCE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp new file mode 100644 index 0000000000..2f239d694b --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -0,0 +1,79 @@ +// Copyright (c) 2022 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_COLLISION_MONITOR__TYPES_HPP_ +#define NAV2_COLLISION_MONITOR__TYPES_HPP_ + +namespace nav2_collision_monitor +{ + +/// @brief Velocity for 2D model of motion +struct Velocity +{ + double x; // x-component of linear velocity + double y; // y-component of linear velocity + double tw; // z-component of angular twist + + inline bool operator<(const Velocity & second) const + { + const double first_vel = x * x + y * y; + const double second_vel = second.x * second.x + second.y * second.y; + return first_vel < second_vel; + } + + inline Velocity operator*(const double & mul) const + { + return {x * mul, y * mul, tw * mul}; + } + + inline bool isZero() const + { + return x == 0.0 && y == 0.0 && tw == 0.0; + } +}; + +/// @brief 2D point +struct Point +{ + double x; // x-coordinate of point + double y; // y-coordinate of point +}; + +/// @brief 2D Pose +struct Pose +{ + double x; // x-coordinate of pose + double y; // y-coordinate of pose + double theta; // rotation angle of pose +}; + +/// @brief Action type for robot +enum ActionType +{ + DO_NOTHING = 0, // No action + STOP = 1, // Stop the robot + SLOWDOWN = 2, // Slowdown in percentage from current operating speed + APPROACH = 3 // Keep constant time interval before collision +}; + +/// @brief Action for robot +struct Action +{ + ActionType action_type; + Velocity req_vel; +}; + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__TYPES_HPP_ diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py new file mode 100644 index 0000000000..7e96aeec2b --- /dev/null +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2022 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Environment + package_dir = get_package_share_directory('nav2_collision_monitor') + + # Constant parameters + lifecycle_nodes = ['collision_monitor'] + autostart = True + + # Launch arguments + # 1. Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + + # 2. Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) + + # Nodes launching commands + start_lifecycle_manager_cmd = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + + start_collision_monitor_cmd = Node( + package='nav2_collision_monitor', + executable='collision_monitor', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params]) + + ld = LaunchDescription() + + # Launch arguments + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + + # Node launching commands + ld.add_action(start_lifecycle_manager_cmd) + ld.add_action(start_collision_monitor_cmd) + + return ld diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml new file mode 100644 index 0000000000..49da184a53 --- /dev/null +++ b/nav2_collision_monitor/package.xml @@ -0,0 +1,32 @@ + + + + nav2_collision_monitor + 1.0.0 + Collision Monitor + Alexey Merzlyakov + Steve Macenski + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_components + tf2 + tf2_ros + tf2_geometry_msgs + sensor_msgs + geometry_msgs + nav2_common + nav2_util + nav2_costmap_2d + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + + + diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml new file mode 100644 index 0000000000..f0fb4ef5dd --- /dev/null +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -0,0 +1,48 @@ +collision_monitor: + ros__parameters: + use_sim_time: True + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_raw" + cmd_vel_out_topic: "cmd_vel" + transform_tolerance: 0.5 + source_timeout: 5.0 + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop" and "slowdown" action types, + # and robot footprint for "approach" action type. + # Footprint could be "polygon" type with dynamically set footprint from footprint_topic + # or "circle" type with static footprint set by radius. "footprint_topic" parameter + # to be ignored in circular case. + polygons: ["PolygonStop"] + PolygonStop: + type: "polygon" + points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + action_type: "stop" + max_points: 3 + visualize: True + polygon_pub_topic: "polygon_stop" + PolygonSlow: + type: "polygon" + points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] + action_type: "slowdown" + max_points: 3 + slowdown_ratio: 0.3 + visualize: True + polygon_pub_topic: "polygon_slowdown" + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + max_points: 5 + visualize: False + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/scan" + pointcloud: + type: "pointcloud" + topic: "/intel_realsense_r200_depth/points" + min_height: 0.1 + max_height: 0.5 diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp new file mode 100644 index 0000000000..6a05477f49 --- /dev/null +++ b/nav2_collision_monitor/src/circle.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2022 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 "nav2_collision_monitor/circle.hpp" + +#include +#include +#include + +#include "nav2_util/node_utils.hpp" + +namespace nav2_collision_monitor +{ + +Circle::Circle( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & polygon_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance) +: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) +{ + RCLCPP_INFO(logger_, "[%s]: Creating Circle", polygon_name_.c_str()); +} + +Circle::~Circle() +{ + RCLCPP_INFO(logger_, "[%s]: Destroying Circle", polygon_name_.c_str()); +} + +void Circle::getPolygon(std::vector & poly) const +{ + // Number of polygon points. More edges means better approximation. + const double polygon_edges = 16; + // Increment of angle during points position calculation + double angle_increment = 2 * M_PI / polygon_edges; + + // Clear polygon before filling + poly.clear(); + + // Making new polygon looks like a circle + Point p; + for (double angle = 0.0; angle < 2 * M_PI; angle += angle_increment) { + p.x = radius_ * std::cos(angle); + p.y = radius_ * std::sin(angle); + poly.push_back(p); + } +} + +int Circle::getPointsInside(const std::vector & points) const +{ + int num = 0; + for (Point point : points) { + if (point.x * point.x + point.y * point.y < radius_squared_) { + num++; + } + } + + return num; +} + +bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!getCommonParameters(polygon_pub_topic)) { + return false; + } + + // There is no footprint subscription for the Circle. Thus, set string as empty. + footprint_topic.clear(); + + try { + // Leave it not initialized: the will cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE); + radius_ = node->get_parameter(polygon_name_ + ".radius").as_double(); + radius_squared_ = radius_ * radius_; + } catch (const std::exception & ex) { + RCLCPP_ERROR( + logger_, + "[%s]: Error while getting circle parameters: %s", + polygon_name_.c_str(), ex.what()); + return false; + } + + return true; +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp new file mode 100644 index 0000000000..46f9989f32 --- /dev/null +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -0,0 +1,473 @@ +// Copyright (c) 2022 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 "nav2_collision_monitor/collision_monitor_node.hpp" + +#include +#include +#include + +#include "tf2_ros/create_timer_ros.h" + +#include "nav2_util/node_utils.hpp" + +#include "nav2_collision_monitor/kinematics.hpp" + +namespace nav2_collision_monitor +{ + +CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("collision_monitor", "", false, options), + process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}}, + stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0) +{ +} + +CollisionMonitor::~CollisionMonitor() +{ + polygons_.clear(); + sources_.clear(); +} + +nav2_util::CallbackReturn +CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + std::string cmd_vel_in_topic; + std::string cmd_vel_out_topic; + + // Obtaining ROS parameters + if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) { + return nav2_util::CallbackReturn::FAILURE; + } + + cmd_vel_in_sub_ = this->create_subscription( + cmd_vel_in_topic, 1, + std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1)); + cmd_vel_out_pub_ = this->create_publisher( + cmd_vel_out_topic, 1); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + // Activating lifecycle publisher + cmd_vel_out_pub_->on_activate(); + + // Activating polygons + for (std::shared_ptr polygon : polygons_) { + polygon->activate(); + } + + // Since polygons are being published when cmd_vel_in appears, + // we need to publish polygons first time to display them at startup + publishPolygons(); + + // Activating main worker + process_active_ = true; + + // Creating bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + // Deactivating main worker + process_active_ = false; + + // Reset action type to default after worker deactivating + robot_action_prev_ = {DO_NOTHING, {-1.0, -1.0, -1.0}}; + + // Deactivating polygons + for (std::shared_ptr polygon : polygons_) { + polygon->deactivate(); + } + + // Deactivating lifecycle publishers + cmd_vel_out_pub_->on_deactivate(); + + // Destroying bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + cmd_vel_in_sub_.reset(); + cmd_vel_out_pub_.reset(); + + polygons_.clear(); + sources_.clear(); + + tf_listener_.reset(); + tf_buffer_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + + return nav2_util::CallbackReturn::SUCCESS; +} + +void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg) +{ + process({msg->linear.x, msg->linear.y, msg->angular.z}); +} + +void CollisionMonitor::publishVelocity(const Action & robot_action) +{ + if (robot_action.req_vel.isZero()) { + if (!robot_action_prev_.req_vel.isZero()) { + // Robot just stopped: saving stop timestamp and continue + stop_stamp_ = this->now(); + } else if (this->now() - stop_stamp_ > stop_pub_timeout_) { + // More than stop_pub_timeout_ passed after robot has been stopped. + // Cease publishing output cmd_vel. + return; + } + } + + std::unique_ptr cmd_vel_out_msg = + std::make_unique(); + cmd_vel_out_msg->linear.x = robot_action.req_vel.x; + cmd_vel_out_msg->linear.y = robot_action.req_vel.y; + cmd_vel_out_msg->angular.z = robot_action.req_vel.tw; + // linear.z, angular.x and angular.y will remain 0.0 + + cmd_vel_out_pub_->publish(std::move(cmd_vel_out_msg)); +} + +bool CollisionMonitor::getParameters( + std::string & cmd_vel_in_topic, + std::string & cmd_vel_out_topic) +{ + std::string base_frame_id, odom_frame_id; + tf2::Duration transform_tolerance; + rclcpp::Duration source_timeout(2.0, 0.0); + + auto node = shared_from_this(); + + nav2_util::declare_parameter_if_not_declared( + node, "cmd_vel_in_topic", rclcpp::ParameterValue("cmd_vel_raw")); + cmd_vel_in_topic = get_parameter("cmd_vel_in_topic").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel")); + cmd_vel_out_topic = get_parameter("cmd_vel_out_topic").as_string(); + + nav2_util::declare_parameter_if_not_declared( + node, "base_frame_id", rclcpp::ParameterValue("base_footprint")); + base_frame_id = get_parameter("base_frame_id").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "odom_frame_id", rclcpp::ParameterValue("odom")); + odom_frame_id = get_parameter("odom_frame_id").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + transform_tolerance = + tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "source_timeout", rclcpp::ParameterValue(2.0)); + source_timeout = + rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double()); + + nav2_util::declare_parameter_if_not_declared( + node, "stop_pub_timeout", rclcpp::ParameterValue(1.0)); + stop_pub_timeout_ = + rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double()); + + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + + if (!configureSources(base_frame_id, odom_frame_id, transform_tolerance, source_timeout)) { + return false; + } + + return true; +} + +bool CollisionMonitor::configurePolygons( + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance) +{ + try { + auto node = shared_from_this(); + + nav2_util::declare_parameter_if_not_declared( + node, "polygons", rclcpp::ParameterValue(std::vector())); + std::vector polygon_names = get_parameter("polygons").as_string_array(); + for (std::string polygon_name : polygon_names) { + // Leave it not initialized: the will cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, polygon_name + ".type", rclcpp::PARAMETER_STRING); + const std::string polygon_type = get_parameter(polygon_name + ".type").as_string(); + + if (polygon_type == "polygon") { + polygons_.push_back( + std::make_shared( + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + } else if (polygon_type == "circle") { + polygons_.push_back( + std::make_shared( + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + } else { // Error if something else + RCLCPP_ERROR( + get_logger(), + "[%s]: Unknown polygon type: %s", + polygon_name.c_str(), polygon_type.c_str()); + return false; + } + + // Configure last added polygon + if (!polygons_.back()->configure()) { + return false; + } + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); + return false; + } + + return true; +} + +bool CollisionMonitor::configureSources( + const std::string & base_frame_id, + const std::string & odom_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout) +{ + try { + auto node = shared_from_this(); + + // Leave it to be not initialized: to intentionally cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); + std::vector source_names = get_parameter("observation_sources").as_string_array(); + for (std::string source_name : source_names) { + nav2_util::declare_parameter_if_not_declared( + node, source_name + ".type", + rclcpp::ParameterValue("scan")); // Laser scanner by default + const std::string source_type = get_parameter(source_name + ".type").as_string(); + + if (source_type == "scan") { + std::shared_ptr s = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout); + + s->configure(); + + sources_.push_back(s); + } else if (source_type == "pointcloud") { + std::shared_ptr p = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout); + + p->configure(); + + sources_.push_back(p); + } else { // Error if something else + RCLCPP_ERROR( + get_logger(), + "[%s]: Unknown source type: %s", + source_name.c_str(), source_type.c_str()); + return false; + } + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); + return false; + } + + return true; +} + +void CollisionMonitor::process(const Velocity & cmd_vel_in) +{ + // Current timestamp for all inner routines prolongation + rclcpp::Time curr_time = this->now(); + + // Do nothing if main worker in non-active state + if (!process_active_) { + return; + } + + // Points array collected from different data sources in a robot base frame + std::vector collision_points; + + // Fill collision_points array from different data sources + for (std::shared_ptr source : sources_) { + source->getData(curr_time, collision_points); + } + + // By default - there is no action + Action robot_action{DO_NOTHING, cmd_vel_in}; + // Polygon causing robot action (if any) + std::shared_ptr action_polygon; + + for (std::shared_ptr polygon : polygons_) { + if (robot_action.action_type == STOP) { + // If robot already should stop, do nothing + break; + } + + const ActionType at = polygon->getActionType(); + if (at == STOP || at == SLOWDOWN) { + // Process STOP/SLOWDOWN for the selected polygon + if (processStopSlowdown(polygon, collision_points, cmd_vel_in, robot_action)) { + action_polygon = polygon; + } + } else if (at == APPROACH) { + // Process APPROACH for the selected polygon + if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) { + action_polygon = polygon; + } + } + } + + if (robot_action.action_type != robot_action_prev_.action_type) { + // Report changed robot behavior + printAction(robot_action, action_polygon); + } + + // Publish requred robot velocity + publishVelocity(robot_action); + + // Publish polygons for better visualization + publishPolygons(); + + robot_action_prev_ = robot_action; +} + +bool CollisionMonitor::processStopSlowdown( + const std::shared_ptr polygon, + const std::vector & collision_points, + const Velocity & velocity, + Action & robot_action) const +{ + if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) { + if (polygon->getActionType() == STOP) { + // Setting up zero velocity for STOP model + robot_action.action_type = STOP; + robot_action.req_vel.x = 0.0; + robot_action.req_vel.y = 0.0; + robot_action.req_vel.tw = 0.0; + return true; + } else { // SLOWDOWN + const Velocity safe_vel = velocity * polygon->getSlowdownRatio(); + // Check that currently calculated velocity is safer than + // chosen for previous shapes one + if (safe_vel < robot_action.req_vel) { + robot_action.action_type = SLOWDOWN; + robot_action.req_vel = safe_vel; + return true; + } + } + } + + return false; +} + +bool CollisionMonitor::processApproach( + const std::shared_ptr polygon, + const std::vector & collision_points, + const Velocity & velocity, + Action & robot_action) const +{ + polygon->updatePolygon(); + + // Obtain time before a collision + const double collision_time = polygon->getCollisionTime(collision_points, velocity); + if (collision_time >= 0.0) { + // If collision will occurr, reduce robot speed + const double change_ratio = collision_time / polygon->getTimeBeforeCollision(); + const Velocity safe_vel = velocity * change_ratio; + // Check that currently calculated velocity is safer than + // chosen for previous shapes one + if (safe_vel < robot_action.req_vel) { + robot_action.action_type = APPROACH; + robot_action.req_vel = safe_vel; + return true; + } + } + + return false; +} + +void CollisionMonitor::printAction( + const Action & robot_action, const std::shared_ptr action_polygon) const +{ + if (robot_action.action_type == STOP) { + RCLCPP_INFO( + get_logger(), + "Robot to stop due to %s polygon", + action_polygon->getName().c_str()); + } else if (robot_action.action_type == SLOWDOWN) { + RCLCPP_INFO( + get_logger(), + "Robot to slowdown for %f percents due to %s polygon", + action_polygon->getSlowdownRatio() * 100, + action_polygon->getName().c_str()); + } else if (robot_action.action_type == APPROACH) { + RCLCPP_INFO( + get_logger(), + "Robot to approach for %f seconds away from collision", + action_polygon->getTimeBeforeCollision()); + } else { // robot_action.action_type == DO_NOTHING + RCLCPP_INFO( + get_logger(), + "Robot to continue normal operation"); + } +} + +void CollisionMonitor::publishPolygons() const +{ + for (std::shared_ptr polygon : polygons_) { + polygon->publish(); + } +} + +} // namespace nav2_collision_monitor + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_collision_monitor::CollisionMonitor) diff --git a/nav2_collision_monitor/src/kinematics.cpp b/nav2_collision_monitor/src/kinematics.cpp new file mode 100644 index 0000000000..1b6ee226b7 --- /dev/null +++ b/nav2_collision_monitor/src/kinematics.cpp @@ -0,0 +1,70 @@ +// Copyright (c) 2022 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 "nav2_collision_monitor/kinematics.hpp" + +#include + +namespace nav2_collision_monitor +{ + +void transformPoints(const Pose & pose, std::vector & points) +{ + const double cos_theta = std::cos(pose.theta); + const double sin_theta = std::sin(pose.theta); + + for (Point & point : points) { + // p = R*p' + pose + // p' = Rt * (p - pose) + // where: + // p - point coordinates in initial frame + // p' - point coordinates in a new frame + // R - rotation matrix = + // [cos_theta -sin_theta] + // [sin_theta cos_theta] + // Rt - transposed (inverted) rotation matrix + const double mul_x = point.x - pose.x; + const double mul_y = point.y - pose.y; + point.x = mul_x * cos_theta + mul_y * sin_theta; + point.y = -mul_x * sin_theta + mul_y * cos_theta; + } +} + +void projectState(const double & dt, Pose & pose, Velocity & velocity) +{ + const double theta = velocity.tw * dt; + const double cos_theta = std::cos(theta); + const double sin_theta = std::sin(theta); + + // p' = p + vel*dt + // where: + // p - initial pose + // p' - projected pose + pose.x = pose.x + velocity.x * dt; + pose.y = pose.y + velocity.y * dt; + // Rotate the pose on theta + pose.theta = pose.theta + theta; + + // vel' = R*vel + // where: + // vel - initial velocity + // R - rotation matrix + // vel' - rotated velocity + const double velocity_upd_x = velocity.x * cos_theta - velocity.y * sin_theta; + const double velocity_upd_y = velocity.x * sin_theta + velocity.y * cos_theta; + velocity.x = velocity_upd_x; + velocity.y = velocity_upd_y; +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/main.cpp b/nav2_collision_monitor/src/main.cpp new file mode 100644 index 0000000000..01ae87e353 --- /dev/null +++ b/nav2_collision_monitor/src/main.cpp @@ -0,0 +1,29 @@ +// Copyright (c) 2022 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 "rclcpp/rclcpp.hpp" + +#include "nav2_collision_monitor/collision_monitor_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp new file mode 100644 index 0000000000..eb25c91114 --- /dev/null +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -0,0 +1,124 @@ +// Copyright (c) 2022 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 "nav2_collision_monitor/pointcloud.hpp" + +#include + +#include "sensor_msgs/point_cloud2_iterator.hpp" + +#include "nav2_util/node_utils.hpp" + +namespace nav2_collision_monitor +{ + +PointCloud::PointCloud( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout) +: Source( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, source_timeout), + data_(nullptr) +{ + RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str()); +} + +PointCloud::~PointCloud() +{ + RCLCPP_INFO(logger_, "[%s]: Destroying PointCloud", source_name_.c_str()); + data_sub_.reset(); +} + +void PointCloud::configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + std::string source_topic; + + getParameters(source_topic); + + rclcpp::QoS pointcloud_qos = rclcpp::SensorDataQoS(); // set to default + data_sub_ = node->create_subscription( + source_topic, pointcloud_qos, + std::bind(&PointCloud::dataCallback, this, std::placeholders::_1)); +} + +void PointCloud::getData( + const rclcpp::Time & curr_time, + std::vector & data) const +{ + // Ignore data from the source if it is not being published yet or + // not published for a long time + if (data_ == nullptr) { + return; + } + if (!sourceValid(data_->header.stamp, curr_time)) { + return; + } + + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + tf2::Transform tf_transform; + if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + return; + } + + sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*data_, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*data_, "z"); + + // Refill data array with PointCloud points in base frame + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + // Transform point coordinates from source frame -> to base frame + tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Refill data array + if (p_v3_b.z() >= min_height_ && p_v3_b.z() <= max_height_) { + data.push_back({p_v3_b.x(), p_v3_b.y()}); + } + } +} + +void PointCloud::getParameters(std::string & source_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + getCommonParameters(source_topic); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".min_height", rclcpp::ParameterValue(0.05)); + min_height_ = node->get_parameter(source_name_ + ".min_height").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".max_height", rclcpp::ParameterValue(0.5)); + max_height_ = node->get_parameter(source_name_ + ".max_height").as_double(); +} + +void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + data_ = msg; +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp new file mode 100644 index 0000000000..0e1c47b59c --- /dev/null +++ b/nav2_collision_monitor/src/polygon.cpp @@ -0,0 +1,380 @@ +// Copyright (c) 2022 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 "nav2_collision_monitor/polygon.hpp" + +#include +#include + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/point32.hpp" + +#include "nav2_util/node_utils.hpp" + +#include "nav2_collision_monitor/kinematics.hpp" + +namespace nav2_collision_monitor +{ + +Polygon::Polygon( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & polygon_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance) +: node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING), + slowdown_ratio_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer), + base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance) +{ + RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str()); +} + +Polygon::~Polygon() +{ + RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str()); + poly_.clear(); +} + +bool Polygon::configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + std::string polygon_pub_topic, footprint_topic; + + if (!getParameters(polygon_pub_topic, footprint_topic)) { + return false; + } + + if (!footprint_topic.empty()) { + footprint_sub_ = std::make_unique( + node, footprint_topic, *tf_buffer_, + base_frame_id_, tf2::durationToSec(transform_tolerance_)); + } + + if (visualize_) { + // Fill polygon_ points for future usage + std::vector poly; + getPolygon(poly); + for (const Point & p : poly) { + geometry_msgs::msg::Point32 p_s; + p_s.x = p.x; + p_s.y = p.y; + // p_s.z will remain 0.0 + polygon_.points.push_back(p_s); + } + + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + polygon_pub_ = node->create_publisher( + polygon_pub_topic, polygon_qos); + } + + return true; +} + +void Polygon::activate() +{ + if (visualize_) { + polygon_pub_->on_activate(); + } +} + +void Polygon::deactivate() +{ + if (visualize_) { + polygon_pub_->on_deactivate(); + } +} + +std::string Polygon::getName() const +{ + return polygon_name_; +} + +ActionType Polygon::getActionType() const +{ + return action_type_; +} + +int Polygon::getMaxPoints() const +{ + return max_points_; +} + +double Polygon::getSlowdownRatio() const +{ + return slowdown_ratio_; +} + +double Polygon::getTimeBeforeCollision() const +{ + return time_before_collision_; +} + +void Polygon::getPolygon(std::vector & poly) const +{ + poly = poly_; +} + +void Polygon::updatePolygon() +{ + if (footprint_sub_ != nullptr) { + // Get latest robot footprint from footprint subscriber + std::vector footprint_vec; + std_msgs::msg::Header footprint_header; + footprint_sub_->getFootprintInRobotFrame(footprint_vec, footprint_header); + + std::size_t new_size = footprint_vec.size(); + poly_.resize(new_size); + polygon_.points.resize(new_size); + + geometry_msgs::msg::Point32 p_s; + for (std::size_t i = 0; i < new_size; i++) { + poly_[i] = {footprint_vec[i].x, footprint_vec[i].y}; + p_s.x = footprint_vec[i].x; + p_s.y = footprint_vec[i].y; + polygon_.points[i] = p_s; + } + } +} + +int Polygon::getPointsInside(const std::vector & points) const +{ + int num = 0; + for (const Point & point : points) { + if (isPointInside(point)) { + num++; + } + } + return num; +} + +double Polygon::getCollisionTime( + const std::vector & collision_points, + const Velocity & velocity) const +{ + // Initial robot pose is {0,0} in base_footprint coordinates + Pose pose = {0.0, 0.0, 0.0}; + Velocity vel = velocity; + + // Array of points transformed to the frame concerned with pose on each simulation step + std::vector points_transformed; + + // Robot movement simulation + for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) { + // Shift the robot pose towards to the vel during simulation_time_step_ time interval + // NOTE: vel is changing during the simulation + projectState(simulation_time_step_, pose, vel); + // Transform collision_points to the frame concerned with current robot pose + points_transformed = collision_points; + transformPoints(pose, points_transformed); + // If the collision occurred on this stage, return the actual time before a collision + // as if robot was moved with given velocity + if (getPointsInside(points_transformed) > max_points_) { + return time; + } + } + + // There is no collision + return -1.0; +} + +void Polygon::publish() const +{ + if (!visualize_) { + return; + } + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Fill PolygonStamped struct + std::unique_ptr poly_s = + std::make_unique(); + poly_s->header.stamp = node->now(); + poly_s->header.frame_id = base_frame_id_; + poly_s->polygon = polygon_; + + // Publish polygon + polygon_pub_->publish(std::move(poly_s)); +} + +bool Polygon::getCommonParameters(std::string & polygon_pub_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + try { + // Get action type. + // Leave it not initialized: the will cause an error if it will not set. + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".action_type", rclcpp::PARAMETER_STRING); + const std::string at_str = + node->get_parameter(polygon_name_ + ".action_type").as_string(); + if (at_str == "stop") { + action_type_ = STOP; + } else if (at_str == "slowdown") { + action_type_ = SLOWDOWN; + } else if (at_str == "approach") { + action_type_ = APPROACH; + } else { // Error if something else + RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str()); + return false; + } + + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".max_points", rclcpp::ParameterValue(3)); + max_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int(); + + if (action_type_ == SLOWDOWN) { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".slowdown_ratio", rclcpp::ParameterValue(0.5)); + slowdown_ratio_ = node->get_parameter(polygon_name_ + ".slowdown_ratio").as_double(); + } + + if (action_type_ == APPROACH) { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".time_before_collision", rclcpp::ParameterValue(2.0)); + time_before_collision_ = + node->get_parameter(polygon_name_ + ".time_before_collision").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".simulation_time_step", rclcpp::ParameterValue(0.1)); + simulation_time_step_ = + node->get_parameter(polygon_name_ + ".simulation_time_step").as_double(); + } + + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".visualize", rclcpp::ParameterValue(false)); + visualize_ = node->get_parameter(polygon_name_ + ".visualize").as_bool(); + if (visualize_) { + // Get polygon topic parameter in case if it is going to be published + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name_)); + polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string(); + } + } catch (const std::exception & ex) { + RCLCPP_ERROR( + logger_, + "[%s]: Error while getting common polygon parameters: %s", + polygon_name_.c_str(), ex.what()); + return false; + } + + return true; +} + +bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!getCommonParameters(polygon_pub_topic)) { + return false; + } + + try { + if (action_type_ == APPROACH) { + // Obtain the footprint topic to make a footprint subscription for approach polygon + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".footprint_topic", + rclcpp::ParameterValue("local_costmap/published_footprint")); + footprint_topic = + node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); + + // This is robot footprint: do not need to get polygon points from ROS parameters. + // It will be set dynamically later. + return true; + } else { + // Make it empty otherwise + footprint_topic.clear(); + } + + // Leave it not initialized: the will cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + std::vector poly_row = + node->get_parameter(polygon_name_ + ".points").as_double_array(); + // Check for points format correctness + if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { + RCLCPP_ERROR( + logger_, + "[%s]: Polygon has incorrect points description", + polygon_name_.c_str()); + return false; + } + + // Obtain polygon vertices + Point point; + bool first = true; + for (double val : poly_row) { + if (first) { + point.x = val; + } else { + point.y = val; + poly_.push_back(point); + } + first = !first; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR( + logger_, + "[%s]: Error while getting polygon parameters: %s", + polygon_name_.c_str(), ex.what()); + return false; + } + + return true; +} + +inline bool Polygon::isPointInside(const Point & point) const +{ + // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." + // Communications of the ACM 5.8 (1962): 434. + // Implementation of ray crossings algorithm for point in polygon task solving. + // Y coordinate is fixed. Moving the ray on X+ axis starting from given point. + // Odd number of intersections with polygon boundaries means the point is inside polygon. + const int poly_size = poly_.size(); + int i, j; // Polygon vertex iterators + bool res = false; // Final result, initialized with already inverted value + + // Starting from the edge where the last point of polygon is connected to the first + i = poly_size - 1; + for (j = 0; j < poly_size; j++) { + // Checking the edge only if given point is between edge boundaries by Y coordinates. + // One of the condition should contain equality in order to exclude the edges + // parallel to X+ ray. + if ((point.y <= poly_[i].y) == (point.y > poly_[j].y)) { + // Calculating the intersection coordinate of X+ ray + const double x_inter = poly_[i].x + + (point.y - poly_[i].y) * (poly_[j].x - poly_[i].x) / + (poly_[j].y - poly_[i].y); + // If intersection with checked edge is greater than point.x coordinate, inverting the result + if (x_inter > point.x) { + res = !res; + } + } + i = j; + } + return res; +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp new file mode 100644 index 0000000000..a73d9e53cf --- /dev/null +++ b/nav2_collision_monitor/src/scan.cpp @@ -0,0 +1,106 @@ +// Copyright (c) 2022 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 "nav2_collision_monitor/scan.hpp" + +#include +#include + +namespace nav2_collision_monitor +{ + +Scan::Scan( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout) +: Source( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, source_timeout), + data_(nullptr) +{ + RCLCPP_INFO(logger_, "[%s]: Creating Scan", source_name_.c_str()); +} + +Scan::~Scan() +{ + RCLCPP_INFO(logger_, "[%s]: Destroying Scan", source_name_.c_str()); + data_sub_.reset(); +} + +void Scan::configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + std::string source_topic; + + // Laser scanner has no own parameters + getCommonParameters(source_topic); + + rclcpp::QoS scan_qos = rclcpp::SensorDataQoS(); // set to default + data_sub_ = node->create_subscription( + source_topic, scan_qos, + std::bind(&Scan::dataCallback, this, std::placeholders::_1)); +} + +void Scan::getData( + const rclcpp::Time & curr_time, + std::vector & data) const +{ + // Ignore data from the source if it is not being published yet or + // not being published for a long time + if (data_ == nullptr) { + return; + } + if (!sourceValid(data_->header.stamp, curr_time)) { + return; + } + + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + tf2::Transform tf_transform; + if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + return; + } + + // Calculate poses and refill data array + float angle = data_->angle_min; + for (size_t i = 0; i < data_->ranges.size(); i++) { + if (data_->ranges[i] >= data_->range_min && data_->ranges[i] <= data_->range_max) { + // Transform point coordinates from source frame -> to base frame + tf2::Vector3 p_v3_s( + data_->ranges[i] * std::cos(angle), + data_->ranges[i] * std::sin(angle), + 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Refill data array + data.push_back({p_v3_b.x(), p_v3_b.y()}); + } + angle += data_->angle_increment; + } +} + +void Scan::dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg) +{ + data_ = msg; +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp new file mode 100644 index 0000000000..e945301640 --- /dev/null +++ b/nav2_collision_monitor/src/source.cpp @@ -0,0 +1,108 @@ +// Copyright (c) 2022 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 "nav2_collision_monitor/source.hpp" + +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" + +#include "tf2/convert.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "nav2_util/node_utils.hpp" + +namespace nav2_collision_monitor +{ + +Source::Source( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout) +: node_(node), source_name_(source_name), tf_buffer_(tf_buffer), + base_frame_id_(base_frame_id), global_frame_id_(global_frame_id), + transform_tolerance_(transform_tolerance), source_timeout_(source_timeout) +{ +} + +Source::~Source() +{ +} + +void Source::getCommonParameters(std::string & source_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".topic", + rclcpp::ParameterValue("scan")); // Set deafult topic for laser scanner + source_topic = node->get_parameter(source_name_ + ".topic").as_string(); +} + +bool Source::sourceValid( + const rclcpp::Time & source_time, + const rclcpp::Time & curr_time) const +{ + // Source is considered as not valid, if latest received data timestamp is earlier + // than current time by source_timeout_ interval + const rclcpp::Duration dt = curr_time - source_time; + if (dt > source_timeout_) { + RCLCPP_WARN( + logger_, + "[%s]: Latest source and current collision monitor node timestamps differ on %f seconds. " + "Ignoring the source.", + source_name_.c_str(), dt.seconds()); + return false; + } + + return true; +} + +bool Source::getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const rclcpp::Time & curr_time, + tf2::Transform & tf2_transform) const +{ + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + + try { + // Obtaining the transform to get data from source to base frame. + // This also considers the time shift between source and base. + transform = tf_buffer_->lookupTransform( + base_frame_id_, curr_time, + source_frame_id, source_time, + global_frame_id_, transform_tolerance_); + } catch (tf2::TransformException & e) { + RCLCPP_ERROR( + logger_, + "[%s]: Failed to get \"%s\"->\"%s\" frame transform: %s", + source_name_.c_str(), source_frame_id.c_str(), base_frame_id_.c_str(), e.what()); + return false; + } + + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt new file mode 100644 index 0000000000..77870826b8 --- /dev/null +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -0,0 +1,35 @@ +# Kinematics test +ament_add_gtest(kinematics_test kinematics_test.cpp) +ament_target_dependencies(kinematics_test + ${dependencies} +) +target_link_libraries(kinematics_test + ${library_name} +) + +# Data sources test +ament_add_gtest(sources_test sources_test.cpp) +ament_target_dependencies(sources_test + ${dependencies} +) +target_link_libraries(sources_test + ${library_name} +) + +# Polygon shapes test +ament_add_gtest(polygons_test polygons_test.cpp) +ament_target_dependencies(polygons_test + ${dependencies} +) +target_link_libraries(polygons_test + ${library_name} +) + +# Collision Monitor node test +ament_add_gtest(collision_monitor_node_test collision_monitor_node_test.cpp) +ament_target_dependencies(collision_monitor_node_test + ${dependencies} +) +target_link_libraries(collision_monitor_node_test + ${library_name} +) diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp new file mode 100644 index 0000000000..094a3f1e91 --- /dev/null +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -0,0 +1,777 @@ +// Copyright (c) 2022 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "geometry_msgs/msg/twist.hpp" + +#include "tf2_ros/transform_broadcaster.h" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/collision_monitor_node.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char BASE_FRAME_ID[]{"base_link"}; +static const char SOURCE_FRAME_ID[]{"base_source"}; +static const char ODOM_FRAME_ID[]{"odom"}; +static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; +static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; +static const char SCAN_NAME[]{"Scan"}; +static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const int MAX_POINTS{1}; +static const double SLOWDOWN_RATIO{0.7}; +static const double TIME_BEFORE_COLLISION{1.0}; +static const double SIMULATION_TIME_STEP{0.01}; +static const double TRANSFORM_TOLERANCE{0.5}; +static const double SOURCE_TIMEOUT{5.0}; +static const double STOP_PUB_TIMEOUT{0.1}; + +enum PolygonType +{ + POLYGON_UNKNOWN = 0, + POLYGON = 1, + CIRCLE = 2 +}; + +enum SourceType +{ + SOURCE_UNKNOWN = 0, + SCAN = 1, + POINTCLOUD = 2 +}; + +class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor +{ +public: + void start() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_activate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void stop() + { + ASSERT_EQ(on_deactivate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_cleanup(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void configure() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void cant_configure() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::FAILURE); + } + + bool isDataReceived(const rclcpp::Time & stamp) + { + for (std::shared_ptr source : sources_) { + std::vector collision_points; + source->getData(stamp, collision_points); + if (collision_points.size() == 0) { + return false; + } + } + return true; + } +}; // CollisionMonitorWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + + // Configuring + void setCommonParameters(); + void addPolygon( + const std::string & polygon_name, const PolygonType type, + const double size, const std::string & at); + void addSource(const std::string & source_name, const SourceType type); + void setVectors( + const std::vector & polygons, + const std::vector & sources); + + // Setting TF chains + void sendTransforms(const rclcpp::Time & stamp); + + // Main topic/data working routines + void publishScan(const double dist, const rclcpp::Time & stamp); + void publishPointCloud(const double dist, const rclcpp::Time & stamp); + void publishCmdVel(const double x, const double y, const double tw); + bool waitData(const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp); + bool waitCmdVel(const std::chrono::nanoseconds & timeout); + +protected: + void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg); + + // CollisionMonitor node + std::shared_ptr cm_; + + // Data source publishers + rclcpp::Publisher::SharedPtr scan_pub_; + rclcpp::Publisher::SharedPtr pointcloud_pub_; + + // Working with cmd_vel_in/cmd_vel_out + rclcpp::Publisher::SharedPtr cmd_vel_in_pub_; + rclcpp::Subscription::SharedPtr cmd_vel_out_sub_; + + geometry_msgs::msg::Twist::SharedPtr cmd_vel_out_; +}; // Tester + +Tester::Tester() +{ + cm_ = std::make_shared(); + + scan_pub_ = cm_->create_publisher( + SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + pointcloud_pub_ = cm_->create_publisher( + POINTCLOUD_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + cmd_vel_in_pub_ = cm_->create_publisher( + CMD_VEL_IN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + cmd_vel_out_sub_ = cm_->create_subscription( + CMD_VEL_OUT_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::cmdVelOutCallback, this, std::placeholders::_1)); +} + +Tester::~Tester() +{ + scan_pub_.reset(); + pointcloud_pub_.reset(); + + cmd_vel_in_pub_.reset(); + cmd_vel_out_sub_.reset(); + + cm_.reset(); +} + +void Tester::setCommonParameters() +{ + cm_->declare_parameter( + "cmd_vel_in_topic", rclcpp::ParameterValue(CMD_VEL_IN_TOPIC)); + cm_->set_parameter( + rclcpp::Parameter("cmd_vel_in_topic", CMD_VEL_IN_TOPIC)); + cm_->declare_parameter( + "cmd_vel_out_topic", rclcpp::ParameterValue(CMD_VEL_OUT_TOPIC)); + cm_->set_parameter( + rclcpp::Parameter("cmd_vel_out_topic", CMD_VEL_OUT_TOPIC)); + + cm_->declare_parameter( + "base_frame_id", rclcpp::ParameterValue(BASE_FRAME_ID)); + cm_->set_parameter( + rclcpp::Parameter("base_frame_id", BASE_FRAME_ID)); + cm_->declare_parameter( + "odom_frame_id", rclcpp::ParameterValue(ODOM_FRAME_ID)); + cm_->set_parameter( + rclcpp::Parameter("odom_frame_id", ODOM_FRAME_ID)); + + cm_->declare_parameter( + "transform_tolerance", rclcpp::ParameterValue(TRANSFORM_TOLERANCE)); + cm_->set_parameter( + rclcpp::Parameter("transform_tolerance", TRANSFORM_TOLERANCE)); + cm_->declare_parameter( + "source_timeout", rclcpp::ParameterValue(SOURCE_TIMEOUT)); + cm_->set_parameter( + rclcpp::Parameter("source_timeout", SOURCE_TIMEOUT)); + + cm_->declare_parameter( + "stop_pub_timeout", rclcpp::ParameterValue(STOP_PUB_TIMEOUT)); + cm_->set_parameter( + rclcpp::Parameter("stop_pub_timeout", STOP_PUB_TIMEOUT)); +} + +void Tester::addPolygon( + const std::string & polygon_name, const PolygonType type, + const double size, const std::string & at) +{ + if (type == POLYGON) { + cm_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + const std::vector points { + size, size, size, -size, -size, -size, -size, size}; + cm_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + } else if (type == CIRCLE) { + cm_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("circle")); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "circle")); + + cm_->declare_parameter( + polygon_name + ".radius", rclcpp::ParameterValue(size)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".radius", size)); + } else { // type == POLYGON_UNKNOWN + cm_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("unknown")); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "unknown")); + } + + cm_->declare_parameter( + polygon_name + ".action_type", rclcpp::ParameterValue(at)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".action_type", at)); + + cm_->declare_parameter( + polygon_name + ".max_points", rclcpp::ParameterValue(MAX_POINTS)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".max_points", MAX_POINTS)); + + cm_->declare_parameter( + polygon_name + ".slowdown_ratio", rclcpp::ParameterValue(SLOWDOWN_RATIO)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO)); + + cm_->declare_parameter( + polygon_name + ".time_before_collision", rclcpp::ParameterValue(TIME_BEFORE_COLLISION)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".time_before_collision", TIME_BEFORE_COLLISION)); + + cm_->declare_parameter( + polygon_name + ".simulation_time_step", rclcpp::ParameterValue(SIMULATION_TIME_STEP)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".simulation_time_step", SIMULATION_TIME_STEP)); + + cm_->declare_parameter( + polygon_name + ".visualize", rclcpp::ParameterValue(false)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".visualize", false)); + + cm_->declare_parameter( + polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".polygon_pub_topic", polygon_name)); +} + +void Tester::addSource( + const std::string & source_name, const SourceType type) +{ + if (type == SCAN) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("scan")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "scan")); + } else if (type == POINTCLOUD) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("pointcloud")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "pointcloud")); + + cm_->declare_parameter( + source_name + ".min_height", rclcpp::ParameterValue(0.1)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".min_height", 0.1)); + cm_->declare_parameter( + source_name + ".max_height", rclcpp::ParameterValue(1.0)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".max_height", 1.0)); + } else { // type == SOURCE_UNKNOWN + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("unknown")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "unknown")); + } + + cm_->declare_parameter( + source_name + ".topic", rclcpp::ParameterValue(source_name)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".topic", source_name)); +} + +void Tester::setVectors( + const std::vector & polygons, + const std::vector & sources) +{ + cm_->declare_parameter("polygons", rclcpp::ParameterValue(polygons)); + cm_->set_parameter(rclcpp::Parameter("polygons", polygons)); + + cm_->declare_parameter("observation_sources", rclcpp::ParameterValue(sources)); + cm_->set_parameter(rclcpp::Parameter("observation_sources", sources)); +} + +void Tester::sendTransforms(const rclcpp::Time & stamp) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(cm_); + + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + // Fill TF buffer ahead for future transform usage in CollisionMonitor::process() + const rclcpp::Duration ahead = 1000ms; + for (rclcpp::Time t = stamp; t <= stamp + ahead; t += rclcpp::Duration(50ms)) { + transform.header.stamp = t; + + // base_frame -> source_frame transform + transform.header.frame_id = BASE_FRAME_ID; + transform.child_frame_id = SOURCE_FRAME_ID; + tf_broadcaster->sendTransform(transform); + + // odom_frame -> base_frame transform + transform.header.frame_id = ODOM_FRAME_ID; + transform.child_frame_id = BASE_FRAME_ID; + tf_broadcaster->sendTransform(transform); + } +} + +void Tester::publishScan(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->angle_min = 0.0; + msg->angle_max = 2 * M_PI; + msg->angle_increment = M_PI / 180; + msg->time_increment = 0.0; + msg->scan_time = 0.0; + msg->range_min = 0.1; + msg->range_max = dist + 1.0; + std::vector ranges(360, dist); + msg->ranges = ranges; + + scan_pub_->publish(std::move(msg)); +} + +void Tester::publishPointCloud(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*msg); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + modifier.resize(2); + + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + + // Point 0: (dist, 0.01, 0.2) + *iter_x = dist; + *iter_y = 0.01; + *iter_z = 0.2; + ++iter_x; ++iter_y; ++iter_z; + + // Point 1: (dist, -0.01, 0.2) + *iter_x = dist; + *iter_y = -0.01; + *iter_z = 0.2; + + pointcloud_pub_->publish(std::move(msg)); +} + +void Tester::publishCmdVel(const double x, const double y, const double tw) +{ + // Reset cmd_vel_out_ before calling CollisionMonitor::process() + cmd_vel_out_ = nullptr; + + std::unique_ptr msg = + std::make_unique(); + + msg->linear.x = x; + msg->linear.y = y; + msg->angular.z = tw; + + cmd_vel_in_pub_->publish(std::move(msg)); +} + +bool Tester::waitData(const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (cm_->isDataReceived(stamp)) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitCmdVel(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (cmd_vel_out_) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg) +{ + cmd_vel_out_ = msg; +} + +TEST_F(Tester, testProcessStopSlowdown) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + setVectors({"SlowDown", "Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + + // 2. Obstacle is in slowdown robot zone + publishScan(1.5, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * SLOWDOWN_RATIO, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * SLOWDOWN_RATIO, EPSILON); + + // 3. Obstacle is inside stop zone + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // 4. Restorint back normal operation + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testProcessApproach) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making one circle footprint for approach. + setCommonParameters(); + addPolygon("Approach", CIRCLE, 1.0, "approach"); + addSource(SCAN_NAME, SCAN); + setVectors({"Approach"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // 2. Approaching obstacle (0.2 m ahead from robot footprint) + publishScan(1.2, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // change_ratio = (0.2 m / speed m/s) / TIME_BEFORE_COLLISION s + // where speed = sqrt(0.5^2 + 0.2^2) ~= 0.538516481 m/s + const double change_ratio = (0.2 / 0.538516481) / TIME_BEFORE_COLLISION; + ASSERT_NEAR( + cmd_vel_out_->linear.x, 0.5 * change_ratio, 0.5 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + ASSERT_NEAR( + cmd_vel_out_->linear.y, 0.2 * change_ratio, 0.2 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // 3. Obstacle is inside robot footprint + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // 4. Restorint back normal operation + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testCrossOver) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner circle + // as robot footprint for approach. + setCommonParameters(); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Approach", CIRCLE, 1.0, "approach"); + addSource(POINTCLOUD_NAME, POINTCLOUD); + setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). + // Robot should approach the obstacle. + publishPointCloud(2.5, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(3.0, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // change_ratio = (1.5 m / 3.0 m/s) / TIME_BEFORE_COLLISION s + double change_ratio = (1.5 / 3.0) / TIME_BEFORE_COLLISION; + ASSERT_NEAR( + cmd_vel_out_->linear.x, 3.0 * change_ratio, 3.0 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // 2. Obstacle is inside slowdown zone, but speed is too slow for approach + publishPointCloud(1.5, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.1, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.1 * SLOWDOWN_RATIO, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // 3. Increase robot speed to return again into approach mode. + // The speed should be safer for approach mode, so robot will go to the approach (ahead in 0.5 m) + // even while it is already inside slowdown area. + publishCmdVel(1.0, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // change_ratio = (0.5 m / 1.0 m/s) / TIME_BEFORE_COLLISION s + change_ratio = (0.5 / 1.0) / TIME_BEFORE_COLLISION; + ASSERT_NEAR( + cmd_vel_out_->linear.x, 1.0 * change_ratio, 1.0 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testCeasePublishZeroVel) +{ + rclcpp::Time curr_time = cm_->now(); + + // Configure stop and approach zones, and basic data source + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("Approach", CIRCLE, 2.0, "approach"); + addSource(SCAN_NAME, SCAN); + setVectors({"Stop", "Approach"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is inside approach footprint: robot should stop + publishScan(1.5, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // Wait more than STOP_PUB_TIMEOUT time + std::this_thread::sleep_for(std::chrono::duration(STOP_PUB_TIMEOUT + 0.01)); + + // 2. Check that zero cmd_vel_out velocity won't be published more for this case + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_FALSE(waitCmdVel(100ms)); + + // 3. Release robot to normal operation + publishScan(3.0, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + + // 4. Obstacle is inside stop zone + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // Wait more than STOP_PUB_TIMEOUT time + std::this_thread::sleep_for(std::chrono::duration(STOP_PUB_TIMEOUT + 0.01)); + + // 5. Check that zero cmd_vel_out velocity won't be published more for this case + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_FALSE(waitCmdVel(100ms)); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testProcessNonActive) +{ + rclcpp::Time curr_time = cm_->now(); + + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Configure Collision Monitor node, but not activate + cm_->configure(); + + // Share TF + sendTransforms(curr_time); + + // Call CollisionMonitor::process() + publishCmdVel(1.0, 0.0, 0.0); + // ... and check that cmd_vel_out was not published + ASSERT_FALSE(waitCmdVel(100ms)); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testIncorrectPolygonType) +{ + setCommonParameters(); + addPolygon("UnknownShape", POLYGON_UNKNOWN, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + setVectors({"UnknownShape"}, {SCAN_NAME}); + + // Check that Collision Monitor node can not be configured for this parameters set + cm_->cant_configure(); +} + +TEST_F(Tester, testIncorrectSourceType) +{ + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource("UnknownSource", SOURCE_UNKNOWN); + setVectors({"Stop"}, {"UnknownSource"}); + + // Check that Collision Monitor node can not be configured for this parameters set + cm_->cant_configure(); +} + +TEST_F(Tester, testPolygonsNotSet) +{ + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + + // Check that Collision Monitor node can not be configured for this parameters set + cm_->cant_configure(); +} + +TEST_F(Tester, testSourcesNotSet) +{ + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + cm_->declare_parameter("polygons", rclcpp::ParameterValue({"Stop"})); + cm_->set_parameter(rclcpp::Parameter("polygons", std::vector{"Stop"})); + + // Check that Collision Monitor node can not be configured for this parameters set + cm_->cant_configure(); +} + +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_collision_monitor/test/kinematics_test.cpp b/nav2_collision_monitor/test/kinematics_test.cpp new file mode 100644 index 0000000000..2c1ec599a7 --- /dev/null +++ b/nav2_collision_monitor/test/kinematics_test.cpp @@ -0,0 +1,98 @@ +// Copyright (c) 2022 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_collision_monitor/types.hpp" +#include "nav2_collision_monitor/kinematics.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(KinematicsTest, testTransformPoints) +{ + // Transform: move frame to (2.0, 1.0) coordinate and rotate it on 30 degrees + const nav2_collision_monitor::Pose tf{2.0, 1.0, M_PI / 6.0}; + // Add two points in the basic frame + std::vector points; + points.push_back({3.0, 2.0}); + points.push_back({0.0, 0.0}); + + // Transform points from basic frame to the new frame + nav2_collision_monitor::transformPoints(tf, points); + + // Check that all points were transformed correctly + // Distance to point in a new frame + double new_point_distance = std::sqrt(1.0 + 1.0); + // Angle of point in a new frame. Calculated as: + // angle of point in a moved frame - frame rotation. + double new_point_angle = M_PI / 4.0 - M_PI / 6.0; + EXPECT_NEAR(points[0].x, new_point_distance * std::cos(new_point_angle), EPSILON); + EXPECT_NEAR(points[0].y, new_point_distance * std::sin(new_point_angle), EPSILON); + + new_point_distance = std::sqrt(1.0 + 4.0); + new_point_angle = M_PI + std::atan(1.0 / 2.0) - M_PI / 6.0; + EXPECT_NEAR(points[1].x, new_point_distance * std::cos(new_point_angle), EPSILON); + EXPECT_NEAR(points[1].y, new_point_distance * std::sin(new_point_angle), EPSILON); +} + +TEST(KinematicsTest, testProjectState) +{ + // Y Y + // ^ ^ + // ' ' + // ' ==> ' * + // ' * <- robot's nose 2.0' o <- moved robot + // 1.0' o <- robot's back ' + // ..........>X ..........>X + // 2.0 2.0 + + // Initial pose of robot + nav2_collision_monitor::Pose pose{2.0, 1.0, M_PI / 4.0}; + // Initial velocity of robot + nav2_collision_monitor::Velocity vel{0.0, 1.0, M_PI / 4.0}; + const double dt = 1.0; + + // Moving robot and rotating velocity + nav2_collision_monitor::projectState(dt, pose, vel); + + // Check pose of moved and rotated robot + EXPECT_NEAR(pose.x, 2.0, EPSILON); + EXPECT_NEAR(pose.y, 2.0, EPSILON); + EXPECT_NEAR(pose.theta, M_PI / 2, EPSILON); + + // Check rotated velocity + // Rotated velocity angle is an initial velocity angle + rotation + const double rotated_vel_angle = M_PI / 2.0 + M_PI / 4.0; + EXPECT_NEAR(vel.x, std::cos(rotated_vel_angle), EPSILON); + EXPECT_NEAR(vel.y, std::sin(rotated_vel_angle), EPSILON); + EXPECT_NEAR(vel.tw, M_PI / 4.0, EPSILON); // should be the same +} diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp new file mode 100644 index 0000000000..ec7a19f124 --- /dev/null +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -0,0 +1,698 @@ +// Copyright (c) 2022 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/polygon.hpp" +#include "nav2_collision_monitor/circle.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char BASE_FRAME_ID[]{"base_link"}; +static const char FOOTPRINT_TOPIC[]{"footprint"}; +static const char POLYGON_PUB_TOPIC[]{"polygon"}; +static const char POLYGON_NAME[]{"TestPolygon"}; +static const char CIRCLE_NAME[]{"TestCircle"}; +static const std::vector SQUARE_POLYGON { + 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}; +static const std::vector ARBITRARY_POLYGON { + 1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 2.0, -1.0, -1.0, -1.0, -1.0, 1.0}; +static const double CIRCLE_RADIUS{0.5}; +static const int MAX_POINTS{1}; +static const double SLOWDOWN_RATIO{0.7}; +static const double TIME_BEFORE_COLLISION{1.0}; +static const double SIMULATION_TIME_STEP{0.01}; +static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; + +class TestNode : public nav2_util::LifecycleNode +{ +public: + TestNode() + : nav2_util::LifecycleNode("test_node"), polygon_received_(nullptr) + { + polygon_sub_ = this->create_subscription( + POLYGON_PUB_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&TestNode::polygonCallback, this, std::placeholders::_1)); + } + + ~TestNode() + { + footprint_pub_.reset(); + } + + void publishFootprint() + { + footprint_pub_ = this->create_publisher( + FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = BASE_FRAME_ID; + msg->header.stamp = this->now(); + + geometry_msgs::msg::Point32 p; + for (unsigned int i = 0; i < SQUARE_POLYGON.size(); i = i + 2) { + p.x = SQUARE_POLYGON[i]; + p.y = SQUARE_POLYGON[i + 1]; + msg->polygon.points.push_back(p); + } + + footprint_pub_->publish(std::move(msg)); + } + + void polygonCallback(geometry_msgs::msg::PolygonStamped::SharedPtr msg) + { + polygon_received_ = msg; + } + + geometry_msgs::msg::PolygonStamped::SharedPtr waitPolygonReceived( + const std::chrono::nanoseconds & timeout) + { + rclcpp::Time start_time = this->now(); + while (rclcpp::ok() && this->now() - start_time <= rclcpp::Duration(timeout)) { + if (polygon_received_) { + return polygon_received_; + } + rclcpp::spin_some(this->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return nullptr; + } + +private: + rclcpp::Publisher::SharedPtr footprint_pub_; + rclcpp::Subscription::SharedPtr polygon_sub_; + + geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received_; +}; // TestNode + +class PolygonWrapper : public nav2_collision_monitor::Polygon +{ +public: + PolygonWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & polygon_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance) + : nav2_collision_monitor::Polygon( + node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) + { + } + + double getSimulationTimeStep() const + { + return simulation_time_step_; + } + + double isVisualize() const + { + return visualize_; + } +}; // PolygonWrapper + +class CircleWrapper : public nav2_collision_monitor::Circle +{ +public: + CircleWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & polygon_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance) + : nav2_collision_monitor::Circle( + node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) + { + } + + double getRadius() const + { + return radius_; + } + + double getRadiusSquared() const + { + return radius_squared_; + } +}; // CircleWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + +protected: + // Working with parameters + void setCommonParameters(const std::string & polygon_name, const std::string & action_type); + void setPolygonParameters(const std::vector & points); + void setCircleParameters(const double radius); + bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); + // Creating routines + void createPolygon(const std::string & action_type); + void createCircle(const std::string & action_type); + + // Wait until footprint will be received + bool waitFootprint( + const std::chrono::nanoseconds & timeout, + std::vector & footprint); + + std::shared_ptr test_node_; + + std::shared_ptr polygon_; + std::shared_ptr circle_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; // Tester + +Tester::Tester() +{ + test_node_ = std::make_shared(); + + tf_buffer_ = std::make_shared(test_node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + polygon_.reset(); + circle_.reset(); + + test_node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setCommonParameters(const std::string & polygon_name, const std::string & action_type) +{ + test_node_->declare_parameter( + polygon_name + ".action_type", rclcpp::ParameterValue(action_type)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".action_type", action_type)); + + test_node_->declare_parameter( + polygon_name + ".max_points", rclcpp::ParameterValue(MAX_POINTS)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".max_points", MAX_POINTS)); + + test_node_->declare_parameter( + polygon_name + ".slowdown_ratio", rclcpp::ParameterValue(SLOWDOWN_RATIO)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO)); + + test_node_->declare_parameter( + polygon_name + ".time_before_collision", + rclcpp::ParameterValue(TIME_BEFORE_COLLISION)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".time_before_collision", TIME_BEFORE_COLLISION)); + + test_node_->declare_parameter( + polygon_name + ".simulation_time_step", rclcpp::ParameterValue(SIMULATION_TIME_STEP)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".simulation_time_step", SIMULATION_TIME_STEP)); + + test_node_->declare_parameter( + polygon_name + ".visualize", rclcpp::ParameterValue(true)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".visualize", true)); + + test_node_->declare_parameter( + polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(POLYGON_PUB_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC)); +} + +void Tester::setPolygonParameters(const std::vector & points) +{ + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".footprint_topic", FOOTPRINT_TOPIC)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points)); +} + +void Tester::setCircleParameters(const double radius) +{ + test_node_->declare_parameter( + std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius)); +} + +bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const std::string & param) +{ + bool ret = false; + + // Check that parameter is not set after configuring + try { + test_node_->get_parameter(polygon_name + "." + param); + } catch (std::exception & ex) { + std::string message = ex.what(); + if (message.find("." + param) != std::string::npos && + message.find("is not initialized") != std::string::npos) + { + ret = true; + } + } + return ret; +} + +void Tester::createPolygon(const std::string & action_type) +{ + setCommonParameters(POLYGON_NAME, action_type); + setPolygonParameters(SQUARE_POLYGON); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + polygon_->activate(); +} + +void Tester::createCircle(const std::string & action_type) +{ + setCommonParameters(CIRCLE_NAME, action_type); + setCircleParameters(CIRCLE_RADIUS); + + circle_ = std::make_shared( + test_node_, CIRCLE_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(circle_->configure()); + circle_->activate(); +} + +bool Tester::waitFootprint( + const std::chrono::nanoseconds & timeout, + std::vector & footprint) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + polygon_->updatePolygon(); + polygon_->getPolygon(footprint); + if (footprint.size() > 0) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +TEST_F(Tester, testPolygonGetStopParameters) +{ + createPolygon("stop"); + + // Check that common parameters set correctly + EXPECT_EQ(polygon_->getName(), POLYGON_NAME); + EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::STOP); + EXPECT_EQ(polygon_->getMaxPoints(), MAX_POINTS); + EXPECT_EQ(polygon_->isVisualize(), true); + + // Check that polygon set correctly + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_EQ(poly.size(), 4u); + EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0], EPSILON); + EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1], EPSILON); + EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2], EPSILON); + EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3], EPSILON); + EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4], EPSILON); + EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5], EPSILON); + EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6], EPSILON); + EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON); +} + +TEST_F(Tester, testPolygonGetSlowdownParameters) +{ + createPolygon("slowdown"); + + // Check that common parameters set correctly + EXPECT_EQ(polygon_->getName(), POLYGON_NAME); + EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::SLOWDOWN); + EXPECT_EQ(polygon_->getMaxPoints(), MAX_POINTS); + EXPECT_EQ(polygon_->isVisualize(), true); + // Check that slowdown_ratio is correct + EXPECT_NEAR(polygon_->getSlowdownRatio(), SLOWDOWN_RATIO, EPSILON); +} + +TEST_F(Tester, testPolygonGetApproachParameters) +{ + createPolygon("approach"); + + // Check that common parameters set correctly + EXPECT_EQ(polygon_->getName(), POLYGON_NAME); + EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::APPROACH); + EXPECT_EQ(polygon_->getMaxPoints(), MAX_POINTS); + EXPECT_EQ(polygon_->isVisualize(), true); + // Check that time_before_collision and simulation_time_step are correct + EXPECT_NEAR(polygon_->getTimeBeforeCollision(), TIME_BEFORE_COLLISION, EPSILON); + EXPECT_NEAR(polygon_->getSimulationTimeStep(), SIMULATION_TIME_STEP, EPSILON); +} + +TEST_F(Tester, testCircleGetParameters) +{ + createCircle("approach"); + + // Check that common parameters set correctly + EXPECT_EQ(circle_->getName(), CIRCLE_NAME); + EXPECT_EQ(circle_->getActionType(), nav2_collision_monitor::APPROACH); + EXPECT_EQ(circle_->getMaxPoints(), MAX_POINTS); + + // Check that Circle-specific parameters were set correctly + EXPECT_NEAR(circle_->getRadius(), CIRCLE_RADIUS, EPSILON); + EXPECT_NEAR(circle_->getRadiusSquared(), CIRCLE_RADIUS * CIRCLE_RADIUS, EPSILON); +} + +TEST_F(Tester, testPolygonUndeclaredActionType) +{ + // "action_type" parameter is not initialized + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); + // Check that "action_type" parameter is not set after configuring + ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "action_type")); +} + +TEST_F(Tester, testPolygonUndeclaredPoints) +{ + // "points" parameter is not initialized + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); + // Check that "points" parameter is not set after configuring + ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "points")); +} + +TEST_F(Tester, testPolygonIncorrectActionType) +{ + setCommonParameters(POLYGON_NAME, "incorrect_action_type"); + setPolygonParameters(SQUARE_POLYGON); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testPolygonIncorrectPoints1) +{ + setCommonParameters(POLYGON_NAME, "stop"); + + std::vector incorrect_points = SQUARE_POLYGON; + incorrect_points.resize(6); // Not enough for triangle + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testPolygonIncorrectPoints2) +{ + setCommonParameters(POLYGON_NAME, "stop"); + + std::vector incorrect_points = SQUARE_POLYGON; + incorrect_points.resize(9); // Odd number of points + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testCircleUndeclaredRadius) +{ + setCommonParameters(CIRCLE_NAME, "stop"); + + circle_ = std::make_shared( + test_node_, CIRCLE_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(circle_->configure()); + + // Check that "radius" parameter is not set after configuring + ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius")); +} + +TEST_F(Tester, testPolygonUpdate) +{ + createPolygon("approach"); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_EQ(poly.size(), 0u); + + test_node_->publishFootprint(); + + std::vector footprint; + ASSERT_TRUE(waitFootprint(500ms, footprint)); + + ASSERT_EQ(footprint.size(), 4u); + EXPECT_NEAR(footprint[0].x, SQUARE_POLYGON[0], EPSILON); + EXPECT_NEAR(footprint[0].y, SQUARE_POLYGON[1], EPSILON); + EXPECT_NEAR(footprint[1].x, SQUARE_POLYGON[2], EPSILON); + EXPECT_NEAR(footprint[1].y, SQUARE_POLYGON[3], EPSILON); + EXPECT_NEAR(footprint[2].x, SQUARE_POLYGON[4], EPSILON); + EXPECT_NEAR(footprint[2].y, SQUARE_POLYGON[5], EPSILON); + EXPECT_NEAR(footprint[3].x, SQUARE_POLYGON[6], EPSILON); + EXPECT_NEAR(footprint[3].y, SQUARE_POLYGON[7], EPSILON); +} + +TEST_F(Tester, testPolygonGetPointsInside) +{ + createPolygon("stop"); + + std::vector points; + + // Out of boundaries points + points.push_back({1.0, 0.0}); + points.push_back({0.0, 1.0}); + points.push_back({-1.0, 0.0}); + points.push_back({0.0, -1.0}); + ASSERT_EQ(polygon_->getPointsInside(points), 0); + + // Add one point inside + points.push_back({-0.1, 0.3}); + ASSERT_EQ(polygon_->getPointsInside(points), 1); +} + +TEST_F(Tester, testPolygonGetPointsInsideEdge) +{ + // Test for checking edge cases in raytracing algorithm. + // All points are lie on the edge lines parallel to OX, where the raytracing takes place. + setCommonParameters(POLYGON_NAME, "stop"); + setPolygonParameters(ARBITRARY_POLYGON); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + + std::vector points; + + // Out of boundaries points + points.push_back({-2.0, -1.0}); + points.push_back({-2.0, 0.0}); + points.push_back({-2.0, 1.0}); + points.push_back({3.0, -1.0}); + points.push_back({3.0, 0.0}); + points.push_back({3.0, 1.0}); + ASSERT_EQ(polygon_->getPointsInside(points), 0); + + // Add one point inside + points.push_back({0.0, 0.0}); + ASSERT_EQ(polygon_->getPointsInside(points), 1); +} + +TEST_F(Tester, testCircleGetPointsInside) +{ + createCircle("stop"); + + std::vector points; + // Point out of radius + points.push_back({1.0, 0.0}); + ASSERT_EQ(circle_->getPointsInside(points), 0); + + // Add one point inside + points.push_back({-0.1, 0.3}); + ASSERT_EQ(circle_->getPointsInside(points), 1); +} + +TEST_F(Tester, testPolygonGetCollisionTime) +{ + createPolygon("approach"); + + // Set footprint for Polygon + test_node_->publishFootprint(); + std::vector footprint; + ASSERT_TRUE(waitFootprint(500ms, footprint)); + ASSERT_EQ(footprint.size(), 4u); + + // Forward movement check + nav2_collision_monitor::Velocity vel{0.5, 0.0, 0.0}; // 0.5 m/s forward movement + // Two points 0.2 m ahead the footprint (0.5 m) + std::vector points{{0.7, -0.01}, {0.7, 0.01}}; + // Collision is expected to be ~= 0.2 m / 0.5 m/s seconds + EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); + + // Backward movement check + vel = {-0.5, 0.0, 0.0}; // 0.5 m/s backward movement + // Two points 0.2 m behind the footprint (0.5 m) + points.clear(); + points = {{-0.7, -0.01}, {-0.7, 0.01}}; + // Collision is expected to be in ~= 0.2 m / 0.5 m/s seconds + EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); + + // Sideway movement check + vel = {0.0, 0.5, 0.0}; // 0.5 m/s sideway movement + // Two points 0.1 m ahead the footprint (0.5 m) + points.clear(); + points = {{-0.01, 0.6}, {0.01, 0.6}}; + // Collision is expected to be in ~= 0.1 m / 0.5 m/s seconds + EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.2, SIMULATION_TIME_STEP); + + // Rotation check + vel = {0.0, 0.0, 1.0}; // 1.0 rad/s rotation + // ^ OX + // ' + // x'x <- 2 collision points + // ' + // ----------- <- robot footprint + // OY | ' | + // <...|....o....|... + // | ' | + // ----------- + // ' + points.clear(); + points = {{0.49, -0.01}, {0.49, 0.01}}; + // Collision is expected to be in ~= 45 degrees * M_PI / (180 degrees * 1.0 rad/s) seconds + double exp_res = 45 / 180 * M_PI; + EXPECT_NEAR(polygon_->getCollisionTime(points, vel), exp_res, EPSILON); + + // Two points are already inside footprint + vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement + // Two points inside + points.clear(); + points = {{0.1, -0.01}, {0.1, 0.01}}; + // Collision already appeared: collision time should be 0 + EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.0, EPSILON); + + // All points are out of simulation prediction + vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement + // Two points 0.6 m ahead the footprint (0.5 m) + points.clear(); + points = {{1.1, -0.01}, {1.1, 0.01}}; + // There is no collision: return value should be negative + EXPECT_LT(polygon_->getCollisionTime(points, vel), 0.0); +} + +TEST_F(Tester, testPolygonPublish) +{ + createPolygon("stop"); + polygon_->publish(); + geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received = + test_node_->waitPolygonReceived(500ms); + + ASSERT_NE(polygon_received, nullptr); + ASSERT_EQ(polygon_received->polygon.points.size(), 4u); + EXPECT_NEAR(polygon_received->polygon.points[0].x, SQUARE_POLYGON[0], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[0].y, SQUARE_POLYGON[1], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[1].x, SQUARE_POLYGON[2], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[1].y, SQUARE_POLYGON[3], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[2].x, SQUARE_POLYGON[4], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[2].y, SQUARE_POLYGON[5], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[3].x, SQUARE_POLYGON[6], EPSILON); + EXPECT_NEAR(polygon_received->polygon.points[3].y, SQUARE_POLYGON[7], EPSILON); + + polygon_->deactivate(); +} + +TEST_F(Tester, testPolygonDefaultVisualize) +{ + // Use default parameters, visualize should be false by-default + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); + setPolygonParameters(SQUARE_POLYGON); + + // Create new polygon + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + polygon_->activate(); + + // Try to publish polygon + polygon_->publish(); + + // Wait for polygon: it should not be published + ASSERT_EQ(test_node_->waitPolygonReceived(100ms), nullptr); +} + +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_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp new file mode 100644 index 0000000000..c199c02286 --- /dev/null +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -0,0 +1,439 @@ +// Copyright (c) 2022 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/scan.hpp" +#include "nav2_collision_monitor/pointcloud.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char BASE_FRAME_ID[]{"base_link"}; +static const char SOURCE_FRAME_ID[]{"base_source"}; +static const char GLOBAL_FRAME_ID[]{"odom"}; +static const char SCAN_NAME[]{"LaserScan"}; +static const char SCAN_TOPIC[]{"scan"}; +static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char POINTCLOUD_TOPIC[]{"pointcloud"}; +static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; +static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)}; + +class TestNode : public nav2_util::LifecycleNode +{ +public: + TestNode() + : nav2_util::LifecycleNode("test_node") + { + } + + ~TestNode() + { + scan_pub_.reset(); + } + + void publishScan(const rclcpp::Time & stamp) + { + scan_pub_ = this->create_publisher( + SCAN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->angle_min = 0.0; + msg->angle_max = 2 * M_PI; + msg->angle_increment = M_PI / 2; + msg->time_increment = 0.0; + msg->scan_time = 0.0; + msg->range_min = 0.1; + msg->range_max = 1.1; + std::vector ranges(4, 1.0); + msg->ranges = ranges; + + scan_pub_->publish(std::move(msg)); + } + + void publishPointCloud(const rclcpp::Time & stamp) + { + pointcloud_pub_ = this->create_publisher( + POINTCLOUD_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*msg); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + modifier.resize(3); + + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + + // Point 0: (0.5, 0.5, 0.2) + *iter_x = 0.5; + *iter_y = 0.5; + *iter_z = 0.2; + ++iter_x; ++iter_y; ++iter_z; + + // Point 1: (-0.5, -0.5, 0.3) + *iter_x = -0.5; + *iter_y = -0.5; + *iter_z = 0.3; + ++iter_x; ++iter_y; ++iter_z; + + // Point 2: (1.0, 1.0, 10.0) + *iter_x = 1.0; + *iter_y = 1.0; + *iter_z = 10.0; + + pointcloud_pub_->publish(std::move(msg)); + } + +private: + rclcpp::Publisher::SharedPtr scan_pub_; + rclcpp::Publisher::SharedPtr pointcloud_pub_; +}; // TestNode + +class ScanWrapper : public nav2_collision_monitor::Scan +{ +public: + ScanWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout) + : nav2_collision_monitor::Scan( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout) + {} + + bool dataReceived() const + { + return data_ != nullptr; + } +}; // ScanWrapper + +class PointCloudWrapper : public nav2_collision_monitor::PointCloud +{ +public: + PointCloudWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout) + : nav2_collision_monitor::PointCloud( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout) + {} + + bool dataReceived() const + { + return data_ != nullptr; + } +}; // PointCloudWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + +protected: + // Setting TF chains + void sendTransforms(const rclcpp::Time & stamp); + + // Data sources working routines + bool waitScan(const std::chrono::nanoseconds & timeout); + bool waitPointCloud(const std::chrono::nanoseconds & timeout); + void checkScan(const std::vector & data); + void checkPointCloud(const std::vector & data); + + std::shared_ptr test_node_; + std::shared_ptr scan_; + std::shared_ptr pointcloud_; + +private: + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; // Tester + +Tester::Tester() +{ + test_node_ = std::make_shared(); + + tf_buffer_ = std::make_shared(test_node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); + + // Create Scan object + test_node_->declare_parameter( + std::string(SCAN_NAME) + ".topic", rclcpp::ParameterValue(SCAN_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(SCAN_NAME) + ".topic", SCAN_TOPIC)); + + scan_ = std::make_shared( + test_node_, SCAN_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT); + scan_->configure(); + + // Create PointCloud object + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".topic", rclcpp::ParameterValue(POINTCLOUD_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".topic", POINTCLOUD_TOPIC)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".min_height", rclcpp::ParameterValue(0.1)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".min_height", 0.1)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".max_height", rclcpp::ParameterValue(1.0)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".max_height", 1.0)); + + pointcloud_ = std::make_shared( + test_node_, POINTCLOUD_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT); + pointcloud_->configure(); +} + +Tester::~Tester() +{ + scan_.reset(); + pointcloud_.reset(); + + test_node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::sendTransforms(const rclcpp::Time & stamp) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(test_node_); + + geometry_msgs::msg::TransformStamped transform; + + // base_frame -> source_frame transform + transform.header.frame_id = BASE_FRAME_ID; + transform.child_frame_id = SOURCE_FRAME_ID; + + transform.header.stamp = stamp; + transform.transform.translation.x = 0.1; + transform.transform.translation.y = 0.1; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); + + // global_frame -> base_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = BASE_FRAME_ID; + + transform.transform.translation.x = 0.0; + transform.transform.translation.y = 0.0; + + tf_broadcaster->sendTransform(transform); +} + +bool Tester::waitScan(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (scan_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitPointCloud(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (pointcloud_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::checkScan(const std::vector & data) +{ + ASSERT_EQ(data.size(), 4u); + + // Point 0: (1.0 + 0.1, 0.0 + 0.1) + EXPECT_NEAR(data[0].x, 1.1, EPSILON); + EXPECT_NEAR(data[0].y, 0.1, EPSILON); + + // Point 1: (0.0 + 0.1, 1.0 + 0.1) + EXPECT_NEAR(data[1].x, 0.1, EPSILON); + EXPECT_NEAR(data[1].y, 1.1, EPSILON); + + // Point 2: (-1.0 + 0.1, 0.0 + 0.1) + EXPECT_NEAR(data[2].x, -0.9, EPSILON); + EXPECT_NEAR(data[2].y, 0.1, EPSILON); + + // Point 3: (0.0 + 0.1, -1.0 + 0.1) + EXPECT_NEAR(data[3].x, 0.1, EPSILON); + EXPECT_NEAR(data[3].y, -0.9, EPSILON); +} + +void Tester::checkPointCloud(const std::vector & data) +{ + ASSERT_EQ(data.size(), 2u); + + // Point 0: (0.5 + 0.1, 0.5 + 0.1) + EXPECT_NEAR(data[0].x, 0.6, EPSILON); + EXPECT_NEAR(data[0].y, 0.6, EPSILON); + + // Point 1: (-0.5 + 0.1, -0.5 + 0.1) + EXPECT_NEAR(data[1].x, -0.4, EPSILON); + EXPECT_NEAR(data[1].y, -0.4, EPSILON); + + // Point 2 should be out of scope by height +} + +TEST_F(Tester, testGetData) +{ + rclcpp::Time curr_time = test_node_->now(); + + sendTransforms(curr_time); + + // Publish data for sources + test_node_->publishScan(curr_time); + test_node_->publishPointCloud(curr_time); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitPointCloud(500ms)); + + // Check Scan data + std::vector data; + scan_->getData(curr_time, data); + checkScan(data); + + // Check Pointcloud data + data.clear(); + pointcloud_->getData(curr_time, data); + checkPointCloud(data); +} + +TEST_F(Tester, testGetOutdatedData) +{ + rclcpp::Time curr_time = test_node_->now(); + + sendTransforms(curr_time); + + // Publish outdated data for sources + test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s); + test_node_->publishPointCloud(curr_time - DATA_TIMEOUT - 1s); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitPointCloud(500ms)); + + // Scan data should be empty + std::vector data; + scan_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Pointcloud data should be empty + pointcloud_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); +} + +TEST_F(Tester, testIncorrectFrameData) +{ + rclcpp::Time curr_time = test_node_->now(); + + // Send incorrect transform + sendTransforms(curr_time - 1s); + + // Publish data for sources + test_node_->publishScan(curr_time); + test_node_->publishPointCloud(curr_time); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitPointCloud(500ms)); + + // Scan data should be empty + std::vector data; + scan_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Pointcloud data should be empty + pointcloud_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); +} + +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; +} From 252d844450a0902bd8492635ff9546d495e3bdb3 Mon Sep 17 00:00:00 2001 From: Pradheep Krishna Date: Thu, 21 Jul 2022 17:59:07 +0200 Subject: [PATCH 026/131] removing the extra argument in class dec (#3084) --- nav2_collision_monitor/src/collision_monitor_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 46f9989f32..5088e2c5a2 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -28,7 +28,7 @@ namespace nav2_collision_monitor { CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("collision_monitor", "", false, options), +: nav2_util::LifecycleNode("collision_monitor", "", options), process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}}, stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0) { From e8047ce99be519d1eb991db008c4db60af0e9b97 Mon Sep 17 00:00:00 2001 From: nakai-omer <108797279+nakai-omer@users.noreply.github.com> Date: Thu, 21 Jul 2022 18:59:53 +0300 Subject: [PATCH 027/131] Fix for #3078, fix image path in YAML (#3082) * Fix for #3078, fix image path in YAML * Update map_io.cpp * Update map_io.cpp * Update map_io.cpp * Update map_io.cpp --- nav2_map_server/src/map_io.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index b26b09be38..050fc63d66 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -504,11 +504,14 @@ void tryWriteMapToFile( tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); double yaw, pitch, roll; mat.getEulerYPR(yaw, pitch, roll); + + const int file_name_index = mapdatafile.find_last_of("/\\"); + std::string image_name = mapdatafile.substr(file_name_index + 1); YAML::Emitter e; e << YAML::Precision(3); e << YAML::BeginMap; - e << YAML::Key << "image" << YAML::Value << mapdatafile; + e << YAML::Key << "image" << YAML::Value << image_name; e << YAML::Key << "mode" << YAML::Value << map_mode_to_string(save_parameters.mode); e << YAML::Key << "resolution" << YAML::Value << map.info.resolution; e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x << From ffa00616a59e9e6226c8396d8dfa9d2447c536ae Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Thu, 21 Jul 2022 12:15:26 -0600 Subject: [PATCH 028/131] do not depend on velocity for approach scaling (#3047) * do not depend on velocity for approach scaling * add approach_velocity_scaling_dist to README * do not pass references to shared ptr * fixup! do not pass references to shared ptr * fix approach velocity scaling compile issues * default approach_velocity_scaling_dist based on costmap size * change default approach_velocity_scaling_dist to 0.6 to match previous behavior * update tests for approach velocity scaling * remove use_approach_linear_velocity_scaling from readme * smooth approach velocity scaling --- .../README.md | 3 +- .../regulated_pure_pursuit_controller.hpp | 15 +++- .../src/regulated_pure_pursuit_controller.cpp | 75 +++++++++++++------ .../test/test_regulated_pp.cpp | 38 ++++++++-- 4 files changed, 96 insertions(+), 35 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 839c0b0925..379e2a56f2 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -61,7 +61,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `transform_tolerance` | The TF transform tolerance | | `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | | `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | -| `use_approach_linear_velocity_scaling` | Whether to scale the linear velocity down on approach to the goal for a smooth stop | +| `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | | `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions, limited to maximum distance of lookahead distance selected | | `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | | `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | @@ -111,6 +111,7 @@ controller_server: use_velocity_scaled_lookahead_dist: false min_approach_linear_velocity: 0.05 use_approach_linear_velocity_scaling: true + approach_velocity_scaling_dist: 1.0 max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true use_cost_regulated_linear_velocity_scaling: false diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 9224921de2..01a41df0ac 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -208,19 +208,27 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double costAtPose(const double & x, const double & y); + double approachVelocityScalingFactor( + const nav_msgs::msg::Path & path + ) const; + + void applyApproachVelocityScaling( + const nav_msgs::msg::Path & path, + double & linear_vel + ) const; + /** * @brief apply regulation constraints to the system * @param linear_vel robot command linear velocity input - * @param dist_error error in the carrot distance and lookahead distance * @param lookahead_dist optimal lookahead distance * @param curvature curvature of path * @param speed Speed of robot * @param pose_cost cost at this pose */ void applyConstraints( - const double & dist_error, const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & speed, - const double & pose_cost, double & linear_vel, double & sign); + const double & pose_cost, const nav_msgs::msg::Path & path, + double & linear_vel, double & sign); /** * @brief Find the intersection a circle and a line segment. @@ -281,6 +289,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool use_velocity_scaled_lookahead_dist_; tf2::Duration transform_tolerance_; double min_approach_linear_velocity_; + double approach_velocity_scaling_dist_; double control_duration_; double max_allowed_time_to_collision_up_to_carrot_; bool use_regulated_linear_velocity_scaling_; diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 33166c7af1..c31660b6a9 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -79,6 +79,9 @@ void RegulatedPurePursuitController::configure( rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); @@ -128,6 +131,14 @@ void RegulatedPurePursuitController::configure( node->get_parameter( plugin_name_ + ".min_approach_linear_velocity", min_approach_linear_velocity_); + node->get_parameter( + plugin_name_ + ".approach_velocity_scaling_dist", + approach_velocity_scaling_dist_); + if (approach_velocity_scaling_dist_ > costmap_->getSizeInMetersX() / 2.0) { + RCLCPP_WARN( + logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " + "leading to permanent slowdown"); + } node->get_parameter( plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", max_allowed_time_to_collision_up_to_carrot_); @@ -325,9 +336,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); } else { applyConstraints( - fabs(lookahead_dist - sqrt(carrot_dist2)), - lookahead_dist, curvature, speed, - costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel, sign); + curvature, speed, + costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, + linear_vel, sign); // Apply curvature to angular velocity after constraining linear velocity angular_vel = sign * linear_vel * curvature; @@ -571,14 +582,48 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double return static_cast(cost); } +double RegulatedPurePursuitController::approachVelocityScalingFactor( + const nav_msgs::msg::Path & transformed_path +) const +{ + // Waiting to apply the threshold based on integrated distance ensures we don't + // erroneously apply approach scaling on curvy paths that are contained in a large local costmap. + double remaining_distance = nav2_util::geometry_utils::calculate_path_length(transformed_path); + if (remaining_distance < approach_velocity_scaling_dist_) { + auto & last = transformed_path.poses.back(); + // Here we will use a regular euclidean distance from the robot frame (origin) + // to get smooth scaling, regardless of path density. + double distance_to_last_pose = std::hypot(last.pose.position.x, last.pose.position.y); + return distance_to_last_pose / approach_velocity_scaling_dist_; + } else { + return 1.0; + } +} + +void RegulatedPurePursuitController::applyApproachVelocityScaling( + const nav_msgs::msg::Path & path, + double & linear_vel +) const +{ + double approach_vel = linear_vel; + double velocity_scaling = approachVelocityScalingFactor(path); + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) { + approach_vel = min_approach_linear_velocity_; + } else { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + linear_vel = std::min(linear_vel, approach_vel); +} + void RegulatedPurePursuitController::applyConstraints( - const double & dist_error, const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, - const double & pose_cost, double & linear_vel, double & sign) + const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) { double curvature_vel = linear_vel; double cost_vel = linear_vel; - double approach_vel = linear_vel; // limit the linear velocity by curvature const double radius = fabs(1.0 / curvature); @@ -605,23 +650,7 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = std::min(cost_vel, curvature_vel); linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); - // if the actual lookahead distance is shorter than requested, that means we're at the - // end of the path. We'll scale linear velocity by error to slow to a smooth stop. - // This expression is eq. to (1) holding time to goal, t, constant using the theoretical - // lookahead distance and proposed velocity and (2) using t with the actual lookahead - // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). - if (dist_error > 2.0 * costmap_->getResolution()) { - double velocity_scaling = 1.0 - (dist_error / lookahead_dist); - double unbounded_vel = approach_vel * velocity_scaling; - if (unbounded_vel < min_approach_linear_velocity_) { - approach_vel = min_approach_linear_velocity_; - } else { - approach_vel *= velocity_scaling; - } - - // Use the lowest velocity between approach and other constraints, if all overlapping - linear_vel = std::min(linear_vel, approach_vel); - } + applyApproachVelocityScaling(path, linear_vel); // Limit linear velocities to be valid linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 9f65a8554f..83d814dda0 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -93,12 +93,11 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure } void applyConstraintsWrapper( - const double & dist_error, const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & curr_speed, - const double & pose_cost, double & linear_vel, double & sign) + const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) { return applyConstraints( - dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + curvature, curr_speed, pose_cost, path, linear_vel, sign); } @@ -478,10 +477,20 @@ TEST(RegulatedPurePursuitTest, applyConstraints) auto costmap = std::make_shared("fake_costmap"); rclcpp_lifecycle::State state; costmap->on_configure(state); + + constexpr double approach_velocity_scaling_dist = 0.6; + nav2_util::declare_parameter_if_not_declared( + node, + name + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(approach_velocity_scaling_dist)); + ctrl->configure(node, name, tf, costmap); - double dist_error = 0.0; - double lookahead_dist = 0.6; + auto no_approach_path = path_utils::generate_path( + geometry_msgs::msg::PoseStamped(), 0.1, { + std::make_unique(approach_velocity_scaling_dist + 1.0) + }); + double curvature = 0.5; geometry_msgs::msg::Twist curr_speed; double pose_cost = 0.0; @@ -491,7 +500,7 @@ TEST(RegulatedPurePursuitTest, applyConstraints) // test curvature regulation (default) curr_speed.linear.x = 0.25; ctrl->applyConstraintsWrapper( - dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + curvature, curr_speed, pose_cost, no_approach_path, linear_vel, sign); EXPECT_EQ(linear_vel, 0.25); // min set speed @@ -499,7 +508,7 @@ TEST(RegulatedPurePursuitTest, applyConstraints) curvature = 0.7407; curr_speed.linear.x = 0.5; ctrl->applyConstraintsWrapper( - dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + curvature, curr_speed, pose_cost, no_approach_path, linear_vel, sign); EXPECT_NEAR(linear_vel, 0.5, 0.01); // lower by curvature @@ -507,10 +516,23 @@ TEST(RegulatedPurePursuitTest, applyConstraints) curvature = 1000.0; curr_speed.linear.x = 0.25; ctrl->applyConstraintsWrapper( - dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + curvature, curr_speed, pose_cost, no_approach_path, linear_vel, sign); EXPECT_NEAR(linear_vel, 0.25, 0.01); // min out by curvature + // Approach velocity scaling on a path with no distance left + auto approach_path = path_utils::generate_path( + geometry_msgs::msg::PoseStamped(), 0.1, { + std::make_unique(0.0) + }); + + linear_vel = 1.0; + curvature = 0.0; + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper( + curvature, curr_speed, pose_cost, approach_path, + linear_vel, sign); + EXPECT_NEAR(linear_vel, 0.05, 0.01); // min out on min approach velocity // now try with cost regulation (turn off velocity and only cost) // ctrl->setCostRegulationScaling(); From ee06ded3b04138dac10e67a079aaf81d2939d0f6 Mon Sep 17 00:00:00 2001 From: Samuel Lindgren Date: Fri, 22 Jul 2022 20:18:07 +0200 Subject: [PATCH 029/131] Use correct timeout when checking future (#3087) --- .../include/nav2_behavior_tree/bt_service_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 636f24415c..c1970e38b1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -166,7 +166,7 @@ class BtServiceNode : public BT::ActionNodeBase auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; rclcpp::FutureReturnCode rc; - rc = callback_group_executor_.spin_until_future_complete(future_result_, server_timeout_); + rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout); if (rc == rclcpp::FutureReturnCode::SUCCESS) { request_sent_ = false; BT::NodeStatus status = on_completion(future_result_.get()); From 5cba0274376dab3311921fb2158ee16c443d548f Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 18 Aug 2022 06:38:41 +0800 Subject: [PATCH 030/131] Adds missing commas so default plugin names are not stuck together (#3093) Signed-off-by: Aaron Chong Signed-off-by: Aaron Chong --- nav2_bt_navigator/src/bt_navigator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 79b50992b4..5002c6f264 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -73,10 +73,10 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_controller_selector_bt_node", "nav2_goal_checker_selector_bt_node", "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node" + "nav2_path_longer_on_approach_bt_node", "nav2_wait_cancel_bt_node", "nav2_spin_cancel_bt_node", - "nav2_back_up_cancel_bt_node" + "nav2_back_up_cancel_bt_node", "nav2_drive_on_heading_cancel_bt_node" }; From 232d6659fe43fff5bf2b7835f1e4f31d9ceba369 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 18 Aug 2022 10:58:15 -0700 Subject: [PATCH 031/131] Fix Costmap Filters system tests (#3120) * Fix Costmap Filters system tests * Update map_io.cpp Co-authored-by: Alexey Merzlyakov --- nav2_map_server/src/map_io.cpp | 2 +- nav2_system_tests/src/costmap_filters/keepout_params.yaml | 1 + nav2_system_tests/src/costmap_filters/speed_global_params.yaml | 2 +- nav2_system_tests/src/costmap_filters/speed_local_params.yaml | 2 +- nav2_system_tests/src/costmap_filters/test_keepout_launch.py | 2 +- nav2_system_tests/src/costmap_filters/test_speed_launch.py | 2 +- 6 files changed, 6 insertions(+), 5 deletions(-) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 050fc63d66..4803b6d3f6 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -504,7 +504,7 @@ void tryWriteMapToFile( tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); double yaw, pitch, roll; mat.getEulerYPR(yaw, pitch, roll); - + const int file_name_index = mapdatafile.find_last_of("/\\"); std::string image_name = mapdatafile.substr(file_name_index + 1); diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index b4f98cedea..22b3b14e6b 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -124,6 +124,7 @@ controller_server: FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True + prune_distance: 1.0 min_vel_x: 0.0 min_vel_y: 0.0 max_vel_x: 0.26 diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 1e021bb4f7..ff0de1abda 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -248,7 +248,7 @@ map_server: map_saver: ros__parameters: use_sim_time: True - save_map_timeout: 5000 + save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: True diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index fe03fe138d..a0a2024abb 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -248,7 +248,7 @@ map_server: map_saver: ros__parameters: use_sim_time: True - save_map_timeout: 5000 + save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: True diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index d8ad112392..796667c65d 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -89,7 +89,7 @@ def generate_launch_description(): parameters=[{ 'node_names': [ - 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + 'filter_mask_server', 'costmap_filter_info_server' ] }, {'autostart': True}]), diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 2603ea0df1..5b0c61ed9d 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -87,7 +87,7 @@ def generate_launch_description(): parameters=[{ 'node_names': [ - 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + 'filter_mask_server', 'costmap_filter_info_server' ] }, {'autostart': True}]), From 289a6860658d4e5ffe39e2d43fe07488700d20a5 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 18 Aug 2022 13:23:58 -0700 Subject: [PATCH 032/131] Finding distance H if obstacle H is <= 0 (#3122) --- nav2_smac_planner/src/node_hybrid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index d75c11ffd3..f23b769e6b 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -595,7 +595,7 @@ float NodeHybrid::getDistanceHeuristic( y_pos * motion_table.num_angle_quantization + theta_pos; motion_heuristic = dist_heuristic_lookup_table[index]; - } else if (obstacle_heuristic == 0.0) { + } else if (obstacle_heuristic <= 0.0) { // If no obstacle heuristic value, must have some H to use // In nominal situations, this should never be called. static ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); From e1830c929a603f8969a633818091c8655ddee707 Mon Sep 17 00:00:00 2001 From: autome Date: Fri, 19 Aug 2022 09:55:54 +0900 Subject: [PATCH 033/131] Removed additional sign multiplication (#3088) --- .../src/regulated_pure_pursuit_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index c31660b6a9..d8721c646f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -341,7 +341,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity linear_vel, sign); // Apply curvature to angular velocity after constraining linear velocity - angular_vel = sign * linear_vel * curvature; + angular_vel = linear_vel * curvature; } // Collision checking on this velocity heading From aab082e6e96a76e0ffa7aed3f7de76621117418b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 19 Aug 2022 15:53:18 -0700 Subject: [PATCH 034/131] adding checks on goal IDs in results for waypoint follower (#3130) --- nav2_util/include/nav2_util/simple_action_server.hpp | 12 ++++++++++++ nav2_waypoint_follower/src/waypoint_follower.cpp | 7 +++++++ 2 files changed, 19 insertions(+) diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 40f1a93860..f05a4602ac 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -390,6 +390,18 @@ class SimpleActionServer return current_handle_->get_goal(); } + const rclcpp_action::GoalUUID get_current_goal_id() const + { + std::lock_guard lock(update_mutex_); + + if (!is_active(current_handle_)) { + error_msg("A goal is not available or has reached a final state"); + return rclcpp_action::GoalUUID(); + } + + return current_handle_->get_goal_id(); + } + /** * @brief Get the pending goal object * @return Goal Ptr to the goal that's pending diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 804de9ad31..5de0a1a3f1 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -286,6 +286,13 @@ void WaypointFollower::resultCallback( const rclcpp_action::ClientGoalHandle::WrappedResult & result) { + if (result.goal_id != future_goal_handle_.get()->get_goal_id()) { + RCLCPP_DEBUG( + get_logger(), + "Goal IDs do not match for the current goal handle and received result." + "Ignoring likely due to receiving result for an old goal."); + } + switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: current_goal_status_ = ActionStatus::SUCCEEDED; From c9bab4df57a2aa2a7a26fcd01b718557db79ed20 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 22 Aug 2022 09:07:58 -0700 Subject: [PATCH 035/131] Update nav2_multirobot_params_1.yaml --- nav2_bringup/params/nav2_multirobot_params_1.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index a35fe9a870..621d93a435 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -113,7 +113,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugins: "goal_checker" + goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] # Progress checker parameters From 72b5348fbda10d3f5294badc4c9176df11b48dd3 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 22 Aug 2022 09:08:09 -0700 Subject: [PATCH 036/131] Update nav2_multirobot_params_2.yaml --- nav2_bringup/params/nav2_multirobot_params_2.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 618ba12bdf..d6c6280768 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -113,7 +113,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugins: "goal_checker" + goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] # Progress checker parameters From 42b5bc863c2b1caa5a2edf1387fb50cdd7615907 Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Mon, 22 Aug 2022 12:26:56 -0400 Subject: [PATCH 037/131] ComputePathToPose Sets empty path on blackboard if action is aborted or cancelled (#3114) * set empty path on black on failure * docs * switched to correct message type * set empty path for compute_path_through_poses --- .../action/compute_path_through_poses_action.hpp | 10 ++++++++++ .../plugins/action/compute_path_to_pose_action.hpp | 10 ++++++++++ .../action/compute_path_through_poses_action.cpp | 12 ++++++++++++ .../plugins/action/compute_path_to_pose_action.cpp | 14 ++++++++++++++ 4 files changed, 46 insertions(+) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index f9f233c233..f7610696f8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -54,6 +54,16 @@ class ComputePathThroughPosesAction */ BT::NodeStatus on_success() override; + /** + * @brief Function to perform some user-defined operation upon abortion of the action + */ + BT::NodeStatus on_aborted() override; + + /** + * @brief Function to perform some user-defined operation upon cancelation of the action + */ + BT::NodeStatus on_cancelled() override; + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports 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 105b52fbf6..f87435995c 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 @@ -51,6 +51,16 @@ class ComputePathToPoseAction : public BtActionNode Date: Mon, 22 Aug 2022 14:57:04 -0500 Subject: [PATCH 038/131] Ignore feedback from old goals in waypoint follower (#3139) --- nav2_waypoint_follower/src/waypoint_follower.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 5de0a1a3f1..58aafef6a3 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -291,6 +291,7 @@ WaypointFollower::resultCallback( get_logger(), "Goal IDs do not match for the current goal handle and received result." "Ignoring likely due to receiving result for an old goal."); + return; } switch (result.code) { From 6fdcfc12a8a027cc8dd6a83fd4811427c16de751 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=EC=A0=95=EC=B0=AC=ED=9D=AC?= <60467877+ladianchad@users.noreply.github.com> Date: Wed, 24 Aug 2022 01:15:15 +0900 Subject: [PATCH 039/131] apply joinchar in pathify (#3141) Co-authored-by: kevin --- nav2_common/nav2_common/launch/rewritten_yaml.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 4e08efbf9d..63d3905d5e 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -157,7 +157,7 @@ def pathify(self, d, p=None, paths=None, joinchar='.'): return paths pn = p if p != "": - pn += '.' + pn += joinchar if isinstance(d, dict): for k in d: v = d[k] From d4886a75d90e250f7f3ca1c2a46448c9f1fff16c Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Tue, 23 Aug 2022 12:26:59 -0400 Subject: [PATCH 040/131] Log level (#3110) * added log level for navigation launch * localization log level * slam log level * revert use_comp * added log level to components --- nav2_bringup/launch/bringup_launch.py | 7 +++++++ nav2_bringup/launch/localization_launch.py | 9 +++++++++ nav2_bringup/launch/navigation_launch.py | 15 ++++++++++++++- nav2_bringup/launch/slam_launch.py | 8 ++++++++ 4 files changed, 38 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index bd47147227..4377f38e20 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -42,6 +42,7 @@ def generate_launch_description(): autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative @@ -107,6 +108,10 @@ def generate_launch_description(): 'use_respawn', default_value='False', description='Whether to respawn if a node crashes. Applied when composition is disabled.') + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace( @@ -119,6 +124,7 @@ def generate_launch_description(): package='rclcpp_components', executable='component_container_isolated', parameters=[configured_params, {'autostart': autostart}], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings, output='screen'), @@ -171,6 +177,7 @@ def generate_launch_description(): ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_composition_cmd) ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 218debd918..9eee3f06a9 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -38,6 +38,7 @@ def generate_launch_description(): use_composition = LaunchConfiguration('use_composition') container_name = LaunchConfiguration('container_name') use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') lifecycle_nodes = ['map_server', 'amcl'] @@ -99,6 +100,10 @@ def generate_launch_description(): 'use_respawn', default_value='False', description='Whether to respawn if a node crashes. Applied when composition is disabled.') + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ @@ -110,6 +115,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_amcl', @@ -119,12 +125,14 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', output='screen', + arguments=['--ros-args', '--log-level', log_level], parameters=[{'use_sim_time': use_sim_time}, {'autostart': autostart}, {'node_names': lifecycle_nodes}]) @@ -172,6 +180,7 @@ def generate_launch_description(): ld.add_action(declare_use_composition_cmd) ld.add_action(declare_container_name_cmd) ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) # Add the actions to launch all of the localiztion nodes ld.add_action(load_nodes) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index fa1be218f2..bcc16a6719 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -37,6 +37,7 @@ def generate_launch_description(): use_composition = LaunchConfiguration('use_composition') container_name = LaunchConfiguration('container_name') use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') lifecycle_nodes = ['controller_server', 'smoother_server', @@ -100,6 +101,10 @@ def generate_launch_description(): 'use_respawn', default_value='False', description='Whether to respawn if a node crashes. Applied when composition is disabled.') + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ @@ -110,6 +115,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), Node( package='nav2_smoother', @@ -119,6 +125,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_planner', @@ -128,6 +135,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_behaviors', @@ -137,6 +145,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_bt_navigator', @@ -146,6 +155,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_waypoint_follower', @@ -155,6 +165,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_velocity_smoother', @@ -164,6 +175,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), Node( @@ -171,6 +183,7 @@ def generate_launch_description(): executable='lifecycle_manager', name='lifecycle_manager_navigation', output='screen', + arguments=['--ros-args', '--log-level', log_level], parameters=[{'use_sim_time': use_sim_time}, {'autostart': autostart}, {'node_names': lifecycle_nodes}]), @@ -248,7 +261,7 @@ def generate_launch_description(): ld.add_action(declare_use_composition_cmd) ld.add_action(declare_container_name_cmd) ld.add_action(declare_use_respawn_cmd) - + ld.add_action(declare_log_level_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(load_nodes) ld.add_action(load_composable_nodes) diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index 6c3b1c1bd3..944b6d1fe1 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -32,6 +32,7 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') autostart = LaunchConfiguration('autostart') use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') # Variables lifecycle_nodes = ['map_saver'] @@ -75,6 +76,10 @@ def generate_launch_description(): 'use_respawn', default_value='False', description='Whether to respawn if a node crashes. Applied when composition is disabled.') + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + # Nodes launching commands start_map_saver_server_cmd = Node( @@ -83,6 +88,7 @@ def generate_launch_description(): output='screen', respawn=use_respawn, respawn_delay=2.0, + arguments=['--ros-args', '--log-level', log_level], parameters=[configured_params]) start_lifecycle_manager_cmd = Node( @@ -90,6 +96,7 @@ def generate_launch_description(): executable='lifecycle_manager', name='lifecycle_manager_slam', output='screen', + arguments=['--ros-args', '--log-level', log_level], parameters=[{'use_sim_time': use_sim_time}, {'autostart': autostart}, {'node_names': lifecycle_nodes}]) @@ -119,6 +126,7 @@ def generate_launch_description(): ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) # Running Map Saver Server ld.add_action(start_map_saver_server_cmd) From 676fe71a19e7f38e9635262a1fd2c5ce429ccc29 Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Tue, 23 Aug 2022 12:40:01 -0400 Subject: [PATCH 041/131] linting and uncrusitfy fixes for CI (#3144) --- .../plugins/action/compute_path_through_poses_action.cpp | 6 ++++-- .../include/nav2_collision_monitor/circle.hpp | 4 ++++ .../nav2_collision_monitor/collision_monitor_node.hpp | 2 +- .../include/nav2_collision_monitor/pointcloud.hpp | 4 ++++ .../include/nav2_collision_monitor/scan.hpp | 4 ++++ nav2_costmap_2d/plugins/voxel_layer.cpp | 3 ++- nav2_waypoint_follower/src/waypoint_follower.cpp | 2 +- 7 files changed, 20 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index 7dc5a6f2ba..c7421d5599 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -44,13 +44,15 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success() return BT::NodeStatus::SUCCESS; } -BT::NodeStatus ComputePathThroughPosesAction::on_aborted() { +BT::NodeStatus ComputePathThroughPosesAction::on_aborted() +{ nav_msgs::msg::Path empty_path; setOutput("path", empty_path); return BT::NodeStatus::FAILURE; } -BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() { +BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() +{ nav_msgs::msg::Path empty_path; setOutput("path", empty_path); return BT::NodeStatus::SUCCESS; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index ec04974ee5..bd902c1ea4 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -15,6 +15,10 @@ #ifndef NAV2_COLLISION_MONITOR__CIRCLE_HPP_ #define NAV2_COLLISION_MONITOR__CIRCLE_HPP_ +#include +#include +#include + #include "nav2_collision_monitor/polygon.hpp" namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 8d4836e980..2de3c22246 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -48,7 +48,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @brief Constructor for the nav2_collision_safery::CollisionMonitor * @param options Additional options to control creation of the node. */ - CollisionMonitor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + explicit CollisionMonitor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Destructor for the nav2_collision_safery::CollisionMonitor */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index 8ccdd310be..db3d5e1135 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -15,6 +15,10 @@ #ifndef NAV2_COLLISION_MONITOR__POINTCLOUD_HPP_ #define NAV2_COLLISION_MONITOR__POINTCLOUD_HPP_ +#include +#include +#include + #include "sensor_msgs/msg/point_cloud2.hpp" #include "nav2_collision_monitor/source.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index f47f1bebfc..f96ae211f8 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -15,6 +15,10 @@ #ifndef NAV2_COLLISION_MONITOR__SCAN_HPP_ #define NAV2_COLLISION_MONITOR__SCAN_HPP_ +#include +#include +#include + #include "sensor_msgs/msg/laser_scan.hpp" #include "nav2_collision_monitor/source.hpp" diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 149dfd6ede..f8c62d06b9 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -275,7 +275,8 @@ void VoxelLayer::raytraceFreespace( if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { RCLCPP_WARN( logger_, - "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). " + "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds" + "(%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). " "The costmap cannot raytrace for it.", ox, oy, oz, ox + getSizeInMetersX(), oy + getSizeInMetersY(), oz + getSizeInMetersZ(), diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 58aafef6a3..a04c404c77 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -291,7 +291,7 @@ WaypointFollower::resultCallback( get_logger(), "Goal IDs do not match for the current goal handle and received result." "Ignoring likely due to receiving result for an old goal."); - return; + return; } switch (result.code) { From 34524ed1ce57c496dd85f87e41e6b958fb6ed74e Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Tue, 23 Aug 2022 17:58:01 -0400 Subject: [PATCH 042/131] start is now added to the path (#3140) * start is now added to the path * lint fix --- nav2_smac_planner/src/node_2d.cpp | 4 ++-- nav2_smac_planner/src/node_hybrid.cpp | 4 ++-- nav2_smac_planner/src/node_lattice.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 442fb62b68..862b1feca6 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -154,11 +154,11 @@ bool Node2D::backtracePath(CoordinateVector & path) NodePtr current_node = this; - while (current_node->parent) { + do { path.push_back( Node2D::getCoords(current_node->getIndex())); current_node = current_node->parent; - } + } while (current_node->parent); return path.size() > 0; } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index f23b769e6b..3ccc17ad0b 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -705,12 +705,12 @@ bool NodeHybrid::backtracePath(CoordinateVector & path) NodePtr current_node = this; - while (current_node->parent) { + do { path.push_back(current_node->pose); // Convert angle to radians path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); current_node = current_node->parent; - } + } while (current_node->parent); return path.size() > 0; } diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index 7e0a917f48..ee61a6df2c 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -547,7 +547,7 @@ bool NodeLattice::backtracePath(CoordinateVector & path) const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution; const float pi_2 = 2.0 * M_PI; - while (current_node->parent) { + do { prim = current_node->getMotionPrimitive(); // if motion primitive is valid, then was searched (rather than analytically expanded), // include dense path of subpoints making up the primitive at grid resolution @@ -576,7 +576,7 @@ bool NodeLattice::backtracePath(CoordinateVector & path) } current_node = current_node->parent; - } + } while (current_node->parent); return path.size() > 0; } From 11285eafe68c37a2dd35bbcef8c85af471246a77 Mon Sep 17 00:00:00 2001 From: Austin Greisman <92941098+austin-InDro@users.noreply.github.com> Date: Wed, 24 Aug 2022 11:55:32 -0400 Subject: [PATCH 043/131] Update README.md (#3147) Current example doesn't work with the updates. --- nav2_smac_planner/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 0f58190b69..be522d7ca4 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -103,7 +103,7 @@ planner_server: use_sim_time: True GridBased: - plugin: "nav2_smac_planner/SmacPlanner" + plugin: "nav2_smac_planner/SmacPlannerHybrid" tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node downsample_costmap: false # whether or not to downsample the map downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) @@ -123,7 +123,7 @@ planner_server: cost_penalty: 2.0 # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0 rotation_penalty: 5.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings. - lookup_table_size: 20 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters. + lookup_table_size: 20.0 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters. cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. From 074e3e16963a4d985358b94f1564733e35466e2a Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 24 Aug 2022 13:56:12 -0700 Subject: [PATCH 044/131] fixing linting problem --- nav2_bringup/launch/navigation_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index bcc16a6719..e33506f82a 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -177,7 +177,7 @@ def generate_launch_description(): parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', From 7a6b11d9f145523896e64dbf8a44ce332bf28210 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 24 Aug 2022 14:41:35 -0700 Subject: [PATCH 045/131] Setting map map's yaml path to empty string, since overridden in launch (#3123) * Update nav2_params.yaml * Update nav2_params.yaml * Update nav2_params.yaml --- nav2_bringup/params/nav2_params.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 41fc36fb64..f45215a21a 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -257,7 +257,9 @@ global_costmap: map_server: ros__parameters: use_sim_time: True - yaml_filename: "turtlebot3_world.yaml" + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" map_saver: ros__parameters: From 344ac1c28de8cc56d46b50afbf8c37d4beab49cb Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 25 Aug 2022 17:13:08 +0200 Subject: [PATCH 046/131] use_sim_time refactoring (#3131) * initial commit * fix * fixes * revert * fix * linting * fixes * fix format * fixing pycodestyle * revert typo * add use_sim_time to costmap * add parameter description * fix defaults * fix formatting --- .../bt_action_server_impl.hpp | 3 + nav2_bringup/launch/bringup_launch.py | 1 - nav2_bringup/launch/localization_launch.py | 57 ++++---- .../launch/multi_tb3_simulation_launch.py | 5 +- nav2_bringup/launch/navigation_launch.py | 127 +++++++++--------- nav2_bringup/launch/rviz_launch.py | 4 +- nav2_bringup/launch/slam_launch.py | 50 ++++--- .../params/nav2_multirobot_params_1.yaml | 24 ---- .../params/nav2_multirobot_params_2.yaml | 24 ---- nav2_bringup/params/nav2_params.yaml | 24 ---- .../params/collision_monitor_params.yaml | 1 - nav2_constrained_smoother/README.md | 1 - nav2_controller/src/controller_server.cpp | 4 +- .../nav2_costmap_2d/costmap_2d_ros.hpp | 7 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 10 +- nav2_planner/src/planner_server.cpp | 4 +- .../README.md | 1 - nav2_rotation_shim_controller/README.md | 1 - nav2_smac_planner/README.md | 1 - .../src/costmap_filters/keepout_params.yaml | 27 ---- .../costmap_filters/speed_global_params.yaml | 27 ---- .../costmap_filters/speed_local_params.yaml | 27 ---- nav2_theta_star_planner/README.md | 1 - 23 files changed, 141 insertions(+), 290 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 592d1257a2..db619e1a70 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -82,6 +82,9 @@ bool BtActionServer::on_configure() "-r", std::string("__node:=") + std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node", + "-p", + "use_sim_time:=" + + std::string(node->get_parameter("use_sim_time").as_bool() ? "true" : "false"), "--"}); // Support for handling the topic-based goal pose from rviz diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 4377f38e20..065337adb8 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -55,7 +55,6 @@ def generate_launch_description(): # Create our own temporary YAML files that include substitutions param_substitutions = { - 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} configured_params = RewrittenYaml( diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 9eee3f06a9..dca5bf23e4 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -20,7 +20,7 @@ from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -53,7 +53,6 @@ def generate_launch_description(): # Create our own temporary YAML files that include substitutions param_substitutions = { - 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} configured_params = RewrittenYaml( @@ -107,6 +106,7 @@ def generate_launch_description(): load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ + SetParameter("use_sim_time", use_sim_time), Node( package='nav2_map_server', executable='map_server', @@ -133,36 +133,39 @@ def generate_launch_description(): name='lifecycle_manager_localization', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}]) ] ) - load_composable_nodes = LoadComposableNodes( + load_composable_nodes = GroupAction( condition=IfCondition(use_composition), - target_container=container_name, - composable_node_descriptions=[ - ComposableNode( - package='nav2_map_server', - plugin='nav2_map_server::MapServer', - name='map_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_amcl', - plugin='nav2_amcl::AmclNode', - name='amcl', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_localization', - parameters=[{'use_sim_time': use_sim_time, - 'autostart': autostart, - 'node_names': lifecycle_nodes}]), - ], + actions=[ + SetParameter("use_sim_time", use_sim_time), + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_amcl', + plugin='nav2_amcl::AmclNode', + name='amcl', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_localization', + parameters=[{'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + ] ) # Create the launch description and populate diff --git a/nav2_bringup/launch/multi_tb3_simulation_launch.py b/nav2_bringup/launch/multi_tb3_simulation_launch.py index d91c5ce063..069ae5bb3d 100644 --- a/nav2_bringup/launch/multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/multi_tb3_simulation_launch.py @@ -116,10 +116,9 @@ def generate_launch_description(): group = GroupAction([ IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py')), + os.path.join(launch_dir, 'rviz_launch.py')), condition=IfCondition(use_rviz), - launch_arguments={ - 'namespace': TextSubstitution(text=robot['name']), + launch_arguments={'namespace': TextSubstitution(text=robot['name']), 'use_namespace': 'True', 'rviz_config': rviz_config_file}.items()), diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index e33506f82a..c548dbf95d 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -20,7 +20,7 @@ from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -58,14 +58,13 @@ def generate_launch_description(): # Create our own temporary YAML files that include substitutions param_substitutions = { - 'use_sim_time': use_sim_time, 'autostart': autostart} configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') @@ -108,6 +107,7 @@ def generate_launch_description(): load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ + SetParameter("use_sim_time", use_sim_time), Node( package='nav2_controller', executable='controller_server', @@ -184,67 +184,70 @@ def generate_launch_description(): name='lifecycle_manager_navigation', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}]), ] ) - load_composable_nodes = LoadComposableNodes( + load_composable_nodes = GroupAction( condition=IfCondition(use_composition), - target_container=container_name, - composable_node_descriptions=[ - ComposableNode( - package='nav2_controller', - plugin='nav2_controller::ControllerServer', - name='controller_server', - parameters=[configured_params], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), - ComposableNode( - package='nav2_smoother', - plugin='nav2_smoother::SmootherServer', - name='smoother_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_planner', - plugin='nav2_planner::PlannerServer', - name='planner_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_behaviors', - plugin='behavior_server::BehaviorServer', - name='behavior_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_bt_navigator', - plugin='nav2_bt_navigator::BtNavigator', - name='bt_navigator', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_waypoint_follower', - plugin='nav2_waypoint_follower::WaypointFollower', - name='waypoint_follower', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_velocity_smoother', - plugin='nav2_velocity_smoother::VelocitySmoother', - name='velocity_smoother', - parameters=[configured_params], - remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_navigation', - parameters=[{'use_sim_time': use_sim_time, - 'autostart': autostart, - 'node_names': lifecycle_nodes}]), - ], + actions=[ + SetParameter("use_sim_time", use_sim_time), + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + ] ) # Create the launch description and populate diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 9f123b81eb..9454441558 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -61,8 +61,8 @@ def generate_launch_description(): output='screen') namespaced_rviz_config_file = ReplaceString( - source_file=rviz_config_file, - replacements={'': ('/', namespace)}) + source_file=rviz_config_file, + replacements={'': ('/', namespace)}) start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index 944b6d1fe1..6621778425 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -17,11 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter from nav2_common.launch import HasNodeParams, RewrittenYaml @@ -43,13 +43,10 @@ def generate_launch_description(): slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'online_sync_launch.py') # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time} - configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites=param_substitutions, + param_rewrites={}, convert_types=True) # Declare the launch arguments @@ -82,24 +79,26 @@ def generate_launch_description(): # Nodes launching commands - start_map_saver_server_cmd = Node( - package='nav2_map_server', - executable='map_saver_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - arguments=['--ros-args', '--log-level', log_level], - parameters=[configured_params]) - - start_lifecycle_manager_cmd = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_slam', - output='screen', - arguments=['--ros-args', '--log-level', log_level], - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]) + start_map_server = GroupAction( + actions=[ + SetParameter("use_sim_time", use_sim_time), + Node( + package='nav2_map_server', + executable='map_saver_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + arguments=['--ros-args', '--log-level', log_level], + parameters=[configured_params]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_slam', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ]) # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load @@ -129,8 +128,7 @@ def generate_launch_description(): ld.add_action(declare_log_level_cmd) # Running Map Saver Server - ld.add_action(start_map_saver_server_cmd) - ld.add_action(start_lifecycle_manager_cmd) + ld.add_action(start_map_server) # Running SLAM Toolbox (Only one of them will be run) ld.add_action(start_slam_toolbox_cmd) diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 621d93a435..11e5502246 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -1,6 +1,5 @@ amcl: ros__parameters: - use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -41,7 +40,6 @@ amcl: bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom @@ -97,17 +95,8 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True - controller_server: ros__parameters: - use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 @@ -170,7 +159,6 @@ controller_server: local_costmap: local_costmap: ros__parameters: - use_sim_time: True global_frame: odom rolling_window: true width: 3 @@ -202,7 +190,6 @@ local_costmap: global_costmap: global_costmap: ros__parameters: - use_sim_time: True robot_radius: 0.22 obstacle_layer: enabled: True @@ -222,13 +209,11 @@ global_costmap: map_server: ros__parameters: - use_sim_time: True yaml_filename: "turtlebot3_world.yaml" save_map_timeout: 5.0 planner_server: ros__parameters: - use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -236,10 +221,6 @@ planner_server: use_astar: false allow_unknown: true -smoother_server: - ros__parameters: - use_sim_time: True - behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw @@ -257,16 +238,11 @@ behavior_server: global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: loop_rate: 20 diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index d6c6280768..7e2b3551c8 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -1,6 +1,5 @@ amcl: ros__parameters: - use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -41,7 +40,6 @@ amcl: bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom @@ -97,17 +95,8 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True - controller_server: ros__parameters: - use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 @@ -170,7 +159,6 @@ controller_server: local_costmap: local_costmap: ros__parameters: - use_sim_time: True global_frame: odom rolling_window: true width: 3 @@ -202,7 +190,6 @@ local_costmap: global_costmap: global_costmap: ros__parameters: - use_sim_time: True robot_radius: 0.22 obstacle_layer: enabled: True @@ -222,13 +209,11 @@ global_costmap: map_server: ros__parameters: - use_sim_time: True yaml_filename: "turtlebot3_world.yaml" save_map_timeout: 5.0 planner_server: ros__parameters: - use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -236,10 +221,6 @@ planner_server: use_astar: false allow_unknown: true -smoother_server: - ros__parameters: - use_sim_time: True - behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw @@ -257,16 +238,11 @@ behavior_server: global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: loop_rate: 20 diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index f45215a21a..c97ed6a2e1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -1,6 +1,5 @@ amcl: ros__parameters: - use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -41,7 +40,6 @@ amcl: bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom @@ -97,17 +95,8 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True - controller_server: ros__parameters: - use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 @@ -183,7 +172,6 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -226,7 +214,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -256,14 +243,12 @@ global_costmap: map_server: ros__parameters: - use_sim_time: True # Overridden in launch by the "map" launch configuration or provided default value. # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. yaml_filename: "" map_saver: ros__parameters: - use_sim_time: True save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -272,7 +257,6 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -283,7 +267,6 @@ planner_server: smoother_server: ros__parameters: - use_sim_time: True smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -308,19 +291,13 @@ behavior_server: global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: - use_sim_time: True loop_rate: 20 stop_on_failure: false waypoint_task_executor_plugin: "wait_at_waypoint" @@ -331,7 +308,6 @@ waypoint_follower: velocity_smoother: ros__parameters: - use_sim_time: True smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index f0fb4ef5dd..4e5f90ae80 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -1,6 +1,5 @@ collision_monitor: ros__parameters: - use_sim_time: True base_frame_id: "base_footprint" odom_frame_id: "odom" cmd_vel_in_topic: "cmd_vel_raw" diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md index 8359e5d3d7..d37eb51b63 100644 --- a/nav2_constrained_smoother/README.md +++ b/nav2_constrained_smoother/README.md @@ -10,7 +10,6 @@ Example of configuration (see indoor_navigation package of this repo for a full ``` smoother_server: ros__parameters: - use_sim_time: True smoother_plugins: ["SmoothPath"] SmoothPath: diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 3f4e71cc1c..2d017b1186 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -62,8 +62,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( - "local_costmap", std::string{get_namespace()}, "local_costmap"); - + "local_costmap", std::string{get_namespace()}, "local_costmap", + get_parameter("use_sim_time").as_bool()); // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); } 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 a5dfc6182a..5b469d42f2 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 @@ -76,19 +76,22 @@ class Costmap2DROS : public nav2_util::LifecycleNode * @brief Constructor for the wrapper, the node will * be placed in a namespace equal to the node's name * @param name Name of the costmap ROS node + * @param use_sim_time Whether to use simulation or real time */ - explicit Costmap2DROS(const std::string & name); + explicit Costmap2DROS(const std::string & name, const bool & use_sim_time = false); /** * @brief Constructor for the wrapper * @param name Name of the costmap ROS node * @param parent_namespace Absolute namespace of the node hosting the costmap node * @param local_namespace Namespace to append to the parent namespace + * @param use_sim_time Whether to use simulation or real time */ explicit Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const std::string & local_namespace); + const std::string & local_namespace, + const bool & use_sim_time); /** * @brief A destructor diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 12d6b2ee68..82aa604d8f 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -58,13 +58,14 @@ using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { -Costmap2DROS::Costmap2DROS(const std::string & name) -: Costmap2DROS(name, "/", name) {} +Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) +: Costmap2DROS(name, "/", name, use_sim_time) {} Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const std::string & local_namespace) + const std::string & local_namespace, + const bool & use_sim_time) : nav2_util::LifecycleNode(name, "", // NodeOption arguments take precedence over the ones provided on the command line // use this to make sure the node is placed on the provided namespace @@ -73,7 +74,8 @@ Costmap2DROS::Costmap2DROS( rclcpp::NodeOptions().arguments({ "--ros-args", "-r", std::string("__ns:=") + nav2_util::add_namespaces(parent_namespace, local_namespace), - "--ros-args", "-r", name + ":" + std::string("__node:=") + name + "--ros-args", "-r", name + ":" + std::string("__node:=") + name, + "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"), })), name_(name), parent_namespace_(parent_namespace), diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6a55fc06a9..e2bc6286a7 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -68,8 +68,8 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Setup the global costmap costmap_ros_ = std::make_shared( - "global_costmap", std::string{get_namespace()}, "global_costmap"); - + "global_costmap", std::string{get_namespace()}, "global_costmap", + get_parameter("use_sim_time").as_bool()); // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); } diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 379e2a56f2..4b929bd700 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -81,7 +81,6 @@ Example fully-described XML with default parameter values: ``` controller_server: ros__parameters: - use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 15185bfa96..ced9c94cb4 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -41,7 +41,6 @@ Example fully-described XML with default parameter values: ``` controller_server: ros__parameters: - use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index be522d7ca4..5a89a608a4 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -100,7 +100,6 @@ See inline description of parameters in the `SmacPlanner`. This includes comment planner_server: ros__parameters: planner_plugins: ["GridBased"] - use_sim_time: True GridBased: plugin: "nav2_smac_planner/SmacPlannerHybrid" diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 22b3b14e6b..d886ca3e4e 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -1,6 +1,5 @@ amcl: ros__parameters: - use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -41,7 +40,6 @@ amcl: bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom @@ -90,17 +88,8 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True - controller_server: ros__parameters: - use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 @@ -172,7 +161,6 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -219,7 +207,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -250,12 +237,10 @@ global_costmap: map_server: ros__parameters: - use_sim_time: True yaml_filename: "turtlebot3_world.yaml" map_saver: ros__parameters: - use_sim_time: True save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -264,7 +249,6 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -272,10 +256,6 @@ planner_server: use_astar: False allow_unknown: True -smoother_server: - ros__parameters: - use_sim_time: True - behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw @@ -293,16 +273,11 @@ behavior_server: global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: loop_rate: 20 @@ -315,7 +290,6 @@ waypoint_follower: costmap_filter_info_server: ros__parameters: - use_sim_time: true type: 0 filter_info_topic: "/costmap_filter_info" mask_topic: "/filter_mask" @@ -324,7 +298,6 @@ costmap_filter_info_server: filter_mask_server: ros__parameters: - use_sim_time: true frame_id: "map" topic_name: "/filter_mask" yaml_filename: "keepout_mask.yaml" diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index ff0de1abda..ba5eede1ad 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -1,6 +1,5 @@ amcl: ros__parameters: - use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -41,7 +40,6 @@ amcl: bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom @@ -91,17 +89,8 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True - controller_server: ros__parameters: - use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 @@ -172,7 +161,6 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -210,7 +198,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -242,12 +229,10 @@ global_costmap: map_server: ros__parameters: - use_sim_time: True yaml_filename: "turtlebot3_world.yaml" map_saver: ros__parameters: - use_sim_time: True save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -256,7 +241,6 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -264,10 +248,6 @@ planner_server: use_astar: False allow_unknown: True -smoother_server: - ros__parameters: - use_sim_time: True - behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw @@ -285,16 +265,11 @@ behavior_server: global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: loop_rate: 20 @@ -307,7 +282,6 @@ waypoint_follower: costmap_filter_info_server: ros__parameters: - use_sim_time: true type: 1 filter_info_topic: "/costmap_filter_info" mask_topic: "/filter_mask" @@ -316,7 +290,6 @@ costmap_filter_info_server: filter_mask_server: ros__parameters: - use_sim_time: true frame_id: "map" topic_name: "/filter_mask" yaml_filename: "speed_mask.yaml" diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index a0a2024abb..fcf5352f9c 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -1,6 +1,5 @@ amcl: ros__parameters: - use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -41,7 +40,6 @@ amcl: bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom @@ -91,17 +89,8 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True - controller_server: ros__parameters: - use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 @@ -172,7 +161,6 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -216,7 +204,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -242,12 +229,10 @@ global_costmap: map_server: ros__parameters: - use_sim_time: True yaml_filename: "turtlebot3_world.yaml" map_saver: ros__parameters: - use_sim_time: True save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -256,7 +241,6 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -264,10 +248,6 @@ planner_server: use_astar: False allow_unknown: True -smoother_server: - ros__parameters: - use_sim_time: True - behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw @@ -285,16 +265,11 @@ behavior_server: global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: loop_rate: 20 @@ -307,7 +282,6 @@ waypoint_follower: costmap_filter_info_server: ros__parameters: - use_sim_time: true type: 1 filter_info_topic: "/costmap_filter_info" mask_topic: "/filter_mask" @@ -316,7 +290,6 @@ costmap_filter_info_server: filter_mask_server: ros__parameters: - use_sim_time: true frame_id: "map" topic_name: "/filter_mask" yaml_filename: "speed_mask.yaml" diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index edaf5e5299..b8e7729904 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -58,7 +58,6 @@ Below are the default values of the parameters : planner_server: ros__parameters: planner_plugin_types: ["nav2_theta_star_planner/ThetaStarPlanner"] - use_sim_time: True planner_plugin_ids: ["GridBased"] GridBased: how_many_corners: 8 From 2b344053a346527f224f1fb9eaab8c9ae0cd6c3f Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Thu, 25 Aug 2022 12:44:50 -0400 Subject: [PATCH 047/131] standalone assisted teleop (#2904) * standalone assisted teleop * added in action message * code review * moved to behavior server * added assisted teleop bt node * revert * added bt node for assisted teleop * lint fix * added cancel assisted teleop node * code review * working * cleanup * updated feeback * code review * update compute velocity * cleanup * lint fixes * cleanup * test fix * starting to add tests for assisted teleop * fixed tests * undo * fixed test * is_recovery * adjust abort result based on recovery or not * code review * added preempt velocity * working preempt assisted teleop test * completed assisted teleop tests * code review * undo * code review * remove sleep * topic rename * missing comma * added comma :( * added comma Co-authored-by: Steve Macenski --- nav2_behavior_tree/CMakeLists.txt | 6 + .../plugins/action/assisted_teleop_action.hpp | 70 +++++ .../action/assisted_teleop_cancel_node.hpp | 59 ++++ nav2_behavior_tree/nav2_tree_nodes.xml | 12 + .../plugins/action/assisted_teleop_action.cpp | 62 ++++ .../action/assisted_teleop_cancel_node.cpp | 47 +++ .../test/plugins/action/CMakeLists.txt | 8 + .../action/test_assisted_teleop_action.cpp | 215 ++++++++++++++ .../test_assisted_teleop_cancel_node.cpp | 173 +++++++++++ nav2_behaviors/CMakeLists.txt | 10 + nav2_behaviors/behavior_plugin.xml | 6 + .../include/nav2_behaviors/timed_behavior.hpp | 13 +- nav2_behaviors/plugins/assisted_teleop.cpp | 186 ++++++++++++ nav2_behaviors/plugins/assisted_teleop.hpp | 105 +++++++ .../params/nav2_multirobot_params_1.yaml | 2 + .../params/nav2_multirobot_params_2.yaml | 2 + nav2_bringup/params/nav2_params.yaml | 6 +- nav2_bt_navigator/src/bt_navigator.cpp | 3 + nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/action/AssistedTeleop.action | 8 + nav2_system_tests/CMakeLists.txt | 1 + .../behavior_tree/test_behavior_tree_node.cpp | 2 + .../behaviors/assisted_teleop/CMakeLists.txt | 23 ++ .../assisted_teleop_behavior_tester.cpp | 277 ++++++++++++++++++ .../assisted_teleop_behavior_tester.hpp | 104 +++++++ .../test_assisted_teleop_behavior_launch.py | 103 +++++++ .../test_assisted_teleop_behavior_node.cpp | 114 +++++++ .../src/costmap_filters/keepout_params.yaml | 2 + .../costmap_filters/speed_global_params.yaml | 2 + .../costmap_filters/speed_local_params.yaml | 2 + 30 files changed, 1622 insertions(+), 2 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp create mode 100644 nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp create mode 100644 nav2_behaviors/plugins/assisted_teleop.cpp create mode 100644 nav2_behaviors/plugins/assisted_teleop.hpp create mode 100644 nav2_msgs/action/AssistedTeleop.action create mode 100644 nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt create mode 100644 nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp create mode 100644 nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp create mode 100755 nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py create mode 100644 nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 0569c58bc9..224e8498e1 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -69,6 +69,9 @@ list(APPEND plugin_libs nav2_spin_cancel_bt_node) add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp) list(APPEND plugin_libs nav2_back_up_cancel_bt_node) +add_library(nav2_assisted_teleop_cancel_bt_node SHARED plugins/action/assisted_teleop_cancel_node.cpp) +list(APPEND plugin_libs nav2_assisted_teleop_cancel_bt_node) + add_library(nav2_drive_on_heading_cancel_bt_node SHARED plugins/action/drive_on_heading_cancel_node.cpp) list(APPEND plugin_libs nav2_drive_on_heading_cancel_bt_node) @@ -90,6 +93,9 @@ list(APPEND plugin_libs nav2_spin_action_bt_node) add_library(nav2_wait_action_bt_node SHARED plugins/action/wait_action.cpp) list(APPEND plugin_libs nav2_wait_action_bt_node) +add_library(nav2_assisted_teleop_action_bt_node SHARED plugins/action/assisted_teleop_action.cpp) +list(APPEND plugin_libs nav2_assisted_teleop_action_bt_node) + add_library(nav2_clear_costmap_service_bt_node SHARED plugins/action/clear_costmap_service.cpp) list(APPEND plugin_libs nav2_clear_costmap_service_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp new file mode 100644 index 0000000000..22cde94c35 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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__ACTION__ASSISTED_TELEOP_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_ + +#include + +#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::AssistedTeleop + */ +class AssistedTeleopAction : public BtActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::nav2_msgs::action::AssistedTeleop + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + AssistedTeleopAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Function to perform some user-defined operation on tick + */ + void on_tick() override; + + BT::NodeStatus on_aborted() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort("time_allowance", 10.0, "Allowed time for running assisted teleop"), + BT::InputPort("is_recovery", false, "If true the recovery count will be incremented") + }); + } + +private: + bool is_recovery_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp new file mode 100644 index 0000000000..55fe337fe8 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/assisted_teleop.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp + */ +class AssistedTeleopCancel : public BtCancelActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::BackUpAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + AssistedTeleopCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 3f5fa4b522..c34a171fc8 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -43,6 +43,11 @@ Server timeout + + Service name to cancel the assisted teleop behavior + Server timeout + + Service name to cancel the wait behavior Server timeout @@ -164,6 +169,13 @@ Server timeout + + Allowed time for spinning + If true recovery count will be incremented + Service name + Server timeout + + Destination diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp new file mode 100644 index 0000000000..025f4786fc --- /dev/null +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -0,0 +1,62 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp" + +namespace nav2_behavior_tree +{ + +AssistedTeleopAction::AssistedTeleopAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ + double time_allowance; + getInput("time_allowance", time_allowance); + getInput("is_recovery", is_recovery_); + + // Populate the input message + goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); +} + +void AssistedTeleopAction::on_tick() +{ + if (is_recovery_) { + increment_recovery_count(); + } +} + +BT::NodeStatus AssistedTeleopAction::on_aborted() +{ + return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory.registerBuilder("AssistedTeleop", builder); +} diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp new file mode 100644 index 0000000000..e226ba1975 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +AssistedTeleopCancel::AssistedTeleopCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory.registerBuilder( + "CancelAssistedTeleop", builder); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 3c6e595ba7..0b7cae1a6c 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -19,6 +19,10 @@ 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_assisted_teleop_action test_assisted_teleop_action.cpp) +target_link_libraries(test_action_assisted_teleop_action nav2_assisted_teleop_action_bt_node) +ament_target_dependencies(test_action_assisted_teleop_action ${dependencies}) + ament_add_gtest(test_action_controller_cancel_action test_controller_cancel_node.cpp) target_link_libraries(test_action_controller_cancel_action nav2_controller_cancel_bt_node) ament_target_dependencies(test_action_controller_cancel_action ${dependencies}) @@ -36,6 +40,10 @@ ament_add_gtest(test_action_back_up_cancel_action test_back_up_cancel_node.cpp) target_link_libraries(test_action_back_up_cancel_action nav2_back_up_cancel_bt_node) ament_target_dependencies(test_action_back_up_cancel_action ${dependencies}) +ament_add_gtest(test_action_assisted_teleop_cancel_action test_assisted_teleop_cancel_node.cpp) +target_link_libraries(test_action_assisted_teleop_cancel_action nav2_assisted_teleop_cancel_bt_node) +ament_target_dependencies(test_action_assisted_teleop_cancel_action ${dependencies}) + ament_add_gtest(test_action_drive_on_heading_cancel_action test_drive_on_heading_cancel_node.cpp) target_link_libraries(test_action_drive_on_heading_cancel_action nav2_drive_on_heading_cancel_bt_node) ament_target_dependencies(test_action_drive_on_heading_cancel_action ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp new file mode 100644 index 0000000000..d5b033184b --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -0,0 +1,215 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp" + +class AssistedTeleopActionServer : public TestActionServer +{ +public: + AssistedTeleopActionServer() + : TestActionServer("assisted_teleop") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) + override + { + nav2_msgs::action::AssistedTeleop::Result::SharedPtr result = + std::make_shared(); + bool return_success = getReturnSuccess(); + if (return_success) { + goal_handle->succeed(result); + } else { + goal_handle->abort(result); + } + } +}; + +class AssistedTeleopActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("backup_action_test_fixture"); + 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_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory_->registerBuilder("AssistedTeleop", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void SetUp() override + { + config_->blackboard->set("number_recoveries", 0); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr AssistedTeleopActionTestFixture::node_ = nullptr; +std::shared_ptr +AssistedTeleopActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * AssistedTeleopActionTestFixture::config_ = nullptr; +std::shared_ptr AssistedTeleopActionTestFixture::factory_ = nullptr; +std::shared_ptr AssistedTeleopActionTestFixture::tree_ = nullptr; + +TEST_F(AssistedTeleopActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("time_allowance"), 10.0); + + xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("time_allowance"), 20.0); +} + +TEST_F(AssistedTeleopActionTestFixture, test_tick) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + + 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(); + EXPECT_EQ(goal->time_allowance.sec, 10.0); +} + +TEST_F(AssistedTeleopActionTestFixture, test_failure) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setReturnSuccess(false); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + + auto goal = action_server_->getCurrentGoal(); + EXPECT_EQ(goal->time_allowance.sec, 10.0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + AssistedTeleopActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(AssistedTeleopActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp new file mode 100644 index 0000000000..006370b697 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -0,0 +1,173 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelAssistedTeleopServer : public TestActionServer +{ +public: + CancelAssistedTeleopServer() + : TestActionServer("assisted_teleop") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // Assisted Teleop here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelAssistedTeleopActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_back_up_action_test_fixture"); + 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_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + client_ = rclcpp_action::create_client( + node_, "assisted_teleop"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory_->registerBuilder( + "CancelAssistedTeleop", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + client_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelAssistedTeleopActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelAssistedTeleopActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelAssistedTeleopActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelAssistedTeleopActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelAssistedTeleopActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelAssistedTeleopActionTestFixture::tree_ = nullptr; + +TEST_F(CancelAssistedTeleopActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client< + nav2_msgs::action::AssistedTeleop>::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::AssistedTeleop::Goal(); + + // BackUping for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and back_up on new thread + CancelAssistedTeleopActionTestFixture::action_server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelAssistedTeleopActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt index 273c5fe664..df200309d9 100644 --- a/nav2_behaviors/CMakeLists.txt +++ b/nav2_behaviors/CMakeLists.txt @@ -79,6 +79,14 @@ ament_target_dependencies(nav2_back_up_behavior ${dependencies} ) +add_library(nav2_assisted_teleop_behavior SHARED + plugins/assisted_teleop.cpp +) + +ament_target_dependencies(nav2_assisted_teleop_behavior + ${dependencies} +) + pluginlib_export_plugin_description_file(nav2_core behavior_plugin.xml) # Library @@ -106,6 +114,7 @@ rclcpp_components_register_nodes(${library_name} "behavior_server::BehaviorServe install(TARGETS ${library_name} nav2_spin_behavior nav2_wait_behavior + nav2_assisted_teleop_behavior nav2_drive_on_heading_behavior nav2_back_up_behavior ARCHIVE DESTINATION lib @@ -140,6 +149,7 @@ ament_export_include_directories(include) ament_export_libraries(${library_name} nav2_spin_behavior nav2_wait_behavior + nav2_assisted_teleop_behavior nav2_drive_on_heading_behavior nav2_back_up_behavior ) diff --git a/nav2_behaviors/behavior_plugin.xml b/nav2_behaviors/behavior_plugin.xml index 5989f566a9..cd20ae7aa9 100644 --- a/nav2_behaviors/behavior_plugin.xml +++ b/nav2_behaviors/behavior_plugin.xml @@ -22,4 +22,10 @@ + + + + + + diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 06528be887..7d38f38167 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -95,6 +95,11 @@ class TimedBehavior : public nav2_core::Behavior { } + // an opportunity for a derived class to do something on action completion + virtual void onActionCompletion() + { + } + // configure the server on lifecycle setup void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, @@ -167,6 +172,7 @@ class TimedBehavior : public nav2_core::Behavior std::string global_frame_; std::string robot_base_frame_; double transform_tolerance_; + rclcpp::Duration elasped_time_{0, 0}; // Clock rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; @@ -203,11 +209,13 @@ class TimedBehavior : public nav2_core::Behavior rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { + elasped_time_ = steady_clock_.now() - start_time; if (action_server_->is_cancel_requested()) { RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); stopRobot(); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = elasped_time_; action_server_->terminate_all(result); + onActionCompletion(); return; } @@ -220,6 +228,7 @@ class TimedBehavior : public nav2_core::Behavior stopRobot(); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_current(result); + onActionCompletion(); return; } @@ -230,12 +239,14 @@ class TimedBehavior : public nav2_core::Behavior "%s completed successfully", behavior_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->succeeded_current(result); + onActionCompletion(); return; case Status::FAILED: RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_current(result); + onActionCompletion(); return; case Status::RUNNING: diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp new file mode 100644 index 0000000000..defdfaf5a7 --- /dev/null +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -0,0 +1,186 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "assisted_teleop.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_behaviors +{ +AssistedTeleop::AssistedTeleop() +: TimedBehavior(), + feedback_(std::make_shared()) +{} + +void AssistedTeleop::onConfigure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // set up parameters + nav2_util::declare_parameter_if_not_declared( + node, + "projection_time", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, + "simulation_time_step", rclcpp::ParameterValue(0.1)); + + nav2_util::declare_parameter_if_not_declared( + node, + "cmd_vel_teleop", rclcpp::ParameterValue(std::string("cmd_vel_teleop"))); + + node->get_parameter("projection_time", projection_time_); + node->get_parameter("simulation_time_step", simulation_time_step_); + + std::string cmd_vel_teleop; + node->get_parameter("cmd_vel_teleop", cmd_vel_teleop); + + vel_sub_ = node->create_subscription( + cmd_vel_teleop, rclcpp::SystemDefaultsQoS(), + std::bind( + &AssistedTeleop::teleopVelocityCallback, + this, std::placeholders::_1)); + + preempt_teleop_sub_ = node->create_subscription( + "preempt_teleop", rclcpp::SystemDefaultsQoS(), + std::bind( + &AssistedTeleop::preemptTeleopCallback, + this, std::placeholders::_1)); +} + +Status AssistedTeleop::onRun(const std::shared_ptr command) +{ + preempt_teleop_ = false; + command_time_allowance_ = command->time_allowance; + end_time_ = steady_clock_.now() + command_time_allowance_; + return Status::SUCCEEDED; +} + +void AssistedTeleop::onActionCompletion() +{ + teleop_twist_ = geometry_msgs::msg::Twist(); + preempt_teleop_ = false; +} + +Status AssistedTeleop::onCycleUpdate() +{ + feedback_->current_teleop_duration = elasped_time_; + action_server_->publish_feedback(feedback_); + + rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { + stopRobot(); + RCLCPP_WARN_STREAM( + logger_, + "Exceeded time allowance before reaching the " << behavior_name_.c_str() << + "goal - Exiting " << behavior_name_.c_str()); + return Status::FAILED; + } + + // user states that teleop was successful + if (preempt_teleop_) { + stopRobot(); + return Status::SUCCEEDED; + } + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_ERROR_STREAM( + logger_, + "Current robot pose is not available for " << + behavior_name_.c_str()); + return Status::FAILED; + } + geometry_msgs::msg::Pose2D projected_pose; + projected_pose.x = current_pose.pose.position.x; + projected_pose.y = current_pose.pose.position.y; + projected_pose.theta = tf2::getYaw(current_pose.pose.orientation); + + geometry_msgs::msg::Twist scaled_twist = teleop_twist_; + for (double time = simulation_time_step_; time < projection_time_; + time += simulation_time_step_) + { + projected_pose = projectPose(projected_pose, teleop_twist_, simulation_time_step_); + + if (!collision_checker_->isCollisionFree(projected_pose)) { + if (time == simulation_time_step_) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, + steady_clock_, + 1000, + behavior_name_.c_str() << " collided on first time step, setting velocity to zero"); + scaled_twist.linear.x = 0.0f; + scaled_twist.linear.y = 0.0f; + scaled_twist.angular.z = 0.0f; + break; + } else { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, + steady_clock_, + 1000, + behavior_name_.c_str() << " collision approaching in " << time << " seconds"); + double scale_factor = time / projection_time_; + scaled_twist.linear.x *= scale_factor; + scaled_twist.linear.y *= scale_factor; + scaled_twist.angular.z *= scale_factor; + break; + } + } + } + vel_pub_->publish(std::move(scaled_twist)); + + return Status::RUNNING; +} + +geometry_msgs::msg::Pose2D AssistedTeleop::projectPose( + const geometry_msgs::msg::Pose2D & pose, + const geometry_msgs::msg::Twist & twist, + double projection_time) +{ + geometry_msgs::msg::Pose2D projected_pose = pose; + + projected_pose.x += projection_time * ( + twist.linear.x * cos(pose.theta) + + twist.linear.y * sin(pose.theta)); + + projected_pose.y += projection_time * ( + twist.linear.x * sin(pose.theta) - + twist.linear.y * cos(pose.theta)); + + projected_pose.theta += projection_time * twist.angular.z; + + return projected_pose; +} + +void AssistedTeleop::teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + teleop_twist_ = *msg; +} + +void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr) +{ + preempt_teleop_ = true; +} + +} // namespace nav2_behaviors + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::AssistedTeleop, nav2_core::Behavior) diff --git a/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/plugins/assisted_teleop.hpp new file mode 100644 index 0000000000..12a85f327c --- /dev/null +++ b/nav2_behaviors/plugins/assisted_teleop.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "std_msgs/msg/empty.hpp" +#include "nav2_behaviors/timed_behavior.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" + +namespace nav2_behaviors +{ +using AssistedTeleopAction = nav2_msgs::action::AssistedTeleop; + +/** + * @class nav2_behaviors::AssistedTeleop + * @brief An action server behavior for assisted teleop + */ +class AssistedTeleop : public TimedBehavior +{ +public: + AssistedTeleop(); + + /** + * @brief Initialization to run behavior + * @param command Goal to execute + * @return Status of behavior + */ + Status onRun(const std::shared_ptr command) override; + + /** + * @brief func to run at the completion of the action + */ + void onActionCompletion() override; + + /** + * @brief Loop function to run behavior + * @return Status of behavior + */ + Status onCycleUpdate() override; + +protected: + /** + * @brief Configuration of behavior action + */ + void onConfigure() override; + + /** + * @brief project a position + * @param pose initial pose to project + * @param twist velocity to project pose by + * @param projection_time time to project by + */ + geometry_msgs::msg::Pose2D projectPose( + const geometry_msgs::msg::Pose2D & pose, + const geometry_msgs::msg::Twist & twist, + double projection_time); + + /** + * @brief Callback function for velocity subscriber + * @param msg received Twist message + */ + void teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + /** + * @brief Callback function to preempt assisted teleop + * @param msg empty message + */ + void preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr msg); + + AssistedTeleopAction::Feedback::SharedPtr feedback_; + + // parameters + double projection_time_; + double simulation_time_step_; + + geometry_msgs::msg::Twist teleop_twist_; + bool preempt_teleop_{false}; + + // subscribers + rclcpp::Subscription::SharedPtr vel_sub_; + rclcpp::Subscription::SharedPtr preempt_teleop_sub_; + + rclcpp::Duration command_time_allowance_{0, 0}; + rclcpp::Time end_time_; +}; +} // namespace nav2_behaviors + +#endif // NAV2_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_ diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 11e5502246..0e674039cf 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -56,6 +56,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -93,6 +94,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 7e2b3551c8..6a8e6400a7 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -56,6 +56,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -93,6 +94,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index c97ed6a2e1..8593587499 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -56,6 +56,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -93,6 +94,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: @@ -279,7 +281,7 @@ behavior_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: plugin: "nav2_behaviors/Spin" backup: @@ -288,6 +290,8 @@ behavior_server: plugin: "nav2_behaviors/DriveOnHeading" wait: plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 5002c6f264..7c83ff9d9a 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -40,6 +40,7 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_follow_path_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", + "nav2_assisted_teleop_action_bt_node", "nav2_back_up_action_bt_node", "nav2_drive_on_heading_bt_node", "nav2_clear_costmap_service_bt_node", @@ -76,6 +77,8 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_path_longer_on_approach_bt_node", "nav2_wait_cancel_bt_node", "nav2_spin_cancel_bt_node", + "nav2_assisted_teleop_cancel_bt_node", + "nav2_back_up_cancel_bt_node", "nav2_back_up_cancel_bt_node", "nav2_drive_on_heading_cancel_bt_node" }; diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index ce7376cebb..2be81db0a7 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ManageLifecycleNodes.srv" "srv/LoadMap.srv" "srv/SaveMap.srv" + "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" "action/ComputePathThroughPoses.action" diff --git a/nav2_msgs/action/AssistedTeleop.action b/nav2_msgs/action/AssistedTeleop.action new file mode 100644 index 0000000000..ba830ebc12 --- /dev/null +++ b/nav2_msgs/action/AssistedTeleop.action @@ -0,0 +1,8 @@ +#goal definition +builtin_interfaces/Duration time_allowance +--- +#result definition +builtin_interfaces/Duration total_elapsed_time +--- +#feedback +builtin_interfaces/Duration current_teleop_duration diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 6cc6a32e49..6d56df1ef7 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -65,6 +65,7 @@ if(BUILD_TESTING) add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/backup) add_subdirectory(src/behaviors/drive_on_heading) + add_subdirectory(src/behaviors/assisted_teleop) add_subdirectory(src/costmap_filters) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index b5bb56a6f1..d7ede55cd1 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -58,6 +58,7 @@ class BehaviorTreeHandler "nav2_follow_path_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", + "nav2_assisted_teleop_action_bt_node", "nav2_back_up_action_bt_node", "nav2_drive_on_heading_bt_node", "nav2_clear_costmap_service_bt_node", @@ -91,6 +92,7 @@ class BehaviorTreeHandler "nav2_goal_checker_selector_bt_node", "nav2_controller_cancel_bt_node", "nav2_path_longer_on_approach_bt_node", + "nav2_assisted_teleop_cancel_bt_node", "nav2_wait_cancel_bt_node", "nav2_spin_cancel_bt_node", "nav2_back_up_cancel_bt_node", diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt new file mode 100644 index 0000000000..c2210b6fcb --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_assisted_teleop_behavior test_assisted_teleop_behavior_node) + +ament_add_gtest_executable(${test_assisted_teleop_behavior} + test_assisted_teleop_behavior_node.cpp + assisted_teleop_behavior_tester.cpp +) + +ament_target_dependencies(${test_assisted_teleop_behavior} + ${dependencies} +) + +ament_add_test(test_assisted_teleop_behavior + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_assisted_teleop_behavior_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp new file mode 100644 index 0000000000..bcc1950bde --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp @@ -0,0 +1,277 @@ +// Copyright (c) 2020 Sarthak Mittal +// 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 +#include +#include +#include +#include +#include +#include +#include + +#include "assisted_teleop_behavior_tester.hpp" +#include "nav2_util/geometry_utils.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester() +: is_active_(false), + initial_pose_received_(false) +{ + node_ = rclcpp::Node::make_shared("assisted_teleop_behavior_test"); + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + client_ptr_ = rclcpp_action::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + "assisted_teleop"); + + initial_pose_pub_ = + node_->create_publisher("initialpose", 10); + + preempt_pub_ = + node_->create_publisher("preempt_teleop", 10); + + cmd_vel_pub_ = + node_->create_publisher("cmd_vel_teleop", 10); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); + + filtered_vel_sub_ = node_->create_subscription( + "cmd_vel", + rclcpp::SystemDefaultsQoS(), + std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1)); + + std::string costmap_topic = "/local_costmap/costmap_raw"; + std::string footprint_topic = "/local_costmap/published_footprint"; + + costmap_sub_ = std::make_shared( + node_, + costmap_topic); + + footprint_sub_ = std::make_shared( + node_, + footprint_topic, + *tf_buffer_); + + collision_checker_ = std::make_unique( + *costmap_sub_, + *footprint_sub_ + ); + + stamp_ = node_->now(); +} + +AssistedTeleopBehaviorTester::~AssistedTeleopBehaviorTester() +{ + if (is_active_) { + deactivate(); + } +} + +void AssistedTeleopBehaviorTester::activate() +{ + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + return; + } + + while (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); + sendInitialPose(); + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node_); + } + + // Wait for lifecycle_manager_navigation to activate behavior_server + std::this_thread::sleep_for(10s); + + if (!client_ptr_) { + RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); + is_active_ = false; + return; + } + + if (!client_ptr_->wait_for_action_server(10s)) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + is_active_ = false; + return; + } + + RCLCPP_INFO(this->node_->get_logger(), "Assisted Teleop action server is ready"); + is_active_ = true; +} + +void AssistedTeleopBehaviorTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( + const float lin_vel, + const float ang_vel) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_ptr_->async_send_goal(nav2_msgs::action::AssistedTeleop::Goal()); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + auto result_future = client_ptr_->async_get_result(goal_handle); + + rclcpp::Rate r(1); + + counter_ = 0; + auto start_time = std::chrono::system_clock::now(); + while (rclcpp::ok()) { + geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist(); + cmd_vel.linear.x = lin_vel; + cmd_vel.angular.z = ang_vel; + cmd_vel_pub_->publish(cmd_vel); + + if (counter_ > 1) { + break; + } + + auto current_time = std::chrono::system_clock::now(); + if (current_time - start_time > 25s) { + RCLCPP_ERROR(node_->get_logger(), "Exceeded Timeout"); + return false; + } + + rclcpp::spin_some(node_); + r.sleep(); + } + + auto preempt_msg = std_msgs::msg::Empty(); + preempt_pub_->publish(preempt_msg); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::WrappedResult + wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was canceled"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "result received"); + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + return false; + } + + geometry_msgs::msg::Pose2D pose_2d; + pose_2d.x = current_pose.pose.position.x; + pose_2d.y = current_pose.pose.position.y; + pose_2d.theta = tf2::getYaw(current_pose.pose.orientation); + + if (!collision_checker_->isCollisionFree(pose_2d)) { + RCLCPP_ERROR(node_->get_logger(), "Ended in collision"); + return false; + } + + return true; +} + +void AssistedTeleopBehaviorTester::sendInitialPose() +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = stamp_; + pose.pose.pose.position.x = -2.0; + pose.pose.pose.position.y = -0.5; + pose.pose.pose.position.z = 0.0; + pose.pose.pose.orientation.x = 0.0; + pose.pose.pose.orientation.y = 0.0; + pose.pose.pose.orientation.z = 0.0; + pose.pose.pose.orientation.w = 1.0; + for (int i = 0; i < 35; i++) { + pose.pose.covariance[i] = 0.0; + } + pose.pose.covariance[0] = 0.08; + pose.pose.covariance[7] = 0.08; + pose.pose.covariance[35] = 0.05; + + initial_pose_pub_->publish(pose); + RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); +} + +void AssistedTeleopBehaviorTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +void AssistedTeleopBehaviorTester::filteredVelCallback( + geometry_msgs::msg::Twist::SharedPtr msg) +{ + if (msg->linear.x == 0.0f) { + counter_++; + } else { + counter_ = 0; + } +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp new file mode 100644 index 0000000000..2cf46827e8 --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2020 Samsung Research +// 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 BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_ + +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_util/robot_utils.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/empty.hpp" + +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace nav2_system_tests +{ + +class AssistedTeleopBehaviorTester +{ +public: + using AssistedTeleop = nav2_msgs::action::AssistedTeleop; + + AssistedTeleopBehaviorTester(); + ~AssistedTeleopBehaviorTester(); + + // Runs a single test with given target yaw + bool defaultAssistedTeleopTest( + const float lin_vel, + const float ang_vel); + + void activate(); + + void deactivate(); + + bool isActive() const + { + return is_active_; + } + +private: + void sendInitialPose(); + + void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + + void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg); + + unsigned int counter_; + bool is_active_; + bool initial_pose_received_; + rclcpp::Time stamp_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Node::SharedPtr node_; + + // Publishers + rclcpp::Publisher::SharedPtr initial_pose_pub_; + rclcpp::Publisher::SharedPtr preempt_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + + // Subscribers + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr filtered_vel_sub_; + + // Action client to call AssistedTeleop action + rclcpp_action::Client::SharedPtr client_ptr_; + + // collision checking + std::shared_ptr costmap_sub_; + std::shared_ptr footprint_sub_; + std::unique_ptr collision_checker_; +}; + +} // namespace nav2_system_tests + +#endif // BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py new file mode 100755 index 0000000000..976eb02c2f --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -0,0 +1,103 @@ +#! /usr/bin/env python3 +# Copyright (c) 2012 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_assisted_teleop_behavior_node', + 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_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp new file mode 100644 index 0000000000..eb847272eb --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp @@ -0,0 +1,114 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2022 Joshua Wallace +// +// 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 "assisted_teleop_behavior_tester.hpp" +#include "nav2_msgs/action/back_up.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::AssistedTeleopBehaviorTester; + +struct TestParameters +{ + float lin_vel; + float ang_vel; +}; + + +std::string testNameGenerator(const testing::TestParamInfo &) +{ + static int test_index = 0; + std::string name = "AssistedTeleopTest" + std::to_string(test_index); + ++test_index; + return name; +} + +class AssistedTeleopBehaviorTestFixture + : public ::testing::TestWithParam +{ +public: + static void SetUpTestCase() + { + assisted_teleop_behavior_tester = new AssistedTeleopBehaviorTester(); + if (!assisted_teleop_behavior_tester->isActive()) { + assisted_teleop_behavior_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete assisted_teleop_behavior_tester; + assisted_teleop_behavior_tester = nullptr; + } + +protected: + static AssistedTeleopBehaviorTester * assisted_teleop_behavior_tester; +}; + +AssistedTeleopBehaviorTester * +AssistedTeleopBehaviorTestFixture::assisted_teleop_behavior_tester = nullptr; + +TEST_P(AssistedTeleopBehaviorTestFixture, testAssistedTeleopBehavior) +{ + auto test_params = GetParam(); + + if (!assisted_teleop_behavior_tester->isActive()) { + assisted_teleop_behavior_tester->activate(); + } + + bool success = false; + success = assisted_teleop_behavior_tester->defaultAssistedTeleopTest( + test_params.lin_vel, + test_params.ang_vel); + + EXPECT_TRUE(success); +} + +std::vector test_params = {TestParameters{-0.1, 0.0}, + TestParameters{0.35, 0.05}}; + +INSTANTIATE_TEST_SUITE_P( + TestAssistedTeleopBehavior, + AssistedTeleopBehaviorTestFixture, + ::testing::Values( + test_params[0], + test_params[1]), + testNameGenerator +); + + +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_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index d886ca3e4e..94cd7553e0 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -54,6 +54,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -86,6 +87,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index ba5eede1ad..e9e0c4550b 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -54,6 +54,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -87,6 +88,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index fcf5352f9c..b2a8f82eb2 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -54,6 +54,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -87,6 +88,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: From 1022f02e926d073f22d11f773e7bf7f8d5bec7e0 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Fri, 26 Aug 2022 19:20:38 +0300 Subject: [PATCH 048/131] Add the support of range sensors to Collision Monitor (#3099) * Support range sensors in Collision Monitor * Adjust README.md * Meet review fixes --- nav2_collision_monitor/CMakeLists.txt | 1 + nav2_collision_monitor/README.md | 2 +- .../include/nav2_collision_monitor/circle.hpp | 2 +- .../collision_monitor_node.hpp | 3 +- .../nav2_collision_monitor/kinematics.hpp | 2 +- .../nav2_collision_monitor/pointcloud.hpp | 4 +- .../nav2_collision_monitor/polygon.hpp | 2 +- .../include/nav2_collision_monitor/range.hpp | 97 +++++++++++ .../include/nav2_collision_monitor/scan.hpp | 4 +- .../include/nav2_collision_monitor/source.hpp | 4 +- .../include/nav2_collision_monitor/types.hpp | 2 +- .../launch/collision_monitor_node.launch.py | 2 +- nav2_collision_monitor/src/circle.cpp | 2 +- .../src/collision_monitor_node.cpp | 10 +- nav2_collision_monitor/src/kinematics.cpp | 2 +- nav2_collision_monitor/src/main.cpp | 2 +- nav2_collision_monitor/src/pointcloud.cpp | 2 +- nav2_collision_monitor/src/polygon.cpp | 2 +- nav2_collision_monitor/src/range.cpp | 145 ++++++++++++++++ nav2_collision_monitor/src/scan.cpp | 2 +- nav2_collision_monitor/src/source.cpp | 2 +- .../test/collision_monitor_node_test.cpp | 94 ++++++++--- .../test/kinematics_test.cpp | 2 +- nav2_collision_monitor/test/polygons_test.cpp | 2 +- nav2_collision_monitor/test/sources_test.cpp | 156 +++++++++++++++++- 25 files changed, 495 insertions(+), 53 deletions(-) create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/range.hpp create mode 100644 nav2_collision_monitor/src/range.cpp diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index e1b1c67567..8544429466 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -47,6 +47,7 @@ add_library(${library_name} SHARED src/source.cpp src/scan.cpp src/pointcloud.cpp + src/range.cpp src/kinematics.cpp ) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index bb6fac5b75..54841675e9 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -35,7 +35,7 @@ The data may be obtained from different data sources: * Laser scanners (`sensor_msgs::msg::LaserScan` messages) * PointClouds (`sensor_msgs::msg::PointCloud2` messages) -* IR/Sonars (`sensor_msgs::msg::Range` messages, not implemented yet) +* IR/Sonars (`sensor_msgs::msg::Range` messages) ## Design diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index bd902c1ea4..807d4b3dc7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 2de3c22246..87f498e401 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -34,6 +34,7 @@ #include "nav2_collision_monitor/source.hpp" #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" +#include "nav2_collision_monitor/range.hpp" namespace nav2_collision_monitor { diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp index 8dc418bd0e..00038eca9b 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index db3d5e1135..c316c065f7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,7 +35,7 @@ class PointCloud : public Source /** * @brief PointCloud constructor * @param node Collision Monitor node pointer - * @param polygon_name Name of data source + * @param source_name Name of data source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 91a95a1251..97423dc411 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp new file mode 100644 index 0000000000..c6e0e484ad --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2022 Samsung R&D Institute 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_COLLISION_MONITOR__RANGE_HPP_ +#define NAV2_COLLISION_MONITOR__RANGE_HPP_ + +#include "sensor_msgs/msg/range.hpp" + +#include "nav2_collision_monitor/source.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Implementation for IR/ultrasound range sensor source + */ +class Range : public Source +{ +public: + /** + * @brief Range constructor + * @param node Collision Monitor node pointer + * @param source_name Name of data source + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. + * @param global_frame_id Global frame ID for correct transform calculation + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + */ + Range( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout); + /** + * @brief Range destructor + */ + ~Range(); + + /** + * @brief Data source configuration routine. Obtains ROS-parameters + * and creates range sensor subscriber. + */ + void configure(); + + /** + * @brief Adds latest data from range sensor to the data array. + * @param curr_time Current node time for data interpolation + * @param data Array where the data from source to be added. + * Added data is transformed to base_frame_id_ coordinate system at curr_time. + */ + void getData( + const rclcpp::Time & curr_time, + std::vector & data) const; + +protected: + /** + * @brief Getting sensor-specific ROS-parameters + * @param source_topic Output name of source subscription topic + */ + void getParameters(std::string & source_topic); + + /** + * @brief Range sensor data callback + * @param msg Shared pointer to Range sensor message + */ + void dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg); + + // ----- Variables ----- + + /// @brief Range sensor data subscriber + rclcpp::Subscription::SharedPtr data_sub_; + + /// @brief Angle increment (in rad) between two obstacle points at the range arc + double obstacles_angle_; + + /// @brief Latest data obtained from range sensor + sensor_msgs::msg::Range::ConstSharedPtr data_; +}; // class Range + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__RANGE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index f96ae211f8..29747e8131 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,7 +35,7 @@ class Scan : public Source /** * @brief Scan constructor * @param node Collision Monitor node pointer - * @param polygon_name Name of data source + * @param source_name Name of data source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 32227e1df3..a24859bb4a 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -40,7 +40,7 @@ class Source /** * @brief Source constructor * @param node Collision Monitor node pointer - * @param polygon_name Name of data source + * @param source_name Name of data source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index 2f239d694b..39ba9d8c6d 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index 7e96aeec2b..d03d723498 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright (c) 2022 Samsung Research Russia +# Copyright (c) 2022 Samsung R&D Institute Russia # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index 6a05477f49..7dc9708967 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 5088e2c5a2..313d71fb0a 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -302,6 +302,14 @@ bool CollisionMonitor::configureSources( p->configure(); sources_.push_back(p); + } else if (source_type == "range") { + std::shared_ptr r = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout); + + r->configure(); + + sources_.push_back(r); } else { // Error if something else RCLCPP_ERROR( get_logger(), diff --git a/nav2_collision_monitor/src/kinematics.cpp b/nav2_collision_monitor/src/kinematics.cpp index 1b6ee226b7..e51b7c4879 100644 --- a/nav2_collision_monitor/src/kinematics.cpp +++ b/nav2_collision_monitor/src/kinematics.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/main.cpp b/nav2_collision_monitor/src/main.cpp index 01ae87e353..6c23d63024 100644 --- a/nav2_collision_monitor/src/main.cpp +++ b/nav2_collision_monitor/src/main.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index eb25c91114..df4e86b63e 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 0e1c47b59c..d17c34b46a 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp new file mode 100644 index 0000000000..3ef51e2b69 --- /dev/null +++ b/nav2_collision_monitor/src/range.cpp @@ -0,0 +1,145 @@ +// Copyright (c) 2022 Samsung R&D Institute 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 "nav2_collision_monitor/range.hpp" + +#include +#include +#include + +#include "nav2_util/node_utils.hpp" + +namespace nav2_collision_monitor +{ + +Range::Range( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout) +: Source( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, source_timeout), + data_(nullptr) +{ + RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str()); +} + +Range::~Range() +{ + RCLCPP_INFO(logger_, "[%s]: Destroying Range", source_name_.c_str()); + data_sub_.reset(); +} + +void Range::configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + std::string source_topic; + + getParameters(source_topic); + + rclcpp::QoS range_qos = rclcpp::SensorDataQoS(); // set to default + data_sub_ = node->create_subscription( + source_topic, range_qos, + std::bind(&Range::dataCallback, this, std::placeholders::_1)); +} + +void Range::getData( + const rclcpp::Time & curr_time, + std::vector & data) const +{ + // Ignore data from the source if it is not being published yet or + // not being published for a long time + if (data_ == nullptr) { + return; + } + if (!sourceValid(data_->header.stamp, curr_time)) { + return; + } + + // Ignore data, if its range is out of scope of range sensor abilities + if (data_->range < data_->min_range || data_->range > data_->max_range) { + RCLCPP_WARN( + logger_, + "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", + source_name_.c_str(), data_->range, data_->min_range, data_->max_range); + return; + } + + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + tf2::Transform tf_transform; + if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + return; + } + + // Calculate poses and refill data array + float angle; + for ( + angle = -data_->field_of_view / 2; + angle < data_->field_of_view / 2; + angle += obstacles_angle_) + { + // Transform point coordinates from source frame -> to base frame + tf2::Vector3 p_v3_s( + data_->range * std::cos(angle), + data_->range * std::sin(angle), + 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Refill data array + data.push_back({p_v3_b.x(), p_v3_b.y()}); + } + + // Make sure that last (field_of_view / 2) point will be in the data array + angle = data_->field_of_view / 2; + + // Transform point coordinates from source frame -> to base frame + tf2::Vector3 p_v3_s( + data_->range * std::cos(angle), + data_->range * std::sin(angle), + 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Refill data array + data.push_back({p_v3_b.x(), p_v3_b.y()}); +} + +void Range::getParameters(std::string & source_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + getCommonParameters(source_topic); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 180)); + obstacles_angle_ = node->get_parameter(source_name_ + ".obstacles_angle").as_double(); +} + +void Range::dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg) +{ + data_ = msg; +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index a73d9e53cf..d1f52b31f1 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index e945301640..bd1028518c 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 094a3f1e91..0bf3c39003 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -26,6 +27,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/range.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -45,6 +47,7 @@ static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char RANGE_NAME[]{"Range"}; static const int MAX_POINTS{1}; static const double SLOWDOWN_RATIO{0.7}; static const double TIME_BEFORE_COLLISION{1.0}; @@ -64,7 +67,8 @@ enum SourceType { SOURCE_UNKNOWN = 0, SCAN = 1, - POINTCLOUD = 2 + POINTCLOUD = 2, + RANGE = 3 }; class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor @@ -93,16 +97,19 @@ class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::FAILURE); } - bool isDataReceived(const rclcpp::Time & stamp) + bool correctDataReceived(const double expected_dist, const rclcpp::Time & stamp) { for (std::shared_ptr source : sources_) { std::vector collision_points; source->getData(stamp, collision_points); - if (collision_points.size() == 0) { - return false; + if (collision_points.size() != 0) { + const double dist = std::hypot(collision_points[0].x, collision_points[0].y); + if (std::fabs(dist - expected_dist) <= EPSILON) { + return true; + } } } - return true; + return false; } }; // CollisionMonitorWrapper @@ -128,8 +135,12 @@ class Tester : public ::testing::Test // Main topic/data working routines void publishScan(const double dist, const rclcpp::Time & stamp); void publishPointCloud(const double dist, const rclcpp::Time & stamp); + void publishRange(const double dist, const rclcpp::Time & stamp); void publishCmdVel(const double x, const double y, const double tw); - bool waitData(const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp); + bool waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp); bool waitCmdVel(const std::chrono::nanoseconds & timeout); protected: @@ -141,6 +152,7 @@ class Tester : public ::testing::Test // Data source publishers rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; // Working with cmd_vel_in/cmd_vel_out rclcpp::Publisher::SharedPtr cmd_vel_in_pub_; @@ -157,6 +169,8 @@ Tester::Tester() SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); pointcloud_pub_ = cm_->create_publisher( POINTCLOUD_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + range_pub_ = cm_->create_publisher( + RANGE_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); cmd_vel_in_pub_ = cm_->create_publisher( CMD_VEL_IN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -169,6 +183,7 @@ Tester::~Tester() { scan_pub_.reset(); pointcloud_pub_.reset(); + range_pub_.reset(); cmd_vel_in_pub_.reset(); cmd_vel_out_sub_.reset(); @@ -302,6 +317,16 @@ void Tester::addSource( source_name + ".max_height", rclcpp::ParameterValue(1.0)); cm_->set_parameter( rclcpp::Parameter(source_name + ".max_height", 1.0)); + } else if (type == RANGE) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("range")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "range")); + + cm_->declare_parameter( + source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); } else { // type == SOURCE_UNKNOWN cm_->declare_parameter( source_name + ".type", rclcpp::ParameterValue("unknown")); @@ -408,6 +433,23 @@ void Tester::publishPointCloud(const double dist, const rclcpp::Time & stamp) pointcloud_pub_->publish(std::move(msg)); } +void Tester::publishRange(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->radiation_type = 0; + msg->field_of_view = M_PI / 10; + msg->min_range = 0.1; + msg->max_range = dist + 0.1; + msg->range = dist; + + range_pub_->publish(std::move(msg)); +} + void Tester::publishCmdVel(const double x, const double y, const double tw) { // Reset cmd_vel_out_ before calling CollisionMonitor::process() @@ -423,11 +465,14 @@ void Tester::publishCmdVel(const double x, const double y, const double tw) cmd_vel_in_pub_->publish(std::move(msg)); } -bool Tester::waitData(const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp) +bool Tester::waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp) { rclcpp::Time start_time = cm_->now(); while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { - if (cm_->isDataReceived(stamp)) { + if (cm_->correctDataReceived(expected_dist, stamp)) { return true; } rclcpp::spin_some(cm_->get_node_base_interface()); @@ -474,7 +519,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 1. Obstacle is far away from robot publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -483,7 +528,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 2. Obstacle is in slowdown robot zone publishScan(1.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON); @@ -492,7 +537,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 3. Obstacle is inside stop zone publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); @@ -501,7 +546,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 4. Restorint back normal operation publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -531,7 +576,7 @@ TEST_F(Tester, testProcessApproach) // 1. Obstacle is far away from robot publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -540,7 +585,7 @@ TEST_F(Tester, testProcessApproach) // 2. Approaching obstacle (0.2 m ahead from robot footprint) publishScan(1.2, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(1.2, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); // change_ratio = (0.2 m / speed m/s) / TIME_BEFORE_COLLISION s @@ -554,7 +599,7 @@ TEST_F(Tester, testProcessApproach) // 3. Obstacle is inside robot footprint publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); @@ -563,7 +608,7 @@ TEST_F(Tester, testProcessApproach) // 4. Restorint back normal operation publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -585,7 +630,8 @@ TEST_F(Tester, testCrossOver) addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); addPolygon("Approach", CIRCLE, 1.0, "approach"); addSource(POINTCLOUD_NAME, POINTCLOUD); - setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME}); + addSource(RANGE_NAME, RANGE); + setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); // Start Collision Monitor node cm_->start(); @@ -596,7 +642,7 @@ TEST_F(Tester, testCrossOver) // 1. Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). // Robot should approach the obstacle. publishPointCloud(2.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); publishCmdVel(3.0, 0.0, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); // change_ratio = (1.5 m / 3.0 m/s) / TIME_BEFORE_COLLISION s @@ -607,8 +653,8 @@ TEST_F(Tester, testCrossOver) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); // 2. Obstacle is inside slowdown zone, but speed is too slow for approach - publishPointCloud(1.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + publishRange(1.5, curr_time); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.1, 0.0, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.1 * SLOWDOWN_RATIO, EPSILON); @@ -650,7 +696,7 @@ TEST_F(Tester, testCeasePublishZeroVel) // 1. Obstacle is inside approach footprint: robot should stop publishScan(1.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); @@ -666,7 +712,7 @@ TEST_F(Tester, testCeasePublishZeroVel) // 3. Release robot to normal operation publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -675,7 +721,7 @@ TEST_F(Tester, testCeasePublishZeroVel) // 4. Obstacle is inside stop zone publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); diff --git a/nav2_collision_monitor/test/kinematics_test.cpp b/nav2_collision_monitor/test/kinematics_test.cpp index 2c1ec599a7..65933daa38 100644 --- a/nav2_collision_monitor/test/kinematics_test.cpp +++ b/nav2_collision_monitor/test/kinematics_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index ec7a19f124..87576ddda9 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index c199c02286..7101b61175 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -26,6 +27,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/range.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include "tf2_ros/buffer.h" @@ -35,6 +37,7 @@ #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" +#include "nav2_collision_monitor/range.hpp" using namespace std::chrono_literals; @@ -47,6 +50,8 @@ static const char SCAN_NAME[]{"LaserScan"}; static const char SCAN_TOPIC[]{"scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char POINTCLOUD_TOPIC[]{"pointcloud"}; +static const char RANGE_NAME[]{"Range"}; +static const char RANGE_TOPIC[]{"range"}; static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)}; @@ -61,9 +66,11 @@ class TestNode : public nav2_util::LifecycleNode ~TestNode() { scan_pub_.reset(); + pointcloud_pub_.reset(); + range_pub_.reset(); } - void publishScan(const rclcpp::Time & stamp) + void publishScan(const rclcpp::Time & stamp, const double range) { scan_pub_ = this->create_publisher( SCAN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -81,7 +88,7 @@ class TestNode : public nav2_util::LifecycleNode msg->scan_time = 0.0; msg->range_min = 0.1; msg->range_max = 1.1; - std::vector ranges(4, 1.0); + std::vector ranges(4, range); msg->ranges = ranges; scan_pub_->publish(std::move(msg)); @@ -129,9 +136,30 @@ class TestNode : public nav2_util::LifecycleNode pointcloud_pub_->publish(std::move(msg)); } + void publishRange(const rclcpp::Time & stamp, const double range) + { + range_pub_ = this->create_publisher( + RANGE_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->radiation_type = 0; + msg->field_of_view = M_PI / 10; + msg->min_range = 0.1; + msg->max_range = 1.1; + msg->range = range; + + range_pub_->publish(std::move(msg)); + } + private: rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; }; // TestNode class ScanWrapper : public nav2_collision_monitor::Scan @@ -178,6 +206,28 @@ class PointCloudWrapper : public nav2_collision_monitor::PointCloud } }; // PointCloudWrapper +class RangeWrapper : public nav2_collision_monitor::Range +{ +public: + RangeWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout) + : nav2_collision_monitor::Range( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout) + {} + + bool dataReceived() const + { + return data_ != nullptr; + } +}; // RangeWrapper + class Tester : public ::testing::Test { public: @@ -191,12 +241,15 @@ class Tester : public ::testing::Test // Data sources working routines bool waitScan(const std::chrono::nanoseconds & timeout); bool waitPointCloud(const std::chrono::nanoseconds & timeout); + bool waitRange(const std::chrono::nanoseconds & timeout); void checkScan(const std::vector & data); void checkPointCloud(const std::vector & data); + void checkRange(const std::vector & data); std::shared_ptr test_node_; std::shared_ptr scan_; std::shared_ptr pointcloud_; + std::shared_ptr range_; private: std::shared_ptr tf_buffer_; @@ -242,12 +295,28 @@ Tester::Tester() BASE_FRAME_ID, GLOBAL_FRAME_ID, TRANSFORM_TOLERANCE, DATA_TIMEOUT); pointcloud_->configure(); + + // Create Range object + test_node_->declare_parameter( + std::string(RANGE_NAME) + ".topic", rclcpp::ParameterValue(RANGE_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(RANGE_NAME) + ".topic", RANGE_TOPIC)); + + test_node_->declare_parameter( + std::string(RANGE_NAME) + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 199)); + + range_ = std::make_shared( + test_node_, RANGE_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT); + range_->configure(); } Tester::~Tester() { scan_.reset(); pointcloud_.reset(); + range_.reset(); test_node_.reset(); @@ -313,6 +382,19 @@ bool Tester::waitPointCloud(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitRange(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (range_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::checkScan(const std::vector & data) { ASSERT_EQ(data.size(), 4u); @@ -349,6 +431,24 @@ void Tester::checkPointCloud(const std::vector & // Point 2 should be out of scope by height } +void Tester::checkRange(const std::vector & data) +{ + ASSERT_EQ(data.size(), 21u); + + const double angle_increment = M_PI / 199; + double angle = -M_PI / (10 * 2); + int i; + for (i = 0; i < 199 / 10 + 1; i++) { + ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON); + ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); + angle += angle_increment; + } + // Check for the latest FoW/2 point + angle = M_PI / (10 * 2); + ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON); + ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); +} + TEST_F(Tester, testGetData) { rclcpp::Time curr_time = test_node_->now(); @@ -356,12 +456,14 @@ TEST_F(Tester, testGetData) sendTransforms(curr_time); // Publish data for sources - test_node_->publishScan(curr_time); + test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); // Check Scan data std::vector data; @@ -372,6 +474,11 @@ TEST_F(Tester, testGetData) data.clear(); pointcloud_->getData(curr_time, data); checkPointCloud(data); + + // Check Range data + data.clear(); + range_->getData(curr_time, data); + checkRange(data); } TEST_F(Tester, testGetOutdatedData) @@ -381,12 +488,14 @@ TEST_F(Tester, testGetOutdatedData) sendTransforms(curr_time); // Publish outdated data for sources - test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s); + test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s, 1.0); test_node_->publishPointCloud(curr_time - DATA_TIMEOUT - 1s); + test_node_->publishRange(curr_time - DATA_TIMEOUT - 1s, 1.0); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); // Scan data should be empty std::vector data; @@ -396,6 +505,10 @@ TEST_F(Tester, testGetOutdatedData) // Pointcloud data should be empty pointcloud_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } TEST_F(Tester, testIncorrectFrameData) @@ -406,12 +519,14 @@ TEST_F(Tester, testIncorrectFrameData) sendTransforms(curr_time - 1s); // Publish data for sources - test_node_->publishScan(curr_time); + test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); // Scan data should be empty std::vector data; @@ -421,6 +536,35 @@ TEST_F(Tester, testIncorrectFrameData) // Pointcloud data should be empty pointcloud_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); +} + +TEST_F(Tester, testIncorrectData) +{ + rclcpp::Time curr_time = test_node_->now(); + + sendTransforms(curr_time); + + // Publish data for sources + test_node_->publishScan(curr_time, 2.0); + test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 2.0); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitRange(500ms)); + + // Scan data should be empty + std::vector data; + scan_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } int main(int argc, char ** argv) From 7a18064e9decf8653ac0e826fb1535c9bf28ad1f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 26 Aug 2022 09:22:53 -0700 Subject: [PATCH 049/131] Fix #3152: Costmap extend did not include Y component (#3153) --- .../src/regulated_pure_pursuit_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index d8721c646f..55898dc339 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -802,7 +802,7 @@ bool RegulatedPurePursuitController::transformPose( double RegulatedPurePursuitController::getCostmapMaxExtent() const { const double max_costmap_dim_meters = std::max( - costmap_->getSizeInMetersX(), costmap_->getSizeInMetersX()); + costmap_->getSizeInMetersX(), costmap_->getSizeInMetersY()); return max_costmap_dim_meters / 2.0; } From 3360f1880b4e56303ec0693a028b4af1039e8101 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Abdullah=20Enes=20BED=C4=B0R?= <46785079+enesbedir1@users.noreply.github.com> Date: Sat, 27 Aug 2022 11:36:21 +0300 Subject: [PATCH 050/131] missing nodes added to nav2_tree_nodes.xml (#3155) --- nav2_behavior_tree/nav2_tree_nodes.xml | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index c34a171fc8..62a5f1f482 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -33,7 +33,7 @@ Server timeout - + Service name to cancel the drive on heading behavior Server timeout @@ -58,6 +58,18 @@ Server timeout + + Service name + Server timeout + Distance from the robot above which obstacles are cleared + + + + Service name + Server timeout + Distance from the robot under which obstacles are cleared + + Destination to plan to Start pose of the path if overriding current robot pose From 12b45f5663513073aa314dbb0fa474cec386a066 Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Tue, 30 Aug 2022 19:45:10 +1000 Subject: [PATCH 051/131] Change deprecated ceres function (#3158) * Change deprecated function * Update smoother_cost_function.hpp --- .../nav2_constrained_smoother/smoother_cost_function.hpp | 2 +- .../include/nav2_constrained_smoother/utils.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp index 8272b2f2aa..c0c3919ccc 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp @@ -159,7 +159,7 @@ class SmootherCostFunction Eigen::Matrix center = arcCenter( pt_prev, pt, pt_next, next_to_last_length_ratio_ < 0); - if (ceres::IsInfinite(center[0])) { + if (ceres::isinf(center[0])) { return; } T turning_rad = (pt - center).norm(); diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp index 9be4e88f0b..682ed1c16b 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp @@ -86,7 +86,7 @@ inline Eigen::Matrix tangentDir( bool is_cusp) { Eigen::Matrix center = arcCenter(pt_prev, pt, pt_next, is_cusp); - if (ceres::IsInfinite(center[0])) { // straight line + if (ceres::isinf(center[0])) { // straight line Eigen::Matrix d1 = pt - pt_prev; Eigen::Matrix d2 = pt_next - pt; From 79c73f645a8fa406e002cd75607cdbe05a54f526 Mon Sep 17 00:00:00 2001 From: Tejas Kumar Shastha Date: Wed, 31 Aug 2022 18:33:31 +0200 Subject: [PATCH 052/131] remove camera_rgb_joint since child frame does not exist (#3162) --- nav2_bringup/worlds/waffle.model | 9 --------- 1 file changed, 9 deletions(-) diff --git a/nav2_bringup/worlds/waffle.model b/nav2_bringup/worlds/waffle.model index cd5a89bf23..2695e97686 100644 --- a/nav2_bringup/worlds/waffle.model +++ b/nav2_bringup/worlds/waffle.model @@ -454,15 +454,6 @@ - - camera_link - camera_rgb_frame - 0.005 0.018 0.013 0 0 0 - - 0 0 1 - - - From c5e4d1b0aaecc1fab7e757bca889ea07a6451bd1 Mon Sep 17 00:00:00 2001 From: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Date: Wed, 31 Aug 2022 16:57:54 -0400 Subject: [PATCH 053/131] bugfix (#3109) deadlock when costmap receives new map (#3145) * bugfix (#3109) deadlock when costmap receives new map Signed-off-by: Daisuke Sato * introduce map_received_in_update_bounds_ flag to make sure processMap will not be called between updateBounds and updateCosts Signed-off-by: Daisuke Sato Signed-off-by: Daisuke Sato --- .../include/nav2_costmap_2d/static_layer.hpp | 2 +- nav2_costmap_2d/plugins/static_layer.cpp | 23 +++++++------------ 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index def79e349f..aec68fac1f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -183,8 +183,8 @@ class StaticLayer : public CostmapLayer unsigned char unknown_cost_value_; bool trinary_costmap_; bool map_received_{false}; + bool map_received_in_update_bounds_{false}; tf2::Duration transform_tolerance_; - std::atomic update_in_progress_; nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 073109c235..18cd51164f 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -161,7 +161,7 @@ StaticLayer::getParameters() // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; - update_in_progress_.store(false); + map_received_in_update_bounds_ = false; transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); @@ -277,17 +277,13 @@ StaticLayer::interpretValue(unsigned char value) void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { - std::lock_guard guard(*getMutex()); if (!map_received_) { - map_received_ = true; - processMap(*new_map); - } - if (update_in_progress_.load()) { - map_buffer_ = new_map; - } else { processMap(*new_map); - map_buffer_ = nullptr; + map_received_ = true; + return; } + std::lock_guard guard(*getMutex()); + map_buffer_ = new_map; } void @@ -338,11 +334,12 @@ StaticLayer::updateBounds( double * max_y) { if (!map_received_) { + map_received_in_update_bounds_ = false; return; } + map_received_in_update_bounds_ = true; std::lock_guard guard(*getMutex()); - update_in_progress_.store(true); // If there is a new available map, load it. if (map_buffer_) { @@ -378,17 +375,15 @@ StaticLayer::updateCosts( { std::lock_guard guard(*getMutex()); if (!enabled_) { - update_in_progress_.store(false); return; } - if (!map_received_) { + if (!map_received_in_update_bounds_) { static int count = 0; // throttle warning down to only 1/10 message rate if (++count == 10) { RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received"); count = 0; } - update_in_progress_.store(false); return; } @@ -411,7 +406,6 @@ StaticLayer::updateCosts( transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what()); - update_in_progress_.store(false); return; } // Copy map data given proper transformations @@ -436,7 +430,6 @@ StaticLayer::updateCosts( } } } - update_in_progress_.store(false); current_ = true; } From 78ed712a46a95b4e8bfd2fd3f11421ac15aff11f Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Wed, 31 Aug 2022 22:59:55 +0200 Subject: [PATCH 054/131] simple command costmap api - first few functions (#3159) * initial commit costmap_2d template Signed-off-by: Stevedan Omodolor * finish task A and tested * lint * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py Co-authored-by: Steve Macenski * fix trailing underscores Signed-off-by: Stevedan Omodolor Co-authored-by: Steve Macenski --- .../nav2_simple_commander/costmap_2d.py | 73 +++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 nav2_simple_commander/nav2_simple_commander/costmap_2d.py diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py new file mode 100644 index 0000000000..fc2cf3613d --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py @@ -0,0 +1,73 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# Copyright 2022 Stevedan Ogochukwu Omodolor +# +# 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 numpy as np + + +class PyCostmap2D: + """ + PyCostmap2D. + + Costmap Python3 API for OccupancyGrids to populate from published messages + """ + + def __init__(self, occupancy_map): + self.size_x = occupancy_map.info.width + self.size_y = occupancy_map.info.height + self.resolution = occupancy_map.info.resolution + self.origin_x = occupancy_map.info.origin.position.x + self.origin_y = occupancy_map.info.origin.position.y + self.global_frame_id = occupancy_map.header.frame_id + self.costmap_timestamp = occupancy_map.header.stamp + # Extract costmap + self.costmap = np.array(occupancy_map.data, dtype=np.int8).reshape( + self.size_y, self.size_x) + + def getSizeInCellsX(self): + """Get map width in cells.""" + return self.size_x + + def getSizeInCellsY(self): + """Get map height in cells.""" + return self.size_y + + def getSizeInMetersX(self): + """Get x axis map size in meters.""" + return (self.size_x - 1 + 0.5) * self.resolution_ + + def getSizeInMetersY(self): + """Get y axis map size in meters.""" + return (self.size_y - 1 + 0.5) * self.resolution_ + + def getOriginX(self): + """Get the origin x axis of the map [m].""" + return self.origin_x + + def getOriginY(self): + """Get the origin y axis of the map [m].""" + return self.origin_y + + def getResolution(self): + """Get map resolution [m/cell].""" + return self.resolution + + def getGlobalFrameID(self): + """Get global frame_id.""" + return self.global_frame_id + + def getCostmapTimestamp(self): + """Get costmap timestamp.""" + return self.costmap_timestamp From 908a765b73e538c84ab475d2d687d2d7565e4b81 Mon Sep 17 00:00:00 2001 From: Lukas Fanta <63977366+fantalukas@users.noreply.github.com> Date: Tue, 6 Sep 2022 18:01:00 +0200 Subject: [PATCH 055/131] Fix missing dependency on nav2_collision_monitor (#3175) --- navigation2/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/navigation2/package.xml b/navigation2/package.xml index 35e61399a3..8475920c57 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -16,6 +16,7 @@ nav2_amcl nav2_behavior_tree nav2_bt_navigator + nav2_collision_monitor nav2_constrained_smoother nav2_controller nav2_core From f42431028e3ae44dd51002dc94dcc0f1b0bdf79d Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Tue, 6 Sep 2022 13:49:16 -0400 Subject: [PATCH 056/131] fixed start (#3168) * fixed start * return true * fix tests --- .../nav2_smac_planner/node_lattice.hpp | 6 ++ nav2_smac_planner/src/node_2d.cpp | 9 ++- nav2_smac_planner/src/node_hybrid.cpp | 11 ++- nav2_smac_planner/src/node_lattice.cpp | 73 +++++++++++-------- nav2_smac_planner/test/test_a_star.cpp | 8 +- 5 files changed, 65 insertions(+), 42 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index b40624efd6..7f6a82d31c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -408,6 +408,12 @@ class NodeLattice */ bool backtracePath(CoordinateVector & path); + /** + * \brief add node to the path + * \param current_node + */ + void addNodeToPath(NodePtr current_node, CoordinateVector & path); + NodeLattice * parent; Coordinates pose; static LatticeMotionTable motion_table; diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 862b1feca6..158a359870 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -154,13 +154,16 @@ bool Node2D::backtracePath(CoordinateVector & path) NodePtr current_node = this; - do { + while (current_node->parent) { path.push_back( Node2D::getCoords(current_node->getIndex())); current_node = current_node->parent; - } while (current_node->parent); + } + + // add the start pose + path.push_back(Node2D::getCoords(current_node->getIndex())); - return path.size() > 0; + return true; } } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 3ccc17ad0b..ebbe357559 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -705,14 +705,19 @@ bool NodeHybrid::backtracePath(CoordinateVector & path) NodePtr current_node = this; - do { + while (current_node->parent) { path.push_back(current_node->pose); // Convert angle to radians path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); current_node = current_node->parent; - } while (current_node->parent); + } + + // add the start pose + path.push_back(current_node->pose); + // Convert angle to radians + path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); - return path.size() > 0; + return true; } } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index ee61a6df2c..cf3fc163b5 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -541,44 +541,53 @@ bool NodeLattice::backtracePath(CoordinateVector & path) return false; } - Coordinates initial_pose, prim_pose; NodePtr current_node = this; + + while (current_node->parent) { + addNodeToPath(current_node, path); + current_node = current_node->parent; + } + + // add start to path + addNodeToPath(current_node, path); + + return true; +} + +void NodeLattice::addNodeToPath( + NodeLattice::NodePtr current_node, + NodeLattice::CoordinateVector & path) +{ + Coordinates initial_pose, prim_pose; MotionPrimitive * prim = nullptr; const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution; const float pi_2 = 2.0 * M_PI; - - do { - prim = current_node->getMotionPrimitive(); - // if motion primitive is valid, then was searched (rather than analytically expanded), - // include dense path of subpoints making up the primitive at grid resolution - if (prim) { - initial_pose.x = current_node->pose.x - (prim->poses.back()._x / grid_resolution); - initial_pose.y = current_node->pose.y - (prim->poses.back()._y / grid_resolution); - initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(prim->start_angle); - - for (auto it = prim->poses.crbegin(); it != prim->poses.crend(); ++it) { - // Convert primitive pose into grid space if it should be checked - prim_pose.x = initial_pose.x + (it->_x / grid_resolution); - prim_pose.y = initial_pose.y + (it->_y / grid_resolution); - // If reversing, invert the angle because the robot is backing into the primitive - // not driving forward with it - if (current_node->isBackward()) { - prim_pose.theta = std::fmod(it->_theta + M_PI, pi_2); - } else { - prim_pose.theta = it->_theta; - } - path.push_back(prim_pose); + prim = current_node->getMotionPrimitive(); + // if motion primitive is valid, then was searched (rather than analytically expanded), + // include dense path of subpoints making up the primitive at grid resolution + if (prim) { + initial_pose.x = current_node->pose.x - (prim->poses.back()._x / grid_resolution); + initial_pose.y = current_node->pose.y - (prim->poses.back()._y / grid_resolution); + initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(prim->start_angle); + + for (auto it = prim->poses.crbegin(); it != prim->poses.crend(); ++it) { + // Convert primitive pose into grid space if it should be checked + prim_pose.x = initial_pose.x + (it->_x / grid_resolution); + prim_pose.y = initial_pose.y + (it->_y / grid_resolution); + // If reversing, invert the angle because the robot is backing into the primitive + // not driving forward with it + if (current_node->isBackward()) { + prim_pose.theta = std::fmod(it->_theta + M_PI, pi_2); + } else { + prim_pose.theta = it->_theta; } - } else { - // For analytic expansion nodes where there is no valid motion primitive - path.push_back(current_node->pose); - path.back().theta = NodeLattice::motion_table.getAngleFromBin(path.back().theta); + path.push_back(prim_pose); } - - current_node = current_node->parent; - } while (current_node->parent); - - return path.size() > 0; + } else { + // For analytic expansion nodes where there is no valid motion primitive + path.push_back(current_node->pose); + path.back().theta = NodeLattice::motion_table.getAngleFromBin(path.back().theta); + } } } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 00852e5298..72a6ba2968 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -72,7 +72,7 @@ TEST(AStarTest, test_a_star_2d) EXPECT_EQ(num_it, 2414); // check path is the right size and collision free - EXPECT_EQ(path.size(), 81u); + EXPECT_EQ(path.size(), 82u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -104,7 +104,7 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); - EXPECT_EQ(path.size(), 20u); + EXPECT_EQ(path.size(), 21u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -164,7 +164,7 @@ TEST(AStarTest, test_a_star_se2) // check path is the right size and collision free EXPECT_EQ(num_it, 3222); - EXPECT_EQ(path.size(), 62u); + EXPECT_EQ(path.size(), 63u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -225,7 +225,7 @@ TEST(AStarTest, test_a_star_lattice) // check path is the right size and collision free EXPECT_EQ(num_it, 21); - EXPECT_EQ(path.size(), 48u); + EXPECT_EQ(path.size(), 49u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } From 8ce0eadf56fb52b6ff1cc52a4dcd1e876f70b868 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Fri, 9 Sep 2022 19:20:44 +0300 Subject: [PATCH 057/131] Fix velocities comparison for rotation at place case (#3177) * Fix velocities comparison for rotation at place case * Meet review item * Remove unnecessary header * Change the comment --- .../include/nav2_collision_monitor/types.hpp | 5 +- .../test/collision_monitor_node_test.cpp | 128 ++++++++++++++++-- 2 files changed, 122 insertions(+), 11 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index 39ba9d8c6d..aa0b9d729b 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -27,8 +27,9 @@ struct Velocity inline bool operator<(const Velocity & second) const { - const double first_vel = x * x + y * y; - const double second_vel = second.x * second.x + second.y * second.y; + const double first_vel = x * x + y * y + tw * tw; + const double second_vel = second.x * second.x + second.y * second.y + second.tw * second.tw; + // This comparison includes rotations in place, where linear velocities are equal to zero return first_vel < second_vel; } diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 0bf3c39003..472530f786 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -30,6 +30,7 @@ #include "sensor_msgs/msg/range.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" #include "tf2_ros/transform_broadcaster.h" @@ -38,13 +39,14 @@ using namespace std::chrono_literals; -static constexpr double EPSILON = std::numeric_limits::epsilon(); +static constexpr double EPSILON = 1e-5; static const char BASE_FRAME_ID[]{"base_link"}; static const char SOURCE_FRAME_ID[]{"base_source"}; static const char ODOM_FRAME_ID[]{"odom"}; static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; +static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; @@ -132,6 +134,9 @@ class Tester : public ::testing::Test // Setting TF chains void sendTransforms(const rclcpp::Time & stamp); + // Publish robot footprint + void publishFootprint(const double radius, const rclcpp::Time & stamp); + // Main topic/data working routines void publishScan(const double dist, const rclcpp::Time & stamp); void publishPointCloud(const double dist, const rclcpp::Time & stamp); @@ -149,6 +154,9 @@ class Tester : public ::testing::Test // CollisionMonitor node std::shared_ptr cm_; + // Footprint publisher + rclcpp::Publisher::SharedPtr footprint_pub_; + // Data source publishers rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; @@ -165,6 +173,9 @@ Tester::Tester() { cm_ = std::make_shared(); + footprint_pub_ = cm_->create_publisher( + FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + scan_pub_ = cm_->create_publisher( SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); pointcloud_pub_ = cm_->create_publisher( @@ -181,6 +192,8 @@ Tester::Tester() Tester::~Tester() { + footprint_pub_.reset(); + scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); @@ -236,12 +249,19 @@ void Tester::addPolygon( cm_->set_parameter( rclcpp::Parameter(polygon_name + ".type", "polygon")); - const std::vector points { - size, size, size, -size, -size, -size, -size, size}; - cm_->declare_parameter( - polygon_name + ".points", rclcpp::ParameterValue(points)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + ".points", points)); + if (at != "approach") { + const std::vector points { + size, size, size, -size, -size, -size, -size, size}; + cm_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + } else { // at == "approach" + cm_->declare_parameter( + polygon_name + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".footprint_topic", FOOTPRINT_TOPIC)); + } } else if (type == CIRCLE) { cm_->declare_parameter( polygon_name + ".type", rclcpp::ParameterValue("circle")); @@ -379,6 +399,31 @@ void Tester::sendTransforms(const rclcpp::Time & stamp) } } +void Tester::publishFootprint(const double radius, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = BASE_FRAME_ID; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 p; + p.x = radius; + p.y = radius; + msg->polygon.points.push_back(p); + p.x = radius; + p.y = -radius; + msg->polygon.points.push_back(p); + p.x = -radius; + p.y = -radius; + msg->polygon.points.push_back(p); + p.x = -radius; + p.y = radius; + msg->polygon.points.push_back(p); + + footprint_pub_->publish(std::move(msg)); +} + void Tester::publishScan(const double dist, const rclcpp::Time & stamp) { std::unique_ptr msg = @@ -544,7 +589,7 @@ TEST_F(Tester, testProcessStopSlowdown) ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); - // 4. Restorint back normal operation + // 4. Restoring back normal operation publishScan(3.0, curr_time); ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); @@ -606,7 +651,7 @@ TEST_F(Tester, testProcessApproach) ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); - // 4. Restorint back normal operation + // 4. Restoring back normal operation publishScan(3.0, curr_time); ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); @@ -619,6 +664,71 @@ TEST_F(Tester, testProcessApproach) cm_->stop(); } +TEST_F(Tester, testProcessApproachRotation) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making one circle footprint for approach. + setCommonParameters(); + addPolygon("Approach", POLYGON, 1.0, "approach"); + addSource(RANGE_NAME, RANGE); + setVectors({"Approach"}, {RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Publish robot footprint + publishFootprint(1.0, curr_time); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishRange(2.0, curr_time); + ASSERT_TRUE(waitData(2.0, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, M_PI / 4, EPSILON); + + // 2. Approaching rotation to obstacle ( M_PI / 4 - M_PI / 20 ahead from robot) + publishRange(1.4, curr_time); + ASSERT_TRUE(waitData(1.4, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR( + cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR( + cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR( + cmd_vel_out_->angular.z, + M_PI / 5, + (M_PI / 4) * (SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION)); + + // 3. Obstacle is inside robot footprint + publishRange(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // 4. Restoring back normal operation + publishRange(2.5, curr_time); + ASSERT_TRUE(waitData(2.5, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, M_PI / 4, EPSILON); + + // Stop Collision Monitor node + cm_->stop(); +} + TEST_F(Tester, testCrossOver) { rclcpp::Time curr_time = cm_->now(); From 72da0173d98d3a4fcae941f488273ea6362f7e0e Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Fri, 9 Sep 2022 16:14:13 -0400 Subject: [PATCH 058/131] set a empty path on halt (#3178) * set a empty path on halt * fixed issues * remove path reset * fixing * reverting * revert * revert * fixed lint * test fix * uncrusify fix --- .../plugins/action/compute_path_to_pose_action.hpp | 5 +++++ .../plugins/action/compute_path_to_pose_action.cpp | 7 +++++++ .../plugins/condition/globally_updated_goal_condition.cpp | 2 +- .../test/plugins/condition/test_globally_updated_goal.cpp | 2 +- .../include/nav2_collision_monitor/range.hpp | 4 ++++ 5 files changed, 18 insertions(+), 2 deletions(-) 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 f87435995c..fc95daaf43 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 @@ -61,6 +61,11 @@ class ComputePathToPoseAction : public BtActionNodeget>("goals", goals_); config().blackboard->get("goal", goal_); - return BT::NodeStatus::FAILURE; + return BT::NodeStatus::SUCCESS; } std::vector current_goals; diff --git a/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp index d41a6d424c..2e6810f510 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp @@ -49,7 +49,7 @@ TEST_F(GloballyUpdatedGoalConditionTestFixture, test_behavior) config_->blackboard->set("goal", goal); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); goal.pose.position.x = 1.0; config_->blackboard->set("goal", goal); diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index c6e0e484ad..0fbe47501a 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -15,6 +15,10 @@ #ifndef NAV2_COLLISION_MONITOR__RANGE_HPP_ #define NAV2_COLLISION_MONITOR__RANGE_HPP_ +#include +#include +#include + #include "sensor_msgs/msg/range.hpp" #include "nav2_collision_monitor/source.hpp" From 7eeb91e1edb1497caf4654d277fce74602934a27 Mon Sep 17 00:00:00 2001 From: Jackson9 Date: Tue, 13 Sep 2022 02:42:41 +0900 Subject: [PATCH 059/131] simple command costmap api - update few functions (#3169) * * add aditional function to costmap_2d.py Signed-off-by: Stevedan Omodolor Updated-by: Jaehun Kim * finish task B * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py * Update method docs * Remove underscores at parameters and split getCost into getCostXY and getCostIdx * Update method docstrings * lint code & update docstring, remove default value of getCostXY * lint code with pep257 & flake8 --- .../nav2_simple_commander/costmap_2d.py | 104 +++++++++++++++++- 1 file changed, 100 insertions(+), 4 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py index fc2cf3613d..b3b9cde869 100644 --- a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py +++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py @@ -1,6 +1,7 @@ #! /usr/bin/env python3 # Copyright 2021 Samsung Research America # Copyright 2022 Stevedan Ogochukwu Omodolor +# Copyright 2022 Jaehun Jackson Kim # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,6 +15,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +""" +This is a Python3 API for costmap 2d messages from the stack. + +It provides the basic conversion, get/set, +and handling semantics found in the costmap 2d C++ API. +""" + import numpy as np @@ -25,6 +33,15 @@ class PyCostmap2D: """ def __init__(self, occupancy_map): + """ + Initializer for costmap2D. + + Initialize instance variables with parameter occupancy_map. + Args: + occupancy_map (OccupancyGrid): 2D OccupancyGrid Map + Returns: + None + """ self.size_x = occupancy_map.info.width self.size_y = occupancy_map.info.height self.resolution = occupancy_map.info.resolution @@ -33,8 +50,7 @@ def __init__(self, occupancy_map): self.global_frame_id = occupancy_map.header.frame_id self.costmap_timestamp = occupancy_map.header.stamp # Extract costmap - self.costmap = np.array(occupancy_map.data, dtype=np.int8).reshape( - self.size_y, self.size_x) + self.costmap = np.array(occupancy_map.data, dtype=np.uint8) def getSizeInCellsX(self): """Get map width in cells.""" @@ -46,11 +62,11 @@ def getSizeInCellsY(self): def getSizeInMetersX(self): """Get x axis map size in meters.""" - return (self.size_x - 1 + 0.5) * self.resolution_ + return (self.size_x - 1 + 0.5) * self.resolution def getSizeInMetersY(self): """Get y axis map size in meters.""" - return (self.size_y - 1 + 0.5) * self.resolution_ + return (self.size_y - 1 + 0.5) * self.resolution def getOriginX(self): """Get the origin x axis of the map [m].""" @@ -71,3 +87,83 @@ def getGlobalFrameID(self): def getCostmapTimestamp(self): """Get costmap timestamp.""" return self.costmap_timestamp + + def getCostXY(self, mx: int, my: int) -> np.uint8: + """ + Get the cost of a cell in the costmap using map coordinate XY. + + Args: + mx (int): map coordinate X to get cost + my (int): map coordinate Y to get cost + Returns: + np.uint8: cost of a cell + """ + return self.costmap[self.getIndex(mx, my)] + + def getCostIdx(self, index: int) -> np.uint8: + """ + Get the cost of a cell in the costmap using Index. + + Args: + index (int): index of cell to get cost + Returns: + np.uint8: cost of a cell + """ + return self.costmap[index] + + def setCost(self, mx: int, my: int, cost: np.uint8) -> None: + """ + Set the cost of a cell in the costmap using map coordinate XY. + + Args: + mx (int): map coordinate X to get cost + my (int): map coordinate Y to get cost + cost (np.uint8): The cost to set the cell + Returns: + None + """ + self.costmap[self.getIndex(mx, my)] = cost + + def mapToWorld(self, mx: int, my: int) -> tuple[float, float]: + """ + Get the world coordinate XY using map coordinate XY. + + Args: + mx (int): map coordinate X to get world coordinate + my (int): map coordinate Y to get world coordinate + Returns: + tuple of float: wx, wy + wx (float) [m]: world coordinate X + wy (float) [m]: world coordinate Y + """ + wx = self.origin_x + (mx + 0.5) * self.resolution + wy = self.origin_y + (my + 0.5) * self.resolution + return (wx, wy) + + def worldToMap(self, wx: float, wy: float) -> tuple[int, int]: + """ + Get the map coordinate XY using world coordinate XY. + + Args: + wx (float) [m]: world coordinate X to get map coordinate + wy (float) [m]: world coordinate Y to get map coordinate + Returns: + tuple of int: mx, my + mx (int): map coordinate X + my (int): map coordinate Y + """ + mx = int((wx - self.origin_x) // self.resolution) + my = int((wy - self.origin_y) // self.resolution) + return (mx, my) + + def getIndex(self, mx: int, my: int) -> int: + """ + Get the index of the cell using map coordinate XY. + + Args: + mx (int): map coordinate X to get Index + my (int): map coordinate Y to get Index + Returns: + int: The index of the cell + """ + return my * self.size_x + mx From 9cbaf1af03852334067a728fecae2bc84cbc0159 Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Mon, 12 Sep 2022 13:54:15 -0400 Subject: [PATCH 060/131] clear names for bt nodes (#3183) --- ...eplanning_only_if_path_becomes_invalid.xml | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index e60a2e7751..d21efa3b4b 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -7,11 +7,11 @@ - - - - - + + + + + @@ -22,21 +22,21 @@ - + - + - - - + + + From 48ca6e1abe38095797642927b2b25c855f411580 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 16 Sep 2022 11:07:29 -0700 Subject: [PATCH 061/131] [Smac] check if a node exists before creating (#3195) * check if a node exists before creating * invert logic to group like with like * Update a_star.cpp --- nav2_smac_planner/src/a_star.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index deaa597b92..b6a56271a9 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -117,8 +117,11 @@ template typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( const unsigned int & index) { - // Emplace will only create a new object if it doesn't already exist. - // If an element exists, it will return the existing object, not create a new one. + auto iter = _graph.find(index); + if (iter != _graph.end()) { + return &(iter->second); + } + return &(_graph.emplace(index, NodeT(index)).first->second); } From 278f896aa0a837021aa71647b91c9b032fc6da9d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 19 Sep 2022 16:21:20 -0700 Subject: [PATCH 062/131] fixing benchmarkign for planners (#3202) --- .../nav2_simple_commander/robot_navigator.py | 17 +++++-- tools/planner_benchmarking/100by100_10.yaml | 6 +++ tools/planner_benchmarking/100by100_15.yaml | 6 +++ tools/planner_benchmarking/100by100_20.yaml | 6 +++ tools/planner_benchmarking/metrics.py | 48 ++++++++++++++----- tools/planner_benchmarking/process_data.py | 7 ++- 6 files changed, 72 insertions(+), 18 deletions(-) create mode 100644 tools/planner_benchmarking/100by100_10.yaml create mode 100644 tools/planner_benchmarking/100by100_15.yaml create mode 100644 tools/planner_benchmarking/100by100_20.yaml diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 1026f8f75d..d2e37c151b 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -296,8 +296,11 @@ def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): self.info('Nav2 is ready for use!') return - def getPath(self, start, goal, planner_id='', use_start=False): - """Send a `ComputePathToPose` action request.""" + def _getPathImpl(self, start, goal, planner_id='', use_start=False): + """ + Send a `ComputePathToPose` action request. + Internal implementation to get the full result, not just the path. + """ self.debug("Waiting for 'ComputePathToPose' action server") while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathToPose' action server not available, waiting...") @@ -324,7 +327,15 @@ def getPath(self, start, goal, planner_id='', use_start=False): self.warn(f'Getting path failed with status code: {self.status}') return None - return self.result_future.result().result.path + return self.result_future.result().result + + def getPath(self, start, goal, planner_id='', use_start=False): + """Send a `ComputePathToPose` action request.""" + rtn = _getPathImpl(start, goal, planner_id='', use_start=False) + if not rtn: + return None + else: + return rtn.path def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): """Send a `ComputePathThroughPoses` action request.""" diff --git a/tools/planner_benchmarking/100by100_10.yaml b/tools/planner_benchmarking/100by100_10.yaml new file mode 100644 index 0000000000..104cb9a6fd --- /dev/null +++ b/tools/planner_benchmarking/100by100_10.yaml @@ -0,0 +1,6 @@ +image: 100by100_10.pgm +resolution: 0.05 +origin: [0.0, 0.000000, 0.000000] +negate: 1 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/tools/planner_benchmarking/100by100_15.yaml b/tools/planner_benchmarking/100by100_15.yaml new file mode 100644 index 0000000000..82277cf15b --- /dev/null +++ b/tools/planner_benchmarking/100by100_15.yaml @@ -0,0 +1,6 @@ +image: 100by100_15.pgm +resolution: 0.05 +origin: [0.0, 0.000000, 0.000000] +negate: 1 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/tools/planner_benchmarking/100by100_20.yaml b/tools/planner_benchmarking/100by100_20.yaml new file mode 100644 index 0000000000..763431b9e4 --- /dev/null +++ b/tools/planner_benchmarking/100by100_20.yaml @@ -0,0 +1,6 @@ +image: 100by100_20.pgm +resolution: 0.05 +origin: [0.0, 0.000000, 0.000000] +negate: 1 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 3865b0bbdb..2ff98a25be 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -21,6 +21,8 @@ import math import os import pickle +import glob +import time import numpy as np from random import seed @@ -29,13 +31,33 @@ from transforms3d.euler import euler2quat -# Note: Map origin is assumed to be (0,0) - +''' +Replace planner server with: + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["SmacHybrid", "Smac2d", "SmacLattice", "Navfn", "ThetaStar"] + SmacHybrid: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + Smac2d: + plugin: "nav2_smac_planner/SmacPlanner2D" + SmacLattice: + plugin: "nav2_smac_planner/SmacPlannerLattice" + Navfn: + plugin: "nav2_navfn_planner/NavfnPlanner" + ThetaStar: + plugin: "nav2_theta_star_planner/ThetaStarPlanner" + +Set global costmap settings to those desired for benchmarking. +The global map will be automatically set in the script. +''' def getPlannerResults(navigator, initial_pose, goal_pose, planners): results = [] for planner in planners: - path = navigator.getPath(initial_pose, goal_pose, planner) + path = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True) if path is not None: results.append(path) return results @@ -111,12 +133,17 @@ def main(): # Wait for navigation to fully activate navigator.waitUntilNav2Active() + # Set map to use, other options: 100by100_15, 100by100_10 + map_path = os.getcwd() + '/' + glob.glob('**/100by100_20.yaml', recursive=True)[0] + navigator.changeMap(map_path) + time.sleep(2) + # Get the costmap for start/goal validation costmap_msg = navigator.getGlobalCostmap() costmap = np.asarray(costmap_msg.data) costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) - planners = ['NavFn', 'Smac2d', 'SmacLattice', 'SmacHybrid'] + planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] max_cost = 210 side_buffer = 100 time_stamp = navigator.get_clock().now().to_msg() @@ -125,7 +152,8 @@ def main(): random_pairs = 100 res = costmap_msg.metadata.resolution - for i in range(random_pairs): + i = 0 + while len(results) != random_pairs: print("Cycle: ", i, "out of: ", random_pairs) start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) @@ -134,22 +162,20 @@ def main(): result = getPlannerResults(navigator, start, goal, planners) if len(result) == len(planners): results.append(result) + i = i + 1 else: print("One of the planners was invalid") print("Write Results...") - nav2_planner_metrics_dir = get_package_share_directory('nav2_planner_metrics') - with open(os.path.join(nav2_planner_metrics_dir, 'results.pickle'), 'wb') as f: + with open(os.getcwd() + '/results.pickle', 'wb+') as f: pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) - with open(os.path.join(nav2_planner_metrics_dir, 'costmap.pickle'), 'wb') as f: + with open(os.getcwd() + '/costmap.pickle', 'wb+') as f: pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL) - with open(os.path.join(nav2_planner_metrics_dir, 'planners.pickle'), 'wb') as f: + with open(os.getcwd() + '/planners.pickle', 'wb+') as f: pickle.dump(planners, f, pickle.HIGHEST_PROTOCOL) print("Write Complete") - - navigator.lifecycleShutdown() exit(0) diff --git a/tools/planner_benchmarking/process_data.py b/tools/planner_benchmarking/process_data.py index 443da00266..50be77cc48 100644 --- a/tools/planner_benchmarking/process_data.py +++ b/tools/planner_benchmarking/process_data.py @@ -126,15 +126,14 @@ def maxPathCost(paths, costmap, num_of_planners): def main(): - nav2_planner_metrics_dir = get_package_share_directory('nav2_planner_metrics') print("Read data") - with open(os.path.join(nav2_planner_metrics_dir, 'results.pickle'), 'rb') as f: + with open(os.getcwd() + '/results.pickle', 'rb') as f: results = pickle.load(f) - with open(os.path.join(nav2_planner_metrics_dir, 'planners.pickle'), 'rb') as f: + with open(os.getcwd() + '/planners.pickle', 'rb') as f: planners = pickle.load(f) - with open(os.path.join(nav2_planner_metrics_dir, 'costmap.pickle'), 'rb') as f: + with open(os.getcwd() + '/costmap.pickle', 'rb') as f: costmap = pickle.load(f) paths = getPaths(results) From 5ae4b73b020283062fc449c551cf00a6fd97d50e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 19 Sep 2022 18:27:07 -0700 Subject: [PATCH 063/131] [Smac] Robin hood data structure improves performance by 10-15%! (#3201) * adding robin_hood unordered_map * using robin_hood node map * ignore robin_hood file * linting * linting cont. for triple pointers * linting cont. for uncrustify --- .../nav2_simple_commander/robot_navigator.py | 2 +- nav2_smac_planner/CMakeLists.txt | 1 + .../include/nav2_smac_planner/a_star.hpp | 3 +- .../nav2_smac_planner/thirdparty/robin_hood.h | 2539 +++++++++++++++++ tools/code_coverage_report.bash | 2 +- 5 files changed, 2544 insertions(+), 3 deletions(-) create mode 100644 nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index d2e37c151b..86251077c3 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -331,7 +331,7 @@ def _getPathImpl(self, start, goal, planner_id='', use_start=False): def getPath(self, start, goal, planner_id='', use_start=False): """Send a `ComputePathToPose` action request.""" - rtn = _getPathImpl(start, goal, planner_id='', use_start=False) + rtn = self._getPathImpl(start, goal, planner_id='', use_start=False) if not rtn: return None else: diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index c21e856da8..79a77ce082 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -152,6 +152,7 @@ install(DIRECTORY lattice_primitives/sample_primitives DESTINATION share/${PROJE if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) add_subdirectory(test) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 2af454bb53..4b4fe55111 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -26,6 +26,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_smac_planner/thirdparty/robin_hood.h" #include "nav2_smac_planner/analytic_expansion.hpp" #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/node_hybrid.hpp" @@ -46,7 +47,7 @@ class AStarAlgorithm { public: typedef NodeT * NodePtr; - typedef std::unordered_map Graph; + typedef robin_hood::unordered_node_map Graph; typedef std::vector NodeVector; typedef std::pair> NodeElement; typedef typename NodeT::Coordinates Coordinates; diff --git a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h new file mode 100644 index 0000000000..5d3812a3a3 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h @@ -0,0 +1,2539 @@ +// Copyright (c) 2018-2021 Martin Ankerl +// ______ _____ ______ _________ +// ______________ ___ /_ ___(_)_______ ___ /_ ______ ______ ______ / +// __ ___/_ __ \__ __ \__ / __ __ \ __ __ \_ __ \_ __ \_ __ / +// _ / / /_/ /_ /_/ /_ / _ / / / _ / / // /_/ // /_/ // /_/ / +// /_/ \____/ /_.___/ /_/ /_/ /_/ ________/_/ /_/ \____/ \____/ \__,_/ +// _/_____/ +// +// Fast & memory efficient hashtable based on robin hood hashing for C++11/14/17/20 +// https://github.com/martinus/robin-hood-hashing +// +// Licensed under the MIT License . +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2021 Martin Ankerl +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ +#define NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ + +// see https://semver.org/ +#define ROBIN_HOOD_VERSION_MAJOR 3 // for incompatible API changes +#define ROBIN_HOOD_VERSION_MINOR 11 // for adding functionality in a backwards-compatible manner +#define ROBIN_HOOD_VERSION_PATCH 5 // for backwards-compatible bug fixes + +#include +#include +#include +#include +#include +#include // only to support hash of smart pointers +#include +#include +#include +#include +#include +#if __cplusplus >= 201703L +# include +#endif +/* *INDENT-OFF* */ + +// #define ROBIN_HOOD_LOG_ENABLED +#ifdef ROBIN_HOOD_LOG_ENABLED +# include +# define ROBIN_HOOD_LOG(...) \ + std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; +#else +# define ROBIN_HOOD_LOG(x) +#endif + +// #define ROBIN_HOOD_TRACE_ENABLED +#ifdef ROBIN_HOOD_TRACE_ENABLED +# include +# define ROBIN_HOOD_TRACE(...) \ + std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; +#else +# define ROBIN_HOOD_TRACE(x) +#endif + +// #define ROBIN_HOOD_COUNT_ENABLED +#ifdef ROBIN_HOOD_COUNT_ENABLED +# include +# define ROBIN_HOOD_COUNT(x) ++counts().x; +namespace robin_hood { +struct Counts { + uint64_t shiftUp{}; + uint64_t shiftDown{}; +}; +inline std::ostream& operator<<(std::ostream& os, Counts const& c) { + return os << c.shiftUp << " shiftUp" << std::endl << c.shiftDown << " shiftDown" << std::endl; +} + +static Counts& counts() { + static Counts counts{}; + return counts; +} +} // namespace robin_hood +#else +# define ROBIN_HOOD_COUNT(x) +#endif + +// all non-argument macros should use this facility. See +// https://www.fluentcpp.com/2019/05/28/better-macros-better-flags/ +#define ROBIN_HOOD(x) ROBIN_HOOD_PRIVATE_DEFINITION_##x() + +// mark unused members with this macro +#define ROBIN_HOOD_UNUSED(identifier) + +// bitness +#if SIZE_MAX == UINT32_MAX +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 32 +#elif SIZE_MAX == UINT64_MAX +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 +#else +# error Unsupported bitness +#endif + +// endianess +#ifdef _MSC_VER +# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() 1 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() 0 +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() \ + (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) +# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) +#endif + +// inline +#ifdef _MSC_VER +# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __declspec(noinline) +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __attribute__((noinline)) +#endif + +// exceptions +#if !defined(__cpp_exceptions) && !defined(__EXCEPTIONS) && !defined(_CPPUNWIND) +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 0 +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 1 +#endif + +// count leading/trailing bits +#if !defined(ROBIN_HOOD_DISABLE_INTRINSICS) +# ifdef _MSC_VER +# if ROBIN_HOOD(BITNESS) == 32 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward64 +# endif +# include +# pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD)) +# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ + [](size_t mask) noexcept -> int { \ + unsigned long index; // NOLINT \ + return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast(index) \ + : ROBIN_HOOD(BITNESS); \ + }(x) +# else +# if ROBIN_HOOD(BITNESS) == 32 +# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzl +# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzl +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzll +# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzll +# endif +# define ROBIN_HOOD_COUNT_LEADING_ZEROES(x) ((x) ? ROBIN_HOOD(CLZ)(x) : ROBIN_HOOD(BITNESS)) +# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) ((x) ? ROBIN_HOOD(CTZ)(x) : ROBIN_HOOD(BITNESS)) +# endif +#endif + +// fallthrough +#ifndef __has_cpp_attribute // For backwards compatibility +# define __has_cpp_attribute(x) 0 +#endif +#if __has_cpp_attribute(clang::fallthrough) +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[clang::fallthrough]] +#elif __has_cpp_attribute(gnu::fallthrough) +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[gnu::fallthrough]] +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() +#endif + +// likely/unlikely +#ifdef _MSC_VER +# define ROBIN_HOOD_LIKELY(condition) condition +# define ROBIN_HOOD_UNLIKELY(condition) condition +#else +# define ROBIN_HOOD_LIKELY(condition) __builtin_expect(condition, 1) +# define ROBIN_HOOD_UNLIKELY(condition) __builtin_expect(condition, 0) +#endif + +// detect if native wchar_t type is availiable in MSVC +#ifdef _MSC_VER +# ifdef _NATIVE_WCHAR_T_DEFINED +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 0 +# endif +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 +#endif + +// detect if MSVC supports the pair(std::piecewise_construct_t,...) consructor being constexpr +#ifdef _MSC_VER +# if _MSC_VER <= 1900 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 1 +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 +# endif +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 +#endif + +// workaround missing "is_trivially_copyable" in g++ < 5.0 +// See https://stackoverflow.com/a/31798726/48181 +#if defined(__GNUC__) && __GNUC__ < 5 +# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) __has_trivial_copy(__VA_ARGS__) +#else +# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) std::is_trivially_copyable<__VA_ARGS__>::value +#endif + +// helpers for C++ versions, see https://gcc.gnu.org/onlinedocs/cpp/Standard-Predefined-Macros.html +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX() __cplusplus +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX98() 199711L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX11() 201103L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX14() 201402L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX17() 201703L + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) +# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() [[nodiscard]] +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() +#endif + +namespace robin_hood { + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) +# define ROBIN_HOOD_STD std +#else + +// c++11 compatibility layer +namespace ROBIN_HOOD_STD { +template +struct alignment_of + : std::integral_constant::type)> {}; + +template +class integer_sequence { +public: + using value_type = T; + static_assert(std::is_integral::value, "not integral type"); + static constexpr std::size_t size() noexcept { + return sizeof...(Ints); + } +}; +template +using index_sequence = integer_sequence; + +namespace detail_ { +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0 && Begin < End, "unexpected argument (Begin<0 || Begin<=End)"); + + template + struct IntSeqCombiner; + + template + struct IntSeqCombiner, integer_sequence> { + using TResult = integer_sequence; + }; + + using TResult = + typename IntSeqCombiner::TResult, + typename IntSeqImpl::TResult>::TResult; +}; + +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0, "unexpected argument (Begin<0)"); + using TResult = integer_sequence; +}; + +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0, "unexpected argument (Begin<0)"); + using TResult = integer_sequence; +}; +} // namespace detail_ + +template +using make_integer_sequence = typename detail_::IntSeqImpl::TResult; + +template +using make_index_sequence = make_integer_sequence; + +template +using index_sequence_for = make_index_sequence; + +} // namespace ROBIN_HOOD_STD + +#endif + +namespace detail { + +// make sure we static_cast to the correct type for hash_int +#if ROBIN_HOOD(BITNESS) == 64 +using SizeT = uint64_t; +#else +using SizeT = uint32_t; +#endif + +template +T rotr(T x, unsigned k) { + return (x >> k) | (x << (8U * sizeof(T) - k)); +} + +// This cast gets rid of warnings like "cast from 'uint8_t*' {aka 'unsigned char*'} to +// 'uint64_t*' {aka 'long unsigned int*'} increases required alignment of target type". Use with +// care! +template +inline T reinterpret_cast_no_cast_align_warning(void* ptr) noexcept { + return reinterpret_cast(ptr); +} + +template +inline T reinterpret_cast_no_cast_align_warning(void const* ptr) noexcept { + return reinterpret_cast(ptr); +} + +// make sure this is not inlined as it is slow and dramatically enlarges code, thus making other +// inlinings more difficult. Throws are also generally the slow path. +template +[[noreturn]] ROBIN_HOOD(NOINLINE) +#if ROBIN_HOOD(HAS_EXCEPTIONS) + void doThrow(Args&&... args) { + throw E(std::forward(args)...); +} +#else + void doThrow(Args&&... ROBIN_HOOD_UNUSED(args) /*unused*/) { + abort(); +} +#endif + +template +T* assertNotNull(T* t, Args&&... args) { + if (ROBIN_HOOD_UNLIKELY(nullptr == t)) { + doThrow(std::forward(args)...); + } + return t; +} + +template +inline T unaligned_load(void const* ptr) noexcept { + // using memcpy so we don't get into unaligned load problems. + // compiler should optimize this very well anyways. + T t; + std::memcpy(&t, ptr, sizeof(T)); + return t; +} + +// Allocates bulks of memory for objects of type T. This deallocates the memory in the destructor, +// and keeps a linked list of the allocated memory around. Overhead per allocation is the size of a +// pointer. +template +class BulkPoolAllocator { +public: + BulkPoolAllocator() noexcept = default; + + // does not copy anything, just creates a new allocator. + BulkPoolAllocator(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept + : mHead(nullptr) + , mListForFree(nullptr) {} + + BulkPoolAllocator(BulkPoolAllocator&& o) noexcept + : mHead(o.mHead) + , mListForFree(o.mListForFree) { + o.mListForFree = nullptr; + o.mHead = nullptr; + } + + BulkPoolAllocator& operator=(BulkPoolAllocator&& o) noexcept { + reset(); + mHead = o.mHead; + mListForFree = o.mListForFree; + o.mListForFree = nullptr; + o.mHead = nullptr; + return *this; + } + + BulkPoolAllocator& + operator=(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept { + // does not do anything + return *this; + } + + ~BulkPoolAllocator() noexcept { + reset(); + } + + // Deallocates all allocated memory. + void reset() noexcept { + while (mListForFree) { + T* tmp = *mListForFree; + ROBIN_HOOD_LOG("std::free") + std::free(mListForFree); + mListForFree = reinterpret_cast_no_cast_align_warning(tmp); + } + mHead = nullptr; + } + + // allocates, but does NOT initialize. Use in-place new constructor, e.g. + // T* obj = pool.allocate(); + // ::new (static_cast(obj)) T(); + T* allocate() { + T* tmp = mHead; + if (!tmp) { + tmp = performAllocation(); + } + + mHead = *reinterpret_cast_no_cast_align_warning(tmp); + return tmp; + } + + // does not actually deallocate but puts it in store. + // make sure you have already called the destructor! e.g. with + // obj->~T(); + // pool.deallocate(obj); + void deallocate(T* obj) noexcept { + *reinterpret_cast_no_cast_align_warning(obj) = mHead; + mHead = obj; + } + + // Adds an already allocated block of memory to the allocator. This allocator is from now on + // responsible for freeing the data (with free()). If the provided data is not large enough to + // make use of, it is immediately freed. Otherwise it is reused and freed in the destructor. + void addOrFree(void* ptr, const size_t numBytes) noexcept { + // calculate number of available elements in ptr + if (numBytes < ALIGNMENT + ALIGNED_SIZE) { + // not enough data for at least one element. Free and return. + ROBIN_HOOD_LOG("std::free") + std::free(ptr); + } else { + ROBIN_HOOD_LOG("add to buffer") + add(ptr, numBytes); + } + } + + void swap(BulkPoolAllocator& other) noexcept { + using std::swap; + swap(mHead, other.mHead); + swap(mListForFree, other.mListForFree); + } + +private: + // iterates the list of allocated memory to calculate how many to alloc next. + // Recalculating this each time saves us a size_t member. + // This ignores the fact that memory blocks might have been added manually with addOrFree. In + // practice, this should not matter much. + ROBIN_HOOD(NODISCARD) size_t calcNumElementsToAlloc() const noexcept { + auto tmp = mListForFree; + size_t numAllocs = MinNumAllocs; + + while (numAllocs * 2 <= MaxNumAllocs && tmp) { + auto x = reinterpret_cast(tmp); // NOLINT + tmp = *x; + numAllocs *= 2; + } + + return numAllocs; + } + + // WARNING: Underflow if numBytes < ALIGNMENT! This is guarded in addOrFree(). + void add(void* ptr, const size_t numBytes) noexcept { + const size_t numElements = (numBytes - ALIGNMENT) / ALIGNED_SIZE; + auto data = reinterpret_cast(ptr); + + // link free list + auto x = reinterpret_cast(data); + *x = mListForFree; + mListForFree = data; + + // create linked list for newly allocated data + auto* const headT = + reinterpret_cast_no_cast_align_warning(reinterpret_cast(ptr) + ALIGNMENT); + + auto* const head = reinterpret_cast(headT); + + // Visual Studio compiler automatically unrolls this loop, which is pretty cool + for (size_t i = 0; i < numElements; ++i) { + *reinterpret_cast_no_cast_align_warning(head + i * ALIGNED_SIZE) = + head + (i + 1) * ALIGNED_SIZE; + } + + // last one points to 0 + *reinterpret_cast_no_cast_align_warning(head + (numElements - 1) * ALIGNED_SIZE) = + mHead; + mHead = headT; + } + + // Called when no memory is available (mHead == 0). + // Don't inline this slow path. + ROBIN_HOOD(NOINLINE) T* performAllocation() { + size_t const numElementsToAlloc = calcNumElementsToAlloc(); + + // alloc new memory: [prev |T, T, ... T] + size_t const bytes = ALIGNMENT + ALIGNED_SIZE * numElementsToAlloc; + ROBIN_HOOD_LOG("std::malloc " << bytes << " = " << ALIGNMENT << " + " << ALIGNED_SIZE + << " * " << numElementsToAlloc) + add(assertNotNull(std::malloc(bytes)), bytes); + return mHead; + } + + // enforce byte alignment of the T's +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) + static constexpr size_t ALIGNMENT = + (std::max)(std::alignment_of::value, std::alignment_of::value); +#else + static const size_t ALIGNMENT = + (ROBIN_HOOD_STD::alignment_of::value > ROBIN_HOOD_STD::alignment_of::value) + ? ROBIN_HOOD_STD::alignment_of::value + : +ROBIN_HOOD_STD::alignment_of::value; // the + is for walkarround +#endif + + static constexpr size_t ALIGNED_SIZE = ((sizeof(T) - 1) / ALIGNMENT + 1) * ALIGNMENT; + + static_assert(MinNumAllocs >= 1, "MinNumAllocs"); + static_assert(MaxNumAllocs >= MinNumAllocs, "MaxNumAllocs"); + static_assert(ALIGNED_SIZE >= sizeof(T*), "ALIGNED_SIZE"); + static_assert(0 == (ALIGNED_SIZE % sizeof(T*)), "ALIGNED_SIZE mod"); + static_assert(ALIGNMENT >= sizeof(T*), "ALIGNMENT"); + + T* mHead{nullptr}; + T** mListForFree{nullptr}; +}; + +template +struct NodeAllocator; + +// dummy allocator that does nothing +template +struct NodeAllocator { + // we are not using the data, so just free it. + void addOrFree(void* ptr, size_t ROBIN_HOOD_UNUSED(numBytes)/*unused*/) noexcept { + ROBIN_HOOD_LOG("std::free") + std::free(ptr); + } +}; + +template +struct NodeAllocator : public BulkPoolAllocator {}; + +// c++14 doesn't have is_nothrow_swappable, and clang++ 6.0.1 doesn't like it either, so I'm making +// my own here. +namespace swappable { +#if ROBIN_HOOD(CXX) < ROBIN_HOOD(CXX17) +using std::swap; +template +struct nothrow { + static const bool value = noexcept(swap(std::declval(), std::declval())); +}; +#else +template +struct nothrow { + static const bool value = std::is_nothrow_swappable::value; +}; +#endif +} // namespace swappable + +} // namespace detail + +struct is_transparent_tag {}; + +// A custom pair implementation is used in the map because std::pair is not is_trivially_copyable, +// which means it would not be allowed to be used in std::memcpy. This struct is copyable, which is +// also tested. +template +struct pair { + using first_type = T1; + using second_type = T2; + + template ::value && + std::is_default_constructible::value>::type> + constexpr pair() noexcept(noexcept(U1()) && noexcept(U2())) + : first() + , second() {} + + // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. + explicit constexpr pair(std::pair const& o) noexcept( + noexcept(T1(std::declval())) && noexcept(T2(std::declval()))) + : first(o.first) + , second(o.second) {} + + // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. + explicit constexpr pair(std::pair&& o) noexcept(noexcept( + T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) + : first(std::move(o.first)) + , second(std::move(o.second)) {} + + constexpr pair(T1&& a, T2&& b) noexcept(noexcept( + T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) + : first(std::move(a)) + , second(std::move(b)) {} + + template + constexpr pair(U1&& a, U2&& b) noexcept(noexcept(T1(std::forward( + std::declval()))) && noexcept(T2(std::forward(std::declval())))) + : first(std::forward(a)) + , second(std::forward(b)) {} + + template + // MSVC 2015 produces error "C2476: ‘constexpr’ constructor does not initialize all members" + // if this constructor is constexpr +#if !ROBIN_HOOD(BROKEN_CONSTEXPR) + constexpr +#endif + pair(std::piecewise_construct_t /*unused*/, std::tuple a, + std::tuple + b) noexcept(noexcept(pair(std::declval&>(), + std::declval&>(), + ROBIN_HOOD_STD::index_sequence_for(), + ROBIN_HOOD_STD::index_sequence_for()))) + : pair(a, b, ROBIN_HOOD_STD::index_sequence_for(), + ROBIN_HOOD_STD::index_sequence_for()) { + } + + // constructor called from the std::piecewise_construct_t ctor + template + pair( + std::tuple& a, std::tuple& b, + ROBIN_HOOD_STD::index_sequence /*unused*/, + ROBIN_HOOD_STD::index_sequence /*unused*/) noexcept( + noexcept(T1(std::forward(std::get( + std::declval&>()))...)) && noexcept(T2(std:: + forward(std::get( + std::declval&>()))...))) + : first(std::forward(std::get(a))...) + , second(std::forward(std::get(b))...) { + // make visual studio compiler happy about warning about unused a & b. + // Visual studio's pair implementation disables warning 4100. + (void)a; + (void)b; + } + + void swap(pair& o) noexcept((detail::swappable::nothrow::value) && + (detail::swappable::nothrow::value)) { + using std::swap; + swap(first, o.first); + swap(second, o.second); + } + + T1 first; // NOLINT + T2 second; // NOLINT +}; + +template +inline void swap(pair& a, pair& b) noexcept( + noexcept(std::declval&>().swap(std::declval&>()))) { + a.swap(b); +} + +template +inline constexpr bool operator==(pair const& x, pair const& y) { + return (x.first == y.first) && (x.second == y.second); +} +template +inline constexpr bool operator!=(pair const& x, pair const& y) { + return !(x == y); +} +template +inline constexpr bool operator<(pair const& x, pair const& y) noexcept(noexcept( + std::declval() < std::declval()) && noexcept(std::declval() < + std::declval())) { + return x.first < y.first || (!(y.first < x.first) && x.second < y.second); +} +template +inline constexpr bool operator>(pair const& x, pair const& y) { + return y < x; +} +template +inline constexpr bool operator<=(pair const& x, pair const& y) { + return !(x > y); +} +template +inline constexpr bool operator>=(pair const& x, pair const& y) { + return !(x < y); +} + +inline size_t hash_bytes(void const* ptr, size_t len) noexcept { + static constexpr uint64_t m = UINT64_C(0xc6a4a7935bd1e995); + static constexpr uint64_t seed = UINT64_C(0xe17a1465); + static constexpr unsigned int r = 47; + + auto const* const data64 = static_cast(ptr); + uint64_t h = seed ^ (len * m); + + size_t const n_blocks = len / 8; + for (size_t i = 0; i < n_blocks; ++i) { + auto k = detail::unaligned_load(data64 + i); + + k *= m; + k ^= k >> r; + k *= m; + + h ^= k; + h *= m; + } + + auto const* const data8 = reinterpret_cast(data64 + n_blocks); + switch (len & 7U) { + case 7: + h ^= static_cast(data8[6]) << 48U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 6: + h ^= static_cast(data8[5]) << 40U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 5: + h ^= static_cast(data8[4]) << 32U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 4: + h ^= static_cast(data8[3]) << 24U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 3: + h ^= static_cast(data8[2]) << 16U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 2: + h ^= static_cast(data8[1]) << 8U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 1: + h ^= static_cast(data8[0]); + h *= m; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + default: + break; + } + + h ^= h >> r; + + // not doing the final step here, because this will be done by keyToIdx anyways + // h *= m; + // h ^= h >> r; + return static_cast(h); +} + +inline size_t hash_int(uint64_t x) noexcept { + // tried lots of different hashes, let's stick with murmurhash3. It's simple, fast, well tested, + // and doesn't need any special 128bit operations. + x ^= x >> 33U; + x *= UINT64_C(0xff51afd7ed558ccd); + x ^= x >> 33U; + + // not doing the final step here, because this will be done by keyToIdx anyways + // x *= UINT64_C(0xc4ceb9fe1a85ec53); + // x ^= x >> 33U; + return static_cast(x); +} + +// A thin wrapper around std::hash, performing an additional simple mixing step of the result. +template +struct hash : public std::hash { + size_t operator()(T const& obj) const + noexcept(noexcept(std::declval>().operator()(std::declval()))) { + // call base hash + auto result = std::hash::operator()(obj); + // return mixed of that, to be save against identity has + return hash_int(static_cast(result)); + } +}; + +template +struct hash> { + size_t operator()(std::basic_string const& str) const noexcept { + return hash_bytes(str.data(), sizeof(CharT) * str.size()); + } +}; + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) +template +struct hash> { + size_t operator()(std::basic_string_view const& sv) const noexcept { + return hash_bytes(sv.data(), sizeof(CharT) * sv.size()); + } +}; +#endif + +template +struct hash { + size_t operator()(T* ptr) const noexcept { + return hash_int(reinterpret_cast(ptr)); + } +}; + +template +struct hash> { + size_t operator()(std::unique_ptr const& ptr) const noexcept { + return hash_int(reinterpret_cast(ptr.get())); + } +}; + +template +struct hash> { + size_t operator()(std::shared_ptr const& ptr) const noexcept { + return hash_int(reinterpret_cast(ptr.get())); + } +}; + +template +struct hash::value>::type> { + size_t operator()(Enum e) const noexcept { + using Underlying = typename std::underlying_type::type; + return hash{}(static_cast(e)); + } +}; + +#define ROBIN_HOOD_HASH_INT(T) \ + template <> \ + struct hash { \ + size_t operator()(T const& obj) const noexcept { \ + return hash_int(static_cast(obj)); \ + } \ + } + +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wuseless-cast" +#endif +// see https://en.cppreference.com/w/cpp/utility/hash +ROBIN_HOOD_HASH_INT(bool); +ROBIN_HOOD_HASH_INT(char); +ROBIN_HOOD_HASH_INT(signed char); +ROBIN_HOOD_HASH_INT(unsigned char); +ROBIN_HOOD_HASH_INT(char16_t); +ROBIN_HOOD_HASH_INT(char32_t); +#if ROBIN_HOOD(HAS_NATIVE_WCHART) +ROBIN_HOOD_HASH_INT(wchar_t); +#endif +ROBIN_HOOD_HASH_INT(short); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned short); // NOLINT +ROBIN_HOOD_HASH_INT(int); +ROBIN_HOOD_HASH_INT(unsigned int); +ROBIN_HOOD_HASH_INT(long); // NOLINT +ROBIN_HOOD_HASH_INT(long long); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned long); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned long long); // NOLINT +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic pop +#endif +namespace detail { + +template +struct void_type { + using type = void; +}; + +template +struct has_is_transparent : public std::false_type {}; + +template +struct has_is_transparent::type> + : public std::true_type {}; + +// using wrapper classes for hash and key_equal prevents the diamond problem when the same type +// is used. see https://stackoverflow.com/a/28771920/48181 +template +struct WrapHash : public T { + WrapHash() = default; + explicit WrapHash(T const& o) noexcept(noexcept(T(std::declval()))) + : T(o) {} +}; + +template +struct WrapKeyEqual : public T { + WrapKeyEqual() = default; + explicit WrapKeyEqual(T const& o) noexcept(noexcept(T(std::declval()))) + : T(o) {} +}; + +// A highly optimized hashmap implementation, using the Robin Hood algorithm. +// +// In most cases, this map should be usable as a drop-in replacement for std::unordered_map, but +// be about 2x faster in most cases and require much less allocations. +// +// This implementation uses the following memory layout: +// +// [Node, Node, ... Node | info, info, ... infoSentinel ] +// +// * Node: either a DataNode that directly has the std::pair as member, +// or a DataNode with a pointer to std::pair. Which DataNode representation to use +// depends on how fast the swap() operation is. Heuristically, this is automatically choosen +// based on sizeof(). there are always 2^n Nodes. +// +// * info: Each Node in the map has a corresponding info byte, so there are 2^n info bytes. +// Each byte is initialized to 0, meaning the corresponding Node is empty. Set to 1 means the +// corresponding node contains data. Set to 2 means the corresponding Node is filled, but it +// actually belongs to the previous position and was pushed out because that place is already +// taken. +// +// * infoSentinel: Sentinel byte set to 1, so that iterator's ++ can stop at end() without the +// need for a idx variable. +// +// According to STL, order of templates has effect on throughput. That's why I've moved the +// boolean to the front. +// https://www.reddit.com/r/cpp/comments/ahp6iu/compile_time_binary_size_reductions_and_cs_future/eeguck4/ +template +class Table + : public WrapHash, + public WrapKeyEqual, + detail::NodeAllocator< + typename std::conditional< + std::is_void::value, Key, + robin_hood::pair::type, T>>::type, + 4, 16384, IsFlat> { +public: + static constexpr bool is_flat = IsFlat; + static constexpr bool is_map = !std::is_void::value; + static constexpr bool is_set = !is_map; + static constexpr bool is_transparent = + has_is_transparent::value && has_is_transparent::value; + + using key_type = Key; + using mapped_type = T; + using value_type = typename std::conditional< + is_set, Key, + robin_hood::pair::type, T>>::type; + using size_type = size_t; + using hasher = Hash; + using key_equal = KeyEqual; + using Self = Table; + +private: + static_assert(MaxLoadFactor100 > 10 && MaxLoadFactor100 < 100, + "MaxLoadFactor100 needs to be >10 && < 100"); + + using WHash = WrapHash; + using WKeyEqual = WrapKeyEqual; + + // configuration defaults + + // make sure we have 8 elements, needed to quickly rehash mInfo + static constexpr size_t InitialNumElements = sizeof(uint64_t); + static constexpr uint32_t InitialInfoNumBits = 5; + static constexpr uint8_t InitialInfoInc = 1U << InitialInfoNumBits; + static constexpr size_t InfoMask = InitialInfoInc - 1U; + static constexpr uint8_t InitialInfoHashShift = 0; + using DataPool = detail::NodeAllocator; + + // type needs to be wider than uint8_t. + using InfoType = uint32_t; + + // DataNode //////////////////////////////////////////////////////// + + // Primary template for the data node. We have special implementations for small and big + // objects. For large objects it is assumed that swap() is fairly slow, so we allocate these + // on the heap so swap merely swaps a pointer. + template + class DataNode {}; + + // Small: just allocate on the stack. + template + class DataNode final { + public: + template + explicit DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, Args&&... args) noexcept( + noexcept(value_type(std::forward(args)...))) + : mData(std::forward(args)...) {} + + DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept( + std::is_nothrow_move_constructible::value) + : mData(std::move(n.mData)) {} + + // doesn't do anything + void destroy(M& ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {} + void destroyDoNotDeallocate() noexcept {} + + value_type const* operator->() const noexcept { + return &mData; + } + value_type* operator->() noexcept { + return &mData; + } + + const value_type& operator*() const noexcept { + return mData; + } + + value_type& operator*() noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData.first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type + getFirst() const noexcept { + return mData.first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() const noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() noexcept { + return mData.second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() const noexcept { + return mData.second; + } + + void swap(DataNode& o) noexcept( + noexcept(std::declval().swap(std::declval()))) { + mData.swap(o.mData); + } + + private: + value_type mData; + }; + + // big object: allocate on heap. + template + class DataNode { + public: + template + explicit DataNode(M& map, Args&&... args) + : mData(map.allocate()) { + ::new (static_cast(mData)) value_type(std::forward(args)...); + } + + DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept + : mData(std::move(n.mData)) {} + + void destroy(M& map) noexcept { + // don't deallocate, just put it into list of datapool. + mData->~value_type(); + map.deallocate(mData); + } + + void destroyDoNotDeallocate() noexcept { + mData->~value_type(); + } + + value_type const* operator->() const noexcept { + return mData; + } + + value_type* operator->() noexcept { + return mData; + } + + const value_type& operator*() const { + return *mData; + } + + value_type& operator*() { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData->first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type + getFirst() const noexcept { + return mData->first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() const noexcept { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() noexcept { + return mData->second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() const noexcept { + return mData->second; + } + + void swap(DataNode& o) noexcept { + using std::swap; + swap(mData, o.mData); + } + + private: + value_type* mData; + }; + + using Node = DataNode; + + // helpers for insertKeyPrepareEmptySpot: extract first entry (only const required) + ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(Node const& n) const noexcept { + return n.getFirst(); + } + + // in case we have void mapped_type, we are not using a pair, thus we just route k through. + // No need to disable this because it's just not used if not applicable. + ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(key_type const& k) const noexcept { + return k; + } + + // in case we have non-void mapped_type, we have a standard robin_hood::pair + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, key_type const&>::type + getFirstConst(value_type const& vt) const noexcept { + return vt.first; + } + + // Cloner ////////////////////////////////////////////////////////// + + template + struct Cloner; + + // fast path: Just copy data, without allocating anything. + template + struct Cloner { + void operator()(M const& source, M& target) const { + auto const* const src = reinterpret_cast(source.mKeyVals); + auto* tgt = reinterpret_cast(target.mKeyVals); + auto const numElementsWithBuffer = target.calcNumElementsWithBuffer(target.mMask + 1); + std::copy(src, src + target.calcNumBytesTotal(numElementsWithBuffer), tgt); + } + }; + + template + struct Cloner { + void operator()(M const& s, M& t) const { + auto const numElementsWithBuffer = t.calcNumElementsWithBuffer(t.mMask + 1); + std::copy(s.mInfo, s.mInfo + t.calcNumBytesInfo(numElementsWithBuffer), t.mInfo); + + for (size_t i = 0; i < numElementsWithBuffer; ++i) { + if (t.mInfo[i]) { + ::new (static_cast(t.mKeyVals + i)) Node(t, *s.mKeyVals[i]); + } + } + } + }; + + // Destroyer /////////////////////////////////////////////////////// + + template + struct Destroyer {}; + + template + struct Destroyer { + void nodes(M& m) const noexcept { + m.mNumElements = 0; + } + + void nodesDoNotDeallocate(M& m) const noexcept { + m.mNumElements = 0; + } + }; + + template + struct Destroyer { + void nodes(M& m) const noexcept { + m.mNumElements = 0; + // clear also resets mInfo to 0, that's sometimes not necessary. + auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); + + for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { + if (0 != m.mInfo[idx]) { + Node& n = m.mKeyVals[idx]; + n.destroy(m); + n.~Node(); + } + } + } + + void nodesDoNotDeallocate(M& m) const noexcept { + m.mNumElements = 0; + // clear also resets mInfo to 0, that's sometimes not necessary. + auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); + for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { + if (0 != m.mInfo[idx]) { + Node& n = m.mKeyVals[idx]; + n.destroyDoNotDeallocate(); + n.~Node(); + } + } + } + }; + + // Iter //////////////////////////////////////////////////////////// + + struct fast_forward_tag {}; + + // generic iterator for both const_iterator and iterator. + template + class Iter { + private: + using NodePtr = typename std::conditional::type; + + public: + using difference_type = std::ptrdiff_t; + using value_type = typename Self::value_type; + using reference = typename std::conditional::type; + using pointer = typename std::conditional::type; + using iterator_category = std::forward_iterator_tag; + + // default constructed iterator can be compared to itself, but WON'T return true when + // compared to end(). + Iter() = default; + + // Rule of zero: nothing specified. The conversion constructor is only enabled for + // iterator to const_iterator, so it doesn't accidentally work as a copy ctor. + + // Conversion constructor from iterator to const_iterator. + template ::type> + Iter(Iter const& other) noexcept + : mKeyVals(other.mKeyVals) + , mInfo(other.mInfo) {} + + Iter(NodePtr valPtr, uint8_t const* infoPtr) noexcept + : mKeyVals(valPtr) + , mInfo(infoPtr) {} + + Iter(NodePtr valPtr, uint8_t const* infoPtr, + fast_forward_tag ROBIN_HOOD_UNUSED(tag) /*unused*/) noexcept + : mKeyVals(valPtr) + , mInfo(infoPtr) { + fastForward(); + } + + template ::type> + Iter& operator=(Iter const& other) noexcept { + mKeyVals = other.mKeyVals; + mInfo = other.mInfo; + return *this; + } + + // prefix increment. Undefined behavior if we are at end()! + Iter& operator++() noexcept { + mInfo++; + mKeyVals++; + fastForward(); + return *this; + } + + Iter operator++(int) noexcept { + Iter tmp = *this; + ++(*this); + return tmp; + } + + reference operator*() const { + return **mKeyVals; + } + + pointer operator->() const { + return &**mKeyVals; + } + + template + bool operator==(Iter const& o) const noexcept { + return mKeyVals == o.mKeyVals; + } + + template + bool operator!=(Iter const& o) const noexcept { + return mKeyVals != o.mKeyVals; + } + + private: + // fast forward to the next non-free info byte + // I've tried a few variants that don't depend on intrinsics, but unfortunately they are + // quite a bit slower than this one. So I've reverted that change again. See map_benchmark. + void fastForward() noexcept { + size_t n = 0; + while (0U == (n = detail::unaligned_load(mInfo))) { + mInfo += sizeof(size_t); + mKeyVals += sizeof(size_t); + } +#if defined(ROBIN_HOOD_DISABLE_INTRINSICS) + // we know for certain that within the next 8 bytes we'll find a non-zero one. + if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { + mInfo += 4; + mKeyVals += 4; + } + if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { + mInfo += 2; + mKeyVals += 2; + } + if (ROBIN_HOOD_UNLIKELY(0U == *mInfo)) { + mInfo += 1; + mKeyVals += 1; + } +#else +# if ROBIN_HOOD(LITTLE_ENDIAN) + auto inc = ROBIN_HOOD_COUNT_TRAILING_ZEROES(n) / 8; +# else + auto inc = ROBIN_HOOD_COUNT_LEADING_ZEROES(n) / 8; +# endif + mInfo += inc; + mKeyVals += inc; +#endif + } + + friend class Table; + NodePtr mKeyVals{nullptr}; + uint8_t const* mInfo{nullptr}; + }; + + //////////////////////////////////////////////////////////////////// + + // highly performance relevant code. + // Lower bits are used for indexing into the array (2^n size) + // The upper 1-5 bits need to be a reasonable good hash, to save comparisons. + template + void keyToIdx(HashKey&& key, size_t* idx, InfoType* info) const { + // In addition to whatever hash is used, add another mul & shift so we get better hashing. + // This serves as a bad hash prevention, if the given data is + // badly mixed. + auto h = static_cast(WHash::operator()(key)); + + h *= mHashMultiplier; + h ^= h >> 33U; + + // the lower InitialInfoNumBits are reserved for info. + *info = mInfoInc + static_cast((h & InfoMask) >> mInfoHashShift); + *idx = (static_cast(h) >> InitialInfoNumBits) & mMask; + } + + // forwards the index by one, wrapping around at the end + void next(InfoType* info, size_t* idx) const noexcept { + *idx = *idx + 1; + *info += mInfoInc; + } + + void nextWhileLess(InfoType* info, size_t* idx) const noexcept { + // unrolling this by hand did not bring any speedups. + while (*info < mInfo[*idx]) { + next(info, idx); + } + } + + // Shift everything up by one element. Tries to move stuff around. + void + shiftUp(size_t startIdx, + size_t const insertion_idx) noexcept(std::is_nothrow_move_assignable::value) { + auto idx = startIdx; + ::new (static_cast(mKeyVals + idx)) Node(std::move(mKeyVals[idx - 1])); + while (--idx != insertion_idx) { + mKeyVals[idx] = std::move(mKeyVals[idx - 1]); + } + + idx = startIdx; + while (idx != insertion_idx) { + ROBIN_HOOD_COUNT(shiftUp) + mInfo[idx] = static_cast(mInfo[idx - 1] + mInfoInc); + if (ROBIN_HOOD_UNLIKELY(mInfo[idx] + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + --idx; + } + } + + void shiftDown(size_t idx) noexcept(std::is_nothrow_move_assignable::value) { + // until we find one that is either empty or has zero offset. + // TODO(martinus) we don't need to move everything, just the last one for the same + // bucket. + mKeyVals[idx].destroy(*this); + + // until we find one that is either empty or has zero offset. + while (mInfo[idx + 1] >= 2 * mInfoInc) { + ROBIN_HOOD_COUNT(shiftDown) + mInfo[idx] = static_cast(mInfo[idx + 1] - mInfoInc); + mKeyVals[idx] = std::move(mKeyVals[idx + 1]); + ++idx; + } + + mInfo[idx] = 0; + // don't destroy, we've moved it + // mKeyVals[idx].destroy(*this); + mKeyVals[idx].~Node(); + } + + // copy of find(), except that it returns iterator instead of const_iterator. + template + ROBIN_HOOD(NODISCARD) + size_t findIdx(Other const& key) const { + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + + do { + // unrolling this twice gives a bit of a speedup. More unrolling did not help. + if (info == mInfo[idx] && + ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { + return idx; + } + next(&info, &idx); + if (info == mInfo[idx] && + ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { + return idx; + } + next(&info, &idx); + } while (info <= mInfo[idx]); + + // nothing found! + return mMask == 0 ? 0 + : static_cast(std::distance( + mKeyVals, reinterpret_cast_no_cast_align_warning(mInfo))); + } + + void cloneData(const Table& o) { + Cloner()(o, *this); + } + + // inserts a keyval that is guaranteed to be new, e.g. when the hashmap is resized. + // @return True on success, false if something went wrong + void insert_move(Node&& keyval) { + // we don't retry, fail if overflowing + // don't need to check max num elements + if (0 == mMaxNumElementsAllowed && !try_increase_info()) { + throwOverflowError(); + } + + size_t idx{}; + InfoType info{}; + keyToIdx(keyval.getFirst(), &idx, &info); + + // skip forward. Use <= because we are certain that the element is not there. + while (info <= mInfo[idx]) { + idx = idx + 1; + info += mInfoInc; + } + + // key not found, so we are now exactly where we want to insert it. + auto const insertion_idx = idx; + auto const insertion_info = static_cast(info); + if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + + // find an empty spot + while (0 != mInfo[idx]) { + next(&info, &idx); + } + + auto& l = mKeyVals[insertion_idx]; + if (idx == insertion_idx) { + ::new (static_cast(&l)) Node(std::move(keyval)); + } else { + shiftUp(idx, insertion_idx); + l = std::move(keyval); + } + + // put at empty spot + mInfo[insertion_idx] = insertion_info; + + ++mNumElements; + } + +public: + using iterator = Iter; + using const_iterator = Iter; + + Table() noexcept(noexcept(Hash()) && noexcept(KeyEqual())) + : WHash() + , WKeyEqual() { + ROBIN_HOOD_TRACE(this) + } + + // Creates an empty hash map. Nothing is allocated yet, this happens at the first insert. + // This tremendously speeds up ctor & dtor of a map that never receives an element. The + // penalty is payed at the first insert, and not before. Lookup of this empty map works + // because everybody points to DummyInfoByte::b. parameter bucket_count is dictated by the + // standard, but we can ignore it. + explicit Table( + size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/, const Hash& h = Hash{}, + const KeyEqual& equal = KeyEqual{}) noexcept(noexcept(Hash(h)) && noexcept(KeyEqual(equal))) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + } + + template + Table(Iter first, Iter last, size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, + const Hash& h = Hash{}, const KeyEqual& equal = KeyEqual{}) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + insert(first, last); + } + + Table(std::initializer_list initlist, + size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, const Hash& h = Hash{}, + const KeyEqual& equal = KeyEqual{}) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + insert(initlist.begin(), initlist.end()); + } + + Table(Table&& o) noexcept + : WHash(std::move(static_cast(o))) + , WKeyEqual(std::move(static_cast(o))) + , DataPool(std::move(static_cast(o))) { + ROBIN_HOOD_TRACE(this) + if (o.mMask) { + mHashMultiplier = std::move(o.mHashMultiplier); + mKeyVals = std::move(o.mKeyVals); + mInfo = std::move(o.mInfo); + mNumElements = std::move(o.mNumElements); + mMask = std::move(o.mMask); + mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); + mInfoInc = std::move(o.mInfoInc); + mInfoHashShift = std::move(o.mInfoHashShift); + // set other's mask to 0 so its destructor won't do anything + o.init(); + } + } + + Table& operator=(Table&& o) noexcept { + ROBIN_HOOD_TRACE(this) + if (&o != this) { + if (o.mMask) { + // only move stuff if the other map actually has some data + destroy(); + mHashMultiplier = std::move(o.mHashMultiplier); + mKeyVals = std::move(o.mKeyVals); + mInfo = std::move(o.mInfo); + mNumElements = std::move(o.mNumElements); + mMask = std::move(o.mMask); + mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); + mInfoInc = std::move(o.mInfoInc); + mInfoHashShift = std::move(o.mInfoHashShift); + WHash::operator=(std::move(static_cast(o))); + WKeyEqual::operator=(std::move(static_cast(o))); + DataPool::operator=(std::move(static_cast(o))); + + o.init(); + + } else { + // nothing in the other map => just clear us. + clear(); + } + } + return *this; + } + + Table(const Table& o) + : WHash(static_cast(o)) + , WKeyEqual(static_cast(o)) + , DataPool(static_cast(o)) { + ROBIN_HOOD_TRACE(this) + if (!o.empty()) { + // not empty: create an exact copy. it is also possible to just iterate through all + // elements and insert them, but copying is probably faster. + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + + ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mHashMultiplier = o.mHashMultiplier; + mKeyVals = static_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + // no need for calloc because clonData does memcpy + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + mNumElements = o.mNumElements; + mMask = o.mMask; + mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; + mInfoInc = o.mInfoInc; + mInfoHashShift = o.mInfoHashShift; + cloneData(o); + } + } + + // Creates a copy of the given map. Copy constructor of each entry is used. + // Not sure why clang-tidy thinks this doesn't handle self assignment, it does + Table& operator=(Table const& o) { + ROBIN_HOOD_TRACE(this) + if (&o == this) { + // prevent assigning of itself + return *this; + } + + // we keep using the old allocator and not assign the new one, because we want to keep + // the memory available. when it is the same size. + if (o.empty()) { + if (0 == mMask) { + // nothing to do, we are empty too + return *this; + } + + // not empty: destroy what we have there + // clear also resets mInfo to 0, that's sometimes not necessary. + destroy(); + init(); + WHash::operator=(static_cast(o)); + WKeyEqual::operator=(static_cast(o)); + DataPool::operator=(static_cast(o)); + + return *this; + } + + // clean up old stuff + Destroyer::value>{}.nodes(*this); + + if (mMask != o.mMask) { + // no luck: we don't have the same array size allocated, so we need to realloc. + if (0 != mMask) { + // only deallocate if we actually have data! + ROBIN_HOOD_LOG("std::free") + std::free(mKeyVals); + } + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mKeyVals = static_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + + // no need for calloc here because cloneData performs a memcpy. + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + // sentinel is set in cloneData + } + WHash::operator=(static_cast(o)); + WKeyEqual::operator=(static_cast(o)); + DataPool::operator=(static_cast(o)); + mHashMultiplier = o.mHashMultiplier; + mNumElements = o.mNumElements; + mMask = o.mMask; + mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; + mInfoInc = o.mInfoInc; + mInfoHashShift = o.mInfoHashShift; + cloneData(o); + + return *this; + } + + // Swaps everything between the two maps. + void swap(Table& o) { + ROBIN_HOOD_TRACE(this) + using std::swap; + swap(o, *this); + } + + // Clears all data, without resizing. + void clear() { + ROBIN_HOOD_TRACE(this) + if (empty()) { + // don't do anything! also important because we don't want to write to + // DummyInfoByte::b, even though we would just write 0 to it. + return; + } + + Destroyer::value>{}.nodes(*this); + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + // clear everything, then set the sentinel again + uint8_t const z = 0; + std::fill(mInfo, mInfo + calcNumBytesInfo(numElementsWithBuffer), z); + mInfo[numElementsWithBuffer] = 1; + + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + // Destroys the map and all it's contents. + ~Table() { + ROBIN_HOOD_TRACE(this) + destroy(); + } + + // Checks if both tables contain the same entries. Order is irrelevant. + bool operator==(const Table& other) const { + ROBIN_HOOD_TRACE(this) + if (other.size() != size()) { + return false; + } + for (auto const& otherEntry : other) { + if (!has(otherEntry)) { + return false; + } + } + + return true; + } + + bool operator!=(const Table& other) const { + ROBIN_HOOD_TRACE(this) + return !operator==(other); + } + + template + typename std::enable_if::value, Q&>::type operator[](const key_type& key) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) + Node(*this, std::piecewise_construct, std::forward_as_tuple(key), + std::forward_as_tuple()); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(key), std::forward_as_tuple()); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + } + + return mKeyVals[idxAndState.first].getSecond(); + } + + template + typename std::enable_if::value, Q&>::type operator[](key_type&& key) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) + Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), + std::forward_as_tuple()); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = + Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), + std::forward_as_tuple()); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + } + + return mKeyVals[idxAndState.first].getSecond(); + } + + template + void insert(Iter first, Iter last) { + for (; first != last; ++first) { + // value_type ctor needed because this might be called with std::pair's + insert(value_type(*first)); + } + } + + void insert(std::initializer_list ilist) { + for (auto&& vt : ilist) { + insert(std::move(vt)); + } + } + + template + std::pair emplace(Args&&... args) { + ROBIN_HOOD_TRACE(this) + Node n{*this, std::forward(args)...}; + auto idxAndState = insertKeyPrepareEmptySpot(getFirstConst(n)); + switch (idxAndState.second) { + case InsertionState::key_found: + n.destroy(*this); + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node(*this, std::move(n)); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = std::move(n); + break; + + case InsertionState::overflow_error: + n.destroy(*this); + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + template + iterator emplace_hint(const_iterator position, Args&&... args) { + (void)position; + return emplace(std::forward(args)...).first; + } + + template + std::pair try_emplace(const key_type& key, Args&&... args) { + return try_emplace_impl(key, std::forward(args)...); + } + + template + std::pair try_emplace(key_type&& key, Args&&... args) { + return try_emplace_impl(std::move(key), std::forward(args)...); + } + + template + iterator try_emplace(const_iterator hint, const key_type& key, Args&&... args) { + (void)hint; + return try_emplace_impl(key, std::forward(args)...).first; + } + + template + iterator try_emplace(const_iterator hint, key_type&& key, Args&&... args) { + (void)hint; + return try_emplace_impl(std::move(key), std::forward(args)...).first; + } + + template + std::pair insert_or_assign(const key_type& key, Mapped&& obj) { + return insertOrAssignImpl(key, std::forward(obj)); + } + + template + std::pair insert_or_assign(key_type&& key, Mapped&& obj) { + return insertOrAssignImpl(std::move(key), std::forward(obj)); + } + + template + iterator insert_or_assign(const_iterator hint, const key_type& key, Mapped&& obj) { + (void)hint; + return insertOrAssignImpl(key, std::forward(obj)).first; + } + + template + iterator insert_or_assign(const_iterator hint, key_type&& key, Mapped&& obj) { + (void)hint; + return insertOrAssignImpl(std::move(key), std::forward(obj)).first; + } + + std::pair insert(const value_type& keyval) { + ROBIN_HOOD_TRACE(this) + return emplace(keyval); + } + + iterator insert(const_iterator hint, const value_type& keyval) { + (void)hint; + return emplace(keyval).first; + } + + std::pair insert(value_type&& keyval) { + return emplace(std::move(keyval)); + } + + iterator insert(const_iterator hint, value_type&& keyval) { + (void)hint; + return emplace(std::move(keyval)).first; + } + + // Returns 1 if key is found, 0 otherwise. + size_t count(const key_type& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { + return 1; + } + return 0; + } + + template + typename std::enable_if::type count(const OtherKey& key) const { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { + return 1; + } + return 0; + } + + bool contains(const key_type& key) const { // NOLINT + return 1U == count(key); + } + + template + typename std::enable_if::type contains(const OtherKey& key) const { + return 1U == count(key); + } + + // Returns a reference to the value found for key. + // Throws std::out_of_range if element cannot be found + template + typename std::enable_if::value, Q&>::type at(key_type const& key) { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { + doThrow("key not found"); + } + return kv->getSecond(); + } + + // Returns a reference to the value found for key. + // Throws std::out_of_range if element cannot be found + template + typename std::enable_if::value, Q const&>::type at(key_type const& key) const { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { + doThrow("key not found"); + } + return kv->getSecond(); + } + + const_iterator find(const key_type& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + template + const_iterator find(const OtherKey& key, is_transparent_tag /*unused*/) const { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + template + typename std::enable_if::type // NOLINT + find(const OtherKey& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + iterator find(const key_type& key) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + template + iterator find(const OtherKey& key, is_transparent_tag/*unused*/) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + template + typename std::enable_if::type find(const OtherKey& key) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + iterator begin() { + ROBIN_HOOD_TRACE(this) + if (empty()) { + return end(); + } + return iterator(mKeyVals, mInfo, fast_forward_tag{}); + } + const_iterator begin() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return cbegin(); + } + const_iterator cbegin() const { // NOLINT + ROBIN_HOOD_TRACE(this) + if (empty()) { + return cend(); + } + return const_iterator(mKeyVals, mInfo, fast_forward_tag{}); + } + + iterator end() { + ROBIN_HOOD_TRACE(this) + // no need to supply valid info pointer: end() must not be dereferenced, and only node + // pointer is compared. + return iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; + } + const_iterator end() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return cend(); + } + const_iterator cend() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return const_iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; + } + + iterator erase(const_iterator pos) { + ROBIN_HOOD_TRACE(this) + // its safe to perform const cast here + return erase(iterator{const_cast(pos.mKeyVals), const_cast(pos.mInfo)}); + } + + // Erases element at pos, returns iterator to the next element. + iterator erase(iterator pos) { + ROBIN_HOOD_TRACE(this) + // we assume that pos always points to a valid entry, and not end(). + auto const idx = static_cast(pos.mKeyVals - mKeyVals); + + shiftDown(idx); + --mNumElements; + + if (*pos.mInfo) { + // we've backward shifted, return this again + return pos; + } + + // no backward shift, return next element + return ++pos; + } + + size_t erase(const key_type& key) { + ROBIN_HOOD_TRACE(this) + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + + // check while info matches with the source idx + do { + if (info == mInfo[idx] && WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { + shiftDown(idx); + --mNumElements; + return 1; + } + next(&info, &idx); + } while (info <= mInfo[idx]); + + // nothing found to delete + return 0; + } + + // reserves space for the specified number of elements. Makes sure the old data fits. + // exactly the same as reserve(c). + void rehash(size_t c) { + // forces a reserve + reserve(c, true); + } + + // reserves space for the specified number of elements. Makes sure the old data fits. + // Exactly the same as rehash(c). Use rehash(0) to shrink to fit. + void reserve(size_t c) { + // reserve, but don't force rehash + reserve(c, false); + } + + // If possible reallocates the map to a smaller one. This frees the underlying table. + // Does not do anything if load_factor is too large for decreasing the table's size. + void compact() { + ROBIN_HOOD_TRACE(this) + auto newSize = InitialNumElements; + while (calcMaxNumElementsAllowed(newSize) < mNumElements && newSize != 0) { + newSize *= 2; + } + if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { + throwOverflowError(); + } + + ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") + + // only actually do anything when the new size is bigger than the old one. This prevents to + // continuously allocate for each reserve() call. + if (newSize < mMask + 1) { + rehashPowerOfTwo(newSize, true); + } + } + + size_type size() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return mNumElements; + } + + size_type max_size() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return static_cast(-1); + } + + ROBIN_HOOD(NODISCARD) bool empty() const noexcept { + ROBIN_HOOD_TRACE(this) + return 0 == mNumElements; + } + + float max_load_factor() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return MaxLoadFactor100 / 100.0F; + } + + // Average number of elements per bucket. Since we allow only 1 per bucket + float load_factor() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return static_cast(size()) / static_cast(mMask + 1); + } + + ROBIN_HOOD(NODISCARD) size_t mask() const noexcept { + ROBIN_HOOD_TRACE(this) + return mMask; + } + + ROBIN_HOOD(NODISCARD) size_t calcMaxNumElementsAllowed(size_t maxElements) const noexcept { + if (ROBIN_HOOD_LIKELY(maxElements <= (std::numeric_limits::max)() / 100)) { + return maxElements * MaxLoadFactor100 / 100; + } + + // we might be a bit inprecise, but since maxElements is quite large that doesn't matter + return (maxElements / 100) * MaxLoadFactor100; + } + + ROBIN_HOOD(NODISCARD) size_t calcNumBytesInfo(size_t numElements) const noexcept { + // we add a uint64_t, which houses the sentinel (first byte) and padding so we can load + // 64bit types. + return numElements + sizeof(uint64_t); + } + + ROBIN_HOOD(NODISCARD) + size_t calcNumElementsWithBuffer(size_t numElements) const noexcept { + auto maxNumElementsAllowed = calcMaxNumElementsAllowed(numElements); + return numElements + (std::min)(maxNumElementsAllowed, (static_cast(0xFF))); + } + + // calculation only allowed for 2^n values + ROBIN_HOOD(NODISCARD) size_t calcNumBytesTotal(size_t numElements) const { +#if ROBIN_HOOD(BITNESS) == 64 + return numElements * sizeof(Node) + calcNumBytesInfo(numElements); +#else + // make sure we're doing 64bit operations, so we are at least safe against 32bit overflows. + auto const ne = static_cast(numElements); + auto const s = static_cast(sizeof(Node)); + auto const infos = static_cast(calcNumBytesInfo(numElements)); + + auto const total64 = ne * s + infos; + auto const total = static_cast(total64); + + if (ROBIN_HOOD_UNLIKELY(static_cast(total) != total64)) { + throwOverflowError(); + } + return total; +#endif + } + +private: + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, bool>::type has(const value_type& e) const { + ROBIN_HOOD_TRACE(this) + auto it = find(e.first); + return it != end() && it->second == e.second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, bool>::type has(const value_type& e) const { + ROBIN_HOOD_TRACE(this) + return find(e) != end(); + } + + void reserve(size_t c, bool forceRehash) { + ROBIN_HOOD_TRACE(this) + auto const minElementsAllowed = (std::max)(c, mNumElements); + auto newSize = InitialNumElements; + while (calcMaxNumElementsAllowed(newSize) < minElementsAllowed && newSize != 0) { + newSize *= 2; + } + if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { + throwOverflowError(); + } + + ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") + + // only actually do anything when the new size is bigger than the old one. This prevents to + // continuously allocate for each reserve() call. + if (forceRehash || newSize > mMask + 1) { + rehashPowerOfTwo(newSize, false); + } + } + + // reserves space for at least the specified number of elements. + // only works if numBuckets if power of two + // True on success, false otherwise + void rehashPowerOfTwo(size_t numBuckets, bool forceFree) { + ROBIN_HOOD_TRACE(this) + + Node* const oldKeyVals = mKeyVals; + uint8_t const* const oldInfo = mInfo; + + const size_t oldMaxElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + + // resize operation: move stuff + initData(numBuckets); + if (oldMaxElementsWithBuffer > 1) { + for (size_t i = 0; i < oldMaxElementsWithBuffer; ++i) { + if (oldInfo[i] != 0) { + // might throw an exception, which is really bad since we are in the middle of + // moving stuff. + insert_move(std::move(oldKeyVals[i])); + // destroy the node but DON'T destroy the data. + oldKeyVals[i].~Node(); + } + } + + // this check is not necessary as it's guarded by the previous if, but it helps + // silence g++'s overeager "attempt to free a non-heap object 'map' + // [-Werror=free-nonheap-object]" warning. + if (oldKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { + // don't destroy old data: put it into the pool instead + if (forceFree) { + std::free(oldKeyVals); + } else { + DataPool::addOrFree(oldKeyVals, calcNumBytesTotal(oldMaxElementsWithBuffer)); + } + } + } + } + + ROBIN_HOOD(NOINLINE) void throwOverflowError() const { +#if ROBIN_HOOD(HAS_EXCEPTIONS) + throw std::overflow_error("robin_hood::map overflow"); +#else + abort(); +#endif + } + + template + std::pair try_emplace_impl(OtherKey&& key, Args&&... args) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node( + *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(args)...)); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(args)...)); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + template + std::pair insertOrAssignImpl(OtherKey&& key, Mapped&& obj) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + mKeyVals[idxAndState.first].getSecond() = std::forward(obj); + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node( + *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(obj))); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(obj))); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + void initData(size_t max_elements) { + mNumElements = 0; + mMask = max_elements - 1; + mMaxNumElementsAllowed = calcMaxNumElementsAllowed(max_elements); + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(max_elements); + + // malloc & zero mInfo. Faster than calloc everything. + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + ROBIN_HOOD_LOG("std::calloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mKeyVals = reinterpret_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + std::memset(mInfo, 0, numBytesTotal - numElementsWithBuffer * sizeof(Node)); + + // set sentinel + mInfo[numElementsWithBuffer] = 1; + + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + enum class InsertionState { overflow_error, key_found, new_node, overwrite_node }; + + // Finds key, and if not already present prepares a spot where to pot the key & value. + // This potentially shifts nodes out of the way, updates mInfo and number of inserted + // elements, so the only operation left to do is create/assign a new node at that spot. + template + std::pair insertKeyPrepareEmptySpot(OtherKey&& key) { + for (int i = 0; i < 256; ++i) { + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + nextWhileLess(&info, &idx); + + // while we potentially have a match + while (info == mInfo[idx]) { + if (WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { + // key already exists, do NOT insert. + // see http://en.cppreference.com/w/cpp/container/unordered_map/insert + return std::make_pair(idx, InsertionState::key_found); + } + next(&info, &idx); + } + + // unlikely that this evaluates to true + if (ROBIN_HOOD_UNLIKELY(mNumElements >= mMaxNumElementsAllowed)) { + if (!increase_size()) { + return std::make_pair(size_t(0), InsertionState::overflow_error); + } + continue; + } + + // key not found, so we are now exactly where we want to insert it. + auto const insertion_idx = idx; + auto const insertion_info = info; + if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + + // find an empty spot + while (0 != mInfo[idx]) { + next(&info, &idx); + } + + if (idx != insertion_idx) { + shiftUp(idx, insertion_idx); + } + // put at empty spot + mInfo[insertion_idx] = static_cast(insertion_info); + ++mNumElements; + return std::make_pair(insertion_idx, idx == insertion_idx + ? InsertionState::new_node + : InsertionState::overwrite_node); + } + + // enough attempts failed, so finally give up. + return std::make_pair(size_t(0), InsertionState::overflow_error); + } + + bool try_increase_info() { + ROBIN_HOOD_LOG("mInfoInc=" << mInfoInc << ", numElements=" << mNumElements + << ", maxNumElementsAllowed=" + << calcMaxNumElementsAllowed(mMask + 1)) + if (mInfoInc <= 2) { + // need to be > 2 so that shift works (otherwise undefined behavior!) + return false; + } + // we got space left, try to make info smaller + mInfoInc = static_cast(mInfoInc >> 1U); + + // remove one bit of the hash, leaving more space for the distance info. + // This is extremely fast because we can operate on 8 bytes at once. + ++mInfoHashShift; + auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + + for (size_t i = 0; i < numElementsWithBuffer; i += 8) { + auto val = unaligned_load(mInfo + i); + val = (val >> 1U) & UINT64_C(0x7f7f7f7f7f7f7f7f); + std::memcpy(mInfo + i, &val, sizeof(val)); + } + // update sentinel, which might have been cleared out! + mInfo[numElementsWithBuffer] = 1; + + mMaxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); + return true; + } + + // True if resize was possible, false otherwise + bool increase_size() { + // nothing allocated yet? just allocate InitialNumElements + if (0 == mMask) { + initData(InitialNumElements); + return true; + } + + auto const maxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); + if (mNumElements < maxNumElementsAllowed && try_increase_info()) { + return true; + } + + ROBIN_HOOD_LOG("mNumElements=" << mNumElements << ", maxNumElementsAllowed=" + << maxNumElementsAllowed << ", load=" + << (static_cast(mNumElements) * 100.0 / + (static_cast(mMask) + 1))) + + if (mNumElements * 2 < calcMaxNumElementsAllowed(mMask + 1)) { + // we have to resize, even though there would still be plenty of space left! + // Try to rehash instead. Delete freed memory so we don't steadyily increase mem in case + // we have to rehash a few times + nextHashMultiplier(); + rehashPowerOfTwo(mMask + 1, true); + } else { + // we've reached the capacity of the map, so the hash seems to work nice. Keep using it. + rehashPowerOfTwo((mMask + 1) * 2, false); + } + return true; + } + + void nextHashMultiplier() { + // adding an *even* number, so that the multiplier will always stay odd. This is necessary + // so that the hash stays a mixing function (and thus doesn't have any information loss). + mHashMultiplier += UINT64_C(0xc4ceb9fe1a85ec54); + } + + void destroy() { + if (0 == mMask) { + // don't deallocate! + return; + } + + Destroyer::value>{} + .nodesDoNotDeallocate(*this); + + // This protection against not deleting mMask shouldn't be needed as it's sufficiently + // protected with the 0==mMask check, but I have this anyways because g++ 7 otherwise + // reports a compile error: attempt to free a non-heap object 'fm' + // [-Werror=free-nonheap-object] + if (mKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { + ROBIN_HOOD_LOG("std::free") + std::free(mKeyVals); + } + } + + void init() noexcept { + mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); + mInfo = reinterpret_cast(&mMask); + mNumElements = 0; + mMask = 0; + mMaxNumElementsAllowed = 0; + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + // members are sorted so no padding occurs + uint64_t mHashMultiplier = UINT64_C(0xc4ceb9fe1a85ec53); // 8 byte 8 + Node* mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); // 8 byte 16 + uint8_t* mInfo = reinterpret_cast(&mMask); // 8 byte 24 + size_t mNumElements = 0; // 8 byte 32 + size_t mMask = 0; // 8 byte 40 + size_t mMaxNumElementsAllowed = 0; // 8 byte 48 + InfoType mInfoInc = InitialInfoInc; // 4 byte 52 + InfoType mInfoHashShift = InitialInfoHashShift; // 4 byte 56 + // 16 byte 56 if NodeAllocator +}; + +} // namespace detail + +// map + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_flat_map = detail::Table; + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_node_map = detail::Table; + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_map = + detail::Table) <= sizeof(size_t) * 6 && + std::is_nothrow_move_constructible>::value && + std::is_nothrow_move_assignable>::value, + MaxLoadFactor100, Key, T, Hash, KeyEqual>; + +// set + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_flat_set = detail::Table; + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_node_set = detail::Table; + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_set = detail::Table < sizeof(Key) <= sizeof(size_t) * 6 && + std::is_nothrow_move_constructible::value && + std::is_nothrow_move_assignable::value, + MaxLoadFactor100, Key, void, Hash, KeyEqual>; + +} // namespace robin_hood +/* *INDENT-ON* */ + +#endif // NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ diff --git a/tools/code_coverage_report.bash b/tools/code_coverage_report.bash index f4510238c6..a1e963a1df 100755 --- a/tools/code_coverage_report.bash +++ b/tools/code_coverage_report.bash @@ -60,7 +60,7 @@ INCLUDE_PACKAGES=$( # Capture executed code data. fastcov --lcov \ -d build \ - --exclude test/ $EXCLUDE_PACKAGES \ + --exclude test/ $EXCLUDE_PACKAGES thirdparty/ \ --include $INCLUDE_PACKAGES \ --process-gcno \ --validate-sources \ From f8964152585b9e5e08486444412a2d64b9d88ec0 Mon Sep 17 00:00:00 2001 From: Lukas Fanta <63977366+fantalukas@users.noreply.github.com> Date: Wed, 21 Sep 2022 18:26:44 +0200 Subject: [PATCH 064/131] [RPP] Add parameter to enable/disable collision detection (#3204) * [RPP] Add parameter to enable/disable collision detection * [RPP] Update README --- nav2_regulated_pure_pursuit_controller/README.md | 4 +++- .../regulated_pure_pursuit_controller.hpp | 1 + .../src/regulated_pure_pursuit_controller.cpp | 8 +++++++- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 4b929bd700..1a31d2e080 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -62,7 +62,8 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | | `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | | `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | -| `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions, limited to maximum distance of lookahead distance selected | +| `use_collision_detection` | Whether to enable collision detection. | +| `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions when `use_collision_detection` is `true`. It is limited to maximum distance of lookahead distance selected. | | `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | | `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | | `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | @@ -111,6 +112,7 @@ controller_server: min_approach_linear_velocity: 0.05 use_approach_linear_velocity_scaling: true approach_velocity_scaling_dist: 1.0 + use_collision_detection: true max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true use_cost_regulated_linear_velocity_scaling: false diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 01a41df0ac..7c244d9393 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -292,6 +292,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller double approach_velocity_scaling_dist_; double control_duration_; double max_allowed_time_to_collision_up_to_carrot_; + bool use_collision_detection_; bool use_regulated_linear_velocity_scaling_; bool use_cost_regulated_linear_velocity_scaling_; double cost_scaling_dist_; diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 55898dc339..4b52dd3616 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -85,6 +85,9 @@ void RegulatedPurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_collision_detection", + rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( @@ -142,6 +145,9 @@ void RegulatedPurePursuitController::configure( node->get_parameter( plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", max_allowed_time_to_collision_up_to_carrot_); + node->get_parameter( + plugin_name_ + ".use_collision_detection", + use_collision_detection_); node->get_parameter( plugin_name_ + ".use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_); @@ -346,7 +352,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Collision checking on this velocity heading const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - if (isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { + if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!"); } From e853746c158c75eb7c166b323959d7919cd80c70 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 21 Sep 2022 10:12:07 -0700 Subject: [PATCH 065/131] Update waffle.model --- nav2_bringup/worlds/waffle.model | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/worlds/waffle.model b/nav2_bringup/worlds/waffle.model index 2695e97686..2ab4e51ce6 100644 --- a/nav2_bringup/worlds/waffle.model +++ b/nav2_bringup/worlds/waffle.model @@ -146,7 +146,7 @@ 0.120000 - 3.5 + 20.0 0.015000 From c6bd5c5cbab47a58b0d46ad7e2b1c39fdcf50ca6 Mon Sep 17 00:00:00 2001 From: milidam Date: Thu, 22 Sep 2022 19:24:58 +0200 Subject: [PATCH 066/131] Add a min_obstacle_height param to the nav2_costmap_2d's ObstacleLayer plugin (#3211) This allows considering full range observations, specified by the .min_obstacle_height and .max_obstacle_height especially used for the raytracing, but to still be able to specify a minimum obstacle height to report obstacles onto the costmap. This is in particular required in the case a PointCloud2 source points slightly towards the ground, sometimes detecting obstacles, that should be cleared once the ground reappears behind the obstacle when it has moved away: we don't want to detect the ground as an obstacle, but still want it to be used in the raytracing to clear the previously detected obstacle. --- .../include/nav2_costmap_2d/obstacle_layer.hpp | 1 + nav2_costmap_2d/plugins/obstacle_layer.cpp | 12 +++++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index fbced80c80..754b434930 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -226,6 +226,7 @@ class ObstacleLayer : public CostmapLayer double * max_y); std::string global_frame_; ///< @brief The global frame for the costmap + double min_obstacle_height_; ///< @brief Max Obstacle Height double max_obstacle_height_; ///< @brief Max Obstacle Height /// @brief Used to project laser scans into point clouds diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index e7836f9639..653c0885ca 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -78,6 +78,7 @@ void ObstacleLayer::onInitialize() declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); + declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0)); declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); declareParameter("combination_method", rclcpp::ParameterValue(1)); declareParameter("observation_sources", rclcpp::ParameterValue(std::string(""))); @@ -89,6 +90,7 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_); 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); @@ -296,7 +298,9 @@ ObstacleLayer::dynamicParametersCallback( const auto & param_name = parameter.get_name(); if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == name_ + "." + "max_obstacle_height") { + if (param_name == name_ + "." + "min_obstacle_height") { + min_obstacle_height_ = parameter.as_double(); + } else if (param_name == name_ + "." + "max_obstacle_height") { max_obstacle_height_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { @@ -453,6 +457,12 @@ ObstacleLayer::updateBounds( for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { double px = *iter_x, py = *iter_y, pz = *iter_z; + // if the obstacle is too low, we won't add it + if (pz < min_obstacle_height_) { + RCLCPP_DEBUG(logger_, "The point is too low"); + continue; + } + // 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(logger_, "The point is too high"); From f2713f70a1f35e7e3a83df07f4ac33947d1cb20a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 22 Sep 2022 14:20:57 -0700 Subject: [PATCH 067/131] add benchmark launch file + instructions (#3218) --- tools/planner_benchmarking/README.md | 30 ++++++++ tools/planner_benchmarking/metrics.py | 35 --------- .../planning_benchmark_bringup.py | 73 +++++++++++++++++++ 3 files changed, 103 insertions(+), 35 deletions(-) create mode 100644 tools/planner_benchmarking/README.md create mode 100644 tools/planner_benchmarking/planning_benchmark_bringup.py diff --git a/tools/planner_benchmarking/README.md b/tools/planner_benchmarking/README.md new file mode 100644 index 0000000000..c10e44003d --- /dev/null +++ b/tools/planner_benchmarking/README.md @@ -0,0 +1,30 @@ +# Planning Benchmark + +This experiment runs a set of planners over randomly generated maps, with randomly generated goals for objective benchmarking. + +To use, modify the Nav2 bringup parameters to include the planners of interest: + +``` +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["SmacHybrid", "Smac2d", "SmacLattice", "Navfn", "ThetaStar"] + SmacHybrid: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + Smac2d: + plugin: "nav2_smac_planner/SmacPlanner2D" + SmacLattice: + plugin: "nav2_smac_planner/SmacPlannerLattice" + Navfn: + plugin: "nav2_navfn_planner/NavfnPlanner" + ThetaStar: + plugin: "nav2_theta_star_planner/ThetaStarPlanner" +``` + +Set global costmap settings to those desired for benchmarking. The global map will be automatically set in the script. Inside of `metrics.py`, you can modify the map or set of planners to use. + +Launch the benchmark via `ros2 launch ./planning_benchmark_bringup.py` to launch the planner and map servers, then run each script in this directory: + +- `metrics.py` to capture data in `.pickle` files. +- `process_data.py` to take the metric files and process them into key results (and plots) diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 2ff98a25be..4cfcf721e3 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -31,28 +31,6 @@ from transforms3d.euler import euler2quat -''' -Replace planner server with: - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: True - planner_plugins: ["SmacHybrid", "Smac2d", "SmacLattice", "Navfn", "ThetaStar"] - SmacHybrid: - plugin: "nav2_smac_planner/SmacPlannerHybrid" - Smac2d: - plugin: "nav2_smac_planner/SmacPlanner2D" - SmacLattice: - plugin: "nav2_smac_planner/SmacPlannerLattice" - Navfn: - plugin: "nav2_navfn_planner/NavfnPlanner" - ThetaStar: - plugin: "nav2_theta_star_planner/ThetaStarPlanner" - -Set global costmap settings to those desired for benchmarking. -The global map will be automatically set in the script. -''' def getPlannerResults(navigator, initial_pose, goal_pose, planners): results = [] @@ -120,19 +98,6 @@ def main(): navigator = BasicNavigator() - # Set our experiment's initial pose - initial_pose = PoseStamped() - initial_pose.header.frame_id = 'map' - initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 1.0 - initial_pose.pose.position.y = 1.0 - initial_pose.pose.orientation.z = 0.0 - initial_pose.pose.orientation.w = 1.0 - navigator.setInitialPose(initial_pose) - - # Wait for navigation to fully activate - navigator.waitUntilNav2Active() - # Set map to use, other options: 100by100_15, 100by100_10 map_path = os.getcwd() + '/' + glob.glob('**/100by100_20.yaml', recursive=True)[0] navigator.changeMap(map_path) diff --git a/tools/planner_benchmarking/planning_benchmark_bringup.py b/tools/planner_benchmarking/planning_benchmark_bringup.py new file mode 100644 index 0000000000..e2a251405d --- /dev/null +++ b/tools/planner_benchmarking/planning_benchmark_bringup.py @@ -0,0 +1,73 @@ +# Copyright (c) 2022 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 + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml') + map_file = os.path.join(nav2_bringup_dir, 'maps', 'map.yaml') + lifecycle_nodes = ['map_server', 'planner_server'] + + return LaunchDescription([ + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'use_sim_time': True}, + {'yaml_filename': map_file}, + {'topic_name': "map"}]), + + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[config]), + + Node( + package = 'tf2_ros', + executable = 'static_transform_publisher', + output = 'screen', + arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"]), + + Node( + package = 'tf2_ros', + executable = 'static_transform_publisher', + output = 'screen', + arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"]), + + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[{'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + ]) From b45381d34941982750af64cf3db4148bff614e80 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 22 Sep 2022 15:02:44 -0700 Subject: [PATCH 068/131] removing hypotf from smac planner heuristic computation (#3217) * removing hypotf * swapping to node2d sqrt --- nav2_smac_planner/src/node_2d.cpp | 4 +++- nav2_smac_planner/src/node_hybrid.cpp | 6 +++--- tools/planner_benchmarking/metrics.py | 2 ++ 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 158a359870..14a901aded 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -86,7 +86,9 @@ float Node2D::getHeuristicCost( { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - return hypotf(goal_coordinates.x - node_coords.x, goal_coordinates.y - node_coords.y); + auto dx = goal_coordinates.x - node_coords.x; + auto dy = goal_coordinates.y - node_coords.y; + return std::sqrt(dx * dx + dy * dy); } void Node2D::initMotionModel( diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index ebbe357559..24b32f527b 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -381,9 +381,9 @@ inline float distanceHeuristic2D( const unsigned int idx, const unsigned int size_x, const unsigned int target_x, const unsigned int target_y) { - return std::hypotf( - static_cast(idx % size_x) - static_cast(target_x), - static_cast(idx / size_x) - static_cast(target_y)); + int dx = static_cast(idx % size_x) - static_cast(target_x); + int dy = static_cast(idx / size_x) - static_cast(target_y); + return std::sqrt(dx * dx + dy * dy); } void NodeHybrid::resetObstacleHeuristic( diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 4cfcf721e3..c4b556e5d7 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -38,6 +38,8 @@ def getPlannerResults(navigator, initial_pose, goal_pose, planners): path = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True) if path is not None: results.append(path) + else: + return results return results From 1b24e286d3e9e4dc588aeb8e6a3671a08d310a0a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 22 Sep 2022 19:00:59 -0700 Subject: [PATCH 069/131] complete smac planner tolerances (#3219) --- nav2_smac_planner/README.md | 4 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 1 + .../smac_planner_lattice.hpp | 1 + nav2_smac_planner/src/a_star.cpp | 5 +++ nav2_smac_planner/src/smac_planner_hybrid.cpp | 39 +++++++++++++++--- .../src/smac_planner_lattice.cpp | 40 +++++++++++++++---- nav2_smac_planner/test/test_smac_hybrid.cpp | 4 ++ nav2_smac_planner/test/test_smac_lattice.cpp | 2 + 8 files changed, 81 insertions(+), 15 deletions(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 5a89a608a4..0396664f8e 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -103,12 +103,12 @@ planner_server: GridBased: plugin: "nav2_smac_planner/SmacPlannerHybrid" - tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters downsample_costmap: false # whether or not to downsample the map downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) allow_unknown: false # allow traveling in unknown space max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 51f6608b5f..c782fe41e0 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -111,6 +111,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner unsigned int _angle_quantizations; bool _allow_unknown; int _max_iterations; + int _max_on_approach_iterations; SearchInfo _search_info; double _max_planning_time; double _lookup_table_size; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index c34204b18e..f5207cd45c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -106,6 +106,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner SearchInfo _search_info; bool _allow_unknown; int _max_iterations; + int _max_on_approach_iterations; float _tolerance; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index b6a56271a9..06bc0520d4 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -326,6 +326,11 @@ bool AStarAlgorithm::createPath( } } + if (_best_heuristic_node.first < getToleranceHeuristic()) { + // If we run out of serach options, return the path that is closest, if within tolerance. + return _graph.at(_best_heuristic_node.second).backtracePath(path); + } + return false; } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 279af1804c..ee614c7b94 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -80,12 +80,18 @@ void SmacPlannerHybrid::configure( _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(0.25)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", _allow_unknown); nav2_util::declare_parameter_if_not_declared( node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", _max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); + node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -139,6 +145,13 @@ void SmacPlannerHybrid::configure( _motion_model_for_search.c_str()); } + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + if (_max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " @@ -180,7 +193,7 @@ void SmacPlannerHybrid::configure( _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, _lookup_table_dim, _angle_quantizations); @@ -205,10 +218,11 @@ void SmacPlannerHybrid::configure( RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerHybrid with " - "maximum iterations %i, and %s. Using motion model: %s.", - _name.c_str(), _max_iterations, + "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f." + "Using motion model: %s.", + _name.c_str(), _max_iterations, _max_on_approach_iterations, _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(_motion_model).c_str()); + _tolerance, toString(_motion_model).c_str()); } void SmacPlannerHybrid::activate() @@ -315,7 +329,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( int num_iterations = 0; std::string error; try { - if (!_a_star->createPath(path, num_iterations, 0.0)) { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); } else { @@ -392,6 +408,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para if (name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); + } else if (name == _name + ".tolerance") { + _tolerance = static_cast(parameter.as_double()); } else if (name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); @@ -452,6 +470,15 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } } else if (name == _name + ".angle_quantization_bins") { reinit_collision_checker = true; reinit_a_star = true; @@ -504,7 +531,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, _lookup_table_dim, _angle_quantizations); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 09a3591928..b7e22ee1f9 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -65,12 +65,18 @@ void SmacPlannerLattice::configure( double analytic_expansion_max_length_m; bool smooth_path; + nav2_util::declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(0.25)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", _allow_unknown); nav2_util::declare_parameter_if_not_declared( node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", _max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); + node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -126,6 +132,13 @@ void SmacPlannerLattice::configure( _metadata.min_turning_radius / (_costmap->getResolution()); _motion_model = MotionModel::STATE_LATTICE; + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + if (_max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " @@ -167,7 +180,7 @@ void SmacPlannerLattice::configure( _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, lookup_table_dim, _metadata.number_of_headings); @@ -182,11 +195,11 @@ void SmacPlannerLattice::configure( RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerLattice with " - "maximum iterations %i, " - "and %s. Using motion model: %s. State lattice file: %s.", - _name.c_str(), _max_iterations, + "maximum iterations %i, max on approach iterations %i, " + "and %s. Tolerance %.2f. Using motion model: %s. State lattice file: %s.", + _name.c_str(), _max_iterations, _max_on_approach_iterations, _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str()); + _tolerance, toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str()); } void SmacPlannerLattice::activate() @@ -262,7 +275,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( int num_iterations = 0; std::string error; try { - if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); } else { @@ -349,6 +364,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par if (name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); + } else if (name == _name + ".tolerance") { + _tolerance = static_cast(parameter.as_double()); } else if (name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); @@ -403,6 +420,15 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _max_iterations = std::numeric_limits::max(); } } + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } } else if (type == ParameterType::PARAMETER_STRING) { if (name == _name + ".lattice_filepath") { reinit_a_star = true; @@ -453,7 +479,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, lookup_table_dim, _metadata.number_of_headings); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 3eb077cfb0..3be1a9e897 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -110,12 +110,14 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.change_penalty", 1.0), rclcpp::Parameter("test.non_straight_penalty", 2.0), rclcpp::Parameter("test.cost_penalty", 2.0), + rclcpp::Parameter("test.tolerance", 0.2), rclcpp::Parameter("test.retrospective_penalty", 0.2), rclcpp::Parameter("test.analytic_expansion_ratio", 4.0), rclcpp::Parameter("test.max_planning_time", 10.0), rclcpp::Parameter("test.lookup_table_size", 30.0), rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), + rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); rclcpp::spin_until_future_complete( @@ -134,11 +136,13 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.non_straight_penalty").as_double(), 2.0); EXPECT_EQ(nodeSE2->get_parameter("test.cost_penalty").as_double(), 2.0); EXPECT_EQ(nodeSE2->get_parameter("test.retrospective_penalty").as_double(), 0.2); + EXPECT_EQ(nodeSE2->get_parameter("test.tolerance").as_double(), 0.2); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_ratio").as_double(), 4.0); EXPECT_EQ(nodeSE2->get_parameter("test.smooth_path").as_bool(), false); EXPECT_EQ(nodeSE2->get_parameter("test.max_planning_time").as_double(), 10.0); EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0); + EXPECT_EQ(nodeSE2->get_parameter("test.max_on_approach_iterations").as_int(), 42); EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index ff813f08c1..9ce99a46b1 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -124,7 +124,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure) rclcpp::Parameter("test.lookup_table_size", 30.0), rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), + rclcpp::Parameter("test.tolerance", 42.0), rclcpp::Parameter("test.rotation_penalty", 42.0), + rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.allow_reverse_expansion", true)}); try { From 810f07666da6b33f043e313eaffc18d265764e33 Mon Sep 17 00:00:00 2001 From: Ruffin Date: Fri, 23 Sep 2022 20:06:01 +0200 Subject: [PATCH 070/131] Disable Output Buffering (#3220) To ensure await asyncio prints [Processing: %s]' every 30s as expected --- .circleci/config.yml | 1 + Dockerfile | 1 + 2 files changed, 2 insertions(+) diff --git a/.circleci/config.yml b/.circleci/config.yml index 8f60f876b4..b4028f653d 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -447,6 +447,7 @@ _environments: RCUTILS_LOGGING_BUFFERED_STREAM: "0" RCUTILS_LOGGING_USE_STDOUT: "0" DEBIAN_FRONTEND: "noninteractive" + PYTHONUNBUFFERED: "1" executors: release_exec: diff --git a/Dockerfile b/Dockerfile index 155f5aefbb..ba26fc246f 100644 --- a/Dockerfile +++ b/Dockerfile @@ -43,6 +43,7 @@ RUN echo '\ APT::Install-Recommends "0";\n\ APT::Install-Suggests "0";\n\ ' > /etc/apt/apt.conf.d/01norecommend +ENV PYTHONUNBUFFERED 1 # install CI dependencies ARG RTI_NC_LICENSE_ACCEPTED=yes From 83f8b4f70d0f9c48f3400d06e04912f09e861d1b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 23 Sep 2022 16:04:50 -0700 Subject: [PATCH 071/131] fix majority of python linting errors introduced in python costmap API additions to get CI turning over again (#3223) * fix majority of python linting errors * finish linting --- .../nav2_simple_commander/costmap_2d.py | 103 +++++++++++------- .../nav2_simple_commander/robot_navigator.py | 1 + 2 files changed, 64 insertions(+), 40 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py index b3b9cde869..82b603dd67 100644 --- a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py +++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py @@ -34,13 +34,12 @@ class PyCostmap2D: def __init__(self, occupancy_map): """ - Initializer for costmap2D. + Initialize costmap2D. + + Args: + ---- + occupancy_map (OccupancyGrid): 2D OccupancyGrid Map - Initialize instance variables with parameter occupancy_map. - Args: - occupancy_map (OccupancyGrid): 2D OccupancyGrid Map - Returns: - None """ self.size_x = occupancy_map.info.width self.size_y = occupancy_map.info.height @@ -92,11 +91,15 @@ def getCostXY(self, mx: int, my: int) -> np.uint8: """ Get the cost of a cell in the costmap using map coordinate XY. - Args: - mx (int): map coordinate X to get cost - my (int): map coordinate Y to get cost - Returns: - np.uint8: cost of a cell + Args + ---- + mx (int): map coordinate X to get cost + my (int): map coordinate Y to get cost + + Returns + ------- + np.uint8: cost of a cell + """ return self.costmap[self.getIndex(mx, my)] @@ -104,10 +107,14 @@ def getCostIdx(self, index: int) -> np.uint8: """ Get the cost of a cell in the costmap using Index. - Args: - index (int): index of cell to get cost - Returns: - np.uint8: cost of a cell + Args + ---- + index (int): index of cell to get cost + + Returns + ------- + np.uint8: cost of a cell + """ return self.costmap[index] @@ -115,12 +122,16 @@ def setCost(self, mx: int, my: int, cost: np.uint8) -> None: """ Set the cost of a cell in the costmap using map coordinate XY. - Args: - mx (int): map coordinate X to get cost - my (int): map coordinate Y to get cost - cost (np.uint8): The cost to set the cell - Returns: - None + Args + ---- + mx (int): map coordinate X to get cost + my (int): map coordinate Y to get cost + cost (np.uint8): The cost to set the cell + + Returns + ------- + None + """ self.costmap[self.getIndex(mx, my)] = cost @@ -128,13 +139,17 @@ def mapToWorld(self, mx: int, my: int) -> tuple[float, float]: """ Get the world coordinate XY using map coordinate XY. - Args: - mx (int): map coordinate X to get world coordinate - my (int): map coordinate Y to get world coordinate - Returns: - tuple of float: wx, wy - wx (float) [m]: world coordinate X - wy (float) [m]: world coordinate Y + Args + ---- + mx (int): map coordinate X to get world coordinate + my (int): map coordinate Y to get world coordinate + + Returns + ------- + tuple of float: wx, wy + wx (float) [m]: world coordinate X + wy (float) [m]: world coordinate Y + """ wx = self.origin_x + (mx + 0.5) * self.resolution wy = self.origin_y + (my + 0.5) * self.resolution @@ -144,13 +159,17 @@ def worldToMap(self, wx: float, wy: float) -> tuple[int, int]: """ Get the map coordinate XY using world coordinate XY. - Args: - wx (float) [m]: world coordinate X to get map coordinate - wy (float) [m]: world coordinate Y to get map coordinate - Returns: - tuple of int: mx, my - mx (int): map coordinate X - my (int): map coordinate Y + Args + ---- + wx (float) [m]: world coordinate X to get map coordinate + wy (float) [m]: world coordinate Y to get map coordinate + + Returns + ------- + tuple of int: mx, my + mx (int): map coordinate X + my (int): map coordinate Y + """ mx = int((wx - self.origin_x) // self.resolution) my = int((wy - self.origin_y) // self.resolution) @@ -160,10 +179,14 @@ def getIndex(self, mx: int, my: int) -> int: """ Get the index of the cell using map coordinate XY. - Args: - mx (int): map coordinate X to get Index - my (int): map coordinate Y to get Index - Returns: - int: The index of the cell + Args + ---- + mx (int): map coordinate X to get Index + my (int): map coordinate Y to get Index + + Returns + ------- + int: The index of the cell + """ return my * self.size_x + mx diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 86251077c3..5807ca09b1 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -299,6 +299,7 @@ def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): def _getPathImpl(self, start, goal, planner_id='', use_start=False): """ Send a `ComputePathToPose` action request. + Internal implementation to get the full result, not just the path. """ self.debug("Waiting for 'ComputePathToPose' action server") From c1f837dd78080cc3d46316791b375ac10eaafdaa Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 26 Sep 2022 20:50:46 +0200 Subject: [PATCH 072/131] Add new constructor to cosmtap2DROS (#3222) * Add new constructor * fix plugins * Use new constructor --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 10 ++++++++ nav2_costmap_2d/src/costmap_2d_node.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 24 ++++++++++++++++--- 3 files changed, 32 insertions(+), 4 deletions(-) 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 5b469d42f2..968bd9b8a4 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 @@ -72,6 +72,11 @@ namespace nav2_costmap_2d class Costmap2DROS : public nav2_util::LifecycleNode { public: + /** + * @brief Constructor for the wrapper + */ + Costmap2DROS(); + /** * @brief Constructor for the wrapper, the node will * be placed in a namespace equal to the node's name @@ -93,6 +98,11 @@ class Costmap2DROS : public nav2_util::LifecycleNode const std::string & local_namespace, const bool & use_sim_time); + /** + * @brief Common initialization for constructors + */ + void init(); + /** * @brief A destructor */ diff --git a/nav2_costmap_2d/src/costmap_2d_node.cpp b/nav2_costmap_2d/src/costmap_2d_node.cpp index 57f7854bf0..cab7978759 100644 --- a/nav2_costmap_2d/src/costmap_2d_node.cpp +++ b/nav2_costmap_2d/src/costmap_2d_node.cpp @@ -43,7 +43,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared("costmap"); + auto node = std::make_shared(); rclcpp::spin(node->get_node_base_interface()); rclcpp::shutdown(); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 82aa604d8f..c9ca90ad07 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -61,6 +61,19 @@ namespace nav2_costmap_2d Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) : Costmap2DROS(name, "/", name, use_sim_time) {} +Costmap2DROS::Costmap2DROS() +: nav2_util::LifecycleNode("costmap", ""), + name_("costmap"), + default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, + default_types_{ + "nav2_costmap_2d::StaticLayer", + "nav2_costmap_2d::ObstacleLayer", + "nav2_costmap_2d::InflationLayer"} +{ + declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + init(); +} + Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, @@ -84,6 +97,14 @@ Costmap2DROS::Costmap2DROS( "nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} +{ + declare_parameter( + "map_topic", rclcpp::ParameterValue( + (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); + init(); +} + +void Costmap2DROS::init() { RCLCPP_INFO(get_logger(), "Creating Costmap"); @@ -96,9 +117,6 @@ Costmap2DROS::Costmap2DROS( declare_parameter("height", rclcpp::ParameterValue(5)); declare_parameter("width", rclcpp::ParameterValue(5)); declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); - declare_parameter( - "map_topic", rclcpp::ParameterValue( - (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); declare_parameter("origin_x", rclcpp::ParameterValue(0.0)); declare_parameter("origin_y", rclcpp::ParameterValue(0.0)); From 8df3c5d76468ac644dbe2d2a4d61e3f7193f9e0e Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Mon, 26 Sep 2022 14:59:41 -0400 Subject: [PATCH 073/131] Assisted teleop simple commander (#3198) * add assisted teleop to python api * cleanup * assisted teleop demo * rename * lint * code review * trigger build * flake8 fix * break cashe * moved all v11 to v12 * lint fix * remove package dep * change default time allowance --- .circleci/config.yml | 6 +- .../include/nav2_behaviors/timed_behavior.hpp | 2 +- .../launch/assisted_teleop_example_launch.py | 78 +++++++++++++++++++ .../example_assisted_teleop.py | 56 +++++++++++++ .../nav2_simple_commander/robot_navigator.py | 23 +++++- nav2_simple_commander/setup.py | 1 + 6 files changed, 161 insertions(+), 5 deletions(-) create mode 100644 nav2_simple_commander/launch/assisted_teleop_example_launch.py create mode 100644 nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py diff --git a/.circleci/config.yml b/.circleci/config.yml index b4028f653d..ede7b367e9 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v11\ + - "<< parameters.key >>-v12\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v11\ + - "<< parameters.key >>-v12\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v11\ + key: "<< parameters.key >>-v12\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 7d38f38167..579a2fd0ce 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -184,7 +184,7 @@ class TimedBehavior : public nav2_core::Behavior // onRun and cycle functions to execute a specific behavior void execute() { - RCLCPP_INFO(logger_, "Attempting %s", behavior_name_.c_str()); + RCLCPP_INFO(logger_, "Running %s", behavior_name_.c_str()); if (!enabled_) { RCLCPP_WARN( diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py new file mode 100644 index 0000000000..59bd6ea629 --- /dev/null +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -0,0 +1,78 @@ +# Copyright (c) 2021 Samsung Research America +# Copyright (c) 2022 Joshua Wallace +# +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_assisted_teleop', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py new file mode 100644 index 0000000000..564342e541 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py @@ -0,0 +1,56 @@ +#! /usr/bin/env python3 +# Copyright 2022 Joshua Wallace +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from time import sleep + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy + +""" +Basic navigation demo to go to pose. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + navigator.assistedTeleop(time_allowance=20) + while not navigator.isTaskComplete(): + # Publish twist commands to be filtered by the assisted teleop action + sleep(0.2) + pass + + navigator.lifecycleShutdown() + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 5807ca09b1..59a8599b03 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -23,7 +23,7 @@ from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState -from nav2_msgs.action import BackUp, Spin +from nav2_msgs.action import AssistedTeleop, BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose from nav2_msgs.action import SmoothPath @@ -75,6 +75,7 @@ def __init__(self): self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') self.spin_client = ActionClient(self, Spin, 'spin') self.backup_client = ActionClient(self, BackUp, 'backup') + self.assisted_teleop_client = ActionClient(self, AssistedTeleop, 'assisted_teleop') self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped, 'amcl_pose', self._amclPoseCallback, @@ -222,6 +223,26 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): self.result_future = self.goal_handle.get_result_async() return True + def assistedTeleop(self, time_allowance=30): + self.debug("Wainting for 'assisted_teleop' action server") + while not self.assisted_teleop_client.wait_for_server(timeout_sec=1.0): + self.info("'assisted_teleop' action server not available, waiting...") + goal_msg = AssistedTeleop.Goal() + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info("Running 'assisted_teleop'....") + send_goal_future = \ + self.assisted_teleop_client.send_goal_async(goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Assisted Teleop request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def followPath(self, path, controller_id='', goal_checker_id=''): """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index ab19a26e47..c7b12a3fcf 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -33,6 +33,7 @@ 'demo_inspection = nav2_simple_commander.demo_inspection:main', 'demo_security = nav2_simple_commander.demo_security:main', 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', + 'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main', ], }, ) From 92dd352128b8d7f6ffc19da1f2f84e60e4f31e50 Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Wed, 28 Sep 2022 15:53:13 -0400 Subject: [PATCH 074/131] added result codes for global planner (#3146) * added result codes for global planner * code review * code review * cleanup * cleanup * update smac lattice planner * update planner instances * cleanup * updates * renaming * fixes * cpplint * uncrusitfy * code review * navfn exceptions * theta_star_planner * fix code review * wrote timeout exception * consistent exception throwing across planners * code review * remove * uncrusitfy * uncrusify * catch exception * expect throw * update string of exceptions * throw with coords * removed start == goal error code * code review * code review * uncrustify * code review * message order * remove remarks * update xml * update xml * Update nav2_behavior_tree/nav2_tree_nodes.xml Co-authored-by: Steve Macenski --- .../action/compute_path_to_pose_action.hpp | 2 + nav2_behavior_tree/nav2_tree_nodes.xml | 1 + .../action/compute_path_to_pose_action.cpp | 7 ++ nav2_core/include/nav2_core/exceptions.hpp | 52 ++++++++++- nav2_msgs/action/ComputePathToPose.action | 13 +++ .../nav2_navfn_planner/navfn_planner.hpp | 1 + nav2_navfn_planner/src/navfn_planner.cpp | 61 ++++++------ .../include/nav2_planner/planner_server.hpp | 20 ++-- nav2_planner/src/planner_server.cpp | 93 ++++++++++++------- .../include/nav2_smac_planner/a_star.hpp | 1 + nav2_smac_planner/src/a_star.cpp | 4 +- nav2_smac_planner/src/smac_planner_2d.cpp | 45 ++++----- nav2_smac_planner/src/smac_planner_hybrid.cpp | 41 ++++---- .../src/smac_planner_lattice.cpp | 39 ++++---- .../theta_star_planner.hpp | 1 + .../src/theta_star_planner.cpp | 38 ++++++-- .../test/test_theta_star.cpp | 4 +- 17 files changed, 266 insertions(+), 157 deletions(-) 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 fc95daaf43..e980012cc4 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 @@ -75,6 +75,8 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), + BT::OutputPort( + "compute_path_to_pose_error_code", "The compute path to pose error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 62a5f1f482..636392929a 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,6 +77,7 @@ Service name Server timeout Path created by ComputePathToPose node + "The compute path to pose error code" 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 07bd9240d2..b5e0e9084c 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 @@ -40,6 +40,9 @@ void ComputePathToPoseAction::on_tick() BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); + // Set empty error code, action was successful + result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; + setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } @@ -47,6 +50,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -54,6 +58,9 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + // Set empty error code, action was cancelled + result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; + setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index be23a91d42..19a113d2e4 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -46,9 +46,57 @@ namespace nav2_core class PlannerException : public std::runtime_error { public: - explicit PlannerException(const std::string description) + explicit PlannerException(const std::string & description) : std::runtime_error(description) {} - using Ptr = std::shared_ptr; +}; + +class StartOccupied : public PlannerException +{ +public: + explicit StartOccupied(const std::string & description) + : PlannerException(description) {} +}; + +class GoalOccupied : public PlannerException +{ +public: + explicit GoalOccupied(const std::string & description) + : PlannerException(description) {} +}; + +class StartOutsideMapBounds : public PlannerException +{ +public: + explicit StartOutsideMapBounds(const std::string & description) + : PlannerException(description) {} +}; + +class GoalOutsideMapBounds : public PlannerException +{ +public: + explicit GoalOutsideMapBounds(const std::string & description) + : PlannerException(description) {} +}; + +class NoValidPathCouldBeFound : public PlannerException +{ +public: + explicit NoValidPathCouldBeFound(const std::string & description) + : PlannerException(description) {} +}; + +class PlannerTimedOut : public PlannerException +{ +public: + explicit PlannerTimedOut(const std::string & description) + : PlannerException(description) {} +}; + +class PlannerTFError : public PlannerException +{ +public: + explicit PlannerTFError(const std::string & description) + : PlannerException(description) {} }; } // namespace nav2_core diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index a052c552ab..7e727088a1 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,3 +1,15 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +int16 NONE=0 +int16 UNKNOWN=1 +int16 TF_ERROR=2 +int16 START_OUTSIDE_MAP=3 +int16 GOAL_OUTSIDE_MAP=4 +int16 START_OCCUPIED=5 +int16 GOAL_OCCUPIED=6 +int16 TIMEOUT=7 +int16 NO_VALID_PATH=8 + #goal definition geometry_msgs/PoseStamped goal geometry_msgs/PoseStamped start @@ -7,5 +19,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time +int16 error_code --- #feedback definition 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 8090d817ef..454dd44858 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -25,6 +25,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/global_planner.hpp" +#include "nav2_core/exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_navfn_planner/navfn.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2ede7f38b8..09d0e18466 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -134,6 +134,30 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( #ifdef BENCHMARK_TESTING steady_clock::time_point a = steady_clock::now(); #endif + unsigned int mx_start, my_start, mx_goal, my_goal; + if (!costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } + + if (!costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } + + if (costmap_->getCost(mx_start, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::StartOccupied( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was in lethal cost"); + } + + if (tolerance_ == 0 && costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::GoalOccupied( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was in lethal cost"); + } // Update planner based on the new costmap size if (isPlannerOutOfDate()) { @@ -148,12 +172,6 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( if (start.pose.position.x == goal.pose.position.x && start.pose.position.y == goal.pose.position.y) { - unsigned int mx, my; - costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); - if (costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) { - RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); - return path; - } path.header.stamp = clock_->now(); path.header.frame_id = global_frame_; geometry_msgs::msg::PoseStamped pose; @@ -172,9 +190,8 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( } if (!makePlan(start.pose, goal.pose, tolerance_, path)) { - RCLCPP_WARN( - logger_, "%s: failed to create plan with " - "tolerance %.2f.", name_.c_str(), tolerance_); + throw nav2_core::NoValidPathCouldBeFound( + "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); } @@ -221,14 +238,7 @@ NavfnPlanner::makePlan( start.position.x, start.position.y, goal.position.x, goal.position.y); unsigned int mx, my; - if (!worldToMap(wx, wy, mx, my)) { - RCLCPP_WARN( - 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?"); - return false; - } + worldToMap(wx, wy, mx, my); // clear the starting cell within the costmap because we know it can't be an obstacle clearRobotCell(mx, my); @@ -251,14 +261,7 @@ NavfnPlanner::makePlan( wx = goal.position.x; wy = goal.position.y; - if (!worldToMap(wx, wy, mx, my)) { - RCLCPP_WARN( - logger_, - "The goal sent to the planner is off the global costmap." - " Planning will always fail to this goal."); - return false; - } - + worldToMap(wx, wy, mx, my); int map_goal[2]; map_goal[0] = mx; map_goal[1] = my; @@ -385,13 +388,7 @@ NavfnPlanner::getPlanFromPotential( // the potential has already been computed, so we won't update our copy of the costmap unsigned int mx, my; - if (!worldToMap(wx, wy, mx, my)) { - RCLCPP_WARN( - logger_, - "The goal sent to the navfn planner is off the global costmap." - " Planning will always fail to this goal."); - return false; - } + worldToMap(wx, wy, mx, my); int map_goal[2]; map_goal[0] = mx; diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index a1f1738c29..3cca065e36 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -39,6 +39,7 @@ #include "pluginlib/class_list_macros.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" +#include "nav2_core/exceptions.hpp" namespace nav2_planner { @@ -152,21 +153,17 @@ class PlannerServer : public nav2_util::LifecycleNode */ template bool getStartPose( - std::unique_ptr> & action_server, typename std::shared_ptr goal, geometry_msgs::msg::PoseStamped & start); /** * @brief Transform start and goal poses into the costmap * global frame for path planning plugins to utilize - * @param action_server Action server to terminate if required * @param start The starting pose to transform * @param goal Goal pose to transform * @return bool If successful in transforming poses */ - template bool transformPosesToGlobalFrame( - std::unique_ptr> & action_server, geometry_msgs::msg::PoseStamped & curr_start, geometry_msgs::msg::PoseStamped & curr_goal); @@ -180,15 +177,10 @@ class PlannerServer : public nav2_util::LifecycleNode */ template bool validatePath( - std::unique_ptr> & action_server, const geometry_msgs::msg::PoseStamped & curr_goal, const nav_msgs::msg::Path & path, const std::string & planner_id); - // Our action server implements the ComputePathToPose action - std::unique_ptr action_server_pose_; - std::unique_ptr action_server_poses_; - /** * @brief The action server callback which calls planner to get the path * ComputePathToPose @@ -216,6 +208,12 @@ class PlannerServer : public nav2_util::LifecycleNode */ void publishPlan(const nav_msgs::msg::Path & path); + void exceptionWarning( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id, + const std::exception & ex); + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -223,6 +221,10 @@ class PlannerServer : public nav2_util::LifecycleNode rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + // Our action server implements the ComputePathToPose action + std::unique_ptr action_server_pose_; + std::unique_ptr action_server_poses_; + // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e2bc6286a7..c87bd997fe 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -289,32 +289,25 @@ void PlannerServer::getPreemptedGoalIfRequested( template bool PlannerServer::getStartPose( - std::unique_ptr> & action_server, typename std::shared_ptr goal, geometry_msgs::msg::PoseStamped & start) { if (goal->use_start) { start = goal->start; } else if (!costmap_ros_->getRobotPose(start)) { - action_server->terminate_current(); return false; } return true; } -template bool PlannerServer::transformPosesToGlobalFrame( - std::unique_ptr> & action_server, geometry_msgs::msg::PoseStamped & curr_start, geometry_msgs::msg::PoseStamped & curr_goal) { if (!costmap_ros_->transformPoseToGlobalFrame(curr_start, curr_start) || !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal)) { - RCLCPP_WARN( - get_logger(), "Could not transform the start or goal pose in the costmap frame"); - action_server->terminate_current(); return false; } @@ -323,17 +316,15 @@ bool PlannerServer::transformPosesToGlobalFrame( template bool PlannerServer::validatePath( - std::unique_ptr> & action_server, const geometry_msgs::msg::PoseStamped & goal, const nav_msgs::msg::Path & path, const std::string & planner_id) { - if (path.poses.size() == 0) { + if (path.poses.empty()) { RCLCPP_WARN( get_logger(), "Planning algorithm %s failed to generate a valid" " path to (%.2f, %.2f)", planner_id.c_str(), goal.pose.position.x, goal.pose.position.y); - action_server->terminate_current(); return false; } @@ -346,8 +337,7 @@ bool PlannerServer::validatePath( return true; } -void -PlannerServer::computePlanThroughPoses() +void PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); @@ -376,8 +366,8 @@ PlannerServer::computePlanThroughPoses() // Use start pose if provided otherwise use current robot pose geometry_msgs::msg::PoseStamped start; - if (!getStartPose(action_server_poses_, goal, start)) { - return; + if (!getStartPose(goal, start)) { + throw nav2_core::PlannerTFError("Unable to get start pose"); } // Get consecutive paths through these points @@ -393,16 +383,15 @@ PlannerServer::computePlanThroughPoses() curr_goal = goal->goals[i]; // Transform them into the global frame - if (!transformPosesToGlobalFrame(action_server_poses_, curr_start, curr_goal)) { - return; + if (!transformPosesToGlobalFrame(curr_start, curr_goal)) { + throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); } // Get plan from start -> goal nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); - // check path for validity - if (!validatePath(action_server_poses_, curr_goal, curr_path, goal->planner_id)) { - return; + if (!validatePath(curr_goal, curr_path, goal->planner_id)) { + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); } // Concatenate paths together @@ -447,6 +436,8 @@ PlannerServer::computePlan() auto goal = action_server_pose_->get_current_goal(); auto result = std::make_shared(); + geometry_msgs::msg::PoseStamped start; + try { if (isServerInactive(action_server_pose_) || isCancelRequested(action_server_pose_)) { return; @@ -457,21 +448,20 @@ PlannerServer::computePlan() getPreemptedGoalIfRequested(action_server_pose_, goal); // Use start pose if provided otherwise use current robot pose - geometry_msgs::msg::PoseStamped start; - if (!getStartPose(action_server_pose_, goal, start)) { - return; + if (!getStartPose(goal, start)) { + throw nav2_core::PlannerTFError("Unable to get start pose"); } // Transform them into the global frame geometry_msgs::msg::PoseStamped goal_pose = goal->goal; - if (!transformPosesToGlobalFrame(action_server_pose_, start, goal_pose)) { - return; + if (!transformPosesToGlobalFrame(start, goal_pose)) { + throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); } result->path = getPlan(start, goal_pose, goal->planner_id); - if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) { - return; + if (!validatePath(goal_pose, result->path, goal->planner_id)) { + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); } // Publish the plan for visualization purposes @@ -488,12 +478,38 @@ PlannerServer::computePlan() } action_server_pose_->succeeded_current(result); + } catch (nav2_core::StartOccupied & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED; + action_server_pose_->terminate_current(result); + } catch (nav2_core::GoalOccupied & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; + action_server_pose_->terminate_current(result); + } catch (nav2_core::NoValidPathCouldBeFound & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; + action_server_pose_->terminate_current(result); + } catch (nav2_core::PlannerTimedOut & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; + action_server_pose_->terminate_current(result); + } catch (nav2_core::StartOutsideMapBounds & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; + action_server_pose_->terminate_current(result); + } catch (nav2_core::GoalOutsideMapBounds & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; + action_server_pose_->terminate_current(result); + } catch (nav2_core::PlannerTFError & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR; + action_server_pose_->terminate_current(result); } catch (std::exception & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), goal->goal.pose.position.x, - goal->goal.pose.position.y, ex.what()); - action_server_pose_->terminate_current(); + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; + action_server_pose_->terminate_current(result); } } @@ -637,7 +653,6 @@ PlannerServer::dynamicParametersCallback(std::vector paramete { std::lock_guard lock(dynamic_params_lock_); rcl_interfaces::msg::SetParametersResult result; - for (auto parameter : parameters) { const auto & type = parameter.get_type(); const auto & name = parameter.get_name(); @@ -661,6 +676,20 @@ PlannerServer::dynamicParametersCallback(std::vector paramete return result; } +void PlannerServer::exceptionWarning( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id, + const std::exception & ex) +{ + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan from (%.2f, %.2f) to (%0.2f, %.2f): \"%s\"", + planner_id.c_str(), + start.pose.position.x, start.pose.position.y, + goal.pose.position.x, goal.pose.position.y, + ex.what()); +} + } // namespace nav2_planner #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 4b4fe55111..7b2ee31c0e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -25,6 +25,7 @@ #include "Eigen/Core" #include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_core/exceptions.hpp" #include "nav2_smac_planner/thirdparty/robin_hood.h" #include "nav2_smac_planner/analytic_expansion.hpp" diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 06bc0520d4..841b43e357 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -207,12 +207,12 @@ bool AStarAlgorithm::areInputsValid() if (getToleranceHeuristic() < 0.001 && !_goal->isNodeValid(_traverse_unknown, _collision_checker)) { - throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance."); + throw nav2_core::GoalOccupied("Goal was in lethal cost"); } // Check if starting point is valid if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan."); + throw nav2_core::StartOccupied("Start was in lethal cost"); } return true; diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 606f6861ef..46dfa98fb2 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -211,11 +211,19 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Set starting point unsigned int mx_start, my_start, mx_goal, my_goal; - costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); + if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } _a_star->setStart(mx_start, my_start, 0); // Set goal point - costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } _a_star->setGoal(mx_goal, my_goal, 0); // Setup message @@ -233,8 +241,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Corner case of start and goal beeing on the same cell if (mx_start == mx_goal && my_start == my_goal) { if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - RCLCPP_WARN(_logger, "Failed to create a unique pose path because of obstacles"); - return plan; + throw nav2_core::StartOccupied("Start was in lethal cost"); } pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal @@ -250,28 +257,16 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Compute plan Node2D::CoordinateVector path; int num_iterations = 0; - std::string error; - try { - if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) - { - if (num_iterations < _a_star->getMaxIterations()) { - error = std::string("no valid path found"); - } else { - error = std::string("exceeded maximum iterations"); - } + // Note: All exceptions thrown are handled by the planner server and returned to the action + if (!_a_star->createPath( + path, num_iterations, + _tolerance / static_cast(costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + throw nav2_core::NoValidPathCouldBeFound("no valid path found"); + } else { + throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); } - } catch (const std::runtime_error & e) { - error = "invalid use: "; - error += e.what(); - } - - if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); - return plan; } // Convert to world coordinates diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index ee614c7b94..2d0351c5b9 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -287,7 +287,12 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; - costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } + double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); @@ -300,7 +305,11 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setStart(mx, my, orientation_bin_id); // Set goal point, in A* bin search coordinates - costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); @@ -327,28 +336,14 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Compute plan NodeHybrid::CoordinateVector path; int num_iterations = 0; - std::string error; - try { - if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) - { - if (num_iterations < _a_star->getMaxIterations()) { - error = std::string("no valid path found"); - } else { - error = std::string("exceeded maximum iterations"); - } - } - } catch (const std::runtime_error & e) { - error = "invalid use: "; - error += e.what(); - } - if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); - return plan; + // Note: All exceptions thrown are handled by the planner server and returned to the action + if (!_a_star->createPath(path, num_iterations, 0)) { + if (num_iterations < _a_star->getMaxIterations()) { + throw nav2_core::NoValidPathCouldBeFound("no valid path found"); + } else { + throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); + } } // Convert to world coordinates diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index b7e22ee1f9..6d00a3479f 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -247,13 +247,21 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; - _costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (!_costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } _a_star->setStart( mx, my, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); // Set goal point, in A* bin search coordinates - _costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + if (!_costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } _a_star->setGoal( mx, my, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); @@ -274,27 +282,14 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::CoordinateVector path; int num_iterations = 0; std::string error; - try { - if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) - { - if (num_iterations < _a_star->getMaxIterations()) { - error = std::string("no valid path found"); - } else { - error = std::string("exceeded maximum iterations"); - } - } - } catch (const std::runtime_error & e) { - error = "invalid use: "; - error += e.what(); - } - if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); - return plan; + // Note: All exceptions thrown are handled by the planner server and returned to the action + if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { + if (num_iterations < _a_star->getMaxIterations()) { + throw nav2_core::NoValidPathCouldBeFound("no valid path found"); + } else { + throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); + } } // Convert to world coordinates diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index 211962090d..7b124f9764 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -25,6 +25,7 @@ #include #include "rclcpp/rclcpp.hpp" #include "nav2_core/global_planner.hpp" +#include "nav2_core/exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index cc98649ccc..9f4930da72 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -89,13 +89,35 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( // Corner case of start and goal beeing on the same cell unsigned int mx_start, my_start, mx_goal, my_goal; - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); - planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + if (!planner_->costmap_->worldToMap( + start.pose.position.x, start.pose.position.y, mx_start, my_start)) + { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } + + if (!planner_->costmap_->worldToMap( + goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) + { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } + + if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::StartOccupied( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was in lethal cost"); + } + + if (planner_->costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::GoalOccupied( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was in lethal cost"); + } + if (mx_start == mx_goal && my_start == my_goal) { - if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); - return global_path; - } global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; geometry_msgs::msg::PoseStamped pose; @@ -154,13 +176,13 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) { std::vector path; if (planner_->isUnsafeToPlan()) { - RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); global_path.poses.clear(); + throw nav2_core::PlannerException("Either of the start or goal pose are an obstacle! "); } else if (planner_->generatePath(path)) { global_path = linearInterpolation(path, planner_->costmap_->getResolution()); } else { - RCLCPP_ERROR(logger_, "Could not generate path between the given poses"); global_path.poses.clear(); + throw nav2_core::NoValidPathCouldBeFound("Could not generate path between the given poses"); } global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 7776179963..264f737de4 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -174,8 +174,8 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { } goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; - path = planner_2d->createPlan(start, goal); - EXPECT_EQ(static_cast(path.poses.size()), 0); + + EXPECT_THROW(planner_2d->createPlan(start, goal), nav2_core::GoalOccupied); planner_2d->deactivate(); planner_2d->cleanup(); From 53a6af56328a72211fb47ce04ee2808d9b39fa41 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 30 Sep 2022 12:50:01 -0500 Subject: [PATCH 075/131] Fix RMW deprecation warnings (#3226) * Fix RMW deprecation warnings * Deprecated OpenCV header --- .../include/nav2_behavior_tree/bt_service_node.hpp | 2 +- nav2_lifecycle_manager/src/lifecycle_manager.cpp | 4 ++-- nav2_util/include/nav2_util/service_client.hpp | 2 +- .../nav2_waypoint_follower/plugins/photo_at_waypoint.hpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index c1970e38b1..b99e5b74d4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -61,7 +61,7 @@ class BtServiceNode : public BT::ActionNodeBase getInput("service_name", service_name_); service_client_ = node_->create_client( service_name_, - rmw_qos_profile_services_default, + rclcpp::SystemDefaultsQoS(), callback_group_); // Make a request for the service without parameter diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index f18436f21f..2cbe22768c 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -62,13 +62,13 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) manager_srv_ = create_service( get_name() + std::string("/manage_nodes"), std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), - rmw_qos_profile_services_default, + rclcpp::SystemDefaultsQoS(), callback_group_); is_active_srv_ = create_service( get_name() + std::string("/is_active"), std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3), - rmw_qos_profile_services_default, + rclcpp::SystemDefaultsQoS(), callback_group_); transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE; diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 9be5912e8c..62e1b58838 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -46,7 +46,7 @@ class ServiceClient callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); client_ = node_->create_client( service_name, - rmw_qos_profile_services_default, + rclcpp::SystemDefaultsQoS(), callback_group_); } diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp index fc4aee5c5e..49b32e4aa4 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -34,7 +34,7 @@ #include "nav2_core/waypoint_task_executor.hpp" #include "opencv4/opencv2/core.hpp" #include "opencv4/opencv2/opencv.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "image_transport/image_transport.hpp" From 37271751775c6a125ea59ab5fa6a441944701f28 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Wed, 5 Oct 2022 20:58:44 +0300 Subject: [PATCH 076/131] Costmap Filter enabling service (#3229) * Add enabling service to costmap filters * Add service testcase * Fix comment * Use toggle_filter service name --- nav2_costmap_2d/CMakeLists.txt | 2 + .../costmap_filters/costmap_filter.hpp | 17 ++ nav2_costmap_2d/package.xml | 1 + .../costmap_filters/costmap_filter.cpp | 21 +++ nav2_costmap_2d/test/unit/CMakeLists.txt | 5 + .../test/unit/costmap_filter_service_test.cpp | 150 ++++++++++++++++++ 6 files changed, 196 insertions(+) create mode 100644 nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 84f86617ab..74f4dd1857 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(rmw REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -66,6 +67,7 @@ set(dependencies rclcpp_lifecycle sensor_msgs std_msgs + std_srvs tf2 tf2_geometry_msgs tf2_ros 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 index c71d3bfea1..c390cd8ec6 100644 --- 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 @@ -42,6 +42,7 @@ #include #include "geometry_msgs/msg/pose2_d.hpp" +#include "std_srvs/srv/set_bool.hpp" #include "nav2_costmap_2d/layer.hpp" namespace nav2_costmap_2d @@ -153,6 +154,17 @@ class CostmapFilter : public Layer virtual void resetFilter() = 0; protected: + /** + * @brief Costmap filter enabling/disabling callback + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void enableCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + /** * @brief: Name of costmap filter info topic */ @@ -168,6 +180,11 @@ class CostmapFilter : public Layer */ tf2::Duration transform_tolerance_; + /** + * @brief: A service to enable/disable costmap filter + */ + rclcpp::Service::SharedPtr enable_service_; + private: /** * @brief: Latest robot position diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index e466dd9847..d13497946d 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -32,6 +32,7 @@ rclcpp_lifecycle sensor_msgs std_msgs + std_srvs tf2 tf2_geometry_msgs tf2_ros diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index cdec0a4b2e..ce2f61c2dc 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -72,6 +72,13 @@ void CostmapFilter::onInitialize() double transform_tolerance; node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + + // Costmap Filter enabling service + enable_service_ = node->create_service( + name_ + "/toggle_filter", + std::bind( + &CostmapFilter::enableCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } catch (const std::exception & ex) { RCLCPP_ERROR(logger_, "Parameter problem: %s", ex.what()); throw ex; @@ -120,4 +127,18 @@ void CostmapFilter::updateCosts( current_ = true; } +void CostmapFilter::enableCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + enabled_ = request->data; + response->success = true; + if (enabled_) { + response->message = "Enabled"; + } else { + response->message = "Disabled"; + } +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 7a1ec6a909..cf7dd6826f 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -34,3 +34,8 @@ ament_add_gtest(copy_window_test copy_window_test.cpp) target_link_libraries(copy_window_test nav2_costmap_2d_core ) + +ament_add_gtest(costmap_filter_service_test costmap_filter_service_test.cpp) +target_link_libraries(costmap_filter_service_test + nav2_costmap_2d_core +) diff --git a/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp new file mode 100644 index 0000000000..2047acbd13 --- /dev/null +++ b/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp @@ -0,0 +1,150 @@ +// Copyright (c) 2022 Samsung R&D Institute 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_util/lifecycle_node.hpp" + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" +#include "std_srvs/srv/set_bool.hpp" + +static const char FILTER_NAME[]{"costmap_filter"}; + +class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter +{ +public: + // Dummy implementations of virtual methods + void initializeFilter( + const std::string &) {} + + void process( + nav2_costmap_2d::Costmap2D &, + int, int, int, int, + const geometry_msgs::msg::Pose2D &) {} + + void resetFilter() {} + + // Actual testing methods + void setName(const std::string & name) + { + name_ = name; + } + + void setNode(const nav2_util::LifecycleNode::WeakPtr & node) + { + node_ = node; + } + + bool getEnabled() + { + return enabled_; + } +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() + { + // Create new LifecycleNode + node_ = std::make_shared("test_node"); + + // Create new CostmapFilter + costmap_filter_ = std::make_shared(); + costmap_filter_->setNode(node_); + costmap_filter_->setName(FILTER_NAME); + + // Set CostmapFilter ROS-parameters + node_->declare_parameter( + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue("filter_info")); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", "filter_info")); + } + + ~TestNode() + { + costmap_filter_.reset(); + node_.reset(); + } + + template + typename T::Response::SharedPtr send_request( + nav2_util::LifecycleNode::SharedPtr node, + typename rclcpp::Client::SharedPtr client, + typename T::Request::SharedPtr request) + { + auto result = client->async_send_request(request); + + // Wait for the result + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + return result.get(); + } else { + return nullptr; + } + } + +protected: + nav2_util::LifecycleNode::SharedPtr node_; + std::shared_ptr costmap_filter_; +}; + +TEST_F(TestNode, testEnableService) +{ + costmap_filter_->onInitialize(); + + RCLCPP_INFO(node_->get_logger(), "Testing enabling service"); + auto req = std::make_shared(); + auto client = node_->create_client( + std::string(FILTER_NAME) + "/toggle_filter"); + + RCLCPP_INFO(node_->get_logger(), "Waiting for enabling service"); + ASSERT_TRUE(client->wait_for_service()); + + // Set costmap filter enabled + req->data = true; + auto resp = send_request(node_, client, req); + + ASSERT_NE(resp, nullptr); + ASSERT_TRUE(resp->success); + ASSERT_EQ(resp->message, "Enabled"); + ASSERT_TRUE(costmap_filter_->getEnabled()); + + // Set costmap filter disabled + req->data = false; + resp = send_request(node_, client, req); + + ASSERT_NE(resp, nullptr); + ASSERT_TRUE(resp->success); + ASSERT_EQ(resp->message, "Disabled"); + ASSERT_FALSE(costmap_filter_->getEnabled()); +} + +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; +} From bd715a50cec1691192676a536cc1b07900f1d62a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 5 Oct 2022 16:27:55 -0700 Subject: [PATCH 077/131] Fixing Ogre deprecation build warning --- .../particle_cloud_display/flat_weighted_arrows_array.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp index 7647aaf67f..23757051c6 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp" From 185b7af5dc1b748a3603cd03c3ca71556fc98f19 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Mon, 10 Oct 2022 20:43:55 +0300 Subject: [PATCH 078/131] Add binary flip costmap filter (#3228) * Add binary flip costmap filter * Move transformPose, worldToMask, getMaskData to CostmapFilter * Added default parametrized binary filter state * Switched to std_msgs/msg/Bool.msg * Use arbitrary filter values --- nav2_costmap_2d/CMakeLists.txt | 1 + nav2_costmap_2d/costmap_plugins.xml | 3 + .../costmap_filters/binary_filter.hpp | 126 +++ .../costmap_filters/costmap_filter.hpp | 43 + .../costmap_filters/filter_values.hpp | 1 + .../costmap_filters/keepout_filter.hpp | 1 - .../costmap_filters/speed_filter.hpp | 32 - .../plugins/costmap_filters/binary_filter.cpp | 268 ++++++ .../costmap_filters/costmap_filter.cpp | 65 ++ .../plugins/costmap_filters/speed_filter.cpp | 72 +- nav2_costmap_2d/test/unit/CMakeLists.txt | 6 + .../test/unit/binary_filter_test.cpp | 877 ++++++++++++++++++ 12 files changed, 1393 insertions(+), 102 deletions(-) create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp create mode 100644 nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp create mode 100644 nav2_costmap_2d/test/unit/binary_filter_test.cpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 74f4dd1857..96b4f49339 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -98,6 +98,7 @@ target_link_libraries(layers add_library(filters SHARED plugins/costmap_filters/keepout_filter.cpp plugins/costmap_filters/speed_filter.cpp + plugins/costmap_filters/binary_filter.cpp ) ament_target_dependencies(filters ${dependencies} diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index d7cb985493..e5867d5108 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -24,6 +24,9 @@ Restricts maximum speed of robot in speed-limit zones marked on map. + + Binary flip filter. + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp new file mode 100644 index 0000000000..e36507ef8f --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp @@ -0,0 +1,126 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022 Samsung R&D Institute 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_FILTERS__BINARY_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__BINARY_FILTER_HPP_ + +#include +#include + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + +#include "std_msgs/msg/bool.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" + +namespace nav2_costmap_2d +{ +/** + * @class BinaryFilter + * @brief Reads in a speed restriction mask and enables a robot to + * dynamically adjust speed based on pose in map to slow in dangerous + * areas. Done via absolute speed setting or percentage of maximum speed + */ +class BinaryFilter : public CostmapFilter +{ +public: + /** + * @brief A constructor + */ + BinaryFilter(); + + /** + * @brief Initialize the filter and subscribe to the info topic + */ + void initializeFilter( + const std::string & filter_info_topic); + + /** + * @brief Process the keepout layer at the current pose / bounds / grid + */ + 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); + + /** + * @brief Reset the costmap filter / topic / info + */ + void resetFilter(); + + /** + * @brief If this filter is active + */ + bool isActive(); + +private: + /** + * @brief Callback for the filter information + */ + void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg); + /** + * @brief Callback for the filter mask + */ + void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + /** + * @brief Changes binary state of filter. Sends a message with new state. + * @param state New binary state + */ + void changeState(const bool state); + + // Working with filter info and mask + rclcpp::Subscription::SharedPtr filter_info_sub_; + rclcpp::Subscription::SharedPtr mask_sub_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr binary_state_pub_; + + nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; + + std::string mask_frame_; // Frame where mask located in + std::string global_frame_; // Frame of currnet layer (master_grid) + + double base_, multiplier_; + // Filter values higher than this threshold, + // will set binary state to non-default + double flip_threshold_; + + bool default_state_; // Default Binary Filter state + bool binary_state_; // Current Binary Filter state +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__BINARY_FILTER_HPP_ 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 index c390cd8ec6..35f53c6b96 100644 --- 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 @@ -44,6 +44,7 @@ #include "geometry_msgs/msg/pose2_d.hpp" #include "std_srvs/srv/set_bool.hpp" #include "nav2_costmap_2d/layer.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" namespace nav2_costmap_2d { @@ -165,6 +166,48 @@ class CostmapFilter : public Layer const std::shared_ptr request, std::shared_ptr response); + /** + * @brief: Transforms robot pose from current layer frame to mask frame + * @param: global_frame Costmap frame to transform from + * @param: global_pose Robot pose in costmap frame + * @param: mask_frame Filter mask frame to transform to + * @param: mask_pose Output robot pose in mask frame + * @return: True if the transformation was successful, false otherwise + */ + bool transformPose( + const std::string global_frame, + const geometry_msgs::msg::Pose2D & global_pose, + const std::string mask_frame, + geometry_msgs::msg::Pose2D & mask_pose) const; + + /** + * @brief: Convert from world coordinates to mask coordinates. + Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. + * @param filter_mask Filter mask on which to convert + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated mask x coordinate + * @param my Will be set to the associated mask y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ + bool worldToMask( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, + double wx, double wy, unsigned int & mx, unsigned int & my) const; + + /** + * @brief Get the data of a cell in the filter mask + * @param filter_mask Filter mask to get the data from + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @return The data of the selected cell + */ + inline int8_t getMaskData( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, + const unsigned int mx, const unsigned int my) const + { + return filter_mask->data[my * filter_mask->info.width + mx]; + } + /** * @brief: Name of costmap filter info topic */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp index 03a157a3e4..89bdacf1ba 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp @@ -47,6 +47,7 @@ namespace nav2_costmap_2d static constexpr uint8_t KEEPOUT_FILTER = 0; static constexpr uint8_t SPEED_FILTER_PERCENT = 1; static constexpr uint8_t SPEED_FILTER_ABSOLUTE = 2; +static constexpr uint8_t BINARY_FILTER = 3; /** Default values for base and multiplier */ static constexpr double BASE_DEFAULT = 0.0; 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 index e5434571d4..54159cf624 100644 --- 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 @@ -44,7 +44,6 @@ #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" #include "rclcpp/rclcpp.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_msgs/msg/costmap_filter_info.hpp" namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp index c4655607aa..3ad4b9ba27 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp @@ -43,7 +43,6 @@ #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_msgs/msg/costmap_filter_info.hpp" #include "nav2_msgs/msg/speed_limit.hpp" @@ -87,37 +86,6 @@ class SpeedFilter : public CostmapFilter */ bool isActive(); -protected: - /** - * @brief: Transforms robot pose from current layer frame to mask frame - * @param: pose Robot pose in costmap frame - * @param: mask_pose Robot pose in mask frame - * @return: True if the transformation was successful, false otherwise - */ - bool transformPose( - const geometry_msgs::msg::Pose2D & pose, - geometry_msgs::msg::Pose2D & mask_pose) const; - - /** - * @brief: Convert from world coordinates to mask coordinates. - Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. - * @param wx The x world coordinate - * @param wy The y world coordinate - * @param mx Will be set to the associated mask x coordinate - * @param my Will be set to the associated mask y coordinate - * @return True if the conversion was successful (legal bounds) false otherwise - */ - bool worldToMask(double wx, double wy, unsigned int & mx, unsigned int & my) const; - - /** - * @brief Get the data of a cell in the filter mask - * @param mx The x coordinate of the cell - * @param my The y coordinate of the cell - * @return The data of the selected cell - */ - inline int8_t getMaskData( - const unsigned int mx, const unsigned int my) const; - private: /** * @brief Callback for the filter information diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp new file mode 100644 index 0000000000..b57a27c414 --- /dev/null +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -0,0 +1,268 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022 Samsung R&D Institute 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/binary_filter.hpp" + +#include +#include +#include +#include + +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_util/occ_grid_values.hpp" + +namespace nav2_costmap_2d +{ + +BinaryFilter::BinaryFilter() +: filter_info_sub_(nullptr), mask_sub_(nullptr), + binary_state_pub_(nullptr), filter_mask_(nullptr), mask_frame_(""), global_frame_(""), + default_state_(false), binary_state_(default_state_) +{ +} + +void BinaryFilter::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"}; + } + + // Declare parameters specific to BinaryFilter only + std::string binary_state_topic; + declareParameter("default_state", rclcpp::ParameterValue(false)); + node->get_parameter(name_ + "." + "default_state", default_state_); + declareParameter("binary_state_topic", rclcpp::ParameterValue("binary_state")); + node->get_parameter(name_ + "." + "binary_state_topic", binary_state_topic); + declareParameter("flip_threshold", rclcpp::ParameterValue(50.0)); + node->get_parameter(name_ + "." + "flip_threshold", flip_threshold_); + + filter_info_topic_ = filter_info_topic; + // Setting new costmap filter info subscriber + RCLCPP_INFO( + logger_, + "BinaryFilter: 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(&BinaryFilter::filterInfoCallback, this, std::placeholders::_1)); + + // Get global frame required for binary state publisher + global_frame_ = layered_costmap_->getGlobalFrameID(); + + // Create new binary state publisher + binary_state_pub_ = node->create_publisher( + binary_state_topic, rclcpp::QoS(10)); + binary_state_pub_->on_activate(); + + // Reset parameters + base_ = BASE_DEFAULT; + multiplier_ = MULTIPLIER_DEFAULT; + + // Initialize state as "false" by-default + changeState(default_state_); +} + +void BinaryFilter::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"}; + } + + if (!mask_sub_) { + RCLCPP_INFO( + logger_, + "BinaryFilter: Received filter info from %s topic.", filter_info_topic_.c_str()); + } else { + RCLCPP_WARN( + logger_, + "BinaryFilter: New costmap filter info arrived from %s topic. Updating old filter info.", + filter_info_topic_.c_str()); + // Resetting previous subscriber each time when new costmap filter information arrives + mask_sub_.reset(); + } + + if (msg->type != BINARY_FILTER) { + RCLCPP_ERROR(logger_, "BinaryFilter: Mode %i is not supported", msg->type); + return; + } + + // Set base_ and multiplier_ + base_ = msg->base; + multiplier_ = msg->multiplier; + // Set topic name to receive filter mask from + mask_topic_ = msg->filter_mask_topic; + + // Setting new filter mask subscriber + RCLCPP_INFO( + logger_, + "BinaryFilter: Subscribing to \"%s\" topic for filter mask...", + mask_topic_.c_str()); + mask_sub_ = node->create_subscription( + mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&BinaryFilter::maskCallback, this, std::placeholders::_1)); +} + +void BinaryFilter::maskCallback( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + if (!filter_mask_) { + RCLCPP_INFO( + logger_, + "BinaryFilter: Received filter mask from %s topic.", mask_topic_.c_str()); + } else { + RCLCPP_WARN( + logger_, + "BinaryFilter: New filter mask arrived from %s topic. Updating old filter mask.", + mask_topic_.c_str()); + filter_mask_.reset(); + } + + filter_mask_ = msg; + mask_frame_ = msg->header.frame_id; +} + +void BinaryFilter::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()); + + if (!filter_mask_) { + // Show warning message every 2 seconds to not litter an output + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "BinaryFilter: Filter mask was not received"); + return; + } + + geometry_msgs::msg::Pose2D mask_pose; // robot coordinates in mask frame + + // Transforming robot pose from current layer frame to mask frame + if (!transformPose(global_frame_, pose, mask_frame_, mask_pose)) { + return; + } + + // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) + unsigned int mask_robot_i, mask_robot_j; + if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + // Robot went out of mask range. Set "false" state by-default + RCLCPP_WARN( + logger_, + "BinaryFilter: Robot is outside of filter mask. Resetting binary state to default."); + changeState(default_state_); + return; + } + + // Getting filter_mask data from cell where the robot placed + int8_t mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j); + if (mask_data == nav2_util::OCC_GRID_UNKNOWN) { + // Corresponding filter mask cell is unknown. + // Warn and do nothing. + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "BinaryFilter: Filter mask [%i, %i] data is unknown. Do nothing.", + mask_robot_i, mask_robot_j); + return; + } + // Check and flip binary state, if necessary + if (base_ + mask_data * multiplier_ > flip_threshold_) { + if (binary_state_ == default_state_) { + changeState(!default_state_); + } + } else { + if (binary_state_ != default_state_) { + changeState(default_state_); + } + } +} + +void BinaryFilter::resetFilter() +{ + std::lock_guard guard(*getMutex()); + + RCLCPP_INFO(logger_, "BinaryFilter: Resetting the filter to default state"); + changeState(default_state_); + + filter_info_sub_.reset(); + mask_sub_.reset(); + if (binary_state_pub_) { + binary_state_pub_->on_deactivate(); + binary_state_pub_.reset(); + } +} + +bool BinaryFilter::isActive() +{ + std::lock_guard guard(*getMutex()); + + if (filter_mask_) { + return true; + } + return false; +} + +void BinaryFilter::changeState(const bool state) +{ + binary_state_ = state; + if (state) { + RCLCPP_INFO(logger_, "BinaryFilter: Switched on"); + } else { + RCLCPP_INFO(logger_, "BinaryFilter: Switched off"); + } + + // Forming and publishing new BinaryState message + std::unique_ptr msg = + std::make_unique(); + msg->data = state; + binary_state_pub_->publish(std::move(msg)); +} + +} // namespace nav2_costmap_2d + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::BinaryFilter, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index ce2f61c2dc..4004549490 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -39,6 +39,9 @@ #include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" + namespace nav2_costmap_2d { @@ -141,4 +144,66 @@ void CostmapFilter::enableCallback( } } +bool CostmapFilter::transformPose( + const std::string global_frame, + const geometry_msgs::msg::Pose2D & global_pose, + const std::string mask_frame, + geometry_msgs::msg::Pose2D & mask_pose) const +{ + if (mask_frame != global_frame) { + // Filter mask and current layer are in different frames: + // Transform (global_pose.x, global_pose.y) point from current layer frame (global_frame) + // to mask_pose point in mask_frame + geometry_msgs::msg::TransformStamped transform; + geometry_msgs::msg::PointStamped in, out; + in.header.stamp = clock_->now(); + in.header.frame_id = global_frame; + in.point.x = global_pose.x; + in.point.y = global_pose.y; + in.point.z = 0; + + try { + tf_->transform(in, out, mask_frame, transform_tolerance_); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, + "CostmapFilter: failed to get costmap frame (%s) " + "transformation to mask frame (%s) with error: %s", + global_frame.c_str(), mask_frame.c_str(), ex.what()); + return false; + } + mask_pose.x = out.point.x; + mask_pose.y = out.point.y; + } else { + // Filter mask and current layer are in the same frame: + // Just use global_pose coordinates + mask_pose = global_pose; + } + + return true; +} + +bool CostmapFilter::worldToMask( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, + double wx, double wy, unsigned int & mx, unsigned int & my) const +{ + double origin_x = filter_mask->info.origin.position.x; + double origin_y = filter_mask->info.origin.position.y; + double resolution = filter_mask->info.resolution; + unsigned int size_x = filter_mask->info.width; + unsigned int size_y = filter_mask->info.height; + + if (wx < origin_x || wy < origin_y) { + return false; + } + + mx = std::round((wx - origin_x) / resolution); + my = std::round((wy - origin_y) / resolution); + if (mx >= size_x || my >= size_y) { + return false; + } + + return true; +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index cc368b4683..5f984a10fc 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -41,8 +41,6 @@ #include #include #include -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -174,70 +172,6 @@ void SpeedFilter::maskCallback( mask_frame_ = msg->header.frame_id; } -bool SpeedFilter::transformPose( - const geometry_msgs::msg::Pose2D & pose, - geometry_msgs::msg::Pose2D & mask_pose) const -{ - if (mask_frame_ != global_frame_) { - // Filter mask and current layer are in different frames: - // Transform (pose.x, pose.y) point from current layer frame (global_frame_) - // to mask_pose point in mask_frame_ - geometry_msgs::msg::TransformStamped transform; - geometry_msgs::msg::PointStamped in, out; - in.header.stamp = clock_->now(); - in.header.frame_id = global_frame_; - in.point.x = pose.x; - in.point.y = pose.y; - in.point.z = 0; - - try { - tf_->transform(in, out, mask_frame_, transform_tolerance_); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - logger_, - "SpeedFilter: failed to get costmap frame (%s) " - "transformation to mask frame (%s) with error: %s", - global_frame_.c_str(), mask_frame_.c_str(), ex.what()); - return false; - } - mask_pose.x = out.point.x; - mask_pose.y = out.point.y; - } else { - // Filter mask and current layer are in the same frame: - // Just use pose coordinates - mask_pose = pose; - } - - return true; -} - -bool SpeedFilter::worldToMask(double wx, double wy, unsigned int & mx, unsigned int & my) const -{ - double origin_x = filter_mask_->info.origin.position.x; - double origin_y = filter_mask_->info.origin.position.y; - double resolution = filter_mask_->info.resolution; - unsigned int size_x = filter_mask_->info.width; - unsigned int size_y = filter_mask_->info.height; - - if (wx < origin_x || wy < origin_y) { - return false; - } - - mx = std::round((wx - origin_x) / resolution); - my = std::round((wy - origin_y) / resolution); - if (mx >= size_x || my >= size_y) { - return false; - } - - return true; -} - -inline int8_t SpeedFilter::getMaskData( - const unsigned int mx, const unsigned int my) const -{ - return filter_mask_->data[my * filter_mask_->info.width + mx]; -} - void SpeedFilter::process( nav2_costmap_2d::Costmap2D & /*master_grid*/, int /*min_i*/, int /*min_j*/, int /*max_i*/, int /*max_j*/, @@ -256,19 +190,19 @@ void SpeedFilter::process( geometry_msgs::msg::Pose2D mask_pose; // robot coordinates in mask frame // Transforming robot pose from current layer frame to mask frame - if (!transformPose(pose, mask_pose)) { + if (!transformPose(global_frame_, pose, mask_frame_, mask_pose)) { return; } // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { return; } // Getting filter_mask data from cell where the robot placed and // calculating speed limit value - int8_t speed_mask_data = getMaskData(mask_robot_i, mask_robot_j); + int8_t speed_mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j); if (speed_mask_data == SPEED_MASK_NO_LIMIT) { // Corresponding filter mask cell is free. // Setting no speed limit there. diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index cf7dd6826f..cb54ecd856 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -30,6 +30,12 @@ target_link_libraries(speed_filter_test filters ) +ament_add_gtest(binary_filter_test binary_filter_test.cpp) +target_link_libraries(binary_filter_test + nav2_costmap_2d_core + filters +) + ament_add_gtest(copy_window_test copy_window_test.cpp) target_link_libraries(copy_window_test nav2_costmap_2d_core diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp new file mode 100644 index 0000000000..31bdfc025b --- /dev/null +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -0,0 +1,877 @@ +// Copyright (c) 2022 Samsung R&D Institute 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "std_msgs/msg/bool.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/filter_values.hpp" +#include "nav2_costmap_2d/costmap_filters/binary_filter.hpp" + +using namespace std::chrono_literals; + +static const char FILTER_NAME[]{"binary_filter"}; +static const char INFO_TOPIC[]{"costmap_filter_info"}; +static const char MASK_TOPIC[]{"mask"}; +static const char BINARY_STATE_TOPIC[]{"binary_state"}; + +static const double NO_TRANSLATION = 0.0; +static const double TRANSLATION_X = 1.0; +static const double TRANSLATION_Y = 1.0; + +static const uint8_t INCORRECT_TYPE = 200; + +class InfoPublisher : public rclcpp::Node +{ +public: + InfoPublisher(uint8_t type, const char * mask_topic, 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 = type; + msg->filter_mask_topic = mask_topic; + msg->base = static_cast(base); + msg->multiplier = static_cast(multiplier); + + publisher_->publish(std::move(msg)); + } + + ~InfoPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // InfoPublisher + +class MaskPublisher : public rclcpp::Node +{ +public: + explicit 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 + +class BinaryStateSubscriber : public rclcpp::Node +{ +public: + explicit BinaryStateSubscriber(const std::string & binary_state_topic, bool default_state) + : Node("binary_state_sub"), binary_state_updated_(false) + { + subscriber_ = this->create_subscription( + binary_state_topic, rclcpp::QoS(10), + std::bind(&BinaryStateSubscriber::binaryStateCallback, this, std::placeholders::_1)); + + // Initialize with default state + msg_ = std::make_shared(); + msg_->data = default_state; + } + + void binaryStateCallback( + const std_msgs::msg::Bool::SharedPtr msg) + { + msg_ = msg; + binary_state_updated_ = true; + } + + std_msgs::msg::Bool::SharedPtr getBinaryState() + { + return msg_; + } + + inline bool binaryStateUpdated() + { + return binary_state_updated_; + } + + inline void resetBinaryStateIndicator() + { + binary_state_updated_ = false; + } + +private: + rclcpp::Subscription::SharedPtr subscriber_; + std_msgs::msg::Bool::SharedPtr msg_; + bool binary_state_updated_; +}; // BinaryStateSubscriber + +class TestMask : public nav_msgs::msg::OccupancyGrid +{ +public: + TestMask( + unsigned int width, unsigned int height, double resolution, + const std::string & mask_frame) + : width_(width), height_(height) + { + // Fill filter mask info + header.frame_id = mask_frame; + info.resolution = resolution; + info.width = width_; + info.height = height_; + info.origin.position.x = 0.0; + info.origin.position.y = 0.0; + info.origin.position.z = 0.0; + info.origin.orientation.x = 0.0; + info.origin.orientation.y = 0.0; + info.origin.orientation.z = 0.0; + info.origin.orientation.w = 1.0; + + // Fill test mask as follows: + // + // mask (10,11) + // *----------------* + // |91|92|...|99|100| + // |... | + // |... | + // |11|12|13|...| 20| + // | 1| 2| 3|...| 10| + // |-1| 0| 0|...| 0| + // *----------------* + // (0,0) + data.resize(width_ * height_, nav2_util::OCC_GRID_UNKNOWN); + + unsigned int mx, my; + data[0] = nav2_util::OCC_GRID_UNKNOWN; + for (mx = 1; mx < width_; mx++) { + data[mx] = nav2_util::OCC_GRID_FREE; + } + unsigned int it; + for (my = 1; my < height_; my++) { + for (mx = 0; mx < width_; mx++) { + it = mx + my * width_; + data[it] = makeData(mx, my); + } + } + } + + inline int8_t makeData(unsigned int mx, unsigned int my) + { + return mx + (my - 1) * width_ + 1; + } + +private: + const unsigned int width_; + const unsigned int height_; +}; // TestMask + +class TestNode : public ::testing::Test +{ +public: + TestNode() + : default_state_(false) {} + + ~TestNode() {} + +protected: + void createMaps(const std::string & mask_frame); + void publishMaps(uint8_t type, const char * mask_topic, double base, double multiplier); + void rePublishInfo(uint8_t type, const char * mask_topic, double base, double multiplier); + void rePublishMask(); + void setDefaultState(bool default_state); // NOTE: must be called before createBinaryFilter() + bool createBinaryFilter(const std::string & global_frame, double flip_threshold); + void createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame); + void publishTransform(); + + // Test methods + void testFullMask( + double base, double multiplier, double flip_threshold, double tr_x, double tr_y); + void testSimpleMask( + double base, double multiplier, double flip_threshold, double tr_x, double tr_y); + void testOutOfMask(); + void testIncorrectTF(); + void testResetFilter(); + + void resetMaps(); + void reset(); + + std::shared_ptr binary_filter_; + std::shared_ptr master_grid_; + + bool default_state_; + +private: + void waitSome(const std::chrono::nanoseconds & duration); + std_msgs::msg::Bool::SharedPtr getBinaryState(); + std_msgs::msg::Bool::SharedPtr waitBinaryState(); + bool getSign( + unsigned int x, unsigned int y, double base, double multiplier, double flip_threshold); + void verifyBinaryState(bool sign, std_msgs::msg::Bool::SharedPtr state); + + const unsigned int width_ = 10; + const unsigned int height_ = 11; + const double resolution_ = 1.0; + + nav2_util::LifecycleNode::SharedPtr node_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr tf_broadcaster_; + std::unique_ptr transform_; + + std::shared_ptr mask_; + + std::shared_ptr info_publisher_; + std::shared_ptr mask_publisher_; + std::shared_ptr binary_state_subscriber_; +}; + +void TestNode::createMaps(const std::string & mask_frame) +{ + // Make map and mask put as follows: + // master_grid (12,13) + // *----------------* + // | | + // | mask (10,11) | + // | *-------* | + // | |///////| | + // | |///////| | + // | |///////| | + // | *-------* | + // | (0,0) | + // | | + // *----------------* + // (-2,-2) + + // Create master_grid_ + master_grid_ = std::make_shared( + width_ + 4, height_ + 4, resolution_, -2.0, -2.0, nav2_costmap_2d::FREE_SPACE); + + // Create mask_ + mask_ = std::make_shared(width_, height_, resolution_, mask_frame); +} + +void TestNode::publishMaps( + uint8_t type, const char * mask_topic, double base, double multiplier) +{ + info_publisher_ = std::make_shared(type, mask_topic, base, multiplier); + mask_publisher_ = std::make_shared(*mask_); +} + +void TestNode::rePublishInfo( + uint8_t type, const char * mask_topic, double base, double multiplier) +{ + info_publisher_.reset(); + info_publisher_ = std::make_shared(type, mask_topic, base, multiplier); + // Allow both CostmapFilterInfo and filter mask subscribers + // to receive a new message + waitSome(100ms); +} + +void TestNode::rePublishMask() +{ + mask_publisher_.reset(); + mask_publisher_ = std::make_shared(*mask_); + // Allow filter mask subscriber to receive a new message + waitSome(100ms); +} + +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()); + rclcpp::spin_some(binary_state_subscriber_); + std::this_thread::sleep_for(10ms); + } +} + +std_msgs::msg::Bool::SharedPtr TestNode::getBinaryState() +{ + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(binary_state_subscriber_); + return binary_state_subscriber_->getBinaryState(); +} + +std_msgs::msg::Bool::SharedPtr TestNode::waitBinaryState() +{ + const std::chrono::nanoseconds timeout = 500ms; + + rclcpp::Time start_time = node_->now(); + binary_state_subscriber_->resetBinaryStateIndicator(); + while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (binary_state_subscriber_->binaryStateUpdated()) { + binary_state_subscriber_->resetBinaryStateIndicator(); + return binary_state_subscriber_->getBinaryState(); + } + rclcpp::spin_some(binary_state_subscriber_); + std::this_thread::sleep_for(10ms); + } + return nullptr; +} + +void TestNode::setDefaultState(bool default_state) +{ + default_state_ = default_state; +} + +bool TestNode::createBinaryFilter(const std::string & global_frame, double flip_threshold) +{ + node_ = std::make_shared("test_node"); + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); + + nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false); + + node_->declare_parameter( + std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".default_state", rclcpp::ParameterValue(default_state_)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".default_state", default_state_)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".binary_state_topic", rclcpp::ParameterValue(BINARY_STATE_TOPIC)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".binary_state_topic", BINARY_STATE_TOPIC)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".flip_threshold", rclcpp::ParameterValue(flip_threshold)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".flip_threshold", flip_threshold)); + + binary_filter_ = std::make_shared(); + binary_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr); + binary_filter_->initializeFilter(INFO_TOPIC); + + binary_state_subscriber_ = + std::make_shared(BINARY_STATE_TOPIC, default_state_); + + // Wait until mask will be received by BinaryFilter + const std::chrono::nanoseconds timeout = 500ms; + rclcpp::Time start_time = node_->now(); + while (!binary_filter_->isActive()) { + if (node_->now() - start_time > rclcpp::Duration(timeout)) { + return false; + } + rclcpp::spin_some(node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return true; +} + +void TestNode::createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame) +{ + tf_broadcaster_ = std::make_shared(node_); + + transform_ = std::make_unique(); + transform_->header.frame_id = mask_frame; + transform_->child_frame_id = global_frame; + + transform_->header.stamp = node_->now() + rclcpp::Duration(100ms); + transform_->transform.translation.x = TRANSLATION_X; + transform_->transform.translation.y = TRANSLATION_Y; + transform_->transform.translation.z = 0.0; + transform_->transform.rotation.x = 0.0; + transform_->transform.rotation.y = 0.0; + transform_->transform.rotation.z = 0.0; + transform_->transform.rotation.w = 1.0; + + tf_broadcaster_->sendTransform(*transform_); + + // Allow tf_buffer_ to be filled by listener + waitSome(100ms); +} + +void TestNode::publishTransform() +{ + if (tf_broadcaster_) { + transform_->header.stamp = node_->now() + rclcpp::Duration(100ms); + tf_broadcaster_->sendTransform(*transform_); + } +} + +bool TestNode::getSign( + unsigned int x, unsigned int y, double base, double multiplier, double flip_threshold) +{ + const int8_t cost = mask_->makeData(x, y); + return base + cost * multiplier > flip_threshold; +} + +void TestNode::verifyBinaryState(bool sign, std_msgs::msg::Bool::SharedPtr state) +{ + ASSERT_TRUE(state != nullptr); + if (sign) { + EXPECT_FALSE(state->data == default_state_); + } else { + EXPECT_TRUE(state->data == default_state_); + } +} + +void TestNode::testFullMask( + double base, double multiplier, double flip_threshold, double tr_x, double tr_y) +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + unsigned int x, y; + bool prev_sign = false; + bool sign; + + // data = 0 + x = 1; + y = 0; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + sign = getSign(x, y, base, multiplier, flip_threshold); + if (sign != prev_sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + + // data in range [1..100] (sparsed for testing speed) + for (y = 1; y < height_; y += 2) { + for (x = 0; x < width_; x += 2) { + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + + sign = getSign(x, y, base, multiplier, flip_threshold); + if (prev_sign != sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + } + } + + // data = -1 (unknown) + bool prev_state = binary_state->data; + pose.x = -tr_x; + pose.y = -tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = getBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, prev_state); // Binary state won't be updated +} + +void TestNode::testSimpleMask( + double base, double multiplier, double flip_threshold, double tr_x, double tr_y) +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + unsigned int x, y; + bool prev_sign = false; + bool sign; + + // data = 0 + x = 1; + y = 0; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + sign = getSign(x, y, base, multiplier, flip_threshold); + if (sign != prev_sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + + // data = + x = width_ / 2 - 1; + y = height_ / 2 - 1; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + + sign = getSign(x, y, base, multiplier, flip_threshold); + if (prev_sign != sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + + // data = 100 + x = width_ - 1; + y = height_ - 1; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + + sign = getSign(x, y, base, multiplier, flip_threshold); + if (prev_sign != sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + + // data = -1 (unknown) + bool prev_state = binary_state->data; + pose.x = -tr_x; + pose.y = -tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = getBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, prev_state); // Binary state won't be updated +} + +void TestNode::testOutOfMask() +{ + // base, multiplier and flip_threshold should have values as below for this test + const double base = 0.0; + const double multiplier = 1.0; + const double flip_threshold = 10.0; + + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + // data = + pose.x = width_ / 2 - 1; + pose.y = height_ / 2 - 1; + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = waitBinaryState(); + verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state); + + // Then go to out of mask bounds and ensure that binary state is set back to default + pose.x = -2.0; + pose.y = -2.0; + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = getBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, default_state_); + + pose.x = width_ + 1.0; + pose.y = height_ + 1.0; + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = getBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, default_state_); +} + +void TestNode::testIncorrectTF() +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + // data = + pose.x = width_ / 2 - 1; + pose.y = height_ / 2 - 1; + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = waitBinaryState(); + ASSERT_TRUE(binary_state == nullptr); +} + +void TestNode::testResetFilter() +{ + // base, multiplier and flip_threshold should have values as below for this test + const double base = 0.0; + const double multiplier = 1.0; + const double flip_threshold = 10.0; + + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + // Switch-on binary filter + pose.x = width_ / 2 - 1; + pose.y = height_ / 2 - 1; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = waitBinaryState(); + verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state); + + // Reset binary filter and check its state was resetted to default + binary_filter_->resetFilter(); + binary_state = waitBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, default_state_); +} + +void TestNode::resetMaps() +{ + mask_.reset(); + master_grid_.reset(); +} + +void TestNode::reset() +{ + resetMaps(); + info_publisher_.reset(); + mask_publisher_.reset(); + binary_state_subscriber_.reset(); + binary_filter_.reset(); + node_.reset(); + tf_listener_.reset(); + tf_broadcaster_.reset(); + tf_buffer_.reset(); +} + +TEST_F(TestNode, testBinaryState) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("map", 10.0)); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testBinaryStateScaled) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 100.0, -1.0); + ASSERT_TRUE(createBinaryFilter("map", 35.0)); + + // Test BinaryFilter + testFullMask(100.0, -1.0, 35.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testInvertedBinaryState) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + setDefaultState(true); + ASSERT_TRUE(createBinaryFilter("map", 10.0)); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testOutOfBounds) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("map", 10.0)); + + // Test BinaryFilter + testOutOfMask(); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testInfoRePublish) +{ + // Initilize test system + createMaps("map"); + // Publish Info with incorrect dummy mask topic + publishMaps(nav2_costmap_2d::BINARY_FILTER, "dummy_topic", 0.0, 1.0); + ASSERT_FALSE(createBinaryFilter("map", 10.0)); + + // Re-publish filter info with correct mask topic + // and ensure that everything works fine + rePublishInfo(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testMaskRePublish) +{ + // Create mask in incorrect frame + createMaps("dummy"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + EXPECT_TRUE(createBinaryFilter("map", 10.0)); + + // Create mask in correct frame + resetMaps(); + createMaps("map"); + // Re-publish correct filter mask and ensure that everything works fine + rePublishMask(); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testIncorrectFilterType) +{ + // Initilize test system + createMaps("map"); + publishMaps(INCORRECT_TYPE, MASK_TOPIC, 0.0, 1.0); + ASSERT_FALSE(createBinaryFilter("map", 10.0)); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testDifferentFrame) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("odom", 10.0)); + createTFBroadcaster("map", "odom"); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, TRANSLATION_X, TRANSLATION_Y); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testIncorrectFrame) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("odom", 10.0)); + // map->odom TF does not exit + + // Test BinaryFilter with incorrect TF chain + testIncorrectTF(); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testResetState) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("map", 10.0)); + + testResetFilter(); + + // Clean-up + // do not need to resetFilter(): this was already done in testResetFilter() + 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; +} From 81811e293ca71c5b40d574190b4db02a103e70d5 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 10 Oct 2022 17:06:50 -0700 Subject: [PATCH 079/131] Update waffle.model --- nav2_bringup/worlds/waffle.model | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/worlds/waffle.model b/nav2_bringup/worlds/waffle.model index 2ab4e51ce6..dd611fa563 100644 --- a/nav2_bringup/worlds/waffle.model +++ b/nav2_bringup/worlds/waffle.model @@ -98,7 +98,7 @@ - -0.052 0 0.111 0 0 0 + -0.064 0 0.15 0 0 0 0.001 0.000 From b908b535d7eb8210f6f89ea49b1c2dfd30a58f7e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 10 Oct 2022 17:10:47 -0700 Subject: [PATCH 080/131] Revert "added result codes for global planner (#3146)" (#3237) This reverts commit 7be609e67c5b8f7e54b3bc2bcd53d41e652c494e. --- .../action/compute_path_to_pose_action.hpp | 2 - nav2_behavior_tree/nav2_tree_nodes.xml | 1 - .../action/compute_path_to_pose_action.cpp | 7 -- nav2_core/include/nav2_core/exceptions.hpp | 52 +---------- nav2_msgs/action/ComputePathToPose.action | 13 --- .../nav2_navfn_planner/navfn_planner.hpp | 1 - nav2_navfn_planner/src/navfn_planner.cpp | 61 ++++++------ .../include/nav2_planner/planner_server.hpp | 20 ++-- nav2_planner/src/planner_server.cpp | 93 +++++++------------ .../include/nav2_smac_planner/a_star.hpp | 1 - nav2_smac_planner/src/a_star.cpp | 4 +- nav2_smac_planner/src/smac_planner_2d.cpp | 45 +++++---- nav2_smac_planner/src/smac_planner_hybrid.cpp | 41 ++++---- .../src/smac_planner_lattice.cpp | 39 ++++---- .../theta_star_planner.hpp | 1 - .../src/theta_star_planner.cpp | 38 ++------ .../test/test_theta_star.cpp | 4 +- 17 files changed, 157 insertions(+), 266 deletions(-) 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 e980012cc4..fc95daaf43 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 @@ -75,8 +75,6 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), - BT::OutputPort( - "compute_path_to_pose_error_code", "The compute path to pose error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 636392929a..62a5f1f482 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,7 +77,6 @@ Service name Server timeout Path created by ComputePathToPose node - "The compute path to pose error code" 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 b5e0e9084c..07bd9240d2 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 @@ -40,9 +40,6 @@ void ComputePathToPoseAction::on_tick() BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); - // Set empty error code, action was successful - result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } @@ -50,7 +47,6 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -58,9 +54,6 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - // Set empty error code, action was cancelled - result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index 19a113d2e4..be23a91d42 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -46,57 +46,9 @@ namespace nav2_core class PlannerException : public std::runtime_error { public: - explicit PlannerException(const std::string & description) + explicit PlannerException(const std::string description) : std::runtime_error(description) {} -}; - -class StartOccupied : public PlannerException -{ -public: - explicit StartOccupied(const std::string & description) - : PlannerException(description) {} -}; - -class GoalOccupied : public PlannerException -{ -public: - explicit GoalOccupied(const std::string & description) - : PlannerException(description) {} -}; - -class StartOutsideMapBounds : public PlannerException -{ -public: - explicit StartOutsideMapBounds(const std::string & description) - : PlannerException(description) {} -}; - -class GoalOutsideMapBounds : public PlannerException -{ -public: - explicit GoalOutsideMapBounds(const std::string & description) - : PlannerException(description) {} -}; - -class NoValidPathCouldBeFound : public PlannerException -{ -public: - explicit NoValidPathCouldBeFound(const std::string & description) - : PlannerException(description) {} -}; - -class PlannerTimedOut : public PlannerException -{ -public: - explicit PlannerTimedOut(const std::string & description) - : PlannerException(description) {} -}; - -class PlannerTFError : public PlannerException -{ -public: - explicit PlannerTFError(const std::string & description) - : PlannerException(description) {} + using Ptr = std::shared_ptr; }; } // namespace nav2_core diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 7e727088a1..a052c552ab 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,15 +1,3 @@ -# Error codes -# Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 START_OUTSIDE_MAP=3 -int16 GOAL_OUTSIDE_MAP=4 -int16 START_OCCUPIED=5 -int16 GOAL_OCCUPIED=6 -int16 TIMEOUT=7 -int16 NO_VALID_PATH=8 - #goal definition geometry_msgs/PoseStamped goal geometry_msgs/PoseStamped start @@ -19,6 +7,5 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time -int16 error_code --- #feedback definition 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 454dd44858..8090d817ef 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -25,7 +25,6 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/global_planner.hpp" -#include "nav2_core/exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_navfn_planner/navfn.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 09d0e18466..2ede7f38b8 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -134,30 +134,6 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( #ifdef BENCHMARK_TESTING steady_clock::time_point a = steady_clock::now(); #endif - unsigned int mx_start, my_start, mx_goal, my_goal; - if (!costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) { - throw nav2_core::StartOutsideMapBounds( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was outside bounds"); - } - - if (!costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { - throw nav2_core::GoalOutsideMapBounds( - "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + - std::to_string(goal.pose.position.y) + ") was outside bounds"); - } - - if (costmap_->getCost(mx_start, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was in lethal cost"); - } - - if (tolerance_ == 0 && costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::GoalOccupied( - "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + - std::to_string(goal.pose.position.y) + ") was in lethal cost"); - } // Update planner based on the new costmap size if (isPlannerOutOfDate()) { @@ -172,6 +148,12 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( if (start.pose.position.x == goal.pose.position.x && start.pose.position.y == goal.pose.position.y) { + unsigned int mx, my; + costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); + return path; + } path.header.stamp = clock_->now(); path.header.frame_id = global_frame_; geometry_msgs::msg::PoseStamped pose; @@ -190,8 +172,9 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( } if (!makePlan(start.pose, goal.pose, tolerance_, path)) { - throw nav2_core::NoValidPathCouldBeFound( - "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); + RCLCPP_WARN( + logger_, "%s: failed to create plan with " + "tolerance %.2f.", name_.c_str(), tolerance_); } @@ -238,7 +221,14 @@ NavfnPlanner::makePlan( start.position.x, start.position.y, goal.position.x, goal.position.y); unsigned int mx, my; - worldToMap(wx, wy, mx, my); + if (!worldToMap(wx, wy, mx, my)) { + RCLCPP_WARN( + 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?"); + return false; + } // clear the starting cell within the costmap because we know it can't be an obstacle clearRobotCell(mx, my); @@ -261,7 +251,14 @@ NavfnPlanner::makePlan( wx = goal.position.x; wy = goal.position.y; - worldToMap(wx, wy, mx, my); + if (!worldToMap(wx, wy, mx, my)) { + RCLCPP_WARN( + logger_, + "The goal sent to the planner is off the global costmap." + " Planning will always fail to this goal."); + return false; + } + int map_goal[2]; map_goal[0] = mx; map_goal[1] = my; @@ -388,7 +385,13 @@ NavfnPlanner::getPlanFromPotential( // the potential has already been computed, so we won't update our copy of the costmap unsigned int mx, my; - worldToMap(wx, wy, mx, my); + if (!worldToMap(wx, wy, mx, my)) { + RCLCPP_WARN( + logger_, + "The goal sent to the navfn planner is off the global costmap." + " Planning will always fail to this goal."); + return false; + } int map_goal[2]; map_goal[0] = mx; diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 3cca065e36..a1f1738c29 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -39,7 +39,6 @@ #include "pluginlib/class_list_macros.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" -#include "nav2_core/exceptions.hpp" namespace nav2_planner { @@ -153,17 +152,21 @@ class PlannerServer : public nav2_util::LifecycleNode */ template bool getStartPose( + std::unique_ptr> & action_server, typename std::shared_ptr goal, geometry_msgs::msg::PoseStamped & start); /** * @brief Transform start and goal poses into the costmap * global frame for path planning plugins to utilize + * @param action_server Action server to terminate if required * @param start The starting pose to transform * @param goal Goal pose to transform * @return bool If successful in transforming poses */ + template bool transformPosesToGlobalFrame( + std::unique_ptr> & action_server, geometry_msgs::msg::PoseStamped & curr_start, geometry_msgs::msg::PoseStamped & curr_goal); @@ -177,10 +180,15 @@ class PlannerServer : public nav2_util::LifecycleNode */ template bool validatePath( + std::unique_ptr> & action_server, const geometry_msgs::msg::PoseStamped & curr_goal, const nav_msgs::msg::Path & path, const std::string & planner_id); + // Our action server implements the ComputePathToPose action + std::unique_ptr action_server_pose_; + std::unique_ptr action_server_poses_; + /** * @brief The action server callback which calls planner to get the path * ComputePathToPose @@ -208,12 +216,6 @@ class PlannerServer : public nav2_util::LifecycleNode */ void publishPlan(const nav_msgs::msg::Path & path); - void exceptionWarning( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id, - const std::exception & ex); - /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -221,10 +223,6 @@ class PlannerServer : public nav2_util::LifecycleNode rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); - // Our action server implements the ComputePathToPose action - std::unique_ptr action_server_pose_; - std::unique_ptr action_server_poses_; - // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index c87bd997fe..e2bc6286a7 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -289,25 +289,32 @@ void PlannerServer::getPreemptedGoalIfRequested( template bool PlannerServer::getStartPose( + std::unique_ptr> & action_server, typename std::shared_ptr goal, geometry_msgs::msg::PoseStamped & start) { if (goal->use_start) { start = goal->start; } else if (!costmap_ros_->getRobotPose(start)) { + action_server->terminate_current(); return false; } return true; } +template bool PlannerServer::transformPosesToGlobalFrame( + std::unique_ptr> & action_server, geometry_msgs::msg::PoseStamped & curr_start, geometry_msgs::msg::PoseStamped & curr_goal) { if (!costmap_ros_->transformPoseToGlobalFrame(curr_start, curr_start) || !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal)) { + RCLCPP_WARN( + get_logger(), "Could not transform the start or goal pose in the costmap frame"); + action_server->terminate_current(); return false; } @@ -316,15 +323,17 @@ bool PlannerServer::transformPosesToGlobalFrame( template bool PlannerServer::validatePath( + std::unique_ptr> & action_server, const geometry_msgs::msg::PoseStamped & goal, const nav_msgs::msg::Path & path, const std::string & planner_id) { - if (path.poses.empty()) { + if (path.poses.size() == 0) { RCLCPP_WARN( get_logger(), "Planning algorithm %s failed to generate a valid" " path to (%.2f, %.2f)", planner_id.c_str(), goal.pose.position.x, goal.pose.position.y); + action_server->terminate_current(); return false; } @@ -337,7 +346,8 @@ bool PlannerServer::validatePath( return true; } -void PlannerServer::computePlanThroughPoses() +void +PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); @@ -366,8 +376,8 @@ void PlannerServer::computePlanThroughPoses() // Use start pose if provided otherwise use current robot pose geometry_msgs::msg::PoseStamped start; - if (!getStartPose(goal, start)) { - throw nav2_core::PlannerTFError("Unable to get start pose"); + if (!getStartPose(action_server_poses_, goal, start)) { + return; } // Get consecutive paths through these points @@ -383,15 +393,16 @@ void PlannerServer::computePlanThroughPoses() curr_goal = goal->goals[i]; // Transform them into the global frame - if (!transformPosesToGlobalFrame(curr_start, curr_goal)) { - throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); + if (!transformPosesToGlobalFrame(action_server_poses_, curr_start, curr_goal)) { + return; } // Get plan from start -> goal nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); - if (!validatePath(curr_goal, curr_path, goal->planner_id)) { - throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + // check path for validity + if (!validatePath(action_server_poses_, curr_goal, curr_path, goal->planner_id)) { + return; } // Concatenate paths together @@ -436,8 +447,6 @@ PlannerServer::computePlan() auto goal = action_server_pose_->get_current_goal(); auto result = std::make_shared(); - geometry_msgs::msg::PoseStamped start; - try { if (isServerInactive(action_server_pose_) || isCancelRequested(action_server_pose_)) { return; @@ -448,20 +457,21 @@ PlannerServer::computePlan() getPreemptedGoalIfRequested(action_server_pose_, goal); // Use start pose if provided otherwise use current robot pose - if (!getStartPose(goal, start)) { - throw nav2_core::PlannerTFError("Unable to get start pose"); + geometry_msgs::msg::PoseStamped start; + if (!getStartPose(action_server_pose_, goal, start)) { + return; } // Transform them into the global frame geometry_msgs::msg::PoseStamped goal_pose = goal->goal; - if (!transformPosesToGlobalFrame(start, goal_pose)) { - throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); + if (!transformPosesToGlobalFrame(action_server_pose_, start, goal_pose)) { + return; } result->path = getPlan(start, goal_pose, goal->planner_id); - if (!validatePath(goal_pose, result->path, goal->planner_id)) { - throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) { + return; } // Publish the plan for visualization purposes @@ -478,38 +488,12 @@ PlannerServer::computePlan() } action_server_pose_->succeeded_current(result); - } catch (nav2_core::StartOccupied & ex) { - exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED; - action_server_pose_->terminate_current(result); - } catch (nav2_core::GoalOccupied & ex) { - exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; - action_server_pose_->terminate_current(result); - } catch (nav2_core::NoValidPathCouldBeFound & ex) { - exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; - action_server_pose_->terminate_current(result); - } catch (nav2_core::PlannerTimedOut & ex) { - exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; - action_server_pose_->terminate_current(result); - } catch (nav2_core::StartOutsideMapBounds & ex) { - exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; - action_server_pose_->terminate_current(result); - } catch (nav2_core::GoalOutsideMapBounds & ex) { - exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; - action_server_pose_->terminate_current(result); - } catch (nav2_core::PlannerTFError & ex) { - exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR; - action_server_pose_->terminate_current(result); } catch (std::exception & ex) { - exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; - action_server_pose_->terminate_current(result); + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", + goal->planner_id.c_str(), goal->goal.pose.position.x, + goal->goal.pose.position.y, ex.what()); + action_server_pose_->terminate_current(); } } @@ -653,6 +637,7 @@ PlannerServer::dynamicParametersCallback(std::vector paramete { std::lock_guard lock(dynamic_params_lock_); rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { const auto & type = parameter.get_type(); const auto & name = parameter.get_name(); @@ -676,20 +661,6 @@ PlannerServer::dynamicParametersCallback(std::vector paramete return result; } -void PlannerServer::exceptionWarning( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id, - const std::exception & ex) -{ - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan from (%.2f, %.2f) to (%0.2f, %.2f): \"%s\"", - planner_id.c_str(), - start.pose.position.x, start.pose.position.y, - goal.pose.position.x, goal.pose.position.y, - ex.what()); -} - } // namespace nav2_planner #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 7b2ee31c0e..4b4fe55111 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -25,7 +25,6 @@ #include "Eigen/Core" #include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_core/exceptions.hpp" #include "nav2_smac_planner/thirdparty/robin_hood.h" #include "nav2_smac_planner/analytic_expansion.hpp" diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 841b43e357..06bc0520d4 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -207,12 +207,12 @@ bool AStarAlgorithm::areInputsValid() if (getToleranceHeuristic() < 0.001 && !_goal->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::GoalOccupied("Goal was in lethal cost"); + throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance."); } // Check if starting point is valid if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::StartOccupied("Start was in lethal cost"); + throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan."); } return true; diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 46dfa98fb2..606f6861ef 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -211,19 +211,11 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Set starting point unsigned int mx_start, my_start, mx_goal, my_goal; - if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) { - throw nav2_core::StartOutsideMapBounds( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was outside bounds"); - } + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); _a_star->setStart(mx_start, my_start, 0); // Set goal point - if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { - throw nav2_core::GoalOutsideMapBounds( - "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + - std::to_string(goal.pose.position.y) + ") was outside bounds"); - } + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); _a_star->setGoal(mx_goal, my_goal, 0); // Setup message @@ -241,7 +233,8 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Corner case of start and goal beeing on the same cell if (mx_start == mx_goal && my_start == my_goal) { if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied("Start was in lethal cost"); + RCLCPP_WARN(_logger, "Failed to create a unique pose path because of obstacles"); + return plan; } pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal @@ -257,16 +250,28 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Compute plan Node2D::CoordinateVector path; int num_iterations = 0; - // Note: All exceptions thrown are handled by the planner server and returned to the action - if (!_a_star->createPath( - path, num_iterations, - _tolerance / static_cast(costmap->getResolution()))) - { - if (num_iterations < _a_star->getMaxIterations()) { - throw nav2_core::NoValidPathCouldBeFound("no valid path found"); - } else { - throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); + std::string error; + try { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _logger, + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; } // Convert to world coordinates diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 2d0351c5b9..ee614c7b94 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -287,12 +287,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; - if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { - throw nav2_core::StartOutsideMapBounds( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was outside bounds"); - } - + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); @@ -305,11 +300,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setStart(mx, my, orientation_bin_id); // Set goal point, in A* bin search coordinates - if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { - throw nav2_core::GoalOutsideMapBounds( - "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + - std::to_string(goal.pose.position.y) + ") was outside bounds"); - } + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); @@ -336,14 +327,28 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Compute plan NodeHybrid::CoordinateVector path; int num_iterations = 0; - - // Note: All exceptions thrown are handled by the planner server and returned to the action - if (!_a_star->createPath(path, num_iterations, 0)) { - if (num_iterations < _a_star->getMaxIterations()) { - throw nav2_core::NoValidPathCouldBeFound("no valid path found"); - } else { - throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); + std::string error; + try { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _logger, + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; } // Convert to world coordinates diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 6d00a3479f..b7e22ee1f9 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -247,21 +247,13 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; - if (!_costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { - throw nav2_core::StartOutsideMapBounds( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was outside bounds"); - } + _costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); _a_star->setStart( mx, my, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); // Set goal point, in A* bin search coordinates - if (!_costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { - throw nav2_core::GoalOutsideMapBounds( - "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + - std::to_string(goal.pose.position.y) + ") was outside bounds"); - } + _costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); _a_star->setGoal( mx, my, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); @@ -282,14 +274,27 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::CoordinateVector path; int num_iterations = 0; std::string error; - - // Note: All exceptions thrown are handled by the planner server and returned to the action - if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { - if (num_iterations < _a_star->getMaxIterations()) { - throw nav2_core::NoValidPathCouldBeFound("no valid path found"); - } else { - throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); + try { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _logger, + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; } // Convert to world coordinates diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index 7b124f9764..211962090d 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -25,7 +25,6 @@ #include #include "rclcpp/rclcpp.hpp" #include "nav2_core/global_planner.hpp" -#include "nav2_core/exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 9f4930da72..cc98649ccc 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -89,35 +89,13 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( // Corner case of start and goal beeing on the same cell unsigned int mx_start, my_start, mx_goal, my_goal; - if (!planner_->costmap_->worldToMap( - start.pose.position.x, start.pose.position.y, mx_start, my_start)) - { - throw nav2_core::StartOutsideMapBounds( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was outside bounds"); - } - - if (!planner_->costmap_->worldToMap( - goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) - { - throw nav2_core::GoalOutsideMapBounds( - "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + - std::to_string(goal.pose.position.y) + ") was outside bounds"); - } - - if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was in lethal cost"); - } - - if (planner_->costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::GoalOccupied( - "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + - std::to_string(goal.pose.position.y) + ") was in lethal cost"); - } - + planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); + planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); if (mx_start == mx_goal && my_start == my_goal) { + if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); + return global_path; + } global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; geometry_msgs::msg::PoseStamped pose; @@ -176,13 +154,13 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) { std::vector path; if (planner_->isUnsafeToPlan()) { + RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); global_path.poses.clear(); - throw nav2_core::PlannerException("Either of the start or goal pose are an obstacle! "); } else if (planner_->generatePath(path)) { global_path = linearInterpolation(path, planner_->costmap_->getResolution()); } else { + RCLCPP_ERROR(logger_, "Could not generate path between the given poses"); global_path.poses.clear(); - throw nav2_core::NoValidPathCouldBeFound("Could not generate path between the given poses"); } global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 264f737de4..7776179963 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -174,8 +174,8 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { } goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; - - EXPECT_THROW(planner_2d->createPlan(start, goal), nav2_core::GoalOccupied); + path = planner_2d->createPlan(start, goal); + EXPECT_EQ(static_cast(path.poses.size()), 0); planner_2d->deactivate(); planner_2d->cleanup(); From b1922b60ef1b4b1bde7e5d27542f2df6b7cf4b10 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 10 Oct 2022 17:13:00 -0700 Subject: [PATCH 081/131] Update waffle.model --- nav2_bringup/worlds/waffle.model | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bringup/worlds/waffle.model b/nav2_bringup/worlds/waffle.model index dd611fa563..3c5923d863 100644 --- a/nav2_bringup/worlds/waffle.model +++ b/nav2_bringup/worlds/waffle.model @@ -98,7 +98,7 @@ - -0.064 0 0.15 0 0 0 + -0.064 0 0.121 0 0 0 0.001 0.000 @@ -133,7 +133,7 @@ true true - -0.064 0 0.121 0 0 0 + -0.064 0 0.15 0 0 0 5 From a96301cfdfa564a1f94ab899ff5c255a1150ee9a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 10 Oct 2022 18:22:56 -0700 Subject: [PATCH 082/131] Update test_actions.cpp --- nav2_util/test/test_actions.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 35043d5df2..26c64b1c7d 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -75,12 +75,12 @@ class FibonacciServerNode : public rclcpp::Node void on_term() { // when nothing's running make sure everything's dead. - const std::shared_ptr a = action_server_->accept_pending_goal(); - const std::shared_ptr b = action_server_->get_current_goal(); - assert(a == b); - assert(action_server_->is_cancel_requested() == false); - auto feedback = std::make_shared(); - action_server_->publish_feedback(feedback); + // const std::shared_ptr a = action_server_->accept_pending_goal(); + // const std::shared_ptr b = action_server_->get_current_goal(); + // assert(a == b); + // assert(action_server_->is_cancel_requested() == false); + // auto feedback = std::make_shared(); + // action_server_->publish_feedback(feedback); action_server_.reset(); } From 5fe3345d19bef649f5a2a60626cf72d5211dd3d0 Mon Sep 17 00:00:00 2001 From: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com> Date: Wed, 12 Oct 2022 02:13:41 +0800 Subject: [PATCH 083/131] odom alpha restriction to avoid overflow caused by user-misconfiguration (#3238) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style --- nav2_amcl/src/amcl_node.cpp | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index a28b4927a3..2e9730ae56 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1158,18 +1158,53 @@ AmclNode::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { alpha1_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha1_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha1 to be negative," + " this isn't allowed, so the alpha1 will be set to be zero."); + alpha1_ = 0.0; + } reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha2_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha2 to be negative," + " this isn't allowed, so the alpha2 will be set to be zero."); + alpha2_ = 0.0; + } reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha3_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha3 to be negative," + " this isn't allowed, so the alpha3 will be set to be zero."); + alpha3_ = 0.0; + } reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha4_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha4 to be negative," + " this isn't allowed, so the alpha4 will be set to be zero."); + alpha4_ = 0.0; + } reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha5_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha5 to be negative," + " this isn't allowed, so the alpha5 will be set to be zero."); + alpha5_ = 0.0; + } reinit_odom = true; } else if (param_name == "beam_skip_distance") { beam_skip_distance_ = parameter.as_double(); From 95fbd238f9ab6d7abe8a45de1524504854e53fb7 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 12 Oct 2022 16:07:15 -0400 Subject: [PATCH 084/131] Expections (#3244) * added result codes for global planner * code review * code review * cleanup * cleanup * update smac lattice planner * update planner instances * cleanup * updates * renaming * fixes * cpplint * uncrusitfy * code review * navfn exceptions * theta_star_planner * fix code review * wrote timeout exception * consistent exception throwing across planners * code review * remove * uncrusitfy * uncrusify * catch exception * expect throw * update string of exceptions * throw with coords * removed start == goal error code * code review * code review * uncrustify * code review * message order * remove remarks * update xml * update xml * Update nav2_behavior_tree/nav2_tree_nodes.xml * fix Co-authored-by: Steve Macenski Co-authored-by: Joshua Wallace --- .../action/compute_path_to_pose_action.hpp | 2 + nav2_behavior_tree/nav2_tree_nodes.xml | 1 + .../action/compute_path_to_pose_action.cpp | 7 ++ nav2_core/include/nav2_core/exceptions.hpp | 52 ++++++++++- nav2_msgs/action/ComputePathToPose.action | 13 +++ .../nav2_navfn_planner/navfn_planner.hpp | 1 + nav2_navfn_planner/src/navfn_planner.cpp | 61 ++++++------ .../include/nav2_planner/planner_server.hpp | 20 ++-- nav2_planner/src/planner_server.cpp | 93 ++++++++++++------- .../include/nav2_smac_planner/a_star.hpp | 1 + nav2_smac_planner/src/a_star.cpp | 4 +- nav2_smac_planner/src/smac_planner_2d.cpp | 45 ++++----- nav2_smac_planner/src/smac_planner_hybrid.cpp | 41 ++++---- .../src/smac_planner_lattice.cpp | 39 ++++---- .../theta_star_planner.hpp | 1 + .../src/theta_star_planner.cpp | 38 ++++++-- .../test/test_theta_star.cpp | 4 +- 17 files changed, 266 insertions(+), 157 deletions(-) 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 fc95daaf43..e980012cc4 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 @@ -75,6 +75,8 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), + BT::OutputPort( + "compute_path_to_pose_error_code", "The compute path to pose error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 62a5f1f482..636392929a 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,6 +77,7 @@ Service name Server timeout Path created by ComputePathToPose node + "The compute path to pose error code" 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 07bd9240d2..b5e0e9084c 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 @@ -40,6 +40,9 @@ void ComputePathToPoseAction::on_tick() BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); + // Set empty error code, action was successful + result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; + setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } @@ -47,6 +50,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -54,6 +58,9 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + // Set empty error code, action was cancelled + result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; + setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index be23a91d42..19a113d2e4 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -46,9 +46,57 @@ namespace nav2_core class PlannerException : public std::runtime_error { public: - explicit PlannerException(const std::string description) + explicit PlannerException(const std::string & description) : std::runtime_error(description) {} - using Ptr = std::shared_ptr; +}; + +class StartOccupied : public PlannerException +{ +public: + explicit StartOccupied(const std::string & description) + : PlannerException(description) {} +}; + +class GoalOccupied : public PlannerException +{ +public: + explicit GoalOccupied(const std::string & description) + : PlannerException(description) {} +}; + +class StartOutsideMapBounds : public PlannerException +{ +public: + explicit StartOutsideMapBounds(const std::string & description) + : PlannerException(description) {} +}; + +class GoalOutsideMapBounds : public PlannerException +{ +public: + explicit GoalOutsideMapBounds(const std::string & description) + : PlannerException(description) {} +}; + +class NoValidPathCouldBeFound : public PlannerException +{ +public: + explicit NoValidPathCouldBeFound(const std::string & description) + : PlannerException(description) {} +}; + +class PlannerTimedOut : public PlannerException +{ +public: + explicit PlannerTimedOut(const std::string & description) + : PlannerException(description) {} +}; + +class PlannerTFError : public PlannerException +{ +public: + explicit PlannerTFError(const std::string & description) + : PlannerException(description) {} }; } // namespace nav2_core diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index a052c552ab..7e727088a1 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,3 +1,15 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +int16 NONE=0 +int16 UNKNOWN=1 +int16 TF_ERROR=2 +int16 START_OUTSIDE_MAP=3 +int16 GOAL_OUTSIDE_MAP=4 +int16 START_OCCUPIED=5 +int16 GOAL_OCCUPIED=6 +int16 TIMEOUT=7 +int16 NO_VALID_PATH=8 + #goal definition geometry_msgs/PoseStamped goal geometry_msgs/PoseStamped start @@ -7,5 +19,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time +int16 error_code --- #feedback definition 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 8090d817ef..454dd44858 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -25,6 +25,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/global_planner.hpp" +#include "nav2_core/exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_navfn_planner/navfn.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2ede7f38b8..53d7276036 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -134,6 +134,30 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( #ifdef BENCHMARK_TESTING steady_clock::time_point a = steady_clock::now(); #endif + unsigned int mx_start, my_start, mx_goal, my_goal; + if (!costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } + + if (!costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } + + if (costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::StartOccupied( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was in lethal cost"); + } + + if (tolerance_ == 0 && costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::GoalOccupied( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was in lethal cost"); + } // Update planner based on the new costmap size if (isPlannerOutOfDate()) { @@ -148,12 +172,6 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( if (start.pose.position.x == goal.pose.position.x && start.pose.position.y == goal.pose.position.y) { - unsigned int mx, my; - costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); - if (costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) { - RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); - return path; - } path.header.stamp = clock_->now(); path.header.frame_id = global_frame_; geometry_msgs::msg::PoseStamped pose; @@ -172,9 +190,8 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( } if (!makePlan(start.pose, goal.pose, tolerance_, path)) { - RCLCPP_WARN( - logger_, "%s: failed to create plan with " - "tolerance %.2f.", name_.c_str(), tolerance_); + throw nav2_core::NoValidPathCouldBeFound( + "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); } @@ -221,14 +238,7 @@ NavfnPlanner::makePlan( start.position.x, start.position.y, goal.position.x, goal.position.y); unsigned int mx, my; - if (!worldToMap(wx, wy, mx, my)) { - RCLCPP_WARN( - 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?"); - return false; - } + worldToMap(wx, wy, mx, my); // clear the starting cell within the costmap because we know it can't be an obstacle clearRobotCell(mx, my); @@ -251,14 +261,7 @@ NavfnPlanner::makePlan( wx = goal.position.x; wy = goal.position.y; - if (!worldToMap(wx, wy, mx, my)) { - RCLCPP_WARN( - logger_, - "The goal sent to the planner is off the global costmap." - " Planning will always fail to this goal."); - return false; - } - + worldToMap(wx, wy, mx, my); int map_goal[2]; map_goal[0] = mx; map_goal[1] = my; @@ -385,13 +388,7 @@ NavfnPlanner::getPlanFromPotential( // the potential has already been computed, so we won't update our copy of the costmap unsigned int mx, my; - if (!worldToMap(wx, wy, mx, my)) { - RCLCPP_WARN( - logger_, - "The goal sent to the navfn planner is off the global costmap." - " Planning will always fail to this goal."); - return false; - } + worldToMap(wx, wy, mx, my); int map_goal[2]; map_goal[0] = mx; diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index a1f1738c29..3cca065e36 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -39,6 +39,7 @@ #include "pluginlib/class_list_macros.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" +#include "nav2_core/exceptions.hpp" namespace nav2_planner { @@ -152,21 +153,17 @@ class PlannerServer : public nav2_util::LifecycleNode */ template bool getStartPose( - std::unique_ptr> & action_server, typename std::shared_ptr goal, geometry_msgs::msg::PoseStamped & start); /** * @brief Transform start and goal poses into the costmap * global frame for path planning plugins to utilize - * @param action_server Action server to terminate if required * @param start The starting pose to transform * @param goal Goal pose to transform * @return bool If successful in transforming poses */ - template bool transformPosesToGlobalFrame( - std::unique_ptr> & action_server, geometry_msgs::msg::PoseStamped & curr_start, geometry_msgs::msg::PoseStamped & curr_goal); @@ -180,15 +177,10 @@ class PlannerServer : public nav2_util::LifecycleNode */ template bool validatePath( - std::unique_ptr> & action_server, const geometry_msgs::msg::PoseStamped & curr_goal, const nav_msgs::msg::Path & path, const std::string & planner_id); - // Our action server implements the ComputePathToPose action - std::unique_ptr action_server_pose_; - std::unique_ptr action_server_poses_; - /** * @brief The action server callback which calls planner to get the path * ComputePathToPose @@ -216,6 +208,12 @@ class PlannerServer : public nav2_util::LifecycleNode */ void publishPlan(const nav_msgs::msg::Path & path); + void exceptionWarning( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id, + const std::exception & ex); + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -223,6 +221,10 @@ class PlannerServer : public nav2_util::LifecycleNode rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + // Our action server implements the ComputePathToPose action + std::unique_ptr action_server_pose_; + std::unique_ptr action_server_poses_; + // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e2bc6286a7..c87bd997fe 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -289,32 +289,25 @@ void PlannerServer::getPreemptedGoalIfRequested( template bool PlannerServer::getStartPose( - std::unique_ptr> & action_server, typename std::shared_ptr goal, geometry_msgs::msg::PoseStamped & start) { if (goal->use_start) { start = goal->start; } else if (!costmap_ros_->getRobotPose(start)) { - action_server->terminate_current(); return false; } return true; } -template bool PlannerServer::transformPosesToGlobalFrame( - std::unique_ptr> & action_server, geometry_msgs::msg::PoseStamped & curr_start, geometry_msgs::msg::PoseStamped & curr_goal) { if (!costmap_ros_->transformPoseToGlobalFrame(curr_start, curr_start) || !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal)) { - RCLCPP_WARN( - get_logger(), "Could not transform the start or goal pose in the costmap frame"); - action_server->terminate_current(); return false; } @@ -323,17 +316,15 @@ bool PlannerServer::transformPosesToGlobalFrame( template bool PlannerServer::validatePath( - std::unique_ptr> & action_server, const geometry_msgs::msg::PoseStamped & goal, const nav_msgs::msg::Path & path, const std::string & planner_id) { - if (path.poses.size() == 0) { + if (path.poses.empty()) { RCLCPP_WARN( get_logger(), "Planning algorithm %s failed to generate a valid" " path to (%.2f, %.2f)", planner_id.c_str(), goal.pose.position.x, goal.pose.position.y); - action_server->terminate_current(); return false; } @@ -346,8 +337,7 @@ bool PlannerServer::validatePath( return true; } -void -PlannerServer::computePlanThroughPoses() +void PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); @@ -376,8 +366,8 @@ PlannerServer::computePlanThroughPoses() // Use start pose if provided otherwise use current robot pose geometry_msgs::msg::PoseStamped start; - if (!getStartPose(action_server_poses_, goal, start)) { - return; + if (!getStartPose(goal, start)) { + throw nav2_core::PlannerTFError("Unable to get start pose"); } // Get consecutive paths through these points @@ -393,16 +383,15 @@ PlannerServer::computePlanThroughPoses() curr_goal = goal->goals[i]; // Transform them into the global frame - if (!transformPosesToGlobalFrame(action_server_poses_, curr_start, curr_goal)) { - return; + if (!transformPosesToGlobalFrame(curr_start, curr_goal)) { + throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); } // Get plan from start -> goal nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); - // check path for validity - if (!validatePath(action_server_poses_, curr_goal, curr_path, goal->planner_id)) { - return; + if (!validatePath(curr_goal, curr_path, goal->planner_id)) { + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); } // Concatenate paths together @@ -447,6 +436,8 @@ PlannerServer::computePlan() auto goal = action_server_pose_->get_current_goal(); auto result = std::make_shared(); + geometry_msgs::msg::PoseStamped start; + try { if (isServerInactive(action_server_pose_) || isCancelRequested(action_server_pose_)) { return; @@ -457,21 +448,20 @@ PlannerServer::computePlan() getPreemptedGoalIfRequested(action_server_pose_, goal); // Use start pose if provided otherwise use current robot pose - geometry_msgs::msg::PoseStamped start; - if (!getStartPose(action_server_pose_, goal, start)) { - return; + if (!getStartPose(goal, start)) { + throw nav2_core::PlannerTFError("Unable to get start pose"); } // Transform them into the global frame geometry_msgs::msg::PoseStamped goal_pose = goal->goal; - if (!transformPosesToGlobalFrame(action_server_pose_, start, goal_pose)) { - return; + if (!transformPosesToGlobalFrame(start, goal_pose)) { + throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); } result->path = getPlan(start, goal_pose, goal->planner_id); - if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) { - return; + if (!validatePath(goal_pose, result->path, goal->planner_id)) { + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); } // Publish the plan for visualization purposes @@ -488,12 +478,38 @@ PlannerServer::computePlan() } action_server_pose_->succeeded_current(result); + } catch (nav2_core::StartOccupied & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED; + action_server_pose_->terminate_current(result); + } catch (nav2_core::GoalOccupied & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; + action_server_pose_->terminate_current(result); + } catch (nav2_core::NoValidPathCouldBeFound & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; + action_server_pose_->terminate_current(result); + } catch (nav2_core::PlannerTimedOut & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; + action_server_pose_->terminate_current(result); + } catch (nav2_core::StartOutsideMapBounds & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; + action_server_pose_->terminate_current(result); + } catch (nav2_core::GoalOutsideMapBounds & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; + action_server_pose_->terminate_current(result); + } catch (nav2_core::PlannerTFError & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR; + action_server_pose_->terminate_current(result); } catch (std::exception & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), goal->goal.pose.position.x, - goal->goal.pose.position.y, ex.what()); - action_server_pose_->terminate_current(); + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; + action_server_pose_->terminate_current(result); } } @@ -637,7 +653,6 @@ PlannerServer::dynamicParametersCallback(std::vector paramete { std::lock_guard lock(dynamic_params_lock_); rcl_interfaces::msg::SetParametersResult result; - for (auto parameter : parameters) { const auto & type = parameter.get_type(); const auto & name = parameter.get_name(); @@ -661,6 +676,20 @@ PlannerServer::dynamicParametersCallback(std::vector paramete return result; } +void PlannerServer::exceptionWarning( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id, + const std::exception & ex) +{ + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan from (%.2f, %.2f) to (%0.2f, %.2f): \"%s\"", + planner_id.c_str(), + start.pose.position.x, start.pose.position.y, + goal.pose.position.x, goal.pose.position.y, + ex.what()); +} + } // namespace nav2_planner #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 4b4fe55111..7b2ee31c0e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -25,6 +25,7 @@ #include "Eigen/Core" #include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_core/exceptions.hpp" #include "nav2_smac_planner/thirdparty/robin_hood.h" #include "nav2_smac_planner/analytic_expansion.hpp" diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 06bc0520d4..841b43e357 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -207,12 +207,12 @@ bool AStarAlgorithm::areInputsValid() if (getToleranceHeuristic() < 0.001 && !_goal->isNodeValid(_traverse_unknown, _collision_checker)) { - throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance."); + throw nav2_core::GoalOccupied("Goal was in lethal cost"); } // Check if starting point is valid if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan."); + throw nav2_core::StartOccupied("Start was in lethal cost"); } return true; diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 606f6861ef..46dfa98fb2 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -211,11 +211,19 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Set starting point unsigned int mx_start, my_start, mx_goal, my_goal; - costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); + if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } _a_star->setStart(mx_start, my_start, 0); // Set goal point - costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } _a_star->setGoal(mx_goal, my_goal, 0); // Setup message @@ -233,8 +241,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Corner case of start and goal beeing on the same cell if (mx_start == mx_goal && my_start == my_goal) { if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - RCLCPP_WARN(_logger, "Failed to create a unique pose path because of obstacles"); - return plan; + throw nav2_core::StartOccupied("Start was in lethal cost"); } pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal @@ -250,28 +257,16 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Compute plan Node2D::CoordinateVector path; int num_iterations = 0; - std::string error; - try { - if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) - { - if (num_iterations < _a_star->getMaxIterations()) { - error = std::string("no valid path found"); - } else { - error = std::string("exceeded maximum iterations"); - } + // Note: All exceptions thrown are handled by the planner server and returned to the action + if (!_a_star->createPath( + path, num_iterations, + _tolerance / static_cast(costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + throw nav2_core::NoValidPathCouldBeFound("no valid path found"); + } else { + throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); } - } catch (const std::runtime_error & e) { - error = "invalid use: "; - error += e.what(); - } - - if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); - return plan; } // Convert to world coordinates diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index ee614c7b94..2d0351c5b9 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -287,7 +287,12 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; - costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } + double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); @@ -300,7 +305,11 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setStart(mx, my, orientation_bin_id); // Set goal point, in A* bin search coordinates - costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); @@ -327,28 +336,14 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Compute plan NodeHybrid::CoordinateVector path; int num_iterations = 0; - std::string error; - try { - if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) - { - if (num_iterations < _a_star->getMaxIterations()) { - error = std::string("no valid path found"); - } else { - error = std::string("exceeded maximum iterations"); - } - } - } catch (const std::runtime_error & e) { - error = "invalid use: "; - error += e.what(); - } - if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); - return plan; + // Note: All exceptions thrown are handled by the planner server and returned to the action + if (!_a_star->createPath(path, num_iterations, 0)) { + if (num_iterations < _a_star->getMaxIterations()) { + throw nav2_core::NoValidPathCouldBeFound("no valid path found"); + } else { + throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); + } } // Convert to world coordinates diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index b7e22ee1f9..6d00a3479f 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -247,13 +247,21 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; - _costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (!_costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } _a_star->setStart( mx, my, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); // Set goal point, in A* bin search coordinates - _costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + if (!_costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } _a_star->setGoal( mx, my, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); @@ -274,27 +282,14 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::CoordinateVector path; int num_iterations = 0; std::string error; - try { - if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) - { - if (num_iterations < _a_star->getMaxIterations()) { - error = std::string("no valid path found"); - } else { - error = std::string("exceeded maximum iterations"); - } - } - } catch (const std::runtime_error & e) { - error = "invalid use: "; - error += e.what(); - } - if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); - return plan; + // Note: All exceptions thrown are handled by the planner server and returned to the action + if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { + if (num_iterations < _a_star->getMaxIterations()) { + throw nav2_core::NoValidPathCouldBeFound("no valid path found"); + } else { + throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); + } } // Convert to world coordinates diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index 211962090d..7b124f9764 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -25,6 +25,7 @@ #include #include "rclcpp/rclcpp.hpp" #include "nav2_core/global_planner.hpp" +#include "nav2_core/exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index cc98649ccc..9f4930da72 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -89,13 +89,35 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( // Corner case of start and goal beeing on the same cell unsigned int mx_start, my_start, mx_goal, my_goal; - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); - planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + if (!planner_->costmap_->worldToMap( + start.pose.position.x, start.pose.position.y, mx_start, my_start)) + { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } + + if (!planner_->costmap_->worldToMap( + goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) + { + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } + + if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::StartOccupied( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was in lethal cost"); + } + + if (planner_->costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::GoalOccupied( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was in lethal cost"); + } + if (mx_start == mx_goal && my_start == my_goal) { - if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); - return global_path; - } global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; geometry_msgs::msg::PoseStamped pose; @@ -154,13 +176,13 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) { std::vector path; if (planner_->isUnsafeToPlan()) { - RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); global_path.poses.clear(); + throw nav2_core::PlannerException("Either of the start or goal pose are an obstacle! "); } else if (planner_->generatePath(path)) { global_path = linearInterpolation(path, planner_->costmap_->getResolution()); } else { - RCLCPP_ERROR(logger_, "Could not generate path between the given poses"); global_path.poses.clear(); + throw nav2_core::NoValidPathCouldBeFound("Could not generate path between the given poses"); } global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 7776179963..264f737de4 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -174,8 +174,8 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { } goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; - path = planner_2d->createPlan(start, goal); - EXPECT_EQ(static_cast(path.poses.size()), 0); + + EXPECT_THROW(planner_2d->createPlan(start, goal), nav2_core::GoalOccupied); planner_2d->deactivate(); planner_2d->cleanup(); From 4b54dfdc28d30c29c7d651a0a3ee032cedb8993b Mon Sep 17 00:00:00 2001 From: Nicolas Rocha Pacheco Date: Wed, 12 Oct 2022 15:07:26 -0500 Subject: [PATCH 085/131] Update controller server goal checker (#3240) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * [FIX] Update controller server goal checker * [FIX] Autoformat code * [FIX] Misplaced tabs. Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> --- nav2_controller/src/controller_server.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2d017b1186..1712453b6b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -528,6 +528,16 @@ void ControllerServer::updateGlobalPath() action_server_->terminate_current(); return; } + std::string current_goal_checker; + if (findGoalCheckerId(goal->goal_checker_id, current_goal_checker)) { + current_goal_checker_ = current_goal_checker; + } else { + RCLCPP_INFO( + get_logger(), "Terminating action, invalid goal checker %s requested.", + goal->goal_checker_id.c_str()); + action_server_->terminate_current(); + return; + } setPlannerPath(goal->path); } } From 5d85e3ffca6ecf92061e7360a40d7300ef5f7b56 Mon Sep 17 00:00:00 2001 From: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com> Date: Sat, 15 Oct 2022 03:41:41 +0800 Subject: [PATCH 086/131] map-size restriction to avoid overflow and nullptr caused by user-misconfiguration (#3242) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * map-size restriction * map-size code style * map-size rejection * map-size codestyle * map-size return false --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index c9ca90ad07..50e74c511b 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -657,11 +657,27 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter } } else if (type == ParameterType::PARAMETER_INTEGER) { if (name == "width") { - resize_map = true; - map_width_meters_ = parameter.as_int(); + if (parameter.as_int() > 0) { + resize_map = true; + map_width_meters_ = parameter.as_int(); + } else { + RCLCPP_ERROR( + get_logger(), "You try to set width of map to be negative or zero," + " this isn't allowed, please give a positive value."); + result.successful = false; + return result; + } } else if (name == "height") { - resize_map = true; - map_height_meters_ = parameter.as_int(); + if (parameter.as_int() > 0) { + resize_map = true; + map_height_meters_ = parameter.as_int(); + } else { + RCLCPP_ERROR( + get_logger(), "You try to set height of map to be negative or zero," + " this isn't allowed, please give a positive value."); + result.successful = false; + return result; + } } } else if (type == ParameterType::PARAMETER_STRING) { if (name == "footprint") { From aeec74a1903240e5bd7421ff8a6bbc05c987306c Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Fri, 14 Oct 2022 22:42:23 +0300 Subject: [PATCH 087/131] Add Path Smoothers Benchmarking suite (#3236) * Add Path Smoothers Benchmarking suite * Meet review items * Update tools/smoother_benchmarking/README.md Co-authored-by: Steve Macenski * Move optional performance patch to the end of README * Fix README Co-authored-by: Steve Macenski --- .../nav2_simple_commander/robot_navigator.py | 19 +- tools/smoother_benchmarking/README.md | 98 ++++++ .../maps/smoothers_world.pgm | Bin 0 -> 90061 bytes .../maps/smoothers_world.yaml | 6 + tools/smoother_benchmarking/metrics.py | 157 +++++++++ tools/smoother_benchmarking/process_data.py | 302 ++++++++++++++++++ .../smoother_benchmark_bringup.py | 94 ++++++ 7 files changed, 673 insertions(+), 3 deletions(-) create mode 100644 tools/smoother_benchmarking/README.md create mode 100644 tools/smoother_benchmarking/maps/smoothers_world.pgm create mode 100644 tools/smoother_benchmarking/maps/smoothers_world.yaml create mode 100644 tools/smoother_benchmarking/metrics.py create mode 100644 tools/smoother_benchmarking/process_data.py create mode 100644 tools/smoother_benchmarking/smoother_benchmark_bringup.py diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 59a8599b03..a844498ab8 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -389,8 +389,12 @@ def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): return self.result_future.result().result.path - def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): - """Send a `SmoothPath` action request.""" + def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): + """ + Send a `SmoothPath` action request. + + Internal implementation to get the full result, not just the path. + """ self.debug("Waiting for 'SmoothPath' action server") while not self.smoother_client.wait_for_server(timeout_sec=1.0): self.info("'SmoothPath' action server not available, waiting...") @@ -417,7 +421,16 @@ def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision self.warn(f'Getting path failed with status code: {self.status}') return None - return self.result_future.result().result.path + return self.result_future.result().result + + def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): + """Send a `SmoothPath` action request.""" + rtn = self._smoothPathImpl( + self, path, smoother_id='', max_duration=2.0, check_for_collision=False) + if not rtn: + return None + else: + return rtn.path def changeMap(self, map_filepath): """Change the current static map in the map server.""" diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md new file mode 100644 index 0000000000..65ea8a4c9d --- /dev/null +++ b/tools/smoother_benchmarking/README.md @@ -0,0 +1,98 @@ +# Planners Smoothing Benchmark + +This experiment runs a set with randomly generated goals for objective benchmarking. + +Bechmarking scripts require the following python packages to be installed: + +``` +pip install transforms3d +pip install seaborn +pip install tabulate +``` + +To use the suite, modify the Nav2 bringup parameters `nav2_params.yaml` to include selected path planner: + +``` +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["SmacHybrid"] + SmacHybrid: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 + motion_model_for_search: "DUBIN" # default, non-reverse motion + smooth_path: false # should be disabled for experiment + analytic_expansion_max_length: 0.3 # decreased to avoid robot jerking +``` + +... and path smoothers for benchmark: + +``` + smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother", "constrained_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + constrained_smoother: + plugin: "nav2_constrained_smoother/ConstrainedSmoother" + w_smooth: 100000.0 # tuned +``` + +Set global costmap, path planner and smoothers parameters to those desired in `nav2_params.yaml`. +Inside of `metrics.py`, you can change reference path planner / path smoothers to use. + +For the benchmarking purposes, the clarification of execution time may be made for planner and smoother servers, to reduce impacts caused by other system actions outside of the planning / smoothing algorithm (optional): + +``` +diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp +index c7a90bcb..6f93edbf 100644 +--- a/nav2_planner/src/planner_server.cpp ++++ b/nav2_planner/src/planner_server.cpp +@@ -381,7 +381,10 @@ void PlannerServer::computePlanThroughPoses() + } + + // Get plan from start -> goal ++ auto planning_start = steady_clock_.now(); + nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); ++ auto planning_duration = steady_clock_.now() - planning_start; ++ result->planning_time = planning_duration; + + if (!validatePath(curr_goal, curr_path, goal->planner_id)) { + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); +@@ -398,7 +401,7 @@ void PlannerServer::computePlanThroughPoses() + publishPlan(result->path); + + auto cycle_duration = steady_clock_.now() - start_time; +- result->planning_time = cycle_duration; ++ // result->planning_time = cycle_duration; + + if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { + RCLCPP_WARN( +diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp +index ada1f664..610e9512 100644 +--- a/nav2_smoother/src/nav2_smoother.cpp ++++ b/nav2_smoother/src/nav2_smoother.cpp +@@ -253,8 +253,6 @@ bool SmootherServer::findSmootherId( + + void SmootherServer::smoothPlan() + { +- auto start_time = steady_clock_.now(); +- + RCLCPP_INFO(get_logger(), "Received a path to smooth."); + + auto result = std::make_shared(); +@@ -271,6 +269,8 @@ void SmootherServer::smoothPlan() + // Perform smoothing + auto goal = action_server_->get_current_goal(); + result->path = goal->path; ++ ++ auto start_time = steady_clock_.now(); + result->was_completed = smoothers_[current_smoother_]->smooth( + result->path, goal->max_smoothing_duration); + result->smoothing_duration = steady_clock_.now() - start_time; +``` + +Then execute the benchmarking: + +- `ros2 launch ./smoother_benchmark_bringup.py` to launch the nav2 stack and path smoothers benchmarking +- `python3 ./process_data.py` to take the metric files and process them into key results (and plots) diff --git a/tools/smoother_benchmarking/maps/smoothers_world.pgm b/tools/smoother_benchmarking/maps/smoothers_world.pgm new file mode 100644 index 0000000000000000000000000000000000000000..a79c5cd79e3e56462738cca592fd764e95df5baa GIT binary patch literal 90061 zcmeI5-HIDW5JvaX_%<&f$aReEV90$gLM|Nq3JEd65S-xrK%Rc1otAogHC?SU?ip!D zJ`2{is=lhOI`gf(fq>t=dH(h8`%mwG{`>u}yI=m>{qW=4ce{VzfBNggA0KzGU%Yzx z;?;M%cR#(|{rUSpzkT!J&GS3IBp`un6SzlT#gs@tvuL<}zMgp- z+%i=#dG3C9#|q|E$%^xom8>`cuGCku;sm%-U&)FSIIkT1boz3>?3^NZJSWWgRFU5{ z54rngf4?fOeLOY?MakR?sUJkCg2{7tS)BzYs|NySF%1G)t)A8KK@N}V_@@7Re)70g zs$lZmof?s~r7BrbQis2WxcrML1s0$i!DWW@<^rM{9C zC%~2ZYI#1|QeVl66X41<_$tkC^2%4Zm{L|8wX9^t32>#pT7Xs9viM96H7qf7V$!Rq zu&=`DWEG+rC1f>15(Z=yq8TM*H9`^wWEG+rC1f>15(Z=yq8TOTV>N2MvOwCjXDC^5 zda{z$n1DZnV;+~V(v-Do2wg^_8s7Jn9fokvP+0BWZobA5`)1&mDuN=gQ)I&~UZkc#}~k*kPo@ zfuP}P!+5dq-_e`U;Y7MxD8XQ9{2+ML1s0$i!D=4Z8Mdi>(emry1%%YzdsTheQ%d7m|Z8U5uek*f<)rhYh-l_rusf!X|c@uSXIf?f{QVU8n2C2 z4N4e`mRcyjGFBm$dSW3cLECk)suO8MgRPbcuZmRzN|aiLO1-PB>Rf73S=Ff2W3sAq zsYPW~qf(EpidFpIfN9L}YkjGOqU&Nc71lyfg0?GTH4Rp<(^BEJvAXB9mO>KtT^}n* zE8J+g#2Q%vTFU`xbY7U%zI||coH-Pv$ttfw!{x(imgYT`ai4}7)q7!(jWbURimRR` zF8?l#(!95rwtj3>?+b!#oCyl9A5Pmh=cofJqK)d9NStZ3yKXA2+?=7k)#qm0b-czSL!QSaROYauVlpuaHYPI6(_)zSMwDg zaWzU=@kb#mS#bhfsjp#pk`*VwmHJ9noB&tqs|&JfU$uV}ZNX}@<=lsJx3AjE zZkAO!`0*^O>cbwe95VSVs~W3BCH?39_qj(@hhtTL4DQv(wUaNK zv*3v6`2C@GIyZ0W*|(>*In?6G1Ei8qc@FOH*|(=Q80S*qfy}ev^>F2IZvTw+?Aud& zDJaoB2~Dv<3PkcDUZ|V8TC%6M*6*URy9ZZ2`zqfltBm6GkkAxQTUMutj_(IDeqJ4M zw7zFoJsgrSg<3@qzg9H!tz=JatMB5u#O1F1Rn*_F>eP>@j=z8oZb`v)dGt#96p~ey)sqkDJ)=P+t2(Ru^6#_5Syln7N?2AEC_zY80hmfy zRuw2gNLB%uN?2AEC_zY80hmgdu}Z4Xd#zl$(U7e8gOk;)ta!5#pk`*Vwl~?msx_E=Hob7DN4N}rqQy`cuGCku;sm%-U&)FS;7WZZD^7qb^_8qR0j|_nvf>1| zQeVl66W~gHB`Z#VEA`b>R*4vV<;`zQ3Hg+=8zaEiDIvf!Z+>IS#D_1n*K_fWDP_eU zsH|kg32>#pk`*VwmHJ9noB&tqD_L;@T&b^Q#R+hwzLFIuz?J$+R-6D=>ML1s0$i!D zWW@<^rM{9CC%~2ZN>-czS5Ea6?|geoS@CBoD_L;@T&b^Q#R+hwzLFIuz?J$+R-6D= q>ML1s0$i!DWW@<^rM{9CC%~2ZN>-czSL&;&tP(L_B_M&T5cnUWhb(3Q literal 0 HcmV?d00001 diff --git a/tools/smoother_benchmarking/maps/smoothers_world.yaml b/tools/smoother_benchmarking/maps/smoothers_world.yaml new file mode 100644 index 0000000000..c093166203 --- /dev/null +++ b/tools/smoother_benchmarking/maps/smoothers_world.yaml @@ -0,0 +1,6 @@ +image: smoothers_world.pgm +resolution: 0.050000 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py new file mode 100644 index 0000000000..6b576b08a9 --- /dev/null +++ b/tools/smoother_benchmarking/metrics.py @@ -0,0 +1,157 @@ +#! /usr/bin/env python3 +# Copyright (c) 2022 Samsung R&D Institute Russia +# Copyright (c) 2022 Joshua Wallace +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy + +import math +import os +import pickle +import numpy as np + +from random import seed +from random import randint +from random import uniform + +from transforms3d.euler import euler2quat + + +# Note: Map origin is assumed to be (0,0) + +def getPlannerResults(navigator, initial_pose, goal_pose, planner): + return navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True) + +def getSmootherResults(navigator, path, smoothers): + smoothed_results = [] + for smoother in smoothers: + smoothed_result = navigator._smoothPathImpl(path, smoother) + if smoothed_result is not None: + smoothed_results.append(smoothed_result) + else: + print(smoother, " failed to smooth the path") + return None + return smoothed_results + +def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): + start = PoseStamped() + start.header.frame_id = 'map' + start.header.stamp = time_stamp + while True: + row = randint(side_buffer, costmap.shape[0]-side_buffer) + col = randint(side_buffer, costmap.shape[1]-side_buffer) + + if costmap[row, col] < max_cost: + start.pose.position.x = col*res + start.pose.position.y = row*res + + yaw = uniform(0, 1) * 2*math.pi + quad = euler2quat(0.0, 0.0, yaw) + start.pose.orientation.w = quad[0] + start.pose.orientation.x = quad[1] + start.pose.orientation.y = quad[2] + start.pose.orientation.z = quad[3] + break + return start + +def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): + goal = PoseStamped() + goal.header.frame_id = 'map' + goal.header.stamp = time_stamp + while True: + row = randint(side_buffer, costmap.shape[0]-side_buffer) + col = randint(side_buffer, costmap.shape[1]-side_buffer) + + start_x = start.pose.position.x + start_y = start.pose.position.y + goal_x = col*res + goal_y = row*res + x_diff = goal_x - start_x + y_diff = goal_y - start_y + dist = math.sqrt(x_diff ** 2 + y_diff ** 2) + + if costmap[row, col] < max_cost and dist > 3.0: + goal.pose.position.x = goal_x + goal.pose.position.y = goal_y + + yaw = uniform(0, 1) * 2*math.pi + quad = euler2quat(0.0, 0.0, yaw) + goal.pose.orientation.w = quad[0] + goal.pose.orientation.x = quad[1] + goal.pose.orientation.y = quad[2] + goal.pose.orientation.z = quad[3] + break + return goal + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Wait for planner and smoother to fully activate + print("Waiting for planner and smoother servers to activate") + navigator.waitUntilNav2Active('smoother_server', 'planner_server') + + # Get the costmap for start/goal validation + costmap_msg = navigator.getGlobalCostmap() + costmap = np.asarray(costmap_msg.data) + costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) + + planner = 'SmacHybrid' + smoothers = ['simple_smoother', 'constrained_smoother'] + max_cost = 210 + side_buffer = 10 + time_stamp = navigator.get_clock().now().to_msg() + results = [] + seed(33) + + random_pairs = 100 + i = 0 + res = costmap_msg.metadata.resolution + while i < random_pairs: + print("Cycle: ", i, "out of: ", random_pairs) + start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) + goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) + print("Start", start) + print("Goal", goal) + result = getPlannerResults(navigator, start, goal, planner) + if result is not None: + smoothed_results = getSmootherResults(navigator, result.path, smoothers) + if smoothed_results is not None: + results.append(result) + results.append(smoothed_results) + i += 1 + else: + print(planner, " planner failed to produce the path") + + print("Write Results...") + benchmark_dir = os.getcwd() + with open(os.path.join(benchmark_dir, 'results.pickle'), 'wb') as f: + pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) + + with open(os.path.join(benchmark_dir, 'costmap.pickle'), 'wb') as f: + pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL) + + smoothers.insert(0, planner) + with open(os.path.join(benchmark_dir, 'methods.pickle'), 'wb') as f: + pickle.dump(smoothers, f, pickle.HIGHEST_PROTOCOL) + print("Write Complete") + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/tools/smoother_benchmarking/process_data.py b/tools/smoother_benchmarking/process_data.py new file mode 100644 index 0000000000..edc0dad811 --- /dev/null +++ b/tools/smoother_benchmarking/process_data.py @@ -0,0 +1,302 @@ +#! /usr/bin/env python3 +# Copyright (c) 2022 Samsung R&D Institute Russia +# Copyright (c) 2022 Joshua Wallace +# Copyright (c) 2021 RoboTech Vision +# +# 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 numpy as np +import math + +import os +import pickle + +import seaborn as sns +import matplotlib.pylab as plt +from tabulate import tabulate + + +def getPaths(results): + paths = [] + for i in range(len(results)): + if (i % 2) == 0: + # Append non-smoothed path + paths.append(results[i].path) + else: + # Append smoothed paths array + for result in results[i]: + paths.append(result.path) + return paths + + +def getTimes(results): + times = [] + for i in range(len(results)): + if (i % 2) == 0: + # Append non-smoothed time + times.append(results[i].planning_time.nanosec/1e09 + results[i].planning_time.sec) + else: + # Append smoothed times array + for result in results[i]: + times.append(result.smoothing_duration.nanosec/1e09 + result.smoothing_duration.sec) + return times + + +def getMapCoordsFromPaths(paths, resolution): + coords = [] + for path in paths: + x = [] + y = [] + for pose in path.poses: + x.append(pose.pose.position.x/resolution) + y.append(pose.pose.position.y/resolution) + coords.append(x) + coords.append(y) + return coords + + +def getPathLength(path): + path_length = 0 + x_prev = path.poses[0].pose.position.x + y_prev = path.poses[0].pose.position.y + for i in range(1, len(path.poses)): + x_curr = path.poses[i].pose.position.x + y_curr = path.poses[i].pose.position.y + path_length = path_length + math.sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) + x_prev = x_curr + y_prev = y_curr + return path_length + +# Path smoothness calculations +def getSmoothness(pt_prev, pt, pt_next): + d1 = pt - pt_prev + d2 = pt_next - pt + delta = d2 - d1 + return np.dot(delta, delta) + +def getPathSmoothnesses(paths): + smoothnesses = [] + pm0 = np.array([0.0, 0.0]) + pm1 = np.array([0.0, 0.0]) + pm2 = np.array([0.0, 0.0]) + for path in paths: + smoothness = 0.0 + for i in range(2, len(path.poses)): + pm0[0] = path.poses[i].pose.position.x + pm0[1] = path.poses[i].pose.position.y + pm1[0] = path.poses[i-1].pose.position.x + pm1[1] = path.poses[i-1].pose.position.y + pm2[0] = path.poses[i-2].pose.position.x + pm2[1] = path.poses[i-2].pose.position.y + smoothness += getSmoothness(pm2, pm1, pm0) + smoothnesses.append(smoothness) + return smoothnesses + +# Curvature calculations +def arcCenter(pt_prev, pt, pt_next): + cusp_thresh = -0.7 + + d1 = pt - pt_prev + d2 = pt_next - pt + + d1_norm = d1 / np.linalg.norm(d1) + d2_norm = d2 / np.linalg.norm(d2) + cos_angle = np.dot(d1_norm, d2_norm) + + if cos_angle < cusp_thresh: + # cusp case + d2 = -d2 + pt_next = pt + d2 + + det = d1[0] * d2[1] - d1[1] * d2[0] + if abs(det) < 1e-4: # straight line + return (float('inf'), float('inf')) + + # circle center is at the intersection of mirror axes of the segments: + # http://paulbourke.net/geometry/circlesphere/ + # line intersection: + # https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines + mid1 = (pt_prev + pt) / 2 + mid2 = (pt + pt_next) / 2 + n1 = (-d1[1], d1[0]) + n2 = (-d2[1], d2[0]) + det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0] + det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0] + center = np.array([(det1 * n2[0] - det2 * n1[0]) / det, (det1 * n2[1] - det2 * n1[1]) / det]) + return center + +def getPathCurvatures(paths): + curvatures = [] + pm0 = np.array([0.0, 0.0]) + pm1 = np.array([0.0, 0.0]) + pm2 = np.array([0.0, 0.0]) + for path in paths: + radiuses = [] + for i in range(2, len(path.poses)): + pm0[0] = path.poses[i].pose.position.x + pm0[1] = path.poses[i].pose.position.y + pm1[0] = path.poses[i-1].pose.position.x + pm1[1] = path.poses[i-1].pose.position.y + pm2[0] = path.poses[i-2].pose.position.x + pm2[1] = path.poses[i-2].pose.position.y + center = arcCenter(pm2, pm1, pm0) + if center[0] != float('inf'): + turning_rad = np.linalg.norm(pm1 - center); + radiuses.append(turning_rad) + curvatures.append(np.average(radiuses)) + return curvatures + +def plotResults(costmap, paths): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + data = np.where(data <= 253, 0, data) + + plt.figure(3) + ax = sns.heatmap(data, cmap='Greys', cbar=False) + for i in range(0, len(coords), 2): + ax.plot(coords[i], coords[i+1], linewidth=0.7) + plt.axis('off') + ax.set_aspect('equal', 'box') + plt.show() + + +def averagePathCost(paths, costmap, num_of_planners): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + + average_path_costs = [] + for i in range(num_of_planners): + average_path_costs.append([]) + + k = 0 + for i in range(0, len(coords), 2): + costs = [] + for j in range(len(coords[i])): + costs.append(data[math.floor(coords[i+1][j])][math.floor(coords[i][j])]) + average_path_costs[k % num_of_planners].append(sum(costs)/len(costs)) + k += 1 + + return average_path_costs + + +def maxPathCost(paths, costmap, num_of_planners): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + + max_path_costs = [] + for i in range(num_of_planners): + max_path_costs.append([]) + + k = 0 + for i in range(0, len(coords), 2): + max_cost = 0 + for j in range(len(coords[i])): + cost = data[math.floor(coords[i+1][j])][math.floor(coords[i][j])] + if max_cost < cost: + max_cost = cost + max_path_costs[k % num_of_planners].append(max_cost) + k += 1 + + return max_path_costs + + +def main(): + # Read the data + benchmark_dir = os.getcwd() + print("Read data") + with open(os.path.join(benchmark_dir, 'results.pickle'), 'rb') as f: + results = pickle.load(f) + + with open(os.path.join(benchmark_dir, 'methods.pickle'), 'rb') as f: + smoothers = pickle.load(f) + planner = smoothers[0] + del smoothers[0] + methods_num = len(smoothers) + 1 + + with open(os.path.join(benchmark_dir, 'costmap.pickle'), 'rb') as f: + costmap = pickle.load(f) + + # Paths (planner and smoothers) + paths = getPaths(results) + path_lengths = [] + + for path in paths: + path_lengths.append(getPathLength(path)) + path_lengths = np.asarray(path_lengths) + total_paths = len(paths) + + # [planner, smoothers] path lenghth in a row + path_lengths.resize((int(total_paths/methods_num), methods_num)) + # [planner, smoothers] path length in a column + path_lengths = path_lengths.transpose() + + # Times + times = getTimes(results) + times = np.asarray(times) + times.resize((int(total_paths/methods_num), methods_num)) + times = np.transpose(times) + + # Costs + average_path_costs = np.asarray(averagePathCost(paths, costmap, methods_num)) + max_path_costs = np.asarray(maxPathCost(paths, costmap, methods_num)) + + # Smoothness + smoothnesses = getPathSmoothnesses(paths) + smoothnesses = np.asarray(smoothnesses) + smoothnesses.resize((int(total_paths/methods_num), methods_num)) + smoothnesses = np.transpose(smoothnesses) + + # Curvatures + curvatures = getPathCurvatures(paths) + curvatures = np.asarray(curvatures) + curvatures.resize((int(total_paths/methods_num), methods_num)) + curvatures = np.transpose(curvatures) + + # Generate table + planner_table = [['Planner', + 'Time (s)', + 'Path length (m)', + 'Average cost', + 'Max cost', + 'Path smoothness (x100)', + 'Average turning rad (m)']] + # for path planner + planner_table.append([planner, + np.average(times[0]), + np.average(path_lengths[0]), + np.average(average_path_costs[0]), + np.average(max_path_costs[0]), + np.average(smoothnesses[0]) * 100, + np.average(curvatures[0])]) + # for path smoothers + for i in range(1, methods_num): + planner_table.append([smoothers[i-1], + np.average(times[i]), + np.average(path_lengths[i]), + np.average(average_path_costs[i]), + np.average(max_path_costs[i]), + np.average(smoothnesses[i]) * 100, + np.average(curvatures[i])]) + + # Visualize results + print(tabulate(planner_table)) + plotResults(costmap, paths) + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py new file mode 100644 index 0000000000..9b0ed744de --- /dev/null +++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py @@ -0,0 +1,94 @@ +# Copyright (c) 2022 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 + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + benchmark_dir = os.getcwd() + metrics_py = os.path.join(benchmark_dir, 'metrics.py') + config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml') + map_file = os.path.join(benchmark_dir, 'maps', 'smoothers_world.yaml') + lifecycle_nodes = ['map_server', 'planner_server', 'smoother_server'] + + static_transform_one = Node( + package = 'tf2_ros', + executable = 'static_transform_publisher', + output = 'screen', + arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"]) + + static_transform_two = Node( + package = 'tf2_ros', + executable = 'static_transform_publisher', + output = 'screen', + arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"]) + + start_map_server_cmd = Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'use_sim_time': True}, + {'yaml_filename': map_file}, + {'topic_name': "map"}]) + + start_planner_server_cmd = Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[config]) + + start_smoother_server_cmd = Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + parameters=[config]) + + start_lifecycle_manager_cmd = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[{'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}]) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + metrics_cmd = ExecuteProcess( + cmd=['python3', '-u', metrics_py], + cwd=[benchmark_dir], output='screen') + + ld = LaunchDescription() + ld.add_action(static_transform_one) + ld.add_action(static_transform_two) + ld.add_action(start_map_server_cmd) + ld.add_action(start_planner_server_cmd) + ld.add_action(start_smoother_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) + ld.add_action(rviz_cmd) + ld.add_action(metrics_cmd) + return ld From e7d4d98e4707264daa131825719ead24d6d3533f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 14 Oct 2022 16:06:22 -0400 Subject: [PATCH 088/131] Controller exceptions (#3227) * added result codes for global planner * code review * code review * cleanup * cleanup * update smac lattice planner * update planner instances * cleanup * added controller exception * renaming * follow path updates * rename exceptions * updated regulated pure pursuit * completed pure pursuit * completed dwb * linting fixes * cleanup * revert planner server * revert planner server * revert planner server * revert planner server * code review * code review * cleanup * cleanup * bug fix * final cleanup * set follow path error on bt * update groot * code review Co-authored-by: Joshua Wallace --- .../plugins/action/follow_path_action.hpp | 23 ++++++- nav2_behavior_tree/nav2_tree_nodes.xml | 1 + .../plugins/action/follow_path_action.cpp | 23 ++++++- .../src/constrained_smoother.cpp | 3 +- .../plugins/simple_progress_checker.cpp | 1 - nav2_controller/src/controller_server.cpp | 65 ++++++++++++++---- .../nav2_core/controller_exceptions.hpp | 67 +++++++++++++++++++ ...{exceptions.hpp => planner_exceptions.hpp} | 6 +- .../dwb_core/include/dwb_core/exceptions.hpp | 20 ++---- .../dwb_core/illegal_trajectory_tracker.hpp | 6 +- .../dwb_core/src/dwb_local_planner.cpp | 35 +++++++--- .../dwb_core/src/trajectory_utils.cpp | 4 +- nav2_msgs/action/FollowPath.action | 11 +++ .../nav2_navfn_planner/navfn_planner.hpp | 2 +- .../include/nav2_planner/planner_server.hpp | 4 +- .../src/regulated_pure_pursuit_controller.cpp | 18 ++--- .../test/test_regulated_pp.cpp | 4 +- .../nav2_rotation_shim_controller.hpp | 2 +- .../src/nav2_rotation_shim_controller.cpp | 12 ++-- .../include/nav2_smac_planner/a_star.hpp | 2 +- nav2_smoother/src/nav2_smoother.cpp | 4 +- nav2_smoother/test/test_smoother_server.cpp | 5 +- .../theta_star_planner.hpp | 2 +- 23 files changed, 237 insertions(+), 83 deletions(-) create mode 100644 nav2_core/include/nav2_core/controller_exceptions.hpp rename nav2_core/include/nav2_core/{exceptions.hpp => planner_exceptions.hpp} (95%) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index a97993c773..369e7ab0f9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -29,6 +29,10 @@ namespace nav2_behavior_tree */ class FollowPathAction : public BtActionNode { + using Action = nav2_msgs::action::FollowPath; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::FollowPathAction @@ -46,13 +50,28 @@ class FollowPathAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to perform some user-defined operation upon successful completion of the action + */ + BT::NodeStatus on_success() override; + + /** + * @brief Function to perform some user-defined operation upon abortion of the action + */ + BT::NodeStatus on_aborted() override; + + /** + * @brief Function to perform some user-defined operation upon cancellation of the action + */ + BT::NodeStatus on_cancelled() override; + /** * @brief Function to perform some user-defined operation after a timeout * waiting for a result that hasn't been received yet * @param feedback shared_ptr to latest feedback message */ void on_wait_for_result( - std::shared_ptr feedback) override; + std::shared_ptr feedback) override; /** * @brief Creates list of BT ports @@ -65,6 +84,8 @@ class FollowPathAction : public BtActionNode BT::InputPort("path", "Path to follow"), BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), + BT::OutputPort( + "follow_path_error_code", "The follow path error code"), }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 636392929a..425875c24c 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -113,6 +113,7 @@ Goal checker Service name Server timeout + Follow Path error code diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 0fc0dc5b57..c90d8fb65a 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -24,7 +24,7 @@ FollowPathAction::FollowPathAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -35,8 +35,27 @@ void FollowPathAction::on_tick() getInput("goal_checker_id", goal_.goal_checker_id); } +BT::NodeStatus FollowPathAction::on_success() +{ + setOutput("follow_path_error_code", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus FollowPathAction::on_aborted() +{ + setOutput("follow_path_error_code", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus FollowPathAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + void FollowPathAction::on_wait_for_result( - std::shared_ptr/*feedback*/) + std::shared_ptr/*feedback*/) { // Grab the new path nav_msgs::msg::Path new_path; diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 4ffa4a21c3..4ce698cda1 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -21,7 +21,6 @@ #include #include "nav2_constrained_smoother/constrained_smoother.hpp" -#include "nav2_core/exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -138,7 +137,7 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat logger_, "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", plugin_name_.c_str()); - throw new nav2_core::PlannerException( + throw std::runtime_error( "Failed to smooth plan, Ceres could not find a usable solution."); } diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 1ced16dc5b..0b2ec3e739 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -17,7 +17,6 @@ #include #include #include -#include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 1712453b6b..c527e41b14 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -19,7 +19,7 @@ #include #include -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/tf_help.hpp" #include "nav2_util/node_utils.hpp" @@ -353,8 +353,7 @@ void ControllerServer::computeControl() if (findControllerId(c_name, current_controller)) { current_controller_ = current_controller; } else { - action_server_->terminate_current(); - return; + throw nav2_core::ControllerException("Failed to find controller name: " + c_name); } std::string gc_name = action_server_->get_current_goal()->goal_checker_id; @@ -362,8 +361,7 @@ void ControllerServer::computeControl() if (findGoalCheckerId(gc_name, current_goal_checker)) { current_goal_checker_ = current_goal_checker; } else { - action_server_->terminate_current(); - return; + throw nav2_core::ControllerException("Failed to find goal checker name: " + gc_name); } setPlannerPath(action_server_->get_current_goal()->path); @@ -405,10 +403,47 @@ void ControllerServer::computeControl() controller_frequency_); } } - } catch (nav2_core::PlannerException & e) { - RCLCPP_ERROR(this->get_logger(), e.what()); + } catch (nav2_core::ControllerTFError & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::TF_ERROR; + action_server_->terminate_current(result); + return; + } catch (nav2_core::NoValidControl & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::NO_VALID_CONTROL; + action_server_->terminate_current(result); + return; + } catch (nav2_core::FailedToMakeProgress & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::FAILED_TO_MAKE_PROGRESS; + action_server_->terminate_current(result); + return; + } catch (nav2_core::PatienceExceeded & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::PATIENCE_EXCEEDED; + action_server_->terminate_current(result); + return; + } catch (nav2_core::InvalidPath & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::INVALID_PATH; + action_server_->terminate_current(result); + return; + } catch (nav2_core::ControllerException & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); - action_server_->terminate_current(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::UNKNOWN; + action_server_->terminate_current(result); return; } @@ -426,7 +461,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) get_logger(), "Providing path to the controller %s", current_controller_.c_str()); if (path.poses.empty()) { - throw nav2_core::PlannerException("Invalid path, Path is empty."); + throw nav2_core::InvalidPath("Path is empty."); } controllers_[current_controller_]->setPlan(path); @@ -446,11 +481,11 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::PoseStamped pose; if (!getRobotPose(pose)) { - throw nav2_core::PlannerException("Failed to obtain robot pose"); + throw nav2_core::ControllerTFError("Failed to obtain robot pose"); } if (!progress_checker_->check(pose)) { - throw nav2_core::PlannerException("Failed to make progress"); + throw nav2_core::FailedToMakeProgress("Failed to make progress"); } nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); @@ -464,7 +499,9 @@ void ControllerServer::computeAndPublishVelocity() nav_2d_utils::twist2Dto3D(twist), goal_checkers_[current_goal_checker_].get()); last_valid_cmd_time_ = now(); - } catch (nav2_core::PlannerException & e) { + // Only no valid control exception types are valid to attempt to have control patience, as + // other types will not be resolved with more attempts + } catch (nav2_core::NoValidControl & e) { if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { RCLCPP_WARN(this->get_logger(), e.what()); cmd_vel_2d.twist.angular.x = 0; @@ -478,10 +515,10 @@ void ControllerServer::computeAndPublishVelocity() if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ && failure_tolerance_ != -1.0) { - throw nav2_core::PlannerException("Controller patience exceeded"); + throw nav2_core::PatienceExceeded("Controller patience exceeded"); } } else { - throw nav2_core::PlannerException(e.what()); + throw nav2_core::NoValidControl(e.what()); } } diff --git a/nav2_core/include/nav2_core/controller_exceptions.hpp b/nav2_core/include/nav2_core/controller_exceptions.hpp new file mode 100644 index 0000000000..bf9c22f4e6 --- /dev/null +++ b/nav2_core/include/nav2_core/controller_exceptions.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2022. Joshua Wallace +// 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__CONTROLLER_EXCEPTIONS_HPP_ +#define NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_ + +#include +#include + +namespace nav2_core +{ + +class ControllerException : public std::runtime_error +{ +public: + explicit ControllerException(const std::string & description) + : std::runtime_error(description) {} +}; + +class ControllerTFError : public ControllerException +{ +public: + explicit ControllerTFError(const std::string & description) + : ControllerException(description) {} +}; + +class FailedToMakeProgress : public ControllerException +{ +public: + explicit FailedToMakeProgress(const std::string & description) + : ControllerException(description) {} +}; + +class PatienceExceeded : public ControllerException +{ +public: + explicit PatienceExceeded(const std::string & description) + : ControllerException(description) {} +}; + +class InvalidPath : public ControllerException +{ +public: + explicit InvalidPath(const std::string & description) + : ControllerException(description) {} +}; + +class NoValidControl : public ControllerException +{ +public: + explicit NoValidControl(const std::string & description) + : ControllerException(description) {} +}; + +} // namespace nav2_core + +#endif // NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_ diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp similarity index 95% rename from nav2_core/include/nav2_core/exceptions.hpp rename to nav2_core/include/nav2_core/planner_exceptions.hpp index 19a113d2e4..622ee0f0db 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -33,8 +33,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef NAV2_CORE__EXCEPTIONS_HPP_ -#define NAV2_CORE__EXCEPTIONS_HPP_ +#ifndef NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ +#define NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ #include #include @@ -101,4 +101,4 @@ class PlannerTFError : public PlannerException } // namespace nav2_core -#endif // NAV2_CORE__EXCEPTIONS_HPP_ +#endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp index 157383a419..66a8bfd9e1 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp @@ -34,35 +34,23 @@ #ifndef DWB_CORE__EXCEPTIONS_HPP_ #define DWB_CORE__EXCEPTIONS_HPP_ -#include #include #include -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" namespace dwb_core { -/** - * @class PlannerTFException - * @brief Thrown when the planner cannot complete its operation due to TF errors - */ -class PlannerTFException : public nav2_core::PlannerException -{ -public: - explicit PlannerTFException(const std::string description) - : nav2_core::PlannerException(description) {} -}; - /** * @class IllegalTrajectoryException * @brief Thrown when one of the critics encountered a fatal error */ -class IllegalTrajectoryException : public nav2_core::PlannerException +class IllegalTrajectoryException : public nav2_core::ControllerException { public: - IllegalTrajectoryException(const std::string critic_name, const std::string description) - : nav2_core::PlannerException(description), critic_name_(critic_name) {} + IllegalTrajectoryException(const std::string & critic_name, const std::string & description) + : nav2_core::ControllerException(description), critic_name_(critic_name) {} std::string getCriticName() const {return critic_name_;} protected: diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp index 31c306f665..aa6a451a1a 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp @@ -39,7 +39,7 @@ #include #include #include "dwb_core/exceptions.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" namespace dwb_core { @@ -66,11 +66,11 @@ class IllegalTrajectoryTracker * @brief Thrown when all the trajectories explored are illegal */ class NoLegalTrajectoriesException - : public nav2_core::PlannerException + : public nav2_core::ControllerException { public: explicit NoLegalTrajectoriesException(const IllegalTrajectoryTracker & tracker) - : PlannerException(tracker.getMessage()), + : ControllerException(tracker.getMessage()), tracker_(tracker) {} IllegalTrajectoryTracker tracker_; }; 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 0be00388ca..da8110a1a5 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -44,11 +44,11 @@ #include "dwb_msgs/msg/critic_score.hpp" #include "nav_2d_msgs/msg/twist2_d.hpp" #include "nav_2d_utils/conversions.hpp" -#include "nav_2d_utils/parameters.hpp" #include "nav_2d_utils/tf_help.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -133,7 +133,9 @@ void DWBLocalPlanner::configure( loadCritics(); } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Couldn't load critics! Caught exception: %s", e.what()); - throw; + throw nav2_core::ControllerException( + "Couldn't load critics! Caught exception: " + + std::string(e.what())); } } @@ -214,7 +216,9 @@ DWBLocalPlanner::loadCritics() plugin->initialize(node, critic_plugin_name, dwb_plugin_name_, costmap_ros_); } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Couldn't initialize critic plugin!"); - throw; + throw nav2_core::ControllerException( + "Couldn't initialize critic plugin: " + + std::string(e.what())); } RCLCPP_INFO(logger_, "Critic plugin initialized"); } @@ -253,9 +257,18 @@ DWBLocalPlanner::computeVelocityCommands( geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.twist = nav_2d_utils::twist2Dto3D(cmd_vel2d.velocity); return cmd_vel; - } catch (const nav2_core::PlannerException & e) { + } catch (const nav2_core::ControllerTFError & e) { pub_->publishEvaluation(results); - throw; + throw e; + } catch (const nav2_core::InvalidPath & e) { + pub_->publishEvaluation(results); + throw e; + } catch (const nav2_core::NoValidControl & e) { + pub_->publishEvaluation(results); + throw e; + } catch (const nav2_core::ControllerException & e) { + pub_->publishEvaluation(results); + throw e; } } @@ -333,7 +346,9 @@ DWBLocalPlanner::computeVelocityCommands( pub_->publishLocalPlan(pose.header, empty_traj); pub_->publishCostGrid(costmap_ros_, critics_); - throw; + throw nav2_core::NoValidControl( + "Could not find a legal trajectory: " + + std::string(e.what())); } } @@ -441,7 +456,7 @@ DWBLocalPlanner::transformGlobalPlan( const nav_2d_msgs::msg::Pose2DStamped & pose) { if (global_plan_.poses.empty()) { - throw nav2_core::PlannerException("Received plan with zero length"); + throw nav2_core::InvalidPath("Received plan with zero length"); } // let's get the pose of the robot in the frame of the plan @@ -450,8 +465,8 @@ DWBLocalPlanner::transformGlobalPlan( tf_, global_plan_.header.frame_id, pose, robot_pose, transform_tolerance_)) { - throw dwb_core:: - PlannerTFException("Unable to transform robot pose into global plan's frame"); + throw nav2_core:: + ControllerTFError("Unable to transform robot pose into global plan's frame"); } // we'll discard points on the plan that are outside the local costmap @@ -535,7 +550,7 @@ DWBLocalPlanner::transformGlobalPlan( } if (transformed_plan.poses.empty()) { - throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); } return transformed_plan; } diff --git a/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp b/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp index a7764d4b7a..b89957f34e 100644 --- a/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp +++ b/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp @@ -49,7 +49,7 @@ const geometry_msgs::msg::Pose2D & getClosestPose( rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(time_offset); const unsigned int num_poses = trajectory.poses.size(); if (num_poses == 0) { - throw nav2_core::PlannerException("Cannot call getClosestPose on empty trajectory."); + throw nav2_core::InvalidPath("Cannot call getClosestPose on empty trajectory."); } unsigned int closest_index = num_poses; double closest_diff = 0.0; @@ -73,7 +73,7 @@ geometry_msgs::msg::Pose2D projectPose( rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(time_offset); const unsigned int num_poses = trajectory.poses.size(); if (num_poses == 0) { - throw nav2_core::PlannerException("Cannot call projectPose on empty trajectory."); + throw nav2_core::InvalidPath("Cannot call projectPose on empty trajectory."); } if (goal_time <= (trajectory.time_offsets[0])) { return trajectory.poses[0]; diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 5462faa239..9ab82ff4d7 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,3 +1,13 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +int16 NONE=0 +int16 UNKNOWN=1 +int16 TF_ERROR=2 +int16 INVALID_PATH=3 +int16 PATIENCE_EXCEEDED=4 +int16 FAILED_TO_MAKE_PROGRESS=5 +int16 NO_VALID_CONTROL=6 + #goal definition nav_msgs/Path path string controller_id @@ -5,6 +15,7 @@ string goal_checker_id --- #result definition std_msgs/Empty result +int16 error_code --- #feedback definition float32 distance_to_goal 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 454dd44858..2330a5165b 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -25,7 +25,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/global_planner.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_navfn_planner/navfn.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 3cca065e36..41e1b93502 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -39,7 +39,7 @@ #include "pluginlib/class_list_macros.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" namespace nav2_planner { @@ -254,7 +254,7 @@ class PlannerServer : public nav2_util::LifecycleNode // Publishers for the path rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; - // Service to deterime if the path is valid + // Service to determine if the path is valid rclcpp::Service::SharedPtr is_path_valid_service_; }; diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 4b52dd3616..d5c648bd1a 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -21,7 +21,7 @@ #include #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -46,7 +46,7 @@ void RegulatedPurePursuitController::configure( auto node = parent.lock(); node_ = parent; if (!node) { - throw nav2_core::PlannerException("Unable to lock node!"); + throw nav2_core::ControllerException("Unable to lock node!"); } costmap_ros_ = costmap_ros; @@ -353,7 +353,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Collision checking on this velocity heading const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { - throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!"); + throw nav2_core::NoValidControl("RegulatedPurePursuitController detected collision ahead!"); } // populate and return message @@ -579,7 +579,7 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double logger_, "The dimensions of the costmap is too small to fully include your robot's footprint, " "thusly the robot cannot proceed further"); - throw nav2_core::PlannerException( + throw nav2_core::ControllerException( "RegulatedPurePursuitController: Dimensions of the costmap are too small " "to encapsulate the robot footprint at current speeds!"); } @@ -690,13 +690,13 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { if (global_plan_.poses.empty()) { - throw nav2_core::PlannerException("Received plan with zero length"); + throw nav2_core::InvalidPath("Received plan with zero length"); } // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { - throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); } // We'll discard points on the plan that are outside the local costmap @@ -729,7 +729,9 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( stamped_pose.header.frame_id = global_plan_.header.frame_id; stamped_pose.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; - transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose); + if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { + throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); + } transformed_pose.pose.position.z = 0.0; return transformed_pose; }; @@ -749,7 +751,7 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( global_path_pub_->publish(transformed_plan); if (transformed_plan.poses.empty()) { - throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); } return transformed_plan; diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 83d814dda0..51bb277fed 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -25,7 +25,7 @@ #include "path_utils/path_utils.hpp" #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" class RclCppFixture { @@ -849,7 +849,7 @@ TEST_F(TransformGlobalPlanTest, all_poses_outside_of_costmap) ctrl_->setPlan(global_plan); // Transform the plan - EXPECT_THROW(ctrl_->transformGlobalPlanWrapper(robot_pose), nav2_core::PlannerException); + EXPECT_THROW(ctrl_->transformGlobalPlanWrapper(robot_pose), nav2_core::ControllerException); } // Should shortcut the circle if the circle is shorter than max_robot_pose_search_dist diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index d3f96dd25e..c3812ec1d3 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -27,7 +27,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_core/controller.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "angles/angles.h" diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index bc86900f64..7d32283db7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include #include @@ -175,7 +174,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() { if (current_path_.poses.size() < 2) { - throw nav2_core::PlannerException( + throw nav2_core::ControllerException( "Path is too short to find a valid sampled path point for rotation."); } @@ -193,7 +192,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - throw nav2_core::PlannerException( + throw nav2_core::ControllerException( std::string( "Unable to find a sampling point at least %0.2f from the robot," "passing off to primary controller plugin.", forward_sampling_distance_)); @@ -204,7 +203,7 @@ RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseS { geometry_msgs::msg::PoseStamped pt_base; if (!nav2_util::transformPoseInTargetFrame(pt, pt_base, *tf_, costmap_ros_->getBaseFrameID())) { - throw nav2_core::PlannerException("Failed to transform pose to base frame!"); + throw nav2_core::ControllerTFError("Failed to transform pose to base frame!"); } return pt_base.pose; } @@ -259,11 +258,12 @@ void RotationShimController::isCollisionFree( if (footprint_cost == static_cast(NO_INFORMATION) && costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { - throw std::runtime_error("RotationShimController detected a potential collision ahead!"); + throw nav2_core::NoValidControl( + "RotationShimController detected a potential collision ahead!"); } if (footprint_cost >= static_cast(LETHAL_OBSTACLE)) { - throw std::runtime_error("RotationShimController detected collision ahead!"); + throw nav2_core::NoValidControl("RotationShimController detected collision ahead!"); } } } diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 7b2ee31c0e..01aeca975d 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -25,7 +25,7 @@ #include "Eigen/Core" #include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" #include "nav2_smac_planner/thirdparty/robin_hood.h" #include "nav2_smac_planner/analytic_expansion.hpp" diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index ada1f664b0..27e88f17b3 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -14,15 +14,13 @@ // limitations under the License. #include -#include #include #include #include #include -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" #include "nav2_smoother/nav2_smoother.hpp" -#include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/tf_help.hpp" diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 8dbc5416db..d7afe9b55d 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -16,9 +16,7 @@ #include #include #include -#include #include -#include #include #include "gtest/gtest.h" @@ -26,10 +24,9 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_core/smoother.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" #include "nav2_msgs/action/smooth_path.hpp" #include "nav2_smoother/nav2_smoother.hpp" -#include "tf2_ros/create_timer_ros.h" using SmoothAction = nav2_msgs::action::SmoothPath; using ClientGoalHandle = rclcpp_action::ClientGoalHandle; diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index 7b124f9764..bcbff2ae7c 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -25,7 +25,7 @@ #include #include "rclcpp/rclcpp.hpp" #include "nav2_core/global_planner.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" From 9b74dd0457f20a4df89c9a0d9422a6479448bcee Mon Sep 17 00:00:00 2001 From: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com> Date: Mon, 24 Oct 2022 13:36:49 +0800 Subject: [PATCH 089/131] CostmapLayer::matchSize may be executed concurrently (#3250) * CostmapLayer::matchSize() add a mutex --- nav2_costmap_2d/src/costmap_layer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 3f9da4013d..ad488c2618 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -55,6 +55,7 @@ void CostmapLayer::touch( void CostmapLayer::matchSize() { + std::lock_guard guard(*getMutex()); Costmap2D * master = layered_costmap_->getCostmap(); resizeMap( master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), From 338e93e90615ee72ec8aa407146c922b26c3b59f Mon Sep 17 00:00:00 2001 From: jaeminSHIN <91681721+woawo1213@users.noreply.github.com> Date: Tue, 1 Nov 2022 13:45:31 +0900 Subject: [PATCH 090/131] Fix typo (#3262) --- nav2_constrained_smoother/README.md | 2 +- nav2_map_server/README.md | 4 ++-- nav2_simple_commander/README.md | 2 +- nav2_smac_planner/README.md | 2 +- nav2_theta_star_planner/README.md | 4 ++-- nav2_velocity_smoother/README.md | 4 ++-- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md index d37eb51b63..d34274cda2 100644 --- a/nav2_constrained_smoother/README.md +++ b/nav2_constrained_smoother/README.md @@ -28,7 +28,7 @@ smoother_server: # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification w_cost_cusp_multiplier: 3.0 # option to have higher weight during forward/reverse direction change which is often accompanied with dangerous rotations - cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight eqals w_cost*w_cost_cusp_multiplier) + cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight equals w_cost*w_cost_cusp_multiplier) # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...] # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots) diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index c0e7a2cf6a..94a6bd66af 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -18,7 +18,7 @@ Currently map server divides into tree parts: - `map_io` library `map_server` is responsible for loading the map from a file through command-line interface -or by using serice requests. +or by using service requests. `map_saver` saves the map into a file. Like `map_server`, it has an ability to save the map from command-line or by calling a service. @@ -27,7 +27,7 @@ command-line or by calling a service. in order to allow easily save/load map from external code just by calling necessary function. This library is also used by `map_loader` and `map_saver` to work. Currently it contains OccupancyGrid saving/loading functions moved from the rest part of map server code. -It is designed to be replaceble for a new IO library (e.g. for library with new map encoding +It is designed to be replaceable for a new IO library (e.g. for library with new map encoding method or any other library supporting costmaps, multifloor maps, etc...). ### CLI-usage diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index c4f398f77d..7af387eb02 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -92,7 +92,7 @@ This will bring up the robot in the AWS Warehouse in a reasonable position, laun ### Manually -The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstation. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment. +The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstration. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment. ``` bash # Terminal 1: launch your robot navigation and simulation (or physical robot). For example diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 0396664f8e..1faa85bf38 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -37,7 +37,7 @@ The `SmacPlannerHybrid` is designed to work with: The `SmacPlannerLattice` is designed to work with: - Arbitrary shaped, non-circular robots requiring kinematically feasible planning with SE2 collision checking using the full capabilities of the drivetrain -- Flexibility to use other robot model types or with provided non-circular differental, ackermann, and omni support +- Flexibility to use other robot model types or with provided non-circular differential, ackermann, and omni support The `SmacPlanner2D` is designed to work with: - Circular, differential or omnidirectional robots diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index b8e7729904..722957f841 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -74,9 +74,9 @@ This planner uses the costs associated with each cell from the `global_costmap` Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother. -`w_heuristic_cost` is an internal setting that is not user changable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent. +`w_heuristic_cost` is an internal setting that is not user changeable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent. -To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. +To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversely make the paths less taut. So it is recommended that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to. diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index bdd5527788..6ffbe5d372 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -3,7 +3,7 @@ The ``nav2_velocity_smoother`` is a package containing a lifecycle-component node for smoothing velocities sent by Nav2 to robot controllers. The aim of this package is to implement velocity, acceleration, and deadband smoothing from Nav2 to reduce wear-and-tear on robot motors and hardware controllers by smoothing out the accelerations/jerky movements that might be present with some local trajectory planners' control efforts. -It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some intepretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). +It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions. @@ -39,7 +39,7 @@ There are two primary operation modes: open and closed loop. In open-loop, the node assumes that the robot was able to achieve the velocity send to it in the last command which was smoothed (which should be a good assumption if acceleration limits are set properly). This is useful when robot odometry is not particularly accurate or has significant latency relative to `smoothing_frequency` so there isn't a delay in the feedback loop. In closed-loop, the node will read from the odometry topic and apply a smoother over it to obtain the robot's current speed. -This will be used to determine the robot's current velocity and therefore achivable velocity targets by the velocity, acceleration, and deadband constraints using live data. +This will be used to determine the robot's current velocity and therefore achievable velocity targets by the velocity, acceleration, and deadband constraints using live data. ## Parameters From a94fdc4958b66d4e5f4430546c2893677074ab76 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Nov 2022 20:33:42 -0700 Subject: [PATCH 091/131] Adding new Nav2 Smoother: Savitzky-Golay Smoother (#3264) * initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions --- nav2_smoother/CMakeLists.txt | 14 +- nav2_smoother/README.md | 2 + .../nav2_smoother/savitzky_golay_smoother.hpp | 108 ++++++ .../include/nav2_smoother/simple_smoother.hpp | 31 +- .../include/nav2_smoother/smoother_utils.hpp | 132 +++++++ nav2_smoother/plugins.xml | 6 + nav2_smoother/src/nav2_smoother.cpp | 1 + nav2_smoother/src/savitzky_golay_smoother.cpp | 198 +++++++++++ nav2_smoother/src/simple_smoother.cpp | 80 +---- nav2_smoother/test/CMakeLists.txt | 13 + .../test/test_savitzky_golay_smoother.cpp | 332 ++++++++++++++++++ nav2_smoother/test/test_simple_smoother.cpp | 1 + 12 files changed, 807 insertions(+), 111 deletions(-) create mode 100644 nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp create mode 100644 nav2_smoother/include/nav2_smoother/smoother_utils.hpp create mode 100644 nav2_smoother/src/savitzky_golay_smoother.cpp create mode 100644 nav2_smoother/test/test_savitzky_golay_smoother.cpp diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index 0df85f54bd..0ab7c83628 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -68,8 +68,18 @@ add_library(simple_smoother SHARED ament_target_dependencies(simple_smoother ${dependencies} ) + +# Savitzky Golay Smoother plugin +add_library(savitzky_golay_smoother SHARED + src/savitzky_golay_smoother.cpp +) +ament_target_dependencies(savitzky_golay_smoother + ${dependencies} +) + pluginlib_export_plugin_description_file(nav2_core plugins.xml) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -82,7 +92,7 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer") install( - TARGETS ${library_name} simple_smoother + TARGETS ${library_name} simple_smoother savitzky_golay_smoother ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -104,6 +114,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name} simple_smoother) +ament_export_libraries(${library_name} simple_smoother savitzky_golay_smoother) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_smoother/README.md b/nav2_smoother/README.md index da007a9096..c612dba43c 100644 --- a/nav2_smoother/README.md +++ b/nav2_smoother/README.md @@ -7,3 +7,5 @@ A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface i See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available smoother plugins. See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. + +This package contains the Simple Smoother and Savitzky-Golay Smoother plugins. diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp new file mode 100644 index 0000000000..090aa07af8 --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -0,0 +1,108 @@ +// Copyright (c) 2022, 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. Reserved. + +#ifndef NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ +#define NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" + +namespace nav2_smoother +{ + +/** + * @class nav2_smoother::SavitzkyGolaySmoother + * @brief A path smoother implementation using Savitzky Golay filters + */ +class SavitzkyGolaySmoother : public nav2_core::Smoother +{ +public: + /** + * @brief A constructor for nav2_smoother::SavitzkyGolaySmoother + */ + SavitzkyGolaySmoother() = default; + + /** + * @brief A destructor for nav2_smoother::SavitzkyGolaySmoother + */ + ~SavitzkyGolaySmoother() override = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string name, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) override; + + /** + * @brief Method to cleanup resources. + */ + void cleanup() override {} + + /** + * @brief Method to activate smoother and any threads involved in execution. + */ + void activate() override {} + + /** + * @brief Method to deactivate smoother and any threads involved in execution. + */ + void deactivate() override {} + + /** + * @brief Method to smooth given path + * + * @param path In-out path to be smoothed + * @param max_time Maximum duration smoothing should take + * @return If smoothing was completed (true) or interrupted by time limit (false) + */ + bool smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) override; + +protected: + /** + * @brief Smoother method - does the smoothing on a segment + * @param path Reference to path + * @param reversing_segment Return if this is a reversing segment + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment); + + bool do_refinement_; + int refinement_num_; + rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")}; +}; + +} // namespace nav2_smoother + +#endif // NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 4b841b3c09..95f19dc54c 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -24,6 +24,7 @@ #include #include "nav2_core/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" @@ -35,19 +36,6 @@ namespace nav2_smoother { -/** - * @class nav2_smoother::PathSegment - * @brief A segment of a path in start/end indices - */ -struct PathSegment -{ - unsigned int start; - unsigned int end; -}; - -typedef std::vector::iterator PathIterator; -typedef std::vector::reverse_iterator ReversePathIterator; - /** * @class nav2_smoother::SimpleSmoother * @brief A path smoother implementation @@ -132,23 +120,6 @@ class SimpleSmoother : public nav2_core::Smoother geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, const double & value); - /** - * @brief Finds the starting and end indices of path segments where - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param path Path in which to look for cusps - * @return Set of index pairs for each segment of the path in a given direction - */ - std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); - - /** - * @brief For a given path, update the path point orientations based on smoothing - * @param path Path to approximate the path orientation in - * @param reversing_segment Return if this is a reversing segment - */ - inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment); - double tolerance_, data_w_, smooth_w_; int max_its_, refinement_ctr_; bool do_refinement_; diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp new file mode 100644 index 0000000000..67d7296353 --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2022, 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. Reserved. + +#ifndef NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ +#define NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" + +namespace smoother_utils +{ + +/** + * @class nav2_smoother::PathSegment + * @brief A segment of a path in start/end indices + */ +struct PathSegment +{ + unsigned int start; + unsigned int end; +}; + +typedef std::vector::iterator PathIterator; +typedef std::vector::reverse_iterator ReversePathIterator; + +inline std::vector findDirectionalPathSegments( + const nav_msgs::msg::Path & path) +{ + std::vector segments; + PathSegment curr_segment; + curr_segment.start = 0; + + // Iterating through the path to determine the position of the cusp + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + double ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existance of cusp, in the path, using the dot product. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + + // Checking for the existance of a differential rotation in place. + double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); + double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); + double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); + if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + } + + curr_segment.end = path.poses.size() - 1; + segments.push_back(curr_segment); + return segments; +} + +inline void updateApproximatePathOrientations( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + double dx, dy, theta, pt_yaw; + reversing_segment = false; + + // Find if this path segment is in reverse + dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; + dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; + theta = atan2(dy, dx); + pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); + if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { + reversing_segment = true; + } + + // Find the angle relative the path position vectors + for (unsigned int i = 0; i != path.poses.size() - 1; i++) { + dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; + dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; + theta = atan2(dy, dx); + + // If points are overlapping, pass + if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { + continue; + } + + // Flip the angle if this path segment is in reverse + if (reversing_segment) { + theta += M_PI; // orientationAroundZAxis will normalize + } + + path.poses[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta); + } +} + +} // namespace smoother_utils + +#endif // NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ diff --git a/nav2_smoother/plugins.xml b/nav2_smoother/plugins.xml index c72f7c25c8..90960a51ef 100644 --- a/nav2_smoother/plugins.xml +++ b/nav2_smoother/plugins.xml @@ -4,4 +4,10 @@ Does a simple smoothing process with collision checking + + + + Does Savitzky-Golay smoothing + + diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 27e88f17b3..f8f48c9886 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2019 RoboTech Vision // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2022 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. diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp new file mode 100644 index 0000000000..6a61196ab3 --- /dev/null +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2022, 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. Reserved. + +#include +#include +#include "nav2_smoother/savitzky_golay_smoother.hpp" + +namespace nav2_smoother +{ + +using namespace smoother_utils; // NOLINT +using namespace nav2_util::geometry_utils; // NOLINT +using namespace std::chrono; // NOLINT +using nav2_util::declare_parameter_if_not_declared; + +void SavitzkyGolaySmoother::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr/*costmap_sub*/, + std::shared_ptr/*footprint_sub*/) +{ + auto node = parent.lock(); + logger_ = node->get_logger(); + + declare_parameter_if_not_declared( + node, name + ".do_refinement", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, name + ".refinement_num", rclcpp::ParameterValue(2)); + node->get_parameter(name + ".do_refinement", do_refinement_); + node->get_parameter(name + ".refinement_num", refinement_num_); +} + +bool SavitzkyGolaySmoother::smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) +{ + steady_clock::time_point start = steady_clock::now(); + double time_remaining = max_time.seconds(); + + bool success = true, reversing_segment; + nav_msgs::msg::Path curr_path_segment; + curr_path_segment.header = path.header; + + std::vector path_segments = findDirectionalPathSegments(path); + + for (unsigned int i = 0; i != path_segments.size(); i++) { + if (path_segments[i].end - path_segments[i].start > 9) { + // Populate path segment + curr_path_segment.poses.clear(); + std::copy( + path.poses.begin() + path_segments[i].start, + path.poses.begin() + path_segments[i].end + 1, + std::back_inserter(curr_path_segment.poses)); + + // Make sure we're still able to smooth with time remaining + steady_clock::time_point now = steady_clock::now(); + time_remaining = max_time.seconds() - duration_cast>(now - start).count(); + + if (time_remaining <= 0.0) { + RCLCPP_WARN( + logger_, + "Smoothing time exceeded allowed duration of %0.2f.", max_time.seconds()); + return false; + } + + // Smooth path segment + success = success && smoothImpl(curr_path_segment, reversing_segment); + + // Assemble the path changes to the main path + std::copy( + curr_path_segment.poses.begin(), + curr_path_segment.poses.end(), + path.poses.begin() + path_segments[i].start); + } + } + + return success; +} + +bool SavitzkyGolaySmoother::smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + // Must be at least 10 in length to enter function + const unsigned int & path_size = path.poses.size(); + + // 7-point SG filter + const std::array filter = { + -2.0 / 21.0, + 3.0 / 21.0, + 6.0 / 21.0, + 7.0 / 21.0, + 6.0 / 21.0, + 3.0 / 21.0, + -2.0 / 21.0}; + + auto applyFilter = [&](const std::vector & data) + -> geometry_msgs::msg::Point + { + geometry_msgs::msg::Point val; + for (unsigned int i = 0; i != filter.size(); i++) { + val.x += filter[i] * data[i].x; + val.y += filter[i] * data[i].y; + } + return val; + }; + + auto applyFilterOverAxes = + [&](std::vector & plan_pts) -> void + { + // Handle initial boundary conditions, first point is fixed + unsigned int idx = 1; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 1].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 2].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + + // Apply nominal filter + for (idx = 3; idx < path_size - 4; ++idx) { + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + } + + // Handle terminal boundary conditions, last point is fixed + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 2].pose.position}); + + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 1].pose.position}); + }; + + applyFilterOverAxes(path.poses); + + // Lets do additional refinement, it shouldn't take more than a couple milliseconds + if (do_refinement_) { + for (int i = 0; i < refinement_num_; i++) { + applyFilterOverAxes(path.poses); + } + } + + updateApproximatePathOrientations(path, reversing_segment); + return true; +} + +} // namespace nav2_smoother + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_smoother::SavitzkyGolaySmoother, nav2_core::Smoother) diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 59a81fd3b1..d04d88bc61 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -18,6 +18,7 @@ namespace nav2_smoother { +using namespace smoother_utils; // NOLINT using namespace nav2_util::geometry_utils; // NOLINT using namespace std::chrono; // NOLINT using nav2_util::declare_parameter_if_not_declared; @@ -214,85 +215,6 @@ void SimpleSmoother::setFieldByDim( } } -std::vector SimpleSmoother::findDirectionalPathSegments( - const nav_msgs::msg::Path & path) -{ - std::vector segments; - PathSegment curr_segment; - curr_segment.start = 0; - - // Iterating through the path to determine the position of the cusp - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - double oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - double ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - double ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existance of cusp, in the path, using the dot product. - double dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - - // Checking for the existance of a differential rotation in place. - double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); - double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); - double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); - if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - } - - curr_segment.end = path.poses.size() - 1; - segments.push_back(curr_segment); - return segments; -} - -void SimpleSmoother::updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment) -{ - double dx, dy, theta, pt_yaw; - reversing_segment = false; - - // Find if this path segment is in reverse - dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; - dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; - theta = atan2(dy, dx); - pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); - if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { - reversing_segment = true; - } - - // Find the angle relative the path position vectors - for (unsigned int i = 0; i != path.poses.size() - 1; i++) { - dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; - dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; - theta = atan2(dy, dx); - - // If points are overlapping, pass - if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { - continue; - } - - // Flip the angle if this path segment is in reverse - if (reversing_segment) { - theta += M_PI; // orientationAroundZAxis will normalize - } - - path.poses[i].pose.orientation = orientationAroundZAxis(theta); - } -} - } // namespace nav2_smoother #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_smoother/test/CMakeLists.txt b/nav2_smoother/test/CMakeLists.txt index 67c98e3e43..623447b5f9 100644 --- a/nav2_smoother/test/CMakeLists.txt +++ b/nav2_smoother/test/CMakeLists.txt @@ -21,3 +21,16 @@ target_link_libraries(test_simple_smoother ament_target_dependencies(test_simple_smoother ${dependencies} ) + + +ament_add_gtest(test_savitzky_golay_smoother + test_savitzky_golay_smoother.cpp +) + +target_link_libraries(test_savitzky_golay_smoother + savitzky_golay_smoother +) + +ament_target_dependencies(test_savitzky_golay_smoother + ${dependencies} +) diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp new file mode 100644 index 0000000000..58860d67db --- /dev/null +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -0,0 +1,332 @@ +// Copyright (c) 2022, 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. Reserved. + +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smoother/savitzky_golay_smoother.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +using namespace smoother_utils; // NOLINT +using namespace nav2_smoother; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(SmootherTest, test_sg_smoother_basics) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + smoother->activate(); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Test regular path, should see no effective change + nav_msgs::msg::Path straight_regular_path, straight_regular_path_baseline; + straight_regular_path.header.frame_id = "map"; + straight_regular_path.header.stamp = node->now(); + straight_regular_path.poses.resize(11); + straight_regular_path.poses[0].pose.position.x = 0.5; + straight_regular_path.poses[0].pose.position.y = 0.1; + straight_regular_path.poses[1].pose.position.x = 0.5; + straight_regular_path.poses[1].pose.position.y = 0.2; + straight_regular_path.poses[2].pose.position.x = 0.5; + straight_regular_path.poses[2].pose.position.y = 0.3; + straight_regular_path.poses[3].pose.position.x = 0.5; + straight_regular_path.poses[3].pose.position.y = 0.4; + straight_regular_path.poses[4].pose.position.x = 0.5; + straight_regular_path.poses[4].pose.position.y = 0.5; + straight_regular_path.poses[5].pose.position.x = 0.5; + straight_regular_path.poses[5].pose.position.y = 0.6; + straight_regular_path.poses[6].pose.position.x = 0.5; + straight_regular_path.poses[6].pose.position.y = 0.7; + straight_regular_path.poses[7].pose.position.x = 0.5; + straight_regular_path.poses[7].pose.position.y = 0.8; + straight_regular_path.poses[8].pose.position.x = 0.5; + straight_regular_path.poses[8].pose.position.y = 0.9; + straight_regular_path.poses[9].pose.position.x = 0.5; + straight_regular_path.poses[9].pose.position.y = 1.0; + straight_regular_path.poses[10].pose.position.x = 0.5; + straight_regular_path.poses[10].pose.position.y = 1.1; + straight_regular_path_baseline = straight_regular_path; + + EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time)); + for (uint i = 0; i != straight_regular_path.poses.size() - 1; i++) { + // Check distances are still the same + EXPECT_NEAR( + fabs( + straight_regular_path.poses[i].pose.position.y - + straight_regular_path_baseline.poses[i].pose.position.y), 0.0, 0.011); + } + + // Attempt smoothing with no time given, should fail + rclcpp::Duration no_time = rclcpp::Duration::from_seconds(-1.0); // 0 seconds + EXPECT_FALSE(smoother->smooth(straight_regular_path, no_time)); + + smoother->deactivate(); + smoother->cleanup(); +} + +TEST(SmootherTest, test_sg_smoother_noisey_path) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Given nominal irregular/noisey path, test that the output is shorter and smoother + nav_msgs::msg::Path noisey_path, noisey_path_baseline; + noisey_path.header.frame_id = "map"; + noisey_path.header.stamp = node->now(); + noisey_path.poses.resize(11); + noisey_path.poses[0].pose.position.x = 0.5; + noisey_path.poses[0].pose.position.y = 0.1; + noisey_path.poses[1].pose.position.x = 0.5; + noisey_path.poses[1].pose.position.y = 0.2; + noisey_path.poses[2].pose.position.x = 0.5; + noisey_path.poses[2].pose.position.y = 0.3; + noisey_path.poses[3].pose.position.x = 0.5; + noisey_path.poses[3].pose.position.y = 0.4; + noisey_path.poses[4].pose.position.x = 0.5; + noisey_path.poses[4].pose.position.y = 0.5; + noisey_path.poses[5].pose.position.x = 0.5; + noisey_path.poses[5].pose.position.y = 0.6; + noisey_path.poses[6].pose.position.x = 0.5; + noisey_path.poses[6].pose.position.y = 0.7; + noisey_path.poses[7].pose.position.x = 0.5; + noisey_path.poses[7].pose.position.y = 0.8; + noisey_path.poses[8].pose.position.x = 0.5; + noisey_path.poses[8].pose.position.y = 0.9; + noisey_path.poses[9].pose.position.x = 0.5; + noisey_path.poses[9].pose.position.y = 1.0; + noisey_path.poses[10].pose.position.x = 0.5; + noisey_path.poses[10].pose.position.y = 1.1; + + // Add random but deterministic noises + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> normal_distribution{0.0, 0.02}; + for (unsigned int i = 0; i != noisey_path.poses.size(); i++) { + auto noise = normal_distribution(gen); + noisey_path.poses[i].pose.position.x += noise; + } + + noisey_path_baseline = noisey_path; + EXPECT_TRUE(smoother->smooth(noisey_path, max_time)); + + // Compute metric, should be shorter if smoother + double length = 0; + double base_length = 0; + for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) { + length += std::hypot( + noisey_path.poses[i + 1].pose.position.x - noisey_path.poses[i].pose.position.x, + noisey_path.poses[i + 1].pose.position.y - noisey_path.poses[i].pose.position.y); + base_length += std::hypot( + noisey_path_baseline.poses[i + 1].pose.position.x - + noisey_path_baseline.poses[i].pose.position.x, + noisey_path_baseline.poses[i + 1].pose.position.y - + noisey_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); + + // Test again with refinement, even shorter and smoother + node->set_parameter(rclcpp::Parameter("test.do_refinement", rclcpp::ParameterValue(true))); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + nav_msgs::msg::Path noisey_path_refined = noisey_path_baseline; + EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time)); + + length = 0; + double non_refined_length = 0; + for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) { + length += std::hypot( + noisey_path_refined.poses[i + 1].pose.position.x - + noisey_path_refined.poses[i].pose.position.x, + noisey_path_refined.poses[i + 1].pose.position.y - + noisey_path_refined.poses[i].pose.position.y); + non_refined_length += std::hypot( + noisey_path.poses[i + 1].pose.position.x - noisey_path_baseline.poses[i].pose.position.x, + noisey_path.poses[i + 1].pose.position.y - noisey_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); +} + +TEST(SmootherTest, test_sg_smoother_reversing) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Test reversing / multiple segments via a cusp + nav_msgs::msg::Path cusp_path, cusp_path_baseline; + cusp_path.header.frame_id = "map"; + cusp_path.header.stamp = node->now(); + cusp_path.poses.resize(22); + cusp_path.poses[0].pose.position.x = 0.5; + cusp_path.poses[0].pose.position.y = 0.1; + cusp_path.poses[1].pose.position.x = 0.5; + cusp_path.poses[1].pose.position.y = 0.2; + cusp_path.poses[2].pose.position.x = 0.5; + cusp_path.poses[2].pose.position.y = 0.3; + cusp_path.poses[3].pose.position.x = 0.5; + cusp_path.poses[3].pose.position.y = 0.4; + cusp_path.poses[4].pose.position.x = 0.5; + cusp_path.poses[4].pose.position.y = 0.5; + cusp_path.poses[5].pose.position.x = 0.5; + cusp_path.poses[5].pose.position.y = 0.6; + cusp_path.poses[6].pose.position.x = 0.5; + cusp_path.poses[6].pose.position.y = 0.7; + cusp_path.poses[7].pose.position.x = 0.5; + cusp_path.poses[7].pose.position.y = 0.8; + cusp_path.poses[8].pose.position.x = 0.5; + cusp_path.poses[8].pose.position.y = 0.9; + cusp_path.poses[9].pose.position.x = 0.5; + cusp_path.poses[9].pose.position.y = 1.0; + cusp_path.poses[10].pose.position.x = 0.5; + cusp_path.poses[10].pose.position.y = 1.1; + cusp_path.poses[11].pose.position.x = 0.5; + cusp_path.poses[11].pose.position.y = 1.0; + cusp_path.poses[12].pose.position.x = 0.5; + cusp_path.poses[12].pose.position.y = 0.9; + cusp_path.poses[13].pose.position.x = 0.5; + cusp_path.poses[13].pose.position.y = 0.8; + cusp_path.poses[14].pose.position.x = 0.5; + cusp_path.poses[14].pose.position.y = 0.7; + cusp_path.poses[15].pose.position.x = 0.5; + cusp_path.poses[15].pose.position.y = 0.6; + cusp_path.poses[16].pose.position.x = 0.5; + cusp_path.poses[16].pose.position.y = 0.5; + cusp_path.poses[17].pose.position.x = 0.5; + cusp_path.poses[17].pose.position.y = 0.4; + cusp_path.poses[18].pose.position.x = 0.5; + cusp_path.poses[18].pose.position.y = 0.3; + cusp_path.poses[19].pose.position.x = 0.5; + cusp_path.poses[19].pose.position.y = 0.2; + cusp_path.poses[20].pose.position.x = 0.5; + cusp_path.poses[20].pose.position.y = 0.1; + cusp_path.poses[21].pose.position.x = 0.5; + cusp_path.poses[21].pose.position.y = 0.0; + + // Add random but deterministic noises + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> normal_distribution{0.0, 0.02}; + for (unsigned int i = 0; i != cusp_path.poses.size(); i++) { + auto noise = normal_distribution(gen); + cusp_path.poses[i].pose.position.x += noise; + } + + cusp_path_baseline = cusp_path; + + EXPECT_TRUE(smoother->smooth(cusp_path, max_time)); + + // If it detected the cusp, the cusp point should be fixed + EXPECT_EQ(cusp_path.poses[10].pose.position.x, cusp_path_baseline.poses[10].pose.position.x); + EXPECT_EQ(cusp_path.poses[10].pose.position.y, cusp_path_baseline.poses[10].pose.position.y); + + // But the path also should be smoother / shorter + double length = 0; + double base_length = 0; + for (unsigned int i = 0; i != cusp_path.poses.size() - 1; i++) { + length += std::hypot( + cusp_path.poses[i + 1].pose.position.x - cusp_path.poses[i].pose.position.x, + cusp_path.poses[i + 1].pose.position.y - cusp_path.poses[i].pose.position.y); + base_length += std::hypot( + cusp_path_baseline.poses[i + 1].pose.position.x - + cusp_path_baseline.poses[i].pose.position.x, + cusp_path_baseline.poses[i + 1].pose.position.y - + cusp_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); +} diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index fccc2c2a7c..7d9b56d457 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -28,6 +28,7 @@ #include "nav2_smoother/simple_smoother.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +using namespace smoother_utils; // NOLINT using namespace nav2_smoother; // NOLINT using namespace std::chrono_literals; // NOLINT From c8053e3e7411f73323d64ecb88d30cd16d5e3557 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Nov 2022 22:50:36 -0700 Subject: [PATCH 092/131] refactoring RPP a bit for cleanliness on way to ROSCon (#3265) * refactor for RPP on way to ROSCon * fixing header * fixing header * fixing header * fix edge cases test samplings * linting --- .../CMakeLists.txt | 5 +- .../collision_checker.hpp | 105 +++ .../parameter_handler.hpp | 104 +++ .../path_handler.hpp | 99 +++ .../regulated_pure_pursuit_controller.hpp | 123 +--- .../regulation_functions.hpp | 138 ++++ .../src/collision_checker.cpp | 174 +++++ .../src/parameter_handler.cpp | 261 ++++++++ .../src/path_handler.cpp | 137 ++++ .../src/regulated_pure_pursuit_controller.cpp | 600 ++---------------- .../test/test_regulated_pp.cpp | 51 +- 11 files changed, 1131 insertions(+), 666 deletions(-) create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp create mode 100644 nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp create mode 100644 nav2_regulated_pure_pursuit_controller/src/path_handler.cpp diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 6c17481925..6f84b05382 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -35,7 +35,10 @@ set(dependencies set(library_name nav2_regulated_pure_pursuit_controller) add_library(${library_name} SHARED - src/regulated_pure_pursuit_controller.cpp) + src/regulated_pure_pursuit_controller.cpp + src/collision_checker.cpp + src/parameter_handler.cpp + src/path_handler.cpp) ament_target_dependencies(${library_name} ${dependencies} diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp new file mode 100644 index 0000000000..baccc6ca20 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" + +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::CollisionChecker + * @brief Checks for collision based on a RPP control command + */ +class CollisionChecker +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::CollisionChecker + */ + CollisionChecker( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_ros, Parameters * params); + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::CollisionChecker + */ + ~CollisionChecker() = default; + + /** + * @brief Whether collision is imminent + * @param robot_pose Pose of robot + * @param carrot_pose Pose of carrot + * @param linear_vel linear velocity to forward project + * @param angular_vel angular velocity to forward project + * @param carrot_dist Distance to the carrot for PP + * @return Whether collision is imminent + */ + bool isCollisionImminent( + const geometry_msgs::msg::PoseStamped &, + const double &, const double &, + const double &); + + /** + * @brief checks for collision at projected pose + * @param x Pose of pose x + * @param y Pose of pose y + * @param theta orientation of Yaw + * @return Whether in collision + */ + bool inCollision( + const double & x, + const double & y, + const double & theta); + + /** + * @brief Cost at a point + * @param x Pose of pose x + * @param y Pose of pose y + * @return Cost of pose in costmap + */ + double costAtPose(const double & x, const double & y); + +protected: + rclcpp::Logger logger_ {rclcpp::get_logger("RPPCollisionChecker")}; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + std::unique_ptr> + footprint_collision_checker_; + Parameters * params_; + std::shared_ptr> carrot_arc_pub_; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp new file mode 100644 index 0000000000..63e1d215e4 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PARAMETER_HANDLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +struct Parameters +{ + double desired_linear_vel, base_desired_linear_vel; + double lookahead_dist; + double rotate_to_heading_angular_vel; + double max_lookahead_dist; + double min_lookahead_dist; + double lookahead_time; + bool use_velocity_scaled_lookahead_dist; + double min_approach_linear_velocity; + double approach_velocity_scaling_dist; + double max_allowed_time_to_collision_up_to_carrot; + bool use_regulated_linear_velocity_scaling; + bool use_cost_regulated_linear_velocity_scaling; + double cost_scaling_dist; + double cost_scaling_gain; + double inflation_cost_scaling_factor; + double regulated_linear_scaling_min_radius; + double regulated_linear_scaling_min_speed; + bool use_rotate_to_heading; + double max_angular_accel; + double rotate_to_heading_min_angle; + bool allow_reversing; + double max_robot_pose_search_dist; + bool use_interpolation; + bool use_collision_detection; + double transform_tolerance; +}; + +/** + * @class nav2_regulated_pure_pursuit_controller::ParameterHandler + * @brief Handles parameters and dynamic parameters for RPP + */ +class ParameterHandler +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::ParameterHandler + */ + ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, + rclcpp::Logger & logger, const double costmap_size_x); + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::ParameterHandler + */ + ~ParameterHandler() = default; + + std::mutex & getMutex() {return mutex_;} + + Parameters * getParams() {return ¶ms_;} + +protected: + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + Parameters params_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PARAMETER_HANDLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp new file mode 100644 index 0000000000..22bc0266a6 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::PathHandler + * @brief Handles input paths to transform them to local frames required + */ +class PathHandler +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler + */ + PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros); + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::PathHandler + */ + ~PathHandler() = default; + + /** + * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint + * Points ineligible to be selected as a lookahead point if they are any of the following: + * - Outside the local_costmap (collision avoidance cannot be assured) + * @param pose pose to transform + * @param max_robot_pose_search_dist Distance to search for matching nearest path point + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist); + + /** + * @brief Transform a pose to another frame. + * @param frame Frame ID to transform to + * @param in_pose Pose input to transform + * @param out_pose transformed output + * @return bool if successful + */ + bool transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const; + + void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;} + + nav_msgs::msg::Path getPlan() {return global_plan_;} + +protected: + /** + * Get the greatest extent of the costmap in meters from the center. + * @return max of distance from center in meters to edge of costmap + */ + double getCostmapMaxExtent() const; + + rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")}; + tf2::Duration transform_tolerance_; + std::shared_ptr tf_; + std::shared_ptr costmap_ros_; + nav_msgs::msg::Path global_plan_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7c244d9393..7b28ca9720 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -22,14 +22,15 @@ #include #include -#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_core/controller.hpp" #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_util/odometry_utils.hpp" -#include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_regulated_pure_pursuit_controller/path_handler.hpp" +#include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" +#include "nav2_regulated_pure_pursuit_controller/regulation_functions.hpp" namespace nav2_regulated_pure_pursuit_controller { @@ -111,28 +112,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller void setSpeedLimit(const double & speed_limit, const bool & percentage) override; protected: - /** - * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint - * Points ineligible to be selected as a lookahead point if they are any of the following: - * - Outside the local_costmap (collision avoidance cannot be assured) - * @param pose pose to transform - * @return Path in new frame - */ - nav_msgs::msg::Path transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose); - - /** - * @brief Transform a pose to another frame. - * @param frame Frame ID to transform to - * @param in_pose Pose input to transform - * @param out_pose transformed output - * @return bool if successful - */ - bool transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const; - /** * @brief Get lookahead distance * @param cmd the current speed to use to compute lookahead point @@ -175,48 +154,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller double & linear_vel, double & angular_vel, const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); - /** - * @brief Whether collision is imminent - * @param robot_pose Pose of robot - * @param carrot_pose Pose of carrot - * @param linear_vel linear velocity to forward project - * @param angular_vel angular velocity to forward project - * @param carrot_dist Distance to the carrot for PP - * @return Whether collision is imminent - */ - bool isCollisionImminent( - const geometry_msgs::msg::PoseStamped &, - const double &, const double &, - const double &); - - /** - * @brief checks for collision at projected pose - * @param x Pose of pose x - * @param y Pose of pose y - * @param theta orientation of Yaw - * @return Whether in collision - */ - bool inCollision( - const double & x, - const double & y, - const double & theta); - /** - * @brief Cost at a point - * @param x Pose of pose x - * @param y Pose of pose y - * @return Cost of pose in costmap - */ - double costAtPose(const double & x, const double & y); - - double approachVelocityScalingFactor( - const nav_msgs::msg::Path & path - ) const; - - void applyApproachVelocityScaling( - const nav_msgs::msg::Path & path, - double & linear_vel - ) const; - /** * @brief apply regulation constraints to the system * @param linear_vel robot command linear velocity input @@ -259,66 +196,24 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); - /** - * Get the greatest extent of the costmap in meters from the center. - * @return max of distance from center in meters to edge of costmap - */ - double getCostmapMaxExtent() const; - - /** - * @brief Callback executed when a parameter change is detected - * @param event ParameterEvent message - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); - rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; std::string plugin_name_; std::shared_ptr costmap_ros_; nav2_costmap_2d::Costmap2D * costmap_; rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; - rclcpp::Clock::SharedPtr clock_; - double desired_linear_vel_, base_desired_linear_vel_; - double lookahead_dist_; - double rotate_to_heading_angular_vel_; - double max_lookahead_dist_; - double min_lookahead_dist_; - double lookahead_time_; - bool use_velocity_scaled_lookahead_dist_; - tf2::Duration transform_tolerance_; - double min_approach_linear_velocity_; - double approach_velocity_scaling_dist_; - double control_duration_; - double max_allowed_time_to_collision_up_to_carrot_; - bool use_collision_detection_; - bool use_regulated_linear_velocity_scaling_; - bool use_cost_regulated_linear_velocity_scaling_; - double cost_scaling_dist_; - double cost_scaling_gain_; - double inflation_cost_scaling_factor_; - double regulated_linear_scaling_min_radius_; - double regulated_linear_scaling_min_speed_; - bool use_rotate_to_heading_; - double max_angular_accel_; - double rotate_to_heading_min_angle_; + Parameters * params_; double goal_dist_tol_; - bool allow_reversing_; - double max_robot_pose_search_dist_; - bool use_interpolation_; + double control_duration_; - nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; std::shared_ptr> carrot_arc_pub_; - std::unique_ptr> - collision_checker_; - - // Dynamic parameters handler - std::mutex mutex_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::unique_ptr path_handler_; + std::unique_ptr param_handler_; + std::unique_ptr collision_checker_; }; } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp new file mode 100644 index 0000000000..61dca3487e --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp @@ -0,0 +1,138 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATION_FUNCTIONS_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATION_FUNCTIONS_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +namespace heuristics +{ + +/** + * @brief apply curvature constraint regulation on the linear velocity + * @param raw_linear_velocity Raw linear velocity desired + * @param curvature Curvature of the current command to follow the path + * @param min_radius Minimum path radius to apply the heuristic + * @return Velocity after applying the curvature constraint + */ +inline double curvatureConstraint( + const double raw_linear_vel, const double curvature, const double min_radius) +{ + const double radius = fabs(1.0 / curvature); + if (radius < min_radius) { + return raw_linear_vel * (1.0 - (fabs(radius - min_radius) / min_radius)); + } else { + return raw_linear_vel; + } +} + +/** + * @brief apply cost constraint regulation on the linear velocity + * @param raw_linear_velocity Raw linear velocity desired + * @param pose_cost Cost at the robot pose + * @param costmap_ros Costmap object to query + * @param params Parameters + * @return Velocity after applying the curvature constraint + */ +inline double costConstraint( + const double raw_linear_vel, + const double pose_cost, + std::shared_ptr costmap_ros, + Parameters * params) +{ + using namespace nav2_costmap_2d; // NOLINT + + if (pose_cost != static_cast(NO_INFORMATION) && + pose_cost != static_cast(FREE_SPACE)) + { + const double & inscribed_radius = costmap_ros->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / params->inflation_cost_scaling_factor) * + std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; + + if (min_distance_to_obstacle < params->cost_scaling_dist) { + return raw_linear_vel * + (params->cost_scaling_gain * min_distance_to_obstacle / params->cost_scaling_dist); + } + } + + return raw_linear_vel; +} + +/** + * @brief Compute the scale factor to apply for linear velocity regulation on approach to goal + * @param transformed_path Path to use to calculate distances to goal + * @param approach_velocity_scaling_dist Minimum distance away to which to apply the heuristic + * @return A scale from 0.0-1.0 of the distance to goal scaled by minimum distance + */ +inline double approachVelocityScalingFactor( + const nav_msgs::msg::Path & transformed_path, + const double approach_velocity_scaling_dist) +{ + using namespace nav2_util::geometry_utils; // NOLINT + + // Waiting to apply the threshold based on integrated distance ensures we don't + // erroneously apply approach scaling on curvy paths that are contained in a large local costmap. + const double remaining_distance = calculate_path_length(transformed_path); + if (remaining_distance < approach_velocity_scaling_dist) { + auto & last = transformed_path.poses.back(); + // Here we will use a regular euclidean distance from the robot frame (origin) + // to get smooth scaling, regardless of path density. + return std::hypot(last.pose.position.x, last.pose.position.y) / approach_velocity_scaling_dist; + } else { + return 1.0; + } +} + +/** + * @brief Velocity on approach to goal heuristic regulation term + * @param constrained_linear_vel Linear velocity already constrained by heuristics + * @param path The path plan in the robot base frame coordinates + * @param min_approach_velocity Minimum velocity to use on approach to goal + * @param approach_velocity_scaling_dist Distance away from goal to start applying this heuristic + * @return Velocity after regulation via approach to goal slow-down + */ +inline double approachVelocityConstraint( + const double constrained_linear_vel, + const nav_msgs::msg::Path & path, + const double min_approach_velocity, + const double approach_velocity_scaling_dist) +{ + double velocity_scaling = approachVelocityScalingFactor(path, approach_velocity_scaling_dist); + double approach_vel = constrained_linear_vel * velocity_scaling; + + if (approach_vel < min_approach_velocity) { + approach_vel = min_approach_velocity; + } + + return std::min(constrained_linear_vel, approach_vel); +} + +} // namespace heuristics + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATION_FUNCTIONS_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp new file mode 100644 index 0000000000..ac654fcab7 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -0,0 +1,174 @@ +// Copyright (c) 2022 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 +#include + +#include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +using namespace nav2_costmap_2d; // NOLINT + +CollisionChecker::CollisionChecker( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_ros, + Parameters * params) +{ + clock_ = node->get_clock(); + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + params_ = params; + + // initialize collision checker and set costmap + footprint_collision_checker_ = std::make_unique>(costmap_); + footprint_collision_checker_->setCostmap(costmap_); + + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); + carrot_arc_pub_->on_activate(); +} + +bool CollisionChecker::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const double & linear_vel, const double & angular_vel, + const double & carrot_dist) +{ + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // odom frame and the carrot_pose is in robot base frame. Just how the data comes to us + + // check current point is OK + if (inCollision( + robot_pose.pose.position.x, robot_pose.pose.position.y, + tf2::getYaw(robot_pose.pose.orientation))) + { + return true; + } + + // visualization messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = robot_pose.header.stamp; + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = arc_pts_msg.header.frame_id; + pose_msg.header.stamp = arc_pts_msg.header.stamp; + + double projection_time = 0.0; + if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) { + // rotating to heading at goal or toward path + // Equation finds the angular distance required for the largest + // part of the robot radius to move to another costmap cell: + // theta_min = 2.0 * sin ((res/2) / r_max) + // via isosceles triangle r_max-r_max-resolution, + // dividing by angular_velocity gives us a timestep. + double max_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); + projection_time = + 2.0 * sin((costmap_->getResolution() / 2) / max_radius) / fabs(angular_vel); + } else { + // Normal path tracking + projection_time = costmap_->getResolution() / fabs(linear_vel); + } + + const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position; + geometry_msgs::msg::Pose2D curr_pose; + curr_pose.x = robot_pose.pose.position.x; + curr_pose.y = robot_pose.pose.position.y; + curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + + // only forward simulate within time requested + int i = 1; + while (i * projection_time < params_->max_allowed_time_to_collision_up_to_carrot) { + i++; + + // apply velocity at curr_pose over distance + curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += projection_time * angular_vel; + + // check if past carrot pose, where no longer a thoughtfully valid command + if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) { + break; + } + + // store it for visualization + pose_msg.pose.position.x = curr_pose.x; + pose_msg.pose.position.y = curr_pose.y; + pose_msg.pose.position.z = 0.01; + arc_pts_msg.poses.push_back(pose_msg); + + // check for collision at the projected pose + if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { + carrot_arc_pub_->publish(arc_pts_msg); + return true; + } + } + + carrot_arc_pub_->publish(arc_pts_msg); + + return false; +} + +bool CollisionChecker::inCollision( + const double & x, + const double & y, + const double & theta) +{ + unsigned int mx, my; + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 30000, + "The dimensions of the costmap is too small to successfully check for " + "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " + "increase your costmap size."); + return false; + } + + double footprint_cost = footprint_collision_checker_->footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint()); + if (footprint_cost == static_cast(NO_INFORMATION) && + costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) + { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost >= static_cast(LETHAL_OBSTACLE); +} + + +double CollisionChecker::costAtPose(const double & x, const double & y) +{ + unsigned int mx, my; + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_FATAL( + logger_, + "The dimensions of the costmap is too small to fully include your robot's footprint, " + "thusly the robot cannot proceed further"); + throw nav2_core::ControllerException( + "RegulatedPurePursuitController: Dimensions of the costmap are too small " + "to encapsulate the robot footprint at current speeds!"); + } + + unsigned char cost = costmap_->getCost(mx, my); + return static_cast(cost); +} + +} // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp new file mode 100644 index 0000000000..9bb4aa9427 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -0,0 +1,261 @@ +// Copyright (c) 2022 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 +#include + +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; + +ParameterHandler::ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, rclcpp::Logger & logger, + const double costmap_size_x) +{ + plugin_name_ = plugin_name; + logger_ = logger; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(costmap_size_x / 2.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_interpolation", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_collision_detection", + rclcpp::ParameterValue(true)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); + params_.base_desired_linear_vel = params_.desired_linear_vel; + node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist); + node->get_parameter(plugin_name_ + ".lookahead_time", params_.lookahead_time); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_angular_vel", + params_.rotate_to_heading_angular_vel); + node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); + node->get_parameter( + plugin_name_ + ".use_velocity_scaled_lookahead_dist", + params_.use_velocity_scaled_lookahead_dist); + node->get_parameter( + plugin_name_ + ".min_approach_linear_velocity", + params_.min_approach_linear_velocity); + node->get_parameter( + plugin_name_ + ".approach_velocity_scaling_dist", + params_.approach_velocity_scaling_dist); + if (params_.approach_velocity_scaling_dist > costmap_size_x / 2.0) { + RCLCPP_WARN( + logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " + "leading to permanent slowdown"); + } + node->get_parameter( + plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + params_.max_allowed_time_to_collision_up_to_carrot); + node->get_parameter( + plugin_name_ + ".use_regulated_linear_velocity_scaling", + params_.use_regulated_linear_velocity_scaling); + node->get_parameter( + plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + params_.use_cost_regulated_linear_velocity_scaling); + node->get_parameter(plugin_name_ + ".cost_scaling_dist", params_.cost_scaling_dist); + node->get_parameter(plugin_name_ + ".cost_scaling_gain", params_.cost_scaling_gain); + node->get_parameter( + plugin_name_ + ".inflation_cost_scaling_factor", + params_.inflation_cost_scaling_factor); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_radius", + params_.regulated_linear_scaling_min_radius); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_speed", + params_.regulated_linear_scaling_min_speed); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); + node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); + node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); + node->get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", + params_.max_robot_pose_search_dist); + node->get_parameter( + plugin_name_ + ".use_interpolation", + params_.use_interpolation); + node->get_parameter( + plugin_name_ + ".use_collision_detection", + params_.use_collision_detection); + + if (params_.inflation_cost_scaling_factor <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + params_.use_cost_regulated_linear_velocity_scaling = false; + } + + /** Possible to drive in reverse direction if and only if + "use_rotate_to_heading" parameter is set to false **/ + + if (params_.use_rotate_to_heading && params_.allow_reversing) { + RCLCPP_WARN( + logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. By default setting use_rotate_to_heading true"); + params_.allow_reversing = false; + } + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &ParameterHandler::dynamicParametersCallback, + this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult +ParameterHandler::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(mutex_); + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".inflation_cost_scaling_factor") { + if (parameter.as_double() <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Ignoring parameter update."); + continue; + } + params_.inflation_cost_scaling_factor = parameter.as_double(); + } else if (name == plugin_name_ + ".desired_linear_vel") { + params_.desired_linear_vel = parameter.as_double(); + params_.base_desired_linear_vel = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_dist") { + params_.lookahead_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".max_lookahead_dist") { + params_.max_lookahead_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".min_lookahead_dist") { + params_.min_lookahead_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_time") { + params_.lookahead_time = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + params_.rotate_to_heading_angular_vel = parameter.as_double(); + } else if (name == plugin_name_ + ".min_approach_linear_velocity") { + params_.min_approach_linear_velocity = parameter.as_double(); + } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { + params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_dist") { + params_.cost_scaling_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_gain") { + params_.cost_scaling_gain = parameter.as_double(); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { + params_.regulated_linear_scaling_min_radius = parameter.as_double(); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { + params_.regulated_linear_scaling_min_speed = parameter.as_double(); + } else if (name == plugin_name_ + ".max_angular_accel") { + params_.max_angular_accel = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { + params_.rotate_to_heading_min_angle = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { + params_.use_velocity_scaled_lookahead_dist = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { + params_.use_regulated_linear_velocity_scaling = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { + params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_collision_detection") { + params_.use_collision_detection = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_rotate_to_heading") { + if (parameter.as_bool() && params_.allow_reversing) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + params_.use_rotate_to_heading = parameter.as_bool(); + } else if (name == plugin_name_ + ".allow_reversing") { + if (params_.use_rotate_to_heading && parameter.as_bool()) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + params_.allow_reversing = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp new file mode 100644 index 0000000000..f4ceec759d --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -0,0 +1,137 @@ +// Copyright (c) 2022 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 +#include + +#include "nav2_regulated_pure_pursuit_controller/path_handler.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +using nav2_util::geometry_utils::euclidean_distance; + +PathHandler::PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros) +: transform_tolerance_(transform_tolerance), tf_(tf), costmap_ros_(costmap_ros) +{ +} + +double PathHandler::getCostmapMaxExtent() const +{ + const double max_costmap_dim_meters = std::max( + costmap_ros_->getCostmap()->getSizeInMetersX(), + costmap_ros_->getCostmap()->getSizeInMetersY()); + return max_costmap_dim_meters / 2.0; +} + +nav_msgs::msg::Path PathHandler::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist) +{ + if (global_plan_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); + } + + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist); + + // First find the closest pose on the path to the robot + // bounded by when the path turns around (if it does) so we don't get a pose from a later + // portion of the path + auto transformation_begin = + nav2_util::geometry_utils::min_by( + global_plan_.poses.begin(), closest_pose_upper_bound, + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // We'll discard points on the plan that are outside the local costmap + const double max_costmap_extent = getCostmapMaxExtent(); + auto transformation_end = std::find_if( + transformation_begin, global_plan_.poses.end(), + [&](const auto & pose) { + return euclidean_distance(pose, robot_pose) > max_costmap_extent; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { + throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); + } + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +bool PathHandler::transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); + } + return false; +} + +} // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index d5c648bd1a..674a456c57 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -30,10 +30,7 @@ using std::hypot; using std::min; using std::max; using std::abs; -using nav2_util::declare_parameter_if_not_declared; -using nav2_util::geometry_utils::euclidean_distance; using namespace nav2_costmap_2d; // NOLINT -using rcl_interfaces::msg::ParameterType; namespace nav2_regulated_pure_pursuit_controller { @@ -54,157 +51,28 @@ void RegulatedPurePursuitController::configure( tf_ = tf; plugin_name_ = name; logger_ = node->get_logger(); - clock_ = node->get_clock(); - double transform_tolerance = 0.1; + // Handles storage and dynamic configuration of parameters. + // Returns pointer to data current param settings. + param_handler_ = std::make_unique( + node, plugin_name_, logger_, costmap_->getSizeInMetersX()); + params_ = param_handler_->getParams(); + + // Handles global path transformations + path_handler_ = std::make_unique( + tf2::durationFromSec(params_->transform_tolerance), tf_, costmap_ros_); + + // Checks for imminent collisions + collision_checker_ = std::make_unique(node, costmap_ros_, params_); + double control_frequency = 20.0; goal_dist_tol_ = 0.25; // reasonable default before first update - declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", - rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".approach_velocity_scaling_dist", - rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_collision_detection", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_robot_pose_search_dist", - rclcpp::ParameterValue(getCostmapMaxExtent())); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_interpolation", - rclcpp::ParameterValue(true)); - - node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); - base_desired_linear_vel_ = desired_linear_vel_; - node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); - node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); - node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); - node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); - node->get_parameter( - plugin_name_ + ".rotate_to_heading_angular_vel", - rotate_to_heading_angular_vel_); - node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); - node->get_parameter( - plugin_name_ + ".use_velocity_scaled_lookahead_dist", - use_velocity_scaled_lookahead_dist_); - node->get_parameter( - plugin_name_ + ".min_approach_linear_velocity", - min_approach_linear_velocity_); - node->get_parameter( - plugin_name_ + ".approach_velocity_scaling_dist", - approach_velocity_scaling_dist_); - if (approach_velocity_scaling_dist_ > costmap_->getSizeInMetersX() / 2.0) { - RCLCPP_WARN( - logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " - "leading to permanent slowdown"); - } - node->get_parameter( - plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - max_allowed_time_to_collision_up_to_carrot_); - node->get_parameter( - plugin_name_ + ".use_collision_detection", - use_collision_detection_); - node->get_parameter( - plugin_name_ + ".use_regulated_linear_velocity_scaling", - use_regulated_linear_velocity_scaling_); - node->get_parameter( - plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - use_cost_regulated_linear_velocity_scaling_); - node->get_parameter(plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_); - node->get_parameter(plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_); - node->get_parameter( - plugin_name_ + ".inflation_cost_scaling_factor", - inflation_cost_scaling_factor_); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_radius", - regulated_linear_scaling_min_radius_); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_speed", - regulated_linear_scaling_min_speed_); - node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_); - node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); - node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); - node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_); node->get_parameter("controller_frequency", control_frequency); - node->get_parameter( - plugin_name_ + ".max_robot_pose_search_dist", - max_robot_pose_search_dist_); - node->get_parameter( - plugin_name_ + ".use_interpolation", - use_interpolation_); - - transform_tolerance_ = tf2::durationFromSec(transform_tolerance); control_duration_ = 1.0 / control_frequency; - if (inflation_cost_scaling_factor_ <= 0.0) { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Disabling cost regulated linear velocity scaling."); - use_cost_regulated_linear_velocity_scaling_ = false; - } - - /** Possible to drive in reverse direction if and only if - "use_rotate_to_heading" parameter is set to false **/ - - if (use_rotate_to_heading_ && allow_reversing_) { - RCLCPP_WARN( - logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. By default setting use_rotate_to_heading true"); - allow_reversing_ = false; - } - global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); - carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); - - // initialize collision checker and set costmap - collision_checker_ = std::make_unique>(costmap_); - collision_checker_->setCostmap(costmap_); } void RegulatedPurePursuitController::cleanup() @@ -216,7 +84,6 @@ void RegulatedPurePursuitController::cleanup() plugin_name_.c_str()); global_path_pub_.reset(); carrot_pub_.reset(); - carrot_arc_pub_.reset(); } void RegulatedPurePursuitController::activate() @@ -228,13 +95,6 @@ void RegulatedPurePursuitController::activate() plugin_name_.c_str()); global_path_pub_->on_activate(); carrot_pub_->on_activate(); - carrot_arc_pub_->on_activate(); - // Add callback for dynamic parameters - auto node = node_.lock(); - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind( - &RegulatedPurePursuitController::dynamicParametersCallback, - this, std::placeholders::_1)); } void RegulatedPurePursuitController::deactivate() @@ -246,8 +106,6 @@ void RegulatedPurePursuitController::deactivate() plugin_name_.c_str()); global_path_pub_->on_deactivate(); carrot_pub_->on_deactivate(); - carrot_arc_pub_->on_deactivate(); - dyn_params_handler_.reset(); } std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( @@ -266,10 +124,11 @@ double RegulatedPurePursuitController::getLookAheadDistance( { // If using velocity-scaled look ahead distances, find and clamp the dist // Else, use the static look ahead distance - double lookahead_dist = lookahead_dist_; - if (use_velocity_scaled_lookahead_dist_) { - lookahead_dist = fabs(speed.linear.x) * lookahead_time_; - lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + double lookahead_dist = params_->lookahead_dist; + if (params_->use_velocity_scaled_lookahead_dist) { + lookahead_dist = fabs(speed.linear.x) * params_->lookahead_time; + lookahead_dist = std::clamp( + lookahead_dist, params_->min_lookahead_dist, params_->max_lookahead_dist); } return lookahead_dist; @@ -280,7 +139,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity const geometry_msgs::msg::Twist & speed, nav2_core::GoalChecker * goal_checker) { - std::lock_guard lock_reinit(mutex_); + std::lock_guard lock_reinit(param_handler_->getMutex()); // Update for the current goal checker's state geometry_msgs::msg::Pose pose_tolerance; @@ -292,15 +151,17 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Transform path to robot base frame - auto transformed_plan = transformGlobalPlan(pose); + auto transformed_plan = path_handler_->transformGlobalPlan( + pose, params_->max_robot_pose_search_dist); + global_path_pub_->publish(transformed_plan); // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); // Check for reverse driving - if (allow_reversing_) { + if (params_->allow_reversing) { // Cusp check - double dist_to_cusp = findVelocitySignChange(transformed_plan); + const double dist_to_cusp = findVelocitySignChange(transformed_plan); // if the lookahead distance is further than the cusp, use the cusp distance instead if (dist_to_cusp < lookahead_dist) { @@ -308,6 +169,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } } + // Get the particular point on the path at the lookahead distance auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -327,11 +189,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Setting the velocity direction double sign = 1.0; - if (allow_reversing_) { + if (params_->allow_reversing) { sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; } - linear_vel = desired_linear_vel_; + linear_vel = params_->desired_linear_vel; // Make sure we're in compliance with basic constraints double angle_to_heading; @@ -343,7 +205,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } else { applyConstraints( curvature, speed, - costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, + collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, linear_vel, sign); // Apply curvature to angular velocity after constraining linear velocity @@ -352,7 +214,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Collision checking on this velocity heading const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { + if (params_->use_collision_detection && + collision_checker_->isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) + { throw nav2_core::NoValidControl("RegulatedPurePursuitController detected collision ahead!"); } @@ -369,7 +233,8 @@ bool RegulatedPurePursuitController::shouldRotateToPath( { // Whether we should rotate robot to rough path heading angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); - return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; + return params_->use_rotate_to_heading && + fabs(angle_to_path) > params_->rotate_to_heading_min_angle; } bool RegulatedPurePursuitController::shouldRotateToGoalHeading( @@ -377,7 +242,7 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading( { // Whether we should rotate robot to goal heading double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_; + return params_->use_rotate_to_heading && dist_to_goal < goal_dist_tol_; } void RegulatedPurePursuitController::rotateToHeading( @@ -387,11 +252,11 @@ void RegulatedPurePursuitController::rotateToHeading( // Rotate in place using max angular velocity / acceleration possible linear_vel = 0.0; const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; - angular_vel = sign * rotate_to_heading_angular_vel_; + angular_vel = sign * params_->rotate_to_heading_angular_vel; const double & dt = control_duration_; - const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; - const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; + const double min_feasible_angular_speed = curr_speed.angular.z - params_->max_angular_accel * dt; + const double max_feasible_angular_speed = curr_speed.angular.z + params_->max_angular_accel * dt; angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); } @@ -443,7 +308,7 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin // If the no pose is not far enough, take the last pose if (goal_pose_it == transformed_plan.poses.end()) { goal_pose_it = std::prev(transformed_plan.poses.end()); - } else if (use_interpolation_ && goal_pose_it != transformed_plan.poses.begin()) { + } else if (params_->use_interpolation && goal_pose_it != transformed_plan.poses.begin()) { // Find the point on the line segment between the two poses // that is exactly the lookahead distance away from the robot pose (the origin) // This can be found with a closed form for the intersection of a segment and a circle @@ -463,300 +328,62 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin return *goal_pose_it; } -bool RegulatedPurePursuitController::isCollisionImminent( - const geometry_msgs::msg::PoseStamped & robot_pose, - const double & linear_vel, const double & angular_vel, - const double & carrot_dist) -{ - // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in - // odom frame and the carrot_pose is in robot base frame. - - // check current point is OK - if (inCollision( - robot_pose.pose.position.x, robot_pose.pose.position.y, - tf2::getYaw(robot_pose.pose.orientation))) - { - return true; - } - - // visualization messages - nav_msgs::msg::Path arc_pts_msg; - arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); - arc_pts_msg.header.stamp = robot_pose.header.stamp; - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.frame_id = arc_pts_msg.header.frame_id; - pose_msg.header.stamp = arc_pts_msg.header.stamp; - - double projection_time = 0.0; - if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) { - // rotating to heading at goal or toward path - // Equation finds the angular distance required for the largest - // part of the robot radius to move to another costmap cell: - // theta_min = 2.0 * sin ((res/2) / r_max) - // via isosceles triangle r_max-r_max-resolution, - // dividing by angular_velocity gives us a timestep. - double max_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); - projection_time = - 2.0 * sin((costmap_->getResolution() / 2) / max_radius) / fabs(angular_vel); - } else { - // Normal path tracking - projection_time = costmap_->getResolution() / fabs(linear_vel); - } - - const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position; - geometry_msgs::msg::Pose2D curr_pose; - curr_pose.x = robot_pose.pose.position.x; - curr_pose.y = robot_pose.pose.position.y; - curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); - - // only forward simulate within time requested - int i = 1; - while (i * projection_time < max_allowed_time_to_collision_up_to_carrot_) { - i++; - - // apply velocity at curr_pose over distance - curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); - curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); - curr_pose.theta += projection_time * angular_vel; - - // check if past carrot pose, where no longer a thoughtfully valid command - if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) { - break; - } - - // store it for visualization - pose_msg.pose.position.x = curr_pose.x; - pose_msg.pose.position.y = curr_pose.y; - pose_msg.pose.position.z = 0.01; - arc_pts_msg.poses.push_back(pose_msg); - - // check for collision at the projected pose - if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { - carrot_arc_pub_->publish(arc_pts_msg); - return true; - } - } - - carrot_arc_pub_->publish(arc_pts_msg); - - return false; -} - -bool RegulatedPurePursuitController::inCollision( - const double & x, - const double & y, - const double & theta) -{ - unsigned int mx, my; - - if (!costmap_->worldToMap(x, y, mx, my)) { - RCLCPP_WARN_THROTTLE( - logger_, *(clock_), 30000, - "The dimensions of the costmap is too small to successfully check for " - "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " - "increase your costmap size."); - return false; - } - - double footprint_cost = collision_checker_->footprintCostAtPose( - x, y, theta, costmap_ros_->getRobotFootprint()); - if (footprint_cost == static_cast(NO_INFORMATION) && - costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) - { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost >= static_cast(LETHAL_OBSTACLE); -} - -double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) -{ - unsigned int mx, my; - - if (!costmap_->worldToMap(x, y, mx, my)) { - RCLCPP_FATAL( - logger_, - "The dimensions of the costmap is too small to fully include your robot's footprint, " - "thusly the robot cannot proceed further"); - throw nav2_core::ControllerException( - "RegulatedPurePursuitController: Dimensions of the costmap are too small " - "to encapsulate the robot footprint at current speeds!"); - } - - unsigned char cost = costmap_->getCost(mx, my); - return static_cast(cost); -} - -double RegulatedPurePursuitController::approachVelocityScalingFactor( - const nav_msgs::msg::Path & transformed_path -) const -{ - // Waiting to apply the threshold based on integrated distance ensures we don't - // erroneously apply approach scaling on curvy paths that are contained in a large local costmap. - double remaining_distance = nav2_util::geometry_utils::calculate_path_length(transformed_path); - if (remaining_distance < approach_velocity_scaling_dist_) { - auto & last = transformed_path.poses.back(); - // Here we will use a regular euclidean distance from the robot frame (origin) - // to get smooth scaling, regardless of path density. - double distance_to_last_pose = std::hypot(last.pose.position.x, last.pose.position.y); - return distance_to_last_pose / approach_velocity_scaling_dist_; - } else { - return 1.0; - } -} - -void RegulatedPurePursuitController::applyApproachVelocityScaling( - const nav_msgs::msg::Path & path, - double & linear_vel -) const -{ - double approach_vel = linear_vel; - double velocity_scaling = approachVelocityScalingFactor(path); - double unbounded_vel = approach_vel * velocity_scaling; - if (unbounded_vel < min_approach_linear_velocity_) { - approach_vel = min_approach_linear_velocity_; - } else { - approach_vel *= velocity_scaling; - } - - // Use the lowest velocity between approach and other constraints, if all overlapping - linear_vel = std::min(linear_vel, approach_vel); -} - void RegulatedPurePursuitController::applyConstraints( const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) { - double curvature_vel = linear_vel; - double cost_vel = linear_vel; + double curvature_vel = linear_vel, cost_vel = linear_vel; // limit the linear velocity by curvature - const double radius = fabs(1.0 / curvature); - const double & min_rad = regulated_linear_scaling_min_radius_; - if (use_regulated_linear_velocity_scaling_ && radius < min_rad) { - curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + if (params_->use_regulated_linear_velocity_scaling) { + curvature_vel = heuristics::curvatureConstraint( + linear_vel, curvature, params_->regulated_linear_scaling_min_radius); } // limit the linear velocity by proximity to obstacles - if (use_cost_regulated_linear_velocity_scaling_ && - pose_cost != static_cast(NO_INFORMATION) && - pose_cost != static_cast(FREE_SPACE)) - { - const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); - const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * - std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; - - if (min_distance_to_obstacle < cost_scaling_dist_) { - cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; - } + if (params_->use_cost_regulated_linear_velocity_scaling) { + cost_vel = heuristics::costConstraint(linear_vel, pose_cost, costmap_ros_, params_); } - // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + // Use the lowest of the 2 constraints, but above the minimum translational speed linear_vel = std::min(cost_vel, curvature_vel); - linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + linear_vel = std::max(linear_vel, params_->regulated_linear_scaling_min_speed); - applyApproachVelocityScaling(path, linear_vel); + // Apply constraint to reduce speed on approach to the final goal pose + linear_vel = heuristics::approachVelocityConstraint( + linear_vel, path, params_->min_approach_linear_velocity, + params_->approach_velocity_scaling_dist); // Limit linear velocities to be valid - linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); + linear_vel = std::clamp(fabs(linear_vel), 0.0, params_->desired_linear_vel); linear_vel = sign * linear_vel; } void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { - global_plan_ = path; + path_handler_->setPlan(path); } void RegulatedPurePursuitController::setSpeedLimit( const double & speed_limit, const bool & percentage) { + std::lock_guard lock_reinit(param_handler_->getMutex()); + if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { // Restore default value - desired_linear_vel_ = base_desired_linear_vel_; + params_->desired_linear_vel = params_->base_desired_linear_vel; } else { if (percentage) { // Speed limit is expressed in % from maximum speed of robot - desired_linear_vel_ = base_desired_linear_vel_ * speed_limit / 100.0; + params_->desired_linear_vel = params_->base_desired_linear_vel * speed_limit / 100.0; } else { // Speed limit is expressed in absolute value - desired_linear_vel_ = speed_limit; + params_->desired_linear_vel = speed_limit; } } } -nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose) -{ - if (global_plan_.poses.empty()) { - throw nav2_core::InvalidPath("Received plan with zero length"); - } - - // let's get the pose of the robot in the frame of the plan - geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { - throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); - } - - // We'll discard points on the plan that are outside the local costmap - double max_costmap_extent = getCostmapMaxExtent(); - - auto closest_pose_upper_bound = - nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); - - // First find the closest pose on the path to the robot - // bounded by when the path turns around (if it does) so we don't get a pose from a later - // portion of the path - auto transformation_begin = - nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), closest_pose_upper_bound, - [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(robot_pose, ps); - }); - - // Find points up to max_transform_dist so we only transform them. - auto transformation_end = std::find_if( - transformation_begin, global_plan_.poses.end(), - [&](const auto & pose) { - return euclidean_distance(pose, robot_pose) > max_costmap_extent; - }); - - // Lambda to transform a PoseStamped from global frame to local - auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_.header.frame_id; - stamped_pose.header.stamp = robot_pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { - throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); - } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; - std::transform( - transformation_begin, transformation_end, - std::back_inserter(transformed_plan.poses), - transformGlobalPoseToLocal); - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = robot_pose.header.stamp; - - // Remove the portion of the global plan that we've already passed so we don't - // process it on the next iteration (this is called path pruning) - global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); - global_path_pub_->publish(transformed_plan); - - if (transformed_plan.poses.empty()) { - throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); - } - - return transformed_plan; -} - double RegulatedPurePursuitController::findVelocitySignChange( const nav_msgs::msg::Path & transformed_plan) { @@ -786,119 +413,6 @@ double RegulatedPurePursuitController::findVelocitySignChange( return std::numeric_limits::max(); } - -bool RegulatedPurePursuitController::transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf_->transform(in_pose, out_pose, frame, transform_tolerance_); - out_pose.header.frame_id = frame; - return true; - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); - } - return false; -} - -double RegulatedPurePursuitController::getCostmapMaxExtent() const -{ - const double max_costmap_dim_meters = std::max( - costmap_->getSizeInMetersX(), costmap_->getSizeInMetersY()); - return max_costmap_dim_meters / 2.0; -} - - -rcl_interfaces::msg::SetParametersResult -RegulatedPurePursuitController::dynamicParametersCallback( - std::vector parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - std::lock_guard lock_reinit(mutex_); - - for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".inflation_cost_scaling_factor") { - if (parameter.as_double() <= 0.0) { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Ignoring parameter update."); - continue; - } - inflation_cost_scaling_factor_ = parameter.as_double(); - } else if (name == plugin_name_ + ".desired_linear_vel") { - desired_linear_vel_ = parameter.as_double(); - base_desired_linear_vel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_dist") { - lookahead_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_lookahead_dist") { - max_lookahead_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".min_lookahead_dist") { - min_lookahead_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_time") { - lookahead_time_ = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { - rotate_to_heading_angular_vel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".min_approach_linear_velocity") { - min_approach_linear_velocity_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { - max_allowed_time_to_collision_up_to_carrot_ = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_dist") { - cost_scaling_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_gain") { - cost_scaling_gain_ = parameter.as_double(); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { - regulated_linear_scaling_min_radius_ = parameter.as_double(); - } else if (name == plugin_name_ + ".transform_tolerance") { - double transform_tolerance = parameter.as_double(); - transform_tolerance_ = tf2::durationFromSec(transform_tolerance); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { - regulated_linear_scaling_min_speed_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_angular_accel") { - max_angular_accel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { - rotate_to_heading_min_angle_ = parameter.as_double(); - } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { - use_velocity_scaled_lookahead_dist_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { - use_regulated_linear_velocity_scaling_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { - use_cost_regulated_linear_velocity_scaling_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_rotate_to_heading") { - if (parameter.as_bool() && allow_reversing_) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - continue; - } - use_rotate_to_heading_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".allow_reversing") { - if (use_rotate_to_heading_ && parameter.as_bool()) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - continue; - } - allow_reversing_ = parameter.as_bool(); - } - } - } - - result.successful = true; - return result; -} - } // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 51bb277fed..09892755d9 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -41,9 +41,9 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {} - nav_msgs::msg::Path getPlan() {return global_plan_;} + nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} - double getSpeed() {return desired_linear_vel_;} + double getSpeed() {return params_->desired_linear_vel;} std::unique_ptr createCarrotMsgWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose) @@ -51,9 +51,9 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return createCarrotMsg(carrot_pose); } - void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;} - void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;} - void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;} + void setVelocityScaledLookAhead() {params_->use_velocity_scaled_lookahead_dist = true;} + void setCostRegulationScaling() {params_->use_cost_regulated_linear_velocity_scaling = true;} + void resetVelocityRegulationScaling() {params_->use_regulated_linear_velocity_scaling = false;} double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist) { @@ -110,7 +110,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure nav_msgs::msg::Path transformGlobalPlanWrapper( const geometry_msgs::msg::PoseStamped & pose) { - return transformGlobalPlan(pose); + return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); } }; @@ -168,8 +168,16 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) TEST(RegulatedPurePursuitTest, findVelocitySignChange) { - auto ctrl = std::make_shared(); auto node = std::make_shared("testRPPfindVelocitySignChange"); + auto ctrl = std::make_shared(); + + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + ctrl->configure(node, name, tf, costmap); + geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "smb"; auto time = node->get_clock()->now(); @@ -604,6 +612,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) rclcpp::Parameter("test.use_velocity_scaled_lookahead_dist", false), rclcpp::Parameter("test.use_regulated_linear_velocity_scaling", false), rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false), + rclcpp::Parameter("test.inflation_cost_scaling_factor", 1.0), rclcpp::Parameter("test.allow_reversing", false), rclcpp::Parameter("test.use_rotate_to_heading", false)}); @@ -630,11 +639,36 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_speed").as_double(), 4.0); EXPECT_EQ(node->get_parameter("test.use_velocity_scaled_lookahead_dist").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_regulated_linear_velocity_scaling").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.inflation_cost_scaling_factor").as_double(), 1.0); EXPECT_EQ( node->get_parameter( "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false); EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false); + + // Should fail + auto results2 = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.inflation_cost_scaling_factor", -1.0)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results2); + + auto results3 = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.use_rotate_to_heading", true)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results3); + + auto results4 = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.allow_reversing", false), + rclcpp::Parameter("test.use_rotate_to_heading", true), + rclcpp::Parameter("test.allow_reversing", true)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results4); } class TransformGlobalPlanTest : public ::testing::Test @@ -644,8 +678,8 @@ class TransformGlobalPlanTest : public ::testing::Test { ctrl_ = std::make_shared(); node_ = std::make_shared("testRPP"); - tf_buffer_ = std::make_shared(node_->get_clock()); costmap_ = std::make_shared("fake_costmap"); + tf_buffer_ = std::make_shared(node_->get_clock()); } void configure_costmap(uint16_t width, double resolution) @@ -736,6 +770,7 @@ TEST_F(TransformGlobalPlanTest, no_pruning_on_large_costmap) robot_pose.pose.position.z = 0.0; // A really big costmap // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); configure_controller(5.0); setup_transforms(robot_pose.pose.position); From 5161f7d99c5dc7db873830160a81ee009a9e4f07 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 3 Nov 2022 11:14:05 -0400 Subject: [PATCH 093/131] exceptions for compute path through poses (#3248) * exceptions for compute path through poses * lint fix * code review * code review Co-authored-by: Joshua Wallace --- .../compute_path_through_poses_action.hpp | 8 ++- .../action/compute_path_to_pose_action.hpp | 12 ++-- nav2_behavior_tree/nav2_tree_nodes.xml | 2 + .../compute_path_through_poses_action.cpp | 5 ++ .../action/compute_path_to_pose_action.cpp | 8 +-- .../include/nav2_core/planner_exceptions.hpp | 7 ++ .../action/ComputePathThroughPoses.action | 14 ++++ .../include/nav2_planner/planner_server.hpp | 2 + nav2_planner/src/planner_server.cpp | 71 +++++++++++++------ 9 files changed, 96 insertions(+), 33 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index f7610696f8..ad5954b6a4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -32,6 +32,10 @@ namespace nav2_behavior_tree class ComputePathThroughPosesAction : public BtActionNode { + using Action = nav2_msgs::action::ComputePathThroughPoses; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::ComputePathThroughPosesAction @@ -72,7 +76,6 @@ class ComputePathThroughPosesAction { return providedBasicPorts( { - BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), BT::InputPort>( "goals", "Destinations to plan through"), @@ -81,6 +84,9 @@ class ComputePathThroughPosesAction BT::InputPort( "planner_id", "", "Mapped name to the planner plugin type to use"), + BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), + BT::OutputPort( + "compute_path_through_poses_error_code", "The compute path through poses error code"), }); } }; 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 e980012cc4..1c7f0526df 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 @@ -29,6 +29,10 @@ namespace nav2_behavior_tree */ class ComputePathToPoseAction : public BtActionNode { + using Action = nav2_msgs::action::ComputePathToPose; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction @@ -57,7 +61,7 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), - BT::OutputPort( - "compute_path_to_pose_error_code", "The compute path to pose error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), BT::InputPort( "planner_id", "", "Mapped name to the planner plugin type to use"), + BT::OutputPort("path", "Path created by ComputePathToPose node"), + BT::OutputPort( + "compute_path_to_pose_error_code", "The compute path to pose error code"), }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 425875c24c..95e5a0917d 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -87,6 +87,8 @@ Server timeout Mapped name to the planner plugin type to use Path created by ComputePathToPose node + "The compute path through poses + pose error code" diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index c7421d5599..ffcbd328ae 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -41,6 +41,8 @@ void ComputePathThroughPosesAction::on_tick() BT::NodeStatus ComputePathThroughPosesAction::on_success() { setOutput("path", result_.result->path); + // Set empty error code, action was successful + setOutput("compute_path_through_poses_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -48,6 +50,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + setOutput("compute_path_through_poses_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -55,6 +58,8 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + // Set empty error code, action was cancelled + setOutput("compute_path_through_poses_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } 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 b5e0e9084c..68a909b52c 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 @@ -24,7 +24,7 @@ ComputePathToPoseAction::ComputePathToPoseAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -41,8 +41,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); // Set empty error code, action was successful - result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("compute_path_to_pose_error_code", result_.result->error_code); + setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -59,8 +58,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled - result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("compute_path_to_pose_error_code", result_.result->error_code); + setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_core/include/nav2_core/planner_exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp index 622ee0f0db..e607d98ccc 100644 --- a/nav2_core/include/nav2_core/planner_exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -99,6 +99,13 @@ class PlannerTFError : public PlannerException : PlannerException(description) {} }; +class NoViapointsGiven : public PlannerException +{ +public: + explicit NoViapointsGiven(const std::string & description) + : PlannerException(description) {} +}; + } // namespace nav2_core #endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index a1f609a9e0..c37297c69e 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,3 +1,16 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +int16 NONE=0 +int16 UNKNOWN=1 +int16 TF_ERROR=2 +int16 START_OUTSIDE_MAP=3 +int16 GOAL_OUTSIDE_MAP=4 +int16 START_OCCUPIED=5 +int16 GOAL_OCCUPIED=6 +int16 TIMEOUT=7 +int16 NO_VALID_PATH=8 +int16 NO_VIAPOINTS_GIVEN=9 + #goal definition geometry_msgs/PoseStamped[] goals geometry_msgs/PoseStamped start @@ -7,5 +20,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time +int16 error_code --- #feedback definition diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 41e1b93502..96c0a892e5 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -107,7 +107,9 @@ class PlannerServer : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; using ActionToPose = nav2_msgs::action::ComputePathToPose; + using ActionToPoseGoal = ActionToPose::Goal; using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses; + using ActionThroughPosesGoal = ActionThroughPoses::Goal; using ActionServerToPose = nav2_util::SimpleActionServer; using ActionServerThroughPoses = nav2_util::SimpleActionServer; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index c87bd997fe..4e49cfcb35 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -343,11 +343,13 @@ void PlannerServer::computePlanThroughPoses() auto start_time = steady_clock_.now(); - // Initialize the ComputePathToPose goal and result + // Initialize the ComputePathThroughPoses goal and result auto goal = action_server_poses_->get_current_goal(); auto result = std::make_shared(); nav_msgs::msg::Path concat_path; + geometry_msgs::msg::PoseStamped curr_start, curr_goal; + try { if (isServerInactive(action_server_poses_) || isCancelRequested(action_server_poses_)) { return; @@ -357,11 +359,8 @@ void PlannerServer::computePlanThroughPoses() getPreemptedGoalIfRequested(action_server_poses_, goal); - if (goal->goals.size() == 0) { - RCLCPP_WARN( - get_logger(), - "Compute path through poses requested a plan with no viapoint poses, returning."); - action_server_poses_->terminate_current(); + if (goal->goals.empty()) { + throw nav2_core::NoViapointsGiven("No viapoints given"); } // Use start pose if provided otherwise use current robot pose @@ -371,8 +370,6 @@ void PlannerServer::computePlanThroughPoses() } // Get consecutive paths through these points - std::vector::iterator goal_iter; - geometry_msgs::msg::PoseStamped curr_start, curr_goal; for (unsigned int i = 0; i != goal->goals.size(); i++) { // Get starting point if (i == 0) { @@ -415,13 +412,42 @@ void PlannerServer::computePlanThroughPoses() } action_server_poses_->succeeded_current(result); + } catch (nav2_core::StartOccupied & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::START_OCCUPIED; + action_server_poses_->terminate_current(result); + } catch (nav2_core::GoalOccupied & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::GOAL_OCCUPIED; + action_server_poses_->terminate_current(result); + } catch (nav2_core::NoValidPathCouldBeFound & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::NO_VALID_PATH; + action_server_poses_->terminate_current(result); + } catch (nav2_core::PlannerTimedOut & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::TIMEOUT; + action_server_poses_->terminate_current(result); + } catch (nav2_core::StartOutsideMapBounds & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::START_OUTSIDE_MAP; + action_server_poses_->terminate_current(result); + } catch (nav2_core::GoalOutsideMapBounds & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::GOAL_OUTSIDE_MAP; + action_server_poses_->terminate_current(result); + } catch (nav2_core::PlannerTFError & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::TF_ERROR; + action_server_poses_->terminate_current(result); + } catch (nav2_core::NoViapointsGiven & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::NO_VIAPOINTS_GIVEN; + action_server_poses_->terminate_current(result); } catch (std::exception & ex) { - RCLCPP_WARN( - get_logger(), - "%s plugin failed to plan through %zu points with final goal (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), goal->goals.size(), goal->goals.back().pose.position.x, - goal->goals.back().pose.position.y, ex.what()); - action_server_poses_->terminate_current(); + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::UNKNOWN; + action_server_poses_->terminate_current(result); } } @@ -476,39 +502,38 @@ PlannerServer::computePlan() "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", 1 / max_planner_duration_, 1 / cycle_duration.seconds()); } - action_server_pose_->succeeded_current(result); } catch (nav2_core::StartOccupied & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED; + result->error_code = ActionToPoseGoal::START_OCCUPIED; action_server_pose_->terminate_current(result); } catch (nav2_core::GoalOccupied & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; + result->error_code = ActionToPoseGoal::GOAL_OCCUPIED; action_server_pose_->terminate_current(result); } catch (nav2_core::NoValidPathCouldBeFound & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; + result->error_code = ActionToPoseGoal::NO_VALID_PATH; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTimedOut & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; + result->error_code = ActionToPoseGoal::TIMEOUT; action_server_pose_->terminate_current(result); } catch (nav2_core::StartOutsideMapBounds & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; + result->error_code = ActionToPoseGoal::START_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::GoalOutsideMapBounds & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; + result->error_code = ActionToPoseGoal::GOAL_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTFError & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR; + result->error_code = ActionToPoseGoal::TF_ERROR; action_server_pose_->terminate_current(result); } catch (std::exception & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; + result->error_code = ActionToPoseGoal::UNKNOWN; action_server_pose_->terminate_current(result); } } From 80bd879e28bc00fbb44f2016210f7c65f3c46f6b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 3 Nov 2022 08:33:54 -0700 Subject: [PATCH 094/131] =?UTF-8?q?Reclaim=20Our=20CI=20Coverage=20from=20?= =?UTF-8?q?the=20Lords=20of=20Painful=20Subtle=20Regressions=20=E2=9A=94?= =?UTF-8?q?=EF=B8=8F=E2=9A=94=EF=B8=8F=E2=9A=94=EF=B8=8F=20(#3266)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * test waypoint follower with composition off for logging * adding no composition to all system tests --- .../assisted_teleop/test_assisted_teleop_behavior_launch.py | 1 + .../src/behaviors/backup/test_backup_behavior_launch.py | 1 + .../drive_on_heading/test_drive_on_heading_behavior_launch.py | 1 + .../src/behaviors/spin/test_spin_behavior_launch.py | 1 + .../src/behaviors/wait/test_wait_behavior_launch.py | 1 + nav2_system_tests/src/costmap_filters/test_keepout_launch.py | 1 + nav2_system_tests/src/costmap_filters/test_speed_launch.py | 1 + nav2_system_tests/src/system/test_multi_robot_launch.py | 1 + nav2_system_tests/src/system/test_system_launch.py | 1 + nav2_system_tests/src/system/test_wrong_init_pose_launch.py | 1 + .../src/system_failure/test_system_failure_launch.py | 1 + nav2_system_tests/src/updown/test_updown_launch.py | 1 + nav2_system_tests/src/waypoint_follower/test_case_py.launch | 1 + 13 files changed, 13 insertions(+) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py index 976eb02c2f..57f2b65625 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index bf7f4f04f1..6091ffb40c 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py index 29530dd475..49959ec86d 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index d26795834b..154f9ed533 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 37f2ebd34c..ee89e416de 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index 796667c65d..5e21a7241a 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -118,6 +118,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), Node( diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 5b0c61ed9d..433dbc184e 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -116,6 +116,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index 4c22101eca..d323f97567 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -106,6 +106,7 @@ def generate_launch_description(): 'params_file': params_file, 'bt_xml_file': bt_xml_file, 'autostart': 'True', + 'use_composition': 'False', 'use_remappings': 'True'}.items()) ]) nav_instances_cmds.append(group) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 6b067deea4..3678b242ac 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -96,6 +96,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 740c267fa2..2c10898a22 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -96,6 +96,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True', 'use_composition': 'False'}.items()), ]) diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 56d4d44fdb..5bc8022766 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -86,6 +86,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index aac4c6e2de..28b20bd4c3 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -61,6 +61,7 @@ def generate_launch_description(): os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={'map': map_yaml_file, 'use_sim_time': 'True', + 'use_composition': 'False', 'autostart': 'False'}.items()) start_test = launch.actions.ExecuteProcess( diff --git a/nav2_system_tests/src/waypoint_follower/test_case_py.launch b/nav2_system_tests/src/waypoint_follower/test_case_py.launch index 98819e6e63..adeb839c48 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_py.launch +++ b/nav2_system_tests/src/waypoint_follower/test_case_py.launch @@ -79,6 +79,7 @@ def generate_launch_description(): launch_arguments={ 'map': map_yaml_file, 'use_sim_time': 'True', + 'use_composition': 'False', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True'}.items()), From 607019c6cabb7e48e638d4f988b80aae62aa4a79 Mon Sep 17 00:00:00 2001 From: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com> Date: Mon, 7 Nov 2022 21:06:49 +0100 Subject: [PATCH 095/131] Added Line Iterator (#3197) * Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan --- .../nav2_simple_commander/line_iterator.py | 177 ++++++++++++++++++ nav2_simple_commander/pytest.ini | 2 + .../test/test_line_iterator.py | 107 +++++++++++ 3 files changed, 286 insertions(+) create mode 100644 nav2_simple_commander/nav2_simple_commander/line_iterator.py create mode 100644 nav2_simple_commander/pytest.ini create mode 100644 nav2_simple_commander/test/test_line_iterator.py diff --git a/nav2_simple_commander/nav2_simple_commander/line_iterator.py b/nav2_simple_commander/nav2_simple_commander/line_iterator.py new file mode 100644 index 0000000000..db19d4180c --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/line_iterator.py @@ -0,0 +1,177 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# Copyright 2022 Afif Swaidan +# +# 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. + +""" +This is a Python3 API for a line iterator. + +It provides the ability to iterate +through the points of a line. +""" + +from cmath import sqrt + + +class LineIterator(): + """ + LineIterator. + + LineIterator Python3 API for iterating along the points of a given line + """ + + def __init__(self, x0, y0, x1, y1, step_size=1.0): + """ + Initialize the LineIterator. + + Args + ---- + x0 (float): Abscissa of the initial point + y0 (float): Ordinate of the initial point + x1 (float): Abscissa of the final point + y1 (float): Ordinate of the final point + step_size (float): Optional, Increments' resolution, defaults to 1 + + Raises + ------ + TypeError: When one (or more) of the inputs is not a number + ValueError: When step_size is not a positive number + + """ + if type(x0) not in [int, float]: + raise TypeError("x0 must be a number (int or float)") + + if type(y0) not in [int, float]: + raise TypeError("y0 must be a number (int or float)") + + if type(x1) not in [int, float]: + raise TypeError("x1 must be a number (int or float)") + + if type(y1) not in [int, float]: + raise TypeError("y1 must be a number (int or float)") + + if type(step_size) not in [int, float]: + raise TypeError("step_size must be a number (int or float)") + + if step_size <= 0: + raise ValueError("step_size must be a positive number") + + self.x0_ = x0 + self.y0_ = y0 + self.x1_ = x1 + self.y1_ = y1 + self.x_ = x0 + self.y_ = y0 + self.step_size_ = step_size + + if x1 != x0 and y1 != y0: + self.valid_ = True + self.m_ = (y1-y0)/(x1-x0) + self.b_ = y1 - (self.m_*x1) + elif x1 == x0 and y1 != y0: + self.valid_ = True + elif y1 == y1 and x1 != x0: + self.valid_ = True + self.m_ = (y1-y0)/(x1-x0) + self.b_ = y1 - (self.m_*x1) + else: + self.valid_ = False + raise ValueError( + "Line has zero length (All 4 points have same coordinates)") + + def isValid(self): + """Check if line is valid.""" + return self.valid_ + + def advance(self): + """Advance to the next point in the line.""" + if self.x1_ > self.x0_: + if self.x_ < self.x1_: + self.x_ = round(self.clamp( + self.x_ + self.step_size_, self.x0_, self.x1_), 5) + self.y_ = round(self.m_ * self.x_ + self.b_, 5) + else: + self.valid_ = False + elif self.x1_ < self.x0_: + if self.x_ > self.x1_: + self.x_ = round(self.clamp( + self.x_ - self.step_size_, self.x1_, self.x0_), 5) + self.y_ = round(self.m_ * self.x_ + self.b_, 5) + else: + self.valid_ = False + else: + if self.y1_ > self.y0_: + if self.y_ < self.y1_: + self.y_ = round(self.clamp( + self.y_ + self.step_size_, self.y0_, self.y1_), 5) + else: + self.valid_ = False + elif self.y1_ < self.y0_: + if self.y_ > self.y1_: + self.y_ = round(self.clamp( + self.y_ - self.step_size_, self.y1_, self.y0_), 5) + else: + self.valid_ = False + else: + self.valid_ = False + + def getX(self): + """Get the abscissa of the current point.""" + return self.x_ + + def getY(self): + """Get the ordinate of the current point.""" + return self.y_ + + def getX0(self): + """Get the abscissa of the initial point.""" + return self.x0_ + + def getY0(self): + """Get the ordinate of the intial point.""" + return self.y0_ + + def getX1(self): + """Get the abscissa of the final point.""" + return self.x1_ + + def getY1(self): + """Get the ordinate of the final point.""" + return self.y1_ + + def get_line_length(self): + """Get the length of the line.""" + return sqrt(pow(self.x1_ - self.x0_, 2) + pow(self.y1_ - self.y0_, 2)) + + def clamp(self, n, min_n, max_n): + """ + Clamp n to be between min_n and max_n. + + Args + ---- + n (float): input value + min_n (float): minimum value + max_n (float): maximum value + + Returns + ------- + n (float): input value clamped between given min and max + + """ + if n < min_n: + return min_n + elif n > max_n: + return max_n + else: + return n diff --git a/nav2_simple_commander/pytest.ini b/nav2_simple_commander/pytest.ini new file mode 100644 index 0000000000..50d6d01257 --- /dev/null +++ b/nav2_simple_commander/pytest.ini @@ -0,0 +1,2 @@ +[pytest] +junit_family=xunit2 \ No newline at end of file diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py new file mode 100644 index 0000000000..4c5e97420a --- /dev/null +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -0,0 +1,107 @@ +# Copyright 2022 Afif Swaidan +# +# 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 unittest +from cmath import sqrt +from nav2_simple_commander.line_iterator import LineIterator + + +class TestLineIterator(unittest.TestCase): + + def test_type_error(self): + # Test if a type error raised when passing invalid arguements types + self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') + + def test_value_error(self): + # Test if a value error raised when passing negative or zero step_size + self.assertRaises(ValueError, LineIterator, 0, 0, 10, 10, -2) + # Test if a value error raised when passing zero length line + self.assertRaises(ValueError, LineIterator, 2, 2, 2, 2, 1) + + def test_get_xy(self): + # Test if the initial and final coordinates are returned correctly + lt = LineIterator(0, 0, 5, 5, 1) + self.assertEqual(lt.getX0(), 0) + self.assertEqual(lt.getY0(), 0) + self.assertEqual(lt.getX1(), 5) + self.assertEqual(lt.getY1(), 5) + + def test_line_length(self): + # Test if the line length is calculated correctly + lt = LineIterator(0, 0, 5, 5, 1) + self.assertEqual(lt.get_line_length(), sqrt(pow(5, 2) + pow(5, 2))) + + def test_straight_line(self): + # Test if the calculations are correct for y = x + lt = LineIterator(0, 0, 5, 5, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + i) + lt.advance() + i += 1 + + # Test if the calculations are correct for y = 2x (positive slope) + lt = LineIterator(0, 0, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + (i*2)) + lt.advance() + i += 1 + + # Test if the calculations are correct for y = -2x (negative slope) + lt = LineIterator(0, 0, 5, -10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + (-i*2)) + lt.advance() + i += 1 + + def test_hor_line(self): + # Test if the calculations are correct for y = 0x+b (horizontal line) + lt = LineIterator(0, 10, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0()) + lt.advance() + i += 1 + + def test_ver_line(self): + # Test if the calculations are correct for x = n (vertical line) + lt = LineIterator(5, 0, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0()) + self.assertEqual(lt.getY(), lt.getY0() + i) + lt.advance() + i += 1 + + def test_clamp(self): + # Test if the increments are clamped to avoid crossing the final points + # when step_size is large with respect to line length + lt = LineIterator(0, 0, 5, 5, 10) + self.assertEqual(lt.getX(), 0) + self.assertEqual(lt.getY(), 0) + lt.advance() + while lt.isValid(): + self.assertEqual(lt.getX(), 5) + self.assertEqual(lt.getY(), 5) + lt.advance() + + +if __name__ == '__main__': + unittest.main() From d81cc62615122703a482f5db60825ab53fb79917 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Mon, 7 Nov 2022 21:53:59 +0100 Subject: [PATCH 096/131] Use SetParameter Launch API to set the yaml filename for map_server (#3174) * implement launch priority for the mapserver parameter yaml_filename minor fix fix commit reincluded rewritten function comment remaining lines for yaml_filename removed default_value issue with composable node alternative soltion to the condition param not working in composable node remove unused import remove comments and reorder composablenode execution fixing commit fixing format fixing lint Update nav2_bringup/params/nav2_params.yaml Co-authored-by: Steve Macenski * state new ros-rolling release changes and deprecation Co-authored-by: Steve Macenski --- nav2_bringup/launch/bringup_launch.py | 7 +-- nav2_bringup/launch/localization_launch.py | 56 +++++++++++++++++++--- nav2_bringup/params/nav2_params.yaml | 11 +++-- 3 files changed, 57 insertions(+), 17 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 065337adb8..8fddcc3253 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -53,14 +53,10 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites=param_substitutions, + param_rewrites={}, convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( @@ -83,6 +79,7 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', + default_value='', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index dca5bf23e4..f2469197bd 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -17,8 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.actions import SetEnvironmentVariable from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node @@ -51,14 +54,10 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites=param_substitutions, + param_rewrites={}, convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( @@ -71,6 +70,7 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', + default_value='', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( @@ -108,6 +108,7 @@ def generate_launch_description(): actions=[ SetParameter("use_sim_time", use_sim_time), Node( + condition=LaunchConfigurationEquals('map', ''), package='nav2_map_server', executable='map_server', name='map_server', @@ -117,6 +118,18 @@ def generate_launch_description(): parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], remappings=remappings), + Node( + condition=LaunchConfigurationNotEquals('map', ''), + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, + {'yaml_filename': map_yaml_file}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), Node( package='nav2_amcl', executable='amcl', @@ -137,13 +150,24 @@ def generate_launch_description(): {'node_names': lifecycle_nodes}]) ] ) - + # LoadComposableNode for map server twice depending if we should use the + # value of map from a CLI or launch default or user defined value in the + # yaml configuration file. They are separated since the conditions + # currently only work on the LoadComposableNodes commands and not on the + # ComposableNode node function itself + # EqualsSubstitution and NotEqualsSubstitution susbsitutions was recently + # added to solve this problem but it has not been ported yet to + # ros-rolling. See https://github.com/ros2/launch_ros/issues/328. + # LaunchConfigurationEquals and LaunchConfigurationNotEquals are scheduled + # for deprecation once a Rolling sync is conducted. Switching to this new + # would be required for both ComposableNode and normal nodes. load_composable_nodes = GroupAction( condition=IfCondition(use_composition), actions=[ SetParameter("use_sim_time", use_sim_time), LoadComposableNodes( target_container=container_name, + condition=LaunchConfigurationEquals('map', ''), composable_node_descriptions=[ ComposableNode( package='nav2_map_server', @@ -151,6 +175,24 @@ def generate_launch_description(): name='map_server', parameters=[configured_params], remappings=remappings), + ], + ), + LoadComposableNodes( + target_container=container_name, + condition=LaunchConfigurationNotEquals('map', ''), + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params, + {'yaml_filename': map_yaml_file}], + remappings=remappings), + ], + ), + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ ComposableNode( package='nav2_amcl', plugin='nav2_amcl::AmclNode', diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 8593587499..21a2946a9c 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -243,11 +243,12 @@ global_costmap: inflation_radius: 0.55 always_send_full_costmap: True -map_server: - ros__parameters: - # Overridden in launch by the "map" launch configuration or provided default value. - # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. - yaml_filename: "" +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" map_saver: ros__parameters: From ad5aa202a5bc503e420d267093265e768782e5f0 Mon Sep 17 00:00:00 2001 From: Pradheep Krishna Date: Thu, 10 Nov 2022 03:32:16 +0530 Subject: [PATCH 097/131] adding reconfigure test to thetastar (#3275) --- .../test/test_theta_star.cpp | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 264f737de4..44b37f4ea7 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -185,3 +185,47 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { life_node.reset(); costmap_ros.reset(); } + +TEST(ThetaStarPlanner, test_theta_star_reconfigure) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr life_node = + std::make_shared("ThetaStarPlannerTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + auto planner = std::make_unique(); + try { + // Expect to throw due to invalid prims file in param + planner->configure(life_node, "test", nullptr, costmap_ros); + } catch (...) { + } + planner->activate(); + + auto rec_param = std::make_shared( + life_node->get_node_base_interface(), life_node->get_node_topics_interface(), + life_node->get_node_graph_interface(), + life_node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.how_many_corners", 8), + rclcpp::Parameter("test.w_euc_cost", 1.0), + rclcpp::Parameter("test.w_traversal_cost", 2.0), + rclcpp::Parameter("test.use_final_approach_orientation", false)}); + + rclcpp::spin_until_future_complete( + life_node->get_node_base_interface(), + results); + + EXPECT_EQ(life_node->get_parameter("test.how_many_corners").as_int(), 8); + EXPECT_EQ( + life_node->get_parameter("test.w_euc_cost").as_double(), + 1.0); + EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0); + EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false); + + rclcpp::spin_until_future_complete( + life_node->get_node_base_interface(), + results); +} From ae268d87c684836368ea6b07389d74cd18aabfbe Mon Sep 17 00:00:00 2001 From: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com> Date: Fri, 11 Nov 2022 04:59:21 +0800 Subject: [PATCH 098/131] Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (#3276) * Update amcl_node.cpp * fit the code style * fit code style --- nav2_amcl/src/amcl_node.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 2e9730ae56..303a753b73 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -808,6 +808,15 @@ bool AmclNode::updateFilter( get_logger(), "Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment); + // Check the validity of range_max, must > 0.0 + if (laser_scan->range_max <= 0.0) { + RCLCPP_WARN( + get_logger(), "wrong range_max of laser_scan data: %f. The message could be malformed." + " Ignore this message and stop updating.", + laser_scan->range_max); + return false; + } + // Apply range min/max thresholds, if the user supplied them if (laser_max_range_ > 0.0) { ldata.range_max = std::min(laser_scan->range_max, static_cast(laser_max_range_)); From 61a967ca66b663cc55a4acbbc2d560611dd3a6ac Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Fri, 11 Nov 2022 19:11:11 +0100 Subject: [PATCH 099/131] BT Service Node to throw if service was not available in time (#3256) * throw if service server wasn't available in time mimic the behavior of the bt action node constructor * throw if action unavailable in bt cancel action * use chrono literals namespace * fix linting errors * fix code style divergence --- .../nav2_behavior_tree/bt_cancel_action_node.hpp | 9 ++++++++- .../include/nav2_behavior_tree/bt_service_node.hpp | 13 ++++++++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index b48478ecc4..504df9157a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -27,6 +27,8 @@ namespace nav2_behavior_tree { +using namespace std::chrono_literals; // NOLINT + /** * @brief Abstract class representing an action for cancelling BT node * @tparam ActionT Type of action @@ -87,7 +89,12 @@ class BtCancelActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - action_client_->wait_for_action_server(); + if (!action_client_->wait_for_action_server(1s)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", + action_name.c_str()); + throw std::runtime_error(std::string("Action server %s not available", action_name.c_str())); + } } /** diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index b99e5b74d4..91b0c0cd9d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "behaviortree_cpp_v3/action_node.h" #include "nav2_util/node_utils.hpp" @@ -26,6 +27,8 @@ namespace nav2_behavior_tree { +using namespace std::chrono_literals; // NOLINT + /** * @brief Abstract class representing a service based BT node * @tparam ServiceT Type of service @@ -71,7 +74,15 @@ class BtServiceNode : public BT::ActionNodeBase RCLCPP_DEBUG( node_->get_logger(), "Waiting for \"%s\" service", service_name_.c_str()); - service_client_->wait_for_service(); + if (!service_client_->wait_for_service(1s)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", + service_node_name.c_str()); + throw std::runtime_error( + std::string( + "Service server %s not available", + service_node_name.c_str())); + } RCLCPP_DEBUG( node_->get_logger(), "\"%s\" BtServiceNode initialized", From 6d325e3cdb66fd00011a13f1413055c08f2de344 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Fri, 11 Nov 2022 12:43:15 -0700 Subject: [PATCH 100/131] remove exec_depend on behaviortree_cpp_v3 (#3279) --- nav2_behavior_tree/package.xml | 1 - nav2_bt_navigator/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 81e24a8cb3..30e1dda75d 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -36,7 +36,6 @@ rclcpp_action rclcpp_lifecycle std_msgs - behaviortree_cpp_v3 builtin_interfaces geometry_msgs sensor_msgs diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index e3287b118d..6f86318a79 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -24,7 +24,6 @@ nav2_util nav2_core - behaviortree_cpp_v3 rclcpp rclcpp_action rclcpp_lifecycle From 9b4bdded228def80cbfa80a763675a00bfa2707f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Nov 2022 17:53:13 -0800 Subject: [PATCH 101/131] add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (#3284) * add parameterized refinement recursion numbers * fix tests --- nav2_smac_planner/include/nav2_smac_planner/smoother.hpp | 2 +- nav2_smac_planner/include/nav2_smac_planner/types.hpp | 4 ++++ nav2_smac_planner/src/smoother.cpp | 5 +++-- nav2_smoother/include/nav2_smoother/simple_smoother.hpp | 2 +- nav2_smoother/src/simple_smoother.cpp | 7 +++++-- nav2_smoother/test/test_simple_smoother.cpp | 6 +++--- 6 files changed, 17 insertions(+), 9 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index 8c2adc5d29..d896cd90b5 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -232,7 +232,7 @@ class Smoother bool & reversing_segment); double min_turning_rad_, tolerance_, data_w_, smooth_w_; - int max_its_, refinement_ctr_; + int max_its_, refinement_ctr_, refinement_num_; bool is_holonomic_, do_refinement_; MotionModel motion_model_; ompl::base::StateSpacePtr state_space_; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index f2cd73d33a..1df34851af 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -87,6 +87,9 @@ struct SmootherParams nav2_util::declare_parameter_if_not_declared( node, local_name + "do_refinement", rclcpp::ParameterValue(true)); node->get_parameter(local_name + "do_refinement", do_refinement_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "refinement_num", rclcpp::ParameterValue(2)); + node->get_parameter(local_name + "refinement_num", refinement_num_); } double tolerance_; @@ -95,6 +98,7 @@ struct SmootherParams double w_smooth_; bool holonomic_; bool do_refinement_; + int refinement_num_; }; /** diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index fd0ac34be2..a69e14011f 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -31,6 +31,7 @@ Smoother::Smoother(const SmootherParams & params) smooth_w_ = params.w_smooth_; is_holonomic_ = params.holonomic_; do_refinement_ = params.do_refinement_; + refinement_num_ = params.refinement_num_; } void Smoother::initialize(const double & min_turning_radius) @@ -49,7 +50,6 @@ bool Smoother::smooth( return false; } - refinement_ctr_ = 0; steady_clock::time_point start = steady_clock::now(); double time_remaining = max_time; bool success = true, reversing_segment; @@ -69,6 +69,7 @@ bool Smoother::smooth( // Make sure we're still able to smooth with time remaining steady_clock::time_point now = steady_clock::now(); time_remaining = max_time - duration_cast>(now - start).count(); + refinement_ctr_ = 0; // Smooth path segment naively const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose; @@ -178,7 +179,7 @@ bool Smoother::smoothImpl( // Lets do additional refinement, it shouldn't take more than a couple milliseconds // but really puts the path quality over the top. - if (do_refinement_ && refinement_ctr_ < 4) { + if (do_refinement_ && refinement_ctr_ < refinement_num_) { refinement_ctr_++; smoothImpl(new_path, reversing_segment, costmap, max_time); } diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 95f19dc54c..2e8fea4a94 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -121,7 +121,7 @@ class SimpleSmoother : public nav2_core::Smoother const double & value); double tolerance_, data_w_, smooth_w_; - int max_its_, refinement_ctr_; + int max_its_, refinement_ctr_, refinement_num_; bool do_refinement_; std::shared_ptr costmap_sub_; rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")}; diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index d04d88bc61..2db6ce3c92 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -44,12 +44,15 @@ void SimpleSmoother::configure( node, name + ".w_smooth", rclcpp::ParameterValue(0.3)); declare_parameter_if_not_declared( node, name + ".do_refinement", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, name + ".refinement_num", rclcpp::ParameterValue(2)); node->get_parameter(name + ".tolerance", tolerance_); node->get_parameter(name + ".max_its", max_its_); node->get_parameter(name + ".w_data", data_w_); node->get_parameter(name + ".w_smooth", smooth_w_); node->get_parameter(name + ".do_refinement", do_refinement_); + node->get_parameter(name + ".refinement_num", refinement_num_); } bool SimpleSmoother::smooth( @@ -58,7 +61,6 @@ bool SimpleSmoother::smooth( { auto costmap = costmap_sub_->getCostmap(); - refinement_ctr_ = 0; steady_clock::time_point start = steady_clock::now(); double time_remaining = max_time.seconds(); @@ -80,6 +82,7 @@ bool SimpleSmoother::smooth( // Make sure we're still able to smooth with time remaining steady_clock::time_point now = steady_clock::now(); time_remaining = max_time.seconds() - duration_cast>(now - start).count(); + refinement_ctr_ = 0; // Smooth path segment naively success = success && smoothImpl( @@ -180,7 +183,7 @@ bool SimpleSmoother::smoothImpl( // Lets do additional refinement, it shouldn't take more than a couple milliseconds // but really puts the path quality over the top. - if (do_refinement_ && refinement_ctr_ < 4) { + if (do_refinement_ && refinement_ctr_ < refinement_num_) { refinement_ctr_++; smoothImpl(new_path, reversing_segment, costmap, max_time); } diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index 7d9b56d457..fc3dceec97 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -128,7 +128,7 @@ TEST(SmootherTest, test_simple_smoother) EXPECT_LT( fabs( straight_irregular_path.poses[i].pose.position.y - - straight_irregular_path.poses[i + 1].pose.position.y), 0.38); + straight_irregular_path.poses[i + 1].pose.position.y), 0.50); } // Test regular path, should see no effective change @@ -181,8 +181,8 @@ TEST(SmootherTest, test_simple_smoother) straight_regular_path.poses[10].pose.position.x = 0.95; straight_regular_path.poses[10].pose.position.y = 0.5; EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time)); - EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.637, 0.01); - EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.353, 0.01); + EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.607, 0.01); + EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.387, 0.01); // Test that collisions are rejected nav_msgs::msg::Path collision_path; From a13d642cca5c6abafda0faa9bd1657bc2645d1ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Alejandro=20Gonz=C3=A1lez?= <71234974+pepisg@users.noreply.github.com> Date: Wed, 16 Nov 2022 16:43:40 -0500 Subject: [PATCH 102/131] Add allow_unknown parameter to theta star planner (#3286) * Add allow unknown parameter to theta star planner * Add allow unknown parameter to tests * missing comma * Change cost of unknown tiles * Uncrustify --- .../include/nav2_theta_star_planner/theta_star.hpp | 13 +++++++++++-- nav2_theta_star_planner/src/theta_star.cpp | 1 + nav2_theta_star_planner/src/theta_star_planner.cpp | 6 ++++++ nav2_theta_star_planner/test/test_theta_star.cpp | 4 +++- 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index dacd8a57bc..d1ddf7354c 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -24,6 +24,8 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" const double INF_COST = DBL_MAX; +const int UNKNOWN_COST = 255; +const int OBS_COST = 254; const int LETHAL_COST = 252; struct coordsM @@ -70,6 +72,8 @@ class ThetaStar double w_heuristic_cost_; /// parameter to set the number of adjacent nodes to be searched on int how_many_corners_; + /// parameter to set weather the planner can plan through unknown space + bool allow_unknown_; /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; @@ -91,7 +95,9 @@ class ThetaStar */ inline bool isSafe(const int & cx, const int & cy) const { - return costmap_->getCost(cx, cy) < LETHAL_COST; + return (costmap_->getCost( + cx, + cy) == UNKNOWN_COST && allow_unknown_) || costmap_->getCost(cx, cy) < LETHAL_COST; } /** @@ -185,7 +191,10 @@ class ThetaStar bool isSafe(const int & cx, const int & cy, double & cost) const { double curr_cost = getCost(cx, cy); - if (curr_cost < LETHAL_COST) { + if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || curr_cost < LETHAL_COST) { + if (costmap_->getCost(cx, cy) == UNKNOWN_COST) { + curr_cost = OBS_COST - 1; + } cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; return true; } else { diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index e0267b34c0..baddc754d7 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -23,6 +23,7 @@ ThetaStar::ThetaStar() w_euc_cost_(2.0), w_heuristic_cost_(1.0), how_many_corners_(8), + allow_unknown_(true), size_x_(0), size_y_(0), index_generated_(0) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 9f4930da72..c509afb065 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -45,6 +45,10 @@ void ThetaStarPlanner::configure( RCLCPP_WARN(logger_, "Your value for - .how_many_corners was overridden, and is now set to 8"); } + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + ".allow_unknown", planner_->allow_unknown_); + nav2_util::declare_parameter_if_not_declared( node, name_ + ".w_euc_cost", rclcpp::ParameterValue(1.0)); node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); @@ -237,6 +241,8 @@ ThetaStarPlanner::dynamicParametersCallback(std::vector param } else if (type == ParameterType::PARAMETER_BOOL) { if (name == name_ + ".use_final_approach_orientation") { use_final_approach_orientation_ = parameter.as_bool(); + } else if (name == name_ + ".allow_unknown") { + planner_->allow_unknown_ = parameter.as_bool(); } } } diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 44b37f4ea7..c2360d9f87 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -212,7 +212,8 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) {rclcpp::Parameter("test.how_many_corners", 8), rclcpp::Parameter("test.w_euc_cost", 1.0), rclcpp::Parameter("test.w_traversal_cost", 2.0), - rclcpp::Parameter("test.use_final_approach_orientation", false)}); + rclcpp::Parameter("test.use_final_approach_orientation", false), + rclcpp::Parameter("test.allow_unknown", false)}); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), @@ -224,6 +225,7 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) 1.0); EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0); EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false); + EXPECT_EQ(life_node->get_parameter("test.allow_unknown").as_bool(), false); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), From 95c951d3e5a9cea3ef0078061313fa2fba342241 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Thu, 17 Nov 2022 00:11:30 +0100 Subject: [PATCH 103/131] Include test cases waypoint follwer (#3288) * WIP * included missed_waypoing check * finished inclding test * fix format * return default sleep value --- .../src/waypoint_follower/tester.py | 55 ++++++++++++++++--- 1 file changed, 47 insertions(+), 8 deletions(-) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index e6d299caa5..2579513797 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -20,10 +20,12 @@ from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped from nav2_msgs.action import FollowWaypoints from nav2_msgs.srv import ManageLifecycleNodes +from rcl_interfaces.srv import SetParameters import rclpy from rclpy.action import ActionClient from rclpy.node import Node +from rclpy.parameter import Parameter from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy from rclpy.qos import QoSProfile @@ -38,6 +40,7 @@ def __init__(self): 'initialpose', 10) self.initial_pose_received = False self.goal_handle = None + self.action_result = None pose_qos = QoSProfile( durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, @@ -47,6 +50,8 @@ def __init__(self): self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos) + self.param_cli = self.create_client(SetParameters, + '/waypoint_follower/set_parameters') def setInitialPose(self, pose): self.init_pose = PoseWithCovarianceStamped() @@ -70,10 +75,10 @@ def setWaypoints(self, waypoints): msg.pose.orientation.w = 1.0 self.waypoints.append(msg) - def run(self, block): - if not self.waypoints: - rclpy.error_msg('Did not set valid waypoints before running test!') - return False + def run(self, block, cancel): + # if not self.waypoints: + # rclpy.error_msg('Did not set valid waypoints before running test!') + # return False while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg("'follow_waypoints' action server not available, waiting...") @@ -98,12 +103,16 @@ def run(self, block): return True get_result_future = self.goal_handle.get_result_async() + if cancel: + time.sleep(2) + self.cancel_goal() self.info_msg("Waiting for 'follow_waypoints' action to complete") try: rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status result = get_result_future.result().result + self.action_result = result except Exception as e: # noqa: B902 self.error_msg(f'Service call failed {e!r}') @@ -121,6 +130,13 @@ def run(self, block): def publishInitialPose(self): self.initial_pose_pub.publish(self.init_pose) + def setStopFailureParam(self, value): + req = SetParameters.Request() + req.parameters = [Parameter('stop_on_failure', + Parameter.Type.BOOL, value).to_parameter_msg()] + future = self.param_cli.call_async(req) + rclpy.spin_until_future_complete(self, future) + def shutdown(self): self.info_msg('Shutting down') @@ -194,15 +210,15 @@ def main(argv=sys.argv[1:]): test.info_msg('Waiting for amcl_pose to be received') rclpy.spin_once(test, timeout_sec=1.0) # wait for poseCallback - result = test.run(True) + result = test.run(True, False) assert result # preempt with new point test.setWaypoints([starting_pose]) - result = test.run(False) + result = test.run(False, False) time.sleep(2) test.setWaypoints([wps[1]]) - result = test.run(False) + result = test.run(False, False) # cancel time.sleep(2) @@ -211,7 +227,30 @@ def main(argv=sys.argv[1:]): # a failure case time.sleep(2) test.setWaypoints([[100.0, 100.0]]) - result = test.run(True) + result = test.run(True, False) + assert not result + result = not result + + # stop on failure test with bogous waypoint + test.setStopFailureParam(True) + bwps = [[-0.52, -0.54], [100.0, 100.0], [0.58, 0.52]] + starting_pose = [-2.0, -0.5] + test.setWaypoints(bwps) + test.setInitialPose(starting_pose) + result = test.run(True, False) + assert not result + result = not result + mwps = test.action_result.missed_waypoints + result = (len(mwps) == 1) & (mwps[0] == 1) + test.setStopFailureParam(False) + + # Zero goal test + test.setWaypoints([]) + result = test.run(True, False) + + # Cancel test + test.setWaypoints(wps) + result = test.run(True, True) assert not result result = not result From ab5dcd96938cc05010aff8d02f9ff3afe212de97 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Thu, 17 Nov 2022 22:31:59 +0300 Subject: [PATCH 104/131] Dynamically changing polygons support (#3245) * Add Collision Monitor polygon topics subscription * Add the support of polygons published in different frame * Internal review * Fix working with polygons visualization * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Move getTransform to nav2_util * Fix misprint * Meet remaining review items: * Update polygon params handling logic * Warn if polygon shape was not set * Publish with ownership movement * Correct polygons_test.cpp parameters handling logic * Adjust README for dynamic polygons logic update Co-authored-by: Steve Macenski --- nav2_collision_monitor/README.md | 8 +- .../include/nav2_collision_monitor/circle.hpp | 11 +- .../nav2_collision_monitor/polygon.hpp | 37 ++- .../include/nav2_collision_monitor/source.hpp | 15 -- nav2_collision_monitor/src/circle.cpp | 8 +- .../src/collision_monitor_node.cpp | 8 + nav2_collision_monitor/src/pointcloud.cpp | 8 +- nav2_collision_monitor/src/polygon.cpp | 191 +++++++++++----- nav2_collision_monitor/src/range.cpp | 8 +- nav2_collision_monitor/src/scan.cpp | 9 +- nav2_collision_monitor/src/source.cpp | 32 --- nav2_collision_monitor/test/polygons_test.cpp | 213 +++++++++++++++--- nav2_util/include/nav2_util/robot_utils.hpp | 53 +++++ nav2_util/src/robot_utils.cpp | 67 ++++++ 14 files changed, 526 insertions(+), 142 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 54841675e9..788a77ebd4 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -27,9 +27,9 @@ The following models of safety behaviors are employed by Collision Monitor: The zones around the robot can take the following shapes: -* Arbitrary user-defined polygon relative to the robot base frame. -* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape. -* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time. +* Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface. +* Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time. +* Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape. The data may be obtained from different data sources: @@ -43,7 +43,7 @@ The Collision Monitor is designed to operate below Nav2 as an independent safety This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate. The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model. -![HDL.png](doc/HLD.png) +![HLD.png](doc/HLD.png) ## Configuration diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index 807d4b3dc7..9cab7485be 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -66,15 +66,24 @@ class Circle : public Polygon */ int getPointsInside(const std::vector & points) const override; + /** + * @brief Specifies that the shape is always set for a circle object + */ + bool isShapeSet() override {return true;} + protected: /** * @brief Supporting routine obtaining polygon-specific ROS-parameters + * @brief polygon_sub_topic Input name of polygon subscription topic * @param polygon_pub_topic Output name of polygon publishing topic * @param footprint_topic Output name of footprint topic. For Circle returns empty string, * there is no footprint subscription in this class. * @return True if all parameters were obtained or false in failure case */ - bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) override; + bool getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic) override; // ----- Variables ----- diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 97423dc411..e1354e434f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -21,7 +21,6 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "geometry_msgs/msg/polygon.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -110,6 +109,12 @@ class Polygon */ virtual void getPolygon(std::vector & poly) const; + /** + * @brief Returns true if polygon points were set. + * Othewise, prints a warning and returns false. + */ + virtual bool isShapeSet(); + /** * @brief Updates polygon from footprint subscriber (if any) */ @@ -138,7 +143,7 @@ class Polygon /** * @brief Publishes polygon message into a its own topic */ - void publish() const; + void publish(); protected: /** @@ -150,11 +155,29 @@ class Polygon /** * @brief Supporting routine obtaining polygon-specific ROS-parameters + * @brief polygon_sub_topic Output name of polygon subscription topic. + * Empty, if no polygon subscription. * @param polygon_pub_topic Output name of polygon publishing topic - * @param footprint_topic Output name of footprint topic. Empty, if no footprint subscription + * @param footprint_topic Output name of footprint topic. + * Empty, if no footprint subscription. * @return True if all parameters were obtained or false in failure case */ - virtual bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic); + virtual bool getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic); + + /** + * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message + * @param msg Message to update polygon from + */ + void updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); + + /** + * @brief Dynamic polygon callback + * @param msg Shared pointer to the polygon message + */ + void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); /** * @brief Checks if point is inside polygon @@ -183,6 +206,8 @@ class Polygon double time_before_collision_; /// @brief Time step for robot movement simulation double simulation_time_step_; + /// @brief Polygon subscription + rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; @@ -197,8 +222,8 @@ class Polygon // Visualization /// @brief Whether to publish the polygon bool visualize_; - /// @brief Polygon points stored for later publishing - geometry_msgs::msg::Polygon polygon_; + /// @brief Polygon stored for later publishing + geometry_msgs::msg::PolygonStamped polygon_; /// @brief Polygon publisher for visualization purposes rclcpp_lifecycle::LifecyclePublisher::SharedPtr polygon_pub_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index a24859bb4a..5fd95de7ee 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -88,21 +88,6 @@ class Source const rclcpp::Time & source_time, const rclcpp::Time & curr_time) const; - /** - * @brief Obtains a transform from source_frame_id at source_time -> - * to base_frame_id_ at curr_time time - * @param source_frame_id Source frame ID to convert data from - * @param source_time Source timestamp to convert data from - * @param curr_time Current node time to interpolate data to - * @param tf_transform Output source->base transform - * @return True if got correct transform, otherwise false - */ - bool getTransform( - const std::string & source_frame_id, - const rclcpp::Time & source_time, - const rclcpp::Time & curr_time, - tf2::Transform & tf_transform) const; - // ----- Variables ----- /// @brief Collision Monitor node diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index 7dc9708967..fa0fc33a4f 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -70,7 +70,10 @@ int Circle::getPointsInside(const std::vector & points) const return num; } -bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) +bool Circle::getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic) { auto node = node_.lock(); if (!node) { @@ -81,7 +84,8 @@ bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footpr return false; } - // There is no footprint subscription for the Circle. Thus, set string as empty. + // There is no polygon or footprint subscription for the Circle. Thus, set strings as empty. + polygon_sub_topic.clear(); footprint_topic.clear(); try { diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 313d71fb0a..d706ba0cd9 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -389,6 +389,10 @@ bool CollisionMonitor::processStopSlowdown( const Velocity & velocity, Action & robot_action) const { + if (!polygon->isShapeSet()) { + return false; + } + if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) { if (polygon->getActionType() == STOP) { // Setting up zero velocity for STOP model @@ -420,6 +424,10 @@ bool CollisionMonitor::processApproach( { polygon->updatePolygon(); + if (!polygon->isShapeSet()) { + return false; + } + // Obtain time before a collision const double collision_time = polygon->getCollisionTime(collision_points, velocity); if (collision_time >= 0.0) { diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index df4e86b63e..282dec5463 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -19,6 +19,7 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor { @@ -78,7 +79,12 @@ void PointCloud::getData( // Obtaining the transform to get data from source frame and time where it was received // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { return; } diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index d17c34b46a..824ee75f67 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -21,6 +21,7 @@ #include "geometry_msgs/msg/point32.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_collision_monitor/kinematics.hpp" @@ -43,6 +44,8 @@ Polygon::Polygon( Polygon::~Polygon() { RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str()); + polygon_sub_.reset(); + polygon_pub_.reset(); poly_.clear(); } @@ -53,20 +56,36 @@ bool Polygon::configure() throw std::runtime_error{"Failed to lock node"}; } - std::string polygon_pub_topic, footprint_topic; + std::string polygon_sub_topic, polygon_pub_topic, footprint_topic; - if (!getParameters(polygon_pub_topic, footprint_topic)) { + if (!getParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) { return false; } + if (!polygon_sub_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Subscribing on %s topic for polygon", + polygon_name_.c_str(), polygon_sub_topic.c_str()); + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + polygon_sub_ = node->create_subscription( + polygon_sub_topic, polygon_qos, + std::bind(&Polygon::polygonCallback, this, std::placeholders::_1)); + } + if (!footprint_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Making footprint subscriber on %s topic", + polygon_name_.c_str(), footprint_topic.c_str()); footprint_sub_ = std::make_unique( node, footprint_topic, *tf_buffer_, base_frame_id_, tf2::durationToSec(transform_tolerance_)); } if (visualize_) { - // Fill polygon_ points for future usage + // Fill polygon_ for future usage + polygon_.header.frame_id = base_frame_id_; std::vector poly; getPolygon(poly); for (const Point & p : poly) { @@ -74,7 +93,7 @@ bool Polygon::configure() p_s.x = p.x; p_s.y = p.y; // p_s.z will remain 0.0 - polygon_.points.push_back(p_s); + polygon_.polygon.points.push_back(p_s); } rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default @@ -129,6 +148,15 @@ void Polygon::getPolygon(std::vector & poly) const poly = poly_; } +bool Polygon::isShapeSet() +{ + if (poly_.empty()) { + RCLCPP_WARN(logger_, "[%s]: Polygon shape is not set yet", polygon_name_.c_str()); + return false; + } + return true; +} + void Polygon::updatePolygon() { if (footprint_sub_ != nullptr) { @@ -139,14 +167,15 @@ void Polygon::updatePolygon() std::size_t new_size = footprint_vec.size(); poly_.resize(new_size); - polygon_.points.resize(new_size); + polygon_.header.frame_id = base_frame_id_; + polygon_.polygon.points.resize(new_size); geometry_msgs::msg::Point32 p_s; for (std::size_t i = 0; i < new_size; i++) { poly_[i] = {footprint_vec[i].x, footprint_vec[i].y}; p_s.x = footprint_vec[i].x; p_s.y = footprint_vec[i].y; - polygon_.points[i] = p_s; + polygon_.polygon.points[i] = p_s; } } } @@ -192,7 +221,7 @@ double Polygon::getCollisionTime( return -1.0; } -void Polygon::publish() const +void Polygon::publish() { if (!visualize_) { return; @@ -203,15 +232,10 @@ void Polygon::publish() const throw std::runtime_error{"Failed to lock node"}; } - // Fill PolygonStamped struct - std::unique_ptr poly_s = - std::make_unique(); - poly_s->header.stamp = node->now(); - poly_s->header.frame_id = base_frame_id_; - poly_s->polygon = polygon_; - - // Publish polygon - polygon_pub_->publish(std::move(poly_s)); + // Actualize the time to current and publish the polygon + polygon_.header.stamp = node->now(); + auto msg = std::make_unique(polygon_); + polygon_pub_->publish(std::move(msg)); } bool Polygon::getCommonParameters(std::string & polygon_pub_topic) @@ -280,7 +304,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) return true; } -bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) +bool Polygon::getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic) { auto node = node_.lock(); if (!node) { @@ -291,48 +318,62 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp return false; } + // Clear the subscription topics. They will be set later, if necessary. + polygon_sub_topic.clear(); + footprint_topic.clear(); + try { - if (action_type_ == APPROACH) { - // Obtain the footprint topic to make a footprint subscription for approach polygon + try { + // Leave it uninitialized: it will throw an inner exception if the parameter is not set nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".footprint_topic", - rclcpp::ParameterValue("local_costmap/published_footprint")); - footprint_topic = - node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); + node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + std::vector poly_row = + node->get_parameter(polygon_name_ + ".points").as_double_array(); + // Check for points format correctness + if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { + RCLCPP_ERROR( + logger_, + "[%s]: Polygon has incorrect points description", + polygon_name_.c_str()); + return false; + } - // This is robot footprint: do not need to get polygon points from ROS parameters. - // It will be set dynamically later. - return true; - } else { - // Make it empty otherwise - footprint_topic.clear(); - } + // Obtain polygon vertices + Point point; + bool first = true; + for (double val : poly_row) { + if (first) { + point.x = val; + } else { + point.y = val; + poly_.push_back(point); + } + first = !first; + } - // Leave it not initialized: the will cause an error if it will not set - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); - std::vector poly_row = - node->get_parameter(polygon_name_ + ".points").as_double_array(); - // Check for points format correctness - if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { - RCLCPP_ERROR( + // Do not need to proceed further, if "points" parameter is defined. + // Static polygon will be used. + return true; + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + RCLCPP_INFO( logger_, - "[%s]: Polygon has incorrect points description", + "[%s]: Polygon points are not defined. Using dynamic subscription instead.", polygon_name_.c_str()); - return false; } - // Obtain polygon vertices - Point point; - bool first = true; - for (double val : poly_row) { - if (first) { - point.x = val; - } else { - point.y = val; - poly_.push_back(point); - } - first = !first; + if (action_type_ == STOP || action_type_ == SLOWDOWN) { + // Dynamic polygon will be used + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING); + polygon_sub_topic = + node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string(); + } else if (action_type_ == APPROACH) { + // Obtain the footprint topic to make a footprint subscription for approach polygon + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".footprint_topic", + rclcpp::ParameterValue("local_costmap/published_footprint")); + footprint_topic = + node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); } } catch (const std::exception & ex) { RCLCPP_ERROR( @@ -345,6 +386,54 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp return true; } +void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) +{ + std::size_t new_size = msg->polygon.points.size(); + + if (new_size < 3) { + RCLCPP_ERROR( + logger_, + "[%s]: Polygon should have at least 3 points", + polygon_name_.c_str()); + return; + } + + // Get the transform from PolygonStamped frame to base_frame_id_ + tf2::Transform tf_transform; + if ( + !nav2_util::getTransform( + msg->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + + // Set main polygon vertices + poly_.resize(new_size); + for (std::size_t i = 0; i < new_size; i++) { + // Transform point coordinates from PolygonStamped frame -> to base frame + tf2::Vector3 p_v3_s(msg->polygon.points[i].x, msg->polygon.points[i].y, 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Fill poly_ array + poly_[i] = {p_v3_b.x(), p_v3_b.y()}; + } + + if (visualize_) { + // Store polygon_ for visualization + polygon_ = *msg; + } +} + +void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) +{ + RCLCPP_INFO( + logger_, + "[%s]: Polygon shape update has been arrived", + polygon_name_.c_str()); + updatePolygon(msg); +} + inline bool Polygon::isPointInside(const Point & point) const { // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 3ef51e2b69..0ba4be75aa 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -19,6 +19,7 @@ #include #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor { @@ -87,7 +88,12 @@ void Range::getData( // Obtaining the transform to get data from source frame and time where it was received // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { return; } diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index d1f52b31f1..63ea1d6a76 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -17,6 +17,8 @@ #include #include +#include "nav2_util/robot_utils.hpp" + namespace nav2_collision_monitor { @@ -76,7 +78,12 @@ void Scan::getData( // Obtaining the transform to get data from source frame and time where it was received // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { return; } diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index bd1028518c..a7de3bbe4e 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -18,9 +18,6 @@ #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2/convert.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - #include "nav2_util/node_utils.hpp" namespace nav2_collision_monitor @@ -76,33 +73,4 @@ bool Source::sourceValid( return true; } -bool Source::getTransform( - const std::string & source_frame_id, - const rclcpp::Time & source_time, - const rclcpp::Time & curr_time, - tf2::Transform & tf2_transform) const -{ - geometry_msgs::msg::TransformStamped transform; - tf2_transform.setIdentity(); // initialize by identical transform - - try { - // Obtaining the transform to get data from source to base frame. - // This also considers the time shift between source and base. - transform = tf_buffer_->lookupTransform( - base_frame_id_, curr_time, - source_frame_id, source_time, - global_frame_id_, transform_tolerance_); - } catch (tf2::TransformException & e) { - RCLCPP_ERROR( - logger_, - "[%s]: Failed to get \"%s\"->\"%s\" frame transform: %s", - source_name_.c_str(), source_frame_id.c_str(), base_frame_id_.c_str(), e.what()); - return false; - } - - // Convert TransformStamped to TF2 transform - tf2::fromMsg(transform.transform, tf2_transform); - return true; -} - } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 87576ddda9..c8c6b46601 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -40,8 +40,10 @@ using namespace std::chrono_literals; static constexpr double EPSILON = std::numeric_limits::epsilon(); static const char BASE_FRAME_ID[]{"base_link"}; +static const char BASE2_FRAME_ID[]{"base2_link"}; static const char FOOTPRINT_TOPIC[]{"footprint"}; -static const char POLYGON_PUB_TOPIC[]{"polygon"}; +static const char POLYGON_SUB_TOPIC[]{"polygon_sub"}; +static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; static const char POLYGON_NAME[]{"TestPolygon"}; static const char CIRCLE_NAME[]{"TestCircle"}; static const std::vector SQUARE_POLYGON { @@ -68,9 +70,38 @@ class TestNode : public nav2_util::LifecycleNode ~TestNode() { + polygon_pub_.reset(); footprint_pub_.reset(); } + void publishPolygon(const std::string & frame_id, const bool is_correct) + { + polygon_pub_ = this->create_publisher( + POLYGON_SUB_TOPIC, rclcpp::SystemDefaultsQoS()); + + std::unique_ptr msg = + std::make_unique(); + + unsigned int polygon_size; + if (is_correct) { + polygon_size = SQUARE_POLYGON.size(); + } else { + polygon_size = 2; + } + + msg->header.frame_id = frame_id; + msg->header.stamp = this->now(); + + geometry_msgs::msg::Point32 p; + for (unsigned int i = 0; i < polygon_size; i = i + 2) { + p.x = SQUARE_POLYGON[i]; + p.y = SQUARE_POLYGON[i + 1]; + msg->polygon.points.push_back(p); + } + + polygon_pub_->publish(std::move(msg)); + } + void publishFootprint() { footprint_pub_ = this->create_publisher( @@ -112,6 +143,7 @@ class TestNode : public nav2_util::LifecycleNode } private: + rclcpp::Publisher::SharedPtr polygon_pub_; rclcpp::Publisher::SharedPtr footprint_pub_; rclcpp::Subscription::SharedPtr polygon_sub_; @@ -177,13 +209,22 @@ class Tester : public ::testing::Test protected: // Working with parameters void setCommonParameters(const std::string & polygon_name, const std::string & action_type); - void setPolygonParameters(const std::vector & points); + void setPolygonParameters( + const std::vector & points, + const bool is_static); void setCircleParameters(const double radius); bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); // Creating routines - void createPolygon(const std::string & action_type); + void createPolygon(const std::string & action_type, const bool is_static); void createCircle(const std::string & action_type); + void sendTransforms(); + + // Wait until polygon will be received + bool waitPolygon( + const std::chrono::nanoseconds & timeout, + std::vector & poly); + // Wait until footprint will be received bool waitFootprint( const std::chrono::nanoseconds & timeout, @@ -257,17 +298,25 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC)); } -void Tester::setPolygonParameters(const std::vector & points) +void Tester::setPolygonParameters( + const std::vector & points, const bool is_static) { - test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".footprint_topic", FOOTPRINT_TOPIC)); - - test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points)); + if (is_static) { + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points)); + } else { + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".polygon_sub_topic", rclcpp::ParameterValue(POLYGON_SUB_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".polygon_sub_topic", POLYGON_SUB_TOPIC)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".footprint_topic", FOOTPRINT_TOPIC)); + } } void Tester::setCircleParameters(const double radius) @@ -296,10 +345,10 @@ bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const st return ret; } -void Tester::createPolygon(const std::string & action_type) +void Tester::createPolygon(const std::string & action_type, const bool is_static) { setCommonParameters(POLYGON_NAME, action_type); - setPolygonParameters(SQUARE_POLYGON); + setPolygonParameters(SQUARE_POLYGON, is_static); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -320,6 +369,45 @@ void Tester::createCircle(const std::string & action_type) circle_->activate(); } +void Tester::sendTransforms() +{ + std::shared_ptr tf_broadcaster = + std::make_shared(test_node_); + + geometry_msgs::msg::TransformStamped transform; + + // base_frame -> base2_frame transform + transform.header.frame_id = BASE_FRAME_ID; + transform.child_frame_id = BASE2_FRAME_ID; + + transform.header.stamp = test_node_->now(); + transform.transform.translation.x = 0.1; + transform.transform.translation.y = 0.1; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +bool Tester::waitPolygon( + const std::chrono::nanoseconds & timeout, + std::vector & poly) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + polygon_->getPolygon(poly); + if (poly.size() > 0) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + bool Tester::waitFootprint( const std::chrono::nanoseconds & timeout, std::vector & footprint) @@ -339,7 +427,7 @@ bool Tester::waitFootprint( TEST_F(Tester, testPolygonGetStopParameters) { - createPolygon("stop"); + createPolygon("stop", true); // Check that common parameters set correctly EXPECT_EQ(polygon_->getName(), POLYGON_NAME); @@ -363,7 +451,7 @@ TEST_F(Tester, testPolygonGetStopParameters) TEST_F(Tester, testPolygonGetSlowdownParameters) { - createPolygon("slowdown"); + createPolygon("slowdown", true); // Check that common parameters set correctly EXPECT_EQ(polygon_->getName(), POLYGON_NAME); @@ -376,7 +464,7 @@ TEST_F(Tester, testPolygonGetSlowdownParameters) TEST_F(Tester, testPolygonGetApproachParameters) { - createPolygon("approach"); + createPolygon("approach", true); // Check that common parameters set correctly EXPECT_EQ(polygon_->getName(), POLYGON_NAME); @@ -415,7 +503,7 @@ TEST_F(Tester, testPolygonUndeclaredActionType) TEST_F(Tester, testPolygonUndeclaredPoints) { - // "points" parameter is not initialized + // "points" and "polygon_sub_topic" parameters are not initialized test_node_->declare_parameter( std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( @@ -424,14 +512,15 @@ TEST_F(Tester, testPolygonUndeclaredPoints) test_node_, POLYGON_NAME, tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(polygon_->configure()); - // Check that "points" parameter is not set after configuring + // Check that "points" and "polygon_sub_topic" parameters are not set after configuring ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "points")); + ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "polygon_sub_topic")); } TEST_F(Tester, testPolygonIncorrectActionType) { setCommonParameters(POLYGON_NAME, "incorrect_action_type"); - setPolygonParameters(SQUARE_POLYGON); + setPolygonParameters(SQUARE_POLYGON, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -486,12 +575,79 @@ TEST_F(Tester, testCircleUndeclaredRadius) ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius")); } -TEST_F(Tester, testPolygonUpdate) +TEST_F(Tester, testPolygonTopicUpdate) +{ + createPolygon("stop", false); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_FALSE(polygon_->isShapeSet()); + ASSERT_EQ(poly.size(), 0u); + + // Publish incorrect shape and check that polygon was not updated + test_node_->publishPolygon(BASE_FRAME_ID, false); + ASSERT_FALSE(waitPolygon(100ms, poly)); + ASSERT_FALSE(polygon_->isShapeSet()); + + // Publush correct polygon and make shure that it was set correctly + test_node_->publishPolygon(BASE_FRAME_ID, true); + ASSERT_TRUE(waitPolygon(500ms, poly)); + ASSERT_TRUE(polygon_->isShapeSet()); + ASSERT_EQ(poly.size(), 4u); + EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0], EPSILON); + EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1], EPSILON); + EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2], EPSILON); + EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3], EPSILON); + EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4], EPSILON); + EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5], EPSILON); + EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6], EPSILON); + EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON); +} + +TEST_F(Tester, testPolygonTopicUpdateDifferentFrame) +{ + createPolygon("stop", false); + sendTransforms(); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_EQ(poly.size(), 0u); + + // Publush polygon in different frame and make shure that it was set correctly + test_node_->publishPolygon(BASE2_FRAME_ID, true); + ASSERT_TRUE(waitPolygon(500ms, poly)); + ASSERT_EQ(poly.size(), 4u); + EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0] + 0.1, EPSILON); + EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1] + 0.1, EPSILON); + EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2] + 0.1, EPSILON); + EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3] + 0.1, EPSILON); + EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4] + 0.1, EPSILON); + EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5] + 0.1, EPSILON); + EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6] + 0.1, EPSILON); + EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7] + 0.1, EPSILON); +} + +TEST_F(Tester, testPolygonTopicUpdateIncorrectFrame) +{ + createPolygon("stop", false); + sendTransforms(); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_EQ(poly.size(), 0u); + + // Publush polygon in incorrect frame and check that polygon was not updated + test_node_->publishPolygon("incorrect_frame", true); + ASSERT_FALSE(waitPolygon(100ms, poly)); +} + +TEST_F(Tester, testPolygonFootprintUpdate) { - createPolygon("approach"); + createPolygon("approach", false); std::vector poly; polygon_->getPolygon(poly); + ASSERT_FALSE(polygon_->isShapeSet()); ASSERT_EQ(poly.size(), 0u); test_node_->publishFootprint(); @@ -499,6 +655,7 @@ TEST_F(Tester, testPolygonUpdate) std::vector footprint; ASSERT_TRUE(waitFootprint(500ms, footprint)); + ASSERT_TRUE(polygon_->isShapeSet()); ASSERT_EQ(footprint.size(), 4u); EXPECT_NEAR(footprint[0].x, SQUARE_POLYGON[0], EPSILON); EXPECT_NEAR(footprint[0].y, SQUARE_POLYGON[1], EPSILON); @@ -512,7 +669,7 @@ TEST_F(Tester, testPolygonUpdate) TEST_F(Tester, testPolygonGetPointsInside) { - createPolygon("stop"); + createPolygon("stop", true); std::vector points; @@ -533,7 +690,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge) // Test for checking edge cases in raytracing algorithm. // All points are lie on the edge lines parallel to OX, where the raytracing takes place. setCommonParameters(POLYGON_NAME, "stop"); - setPolygonParameters(ARBITRARY_POLYGON); + setPolygonParameters(ARBITRARY_POLYGON, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -572,7 +729,7 @@ TEST_F(Tester, testCircleGetPointsInside) TEST_F(Tester, testPolygonGetCollisionTime) { - createPolygon("approach"); + createPolygon("approach", false); // Set footprint for Polygon test_node_->publishFootprint(); @@ -640,7 +797,7 @@ TEST_F(Tester, testPolygonGetCollisionTime) TEST_F(Tester, testPolygonPublish) { - createPolygon("stop"); + createPolygon("stop", true); polygon_->publish(); geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received = test_node_->waitPolygonReceived(500ms); @@ -666,7 +823,7 @@ TEST_F(Tester, testPolygonDefaultVisualize) std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); - setPolygonParameters(SQUARE_POLYGON); + setPolygonParameters(SQUARE_POLYGON, true); // Create new polygon polygon_ = std::make_shared( diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index b9712044b6..22306cb545 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -18,8 +18,10 @@ #define NAV2_UTIL__ROBOT_UTILS_HPP_ #include +#include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" @@ -56,6 +58,57 @@ bool transformPoseInTargetFrame( tf2_ros::Buffer & tf_buffer, const std::string target_frame, const double transform_timeout = 0.1); +/** + * @brief Obtains a transform from source_frame_id at source_time -> + * to target_frame_id at target_time time + * @param source_frame_id Source frame ID to convert from + * @param source_time Source timestamp to convert from + * @param target_frame_id Target frame ID to convert to + * @param target_time Target time to interpolate to + * @param transform_tolerance Transform tolerance + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ + +/** + * @brief Obtains a transform from source_frame_id -> to target_frame_id + * @param source_frame_id Source frame ID to convert from + * @param target_frame_id Target frame ID to convert to + * @param transform_tolerance Transform tolerance + * @param tf_buffer TF buffer to use for the transformation + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ +bool getTransform( + const std::string & source_frame_id, + const std::string & target_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform); + +/** + * @brief Obtains a transform from source_frame_id at source_time -> + * to target_frame_id at target_time time + * @param source_frame_id Source frame ID to convert from + * @param source_time Source timestamp to convert from + * @param target_frame_id Target frame ID to convert to + * @param target_time Current node time to interpolate to + * @param fixed_frame_id The frame in which to assume the transform is constant in time + * @param transform_tolerance Transform tolerance + * @param tf_buffer TF buffer to use for the transformation + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ +bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform); + } // end namespace nav2_util #endif // NAV2_UTIL__ROBOT_UTILS_HPP_ diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index e99d9e3334..8714f05a99 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -17,6 +17,7 @@ #include #include +#include "tf2/convert.h" #include "nav2_util/robot_utils.hpp" #include "rclcpp/logger.hpp" @@ -75,4 +76,70 @@ bool transformPoseInTargetFrame( return false; } +bool getTransform( + const std::string & source_frame_id, + const std::string & target_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform) +{ + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + + if (source_frame_id == target_frame_id) { + // We are already in required frame + return true; + } + + try { + // Obtaining the transform to get data from source to target frame + transform = tf_buffer->lookupTransform( + target_frame_id, source_frame_id, + tf2::TimePointZero, transform_tolerance); + } catch (tf2::TransformException & e) { + RCLCPP_ERROR( + rclcpp::get_logger("getTransform"), + "Failed to get \"%s\"->\"%s\" frame transform: %s", + source_frame_id.c_str(), target_frame_id.c_str(), e.what()); + return false; + } + + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; +} + +bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform) +{ + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + + try { + // Obtaining the transform to get data from source to target frame. + // This also considers the time shift between source and target. + transform = tf_buffer->lookupTransform( + target_frame_id, target_time, + source_frame_id, source_time, + fixed_frame_id, transform_tolerance); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("getTransform"), + "Failed to get \"%s\"->\"%s\" frame transform: %s", + source_frame_id.c_str(), target_frame_id.c_str(), ex.what()); + return false; + } + + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; +} + } // end namespace nav2_util From 1f74b4d39200febec367354bfacbfac1ffed4250 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 17 Nov 2022 15:03:20 -0800 Subject: [PATCH 105/131] adding getCostScalingFactor (#3290) --- nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp | 5 +++++ nav2_costmap_2d/plugins/inflation_layer.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 5f68dcaed1..c330cd4ef6 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -172,6 +172,11 @@ class InflationLayer : public Layer return access_; } + double getCostScalingFactor() + { + return cost_scaling_factor_; + } + protected: /** * @brief Process updates on footprint changes to the inflation layer diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 796c2fd62f..067877e548 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -442,7 +442,7 @@ InflationLayer::dynamicParametersCallback( need_reinflation_ = true; need_cache_recompute = true; } else if (param_name == name_ + "." + "cost_scaling_factor" && // NOLINT - cost_scaling_factor_ != parameter.as_double()) + getCostScalingFactor() != parameter.as_double()) { cost_scaling_factor_ = parameter.as_double(); need_reinflation_ = true; From 94fde9ea8579de205bdbe31bfa0514d575b18c33 Mon Sep 17 00:00:00 2001 From: Owen Hooper <17ofh@queensu.ca> Date: Fri, 18 Nov 2022 13:40:52 -0500 Subject: [PATCH 106/131] Implemented smoother selector bt node (#3283) * Implemented smoother selector bt node Signed-off-by: Owen Hooper <17ofh@queensu.ca> * updated copyright in modified file Signed-off-by: Owen Hooper <17ofh@queensu.ca> Signed-off-by: Owen Hooper <17ofh@queensu.ca> --- nav2_behavior_tree/CMakeLists.txt | 3 + .../plugins/action/smoother_selector_node.hpp | 99 ++++++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 6 + .../plugins/action/smoother_selector_node.cpp | 92 +++++++++ .../test/plugins/action/CMakeLists.txt | 4 + .../action/test_smoother_selector_node.cpp | 187 ++++++++++++++++++ 6 files changed, 391 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/smoother_selector_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 224e8498e1..1767bdcf32 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -183,6 +183,9 @@ list(APPEND plugin_libs nav2_planner_selector_bt_node) add_library(nav2_controller_selector_bt_node SHARED plugins/action/controller_selector_node.cpp) list(APPEND plugin_libs nav2_controller_selector_bt_node) +add_library(nav2_smoother_selector_bt_node SHARED plugins/action/smoother_selector_node.cpp) +list(APPEND plugin_libs nav2_smoother_selector_bt_node) + add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp) list(APPEND plugin_libs nav2_goal_checker_selector_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp new file mode 100644 index 0000000000..f9bb293d26 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// 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__ACTION__SMOOTHER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_ + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The SmootherSelector behavior is used to switch the smoother + * that will be used by the smoother server. It subscribes to a topic "smoother_selector" + * to get the decision about what smoother must be used. It is usually used before of + * the FollowPath. The selected_smoother output port is passed to smoother_id + * input port of the FollowPath + */ +class SmootherSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::SmootherSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + SmootherSelector( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "default_smoother", + "the default smoother to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "smoother_selector", + "the input topic name to select the smoother"), + + BT::OutputPort( + "selected_smoother", + "Selected smoother by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the smoother_selector topic + * + * @param msg the message with the id of the smoother_selector + */ + void callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr smoother_selector_sub_; + + std::string last_selected_smoother_; + + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 95e5a0917d..cf3ff5f903 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -166,6 +166,12 @@ Name of the selected controller received from the topic subcription + + Name of the topic to receive smoother selection commands + Default smoother of the smoother selector + Name of the selected smoother received from the topic subcription + + Name of the topic to receive goal checker selection commands Default goal checker of the controller selector diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp new file mode 100644 index 0000000000..0a84062e08 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp @@ -0,0 +1,92 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// Copyright (c) 2022 Owen Hooper +// +// 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 "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +SmootherSelector::SmootherSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + smoother_selector_sub_ = node_->create_subscription( + topic_name_, + qos, + std::bind(&SmootherSelector::callbackSmootherSelect, this, _1), + sub_option); +} + +BT::NodeStatus SmootherSelector::tick() +{ + callback_group_executor_.spin_some(); + + // This behavior always use the last selected smoother received from the topic input. + // When no input is specified it uses the default smoother. + // If the default smoother is not specified then we work in "required smoother mode": + // In this mode, the behavior returns failure if the smoother selection is not received from + // the topic input. + if (last_selected_smoother_.empty()) { + std::string default_smoother; + getInput("default_smoother", default_smoother); + if (default_smoother.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_smoother_ = default_smoother; + } + } + + setOutput("selected_smoother", last_selected_smoother_); + + return BT::NodeStatus::SUCCESS; +} + +void +SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_smoother_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("SmootherSelector"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 0b7cae1a6c..c640e82424 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -100,6 +100,10 @@ ament_add_gtest(test_controller_selector_node test_controller_selector_node.cpp) target_link_libraries(test_controller_selector_node nav2_controller_selector_bt_node) ament_target_dependencies(test_controller_selector_node ${dependencies}) +ament_add_gtest(test_smoother_selector_node test_smoother_selector_node.cpp) +target_link_libraries(test_smoother_selector_node nav2_smoother_selector_bt_node) +ament_target_dependencies(test_smoother_selector_node ${dependencies}) + ament_add_gtest(test_goal_checker_selector_node test_goal_checker_selector_node.cpp) target_link_libraries(test_goal_checker_selector_node nav2_goal_checker_selector_bt_node) ament_target_dependencies(test_goal_checker_selector_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp new file mode 100644 index 0000000000..c59cac1b2c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// 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 "../../test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class SmootherSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("smoother_selector_test_fixture"); + 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_); + + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, config); + }; + + factory_->registerBuilder( + "SmootherSelector", + builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr SmootherSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * SmootherSelectorTestFixture::config_ = nullptr; +std::shared_ptr SmootherSelectorTestFixture::factory_ = nullptr; +std::shared_ptr SmootherSelectorTestFixture::tree_ = nullptr; + +TEST_F(SmootherSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_smoother_result; + config_->blackboard->get("selected_smoother", selected_smoother_result); + + EXPECT_EQ(selected_smoother_result, "DWB"); + + std_msgs::msg::String selected_smoother_cmd; + + selected_smoother_cmd.data = "DWC"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto smoother_selector_pub = + node_->create_publisher("smoother_selector_custom_topic_name", qos); + + // publish a few updates of the selected_smoother + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + smoother_selector_pub->publish(selected_smoother_cmd); + + rclcpp::spin_some(node_); + } + + // check smoother updated + config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_EQ("DWC", selected_smoother_result); +} + +TEST_F(SmootherSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_smoother_result; + config_->blackboard->get("selected_smoother", selected_smoother_result); + + EXPECT_EQ(selected_smoother_result, "GridBased"); + + std_msgs::msg::String selected_smoother_cmd; + + selected_smoother_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto smoother_selector_pub = + node_->create_publisher("smoother_selector", qos); + + // publish a few updates of the selected_smoother + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + smoother_selector_pub->publish(selected_smoother_cmd); + + rclcpp::spin_some(node_); + } + + // check smoother updated + config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_EQ("RRT", selected_smoother_result); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} From cb391ed48435a9a31d244b0f77ba20e97d181642 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 18 Nov 2022 14:26:27 -0500 Subject: [PATCH 107/131] Pipe error codes (#3251) * issue with finding key * passed up codes to bt_navigator * lint fix * updates * adding error_code_id back in * error codes names in params * bump error codes * lint * spelling * test fix * update behavior trees * cleanup * Update bt_action_server_impl.hpp * code review * lint * code review * log fix * error code for waypoint follower * clean up * remove waypoint error test, too flaky on CI * lint and code review * rough imp for waypoint changes * lint * code review * build fix * clean up * revert * space * remove * try to make github happ * stop gap * loading in param file * working tests :) * lint * fixed cmake * lint * lint * trigger build * added invalid plugin error * added test for piping up error codes * clean up * test waypoint follower * only launch what is needed * waypoint test * revert lines for robot navigator * fix test * waypoint test * switched to uint16 * clean up * code review * todo to note * lint * remove comment * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp Co-authored-by: Steve Macenski * rename error_codes * error code for navigate to pose * error codes for navigate through poses. * error codes for navigate through poses * message update for waypoint follower * rename to error code * update node xml Co-authored-by: Joshua Wallace Co-authored-by: Steve Macenski --- .../nav2_behavior_tree/bt_action_server.hpp | 10 + .../bt_action_server_impl.hpp | 49 +++++ .../compute_path_through_poses_action.hpp | 2 +- .../action/compute_path_to_pose_action.hpp | 2 +- .../plugins/action/follow_path_action.hpp | 2 +- .../action/navigate_through_poses_action.hpp | 21 +++ .../action/navigate_to_pose_action.hpp | 21 +++ nav2_behavior_tree/nav2_tree_nodes.xml | 9 +- .../compute_path_through_poses_action.cpp | 6 +- .../action/compute_path_to_pose_action.cpp | 6 +- .../plugins/action/follow_path_action.cpp | 6 +- .../action/navigate_through_poses_action.cpp | 20 ++ .../action/navigate_to_pose_action.cpp | 24 ++- nav2_bringup/params/nav2_params.yaml | 96 +++++----- .../behavior_trees/follow_point.xml | 4 +- ...replanning_and_if_path_becomes_invalid.xml | 4 +- ...hrough_poses_w_replanning_and_recovery.xml | 6 +- ...gate_to_pose_w_replanning_and_recovery.xml | 4 +- ..._replanning_goal_patience_and_recovery.xml | 8 +- ...eplanning_only_if_path_becomes_invalid.xml | 4 +- .../navigate_w_replanning_distance.xml | 4 +- ...e_w_replanning_only_if_goal_is_updated.xml | 4 +- ...eplanning_only_if_path_becomes_invalid.xml | 8 +- .../navigate_w_replanning_speed.xml | 4 +- .../navigate_w_replanning_time.xml | 4 +- nav2_bt_navigator/src/bt_navigator.cpp | 2 - .../src/navigators/navigate_to_pose.cpp | 1 - nav2_controller/src/controller_server.cpp | 9 +- .../nav2_core/controller_exceptions.hpp | 7 + .../include/nav2_core/planner_exceptions.hpp | 7 + .../costmap_filters/costmap_filter.hpp | 1 + nav2_msgs/CMakeLists.txt | 1 + .../action/ComputePathThroughPoses.action | 23 +-- nav2_msgs/action/ComputePathToPose.action | 21 ++- nav2_msgs/action/FollowPath.action | 17 +- nav2_msgs/action/FollowWaypoints.action | 8 +- nav2_msgs/action/NavigateThroughPoses.action | 5 + nav2_msgs/action/NavigateToPose.action | 5 + nav2_msgs/msg/MissedWaypoint.msg | 3 + nav2_planner/src/planner_server.cpp | 13 +- .../nav2_simple_commander/robot_navigator.py | 28 ++- nav2_system_tests/CMakeLists.txt | 37 ++++ nav2_system_tests/package.xml | 2 + .../src/error_codes/CMakeLists.txt | 12 ++ .../controller/controller_error_plugins.cpp | 25 +++ .../controller/controller_error_plugins.hpp | 114 ++++++++++++ .../src/error_codes/controller_plugins.xml | 26 +++ .../src/error_codes/error_code_param.yaml | 175 ++++++++++++++++++ .../planner/planner_error_plugin.cpp | 26 +++ .../planner/planner_error_plugin.hpp | 144 ++++++++++++++ .../src/error_codes/planner_plugins.xml | 42 +++++ .../src/error_codes/test_error_codes.py | 130 +++++++++++++ .../error_codes/test_error_codes_launch.py | 94 ++++++++++ .../src/planning/test_planner_plugins.cpp | 9 +- .../src/waypoint_follower/tester.py | 10 +- .../waypoint_follower.hpp | 11 +- .../src/waypoint_follower.cpp | 46 +++-- 57 files changed, 1222 insertions(+), 160 deletions(-) create mode 100644 nav2_msgs/msg/MissedWaypoint.msg create mode 100644 nav2_system_tests/src/error_codes/CMakeLists.txt create mode 100644 nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp create mode 100644 nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp create mode 100644 nav2_system_tests/src/error_codes/controller_plugins.xml create mode 100644 nav2_system_tests/src/error_codes/error_code_param.yaml create mode 100644 nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp create mode 100644 nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp create mode 100644 nav2_system_tests/src/error_codes/planner_plugins.xml create mode 100755 nav2_system_tests/src/error_codes/test_error_codes.py create mode 100755 nav2_system_tests/src/error_codes/test_error_codes_launch.py diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 7558080347..79b1df5bb4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -189,6 +189,13 @@ class BtActionServer */ void executeCallback(); + /** + * @brief updates the action server result to the highest priority error code posted on the + * blackboard + * @param result the action server result to be updated + */ + void populateErrorCode(typename std::shared_ptr result); + // Action name std::string action_name_; @@ -211,6 +218,9 @@ class BtActionServer // Libraries to pull plugins (BT Nodes) from std::vector plugin_lib_names_; + // Error code id names + std::vector error_code_names_; + // A regular, non-spinning ROS node that we can use for calls to the action client rclcpp::Node::SharedPtr client_node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index db619e1a70..c3f9244f17 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" @@ -59,6 +60,24 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + + std::vector error_code_names = { + "follow_path_error_code", + "compute_path_error_code" + }; + + if (!node->has_parameter("error_code_names")) { + std::string error_codes_str; + for (const auto & error_code : error_code_names) { + error_codes_str += error_code + "\n"; + } + RCLCPP_WARN_STREAM( + logger_, "Error_code parameters were not set. Using default values of: " + << error_codes_str + << "Make sure these match your BT and there are not other sources of error codes you want " + "reported to your application"); + node->declare_parameter("error_code_names", error_code_names); + } } template @@ -104,6 +123,9 @@ bool BtActionServer::on_configure() node->get_parameter("default_server_timeout", timeout); default_server_timeout_ = std::chrono::milliseconds(timeout); + // Get error code id names to grab off of the blackboard + error_code_names_ = node->get_parameter("error_code_names").as_string_array(); + // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -226,6 +248,9 @@ void BtActionServer::executeCallback() // Give server an opportunity to populate the result message or simple give // an indication that the action is complete. auto result = std::make_shared(); + + populateErrorCode(result); + on_completion_callback_(result, rc); switch (rc) { @@ -246,6 +271,30 @@ void BtActionServer::executeCallback() } } +template +void BtActionServer::populateErrorCode( + typename std::shared_ptr result) +{ + int highest_priority_error_code = std::numeric_limits::max(); + for (const auto & error_code : error_code_names_) { + try { + int current_error_code = blackboard_->get(error_code); + if (current_error_code != 0 && current_error_code < highest_priority_error_code) { + highest_priority_error_code = current_error_code; + } + } catch (...) { + RCLCPP_ERROR( + logger_, + "Failed to get error code: %s from blackboard", + error_code.c_str()); + } + } + + if (highest_priority_error_code != std::numeric_limits::max()) { + result->error_code = highest_priority_error_code; + } +} + } // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index ad5954b6a4..0b5f3b0972 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -86,7 +86,7 @@ class ComputePathThroughPosesAction "Mapped name to the planner plugin type to use"), BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), BT::OutputPort( - "compute_path_through_poses_error_code", "The compute path through poses error code"), + "error_code_id", "The compute path through poses error code"), }); } }; 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 1c7f0526df..829fd04428 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 @@ -86,7 +86,7 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), BT::OutputPort( - "compute_path_to_pose_error_code", "The compute path to pose error code"), + "error_code_id", "The compute path to pose error code"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index 369e7ab0f9..4460b0e9a4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -85,7 +85,7 @@ class FollowPathAction : public BtActionNode BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), BT::OutputPort( - "follow_path_error_code", "The follow path error code"), + "error_code_id", "The follow path error code"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 7e7f3d5c30..45d0adff10 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -30,6 +30,10 @@ namespace nav2_behavior_tree */ class NavigateThroughPosesAction : public BtActionNode { + using Action = nav2_msgs::action::NavigateThroughPoses; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::NavigateThroughPosesAction @@ -47,6 +51,21 @@ class NavigateThroughPosesAction : public BtActionNode("goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), + BT::OutputPort( + "error_code_id", "The navigate through poses error code"), }); } }; 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 3708017f16..51c5974390 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 @@ -30,6 +30,10 @@ namespace nav2_behavior_tree */ class NavigateToPoseAction : public BtActionNode { + using Action = nav2_msgs::action::NavigateToPose; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::NavigateToPoseAction @@ -47,6 +51,21 @@ class NavigateToPoseAction : public BtActionNode("goal", "Destination to plan to"), BT::InputPort("behavior_tree", "Behavior tree to run"), + BT::OutputPort( + "error_code_id", "Navigate to pose error code"), }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index cf3ff5f903..6b6ac08eec 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,7 +77,7 @@ Service name Server timeout Path created by ComputePathToPose node - "The compute path to pose error code" + "Compute path to pose error code" @@ -87,8 +87,7 @@ Server timeout Mapped name to the planner plugin type to use Path created by ComputePathToPose node - "The compute path through poses - pose error code" + "Compute path through poses error code" @@ -115,7 +114,7 @@ Goal checker Service name Server timeout - Follow Path error code + Follow Path error code @@ -123,6 +122,7 @@ Service name Server timeout Behavior tree to run + Navigate to pose error code @@ -130,6 +130,7 @@ Service name Server timeout Behavior tree to run + Navigate through poses error code diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index ffcbd328ae..5fede84f2c 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -42,7 +42,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success() { setOutput("path", result_.result->path); // Set empty error code, action was successful - setOutput("compute_path_through_poses_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -50,7 +50,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - setOutput("compute_path_through_poses_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -59,7 +59,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled - setOutput("compute_path_through_poses_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } 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 68a909b52c..0619a41ee0 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 @@ -41,7 +41,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); // Set empty error code, action was successful - setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -49,7 +49,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - setOutput("compute_path_to_pose_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -58,7 +58,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled - setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); 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 c90d8fb65a..3649fad8a1 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -37,20 +37,20 @@ void FollowPathAction::on_tick() BT::NodeStatus FollowPathAction::on_success() { - setOutput("follow_path_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } BT::NodeStatus FollowPathAction::on_aborted() { - setOutput("follow_path_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } BT::NodeStatus FollowPathAction::on_cancelled() { // Set empty error code, action was cancelled - setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index 693fbfa146..5e3e65f424 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -39,6 +39,26 @@ void NavigateThroughPosesAction::on_tick() getInput("behavior_tree", goal_.behavior_tree); } +BT::NodeStatus NavigateThroughPosesAction::on_success() +{ + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus NavigateThroughPosesAction::on_aborted() +{ + setOutput("error_code_id", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus NavigateThroughPosesAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" 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 82cdab44f8..dabc576fcc 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -24,9 +24,8 @@ NavigateToPoseAction::NavigateToPoseAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) -{ -} +: BtActionNode(xml_tag_name, action_name, conf) +{} void NavigateToPoseAction::on_tick() { @@ -39,6 +38,25 @@ void NavigateToPoseAction::on_tick() getInput("behavior_tree", goal_.behavior_tree); } +BT::NodeStatus NavigateToPoseAction::on_success() +{ + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus NavigateToPoseAction::on_aborted() +{ + setOutput("error_code_id", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus NavigateToPoseAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 21a2946a9c..594bf60c3e 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -50,52 +50,56 @@ bt_navigator: # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code + controller_server: ros__parameters: diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index ffae8d9b2a..930a2691fb 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -8,13 +8,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index f4ed518444..0a00a60a4f 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -19,13 +19,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 1378863cdf..3dc5a76503 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -11,13 +11,13 @@ - + - + ` - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 2396b844ae..d50bd7fe5d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -10,12 +10,12 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index 6b5883bfe7..e2bbeb12c7 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -1,8 +1,8 @@ @@ -11,7 +11,7 @@ - + @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index d21efa3b4b..bd4e1c9aca 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -17,13 +17,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index 8ca5dbbc5a..9345d8f66d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 3e889440a8..4ba2f67cc9 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index e921db5557..753de5d780 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -5,17 +5,17 @@ - + - + - + - \ No newline at end of file + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 049e78794b..97b70b7a62 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 33c2c411ad..db0e733db1 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 7c83ff9d9a..3c6286ba01 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -17,8 +17,6 @@ #include #include #include -#include -#include #include #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 82727af5e2..226a673512 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c527e41b14..9d341394a6 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -353,7 +353,7 @@ void ControllerServer::computeControl() if (findControllerId(c_name, current_controller)) { current_controller_ = current_controller; } else { - throw nav2_core::ControllerException("Failed to find controller name: " + c_name); + throw nav2_core::InvalidController("Failed to find controller name: " + c_name); } std::string gc_name = action_server_->get_current_goal()->goal_checker_id; @@ -403,6 +403,13 @@ void ControllerServer::computeControl() controller_frequency_); } } + } catch (nav2_core::InvalidController & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::INVALID_CONTROLLER; + action_server_->terminate_current(result); + return; } catch (nav2_core::ControllerTFError & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); diff --git a/nav2_core/include/nav2_core/controller_exceptions.hpp b/nav2_core/include/nav2_core/controller_exceptions.hpp index bf9c22f4e6..5f87c4c287 100644 --- a/nav2_core/include/nav2_core/controller_exceptions.hpp +++ b/nav2_core/include/nav2_core/controller_exceptions.hpp @@ -27,6 +27,13 @@ class ControllerException : public std::runtime_error : std::runtime_error(description) {} }; +class InvalidController : public ControllerException +{ +public: + explicit InvalidController(const std::string & description) + : ControllerException(description) {} +}; + class ControllerTFError : public ControllerException { public: diff --git a/nav2_core/include/nav2_core/planner_exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp index e607d98ccc..2680b3c669 100644 --- a/nav2_core/include/nav2_core/planner_exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -50,6 +50,13 @@ class PlannerException : public std::runtime_error : std::runtime_error(description) {} }; +class InvalidPlanner : public PlannerException +{ +public: + explicit InvalidPlanner(const std::string & description) + : PlannerException(description) {} +}; + class StartOccupied : public PlannerException { public: 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 index 35f53c6b96..eb1630be26 100644 --- 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 @@ -40,6 +40,7 @@ #include #include +#include #include "geometry_msgs/msg/pose2_d.hpp" #include "std_srvs/srv/set_bool.hpp" diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 2be81db0a7..09fbc08226 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" + "msg/MissedWaypoint.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index c37297c69e..38c7f70a5d 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,15 +1,16 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 START_OUTSIDE_MAP=3 -int16 GOAL_OUTSIDE_MAP=4 -int16 START_OCCUPIED=5 -int16 GOAL_OCCUPIED=6 -int16 TIMEOUT=7 -int16 NO_VALID_PATH=8 -int16 NO_VIAPOINTS_GIVEN=9 +uint16 NONE=0 +uint16 UNKNOWN=300 +uint16 INVALID_PLANNER=301 +uint16 TF_ERROR=3002 +uint16 START_OUTSIDE_MAP=303 +uint16 GOAL_OUTSIDE_MAP=304 +uint16 START_OCCUPIED=305 +uint16 GOAL_OCCUPIED=306 +uint16 TIMEOUT=3007 +uint16 NO_VALID_PATH=308 +uint16 NO_VIAPOINTS_GIVEN=309 #goal definition geometry_msgs/PoseStamped[] goals @@ -20,6 +21,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time -int16 error_code +uint16 error_code --- #feedback definition diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 7e727088a1..5abf2e6826 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,14 +1,15 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 START_OUTSIDE_MAP=3 -int16 GOAL_OUTSIDE_MAP=4 -int16 START_OCCUPIED=5 -int16 GOAL_OCCUPIED=6 -int16 TIMEOUT=7 -int16 NO_VALID_PATH=8 +uint16 NONE=0 +uint16 UNKNOWN=200 +uint16 INVALID_PLANNER=201 +uint16 TF_ERROR=2002 +uint16 START_OUTSIDE_MAP=203 +uint16 GOAL_OUTSIDE_MAP=204 +uint16 START_OCCUPIED=205 +uint16 GOAL_OCCUPIED=206 +uint16 TIMEOUT=207 +uint16 NO_VALID_PATH=208 #goal definition geometry_msgs/PoseStamped goal @@ -19,6 +20,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time -int16 error_code +uint16 error_code --- #feedback definition diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 9ab82ff4d7..c06b918c54 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,12 +1,13 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 INVALID_PATH=3 -int16 PATIENCE_EXCEEDED=4 -int16 FAILED_TO_MAKE_PROGRESS=5 -int16 NO_VALID_CONTROL=6 +uint16 NONE=0 +uint16 UNKNOWN=100 +uint16 INVALID_CONTROLLER=101 +uint16 TF_ERROR=102 +uint16 INVALID_PATH=103 +uint16 PATIENCE_EXCEEDED=104 +uint16 FAILED_TO_MAKE_PROGRESS=105 +uint16 NO_VALID_CONTROL=106 #goal definition nav_msgs/Path path @@ -15,7 +16,7 @@ string goal_checker_id --- #result definition std_msgs/Empty result -int16 error_code +uint16 error_code --- #feedback definition float32 distance_to_goal diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index c4b4764821..12f9a39760 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -1,8 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 UNKNOWN=600 +uint16 TASK_EXECUTOR_FAILED=601 + #goal definition geometry_msgs/PoseStamped[] poses --- #result definition -int32[] missed_waypoints +MissedWaypoint[] missed_waypoints --- #feedback definition uint32 current_waypoint diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index e2a57f25a9..ed1f8ca5f1 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,9 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 + #goal definition geometry_msgs/PoseStamped[] poses string behavior_tree --- #result definition std_msgs/Empty result +uint16 error_code --- #feedback definition geometry_msgs/PoseStamped current_pose diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index b707792241..b38aa0002c 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -1,9 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 + #goal definition geometry_msgs/PoseStamped pose string behavior_tree --- #result definition std_msgs/Empty result +uint16 error_code --- #feedback definition geometry_msgs/PoseStamped current_pose diff --git a/nav2_msgs/msg/MissedWaypoint.msg b/nav2_msgs/msg/MissedWaypoint.msg new file mode 100644 index 0000000000..44b2dc3b9c --- /dev/null +++ b/nav2_msgs/msg/MissedWaypoint.msg @@ -0,0 +1,3 @@ +uint32 index +geometry_msgs/PoseStamped goal +uint16 error_code diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 4e49cfcb35..c2b46c0f2d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -388,7 +388,7 @@ void PlannerServer::computePlanThroughPoses() nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); if (!validatePath(curr_goal, curr_path, goal->planner_id)) { - throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); } // Concatenate paths together @@ -412,6 +412,10 @@ void PlannerServer::computePlanThroughPoses() } action_server_poses_->succeeded_current(result); + } catch (nav2_core::InvalidPlanner & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionToPoseGoal::INVALID_PLANNER; + action_server_poses_->terminate_current(result); } catch (nav2_core::StartOccupied & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); result->error_code = ActionThroughPosesGoal::START_OCCUPIED; @@ -487,7 +491,7 @@ PlannerServer::computePlan() result->path = getPlan(start, goal_pose, goal->planner_id); if (!validatePath(goal_pose, result->path, goal->planner_id)) { - throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); } // Publish the plan for visualization purposes @@ -503,6 +507,10 @@ PlannerServer::computePlan() 1 / max_planner_duration_, 1 / cycle_duration.seconds()); } action_server_pose_->succeeded_current(result); + } catch (nav2_core::InvalidPlanner & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = ActionToPoseGoal::INVALID_PLANNER; + action_server_pose_->terminate_current(result); } catch (nav2_core::StartOccupied & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = ActionToPoseGoal::START_OCCUPIED; @@ -563,6 +571,7 @@ PlannerServer::getPlan( get_logger(), "planner %s is not a valid planner. " "Planner names are: %s", planner_id.c_str(), planner_ids_concat_.c_str()); + throw nav2_core::InvalidPlanner("Planner id " + planner_id + " is invalid"); } } diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index a844498ab8..5b00c6a49e 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -345,22 +345,28 @@ def _getPathImpl(self, start, goal, planner_id='', use_start=False): self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) self.status = self.result_future.result().status - if self.status != GoalStatus.STATUS_SUCCEEDED: - self.warn(f'Getting path failed with status code: {self.status}') - return None return self.result_future.result().result def getPath(self, start, goal, planner_id='', use_start=False): """Send a `ComputePathToPose` action request.""" rtn = self._getPathImpl(start, goal, planner_id='', use_start=False) + + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn(f'Getting path failed with status code: {self.status}') + return None + if not rtn: return None else: return rtn.path - def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): - """Send a `ComputePathThroughPoses` action request.""" + def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False): + """ + Send a `ComputePathThroughPoses` action request. + + Internal implementation to get the full result, not just the path. + """ self.debug("Waiting for 'ComputePathThroughPoses' action server") while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathThroughPoses' action server not available, waiting...") @@ -383,11 +389,21 @@ def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) self.status = self.result_future.result().status + + return self.result_future.result().result + + def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): + """Send a `ComputePathThroughPoses` action request.""" + rtn = self.__getPathThroughPosesImpl(start, goals, planner_id='', use_start=False) + if self.status != GoalStatus.STATUS_SUCCEEDED: self.warn(f'Getting path failed with status code: {self.status}') return None - return self.result_future.result().result.path + if not rtn: + return None + else: + return rtn.path def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): """ diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 6d56df1ef7..558ae809bd 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -47,6 +47,40 @@ set(dependencies behaviortree_cpp_v3 ) +set(local_controller_plugin_lib local_controller) + +add_library(${local_controller_plugin_lib} SHARED src/error_codes/controller/controller_error_plugins.cpp) +ament_target_dependencies(${local_controller_plugin_lib} ${dependencies}) +target_compile_definitions(${local_controller_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(nav2_core src/error_codes/controller_plugins.xml) + +install(TARGETS ${local_controller_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(FILES src/error_codes/controller_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + +set(global_planner_plugin_lib global_planner) + +add_library(${global_planner_plugin_lib} SHARED src/error_codes/planner/planner_error_plugin.cpp) +ament_target_dependencies(${global_planner_plugin_lib} ${dependencies}) +target_compile_definitions(${global_planner_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(nav2_core src/error_codes/planner_plugins.xml) + +install(TARGETS ${global_planner_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(FILES src/error_codes/planner_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -67,8 +101,11 @@ if(BUILD_TESTING) add_subdirectory(src/behaviors/drive_on_heading) add_subdirectory(src/behaviors/assisted_teleop) add_subdirectory(src/costmap_filters) + add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() +ament_export_libraries(${local_controller_plugin_lib}) +ament_export_libraries(${global_planner_plugin_lib}) ament_package() diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 0940173e04..12f269a2d9 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -65,5 +65,7 @@ ament_cmake + + diff --git a/nav2_system_tests/src/error_codes/CMakeLists.txt b/nav2_system_tests/src/error_codes/CMakeLists.txt new file mode 100644 index 0000000000..be7052be16 --- /dev/null +++ b/nav2_system_tests/src/error_codes/CMakeLists.txt @@ -0,0 +1,12 @@ +ament_add_test(test_error_codes + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_error_codes_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp new file mode 100644 index 0000000000..efcdac9c82 --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp @@ -0,0 +1,25 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "controller_error_plugins.hpp" + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::UnknownErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS( + nav2_system_tests::FailedToMakeProgressErrorController, + nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::PatienceExceededErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::InvalidPathErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidControlErrorController, nav2_core::Controller) diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp new file mode 100644 index 0000000000..ba194b195d --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -0,0 +1,114 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 ERROR_CODES__CONTROLLER__CONTROLLER_ERROR_PLUGINS_HPP_ +#define ERROR_CODES__CONTROLLER__CONTROLLER_ERROR_PLUGINS_HPP_ + +#include +#include + +#include "nav2_core/controller.hpp" +#include "nav2_core/controller_exceptions.hpp" + +namespace nav2_system_tests +{ + +class UnknownErrorController : public nav2_core::Controller +{ +public: + UnknownErrorController() = default; + ~UnknownErrorController() = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string, std::shared_ptr, + std::shared_ptr) override {} + + void cleanup() {} + + void activate() {} + + void deactivate() {} + + void setPlan(const nav_msgs::msg::Path &) {} + + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::ControllerException("Unknown Error"); + } + + void setSpeedLimit(const double &, const bool &) {} +}; + +class TFErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::ControllerTFError("TF error"); + } +}; + +class FailedToMakeProgressErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::FailedToMakeProgress("Failed to make progress"); + } +}; + +class PatienceExceededErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::PatienceExceeded("Patience exceeded"); + } +}; + +class InvalidPathErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::InvalidPath("Invalid path"); + } +}; + +class NoValidControlErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::NoValidControl("No valid control"); + } +}; + +} // namespace nav2_system_tests + +#endif // ERROR_CODES__CONTROLLER__CONTROLLER_ERROR_PLUGINS_HPP_ diff --git a/nav2_system_tests/src/error_codes/controller_plugins.xml b/nav2_system_tests/src/error_codes/controller_plugins.xml new file mode 100644 index 0000000000..c20fe0ab5e --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller_plugins.xml @@ -0,0 +1,26 @@ + + + This local planner produces the general exception. + + + This local planner produces a tf exception. + + + This local planner produces a failed to make progress exception. + + + This local planner produces a start patience exceeded exception. + + + This local planner produces an invalid path exception. + + + This local planner produces a no valid control exception. + + diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml new file mode 100644 index 0000000000..4c3e839520 --- /dev/null +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -0,0 +1,175 @@ +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.2 + failure_tolerance: -0.1 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded","failed_to_make_progress", "no_valid_control"] + unknown: + plugin: "nav2_system_tests::UnknownErrorController" + tf_error: + plugin: "nav2_system_tests::TFErrorController" + invalid_path: + plugin: "nav2_system_tests::InvalidPathErrorController" + patience_exceeded: + plugin: "nav2_system_tests::PatienceExceededErrorController" + failed_to_make_progress: + plugin: "nav2_system_tests::FailedToMakeProgressErrorController" + no_valid_control: + plugin: "nav2_system_tests::NoValidControlErrorController" + + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["obstacle_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: [ "unknown", "tf_error", "start_outside_map", "goal_outside_map", + "start_occupied", "goal_occupied", "timeout","no_valid_path", + "no_viapoints_given" ] + unknown: + plugin: "nav2_system_tests::UnknownErrorPlanner" + tf_error: + plugin: "nav2_system_tests::TFErrorPlanner" + start_outside_map: + plugin: "nav2_system_tests::StartOutsideMapErrorPlanner" + goal_outside_map: + plugin: "nav2_system_tests::GoalOutsideMapErrorPlanner" + start_occupied: + plugin: "nav2_system_tests::StartOccupiedErrorPlanner" + goal_occupied: + plugin: "nav2_system_tests::GoalOccupiedErrorPlanner" + timeout: + plugin: "nav2_system_tests::TimedOutErrorPlanner" + no_valid_path: + plugin: "nav2_system_tests::NoValidPathErrorPlanner" + no_viapoints_given: + plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner" diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp new file mode 100644 index 0000000000..90b18c5b11 --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "planner_error_plugin.hpp" + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::UnknownErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::StartOccupiedErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::GoalOccupiedErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::StartOutsideMapErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::GoalOutsideMapErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidPathErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TimedOutErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoViapointsGivenErrorPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp new file mode 100644 index 0000000000..2b6ae7971d --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -0,0 +1,144 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 ERROR_CODES__PLANNER__PLANNER_ERROR_PLUGIN_HPP_ +#define ERROR_CODES__PLANNER__PLANNER_ERROR_PLUGIN_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_core/planner_exceptions.hpp" + +namespace nav2_system_tests +{ + +class UnknownErrorPlanner : public nav2_core::GlobalPlanner +{ +public: + UnknownErrorPlanner() = default; + ~UnknownErrorPlanner() = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string, std::shared_ptr, + std::shared_ptr) override {} + + void cleanup() override {} + + void activate() override {} + + void deactivate() override {} + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::PlannerException("Unknown Error"); + } +}; + +class StartOccupiedErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::StartOccupied("Start Occupied"); + } +}; + +class GoalOccupiedErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::GoalOccupied("Goal occupied"); + } +}; + +class StartOutsideMapErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::StartOutsideMapBounds("Start OutsideMapBounds"); + } +}; + +class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + } +}; + +class NoValidPathErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + return nav_msgs::msg::Path(); + } +}; + + +class TimedOutErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::PlannerTimedOut("Planner Timed Out"); + } +}; + +class TFErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::PlannerTFError("TF Error"); + } +}; + +class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::NoViapointsGiven("No Via points given"); + } +}; + +} // namespace nav2_system_tests + + +#endif // ERROR_CODES__PLANNER__PLANNER_ERROR_PLUGIN_HPP_ diff --git a/nav2_system_tests/src/error_codes/planner_plugins.xml b/nav2_system_tests/src/error_codes/planner_plugins.xml new file mode 100644 index 0000000000..9d6f7f0545 --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner_plugins.xml @@ -0,0 +1,42 @@ + + + This global planner produces a goal a timeout exception. + + + This global planner produces a start occupied exception. + + + This global planner produces a goal occupied exception. + + + This global planner produces a start outside map bounds exception. + + + This global planner produces a goal outside map bounds exception. + + + This global planner produces a no valid path exception. + + + This global planner produces a no via points given exception. + + + This global planner produces a timed out exception. + + + This global planner produces a planner tf error exception. + + + This global planner produces a no via points exception. + + diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py new file mode 100755 index 0000000000..3079e3a3fc --- /dev/null +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -0,0 +1,130 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import time + +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Path +from nav2_simple_commander.robot_navigator import BasicNavigator + +import rclpy +from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses + + +def main(argv=sys.argv[1:]): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + + # Initial pose does not respect the orientation... not sure why + initial_pose.pose.position.x = -2.00 + initial_pose.pose.position.y = -0.50 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 + + # Follow path error codes + path = Path() + + goal_pose = initial_pose + goal_pose.pose.position.x += 0.25 + + goal_pose1 = goal_pose + goal_pose1.pose.position.x += 0.25 + + path.poses.append(initial_pose) + path.poses.append(goal_pose) + path.poses.append(goal_pose1) + + follow_path = { + 'unknown': FollowPath.Goal().UNKNOWN, + 'invalid_controller': FollowPath.Goal().INVALID_CONTROLLER, + 'tf_error': FollowPath.Goal().TF_ERROR, + 'invalid_path': FollowPath.Goal().INVALID_PATH, + 'patience_exceeded': FollowPath.Goal().PATIENCE_EXCEEDED, + 'failed_to_make_progress': FollowPath.Goal().FAILED_TO_MAKE_PROGRESS, + 'no_valid_control': FollowPath.Goal().NO_VALID_CONTROL + } + + for controller, error_code in follow_path.items(): + success = navigator.followPath(path, controller) + + if success: + while not navigator.isTaskComplete(): + time.sleep(0.5) + + assert navigator.result_future.result().result.error_code == error_code, \ + "Follow path error code does not match" + + else: + assert False, "Follow path was rejected" + + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + + # Check compute path to pose error codes + goal_pose.pose.position.x = -1.00 + goal_pose.pose.position.y = -0.50 + goal_pose.pose.orientation.z = 0.0 + goal_pose.pose.orientation.w = 1.0 + + compute_path_to_pose = { + 'unknown': ComputePathToPose.Goal().UNKNOWN, + 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, + 'tf_error': ComputePathToPose.Goal().TF_ERROR, + 'start_outside_map': ComputePathToPose.Goal().START_OUTSIDE_MAP, + 'goal_outside_map': ComputePathToPose.Goal().GOAL_OUTSIDE_MAP, + 'start_occupied': ComputePathToPose.Goal().START_OCCUPIED, + 'goal_occupied': ComputePathToPose.Goal().GOAL_OCCUPIED, + 'timeout': ComputePathToPose.Goal().TIMEOUT, + 'no_valid_path': ComputePathToPose.Goal().NO_VALID_PATH} + + for planner, error_code in compute_path_to_pose.items(): + result = navigator._getPathImpl(initial_pose, goal_pose, planner) + assert result.error_code == error_code, "Compute path to pose error does not match" + + # Check compute path through error codes + goal_pose1 = goal_pose + goal_poses = [goal_pose, goal_pose1] + + compute_path_through_poses = { + 'unknown': ComputePathThroughPoses.Goal().UNKNOWN, + 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, + 'tf_error': ComputePathThroughPoses.Goal().TF_ERROR, + 'start_outside_map': ComputePathThroughPoses.Goal().START_OUTSIDE_MAP, + 'goal_outside_map': ComputePathThroughPoses.Goal().GOAL_OUTSIDE_MAP, + 'start_occupied': ComputePathThroughPoses.Goal().START_OCCUPIED, + 'goal_occupied': ComputePathThroughPoses.Goal().GOAL_OCCUPIED, + 'timeout': ComputePathThroughPoses.Goal().TIMEOUT, + 'no_valid_path': ComputePathThroughPoses.Goal().NO_VALID_PATH, + 'no_viapoints_given': ComputePathThroughPoses.Goal().NO_VIAPOINTS_GIVEN} + + for planner, error_code in compute_path_through_poses.items(): + result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) + assert result.error_code == error_code, "Compute path through pose error does not match" + + navigator.lifecycleShutdown() + rclpy.shutdown() + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py new file mode 100755 index 0000000000..165e1806ce --- /dev/null +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -0,0 +1,94 @@ +#! /usr/bin/env python3 +# Copyright (c) 2019 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_testing.legacy import LaunchTestService +from launch_ros.actions import Node +from launch.actions import GroupAction + + +def generate_launch_description(): + params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') + + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + lifecycle_nodes = ['controller_server', + 'planner_server'] + + load_nodes = GroupAction( + actions=[ + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link']), + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn_delay=2.0, + parameters=[params_file], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn_delay=2.0, + parameters=[params_file], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[{'autostart': True}, + {'node_names': lifecycle_nodes}]) + ]) + + ld = LaunchDescription() + ld.add_action(load_nodes) + + return ld + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test_error_codes_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'test_error_codes.py')], + name='test_error_codes', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test_error_codes_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/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index a56a7216b1..906e8f44d3 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -21,6 +21,7 @@ #include "planner_tester.hpp" #include "nav2_util/lifecycle_utils.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_core/planner_exceptions.hpp" using namespace std::chrono_literals; @@ -129,8 +130,12 @@ TEST(testPluginMap, Failures) auto path = obj->getPlan(start, goal, plugin_none); EXPECT_EQ(path.header.frame_id, std::string("map")); - path = obj->getPlan(start, goal, plugin_fake); - EXPECT_EQ(path.poses.size(), 0ul); + try { + path = obj->getPlan(start, goal, plugin_fake); + FAIL() << "Failed to throw invalid planner id exception"; + } catch (const nav2_core::InvalidPlanner & ex) { + EXPECT_EQ(ex.what(), std::string("Planner id fake is invalid")); + } obj->onCleanup(state); } diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 2579513797..2e88a63a7a 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -18,7 +18,7 @@ from action_msgs.msg import GoalStatus from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped -from nav2_msgs.action import FollowWaypoints +from nav2_msgs.action import FollowWaypoints, ComputePathToPose from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters @@ -119,9 +119,9 @@ def run(self, block, cancel): if status != GoalStatus.STATUS_SUCCEEDED: self.info_msg(f'Goal failed with status code: {status}') return False - if len(result.missed_waypoints) > 0: + if len(self.action_result.missed_waypoints) > 0: self.info_msg('Goal failed to process all waypoints,' - ' missed {0} wps.'.format(len(result.missed_waypoints))) + ' missed {0} wps.'.format(len(self.action_result.missed_waypoints))) return False self.info_msg('Goal succeeded!') @@ -224,12 +224,14 @@ def main(argv=sys.argv[1:]): time.sleep(2) test.cancel_goal() - # a failure case + # set waypoint outside of map time.sleep(2) test.setWaypoints([[100.0, 100.0]]) result = test.run(True, False) assert not result result = not result + assert test.action_result.missed_waypoints[0].error_code == \ + ComputePathToPose.Goal().GOAL_OUTSIDE_MAP # stop on failure test with bogous waypoint test.setStopFailureParam(True) 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 b7cd82d684..e5c59aa20b 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -23,6 +23,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" +#include "nav2_msgs/msg/missed_waypoint.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -43,6 +44,12 @@ enum class ActionStatus SUCCEEDED = 3 }; +struct GoalStatus +{ + ActionStatus status; + int error_code; +}; + /** * @class nav2_waypoint_follower::WaypointFollower * @brief An action server that uses behavior tree for navigating a robot to its @@ -133,10 +140,10 @@ class WaypointFollower : public nav2_util::LifecycleNode rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::shared_future::SharedPtr> future_goal_handle_; + bool stop_on_failure_; - ActionStatus current_goal_status_; int loop_rate_; - std::vector failed_ids_; + GoalStatus current_goal_status_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index a04c404c77..2bb95fea7d 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -205,30 +205,33 @@ WaypointFollower::followWaypoints() std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); - current_goal_status_ = ActionStatus::PROCESSING; + current_goal_status_.status = ActionStatus::PROCESSING; } feedback->current_waypoint = goal_index; action_server_->publish_feedback(feedback); - if (current_goal_status_ == ActionStatus::FAILED) { - failed_ids_.push_back(goal_index); + if (current_goal_status_.status == ActionStatus::FAILED) { + nav2_msgs::msg::MissedWaypoint missedWaypoint; + missedWaypoint.index = goal_index; + missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.error_code = current_goal_status_.error_code; + result->missed_waypoints.push_back(missedWaypoint); if (stop_on_failure_) { RCLCPP_WARN( get_logger(), "Failed to process waypoint %i in waypoint " "list and stop on failure is enabled." " Terminating action.", goal_index); - result->missed_waypoints = failed_ids_; action_server_->terminate_current(result); - failed_ids_.clear(); + current_goal_status_.error_code = 0; return; } else { RCLCPP_INFO( get_logger(), "Failed to process waypoint %i," " moving to next.", goal_index); } - } else if (current_goal_status_ == ActionStatus::SUCCEEDED) { + } else if (current_goal_status_.status == ActionStatus::SUCCEEDED) { RCLCPP_INFO( get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", goal_index); @@ -237,16 +240,23 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Task execution at waypoint %i %s", goal_index, is_task_executed ? "succeeded" : "failed!"); + + if (!is_task_executed) { + nav2_msgs::msg::MissedWaypoint missedWaypoint; + missedWaypoint.index = goal_index; + missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.error_code = nav2_msgs::action::FollowWaypoints::Goal::TASK_EXECUTOR_FAILED; + result->missed_waypoints.push_back(missedWaypoint); + } // if task execution was failed and stop_on_failure_ is on , terminate action if (!is_task_executed && stop_on_failure_) { - failed_ids_.push_back(goal_index); RCLCPP_WARN( get_logger(), "Failed to execute task at waypoint %i " " stop on failure is enabled." " Terminating action.", goal_index); - result->missed_waypoints = failed_ids_; + action_server_->terminate_current(result); - failed_ids_.clear(); + current_goal_status_.error_code = 0; return; } else { RCLCPP_INFO( @@ -255,8 +265,8 @@ WaypointFollower::followWaypoints() } } - if (current_goal_status_ != ActionStatus::PROCESSING && - current_goal_status_ != ActionStatus::UNKNOWN) + if (current_goal_status_.status != ActionStatus::PROCESSING && + current_goal_status_.status != ActionStatus::UNKNOWN) { // Update server state goal_index++; @@ -265,9 +275,8 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", goal->poses.size()); - result->missed_waypoints = failed_ids_; action_server_->succeeded_current(result); - failed_ids_.clear(); + current_goal_status_.error_code = 0; return; } } else { @@ -296,16 +305,17 @@ WaypointFollower::resultCallback( switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - current_goal_status_ = ActionStatus::SUCCEEDED; + current_goal_status_.status = ActionStatus::SUCCEEDED; return; case rclcpp_action::ResultCode::ABORTED: - current_goal_status_ = ActionStatus::FAILED; + current_goal_status_.status = ActionStatus::FAILED; + current_goal_status_.error_code = result.result->error_code; return; case rclcpp_action::ResultCode::CANCELED: - current_goal_status_ = ActionStatus::FAILED; + current_goal_status_.status = ActionStatus::FAILED; return; default: - current_goal_status_ = ActionStatus::UNKNOWN; + current_goal_status_.status = ActionStatus::UNKNOWN; return; } } @@ -318,7 +328,7 @@ WaypointFollower::goalResponseCallback( RCLCPP_ERROR( get_logger(), "navigate_to_pose action client failed to send goal to server."); - current_goal_status_ = ActionStatus::FAILED; + current_goal_status_.status = ActionStatus::FAILED; } } From 1a5313a3d3d76adf7a28e403cd0665e449d9d523 Mon Sep 17 00:00:00 2001 From: MartiBolet <43337758+MartiBolet@users.noreply.github.com> Date: Sun, 20 Nov 2022 09:05:06 +0100 Subject: [PATCH 108/131] Solve bug when CostmapInfoServer is reactivated (#3292) * Solve bug when CostmapInfoServer is reactivated --- .../costmap_filter_info_server.hpp | 2 +- .../costmap_filter_info_server.cpp | 17 +++++----- .../unit/test_costmap_filter_info_server.cpp | 31 +++++++++++++++++++ 3 files changed, 41 insertions(+), 9 deletions(-) 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 index 99311deff7..14a69ad229 100644 --- 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 @@ -71,7 +71,7 @@ class CostmapFilterInfoServer : public nav2_util::LifecycleNode private: rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; - std::unique_ptr msg_; + nav2_msgs::msg::CostmapFilterInfo msg_; }; // CostmapFilterInfoServer } // namespace nav2_map_server 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 index 7b1c618315..f6671b0291 100644 --- 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 @@ -48,13 +48,13 @@ CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/) publisher_ = this->create_publisher( filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - msg_ = std::make_unique(); - msg_->header.frame_id = ""; - msg_->header.stamp = now(); - msg_->type = get_parameter("type").as_int(); - msg_->filter_mask_topic = get_parameter("mask_topic").as_string(); - msg_->base = static_cast(get_parameter("base").as_double()); - msg_->multiplier = static_cast(get_parameter("multiplier").as_double()); + msg_ = nav2_msgs::msg::CostmapFilterInfo(); + msg_.header.frame_id = ""; + msg_.header.stamp = now(); + msg_.type = get_parameter("type").as_int(); + msg_.filter_mask_topic = get_parameter("mask_topic").as_string(); + msg_.base = static_cast(get_parameter("base").as_double()); + msg_.multiplier = static_cast(get_parameter("multiplier").as_double()); return nav2_util::CallbackReturn::SUCCESS; } @@ -65,7 +65,8 @@ CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Activating"); publisher_->on_activate(); - publisher_->publish(std::move(msg_)); + auto costmap_filter_info = std::make_unique(msg_); + publisher_->publish(std::move(costmap_filter_info)); // create bond connection createBond(); 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 index 168fbc9f7b..072aaf8d1b 100644 --- a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -59,6 +59,16 @@ class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer on_cleanup(get_current_state()); on_shutdown(get_current_state()); } + + void deactivate() + { + on_deactivate(get_current_state()); + } + + void activate() + { + on_activate(get_current_state()); + } }; class InfoServerTester : public ::testing::Test @@ -144,3 +154,24 @@ TEST_F(InfoServerTester, testCostmapFilterInfoPublish) EXPECT_NEAR(info_->base, BASE, EPSILON); EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); } + +TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate) +{ + info_server_->deactivate(); + info_ = nullptr; + info_server_->activate(); + + 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); +} From c415c760e5fc5de132b34b0c92cbaaa377a9fb0d Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Sun, 20 Nov 2022 21:59:00 +0300 Subject: [PATCH 109/131] Smoothness metrics update (#3294) * Update metrics for path smoothness * Support Savitzky-Golay smoother --- tools/smoother_benchmarking/README.md | 4 +++- tools/smoother_benchmarking/metrics.py | 2 +- tools/smoother_benchmarking/process_data.py | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md index 65ea8a4c9d..aa9a5c83fe 100644 --- a/tools/smoother_benchmarking/README.md +++ b/tools/smoother_benchmarking/README.md @@ -30,12 +30,14 @@ planner_server: ``` smoother_server: ros__parameters: - smoother_plugins: ["simple_smoother", "constrained_smoother"] + smoother_plugins: ["simple_smoother", "constrained_smoother", "sg_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" constrained_smoother: plugin: "nav2_constrained_smoother/ConstrainedSmoother" w_smooth: 100000.0 # tuned + sg_smoother: + plugin: "nav2_smoother::SavitzkyGolaySmoother" ``` Set global costmap, path planner and smoothers parameters to those desired in `nav2_params.yaml`. diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py index 6b576b08a9..9636eee11f 100644 --- a/tools/smoother_benchmarking/metrics.py +++ b/tools/smoother_benchmarking/metrics.py @@ -111,7 +111,7 @@ def main(): costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) planner = 'SmacHybrid' - smoothers = ['simple_smoother', 'constrained_smoother'] + smoothers = ['simple_smoother', 'constrained_smoother', 'sg_smoother'] max_cost = 210 side_buffer = 10 time_stamp = navigator.get_clock().now().to_msg() diff --git a/tools/smoother_benchmarking/process_data.py b/tools/smoother_benchmarking/process_data.py index edc0dad811..43392833bd 100644 --- a/tools/smoother_benchmarking/process_data.py +++ b/tools/smoother_benchmarking/process_data.py @@ -82,7 +82,7 @@ def getSmoothness(pt_prev, pt, pt_next): d1 = pt - pt_prev d2 = pt_next - pt delta = d2 - d1 - return np.dot(delta, delta) + return np.linalg.norm(delta) def getPathSmoothnesses(paths): smoothnesses = [] From 806bc688e8ceee4c56e992c709a49d969ee06232 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Tue, 29 Nov 2022 20:31:48 +0100 Subject: [PATCH 110/131] preempt/cancel test for time behavior, spin pluguin (#3301) * include preempt/cancel test for time behavior, spin pluguin * linting * fix bug in code --- .../behaviors/spin/spin_behavior_tester.cpp | 15 ++++++-- .../behaviors/spin/spin_behavior_tester.hpp | 4 ++- .../spin/test_spin_behavior_node.cpp | 34 +++++++++++++++++++ 3 files changed, 50 insertions(+), 3 deletions(-) diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp index 557f5b7959..27b1f5e800 100644 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -123,7 +123,9 @@ void SpinBehaviorTester::deactivate() bool SpinBehaviorTester::defaultSpinBehaviorTest( const float target_yaw, - const double tolerance) + const double tolerance, + const bool nonblocking_action, + const bool cancel_action) { if (!is_active_) { RCLCPP_ERROR(node_->get_logger(), "Not activated"); @@ -181,6 +183,16 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( return false; } + if (!nonblocking_action) { + return true; + } + if (cancel_action) { + sleep(2); + // cancel the goal + auto cancel_response = client_ptr_->async_cancel_goal(goal_handle_future.get()); + rclcpp::spin_until_future_complete(node_, cancel_response); + } + // Wait for the server to be done with the goal auto result_future = client_ptr_->async_get_result(goal_handle); @@ -347,5 +359,4 @@ void SpinBehaviorTester::amclPoseCallback( { initial_pose_received_ = true; } - } // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp index 745cfe4212..c7fb6f719d 100644 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp @@ -57,7 +57,9 @@ class SpinBehaviorTester // Runs a single test with given target yaw bool defaultSpinBehaviorTest( float target_yaw, - double tolerance = 0.1); + double tolerance = 0.1, + bool nonblocking_action = true, + bool cancel_action = false); void activate(); diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp index 150a6599fa..a1efed8cd9 100644 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp @@ -80,6 +80,40 @@ TEST_P(SpinBehaviorTestFixture, testSpinRecovery) } } +TEST_F(SpinBehaviorTestFixture, testSpinPreemption) +{ + // Goal + float target_yaw = 3.0 * M_PIf32; + float tolerance = 0.1; + bool nonblocking_action = false; + bool success = false; + + // Send the first goal + success = spin_recovery_tester->defaultSpinBehaviorTest( + target_yaw, tolerance, + nonblocking_action); + EXPECT_EQ(true, success); + // Preempt goal + sleep(2); + success = false; + float prempt_target_yaw = 4.0 * M_PIf32; + float preempt_tolerance = 0.1; + success = spin_recovery_tester->defaultSpinBehaviorTest(prempt_target_yaw, preempt_tolerance); + EXPECT_EQ(false, success); +} + +TEST_F(SpinBehaviorTestFixture, testSpinCancel) +{ + // Goal + float target_yaw = 4.0 * M_PIf32; + float tolerance = 0.1; + bool nonblocking_action = true, cancel_action = true, success = false; + success = spin_recovery_tester->defaultSpinBehaviorTest( + target_yaw, tolerance, + nonblocking_action, cancel_action); + EXPECT_EQ(false, success); +} + INSTANTIATE_TEST_SUITE_P( SpinRecoveryTests, SpinBehaviorTestFixture, From 6202f574bddaa5e521b0eb93dbfd8c7cd3886aa7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 1 Dec 2022 14:05:27 -0800 Subject: [PATCH 111/131] Migrate to createTreeFromFile for loading BTs (#3306) --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index c3f9244f17..11110b09c2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -192,13 +192,9 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena return false; } - auto xml_string = std::string( - std::istreambuf_iterator(xml_file), - std::istreambuf_iterator()); - // Create the Behavior Tree from the XML input try { - tree_ = bt_->createTreeFromText(xml_string, blackboard_); + tree_ = bt_->createTreeFromFile(filename, blackboard_); } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); return false; From 5c27a3e4884d388281bc50d16331fbbc1fabadc4 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 5 Dec 2022 13:27:08 -0500 Subject: [PATCH 112/131] update comment (#3309) Co-authored-by: Joshua Wallace --- nav2_common/cmake/nav2_package.cmake | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index b00d30c17b..82a0fe92b1 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -27,9 +27,13 @@ macro(nav2_package) "Debug" "Release" "MinSizeRel" "RelWithDebInfo") endif() - # Default to C++14 + # Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + if ("cxx_std_17" IN_LIST CMAKE_CXX_COMPILE_FEATURES) + set(CMAKE_CXX_STANDARD 17) + else() + message( FATAL_ERROR "cxx_std_17 could not be found.") + endif() endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") From 7c88750511ae0e6818a47716c864c3b213c16e5a Mon Sep 17 00:00:00 2001 From: Owen Hooper <17ofh@queensu.ca> Date: Mon, 5 Dec 2022 15:35:54 -0500 Subject: [PATCH 113/131] Added a flag to not send goal in BtActionNode (#3293) * Added a flag to not send goal in BtActionNode Signed-off-by: Owen Hooper <17ofh@queensu.ca> * Fixed cpplint and uncrustify issues. * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp Signed-off-by: Owen Hooper <17ofh@queensu.ca> Co-authored-by: Steve Macenski --- .../nav2_behavior_tree/bt_action_node.hpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 06fc6dd810..cde12f8e5f 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 @@ -47,7 +47,7 @@ class BtActionNode : public BT::ActionNodeBase const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) - : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) + : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name), should_send_goal_(true) { node_ = config().blackboard->template get("node"); callback_group_ = node_->create_callback_group( @@ -188,10 +188,15 @@ class BtActionNode : public BT::ActionNodeBase // setting the status to RUNNING to notify the BT Loggers (if any) setStatus(BT::NodeStatus::RUNNING); - // user defined callback + // reset the flag to send the goal or not, allowing the user the option to set it in on_tick + should_send_goal_ = true; + + // user defined callback, may modify "should_send_goal_". on_tick(); - send_new_goal(); + if (should_send_goal_) { + send_new_goal(); + } } try { @@ -223,7 +228,8 @@ class BtActionNode : public BT::ActionNodeBase feedback_.reset(); auto goal_status = goal_handle_->get_status(); - if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + if (goal_updated_ && + (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { goal_updated_ = false; @@ -444,6 +450,9 @@ class BtActionNode : public BT::ActionNodeBase std::shared_ptr::SharedPtr>> future_goal_handle_; rclcpp::Time time_goal_sent_; + + // Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent. + bool should_send_goal_; }; } // namespace nav2_behavior_tree From 614329538af1797e7ddbd486d3a1c2975bc9b2df Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Tue, 6 Dec 2022 21:44:16 +0300 Subject: [PATCH 114/131] Ensure that plugin initialization to be called before updating routines (#3307) --- nav2_costmap_2d/CMakeLists.txt | 1 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 39 ++++++------ .../test/regression/CMakeLists.txt | 20 +++++++ .../test/regression/order_layer.cpp | 60 +++++++++++++++++++ .../test/regression/order_layer.hpp | 46 ++++++++++++++ .../test/regression/order_layer.xml | 7 +++ .../test/regression/plugin_api_order.cpp | 53 ++++++++++++++++ 7 files changed, 208 insertions(+), 18 deletions(-) create mode 100644 nav2_costmap_2d/test/regression/order_layer.cpp create mode 100644 nav2_costmap_2d/test/regression/order_layer.hpp create mode 100644 nav2_costmap_2d/test/regression/order_layer.xml create mode 100644 nav2_costmap_2d/test/regression/plugin_api_order.cpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 96b4f49339..683cb04fdd 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -180,6 +180,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) add_subdirectory(test) + pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml) endif() ament_export_include_directories(include) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 50e74c511b..52b538b6bb 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -435,24 +435,27 @@ Costmap2DROS::mapUpdateLoop(double frequency) while (rclcpp::ok() && !map_update_thread_shutdown_) { nav2_util::ExecutionTimer timer; - // Measure the execution time of the updateMap method - timer.start(); - updateMap(); - timer.end(); - - RCLCPP_DEBUG(get_logger(), "Map update time: %.9f", timer.elapsed_time_in_seconds()); - if (publish_cycle_ > rclcpp::Duration(0s) && layered_costmap_->isInitialized()) { - unsigned int x0, y0, xn, yn; - layered_costmap_->getBounds(&x0, &xn, &y0, &yn); - costmap_publisher_->updateBounds(x0, xn, y0, yn); - - auto current_time = now(); - if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT - { - RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); - costmap_publisher_->publishCostmap(); - last_publish_ = current_time; + // Execute after start() will complete plugins activation + if (!stopped_) { + // Measure the execution time of the updateMap method + timer.start(); + updateMap(); + timer.end(); + + RCLCPP_DEBUG(get_logger(), "Map update time: %.9f", timer.elapsed_time_in_seconds()); + if (publish_cycle_ > rclcpp::Duration(0s) && layered_costmap_->isInitialized()) { + unsigned int x0, y0, xn, yn; + layered_costmap_->getBounds(&x0, &xn, &y0, &yn); + costmap_publisher_->updateBounds(x0, xn, y0, yn); + + auto current_time = now(); + if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due + (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + { + RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); + costmap_publisher_->publishCostmap(); + last_publish_ = current_time; + } } } diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index c492459d2f..c9a52aa8af 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,4 +1,24 @@ +# Bresenham2D corner cases test ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) target_link_libraries(costmap_bresenham_2d nav2_costmap_2d_core ) + +# OrderLayer for checking Costmap2D plugins API calling order +add_library(order_layer SHARED + order_layer.cpp) +ament_target_dependencies(order_layer + ${dependencies} +) +install(TARGETS + order_layer + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Costmap2D plugins API calling order test +ament_add_gtest(plugin_api_order plugin_api_order.cpp) +target_link_libraries(plugin_api_order + nav2_costmap_2d_core +) diff --git a/nav2_costmap_2d/test/regression/order_layer.cpp b/nav2_costmap_2d/test/regression/order_layer.cpp new file mode 100644 index 0000000000..6e69e210a7 --- /dev/null +++ b/nav2_costmap_2d/test/regression/order_layer.cpp @@ -0,0 +1,60 @@ +// Copyright (c) 2022 Samsung R&D Institute 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 "order_layer.hpp" + +#include +#include + +using namespace std::chrono_literals; + +namespace nav2_costmap_2d +{ + +OrderLayer::OrderLayer() +: activated_(false) +{ +} + +void OrderLayer::activate() +{ + std::this_thread::sleep_for(100ms); + activated_ = true; +} + +void OrderLayer::deactivate() +{ + activated_ = false; +} + +void OrderLayer::updateBounds( + double, double, double, double *, double *, double *, double *) +{ + if (!activated_) { + throw std::runtime_error("update before activated"); + } +} + +void OrderLayer::updateCosts( + nav2_costmap_2d::Costmap2D &, int, int, int, int) +{ + if (!activated_) { + throw std::runtime_error("update before activated"); + } +} + +} // namespace nav2_costmap_2d + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::OrderLayer, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/test/regression/order_layer.hpp b/nav2_costmap_2d/test/regression/order_layer.hpp new file mode 100644 index 0000000000..35bb2aeabb --- /dev/null +++ b/nav2_costmap_2d/test/regression/order_layer.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 Samsung R&D Institute 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. + +#ifndef NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ +#define NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ + +#include "nav2_costmap_2d/layer.hpp" + +namespace nav2_costmap_2d +{ + +class OrderLayer : public nav2_costmap_2d::Layer +{ +public: + OrderLayer(); + + virtual void activate(); + virtual void deactivate(); + + virtual void reset() {} + virtual bool isClearable() {return false;} + + virtual void updateBounds( + double, double, double, double *, double *, double *, double *); + + virtual void updateCosts( + nav2_costmap_2d::Costmap2D &, int, int, int, int); + +private: + bool activated_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ diff --git a/nav2_costmap_2d/test/regression/order_layer.xml b/nav2_costmap_2d/test/regression/order_layer.xml new file mode 100644 index 0000000000..ffc553a152 --- /dev/null +++ b/nav2_costmap_2d/test/regression/order_layer.xml @@ -0,0 +1,7 @@ + + + + Plugin checking order of activate() and updateBounds()/updateCosts() calls + + + diff --git a/nav2_costmap_2d/test/regression/plugin_api_order.cpp b/nav2_costmap_2d/test/regression/plugin_api_order.cpp new file mode 100644 index 0000000000..c7baf34453 --- /dev/null +++ b/nav2_costmap_2d/test/regression/plugin_api_order.cpp @@ -0,0 +1,53 @@ +// Copyright (c) 2022 Samsung R&D Institute 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 + +TEST(CostmapPluginsTester, checkPluginAPIOrder) +{ + std::shared_ptr costmap_ros = + std::make_shared("costmap_ros"); + + // Workaround to avoid setting base_link->map transform + costmap_ros->set_parameter(rclcpp::Parameter("robot_base_frame", "map")); + // Specifying order verification plugin in the parameters + std::vector plugins_str; + plugins_str.push_back("order_layer"); + costmap_ros->set_parameter(rclcpp::Parameter("plugins", plugins_str)); + costmap_ros->declare_parameter( + "order_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::OrderLayer"))); + + // Do actual test: ensure that plugin->updateBounds()/updateCosts() + // will be called after plugin->activate() + costmap_ros->on_configure(costmap_ros->get_current_state()); + costmap_ros->on_activate(costmap_ros->get_current_state()); + + // Do cleanup + costmap_ros->on_deactivate(costmap_ros->get_current_state()); + costmap_ros->on_cleanup(costmap_ros->get_current_state()); + costmap_ros->on_shutdown(costmap_ros->get_current_state()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From c119017be6c00b97c03782e2775e9492831fd77c Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Tue, 6 Dec 2022 21:45:41 +0300 Subject: [PATCH 115/131] Make mapUpdateLoop() indicator variables to be thread-safe (#3308) --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 968bd9b8a4..2124a12a44 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 @@ -38,6 +38,7 @@ #ifndef NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_ #define NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_ +#include #include #include #include @@ -342,9 +343,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ void mapUpdateLoop(double frequency); bool map_update_thread_shutdown_{false}; - bool stop_updates_{false}; - bool initialized_{false}; - bool stopped_{true}; + std::atomic stop_updates_{false}; + std::atomic initialized_{false}; + std::atomic stopped_{true}; std::unique_ptr map_update_thread_; ///< @brief A thread for updating the map rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME}; rclcpp::Duration publish_cycle_{1, 0}; From fe776fb353ea82c7261d154b2803cece70cabc86 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Wed, 7 Dec 2022 05:44:54 +0800 Subject: [PATCH 116/131] Fix Wrong Map Pointer (#3311) (#3315) Not assigning fixed map pointer to particle filter, using latest when resample. (cherry picked from commit cc6f205b5140b2485c1f01a7249b6cbddad69ab4) Signed-off-by: Borong Yuan Signed-off-by: Borong Yuan --- nav2_amcl/include/nav2_amcl/pf/pf.hpp | 5 ++--- nav2_amcl/src/amcl_node.cpp | 5 ++--- nav2_amcl/src/pf/pf.c | 7 +++---- 3 files changed, 7 insertions(+), 10 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/pf/pf.hpp b/nav2_amcl/include/nav2_amcl/pf/pf.hpp index afcb9daafb..c4759d06b4 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf.hpp @@ -129,7 +129,6 @@ typedef struct _pf_t // Function used to draw random pose samples pf_init_model_fn_t random_pose_fn; - void * random_pose_data; double dist_threshold; // distance threshold in each axis over which the pf is considered to not // be converged @@ -141,7 +140,7 @@ typedef struct _pf_t pf_t * pf_alloc( int min_samples, int max_samples, double alpha_slow, double alpha_fast, - pf_init_model_fn_t random_pose_fn, void * random_pose_data); + pf_init_model_fn_t random_pose_fn); // Free an existing filter void pf_free(pf_t * pf); @@ -159,7 +158,7 @@ void pf_init_model(pf_t * pf, pf_init_model_fn_t init_fn, void * init_data); void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data); // Resample the distribution -void pf_update_resample(pf_t * pf); +void pf_update_resample(pf_t * pf, void * random_pose_data); // Compute the CEP statistics (mean and variance). // void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var); diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 303a753b73..5620ae5edd 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -679,7 +679,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) // Resample the particles if (!(++resample_count_ % resample_interval_)) { - pf_update_resample(pf_); + pf_update_resample(pf_, reinterpret_cast(map_)); resampled = true; } @@ -1580,8 +1580,7 @@ AmclNode::initParticleFilter() // Create the particle filter pf_ = pf_alloc( min_particles_, max_particles_, alpha_slow_, alpha_fast_, - (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, - reinterpret_cast(map_)); + (pf_init_model_fn_t)AmclNode::uniformPoseGenerator); pf_->pop_err = pf_err_; pf_->pop_z = pf_z_; diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index 63d71b99bd..de4e3247d1 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -47,7 +47,7 @@ static int pf_resample_limit(pf_t * pf, int k); pf_t * pf_alloc( int min_samples, int max_samples, double alpha_slow, double alpha_fast, - pf_init_model_fn_t random_pose_fn, void * random_pose_data) + pf_init_model_fn_t random_pose_fn) { int i, j; pf_t * pf; @@ -59,7 +59,6 @@ pf_t * pf_alloc( pf = calloc(1, sizeof(pf_t)); pf->random_pose_fn = random_pose_fn; - pf->random_pose_data = random_pose_data; pf->min_samples = min_samples; pf->max_samples = max_samples; @@ -291,7 +290,7 @@ void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_d // Resample the distribution -void pf_update_resample(pf_t * pf) +void pf_update_resample(pf_t * pf, void * random_pose_data) { int i; double total; @@ -344,7 +343,7 @@ void pf_update_resample(pf_t * pf) sample_b = set_b->samples + set_b->sample_count++; if (drand48() < w_diff) { - sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data); + sample_b->pose = (pf->random_pose_fn)(random_pose_data); } else { // Can't (easily) combine low-variance sampler with KLD adaptive // sampling, so we'll take the more traditional route. From bd3afbc7b130721b58882808ce6566ab119fe418 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 8 Dec 2022 13:48:18 -0500 Subject: [PATCH 117/131] Add ability to publish layers of the costmap (#3254) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * publish layers of costmap * lint fix * lint round 2 :) * code review * remove isPublishable * lint * test running * rough structure complete * completed test * lint * code review * CI * CI * linting * completed pub test * CostmapLayer::matchSize may be executed concurrently (#3250) * CostmapLayer::matchSize() add a mutex * Fix typo (#3262) * Adding new Nav2 Smoother: Savitzky-Golay Smoother (#3264) * initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions * refactoring RPP a bit for cleanliness on way to ROSCon (#3265) * refactor for RPP on way to ROSCon * fixing header * fixing header * fixing header * fix edge cases test samplings * linting * exceptions for compute path through poses (#3248) * exceptions for compute path through poses * lint fix * code review * code review Co-authored-by: Joshua Wallace * Reclaim Our CI Coverage from the Lords of Painful Subtle Regressions ⚔️⚔️⚔️ (#3266) * test waypoint follower with composition off for logging * adding no composition to all system tests * Added Line Iterator (#3197) * Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan * Use SetParameter Launch API to set the yaml filename for map_server (#3174) * implement launch priority for the mapserver parameter yaml_filename minor fix fix commit reincluded rewritten function comment remaining lines for yaml_filename removed default_value issue with composable node alternative soltion to the condition param not working in composable node remove unused import remove comments and reorder composablenode execution fixing commit fixing format fixing lint Update nav2_bringup/params/nav2_params.yaml Co-authored-by: Steve Macenski * state new ros-rolling release changes and deprecation Co-authored-by: Steve Macenski * adding reconfigure test to thetastar (#3275) * Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (#3276) * Update amcl_node.cpp * fit the code style * fit code style * BT Service Node to throw if service was not available in time (#3256) * throw if service server wasn't available in time mimic the behavior of the bt action node constructor * throw if action unavailable in bt cancel action * use chrono literals namespace * fix linting errors * fix code style divergence * remove exec_depend on behaviortree_cpp_v3 (#3279) * add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (#3284) * add parameterized refinement recursion numbers * fix tests * Add allow_unknown parameter to theta star planner (#3286) * Add allow unknown parameter to theta star planner * Add allow unknown parameter to tests * missing comma * Change cost of unknown tiles * Uncrustify * Include test cases waypoint follwer (#3288) * WIP * included missed_waypoing check * finished inclding test * fix format * return default sleep value * Dynamically changing polygons support (#3245) * Add Collision Monitor polygon topics subscription * Add the support of polygons published in different frame * Internal review * Fix working with polygons visualization * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Move getTransform to nav2_util * Fix misprint * Meet remaining review items: * Update polygon params handling logic * Warn if polygon shape was not set * Publish with ownership movement * Correct polygons_test.cpp parameters handling logic * Adjust README for dynamic polygons logic update Co-authored-by: Steve Macenski * adding getCostScalingFactor (#3290) * Implemented smoother selector bt node (#3283) * Implemented smoother selector bt node Signed-off-by: Owen Hooper <17ofh@queensu.ca> * updated copyright in modified file Signed-off-by: Owen Hooper <17ofh@queensu.ca> Signed-off-by: Owen Hooper <17ofh@queensu.ca> * Pipe error codes (#3251) * issue with finding key * passed up codes to bt_navigator * lint fix * updates * adding error_code_id back in * error codes names in params * bump error codes * lint * spelling * test fix * update behavior trees * cleanup * Update bt_action_server_impl.hpp * code review * lint * code review * log fix * error code for waypoint follower * clean up * remove waypoint error test, too flaky on CI * lint and code review * rough imp for waypoint changes * lint * code review * build fix * clean up * revert * space * remove * try to make github happ * stop gap * loading in param file * working tests :) * lint * fixed cmake * lint * lint * trigger build * added invalid plugin error * added test for piping up error codes * clean up * test waypoint follower * only launch what is needed * waypoint test * revert lines for robot navigator * fix test * waypoint test * switched to uint16 * clean up * code review * todo to note * lint * remove comment * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp Co-authored-by: Steve Macenski * rename error_codes * error code for navigate to pose * error codes for navigate through poses. * error codes for navigate through poses * message update for waypoint follower * rename to error code * update node xml Co-authored-by: Joshua Wallace Co-authored-by: Steve Macenski * Solve bug when CostmapInfoServer is reactivated (#3292) * Solve bug when CostmapInfoServer is reactivated * Smoothness metrics update (#3294) * Update metrics for path smoothness * Support Savitzky-Golay smoother * preempt/cancel test for time behavior, spin pluguin (#3301) * include preempt/cancel test for time behavior, spin pluguin * linting * fix bug in code * lint fix * clean up test * lint * cleaned up test * update * revert Cmake Signed-off-by: Owen Hooper <17ofh@queensu.ca> Co-authored-by: Joshua Wallace Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com> Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com> Co-authored-by: Steve Macenski Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com> Co-authored-by: Afif Swaidan Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Co-authored-by: Pradheep Krishna Co-authored-by: Erwin Lejeune Co-authored-by: Adam Aposhian Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Owen Hooper <17ofh@queensu.ca> Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com> --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 4 +- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 48 ++++- .../test/integration/CMakeLists.txt | 22 +++ .../test/integration/costmap_tests_launch.py | 16 ++ .../integration/test_costmap_2d_publisher.cpp | 178 ++++++++++++++++++ 6 files changed, 263 insertions(+), 7 deletions(-) create mode 100644 nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp 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 2124a12a44..46a71c8e76 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 @@ -320,7 +320,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - std::unique_ptr costmap_publisher_{nullptr}; + std::unique_ptr costmap_publisher_; + + std::vector> layer_publishers_; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 906a9f0324..87881bc7ca 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -187,8 +187,8 @@ void Costmap2DPublisher::publishCostmap() prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); } - float resolution = costmap_->getResolution(); + float resolution = costmap_->getResolution(); if (always_send_full_costmap_ || grid_resolution != resolution || grid_width != costmap_->getSizeInCellsX() || grid_height != costmap_->getSizeInCellsY() || diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 52b538b6bb..aa7fd7f12b 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -210,6 +210,20 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); + auto layers = layered_costmap_->getPlugins(); + + for (auto & layer : *layers) { + auto costmap_layer = std::dynamic_pointer_cast(layer); + if (costmap_layer != nullptr) { + layer_publishers_.emplace_back( + std::make_unique( + shared_from_this(), + costmap_layer.get(), global_frame_, + layer->getName(), always_send_full_costmap_) + ); + } + } + // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); @@ -233,8 +247,12 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_publisher_->on_activate(); footprint_pub_->on_activate(); + costmap_publisher_->on_activate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_activate(); + } // First, make sure that the transform between the robot base frame // and the global frame is available @@ -280,8 +298,13 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); dyn_params_handler.reset(); - costmap_publisher_->on_deactivate(); + footprint_pub_->on_deactivate(); + costmap_publisher_->on_deactivate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_deactivate(); + } stop(); @@ -297,6 +320,13 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + costmap_publisher_.reset(); + clear_costmap_service_.reset(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub.reset(); + } + layered_costmap_.reset(); tf_listener_.reset(); @@ -305,8 +335,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); - costmap_publisher_.reset(); - clear_costmap_service_.reset(); executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; @@ -448,12 +476,22 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); + for (auto &layer_pub: layer_publishers_) { + layer_pub->updateBounds(x0, xn, y0, yn); + } + auto current_time = now(); if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + (current_time < + last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT { RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); + + for (auto &layer_pub: layer_publishers_) { + layer_pub->publishCostmap(); + } + last_publish_ = current_time; } } diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index ef0b148440..0768972a0b 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -64,6 +64,18 @@ target_link_libraries(dyn_params_tests nav2_costmap_2d_core ) +ament_add_gtest_executable(test_costmap_publisher_exec + test_costmap_2d_publisher.cpp +) +ament_target_dependencies(test_costmap_publisher_exec + ${dependencies} +) +target_link_libraries(test_costmap_publisher_exec + nav2_costmap_2d_core + nav2_costmap_2d_client + layers +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -114,6 +126,16 @@ ament_add_test(range_tests TEST_EXECUTABLE=$ ) +ament_add_test(test_costmap_publisher_exec + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback # ament_add_gtest_executable(costmap_tester diff --git a/nav2_costmap_2d/test/integration/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py index 5e8c19a485..706290d826 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py @@ -30,6 +30,20 @@ def main(argv=sys.argv[1:]): launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py') testExecutable = os.getenv('TEST_EXECUTABLE') + map_to_odom = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'] + ) + + odom_to_base_link = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link'] + ) + lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -39,6 +53,8 @@ def main(argv=sys.argv[1:]): ld = LaunchDescription([ IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + map_to_odom, + odom_to_base_link, lifecycle_manager ]) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp new file mode 100644 index 0000000000..1e678d67c7 --- /dev/null +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -0,0 +1,178 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "tf2_ros/transform_listener.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class CostmapRosLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit CostmapRosLifecycleNode(const std::string & name) + : LifecycleNode(name), + name_(name) {} + + ~CostmapRosLifecycleNode() override = default; + + nav2_util::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) override + { + costmap_ros_ = std::make_shared( + name_, + std::string{get_namespace()}, + name_, + get_parameter("use_sim_time").as_bool()); + costmap_thread_ = std::make_unique(costmap_ros_); + + costmap_ros_->configure(); + + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_activate(const rclcpp_lifecycle::State &) override + { + costmap_ros_->activate(); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &) override + { + costmap_ros_->deactivate(); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) override + { + costmap_thread_.reset(); + costmap_ros_->deactivate(); + return nav2_util::CallbackReturn::SUCCESS; + } + +protected: + std::shared_ptr costmap_ros_; + std::unique_ptr costmap_thread_; + const std::string name_; +}; + +class LayerSubscriber +{ +public: + explicit LayerSubscriber(const nav2_util::LifecycleNode::WeakPtr & parent) + { + auto node = parent.lock(); + + callback_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + + std::string topic_name = "/fake_costmap/static_layer_raw"; + layer_sub_ = node->create_subscription( + topic_name, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&LayerSubscriber::layerCallback, this, std::placeholders::_1), + sub_option); + + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, node->get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); + } + + ~LayerSubscriber() + { + executor_thread_.reset(); + } + + std::promise layer_promise_; + +protected: + void layerCallback(const nav2_msgs::msg::Costmap::SharedPtr layer) + { + if (!callback_hit_) { + layer_promise_.set_value(layer); + callback_hit_ = true; + } + } + + rclcpp::Subscription::SharedPtr layer_sub_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + bool callback_hit_{false}; +}; + +class CostmapRosTestFixture : public ::testing::Test +{ +public: + CostmapRosTestFixture() + { + costmap_lifecycle_node_ = std::make_shared("fake_costmap"); + layer_subscriber_ = std::make_shared( + costmap_lifecycle_node_->shared_from_this()); + costmap_lifecycle_node_->on_configure(costmap_lifecycle_node_->get_current_state()); + costmap_lifecycle_node_->on_activate(costmap_lifecycle_node_->get_current_state()); + } + + ~CostmapRosTestFixture() override + { + costmap_lifecycle_node_->on_deactivate(costmap_lifecycle_node_->get_current_state()); + costmap_lifecycle_node_->on_cleanup(costmap_lifecycle_node_->get_current_state()); + } + +protected: + std::shared_ptr costmap_lifecycle_node_; + std::shared_ptr layer_subscriber_; +}; + +TEST_F(CostmapRosTestFixture, costmap_pub_test) +{ + auto future = layer_subscriber_->layer_promise_.get_future(); + auto status = future.wait_for(std::chrono::seconds(5)); + EXPECT_TRUE(status == std::future_status::ready); + + auto costmap_raw = future.get(); + + // Check first 20 cells of the 10by10 map + unsigned int i = 0; + for (; i < 7; ++i) { + EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + } + for (; i < 10; ++i) { + EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); + } + for (; i < 17; ++i) { + EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + } + for (; i < 20; ++i) { + EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); + } +} From 03876eabe5fdc14deced4d1ccec8e8beba03ce73 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 8 Dec 2022 10:49:17 -0800 Subject: [PATCH 118/131] Revert "Add ability to publish layers of the costmap (#3254)" (#3319) This reverts commit 9538ada18a1d0d41b1283e8be6267c2a47ff8767. --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 4 +- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 48 +---- .../test/integration/CMakeLists.txt | 22 --- .../test/integration/costmap_tests_launch.py | 16 -- .../integration/test_costmap_2d_publisher.cpp | 178 ------------------ 6 files changed, 7 insertions(+), 263 deletions(-) delete mode 100644 nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp 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 46a71c8e76..2124a12a44 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 @@ -320,9 +320,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - std::unique_ptr costmap_publisher_; - - std::vector> layer_publishers_; + std::unique_ptr costmap_publisher_{nullptr}; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 87881bc7ca..906a9f0324 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -187,8 +187,8 @@ void Costmap2DPublisher::publishCostmap() prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); } - float resolution = costmap_->getResolution(); + if (always_send_full_costmap_ || grid_resolution != resolution || grid_width != costmap_->getSizeInCellsX() || grid_height != costmap_->getSizeInCellsY() || diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index aa7fd7f12b..52b538b6bb 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -210,20 +210,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); - auto layers = layered_costmap_->getPlugins(); - - for (auto & layer : *layers) { - auto costmap_layer = std::dynamic_pointer_cast(layer); - if (costmap_layer != nullptr) { - layer_publishers_.emplace_back( - std::make_unique( - shared_from_this(), - costmap_layer.get(), global_frame_, - layer->getName(), always_send_full_costmap_) - ); - } - } - // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); @@ -247,12 +233,8 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - footprint_pub_->on_activate(); costmap_publisher_->on_activate(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub->on_activate(); - } + footprint_pub_->on_activate(); // First, make sure that the transform between the robot base frame // and the global frame is available @@ -298,13 +280,8 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); dyn_params_handler.reset(); - - footprint_pub_->on_deactivate(); costmap_publisher_->on_deactivate(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub->on_deactivate(); - } + footprint_pub_->on_deactivate(); stop(); @@ -320,13 +297,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - costmap_publisher_.reset(); - clear_costmap_service_.reset(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub.reset(); - } - layered_costmap_.reset(); tf_listener_.reset(); @@ -335,6 +305,8 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); + costmap_publisher_.reset(); + clear_costmap_service_.reset(); executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; @@ -476,22 +448,12 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); - for (auto &layer_pub: layer_publishers_) { - layer_pub->updateBounds(x0, xn, y0, yn); - } - auto current_time = now(); if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < - last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT { RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); - - for (auto &layer_pub: layer_publishers_) { - layer_pub->publishCostmap(); - } - last_publish_ = current_time; } } diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 0768972a0b..ef0b148440 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -64,18 +64,6 @@ target_link_libraries(dyn_params_tests nav2_costmap_2d_core ) -ament_add_gtest_executable(test_costmap_publisher_exec - test_costmap_2d_publisher.cpp -) -ament_target_dependencies(test_costmap_publisher_exec - ${dependencies} -) -target_link_libraries(test_costmap_publisher_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers -) - ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -126,16 +114,6 @@ ament_add_test(range_tests TEST_EXECUTABLE=$ ) -ament_add_test(test_costmap_publisher_exec - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ENV - TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml - TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} - TEST_EXECUTABLE=$ -) - ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback # ament_add_gtest_executable(costmap_tester diff --git a/nav2_costmap_2d/test/integration/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py index 706290d826..5e8c19a485 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py @@ -30,20 +30,6 @@ def main(argv=sys.argv[1:]): launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py') testExecutable = os.getenv('TEST_EXECUTABLE') - map_to_odom = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'] - ) - - odom_to_base_link = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link'] - ) - lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -53,8 +39,6 @@ def main(argv=sys.argv[1:]): ld = LaunchDescription([ IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), - map_to_odom, - odom_to_base_link, lifecycle_manager ]) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp deleted file mode 100644 index 1e678d67c7..0000000000 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ /dev/null @@ -1,178 +0,0 @@ -// Copyright (c) 2022 Joshua Wallace -// -// 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 "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_costmap_2d/cost_values.hpp" -#include "tf2_ros/transform_listener.h" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" - -class RclCppFixture -{ -public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} -}; -RclCppFixture g_rclcppfixture; - -class CostmapRosLifecycleNode : public nav2_util::LifecycleNode -{ -public: - explicit CostmapRosLifecycleNode(const std::string & name) - : LifecycleNode(name), - name_(name) {} - - ~CostmapRosLifecycleNode() override = default; - - nav2_util::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) override - { - costmap_ros_ = std::make_shared( - name_, - std::string{get_namespace()}, - name_, - get_parameter("use_sim_time").as_bool()); - costmap_thread_ = std::make_unique(costmap_ros_); - - costmap_ros_->configure(); - - return nav2_util::CallbackReturn::SUCCESS; - } - - nav2_util::CallbackReturn - on_activate(const rclcpp_lifecycle::State &) override - { - costmap_ros_->activate(); - return nav2_util::CallbackReturn::SUCCESS; - } - - nav2_util::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &) override - { - costmap_ros_->deactivate(); - return nav2_util::CallbackReturn::SUCCESS; - } - - nav2_util::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) override - { - costmap_thread_.reset(); - costmap_ros_->deactivate(); - return nav2_util::CallbackReturn::SUCCESS; - } - -protected: - std::shared_ptr costmap_ros_; - std::unique_ptr costmap_thread_; - const std::string name_; -}; - -class LayerSubscriber -{ -public: - explicit LayerSubscriber(const nav2_util::LifecycleNode::WeakPtr & parent) - { - auto node = parent.lock(); - - callback_group_ = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, false); - - rclcpp::SubscriptionOptions sub_option; - sub_option.callback_group = callback_group_; - - std::string topic_name = "/fake_costmap/static_layer_raw"; - layer_sub_ = node->create_subscription( - topic_name, - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&LayerSubscriber::layerCallback, this, std::placeholders::_1), - sub_option); - - executor_ = std::make_shared(); - executor_->add_callback_group(callback_group_, node->get_node_base_interface()); - executor_thread_ = std::make_unique(executor_); - } - - ~LayerSubscriber() - { - executor_thread_.reset(); - } - - std::promise layer_promise_; - -protected: - void layerCallback(const nav2_msgs::msg::Costmap::SharedPtr layer) - { - if (!callback_hit_) { - layer_promise_.set_value(layer); - callback_hit_ = true; - } - } - - rclcpp::Subscription::SharedPtr layer_sub_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; - std::unique_ptr executor_thread_; - bool callback_hit_{false}; -}; - -class CostmapRosTestFixture : public ::testing::Test -{ -public: - CostmapRosTestFixture() - { - costmap_lifecycle_node_ = std::make_shared("fake_costmap"); - layer_subscriber_ = std::make_shared( - costmap_lifecycle_node_->shared_from_this()); - costmap_lifecycle_node_->on_configure(costmap_lifecycle_node_->get_current_state()); - costmap_lifecycle_node_->on_activate(costmap_lifecycle_node_->get_current_state()); - } - - ~CostmapRosTestFixture() override - { - costmap_lifecycle_node_->on_deactivate(costmap_lifecycle_node_->get_current_state()); - costmap_lifecycle_node_->on_cleanup(costmap_lifecycle_node_->get_current_state()); - } - -protected: - std::shared_ptr costmap_lifecycle_node_; - std::shared_ptr layer_subscriber_; -}; - -TEST_F(CostmapRosTestFixture, costmap_pub_test) -{ - auto future = layer_subscriber_->layer_promise_.get_future(); - auto status = future.wait_for(std::chrono::seconds(5)); - EXPECT_TRUE(status == std::future_status::ready); - - auto costmap_raw = future.get(); - - // Check first 20 cells of the 10by10 map - unsigned int i = 0; - for (; i < 7; ++i) { - EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); - } - for (; i < 10; ++i) { - EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); - } - for (; i < 17; ++i) { - EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); - } - for (; i < 20; ++i) { - EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); - } -} From 5e16d1001a5474bda4522b938abe8aeb66e2cb8b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 8 Dec 2022 20:08:17 -0500 Subject: [PATCH 119/131] Publish layers (#3320) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * publish layers of costmap * lint fix * lint round 2 :) * code review * remove isPublishable * lint * test running * rough structure complete * completed test * lint * code review * CI * CI * linting * completed pub test * CostmapLayer::matchSize may be executed concurrently (#3250) * CostmapLayer::matchSize() add a mutex * Fix typo (#3262) * Adding new Nav2 Smoother: Savitzky-Golay Smoother (#3264) * initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions * refactoring RPP a bit for cleanliness on way to ROSCon (#3265) * refactor for RPP on way to ROSCon * fixing header * fixing header * fixing header * fix edge cases test samplings * linting * exceptions for compute path through poses (#3248) * exceptions for compute path through poses * lint fix * code review * code review Co-authored-by: Joshua Wallace * Reclaim Our CI Coverage from the Lords of Painful Subtle Regressions ⚔️⚔️⚔️ (#3266) * test waypoint follower with composition off for logging * adding no composition to all system tests * Added Line Iterator (#3197) * Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan * Use SetParameter Launch API to set the yaml filename for map_server (#3174) * implement launch priority for the mapserver parameter yaml_filename minor fix fix commit reincluded rewritten function comment remaining lines for yaml_filename removed default_value issue with composable node alternative soltion to the condition param not working in composable node remove unused import remove comments and reorder composablenode execution fixing commit fixing format fixing lint Update nav2_bringup/params/nav2_params.yaml Co-authored-by: Steve Macenski * state new ros-rolling release changes and deprecation Co-authored-by: Steve Macenski * adding reconfigure test to thetastar (#3275) * Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (#3276) * Update amcl_node.cpp * fit the code style * fit code style * BT Service Node to throw if service was not available in time (#3256) * throw if service server wasn't available in time mimic the behavior of the bt action node constructor * throw if action unavailable in bt cancel action * use chrono literals namespace * fix linting errors * fix code style divergence * remove exec_depend on behaviortree_cpp_v3 (#3279) * add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (#3284) * add parameterized refinement recursion numbers * fix tests * Add allow_unknown parameter to theta star planner (#3286) * Add allow unknown parameter to theta star planner * Add allow unknown parameter to tests * missing comma * Change cost of unknown tiles * Uncrustify * Include test cases waypoint follwer (#3288) * WIP * included missed_waypoing check * finished inclding test * fix format * return default sleep value * Dynamically changing polygons support (#3245) * Add Collision Monitor polygon topics subscription * Add the support of polygons published in different frame * Internal review * Fix working with polygons visualization * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Move getTransform to nav2_util * Fix misprint * Meet remaining review items: * Update polygon params handling logic * Warn if polygon shape was not set * Publish with ownership movement * Correct polygons_test.cpp parameters handling logic * Adjust README for dynamic polygons logic update Co-authored-by: Steve Macenski * adding getCostScalingFactor (#3290) * Implemented smoother selector bt node (#3283) * Implemented smoother selector bt node Signed-off-by: Owen Hooper <17ofh@queensu.ca> * updated copyright in modified file Signed-off-by: Owen Hooper <17ofh@queensu.ca> Signed-off-by: Owen Hooper <17ofh@queensu.ca> * Pipe error codes (#3251) * issue with finding key * passed up codes to bt_navigator * lint fix * updates * adding error_code_id back in * error codes names in params * bump error codes * lint * spelling * test fix * update behavior trees * cleanup * Update bt_action_server_impl.hpp * code review * lint * code review * log fix * error code for waypoint follower * clean up * remove waypoint error test, too flaky on CI * lint and code review * rough imp for waypoint changes * lint * code review * build fix * clean up * revert * space * remove * try to make github happ * stop gap * loading in param file * working tests :) * lint * fixed cmake * lint * lint * trigger build * added invalid plugin error * added test for piping up error codes * clean up * test waypoint follower * only launch what is needed * waypoint test * revert lines for robot navigator * fix test * waypoint test * switched to uint16 * clean up * code review * todo to note * lint * remove comment * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp Co-authored-by: Steve Macenski * rename error_codes * error code for navigate to pose * error codes for navigate through poses. * error codes for navigate through poses * message update for waypoint follower * rename to error code * update node xml Co-authored-by: Joshua Wallace Co-authored-by: Steve Macenski * Solve bug when CostmapInfoServer is reactivated (#3292) * Solve bug when CostmapInfoServer is reactivated * Smoothness metrics update (#3294) * Update metrics for path smoothness * Support Savitzky-Golay smoother * preempt/cancel test for time behavior, spin pluguin (#3301) * include preempt/cancel test for time behavior, spin pluguin * linting * fix bug in code * lint fix * clean up test * lint * cleaned up test * update * revert Cmake * fixed error code test * half time * bump build * revert * lint error Signed-off-by: Owen Hooper <17ofh@queensu.ca> Co-authored-by: Joshua Wallace Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com> Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com> Co-authored-by: Steve Macenski Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com> Co-authored-by: Afif Swaidan Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Co-authored-by: Pradheep Krishna Co-authored-by: Erwin Lejeune Co-authored-by: Adam Aposhian Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Owen Hooper <17ofh@queensu.ca> Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com> --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 4 +- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 48 ++++- .../test/integration/CMakeLists.txt | 22 +++ .../test/integration/costmap_tests_launch.py | 16 ++ .../integration/test_costmap_2d_publisher.cpp | 178 ++++++++++++++++++ .../src/error_codes/test_error_codes.py | 1 + 7 files changed, 264 insertions(+), 7 deletions(-) create mode 100644 nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp 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 2124a12a44..46a71c8e76 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 @@ -320,7 +320,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - std::unique_ptr costmap_publisher_{nullptr}; + std::unique_ptr costmap_publisher_; + + std::vector> layer_publishers_; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 906a9f0324..87881bc7ca 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -187,8 +187,8 @@ void Costmap2DPublisher::publishCostmap() prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); } - float resolution = costmap_->getResolution(); + float resolution = costmap_->getResolution(); if (always_send_full_costmap_ || grid_resolution != resolution || grid_width != costmap_->getSizeInCellsX() || grid_height != costmap_->getSizeInCellsY() || diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 52b538b6bb..aa8983c153 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -210,6 +210,20 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); + auto layers = layered_costmap_->getPlugins(); + + for (auto & layer : *layers) { + auto costmap_layer = std::dynamic_pointer_cast(layer); + if (costmap_layer != nullptr) { + layer_publishers_.emplace_back( + std::make_unique( + shared_from_this(), + costmap_layer.get(), global_frame_, + layer->getName(), always_send_full_costmap_) + ); + } + } + // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); @@ -233,8 +247,12 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_publisher_->on_activate(); footprint_pub_->on_activate(); + costmap_publisher_->on_activate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_activate(); + } // First, make sure that the transform between the robot base frame // and the global frame is available @@ -280,8 +298,13 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); dyn_params_handler.reset(); - costmap_publisher_->on_deactivate(); + footprint_pub_->on_deactivate(); + costmap_publisher_->on_deactivate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_deactivate(); + } stop(); @@ -297,6 +320,13 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + costmap_publisher_.reset(); + clear_costmap_service_.reset(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub.reset(); + } + layered_costmap_.reset(); tf_listener_.reset(); @@ -305,8 +335,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); - costmap_publisher_.reset(); - clear_costmap_service_.reset(); executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; @@ -448,12 +476,22 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); + for (auto & layer_pub: layer_publishers_) { + layer_pub->updateBounds(x0, xn, y0, yn); + } + auto current_time = now(); if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + (current_time < + last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT { RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); + + for (auto & layer_pub: layer_publishers_) { + layer_pub->publishCostmap(); + } + last_publish_ = current_time; } } diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index ef0b148440..0768972a0b 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -64,6 +64,18 @@ target_link_libraries(dyn_params_tests nav2_costmap_2d_core ) +ament_add_gtest_executable(test_costmap_publisher_exec + test_costmap_2d_publisher.cpp +) +ament_target_dependencies(test_costmap_publisher_exec + ${dependencies} +) +target_link_libraries(test_costmap_publisher_exec + nav2_costmap_2d_core + nav2_costmap_2d_client + layers +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -114,6 +126,16 @@ ament_add_test(range_tests TEST_EXECUTABLE=$ ) +ament_add_test(test_costmap_publisher_exec + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback # ament_add_gtest_executable(costmap_tester diff --git a/nav2_costmap_2d/test/integration/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py index 5e8c19a485..706290d826 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py @@ -30,6 +30,20 @@ def main(argv=sys.argv[1:]): launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py') testExecutable = os.getenv('TEST_EXECUTABLE') + map_to_odom = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'] + ) + + odom_to_base_link = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link'] + ) + lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -39,6 +53,8 @@ def main(argv=sys.argv[1:]): ld = LaunchDescription([ IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + map_to_odom, + odom_to_base_link, lifecycle_manager ]) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp new file mode 100644 index 0000000000..1e678d67c7 --- /dev/null +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -0,0 +1,178 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "tf2_ros/transform_listener.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class CostmapRosLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit CostmapRosLifecycleNode(const std::string & name) + : LifecycleNode(name), + name_(name) {} + + ~CostmapRosLifecycleNode() override = default; + + nav2_util::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) override + { + costmap_ros_ = std::make_shared( + name_, + std::string{get_namespace()}, + name_, + get_parameter("use_sim_time").as_bool()); + costmap_thread_ = std::make_unique(costmap_ros_); + + costmap_ros_->configure(); + + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_activate(const rclcpp_lifecycle::State &) override + { + costmap_ros_->activate(); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &) override + { + costmap_ros_->deactivate(); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) override + { + costmap_thread_.reset(); + costmap_ros_->deactivate(); + return nav2_util::CallbackReturn::SUCCESS; + } + +protected: + std::shared_ptr costmap_ros_; + std::unique_ptr costmap_thread_; + const std::string name_; +}; + +class LayerSubscriber +{ +public: + explicit LayerSubscriber(const nav2_util::LifecycleNode::WeakPtr & parent) + { + auto node = parent.lock(); + + callback_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + + std::string topic_name = "/fake_costmap/static_layer_raw"; + layer_sub_ = node->create_subscription( + topic_name, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&LayerSubscriber::layerCallback, this, std::placeholders::_1), + sub_option); + + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, node->get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); + } + + ~LayerSubscriber() + { + executor_thread_.reset(); + } + + std::promise layer_promise_; + +protected: + void layerCallback(const nav2_msgs::msg::Costmap::SharedPtr layer) + { + if (!callback_hit_) { + layer_promise_.set_value(layer); + callback_hit_ = true; + } + } + + rclcpp::Subscription::SharedPtr layer_sub_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + bool callback_hit_{false}; +}; + +class CostmapRosTestFixture : public ::testing::Test +{ +public: + CostmapRosTestFixture() + { + costmap_lifecycle_node_ = std::make_shared("fake_costmap"); + layer_subscriber_ = std::make_shared( + costmap_lifecycle_node_->shared_from_this()); + costmap_lifecycle_node_->on_configure(costmap_lifecycle_node_->get_current_state()); + costmap_lifecycle_node_->on_activate(costmap_lifecycle_node_->get_current_state()); + } + + ~CostmapRosTestFixture() override + { + costmap_lifecycle_node_->on_deactivate(costmap_lifecycle_node_->get_current_state()); + costmap_lifecycle_node_->on_cleanup(costmap_lifecycle_node_->get_current_state()); + } + +protected: + std::shared_ptr costmap_lifecycle_node_; + std::shared_ptr layer_subscriber_; +}; + +TEST_F(CostmapRosTestFixture, costmap_pub_test) +{ + auto future = layer_subscriber_->layer_promise_.get_future(); + auto status = future.wait_for(std::chrono::seconds(5)); + EXPECT_TRUE(status == std::future_status::ready); + + auto costmap_raw = future.get(); + + // Check first 20 cells of the 10by10 map + unsigned int i = 0; + for (; i < 7; ++i) { + EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + } + for (; i < 10; ++i) { + EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); + } + for (; i < 17; ++i) { + EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + } + for (; i < 20; ++i) { + EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); + } +} diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index 3079e3a3fc..aeaff1ac44 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -26,6 +26,7 @@ def main(argv=sys.argv[1:]): rclpy.init() + time.sleep(5) navigator = BasicNavigator() From fe9519d5bbd95ac3b8191faa038a394e3641d73c Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 9 Dec 2022 14:09:28 -0500 Subject: [PATCH 120/131] Smoother error codes (#3296) * minimum error code set * test for invalid smoother * undo * added rest of error tests * Solve bug when CostmapInfoServer is reactivated (#3292) * Solve bug when CostmapInfoServer is reactivated * Smoothness metrics update (#3294) * Update metrics for path smoothness * Support Savitzky-Golay smoother * preempt/cancel test for time behavior, spin pluguin (#3301) * include preempt/cancel test for time behavior, spin pluguin * linting * fix bug in code * removed changes to simple_smoother * reverted simple_smoother * revert * revert * updated constrained smoother * revert * added smoother error for invalid path * linting * invalid path test * added error codes * Timeout exception thrown by smoothers * code review Co-authored-by: Joshua Wallace Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> --- .../plugins/action/smooth_path_action.hpp | 28 ++++-- .../plugins/action/smooth_path_action.cpp | 15 +++ .../nav2_constrained_smoother/smoother.hpp | 5 +- .../src/constrained_smoother.cpp | 6 +- .../include/nav2_core/smoother_exceptions.hpp | 67 ++++++++++++++ nav2_msgs/action/SmoothPath.action | 11 +++ .../nav2_simple_commander/robot_navigator.py | 10 +- .../include/nav2_smoother/nav2_smoother.hpp | 8 ++ nav2_smoother/src/nav2_smoother.cpp | 61 ++++++++++-- nav2_smoother/src/savitzky_golay_smoother.cpp | 3 +- nav2_smoother/src/simple_smoother.cpp | 18 +++- .../test/test_savitzky_golay_smoother.cpp | 3 +- nav2_smoother/test/test_simple_smoother.cpp | 18 ++-- nav2_system_tests/CMakeLists.txt | 18 ++++ nav2_system_tests/package.xml | 1 + .../src/error_codes/error_code_param.yaml | 15 +++ .../smoother/smoother_error_plugin.cpp | 22 +++++ .../smoother/smoother_error_plugin.hpp | 92 +++++++++++++++++++ .../src/error_codes/smoother_plugins.xml | 22 +++++ .../src/error_codes/test_error_codes.py | 37 +++++++- .../error_codes/test_error_codes_launch.py | 11 ++- 21 files changed, 429 insertions(+), 42 deletions(-) create mode 100644 nav2_core/include/nav2_core/smoother_exceptions.hpp create mode 100644 nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.cpp create mode 100644 nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp create mode 100644 nav2_system_tests/src/error_codes/smoother_plugins.xml diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp index 982f4161ea..4bf6dbb934 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp @@ -30,6 +30,10 @@ namespace nav2_behavior_tree */ class SmoothPathAction : public nav2_behavior_tree::BtActionNode { + using Action = nav2_msgs::action::SmoothPath; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::SmoothPathAction @@ -52,6 +56,16 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode( - "smoothed_path", - "Path smoothed by SmootherServer node"), - BT::OutputPort("smoothing_duration", "Time taken to smooth path"), - BT::OutputPort( - "was_completed", "True if smoothing was not interrupted by time limit"), BT::InputPort("unsmoothed_path", "Path to be smoothed"), BT::InputPort("max_smoothing_duration", 3.0, "Maximum smoothing duration"), BT::InputPort( "check_for_collisions", false, "If true collision check will be performed after smoothing"), BT::InputPort("smoother_id", ""), + BT::OutputPort( + "smoothed_path", + "Path smoothed by SmootherServer node"), + BT::OutputPort("smoothing_duration", "Time taken to smooth path"), + BT::OutputPort( + "was_completed", "True if smoothing was not interrupted by time limit"), + BT::OutputPort( + "error_code_id", "The smooth path error code"), }); } }; diff --git a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp index bfbc841ff1..b3291b73cc 100644 --- a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -44,6 +44,21 @@ BT::NodeStatus SmoothPathAction::on_success() setOutput("smoothed_path", result_.result->path); setOutput("smoothing_duration", rclcpp::Duration(result_.result->smoothing_duration).seconds()); setOutput("was_completed", result_.result->was_completed); + // Set empty error code, action was successful + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus SmoothPathAction::on_aborted() +{ + setOutput("error_code_id", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus SmoothPathAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp index b5432bf659..aa3ba1eb02 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp @@ -28,6 +28,7 @@ #include "nav2_constrained_smoother/smoother_cost_function.hpp" #include "nav2_constrained_smoother/utils.hpp" +#include "nav2_core/smoother_exceptions.hpp" #include "ceres/ceres.h" #include "Eigen/Core" @@ -94,7 +95,7 @@ class Smoother { // Path has always at least 2 points if (path.size() < 2) { - throw std::runtime_error("Constrained smoother: Path must have at least 2 points"); + throw nav2_core::InvalidPath("Constrained smoother: Path must have at least 2 points"); } options_.max_solver_time_in_seconds = params.max_time; @@ -110,7 +111,7 @@ class Smoother RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "%s", summary.FullReport().c_str()); } if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost < 0.0) { - return false; + throw nav2_core::FailedToSmoothPath("Solution is not usable"); } } else { RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "Path too short to optimize"); diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 4ce698cda1..305f4141f3 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -23,7 +23,7 @@ #include "nav2_constrained_smoother/constrained_smoother.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_core/smoother_exceptions.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" @@ -137,8 +137,8 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat logger_, "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", plugin_name_.c_str()); - throw std::runtime_error( - "Failed to smooth plan, Ceres could not find a usable solution."); + throw nav2_core::FailedToSmoothPath( + "Failed to smooth plan, Ceres could not find a usable solution"); } // populate final path diff --git a/nav2_core/include/nav2_core/smoother_exceptions.hpp b/nav2_core/include/nav2_core/smoother_exceptions.hpp new file mode 100644 index 0000000000..fcd38dc3b4 --- /dev/null +++ b/nav2_core/include/nav2_core/smoother_exceptions.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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__SMOOTHER_EXCEPTIONS_HPP_ +#define NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_ + +#include +#include + +namespace nav2_core +{ + +class SmootherException : public std::runtime_error +{ +public: + explicit SmootherException(const std::string & description) + : std::runtime_error(description) {} +}; + +class InvalidSmoother : public SmootherException +{ +public: + explicit InvalidSmoother(const std::string & description) + : SmootherException(description) {} +}; + +class InvalidPath : public SmootherException +{ +public: + explicit InvalidPath(const std::string & description) + : SmootherException(description) {} +}; + +class SmootherTimedOut : public SmootherException +{ +public: + explicit SmootherTimedOut(const std::string & description) + : SmootherException(description) {} +}; + +class SmoothedPathInCollision : public SmootherException +{ +public: + explicit SmoothedPathInCollision(const std::string & description) + : SmootherException(description) {} +}; + +class FailedToSmoothPath : public SmootherException +{ +public: + explicit FailedToSmoothPath(const std::string & description) + : SmootherException(description) {} +}; + +} // namespace nav2_core +#endif // NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/SmoothPath.action b/nav2_msgs/action/SmoothPath.action index 18560d70b9..d7443fce5a 100644 --- a/nav2_msgs/action/SmoothPath.action +++ b/nav2_msgs/action/SmoothPath.action @@ -1,3 +1,13 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 UNKNOWN=500 +uint16 INVALID_SMOOTHER=501 +uint16 TIMEOUT=502 +uint16 SMOOTHED_PATH_IN_COLLISION=503 +uint16 FAILED_TO_SMOOTH_PATH=504 +uint16 INVALID_PATH=505 + #goal definition nav_msgs/Path path string smoother_id @@ -8,5 +18,6 @@ bool check_for_collisions nav_msgs/Path path builtin_interfaces/Duration smoothing_duration bool was_completed +uint16 error_code --- #feedback definition diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 5b00c6a49e..0ce475f63f 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -433,16 +433,18 @@ def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_coll self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) self.status = self.result_future.result().status - if self.status != GoalStatus.STATUS_SUCCEEDED: - self.warn(f'Getting path failed with status code: {self.status}') - return None return self.result_future.result().result def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): """Send a `SmoothPath` action request.""" rtn = self._smoothPathImpl( - self, path, smoother_id='', max_duration=2.0, check_for_collision=False) + path, smoother_id='', max_duration=2.0, check_for_collision=False) + + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn(f'Getting path failed with status code: {self.status}') + return None + if not rtn: return None else: diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index 1a4d416503..b52ebf6521 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -114,6 +114,7 @@ class SmootherServer : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; using Action = nav2_msgs::action::SmoothPath; + using ActionGoal = Action::Goal; using ActionServer = nav2_util::SimpleActionServer; /** @@ -135,6 +136,13 @@ class SmootherServer : public nav2_util::LifecycleNode */ bool findSmootherId(const std::string & c_name, std::string & name); + /** + * @brief Validate that the path contains a meaningful path for smoothing + * @param path current path + * return bool if the path is valid + */ + bool validate(const nav_msgs::msg::Path & path); + // Our action server implements the SmoothPath action std::unique_ptr action_server_; diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index f8f48c9886..4e3588436a 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -20,7 +20,7 @@ #include #include -#include "nav2_core/planner_exceptions.hpp" +#include "nav2_core/smoother_exceptions.hpp" #include "nav2_smoother/nav2_smoother.hpp" #include "nav2_util/node_utils.hpp" #include "nav_2d_utils/conversions.hpp" @@ -263,13 +263,17 @@ void SmootherServer::smoothPlan() if (findSmootherId(c_name, current_smoother)) { current_smoother_ = current_smoother; } else { - action_server_->terminate_current(); - return; + throw nav2_core::InvalidSmoother("Invalid Smoother: " + c_name); } // Perform smoothing auto goal = action_server_->get_current_goal(); result->path = goal->path; + + if (!validate(result->path)) { + throw nav2_core::InvalidPath("Requested path to smooth is invalid"); + } + result->was_completed = smoothers_[current_smoother_]->smooth( result->path, goal->max_smoothing_duration); result->smoothing_duration = steady_clock_.now() - start_time; @@ -283,6 +287,7 @@ void SmootherServer::smoothPlan() rclcpp::Duration(goal->max_smoothing_duration).seconds(), rclcpp::Duration(result->smoothing_duration).seconds()); } + plan_publisher_->publish(result->path); // Check for collisions @@ -299,8 +304,11 @@ void SmootherServer::smoothPlan() get_logger(), "Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf", pose2d.x, pose2d.y, pose2d.theta); - action_server_->terminate_current(result); - return; + throw nav2_core::SmoothedPathInCollision( + "Smoothed Path collided at" + "X: " + std::to_string(pose2d.x) + + "Y: " + std::to_string(pose2d.y) + + "Theta: " + std::to_string(pose2d.theta)); } fetch_data = false; } @@ -311,13 +319,50 @@ void SmootherServer::smoothPlan() rclcpp::Duration(result->smoothing_duration).seconds()); action_server_->succeeded_current(result); - } catch (nav2_core::PlannerException & e) { - RCLCPP_ERROR(this->get_logger(), e.what()); - action_server_->terminate_current(); + } catch (nav2_core::InvalidSmoother & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::INVALID_SMOOTHER; + action_server_->terminate_current(result); + return; + } catch (nav2_core::SmootherTimedOut & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::TIMEOUT; + action_server_->terminate_current(result); + return; + } catch (nav2_core::SmoothedPathInCollision & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::SMOOTHED_PATH_IN_COLLISION; + action_server_->terminate_current(result); + return; + } catch (nav2_core::FailedToSmoothPath & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::FAILED_TO_SMOOTH_PATH; + action_server_->terminate_current(result); + return; + } catch (nav2_core::InvalidPath & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::INVALID_PATH; + action_server_->terminate_current(result); + return; + } catch (nav2_core::SmootherException & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::UNKNOWN; + action_server_->terminate_current(result); return; } } +bool SmootherServer::validate(const nav_msgs::msg::Path & path) +{ + if (path.poses.empty()) { + RCLCPP_WARN(get_logger(), "Requested path to smooth is empty"); + return false; + } + + RCLCPP_DEBUG(get_logger(), "Requested path to smooth is valid"); + return true; +} + } // namespace nav2_smoother #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 6a61196ab3..17fe63a926 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -15,6 +15,7 @@ #include #include #include "nav2_smoother/savitzky_golay_smoother.hpp" +#include "nav2_core/smoother_exceptions.hpp" namespace nav2_smoother { @@ -71,7 +72,7 @@ bool SavitzkyGolaySmoother::smooth( RCLCPP_WARN( logger_, "Smoothing time exceeded allowed duration of %0.2f.", max_time.seconds()); - return false; + throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration"); } // Smooth path segment diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 2db6ce3c92..29f3b89a82 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -15,6 +15,7 @@ #include #include #include "nav2_smoother/simple_smoother.hpp" +#include "nav2_core/smoother_exceptions.hpp" namespace nav2_smoother { @@ -65,6 +66,7 @@ bool SimpleSmoother::smooth( double time_remaining = max_time.seconds(); bool success = true, reversing_segment; + unsigned int segments_smoothed = 0; nav_msgs::msg::Path curr_path_segment; curr_path_segment.header = path.header; @@ -84,10 +86,16 @@ bool SimpleSmoother::smooth( time_remaining = max_time.seconds() - duration_cast>(now - start).count(); refinement_ctr_ = 0; - // Smooth path segment naively - success = success && smoothImpl( + bool segment_was_smoothed = smoothImpl( curr_path_segment, reversing_segment, costmap.get(), time_remaining); + if (segment_was_smoothed) { + segments_smoothed++; + } + + // Smooth path segment naively + success = success && segment_was_smoothed; + // Assemble the path changes to the main path std::copy( curr_path_segment.poses.begin(), @@ -96,6 +104,10 @@ bool SimpleSmoother::smooth( } } + if (segments_smoothed == 0) { + throw nav2_core::FailedToSmoothPath("No segments were smoothed"); + } + return success; } @@ -140,7 +152,7 @@ bool SimpleSmoother::smoothImpl( "Smoothing time exceeded allowed duration of %0.2f.", max_time); path = last_path; updateApproximatePathOrientations(path, reversing_segment); - return false; + throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration"); } for (unsigned int i = 1; i != path_size - 1; i++) { diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp index 58860d67db..89df4c3a4f 100644 --- a/nav2_smoother/test/test_savitzky_golay_smoother.cpp +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -22,6 +22,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" +#include "nav2_core/smoother_exceptions.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_msgs/msg/costmap.hpp" @@ -109,7 +110,7 @@ TEST(SmootherTest, test_sg_smoother_basics) // Attempt smoothing with no time given, should fail rclcpp::Duration no_time = rclcpp::Duration::from_seconds(-1.0); // 0 seconds - EXPECT_FALSE(smoother->smooth(straight_regular_path, no_time)); + EXPECT_THROW(smoother->smooth(straight_regular_path, no_time), nav2_core::SmootherTimedOut); smoother->deactivate(); smoother->cleanup(); diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index fc3dceec97..c8f7da8e87 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -14,19 +14,15 @@ #include #include -#include #include -#include -#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_msgs/msg/costmap.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_smoother/simple_smoother.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "nav2_core/smoother_exceptions.hpp" using namespace smoother_utils; // NOLINT using namespace nav2_smoother; // NOLINT @@ -121,7 +117,7 @@ TEST(SmootherTest, test_simple_smoother) rclcpp::Duration no_time = rclcpp::Duration::from_seconds(0.0); // 0 seconds rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1); // 1 second - EXPECT_FALSE(smoother->smooth(straight_irregular_path, no_time)); + EXPECT_THROW(smoother->smooth(straight_irregular_path, no_time), nav2_core::SmootherTimedOut); EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time)); for (uint i = 0; i != straight_irregular_path.poses.size() - 1; i++) { // Check distances are more evenly spaced out now @@ -209,7 +205,7 @@ TEST(SmootherTest, test_simple_smoother) collision_path.poses[9].pose.position.y = 1.4; collision_path.poses[10].pose.position.x = 1.5; collision_path.poses[10].pose.position.y = 1.5; - EXPECT_FALSE(smoother->smooth(collision_path, max_time)); + EXPECT_THROW(smoother->smooth(collision_path, max_time), nav2_core::FailedToSmoothPath); // test cusp / reversing segments nav_msgs::msg::Path reversing_path; @@ -236,9 +232,9 @@ TEST(SmootherTest, test_simple_smoother) reversing_path.poses[9].pose.position.y = 0.1; reversing_path.poses[10].pose.position.x = 0.5; reversing_path.poses[10].pose.position.y = 0.0; - EXPECT_TRUE(smoother->smooth(reversing_path, max_time)); + EXPECT_THROW(smoother->smooth(reversing_path, max_time), nav2_core::FailedToSmoothPath); - // // test rotate in place + // test rotate in place tf2::Quaternion quat1, quat2; quat1.setRPY(0.0, 0.0, 0.0); quat2.setRPY(0.0, 0.0, 1.0); @@ -248,7 +244,7 @@ TEST(SmootherTest, test_simple_smoother) straight_irregular_path.poses[6].pose.position.x = 0.5; straight_irregular_path.poses[6].pose.position.y = 0.5; straight_irregular_path.poses[6].pose.orientation = tf2::toMsg(quat2); - EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time)); + EXPECT_THROW(smoother->smooth(straight_irregular_path, max_time), nav2_core::FailedToSmoothPath); // test max iterations smoother->setMaxItsToInvalid(); @@ -276,5 +272,5 @@ TEST(SmootherTest, test_simple_smoother) max_its_path.poses[9].pose.position.y = 0.9; max_its_path.poses[10].pose.position.x = 0.5; max_its_path.poses[10].pose.position.y = 1.0; - EXPECT_FALSE(smoother->smooth(max_its_path, max_time)); + EXPECT_THROW(smoother->smooth(max_its_path, max_time), nav2_core::FailedToSmoothPath); } diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 558ae809bd..e65b2f928d 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -81,6 +81,23 @@ install(FILES src/error_codes/planner_plugins.xml DESTINATION share/${PROJECT_NAME} ) +set(smoother_plugin_lib smoother) + +add_library(${smoother_plugin_lib} SHARED src/error_codes/smoother/smoother_error_plugin.cpp) +ament_target_dependencies(${smoother_plugin_lib} ${dependencies}) +target_compile_definitions(${smoother_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(nav2_core src/error_codes/smoother_plugins.xml) + +install(TARGETS ${smoother_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(FILES src/error_codes/smoother_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -108,4 +125,5 @@ endif() ament_export_libraries(${local_controller_plugin_lib}) ament_export_libraries(${global_planner_plugin_lib}) +ament_export_libraries(${smoother_plugin_lib}) ament_package() diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 12f269a2d9..32d05a49f8 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -67,5 +67,6 @@ ament_cmake + diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 4c3e839520..388e6f2cdb 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -173,3 +173,18 @@ planner_server: plugin: "nav2_system_tests::NoValidPathErrorPlanner" no_viapoints_given: plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner" + +smoother_server: + ros__parameters: + smoother_plugins: ["unknown", "timeout", "smoothed_path_in_collision", + "failed_to_smooth_path", "invalid_path"] + unknown: + plugin: "nav2_system_tests::UnknownErrorSmoother" + timeout: + plugin: "nav2_system_tests::TimeOutErrorSmoother" + smoothed_path_in_collision: + plugin: "nav2_system_tests::SmoothedPathInCollision" + failed_to_smooth_path: + plugin: "nav2_system_tests::FailedToSmoothPath" + invalid_path: + plugin: "nav2_system_tests::InvalidPath" diff --git a/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.cpp b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.cpp new file mode 100644 index 0000000000..265fd55310 --- /dev/null +++ b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.cpp @@ -0,0 +1,22 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "smoother_error_plugin.hpp" + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::UnknownErrorSmoother, nav2_core::Smoother) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TimeOutErrorSmoother, nav2_core::Smoother) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::SmoothedPathInCollision, nav2_core::Smoother) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::FailedToSmoothPath, nav2_core::Smoother) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::InvalidPath, nav2_core::Smoother) diff --git a/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp new file mode 100644 index 0000000000..45ad7f469e --- /dev/null +++ b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp @@ -0,0 +1,92 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 ERROR_CODES__SMOOTHER__SMOOTHER_ERROR_PLUGIN_HPP_ +#define ERROR_CODES__SMOOTHER__SMOOTHER_ERROR_PLUGIN_HPP_ + +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_core/smoother_exceptions.hpp" + +namespace nav2_system_tests +{ + +class UnknownErrorSmoother : public nav2_core::Smoother +{ +public: + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) override {} + + void cleanup() override {} + + void activate() override {} + + void deactivate() override {} + + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) override + { + throw nav2_core::SmootherException("Unknown Smoother exception"); + } +}; + +class TimeOutErrorSmoother : public UnknownErrorSmoother +{ + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) + { + throw nav2_core::SmootherTimedOut("Smoother timedOut"); + } +}; + +class SmoothedPathInCollision : public UnknownErrorSmoother +{ + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) + { + throw nav2_core::SmoothedPathInCollision("Smoother path in collision"); + } +}; + +class FailedToSmoothPath : public UnknownErrorSmoother +{ + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) + { + throw nav2_core::FailedToSmoothPath("Failed to smooth path"); + } +}; + +class InvalidPath : public UnknownErrorSmoother +{ + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) + { + throw nav2_core::InvalidPath("Invalid path"); + } +}; + +} // namespace nav2_system_tests + +#endif // ERROR_CODES__SMOOTHER__SMOOTHER_ERROR_PLUGIN_HPP_ diff --git a/nav2_system_tests/src/error_codes/smoother_plugins.xml b/nav2_system_tests/src/error_codes/smoother_plugins.xml new file mode 100644 index 0000000000..fb11cad0b1 --- /dev/null +++ b/nav2_system_tests/src/error_codes/smoother_plugins.xml @@ -0,0 +1,22 @@ + + + This smoother produces a unknown exception. + + + This smoother produces a TimedOut exception. + + + This smoother produces a path in collision exception. + + + This smoother produces a failed to smooth exception. + + + This smoother produces an invalid path exception. + + diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index aeaff1ac44..d167d0b00e 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -21,13 +21,16 @@ from nav2_simple_commander.robot_navigator import BasicNavigator import rclpy -from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses +from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses, SmoothPath def main(argv=sys.argv[1:]): rclpy.init() time.sleep(5) + # Wait for lifecycle nodes to come up fully + time.sleep(10) + navigator = BasicNavigator() # Set our demo's initial pose @@ -35,7 +38,7 @@ def main(argv=sys.argv[1:]): initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() - # Initial pose does not respect the orientation... not sure why + # Initial pose initial_pose.pose.position.x = -2.00 initial_pose.pose.position.y = -0.50 initial_pose.pose.orientation.z = 0.0 @@ -122,6 +125,36 @@ def main(argv=sys.argv[1:]): result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) assert result.error_code == error_code, "Compute path through pose error does not match" + # Check compute path to pose error codes + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = navigator.get_clock().now().to_msg() + + pose.pose.position.x = -1.00 + pose.pose.position.y = -0.50 + pose.pose.orientation.z = 0.0 + pose.pose.orientation.w = 1.0 + + pose1 = pose + pose1.pose.position.x = -0.5 + + a_path = Path() + a_path.poses.append(pose) + a_path.poses.append(pose1) + + smoother = { + 'invalid_smoother': SmoothPath.Goal().INVALID_SMOOTHER, + 'unknown': SmoothPath.Goal().UNKNOWN, + 'timeout': SmoothPath.Goal().TIMEOUT, + 'smoothed_path_in_collision': SmoothPath.Goal().SMOOTHED_PATH_IN_COLLISION, + 'failed_to_smooth_path': SmoothPath.Goal().FAILED_TO_SMOOTH_PATH, + 'invalid_path': SmoothPath.Goal().INVALID_PATH + } + + for smoother, error_code in smoother.items(): + result = navigator._smoothPathImpl(a_path, smoother) + assert result.error_code == error_code, "Smoother error does not match" + navigator.lifecycleShutdown() rclpy.shutdown() exit(0) diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py index 165e1806ce..8a2ceb8481 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -31,7 +31,8 @@ def generate_launch_description(): ('/tf_static', 'tf_static')] lifecycle_nodes = ['controller_server', - 'planner_server'] + 'planner_server', + 'smoother_server'] load_nodes = GroupAction( actions=[ @@ -60,6 +61,14 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[params_file], remappings=remappings), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn_delay=2.0, + parameters=[params_file], + remappings=remappings), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', From 7f412342bdfd0c745edcace8df248898a7243186 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sat, 10 Dec 2022 16:06:31 -0500 Subject: [PATCH 121/131] fix (#3321) Co-authored-by: Joshua Wallace --- nav2_system_tests/src/error_codes/test_error_codes.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index d167d0b00e..edab4390ec 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -26,10 +26,6 @@ def main(argv=sys.argv[1:]): rclpy.init() - time.sleep(5) - - # Wait for lifecycle nodes to come up fully - time.sleep(10) navigator = BasicNavigator() @@ -57,6 +53,7 @@ def main(argv=sys.argv[1:]): path.poses.append(goal_pose) path.poses.append(goal_pose1) + navigator._waitForNodeToActivate("controller_server") follow_path = { 'unknown': FollowPath.Goal().UNKNOWN, 'invalid_controller': FollowPath.Goal().INVALID_CONTROLLER, @@ -90,6 +87,7 @@ def main(argv=sys.argv[1:]): goal_pose.pose.orientation.z = 0.0 goal_pose.pose.orientation.w = 1.0 + navigator._waitForNodeToActivate("planner_server") compute_path_to_pose = { 'unknown': ComputePathToPose.Goal().UNKNOWN, 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, @@ -142,6 +140,7 @@ def main(argv=sys.argv[1:]): a_path.poses.append(pose) a_path.poses.append(pose1) + navigator._waitForNodeToActivate("smoother_server") smoother = { 'invalid_smoother': SmoothPath.Goal().INVALID_SMOOTHER, 'unknown': SmoothPath.Goal().UNKNOWN, From 6b02da4a6c7398c3a724284a5e30a35bf8a35d6c Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 14 Dec 2022 14:26:00 -0500 Subject: [PATCH 122/131] rebase --- .../include/nav2_behavior_tree/bt_service_node.hpp | 1 + nav2_lifecycle_manager/src/lifecycle_manager.cpp | 1 + nav2_util/include/nav2_util/service_client.hpp | 1 + 3 files changed, 3 insertions(+) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 91b0c0cd9d..0b7d2cb68b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -62,6 +62,7 @@ class BtServiceNode : public BT::ActionNodeBase // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); + rclcpp::QoS test(1); service_client_ = node_->create_client( service_name_, rclcpp::SystemDefaultsQoS(), diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 2cbe22768c..1bb7759d15 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -59,6 +59,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_); callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::QoS test(10); manager_srv_ = create_service( get_name() + std::string("/manage_nodes"), std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 62e1b58838..3bc16db65e 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -44,6 +44,7 @@ class ServiceClient rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + rclcpp::QoS test(10); client_ = node_->create_client( service_name, rclcpp::SystemDefaultsQoS(), From e32431c186b6e2bf90084a905d80ef7ddd9a2332 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 5 Aug 2022 18:12:48 -0400 Subject: [PATCH 123/131] Revert "rolling issue" This reverts commit 7a14c46294f87afa921bf5f39b3cd345094f6dc9. --- .../include/nav2_behavior_tree/bt_service_node.hpp | 1 - nav2_lifecycle_manager/src/lifecycle_manager.cpp | 1 - nav2_util/include/nav2_util/service_client.hpp | 1 - 3 files changed, 3 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 0b7d2cb68b..91b0c0cd9d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -62,7 +62,6 @@ class BtServiceNode : public BT::ActionNodeBase // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); - rclcpp::QoS test(1); service_client_ = node_->create_client( service_name_, rclcpp::SystemDefaultsQoS(), diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 1bb7759d15..2cbe22768c 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -59,7 +59,6 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_); callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - rclcpp::QoS test(10); manager_srv_ = create_service( get_name() + std::string("/manage_nodes"), std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 3bc16db65e..62e1b58838 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -44,7 +44,6 @@ class ServiceClient rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - rclcpp::QoS test(10); client_ = node_->create_client( service_name, rclcpp::SystemDefaultsQoS(), From d50a2d1938bbb3956f0c75b497a3bf1a46eab2ec Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 14 Dec 2022 14:43:13 -0500 Subject: [PATCH 124/131] added Path with cost message --- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/PathWithCost.msg | 2 ++ 2 files changed, 3 insertions(+) create mode 100644 nav2_msgs/msg/PathWithCost.msg diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 09fbc08226..a9800ac76f 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" + "msg/PathWithCost.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/msg/PathWithCost.msg b/nav2_msgs/msg/PathWithCost.msg new file mode 100644 index 0000000000..6ef5c69997 --- /dev/null +++ b/nav2_msgs/msg/PathWithCost.msg @@ -0,0 +1,2 @@ +geometry_msgs/PoseStamped[] poses +float32[] costs From 6664b93590f680e0fe74b7fcc0ea6223edee0ec0 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 14 Dec 2022 16:56:10 -0500 Subject: [PATCH 125/131] switch to PathWithCost --- .../compute_path_through_poses_action.hpp | 2 +- .../action/compute_path_to_pose_action.hpp | 4 +-- .../plugins/action/follow_path_action.hpp | 2 +- .../plugins/action/smooth_path_action.hpp | 6 ++--- .../plugins/action/truncate_path_action.hpp | 6 ++--- .../action/truncate_path_local_action.hpp | 10 +++---- .../condition/is_path_valid_condition.hpp | 2 +- .../path_expiring_timer_condition.hpp | 6 ++--- .../decorator/path_longer_on_approach.hpp | 18 ++++++------- .../compute_path_through_poses_action.cpp | 4 +-- .../action/compute_path_to_pose_action.cpp | 6 ++--- .../plugins/action/follow_path_action.cpp | 2 +- .../action/remove_passed_goals_action.cpp | 2 +- .../plugins/action/truncate_path_action.cpp | 5 ++-- .../action/truncate_path_local_action.cpp | 6 ++--- .../condition/is_path_valid_condition.cpp | 2 +- .../path_expiring_timer_condition.cpp | 2 +- .../decorator/path_longer_on_approach.cpp | 10 +++---- ...test_compute_path_through_poses_action.cpp | 14 +++++----- .../test_compute_path_to_pose_action.cpp | 14 +++++----- .../action/test_controller_selector_node.cpp | 2 +- .../action/test_follow_path_action.cpp | 8 +++--- .../test_goal_checker_selector_node.cpp | 2 +- .../action/test_planner_selector_node.cpp | 2 +- .../test_remove_passed_goals_action.cpp | 2 +- .../action/test_smooth_path_action.cpp | 8 +++--- .../action/test_smoother_selector_node.cpp | 2 +- .../action/test_truncate_path_action.cpp | 6 ++--- .../test_truncate_path_local_action.cpp | 26 +++++++++---------- .../condition/test_path_expiring_timer.cpp | 4 +-- .../decorator/test_goal_updater_node.cpp | 2 +- .../test_path_longer_on_approach.cpp | 12 ++++----- .../navigators/navigate_through_poses.hpp | 2 +- .../navigators/navigate_to_pose.hpp | 2 +- .../src/navigators/navigate_through_poses.cpp | 4 +-- .../src/navigators/navigate_to_pose.cpp | 4 +-- .../constrained_smoother.hpp | 2 +- .../src/constrained_smoother.cpp | 2 +- .../test/test_constrained_smoother.cpp | 2 +- .../nav2_controller/controller_server.hpp | 4 +-- nav2_controller/src/controller_server.cpp | 4 +-- nav2_core/include/nav2_core/controller.hpp | 4 +-- .../include/nav2_core/global_planner.hpp | 4 +-- nav2_core/include/nav2_core/smoother.hpp | 4 +-- .../include/dwb_core/dwb_local_planner.hpp | 2 +- .../dwb_core/include/dwb_core/publisher.hpp | 10 +++---- .../dwb_core/src/dwb_local_planner.cpp | 4 +-- .../dwb_core/src/publisher.cpp | 12 ++++----- .../include/nav_2d_utils/conversions.hpp | 10 +++---- .../nav_2d_utils/src/conversions.cpp | 14 +++++----- .../nav_2d_utils/test/2d_utils_test.cpp | 8 +++--- .../action/ComputePathThroughPoses.action | 2 +- nav2_msgs/action/ComputePathToPose.action | 2 +- nav2_msgs/action/FollowPath.action | 2 +- nav2_msgs/action/SmoothPath.action | 4 +-- nav2_msgs/msg/PathWithCost.msg | 1 + nav2_msgs/srv/IsPathValid.srv | 4 +-- .../nav2_navfn_planner/navfn_planner.hpp | 10 +++---- nav2_navfn_planner/src/navfn_planner.cpp | 10 +++---- .../include/nav2_planner/planner_server.hpp | 10 +++---- nav2_planner/src/planner_server.cpp | 18 ++++++------- .../CMakeLists.txt | 2 ++ .../collision_checker.hpp | 2 +- .../path_handler.hpp | 8 +++--- .../regulated_pure_pursuit_controller.hpp | 12 ++++----- .../regulation_functions.hpp | 4 +-- .../package.xml | 2 +- .../src/collision_checker.cpp | 4 +-- .../src/path_handler.cpp | 4 +-- .../src/regulated_pure_pursuit_controller.cpp | 10 +++---- .../test/CMakeLists.txt | 2 +- .../test/path_utils/path_utils.cpp | 10 +++---- .../test/path_utils/path_utils.hpp | 10 +++---- .../test/test_regulated_pp.cpp | 16 ++++++------ .../nav2_rotation_shim_controller.hpp | 4 +-- .../src/nav2_rotation_shim_controller.cpp | 2 +- .../test/test_shim_controller.cpp | 12 ++++----- .../nav2_smac_planner/smac_planner_2d.hpp | 6 ++--- .../nav2_smac_planner/smac_planner_hybrid.hpp | 6 ++--- .../smac_planner_lattice.hpp | 6 ++--- .../include/nav2_smac_planner/smoother.hpp | 14 +++++----- nav2_smac_planner/src/smac_planner_2d.cpp | 6 ++--- nav2_smac_planner/src/smac_planner_hybrid.cpp | 6 ++--- .../src/smac_planner_lattice.cpp | 6 ++--- nav2_smac_planner/src/smoother.cpp | 18 ++++++------- nav2_smac_planner/test/test_smoother.cpp | 4 +-- .../include/nav2_smoother/nav2_smoother.hpp | 4 +-- .../nav2_smoother/savitzky_golay_smoother.hpp | 6 ++--- .../include/nav2_smoother/simple_smoother.hpp | 6 ++--- .../include/nav2_smoother/smoother_utils.hpp | 6 ++--- nav2_smoother/src/nav2_smoother.cpp | 4 +-- nav2_smoother/src/savitzky_golay_smoother.cpp | 6 ++--- nav2_smoother/src/simple_smoother.cpp | 10 +++---- .../test/test_savitzky_golay_smoother.cpp | 8 +++--- nav2_smoother/test/test_simple_smoother.cpp | 14 +++++----- nav2_smoother/test/test_smoother_server.cpp | 2 +- .../controller/controller_error_plugins.hpp | 2 +- .../planner/planner_error_plugin.hpp | 22 ++++++++-------- .../smoother/smoother_error_plugin.hpp | 10 +++---- .../src/planning/planner_tester.cpp | 2 +- .../src/planning/planner_tester.hpp | 6 ++--- .../planning/test_planner_costmaps_node.cpp | 2 +- .../planning/test_planner_is_path_valid.cpp | 2 +- .../src/planning/test_planner_plugins.cpp | 6 ++--- .../src/planning/test_planner_random_node.cpp | 2 +- .../theta_star_planner.hpp | 8 +++--- .../src/theta_star_planner.cpp | 10 +++---- .../test/test_theta_star.cpp | 2 +- .../include/nav2_util/geometry_utils.hpp | 4 +-- nav2_util/test/test_geometry_utils.cpp | 6 ++--- .../waypoint_follower.hpp | 2 +- tools/smoother_benchmarking/README.md | 2 +- 112 files changed, 346 insertions(+), 344 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index 0b5f3b0972..c2194116f8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -84,7 +84,7 @@ class ComputePathThroughPosesAction BT::InputPort( "planner_id", "", "Mapped name to the planner plugin type to use"), - BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), + BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), BT::OutputPort( "error_code_id", "The compute path through poses error code"), }); 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 829fd04428..a1b2998399 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 @@ -18,7 +18,7 @@ #include #include "nav2_msgs/action/compute_path_to_pose.hpp" -#include "nav_msgs/msg/path.h" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" namespace nav2_behavior_tree @@ -84,7 +84,7 @@ class ComputePathToPoseAction : public BtActionNode( "planner_id", "", "Mapped name to the planner plugin type to use"), - BT::OutputPort("path", "Path created by ComputePathToPose node"), + BT::OutputPort("path", "Path created by ComputePathToPose node"), BT::OutputPort( "error_code_id", "The compute path to pose error code"), }); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index 4460b0e9a4..2ba06394a0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -81,7 +81,7 @@ class FollowPathAction : public BtActionNode { return providedBasicPorts( { - BT::InputPort("path", "Path to follow"), + BT::InputPort("path", "Path to follow"), BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp index 4bf6dbb934..cdd4b537b0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp @@ -19,7 +19,7 @@ #include #include "nav2_msgs/action/smooth_path.hpp" -#include "nav_msgs/msg/path.h" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" namespace nav2_behavior_tree @@ -74,13 +74,13 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode("unsmoothed_path", "Path to be smoothed"), + BT::InputPort("unsmoothed_path", "Path to be smoothed"), BT::InputPort("max_smoothing_duration", 3.0, "Maximum smoothing duration"), BT::InputPort( "check_for_collisions", false, "If true collision check will be performed after smoothing"), BT::InputPort("smoother_id", ""), - BT::OutputPort( + BT::OutputPort( "smoothed_path", "Path smoothed by SmootherServer node"), BT::OutputPort("smoothing_duration", "Time taken to smooth path"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp index 2604bcfea1..76d67dc364 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp @@ -19,7 +19,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "behaviortree_cpp_v3/action_node.h" @@ -48,8 +48,8 @@ class TruncatePath : public BT::ActionNodeBase static BT::PortsList providedPorts() { return { - BT::InputPort("input_path", "Original Path"), - BT::OutputPort("output_path", "Path truncated to a certain distance"), + BT::InputPort("input_path", "Original Path"), + BT::OutputPort("output_path", "Path truncated to a certain distance"), BT::InputPort("distance", 1.0, "distance"), }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp index 113e0d83e7..9026e471ee 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp @@ -20,7 +20,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "behaviortree_cpp_v3/action_node.h" #include "tf2_ros/buffer.h" @@ -50,8 +50,8 @@ class TruncatePathLocal : public BT::ActionNodeBase static BT::PortsList providedPorts() { return { - BT::InputPort("input_path", "Original Path"), - BT::OutputPort( + BT::InputPort("input_path", "Original Path"), + BT::OutputPort( "output_path", "Path truncated to a certain distance around robot"), BT::InputPort( "distance_forward", 8.0, @@ -115,8 +115,8 @@ class TruncatePathLocal : public BT::ActionNodeBase std::shared_ptr tf_buffer_; - nav_msgs::msg::Path path_; - nav_msgs::msg::Path::_poses_type::iterator closest_pose_detection_begin_; + nav2_msgs::msg::PathWithCost path_; + nav2_msgs::msg::PathWithCost::_poses_type::iterator closest_pose_detection_begin_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 4a7a844dd9..5cf2d242ff 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -57,7 +57,7 @@ class IsPathValidCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("path", "Path to Check"), + BT::InputPort("path", "Path to Check"), BT::InputPort("server_timeout"), BT::InputPort("consider_cost_change", true, "Consider cost changes") }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp index fb2f42f3d4..cef7eff98a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp @@ -19,7 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp_v3/condition_node.h" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" namespace nav2_behavior_tree { @@ -56,14 +56,14 @@ class PathExpiringTimerCondition : public BT::ConditionNode { return { BT::InputPort("seconds", 1.0, "Seconds"), - BT::InputPort("path") + BT::InputPort("path") }; } private: rclcpp::Node::SharedPtr node_; rclcpp::Time start_; - nav_msgs::msg::Path prev_path_; + nav2_msgs::msg::PathWithCost prev_path_; double period_; bool first_time_; }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp index 6e41516434..508df776b2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp @@ -20,7 +20,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "behaviortree_cpp_v3/decorator_node.h" #include "rclcpp/rclcpp.hpp" @@ -50,7 +50,7 @@ class PathLongerOnApproach : public BT::DecoratorNode static BT::PortsList providedPorts() { return { - BT::InputPort("path", "Planned Path"), + BT::InputPort("path", "Planned Path"), BT::InputPort( "prox_len", 3.0, "Proximity length (m) for the path to be longer on approach"), @@ -74,8 +74,8 @@ class PathLongerOnApproach : public BT::DecoratorNode * @return whether the path is updated for the current goal */ bool isPathUpdated( - nav_msgs::msg::Path & new_path, - nav_msgs::msg::Path & old_path); + nav2_msgs::msg::PathWithCost & new_path, + nav2_msgs::msg::PathWithCost & old_path); /** * @brief Checks if the robot is in the goal proximity @@ -84,7 +84,7 @@ class PathLongerOnApproach : public BT::DecoratorNode * @return whether the robot is in the goal proximity */ bool isRobotInGoalProximity( - nav_msgs::msg::Path & old_path, + nav2_msgs::msg::PathWithCost & old_path, double & prox_leng); /** @@ -95,13 +95,13 @@ class PathLongerOnApproach : public BT::DecoratorNode * @return whether the new path is longer */ bool isNewPathLonger( - nav_msgs::msg::Path & new_path, - nav_msgs::msg::Path & old_path, + nav2_msgs::msg::PathWithCost & new_path, + nav2_msgs::msg::PathWithCost & old_path, double & length_factor); private: - nav_msgs::msg::Path new_path_; - nav_msgs::msg::Path old_path_; + nav2_msgs::msg::PathWithCost new_path_; + nav2_msgs::msg::PathWithCost old_path_; double prox_len_ = std::numeric_limits::max(); double length_factor_ = std::numeric_limits::max(); rclcpp::Node::SharedPtr node_; diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index 5fede84f2c..e70415c11f 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -48,7 +48,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success() BT::NodeStatus ComputePathThroughPosesAction::on_aborted() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; @@ -56,7 +56,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_aborted() BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled setOutput("error_code_id", ActionGoal::NONE); 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 0619a41ee0..5d928e3aef 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 @@ -47,7 +47,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() BT::NodeStatus ComputePathToPoseAction::on_aborted() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; @@ -55,7 +55,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() BT::NodeStatus ComputePathToPoseAction::on_cancelled() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled setOutput("error_code_id", ActionGoal::NONE); @@ -64,7 +64,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() void ComputePathToPoseAction::halt() { - nav_msgs::msg::Path empty_path; + nav2_msgs::msg::PathWithCost empty_path; setOutput("path", empty_path); BtActionNode::halt(); } diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 3649fad8a1..7d6f7df30d 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -58,7 +58,7 @@ void FollowPathAction::on_wait_for_result( std::shared_ptr/*feedback*/) { // Grab the new path - nav_msgs::msg::Path new_path; + nav2_msgs::msg::PathWithCost new_path; getInput("path", new_path); // Check if it is not same with the current one diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 99568f933f..c9fb94089e 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -16,7 +16,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp" diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp index 7f919a30f3..6cd0e81d25 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -15,9 +15,8 @@ #include #include -#include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "behaviortree_cpp_v3/decorator_node.h" @@ -40,7 +39,7 @@ inline BT::NodeStatus TruncatePath::tick() { setStatus(BT::NodeStatus::RUNNING); - nav_msgs::msg::Path input_path; + nav2_msgs::msg::PathWithCost input_path; getInput("input_path", input_path); diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp index 934f2b86bb..da8d1aaae3 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "tf2_ros/create_timer_ros.h" #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp" @@ -54,7 +54,7 @@ inline BT::NodeStatus TruncatePathLocal::tick() getInput("max_robot_pose_search_dist", max_robot_pose_search_dist); bool path_pruning = std::isfinite(max_robot_pose_search_dist); - nav_msgs::msg::Path new_path; + nav2_msgs::msg::PathWithCost new_path; getInput("input_path", new_path); if (!path_pruning || new_path != path_) { path_ = new_path; @@ -96,7 +96,7 @@ inline BT::NodeStatus TruncatePathLocal::tick() auto backward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance( std::reverse_iterator(current_pose + 1), path_.poses.rend(), distance_backward); - nav_msgs::msg::Path output_path; + nav2_msgs::msg::PathWithCost output_path; output_path.header = path_.header; output_path.poses = std::vector( backward_pose_it.base(), forward_pose_it); diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 570b80d0d4..a9abe347a5 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -34,7 +34,7 @@ IsPathValidCondition::IsPathValidCondition( BT::NodeStatus IsPathValidCondition::tick() { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; getInput("path", path); bool consider_cost_change; getInput("consider_cost_change", consider_cost_change); diff --git a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp index e018e02535..93509013ad 100644 --- a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp @@ -43,7 +43,7 @@ BT::NodeStatus PathExpiringTimerCondition::tick() } // Grab the new path - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; getInput("path", path); // Reset timer if the path has been updated diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index d4b40b0964..b17d845560 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -31,8 +31,8 @@ PathLongerOnApproach::PathLongerOnApproach( } bool PathLongerOnApproach::isPathUpdated( - nav_msgs::msg::Path & new_path, - nav_msgs::msg::Path & old_path) + nav2_msgs::msg::PathWithCost & new_path, + nav2_msgs::msg::PathWithCost & old_path) { return new_path != old_path && old_path.poses.size() != 0 && new_path.poses.size() != 0 && @@ -40,15 +40,15 @@ bool PathLongerOnApproach::isPathUpdated( } bool PathLongerOnApproach::isRobotInGoalProximity( - nav_msgs::msg::Path & old_path, + nav2_msgs::msg::PathWithCost & old_path, double & prox_leng) { return nav2_util::geometry_utils::calculate_path_length(old_path, 0) < prox_leng; } bool PathLongerOnApproach::isNewPathLonger( - nav_msgs::msg::Path & new_path, - nav_msgs::msg::Path & old_path, + nav2_msgs::msg::PathWithCost & new_path, + nav2_msgs::msg::PathWithCost & old_path, double & length_factor) { return nav2_util::geometry_utils::calculate_path_length(new_path, 0) > diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 122c764621..8190634440 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -19,7 +19,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" @@ -152,8 +152,8 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct - nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + nav2_msgs::msg::PathWithCost path; + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); @@ -173,7 +173,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -218,8 +218,8 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct - nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + nav2_msgs::msg::PathWithCost path; + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); @@ -241,7 +241,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index 5e35a1981d..ed395ccb4c 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 @@ -18,7 +18,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" @@ -149,8 +149,8 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct - nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + nav2_msgs::msg::PathWithCost path; + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); @@ -170,7 +170,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -215,8 +215,8 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct - nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + nav2_msgs::msg::PathWithCost path; + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); @@ -238,7 +238,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 64ce713888..487f2ae9ca 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -22,7 +22,7 @@ #include "../../test_action_server.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "std_msgs/msg/string.hpp" class ControllerSelectorTestFixture : public ::testing::Test 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 1325070e00..a71b164a15 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 @@ -18,7 +18,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" @@ -124,10 +124,10 @@ TEST_F(FollowPathActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // set new path on blackboard - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(1); path.poses[0].pose.position.x = 1.0; - config_->blackboard->set("path", path); + config_->blackboard->set("path", path); // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { @@ -147,7 +147,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) // set new goal path.poses[0].pose.position.x = -2.5; - config_->blackboard->set("path", path); + config_->blackboard->set("path", path); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index cc0b574ff9..861ea15dd2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -22,7 +22,7 @@ #include "../../test_action_server.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "std_msgs/msg/string.hpp" class GoalCheckerSelectorTestFixture : public ::testing::Test diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index baf856de64..7dad38cb33 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -22,7 +22,7 @@ #include "../../test_action_server.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "std_msgs/msg/string.hpp" class PlannerSelectorTestFixture : public ::testing::Test diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 4dc971a2ca..ff3ec06755 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -19,7 +19,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index 5bbf108c8f..378032bb7d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -19,7 +19,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "behaviortree_cpp_v3/bt_factory.h" @@ -123,7 +123,7 @@ TEST_F(SmoothPathActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { @@ -143,13 +143,13 @@ TEST_F(SmoothPathActionTestFixture, test_tick) pose.pose.position.x = -2.5; pose.pose.orientation.x = 1.0; path.poses.push_back(pose); - config_->blackboard->set("unsmoothed_path", path); + config_->blackboard->set("unsmoothed_path", path); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path path_empty; + nav2_msgs::msg::PathWithCost path_empty; EXPECT_NE(path_empty, path); EXPECT_EQ(action_server_->getCurrentGoal()->path, path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index c59cac1b2c..7bcccaf95a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -22,7 +22,7 @@ #include "../../test_action_server.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "std_msgs/msg/string.hpp" class SmootherSelectorTestFixture : public ::testing::Test diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp index 11c56ef082..fbea4b76f9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" @@ -96,7 +96,7 @@ TEST_F(TruncatePathTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.stamp = node_->now(); geometry_msgs::msg::PoseStamped pose; @@ -126,7 +126,7 @@ TEST_F(TruncatePathTestFixture, test_tick) tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_NE(path, truncated_path); diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp index 4947c6bc8c..76778ab042 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp @@ -17,7 +17,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" @@ -64,9 +64,9 @@ class TruncatePathLocalTestFixture : public nav2_behavior_tree::BehaviorTreeTest return pose; } - nav_msgs::msg::Path createLoopCrossingTestPath() + nav2_msgs::msg::PathWithCost createLoopCrossingTestPath() { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.stamp = node_->now(); path.header.frame_id = "map"; @@ -125,7 +125,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create path and set it on blackboard - nav_msgs::msg::Path path = createLoopCrossingTestPath(); + nav2_msgs::msg::PathWithCost path = createLoopCrossingTestPath(); EXPECT_EQ(path.poses.size(), 12u); config_->blackboard->set("path", path); @@ -137,7 +137,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -220,7 +220,7 @@ TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create path and set it on blackboard - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.stamp = node_->now(); path.header.frame_id = "map"; @@ -232,7 +232,7 @@ TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path) { tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -263,7 +263,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create path and set it on blackboard - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.stamp = node_->now(); path.header.frame_id = "map"; @@ -275,7 +275,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose) { tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); @@ -305,7 +305,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav_msgs::msg::Path path = createLoopCrossingTestPath(); + nav2_msgs::msg::PathWithCost path = createLoopCrossingTestPath(); EXPECT_EQ(path.poses.size(), 12u); config_->blackboard->set("path", path); @@ -316,7 +316,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame) { tree_->rootNode()->executeTick(); } - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->get("truncated_path", truncated_path); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); @@ -347,8 +347,8 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create path and set it on blackboard - nav_msgs::msg::Path path = createLoopCrossingTestPath(); - nav_msgs::msg::Path truncated_path; + nav2_msgs::msg::PathWithCost path = createLoopCrossingTestPath(); + nav2_msgs::msg::PathWithCost truncated_path; config_->blackboard->set("path", path); diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp index 385b1cfed8..cae182b3a5 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp @@ -73,12 +73,12 @@ TEST_F(PathExpiringTimerConditionTestFixture, test_behavior) } // place a new path on the blackboard to reset the timer - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = 1.0; path.poses.push_back(pose); - config_->blackboard->set("path", path); + config_->blackboard->set("path", path); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); rclcpp::sleep_for(1500ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 341192757f..6e4417c0ce 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -18,7 +18,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp index 7a71f0a74b..1cb3a5030f 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp @@ -20,7 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "../../test_behavior_tree_fixture.hpp" #include "../../test_dummy_tree_node.hpp" @@ -100,13 +100,13 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // set new path on blackboard - nav_msgs::msg::Path new_path; + nav2_msgs::msg::PathWithCost new_path; new_path.poses.resize(10); for (unsigned int i = 0; i < new_path.poses.size(); i++) { // Assuming distance between waypoints to be 1.5m new_path.poses[i].pose.position.x = 1.5 * i; } - config_->blackboard->set("path", new_path); + config_->blackboard->set("path", new_path); tree_->rootNode()->executeTick(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -126,13 +126,13 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // set old path on blackboard - nav_msgs::msg::Path old_path; + nav2_msgs::msg::PathWithCost old_path; old_path.poses.resize(5); for (unsigned int i = 1; i <= old_path.poses.size(); i++) { // Assuming distance between waypoints to be 3.0m old_path.poses[i - 1].pose.position.x = 3.0 * i; } - config_->blackboard->set("path", old_path); + config_->blackboard->set("path", old_path); tree_->rootNode()->executeTick(); // set new path on blackboard @@ -141,7 +141,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // Assuming distance between waypoints to be 1.5m new_path.poses[i].pose.position.x = 1.5 * i; } - config_->blackboard->set("path", new_path); + config_->blackboard->set("path", new_path); tree_->rootNode()->executeTick(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index d6e218d860..653391eb75 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_bt_navigator/navigator.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/odometry_utils.hpp" diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 20230aa10e..88d7fe98d3 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -25,7 +25,7 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/odometry_utils.hpp" namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 34a3f429a4..29aedb4155 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -120,8 +120,8 @@ NavigateThroughPosesNavigator::onLoop() try { // Get current path points - nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + nav2_msgs::msg::PathWithCost current_path; + blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 226a673512..8b16691574 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -123,8 +123,8 @@ NavigateToPoseNavigator::onLoop() try { // Get current path points - nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + nav2_msgs::msg::PathWithCost current_path; + blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp index bfd564772e..e4729e11e7 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp @@ -85,7 +85,7 @@ class ConstrainedSmoother : public nav2_core::Smoother * @return Smoothed path */ bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) override; protected: diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 305f4141f3..e945645ab5 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -86,7 +86,7 @@ void ConstrainedSmoother::deactivate() plugin_name_.c_str()); } -bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Duration & max_time) +bool ConstrainedSmoother::smooth(nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) { if (path.poses.size() < 2) { return true; diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index f50d2fb780..60563269d0 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -187,7 +187,7 @@ class SmootherTest : public ::testing::Test const std::vector & input, std::vector & output, bool publish = false, bool cmp = false) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.reserve(input.size()); for (auto & xya : input) { geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index d2e70d67d3..6818095f9a 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -146,7 +146,7 @@ class ControllerServer : public nav2_util::LifecycleNode * @brief Assigns path to controller * @param path Path received from action server */ - void setPlannerPath(const nav_msgs::msg::Path & path); + void setPlannerPath(const nav2_msgs::msg::PathWithCost & path); /** * @brief Calculates velocity and publishes to "cmd_vel" topic */ @@ -262,7 +262,7 @@ class ControllerServer : public nav2_util::LifecycleNode rclcpp::Time last_valid_cmd_time_; // Current path container - nav_msgs::msg::Path current_path_; + nav2_msgs::msg::PathWithCost current_path_; private: /** diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 9d341394a6..9a85b35a6d 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -462,7 +462,7 @@ void ControllerServer::computeControl() action_server_->succeeded_current(); } -void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) +void ControllerServer::setPlannerPath(const nav2_msgs::msg::PathWithCost & path) { RCLCPP_DEBUG( get_logger(), @@ -533,7 +533,7 @@ void ControllerServer::computeAndPublishVelocity() feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y); // Find the closest pose to current pose on global path - nav_msgs::msg::Path & current_path = current_path_; + nav2_msgs::msg::PathWithCost & current_path = current_path_; auto find_closest_pose_idx = [&pose, ¤t_path]() { size_t closest_pose_idx = 0; diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 4e89e0b117..8f48724876 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -46,7 +46,7 @@ #include "pluginlib/class_loader.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_core/goal_checker.hpp" @@ -96,7 +96,7 @@ class Controller * @brief local setPlan - Sets the global plan * @param path The global plan */ - virtual void setPlan(const nav_msgs::msg::Path & path) = 0; + virtual void setPlan(const nav2_msgs::msg::PathWithCost & path) = 0; /** * @brief Controller computeVelocityCommands - calculates the best command given the current pose and velocity diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index dff1207b88..4001cb66fa 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -20,7 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "tf2_ros/buffer.h" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -73,7 +73,7 @@ class GlobalPlanner * @param goal The goal pose of the robot * @return The sequence of poses to get from start to goal, if any */ - virtual nav_msgs::msg::Path createPlan( + virtual nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) = 0; }; diff --git a/nav2_core/include/nav2_core/smoother.hpp b/nav2_core/include/nav2_core/smoother.hpp index e58b4a5ec6..ee0c999639 100644 --- a/nav2_core/include/nav2_core/smoother.hpp +++ b/nav2_core/include/nav2_core/smoother.hpp @@ -25,7 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "pluginlib/class_loader.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" namespace nav2_core @@ -74,7 +74,7 @@ class Smoother * @return If smoothing was completed (true) or interrupted by time limit (false) */ virtual bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) = 0; }; diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index ba2c87d67b..ae20fd289f 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -92,7 +92,7 @@ class DWBLocalPlanner : public nav2_core::Controller * @brief nav2_core setPlan - Sets the global plan * @param path The global plan */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan(const nav2_msgs::msg::PathWithCost & path) override; /** * @brief nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity 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 bd2b210e03..3e02b9e002 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp @@ -42,7 +42,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "dwb_core/trajectory_critic.hpp" #include "dwb_msgs/msg/local_plan_evaluation.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -104,7 +104,7 @@ class DWBPublisher // Helper function for publishing other plans void publishGenericPlan( const nav_2d_msgs::msg::Path2D plan, - rclcpp::Publisher & pub, bool flag); + rclcpp::Publisher & pub, bool flag); // Flags for turning on/off publishing specific components bool publish_evaluation_; @@ -120,9 +120,9 @@ class DWBPublisher // Publisher Objects std::shared_ptr> eval_pub_; - std::shared_ptr> global_pub_; - std::shared_ptr> transformed_pub_; - std::shared_ptr> local_pub_; + std::shared_ptr> global_pub_; + std::shared_ptr> transformed_pub_; + std::shared_ptr> local_pub_; std::shared_ptr> marker_pub_; std::shared_ptr> cost_grid_pc_pub_; 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 da8110a1a5..c2407dab0a 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -50,7 +50,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_core/controller_exceptions.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" using nav2_util::declare_parameter_if_not_declared; @@ -225,7 +225,7 @@ DWBLocalPlanner::loadCritics() } void -DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) +DWBLocalPlanner::setPlan(const nav2_msgs::msg::PathWithCost & path) { auto path2d = nav_2d_utils::pathToPath2D(path); for (TrajectoryCritic::Ptr & critic : critics_) { diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index 7b860fef65..f951fa968b 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -102,9 +102,9 @@ DWBPublisher::on_configure() 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); + 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); @@ -238,7 +238,7 @@ DWBPublisher::publishLocalPlan( if (!publish_local_plan_) {return;} auto path = - std::make_unique( + std::make_unique( nav_2d_utils::poses2DToPath( traj.poses, header.frame_id, header.stamp)); @@ -358,11 +358,11 @@ DWBPublisher::publishLocalPlan(const nav_2d_msgs::msg::Path2D plan) void DWBPublisher::publishGenericPlan( const nav_2d_msgs::msg::Path2D plan, - rclcpp::Publisher & pub, bool flag) + rclcpp::Publisher & pub, bool flag) { if (pub.get_subscription_count() < 1) {return;} if (!flag) {return;} - auto path = std::make_unique(nav_2d_utils::pathToPath(plan)); + auto path = std::make_unique(nav_2d_utils::pathToPath(plan)); pub.publish(std::move(path)); } diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp index 910361615e..fc09f501c8 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp @@ -42,7 +42,7 @@ #include "nav_2d_msgs/msg/twist2_d.hpp" #include "nav_2d_msgs/msg/path2_d.hpp" #include "nav_2d_msgs/msg/pose2_d_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/convert.h" @@ -59,12 +59,12 @@ geometry_msgs::msg::PoseStamped pose2DToPoseStamped( geometry_msgs::msg::PoseStamped pose2DToPoseStamped( const geometry_msgs::msg::Pose2D & pose2d, const std::string & frame, const rclcpp::Time & stamp); -nav_msgs::msg::Path posesToPath(const std::vector & poses); -nav_2d_msgs::msg::Path2D pathToPath2D(const nav_msgs::msg::Path & path); -nav_msgs::msg::Path poses2DToPath( +nav2_msgs::msg::PathWithCost posesToPath(const std::vector & poses); +nav_2d_msgs::msg::Path2D pathToPath2D(const nav2_msgs::msg::PathWithCost & path); +nav2_msgs::msg::PathWithCost poses2DToPath( const std::vector & poses, const std::string & frame, const rclcpp::Time & stamp); -nav_msgs::msg::Path pathToPath(const nav_2d_msgs::msg::Path2D & path2d); +nav2_msgs::msg::PathWithCost pathToPath(const nav_2d_msgs::msg::Path2D & path2d); } // namespace nav_2d_utils diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 077707b394..6ce0dc7f20 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -132,9 +132,9 @@ geometry_msgs::msg::PoseStamped pose2DToPoseStamped( return pose; } -nav_msgs::msg::Path posesToPath(const std::vector & poses) +nav2_msgs::msg::PathWithCost posesToPath(const std::vector & poses) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; if (poses.empty()) { return path; } @@ -147,7 +147,7 @@ nav_msgs::msg::Path posesToPath(const std::vector & poses, const std::string & frame, const rclcpp::Time & stamp) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(poses.size()); path.header.frame_id = frame; path.header.stamp = stamp; @@ -172,9 +172,9 @@ nav_msgs::msg::Path poses2DToPath( return path; } -nav_msgs::msg::Path pathToPath(const nav_2d_msgs::msg::Path2D & path2d) +nav2_msgs::msg::PathWithCost pathToPath(const nav_2d_msgs::msg::Path2D & path2d) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header = path2d.header; path.poses.resize(path2d.poses.size()); for (unsigned int i = 0; i < path.poses.size(); i++) { diff --git a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp index 8f2bd171b2..6c59594ed4 100644 --- a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp +++ b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp @@ -47,7 +47,7 @@ using nav_2d_utils::pathToPath; TEST(nav_2d_utils, PosesToPathEmpty) { std::vector poses; - nav_msgs::msg::Path path = posesToPath(poses); + nav2_msgs::msg::PathWithCost path = posesToPath(poses); EXPECT_EQ(path.poses.size(), 0ul); } @@ -87,7 +87,7 @@ TEST(nav_2d_utils, PosesToPathNonEmpty) poses.push_back(pose1); poses.push_back(pose2); - nav_msgs::msg::Path path = posesToPath(poses); + nav2_msgs::msg::PathWithCost path = posesToPath(poses); EXPECT_EQ(path.poses.size(), 2ul); EXPECT_EQ(path.poses[0].pose.position.x, 1.0); @@ -104,7 +104,7 @@ TEST(nav_2d_utils, PosesToPathNonEmpty) TEST(nav_2d_utils, PathToPathEmpty) { nav_2d_msgs::msg::Path2D path2d; - nav_msgs::msg::Path path = pathToPath(path2d); + nav2_msgs::msg::PathWithCost path = pathToPath(path2d); EXPECT_EQ(path.poses.size(), 0ul); } @@ -125,7 +125,7 @@ TEST(nav_2d_utils, PathToPathNoNEmpty) path2d.poses.push_back(pose1); path2d.poses.push_back(pose2); - nav_msgs::msg::Path path = pathToPath(path2d); + nav2_msgs::msg::PathWithCost path = pathToPath(path2d); EXPECT_EQ(path.poses.size(), 2ul); EXPECT_EQ(path.poses[0].pose.position.x, 1.0); EXPECT_EQ(path.poses[0].pose.position.y, 2.0); diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index 38c7f70a5d..c29ec0caca 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -19,7 +19,7 @@ string planner_id bool use_start # If false, use current robot pose as path start, if true, use start above instead --- #result definition -nav_msgs/Path path +PathWithCost path builtin_interfaces/Duration planning_time uint16 error_code --- diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 5abf2e6826..3c6ec3fcbd 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -18,7 +18,7 @@ string planner_id bool use_start # If false, use current robot pose as path start, if true, use start above instead --- #result definition -nav_msgs/Path path +PathWithCost path builtin_interfaces/Duration planning_time uint16 error_code --- diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index c06b918c54..65c5c116a8 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -10,7 +10,7 @@ uint16 FAILED_TO_MAKE_PROGRESS=105 uint16 NO_VALID_CONTROL=106 #goal definition -nav_msgs/Path path +PathWithCost path string controller_id string goal_checker_id --- diff --git a/nav2_msgs/action/SmoothPath.action b/nav2_msgs/action/SmoothPath.action index d7443fce5a..96c279df1b 100644 --- a/nav2_msgs/action/SmoothPath.action +++ b/nav2_msgs/action/SmoothPath.action @@ -9,13 +9,13 @@ uint16 FAILED_TO_SMOOTH_PATH=504 uint16 INVALID_PATH=505 #goal definition -nav_msgs/Path path +PathWithCost path string smoother_id builtin_interfaces/Duration max_smoothing_duration bool check_for_collisions --- #result definition -nav_msgs/Path path +PathWithCost path builtin_interfaces/Duration smoothing_duration bool was_completed uint16 error_code diff --git a/nav2_msgs/msg/PathWithCost.msg b/nav2_msgs/msg/PathWithCost.msg index 6ef5c69997..6118c8cd9c 100644 --- a/nav2_msgs/msg/PathWithCost.msg +++ b/nav2_msgs/msg/PathWithCost.msg @@ -1,2 +1,3 @@ +std_msgs/Header header geometry_msgs/PoseStamped[] poses float32[] costs diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv index 72b5f20b55..50cf30e560 100644 --- a/nav2_msgs/srv/IsPathValid.srv +++ b/nav2_msgs/srv/IsPathValid.srv @@ -1,7 +1,7 @@ #Determine if the current path is still valid -nav_msgs/Path path +PathWithCost path bool consider_cost_change --- bool is_valid -int32[] invalid_pose_indices +int32[] invalid_pose_indices 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 2330a5165b..588e004a24 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -26,7 +26,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_core/planner_exceptions.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_navfn_planner/navfn.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -83,7 +83,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @param goal Goal pose * @return nav_msgs::Path of the generated path */ - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -99,7 +99,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, - nav_msgs::msg::Path & plan); + nav2_msgs::msg::PathWithCost & plan); /** * @brief Compute the navigation function given a seed point in the world to start from @@ -116,7 +116,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner */ bool getPlanFromPotential( const geometry_msgs::msg::Pose & goal, - nav_msgs::msg::Path & plan); + nav2_msgs::msg::PathWithCost & plan); /** * @brief Remove artifacts at the end of the path - originated from planning on a discretized world @@ -125,7 +125,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner */ void smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, - nav_msgs::msg::Path & plan); + nav2_msgs::msg::PathWithCost & plan); /** * @brief Compute the potential, or navigation cost, at a given point in the world diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 53d7276036..b6c3da69b6 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -127,7 +127,7 @@ NavfnPlanner::cleanup() planner_.reset(); } -nav_msgs::msg::Path NavfnPlanner::createPlan( +nav2_msgs::msg::PathWithCost NavfnPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -166,7 +166,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( costmap_->getSizeInCellsY()); } - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; // Corner case of the start(x,y) = goal(x,y) if (start.pose.position.x == goal.pose.position.x && @@ -220,7 +220,7 @@ bool NavfnPlanner::makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, - nav_msgs::msg::Path & plan) + nav2_msgs::msg::PathWithCost & plan) { // clear the plan, just in case plan.poses.clear(); @@ -354,7 +354,7 @@ NavfnPlanner::makePlan( void NavfnPlanner::smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, - nav_msgs::msg::Path & plan) + nav2_msgs::msg::PathWithCost & plan) { // Replace the last pose of the computed path if it's actually further away // to the second to last pose than the goal pose. @@ -377,7 +377,7 @@ NavfnPlanner::smoothApproachToGoal( bool NavfnPlanner::getPlanFromPotential( const geometry_msgs::msg::Pose & goal, - nav_msgs::msg::Path & plan) + nav2_msgs::msg::PathWithCost & plan) { // clear the plan, just in case plan.poses.clear(); diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 96c0a892e5..ee14a05903 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -24,7 +24,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/compute_path_to_pose.hpp" #include "nav2_msgs/action/compute_path_through_poses.hpp" @@ -69,7 +69,7 @@ class PlannerServer : public nav2_util::LifecycleNode * @param goal goal request * @return Path */ - nav_msgs::msg::Path getPlan( + nav2_msgs::msg::PathWithCost getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, const std::string & planner_id); @@ -180,7 +180,7 @@ class PlannerServer : public nav2_util::LifecycleNode template bool validatePath( const geometry_msgs::msg::PoseStamped & curr_goal, - const nav_msgs::msg::Path & path, + const nav2_msgs::msg::PathWithCost & path, const std::string & planner_id); /** @@ -208,7 +208,7 @@ class PlannerServer : public nav2_util::LifecycleNode * @brief Publish a path for visualization purposes * @param path Reference to Global Path */ - void publishPlan(const nav_msgs::msg::Path & path); + void publishPlan(const nav2_msgs::msg::PathWithCost & path); void exceptionWarning( const geometry_msgs::msg::PoseStamped & start, @@ -254,7 +254,7 @@ class PlannerServer : public nav2_util::LifecycleNode nav2_costmap_2d::Costmap2D * costmap_; // Publishers for the path - rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; // Service to determine if the path is valid rclcpp::Service::SharedPtr is_path_valid_service_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index c2b46c0f2d..0c9f893116 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -138,7 +138,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) } // Initialize pubs & subs - plan_publisher_ = create_publisher("plan", 1); + plan_publisher_ = create_publisher("plan", 1); // Create the action servers for path planning to a pose and through poses action_server_pose_ = std::make_unique( @@ -317,7 +317,7 @@ bool PlannerServer::transformPosesToGlobalFrame( template bool PlannerServer::validatePath( const geometry_msgs::msg::PoseStamped & goal, - const nav_msgs::msg::Path & path, + const nav2_msgs::msg::PathWithCost & path, const std::string & planner_id) { if (path.poses.empty()) { @@ -346,7 +346,7 @@ void PlannerServer::computePlanThroughPoses() // Initialize the ComputePathThroughPoses goal and result auto goal = action_server_poses_->get_current_goal(); auto result = std::make_shared(); - nav_msgs::msg::Path concat_path; + nav2_msgs::msg::PathWithCost concat_path; geometry_msgs::msg::PoseStamped curr_start, curr_goal; @@ -385,7 +385,7 @@ void PlannerServer::computePlanThroughPoses() } // Get plan from start -> goal - nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + nav2_msgs::msg::PathWithCost curr_path = getPlan(curr_start, curr_goal, goal->planner_id); if (!validatePath(curr_goal, curr_path, goal->planner_id)) { throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); @@ -546,7 +546,7 @@ PlannerServer::computePlan() } } -nav_msgs::msg::Path +nav2_msgs::msg::PathWithCost PlannerServer::getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, @@ -575,13 +575,13 @@ PlannerServer::getPlan( } } - return nav_msgs::msg::Path(); + return nav2_msgs::msg::PathWithCost(); } void -PlannerServer::publishPlan(const nav_msgs::msg::Path & path) +PlannerServer::publishPlan(const nav2_msgs::msg::PathWithCost & path) { - auto msg = std::make_unique(path); + auto msg = std::make_unique(path); if (plan_publisher_->is_activated() && plan_publisher_->get_subscription_count() > 0) { plan_publisher_->publish(std::move(msg)); } @@ -673,7 +673,7 @@ void PlannerServer::isPathValid( // come from the same distribution (e.g. there is not a statistically significant change) // Thus, the difference in population mean is 0 and the sample sizes are the same float z = (mean_current - mean_orginal) / std::sqrt((var_current + var_orginal) / current_costs.size()); - + // TODO try single tail or tune strictness? Parameterize? // 1.65 95% single tail; 2.55 for 99% dual, 2.33 99% single; 90% 1.65 dual, 90% 1.2 single. if (z > z_score_) { // critical z score for 95% level diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 6f84b05382..96d88564f3 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -28,6 +29,7 @@ set(dependencies nav_msgs nav2_util nav2_core + nav2_msgs tf2 tf2_geometry_msgs ) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp index baccc6ca20..7508cab929 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp @@ -96,7 +96,7 @@ class CollisionChecker std::unique_ptr> footprint_collision_checker_; Parameters * params_; - std::shared_ptr> carrot_arc_pub_; + std::shared_ptr> carrot_arc_pub_; rclcpp::Clock::SharedPtr clock_; }; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp index 22bc0266a6..00338b4380 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -60,7 +60,7 @@ class PathHandler * @param max_robot_pose_search_dist Distance to search for matching nearest path point * @return Path in new frame */ - nav_msgs::msg::Path transformGlobalPlan( + nav2_msgs::msg::PathWithCost transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist); @@ -76,9 +76,9 @@ class PathHandler const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose) const; - void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;} + void setPlan(const nav2_msgs::msg::PathWithCost & path) {global_plan_ = path;} - nav_msgs::msg::Path getPlan() {return global_plan_;} + nav2_msgs::msg::PathWithCost getPlan() {return global_plan_;} protected: /** @@ -91,7 +91,7 @@ class PathHandler tf2::Duration transform_tolerance_; std::shared_ptr tf_; std::shared_ptr costmap_ros_; - nav_msgs::msg::Path global_plan_; + nav2_msgs::msg::PathWithCost global_plan_; }; } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7b28ca9720..78d783fa43 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -100,7 +100,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @brief nav2_core setPlan - Sets the global plan * @param path The global plan */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan(const nav2_msgs::msg::PathWithCost & path) override; /** * @brief Limits the maximum linear speed of the robot. @@ -164,7 +164,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ void applyConstraints( const double & curvature, const geometry_msgs::msg::Twist & speed, - const double & pose_cost, const nav_msgs::msg::Path & path, + const double & pose_cost, const nav2_msgs::msg::PathWithCost & path, double & linear_vel, double & sign); /** @@ -187,14 +187,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param path Current global path * @return Lookahead point */ - geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav2_msgs::msg::PathWithCost &); /** * @brief checks for the cusp position * @param pose Pose input to determine the cusp position * @return robot distance from the cusp */ - double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); + double findVelocitySignChange(const nav2_msgs::msg::PathWithCost & transformed_plan); rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; @@ -207,10 +207,10 @@ class RegulatedPurePursuitController : public nav2_core::Controller double goal_dist_tol_; double control_duration_; - std::shared_ptr> global_path_pub_; + std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; - std::shared_ptr> carrot_arc_pub_; + std::shared_ptr> carrot_arc_pub_; std::unique_ptr path_handler_; std::unique_ptr param_handler_; std::unique_ptr collision_checker_; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp index 61dca3487e..9c39151b18 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp @@ -89,7 +89,7 @@ inline double costConstraint( * @return A scale from 0.0-1.0 of the distance to goal scaled by minimum distance */ inline double approachVelocityScalingFactor( - const nav_msgs::msg::Path & transformed_path, + const nav2_msgs::msg::PathWithCost & transformed_path, const double approach_velocity_scaling_dist) { using namespace nav2_util::geometry_utils; // NOLINT @@ -117,7 +117,7 @@ inline double approachVelocityScalingFactor( */ inline double approachVelocityConstraint( const double constrained_linear_vel, - const nav_msgs::msg::Path & path, + const nav2_msgs::msg::PathWithCost & path, const double min_approach_velocity, const double approach_velocity_scaling_dist) { diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index cffd2c1f49..638ce16173 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -16,7 +16,7 @@ nav2_costmap_2d rclcpp geometry_msgs - nav2_msgs + pluginlib tf2 tf2_geometry_msgs diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp index ac654fcab7..f949f2e7b3 100644 --- a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -41,7 +41,7 @@ CollisionChecker::CollisionChecker( FootprintCollisionChecker>(costmap_); footprint_collision_checker_->setCostmap(costmap_); - carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); carrot_arc_pub_->on_activate(); } @@ -62,7 +62,7 @@ bool CollisionChecker::isCollisionImminent( } // visualization messages - nav_msgs::msg::Path arc_pts_msg; + nav2_msgs::msg::PathWithCost arc_pts_msg; arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); arc_pts_msg.header.stamp = robot_pose.header.stamp; geometry_msgs::msg::PoseStamped pose_msg; diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index f4ceec759d..46b5e79d19 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -45,7 +45,7 @@ double PathHandler::getCostmapMaxExtent() const return max_costmap_dim_meters / 2.0; } -nav_msgs::msg::Path PathHandler::transformGlobalPlan( +nav2_msgs::msg::PathWithCost PathHandler::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist) { @@ -95,7 +95,7 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( }; // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; + nav2_msgs::msg::PathWithCost transformed_plan; std::transform( transformation_begin, transformation_end, std::back_inserter(transformed_plan.poses), diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 674a456c57..5f748a1863 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -71,7 +71,7 @@ void RegulatedPurePursuitController::configure( node->get_parameter("controller_frequency", control_frequency); control_duration_ = 1.0 / control_frequency; - global_path_pub_ = node->create_publisher("received_global_plan", 1); + global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); } @@ -297,7 +297,7 @@ geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersect geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( const double & lookahead_dist, - const nav_msgs::msg::Path & transformed_plan) + const nav2_msgs::msg::PathWithCost & transformed_plan) { // Find the first pose which is at a distance greater than the lookahead distance auto goal_pose_it = std::find_if( @@ -330,7 +330,7 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin void RegulatedPurePursuitController::applyConstraints( const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, - const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) + const double & pose_cost, const nav2_msgs::msg::PathWithCost & path, double & linear_vel, double & sign) { double curvature_vel = linear_vel, cost_vel = linear_vel; @@ -359,7 +359,7 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = sign * linear_vel; } -void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +void RegulatedPurePursuitController::setPlan(const nav2_msgs::msg::PathWithCost & path) { path_handler_->setPlan(path); } @@ -385,7 +385,7 @@ void RegulatedPurePursuitController::setSpeedLimit( } double RegulatedPurePursuitController::findVelocitySignChange( - const nav_msgs::msg::Path & transformed_plan) + const nav2_msgs::msg::PathWithCost & transformed_plan) { // Iterating through the transformed global path to determine the position of the cusp for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) { diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt index 3e8d4c5b0a..f69ba2266c 100644 --- a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -12,4 +12,4 @@ target_link_libraries(test_regulated_pp # Path utils test ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp) -ament_target_dependencies(test_path_utils nav_msgs geometry_msgs tf2_geometry_msgs) +ament_target_dependencies(test_path_utils nav2_msgs geometry_msgs tf2_geometry_msgs) diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp index 4b516ec1f3..d5740d33b8 100644 --- a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp @@ -22,7 +22,7 @@ namespace path_utils { void append_transform_to_path( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, tf2::Transform & relative_transform) { // Add a new empty pose @@ -42,7 +42,7 @@ void append_transform_to_path( new_pose.header.frame_id = previous_pose.header.frame_id; } -void Straight::append(nav_msgs::msg::Path & path, double spacing) const +void Straight::append(nav2_msgs::msg::PathWithCost & path, double spacing) const { auto num_points = std::floor(length_ / spacing); path.poses.reserve(path.poses.size() + num_points); @@ -57,7 +57,7 @@ double chord_length(double radius, double radians) return 2 * radius * sin(radians / 2); } -void Arc::append(nav_msgs::msg::Path & path, double spacing) const +void Arc::append(nav2_msgs::msg::PathWithCost & path, double spacing) const { double length = radius_ * std::abs(radians_); size_t num_points = std::floor(length / spacing); @@ -71,12 +71,12 @@ void Arc::append(nav_msgs::msg::Path & path, double spacing) const } } -nav_msgs::msg::Path generate_path( +nav2_msgs::msg::PathWithCost generate_path( geometry_msgs::msg::PoseStamped start, double spacing, std::initializer_list> segments) { - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header = start.header; path.poses.push_back(start); for (const auto & segment : segments) { diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp index c079e21614..57d4c24722 100644 --- a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp @@ -21,7 +21,7 @@ #include #include -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" namespace path_utils { @@ -32,7 +32,7 @@ namespace path_utils class PathSegment { public: - virtual void append(nav_msgs::msg::Path & path, double spacing) const = 0; + virtual void append(nav2_msgs::msg::PathWithCost & path, double spacing) const = 0; virtual ~PathSegment() {} }; @@ -41,7 +41,7 @@ class Arc : public PathSegment public: explicit Arc(double radius, double radians) : radius_(radius), radians_(radians) {} - void append(nav_msgs::msg::Path & path, double spacing) const override; + void append(nav2_msgs::msg::PathWithCost & path, double spacing) const override; private: double radius_; @@ -53,7 +53,7 @@ class Straight : public PathSegment public: explicit Straight(double length) : length_(length) {} - void append(nav_msgs::msg::Path & path, double spacing) const override; + void append(nav2_msgs::msg::PathWithCost & path, double spacing) const override; private: double length_; @@ -101,7 +101,7 @@ class RightCircle : public Arc : Arc(radius, -2.0 * M_PI) {} }; -nav_msgs::msg::Path generate_path( +nav2_msgs::msg::PathWithCost generate_path( geometry_msgs::msg::PoseStamped start, double spacing, std::initializer_list> segments); diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 09892755d9..945ca2af06 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -41,7 +41,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {} - nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} + nav2_msgs::msg::PathWithCost getPlan() {return path_handler_->getPlan();} double getSpeed() {return params_->desired_linear_vel;} @@ -69,7 +69,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure } geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( - const double & dist, const nav_msgs::msg::Path & path) + const double & dist, const nav2_msgs::msg::PathWithCost & path) { return getLookAheadPoint(dist, path); } @@ -94,7 +94,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure void applyConstraintsWrapper( const double & curvature, const geometry_msgs::msg::Twist & curr_speed, - const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) + const double & pose_cost, const nav2_msgs::msg::PathWithCost & path, double & linear_vel, double & sign) { return applyConstraints( curvature, curr_speed, pose_cost, path, @@ -102,12 +102,12 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure } double findVelocitySignChangeWrapper( - const nav_msgs::msg::Path & transformed_plan) + const nav2_msgs::msg::PathWithCost & transformed_plan) { return findVelocitySignChange(transformed_plan); } - nav_msgs::msg::Path transformGlobalPlanWrapper( + nav2_msgs::msg::PathWithCost transformGlobalPlanWrapper( const geometry_msgs::msg::PoseStamped & pose) { return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); @@ -130,7 +130,7 @@ TEST(RegulatedPurePursuitTest, basicAPI) ctrl->cleanup(); // setPlan and get plan - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(2); path.poses[0].header.frame_id = "fake_frame"; ctrl->setPlan(path); @@ -185,7 +185,7 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) pose.pose.position.x = 1.0; pose.pose.position.y = 0.0; - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(3); path.header.frame_id = "smb"; path.header.stamp = pose.header.stamp; @@ -369,7 +369,7 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) // test getLookAheadPoint double dist = 1.0; - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.poses.resize(10); for (uint i = 0; i != path.poses.size(); i++) { path.poses[i].pose.position.x = static_cast(i); diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index c3812ec1d3..98b77bff19 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -95,7 +95,7 @@ class RotationShimController : public nav2_core::Controller * @brief nav2_core setPlan - Sets the global plan * @param path The global plan */ - void setPlan(const nav_msgs::msg::Path & path) override; + void setPlan(const nav2_msgs::msg::PathWithCost & path) override; /** * @brief Limits the maximum linear speed of the robot. @@ -164,7 +164,7 @@ class RotationShimController : public nav2_core::Controller pluginlib::ClassLoader lp_loader_; nav2_core::Controller::Ptr primary_controller_; bool path_updated_; - nav_msgs::msg::Path current_path_; + nav2_msgs::msg::PathWithCost current_path_; double forward_sampling_distance_, angular_dist_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 7d32283db7..792910aec2 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -268,7 +268,7 @@ void RotationShimController::isCollisionFree( } } -void RotationShimController::setPlan(const nav_msgs::msg::Path & path) +void RotationShimController::setPlan(const nav2_msgs::msg::PathWithCost & path) { path_updated_ = true; current_path_ = path; diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index e1bc30f80e..e2a6020387 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -47,7 +47,7 @@ class RotationShimShim : public nav2_rotation_shim_controller::RotationShimContr return primary_controller_; } - nav_msgs::msg::Path getPath() + nav2_msgs::msg::PathWithCost getPath() { return current_path_; } @@ -135,7 +135,7 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) controller->activate(); // Test state update and path setting - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.frame_id = "hi mate!"; path.poses.resize(10); path.poses[1].pose.position.x = 0.1; @@ -155,11 +155,11 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) EXPECT_EQ(pose.pose.position.x, 1.0); // default forward sampling is 0.5 EXPECT_EQ(pose.pose.position.y, 1.0); - nav_msgs::msg::Path path_invalid_leng; + nav2_msgs::msg::PathWithCost path_invalid_leng; controller->setPlan(path_invalid_leng); EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error); - nav_msgs::msg::Path path_invalid_dists; + nav2_msgs::msg::PathWithCost path_invalid_dists; path.poses.resize(10); controller->setPlan(path_invalid_dists); EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error); @@ -185,7 +185,7 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) controller->activate(); // Test state update and path setting - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.frame_id = "fake_frame"; path.poses.resize(10); path.poses[1].pose.position.x = 0.1; @@ -255,7 +255,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) controller->activate(); // Test state update and path setting - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; path.header.frame_id = "fake_frame"; path.poses.resize(10); diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index c499d0796f..d57db52e68 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -26,7 +26,7 @@ #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -84,7 +84,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner * @param goal Goal pose * @return nav2_msgs::Path of the generated path */ - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -107,7 +107,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner float _tolerance; int _downsampling_factor; bool _downsample_costmap; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; bool _allow_unknown; int _max_iterations; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index c782fe41e0..f4c57d010a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -25,7 +25,7 @@ #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -82,7 +82,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner * @param goal Goal pose * @return nav2_msgs::Path of the generated path */ - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -118,7 +118,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner double _minimum_turning_radius_global_coords; std::string _motion_model_for_search; MotionModel _motion_model; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index f5207cd45c..e39904995e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -24,7 +24,7 @@ #include "nav2_smac_planner/utils.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -81,7 +81,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner * @param goal Goal pose * @return nav2_msgs::Path of the generated path */ - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -108,7 +108,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner int _max_iterations; int _max_on_approach_iterations; float _tolerance; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; double _lookup_table_size; std::mutex _mutex; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index d896cd90b5..530e1485af 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -26,7 +26,7 @@ #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "angles/angles.h" #include "tf2/utils.h" #include "ompl/base/StateSpace.h" @@ -113,7 +113,7 @@ class Smoother * @return If smoothing was successful */ bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time); @@ -127,7 +127,7 @@ class Smoother * @return If smoothing was successful */ bool smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time); @@ -158,7 +158,7 @@ class Smoother * @param path Path in which to look for cusps * @return Set of index pairs for each segment of the path in a given direction */ - std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); + std::vector findDirectionalPathSegments(const nav2_msgs::msg::PathWithCost & path); /** * @brief Enforced minimum curvature boundary conditions on plan output @@ -170,7 +170,7 @@ class Smoother */ void enforceStartBoundaryConditions( const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const bool & reversing_segment); @@ -184,7 +184,7 @@ class Smoother */ void enforceEndBoundaryConditions( const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const bool & reversing_segment); @@ -228,7 +228,7 @@ class Smoother * @param reversing_segment Return if this is a reversing segment */ inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment); double min_turning_rad_, tolerance_, data_w_, smooth_w_; diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 46dfa98fb2..af58f7df64 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -139,7 +139,7 @@ void SmacPlanner2D::configure( node, _global_frame, topic_name, _costmap, _downsampling_factor); } - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlanner2D with " @@ -190,7 +190,7 @@ void SmacPlanner2D::cleanup() _raw_plan_publisher.reset(); } -nav_msgs::msg::Path SmacPlanner2D::createPlan( +nav2_msgs::msg::PathWithCost SmacPlanner2D::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -227,7 +227,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _a_star->setGoal(mx_goal, my_goal, 0); // Setup message - nav_msgs::msg::Path plan; + nav2_msgs::msg::PathWithCost plan; plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 2d0351c5b9..13c99405dc 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -214,7 +214,7 @@ void SmacPlannerHybrid::configure( node, _global_frame, topic_name, _costmap, _downsampling_factor); } - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerHybrid with " @@ -266,7 +266,7 @@ void SmacPlannerHybrid::cleanup() _raw_plan_publisher.reset(); } -nav_msgs::msg::Path SmacPlannerHybrid::createPlan( +nav2_msgs::msg::PathWithCost SmacPlannerHybrid::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -322,7 +322,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setGoal(mx, my, orientation_bin_id); // Setup message - nav_msgs::msg::Path plan; + nav2_msgs::msg::PathWithCost plan; plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 6d00a3479f..424930430f 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -57,7 +57,7 @@ void SmacPlannerLattice::configure( _costmap_ros = costmap_ros; _name = name; _global_frame = costmap_ros->getGlobalFrameID(); - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerLattice", name.c_str()); @@ -233,7 +233,7 @@ void SmacPlannerLattice::cleanup() _raw_plan_publisher.reset(); } -nav_msgs::msg::Path SmacPlannerLattice::createPlan( +nav2_msgs::msg::PathWithCost SmacPlannerLattice::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -267,7 +267,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); // Setup message - nav_msgs::msg::Path plan; + nav2_msgs::msg::PathWithCost plan; plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index a69e14011f..c52e0d087d 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -41,7 +41,7 @@ void Smoother::initialize(const double & min_turning_radius) } bool Smoother::smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time) { @@ -53,7 +53,7 @@ bool Smoother::smooth( steady_clock::time_point start = steady_clock::now(); double time_remaining = max_time; bool success = true, reversing_segment; - nav_msgs::msg::Path curr_path_segment; + nav2_msgs::msg::PathWithCost curr_path_segment; curr_path_segment.header = path.header; std::vector path_segments = findDirectionalPathSegments(path); @@ -96,7 +96,7 @@ bool Smoother::smooth( } bool Smoother::smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time) @@ -110,8 +110,8 @@ bool Smoother::smoothImpl( double x_i, y_i, y_m1, y_ip1, y_i_org; unsigned int mx, my; - nav_msgs::msg::Path new_path = path; - nav_msgs::msg::Path last_path = path; + nav2_msgs::msg::PathWithCost new_path = path; + nav2_msgs::msg::PathWithCost last_path = path; while (change >= tolerance_) { its += 1; @@ -214,7 +214,7 @@ void Smoother::setFieldByDim( } } -std::vector Smoother::findDirectionalPathSegments(const nav_msgs::msg::Path & path) +std::vector Smoother::findDirectionalPathSegments(const nav2_msgs::msg::PathWithCost & path) { std::vector segments; PathSegment curr_segment; @@ -265,7 +265,7 @@ std::vector Smoother::findDirectionalPathSegments(const nav_msgs::m } void Smoother::updateApproximatePathOrientations( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment) { double dx, dy, theta, pt_yaw; @@ -421,7 +421,7 @@ BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, It void Smoother::enforceStartBoundaryConditions( const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const bool & reversing_segment) { @@ -467,7 +467,7 @@ void Smoother::enforceStartBoundaryConditions( void Smoother::enforceEndBoundaryConditions( const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const nav2_costmap_2d::Costmap2D * costmap, const bool & reversing_segment) { diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index d7d27f1d20..e8f4dc7290 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -46,7 +46,7 @@ class SmootherWrapper : public nav2_smac_planner::Smoother : nav2_smac_planner::Smoother(params) {} - std::vector findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path) + std::vector findDirectionalPathSegmentsWrapper(nav2_msgs::msg::PathWithCost path) { return findDirectionalPathSegments(path); } @@ -106,7 +106,7 @@ TEST(SmootherTest, test_full_smoother) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); // Convert to world coordinates and get length to compare to smoothed length - nav_msgs::msg::Path plan; + nav2_msgs::msg::PathWithCost plan; plan.header.stamp = node->now(); plan.header.frame_id = "map"; geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index b52ebf6521..22abbd8402 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -141,7 +141,7 @@ class SmootherServer : public nav2_util::LifecycleNode * @param path current path * return bool if the path is valid */ - bool validate(const nav_msgs::msg::Path & path); + bool validate(const nav2_msgs::msg::PathWithCost & path); // Our action server implements the SmoothPath action std::unique_ptr action_server_; @@ -151,7 +151,7 @@ class SmootherServer : public nav2_util::LifecycleNode std::shared_ptr transform_listener_; // Publishers and subscribers - rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; // Smoother Plugins pluginlib::ClassLoader lp_loader_; diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp index 090aa07af8..80eaee529f 100644 --- a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -29,7 +29,7 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "angles/angles.h" #include "tf2/utils.h" @@ -82,7 +82,7 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother * @return If smoothing was completed (true) or interrupted by time limit (false) */ bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) override; protected: @@ -95,7 +95,7 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother * @return If smoothing was successful */ bool smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment); bool do_refinement_; diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 2e8fea4a94..bd5bbaa97e 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -29,7 +29,7 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "angles/angles.h" #include "tf2/utils.h" @@ -82,7 +82,7 @@ class SimpleSmoother : public nav2_core::Smoother * @return If smoothing was completed (true) or interrupted by time limit (false) */ bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) override; protected: @@ -95,7 +95,7 @@ class SimpleSmoother : public nav2_core::Smoother * @return If smoothing was successful */ bool smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time); diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp index 67d7296353..3772c37418 100644 --- a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp +++ b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp @@ -28,7 +28,7 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "angles/angles.h" #include "tf2/utils.h" @@ -49,7 +49,7 @@ typedef std::vector::iterator PathIterator; typedef std::vector::reverse_iterator ReversePathIterator; inline std::vector findDirectionalPathSegments( - const nav_msgs::msg::Path & path) + const nav2_msgs::msg::PathWithCost & path) { std::vector segments; PathSegment curr_segment; @@ -92,7 +92,7 @@ inline std::vector findDirectionalPathSegments( } inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment) { double dx, dy, theta, pt_yaw; diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 4e3588436a..31fad2a45f 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -102,7 +102,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &) } // Initialize pubs & subs - plan_publisher_ = create_publisher("plan_smoothed", 1); + plan_publisher_ = create_publisher("plan_smoothed", 1); // Create the action server that we implement with our smoothPath method action_server_ = std::make_unique( @@ -352,7 +352,7 @@ void SmootherServer::smoothPlan() } } -bool SmootherServer::validate(const nav_msgs::msg::Path & path) +bool SmootherServer::validate(const nav2_msgs::msg::PathWithCost & path) { if (path.poses.empty()) { RCLCPP_WARN(get_logger(), "Requested path to smooth is empty"); diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 17fe63a926..b812fb99b5 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -43,14 +43,14 @@ void SavitzkyGolaySmoother::configure( } bool SavitzkyGolaySmoother::smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) { steady_clock::time_point start = steady_clock::now(); double time_remaining = max_time.seconds(); bool success = true, reversing_segment; - nav_msgs::msg::Path curr_path_segment; + nav2_msgs::msg::PathWithCost curr_path_segment; curr_path_segment.header = path.header; std::vector path_segments = findDirectionalPathSegments(path); @@ -90,7 +90,7 @@ bool SavitzkyGolaySmoother::smooth( } bool SavitzkyGolaySmoother::smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment) { // Must be at least 10 in length to enter function diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 29f3b89a82..2cec692a93 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -57,7 +57,7 @@ void SimpleSmoother::configure( } bool SimpleSmoother::smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) { auto costmap = costmap_sub_->getCostmap(); @@ -67,7 +67,7 @@ bool SimpleSmoother::smooth( bool success = true, reversing_segment; unsigned int segments_smoothed = 0; - nav_msgs::msg::Path curr_path_segment; + nav2_msgs::msg::PathWithCost curr_path_segment; curr_path_segment.header = path.header; std::vector path_segments = findDirectionalPathSegments(path); @@ -112,7 +112,7 @@ bool SimpleSmoother::smooth( } bool SimpleSmoother::smoothImpl( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, const double & max_time) @@ -126,8 +126,8 @@ bool SimpleSmoother::smoothImpl( double x_i, y_i, y_m1, y_ip1, y_i_org; unsigned int mx, my; - nav_msgs::msg::Path new_path = path; - nav_msgs::msg::Path last_path = path; + nav2_msgs::msg::PathWithCost new_path = path; + nav2_msgs::msg::PathWithCost last_path = path; while (change >= tolerance_) { its += 1; diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp index 89df4c3a4f..624a29ef19 100644 --- a/nav2_smoother/test/test_savitzky_golay_smoother.cpp +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -71,7 +71,7 @@ TEST(SmootherTest, test_sg_smoother_basics) rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds // Test regular path, should see no effective change - nav_msgs::msg::Path straight_regular_path, straight_regular_path_baseline; + nav2_msgs::msg::PathWithCost straight_regular_path, straight_regular_path_baseline; straight_regular_path.header.frame_id = "map"; straight_regular_path.header.stamp = node->now(); straight_regular_path.poses.resize(11); @@ -144,7 +144,7 @@ TEST(SmootherTest, test_sg_smoother_noisey_path) rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds // Given nominal irregular/noisey path, test that the output is shorter and smoother - nav_msgs::msg::Path noisey_path, noisey_path_baseline; + nav2_msgs::msg::PathWithCost noisey_path, noisey_path_baseline; noisey_path.header.frame_id = "map"; noisey_path.header.stamp = node->now(); noisey_path.poses.resize(11); @@ -202,7 +202,7 @@ TEST(SmootherTest, test_sg_smoother_noisey_path) // Test again with refinement, even shorter and smoother node->set_parameter(rclcpp::Parameter("test.do_refinement", rclcpp::ParameterValue(true))); smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); - nav_msgs::msg::Path noisey_path_refined = noisey_path_baseline; + nav2_msgs::msg::PathWithCost noisey_path_refined = noisey_path_baseline; EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time)); length = 0; @@ -249,7 +249,7 @@ TEST(SmootherTest, test_sg_smoother_reversing) rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds // Test reversing / multiple segments via a cusp - nav_msgs::msg::Path cusp_path, cusp_path_baseline; + nav2_msgs::msg::PathWithCost cusp_path, cusp_path_baseline; cusp_path.header.frame_id = "map"; cusp_path.header.stamp = node->now(); cusp_path.poses.resize(22); diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index c8f7da8e87..9639451a51 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -44,7 +44,7 @@ class SmootherWrapper : public nav2_smoother::SimpleSmoother { } - std::vector findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path) + std::vector findDirectionalPathSegmentsWrapper(nav2_msgs::msg::PathWithCost path) { return findDirectionalPathSegments(path); } @@ -88,7 +88,7 @@ TEST(SmootherTest, test_simple_smoother) smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); // Test that an irregular distributed path becomes more distributed - nav_msgs::msg::Path straight_irregular_path; + nav2_msgs::msg::PathWithCost straight_irregular_path; straight_irregular_path.header.frame_id = "map"; straight_irregular_path.header.stamp = node->now(); straight_irregular_path.poses.resize(11); @@ -128,7 +128,7 @@ TEST(SmootherTest, test_simple_smoother) } // Test regular path, should see no effective change - nav_msgs::msg::Path straight_regular_path; + nav2_msgs::msg::PathWithCost straight_regular_path; straight_regular_path.header = straight_irregular_path.header; straight_regular_path.poses.resize(11); straight_regular_path.poses[0].pose.position.x = 0.5; @@ -164,7 +164,7 @@ TEST(SmootherTest, test_simple_smoother) } // test shorter and curved if given a right angle - nav_msgs::msg::Path right_angle_path; + nav2_msgs::msg::PathWithCost right_angle_path; right_angle_path = straight_regular_path; straight_regular_path.poses[6].pose.position.x = 0.6; straight_regular_path.poses[6].pose.position.y = 0.5; @@ -181,7 +181,7 @@ TEST(SmootherTest, test_simple_smoother) EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.387, 0.01); // Test that collisions are rejected - nav_msgs::msg::Path collision_path; + nav2_msgs::msg::PathWithCost collision_path; collision_path.poses.resize(11); collision_path.poses[0].pose.position.x = 0.0; collision_path.poses[0].pose.position.y = 0.0; @@ -208,7 +208,7 @@ TEST(SmootherTest, test_simple_smoother) EXPECT_THROW(smoother->smooth(collision_path, max_time), nav2_core::FailedToSmoothPath); // test cusp / reversing segments - nav_msgs::msg::Path reversing_path; + nav2_msgs::msg::PathWithCost reversing_path; reversing_path.poses.resize(11); reversing_path.poses[0].pose.position.x = 0.5; reversing_path.poses[0].pose.position.y = 0.0; @@ -248,7 +248,7 @@ TEST(SmootherTest, test_simple_smoother) // test max iterations smoother->setMaxItsToInvalid(); - nav_msgs::msg::Path max_its_path; + nav2_msgs::msg::PathWithCost max_its_path; max_its_path.poses.resize(11); max_its_path.poses[0].pose.position.x = 0.5; max_its_path.poses[0].pose.position.y = 0.0; diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index d7afe9b55d..ffa1035dfe 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -56,7 +56,7 @@ class DummySmoother : public nav2_core::Smoother virtual void deactivate() {} virtual bool smooth( - nav_msgs::msg::Path & path, + nav2_msgs::msg::PathWithCost & path, const rclcpp::Duration & max_time) { assert(path.poses.size() == 2); diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp index ba194b195d..3a2c3862ad 100644 --- a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -41,7 +41,7 @@ class UnknownErrorController : public nav2_core::Controller void deactivate() {} - void setPlan(const nav_msgs::msg::Path &) {} + void setPlan(const nav2_msgs::msg::PathWithCost &) {} virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped &, diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp index 2b6ae7971d..77b72d22e5 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -49,7 +49,7 @@ class UnknownErrorPlanner : public nav2_core::GlobalPlanner void deactivate() override {} - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -59,7 +59,7 @@ class UnknownErrorPlanner : public nav2_core::GlobalPlanner class StartOccupiedErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -69,7 +69,7 @@ class StartOccupiedErrorPlanner : public UnknownErrorPlanner class GoalOccupiedErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -79,7 +79,7 @@ class GoalOccupiedErrorPlanner : public UnknownErrorPlanner class StartOutsideMapErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -89,7 +89,7 @@ class StartOutsideMapErrorPlanner : public UnknownErrorPlanner class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -99,18 +99,18 @@ class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner class NoValidPathErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { - return nav_msgs::msg::Path(); + return nav2_msgs::msg::PathWithCost(); } }; class TimedOutErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -120,7 +120,7 @@ class TimedOutErrorPlanner : public UnknownErrorPlanner class TFErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { @@ -130,7 +130,7 @@ class TFErrorPlanner : public UnknownErrorPlanner class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner { - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { diff --git a/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp index 45ad7f469e..5bb5b740a3 100644 --- a/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp @@ -40,7 +40,7 @@ class UnknownErrorSmoother : public nav2_core::Smoother void deactivate() override {} bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) override { throw nav2_core::SmootherException("Unknown Smoother exception"); @@ -50,7 +50,7 @@ class UnknownErrorSmoother : public nav2_core::Smoother class TimeOutErrorSmoother : public UnknownErrorSmoother { bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) { throw nav2_core::SmootherTimedOut("Smoother timedOut"); @@ -60,7 +60,7 @@ class TimeOutErrorSmoother : public UnknownErrorSmoother class SmoothedPathInCollision : public UnknownErrorSmoother { bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) { throw nav2_core::SmoothedPathInCollision("Smoother path in collision"); @@ -70,7 +70,7 @@ class SmoothedPathInCollision : public UnknownErrorSmoother class FailedToSmoothPath : public UnknownErrorSmoother { bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) { throw nav2_core::FailedToSmoothPath("Failed to smooth path"); @@ -80,7 +80,7 @@ class FailedToSmoothPath : public UnknownErrorSmoother class InvalidPath : public UnknownErrorSmoother { bool smooth( - nav_msgs::msg::Path &, + nav2_msgs::msg::PathWithCost &, const rclcpp::Duration &) { throw nav2_core::InvalidPath("Invalid path"); diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 0a2368bab6..298bd9b60b 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -397,7 +397,7 @@ TaskStatus PlannerTester::createPlan( return TaskStatus::FAILED; } -bool PlannerTester::isPathValid(nav_msgs::msg::Path & path) +bool PlannerTester::isPathValid(nav2_msgs::msg::PathWithCost & path) { planner_tester_->setCostmap(costmap_.get()); // create a fake service request diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index b849e89908..cb46104557 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -77,7 +77,7 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer bool createPath( const geometry_msgs::msg::PoseStamped & goal, - nav_msgs::msg::Path & path) + nav2_msgs::msg::PathWithCost & path) { geometry_msgs::msg::PoseStamped start; if (!nav2_util::getCurrentPose(start, *tf_, "map", "base_link", 0.1)) { @@ -129,7 +129,7 @@ class PlannerTester : public rclcpp::Node { public: using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; - using ComputePathToPoseResult = nav_msgs::msg::Path; + using ComputePathToPoseResult = nav2_msgs::msg::PathWithCost; PlannerTester(); ~PlannerTester(); @@ -157,7 +157,7 @@ class PlannerTester : public rclcpp::Node const unsigned int number_tests, const float acceptable_fail_ratio); - bool isPathValid(nav_msgs::msg::Path & path); + bool isPathValid(nav2_msgs::msg::PathWithCost & path); private: void setCostmap(); diff --git a/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp b/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp index 27b953efcf..a68d7b10a9 100644 --- a/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp +++ b/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp @@ -26,7 +26,7 @@ using nav2_system_tests::PlannerTester; using nav2_util::TestCostmap; using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; -using ComputePathToPoseResult = nav_msgs::msg::Path; +using ComputePathToPoseResult = nav2_msgs::msg::PathWithCost; TEST(testSimpleCostmaps, testSimpleCostmaps) { diff --git a/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp index 958891e02a..12c9bafd0b 100644 --- a/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp +++ b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp @@ -30,7 +30,7 @@ TEST(testIsPathValid, testIsPathValid) planner_tester->activate(); planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle); - nav_msgs::msg::Path path; + nav2_msgs::msg::PathWithCost path; // empty path bool is_path_valid = planner_tester->isPathValid(path); diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index 906e8f44d3..3475eb1dcd 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -29,9 +29,9 @@ using nav2_system_tests::PlannerTester; using nav2_util::TestCostmap; using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; -using ComputePathToPoseResult = nav_msgs::msg::Path; +using ComputePathToPoseResult = nav2_msgs::msg::PathWithCost; -void callback(const nav_msgs::msg::Path::ConstSharedPtr /*grid*/) +void callback(const nav2_msgs::msg::PathWithCost::ConstSharedPtr /*grid*/) { } @@ -120,7 +120,7 @@ TEST(testPluginMap, Failures) rclcpp_lifecycle::State state; obj->set_parameter(rclcpp::Parameter("expected_planner_frequency", 100000.0)); obj->onConfigure(state); - obj->create_subscription( + obj->create_subscription( "plan", rclcpp::SystemDefaultsQoS(), callback); geometry_msgs::msg::PoseStamped start; diff --git a/nav2_system_tests/src/planning/test_planner_random_node.cpp b/nav2_system_tests/src/planning/test_planner_random_node.cpp index 3e072414c7..6a51a0abe4 100644 --- a/nav2_system_tests/src/planning/test_planner_random_node.cpp +++ b/nav2_system_tests/src/planning/test_planner_random_node.cpp @@ -26,7 +26,7 @@ using nav2_system_tests::PlannerTester; using nav2_util::TestCostmap; using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; -using ComputePathToPoseResult = nav_msgs::msg::Path; +using ComputePathToPoseResult = nav2_msgs::msg::PathWithCost; TEST(testWithHundredRandomEndPoints, testWithHundredRandomEndPoints) { diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index bcbff2ae7c..598637e630 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -26,7 +26,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_core/planner_exceptions.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -54,7 +54,7 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner void deactivate() override; - nav_msgs::msg::Path createPlan( + nav2_msgs::msg::PathWithCost createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; @@ -78,7 +78,7 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner * @brief the function responsible for calling the algorithm and retrieving a path from it * @return global_path is the planned path to be taken */ - void getPlan(nav_msgs::msg::Path & global_path); + void getPlan(nav2_msgs::msg::PathWithCost & global_path); /** * @brief interpolates points between the consecutive waypoints of the path @@ -86,7 +86,7 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner * @param dist_bw_points is used to send in the interpolation_resolution (which has been set as the costmap resolution) * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other */ - static nav_msgs::msg::Path linearInterpolation( + static nav2_msgs::msg::PathWithCost linearInterpolation( const std::vector & raw_path, const double & dist_bw_points); diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index c509afb065..65cd5be08a 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -84,11 +84,11 @@ void ThetaStarPlanner::deactivate() RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); } -nav_msgs::msg::Path ThetaStarPlanner::createPlan( +nav2_msgs::msg::PathWithCost ThetaStarPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { - nav_msgs::msg::Path global_path; + nav2_msgs::msg::PathWithCost global_path; auto start_time = std::chrono::steady_clock::now(); // Corner case of start and goal beeing on the same cell @@ -176,7 +176,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( return global_path; } -void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) +void ThetaStarPlanner::getPlan(nav2_msgs::msg::PathWithCost & global_path) { std::vector path; if (planner_->isUnsafeToPlan()) { @@ -192,11 +192,11 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) global_path.header.frame_id = global_frame_; } -nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( +nav2_msgs::msg::PathWithCost ThetaStarPlanner::linearInterpolation( const std::vector & raw_path, const double & dist_bw_points) { - nav_msgs::msg::Path pa; + nav2_msgs::msg::PathWithCost pa; geometry_msgs::msg::PoseStamped p1; for (unsigned int j = 0; j < raw_path.size() - 1; j++) { diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index c2360d9f87..a50ccd2748 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -163,7 +163,7 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { planner_2d->configure(life_node, "test", nullptr, costmap_ros); planner_2d->activate(); - nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); + nav2_msgs::msg::PathWithCost path = planner_2d->createPlan(start, goal); EXPECT_GT(static_cast(path.poses.size()), 0); // test if the goal is unsafe diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 884b7b366e..8215d67839 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" namespace nav2_util { @@ -167,7 +167,7 @@ inline Iter first_after_integrated_distance(Iter begin, Iter end, Getter getComp * subset of the path. * @return double Path length */ -inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t start_index = 0) +inline double calculate_path_length(const nav2_msgs::msg::PathWithCost & path, size_t start_index = 0) { if (start_index + 1 >= path.poses.size()) { return 0.0; diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 38f27ad304..e8de718c24 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -16,7 +16,7 @@ #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "gtest/gtest.h" using nav2_util::geometry_utils::euclidean_distance; @@ -84,7 +84,7 @@ TEST(GeometryUtils, euclidean_distance_pose_2d) TEST(GeometryUtils, calculate_path_length) { - nav_msgs::msg::Path straight_line_path; + nav2_msgs::msg::PathWithCost straight_line_path; size_t nb_path_points = 10; float distance_between_poses = 2.0; float current_x_loc = 0.0; @@ -106,7 +106,7 @@ TEST(GeometryUtils, calculate_path_length) calculate_path_length(straight_line_path, straight_line_path.poses.size()), 0.0, 1e-5); - nav_msgs::msg::Path circle_path; + nav2_msgs::msg::PathWithCost circle_path; float polar_distance = 2.0; uint32_t current_polar_angle_deg = 0; constexpr float pi = 3.14159265358979; 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 e5c59aa20b..2d62af0410 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -24,7 +24,7 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" #include "nav2_msgs/msg/missed_waypoint.hpp" -#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_with_cost.hpp" #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md index aa9a5c83fe..3c3d1a950c 100644 --- a/tools/smoother_benchmarking/README.md +++ b/tools/smoother_benchmarking/README.md @@ -55,7 +55,7 @@ index c7a90bcb..6f93edbf 100644 // Get plan from start -> goal + auto planning_start = steady_clock_.now(); - nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + nav2_msgs::msg::PathWithCost curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + auto planning_duration = steady_clock_.now() - planning_start; + result->planning_time = planning_duration; From 444b50699214830e45f08b8ebfe01cd4619a883b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 14 Dec 2022 17:40:49 -0500 Subject: [PATCH 126/131] fix --- nav2_planner/include/nav2_planner/planner_server.hpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index c06c221ba4..ee14a05903 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -210,12 +210,6 @@ class PlannerServer : public nav2_util::LifecycleNode */ void publishPlan(const nav2_msgs::msg::PathWithCost & path); - void exceptionWarning( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id, - const std::exception & ex); - void exceptionWarning( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, From 5f08d1ffe9780e3f46d07b04b0d44190201246ea Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 15 Dec 2022 17:31:20 -0500 Subject: [PATCH 127/131] able to open in rviz --- .../path_with_cost_display.hpp | 164 +++++ .../src/path_with_cost_display.cpp | 561 ++++++++++++++++++ 2 files changed, 725 insertions(+) create mode 100644 nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp create mode 100644 nav2_rviz_plugins/src/path_with_cost_display.cpp diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp new file mode 100644 index 0000000000..5ed3f2d9e8 --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp @@ -0,0 +1,164 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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 Willow Garage, Inc. 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. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__PATH__PATH_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__PATH__PATH_DISPLAY_HPP_ + +#include + +#include "nav2_msgs/msg/path_with_cost.hpp" + +#include "rviz_common/message_filter_display.hpp" + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/axes.hpp" +#include "rviz_rendering/objects/billboard_line.hpp" + +#include "rviz_default_plugins/visibility_control.hpp" + +namespace Ogre +{ +class ManualObject; +} + +namespace rviz_common +{ +namespace properties +{ +class ColorProperty; +class FloatProperty; +class IntProperty; +class EnumProperty; +class VectorProperty; +} +} // namespace rviz_common + +namespace rviz_default_plugins +{ +namespace displays +{ +/** + * \class PathWithCostDisplay + * \brief Displays a nav2_msgs::msg::PathWithCost message + */ +class RVIZ_DEFAULT_PLUGINS_PUBLIC PathWithCostDisplay : public + rviz_common::MessageFilterDisplay +{ + Q_OBJECT + + public: + // TODO(Martin-Idel-SI): Constructor for testing, remove once ros_nodes can be mocked and call + // initialize() instead + explicit PathWithCostDisplay(rviz_common::DisplayContext * context); + PathWithCostDisplay(); + ~PathWithCostDisplay() override; + + /** @brief Overridden from Display. */ + void reset() override; + + /** @brief Overridden from MessageFilterDisplay. */ + void processMessage(nav2_msgs::msg::PathWithCost::ConstSharedPtr msg) override; + + protected: + /** @brief Overridden from Display. */ + void onInitialize() override; + + private Q_SLOTS: + void updateBufferLength(); + void updateStyle(); + void updateLineWidth(); + void updateOffset(); + void updatePoseStyle(); + void updatePoseAxisGeometry(); + void updatePoseArrowColor(); + void updatePoseArrowGeometry(); + + private: + void destroyObjects(); + void allocateArrowVector(std::vector & arrow_vect, size_t num); + void allocateAxesVector(std::vector & axes_vect, size_t num); + void destroyPoseAxesChain(); + void destroyPoseArrowChain(); + void updateManualObject( + Ogre::ManualObject * manual_object, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); + void updateBillBoardLine( + rviz_rendering::BillboardLine * billboard_line, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); + void updatePoseMarkers( + size_t buffer_index, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform); + void updateAxesMarkers( + std::vector & axes_vect, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); + void updateArrowMarkers( + std::vector & arrow_vect, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); + + std::vector manual_objects_; + std::vector billboard_lines_; + std::vector> axes_chain_; + std::vector> arrow_chain_; + Ogre::MaterialPtr lines_material_; + + rviz_common::properties::EnumProperty * style_property_; + rviz_common::properties::ColorProperty * color_property_; + rviz_common::properties::FloatProperty * alpha_property_; + rviz_common::properties::FloatProperty * line_width_property_; + rviz_common::properties::IntProperty * buffer_length_property_; + rviz_common::properties::VectorProperty * offset_property_; + + enum LineStyle + { + LINES, + BILLBOARDS + }; + + // pose marker property + rviz_common::properties::EnumProperty * pose_style_property_; + rviz_common::properties::FloatProperty * pose_axes_length_property_; + rviz_common::properties::FloatProperty * pose_axes_radius_property_; + rviz_common::properties::ColorProperty * pose_arrow_color_property_; + rviz_common::properties::FloatProperty * pose_arrow_shaft_length_property_; + rviz_common::properties::FloatProperty * pose_arrow_head_length_property_; + rviz_common::properties::FloatProperty * pose_arrow_shaft_diameter_property_; + rviz_common::properties::FloatProperty * pose_arrow_head_diameter_property_; + + enum PoseStyle + { + NONE, + AXES, + ARROWS, + }; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__PATH__PATH_DISPLAY_HPP_ diff --git a/nav2_rviz_plugins/src/path_with_cost_display.cpp b/nav2_rviz_plugins/src/path_with_cost_display.cpp new file mode 100644 index 0000000000..80bad60d29 --- /dev/null +++ b/nav2_rviz_plugins/src/path_with_cost_display.cpp @@ -0,0 +1,561 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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 Willow Garage, Inc. 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. + */ + +#include "nav2_rviz_plugins/path_with_cost_display.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include "rviz_common/logging.hpp" +#include "rviz_common/msg_conversions.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/color_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/int_property.hpp" +#include "rviz_common/properties/vector_property.hpp" +#include "rviz_common/uniform_string_stream.hpp" +#include "rviz_common/validate_floats.hpp" + +#include "rviz_rendering/material_manager.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +PathWithCostDisplay::PathWithCostDisplay(rviz_common::DisplayContext * context) + : PathWithCostDisplay() +{ + context_ = context; + scene_manager_ = context->getSceneManager(); + scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); + updateBufferLength(); +} + +PathWithCostDisplay::PathWithCostDisplay() +{ + style_property_ = new rviz_common::properties::EnumProperty( + "Line Style", "Lines", + "The rendering operation to use to draw the grid lines.", + this, SLOT(updateStyle())); + + style_property_->addOption("Lines", LINES); + style_property_->addOption("Billboards", BILLBOARDS); + + line_width_property_ = new rviz_common::properties::FloatProperty( + "Line Width", 0.03f, + "The width, in meters, of each path line." + "Only works with the 'Billboards' style.", + this, SLOT(updateLineWidth()), this); + line_width_property_->setMin(0.001f); + line_width_property_->hide(); + + color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(25, 255, 0), + "Color to draw the path.", this); + + alpha_property_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, + "Amount of transparency to apply to the path.", this); + + buffer_length_property_ = new rviz_common::properties::IntProperty( + "Buffer Length", 1, + "Number of paths to display.", + this, SLOT(updateBufferLength())); + buffer_length_property_->setMin(1); + + offset_property_ = new rviz_common::properties::VectorProperty( + "Offset", Ogre::Vector3::ZERO, + "Allows you to offset the path from the origin of the reference frame. In meters.", + this, SLOT(updateOffset())); + + pose_style_property_ = new rviz_common::properties::EnumProperty( + "Pose Style", "None", + "Shape to display the pose as.", + this, SLOT(updatePoseStyle())); + pose_style_property_->addOption("None", NONE); + pose_style_property_->addOption("Axes", AXES); + pose_style_property_->addOption("Arrows", ARROWS); + + pose_axes_length_property_ = new rviz_common::properties::FloatProperty( + "Length", 0.3f, + "Length of the axes.", + this, SLOT(updatePoseAxisGeometry()) ); + pose_axes_radius_property_ = new rviz_common::properties::FloatProperty( + "Radius", 0.03f, + "Radius of the axes.", + this, SLOT(updatePoseAxisGeometry()) ); + + pose_arrow_color_property_ = new rviz_common::properties::ColorProperty( + "Pose Color", + QColor(255, 85, 255), + "Color to draw the poses.", + this, SLOT(updatePoseArrowColor())); + pose_arrow_shaft_length_property_ = new rviz_common::properties::FloatProperty( + "Shaft Length", + 0.1f, + "Length of the arrow shaft.", + this, + SLOT(updatePoseArrowGeometry())); + pose_arrow_head_length_property_ = new rviz_common::properties::FloatProperty( + "Head Length", 0.2f, + "Length of the arrow head.", + this, + SLOT(updatePoseArrowGeometry())); + pose_arrow_shaft_diameter_property_ = new rviz_common::properties::FloatProperty( + "Shaft Diameter", + 0.1f, + "Diameter of the arrow shaft.", + this, + SLOT(updatePoseArrowGeometry())); + pose_arrow_head_diameter_property_ = new rviz_common::properties::FloatProperty( + "Head Diameter", + 0.3f, + "Diameter of the arrow head.", + this, + SLOT(updatePoseArrowGeometry())); + pose_axes_length_property_->hide(); + pose_axes_radius_property_->hide(); + pose_arrow_color_property_->hide(); + pose_arrow_shaft_length_property_->hide(); + pose_arrow_head_length_property_->hide(); + pose_arrow_shaft_diameter_property_->hide(); + pose_arrow_head_diameter_property_->hide(); + + static int count = 0; + std::string material_name = "LinesMaterial" + std::to_string(count++); + lines_material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting(material_name); +} + +PathWithCostDisplay::~PathWithCostDisplay() +{ + destroyObjects(); + destroyPoseAxesChain(); + destroyPoseArrowChain(); +} + +void PathWithCostDisplay::onInitialize() +{ + MFDClass::onInitialize(); + updateBufferLength(); +} + +void PathWithCostDisplay::reset() +{ + MFDClass::reset(); + updateBufferLength(); +} + + +void PathWithCostDisplay::allocateAxesVector(std::vector & axes_vect, size_t num) +{ + auto vector_size = axes_vect.size(); + if (num > vector_size) { + axes_vect.reserve(num); + for (auto i = vector_size; i < num; ++i) { + axes_vect.push_back( + new rviz_rendering::Axes( + scene_manager_, scene_node_, + pose_axes_length_property_->getFloat(), + pose_axes_radius_property_->getFloat())); + } + } else if (num < vector_size) { + for (auto i = num; i < vector_size; ++i) { + delete axes_vect[i]; + } + axes_vect.resize(num); + } +} + +void PathWithCostDisplay::allocateArrowVector(std::vector & arrow_vect, size_t num) +{ + auto vector_size = arrow_vect.size(); + if (num > vector_size) { + arrow_vect.reserve(num); + for (auto i = vector_size; i < num; ++i) { + arrow_vect.push_back(new rviz_rendering::Arrow(scene_manager_, scene_node_)); + } + } else if (num < vector_size) { + for (auto i = num; i < vector_size; ++i) { + delete arrow_vect[i]; + } + arrow_vect.resize(num); + } +} + +void PathWithCostDisplay::destroyPoseAxesChain() +{ + for (auto & axes_vect : axes_chain_) { + allocateAxesVector(axes_vect, 0); + } + axes_chain_.clear(); +} + +void PathWithCostDisplay::destroyPoseArrowChain() +{ + for (auto & arrow_vect : arrow_chain_) { + allocateArrowVector(arrow_vect, 0); + } + arrow_chain_.clear(); +} + +void PathWithCostDisplay::updateStyle() +{ + auto style = static_cast(style_property_->getOptionInt()); + + if (style == BILLBOARDS) { + line_width_property_->show(); + } else { + line_width_property_->hide(); + } + + updateBufferLength(); +} + +void PathWithCostDisplay::updateLineWidth() +{ + auto style = static_cast(style_property_->getOptionInt()); + float line_width = line_width_property_->getFloat(); + + if (style == BILLBOARDS) { + for (auto billboard_line : billboard_lines_) { + if (billboard_line) { + billboard_line->setLineWidth(line_width); + } + } + } + context_->queueRender(); +} + +void PathWithCostDisplay::updateOffset() +{ + scene_node_->setPosition(offset_property_->getVector() ); + context_->queueRender(); +} + +void PathWithCostDisplay::updatePoseStyle() +{ + auto pose_style = static_cast(pose_style_property_->getOptionInt()); + switch (pose_style) { + case AXES: + pose_axes_length_property_->show(); + pose_axes_radius_property_->show(); + pose_arrow_color_property_->hide(); + pose_arrow_shaft_length_property_->hide(); + pose_arrow_head_length_property_->hide(); + pose_arrow_shaft_diameter_property_->hide(); + pose_arrow_head_diameter_property_->hide(); + break; + case ARROWS: + pose_axes_length_property_->hide(); + pose_axes_radius_property_->hide(); + pose_arrow_color_property_->show(); + pose_arrow_shaft_length_property_->show(); + pose_arrow_head_length_property_->show(); + pose_arrow_shaft_diameter_property_->show(); + pose_arrow_head_diameter_property_->show(); + break; + default: + pose_axes_length_property_->hide(); + pose_axes_radius_property_->hide(); + pose_arrow_color_property_->hide(); + pose_arrow_shaft_length_property_->hide(); + pose_arrow_head_length_property_->hide(); + pose_arrow_shaft_diameter_property_->hide(); + pose_arrow_head_diameter_property_->hide(); + } + updateBufferLength(); +} + +void PathWithCostDisplay::updatePoseAxisGeometry() +{ + for (auto & axes_vect : axes_chain_) { + for (auto & axes : axes_vect) { + axes->set( + pose_axes_length_property_->getFloat(), + pose_axes_radius_property_->getFloat() ); + } + } + context_->queueRender(); +} + +void PathWithCostDisplay::updatePoseArrowColor() +{ + QColor color = pose_arrow_color_property_->getColor(); + + for (auto & arrow_vect : arrow_chain_) { + for (auto & arrow : arrow_vect) { + arrow->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f); + } + } + context_->queueRender(); +} + +void PathWithCostDisplay::updatePoseArrowGeometry() +{ + for (auto & arrow_vect : arrow_chain_) { + for (auto & arrow : arrow_vect) { + arrow->set( + pose_arrow_shaft_length_property_->getFloat(), + pose_arrow_shaft_diameter_property_->getFloat(), + pose_arrow_head_length_property_->getFloat(), + pose_arrow_head_diameter_property_->getFloat()); + } + } + context_->queueRender(); +} + +void PathWithCostDisplay::destroyObjects() +{ + // Destroy all simple lines, if any + for (auto manual_object : manual_objects_) { + manual_object->clear(); + scene_manager_->destroyManualObject(manual_object); + } + manual_objects_.clear(); + + // Destroy all billboards, if any + for (auto billboard_line : billboard_lines_) { + delete billboard_line; // also destroys the corresponding scene node + } + billboard_lines_.clear(); +} + +void PathWithCostDisplay::updateBufferLength() +{ + // Destroy all path objects + destroyObjects(); + + // Destroy all axes and arrows + destroyPoseAxesChain(); + destroyPoseArrowChain(); + + // Read options + auto buffer_length = static_cast(buffer_length_property_->getInt()); + auto style = static_cast(style_property_->getOptionInt()); + + // Create new path objects + switch (style) { + case LINES: // simple lines with fixed width of 1px + manual_objects_.reserve(buffer_length); + for (size_t i = 0; i < buffer_length; i++) { + auto manual_object = scene_manager_->createManualObject(); + manual_object->setDynamic(true); + scene_node_->attachObject(manual_object); + + manual_objects_.push_back(manual_object); + } + break; + + case BILLBOARDS: // billboards with configurable width + billboard_lines_.reserve(buffer_length); + for (size_t i = 0; i < buffer_length; i++) { + auto billboard_line = new rviz_rendering::BillboardLine(scene_manager_, scene_node_); + billboard_lines_.push_back(billboard_line); + } + break; + } + axes_chain_.resize(buffer_length); + arrow_chain_.resize(buffer_length); +} + +bool validateFloats(const nav2_msgs::msg::PathWithCost & msg) +{ + bool valid = true; + valid = valid && rviz_common::validateFloats(msg.poses); + return valid; +} + +void PathWithCostDisplay::processMessage(nav2_msgs::msg::PathWithCost::ConstSharedPtr msg) +{ + // Calculate index of oldest element in cyclic buffer + size_t bufferIndex = messages_received_ % buffer_length_property_->getInt(); + + auto style = static_cast(style_property_->getOptionInt()); + Ogre::ManualObject * manual_object = nullptr; + rviz_rendering::BillboardLine * billboard_line = nullptr; + + // Delete oldest element + switch (style) { + case LINES: + manual_object = manual_objects_[bufferIndex]; + manual_object->clear(); + break; + + case BILLBOARDS: + billboard_line = billboard_lines_[bufferIndex]; + billboard_line->clear(); + break; + } + + // Check if path contains invalid coordinate values + if (!validateFloats(*msg)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", "Message contained invalid " + "floating point " + "values (nans or infs)"); + return; + } + + // Lookup transform into fixed frame + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) { + setMissingTransformToFixedFrame(msg->header.frame_id); + return; + } + setTransformOk(); + + Ogre::Matrix4 transform(orientation); + transform.setTrans(position); + + switch (style) { + case LINES: + updateManualObject(manual_object, msg, transform); + break; + + case BILLBOARDS: + updateBillBoardLine(billboard_line, msg, transform); + break; + } + updatePoseMarkers(bufferIndex, msg, transform); + + context_->queueRender(); +} + +void PathWithCostDisplay::updateManualObject( + Ogre::ManualObject * manual_object, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) +{ + auto color = color_property_->getOgreColor(); + color.a = alpha_property_->getFloat(); + + manual_object->estimateVertexCount(msg->poses.size()); + manual_object->begin( + lines_material_->getName(), Ogre::RenderOperation::OT_LINE_STRIP, "rviz_rendering"); + + for (auto pose_stamped : msg->poses) { + manual_object->position(transform * rviz_common::pointMsgToOgre(pose_stamped.pose.position)); + rviz_rendering::MaterialManager::enableAlphaBlending(lines_material_, color.a); + manual_object->colour(color); + } + + manual_object->end(); +} + +void PathWithCostDisplay::updateBillBoardLine( + rviz_rendering::BillboardLine * billboard_line, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) +{ + auto color = color_property_->getOgreColor(); + color.a = alpha_property_->getFloat(); + + billboard_line->setNumLines(1); + billboard_line->setMaxPointsPerLine(static_cast(msg->poses.size())); + billboard_line->setLineWidth(line_width_property_->getFloat()); + + for (auto pose_stamped : msg->poses) { + Ogre::Vector3 xpos = transform * rviz_common::pointMsgToOgre(pose_stamped.pose.position); + billboard_line->addPoint(xpos, color); + } +} + +void PathWithCostDisplay::updatePoseMarkers( + size_t buffer_index, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform) +{ + auto pose_style = static_cast(pose_style_property_->getOptionInt()); + auto & arrow_vect = arrow_chain_[buffer_index]; + auto & axes_vect = axes_chain_[buffer_index]; + + if (pose_style == AXES) { + updateAxesMarkers(axes_vect, msg, transform); + } + if (pose_style == ARROWS) { + updateArrowMarkers(arrow_vect, msg, transform); + } +} + +void PathWithCostDisplay::updateAxesMarkers( + std::vector & axes_vect, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) +{ + auto num_points = msg->poses.size(); + allocateAxesVector(axes_vect, num_points); + for (size_t i = 0; i < num_points; ++i) { + const geometry_msgs::msg::Point & pos = msg->poses[i].pose.position; + axes_vect[i]->setPosition(transform * rviz_common::pointMsgToOgre(pos)); + Ogre::Quaternion orientation(rviz_common::quaternionMsgToOgre(msg->poses[i].pose.orientation)); + + // Extract the rotation part of the transformation matrix as a quaternion + Ogre::Quaternion transform_orientation = transform.linear(); + + axes_vect[i]->setOrientation(transform_orientation * orientation); + } +} + +void PathWithCostDisplay::updateArrowMarkers( + std::vector & arrow_vect, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) +{ + auto num_points = msg->poses.size(); + allocateArrowVector(arrow_vect, num_points); + for (size_t i = 0; i < num_points; ++i) { + QColor color = pose_arrow_color_property_->getColor(); + arrow_vect[i]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f); + + arrow_vect[i]->set( + pose_arrow_shaft_length_property_->getFloat(), + pose_arrow_shaft_diameter_property_->getFloat(), + pose_arrow_head_length_property_->getFloat(), + pose_arrow_head_diameter_property_->getFloat()); + const geometry_msgs::msg::Point & pos = msg->poses[i].pose.position; + arrow_vect[i]->setPosition(transform * rviz_common::pointMsgToOgre(pos)); + Ogre::Quaternion orientation(rviz_common::quaternionMsgToOgre(msg->poses[i].pose.orientation)); + + // Extract the rotation part of the transformation matrix as a quaternion + Ogre::Quaternion transform_orientation = transform.linear(); + + Ogre::Vector3 dir(1, 0, 0); + dir = transform_orientation * orientation * dir; + arrow_vect[i]->setDirection(dir); + } +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::PathWithCostDisplay, rviz_common::Display) From f3b3dc65696642c6a8b45b31cd570b3b0ea4176e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 16 Dec 2022 10:15:53 -0500 Subject: [PATCH 128/131] path with cost display working with new message type --- nav2_bringup/rviz/nav2_default_view.rviz | 4 +- nav2_rviz_plugins/CMakeLists.txt | 2 + .../path_with_cost_display.hpp | 42 +++-- nav2_rviz_plugins/plugins_description.xml | 8 + .../src/path_with_cost_display.cpp | 167 +++++++++--------- 5 files changed, 123 insertions(+), 100 deletions(-) diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index d2a4b5e136..b39d751612 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -272,7 +272,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz_default_plugins/Path + Class: nav2_rviz_plugins/PathWithCost Color: 255; 0; 0 Enabled: true Head Diameter: 0.019999999552965164 @@ -372,7 +372,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz_default_plugins/Path + Class: nav2_rviz_plugins/PathWithCost Color: 0; 12; 255 Enabled: true Head Diameter: 0.30000001192092896 diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index 39bdbabae4..fcb685328e 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -39,6 +39,7 @@ set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/nav2_panel.hpp include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp + include/nav2_rviz_plugins/path_with_cost_display.hpp ) include_directories( @@ -52,6 +53,7 @@ add_library(${library_name} SHARED src/nav2_panel.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp + src/path_with_cost_display.cpp ${nav2_rviz_plugins_headers_to_moc} ) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp index 5ed3f2d9e8..d792005f87 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/path_with_cost_display.hpp @@ -28,8 +28,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__PATH__PATH_DISPLAY_HPP_ -#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__PATH__PATH_DISPLAY_HPP_ +#ifndef NAV2_RVIZ_PLUGINS__PATH_WITH_COST_DISPLAY_HPP_ +#define NAV2_RVIZ_PLUGINS__PATH_WITH_COST_DISPLAY_HPP_ #include @@ -69,11 +69,11 @@ namespace displays * \brief Displays a nav2_msgs::msg::PathWithCost message */ class RVIZ_DEFAULT_PLUGINS_PUBLIC PathWithCostDisplay : public - rviz_common::MessageFilterDisplay + rviz_common::MessageFilterDisplay { - Q_OBJECT + Q_OBJECT - public: +public: // TODO(Martin-Idel-SI): Constructor for testing, remove once ros_nodes can be mocked and call // initialize() instead explicit PathWithCostDisplay(rviz_common::DisplayContext * context); @@ -86,11 +86,11 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC PathWithCostDisplay : public /** @brief Overridden from MessageFilterDisplay. */ void processMessage(nav2_msgs::msg::PathWithCost::ConstSharedPtr msg) override; - protected: +protected: /** @brief Overridden from Display. */ void onInitialize() override; - private Q_SLOTS: +private Q_SLOTS: void updateBufferLength(); void updateStyle(); void updateLineWidth(); @@ -100,26 +100,32 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC PathWithCostDisplay : public void updatePoseArrowColor(); void updatePoseArrowGeometry(); - private: +private: void destroyObjects(); void allocateArrowVector(std::vector & arrow_vect, size_t num); void allocateAxesVector(std::vector & axes_vect, size_t num); void destroyPoseAxesChain(); void destroyPoseArrowChain(); void updateManualObject( - Ogre::ManualObject * manual_object, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, - const Ogre::Matrix4 & transform); + Ogre::ManualObject * manual_object, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); void updateBillBoardLine( - rviz_rendering::BillboardLine * billboard_line, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, - const Ogre::Matrix4 & transform); + rviz_rendering::BillboardLine * billboard_line, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); void updatePoseMarkers( - size_t buffer_index, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform); + size_t buffer_index, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); void updateAxesMarkers( - std::vector & axes_vect, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, - const Ogre::Matrix4 & transform); + std::vector & axes_vect, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); void updateArrowMarkers( - std::vector & arrow_vect, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, - const Ogre::Matrix4 & transform); + std::vector & arrow_vect, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform); std::vector manual_objects_; std::vector billboard_lines_; @@ -161,4 +167,4 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC PathWithCostDisplay : public } // namespace displays } // namespace rviz_default_plugins -#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__PATH__PATH_DISPLAY_HPP_ +#endif // NAV2_RVIZ_PLUGINS__PATH_WITH_COST_DISPLAY_HPP_ diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index 197a9a750d..fe89ba96c5 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -18,4 +18,12 @@ The Particle Cloud rviz display. + + Displays data from a data from nav2_msgs::msg::PathWithCost. + nav2_msgs/msg/PathWithCost + + + diff --git a/nav2_rviz_plugins/src/path_with_cost_display.cpp b/nav2_rviz_plugins/src/path_with_cost_display.cpp index 80bad60d29..b148523970 100644 --- a/nav2_rviz_plugins/src/path_with_cost_display.cpp +++ b/nav2_rviz_plugins/src/path_with_cost_display.cpp @@ -57,7 +57,7 @@ namespace displays { PathWithCostDisplay::PathWithCostDisplay(rviz_common::DisplayContext * context) - : PathWithCostDisplay() +: PathWithCostDisplay() { context_ = context; scene_manager_ = context->getSceneManager(); @@ -68,85 +68,85 @@ PathWithCostDisplay::PathWithCostDisplay(rviz_common::DisplayContext * context) PathWithCostDisplay::PathWithCostDisplay() { style_property_ = new rviz_common::properties::EnumProperty( - "Line Style", "Lines", - "The rendering operation to use to draw the grid lines.", - this, SLOT(updateStyle())); + "Line Style", "Lines", + "The rendering operation to use to draw the grid lines.", + this, SLOT(updateStyle())); style_property_->addOption("Lines", LINES); style_property_->addOption("Billboards", BILLBOARDS); line_width_property_ = new rviz_common::properties::FloatProperty( - "Line Width", 0.03f, - "The width, in meters, of each path line." - "Only works with the 'Billboards' style.", - this, SLOT(updateLineWidth()), this); + "Line Width", 0.03f, + "The width, in meters, of each path line." + "Only works with the 'Billboards' style.", + this, SLOT(updateLineWidth()), this); line_width_property_->setMin(0.001f); line_width_property_->hide(); color_property_ = new rviz_common::properties::ColorProperty( - "Color", QColor(25, 255, 0), - "Color to draw the path.", this); + "Color", QColor(25, 255, 0), + "Color to draw the path.", this); alpha_property_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, - "Amount of transparency to apply to the path.", this); + "Alpha", 1.0, + "Amount of transparency to apply to the path.", this); buffer_length_property_ = new rviz_common::properties::IntProperty( - "Buffer Length", 1, - "Number of paths to display.", - this, SLOT(updateBufferLength())); + "Buffer Length", 1, + "Number of paths to display.", + this, SLOT(updateBufferLength())); buffer_length_property_->setMin(1); offset_property_ = new rviz_common::properties::VectorProperty( - "Offset", Ogre::Vector3::ZERO, - "Allows you to offset the path from the origin of the reference frame. In meters.", - this, SLOT(updateOffset())); + "Offset", Ogre::Vector3::ZERO, + "Allows you to offset the path from the origin of the reference frame. In meters.", + this, SLOT(updateOffset())); pose_style_property_ = new rviz_common::properties::EnumProperty( - "Pose Style", "None", - "Shape to display the pose as.", - this, SLOT(updatePoseStyle())); + "Pose Style", "None", + "Shape to display the pose as.", + this, SLOT(updatePoseStyle())); pose_style_property_->addOption("None", NONE); pose_style_property_->addOption("Axes", AXES); pose_style_property_->addOption("Arrows", ARROWS); pose_axes_length_property_ = new rviz_common::properties::FloatProperty( - "Length", 0.3f, - "Length of the axes.", - this, SLOT(updatePoseAxisGeometry()) ); + "Length", 0.3f, + "Length of the axes.", + this, SLOT(updatePoseAxisGeometry()) ); pose_axes_radius_property_ = new rviz_common::properties::FloatProperty( - "Radius", 0.03f, - "Radius of the axes.", - this, SLOT(updatePoseAxisGeometry()) ); + "Radius", 0.03f, + "Radius of the axes.", + this, SLOT(updatePoseAxisGeometry()) ); pose_arrow_color_property_ = new rviz_common::properties::ColorProperty( - "Pose Color", - QColor(255, 85, 255), - "Color to draw the poses.", - this, SLOT(updatePoseArrowColor())); + "Pose Color", + QColor(255, 85, 255), + "Color to draw the poses.", + this, SLOT(updatePoseArrowColor())); pose_arrow_shaft_length_property_ = new rviz_common::properties::FloatProperty( - "Shaft Length", - 0.1f, - "Length of the arrow shaft.", - this, - SLOT(updatePoseArrowGeometry())); + "Shaft Length", + 0.1f, + "Length of the arrow shaft.", + this, + SLOT(updatePoseArrowGeometry())); pose_arrow_head_length_property_ = new rviz_common::properties::FloatProperty( - "Head Length", 0.2f, - "Length of the arrow head.", - this, - SLOT(updatePoseArrowGeometry())); + "Head Length", 0.2f, + "Length of the arrow head.", + this, + SLOT(updatePoseArrowGeometry())); pose_arrow_shaft_diameter_property_ = new rviz_common::properties::FloatProperty( - "Shaft Diameter", - 0.1f, - "Diameter of the arrow shaft.", - this, - SLOT(updatePoseArrowGeometry())); + "Shaft Diameter", + 0.1f, + "Diameter of the arrow shaft.", + this, + SLOT(updatePoseArrowGeometry())); pose_arrow_head_diameter_property_ = new rviz_common::properties::FloatProperty( - "Head Diameter", - 0.3f, - "Diameter of the arrow head.", - this, - SLOT(updatePoseArrowGeometry())); + "Head Diameter", + 0.3f, + "Diameter of the arrow head.", + this, + SLOT(updatePoseArrowGeometry())); pose_axes_length_property_->hide(); pose_axes_radius_property_->hide(); pose_arrow_color_property_->hide(); @@ -156,7 +156,7 @@ PathWithCostDisplay::PathWithCostDisplay() pose_arrow_head_diameter_property_->hide(); static int count = 0; - std::string material_name = "LinesMaterial" + std::to_string(count++); + std::string material_name = "LinesMaterialPathWithCost" + std::to_string(count++); lines_material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting(material_name); } @@ -180,17 +180,19 @@ void PathWithCostDisplay::reset() } -void PathWithCostDisplay::allocateAxesVector(std::vector & axes_vect, size_t num) +void PathWithCostDisplay::allocateAxesVector( + std::vector & axes_vect, + size_t num) { auto vector_size = axes_vect.size(); if (num > vector_size) { axes_vect.reserve(num); for (auto i = vector_size; i < num; ++i) { axes_vect.push_back( - new rviz_rendering::Axes( - scene_manager_, scene_node_, - pose_axes_length_property_->getFloat(), - pose_axes_radius_property_->getFloat())); + new rviz_rendering::Axes( + scene_manager_, scene_node_, + pose_axes_length_property_->getFloat(), + pose_axes_radius_property_->getFloat())); } } else if (num < vector_size) { for (auto i = num; i < vector_size; ++i) { @@ -200,7 +202,9 @@ void PathWithCostDisplay::allocateAxesVector(std::vector } } -void PathWithCostDisplay::allocateArrowVector(std::vector & arrow_vect, size_t num) +void PathWithCostDisplay::allocateArrowVector( + std::vector & arrow_vect, + size_t num) { auto vector_size = arrow_vect.size(); if (num > vector_size) { @@ -305,8 +309,8 @@ void PathWithCostDisplay::updatePoseAxisGeometry() for (auto & axes_vect : axes_chain_) { for (auto & axes : axes_vect) { axes->set( - pose_axes_length_property_->getFloat(), - pose_axes_radius_property_->getFloat() ); + pose_axes_length_property_->getFloat(), + pose_axes_radius_property_->getFloat() ); } } context_->queueRender(); @@ -329,10 +333,10 @@ void PathWithCostDisplay::updatePoseArrowGeometry() for (auto & arrow_vect : arrow_chain_) { for (auto & arrow : arrow_vect) { arrow->set( - pose_arrow_shaft_length_property_->getFloat(), - pose_arrow_shaft_diameter_property_->getFloat(), - pose_arrow_head_length_property_->getFloat(), - pose_arrow_head_diameter_property_->getFloat()); + pose_arrow_shaft_length_property_->getFloat(), + pose_arrow_shaft_diameter_property_->getFloat(), + pose_arrow_head_length_property_->getFloat(), + pose_arrow_head_diameter_property_->getFloat()); } } context_->queueRender(); @@ -424,9 +428,9 @@ void PathWithCostDisplay::processMessage(nav2_msgs::msg::PathWithCost::ConstShar // Check if path contains invalid coordinate values if (!validateFloats(*msg)) { setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", "Message contained invalid " - "floating point " - "values (nans or infs)"); + rviz_common::properties::StatusProperty::Error, "Topic", "Message contained invalid " + "floating point " + "values (nans or infs)"); return; } @@ -457,15 +461,15 @@ void PathWithCostDisplay::processMessage(nav2_msgs::msg::PathWithCost::ConstShar } void PathWithCostDisplay::updateManualObject( - Ogre::ManualObject * manual_object, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, - const Ogre::Matrix4 & transform) + Ogre::ManualObject * manual_object, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) { auto color = color_property_->getOgreColor(); color.a = alpha_property_->getFloat(); manual_object->estimateVertexCount(msg->poses.size()); manual_object->begin( - lines_material_->getName(), Ogre::RenderOperation::OT_LINE_STRIP, "rviz_rendering"); + lines_material_->getName(), Ogre::RenderOperation::OT_LINE_STRIP, "rviz_rendering"); for (auto pose_stamped : msg->poses) { manual_object->position(transform * rviz_common::pointMsgToOgre(pose_stamped.pose.position)); @@ -477,8 +481,8 @@ void PathWithCostDisplay::updateManualObject( } void PathWithCostDisplay::updateBillBoardLine( - rviz_rendering::BillboardLine * billboard_line, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, - const Ogre::Matrix4 & transform) + rviz_rendering::BillboardLine * billboard_line, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) { auto color = color_property_->getOgreColor(); color.a = alpha_property_->getFloat(); @@ -494,7 +498,8 @@ void PathWithCostDisplay::updateBillBoardLine( } void PathWithCostDisplay::updatePoseMarkers( - size_t buffer_index, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform) + size_t buffer_index, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform) { auto pose_style = static_cast(pose_style_property_->getOptionInt()); auto & arrow_vect = arrow_chain_[buffer_index]; @@ -509,8 +514,9 @@ void PathWithCostDisplay::updatePoseMarkers( } void PathWithCostDisplay::updateAxesMarkers( - std::vector & axes_vect, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, - const Ogre::Matrix4 & transform) + std::vector & axes_vect, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) { auto num_points = msg->poses.size(); allocateAxesVector(axes_vect, num_points); @@ -527,8 +533,9 @@ void PathWithCostDisplay::updateAxesMarkers( } void PathWithCostDisplay::updateArrowMarkers( - std::vector & arrow_vect, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, - const Ogre::Matrix4 & transform) + std::vector & arrow_vect, + nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, + const Ogre::Matrix4 & transform) { auto num_points = msg->poses.size(); allocateArrowVector(arrow_vect, num_points); @@ -537,10 +544,10 @@ void PathWithCostDisplay::updateArrowMarkers( arrow_vect[i]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f); arrow_vect[i]->set( - pose_arrow_shaft_length_property_->getFloat(), - pose_arrow_shaft_diameter_property_->getFloat(), - pose_arrow_head_length_property_->getFloat(), - pose_arrow_head_diameter_property_->getFloat()); + pose_arrow_shaft_length_property_->getFloat(), + pose_arrow_shaft_diameter_property_->getFloat(), + pose_arrow_head_length_property_->getFloat(), + pose_arrow_head_diameter_property_->getFloat()); const geometry_msgs::msg::Point & pos = msg->poses[i].pose.position; arrow_vect[i]->setPosition(transform * rviz_common::pointMsgToOgre(pos)); Ogre::Quaternion orientation(rviz_common::quaternionMsgToOgre(msg->poses[i].pose.orientation)); From 0903d22e6751fdb8e79fca171e159cbae21449a9 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 19 Dec 2022 15:08:49 -0500 Subject: [PATCH 129/131] updates --- .../src/navigators/navigate_to_pose.cpp | 2 +- nav2_navfn_planner/src/navfn_planner.cpp | 18 +++++++ nav2_planner/src/planner_server.cpp | 3 ++ .../src/path_with_cost_display.cpp | 50 ++++++++++++++++--- 4 files changed, 64 insertions(+), 9 deletions(-) diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 8b16691574..71f55b5291 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -66,7 +66,7 @@ NavigateToPoseNavigator::getDefaultBTFilepath( node->declare_parameter( "default_nav_to_pose_bt_xml", pkg_share_dir + - "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"); + "/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml"); } node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename); diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index b6c3da69b6..148aae9f7b 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -185,6 +185,7 @@ nav2_msgs::msg::PathWithCost NavfnPlanner::createPlan( if (start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_) { pose.pose.orientation = goal.pose.orientation; } + path.costs.push_back(costmap_->getCost(mx_start, my_start)); path.poses.push_back(pose); return path; } @@ -194,6 +195,23 @@ nav2_msgs::msg::PathWithCost NavfnPlanner::createPlan( "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); } + for ([[maybe_unused]] const auto & pose : path.poses) { + unsigned int mx, my; + costmap_->worldToMap(pose.pose.position.x, pose.pose.position.y, mx, my); + auto cost = costmap_->getCost(mx, my); + path.costs.push_back(cost); +// if ([[maybe_unused]] auto cost = costmap_->getCost(mx, my)) { +// path.costs.push_back(50); +// } else { +// path.costs.push_back(0); +// } + } + + if (path.poses.size() != path.costs.size()) + { + RCLCPP_INFO_STREAM(logger_, "Poses and costs do not match!"); + } + #ifdef BENCHMARK_TESTING steady_clock::time_point b = steady_clock::now(); diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 0c9f893116..bf82e0172d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -598,6 +598,9 @@ void PlannerServer::isPathValid( return; } + RCLCPP_INFO_STREAM(get_logger(), "Path and cost" << request->path.poses.size() << " " << + request->path.costs.size()); + // Find the closest point to the robot to evaluate from // TODO add orientation element like `truncate_path_local_action` BT node for looping paths geometry_msgs::msg::PoseStamped current_pose; diff --git a/nav2_rviz_plugins/src/path_with_cost_display.cpp b/nav2_rviz_plugins/src/path_with_cost_display.cpp index b148523970..bc77f8ff94 100644 --- a/nav2_rviz_plugins/src/path_with_cost_display.cpp +++ b/nav2_rviz_plugins/src/path_with_cost_display.cpp @@ -405,6 +405,8 @@ bool validateFloats(const nav2_msgs::msg::PathWithCost & msg) void PathWithCostDisplay::processMessage(nav2_msgs::msg::PathWithCost::ConstSharedPtr msg) { +// std::cout << "ProcessMessage: Costs Size: " << msg->costs.size() << " Path Size: " << +// msg->poses.size() << std::endl; // Calculate index of oldest element in cyclic buffer size_t bufferIndex = messages_received_ % buffer_length_property_->getInt(); @@ -448,13 +450,15 @@ void PathWithCostDisplay::processMessage(nav2_msgs::msg::PathWithCost::ConstShar switch (style) { case LINES: - updateManualObject(manual_object, msg, transform); +// updateManualObject(manual_object, msg, transform); break; case BILLBOARDS: updateBillBoardLine(billboard_line, msg, transform); break; } + +// updateManualObject(manual_object, msg, transform); updatePoseMarkers(bufferIndex, msg, transform); context_->queueRender(); @@ -464,15 +468,38 @@ void PathWithCostDisplay::updateManualObject( Ogre::ManualObject * manual_object, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform) { - auto color = color_property_->getOgreColor(); - color.a = alpha_property_->getFloat(); +// auto color = color_property_->getOgreColor(); +// color.a = alpha_property_->getFloat(); +// std::cout << "Manual Object: Costs Size: " << msg->costs.size() << "Path Size: " << msg->poses +// .size() +// <estimateVertexCount(msg->poses.size()); manual_object->begin( lines_material_->getName(), Ogre::RenderOperation::OT_LINE_STRIP, "rviz_rendering"); - for (auto pose_stamped : msg->poses) { - manual_object->position(transform * rviz_common::pointMsgToOgre(pose_stamped.pose.position)); + for (size_t i = 0; i < (msg->poses.size() && msg->costs.size()); ++i) { + manual_object->position(transform * rviz_common::pointMsgToOgre(msg->poses[i].pose.position)); + +// int r = i * 5; +// +// while ( r > 255) { +// r = 255 - r; +// } +// +// Ogre::ColourValue color(r, 0, 0); + + [[maybe_unused]] auto r = msg->costs[i]; + + if ( r > 255 ) { + r = 255; + } + if (r < 0 ) { + r = 0; + } + + Ogre::ColourValue color(r, 255 - r, 0); +// Ogre::ColourValue color(10, 10, 10); rviz_rendering::MaterialManager::enableAlphaBlending(lines_material_, color.a); manual_object->colour(color); } @@ -484,8 +511,8 @@ void PathWithCostDisplay::updateBillBoardLine( rviz_rendering::BillboardLine * billboard_line, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform) { - auto color = color_property_->getOgreColor(); - color.a = alpha_property_->getFloat(); +// auto color = color_property_->getOgreColor(); +// color.a = alpha_property_->getFloat(); billboard_line->setNumLines(1); billboard_line->setMaxPointsPerLine(static_cast(msg->poses.size())); @@ -493,6 +520,7 @@ void PathWithCostDisplay::updateBillBoardLine( for (auto pose_stamped : msg->poses) { Ogre::Vector3 xpos = transform * rviz_common::pointMsgToOgre(pose_stamped.pose.position); + Ogre::ColourValue color(10, 10, 10); billboard_line->addPoint(xpos, color); } } @@ -501,6 +529,8 @@ void PathWithCostDisplay::updatePoseMarkers( size_t buffer_index, nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform) { +// std::cout << "UpdatePoseMarkers: Costs Size: " << msg->costs.size() << " Path Size: " << +// msg->poses.size() << std::endl; auto pose_style = static_cast(pose_style_property_->getOptionInt()); auto & arrow_vect = arrow_chain_[buffer_index]; auto & axes_vect = axes_chain_[buffer_index]; @@ -509,6 +539,8 @@ void PathWithCostDisplay::updatePoseMarkers( updateAxesMarkers(axes_vect, msg, transform); } if (pose_style == ARROWS) { +// std::cout << "UpdatePoseMarkersCase: Costs Size: " << msg->costs.size() << " Path Size: " << +// msg->poses.size() << std::endl; updateArrowMarkers(arrow_vect, msg, transform); } } @@ -537,10 +569,12 @@ void PathWithCostDisplay::updateArrowMarkers( nav2_msgs::msg::PathWithCost::ConstSharedPtr msg, const Ogre::Matrix4 & transform) { +// std::cout << "Arrow Markers: Costs Size: " << msg->costs.size() << "Path Size: " << msg->poses +// .size() << std::endl; auto num_points = msg->poses.size(); allocateArrowVector(arrow_vect, num_points); for (size_t i = 0; i < num_points; ++i) { - QColor color = pose_arrow_color_property_->getColor(); + QColor color(msg->costs[i], 255-msg->costs[i], 0); arrow_vect[i]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f); arrow_vect[i]->set( From acb0458b01daabe2ae413b44c51e4e585ec19759 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 20 Dec 2022 16:25:59 -0500 Subject: [PATCH 130/131] code review --- nav2_behavior_tree/nav2_tree_nodes.xml | 1 + nav2_planner/include/nav2_planner/planner_server.hpp | 2 +- nav2_planner/src/planner_server.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 6b6ac08eec..bbf115490e 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -245,6 +245,7 @@ Path to validate Server timeout + Check for significant cost change diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index ee14a05903..40facb0e3f 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -239,7 +239,7 @@ class PlannerServer : public nav2_util::LifecycleNode std::vector planner_ids_; std::vector planner_types_; double max_planner_duration_; - double z_score_; + double cost_change_z_score_; std::string planner_ids_concat_; // Clock diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index bf82e0172d..da49f0d613 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -54,10 +54,10 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter("expected_planner_frequency", 1.0); nav2_util::declare_parameter_if_not_declared( - this, "z_score", rclcpp::ParameterValue(2.55)); + this, "cost_change_z_score", rclcpp::ParameterValue(2.55)); - get_parameter("z_score", z_score_); + get_parameter("cost_change_z_score", cost_change_z_score_); get_parameter("planner_plugins", planner_ids_); if (planner_ids_ == default_ids_) { @@ -679,8 +679,8 @@ void PlannerServer::isPathValid( // TODO try single tail or tune strictness? Parameterize? // 1.65 95% single tail; 2.55 for 99% dual, 2.33 99% single; 90% 1.65 dual, 90% 1.2 single. - if (z > z_score_) { // critical z score for 95% level - RCLCPP_DEBUG_STREAM(get_logger(), "Z-test triggered new global plan. The z score was: " << z << "and the threshold was" << z_score_); + if (z > cost_change_z_score_) { // critical z score for 95% level + RCLCPP_DEBUG_STREAM(get_logger(), "Z-test triggered new global plan. The z score was: " << z << "and the threshold was" << cost_change_z_score_); response->is_valid = false; } } From 27adc5582bc5ba6bbb3961740107e772924fa3da Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Dec 2022 13:25:44 -0500 Subject: [PATCH 131/131] initial tests for path validator --- nav2_planner/CMakeLists.txt | 8 ++ .../include/nav2_planner/path_validator.hpp | 64 +++++++++++ nav2_planner/src/path_validator.cpp | 107 ++++++++++++++++++ nav2_planner/test/CMakeLists.txt | 11 ++ nav2_planner/test/test_path_validator.cpp | 51 +++++++++ 5 files changed, 241 insertions(+) create mode 100644 nav2_planner/include/nav2_planner/path_validator.hpp create mode 100644 nav2_planner/src/path_validator.cpp create mode 100644 nav2_planner/test/test_path_validator.cpp diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt index 197150837c..5736e3fb84 100644 --- a/nav2_planner/CMakeLists.txt +++ b/nav2_planner/CMakeLists.txt @@ -27,6 +27,7 @@ include_directories( set(executable_name planner_server) set(library_name ${executable_name}_core) +set(path_validator path_validator_lib) set(dependencies rclcpp @@ -54,6 +55,13 @@ ament_target_dependencies(${library_name} ${dependencies} ) +add_library(${path_validator} SHARED + src/path_validator.cpp) + +ament_target_dependencies(${path_validator} + ${dependencies} +) + add_executable(${executable_name} src/main.cpp ) diff --git a/nav2_planner/include/nav2_planner/path_validator.hpp b/nav2_planner/include/nav2_planner/path_validator.hpp new file mode 100644 index 0000000000..d654a193b8 --- /dev/null +++ b/nav2_planner/include/nav2_planner/path_validator.hpp @@ -0,0 +1,64 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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_WS_SRC_NAVIGATION2_NAV2_PLANNER_INCLUDE_NAV2_PLANNER_PATH_VALIDATOR_HPP_ +#define NAV2_WS_SRC_NAVIGATION2_NAV2_PLANNER_INCLUDE_NAV2_PLANNER_PATH_VALIDATOR_HPP_ + +#include "nav2_msgs/msg/path_with_cost.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_planner +{ +using Path = nav2_msgs::msg::PathWithCost; +using Pose = geometry_msgs::msg::PoseStamped; +using PathIndex = unsigned int; +using Point = geometry_msgs::msg::Point; +using Costs = std::vector; + +class [[maybe_unused]] PathValidator +{ +public: + PathValidator() = default; + /** + * @brief check if the path is valid + * \param path the path to check + * \param cost_change_z_score + * \param consider_cost_change true, change in cost in considered + * \return true path is valid + */ + bool check(const Path & path, float cost_change_z_score, bool consider_cost_change); + + /** + * @brief determine if the path is lethal + * @param path the path to check + * @return true, if path is lethal + */ + bool isThisALethal(const Path & path); + + /** + * @brief find the path index closest to the robot + * @param path the path to evalutate + * \param pose the current pose of the robotic platform + * \return PathIndex, the index of the pose closest to the robot + */ + PathIndex truncate(const Path &path, const Pose &pose); + + bool isCostChangeSignifant(const Costs &original_costs, + const Costs ¤t_costs, + float z_score); +}; +} + +#endif //NAV2_WS_SRC_NAVIGATION2_NAV2_PLANNER_INCLUDE_NAV2_PLANNER_PATH_VALIDATOR_HPP_ diff --git a/nav2_planner/src/path_validator.cpp b/nav2_planner/src/path_validator.cpp new file mode 100644 index 0000000000..7df2adbcac --- /dev/null +++ b/nav2_planner/src/path_validator.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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_planner/path_validator.hpp" +#include "nav2_util/geometry_utils.hpp" + + +namespace nav2_planner +{ + +bool PathValidator::check(const Path &, float, bool) +{ + return true; +} + +PathIndex PathValidator::truncate(const Path & path, const Pose & current_pose) +{ + PathIndex closest_point_index = 0; + float current_distance = std::numeric_limits::max(); + float closest_distance = current_distance; + geometry_msgs::msg::Point current_point = current_pose.pose.position; + for (unsigned int i = 0; i != path.poses.size(); ++i) { + Point path_point = path.poses[i].pose.position; + + current_distance = nav2_util::geometry_utils::euclidean_distance( + current_point, + path_point); + + if (current_distance < closest_distance) { + closest_point_index = i; + closest_distance = current_distance; + } + } + return closest_point_index; +} +bool PathValidator::isCostChangeSignifant( + const Costs & original_costs, + const Costs & current_costs, + float z_score) +{ + if (original_costs.size() != current_costs.size()) { + return true; + } + + float original_costs_sum = std::accumulate(original_costs.begin(), original_costs.end(), 0.0f); + float original_costs_mean = original_costs_sum / static_cast(original_costs.size()); + + std::cout << "Original Mean: " << original_costs_mean << std::endl; + float current_costs_sum = std::accumulate(current_costs.begin(), current_costs.end(), 0.0f); + float current_costs_mean = current_costs_sum / static_cast(current_costs.size()); + + std::cout << "Current Mean: " << current_costs_mean << std::endl; + float variance_original = 0; + for (const auto & cost : original_costs) { + variance_original += (cost - original_costs_mean) * (cost - original_costs_mean); + } + variance_original /= static_cast(original_costs.size() - 1); + + + float variance_current = 0; + for (const auto & cost : current_costs) { + variance_current += (cost - current_costs_mean) * (cost - current_costs_mean); + } + variance_current /= static_cast(current_costs.size() - 1); + + // Conduct a two sample Z-test, with the null hypothesis is that both path cost samples + // come from the same distribution (e.g. there is not a statistically significant change) + // Thus, the difference in population mean is 0 and the sample sizes are the same + float numerator = current_costs_mean - original_costs_mean; + float denominator = std::sqrt((variance_current + variance_original) / + static_cast(current_costs.size())); + + // if the variance of the current and original costs are 0 + if (denominator <= 0) { + if ( std::fabs(current_costs_mean - original_costs_mean) > 10) { + return true; + } else { + return false; + } + } + + float cost_change_z_score = numerator / denominator; + + std::cout << "Cost Change z score: " << cost_change_z_score << std::endl; + if (cost_change_z_score > z_score) { + return false; + } + return true; +} + +bool isThisALethal(const Path) +{ + return true; +} +} diff --git a/nav2_planner/test/CMakeLists.txt b/nav2_planner/test/CMakeLists.txt index d415d906ef..200feb2274 100644 --- a/nav2_planner/test/CMakeLists.txt +++ b/nav2_planner/test/CMakeLists.txt @@ -8,3 +8,14 @@ ament_target_dependencies(test_dynamic_parameters target_link_libraries(test_dynamic_parameters ${library_name} ) + +#Test path validator +ament_add_gtest(test_path_validator + test_path_validator.cpp +) +ament_target_dependencies(test_path_validator + ${dependencies} +) +target_link_libraries(test_path_validator + ${path_validator} +) \ No newline at end of file diff --git a/nav2_planner/test/test_path_validator.cpp b/nav2_planner/test/test_path_validator.cpp new file mode 100644 index 0000000000..908c78106e --- /dev/null +++ b/nav2_planner/test/test_path_validator.cpp @@ -0,0 +1,51 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_planner/path_validator.hpp" + +using PathValidator = nav2_planner::PathValidator; +using Costs = nav2_planner::Costs; + +class RclCppFixture +{ + public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(PathValidatorTest, is_cost_change_signifant) +{ + PathValidator path_validator; + + // free space test + Costs original_costs(30); + std::fill(original_costs.begin(), original_costs.end(), 0); + + Costs current_costs(30); + std::fill( current_costs.begin(), current_costs.end(), 0); + float z_score = 2.55; + + bool is_cost_change_significant = + path_validator.isCostChangeSignifant(original_costs, current_costs, z_score); + + EXPECT_TRUE(is_cost_change_significant); +}