From 61161674f6fd276bb607c4072cdbdd9dfec0e29e Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 21 Nov 2022 18:12:36 +0900 Subject: [PATCH 01/48] first commit Signed-off-by: kminoda --- localization/ndt_scan_matcher/CMakeLists.txt | 1 + .../include/ndt_scan_matcher/map_module.hpp | 2 +- .../ndt_scan_matcher/map_update_module.hpp | 103 +++++++ .../ndt_scan_matcher_core.hpp | 2 +- .../pose_initialization_module.hpp | 2 +- localization/ndt_scan_matcher/package.xml | 1 + .../src/map_update_module.cpp | 289 ++++++++++++++++++ .../src/ndt_scan_matcher_core.cpp | 4 +- 8 files changed, 399 insertions(+), 5 deletions(-) create mode 100644 localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp create mode 100644 localization/ndt_scan_matcher/src/map_update_module.cpp diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 0f5b761baf6f0..0a04f1bb60466 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -34,6 +34,7 @@ ament_auto_add_executable(ndt_scan_matcher src/tf2_listener_module.cpp src/map_module.cpp src/pose_initialization_module.cpp + src/map_update_module.cpp ) link_directories(${PCL_LIBRARY_DIRS}) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp index 871ff2760acf7..1801f3d6dd5bf 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp new file mode 100644 index 0000000000000..7688a35cde1d3 --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -0,0 +1,103 @@ +// Copyright 2022 Autoware Foundation +// +// 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 NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ + +#include "ndt_scan_matcher/particle.hpp" +#include "ndt_scan_matcher/debug.hpp" +#include "ndt_scan_matcher/tf2_listener_module.hpp" +#include "ndt_scan_matcher/util_func.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +class MapUpdateModule { + using PointSource = pcl::PointXYZ; + using PointTarget = pcl::PointXYZ; + using NormalDistributionsTransform = + pclomp::NormalDistributionsTransform; + +public: + MapUpdateModule( + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, + std::shared_ptr ndt_ptr, + std::shared_ptr tf2_listener_module, + std::string map_frame, + rclcpp::CallbackGroup::SharedPtr main_callback_group, + rclcpp::CallbackGroup::SharedPtr map_callback_group, + std::shared_ptr> state_ptr); + +private: + void service_ndt_align( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); + void callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr); + void map_update_timer_callback(); + + void update_ndt(const std::vector & maps_to_add, + const std::vector & map_ids_to_remove); + void update_map(const geometry_msgs::msg::Point & position); + bool should_update_map(const geometry_msgs::msg::Point & position) const; + void publish_partial_pcd_map(); + geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( + const std::shared_ptr & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); + void publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr); + + rclcpp::Publisher::SharedPtr loaded_pcd_pub_; + rclcpp::Publisher::SharedPtr + ndt_monte_carlo_initial_pose_marker_pub_; + rclcpp::Publisher::SharedPtr sensor_aligned_pose_pub_; + + rclcpp::Service::SharedPtr service_; + rclcpp::Client::SharedPtr pcd_loader_client_; + rclcpp::TimerBase::SharedPtr map_update_timer_; + + rclcpp::Subscription::SharedPtr ekf_odom_sub_; + + rclcpp::Subscription::SharedPtr map_points_sub_; + + std::shared_ptr ndt_ptr_; + std::mutex * ndt_ptr_mutex_; + std::string map_frame_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr tf2_listener_module_; + std::shared_ptr> state_ptr_; + + int initial_estimate_particles_num_; + std::mutex pcd_loader_client_mutex_; + std::mutex initial_pose_pcd_loader_client_mutex_; + std::condition_variable condition_; + bool value_ready_ = false; + geometry_msgs::msg::Point::SharedPtr last_update_position_ptr_; + double dml_update_map_distance_; + double dml_loading_radius_; + geometry_msgs::msg::Point::SharedPtr current_position_ptr_; +}; + +#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ \ No newline at end of file diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index ed791bd6cda16..0df5f3521f1f5 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp index 7745ecf8a2148..d08e9f5fd3055 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 13b0f186ceb98..581879322caf3 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_map_msgs diagnostic_msgs fmt geometry_msgs diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp new file mode 100644 index 0000000000000..bd23ad63f2324 --- /dev/null +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -0,0 +1,289 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "ndt_scan_matcher/map_update_module.hpp" + +template +double norm_xy(const T p1, const U p2) +{ + double dx = p1.x - p2.x; + double dy = p1.y - p2.y; + return std::sqrt(dx * dx + dy * dy); +} + +MapUpdateModule::MapUpdateModule( + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, + std::shared_ptr ndt_ptr, + std::shared_ptr tf2_listener_module, + std::string map_frame, + rclcpp::CallbackGroup::SharedPtr main_callback_group, + rclcpp::CallbackGroup::SharedPtr map_callback_group, + std::shared_ptr> state_ptr) +: ndt_ptr_(ndt_ptr), + ndt_ptr_mutex_(ndt_ptr_mutex), + map_frame_(map_frame), + logger_(node->get_logger()), + clock_(node->get_clock()), + tf2_listener_module_(tf2_listener_module), + state_ptr_(state_ptr), + dml_update_map_distance_(node->declare_parameter("dml_update_map_distance")), + dml_loading_radius_(node->declare_parameter("dml_loading_radius")) +{ + initial_estimate_particles_num_ = + node->declare_parameter("initial_estimate_particles_num"); + + sensor_aligned_pose_pub_ = + node->create_publisher("debug/monte_carlo_points_aligned", 10); + ndt_monte_carlo_initial_pose_marker_pub_ = + node->create_publisher( + "monte_carlo_initial_pose_marker", 10); + + auto main_sub_opt = rclcpp::SubscriptionOptions(); + main_sub_opt.callback_group = main_callback_group; + + ekf_odom_sub_ = + node->create_subscription( + "ekf_odom", 100, + std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), main_sub_opt); + + loaded_pcd_pub_ = + node->create_publisher("debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); + + service_ = node->create_service( + "ndt_align_srv", + std::bind(&MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group); + + pcd_loader_client_ = node->create_client( + "pcd_loader_service", rmw_qos_profile_services_default); + while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO(logger_, "Waiting for pcd loader service..."); + } + last_update_position_ptr_ = nullptr; + current_position_ptr_ = nullptr; + + double map_update_dt = 1.0; + auto period_ns = + std::chrono::duration_cast(std::chrono::duration(map_update_dt)); + map_update_timer_ = rclcpp::create_timer( + node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), map_callback_group); +} + +void MapUpdateModule::service_ndt_align( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) +{ + // get TF from pose_frame to map_frame + auto TF_pose_to_map_ptr = std::make_shared(); + tf2_listener_module_->get_transform( + clock_->now(), map_frame_, req->pose_with_covariance.header.frame_id, TF_pose_to_map_ptr); + + // transform pose_frame to map_frame + const auto mapTF_initial_pose_msg = transform(req->pose_with_covariance, *TF_pose_to_map_ptr); + update_map(mapTF_initial_pose_msg.pose.pose.position); + + if (ndt_ptr_->getInputTarget() == nullptr) { + res->success = false; + RCLCPP_WARN(logger_, "No InputTarget"); + return; + } + + if (ndt_ptr_->getInputSource() == nullptr) { + res->success = false; + RCLCPP_WARN(logger_, "No InputSource"); + return; + } + + // mutex Map + std::lock_guard lock(*ndt_ptr_mutex_); + + (*state_ptr_)["state"] = "Aligning"; + res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, mapTF_initial_pose_msg); + (*state_ptr_)["state"] = "Sleeping"; + res->success = true; + res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; + + last_update_position_ptr_ = std::make_shared(res->pose_with_covariance.pose.pose.position); +} + +void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) +{ + current_position_ptr_ = std::make_shared(odom_ptr->pose.pose.position); + + if (last_update_position_ptr_ == nullptr) {return;} + double distance = norm_xy(*current_position_ptr_, *last_update_position_ptr_); + double LIDAR_CROP_DISTANCE = 100; // TODO (koji minoda): parametrize before merge!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + if (distance + LIDAR_CROP_DISTANCE > dml_loading_radius_) { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); + } +} + +void MapUpdateModule::map_update_timer_callback() +{ + if (current_position_ptr_ == nullptr) return; + if (last_update_position_ptr_ == nullptr) return; + + // continue only if we should update the map + if (should_update_map(*current_position_ptr_)) + { + RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)"); + update_map(*current_position_ptr_); + *last_update_position_ptr_ = *current_position_ptr_; + } +} + +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const +{ + if (last_update_position_ptr_ == nullptr) return false; + double distance = norm_xy(position, *last_update_position_ptr_); + return distance > dml_update_map_distance_; +} + +void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) +{ + // create a loading request with mode = 1 + auto request = std::make_shared(); + request->area.center = position; + request->area.radius = dml_loading_radius_; + request->cached_ids = ndt_ptr_->getCurrentMapIDs(); + + // send a request to map_loader + auto result{pcd_loader_client_->async_send_request( + request, + [this](const rclcpp::Client::SharedFuture + response) { + (void)response; + std::lock_guard lock{pcd_loader_client_mutex_}; + value_ready_ = true; + condition_.notify_all(); + })}; + std::unique_lock lock{pcd_loader_client_mutex_}; + condition_.wait(lock, [this]() { return value_ready_; }); + + update_ndt(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); +} + +void MapUpdateModule::update_ndt( + const std::vector & maps_to_add, + const std::vector & map_ids_to_remove) +{ + RCLCPP_INFO(logger_, "Update map (Add: %d, Remove: %d)", int(maps_to_add.size()), int(map_ids_to_remove.size())); + if ((int(maps_to_add.size()) == 0) & (int(map_ids_to_remove.size()) == 0)) { + RCLCPP_INFO(logger_, "Skip map update"); + return; + } + const auto exe_start_time = std::chrono::system_clock::now(); + + NormalDistributionsTransform backup_ndt = *ndt_ptr_; + backup_ndt.setInputSource(ndt_ptr_->getInputSource()); + + // Add pcd + for (const auto & map_to_add: maps_to_add) { + pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); + pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); + backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); + } + + // Remove pcd + for (const std::string map_id_to_remove: map_ids_to_remove) { + backup_ndt.removeTarget(map_id_to_remove); + } + + backup_ndt.createVoxelKdtree(); + + const auto exe_end_time = std::chrono::system_clock::now(); + const double exe_time = std::chrono::duration_cast(exe_end_time - exe_start_time).count() / 1000.0; + RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %f [ms]", exe_time); + + // swap + (*ndt_ptr_mutex_).lock(); + *ndt_ptr_ = backup_ndt; + (*ndt_ptr_mutex_).unlock(); + + publish_partial_pcd_map(); + + // // TODO (koji minoda): Any way to simplify this copy part? + // using T = NormalDistributionsTransformOMPMultiVoxel; + // backup_ndt_ptr_ = std::make_shared(*std::dynamic_pointer_cast(ndt_ptr_)); +} + +geometry_msgs::msg::PoseWithCovarianceStamped MapUpdateModule::align_using_monte_carlo( + const std::shared_ptr & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) +{ + if (ndt_ptr->getInputTarget() == nullptr || ndt_ptr->getInputSource() == nullptr) { + RCLCPP_WARN(logger_, "No Map or Sensor PointCloud"); + return geometry_msgs::msg::PoseWithCovarianceStamped(); + } + + // generateParticle + const auto initial_poses = + create_random_pose_array(initial_pose_with_cov, initial_estimate_particles_num_); + + std::vector particle_array; + auto output_cloud = std::make_shared>(); + + for (unsigned int i = 0; i < initial_poses.size(); i++) { + const auto & initial_pose = initial_poses[i]; + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); + ndt_ptr->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr->getResult(); + + Particle particle( + initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, + ndt_result.iteration_num); + particle_array.push_back(particle); + const auto marker_array = make_debug_markers( + clock_->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, + i); + ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + + auto sensor_points_mapTF_ptr = std::make_shared>(); + pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); + publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr); + } + + auto best_particle_ptr = std::max_element( + std::begin(particle_array), std::end(particle_array), + [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); + + geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; + result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; + result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; + + return result_pose_with_cov_msg; +} + +void MapUpdateModule::publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr) +{ + sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; + pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); + sensor_points_mapTF_msg.header.stamp = sensor_ros_time; + sensor_points_mapTF_msg.header.frame_id = frame_id; + sensor_aligned_pose_pub_->publish(sensor_points_mapTF_msg); +} + +void MapUpdateModule::publish_partial_pcd_map() +{ + pcl::PointCloud map_pcl = ndt_ptr_->getVoxelPCD(); + + sensor_msgs::msg::PointCloud2 map_msg; + pcl::toROSMsg(map_pcl, map_msg); + map_msg.header.frame_id = "map"; + + loaded_pcd_pub_->publish(map_msg); +} \ No newline at end of file diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 4ae16e6999624..415cb038bfb9a 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -113,8 +113,8 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.step_size = this->declare_parameter("step_size"); ndt_params.resolution = this->declare_parameter("resolution"); ndt_params.max_iterations = this->declare_parameter("max_iterations"); - int search_method = this->declare_parameter("neighborhood_search_method"); - ndt_params.search_method = static_cast(search_method); + // int search_method = this->declare_parameter("neighborhood_search_method"); + // ndt_params.search_method = static_cast(search_method); ndt_params.num_threads = this->declare_parameter("num_threads"); ndt_params.num_threads = std::max(ndt_params.num_threads, 1); ndt_params.regularization_scale_factor = From fc06c349c71ee9977dbcfa84325ab770dd692e8a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 21 Nov 2022 09:15:06 +0000 Subject: [PATCH 02/48] ci(pre-commit): autofix --- .../include/ndt_scan_matcher/map_module.hpp | 2 +- .../ndt_scan_matcher/map_update_module.hpp | 26 +++++---- .../ndt_scan_matcher_core.hpp | 2 +- .../pose_initialization_module.hpp | 2 +- .../src/map_update_module.cpp | 58 ++++++++++--------- 5 files changed, 49 insertions(+), 41 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp index 1801f3d6dd5bf..f95a0040586e8 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -19,9 +19,9 @@ #include +#include #include #include -#include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 7688a35cde1d3..a7068f618d4ad 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -15,25 +15,26 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "ndt_scan_matcher/particle.hpp" #include "ndt_scan_matcher/debug.hpp" +#include "ndt_scan_matcher/particle.hpp" #include "ndt_scan_matcher/tf2_listener_module.hpp" #include "ndt_scan_matcher/util_func.hpp" #include +#include + +#include +#include #include #include #include -#include -#include - -#include -#include #include +#include #include -class MapUpdateModule { +class MapUpdateModule +{ using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = @@ -43,8 +44,7 @@ class MapUpdateModule { MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, std::shared_ptr ndt_ptr, - std::shared_ptr tf2_listener_module, - std::string map_frame, + std::shared_ptr tf2_listener_module, std::string map_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group, rclcpp::CallbackGroup::SharedPtr map_callback_group, std::shared_ptr> state_ptr); @@ -56,7 +56,8 @@ class MapUpdateModule { void callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr); void map_update_timer_callback(); - void update_ndt(const std::vector & maps_to_add, + void update_ndt( + const std::vector & maps_to_add, const std::vector & map_ids_to_remove); void update_map(const geometry_msgs::msg::Point & position); bool should_update_map(const geometry_msgs::msg::Point & position) const; @@ -74,7 +75,8 @@ class MapUpdateModule { rclcpp::Publisher::SharedPtr sensor_aligned_pose_pub_; rclcpp::Service::SharedPtr service_; - rclcpp::Client::SharedPtr pcd_loader_client_; + rclcpp::Client::SharedPtr + pcd_loader_client_; rclcpp::TimerBase::SharedPtr map_update_timer_; rclcpp::Subscription::SharedPtr ekf_odom_sub_; @@ -100,4 +102,4 @@ class MapUpdateModule { geometry_msgs::msg::Point::SharedPtr current_position_ptr_; }; -#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ \ No newline at end of file +#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 0df5f3521f1f5..3dc70e9011cbd 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -34,8 +34,8 @@ #include #include -#include #include +#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp index d08e9f5fd3055..7d47a1cc13838 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp @@ -27,8 +27,8 @@ #include #include -#include #include +#include #include #include diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index bd23ad63f2324..56767491e98d6 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -25,8 +25,7 @@ double norm_xy(const T p1, const U p2) MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, std::shared_ptr ndt_ptr, - std::shared_ptr tf2_listener_module, - std::string map_frame, + std::shared_ptr tf2_listener_module, std::string map_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group, rclcpp::CallbackGroup::SharedPtr map_callback_group, std::shared_ptr> state_ptr) @@ -40,8 +39,7 @@ MapUpdateModule::MapUpdateModule( dml_update_map_distance_(node->declare_parameter("dml_update_map_distance")), dml_loading_radius_(node->declare_parameter("dml_loading_radius")) { - initial_estimate_particles_num_ = - node->declare_parameter("initial_estimate_particles_num"); + initial_estimate_particles_num_ = node->declare_parameter("initial_estimate_particles_num"); sensor_aligned_pose_pub_ = node->create_publisher("debug/monte_carlo_points_aligned", 10); @@ -52,17 +50,17 @@ MapUpdateModule::MapUpdateModule( auto main_sub_opt = rclcpp::SubscriptionOptions(); main_sub_opt.callback_group = main_callback_group; - ekf_odom_sub_ = - node->create_subscription( - "ekf_odom", 100, - std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), main_sub_opt); + ekf_odom_sub_ = node->create_subscription( + "ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), + main_sub_opt); - loaded_pcd_pub_ = - node->create_publisher("debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); + loaded_pcd_pub_ = node->create_publisher( + "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); service_ = node->create_service( "ndt_align_srv", - std::bind(&MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), + std::bind( + &MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group); pcd_loader_client_ = node->create_client( @@ -74,10 +72,11 @@ MapUpdateModule::MapUpdateModule( current_position_ptr_ = nullptr; double map_update_dt = 1.0; - auto period_ns = - std::chrono::duration_cast(std::chrono::duration(map_update_dt)); + auto period_ns = std::chrono::duration_cast( + std::chrono::duration(map_update_dt)); map_update_timer_ = rclcpp::create_timer( - node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), map_callback_group); + node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), + map_callback_group); } void MapUpdateModule::service_ndt_align( @@ -114,16 +113,20 @@ void MapUpdateModule::service_ndt_align( res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; - last_update_position_ptr_ = std::make_shared(res->pose_with_covariance.pose.pose.position); + last_update_position_ptr_ = + std::make_shared(res->pose_with_covariance.pose.pose.position); } void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) { current_position_ptr_ = std::make_shared(odom_ptr->pose.pose.position); - - if (last_update_position_ptr_ == nullptr) {return;} + + if (last_update_position_ptr_ == nullptr) { + return; + } double distance = norm_xy(*current_position_ptr_, *last_update_position_ptr_); - double LIDAR_CROP_DISTANCE = 100; // TODO (koji minoda): parametrize before merge!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + double LIDAR_CROP_DISTANCE = + 100; // TODO (koji minoda): parametrize before merge!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! if (distance + LIDAR_CROP_DISTANCE > dml_loading_radius_) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); } @@ -135,8 +138,7 @@ void MapUpdateModule::map_update_timer_callback() if (last_update_position_ptr_ == nullptr) return; // continue only if we should update the map - if (should_update_map(*current_position_ptr_)) - { + if (should_update_map(*current_position_ptr_)) { RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)"); update_map(*current_position_ptr_); *last_update_position_ptr_ = *current_position_ptr_; @@ -162,7 +164,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) auto result{pcd_loader_client_->async_send_request( request, [this](const rclcpp::Client::SharedFuture - response) { + response) { (void)response; std::lock_guard lock{pcd_loader_client_mutex_}; value_ready_ = true; @@ -178,7 +180,9 @@ void MapUpdateModule::update_ndt( const std::vector & maps_to_add, const std::vector & map_ids_to_remove) { - RCLCPP_INFO(logger_, "Update map (Add: %d, Remove: %d)", int(maps_to_add.size()), int(map_ids_to_remove.size())); + RCLCPP_INFO( + logger_, "Update map (Add: %d, Remove: %d)", int(maps_to_add.size()), + int(map_ids_to_remove.size())); if ((int(maps_to_add.size()) == 0) & (int(map_ids_to_remove.size()) == 0)) { RCLCPP_INFO(logger_, "Skip map update"); return; @@ -189,21 +193,23 @@ void MapUpdateModule::update_ndt( backup_ndt.setInputSource(ndt_ptr_->getInputSource()); // Add pcd - for (const auto & map_to_add: maps_to_add) { + for (const auto & map_to_add : maps_to_add) { pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); } // Remove pcd - for (const std::string map_id_to_remove: map_ids_to_remove) { + for (const std::string map_id_to_remove : map_ids_to_remove) { backup_ndt.removeTarget(map_id_to_remove); } backup_ndt.createVoxelKdtree(); const auto exe_end_time = std::chrono::system_clock::now(); - const double exe_time = std::chrono::duration_cast(exe_end_time - exe_start_time).count() / 1000.0; + const double exe_time = + std::chrono::duration_cast(exe_end_time - exe_start_time).count() / + 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %f [ms]", exe_time); // swap @@ -286,4 +292,4 @@ void MapUpdateModule::publish_partial_pcd_map() map_msg.header.frame_id = "map"; loaded_pcd_pub_->publish(map_msg); -} \ No newline at end of file +} From 76ef35097c02f258860f3d24d4fc355779d5e492 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Nov 2022 09:14:38 +0900 Subject: [PATCH 03/48] import map update module in core Signed-off-by: kminoda --- .../ndt_scan_matcher/ndt_scan_matcher_core.hpp | 4 ++++ .../src/ndt_scan_matcher_core.cpp | 18 ++++++++++++------ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 0df5f3521f1f5..68996f1c07ca5 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -19,6 +19,7 @@ #include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/pose_initialization_module.hpp" +#include "ndt_scan_matcher/map_update_module.hpp" #include "ndt_scan_matcher/tf2_listener_module.hpp" #include @@ -195,6 +196,9 @@ class NDTScanMatcher : public rclcpp::Node std::shared_ptr tf2_listener_module_; std::unique_ptr map_module_; std::unique_ptr pose_init_module_; + std::unique_ptr map_update_module_; + + bool use_dynamic_map_loading_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 415cb038bfb9a..c9eb72802ba68 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -95,6 +95,8 @@ NDTScanMatcher::NDTScanMatcher() oscillation_threshold_(10), regularization_enabled_(declare_parameter("regularization_enabled", false)) { + use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); + (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; @@ -113,8 +115,6 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.step_size = this->declare_parameter("step_size"); ndt_params.resolution = this->declare_parameter("resolution"); ndt_params.max_iterations = this->declare_parameter("max_iterations"); - // int search_method = this->declare_parameter("neighborhood_search_method"); - // ndt_params.search_method = static_cast(search_method); ndt_params.num_threads = this->declare_parameter("num_threads"); ndt_params.num_threads = std::max(ndt_params.num_threads, 1); ndt_params.regularization_scale_factor = @@ -211,10 +211,16 @@ NDTScanMatcher::NDTScanMatcher() diagnostic_thread_.detach(); tf2_listener_module_ = std::make_shared(this); - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); - pose_init_module_ = std::make_unique( - this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, - state_ptr_); + + if (use_dynamic_map_loading_) { + map_update_module_ = std::make_unique(this, &ndt_ptr_mtsx_, ndt_ptr_, tf2_listener_module_, + map_frame_, main_callback_group, map_callback_group, state_ptr); + } else { + map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); + pose_init_module_ = std::make_unique( + this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, + state_ptr_); + } } void NDTScanMatcher::timer_diagnostic() From 36c2c7b3c3d25bce06f31321d82a856de8ef2f7d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 22 Nov 2022 00:17:09 +0000 Subject: [PATCH 04/48] ci(pre-commit): autofix --- .../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp | 2 +- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index c5a2b88795012..b4e2f2ac6828c 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -18,8 +18,8 @@ #define FMT_HEADER_ONLY #include "ndt_scan_matcher/map_module.hpp" -#include "ndt_scan_matcher/pose_initialization_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" +#include "ndt_scan_matcher/pose_initialization_module.hpp" #include "ndt_scan_matcher/tf2_listener_module.hpp" #include diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index c9eb72802ba68..dc812d29f168c 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -213,8 +213,9 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_module_ = std::make_shared(this); if (use_dynamic_map_loading_) { - map_update_module_ = std::make_unique(this, &ndt_ptr_mtsx_, ndt_ptr_, tf2_listener_module_, - map_frame_, main_callback_group, map_callback_group, state_ptr); + map_update_module_ = std::make_unique( + this, &ndt_ptr_mtsx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, + map_callback_group, state_ptr); } else { map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); pose_init_module_ = std::make_unique( From cd00a819036c8f07e26c76e4ae4207cefae48c0f Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Nov 2022 10:20:57 +0900 Subject: [PATCH 05/48] minor fixes. Now map update module launches!!! Signed-off-by: kminoda --- .../config/ndt_scan_matcher.param.yaml | 14 +++++++++----- .../config/ndt_scan_matcher.param.yaml | 12 ++++++++---- .../include/ndt_scan_matcher/map_update_module.hpp | 2 +- .../ndt_scan_matcher/src/map_update_module.cpp | 7 ++++--- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 4 ++-- 5 files changed, 24 insertions(+), 15 deletions(-) diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml index 3b7f113c43a94..f7902676ea74a 100644 --- a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml +++ b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # Use dynamic map loading + use_dynamic_map_loading: true # Vehicle reference frame base_frame: "base_link" @@ -41,10 +43,6 @@ # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 - # neighborhood search method - # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - neighborhood_search_method: 0 - # Number of threads used for parallel computing num_threads: 4 @@ -63,5 +61,11 @@ # Regularization switch regularization_enabled: false - # regularization scale factor + # Regularization scale factor regularization_scale_factor: 0.01 + + # Dynamic map loading distance + dml_update_map_distance: 20.0 + + # Dynamic map loading loading radius + dml_loading_radius: 120.0 diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index decc41795f496..f7902676ea74a 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # Use dynamic map loading + use_dynamic_map_loading: true # Vehicle reference frame base_frame: "base_link" @@ -41,10 +43,6 @@ # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 - # neighborhood search method - # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - neighborhood_search_method: 0 - # Number of threads used for parallel computing num_threads: 4 @@ -65,3 +63,9 @@ # Regularization scale factor regularization_scale_factor: 0.01 + + # Dynamic map loading distance + dml_update_map_distance: 20.0 + + # Dynamic map loading loading radius + dml_loading_radius: 120.0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index a7068f618d4ad..4e9eb09fa213f 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -46,7 +46,6 @@ class MapUpdateModule std::shared_ptr ndt_ptr, std::shared_ptr tf2_listener_module, std::string map_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group, - rclcpp::CallbackGroup::SharedPtr map_callback_group, std::shared_ptr> state_ptr); private: @@ -82,6 +81,7 @@ class MapUpdateModule rclcpp::Subscription::SharedPtr ekf_odom_sub_; rclcpp::Subscription::SharedPtr map_points_sub_; + rclcpp::CallbackGroup::SharedPtr map_callback_group_; std::shared_ptr ndt_ptr_; std::mutex * ndt_ptr_mutex_; diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 56767491e98d6..42d1bc68a3824 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -27,7 +27,6 @@ MapUpdateModule::MapUpdateModule( std::shared_ptr ndt_ptr, std::shared_ptr tf2_listener_module, std::string map_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group, - rclcpp::CallbackGroup::SharedPtr map_callback_group, std::shared_ptr> state_ptr) : ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex), @@ -50,6 +49,8 @@ MapUpdateModule::MapUpdateModule( auto main_sub_opt = rclcpp::SubscriptionOptions(); main_sub_opt.callback_group = main_callback_group; + map_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + ekf_odom_sub_ = node->create_subscription( "ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), main_sub_opt); @@ -61,7 +62,7 @@ MapUpdateModule::MapUpdateModule( "ndt_align_srv", std::bind( &MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), - rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group); + rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_); pcd_loader_client_ = node->create_client( "pcd_loader_service", rmw_qos_profile_services_default); @@ -76,7 +77,7 @@ MapUpdateModule::MapUpdateModule( std::chrono::duration(map_update_dt)); map_update_timer_ = rclcpp::create_timer( node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), - map_callback_group); + map_callback_group_); } void MapUpdateModule::service_ndt_align( diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index c9eb72802ba68..9313f9d8c4dbe 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -213,8 +213,8 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_module_ = std::make_shared(this); if (use_dynamic_map_loading_) { - map_update_module_ = std::make_unique(this, &ndt_ptr_mtsx_, ndt_ptr_, tf2_listener_module_, - map_frame_, main_callback_group, map_callback_group, state_ptr); + map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, + map_frame_, main_callback_group, state_ptr_); } else { map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); pose_init_module_ = std::make_unique( From 6dd63476afd0d359b0b34d1075ffd7582e6ea5da Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 22 Nov 2022 01:23:26 +0000 Subject: [PATCH 06/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 9313f9d8c4dbe..f56e2950c747a 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -213,8 +213,9 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_module_ = std::make_shared(this); if (use_dynamic_map_loading_) { - map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, - map_frame_, main_callback_group, state_ptr_); + map_update_module_ = std::make_unique( + this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, + state_ptr_); } else { map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); pose_init_module_ = std::make_unique( From b2638209069912497d9843e025a62ff2eb5b0d0b Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 14 Dec 2022 10:17:25 +0900 Subject: [PATCH 07/48] debugged Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_update_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 42d1bc68a3824..c22f3066d4159 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -201,7 +201,7 @@ void MapUpdateModule::update_ndt( } // Remove pcd - for (const std::string map_id_to_remove : map_ids_to_remove) { + for (const std::string & map_id_to_remove : map_ids_to_remove) { backup_ndt.removeTarget(map_id_to_remove); } From e3f83d6f8f511896b44c3a60baa81b98e7eb9d05 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 16 Dec 2022 13:41:04 +0900 Subject: [PATCH 08/48] revert unnecessary fix Signed-off-by: kminoda --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index f7902676ea74a..57e36cac6d726 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -61,7 +61,7 @@ # Regularization switch regularization_enabled: false - # Regularization scale factor + # regularization scale factor regularization_scale_factor: 0.01 # Dynamic map loading distance From c702184a0a86a2687ce5463f4224e151f7d4df30 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 16 Dec 2022 13:44:51 +0900 Subject: [PATCH 09/48] minor fixes Signed-off-by: kminoda --- .../ndt_scan_matcher/src/map_update_module.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index c22f3066d4159..53dcc8d68b845 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -135,7 +135,10 @@ void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr void MapUpdateModule::map_update_timer_callback() { - if (current_position_ptr_ == nullptr) return; + if (current_position_ptr_ == nullptr) { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Cannot find the reference position for map update. Please check if the EKF odometry is provided to NDT."); + return; + } if (last_update_position_ptr_ == nullptr) return; // continue only if we should update the map @@ -190,6 +193,7 @@ void MapUpdateModule::update_ndt( } const auto exe_start_time = std::chrono::system_clock::now(); + // ToDo (kminoda): Here the NDT is copied during the new map loading phase, which should ideally be done beforehand. NormalDistributionsTransform backup_ndt = *ndt_ptr_; backup_ndt.setInputSource(ndt_ptr_->getInputSource()); @@ -219,10 +223,6 @@ void MapUpdateModule::update_ndt( (*ndt_ptr_mutex_).unlock(); publish_partial_pcd_map(); - - // // TODO (koji minoda): Any way to simplify this copy part? - // using T = NormalDistributionsTransformOMPMultiVoxel; - // backup_ndt_ptr_ = std::make_shared(*std::dynamic_pointer_cast(ndt_ptr_)); } geometry_msgs::msg::PoseWithCovarianceStamped MapUpdateModule::align_using_monte_carlo( From 638a62166129dadfe85791a0812d235f74da5d00 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 16 Dec 2022 13:49:32 +0900 Subject: [PATCH 10/48] update launch file Signed-off-by: kminoda --- .../ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 9e3d966bd770a..b442ac1b3d843 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -7,10 +7,13 @@ + + + @@ -23,6 +26,9 @@ + + + From 30198430222911baad4a339d418e82586f9d5a3a Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 16 Dec 2022 13:53:58 +0900 Subject: [PATCH 11/48] update comment Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_update_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 53dcc8d68b845..927055686c0d1 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -193,7 +193,6 @@ void MapUpdateModule::update_ndt( } const auto exe_start_time = std::chrono::system_clock::now(); - // ToDo (kminoda): Here the NDT is copied during the new map loading phase, which should ideally be done beforehand. NormalDistributionsTransform backup_ndt = *ndt_ptr_; backup_ndt.setInputSource(ndt_ptr_->getInputSource()); @@ -219,6 +218,7 @@ void MapUpdateModule::update_ndt( // swap (*ndt_ptr_mutex_).lock(); + // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should ideally be done beforehand. *ndt_ptr_ = backup_ndt; (*ndt_ptr_mutex_).unlock(); From 293176725d29b69b4401a5045ef804229d9be620 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 16 Dec 2022 04:55:32 +0000 Subject: [PATCH 12/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/src/map_update_module.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 927055686c0d1..0bf9afb1919f7 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -136,7 +136,10 @@ void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr void MapUpdateModule::map_update_timer_callback() { if (current_position_ptr_ == nullptr) { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Cannot find the reference position for map update. Please check if the EKF odometry is provided to NDT."); + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 1, + "Cannot find the reference position for map update. Please check if the EKF odometry is " + "provided to NDT."); return; } if (last_update_position_ptr_ == nullptr) return; @@ -218,7 +221,8 @@ void MapUpdateModule::update_ndt( // swap (*ndt_ptr_mutex_).lock(); - // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should ideally be done beforehand. + // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should + // ideally be done beforehand. *ndt_ptr_ = backup_ndt; (*ndt_ptr_mutex_).unlock(); From dbf7d81c99884548e1e7d8e7ce89792804ec9dbb Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 16 Dec 2022 13:56:57 +0900 Subject: [PATCH 13/48] update comment Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_update_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 0bf9afb1919f7..4f9b7e3f79b37 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -222,7 +222,7 @@ void MapUpdateModule::update_ndt( // swap (*ndt_ptr_mutex_).lock(); // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should - // ideally be done beforehand. + // ideally be swapped instead of copying. *ndt_ptr_ = backup_ndt; (*ndt_ptr_mutex_).unlock(); From 5ae3f2894f739c1b650bc3dc5d7c345d21feb837 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 19 Dec 2022 16:49:59 +0900 Subject: [PATCH 14/48] update comment Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_update_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 4f9b7e3f79b37..9679ab34e94d1 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -222,7 +222,7 @@ void MapUpdateModule::update_ndt( // swap (*ndt_ptr_mutex_).lock(); // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should - // ideally be swapped instead of copying. + // ideally be avoided. But I will leave this for now since I cannot come up with a solution other than using pointer of pointer. *ndt_ptr_ = backup_ndt; (*ndt_ptr_mutex_).unlock(); From 506d0af293ef1054847422fac113bf3d1cf47efc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 19 Dec 2022 07:51:34 +0000 Subject: [PATCH 15/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/src/map_update_module.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 9679ab34e94d1..e68675b3fbc87 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -222,7 +222,8 @@ void MapUpdateModule::update_ndt( // swap (*ndt_ptr_mutex_).lock(); // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should - // ideally be avoided. But I will leave this for now since I cannot come up with a solution other than using pointer of pointer. + // ideally be avoided. But I will leave this for now since I cannot come up with a solution other + // than using pointer of pointer. *ndt_ptr_ = backup_ndt; (*ndt_ptr_mutex_).unlock(); From 6f4ff797d275ce62d4ff2a04556ede8862e9353e Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 19 Dec 2022 17:25:09 +0900 Subject: [PATCH 16/48] update comment Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_update_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index e68675b3fbc87..3e206bf529dba 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -67,7 +67,7 @@ MapUpdateModule::MapUpdateModule( pcd_loader_client_ = node->create_client( "pcd_loader_service", rmw_qos_profile_services_default); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO(logger_, "Waiting for pcd loader service..."); + RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check if the enable_differential_load in pointcloud_map_loader is set `true`."); } last_update_position_ptr_ = nullptr; current_position_ptr_ = nullptr; From 8942f7a4f20b8a98475640d62e44466898423429 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 19 Dec 2022 08:27:13 +0000 Subject: [PATCH 17/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/src/map_update_module.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 3e206bf529dba..b65675055a097 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -67,7 +67,10 @@ MapUpdateModule::MapUpdateModule( pcd_loader_client_ = node->create_client( "pcd_loader_service", rmw_qos_profile_services_default); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check if the enable_differential_load in pointcloud_map_loader is set `true`."); + RCLCPP_INFO( + logger_, + "Waiting for pcd loader service. Check if the enable_differential_load in " + "pointcloud_map_loader is set `true`."); } last_update_position_ptr_ = nullptr; current_position_ptr_ = nullptr; From 283e6b3d1561fc029aee0b314a768020d01447ff Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 20 Dec 2022 15:43:37 +0900 Subject: [PATCH 18/48] update for ndt_omp Signed-off-by: kminoda --- .../ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp | 4 ++-- .../include/ndt_scan_matcher/map_update_module.hpp | 4 ++-- .../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp | 4 ++-- .../include/ndt_scan_matcher/pose_initialization_module.hpp | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp index f95a0040586e8..5253879a28937 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include #include @@ -30,7 +30,7 @@ class MapModule using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: MapModule( diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 4e9eb09fa213f..b40e98447e21d 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include class MapUpdateModule @@ -38,7 +38,7 @@ class MapUpdateModule using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: MapUpdateModule( diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index fed9b74ed6091..358331601c171 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include @@ -72,7 +72,7 @@ class NDTScanMatcher : public rclcpp::Node using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: NDTScanMatcher(); diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp index 7d47a1cc13838..0e1a6e6816413 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include @@ -39,7 +39,7 @@ class PoseInitializationModule using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: PoseInitializationModule( From 76ef2f4eec09d08a19101ead340604bec1bd0f55 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 5 Jan 2023 12:43:28 +0900 Subject: [PATCH 19/48] changed parameter names Signed-off-by: kminoda --- .../config/ndt_scan_matcher.param.yaml | 9 ++++++--- .../include/ndt_scan_matcher/map_update_module.hpp | 5 +++-- .../launch/ndt_scan_matcher.launch.xml | 2 +- .../ndt_scan_matcher/src/map_update_module.cpp | 13 ++++++------- 4 files changed, 16 insertions(+), 13 deletions(-) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 57e36cac6d726..6ded458f9f9ee 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: # Use dynamic map loading - use_dynamic_map_loading: true + use_dynamic_map_loading: false # Vehicle reference frame base_frame: "base_link" @@ -65,7 +65,10 @@ regularization_scale_factor: 0.01 # Dynamic map loading distance - dml_update_map_distance: 20.0 + dynamic_map_loading_update_distance: 20.0 # Dynamic map loading loading radius - dml_loading_radius: 120.0 + dynamic_map_loading_map_radius: 150.0 + + # LiDAR input LiDAR radius (used for diagnostics of dynamic map loading) + lidar_radius: 100.0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index b40e98447e21d..da33c5ce6d1e2 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -97,8 +97,9 @@ class MapUpdateModule std::condition_variable condition_; bool value_ready_ = false; geometry_msgs::msg::Point::SharedPtr last_update_position_ptr_; - double dml_update_map_distance_; - double dml_loading_radius_; + double dynamic_map_loading_update_distance_; + double dynamic_map_loading_map_radius_; + double lidar_radius_; geometry_msgs::msg::Point::SharedPtr current_position_ptr_; }; diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index b442ac1b3d843..59a318c24744e 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index b65675055a097..13324c0d2f218 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -35,8 +35,9 @@ MapUpdateModule::MapUpdateModule( clock_(node->get_clock()), tf2_listener_module_(tf2_listener_module), state_ptr_(state_ptr), - dml_update_map_distance_(node->declare_parameter("dml_update_map_distance")), - dml_loading_radius_(node->declare_parameter("dml_loading_radius")) + dynamic_map_loading_update_distance_(node->declare_parameter("dynamic_map_loading_update_distance")), + dynamic_map_loading_map_radius_(node->declare_parameter("dynamic_map_loading_map_radius")), + lidar_radius_(node->declare_parameter("lidar_radius")) { initial_estimate_particles_num_ = node->declare_parameter("initial_estimate_particles_num"); @@ -129,9 +130,7 @@ void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr return; } double distance = norm_xy(*current_position_ptr_, *last_update_position_ptr_); - double LIDAR_CROP_DISTANCE = - 100; // TODO (koji minoda): parametrize before merge!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - if (distance + LIDAR_CROP_DISTANCE > dml_loading_radius_) { + if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); } } @@ -159,7 +158,7 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi { if (last_update_position_ptr_ == nullptr) return false; double distance = norm_xy(position, *last_update_position_ptr_); - return distance > dml_update_map_distance_; + return distance > dynamic_map_loading_update_distance_; } void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) @@ -167,7 +166,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) // create a loading request with mode = 1 auto request = std::make_shared(); request->area.center = position; - request->area.radius = dml_loading_radius_; + request->area.radius = dynamic_map_loading_map_radius_; request->cached_ids = ndt_ptr_->getCurrentMapIDs(); // send a request to map_loader From e2ae1d87a7efd1acfa37dadbdfdfe5be4db109b7 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 5 Jan 2023 03:44:54 +0000 Subject: [PATCH 20/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/src/map_update_module.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 13324c0d2f218..6aefed0338bb8 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -35,8 +35,10 @@ MapUpdateModule::MapUpdateModule( clock_(node->get_clock()), tf2_listener_module_(tf2_listener_module), state_ptr_(state_ptr), - dynamic_map_loading_update_distance_(node->declare_parameter("dynamic_map_loading_update_distance")), - dynamic_map_loading_map_radius_(node->declare_parameter("dynamic_map_loading_map_radius")), + dynamic_map_loading_update_distance_( + node->declare_parameter("dynamic_map_loading_update_distance")), + dynamic_map_loading_map_radius_( + node->declare_parameter("dynamic_map_loading_map_radius")), lidar_radius_(node->declare_parameter("lidar_radius")) { initial_estimate_particles_num_ = node->declare_parameter("initial_estimate_particles_num"); From 057ccb04a9b68c74ba2fd7a19b206d3956dd42b4 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 5 Jan 2023 13:47:46 +0900 Subject: [PATCH 21/48] apply pre-commit- --- .../include/ndt_scan_matcher/map_update_module.hpp | 5 +++++ localization/ndt_scan_matcher/src/map_update_module.cpp | 6 +++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index da33c5ce6d1e2..d846ca04d01bc 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -33,6 +33,11 @@ #include #include +#include +#include +#include +#include + class MapUpdateModule { using PointSource = pcl::PointXYZ; diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 6aefed0338bb8..e934b871dd51f 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -192,9 +192,9 @@ void MapUpdateModule::update_ndt( const std::vector & map_ids_to_remove) { RCLCPP_INFO( - logger_, "Update map (Add: %d, Remove: %d)", int(maps_to_add.size()), - int(map_ids_to_remove.size())); - if ((int(maps_to_add.size()) == 0) & (int(map_ids_to_remove.size()) == 0)) { + logger_, "Update map (Add: %d, Remove: %d)", static_cast(maps_to_add.size()), + static_cast(map_ids_to_remove.size())); + if ((static_cast(maps_to_add.size()) == 0) & (static_cast(map_ids_to_remove.size()) == 0)) { RCLCPP_INFO(logger_, "Skip map update"); return; } From fb77826e2700db44dd6fb16b1b214657de4fb709 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 5 Jan 2023 04:49:27 +0000 Subject: [PATCH 22/48] ci(pre-commit): autofix --- .../include/ndt_scan_matcher/map_update_module.hpp | 2 +- localization/ndt_scan_matcher/src/map_update_module.cpp | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index d846ca04d01bc..67401f49c7fad 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -33,10 +33,10 @@ #include #include -#include #include #include #include +#include class MapUpdateModule { diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index e934b871dd51f..58ffac8adc075 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -194,7 +194,9 @@ void MapUpdateModule::update_ndt( RCLCPP_INFO( logger_, "Update map (Add: %d, Remove: %d)", static_cast(maps_to_add.size()), static_cast(map_ids_to_remove.size())); - if ((static_cast(maps_to_add.size()) == 0) & (static_cast(map_ids_to_remove.size()) == 0)) { + if ( + (static_cast(maps_to_add.size()) == 0) & + (static_cast(map_ids_to_remove.size()) == 0)) { RCLCPP_INFO(logger_, "Skip map update"); return; } From 2f5cf8b729adc8172d2162036b8317a8ccf7ce29 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 5 Jan 2023 16:22:21 +0900 Subject: [PATCH 23/48] update readme Signed-off-by: kminoda --- localization/ndt_scan_matcher/README.md | 30 +++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 128214843d0a9..a536e242ce4be 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -169,3 +169,33 @@ The color of the trajectory indicates the error (meter) from the reference traje - The right figure shows that the regularization suppresses the longitudinal error. drawing drawing + +## Dynamic map loading +Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this feature, NDT dynamically requests for the surrounding pointcloud map to `pointcloud_map_loader`, and then receive and preprocess the map in an online fashion. + +Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error) + +### Parameters + +| Name | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------- | +| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (FALSE by default) | +| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | +| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | +| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | + +### Enabling the dynamic map loading feature +The dynamic map loading feature is disabled by default. To use the feature, please follow the next instructions. + +1. enable dynamic map loading in `ndt_scan_matcher` (by setting `use_dynamic_map_loading` to true) +2. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) +3. split the PCD files into grids (recommended split size: 20[m] x 20[m]) + +Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) + +| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | +|:----------:|:-------------:|:-----------:|:-------------:| +| single file | true/false | true/false | at once (standard) | +| splitted | true | false | **does NOT work** | +| splitted | true | true | dynamically | +| splitted | false | true/false | at once (standard) | From 70858137e1ab0d5eacc5ac14999fd1ad99f30154 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 5 Jan 2023 07:23:48 +0000 Subject: [PATCH 24/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/README.md | 26 +++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index a536e242ce4be..14c86537d4cf7 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -171,20 +171,22 @@ The color of the trajectory indicates the error (meter) from the reference traje drawing drawing ## Dynamic map loading + Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this feature, NDT dynamically requests for the surrounding pointcloud map to `pointcloud_map_loader`, and then receive and preprocess the map in an online fashion. Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error) ### Parameters -| Name | Type | Description | -| ----------------------------- | ------ | ---------------------------------------------------------------------- | -| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (FALSE by default) | -| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | -| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | -| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | +| Name | Type | Description | +| ------------------------------------- | ------ | --------------------------------------------------------------------- | +| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (FALSE by default) | +| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | +| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | +| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | ### Enabling the dynamic map loading feature + The dynamic map loading feature is disabled by default. To use the feature, please follow the next instructions. 1. enable dynamic map loading in `ndt_scan_matcher` (by setting `use_dynamic_map_loading` to true) @@ -193,9 +195,9 @@ The dynamic map loading feature is disabled by default. To use the feature, plea Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) -| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | -|:----------:|:-------------:|:-----------:|:-------------:| -| single file | true/false | true/false | at once (standard) | -| splitted | true | false | **does NOT work** | -| splitted | true | true | dynamically | -| splitted | false | true/false | at once (standard) | +| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | +| :---------: | :-----------------------: | :------------------------: | :------------------: | +| single file | true/false | true/false | at once (standard) | +| splitted | true | false | **does NOT work** | +| splitted | true | true | dynamically | +| splitted | false | true/false | at once (standard) | From a88f0fb9ec90199850f89332ff879a05d4977dad Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 5 Jan 2023 16:31:06 +0900 Subject: [PATCH 25/48] update readme Signed-off-by: kminoda --- localization/ndt_scan_matcher/README.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 14c86537d4cf7..2ef8a011e339e 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -176,6 +176,22 @@ Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error) +### Additional interfaces +#### Additional inputs +| Name | Type | Description | +| ----------------------------------- | ----------------------------------------------- | ------------------------------------- | +| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | + +#### Additional outputs +| Name | Type | Description | +| ----------------------------------- | ----------------------------------------------- | ------------------------------------- | +| `debug/loaded_pointcloud_map` | `sensor_msgs::msg::PointCloud2` | pointcloud maps used for localization (for debug) | + +#### Additional client +| Name | Type | Description | +| ----------------------------------- | ----------------------------------------------- | ------------------------------------- | +| `client_map_loader` | `autoware_map_msgs::srv::GetDifferentialPointCloudMap` | map loading client | + ### Parameters | Name | Type | Description | From 9e5cea6ad947e3c3f9a0d573c221d8b266bf95fb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 5 Jan 2023 07:32:35 +0000 Subject: [PATCH 26/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/README.md | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 2ef8a011e339e..6b58682065296 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -177,20 +177,24 @@ Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error) ### Additional interfaces + #### Additional inputs -| Name | Type | Description | -| ----------------------------------- | ----------------------------------------------- | ------------------------------------- | -| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | + +| Name | Type | Description | +| ---------------- | ------------------------- | ----------------------------------------------------------- | +| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | #### Additional outputs -| Name | Type | Description | -| ----------------------------------- | ----------------------------------------------- | ------------------------------------- | -| `debug/loaded_pointcloud_map` | `sensor_msgs::msg::PointCloud2` | pointcloud maps used for localization (for debug) | + +| Name | Type | Description | +| ----------------------------- | ------------------------------- | ------------------------------------------------- | +| `debug/loaded_pointcloud_map` | `sensor_msgs::msg::PointCloud2` | pointcloud maps used for localization (for debug) | #### Additional client -| Name | Type | Description | -| ----------------------------------- | ----------------------------------------------- | ------------------------------------- | -| `client_map_loader` | `autoware_map_msgs::srv::GetDifferentialPointCloudMap` | map loading client | + +| Name | Type | Description | +| ------------------- | ------------------------------------------------------ | ------------------ | +| `client_map_loader` | `autoware_map_msgs::srv::GetDifferentialPointCloudMap` | map loading client | ### Parameters From fd3b48409e457fab56c058d319458418b74facb2 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 5 Jan 2023 16:49:40 +0900 Subject: [PATCH 27/48] simplify client implementation Signed-off-by: kminoda --- .../src/map_update_module.cpp | 34 ++++++++++++------- 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 58ffac8adc075..39fc10f95dd3d 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -171,19 +171,29 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) request->area.radius = dynamic_map_loading_map_radius_; request->cached_ids = ndt_ptr_->getCurrentMapIDs(); - // send a request to map_loader + // // send a request to map_loader + // auto result{pcd_loader_client_->async_send_request( + // request, + // [this](const rclcpp::Client::SharedFuture + // response) { + // (void)response; + // std::lock_guard lock{pcd_loader_client_mutex_}; + // value_ready_ = true; + // condition_.notify_all(); + // })}; + // std::unique_lock lock{pcd_loader_client_mutex_}; + // condition_.wait(lock, [this]() { return value_ready_; }); auto result{pcd_loader_client_->async_send_request( - request, - [this](const rclcpp::Client::SharedFuture - response) { - (void)response; - std::lock_guard lock{pcd_loader_client_mutex_}; - value_ready_ = true; - condition_.notify_all(); - })}; - std::unique_lock lock{pcd_loader_client_mutex_}; - condition_.wait(lock, [this]() { return value_ready_; }); - + request, [](rclcpp::Client::SharedFuture) {})}; + + std::future_status status = result.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + RCLCPP_INFO(logger_, "waiting response"); + if (!rclcpp::ok()) { + return; + } + status = result.wait_for(std::chrono::seconds(1)); + } update_ndt(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); } From 3a2082f25eccc7ac2992b46aa2a89a738a46b814 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 5 Jan 2023 16:50:12 +0900 Subject: [PATCH 28/48] remove unnecessary comments Signed-off-by: kminoda --- .../ndt_scan_matcher/src/map_update_module.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 39fc10f95dd3d..bff71bd9efd66 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -165,24 +165,12 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) { - // create a loading request with mode = 1 auto request = std::make_shared(); request->area.center = position; request->area.radius = dynamic_map_loading_map_radius_; request->cached_ids = ndt_ptr_->getCurrentMapIDs(); // // send a request to map_loader - // auto result{pcd_loader_client_->async_send_request( - // request, - // [this](const rclcpp::Client::SharedFuture - // response) { - // (void)response; - // std::lock_guard lock{pcd_loader_client_mutex_}; - // value_ready_ = true; - // condition_.notify_all(); - // })}; - // std::unique_lock lock{pcd_loader_client_mutex_}; - // condition_.wait(lock, [this]() { return value_ready_; }); auto result{pcd_loader_client_->async_send_request( request, [](rclcpp::Client::SharedFuture) {})}; From 0be1e626d1a74f5d5343523b3cdc63d8becc1e78 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 5 Jan 2023 07:51:48 +0000 Subject: [PATCH 29/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/src/map_update_module.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index bff71bd9efd66..77548238ab941 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -172,7 +172,8 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) // // send a request to map_loader auto result{pcd_loader_client_->async_send_request( - request, [](rclcpp::Client::SharedFuture) {})}; + request, + [](rclcpp::Client::SharedFuture) {})}; std::future_status status = result.wait_for(std::chrono::seconds(0)); while (status != std::future_status::ready) { From 6d78517e3d5c04f911dd687beacf557e1f8dc1bc Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 5 Jan 2023 16:54:01 +0900 Subject: [PATCH 30/48] removed unused member variables Signed-off-by: kminoda --- .../include/ndt_scan_matcher/map_update_module.hpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 67401f49c7fad..e5bf4d6d607e0 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -85,7 +85,6 @@ class MapUpdateModule rclcpp::Subscription::SharedPtr ekf_odom_sub_; - rclcpp::Subscription::SharedPtr map_points_sub_; rclcpp::CallbackGroup::SharedPtr map_callback_group_; std::shared_ptr ndt_ptr_; @@ -97,10 +96,6 @@ class MapUpdateModule std::shared_ptr> state_ptr_; int initial_estimate_particles_num_; - std::mutex pcd_loader_client_mutex_; - std::mutex initial_pose_pcd_loader_client_mutex_; - std::condition_variable condition_; - bool value_ready_ = false; geometry_msgs::msg::Point::SharedPtr last_update_position_ptr_; double dynamic_map_loading_update_distance_; double dynamic_map_loading_map_radius_; From 67585f8ceec126271389e102c91b06903fa0a42d Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 5 Jan 2023 17:14:23 +0900 Subject: [PATCH 31/48] set default use_dynamic_map_loading to true Signed-off-by: kminoda --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 6ded458f9f9ee..0b639b481703e 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: # Use dynamic map loading - use_dynamic_map_loading: false + use_dynamic_map_loading: true # Vehicle reference frame base_frame: "base_link" From bf09518c99406ba826fba84764de62f93c1a9650 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 6 Jan 2023 12:13:45 +0900 Subject: [PATCH 32/48] changed readme Signed-off-by: kminoda --- localization/ndt_scan_matcher/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 6b58682065296..de3b50a3e650a 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -200,18 +200,18 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma | Name | Type | Description | | ------------------------------------- | ------ | --------------------------------------------------------------------- | -| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (FALSE by default) | +| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | | `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | | `dynamic_map_loading_map_radius` | double | Map loading radius for every update | | `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | ### Enabling the dynamic map loading feature -The dynamic map loading feature is disabled by default. To use the feature, please follow the next instructions. +To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node. +Follow the next two instructions. -1. enable dynamic map loading in `ndt_scan_matcher` (by setting `use_dynamic_map_loading` to true) -2. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) -3. split the PCD files into grids (recommended split size: 20[m] x 20[m]) +1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) +2. split the PCD files into grids (recommended split size: 20[m] x 20[m]) Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) From bcfed6839afc964ccc67a044f7a82589d659a298 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 6 Jan 2023 03:15:18 +0000 Subject: [PATCH 33/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index de3b50a3e650a..ea0ff0225e640 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -198,17 +198,17 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | --------------------------------------------------------------------- | -| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | -| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | -| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | -| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | +| Name | Type | Description | +| ------------------------------------- | ------ | -------------------------------------------------------------------- | +| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | +| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | +| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | +| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | ### Enabling the dynamic map loading feature To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node. -Follow the next two instructions. +Follow the next two instructions. 1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) 2. split the PCD files into grids (recommended split size: 20[m] x 20[m]) From 13dd04b488ed7d9e133c0cddf2aef8db33bf3799 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 6 Jan 2023 13:45:48 +0900 Subject: [PATCH 34/48] reflected comments Signed-off-by: kminoda --- localization/ndt_scan_matcher/README.md | 7 ++++--- .../include/ndt_scan_matcher/map_update_module.hpp | 6 +++--- .../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp | 2 -- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 4 +--- 4 files changed, 8 insertions(+), 11 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index ea0ff0225e640..a84f6be52818e 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -62,7 +62,6 @@ One optional function is regularization. Please see the regularization chapter i | `max_iterations` | int | The number of iterations required to calculate alignment | | `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | -| `neighborhood_search_method` | int | neighborhood search method (0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1) | | `num_threads` | int | Number of threads used for parallel computing | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) @@ -217,7 +216,9 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample | PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | | :---------: | :-----------------------: | :------------------------: | :------------------: | -| single file | true/false | true/false | at once (standard) | -| splitted | true | false | **does NOT work** | +| single file | true | true | at once (standard) | +| single file | true | false | **does NOT work** | +| single file | false | true/false | at once (standard) | | splitted | true | true | dynamically | +| splitted | true | false | **does NOT work** | | splitted | false | true/false | at once (standard) | diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index e5bf4d6d607e0..430e9e12e9a72 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -97,9 +97,9 @@ class MapUpdateModule int initial_estimate_particles_num_; geometry_msgs::msg::Point::SharedPtr last_update_position_ptr_; - double dynamic_map_loading_update_distance_; - double dynamic_map_loading_map_radius_; - double lidar_radius_; + const double dynamic_map_loading_update_distance_; + const double dynamic_map_loading_map_radius_; + const double lidar_radius_; geometry_msgs::msg::Point::SharedPtr current_position_ptr_; }; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 358331601c171..bf46c90c322f9 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -197,8 +197,6 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr map_module_; std::unique_ptr pose_init_module_; std::unique_ptr map_update_module_; - - bool use_dynamic_map_loading_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index f56e2950c747a..e943adb2df821 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -95,8 +95,6 @@ NDTScanMatcher::NDTScanMatcher() oscillation_threshold_(10), regularization_enabled_(declare_parameter("regularization_enabled", false)) { - use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); - (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; @@ -212,7 +210,7 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_module_ = std::make_shared(this); - if (use_dynamic_map_loading_) { + if (this->declare_parameter("use_dynamic_map_loading")) { map_update_module_ = std::make_unique( this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, state_ptr_); From 0844435c0c473a975a18cd40b44b38e98c7cc191 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 6 Jan 2023 14:43:50 +0900 Subject: [PATCH 35/48] use std::optional instead of shared_ptr Signed-off-by: kminoda --- .../ndt_scan_matcher/map_update_module.hpp | 5 ++-- .../src/map_update_module.cpp | 25 ++++++++----------- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 430e9e12e9a72..a89c115955911 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -37,6 +37,7 @@ #include #include #include +#include class MapUpdateModule { @@ -96,11 +97,11 @@ class MapUpdateModule std::shared_ptr> state_ptr_; int initial_estimate_particles_num_; - geometry_msgs::msg::Point::SharedPtr last_update_position_ptr_; + std::optional last_update_position_ = std::nullopt; + std::optional current_position_ = std::nullopt; const double dynamic_map_loading_update_distance_; const double dynamic_map_loading_map_radius_; const double lidar_radius_; - geometry_msgs::msg::Point::SharedPtr current_position_ptr_; }; #endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 77548238ab941..34abd5f896b48 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -75,8 +75,6 @@ MapUpdateModule::MapUpdateModule( "Waiting for pcd loader service. Check if the enable_differential_load in " "pointcloud_map_loader is set `true`."); } - last_update_position_ptr_ = nullptr; - current_position_ptr_ = nullptr; double map_update_dt = 1.0; auto period_ns = std::chrono::duration_cast( @@ -120,18 +118,17 @@ void MapUpdateModule::service_ndt_align( res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; - last_update_position_ptr_ = - std::make_shared(res->pose_with_covariance.pose.pose.position); + last_update_position_ = res->pose_with_covariance.pose.pose.position; } void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) { - current_position_ptr_ = std::make_shared(odom_ptr->pose.pose.position); + current_position_ = odom_ptr->pose.pose.position; - if (last_update_position_ptr_ == nullptr) { + if (last_update_position_ == std::nullopt) { return; } - double distance = norm_xy(*current_position_ptr_, *last_update_position_ptr_); + double distance = norm_xy(current_position_.value(), last_update_position_.value()); if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); } @@ -139,27 +136,27 @@ void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr void MapUpdateModule::map_update_timer_callback() { - if (current_position_ptr_ == nullptr) { + if (current_position_ == std::nullopt) { RCLCPP_ERROR_STREAM_THROTTLE( logger_, *clock_, 1, "Cannot find the reference position for map update. Please check if the EKF odometry is " "provided to NDT."); return; } - if (last_update_position_ptr_ == nullptr) return; + if (last_update_position_ == std::nullopt) return; // continue only if we should update the map - if (should_update_map(*current_position_ptr_)) { + if (should_update_map(current_position_.value())) { RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)"); - update_map(*current_position_ptr_); - *last_update_position_ptr_ = *current_position_ptr_; + update_map(current_position_.value()); + last_update_position_ = current_position_; } } bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const { - if (last_update_position_ptr_ == nullptr) return false; - double distance = norm_xy(position, *last_update_position_ptr_); + if (last_update_position_ == std::nullopt) return false; + double distance = norm_xy(position, last_update_position_.value()); return distance > dynamic_map_loading_update_distance_; } From f8da47bc91221f0846bc6d53cfa85d64362faacd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 6 Jan 2023 05:45:28 +0000 Subject: [PATCH 36/48] ci(pre-commit): autofix --- .../include/ndt_scan_matcher/map_update_module.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index a89c115955911..fb9577ca42934 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -35,9 +35,9 @@ #include #include +#include #include #include -#include class MapUpdateModule { From 50b3855eaf90a458b1210417722a3924d5a49910 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 10 Jan 2023 13:49:40 +0900 Subject: [PATCH 37/48] fix parameter description Signed-off-by: kminoda --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 0b639b481703e..259d29a608547 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -70,5 +70,5 @@ # Dynamic map loading loading radius dynamic_map_loading_map_radius: 150.0 - # LiDAR input LiDAR radius (used for diagnostics of dynamic map loading) + # Radius of input LiDAR range (used for diagnostics of dynamic map loading) lidar_radius: 100.0 From 950ac42cffc1c8d1066e0088ef25f75b9ef359c4 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 10 Jan 2023 13:53:44 +0900 Subject: [PATCH 38/48] revert launch output config Signed-off-by: kminoda --- .../ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 59a318c24744e..b442ac1b3d843 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -16,7 +16,7 @@ - + From 103ff1873d280300df59871c101af7f28df106a1 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 10 Jan 2023 14:01:07 +0900 Subject: [PATCH 39/48] change default subscriber name Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_update_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 34abd5f896b48..99dbcfe1db83a 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -44,7 +44,7 @@ MapUpdateModule::MapUpdateModule( initial_estimate_particles_num_ = node->declare_parameter("initial_estimate_particles_num"); sensor_aligned_pose_pub_ = - node->create_publisher("debug/monte_carlo_points_aligned", 10); + node->create_publisher("monte_carlo_points_aligned", 10); ndt_monte_carlo_initial_pose_marker_pub_ = node->create_publisher( "monte_carlo_initial_pose_marker", 10); From 30ec6f6ad413f0636b2520686b49f18dfdcb87bb Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 10 Jan 2023 15:14:54 +0900 Subject: [PATCH 40/48] remove unnecessary setInputSource Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_update_module.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 99dbcfe1db83a..fe845dee549c5 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -199,7 +199,6 @@ void MapUpdateModule::update_ndt( const auto exe_start_time = std::chrono::system_clock::now(); NormalDistributionsTransform backup_ndt = *ndt_ptr_; - backup_ndt.setInputSource(ndt_ptr_->getInputSource()); // Add pcd for (const auto & map_to_add : maps_to_add) { From 026ea615213db7e07749059224bb9eab78933d8e Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 11 Jan 2023 10:32:39 +0900 Subject: [PATCH 41/48] add gif Signed-off-by: kminoda --- localization/ndt_scan_matcher/README.md | 4 +++- .../media/differential_area_loading.gif | Bin 0 -> 89542 bytes 2 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 localization/ndt_scan_matcher/media/differential_area_loading.gif diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 43b1a4e374edd..b8f0410da384a 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -172,11 +172,13 @@ The color of the trajectory indicates the error (meter) from the reference traje drawing drawing ## Dynamic map loading - Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this feature, NDT dynamically requests for the surrounding pointcloud map to `pointcloud_map_loader`, and then receive and preprocess the map in an online fashion. Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error) +drawing + + ### Additional interfaces #### Additional inputs diff --git a/localization/ndt_scan_matcher/media/differential_area_loading.gif b/localization/ndt_scan_matcher/media/differential_area_loading.gif new file mode 100644 index 0000000000000000000000000000000000000000..4283b738a1dfa092f5af98e4e1c88b7c93743626 GIT binary patch literal 89542 zcmdqIWl$VZ)aTt}Ffh2g1q%|KkOat}!9swbK{6z0@Zdg!dvJFT9$dqa0Kp|e26qXP zpn({(*=MWX_jx|-Q?>hXf7M;pea`8+eY)!Qy{GTLAE~R#+_U6>d;kFez==m_tLQ&f zQqWNmzat9y57SR5VD-->^lu0LXILZ%1cE}L^pq4&X^E1VAjK@8EOvrGZb}_tUTFyl zMMXuO$B&JTj7&|<%*`zS?mrk0kLj*gDr-d-#g+t=6k@jvMA?;ji-9Q{v>{U@fTrl$Y*eE$3y zhr=!WSN>W47xVM;b8~b5j{iUZ%F4>de`0HE>)_zv>gwv}fAHtepPT;*9*_U8hX4QU z|5ew&4FB&^`|mIO@4f#`i~sA1{~rbX3x?wXN+K4uhQhv3IJJP;P(#tjNQzq;xoYo< z2Vxl{UM&y3EBW+>Ln($uy|HvSh5u=l*>GdoXol#^kzDnr^6?xQ_k-o(ri#e|WGFGK zMswwKiCT()`AGBIFBLi^8hIKmRkPK`O|Mo)TB_&kQ6FPiHCt;In(V$*nUA*CF15LC zjpS*z)va{-oFA-=w$-ooq9F)2t@ehEk5SZu7Gv%2wmv1^(#+TDXxtgilyF!b>uB1W zEL4hRd)(Rl^-JZ`YK!sCmc#jmm!tWQyIPNz+T9OV$Gh6TuVF(G?AqP!r&~iQg3l+q zJI?o}N;C_!dpa)M+q47D_*-+zl{w-mRX;|QE zNmT&s!8yf<02&mFiF}##GG9jEY4*x9R0XKYn{7tE*0zR7R@Y=lgjK86X+WzAnBM|e z7}U~|zZy>ao3bVa+*RGs6Le8s-xne&)H?gxUs&z){jdnIA#Wizm7t~YL)Xa%W{Be3 z3C+YbQ$)u1uyOVl5%1NASfrn#fqXMxAiFfaqm<1F?n+wk;9=tK5cn=c;&k55VkJlJcR}bZ~%jkI0tozfv_KjgSQp#1rJlDRR%$MJ; zCV_21ohp$iVTyG+qM(zk7?=L*O%L|`PSvRD)y|`YF0u~QQg-DI1qO$1w@-i>NF2By zCEvRJm-n&EjMk*=^}={{px-f5EnA057yS%`blH1Njt(vbAon3foJhcmq2Pf^xdeIO z;&d-QU}@sXq}PTo*LR&C^eq4u0_^|+G^p;aq|!h)rIlmx7d>(PWM1_z*N>|1D#Xc7 zVJQ36AQFx5LmUBseI3p?VOuXo6JvMqWo@8RzO@}P5fPLi)}-OxFa7XnsMcP<|tOq+vDnEv1RWGr8BZzO!P9S!eGgW{Q!~2v%3S;ovsdyX zTGL9xzstUPWmNZr9*fU=_1Y*K_3n;LDOi4+Bm}uZY}?;EHhdT7Uh|Uv9M>;%;}#Vvv`zlKeTaju z&->O6*%8ZG7$I-RX3Y!jNH%5R z$E}0Tv$uq7CAvB$ybLAs2ekI&ygMd?R?qSv<2!_zF~h}XuNAd_&w#&QV9Dg~giao4 zu50^MMrli8m;;RZs=iETUN+xTh+o8QN`DbGXjaUNHB;}(_kcW(oooDrr= z3-W3$re&{e=#}3OMYO&h|9u?l$jrGF;oY{v>Kxi_x_mCLX003Vtj%n^pnKV7s`A&) zHdDwlvq-5giaY6GN=-?grm)>|yTbJUyKhdpZRA;i<) zQyi^5nSR$@UNcyS<-9qu3JqwGGP)WdYCLuCHQQis^Bnq#IMKeFyK>L_dipfyAU!x` zLqf#!b5FKQ+>=}m#tNU=miOlvtGqqayV+l2JHF;H_v{xWbdEo^F#pl{ciW2n_sHm- z3$^O*<4F}??6mwz8GF~l>qya=@f(+C-+R8<-j$u0t#enKJloqPyFdI?+hc&9?X<+} z&kkR(PYr=Z2ZwL#XT~R&o`-FR;IH>^x^JTD$(1j8LS>tH>b<*+@{f7;{%rnad(~^& z^|gS$yXj^A@5PF|E4{0pwu%IcxsR(mee^fgQBQuhZLxmyZM!LRd;a(6W$?G@g17i1 zVa>lq4#9KFcYCkuCvP?j@N)?Ahq7Pme)m`Oo)515IsX3b=F$cK^=#gJi_foNC&2Q` zj}Yf)wC9P)^dpLbPwx4FIWZL1Xx2!(=BuEC`Jnxb;K0ZLl!_m$(ihBtKELaed4);O z@C~a7xoY#jjq~RiGU7E3;WrNcb@%n3`M}!N(38l}JNu!VMxo1+=p9>sCU~ewX6Vg4 z=66Qe55BNBkHTQqVOx(dG}i&NLw+XLXi6NK*v`*fJm`;2xEVa+*>(8uvd|~EFxl&1 zw@2Yg))6Cw&M&V0J=I*i+XJ|80o=x6uhl~D?qkH;Ltylw!*@d)(;}(bBB$|+5p0hl z1AY90)!b9~of7Qa;_N)LhTIaPFv+-(@b)mFs3-@%@K3fe@fA@ObWyc30Y>l$b6m7B zU##7JI8ju18$6(Hh=OrHf*Bs^2#-^{4rF17`|BO2go}O85K|@N7hxA&oEbIE5c0(* z#t9xYR>?mB4=ag^Dcg^qQHzP>Pr%83`oZv0 zKK%`?;hP`(Z_cCM{9%BCv)%wiZwMC>U|DZ8$`gn;tilGZjmxbmO{~#|NqSyM3`Eu_ zRWRGnBo4-8u6xPc>R?urWd5vV&JGZHVe*}~$wEJq#nqGf?xje7O$x=gCh3+ZF%PFO zWTo66PKw-3{52OJCX)b4f#@`(YAvK{|4h|kOw%a@Y1~dT^i9*g4dFqjDIsk#&#gF7 zU_o@6=G!zY-*iLuG`o)UCvQO=cCpR;Udkq^7v8CR?J@?#>3(n1{f5(p*8z?b08yyI zf0BA{{Q=W0KxY_Cp9mJ=fLeE?1rDbN+{?22nr_RG@$PPh$Gs?>p|GlyjCA$%w0qgQ zL|`JUR8$A(Ne3h)Nn7cCyn z(e20?=}0Xs7As8#5LoaNdzc&wrWFm9D}+djLUghO>WKhu{8a8}R_^vf>U-a499^!p zWNyyatbT^@A>aI+d)bN&U=j{M=_FMs5go&db-h1wlpa09^K0O|Nz zw3n4X-ySo|A4R{I|Aju5hAErxAXSn$O#%(-apwUQ0(Uslc$rF*7}J`fd5Dl;@%vdy zzY3{ep}T#GdU2s%zQ!yX1x5#{n~)qbELhDo?eRgW!(ysX17Mgerd|cM*T}MbR$}ri z2i{)xZJ|uuyTrn?V*6hC6rj#2!Y31th0(mWgXKh%40yeF9&l4*rZk|5~a!oFPgf&R7Bni58Zt*KNJ3 zOYcwuH2`BV`N#LOr+<}Asnz;kMt5q|pAu!yHZ`QHr-@OM zI-<>7OHGFU`64>};3??O&el|cR_)Pe*&|tErg4vBGx&(BpI0}UJjjmhN*5A+tCI81 z&-C369Ia`Zo9Iw)5Zb024Wmy4y!_iD5N&%|?b7}&q+M-ah!fCuwZVb~<;<0@W5osz zI<5se1J%3y-=<2Qyi<7AnHHP60QbxBYwfw;^d_t$!4FiG1t0{v9Em$QG&}F1>p+R1 zum_cInA=BW!m9+k`w{K*)wvRQv{=7Jd%S7a#8PgnL&Z~n9-_ns|XLHMR#4(9!V|U_1wAN4s78 zTfQ<^Yeqv+?mQ$q6-`yxkFmjSG94#GeLMl`g9u2e_*n`A5N`OGSrsWS8-X0_5$_rlIvS8&8W1=vGGHD!@vna9IDmUL7~fvx_!jbVxK;g6 zhjT!2clC#-ED6aE>dEnO0}*E34)-8?5`!!(L#aolJIpP5V}ts+Lw#LCN<#gTvFWe? z9&%vFOlZh$xlU-kH#KW`^)QSm@{`=NPai(i3KwS60}Zym!xf$VpuJ9TdW8a)|)mafH!v`nV=T z5Gw_4=w|slPJ$N-{6>_dz-Y~wXnn6I1vU3YXE%j5%Z5>BMm2CoEpJAnXNGYdpvTVW zu+FOgwbnDA)q6Z^m^Z6&JoC(H=8pitpfjsYI%DeAiWME zqQFJUEu@a)gpn^G3vcrOE?K^ta?&>?ZoH=-1 zGyJ)E@yl|}3ypEDktIL-@h+0(Cb_N_*52W&NE5B_A>q#^gqc@>Vml zG6Y?Dz{sl5`)iLG*Jj7o23B&p8>aE2JQS< z>y=^Zn-MyzE&I2r>x^A2+FNW1)8if67xBvqH=(&lZLb`+lft%-eoaJt*m@i^E%+P&xtIa%fz;C@Yk^Zr9*uS}9?tqjo2& ze`geL0vU?kwUpm|AwRXp2sUa2g2~n#EWV8H?g}<j?d7#Yd%~A8szEr}| zDBHK$_kN$h9ek8;9bhXpJOP;V59sTT_H(}d`L=m7F&1ppI-+$9U2_<$6S`qJxodGa z{QjGc{K-Uot+DV%=UvdZ6ZLQ3!6Zx9*`P(T?dhP?i-Z%}hmMUmxu#!FKaGET zf?xft{R+m%0f;Bm^jb_xGkm^W?F^SXkOWlQiO$u8&Rg%C@l`=krK9O=`x-tw?)Klf zB9;WxH33ic`iL8u{k04@LH)ceGS z{kuiv{-4f`wftkVf*jwV3vJ^3Et5^1-eEqoAC_*5&(FTHU7Hq+uyMR~7x_lV|8D2oPqx5)hlf8q-n0+l z$$OO6>$Ts!?{nFSesdZd{G;vrRr1;;)su6wpzA2Jo7=%xM3ZL=1%M)Yg$9Fj6*U{s;_17%8i?GS%jWa!gcWDmqzrOErP@p2caUu2MoG&*N+sU%ed- zMh2tzLJL&8V+ptISHoUV^Jy_s3Es#uQ10u5a3)fyRE2Qrl|Rmt4`(ts(62J67HD~; zv+=nG67I$0>*W-uuYS$`-Y=gtSO)%3v7%`})`XI> ztL7S?z9?acazwgJ~J3;@@O>7}d5zW01|R{FT&>R-L}kDDs8H=pqozpGg}9bb zA5GI3Plu$$ILVZjxXk4Yi@Vyby?$M_``>F$F^H4xbIctJsHBd9Wez7?2~Vyfsr#={ z4*~*WYe_wI#z4+cb&&zdD0=yZqNh(D!lY)j&D)Pax^g{0#8V^dv{`-AaH%;unfR7D z1KSpJr5e2>4O7RfowjFzC7VAK{N9F8R6qf> z!a6TqQ{xuTS*a^C_a!b`bfn8cy~1HaNc>{ZLjuOB;38sNKc+$;vz(WKVjUcQt5u}V z<7XE`XN1!YTm!Z%HD*zLGX82iF_O1ax+qVeV>P8r<0r3BsnXZo-}ut>j@>SsLEjf7 zj1*+AvN94z_NvgW2m8g7s$E-s$-E4XmDyfK4j#4Th{Ly*gNfmzhV6^T75(KgN7b_0 zU9KNz3F4em|9TNSN1WzAc+vGW=H8vIL<$ZN-LG$1r{&O!k)w}~_yYN-lk{@!C$nDX zHjS+K1=b8#N0@y}XyABv9#8t9$73z{Zsk>x9=X3~>9fhj4><4amD=6;XF(PFV$%8i z*a~o6!UMmzjTr~OzkPqQ6cqCT&wNub zLat6>2c&#mDMd@Dv!BlqcKr}L3G8Gh`CDB4@Q;D06-@vf&$QQzj{&A`@kMI%D`iY21amf95iM}kQQGbfL6e>|uA$R-JNtuiL%7;#z5Zh{yu+T2bP8~nheg=wJ zqX#>jNxY1QbR>L=A1n4LB8^B=X~{n*&8eSqvK)UW=Q!B2S#S% zyjUsVnnvKhO004Jt&nfV6nEG}5-4*;`pfYH$tKtACba`AD`QdT%tyUXLS}yMQm0 zN{tAFC5RAuNfhe)0m&DzTP)XrrmTju#K=l!Wt36>Hg~zfPjl z+n|oMAG8EFadWlD!L1|5L&m0Wr#i+7mED^Z8gVy8xY<~Nhc(=@#pSn@Igmug+AdzD zFG|fS1a+aBpy@9~bH(LS_)ilJm8ZmC?Xk!=L}DtPdxqOyuR_XCX}O{$3=UREOAJluh|Ev7l6)u6!s z@#^gd*|~4t*GPcW0IE5#3Z|o_$W5ij>3xjBTIy^){>iMM$?2>2i4;k-Ob^}~Qg&4d!KMDl;K9CsefGPFcI=PJ%NU~9eyL69k5xat(cvtHHq*8W+ zJkEYi;TFXlcOyn-%gLw;0pgXJj(fuD6k3I7nBzKB;9mu&I|U-($3kTG%O7bR>hn)Y zcDGoQ1*9R&mK%(E=)Ke&jr`}c#@5C8k%CUFbX0=PNDxb9A1xILjB^T0Y}g(`rdEii zHBP^UxU=5}Jz4J_eDtA)M<60xRsjJS=%IGQ+mfdlzZ+keFDVH`v^VI4z=qISl4=<1 z=dPg9XMF^u?bs;QFXr9Ndnnz_cXsUXx$swBLoNw>iw6Qr4HL>PfSQDwN`MrZ6bM4< zQ$l;kH3aIiFc>leKxW=jvK-#KWvlA`=7FNM+WW-rD{+^F7!q4fhxP`R2zBB?+RfiF zHun@8lIQIj(06V6r`k;Wz-A3Ee%4??ZG)b5o;?m053oWV5udHOYHd=WkATp>ZZ<{3 z_BK!Yzwo{kc&y=?#hmzSIJzH1V$lFl;$$Ga#;4T}UW7XpU>naUiyLNpoW0tXR*qN2 z-f1?@RR>8|k2-t3CqHv6Q}O5JY>(^4=Z+cie=3C<;=b3K095ff^q??WrvDKZ9c3iD z$)|>eS&2dg3xBV$6Z#Hq9V1h)hq%jf_ngyOmvm~IFSPg0=r^jB$<27-bG+BfVTAyz znZf|eI}CxSs7LY3b+F2#UsEEk7_bBfgw@zh^)u`1Dx<&Rx7bJB**I9Np8vAVVlx{Z zkf#W08()KV$ITOGyEh)7K#_Apu{-kKo%4SOjC`0#?j)iK+R+<&ZGP)-6FhV6@(%ge z8gy1?0uP?L_|3Mh zUJBum2=O4$bl2*SJX-|<1%)xj*ZXkgqa~>xH>8EXoQTc=F6tD zXR;KIt&OI+OW)j3pZXeECpAs>az1~4}Rc-cOh0|z23RA6paU~w!;!F2-ro$US1 z95jmBw-jf^KBP^7@C>e?b|qo%8YWnSkPuD;rUOvuJjg7>5H={q#DX#^CHX5Agh^!Y z94V&wfT(3KqE01OJv(2`cL|8bU~9 zKoL#g7c?M#ELdnJO2JVj(O<#2|+F<8dG+(VanU;%6Kxjnn#*EcCP!jh`(Q zdV|A^?gOgLZS}H3#f6{%tBKv_M@)(1(?#RjjpJ2dbRK5xd7Mz2><8*GI0KG(ORM99 zmiCtgkO*?}LVxno>dD12h&F2CW7dROmG&EBbe!fRc*@x7h4#FC=7Z5DKCUsbQWabO zdt&IxYoXkq-Q~Yh+rBMNz*in0AHhAq=w-$3Cpjt~&FU+VIxzDooEDsFMu$2~rz}-x zRd$+aHcI4A7Y)-?{jHA$4F%*d-DbE2mMge%xOj2!s<~aX+&nb#_h$UMC zP-P7T%mC07+5`y=E4mFrYsZ{~K>4G9c73iqL%CNEHPE! zQ|j<&+V(i$vhLO8v_PPt)bYq%Zo$0doS_pjvmD%oHj2szzzb0T5)AM=D188}uWXE_ z<$%)-u@Kk+%A&DACCJk0-_4o$_sqX^>L(;BZSE&o>DII3!AV_CO8J{G3Qs{zJ{uXJ z>3u-Pc_0P~!_X-lW*?1UfQwQ9s^brn4L~ZibA>8UhDxLK_@@48BgMShoRizxQaGLC zr@30i>LXDG`n;rHw-1&=}W$T`9`omWd!T43!rSG~i4q zfZAH_fSr;$sV=izA}(4}J)R}WvgOg8EQi`5$1-@E3OtH6N(_#UmjhAj%wyEh^y^D7 z3OFNb1==u!QVIR1ds9Z54XiAFa$V*MGs9=DD*%N|t&Vp_%d%#YKReBs&2O7QRAweU zfzWm|45iOZfo8=SaG!vMhS1OkV+;cX!~hnbdYBwv+6C^4lcL6+Po3g7>Yqc2lA1JW!)&%XgmKUXfw5Fg1r?PgDz4dmh*|g=bnTDZ-rYc;^YsV;k z^VOochErrJqJaXb3E3kEh8jAS;JcdQKluCvKm=(d_Trw^^|Ji2*o z`p{x2{c+5>ol=ieM-%IOJ921UV0|RSbL9q-~Z37 zw+-c|lK1LK-QnppqwaQijp^>4-`y~vX7+(xO!@xSm;I&Co&<~cZwS6FZS5yH_asZ( z&p5n~DyP|h`BgdVeTE^;q0+v|$a}Isl;1vnWe4@--RL@wH&$fMo+)`#y=h#$GFC?@6|8Ze`{s)Y^s(_h4|@ zDq%O6cMDB+$jo&3IBiGu^G@@e?V_cFUfuKHIU9rY!;ZSb&WZBIK_a(4iJrli0>=(* z5st5Sx4rN0ZE_vSiXV9?9_0*SF0S8B|FIIbu#q%4blx3)U0yO`u{=6hG8VMcLea&kg$8wYKR8rMFx zd4GH|;WEnU0-ONbe_h-<%pvo*7>qo2<1x!y7|Y*9j}l#oN(ilxR8%xII1kvJx)Zu;hd{c7w&3c*4n1 zn2<~uoz4Of0H_UsVL>=?jyrmHol`0^FZqAyw_WrJxM@Y|&ZJ*nkg@$MVWMhhsgYp> zuIR;MFdkHi`N9LCp+GO@!6mREmdA!93^5g+QHT@c5DF?a6$riP6}irJXyvt^tw%dA z!?XuA9nSkzkIN^5>km&oRS(zhvLDHKBbb6`_i8r?DIkli@L5@Nj1Jm^J*x9DK+UmE zFhtJEupov)LjlmGIViy;GH(!&{i(5-h7 zbgm$JESeX&PLINrncipMzyLacucxqouuEGzNU#^3d)Mp4`Er*HK~K>X$X63+K6?(n zEHFAVJ`cH5h=JOH7~xkKtf?|Z=M*iPmtt+c2hFh$K9|FosDWxf0pcgKCNdyAjKPSZ z(A09)RI(vX%oPNix`a+)m@^H;8015AfDM-*Rydko24bn=t2_lNqFs!8{&Pe8PrWcH zMSC0+6%h2$>reSv-Vg}7$}+O{(<0)WM8XGyHb^rE=rUo`52HTMqe&Q|oE%o>VNtwM z-P}kTGzpgkm_kgs85~LqBbj8$mFgb|4+nR1Wb>d%AOr&XLf7kkP$d#Uru5sHC_*LB z3me)$u3+5^&ij#33fzF@$3n$OD&bcb{GUrTGQ=NO72AHPBpDDz3$)x$rZ6hwx((V9 z#5KJ5l+5bS^H27DXQa_-$xw-`?e*Ts$^EN4OPxMf3E`vk^&!ndh4|~gqec;PwZ{L{ zrc7Yyh&wf%VT9G8doFi2fnGH%}*&JYp#vH z#rn17B9({&lAAK;1T!9P9W=cBWu1$E~5j+O#6>Yq9h{G8Hw-8ic6?uM*%E}QH_1U%|QX@rwo6A4>$K?TnMMC+p`WcANhnb zkrs-wF?MJq3J-RPp{>Y8bqp7ffdLLQ8Syj2JtMN@VdHQbKSrCUjJs`?y1AsYIRFv7 zfyEwdT!p1jY)M3d5$*h@3Q8VTSVdB~FgPNQ7X{%$c>KaXF^~I2$w=&41u)vmVC}(A zT028Y!2BneK*>Z*#GCi|t(zI+_q%9{PvY%)i>UI6DhqInKluthyqGy!y(er{P(Wcr zZL6!{Z&X&eq8Gn^^lJx(65aTXa7K;z4X~1sna&I&O3R3b5rvbUb)}AWJ8>s^x+kji z*lb~nv8kaD?PPT#L&HSz@VD3yiOSMyGi|GbFuolf6T3SC^E2HWeck$J9u9B4SDzcA z_q(jfJfSAF_+Ha-7SH+iZd{nN~k{r&yJ+K-1jo91_&SGPRr zq?N$pxP(r0Mpy3HH@i)g$MbGH{r#HYornh*gvo$U@= ziWP@L|L@DaKYyAVqjdi)HDy%D1$b!M$>DV;@MZTee;)SYealnKW((izKR7k#5GHQ; z@H6fc`x)zGK(Z#iB@O)kRT5#4`z`a8KZKG*>9F8Gzd7JWk9r4DlWsIDlK}N~nqcZ7 zSb(JfB22F|=H@ED-$im}Rna|fJ`faeih_soEBD)rIsnUzSvJOnf1wIb0G&RFC0v2v zY%e0?-RASxnLX4okAldVHfCSR&~cExi?azMXK2$?Tyzr?za96Y@PY>2GIiplA;b=S z2TvEQLEZ7wGG%6d*>~YX8Ly%|LM)36Q4w!UN(k8Ik}Ir{&}&TIR^Kw1yMB)p*o_y; zF+$Aoq8Vp&W4y*n7(Q+SfG8@^1+TtiDbtarkX?~@+{djnv>tAL)|m1BkF`jJ&9EgE zwNj-1GUXO8_Q8$qV_r2)utv17KuAKCK^Tu^;X)|Ll3L`Mt<|Gk zy63B!OVyXOU5^TRDf`F4ucn)!g`m5nRXYD<#EIf*C2URt`s}~Z2<4V4^8FCSrYeKP zwDH%s@+X5I8J)F^VwbN(!|digZjyQ9Iq&NYd_9F`9}ot5GV=pD=; zG)@;`+W$re<2Nm*X_NgVEP@{WxmJoblx<$-cG~+4w)^o_qzyJja0=^2QCY*0j| zSFq^DLa!Ti(_l!5k4pu2x%Fz^6ZI{9K+m132}rxTI{m(7$ufZxCI4|NVxe-))y7p& z5(!HDCp6`xHW`ux#FFv6neo zd}R0Z6`N??$A)_t3oi_p1$I_So}7 z=V;BpyCjN=U_B&V-q!DfuZwTIK&~J(IqQzp8yU{O|0bPLfk)K*T{;$1d9WH39jkg; zg9kS88$TWk3HR5Bgf-?wj{)**XmwpZj?i+n6~C1zsGbcS9ub$%|1$AUWQHS6HezP? z)WP##A3b#(m_P8|qa(iWE#LR3Y=LJ@3P)~n{q7dqUBr|+Db-0HIX{4~h0h&;Tthkc z|4BNAjP!<1z(rG)ocD3vzI0PRi_O*_XuVCar78q}DZ~(TC_x!Dq-dxEfc{1i;$zyi zM#k_{r-7!^PsB1wzy)RJ1>4vO%UulP`d5ZC&O9rjwa-!oZ$02#nOI1i0${9yU@@^5 zg)(ttucg9hWzi5U);kl8kVH}!LK#y5NIUYUSQrG3^`1gPB(X_S{aQ(@jNb*r6#1@+#Zm^v@I&JZR|6kYrpR$~_dwxY1<$ z*g#1rJP1o61Q2nAfa8$#K1hiEC!r%G5J%ASnA2ej${?xy+u#!d)#sACj_~M%c@RJx z0UA*>qu2A4R3tTyn9C7OB8f@$!N7C3?j-J36T*m&RNKjk!wqp@ic7v~b`f4!dv3y(OiLuwvtCCvZg3 zD(LiM9ZQs#Ih3%-M|zLmwxb~=7-b8!7gB1A5dK$h^i zI1;Rt2F5;D%-n!L`v{MUy#fd!a0UHMr1G;7MlI_jgEBocYtzNm<;pSyHxd}bBF{=z ziqxq+uyWdI&svF06&cNl2MnoZJd0U3#JIn7EEABa)NdzWB*B%oqYY4fSDav?01dVG z{m9f(Wi;{?R|sKL@hfqt^(x%c3}9-|QNYD)zh&pjWoIE!`8|M5zK8-(6>pi7FgZfG zF3^lNSdvO?uYM$jEB8&7rA{sq53`+7-|TWS_I#nRC%I=VUx7wph-7p*lc{3s%4Dmk z-;9%+hwPQDl$F3?G~8$~b_Ak@f5f>M17OiQj_4{Zp8`h+2{)QBQA5WE158;uSRld1 zFh@OJu0nJwFOonk4##>h@IPjV4r#CGYpRWJM~Uq z-@!Q0LovX(50JPjC>@2-ijKL%F$_Kmnp|MI?O4VdHm)cPNgU?i$4?q4h$|JO6|$y$ zaljySOB)N+k%igEfebgGIzm`dp9uT05HgAh_kJ{maoCn`U)>!+u3QYb9m$AN_+$)X z6vbXg_mQ>>D3Rc|n!XjeQe_s$FqTm1d0M=pE}F$D3z*pD8tI%dl8XxLQ0fd@sabD4h5eV6W{w^W+28Xgib>5eZEjD zG@)$O-gb3}l|YlHfRdx_(|}hF;Z>EZ*1jGXqEuj?HGw`A+f4_fYe4@igb)q4@-U7{ zQAha;LR{-Y!NWpaLE)Q}TmUzg2wUUK!)0-l-=n_XbLUl`aLt+2(MI&%LeKFmi*U-U zU;>w8fdMN`Zd&v4FeFj9reohs5<|?t$LY1aGA#tstDU`1uqjPGc~&|3;dn{$Sbxgt zL;kVs3w&)3JUx!iIiuDwb`?f~CFYa|T3PE>f^t6mz$r>QZfn#n=sMRutHa#BGwZd3 zNP^L!?Ipu&AqfK{R?vB3Gz4X`P<&^1&PlQH&d1KWr4Qe>M~;uC>kfG9Q|<43$1NH= zVu`MgdqbR!%_cCePT)cSr^kGj&A(J8d|p?3t|;;++bBb=wmjKU*s|V|1k=nIk2o>E z4iPanBNOvFCb&q6w|CxpU-oz6C3t>vD`f9*@cSnX5kT)dwydsc_R$s}r&($ii~}_ zg32Wxv0xlPY|bF5`Dx zW9(fq``FlVbwX-`(rXulehEYr?cy2(gVOG5}E)=d?i!=U@@t0Fmu6yQ$0ZTm$FZMlt&HKhIn_GS2=EzFzQw zQ4IlzM1VjBU4eTITaj6P4A4riL;mcEwMh4v+l@)C{G zlV+`!8HLbP1>0s-C&@=oXY>OvRL#@mGbA%h)9eRkO^Q4;nhfk(nl*cAwLa3?k4rwz zNPpaxrnT3sect?Rw)sg4tq!55agU^KaGEarr4IY0QBk`3!^?FZclo>hn+Zn;L3yG^ z-*3CS89r~|SQ6EbY_Zbv+)4JFWji(cG%2?x^vtE@S-tzQxs>tq0s6>y=7cH6^vw5j zJPn>4%Mh}eA=aPIe|}crDOd2!^45oYwde}T)|U^ZtsY8K>`6syc%nYxUp{+2W&Lso zC2?XyUtxAjB7y-4N^P|-kbYI+WzUQ~*ZWbF?J4MBZTiyFrsSB>!OJe^%n8y8Q$*NT zOS@J}J9Pc{^1aFXtsMzYZy9zP7XbZ3-rvE`)8K^P`m0}=w};-n#stg6zie;fX`L8p%M?`2dP?AV zD1&y10AIWYCs%u|5 zoMH z6rlOm2MX?5qV83A%^E`?(6{N=k5#5l#nuw4XclA0GVxf7F@T;MsSO8+;y_w+zZp{h zjr@*Lls-Q5-(xjREU#L6C)?$&{JHSI&Gb|g4t*-OR3i&PkTCS$z!x7_NC%)u#$p19 z#~?%&i!ehgTZJJ=z72AK@3kW1TCo=50C6RnAh(b(SFQ1ips6ATA$t2=K0JByXX*r) zNT@J9xg0R-v#rENuLD;3|Ax;3L@6>v8_MDwgAl^1&U9K&{C=6?|DOCh#-+0-#J6wW z$BbI`1#`FHE!j$eKNTfjA9FNH)kYk^+yi%&3GJ-f!GNMH_AyjJVGLl*1d$5$!H(zAQojbpq&(Hazmt8BsbPnj#S!F-O2v=|Hqxu_FjgKj%qA z-J{Q5gKa;5vQ5PGKgj#bptj=nZ}d$oev*($;&pdDTn=^BMvnE+HnXJs3HJMD-^}Rk9HZqO{F^L_b z{I1-rMJOUQ`_S)($R0_(uDK13aR8r^{jHx z@Kv;2XtJzodV>N%pGj;4Kg;3By}o6#KI!fW3FKva ztsM)1=0W&yJABjqKoK7We_yDfyFsg48k&qdR&k!@+oou8WQ}*e_a4;iMt;cyQWO1lp`~JROKRmW0K>t z?`f47e<;%v4uR~KwL|-*!nN$r5(1A{uQ$tnaos*|*}#nMv&;=9H&iPQAu|snjwZvM zL`MFyhJTBEqTr8=g6bINhWz89dL2dn-hM4a(2@f?k`Bh14-q)R*IReg_-*CyhVPCkN9H9q?d_ip_KOw> z!nwZNnJ=FFAi`Q4Y&1A}f5dgJ@8CLcCw1;xvR_DjHuM{>HNOU@Ghw?&nSc3jB)KBz zdGz7-z0-)%>b-jrt*e=P3Y@SVP^)>oe6>o1Bf;61p8la|aMcsNd_3*YxO_90D{bJH z6O8`gwp_JAdwtk!(0ZF27TV%HpY2NLy=xbG_u*^BeL((_>}5sJd7 z{Nf013kM0l{JdEn8^Y_kUd~4$OGy9;c{-7!eTfxML(KY-`Q8%8->~i9vP~#mct(fB z3t3h^LHB3D1NcM2a&gK;u55o(NG)`F^0g*xWJu_-eg(@<3C2a)vSdE70Lkar$1yuE z^%JRr6lveZxtilKQ(P!2CiZJPC=36l2hK}LZ)ke4kmS*#1?~Fz-@74vml&9zi1foX z;^T$CZmLgl@JrGW#woAoD=rnsX+4*Z65(WJr@d+6%B_j7@GO|)bNKP{-$^VJIFIRk zPWe^Zmn0vOO>QLR@RPo?@Y>BS!FeoYdZzQF!E#nUNk5eoBHff}&jQ>xFtE$pg!JXV z+b_CAF)(eOWNZnui&(@WJt`A2Wt8m1*KtPi+8DAfHn$}X9magNYqRb)*(FYIG!c`! zIR}63WXAlof|H)+K%W(17dxnjW1fMjPEhiKX^(x-U-N0F0T?_B+Ho;;`Q*L;W$iQ_ z!W6v%-oqUXzBQfHHzb8mGdWdVxO6kN>k6K2IcUDVMgRJGq*u(s>ZmQhr~57DFYrg& zj&Am#N}^4?dHQBHUow_DjWH=pws4;CACV4)f5fG>6@_}LFJCdeyI{`m2{RA}85C5L ze)WvzHf)WUZHagmTozSq1ay+AiG#f-{$$EofVnMiqX3|N5+AS7niC_{~J6i}NA%#qG470#R8~`XQA?8Bue`aaGX; zucN=J;p_L8H960Ygu?e#?INDnh5y}m%@zMKm1fh_U;p;?7n7w;F`v4H_R4q6)Jr=t z`Ay7@(MRNM^R`Fce}+~R2Yr&`>W+~} zJM-wA4nNH3wSX;sp4I=2|xvbGP<)MAZO6lb6bC2`-u!+TN)Gb_S{^b+RmQ^9a zU}b$xoOJqcwjbm}a+mLt{djBzXLp)vUP4+RsKfhlkw!2KH)N?KG-jo{g|eXaD{p~Mb%|3F$7E4%SuK}TLC$xF`WPw(az8lE@4;B|5D4E;r0`MgwF z;`fVu@zK4Ni$=}pLxC^pD~z01kz*Cgwid=yqjX*ktq-oQcWHH3l~)mFna8C^>1%5% z-fhe3JE4T8E9+9%y-(xag6!C5C{pX?9o1D{*OJZq)A_xheXxnDT4JJ-_6yQ=IrC%} z-!}Mg6T0f6oK(BC^%U(f$SviL3`*U3dDU3?somrKRok*K*83ji_LDUF#XqT;w-kv1 zZiPWqD?tADv!OA61E@0(?mv6a)OWZya4hYrqZ@Cgt;{d@x@rHIy83$PM_>MW`>>$% zsbzTi?L}v&zxb=D;sr?t4*K@My3%yT#XF-{F`3ZD;73lUQb%zf>fcgqim`;7)yqc5ufH^%@X7#iCR_!teNt^Rk! z4`Pi4{iy=-#y+U6{T(MPwFf}p0$_PJ@Ue0=SRDqISp&avgQ$;#4No8z;}9!1OjRvR zhZ9WMaZL9UOfNSqyK1bDu@IyHwt*XV_z5`r1lvgq=XETOg%-rJ7^A=qx1<`k;sm!w z09x+`ZLWs4pFq0=@Os_w2CDH!PVgoK@TcAI=d1CTPw>|T2)5h^cB=^vPY6y02rt|S zZ>k9&P6&a5Fo-)0rv`?93L_FEB6TOCtRbR3C1MmLW_2g#tRdz-B^DHX^345-Xw4JJ z(Ngqz4l7|cY-kB^B7`j43g0)eowum}hUNXeOdX7l=%>Lgt4bSgy-8}wFsQuskG=G2p|N3eFCDf2e)_zv^z`)d^78if_Tk~-zc$zZ z*T??@*!BPWFa5v4%Ki^$_y6Xw|L6Dq_Y(Z?AN{!hXrLMW|D}P(7aIM)479B1G+xKS zG7Elle5%Wn`PMI**%E1gFU~)8R#~9$|J^)bF?_toS7Hdj9K13QJi+PDc@L2)UZHL| z*p#PP6a@zpNP0dRXbhh@Hp0a8Q0rkFg(7+Q6vaCmk&@NezoVF~Eis}+0_8nq#w=LqrRe+*dg5(wRp}cJi4-7dY}6 zStE)w<8-Kqb4hAo9EvSq;7(B>Q;&nv=NUh?LPq7ZlG17obi|*6vQe(d5@Np=Rz)kh zSU3hO*0@xrDc~#Dx5V$b@nsc`w{Zmn3k%NWb$rG;H3ZW{G8i}#u&zW*{L>#zoALf~ zFGX1%TW?wj(qRp>v?jdYhs5rPYR9ToODBVW3`BN zu%{|pvATxh+R&5!Rqb@^`$TmN$Tq=F`h63j0Xn0l@g6Kv#-1U8ALTuR-8ww6gH%>D z-v?;Jmjr*(`Yk;jqq(6G9;d-E6P}=^%oLuaUT=d9(P_J!kAQ-{op=1Zgr2lnYTkME zS@}!GPvIJk#4S9}|2Q$j?jw0NCqApvIB)YO`l`@K5#7@O7q4cxUJE)F^BQrvIwV?> zUF6+gc6YjeI;blC&k;rTFrw0p_w+zOHjQw!w``IoDjou-7g{Aq@92snq~p+%K`G~XP5UKh6}bZ?S9c_4f*-KGqq z6p|zKu4baIQ^J;MlHn$E57&^hKJQKu;!vnz(=}Bht?`wQu&H4(nX)FIzwV{sstH%V zUwJY%mmth#6aCitx7>u_eeUu?qSPgxTmYrBB(g8{H`D>Z%ZsPe`9iro78 znzJ$di3{;&`Y*g9QP~xU0-Po(l_b(SEGq-}kLTs5uh@%nZ;#`gSaZPHkqXDmp z(XjbUOfK!N{ij`{(eL`c6$Q4|*`kIh^vos0rwb@XaBwnbReifN%TDx~qOcht#V*1)TpU5X$Vq%tXfPx4qRlT1bTDfew zYUe8kykAvaz3oVbkXKg)TybOEpIvnxw*#O6wY3Y}CKTk7KM<#U4T8Qx5d@qdnA=t{ zb>}+BIs`s4mapGhGvIH%8X$~!3n5Ow?%~j=i7?0r#*Ovu6^J>F2pG@vYS2m_ zQmM`eotbugRxk3b*09ytjPEB5UFRov?mNw6EE+0Y?iF+H;PB_obGzCxz2I|-3!Zt& zS=~0D!dk~eiS5kWc@_%=PfyYZQz81a$zQg@)~6Y9>r!HqNuwUQiY^N>2~_pLe21ww zmpfs3D&wVm=Qai|dxlPAziz$?>kcgL33ZbV3zME#K3g_5Wfz++V5_L|KiqlQB{n}^ zcu`^-?d-sv{(WMGvn;jUrB;f1ac!m{SKh_R$Be#=}fnNKshYB=38z4869wjTx{ECsS=*ZG`~3J z)8FkLxs{2HTwJDNX>4nES^D_zEVk&wCeGFE_lcN`P({gMZ|UbNPyczfEy=%oI1qim6RmOy{|=Xm8t{fi_Mjos5f?*Fbc*rjG) z{`|0IqUrx*KjS#!>TX@W%6+ny?jV*h;E+iAPQxi{xr|WqK#|{n%fI7lB)RRcS*OpE z0n5!V`i|p^qYuBHc22ZfNTL5;xI8R9mAoAL^zkn*+J9Uz>tUJjeeICuzjL(Y91^SZ zH&XNCkzF>L)yMzuPt~7TS^%t1=syA7SO~mg#6vQottjB)1p-{`f2a82Un7Eo;*-|Z z#}l%E+hjiy5u&+J1RDi{_!>#x^zpdp<3@5IsYqb40fN*E!Iu)yYv*536ezlbz^6b; zrg#g91d%lb+4#KI3k_I#5g^|f$WIYGs~FU-ANZmvSYQsR5f(&wjbOhH+W+}Zb_b@n z6Jk6U$T1fHZwj&NN9z9!7Nq!W#uVaw<^9$G=4J9Tv-{t4a34DDeG zeXjIbA3KCG?6YR^XEAhtm~Am~TQMxiI!wnm)Y~u=TodZc;gdQSD#Yr6C=Na=diT{A z_GjAag4E)?AmS*|sU2p$QBL^ly79mPDF2U#1G#{ zdhdv!Gi#$hYre}U0e#y^U+XEuXx14Jq&j+OE_&rUdW|AxQ3)tN4&L^S*-44nYl=A# z0SlvIPAI;d?!=rKez}|j^Jv9fHGSFd2hqbo0Loa9Xe`($7IPQ)>~l2dqaG7xi|b%* z(iep*_$aU;-ShyU*f^@@IGXvmM*|Hc0MtTYvKYm)`TfU0;|d3h!Q+jq!HU&j&Rq}> z0{A=}7>NE25^#fXP-0rU#VehF^|Q0DIt5O=8)Y&rXS}{q@+(eEO)an# z3MA;3Kmh}D8YKkmCYvDQt0}6D>v4zKNz;<3uQJITD8_B`Ns~u(G>yCV$6C_QXkR2*;EaNC{}h;N%3$0G`Ug zz;FOq;v|8c6Wqm=iu(kJe@=YeyZQ?p*SgummR&!GHToPt|)}deCadW1L z7Fb#r6tT2;9*1--OAJ^J z0M7&HHDAb?3Vu$BAyW;uVJOIQh?jxq@*==Gj`4bq1=JA*SL7ZH=$14ls&F&xm)2s1 z-;E0OOTZ3$$=5fp^&E=4!ZY3(=McqWXd5SHz0U~R1-*3pwzT_=VqBE)1f-!=92!x) zT2j1Y6anfl*xq$X7D)nWCt+A&B&d|QJCwW%FM7TUlF0%0Ig|?DmOy_2$OXXv91@G( z$Lk1yMFdiWPx2aXiddB~-kM;$@jn@ zszinJC;9TH8!+nhVPcrWLCu@9J{)lS;Ugnfy{j=G(U-F);puY)v|XT3g9W!D4RN zvX&VK{^?dZ7-Xus`z_6bPMv1qoh-MxK^$=p;vQ2_!x`>+CS^>~oDRIUCVUhKb=Qq= z+M%*mO`!$Og^r0gmTPtA`*y!i>E>FjS{Z8E;_f9MY!QTG zfK0mZOCdbf0QgXs>~Nb8w_)i(nF^k-1{4JxjssW6X+?eGQ0{H!{z=Y-xvAPIv6Q1} z`PjiIBnZ$eYG$l&Z8k=?mICmmw zN_d()RsH+eo%(%K`rjD=NG%gcs~i2C)8$a21hBefng-#vX6Dpy{oG9y)j+G^T=|So zEZFVY`u)vf?L*ds49)#7lz}lzgJ{YDlEI&BqC+|=BL}z;>+xhktLouv7BFVRbX*q+6S+3b)Y;`TFQ z2=o{xpn`+eB}T&xM}vk72+|!cO#sIbAUL>!QSzr}!(WOtJ~Q)iFwJvPOAo53k5C`FZTrE=2@w&1G^gjX=6cJ1l%mDt zS%aKVE;W`c6NxWSq9eyQ)D{h{?S`Wl9~&X|=CjPBflm;5=N}+l#6XZ)QGwaW#mA8b z(N6E?fuYEy*p|-{Pf&<5HA>>v#LL^ zO3ko%w6}WYINfKkVno}&$0I(Y3Md&~fsU>vr*%!HE|Yd-F16Q6siUL?R;k}Ena%@o zVT;tz4R*ebk3Ng6M*{<9(#0kKGRuS$i9*+M@VW_5Ok?$j*^eKY1MDS`%1EHN)|Qh= z8g-5U{B7CGmDvc2VU~{B=IFkP_(rv(%|6^-WmpGA&bq$T(zBS{pmxYYBv1^n{c-rW zspvM&qga8q+}_CC_8aNMc)MYrwcQzo;*V>@FdHep~b?m3@T&D=(!@bP|mDH-6QvY~)7)Ckor0zTh0sI?Y z)ODuV?Q+F+5#ddoYtIM5%K-F}Bk5gQcAkb-g5dZ~m*zMQY^a^^K4aGGGqJ!d%zwcfcE1Vnq6G4qBUX5{n_W(dD ztf7!LhIouAFnnSHP8z9T3hfQK2u`Uv%f$aN&?EzFY=1zN;#npC`M!QM(2{t~I)W*6 zx7E^~xvYT0iP`&Kkm?-$-GMNV=R5(^$fxP;J{o8cH3XBE8UY>!jH@T@*?uG|jF=@h z_tn8;*10j^q^|jmkW?JIa)0FA;lv?NUzGA%!h# zzVCctJ>Td9rN@Qh@zzZp?i+i7@Or4}u-vM1sZ1(s_6lVMWFX|GC#dd9C!SX9#{J5d z1v!&}Qe@u{UvfAolq``fVt=8}iB%f%%RUy<_?OV^V3p9)2qhx?cH+6*LFCG>_#rM^z2nekY|yV~tZ-+< zkKlJpf+#vl?pQ&V=x?+8pKF`6m|KE$i8H#I~u)W(ZP zE->fDprpCDUWRSSv)}oA`M{$_gnZ~+LswJ&*>@e+g(re+f*!JuRp zhw-!g=enBVrS=rZlfNp5t<*K8C6Cm#KM4!;>lGC=7-#6$mrQq?dp*-Pl7y-o2uFpC zO*|Q4ja!-vwnPq6AfsI(Ft2r!jI`*fNd=uNwi+!yy;w+Qa|z$@HgWCr;xaXxenq%U zK>PX2`mdg*F&*F6)Pa?}$@S#TT0}#Q)d)0evyHA|l7nRqF59(baH@`>cDQR$Q+wKU z`b|T|>({lfQ5g_QW6^?V&Gz}na-wXBKGuG=$rc?a8=mL_zS&t4S2ou_b9GV`cG;Yu zmg?AWm0SDTv83{Yyt zI&wP+_dKNa9b4n#XX%XL9+HEl0U-8JQPHxzB?oU8n?&3)dE zT+};*snOxx05{q@uoQPLLxQYB<%8oZ|J%KEbp&ahYKnoiN`t#t#Lc_L6+;M2?@A z!bH#hy33HPGY6mF$9W;Y5isI657siqvqS{rFq7~0k7?y_Ip{HeX!{Oew-gHRG&N)c z`gBVa_H*OHkerNx@)`E0Y?U^)!kUngFDUdG>-Q*DUP)a5u9khgFHSec?eC#+_wQEQ z!)XGkbHU2(@UMp@n|EK0+9}>>$HpYQks>S^Of~$)I@ebCT>2^E+lO=EhL_f&FO(I< z2&`0IiMas@K>@EdLwX?FEiv&(NRN=SUaaj}JT#R<#Yl*cKB*o-v`Pwzpts(uoVTtfP z2Yuzfi5RdKt1PLbcB`yT1}$l@y4MJCy4_42%e3^X`qFprTI4Hc5-m)(!VF%4W~w5* z^qu7+P27X^)k{gLi$%-*%az~}3q5)0XI*7xtWfcvJJT#5mdh-pW%X<15;NRQi>)ZB zT6%S=YM%M=s56Nh&Fs~Em+Bf-_ra>qcdG?}A<;%2CjeUC8DCsT;V>~bS=VhrU70wq zx{i3lbi+(@>BH`VB{k!G{Feq$bpgbUx_)u07}$|$;pYEr!E*1gDOmMLEr~l}TWYqQ z-=WG@HE@3DmNXvgx3hlK2`O2%XHyR&kNH%@6uK{zL2KM_kIp)fFh!_c$jDV~;m#bl zjHTn3nT{8yl8Nwh*3K=3KxFUbBMmS1x*C-+5CYtwIK?mcJdSJCrh_Ls#}QH zeH=Z@I<7S4yVwv38zyp59MTiCdt74MCexE#h}!3OU#sB3I3%b+gT8l3^-)_;%N&qN zgAnAiC0HnVe)thHBZB`028mB$%dnIBaWnD}(=!#VwoQ7+ zF#e9%~xZ>*9IN0SNNWoQ`3#DgRSNbt4Y`OS;P`MVNC zZRSU$jq2jUY~OPN&jgpZJ|>G4C*DmJ>Mrid>GPX!o_|ES965C|V9+2M?`Qz7?`IcR zuKIl1pSn4jU8PSy;sTv$NR}hsH>)Wu%`GR{2W|X2m}5sP%G-mQx}C=o8pe7$4QkUr z!?o#kWzmdr{DvShty$m8+Z41^TkS(k`1@ydepl1ssUm{1M@_f~)r5%3O zgA3WM*A(~y!m#We&&m|awlsg*z1~zMz8!5Wjvcn_o)Y~92IT&slqMh7LZx56=$(OwLkj5>O+G~-z!;t(}OZh+zJ*I9yW8!V%%DGU@!I*-_D53C7;Fz0y zC)|SAo`l3x5gWJVd_Z_M5h|GhmDFE@e^5g3RrK&WX_2Z-O(hU@Cp8XgEpdnUm6eix z=om;Br~E2I)z(UQpMV?oZDf!I%Mju2$c;`wVGGM(ez=o?hx`0Hir*$&z%*Qt<;Mr1A2+f;aJyPM10xx(ai^(U zC#m^~?nx_R0~tkH>e8_>Ri*#tbxH`o+}ZnqasMN+xu+vO5qc{@SjLQpT@$G)N3YkT zVAF%7z{gvY0eUb>rPToLj=zRdAx6o<{45J&eWE5Vn#L1);3TfK%8oXtSz4Sn<>hYi9o@)o^E$Z zvcC%P{qo7b`dpTJE90d#@#K!QWHDer>+A!oa*#H9y+Jf$XefV0&X<>dKf?rokk}%p z58~+Z7RCAB3InN%v2%EEO1YOdP+2vAoLaBEoFapBFETd%w|{?_8v<-E9`To`skMJ_ zsDEj`|I3aNw50%nS6*db!}axtsEYE}Vccd*VBHfM?hR(uo<5mU<)K#P(}97!6D8bY zK)xfC#utEP$;3U}-KC?rz>rMVh2XST4zsTV$8zwO55^QLVh#Q*o+>K&n$x%$mI>|( z)KMZ~X{(lt%R7*+fsv7@sLtHsmd5`qiV|xH7Hi#PY+J%^Um7wc9>&6};1!mvuvaV- zQ40kiee1|2Ov9T#NdJry@26oNn8F?`SIaP=s~#R6nNka3z+mJ6)ooI83jwiGxX`J= z32HW#&2sAcIwSJdBT;T6?ZN7(xRG3*k{Y!#KAMqDJe>8mwv7)hzoS_9O*LT7qgBD9 z9`U1dazWf$gYR-R-Y}}0_GsWRYaH^h9JR3>Pw_l&Q#+#>dqYxzU5)rl@-p38Z8lyr zIDhQZ_p!mvp)(Qz)O~X~0|q-bh^!cZTRje~0FYsW*a7?^LXzz{-J3>Znhu%?0wV() zS{Tt|Q7K|EYFZq-g>efEWC(zVu+n3p6o~*)^p7)e1hN1o8F&M6DgcyjAi%|Va2@f# zmNX)l1Y+LsCtEe7>JwCATA%k7-A;ulw-{19M<3KmkT49!901Ll7H=36ny#2a&@GS} z_VnazXvJi*3L8q)F1!DAG9hP@%QlCbw3M8d6OTcmLJisuS9mn&MM-|q=OCB_q6MlE zIZ+4>25dnF5bike>{mD8Lqrt{sZs#q2$OH1$eAruD*FA4_WV~BXbQKcfPS!w>V`&c zWWtstOrRQxYX~x^LvW<&g&2Ym34wpV5^-__zLuY{c{O8eJ7d!hSo#~G(HoDn4=hz; zG%61xv`0XSd-iCwT2x-qnoPGlP9M3wG+h2qSGjTTg#2)9Er~8}7$@@jRmfwN5H3{k zHI6%OCg5WB6J!qY3K04=uv8#$d3dI3NB^?t*HurAB>wCnv}7trwk`e{G$Cg- zE`l-fsY9=DCgN#i+=+s2n*z1l@rC9BS=Nxa2n0Hb7JGPlY3*VbQq3CwcPSjO3{Ej= zyw^|UHoIB?((WL43s&~(SN3~X4yKW~u`34`E5{?|)F{9S>+0Ddkb39Y>!;8EYFk*z zHn=79V_UxPeBJ1_*l>&ANPWG4ZP@6RkEASq{x$(wWs9^eBU+bVTQ7JVDPZ9<^ukyC z3s#b){}AHz*($Wp9IqOPx4pJ!3k>o%{}Zu7aI|)KXinT=c`9#3(17H^s3rfrPX2a^ zEoYsoVV#;FkbxnPdV8JjZJ_t*Is@4T109HpVuM-1nlTJPcCF3QuzpwpV1`>W1DYs` zt=81nz@GsOZX0xTAOdWp0NHOrf*8T)ztK+>ehXXzpypQR(`(N=)(8o#{)m}nE}3BB zjZsINnN0x67b?N0N8Fwa6)vi5OUmhrNy>WG^<>UAVR#MQ(}A0 zTb+mo0enIf#M+<22A`}s31p@SWMVK8hGCEcKu@}?Uo!XD~VV}#~F~cR)@xlsr@@h6?ip5Gjz8P%U ze~+F1vhrG`?cQs_J^|> zGrNp`jVAH;DC_LGcpYOrSyn0aI!U3jwvO+Qh-tJy#R~Gsra)}?zHI@JA}o*Io}A?N z&zm08hPO_@!!741r)CX1i0L2r$Y4I}V**vh+r@D_ZP`94LMq?@sTElB7N4^iArbMb zZQWoX%h`5g_sfX`0eBBR^e;JgR8>ub0fFf195Yv8v5;O zNI}qQ{o#0dg_2VuOqH6j0MWrkxlOKt74y41~1$0l-J8E_)i z^#{x+gaA>+E>UBvG{7)I^AL9Ri3bGy?Ky`AYA!TJ<*%856ZoUquUw@woV>DPrI9SA zAmuth3^=Th4!pZrIie7?_m|`e`L)`)DgYBOY8U60qy-YmGG|S3%N}$lTymDVryZk% z>O9htSt!`56eg)A7eI^iMlZM^EIDdGNx`)>r0TH*$XRs8)p*98ea3l>q*6@7ojrhl zivP0YR(~CVPRp_*qF?H28y4R{l4v6Rtfa7g+?zgkCHn{Q{y7t~I-c=#Eu;Y8RU^u& z&uhj5v0zUpF83$&b-IMH^Htr64LisP)5uq4#ebelyoi@H7?ry6;Mi5Y*Es*RE}af} zr&FJosg6i{cZOZ$Az0_B_VWq+WEqtc491Pv>_ntMreNK`qVOlLIVM=YRR& z$YxJNdSZ3Ekd~Q&yAOYe6g?OXJRIzHOPc~oiu%CVKxH3Vm05WkgG(kWZ@b1p+4Vw&{L0vWETK7bG%{9c6$KZHFUvaviOO5y0=G@u|s--S?r+ zJ72FWtkSvo!`#O!Yz9ljxf1|;+*c$FAnSu5ABX5`AH zPa|qf7|Rmd4hEk3AN(%9-mqtHzAo~O1%ROaVPf#-`mw7~u|Zf2puFUuC?yakhhOwK zL~JcgtjS;B?bcoMCh9Hj8Yg{5D&nNXYlai&s@wwjKm`Unu;eb8rPM0j*=$ z!sEUW-@B+B0K;4ugBya1^^VF6QE3?>;0u`ibW4r)kp_}z(|CRtkcUt>!^Upq6eZ9; zSh;R9af{ER-eLOx)J%<7#byvVU;+eU!lbcJut#RYC@g`|_Cc8AzB0BUSl38V!}oQr z{>*d!w#WV@v+j5i4>d65-}q;XKW~>bKd|~Zg-dm(eY#KgL6ij`*(^g?EN^4n{zX{= zDFE2|pin(naWJS1%f_(8FUJW;tKUL_U2cEM)E7nd;@!c{DRX};lVUuN-|0{41g2Fz zr4r(Aqmrf~5LPxW|*i9GAB;o~M|=C%I6y8^L2%NJddx-jZ*_La{XlCRl0(7YmK@mO-fjC_DM1F-8m@7^& zPzbfl6HB))010l+q9F+j3PE8Z$iA%G7pIgM&Mx&VFTz$maSA4-&S+-r>JWjwMcY#y%lUqt;aTR>e}iPqP3)ke6~ z&6LmDv>U1m7!_zYj6b-S5SH1zb=>hTKWd@2){^<$G;uqPhTxlORkh!i?wp0+A3^!4 zWUlK%P%bj4uQ6D5$!<>2#zMiM)ff`NK4tFqzrG3yeKihriuh+O;HjG-<5>XB*vYl^bC?*L8NIP9Gn~2)m+re8+#8@6{;=l)YhDElAI*+hLS3OPea!HM=hV z@{>nAI(mPv-qUm}>nlvCaCs&g2a$8*oiTJM`Xk4#^uF+0_CJZg`Go|Nd*ABcKI~T; zDES{P(TBFaTca-$4`*f*`!VELi_dxL-s`fkRCpu#5xp|8`03Nx+AHaWi+j7Ok6Idq z+-D(U-oeE5?%2f{MB$7$N=gpV%aF z?F$<@QS}49x)_%IU?mO+F&46`X5Gj>1%5oOg3GDQjlOGPAHaR;R1$e zeArO+UIS5T)kNsGF_G04%aiL_Mi}2SQPlSbLW3U-w68!YmjLqF_nPRpADC%=iy(2U zb0FSr%yj3)isHs=AphZf$lmo&8RD}quX$LQp3Dcza($2U$NR?29ImLOA{5V44aXEJ z8Bl-sJt6DfhHb#NPy5X&h=+ldt0D|346jM7L{BaLCv0dm`8}zjjg`+MH5j*F=syM; z-(%R&lDIa-fT0_Vn=@=HRGT_#vkiXyb+Y410Lj~fp4XO)xV-bgIST~5XdVc3`dpi_ zEn)ZMGaSS7du`^fX_5GE%2D^h+N_0dMUwlXV1nSB?0@&{a15?-q_JH}p9BY=$)f`; zRG0L^a)q0jOB+%GGvP5!gqmT=W*A~)fK#IJI<0{;oLyXE5>SGH_{oY>w@|C}QNr9r z?MduYVOS=IhDT{v`l!vPyxmnz^i`?ofC>k%d<4;3+sXXWDFo7Z*EK~YDWP;KRIq+c zO;Pt3$(de}`Gcb}*8)-YXUt+-(miw;2LcHFs>aUv5Ya{jV=C1m<<%IOHM6_asiCxm z+KA-5s@h9}PLlY^?sFMFDVvK9OJug0jx=J7oDm_qaD1Css{Ic2l|iqdvUi|t<8p7l zm@EYEX_yRARw)^lgr#pFt+T#-?ew^?cTGd0z00#R!&bSZ#)Ro{z+-*Uhw_!Eu_dZa zEYfHl(cm<~yZUbbf*%iCrXH*cfLIKXbvsU1d&(ZT{D96Zy|HP0x_jvN$%$Qz)IgNN zQpNEH@p1(`u{kgESbz9C23|G5?q&nv`tI8@UGH<@+G(g?Z<*yiD}^h$qydt(Ron1J z&dl{;kT^vi*s?Lpb+I)VdX8RPFUYgPl@guN4f<0KbbZn7E$WtrYF(r3{N6<-c&s{} z{`#VFHb0qF8@xc4Ng7QdKUT5)F~RvMp3*wvOa+ByxU|9W-XK8ec|UP&`4*jUT%Wl@ zLiDTiwU}m+_9vst!1#IdEfOm%57skP*>Y0RgzEyGi9pR5wIoJ?As3wn(^L!4@6_Qh z88tPK|0Ao)LRKn*{r=jUfu(hiVL+*FVTF+=Q^(AuL$re z&mWmKDl=w@e;)4a<|2f){2}V>SvlA!%45uM{q0+D3qV&T)A?vHX&%t>@2c4%2MJrGYpcY*WNcYk$1CU7ER^Oy+o#fq zcXp!wwxls=FGF)OMj+wLSmbJnHT)K%b+K?X=|vqU-#MdE#(Rp!tjD1or!lKG0E6?Z zEcdNEv*Tw=XS{w@RK0F`y8Bx{Q?G{eSA-q0j^N_QzCa!XQVdATBQ*YN9BBSGu6Ed* zJ=ZymN3cd4CiIM z+ydS_YZwPu#^Aw9t7;#CH8$`ipof84b zg)o;$D^{ZrhQXTm=CpWS7z~*BJPHZ|2s?_^)8)mL2ffvao-<(bj2U44V+eUQKE^P% zML6xmu5lF}k|aD_v)^=j!FqIhG~bA!`EV2YyV0UYwmQ1n>H%K>&@ z$q^r95sae&+=g%h2?PlYlSU5!9!LGdkwXJ5QVapsu|UW{*2zE@L<bqaG(9y=u=<3AQloTHH#dhZM_Dadcr| z1BoTz3c?EVZ3y(bB1SOqlPMJvtyzjG&Lnbr3%%1FIlC1w)za{_g zgCiIEO(!F!4M~=)b~p-zC}PmW@Q`D1RQj|fxDf?^_ny2JNdgTg@@CEQR304(!D$R7 zCs&m026T8%osc>j7AP_&E`u}Djc9G3jKu(N6n#i1VM3!swGMjJ3}*f1-sW;mok(x{ zk_QMQ9A}^hz$b4{}4pv4mfMu z?rT~ek}DqahMvA)<6j#9E@p~@SZXASsHo`oriNLlS$u46VX|C_HIj^6i9lfA@sGOY z{hVd$h~m@yuRzTEKlqMH&hOL2kk3UT50%iA=Hc*0!?#PEDCa>O&o~XTQVb*nM+c1~ z2IO^RhLpwAU)p9Y71G<|pMI2MFh>8BLq5VCCr5A~kID<#{7kCjNWve=R6Ce6ZFHYX5r*VoG_IdLS}bjr%qUTr*tmYhii zRZ7#0Ql68e1K280%hh0uJ77G#OM`S-JoLu+!Jb@aGH4P4FvEd7ToE}s9a3c&zCp_4 zLCX`){Jf+-d$rHm2zm5MohL>yL_4{>T!g^%c}0@f_cEc)?@o74 z=``gTW(#(SB-EL};d!Cz>Ja^FCE80d=GO+UZw^~x(S!|cc}{MkuW_=t{i}Oth)=a@ z&{1C`&!5pe=jlOGIIzw%o|ZZU&L%guc5E5MHL`#F`s7&o^2LX{7oI}j>tXw7+*neb znRazN3Pk|!JLqtG(r_erWP!dbF`_H9cKS=r`128F_YF@GtH$1JOSO=d4dS+kPD?D1 zDLuCo;gk0Qo6T>WQevy;b{Xak^TVZ&b7FuxGjMW$4v5&1zLwjS9OteyW9Lsk)l1j$ zdp=|E$)^>w&m7yIQ|rD^L%M8G)0S&9Y$zDAZc3SVLmpm{Q>*;h?Ld`}NbkqinCUhn z=4c!AQ)^=a3xH|wA55zP*SXLycb9i13)(;{8}jbTs_tODeQryEQ3v-HcY*Hp!bp(cLQl6Fc;Tr)FNH?s{AK_{}buWh!FQD69BNK*Xr)c&pXBp z?3)OhFGSD+Y@ihSM(OrC#Wk@@K)I;49Uo5f52f9?dXte6b%}eilDtl!4rjvfz2pO) z3h>}1jl${aa7HqtBpSXU8&uHs(%nLYVLyOQ<{+GUSj=ETtN@rviTZ~oj@aN??9NoA zQ`3&~?}SlOMCM^AHTggSxgVHnSPgvt||x5e`-|HYu`$TQDx09w#dqF2cw|Y=J!_m8ansXABv0 zVG4KOFRr>&#!tWce(NbPFA03{5}->Hx)M>i6A@xEAjH;nF^P(pEf!CtN%D(|4yTEB zJa{sZMq1kh?|eWQ1dxNg!^>*MbrpR?w7P+YEfZTMCt>^y&COVHgTpz zUsqw80eWy++I*_d#^n>EI{RZB+DPQfpviDiBW*Uq@S>ce;}ybDc*bN?E-*^wRa0xV z$5eg2VcxgHZ=YmO#}kmKS@O#X<<{rwg2nt|3Uq}E7YvGc^NRZ8DpzzTFyQ*bbV%um ziL;!Rpo+?AbT_G(S_?=iewoxO<_1SWG?8X?aRY})kO7*+YFjHU zA^%D&E~a%J*GFDzT9@@|np@mc#Hal6m22g)wr1OcX#~}ovWIlzn=coTxD2Bja~%@# zS<1HcDpOK78XG3v%!PoU1=9Bw`E_`RtO)YU%qm=) zcO$1xyAfsKkzkaBPUFv;N&F3+GBuX*D^tAo<~8>DaOPvD<99l;6Y}frg92)7W5z+Y z(g445=^*9V1)<^~=&BVuCyt{?lRSC7^3dXq5j^<2pZ|%U^z|dHJ3k#bh1u6NqaWy| zXK0Jkm**^_WUVOe9n=knj*4H)pS9~2l#lX_&t`@%_is!@;;Cv>V5eJ%e}7D;VTyl@ z85+ezAzt5+;^wo#2%*v8!TpCcYxvkN#`T1)5}0!SzgttY=wNvU{k%iJZy)8#5G0jqL# z?CCrr5WwvO{2Pl;j0SiKqqXC@N?XidC4DG%QYqoRH4+Xmij-WJ3&^{nZ)J=kJF{!k z3$Tn6hA;X5xs1nqjI6>DuhHJBG@EMpFj`gf#f@Nm6Q38n;X>}e=QfEZa01gKTu{bf z2B?+GG(aCKiX+uj$ANaH;q(W{JWF(gVWjd1CWL%CtNJ*vXsUgn-)BcXidq_tol<|n z(Fs4ksE<3#CAn?fe$g~M+H}G6&OPK&$`%X>7)1>7i$p$f4Iwv2y$K4ZCl9$dQ6#tK zaM3}vJAr*-6obvfBkoalOCe-P)Y-?u4AT)}gpF;3gwar7V_0F$uo#zinqL~UP3tJn9{>z`8bR5|+JE&i5{jJF6pc;<0Qe7FuQB+n+9yFx@9v%YZ!sT>lLWm8} zWM<(IG4u)l;4Q;L-swSNxFXCyl70gK*+%u9zwETz>_WWTD2}gix!HsqXjsVtA-R9hST!_h+O5s`$*Idl6D#ixvUV-lvCKMpho?J zBftjja!NW>u4Ez9o{kKkZD`FGvW5W2qN+L6GdV*&fu)#aR?Tp{T1y1}Fd{yOj!JOG zCIpx$m&7C&NiLCUua?HCcJCjS>VPdRHEe%pG8dPS{piJ3o=YhTP$(OMOCh=R9fePw zY|14BB%rO(E>=$G5y!v?A^2osfL-#K#44-7$Tw@xTuqwHP>8GN87KPO76E%#i9Y31|3x_59Hj3rB`zDr* z}q(%@UjkrwH2p zA}K?I>E{|ivb!;r5E@uDfFOBjCzNf?wk8^{Q5^x|9w}*yT z78S8;1|o@;yY;OnE(vlH(g~swb&`hrsvgmVt=6cBM<&U3c#JHndm+r0)?XzIvFmMN z%+lTNcvJ+Tr9ep@e*g|8i%=^<61{&xK32)>e*|6Pv^BE7DZL}vIWzH{(Y6Shb5Qsf5=jFK5#nAZ@ z&KovOgL}|BHC5*e9z8Zhq8K!kJL$ph#0;@*QEBLqCt8}mdBdx1@x66K!e>~jtr#uT z0izTdJa2FTk}$PnC#F1t;crGJi&PO|hz zf%>%5u9?~BlbJ&^i1fljXmS*eLVR!poIl<$W_<}!*Ty-C#0^;2r&5ybep(E*^aR8{ zrbQJ)qJrY2LsgT|MO_5ACF|lAw3UFpq)1P|T-Iw65wjedoZoACgd1Jo_ukrK)DIi8 zEE%i6`B8wlDI-;NkBTSILO@(wC~I}yEaSql!QjlisqTcb%letVDqY#*&(N4eu2|%7 z5&s$IuyoS?9Ee!F>;r*tRur)<7dhE3JTj^13qfsB7}>kVfzTzn5Zo6EatqdA97;u^ z`WOY;$IAC%6^TIDX98TttO=X=gU)1^H;^KaEYf3ANW zHW~Y@KoN(=3(gF~I&Wh~$>gn>D2R?V)C86iH6#>Xn*9~x$TwBiY*=ivpeNX_sKars z@WFzoCDUIzm$4#SCqm$z-}2I|I3`8|12B+X-{%PH{qvD6TvT2Z0iskY;}ro9KBox) z5VvB>6PrizkwWuaqk6OL!4&hi}>%^URx0f?uRtqTg8tn%RcPzq>eE59iLG7gbrQbT;f4a;6rD=JUcz!zBP@qlI0>L4fNQTIzlKG_>t3*Ta zt-IKbhlw*gRj*F0$fa~QANqDBMcCTYiLF*9GvjMzsXuU(N`F=olFgk!KvDlW-zh$)|&Vq z)wukl*T)WVl$32aEVVCPiutFA+dGoIV%c46#|0?lyS|XLiT5}+rbD84c>k=m_v5

ji4ZJUFzms{6dYQKuIrCT+GIJbCG@-Ncwr?*IsqlyaC=j+hc4%6Qyi z%f}7<0wGmhdG~flsje-1H0q&u-uwrn1gf>TBQID}1`te`-#>lTwWD(w>TSvSx z(dp}{vboEQi@qxc@8S>f+x+JpHUTpi`5pV8=lCCU`!t5D`g!v>6iDL6!yRFt4Hk{N z&<3rWS704#9*d_JSML2se||X}@?q2@7mxXiN=Wj5`ef3~`FMGQI~dqKnBviRTRW`^ zCWIP~L#0E6rVTX8@jSfbV9rHhOkg=EvGNdMG$7pine{jJO!W%d9aM@wc4lj1mw|!Q zjo+!*7AE8Q#+|5N&uqc(BaI;n&%(gX(}L^1>o;UPo`KsWg*W}2@`SO4L0_%D+)VlP zpPE^tK$?ZO%N_SuO@()OM1#=H>-+0Lrr`4=S*Rd2=Ffc6{jUnFf%XV=wZVirZ$aZQ zHDd103*X+I7GfBX_ZL8+>l{GIvZ#f|w*jG|nK}Lbwi|V2egvtux%0LSPq9 zJSnV!b_Vhhf=a+~V#k3gLiklKKqvXr(D>7*L#H|{W8S#GcQpMiNmjC;gb5!Bt`vU1 z6M{t%!?cF*edGyB;RITt1aTh-Es{Pr3lVp~iF?kzk6;b7GnK5y?MOG`J0aKBxl{l# z{HqSUtI>4h?5B6=igz6;L&~RLLSztEl7xB^q7-6MVR+La)=484J|`0uUhxJ&Ig0*o z15LYWmaT~*FbbwI1hYzb@R5_0X^OJY6aGT?0joMy%^b1EE*^m7HNno9`8l^A9k=saJ1Ny?$DMMNg)3UB2OFjt>j;+07!E zzZk;K5f)1PhkT$|#04z?KWG>jjU@Cf0!qpQBT-P*SdhDlLC%glEcCr}C~gq8i5&rg zLy4mSAYVLCJA$osN0WAuXUdc7ZeE1*f+P18LU569N<=d07q?UzTf#i+%6o*DwKdle z!nr7_XkVNjkZfE`Obo}1M-ubYiyl}*-FNWJ6$plk@B)y+L@W~FsOWu^paZY?J_71n zqz(fSvlM|~mH60k92{eaIctIdv>Nr2oQVzL5CZBAAT9&o0q}_fka!r__>C6mUB!sKc9N*)gfU?fxeF5fB5dNKqB5x>7Mdbqtq77~ zQ8S2`NwL&%5y%yZPj4++5(BaPrb}G}iYx-!)_`^t5N1 z;*u`;Lfr5rg;a;G6l)leaR=BdMjVS2MV3gLFykWkbTMQ_q5^Rg^+?g8Fyagpq9C1u z1t4QmghvnxvL}3X6BZZ0j3nBH_&zY=S{AD+lcYxw>?4U8k%XQAv0ZE6bQmZWiD!y_ z$pz3OTePq}w&)VG5<*BC$`e5433tLCE67{X&OMU@iRA0@DunXN4e_4i*j*E=#gR+8 zrK>uSQ3tkhb}S-LD9}<-*uQ3=qdtrrYQk@g2MZ-W`zmQXNGPTNRh|LWwng25^}E|N zjRzt2$IL@*Vp;o735U#01}zCov|%i$oD6ps1gHpr#}naA5UN@98zD9f;S$hRu7A#2 zBp8Ll%Rn2aqs4mTDWYG!3Kci5K->5kNMP(p4U@?Eq<$O98XBf8+4(F>3a5#xJmk^H zm>FW1VL)2=FoW#drNf4U8Ot3%LY!>u*oV-jAucA;d$?R}&V(#a+fa5v3N9u<+?lpe z1VE;I1sJ>dXczE;h(l&v5ijEzRI`*BfksVch_Pi-P~#KCw}HHo#0W$%^+ShrKjJd9 z@e2}L4X8&0T=n|5{0g;c)>8>Ri(cV4)P!R(9GaPa&_FNdH{&NH}XW-lB zs~KIS>jy9?Dnd|xQ>RBE8Bi}VYem}3MGyfrkbcOC6%C^PfEWk7I5OlqF=XX7REZZ0 zg!lyxG)I0-uv0KpTaHqr8&Mm@R?&cn#af}i@ub3wpurWyzMLOn-=GG2p?fIkJ^&{O z0R0yB7L`VT?d1$Igz2$9d5eH$px%a}ZOW|iB6s8nK}7WL5yl2kNu0Oz&VCt);9UeX zXb0MWBsA{CLs>_BV1w?XpbJQY?II|T5)rNhTYQnAnRq0Vet@5`M{Zh7et8vtHrlOd^JYkJBpIMTt#@BM85XVlIhC2 zTZyc=2=KVc>!ac?ugtFcm9LPQ8fZ|!n~^Cqlqn-`X9LT=B261z4ZxwwSCE)%PP6~= zr%0(JmvA@NVxfQtRJa^g=pbJxERvT*lQlsTCAb!)4GJQnEtWGSvH4uM?U^Ull_zSN zk6Fn|nJFd=EqS1jJ^ZLl&a`-TI8`I3{F zc3$@-hoUEk2Q!dU;NoBX!8B^hwDh98`dYHGDaXhml$4>V(a)_uWv#x{sOdDjiutyj zo%W}yWbIW>wT~nf@4ApHZIc9@nKEs|qlYbiw5`JH4gBl1n1CWkPm4)lemZHbLT+mm zUo&1ExyDpqOsgW~pY@t9`Xzrf=4j6xdeY(;N1yt*WQ0PpW4kr`dt7x-F1sk1nq%uw7}T z_na=~$$DoUZBe#a?_atuj9J~E+nyN70aEGW2ohi>$qi-RV44Sj+58V%&`{hgfZzO& zko0g&!w^QI_gebju%E}UQt!2D?}(4!h;H7Wdyi3{(-Dh*1mr@4Ejc;Ba7P$DLcaeQ_O0KVgVRIQGJ*SdCr<>3GEPu>|p;3EtPGfULiDAXcVp>LK zVP5(ZrZn(V*xiCt{*u<+>g?^xi#KZ#GBqiiz3H0^F@1Ay^VhQT*In)wR(1S(=1WiCz~*M@o2?Lw&Fj96+R! z))g#vwEJi50`}AHw(87QSQ%$^@AsZuXoRVdnj;0u~tFI-HRC zF}k&biMc;+>;G=XxJMd%V$%QP{C?X``)H35eyK7{N`wC@(Eb**`e!n5`GV=9LTI)p^*(U5 zY4czIGA4gj_NH*)0vMW(4gnL=N{>%QQia3lL@sTIi$`K=5qiDRI?Q7UU~#VvXKb-4 zw*N&qfKLruZTgPStUCx_^!;?Um_v1~+L=~F`eR7s50ezOxip!Wm4B-bxSy7aXU`Y} z7&y;IAWb%FW5sJ$>dbp$d)M?iXUp_n%9kH{JFaCp6=t}p`#77oc(}BucRl1VZV38& zdq{*?-ap`L@F80AFQ)IkK7o1f|ieLV)2q>2kt*^;Gc>~Gr68lK#E>J z!?<*ihEg>}k%m!m`_I4mtEFla0aN=-_F9)lbtmL%;=>mMoD8F)C}SIEn^>N2Q7r1T zl9ViVtnS9PQ4dq0+wtOUowi9&B5|<_Doo8jMVT?iK2_2HVW0Z+Np=|#HB!wXT}IW# zAw$j9IxNGIa^h>Il`Z%CO#6&%hb-s!t6y`T6&!NB^Bg-Y%kh3P0iu`vIpO5z)d+Mh z3ME!|E{<$29;19tKj~Z=UL6uynkq?fSe8K>J64kEPV4+03p5>8+<_V!&>q7B= z48T~ijB{&WbDwhS*a{|e@7zt)aPRt75a-@~R6ph3^Ru7OqxaX0hDYD;tvHYV+n-Y& z1NZlYo@fw>rsp6oQ@rO8f#9^~FtIG`wwhKQO+9d5yCqX?jg? z6vlf^@-$3)O$iLZUQLV4YQFmTa6A6hjO6L`tJxG@5QIG z*xUpV_&=%D#Y~VaPIwR#Op*i_2fHf(fCB~-6O&L<(eZI|sy-AoQg~voreLP6WnyS( zYGPu6#j35W?Cl-QZJrxgd1+e(XxpOHY@+1svSnTBWZk=;cn&`C8k6>!{eMW+YCiqX zUj})4zQo2WFR%ZFRlokv_%E#bzr5d?^8@bGY~R~-`*gY~MhfhDVx{xecj zQ!_I&^YimdN=p8BwYt5%y{oJ1{|Hx)jg3uAO#JU~HI}UY?=ki_=yzYy!Ktu3rw{l5ZhSYrk2qoX6NV109Q^M4eq|GS|4U#9Z^di?+XrvI6U{vWRY ze?IVk-ugeE_J2MWyY>IE1NLnHzg4T&Tt%tN&7y5PQd*?)aorQy8m0P4$A$1 zSF7oJcfvnVo6a?DD#&i2hyP2hX3Ovo12bk+?#8mu=m6t{#>T_qL{^M#A)=ov?V=z5 z>|{-Nas;(c7XPhkpCZ9XvzM&iJZ_)HLyP}4mcrRNg-p}i9*=+kvBQp|?mC`K@+w&@ z3ha;w9)q#hnJmDpdub|iTvPWs)I29$obtVo)X1_azoJlZ`Y-bU5bX2WFc^;w6`n-^ zlw8PmiS>3W^q-N?E6DQYA*;xJPPJ*W@0u|r8P30?J6Ei4{sP{^;Fc@Uu^s!Dv~w?D zy|D|w)U3Yyts!kn&zT6~RBkEYu;~M-w3rGA|1-0v9Qc*}*$7mywowHZs3@Q++5>Bv zCf*BbQQ>~HCoC%DXD3zK5sH*`M=4Q%L6>uY=@T{bfNNV;P7 zqL1{GRbZd)EbNY+jIix?>JsVf1il(3Q_(yh^e+s2H1&ENBcr!LP<5iqSSw(CvhFEW z$DiY~Xz^o9mzA(*`nqwoW;TqMbkA3-Owg8Po)YaCgDL;Q0;7N0wJJHX8Rg zKQg2QTt_M0o}7H>jmm>FEQwgNAAzdeNsmpYf~8JUdbY%W*u~Y6uF{2kOgYCpe5ymC z>5YAbqL?mm+u&K6LA#D3TVBW0R_=i(4caro+Yk3FQ+`vf%JD-w3X<;2j!L)xU9R*4 zA2y)QA6Wm||Ijim6YeIo1FfOmi6k(cBWjkX|IHQJ`&pvFbuPEyQbb-R!MGv7k@X8y zSZ^pkPluI`4_b$%91->-#P~a#ABM{0@T&PxXJ!Zxrm*7)v%qOIo&f@SqCzU&hyucH z{e114+9(e@6^gT1?PzoVz%(in@3N6lXAQzr-6S^Rg=${PtH=@XQwbx z6SMu9A{kSr0Gb%0uq=z9eqBtcZ#*d%pqH2@&BnS2oy?1FW-gO1=jIQcjH?t$uU#tV z701^gp?sN9So>AWeInWXmscjmVYyQ9;gt20S6SnHjtogLLp~CE*}OL2ob?oQ zD2l&{)tNkX_0>;$z5gD6)r9nA=&Pi|zwd=)Z!j~=?hle`V8w_(651YHEgZi!p%Sw$ zGciSJEJvE0G7M`vG1A^Fq!9;-{HF8K<_0V@`Kt0Hu_-Bx_Qm8rPI6x)bYA1WE3aca z(4bRW$j(|qaDA;*cgj(<1CeKe5~?&pCP#Am_uoS%s#K&1Qj(O}$`G3edVcB#Z^tew zmc=S{nX6{^wDdAC8*IX&hx$#9X|*=X-&VW3m+fN}tF#=B6!Hj2E1x{8ep&w9(58E~ z_-FciHy?aGe9)(^$z^8mH_;Zyu|~s3KKXJdyoStXOXzC*MjUQFTaxwU5lQ=|ShpXJ zOuX~smiq6Xm6to1gqnmnx3}auL_5)e)@CuWlq22D8c~st4&S$+ENOn#3C}Z^N;mh3Ijdas=bUytHoIi1?^tC$>cw%EX zr{)&|Gh+yue`Ca>X&b^)6o`SiDcb7n3u*4>A#&tkj1k`#@|LVo_TckaJ3hMyJ)L6@ z4TTexH1`<%IzK$wI!g$f{zA|hGI;k4M42(Y&mr|^LMO4AjIC&&%k0mjE=MRqz2?x+>UOc7<=DeH;=f&^=&H5Ku0@g!C7p>)m}64Yf+Vg)6|BIui_2YAF<2J z`Ap_JLP^(Xw@uG5%=$MOjKqw7;$`Wc=z;cI-jCToUsas;9vT=)&b}(}t_h<*(%oKO zGD|#9lS2X=)>gC_}v?oxbwjmD-*OmdgpWy{P@>?2P7aV21 z2Miv&#|58y9(A8aJqg?aVtl(qx0(`XAI}r^yS3Tn|KPz$eidXnZ&ukl;KT%VOEvf{ zKC=A9LLvQGBgtzncjMxjQ{a)u>~(7g<4$Qq#Sc>Xn_)Ue=4frz0IKGoy-%4KXwYmeUiHF|S-4R56i_WvHd{r%0K3e-RpD(_Knnh}chC8~?+?aqfl z{DZgc5L8xFuy2PIIhXtIDE*sHZ`-a>*y>)siqKzpZ;3j6*`I|nIfV#Chw@gwUcCfv;g!guv(UXVhUj zkHhp-!@qZgm8QQwz7EsQ3d5?^oeq%{s$oEt2pBZnJUYS>8uB7K-1~FH6Y5tsk0U&@ zOgOV5sX8NkJELAbMv-A#496`@F;`J3WYP8F(JEP?Zz`k055grshowhHG2i$lXGx~2 zTD%*NR?dpD@{fw}kEx1|HXV<>po-Pf55N5sBj+ETJsuT%fa=PM&Q(P6<{7k+24}QCLafe{Z)#ZG#o8zg8w} zKTZ@(PTcWN{B)4GIG(uUY(JRA`|P)qp0dM@vqNBelGaMneW!geWwNs}0QYq=G&`A~ zD;egJEW87T)1;6|q>!7WP#%Ki&yuOIYPDJlmkt(Gh2&8Xx03X`pn4}qF;Lcak{H4Z0<9~lb*3tTaqckZ#Pj7jW* zQC#JXxbo_|fLzWb09ktAksSBrYonW3i8YZOo0YH27u+^UbXxW?HFzS*T* z@8oE*AsFFoJYk%C{G0>)9BuCWc?oc1SB|80dY76Q5&)J(!IvyCkj>A{R?jOT%%vlEt8&0Do?vFaW!d3}Kf{S7~(7CSiV{kg~Ou*)YZvRKxJsrrfUPhTywdvB-72&Aoa5f{Xb>8n6=7i ziSj)EH-@*ede`~8YVaNI>{z3BR%=z(Ywt^|APSy1jV6`G6Yqb}Kt$&NcxS@oh^#s8 zbah0AC;~iIRp|z=(W3!d=>Zi`xNj#b6W1z}Cj-7cs;WgqjT^^zj+YnQmfP@TsaxYT z#(*Sr9K&L&%ecX{x55NFfQLKa#_kGz8gS!kp2=B7H*GrMkuaeSP*taFNU~PPv^I{5 z6#l_#axK35CT@G7e73rvh&HQ*CxZtL)<`Zat*R@XsIhDYkR!m{lJy3!OXRT2_Axjb zI%UVS>1vH2*jzQ^FhCAae4*a(;CX`?Pt|yL!}Lva4^$LD*H}PP7lM(%6@!B{0kuCS z8p9-TVLCuXbj$o|lb!_RpGZcGX^Ap#x*igTL6nPwx0oxpIa3n8UsXJADosZa3iNMa zQg2~?-r|M^YsA&)9=7PIf!X8QyVUDL4#7wv9L2vyS{msZC=gLmos+>!YapMR$|rHSa-Li;p)Rd}j#3FoP+WSRRF0w$B=oT3 zEpK_q6hT;EIsbZF!hr= z(uV_E)T%2g)Y~eKvv}fS9(?W|9*=C)C~wy20mXp@g)+D>aPZ+#yUA-D=QAAL8URIO z<`7{AiD@GR(s6<}eJU&G^!fp6Uq>6Y%<^;F_r+|sVN91m)zwJMd72L z-$a__)(X~Uo2vIwD-E~47?yyI=uNyQLV@;p$JC`q6wh)fJm2eQk7>}i5eHz${q*tn zR=s;Y3U-Y(g}uQ}+l$v|N7lAG|LA|qgDY2LTZke2VEnpYr>s$zZ`2?t&f2)sL^D2! zZ`h^=ZN1is{E;3f*%$#EG_f8>zy|FQ^>*7(#d;k(1pasvk z77s2(GQcgjy#zKAKV2~TqpWsgGVMn^$%lMWzlqNpertbCPGbr(ur-{lpPJQhi`^N5_jMqyE1(8 z`@gaGwZnGs45Uz#WIUJ}Ac*CEr0lYzHx(dp*%VJB^uuy&t_&*~BYC;rA* z9!PaiwM#<#zGluGK5mZ*kY8xv7eOCwvI1e_Ov%S(7Uyy+hT-{M|F_c)z6%!KDD;=JD?wb+7)Y+Zp89-jbO|RW3Huuk3vs70oNa znVTossXCNNH}>Qr-dS(u-#n4YVi#B?uFqmcHJ4gU$#Z?Sb*h@Ze*2Lq2L}$c#Rmce zN*EZ^b?1bM5V>#Sc5F#S8 z>cJumKo)^j#hE5&OyWVP8u(HCh{ls*M#SRLk4n9t4Z+4=Df>2NsCt&Bzgs+QIpjaE z)B`g0Hb%{~#v~r8*0;JpDP}?}4$FLp*ISYm97^X;Q1(x-u6=*oKd;Gu1l0j*J^!Yq zwM*a*uH9V+eYvriwfOO_P@p~G4UT0fXfWUi970z|9GQIf;O#l}+w;eH=k(j>%8loY7@SAaV8(_sq$iLe`PYA_)rG$T z?%3()eu-eVfpo~U$K1(2z84aQ)Bo5Lj-WQY$u_biHfO5#N=)|TLTBm&mzvv`PcfJ3 zOh6$Woc~a(-(DFPUYS~rm~LNLo?e+_u54w0QwUwzz5Q*W4RlQUZJP~pw!*Q&{1%2w zi+cX%v&Jgd*8v09LEG2Cm}?p#;O@=EW%OZ1{&vJQ$M?-DlH?;ifz1*3)w#M6MNb@i z%kJH$5RQat(z-0F#%>ak=| zm6;U8!h01VQQzPne-`&(AV56hf2q{~M#cH&5_t$aex2p$8tw#9h#n_V?@XeRZC5q!SDd!pghg$uN_G!Gn({525 zFE7WM!Ns?7l7i1!lKTRtEq1gB%7YXHnbBXHYQ%x)Y%~~djP47$+-n$%jMaz9;rQ)0sW90roO43x?BnG| zYSV<&0JvEM5N`E6-G~PAN&@xEBJ7YfuD0G6@zn3D$rOHjA?reZflKC#m7#+kOm zMQ4~cTU}H~;-z8QL&*jrLwN6bKpJ8L@(4UROv8qrGGABk3j%T;?6d^o)i6LC^$g+D zkK%Vzq(})_h`7PGgS?3uX$Ceg;U|B469EK4aPp4?)F_0ZqK5tDu=y7r+iLomB_73# z=1vm)2$5y)#}Bdncm_^|5i8}6b|UFQG7;UsE1sEr{Q=~#CCVUFYwCSAVzDaKQMbl} zu+{q=`8gR#*e3sQ6`djDy5@ZSwPQQ}@=cYgt8QP~&T#uz!+p)5VSg023Gf$|HDS8K+G?M7iFPDg%w4!%XrU=Vf$JGQN&hR3e;5?r-6z zWw=E}p?Hr4WgZIkKSx|H`Veol?W@rJlUgHGQYsXJ~S{!OYP zs!9>z@r{n?fhC*Z-ZJbp{2eu}ifAMT3%o(E!U$=3Mp6F`8(0>msD1-Fw^`u4fle(#?k!j7lvj?X^_) zIhr2zHemmwiLaw8vcMLT;>zp5q_=skry-Kge`b?eCsYK0d=^WhNSILe>t6oqEYA9b z=(*}!o|SiSim0dT33`wUL#q<;Vt0oGf+3e0nCDK`_y|vO!Njxv{)rlDAg_9OK=dSH#{DYc8;KFwPF$vFFZRNwx6gMXkjHubSv8wi5 ze&ZDH7oTT&VgRIbc|#ikIJi52&RrOb#3-F*H^-;^DL40kz{`!#>iXRU=_X(vWXO-s zbfD1f&v;G3%(3>-PIu;?FDI7QmiyY-GoE>b>JC|MynHC^nD6k$&knp)WZl+7|I_Z7 zWU?^$vmMD7?9VZ0>kNePASWY!nAG|X>ock-{g>xBexg8Z_hKskp`e-9KSQEdT8|Ww zK!RTyoticPae5&PbBvL#E%~Et4t2EryK-p<9;4MsBg0T|QILax-=9fEosi5?X-7U( z_Ow%fGl58l)t$pSy31)aFT~U+)W@5cjF|t-bfizSF)_+TQhb ztzExfjEQv!j9eN=U&MciqTe%;HweV#={xeu0{CcpZ8v|xwN~cV^4X|y%G!?%ecp0F zxVdp>=xxI@y`KV1nh==1LPiQ{W5~p=*h|_0{~3)ETRfGHy;@r`YK5P)LZ9d3IXDF; zWomzw_wZ)v`cOAN5gN=H(Eo1qopnav)+~*@oz0SSLUrKaB~2+*oS32RAOQ9dR>};a zI50js5-%t1T=`Z%9I$nj^OCZVp#RMgC!}5Cav2Z;BskMq6Ry2%i%URcKa~!Q=6J1InZ?+$-${nvSopkU^ zJ(-#$WC^~b1Vd$^`n0m`Aw)qte-6@v{}C$OrS254Su^3^%MO_-QCg+GpKV;kLGG&y zum{3!(`3o_dXF!9@fM$Ket9x+@q{54$@l=tWXFFYf_`V+4Bn{5!rh5~d$$PdMhW}# zy3nPQXi#r&?FE_&j1TU`KPHs>QImsbKmdaFZ5?s2M@z3%qFVs6na_KFnhpA{q1VFX zNWg<3q;hx$srYb7M*0MZDj<|>2mu~q38%);$&a!K4XX{M9iZ9cD>%yXI9;1CZ1~-8 zj|O^@K}$Lzt$ID=VpM`l@YkQvLIjeiX?#c$hE|at ze0U&ar#lb>6~=`tJelT^{iiTZFoHun@`INrS)sNEo&-k7l*Nrm?GK5q(IQaqnC%M4 zx&cd_yvZ~QX~YevQ2Q{$t!j~Cc-U3s{CK%-E~kKZ{I)qBqR`myF+ld z7KhT{ZpF2MLUEVkP0(O1UW!xPiWVtQf)r?h0>!0RacQwqpu)-bxu0{N=lssh{rmSf zbM}vGGPAQg*_mYL+Wma4*IV_TCNma|s^MjCPJq67c{k5yMOJ@H?cJFgBS_tof3S3q zMLK+-4S^@0h;o!rqIOg#*2VAvGPN_I^C|!eK}L731luw7`%6Qh1NG7CA#QZjD>p9Q zM(O|s*$sEK{>5Bz7rx+3%>7z*y_K%;neN*NiL%I;mroT29UPP37OK&@axtwcoUJfM zL+SWr%ubT#Ug=0}i>7k_2>3?hbg|DjoMa{klxm|k&7eitok-o(=#r25rHy(!pq5Z4 zT zD+=XXe2f9gOc&~u7d-)v!>=9RH&A1^auS)Sm_OvI=gMnmJL)94YyV8wd0I9~Iio{! zq}9+gDwP^l?LxPkCKOVp5!N)kFR1$>KqsZK_xE(qFQ)Od_4`xHTIqv2g~VE(IlXiS@-Htci{I}L)55*?$~Xzjc$gU?r@lHZ{Gmtpx_&Y#xG-f5svz1P`zS4 zz5T78b&13p6HO=T53?6qXeBi*$6UHrJRE0Xx9XkYJbqCNzt8?fO6mvU(Dn+I~1N%E1nv~ zi~+zA{D4x!8O;=;_Xf9TLZ89%pI5XW;3BCVs7Kml)c=M~d_LDEC5n>EBikxfU4>Fd zBccdVKq0vC#sNDndV#$hO0fqZ^|?PjHp#s_DOq1A)l)D1rowxShw{ybmX3zaw9gwy z%2DKaEEp8$2`NV^K!pVggPKI8Mrl}rP88B`K()>6tnK!!-H%zDKS1h9)7(4Lv`8Se zX76lpA>}$?hJ4I%aqKx(L&ug`_xSDp7NN&6MkF1@zoluYhAJ%`;1@PU1ET43P*yb1 zIB?Wmv}r8>4|i=GSFfqGOzys)51*Zn*q#r+2Oy|I&2oX^XKZH=Xbjng#mv+nf(*#Mx=IzR$wbJe}6 z_>85RSQHu*#aRWCu&{jbW2r%M8m|iL^an6AqMKJRgEwO-74fO?82aaYv1ov_q`YnW?Lls~jr&ezR58kgH+pc;{ zx4F3&f2Voq)zwboii$XIM|HJdm_7`t1rWFZ2y##nc|dv;kP#ydOGV)uN_%&Su0W6x zdH`ydhd3H0?yiiH$~r{9gD?m9Eq47Ychx%0>TU}1o2MPoJLJ*qD#^|&=_T@rrAU@@ zgHmLJfDgoLxIy!HgEj~R2Y@(yHW(h;vtxjaLmSMW?3v~^STF5)gV)$`eEZb~&0jQx z2E-OO#e_o9M3w>K9PZYmID+iiu>dmcrqG;2h$Bk)a#M75lPGwdoOVO17oz*Yb}L zGb_6>%k8sf&t&un7zNliW%v3+I*<5Kd-T`FY#$^qb;1wye!QlooKlp zSU%Ra^1^J+BE| zDH1|0(THefu{oTl-`(RJ?&TD*>TIFD@N}m}f_7YTj#vN391dkR9AA#ZXXdg%-oyToe(?&Ft^2~TVd?13WZ1j4RT#}m|&PE4_u%{}sW1?!a7s)%#UEWRPo$c0t zdyg90>E}_sZ~wVTorte~zhy^DA{b3iKmG7E0Ecb!=a_%TE&QZ!{mSeLJ~`}tSJS$U z)u!=yYVUeb=fth%al>fURxA#2Ihu(Xd!2)#;fvx#Zr*ZDx<`vyvLT|_QPjeFQ?LVe z^T$!_Vo`yQQ|6Wc_(+ts>M~Z*bN9#%{ko6hD7%`k+MAo`LKcKPx|uqm5fDca~AcuwN#>!djj?a~;t&u0AxuXhfmj*fU-J}qc|XC-1rko20= z$&Zr}uu*irNZ;8uI5jZ=P*M>;x%8=jgw5XWt5iRE%#4Bp65CZ>ZgbFfTwO>rD>tUd=(u` z-vuX%h2p<O@ML{mBddy)gm36PuujBrwqeqL*Z{7w`~)lV`9BF`n>)Iq5+1xHb&;U z4;}_M#hzqFT^!q}ZM+7U&-x~$%95c1*UM-0D+5!S0}X9FWw?JlX!jQu{UqYtAH??X($f2qMLa+&XusL*(fikz3L1eP_Yb+Dpw_SFm@B{~lYzLX=i~2Cd7tRW z_UM#gJie;v`-^ud`}Jh=r-V(AaRUCga!@-2oFR>c$|C_B!PifnW4R0gvGY(nY;?*9Ub8bK z-}}n>_Pxv6%&S)bS>dU+Jz!7OLdk6)?VGS1m3<4cpU=szQ-W`jG1cORsHS0z6#W(6 zdZ-x+#Wo2Ya0d{gLaFrtJT3q;ayN(zWO@)G7Mdgy zDS?d5-Hip)huImTc6JxXV}oo6aB4LHrI5?yI_{Mx?H!LjpA8g@hE<_%L0bb z!w3cf^DCr#L~p5o$P_RwwAnv%VxncR|IEeT!t*Ib*ndYgy#zd2tQ=1%?AAI65hHkU z$5U={n7LB7L_1J!m|&WX3N~kC=v@4axsG&!VF5LQ;2;O{SdL;z1f_}3xH<+BUqsb} z{yaNA{3*JQjk>*kz#k}NTL+4lfM!Ai0;ED*LXswC(!9?-`fX#IR5qx5&{jdkhK(Z5~7!WWFm&W z+xQ725(Mvv6?{PhhSw!>U3vahxVbneAT@Yyae6ay2>DoKV0pd+qXw_{%24rwp?qRP zIloFo3noJWJ~l`EW-rV+p%rxMbJiVuvEVas^1T|J+0Fs>Ah zkgY4`Dv4!e+$?-9E6bL7AeqaWWeOrUdTdpwWlYUp0^rGIP2;b~b%_(GaDSwEftP=u z1fmRriWyeMKY9IP&*X$6el|2jW*q<~UI^uD?1PQc)yD}lmZv>`MOR%$$;G>$Eh~;f z;N=H>oT?;$20JKskE>3jq-04GFk*^n1=g{Atg&k%cr$8Jbg1=WxbeqQn77h*{Ghth z2X_v>hmN_)0@bpgsiSMbuQQKh*X+9L+rDJ)Tai?XJ%&e5dpGz`Xk^ zH~h}+$E4WbbiMohVXLD%gez-+bP;M{eH|`}+<}5Sb?`wF{D@+AArnKq>Kd4&^DwJm z?A}Ody^Y}qe(MOyIUPMu2L}VtN<-S+Wsj3LjDK_kCSGaV>`b;~@x-bsbKRpRV7H&V z@M0@%|1pPWS|GWOE?W6%CtON*?v2smxm>u~IPzbj()i@L$n<91%9Ywo?EQ+eZ6o`t z#(o6rn$h`>_Vw!8NWP8mpg-Qt4^?uVzCODD&2#&)0juw~$2)ep#57p3rq2j*mbXTw zdFD6Bss@}|9i00&Z$oS}^6JN~xLuf2yUyyb#TMI7OIHzEL?@%PHxNcH%qChnNtyb4!qF|0jNz{8B^aJ$jtcf_a+5`xx z2p*DrES};KUP`yYr$Mc|4*Hi`J$&!%6hw&tQs7xzDeyRf7UlC~ylHS8E%~34&^Gb+_>n*JJ*rR&NV& z;$}4K>DkH{CDCYpUHiX&I9;~3E*F9bxYpr1r~x4 z`{*l|jt^GllACs|nlpWmN*G-QZd^0ryX^oiYIc`g?%>|SAqn~WkZu60zVNfxejpaT ze5nzos_K>!Ql~GnbYJy9@J))*un^x9_Al2Cd|oUn9#!#|K?%Z=$xACytuckKNFpE+ za+YVR$|f-Vk}<9zK7^&Yn^tPcR}^&9x9d^y1`r$KRwgk1Maw_k3YIX3L zbSSgJtfywCMj$7F$FPw4$HAjJHB(_h0aDUX!dr6k8I!SkNlBgh57r@ZaZCWTzoKzg zk_6oJ72kJhsrLd%r>PwY<$qhB*gAD zUz5^F?t|_jEpJF?@IV?T>CcFiH_}3C?d{!j-UmIeBb}+!5gTs- z3&CM?b!1SH1$dTrsUqt1!pv0AHn;f_{+dPC0hsZncXEz{NqE7NY%s=5skl9xM&yAn z(!bxY(nf9mD}+CU+FRy*X4}*eHGLEr>*D;p+>ZGDeWrl zkM4an@5N+z8Z-R`sQ%S-9NJ-N_{8(&OuiGvU-M0F!Tmw-qBpYbkdFT+H}*{!JfC^Y zg!u(YzX&i&H3vCKOkg=O!2A zpyhhylta8R>?GwML3qCVtnqsMSzJregQGk0x3M3?`&y1^Q!>(}IwlIvnzaSYfKSat zAiHuB4?cCb?}=zlGr3-rKI%Cj|Iqn4v+%6Gc4ZSEG+P|K^RjZ*tW#jC7_v@9Sz}z_ zve9A1*_bWanlR>;t;HDDw9Ma7GV5`RJii`V4f@_dH29f>%+hI^7DLwd=g6f+UQGYe z`c3^4Pg>_L4lv;Rc$+PyNU`=4sn^U^=9E5ZuO${g#z{2IeAx-Qo3FaNvovt+u-~I= zBbsyh+7np3I|c2#d+_z5V&FI(T^X~r^XE&)uJyNEMTIYQmOt&z3~6ar`$)T8uIhG; z-EAh86}vuNs&h`Jrkp!=6W&c2q00`)%30yL@f!bp`|@n(b+}l#v5aw4F{)m)7!=fr z2cyNJAW$W^wK*2_vG#?GL@}HQu1Rh(NQXZb&Myj8Rtlzw!Iwa2(pvbPJ@s`1l^;Vg2g-77SrmA{=S|eJpa{b{}F8Gz;0|LB5EKKzkbO^75u_)|WF~My zn{^f;EagcQDRlxKt^&h{W`3*13I&1|`p9KaEHgkn%|0rwHD)2~31JN0WFNT%ibYci z$4j%0Y@9XWz+|)#tr9?vjY6HR-e^tUvM5xhWXlw#tZJ`ctb`XcNZh#g!Jhr*<&aJ6 zB3&Y(_!$*ND*IkF<9@Nd86Ueg)1#HfLUOJuquSr2~@CEjV(^eW~4-T z^H$03qL3>25o;z~)TNI+hDA^a%d`@EtcC(#p~z$a#8?2%mWCy7va}Z9!3IHNK+%p@ z79D6>n=)FVc;Qut`YjGhWqhiG_h0q!FK3t4^{@cwBYm+xM`6I{fV?|it7vkxYT4{v z>ZTiNC)yfU~T6fGyB^rV~9)<4r<(kOV?!BZ9B7ua6RgnYlF> zRX3aZH-*_nKk4_F4yk`xrVRoU$Eb$%aqS9o8aVdRR--&t%AypMbZXcnU)?Slz%&e> z4H;S?jbJgBRvLV)XuysVIiLtJ*ar$%^SdZ{GGdQc{FsJ*6XGyzt_u7z`#!jv1-xFs zboG$L1ckUl@|-ENN?`fg9M}?-Sh)yTaGj$c5z10)$3MzPg0~X;NPB21)DQ?U9v+S4 zzT|Ff@4X7$3Vg;^Ey&T>jnX4=4wRnPPD$Y7hskzQ*@Z5Qs^;%TK#kiL`2~1BN=o3q zWCY-O_j~!~C|WU;`KMCI4Qte8A{A;_`T>Hl4HzSZI$lISyC`7U>I0v!_)~(x$!vJ^ zj?yB17+N&wH&zKsxbC_ebdWC+Ig1q0(TZ|@XZnfSf_+A*Vlw!4jl6X}D&d0z2Lg=L zG0>)oq#SiVQ+1JxW$8eh%BissqY!<9*5hF zJa^?W$S#o-naJEx%~BjMI*rG<{Q+>IE>clZ>0a!HhPOOgWp}@{RC?e+cQ^%_JrqA06C}2{coJxO7v%Af}HLtaL{JLso^VB(7DaajzA)Wo&*Rc=5*x`M^e#hrR7X4ax~j zM?U-0Bc}K9%Tm@6ETqn9m8L?(I&!tBo0YZ4;n3anP{G&T-T_Qig(AbUeQO2M`w(mq z*IlJL$Aq&wKdyP`DWD(VHZZg_U?pgr>}ZS?GQN8CRTUkEF9>#x0mpzt|GMk`J#tQ%kCvLiOQi4(w&u+7RzV30(^@ zQfjK=ZhooHhRB!!Ana;CxyTv4h{)eqmXqf{Bfm503VxHz{iZ1Lo$~RPoN8+^n(S!d zL_|O%0I4a1{S{fe8@W^zXa*?;P}{KUQ|F>4Ee^~|`=2!c@ycrp8{R}X-`r?~v=QUaJ-FsAU&I~wP28E9gxkQF!M255{)-i#W)0>LGhms{Mo!PvZ_vJLb z_u1vp%jq+^nK+uoxUBDQb0&HC*J$~5G3Y51)JiJ;WoVpBxemdK;8sd=r*E@J7tyrU zJny_EpD4M=gc-j-88H8>Y}NbqyTLRopKE(>f`CWEGky!iEwew)M5yeq^{vDvvL*58 z-_y3JFrSzH^&GJmdptErnd9$*y!9pwMob%?XNLR#aca4j1eGeaP^5|(6H&X*Hb^{q zUf_F}wZ6QNdafhX2hv3RX$IeMSDhBdjL22@eQ1e*N`P>yM!e5K@*-u()`wz~0OXLW z&zh3-Y&eeBUuXywIKN@eRU;tQ+}$0Bu)ut2C3J0#@NMmG@)Z4aQSN&PxH9p}X)S}d z>YO~*lmJ|~73xF_YbARjesIO}ndsgI05hNjs&~x&QJV)6fyZ~d6C@)R?mWr~GzJH} za~lF6P^y{;an3h7$iDD9sP&ugyKMo#VM<_m(^WXi&U4cAX@Hpyi9h=ULAay^5v?WO zj_8D?^do9R>5`d>GF8J{1pnw89bEE6OblpUXk9UI4dn5Qm1L@n8S0V@+Qa`%eA%pf z*##T6kJon)ZF4&f_;D>^f7j>fg7J`R+~cm8>KIkfI-otFO~5~fmbx;}BIhC1dxDW> zkV_7-^v78L9@RY`2VnNUFp z6wW{%Pxd<6E|EVsK9fa0(R~soZH8`tmiRL=<4MSUK=Ou$57iO~>f_Qb{nri5$Xu;@ zg;4R%#kWV)cEH@N-vo-rS~a}3_clZkb(m5FmoRpWok(j*HD<}=n8-`WJs;A;^pP6U zyWD?*tHTX(VZR*RW<|CK+jFiytsG4#z0$1?Zfm|$=nS?>^8YH)F`?bjh(^OqurSP` zj=^Yv^nB|5khH@OX#?8ng4ZhIA%k+)n~3W%?am&1wyftv^ub~RoYIqHgOpp?(B1&I z7qVwDA$iYb!vJsN^RE%qN_KKsvX%VRkSaV)Apg!b5vDHhPIb-sYWb)->03C0l?hodl55l-`GlIHQ z*otc9ho*;a&4h3HsN{TbO;GT+5M`{|9U$I13$X;2ehY{zaRFGdR4!=T19_lHEDLqt zH;Nc)m)jd}1fWt>4MJUPH_5G}TS=Cw#ww(QN4(0S>7sCRl6zeRc#Aq-^NqM!8L={m&0ft~W1jT@dDLIH0B~pYF(HJWU0!y+2q+hpXt-_kq zqp3T($Tj5z$-bQZ|FHY`@OSl*BD*{(0GZ|{^PDZL5sTp3B=_l)zO=_BB{J$dKw6a`^sjJJRHl;( zoI-6&7Q@1YAa((Q7XZd{C?+flj3f{>R03rdv>D%fuItE5tlIsEfFiliyheed1IW^( z4DL<<=l39zyI$jj?NN3jIGR4Vi)j+|tLy&4A_mLbcs+OPrQ&4|LwO%+WHU$VV}J+~ zO%$Lnip?{1#ib?HUL~H4UEO1VBT+SpZrSD9s05UjKqDR(&d1#k1QXxMH0(@w(s?0j zpS>p*ZK{_i?YlG6nK4T&rhvwu-GB6KvOrOB(B$J;G+E(&N0#GZg)35kHx zTDW?=h6|8yR(uRFEa8!(#d3o11rU8`IsL(}gSi4&4typzDaOmP@z2fBpMiC^RP4S!oSxW+78ipE!B^iDlZQH#dcom_-zQU7!e-dH ztQJHF1eWl$KF6W>p2G>U5&gXotw1fry`N*o22%dUjV3P`SN7uxyp%1-zcFgU@XlVjtV>P=*z5J3z2M7iYxQSo24jL}hi*FVKv_ zpShsu%o1G?F#I`4KU#s7yeU!8(nyJrt=r-DP7+stx?UwS5;40jVOf5z7k#Yy#{S;R zyEzm0`QLfrEY`aL#t9V|oGK_a#P?b6WtF`=ZMuugmR^Djwi|-7(3~-MHQP5a;Z&F0 zShlIREVl^Em8@iCsX_r^mvY5G0(uK9G~WUV098Myp%6ApMXo~#R+~SNK#&&6ocNIu zO;%<>NB};*S5*SA!Nme8${vEIto(R}0z_4n7N^Hj=j`Zs5}e#q7RTqnAkpQEn@(ux z+}!yhzkPhh^!bLoe;obWM6F)^uc@5Mc#bpNR@a6S4(rfZ_935<9~y#i zO*HKw(&cfHP7i~F>G!dfs=^5TTe(WP)k#7!{zR7n9^opD=+?ZWktkB>m?~73xaPp4 zWbU*o72^CQY*dzfw#DX6?wL$)y^NdR0Os`2d6bl5Vu>b-cV(u7pUd7nP)39C;ceU%tHf_{j)ZOla<(&hZjn=DV6rh2nnNwuNL=4*q>m*1je0@% z20oX`G(8Ft%}Cs5^D1B)N^+?#H{PVzYLdadEFrG2L!n@W>I)KB$Kxp$NpBYi?uNM*ulOVlKg~jIngViEyGS1GS7`qa4S~XD22ZS|MMjwq#?=Y=0b> zM?}wV6r1P?s%2P`%sj|`M_mz-?yAxXkD&>0G?w+$q{iJ&!6$o0V?rQOlqH&(4@g2f zYoZErOaCJShsM$*sFk}3JZYY(fu2`*N?Z%1MrE}WKx!=_ROTc~0ug1N&#b1(M%#ui zDoC4n?=ykoxM#jK)dxZavi8Y2Ce0k-bvrvhfh+q=W zLcfniMt6LX_ci zAqdE?Ptm2h>sbmv3z&N9j3QBJV%?V?P20}0DwEM9mN}!gZOn1rr5m3`g2y)N9YfTZ zRp=hZ5UH)3Blj6UNIrCg$)iw_4#@y$E1Kx;ky1Iz70l$Yp+Rneav-DxvT|^J(9R}& zX|TM$qp(2g-FQ(hUf`XRp6_ATVC zBF%{>Q(0;>EgT3-btGfNG=g4J#6Udvl#mRQ0B9Lv@+ZYWnr|vD0|XaG4()$Mk&omt zp8HUIdD-4c{CGL_?LewX`%CV)9kUFJvbvG8@AP_^20>rRgzzOc|X+vPO za!h4)VKkabqO9EdGSzBrWuEwD6y-Aka#u2H4pMqKCDKiDFUB4PZh?){AoKdwU7RQPT~DKP)sO4(-W8X~@|x-|Tk7psD|@R{!}< zXb7OCM_;{{{(G8+Q>&SP(1C}4&r6<%Q$yKrW}pAPs4x1ng|`7>Ir(q3y688{0_woi z``2RS+NDJC@9hn?+pk+li0J>LRtu&g{#=U!AgQ;8C=mV$fZ_xK6T>Yzy5&QzGKzt| ze+P0Kg6JASoF^dhR3N_(o-8@&-U&cb417!A1J)7)$e(~Aji4)yzcwcj-sgv_4KwhD zZMTzcE`uY#Nqo}-eE&yBiXH@k1KWPlP+vS~{MQ4d522s|AzFD`f1JQ~0P2lL$T@Ks zDz+6zK-5X@9OI++IF;}{IetMZZ}Fsb)uhztR79&!b{VUS(hCnjwjQ8i}P5g=y1= z&?$O09c zn#`z`#xi{hzdmJM7GV2%%HB}JeD{dCg@V1wh-GSuqkRfK3WK8^Sn)oS`VP?dOx5>O za2VYo9z+g3aI#cdQ?mx!;PF9}N}d zTZc2YdSA|Yan2G-&ZJ7_=M;P)4upINLklFKVHvJj3=+dBNaYQc8$o8`lxJm5FA#2B zZ+<;gxAMDoy-oJjlp=VIMyL^$;(qVUbqo#SBDUY(#Q>vP|^eV+Im0a9F zMJxSi2Y9rYV?^@^&l&2{4GTS5CkR3co**Qu z#AE%WC_}Oq_KL>eBAOu#ybAkYJ;AYXNCN98T8Red> zSD^fl1oQx9cV%K)IMfFLJTnGzAqm2fkh5Z-9RfpmSJk+uUzsg%xb1R=E{G z5Lf_-8vDghP=xxWmV&d&ZXif-F%Jj){x*KU8Nliogy?jTWB@$1ndXVYs?j50M_5vN zIFTDx@Bk@cQU>(`Sj7N9b!DVgU@|xyx*Dj{btazZ0Mq9r5_=?tBkR3dO#Myxf~ZK7 zE?}~E%mjm^)2C(QjCq|*`IAm%>mQiGu_SV(Jk3a;x<7G0!ra3Fz6^#{wxywI1p+C(<2HLy2@{#sXgz<1fW&yD5V< zk9CG1f+p((kpRtfPd65Tu&g7lHB&MhE);{(dEDYfJaJNiwYQx#z4dl*2t0k{JuBu; zBd$biZu{9Rp^1j!vw3-jTu%m2AcH2j59kmif4eDS$*wa|{`kHmFp-0}CaV?^E9g?p zL+?%;69j1n*j;V9ShBl5SBqo>SPP%KGN6c#UXj*i(==w&w9*n}nnD34#BZI#;aHE- zVV*HQn1u?+2TRIW96~?mSn!i{k5-gZitmJ$_c;}BB9*K>EuWMbYlFG|yLq0sFId~> zMF-k>n(26lej-$}#aXy`^HBo9@Dun2;FYR#yXj-W`N#E+1aT<$Gn8)hy1`mJDLo3p zhtyMR=h@R>-dT5p)uGyiP+4uV!zzi!q3))0sznh6!fSV6uM5}fER{rfx*T|gU#;GYe2&h z#5sVlJ%1ZNBy@5{q_3Q(0vXdDMdbLCbS#@Nw~`c9OkhP$)C`CZStszKe*(#duB?;9 zAW2Vt2r*moCSyI=v*TmwA3eSDj-J7l_gKPm_(kWMLroLP{gZO%R9TS3nW(#+#HP4@ z@f9SY02X-gfcSWhr)ECvN084oZRY*OOqIo)Ta6G*QB?%Ve7=b;@7X#52MRRSKbnt% zaI6bvASL8aU$}GRB%Fu4XXnqq2pgT3iD!73B#U|RGKc1F!V}ko_~ydr&Y?x*W?won zFFP}d&N9t~3j#Ou$Db519BEo}YfaQ<9e0*^gYtD$ii{YF?eApq-sF%iy}D&6#bYed zcu-OuUC0tz#>QAQZum<2reN_U2Jc0wpxn!!o#mEq^UOjD8FI^x8D70#d~`PmE}4r@ z^0xHp%}VGE0VG#C|7DhMXtmB#hUv{KXRFL>%a_mT3!Or1%-_CrXMC=jTg_xuX~bBS zWF?blT*sfwGn!j9AqTy8BgG?5wITOfdaCwTgRxHUZSk;`Y<#ZldG4d>+=lrjO_e$6 zi&Qd_yy?Br>f^T!^*@;QEnObg)fCoVnpqeSlbo8 zJp%{?7i(-d^m792$$aahFnlZu31Pks|2<4I35c~B#;Mg|%-}SIk*DFqAuAvJ^FKVz zhdk{57}DR;+0}N=g#C5-ZnmqS1Thr}kHRe=)r!`7=4o zoO77h6Te*bC0}}@`|Y+(=MUzYubrQ3nWs4Kjj^#z64=ff|C;^LHOqbvs~3SavYk_l zn6msbVH`1MZ~OUC#OH#RIfFl+2V|#jIe$%e$Mz%VCUa+PSu~;3kUevWt^LMrA-g;@o8hhk2%eyHp z+gfhcZhpnDUv0nWz1t-DyT!1&nfGUY_uf`S*p~IXEsDag83p6OmTCT==?`z;*spdv zv94RLu9@5J8Nb^}diVAA&sWz!yXNwHS$1C~+nV3MI2g5DthC!br`<(QAFwB{6#OMC z`MVQh*D#-V)RN!vi{+S2@i0R1sHkw|Haf3+bhW`Z@AzKi*9_M0FYLavQ+x{xJ9?w= z{mb7Yg5K{$PmjqS9?U9k0*hWl*m^o6i+c)-_Z5$2|9qn>+A;sT&9+v{c{|SgFzn5{ zCXJ$IorgIFYzC{p&VA)B?nV5lRs1o`aLBZ_BmHzkboCskR&TQ|`aj)!xw>ui?&=BK zzHHH!c;rgh{U-aTyW+jOBoEIH?Zjw`Zc5jVK6GD4tW^`T{i1qzp7M0(#ly9XqTi;4 z*PTVxMNbcF*SaOye)rx#uY7p1w$`&2`RDNF=ZAZ3Q%~CdtyW`v@ag|itHF2wZ?!td z2d7r!(fDn-oPHfj<+v3M6$+UeX8vE)YM*B@IJG+c=$dNxLt$OqCb1P2zh0@bBq&9* zmBZwfuKQ~8|4Xf|w8;8Y_jYD~s@k#i)xBCN{^d6oYfH?Bk`1dp;lKRANyZ)~Eq4fq zTf<~T^ajKjdR2+3nstUVcIPsWC>z(t^X0>MYF<_#Uu9ZnKTha$a?7l=wAQyaKOLLn zc1^NLxpT4mr914GdX@FgUR&A2fs}g-!S>q;X>_8a3e86^`<`0&KD*}~m@}}?^F%)| zRCk%}ga4o{WyJNK=(xAZt#8=n^~GUt=J)06uaA|nj8@vuc$XXXx>V#J2AudN|J?#1tzZR2OT z{lYqqzaq;n+qELwMO$kpSFdej>)D&$>mid z{((AElHyyJ(o}sp7beVu#Ok8#$GO#I`3?vBl^I?F2URb!8LLaH6d4a{64DRcUiqKA za;|S%P5j@g)dm0Os@3!O;Qu66moWni*`YZ+q?!Cwse*JUUK(y<0?9`S|+b&}u(FKYxG!z`(#KPo4w^2mi~h#*Go-;s0{0 zBO@dKO;<-pNB^6y#<|tco;}0K)i}30CnpEzR+p5NR8&;d*Vi{SH8nRkx3;$aFKBgt zfB)d%Adaj4`0*o-s~-EWZG3zjr&Rw7r~dbdBdX`-=KiNr{V$gKU*po!63(Yy`>$>N ze{49V`s@GLc6N5Yef##~$B*ml>tDZq{rU6fUug2b$N%6j|DV6>f6<@+eNq4V@E^Ya ze?IU(@BPoG{m;k#f4>;+Z2$iit1JI4R=-Qh6?FSYthO6x_;0cL_sjpSSY3z{s{=IX zY!`51wbIf0zs2f~Xvn|B>Lt`aV)fpCiPaqVQvZ9gnqks+ic&AYQ3FOQ)QG}kaOv1k zMvN$>Gt=hCrqfDg4nEeBasgK6ySHh5&f|J7yIT-6*U78pWHmSdQuWEirNpXE;*nDd z!GWxg1gQ5kU26C#3nO>5b~QsEA5?1=_`P-2%&}YIttyR^;8)`(|7b+)PVvDGK< zhy2{{j>%+LKNuwu&YPrA1q-)M>WU2xKu9G3gCB(+5`7*ebgw-hHyTzr9iw?PmiC^( zc!gqu`1&Y~g;Qk`uS}N=9HeeoYJJ{l(k1+6%=i9psYa9>OWFdi+57TGN(hC=c+{L|F_-QsA)K^5-TtB_eR4~uXUT$x43j!=BM6Tq1h7uhF21VjYT0PO|_RwuD9-@+F9DWy&#@Fh4v3>Jge zA4EAXNAt;PqT*>)0U)IXsAvovjnDNF5(sc0pHNk$QG6cjVpGWQGH2-SYl^6jX)V^l zsy2SRm>7KtcBY4W>XNThpVzx{P>WYm5Pmk|j*Ee^-c{~ZU~P(w8Pw!txK>S8HR61c z>Bud5M@vVYk~2YJOG`PV`OZz#bKc*a0(gWY_g*(g=={!QQCp;ZAmx~rJHyFrz29&A z7AIC;Y>O;P>R7!V%qH&R=9P9$)3@WwC=4i~T!!je!0zOR+ zddwiw_=RTjw!~V_CmFYMo)r^}^Z=oLBBq67i@8#EKjBtivwtE@Yj8_ZkaFV5YD3QM z-EF}z%W->r|B}DqT-?9-4fh#lcpz#c3dfcc2`d>K*Mqx=drL+M;Q<`%kbO-(tIwEx z6Au0YO?6S2c8tDbDOdR({8rY;)y(8o=f%E$i`MkZu^G6GQ?-6mwLZ$OC0~KW4H3_% zlt)Vw=h#}KV+~7tq0UtyX1j0p0%lq)l2Lclx@Y>xZzhT>uuj=YK(hYBCydmaO5-d+ zvw2uX8|7BL?e>96H;ke&(zFh(R;yVWI@gi?rY8Kj){LflGTv&gq_?bAZ27}zpx4~% zZ^<6QyuZk?q-Kqz>_;lZ))s?>W=#T4N3tw=X0L7&v$-Q#Fs{Z)OL0|m>~*&F?jc0h znWS_b!%idK5zE$b;Cc4W9^sdo*4hVm+B@vCgzvj{S+snTYIsgkA7D(0t1n)@T}i2b z(oVFxv}#&o5ZDkrEU$gY)6$$9yJxVryl@%xqIWIj`_o;Whi?+Mx<7h;Q_enI*^-j! zk+uDfx_>zHNxKQlzoA6rfZZSsyBdJw#A=))LXvbf$Vcl76G`2q?UKP4jBH9)3&*9x zUk$Oo7Juf;{FP-5Cst=SJ&P7{WTIMpk5KeW%&6OSvafed(bhTCfj$6#6JV?joQCW?!yt z(F5A43gWGM*!{^g`<0Yfn{o;{zNkeXEW}GCAj18KHE5RNQnZGU+1)V1YN0PuvhsY{ zwMJlhx_(YjJc-0o+sJBqJo~buq~Q3(YGF3;i)d4IguVOp4ZO`v^DR~XVQi~B`{8TJ z*Nexw-F?k)ub1u(NItRAndH8y3boEZ$ia7Q%+1Y`AGH%v?c`}Gs_{BInf2X%icz85o z-QAz*Yq79wm@lh;-h{#m_{r6$5JmT#-nl(l-kWI^MUTW%n?0~?XRq})|LCIdO@d*m z$@eLNXlq=~62Es{=j$KnL8dR)iX&b7#{rwlJ$nz@`y{nLrI>m;q4D&9F5vGj3Yzou>Itf~(hrss zSSS5d+B_sWKJs=K-AjjSsR}=?jBu}v5UKRqGACcvAm4N*7uO^g-3tjsMwdII-v30==b*!XK7sS0>2hLdI$|_kp1v`Ug>*#fLn)N1 zV(>;{Ombo!W#XtU;>=`X7v^JgE2C^*#93Lyn9IbF;D8I6!0s3BvM$0N9VoA=2uVJ2 z^a$)OCVF}}YKPY*9^;)r?;3Ht7yGmWb2ArB))pNc9G~ct5K4gJy~22Qpt-I>RQdeW z>7#fw6Oo$n{tF3(d(mlC390c3p9zw3=$^jJiBHx{D4k0rK8^Jaj&bXVb)k=|#U%E+ zB$kdm}%`^;my+Tyo)6a1W0^ahf87LtwrFP`o@oa*-v{P>xig=24y zy|-kgb7ZejgzOMylhr!s7$Icu9kLx#c8-_UR z_v^mz$J2^9ATent`nEj%sV=7 zUvLM#IE5wtD1Y&f4Gd{{0TO!w-%g-zd13xGA(`JT?!?teDd|p>J4=NtTUL_QTX!z& zyTp?uYc~j!DL^usX87h+GHSx^q*L5@+;MpJq<$w@`)u|ZMvhg(C?JvD%s-vtyGvW0B4nS7wdGl z&~$e(05u8hI+^bME8Ukp!(S!CbrNLi2sKWG7-KRd6M;{*GYWb@(xy~3mxX4RXJuDG$E;@5D0f0Z?68i27m16ETQ?7O1}^_ZB)sClM$` z0&AK=jL48{vOq=*kPa(vj?TC|sU~;_=zk6e(xQQki6Ch*=*6#`J@)LEE!l_c1?7v` z4e4nRz8v4dw0;3G7kJe=0uP> z7Gfk@WNJfYhXG$4H$amCBI0Ft3^Jqn4n;Cl+8l|@L>aBP>ekaG$6rxJX9o)`(u3Bi1g|LwF1bP4~ zF`1_5>?glK*CrvmDrLFbS^O`r;U;s-f9F?KR@SuMdSLT%F*rdwBphrO{dp(bd;V3^ z?}C<{EW@?BQt zGUC-rW#C6u9VL#~(BYWZ;`%$SwXKV_fh~}yLm*9bS?6}$yG79pt%soyYy!v@zD;F@ zg(#D96-W@=xoSE)JQ}&5 zH9kPb1*;L)w#*-nG_`LxI~76>e}kqtnq@el?2|%Rbn}CjYOmjrg`Kh#wK97^^S(!m zY;4PK-KOAs^%Z}i2BBwAg>3Y~l*@{YL>WZS=C2(x1`BcP)JS?Y8MV#h!vR5R|5 zRN&6Z07l31wvjeD@utYvaYL-_529c8Sv}bFZy$VKm5>FIJIoN1g{&ak>UYykF8~s8 zpyna;{c;)REaQVj#>eF{{?~x@sv;ki#!ZP=TZZ-7BkxK@BmZQ*924yP_7ET?3w~Mw z(mE{pvz#6i3T4A~X!~~my-)7=opH^v+pC2bsuwV$a-eTp1{dBP z>$VrFb#g!N)W-Hm=VsbZk{KK!(sAAM%VkC+u#se$k|%j)SIDuUnBh-P*>;;Pti8~^ zx96-S|F#@YTsw6kSk<)K0+sQ6SX;_=KnB%jB>R>fUvcSC8=(yny)Hy=+NxpHUwZD* z57&FCRo(R)-s!1sLcBT@P7ll%J#>OX5*^gVWYbm zYxP(4dOZ%Br}=?UB(s)Q}t7R4!z~Lx1e) zMe)~hLi!Pe<^w>iu$&?}y85Ba01sX|14(g?A1neL)`4)Z@%z#J%vJq4=L_QxTZNe( zr$b~X*zrJWSwPdHZYM05l>?}UY38dL5Rj@-qzq_b$q)XN(ToBZFrdNqNz~uL@eiY3 zTqF9UP1JbclbETyp+jJCXyo3MXY1^p;rH=pP(Th)889mHr%ZqhWJLgZO~ zy?2Le2wn4%VWq3s5XkB~=Hc=Ft3G614LbaLLeFdYl_$ym0!~NGW!tsz;~Ns3=h``! zs<-CXS{qx+Uzt3gDK-SWEFa{Cq)*nN0>rQYW2 zJG%I$lw2DXuPV}5=fX<<(cc^;E~ql)O=nFu@5MnN*{=Q?g0u0N1k&ovk? zv|E+W`!F4yz$HC$wEPlcI>z*O)ejCy{k0(dd_5brkzKQ1VYnERJA7Ufhw|z&;QaK- zAb#Jo9nd=SBy<6;QK(c{MuUdzZBra1HV6{qOyUsj@qEcA*?eT60I5>{U&P?b#>ePY z&b>LiAN^2r5xe8(TjTjP$3j}B>&;^mYpAv5JRp{`?0t@W#XotCX|+xI$r>oX0Gz)x zX6!UoxZ1Y!@^M*zKNI#p7l>{8ghCZ58ETHr7sbZv+R{4O`WG6zv|hWHq!x5P z6tuML!fMme|8~)2Nb;4PT!TH$s&zZrw)M!6S`x zD2cZqiN_;N;5h;luCgug}B|!=Ii; zUp-~RfMY(MCcFSYKR->DIgPyf>&eFqY0R%QpI?u!g86#@QBP0VNI)*r-(^pKmlyo5 z?EKB;3%tQXnKPmIn^c+J1Q`5TLKGbKBpiB;oyBvO*c{Hf<)6iH0&VvjFo~1pL%^4I z2aVoM4FzB6@0>LiED*f+knhf#$m%)Wt4W;2x7absSMbL07%{qTt(~7O; zx9p9cj7pE=hA)X?up9pqt3w@i+0$JU+7qYEH;u9{-4(Z1`DOrNcJllOSpp$Ui?v-5 zm$JW$n3NiO$UoqCV}wKtSKIexTkKla3Ml^@OD?v-3J@R8CIt1Dc?Pg%Wm}O6xja_J8^gYS-Qe{dF7YygP)(j-agmw zcJSwtCbp(4a}o(qOte_*D1^yJm5O@LTaJ%bpj&uXTA`Z&H81pNKh~JtX+O~stk+_6 zdU(C=)H|j8Ez8j-_ol>?mq~)%QOc<3uMp3@7-|+@0368r=&*&~wbiszI%)(D-!ESF z08&Mv>%`G@?FPM!O&bk{G71Jqwaf>VFKXoHTd&vsxmRLVDny%8)Iwc!tVtYv;+Qzd z|HDOAzoJmt2r7~A(gaR(&g;G|DpB8VJ|e>#sc6pFQ*z(vx4nzs8^wT}q%kEAy)}|( zLWFqapm<2)ggUq6^!183WzT2a*8UIbdDBw@Dz(}FPMYaIg(@w)6O9}(ZyvviTx?V- zy>EQ|u^J$}Cr-HmRTe8NOO!BoeK6sD7I#tT1GJ0Tj!8zx^=t%vPE&V?jJ#|gdWAn- z>dshi>gc1Kc=}G03++0R6<_{-DXn#oOb~3BS{NkWkH4pHmZbQzL7M;KaM!&DUaY;r zU!W>3N!As?H4o>_S!z-zoZr8D3S==VmDZ?wTW`khZ8fJUVIK!q?ol}H65vqwy-la4 z@%2+uhMVei@|>#5;^XrddD%7THBZ<#yFwCZoxcQ@(RzMqPq-J&q*Ru6e>gQ93r|Q2 z3a%~tQr0%|Y9>~-CT(n~t-5Mz)aL%bS)~NesosCTZ}Y%|ho9rbq>qv8Zy$4df7_^4 z3mBVkj|J!Mmp%6Cd-mwv?j~zWM^KOt9eu!R(-#e?k7Jq?v?D^Ro?bFCM#~Y_jY|DS?@Jj-Y2#aVmI2lUlk#IP4c4wiUMiVDg z$xgJ6UHA0Ybm_>rp0eYvd3wJfnmZtR&1iP?=|lw3)`#*dsSRrKy#I=X}|q!@zMQpL}yTLY@rfv&6`AP2RCAT+lW5-hbf)cd=^B zOtNs^NbED*c!KiHL?|8WC3;{PWEYtX-cKZ_E|!|rqAQsJU-hI;lfatjVaXy=>V#sc z!qHnaYt4CC8gpo=RjyF8wp-y1m%s#i6XwR;zsd1c_u1&n(iCVVpJiFH3n_k9X}A$4 zm}V`x!Thaa>`8@K`ebowJeNMB+9ixx*UgtI5{vzWf4xY_K`b&ZOB{{G+%ajiRsYgr z7CQG@;OE}pB{BZ#7?;CIV^yn5e-Xf!7fECjOdF8KBf5^qpE;^dxby-C&iG} zE)AO%gGOGm1H~kMRt%Sh^_G}udyI;k8h&0&6*3zngT13nUrx@7*;^oPJx_*s=r`Jw z)VqUaV;hTD=4xK3it8lD(kE+_)Y69Tu)JK#IeF8lrzjvo?PYT#3SkC+HbkZ$w@E9I z#+08IdG6j8YAbEFbg5--kLjXVX_qJNS}E zK+Sq7p}c{qWl!&N)lK6EZ>=BJn7OlFwplMuDKZ_jxL&gkq?KfBn6CB;ydJn5vk}Cf zr40+BOP-JO{Px=7lH@%#7n{+%jw>2fWU9ZTso9Cw*l;b|)*T z(M`5}%$`orQQXPuc*mjkA*t|8?cOkTXY1FRGW>E`lnIk2K%*-3@+XJE&(&mhJ71#q z2aaE=5$|ZHYH{Ya=>1mz?%^uH*>6s687(r%`^-m0?t?h3!$Qx2`dW14XghL{!)dI&!HNJ zPkivRWje|_A(KQl#Sd$A&q8{IG>%xT@N0|%A^jrxM5OzNPrNUH12~Q27zYegQ1@4l z3>KK)^x?D2v(OO__JEAL0P4#Fq2mwU1>~K>K;s^3&92%C~40citQcG+%Yq3s$$4H+wYcAWqYhIc^+++1nV?{6i~?8#(S zG}a}(8)cYwVro@IFd$nb{#-P6V=_k$hGx#D()PV?Icgn*+su z6IA29(X&mX?qBQ6#>ZqYQOAv2XYC9NYgx&E@yceyNimd5o}n?Y*9wCDb{_0aV>Keb zh@-NSt?KzzmxnyW?^t&}id9_2xM~c>9{Ibq%P$22)UZ%l-=8BA6(Bmtu6?x^uRn4& z357q^T=+u`>V3HQW7MJT8xr)G`3CP$@-2nY=8u2-LU73S0mX@L9Y!fJ;>sv-GqgxU zIkl=N>e$m=UQX6aB8+(;|tM7zUyN6zik~O z++hRI8C{aT+%iqgi^&I$8lJQoBJ3ibol3bV1tO>{(my zt&{6c0QQ+asxx!ZI`-45BJ@iC=3hRGM{BZD>q(D)BCnod=>)Yd-_&yW(|51B))A8D zq?O{VC8#u1q%=dvC5ebj!W*^sNyPQ@RL6*l5~m+>pCAad$~gO8o%_{Nj_tZk*ZNrZ z23{QMSUIP77i;}9jFrG-qp$;aje3p)^s*+!$PM|^Fv9tD=c>oOx-C42ssxBtcH7G@F# z^{!6NXhDy9Y+Q7+Ng)52p=mN3RpUU8ZF{bfVqR{?pJh4_w%?@$bv+FK&0whH4?N9w z@L6h%DPQKRz1UA+MV)&6%0P*#xVF5A?tF--AD8~HtZu#faIcg8ly4d(g%F|`Pa7S# zQKcW3YM4-LIQZI7esdOSd@c@ZWqqKWQ=N=G~EPNVDFEK_ko*u)I9bq_sbL?)| z=)&_+-CjdZ)Ns1dfU)g>WMj4-w~wKN7sIsc>CHYr^~H^?%M20<9(`C?jA^j4XyIAUgg}KN}W}pS@_ap75P=Bjwpk=gaQ-5Nv-wy?FAvL+?rer@&d3`tg=44jxZ^k2TO847P zN)}XBg_r(e{&fZ@0MNYqNKZ%^ojI$0PH#LWTsp~>ca4dempRQG)6rFdkp7)SgeBq; zM}~A#{DoT}ei9zJ@E@2hguSU{LFAt#Ae#aF?)~b|bri0kMXb#4xyx^3O+F}Nk6LXvl zF}G)o?23%zXG}_qu79n4Y9p8gG5Ad{lA<;DQ$5!Q9xi;b)g6+{M<#Id5%Wc{tR%cp zW}3awjr%~$0cVhC;^5TH8<#J=|BAEJOuliwbS62Z;nw%zh8UE_uCe9AYtyF^SmK<|g#c{XN2fVA+uZy|2ZDxQYe+r5CXnumtqk*4MhzeyJ*#iO{% zQ0Dru$fAdK?;i=lk%fh7X|cDh@yXiy_d5!*PKo@Pcs{^9QnoCr)JmWnz*ES6RcJrV_s7+0zOf9ou-6cViopybVwfO@C zXEe&`lEIs=ixSNMW&j9-y^*d=gy|8cgjNulfN5>J={`J6Z;3aNpl5N5oVK!P2Y`p% zvRAdnsMz+VNuT$*iuX(L43xqSKyd}BAIhQ$gXP3VDRZ(55rJQkPXzH7uI^lj)qVCp zm+Vk5kf`#lCFsY_Jp2}SJe@@O%IB3wfj7(>qn}n^d+|q#LvGc*reH97Zp3T#svQwg z4w66{Pw5d^MTt!1cy23u8~8^^!V;WBs+Ip3hx_NVAIw3BW^I5MJ2Ov;-l`0UYShx(O~K zjzoSoB3yPOxp&>#WZF#0ry*ve3%n;AhD#{x10a_I6{&i}rO#LuNs%VO{bmbjdv7b`frzA%<0tuEmBpN)o^EPqxQL;XU0iST*^|v2e#NT*m@xIQ^Ez{%fc?EX< zh`;v8HAdEoh$P`H`v5jK5i$Ux$i*7F887$at^w5jka>&XW*+fGB4D#7%KO7JmKdO@ zo>d@16a(Tbzgx;=Wl@8VOd}%DL}?gUYKwr_@+f8_BCJ3%r`GU8U>__z#l$1+pIdtW z&5Zn!A9hi4La_hVnU*7?M6$ zmA{haHkrP8ub8-fX0S=cu$uh))#EqTPwfEsJ_n0AIT!)u4lY?f*PxdB) zX5l0=n=S&JZ5BZplP_P0cm4YQ?LxwsRifQ3GVNXs&t62-i@N=#bCTuekWodVr{}B8 z_oJ)UcQszgCK3hpfF}NN?9ssa_f`U#`0a~SK=gt@$WG~p&B4O2OTw0+9bSoIo}-#y zPAt9%FVmUP?3c;T4QdrL;5DTWvpn=bA7dbLsady@o+xuE@t)4$c=qk+fK0p3)aYwG;{XUb! zwx0*ViMjh?9S~jy6E44lbOXu!WRTT;%@thP`vkUb98*~uFlBvk-RJhzrW^?C_W+Fpo#huG?Dqyc)F2+f_jAm~ z;ToG%IO`zbXW8{ytkFi#x0rHC6#(eQ7Y6K?fgYZ~?9E}M@9U200yr696C7~+Szu!z zYWiisY;;_cXX;PR6X(k(+nNum+YR!+d%XhOkKsk|D1$ zx2RM?f;G&u*xJGSKWZ}dqp2Amv;K@}s)_9kd7d#Ecey8?wJ7*)5MUrE9X1v<5F4v# zoH_uDx%(^q=LHJx5jSxy$n7Wc?(fUNztbl{!f1y4&R8@B4;u_LrVdW?iB>fYud53u zx`Rav!wIKhmqZ`Nk;7F)VVBr~T{^D@bdCf%Oq4$T&PHSMRge^NZf~XX-Yh&);l9mpz5O0>Zgw_%OBl&cBZBnc8fo- zJ+>0;sPelsALa;HH@P(>G8u6`>G5a(-POXh%dk0K7&jRpzxHUvF~5wIJtOg;nj*<8Al1aKHh75+6$xGLD8gzc1yxPW4|(grx|8bd4@<4CI0 zSn<{N|1VZk7o8*eB?dHSyN%y0m`bMA-+86fsO8;?7TAnqqH}ptK6Q5hh7|1GgD<&_ z&6GP;rhBwL7?~&C0vX>sJDDnd3a2i+cd0uPDV0mIVVHMZAc)zbB%N`XlsELsFYQ%_cYI8}Q0Uwm-U{@C43-#z3j*yx?DZ@X270e_9?)-0 zk-3-re)7a$S_=7uWg^~PeYh9%}xnJx6;j1Md;fp-iv^5mzsaS@ZO0F9zFKVV*C|MO&C>B+^ zR&u*q>(Ddl)4Q4K#mJgN+=PXk9A51AmEp8U9VfZ}8wWuQ;@EHy1{aJ7&cukaXTvi+5*P_Uf&VuWs)^y4*|OhPkGl%AfAMJKn!& zdNvAwCZuq0#WK}BiqE-dT@Lw$W7Qkc(tXUgjV8Mj(w^J5Zcc|(glM%II#A{IEoYxU zp~1H>bi#K=C?iyFCBLQHn+o>gojLjlDQ|deZ+3eWx42oXf0pYiq;L0=n=w|%u6O3V zu_QOZTlc)8jn1Ml{c+)Mric_Rl48Jk)#m-6&Km#J&kRgLNv|h#rq0L{Ei`$ovp1M) zcsLw3))Xm(&Wt8U^QX5ef}?|}{S&{sT}guGyKVk3F;~Xm=1O+{)X)-I{~VR{Mtee^*XfKHBp+Z%vnnxWjE-ibu&yD{Gi7?jP0$Az#(lzs;73(+tZ5 zj#YgmMq{eB`@BS(%*3-Rs z@D;are0EYUZt8cmrnlNpJN%TqBE~pwGaAg9*1ZTiLoz-p9JXj^NUJ?}5n|*J8?j#3 zc9v@vk`AaCu{&A!(pIAMU z&1MBu?hzaJOHRoZE61wbN+kG{z0TYB6jA&rHW6Bj$^YIiqB0*d@i_KI!JmyUP$kSH zfuXUG>WZi)^d3-gvZ08!h7Fx=8X05WSj^HPs-w^fLFy^Ke#5dJ$S0v7DW-2YwMn^^7kE_DmPpkAS zy^?p1xrL#!Nhq{qh+nEcla4t7OjA#);rQ%EEoZNvz*-z~Am_9q0jo^fpEF+U1OL-y{tfMTHh+k4D>+)^y3|TzxvnFzB zX*^`)Q`V<0`Rw9hTxwPDeFNbn%Y5hd?!bb1Q!C_=;Qqa;w&jxqyU;nKp~=jp%n*Y8 z%&$TJr~XUtADO>lNA;lW0YYNi)nN1%0C0QRa`$+?_4TE=AfM=E8Cqp|7b9MuuL=MZ zM!%iP0JkuywmQ?8(Xng17lBJ*6gJiJ{2tHQ!F5j7{p;M+Dg0#XX$_krC?F#_hNg^x%GU#W3g|r*r!5R(u2;KQmxYK+j5`sy1$4T&7Rd0o zbVJFh`n+`|AfzLiA*B0r&dtnJb+`RYFCv4OsUU3fN0vLAsqprG@K8{YdrHo_wrJC7 z?dsoJ`}xIrO4%<#lp}yRU6mq#<8<_@bmRRX*;bU++L$H~Sep>%a`RooO#TT@@<8$) z=Vz6m>*@ZfJ()XlJ8y}27CcZC^GQm(SGBZ+yZ3Kkxez z>*O$S5D=L5{o$n*%TEh%&Q6=p^Pg#+#?G$T{b=d=^EKd2n|<>>Ysb&N-8+!5_f(Dz zaU~rBmV;P^VGaMJk%)~K|4#2seO<8cJI~us&xq|q62H~lfBYryK;_*C+X>hf(;jho zj(IDsy8CJG(awre)qCmC`O4dy{=o)3s*QSTvjo=Z9L9R|&g4dxRkF+8Cksr<2DJ>(w z*=Lh^QB3M&kUIW5lO0Jezk|(gV{A3h>!U}Y1Fjh^F-w5pqoKBdUwJ@?A}%^QlKVTr zhSJ9!g2M^MUOWNfun)XE zp;S~0z$HNHDilqo%I~L99&(JqG+7r^Fm=(ZXRN=8V1nQRrxus{xPGqWBO5?;wq#D< zL98N?52e>@Za35NHXXdkDc@kz5mx`9UyE+qh0eG2(8B$iR_vizp*8hM(>9 zpylUX?6Nor8ppt;i;KRXtg-aJ;gCQ=p`I?{CXl078+r(k`GXf`DB{Kf7%vd+K%^y; zt}Vcb2i*ml#nBr94z#gM>V$uJeUigaZmbhGB%Pkpb<2oPLJMc<(#L00Bv0GVqH1j{ zt;4!+XL7P~WNp2m57EYue{>n_=#c{mFF6f{RGw(DZ2`2Sm(8;wI@z~Rww-1d^DWe| zuLt>MM7#cce!2e!fJF7iz#?KveSFta0^&SEA#BaWfO6N| z@%q0H*OelUUCB_CEBn>}h>g`en`Lh-ivGjs5-Fc0f{p5?P>h``uZ?yTx))>5C&5HG z-~I$?UiU553nW3Gllz{#P)p2y6s{yQ1R~GoE}%*P9UBm`CDgd}5mEx6k9&171EM1j zNKSV_+6O=W(KoDx8omWgW5$Xc-P|?@jk9f@@JA^6I|Vx#dk{13zm26=#<$eO)e4n1 z(daRkV5vytVXe=)j`);OEYE%!f=Gqz{_Mk`1uek_4FVy8Kz@4{Iz+E^3ACuZ6q2~b zBw2!(>;ezMnYMW2NTA>bP3Z;@ha&cZ1C?sfyy+zUfId6UE>`&>v%4kT*hx_Rsdo+4%e+;P>%)Bi1=}Sx?7e<|u zr5bI&+FSi+v|8LofCel%GzxaALWbDla46tw6e55nmRSohttTRa0KU`#_{BLx9r4Wh zz?TL98(1vB--X8(4=^C>nc$g3fiK7LU@ZXSERKh*3k>PPc_JCcH{2RD@9vbOo1ioP z+2OOxl$;mLvOS2|g@j%MDp9)}c|GQcOzyD?vAJ#e)ouG(0*9#MjntCAt^iF+7pLGS z+ybtNwwG(qfu|i9h=6copBIeYV-?)SQqF-gu&%K5f;Y5U`(|QK?iKZ0rxyU->;Bz) z5qXEcYYm@x2Z?cK*AvwZSp+TM4Y!2z_KO;y3~*~<5u<=Y*)e7m1$9bBD37GBOCYy* z!JqGW``LPcJcq=}zn~@Yxm;PT!wcMm6eF^k>U~~Zm7B?4@B)s3t*hD+#eo6ROX4PD zJy(*d1=FgZ|6S~is<_w6L+&fPhyLm(1S?^Ijq4*s0PO~iiCrJ7Evi+~*tY1(@^_D;BR4CF(NNRp$WUJV(SzxUZ|esggC z{r>9}=^4v|H!4yG{{|RMwGh>{he29w5${2^fL+8c@wXOzh|CCfiJ_gQ+j#$*s_l1! zx^lCF>D7aNC!b+{y)IWgGIjiCP%krT8P%Lbrsa0;H9$n?Y?5`DE&zK z7(nYnqVkcY4)&!Q0aQ;P^e+0?S^NCk-~acd7j4>0gQdS z>ZKK4ckd#l^+E=`$KRdjadRcp^ZIc{RKs3yz`sfSb~^Q2lj7)%dyiilvABs&=;KF%DNYf(+kNFAg=l!w534QkMv zn#`}|cxN?n4f35Px*e$oUN;rgUh5a7DpoOFy7gMWo=NFds*;a_K06S4mkDMOtzLV|@y@YtuXqEED;Z}vU+L3j^;g5f?+jn~H%>ce)Lm^-BDa!bZ(t^F(dyJvgmyd~5e{0$QATjLuWHwbarj!!HB8{sVd&`6(=#pBn|M!wQT^K%nLE?XY#nu(!DHv;L~C54E=60>JZhp+13-Mmt1lSa5ag z%cj<<35M#2Wz}seH6IgenqENHR1IOkB8N9FGhVDPUS50PgKTvc8GF+6&PT7YZ*0f084K^zg>d)RVkGGDfk zX6{DJCAHeODplHNtj20o`KQje+YHdRAd0nyf+4#x?W3DNHaH&k6{+o(tJ%`-Fe~C9 zryo&FK0Jb5U@m^JdKa@+m)Ls=%>`TxDneL?@E~Xa@<&b6h0MPRy5{5M-M6aw@^BT~$`fTOj=GVfuzweZ}wKT(riAv)(=FwFGBQ>*12eKobd8(A&S9{-ey zT4ap@mMbNPAO>=@SY7Z5x>)+b4vQ3ZMhcd`e3M=aTg+>{Iow4 zP83uX9is1njYxyYFs7r61|_;V!`Y)|362{gFVyMd3ibU6=N3>_S4pfg+h@&uxN55r zW`SRymD+ChgK5=!lUP(BZGW0thPPc?@X_WNQY*o}GvKq;)>(!xPD%+OqLJH7qLGS& zt3~bILPCKVa(CaiXT82SMX%)6?2EnFs&zGeW!aJa;QpKrzoZozLu8n5^84fs6be3A zzD4wO@mmV^o4**?hG4q20=`TX>1Iej?)fwn_Dh}$M3xl7fnWl7sVeI={NwSxV zaI9h|;VjchSRd?$RR!NEwuqbh_LmV%G%>2MvQU+4Fr1lRI;4jnobA{RR1@5Ceuk7S z562YpRDxrFC2$mziLKEXI>nvx^+@=>FM!T+b@F@|!g6rpA;8pl3ZFk?uSJ>< z@>vfh=(U14_w-s=Zu{2?=fxQ|h?X`c81zW)8jVQFDHIYXMPbQ}IP3jb4d~fgh%w^#dTSw!kj~}K`=2AoW6nRQa*T=(eJ?{o7 zvidp_>6ykdRV9!XKkKwE{gYO2CHl@{;H|Ea-F%W#da=|LemSC=RackBC9N7ggyqud zAm*lZ5udfCv-=j=>TaY(k*fUQCnWrRxN2->x+8QBwx#F zDqWr67|P2}Knc&AzdBn+wdcM0t|TO{1gw=d9EfhmXWZ$0-8=6)1}nsjKI03jA;P=e zZ7;8K_<){G+K_5c&xEdjjb-9Wj3*#sDVU8JYVO?O_WDn>55BMoEp8Bpqa8u0T#G~? zTsRRx6j;C4RU9MUZGF`;Ircm`i>G#*M2DmJqkmbH1jJ#ZV1c5dpi^%=7UvYx zjgc;C&n~=JpqdUtnR{(a&)}8D{9qc1asX(Rc^yhbw6kRJW64&*3XaZJO(h*2NV%`~ zDD5|?n_<#J_Rn4yy8WJOdCz7PUcE-?iXtU3F?As;)^e#L$s%wrr+~7DdHt}+TF5%o zMJ8^=c4soGg~Er>RSw71qI+fAw=fS#AnsW#6T0 zWE{jw$H;?9EaRGuV+YS5w#_a%nNi(b_>u?Cgn@(N~#6;gm$rK)t5hbqo!g z5gZZSYIm(i;#L~&B>c(T^;B-qEFw^=E-Wg3o~GhH{ibJ+?xyj*h$Zr+7Dqdmr_^pY zc{&H=tg16y?zC?9<&h3|8pOHF10sYKY13p17eM@2aNuu={FZG^`Y6ZTu(uwTr@iqE z?iJ_Pu}9ww=Bh`e&E`j9rPC^m^UBP^Q>R3YlZl*fN_1l&=KO?!lGAk;w$ws!>Npuz zSW=;C!5E2(kT$ODzF}6clA-CqNieL&@4`ilk zZd<>;sHYtke3NpEOl3eaBktHHhN+gyr#JUqlBVG{6dbg)NKUm&wtO9H!RL4B^{+73 zd)0ty6l$-=-D`|BN|r!{CBJep_hL^cfm7Y^)P~Wyo^{I{!j9y|1_BmQOyMj%-w!IE z?z`>(mEl7o&Ch12lCR%WwXDxiW?E2hC0h&C>_#nb8RodsSmjM}h@=Ml_xXY7ZEi%M zzHGK@!{U+ArnmC?GcIXUZh(a`5Zd$ab$=pFd^oGPdxV+msZWkx5b$h&ZJv|ov`6^s2#iLUx zpEe;w(B;}!tL7_I$9YHXdMYAOgV%~H!}<0;Vzia@X6=sMByG6_s+*u#Sxu(Zs_bu@ zRPaa1#$Z7_lZ0M2+>otX-+C~c{dJbI;~3$ec1Di-_voF=kxeL?V~mFq!Vaz;H1l( z{Kzn-^VyIH1jDXjt2p~e60?ad&*v2s7;HK_Ex$PJz#tH-am`^UAN4`O9 z;o-O4X0oP`-izwk$4OCa?Vs{g+ve0LSAME!hVExL3$(ZBNzZn3YdA&U(OLIITw~%H z<6yPD4V5F~9IOK%d@w%P6-pP44Jm1%xD0U(eWY^}gAtm&Y>Yl9*&Cy% zvp&KYzQ2pGI4DEXC8`uCb!4;V+AFcT)6ig-abfS3S0m5s*8K|3VtF)V;)8>_pUV8I z63;r0_HNh+x)UiZ2z+HX0Yuw;3cvk^0##f9q9d>o;)dD&>pVo-GV`D8BV-^g9={y= zQ>ePl{N(k-uXs*c_SN0|e?NXAFSbpfe-8@I|NcBb2l~P#G3NjhfCdeScZAaV0@+Cj zhKqaKXuvHpjGqMhFXbZtt4#7BCh_)}}oW2m6NP(EiLM?ou)+A^W1{zI< zUd+R-(NrFXRCi5bMri8LL};WVSmT17_l3ofVDX2rtBzn%S+K+*71j8^LRkbnoZ1FM zRegx4mqotuMYfQjl7)yCSyW#l@|81c>=1RQ5H;mX({uQt3YIe{ z95I}2)8pirlAa$4C%sfY+FE}hmzW^OL$|0{-=HeTV&?aS-;Bwt{@><-^rMM2{0mlx zqZadeS(~YaCr2-dNwQH%Tk7@Y_l_{vgo*(GX_?Le2ogC&WJpm^F8YRz%JNDkJm*y~M z3_t&*5n8Q2|?g`Q2zxHZT* zo6Fe9pGr@&-9P3X@e`nKz|c*L)EukoFz7e>H4N7CzVzoTm=?>KX6`ZPWj0p}^e+iH z7GF%xCLYTen2WZV`@t35L5e-NdUzV2@5s(1hkr=3#6p|PFu`U{O zEG=~`@zY$!%Us+~UaH4K3aX%Ra3B>qE?dOF*dedjWiC&sm(EI7>^l~|+An_zuJDNn zmv(e;{67KC3^DWTXE0y_QXq_C69ZJB2o4Ed#_b@i;2rS*XPn9kR$u}qAO;w*4kjut z6bT9mQ!e7NF%eTGP16^vli98lAG4F;Xb~*|lVm2~2#f$Zl7JP404l722Ot107@!D@ zKnNxxfY8MWfX5(?fFcZn27bV)W?%x6pbqH32qd5g3PJ*|U@ok{RqQW1+sHF95;XG? zG7l3q&5}YtlOHFPGFLM^=b~f`A_xC4z(4eetXf88AYhnSArB}J2adoFx=tbRpa^aN z2oxd;y1)l~pgCE=LWV#J&Y%v)Z3w&oKkPsVdSD3jlaMCB0=|G1oZ#xHkrmEB3Iqa5 zabO5&<_fw%2zG!Dq5ubmi4J^V2cAd@zNia!#zJf~E|e$_q+kcGUhhT-4hZk@F0)ij~vg`rA;3j~eSfQX* z3IafzAOgq@YYYMpB)|?rq6jA70os5@?=(;Ir(V0jPgN8bIa4nN)leOCGBmDp+XDl7fEB7>J(j=@@Sq8_V!Ot* zQVJpkd;n8;HBySC1wNKQmULVC69UR;0#Ft`L8K&@GXkst2inF(s=^3%UDMIZ5YdGT(Hib9nYEPw~nb#9P?6|`VIid6!#Vp#4oa4}_7Sb+t2 z^;3a0KPP2oTcAH1fQv97WzaJN3IfxRa{{0M2f9Zo3c>;wEoGu$Jy;+Jf}kNHfOnA7 zKsTianqWOxfe9#r2w(z1i6UYnRtmTlI=vPz`ZX^al{+W(LN!!i1GZo@lzE*u&MMVI zGUVSgjxeyhbPv z!Ug&>0=`HBayEp#CqdD(4n+4=WW;oLAY<(}EPNs+WY-A5mp*U+3T)Q`c7Q^3NNedf zgY6)7r7%zpqImxq^-x22J5_N}@8WnVGIrK2g_}D5^k;6Ae`X7e4ux-h5^bc zD^@mqhh==76#{y|T_Ys}aJ5;Vbq*rH1uozL;P+*>vYvAP9D(hz4{DOqXH{qDZl*2!Nmqn!p)_U}vI$2a14;3gQf=#$|Y52(l)K zX4zA8s1E;x;0GA73ar2&!oUZDzNyuM%w+B47WU;-+@htOcYCV)q6(Sr*Dgilz651ND% zgKdo$q3r^Nky4@MvYZ#%gmr0p-?ph9Z7%XaZZQBqfXxY7Ab(Oo3Fy-r37QoQ`k@`V zE+85vC7L?T);rPmG!&cS~F96rCqwM**dO`(xL-mh8^R|%!e2bbX*`e{+{8sY#dRDfSG=HGT)`z= ztO2}`ZyYi-8!wKWr4d`mDSXGBT$c!t2^&xVqg=`t@Byv70kJ&G7Z3xvyvvvH%L{PJ z#T)?@u*?h4%hCM*)O^kPkIdbC|KL2%tDw#4yb7G~$@Tm$s4x-5&MZxCmqr+V9~WO(*3;B3w;sy{L_J}$5r~rXS>Kt_`?&L zxDnO0=OW1uTh)=A)#W0kK|L_3yTn}_#+h8k?fSd1Tflj}#{06?iM>cfJ;VQBJH(G$ zzHJ=XcU{=qJIH^1tc!imZ{5^OeZkio+i8-&ja=5xd&9#W)i1i*LH*jDz1fHS+0}i< z*?rpGz1?+v*v;MAkDb)J9oJ8N#M4{fMcmu7UE1Hfsq5Xs(Oux9-PZ>`-L?C{q20k5 zp22gv;GO*7ZVLjCW9ujGO$88?x<^ADhe&=%?={26)CpzdYYUp*m=!rby zlYZSXUg`H8>Pg(`W81!^p1!I6=&K&<7hdZF{^hU!=F48~RXprbeC+?T{^E7s>|vhn z*FNO$p6y-!z~R2a<^JQ1{_O3Z=h1%IJAUr@p75Le@5MXt_kQr}-tB=L@0&jH1;6d} z9xFxN@dvu(e_qM?y|6?7)w%xby}sWu|H3_A>_K1IpPt$KUh^eC@0q^x7k~2ozV#pf z^*28DSKs!@e)t)G^Ov6XbDr~g-}Mpv_q)F5w?6fY`}9d4_MgA--+uYS8}bd`@_l~v zQNHx)V&%sl+zsO7L0tAlUi*8S`;VXab^rF$KKF%x_TfMBlRx{}zqH*S_iO+10fLpl zfdmU0JV?-n7K98NGSp(jp~Q)-AX2=DP$9;R2^VtQXz?S+h$8{*c{(33+w_WXHNXiuU`jp9W5R4GoTIBx>IiZ!d&tz2PR z?WvPzQmjyGQZ?(eW?7nRXP%|`R%TndQqf}l+BL7pI^y}NtjXm(r;_0u(hku?+?{PHXM*shDbRb3rD%f6f0lGBbeG*Pc z;Y1O(1fE(AmbBnS37YhvdmA>lUQzs=XyR2DR>)$D6RwC-hLxe%REgoyI8}(J>B!-T zA9YC4hbiUfVv$Bl2xCby*4AH&PTt6#jXC0|p_DZxd1RJa_J!p~OVZV3jsEdSB9(BF zDUy&A0lB7+2%`B>ms`$Br;0>|wqKNHx&`N(dxANpl}!F=-gR{zis)W)ZuF&eVNMyS zo|20BWR-+IXJ?|CZkj1jjV|WqNP_ZtDSDAsI^(2}0<|couD*Iu5IgulYpu54ifgXA z?qI>Jy8a5Rt~Ov0Y_Z1rFzm6)>WYC4%RXysv(W!So9wjDR%@-Y(q^lzv)mes?YF^( zEAFq`mJ4hKbim4PyByikM!fRQOK-jQ-rI&6SEzw+zy8*%#u@(({BOVnAAIi_{36V- zyb3qmu)z>BEb+t#KWy>E3uj!gz#I=;@yGpQEONhc@ZxUDDzD6P%PzkRbIdZ&OmodP z-;8t4I`7PL&p!VQbkIT%O?1&lAB}X_>Z-5Kdh4#g4twmf Q&rW;ow%?9B*8u?lJIV*8p#T5? literal 0 HcmV?d00001 From bd01c6a78ebd15752fa8919bab6428c87ca69fc3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 11 Jan 2023 01:34:32 +0000 Subject: [PATCH 42/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index b8f0410da384a..15d17c4871701 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -172,13 +172,13 @@ The color of the trajectory indicates the error (meter) from the reference traje drawing drawing ## Dynamic map loading + Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this feature, NDT dynamically requests for the surrounding pointcloud map to `pointcloud_map_loader`, and then receive and preprocess the map in an online fashion. Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error) drawing - ### Additional interfaces #### Additional inputs From ee26d573c49dd4684e7013028ff67441ef2916fb Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 11 Jan 2023 17:09:20 +0900 Subject: [PATCH 43/48] minor fix Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_update_module.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index fe845dee549c5..d44d10316e17a 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -190,9 +190,7 @@ void MapUpdateModule::update_ndt( RCLCPP_INFO( logger_, "Update map (Add: %d, Remove: %d)", static_cast(maps_to_add.size()), static_cast(map_ids_to_remove.size())); - if ( - (static_cast(maps_to_add.size()) == 0) & - (static_cast(map_ids_to_remove.size()) == 0)) { + if (maps_to_add.empty() & map_ids_to_remove.empty()) { RCLCPP_INFO(logger_, "Skip map update"); return; } From f61552f3fb079be0d44697b36820cf4c7b5c8588 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 11 Jan 2023 17:23:00 +0900 Subject: [PATCH 44/48] Update localization/ndt_scan_matcher/src/map_update_module.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> --- localization/ndt_scan_matcher/src/map_update_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index d44d10316e17a..5a335a3896d45 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -190,7 +190,7 @@ void MapUpdateModule::update_ndt( RCLCPP_INFO( logger_, "Update map (Add: %d, Remove: %d)", static_cast(maps_to_add.size()), static_cast(map_ids_to_remove.size())); - if (maps_to_add.empty() & map_ids_to_remove.empty()) { + if (maps_to_add.empty() && map_ids_to_remove.empty()) { RCLCPP_INFO(logger_, "Skip map update"); return; } From ac3f0439fcae66d840afe5cdcc6f941ade211265 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 11 Jan 2023 17:30:48 +0900 Subject: [PATCH 45/48] update literals Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_update_module.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 5a335a3896d45..2e8052b41d413 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -188,8 +188,7 @@ void MapUpdateModule::update_ndt( const std::vector & map_ids_to_remove) { RCLCPP_INFO( - logger_, "Update map (Add: %d, Remove: %d)", static_cast(maps_to_add.size()), - static_cast(map_ids_to_remove.size())); + logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size()); if (maps_to_add.empty() && map_ids_to_remove.empty()) { RCLCPP_INFO(logger_, "Skip map update"); return; @@ -216,7 +215,7 @@ void MapUpdateModule::update_ndt( const double exe_time = std::chrono::duration_cast(exe_end_time - exe_start_time).count() / 1000.0; - RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %f [ms]", exe_time); + RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); // swap (*ndt_ptr_mutex_).lock(); From de0ef189e157cacf7adab451f9d5660742bd6790 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 19 Jan 2023 15:56:32 +0900 Subject: [PATCH 46/48] update map_loader default parameters --- map/map_loader/config/pointcloud_map_loader.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index 8f3ccbff00360..61b02c490c663 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -2,8 +2,8 @@ ros__parameters: enable_whole_load: true enable_downsampled_whole_load: false - enable_partial_load: false - enable_differential_load: false + enable_partial_load: true + enable_differential_load: true # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] From fbaac694f6c212acc621bbe7016fd32aeb77c6dc Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 27 Jan 2023 10:55:38 +0900 Subject: [PATCH 47/48] update readme Signed-off-by: kminoda --- localization/ndt_scan_matcher/README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 15d17c4871701..1dd3c9f625c5b 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -214,7 +214,11 @@ To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appr Follow the next two instructions. 1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) -2. split the PCD files into grids (recommended split size: 20[m] x 20[m]) +2. split the PCD files into grids (recommended size: 20[m] x 20[m]) + +Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of +- one PCD map file +- multiple PCD map files divided into small size (~20[m]) Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) From 1503f7da9b00d165a2141a9620caf357d3367cb9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 27 Jan 2023 01:57:16 +0000 Subject: [PATCH 48/48] ci(pre-commit): autofix --- localization/ndt_scan_matcher/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 1dd3c9f625c5b..7c63785aebcd2 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -217,6 +217,7 @@ Follow the next two instructions. 2. split the PCD files into grids (recommended size: 20[m] x 20[m]) Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of + - one PCD map file - multiple PCD map files divided into small size (~20[m])