From 6ff47cf181fb1c2ee3f226f06f56e4fde9d32cd0 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 9 Jan 2024 09:36:15 +0900 Subject: [PATCH 001/154] refactor(ndt_scan_matcher): removed tf2_listener_module (#6008) Removed tf2_listener_module Signed-off-by: Shintaro SAKODA --- localization/localization_util/CMakeLists.txt | 1 - .../localization_util/tf2_listener_module.hpp | 43 ------------- .../src/tf2_listener_module.cpp | 60 ------------------- .../ndt_scan_matcher/map_update_module.hpp | 1 - .../ndt_scan_matcher_core.hpp | 6 +- .../src/ndt_scan_matcher_core.cpp | 50 ++++++++++++---- 6 files changed, 43 insertions(+), 118 deletions(-) delete mode 100644 localization/localization_util/include/localization_util/tf2_listener_module.hpp delete mode 100644 localization/localization_util/src/tf2_listener_module.cpp diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index ade446020d101..fe65077d89649 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -6,7 +6,6 @@ autoware_package() ament_auto_add_library(localization_util SHARED src/util_func.cpp - src/tf2_listener_module.cpp src/smart_pose_buffer.cpp ) diff --git a/localization/localization_util/include/localization_util/tf2_listener_module.hpp b/localization/localization_util/include/localization_util/tf2_listener_module.hpp deleted file mode 100644 index b332f9effe0e0..0000000000000 --- a/localization/localization_util/include/localization_util/tf2_listener_module.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2015-2019 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 LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ -#define LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ - -#include - -#include -#include -#include - -#include - -class Tf2ListenerModule -{ - using TransformStamped = geometry_msgs::msg::TransformStamped; - -public: - explicit Tf2ListenerModule(rclcpp::Node * node); - bool get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr) const; - -private: - rclcpp::Logger logger_; - tf2_ros::Buffer tf2_buffer_; - tf2_ros::TransformListener tf2_listener_; -}; - -#endif // LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ diff --git a/localization/localization_util/src/tf2_listener_module.cpp b/localization/localization_util/src/tf2_listener_module.cpp deleted file mode 100644 index 8a19ceec9f30d..0000000000000 --- a/localization/localization_util/src/tf2_listener_module.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2015-2019 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 "localization_util/tf2_listener_module.hpp" - -#include - -geometry_msgs::msg::TransformStamped identity_transform_stamped( - const builtin_interfaces::msg::Time & timestamp, const std::string & header_frame_id, - const std::string & child_frame_id) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = timestamp; - transform.header.frame_id = header_frame_id; - transform.child_frame_id = child_frame_id; - transform.transform.rotation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); - transform.transform.translation = tier4_autoware_utils::createTranslation(0.0, 0.0, 0.0); - return transform; -} - -Tf2ListenerModule::Tf2ListenerModule(rclcpp::Node * node) -: logger_(node->get_logger()), tf2_buffer_(node->get_clock()), tf2_listener_(tf2_buffer_) -{ -} - -bool Tf2ListenerModule::get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, const TransformStamped::SharedPtr & transform_stamped_ptr) const -{ - const TransformStamped identity = - identity_transform_stamped(timestamp, target_frame, source_frame); - - if (target_frame == source_frame) { - *transform_stamped_ptr = identity; - return true; - } - - try { - *transform_stamped_ptr = - tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(logger_, "%s", ex.what()); - RCLCPP_ERROR(logger_, "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - *transform_stamped_ptr = identity; - return false; - } - return true; -} 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 5457a6c310f5d..8b192b0186b42 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,7 +15,6 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "localization_util/tf2_listener_module.hpp" #include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.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 6a5c61041f0da..b8792c20c323e 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,7 +18,6 @@ #define FMT_HEADER_ONLY #include "localization_util/smart_pose_buffer.hpp" -#include "localization_util/tf2_listener_module.hpp" #include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" @@ -41,6 +40,8 @@ #include #include #include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -176,6 +177,8 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Service::SharedPtr service_trigger_node_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + tf2_ros::Buffer tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; rclcpp::CallbackGroup::SharedPtr timer_callback_group_; @@ -213,7 +216,6 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::shared_ptr tf2_listener_module_; std::unique_ptr map_module_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; 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 7f6fe605c42b0..7108dca20d8d7 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -68,6 +68,8 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), + tf2_buffer_(this->get_clock()), + tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), output_pose_covariance_({}), @@ -262,8 +264,6 @@ NDTScanMatcher::NDTScanMatcher() &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - tf2_listener_module_ = std::make_shared(this); - use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); if (use_dynamic_map_loading_) { map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); @@ -574,11 +574,25 @@ void NDTScanMatcher::transform_sensor_measurement( const pcl::shared_ptr> & sensor_points_input_ptr, pcl::shared_ptr> & sensor_points_output_ptr) { - auto tf_target_to_source_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - this->now(), target_frame, source_frame, tf_target_to_source_ptr); + if (source_frame == target_frame) { + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + // Since there is no clear error handling policy, temporarily return as is. + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - tier4_autoware_utils::transform2pose(*tf_target_to_source_ptr); + tier4_autoware_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); tier4_autoware_utils::transformPointCloud( @@ -863,13 +877,27 @@ void NDTScanMatcher::service_ndt_align( 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( - get_clock()->now(), map_frame_, req->pose_with_covariance.header.frame_id, tf_pose_to_map_ptr); + const std::string & target_frame = map_frame_; + const std::string & source_frame = req->pose_with_covariance.header.frame_id; + + geometry_msgs::msg::TransformStamped transform_s2t; + try { + transform_s2t = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + // Note: Up to AWSIMv1.1.0, there is a known bug where the GNSS frame_id is incorrectly set to + // "gnss_link" instead of "map". The ndt_align is designed to return identity when this issue + // occurs. However, in the future, converting to a non-existent frame_id should be prohibited. + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + transform_s2t.header.stamp = get_clock()->now(); + transform_s2t.header.frame_id = target_frame; + transform_s2t.child_frame_id = source_frame; + transform_s2t.transform = tf2::toMsg(tf2::Transform::getIdentity()); + } // transform pose_frame to map_frame - const auto initial_pose_msg_in_map_frame = - transform(req->pose_with_covariance, *tf_pose_to_map_ptr); + const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); if (use_dynamic_map_loading_) { map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); } From 1ab746f82f568883b726184bf21ac50d6422d24b Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Tue, 9 Jan 2024 09:49:43 +0900 Subject: [PATCH 002/154] docs(ekf_localizer): improve readme and explain confusing process (#5834) * update ekf_localizer/README.md Signed-off-by: Kento Yabuuchi * add comment to explain Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix * update readme Signed-off-by: Kento Yabuuchi * rename pi Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix * fix typo Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ekf_localizer/README.md | 76 +++++-------------- localization/ekf_localizer/src/ekf_module.cpp | 10 ++- 2 files changed, 29 insertions(+), 57 deletions(-) diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index bda1c84a2747c..87cc510c5fe14 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -32,71 +32,33 @@ This package includes the following features:

-## Launch - -The `ekf_localizer` starts with the default parameters with the following command. - -```sh -roslaunch ekf_localizer ekf_localizer.launch -``` - -The parameters and input topic names can be set in the `ekf_localizer.launch` file. - ## Node ### Subscribed Topics -- measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Input pose source with the measurement covariance matrix. - -- measured_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - Input twist source with the measurement covariance matrix. - -- initialpose (geometry_msgs/PoseWithCovarianceStamped) - - Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. +| Name | Type | Description | +| -------------------------------- | ------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `measured_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose source with the measurement covariance matrix. | +| `measured_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Input twist source with the measurement covariance matrix. | +| `initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. | ### Published Topics -- ekf_odom (nav_msgs/Odometry) - - Estimated odometry. - -- ekf_pose (geometry_msgs/PoseStamped) - - Estimated pose. - -- ekf_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance. - -- ekf_biased_pose (geometry_msgs/PoseStamped) - - Estimated pose including the yaw bias - -- ekf_biased_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance including the yaw bias - -- ekf_twist (geometry_msgs/TwistStamped) - - Estimated twist. - -- ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - The estimated twist with covariance. - -- diagnostics (diagnostic_msgs/DiagnosticArray) - - The diagnostic information. +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ----------------------------------------------------- | +| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | +| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | +| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | +| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | +| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | +| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | +| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | +| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | ### Published TF - base_link - - TF from "map" coordinate to estimated pose. + TF from `map` coordinate to estimated pose. ## Functions @@ -205,7 +167,9 @@ Increasing the number will improve the smoothness of the estimation, but may hav -where `b_k` is the yawbias. +where, $\theta_k$ represents the vehicle's heading angle, including the mounting angle bias. +$b_k$ is a correction term for the yaw bias, and it is modeled so that $(\theta_k+b_k)$ becomes the heading angle of the base_link. +The pose_estimator is expected to publish the base_link in the map coordinate system. However, the yaw angle may be offset due to calibration errors. This model compensates this error and improves estimation accuracy. ### time delay model @@ -240,7 +204,7 @@ Note that, although the dimension gets larger since the analytical expansion can ## Known issues -- In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. +- If multiple pose_estimators are used, the input to the EKF will include multiple yaw biases corresponding to each source. However, the current EKF assumes the existence of only one yaw bias. Therefore, yaw bias `b_k` in the current EKF state would not make any sense and cannot correctly handle these multiple yaw biases. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. ## reference diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 573472fe2dabf..8977d82f34138 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -87,9 +87,15 @@ geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( { const double x = kalman_filter_.getXelement(IDX::X); const double y = kalman_filter_.getXelement(IDX::Y); + /* + getXelement(IDX::YAW) is surely `biased_yaw`. + Please note how `yaw` and `yaw_bias` are used in the state transition model and + how the observed pose is handled in the measurement pose update. + */ const double biased_yaw = kalman_filter_.getXelement(IDX::YAW); const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB); const double yaw = biased_yaw + yaw_bias; + Pose current_ekf_pose; current_ekf_pose.header.frame_id = params_.pose_frame_id; current_ekf_pose.header.stamp = current_time; @@ -224,7 +230,9 @@ bool EKFModule::measurementUpdatePose( return false; } - /* Set yaw */ + /* Since the kalman filter cannot handle the rotation angle directly, + offset the yaw angle so that the difference from the yaw angle that ekf holds internally is less + than 2 pi. */ double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi From a96c71cd86499eb85e24a80f9b5dbc005b11ce8b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 9 Jan 2024 11:10:00 +0900 Subject: [PATCH 003/154] refactor(dynamic_avoidance): separate dynamic avoidance module (#6019) * refactor(dynamic_avoidance): separate dynamic avoidance module Signed-off-by: Takayuki Murooka * move figures Signed-off-by: Takayuki Murooka * update .pages Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- planning/.pages | 2 +- .../CMakeLists.txt | 25 ++++ .../README.md} | 6 +- .../config}/dynamic_avoidance.param.yaml | 0 .../drivable_area_extraction_width.drawio.svg | 0 .../image}/opposite_directional_object.svg | 0 .../image}/same_directional_object.svg | 0 .../manager.hpp | 8 +- .../scene.hpp} | 9 +- .../package.xml | 42 ++++++ .../plugins.xml | 2 +- .../src}/manager.cpp | 2 +- .../src/scene.cpp} | 2 +- ...t_behavior_path_planner_node_interface.cpp | 123 ++++++++++++++++++ planning/behavior_path_planner/CMakeLists.txt | 4 - planning/behavior_path_planner/README.md | 2 +- ...t_behavior_path_planner_node_interface.cpp | 15 +-- 17 files changed, 211 insertions(+), 31 deletions(-) create mode 100644 planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt rename planning/{behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md => behavior_path_dynamic_avoidance_module/README.md} (94%) rename planning/{behavior_path_planner/config/dynamic_avoidance => behavior_path_dynamic_avoidance_module/config}/dynamic_avoidance.param.yaml (100%) rename planning/{behavior_path_planner/image/dynamic_avoidance => behavior_path_dynamic_avoidance_module/image}/drivable_area_extraction_width.drawio.svg (100%) rename planning/{behavior_path_planner/image/dynamic_avoidance => behavior_path_dynamic_avoidance_module/image}/opposite_directional_object.svg (100%) rename planning/{behavior_path_planner/image/dynamic_avoidance => behavior_path_dynamic_avoidance_module/image}/same_directional_object.svg (100%) rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance => behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module}/manager.hpp (82%) rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp => behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp} (97%) create mode 100644 planning/behavior_path_dynamic_avoidance_module/package.xml rename planning/{behavior_path_planner => behavior_path_dynamic_avoidance_module}/plugins.xml (72%) rename planning/{behavior_path_planner/src/scene_module/dynamic_avoidance => behavior_path_dynamic_avoidance_module/src}/manager.cpp (99%) rename planning/{behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp => behavior_path_dynamic_avoidance_module/src/scene.cpp} (99%) create mode 100644 planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp diff --git a/planning/.pages b/planning/.pages index c41a1cc8c6603..e244f1d1ea25d 100644 --- a/planning/.pages +++ b/planning/.pages @@ -12,7 +12,7 @@ nav: - 'Scene Module': - 'Avoidance': planning/behavior_path_avoidance_module - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module - - 'Dynamic Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design + - 'Dynamic Avoidance': planning/behavior_path_dynamic_avoidance_module - 'Goal Planner': planning/behavior_path_goal_planner_module - 'Lane Change': planning/behavior_path_lane_change_module - 'Side Shift': planning/behavior_path_side_shift_module diff --git a/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt b/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt new file mode 100644 index 0000000000000..98874b8923324 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_dynamic_avoidance_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/manager.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/planning/behavior_path_dynamic_avoidance_module/README.md similarity index 94% rename from planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md rename to planning/behavior_path_dynamic_avoidance_module/README.md index f303937bd0f78..5e8be9136886c 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md +++ b/planning/behavior_path_dynamic_avoidance_module/README.md @@ -32,16 +32,16 @@ First, a maximum lateral offset to avoid is calculated as follows. The polygon's width to extract from the drivable area is the obstacle width and double `drivable_area_generation.lat_offset_from_obstacle`. We can limit the lateral shift offset by `drivable_area_generation.max_lat_offset_to_avoid`. -![drivable_area_extraction_width](../image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg) +![drivable_area_extraction_width](./image/drivable_area_extraction_width.drawio.svg) Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows considering TTC (time to collision). Regarding the same directional obstacles, obstacles whose TTC is negative will be ignored (e.g. The obstacle is in front of the ego, and the obstacle's velocity is larger than the ego's velocity.). Same directional obstacles -![same_directional_object](../image/dynamic_avoidance/same_directional_object.svg) +![same_directional_object](./image/same_directional_object.svg) Opposite directional obstacles -![opposite_directional_object](../image/dynamic_avoidance/opposite_directional_object.svg) +![opposite_directional_object](./image/opposite_directional_object.svg) ## Parameters diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml rename to planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg b/planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg rename to planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg b/planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg rename to planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg b/planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg rename to planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp rename to planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp index 9e08f309a78b3..c2c17c4e55c1e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_dynamic_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -52,4 +52,4 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp similarity index 97% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp rename to planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index 50f5b61e15169..d2f9cd95066d9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ #include "behavior_path_planner_common/interface/scene_module_interface.hpp" @@ -23,9 +23,6 @@ #include #include #include -#include -#include -#include #include #include @@ -413,4 +410,4 @@ class DynamicAvoidanceModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_dynamic_avoidance_module/package.xml b/planning/behavior_path_dynamic_avoidance_module/package.xml new file mode 100644 index 0000000000000..11792a15b2cd2 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/package.xml @@ -0,0 +1,42 @@ + + + + behavior_path_dynamic_avoidance_module + 0.1.0 + The behavior_path_dynamic_avoidance_module package + + Takayuki Murooka + Yuki Takagi + Satoshi Ota + Kosuke Takeuchi + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + behavior_path_planner + behavior_path_planner_common + geometry_msgs + lanelet2_core + lanelet2_extension + motion_utils + object_recognition_utils + pluginlib + rclcpp + signal_processing + tier4_autoware_utils + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_dynamic_avoidance_module/plugins.xml similarity index 72% rename from planning/behavior_path_planner/plugins.xml rename to planning/behavior_path_dynamic_avoidance_module/plugins.xml index a16f3dd7bddee..fd2e1bc4137b7 100644 --- a/planning/behavior_path_planner/plugins.xml +++ b/planning/behavior_path_dynamic_avoidance_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp rename to planning/behavior_path_dynamic_avoidance_module/src/manager.cpp index f39727246d722..569c6a17b5b32 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "behavior_path_dynamic_avoidance_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp rename to planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index 01635d7a6cb70..ca58bbf0adcaa 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_dynamic_avoidance_module/scene.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/utils.hpp" diff --git a/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..16b4ffbc688ae --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,123 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "behavior_path_planner/behavior_path_planner_node.hpp" + +#include +#include +#include + +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_dynamic_avoidance_module") + + "/config/dynamic_avoidance.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f3727f35a492f..54b4bf122da00 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,13 +7,9 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) - ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp src/behavior_path_planner_node.cpp - src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp - src/scene_module/dynamic_avoidance/manager.cpp ) target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 6f462e17248f0..03f8c8d2a5696 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -28,7 +28,7 @@ Behavior Path Planner has following scene modules | :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------------------- | | Lane Following | this module generates reference path from lanelet centerline. | LINK | | Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_module/README.md) | -| Dynamic Avoidance | WIP | LINK | +| Dynamic Avoidance | WIP | [LINK](../behavior_path_dynamic_avoidance_module/README.md) | | Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | | Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | | External Lane Change | WIP | LINK | diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 28ee6d31e80dc..7ba934e873a8d 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -50,21 +50,18 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml"}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml"}); return std::make_shared(node_options); } From 4a419f834f6aa5b1239ccbba07ac251a30a935be Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 9 Jan 2024 11:11:13 +0900 Subject: [PATCH 004/154] feat(probabilistic_occupancy_grid_map): add grid map fusion node (#5993) * add synchronized ogm fusion node Signed-off-by: yoshiri * add launch test for grid map fusion node Signed-off-by: yoshiri * fix test cases input msg error Signed-off-by: yoshiri * change default fusion parameter Signed-off-by: yoshiri * rename parameter for ogm fusion Signed-off-by: yoshiri * feat: add multi_lidar_ogm generation method Signed-off-by: yoshiri * enable ogm creation launcher in tier4_perception_launch to call multi_lidar ogm creation Signed-off-by: yoshiri * fix: change ogm fusion node pub policy to reliable Signed-off-by: yoshiri * chore: remove files outof scope with divied PR Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- ...robabilistic_occupancy_grid_map.launch.xml | 16 +- .../CMakeLists.txt | 27 ++ .../README.md | 17 + ...nchronized_grid_map_fusion_node.param.yaml | 20 + .../synchronized_grid_map_fusion.drawio.svg | 290 +++++++++++ .../include/cost_value.hpp | 33 +- .../fusion/single_frame_fusion_policy.hpp | 66 +++ .../synchronized_grid_map_fusion_node.hpp | 127 +++++ ...grid_map_log_odds_bayes_filter_updater.hpp | 48 ++ .../include/utils/utils.hpp | 3 + ...nized_occupancy_grid_map_fusion.launch.xml | 10 + .../src/fusion/single_frame_fusion_policy.cpp | 322 ++++++++++++ .../synchronized_grid_map_fusion_node.cpp | 458 ++++++++++++++++++ ...grid_map_log_odds_bayes_filter_updater.cpp | 69 +++ .../src/utils/utils.cpp | 16 + .../synchronized_grid_map_fusion.md | 168 +++++++ .../test/cost_value_test.cpp | 47 ++ .../test/fusion_policy_test.cpp | 75 +++ .../test_synchronized_grid_map_fusion_node.py | 255 ++++++++++ 19 files changed, 2064 insertions(+), 3 deletions(-) create mode 100644 perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml create mode 100644 perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg create mode 100644 perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp create mode 100644 perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp create mode 100644 perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp create mode 100644 perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml create mode 100644 perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp create mode 100644 perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp create mode 100644 perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp create mode 100644 perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md create mode 100644 perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp create mode 100644 perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp create mode 100644 perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index 751eeea66c7b6..281a52a85af71 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -8,7 +8,7 @@ - + @@ -48,4 +48,18 @@ + + + + + + + + + + + + + + diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 035845772ce53..928532f928e38 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -55,6 +55,24 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map EXECUTABLE laserscan_based_occupancy_grid_map_node ) +# GridMapFusionNode +ament_auto_add_library(synchronized_grid_map_fusion SHARED + src/fusion/synchronized_grid_map_fusion_node.cpp + src/fusion/single_frame_fusion_policy.cpp + src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp + src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp + src/utils/utils.cpp +) + +target_link_libraries(synchronized_grid_map_fusion + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(synchronized_grid_map_fusion + PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode" + EXECUTABLE synchronized_grid_map_fusion_node +) + ament_auto_package( INSTALL_TO_SHARE launch @@ -66,13 +84,22 @@ if(BUILD_TESTING) # launch_testing find_package(launch_testing_ament_cmake REQUIRED) add_launch_test(test/test_pointcloud_based_method.py) + add_launch_test(test/test_synchronized_grid_map_fusion_node.py) # gtest ament_add_gtest(test_utils test/test_utils.cpp ) + ament_add_gtest(costmap_unit_tests + test/cost_value_test.cpp) + ament_add_gtest(fusion_policy_unit_tests + test/fusion_policy_test.cpp + src/fusion/single_frame_fusion_policy.cpp + ) target_link_libraries(test_utils ${PCL_LIBRARIES} ${PROJECT_NAME}_common ) + target_include_directories(costmap_unit_tests PRIVATE "include") + target_include_directories(fusion_policy_unit_tests PRIVATE "include") endif() diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 962bcdc6f154f..084c55d80d629 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -9,6 +9,7 @@ This package outputs the probability of having an obstacle as occupancy grid map - [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md) - [Laserscan based occupancy grid map](laserscan-based-occupancy-grid-map.md) +- [Grid map fusion](synchronized_grid_map_fusion.md) ## Settings @@ -70,3 +71,19 @@ Additional argument is shown below: | `container_name` | `occupancy_grid_map_container` | | | `input_obstacle_pointcloud` | `false` | only for laserscan based method. If true, the node subscribe obstacle pointcloud | | `input_obstacle_and_raw_pointcloud` | `true` | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud | + +### Test + +This package provides unit tests using `gtest`. +You can run the test by the following command. + +```bash +colcon test --packages-select probabilistic_occupancy_grid_map --event-handlers console_direct+ +``` + +Test contains the following. + +- Unit test for cost value conversion function +- Unit test for utility functions +- Unit test for occupancy grid map fusion functions +- Input/Output test for pointcloud based occupancy grid map diff --git a/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml b/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml new file mode 100644 index 0000000000000..f8a2dc2fbc7de --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + # 1. fusion parameters + fusion_input_ogm_topics: ["topic1", "topic2"] + input_ogm_reliabilities: [0.8, 0.2] + fusion_method: "overwrite" # choose from ["overwrite", "log-odds", "dempster-shafer"] + + # 2. synchronization settings + match_threshold_sec: 0.01 # 10ms + timeout_sec: 0.1 # 100ms + input_offset_sec: [0.0, 0.0] # no offset + + # 3. settings for fused fusion map + # remember resolution and map size should be same with input maps + map_frame_: "map" + base_link_frame_: "base_link" + grid_map_origin_frame_: "base_link" + fusion_map_length_x: 100.0 + fusion_map_length_y: 100.0 + fusion_map_resolution: 0.5 diff --git a/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg b/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg new file mode 100644 index 0000000000000..6bf5ed98e4edf --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg @@ -0,0 +1,290 @@ + + + + + + + + + + +
+
+
+ OGM1 +
+
+
+ + OGM1 + + + + + + + + +
+
+
+ OGM2 +
+
+
+ + OGM2 + + + + + + + + +
+
+
+ OGM3 +
+
+
+ + OGM3 + + + + + + + + +
+
+
+ Fused +
+ OGM +
+
+
+ + Fused... + + + + + + + + +
+
+
+ Fused +
+ OGM +
+ (Single Frame) +
+
+
+ + Fused... + + + + + + + + +
+
+
+ + Single Frame +
+ Fusion + +
+
+
+ + Single Frame... + + + + + + + + +
+
+
+ + Multi Frame +
+ Fusion + +
+
+
+ + Multi Frame... + + + + + + + + +
+
+
+ Fused +
+ OGM +
+
+
+ + Fused... + + + + + + +
+
+
`t=t_n`
+
+
+ + `t=t_n` + + + + + + +
+
+
`t=t_{n-1}`
+
+
+ + `t=t_{n-1}` + + + + + + +
+
+
`t=t_n`
+
+
+ + `t=t_n` + + + + + + +
+
+
+ Publish +
+
+
+ + Publish + + + + + + +
+
+
+ input topics +
+
+
+ + input topics + + + + + + +
+
+
+ output topics +
+
+
+ + output topics + + + + + + +
+
+
+ grid_map_fusion::GridMapFusionNode +
+
+
+ + grid_map_fusion::GridMapFusionNode + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp index e9667ccf65577..e0f18cedcdff1 100644 --- a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp @@ -59,19 +59,48 @@ static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 static const unsigned char LETHAL_OBSTACLE = 255; static const unsigned char FREE_SPACE = 0; +static const unsigned char OCCUPIED_THRESHOLD = 180; +static const unsigned char FREE_THRESHOLD = 50; + struct CostTranslationTable { CostTranslationTable() { for (int i = 0; i < 256; i++) { - const auto value = static_cast(static_cast(i) * 100.f / 255.f); - data[i] = std::max(std::min(value, static_cast(99)), static_cast(1)); + const auto value = + static_cast(static_cast(i) * 100.f / 255.f); // 0-255 to 0-100 + data[i] = + std::max(std::min(value, static_cast(99)), static_cast(1)); // 0-100 to 1-99 } } char operator[](unsigned char n) const { return data[n]; } char data[256]; }; +struct InverseCostTranslationTable +{ + InverseCostTranslationTable() + { + // 0-100 to 0-255 + for (int i = 0; i < 100; i++) { + data[i] = static_cast(i * 255 / 99); + } + } + unsigned char operator[](char n) const + { + if (n > 99) { + return data[99]; + } else if (n < 1) { + return data[1]; + } else { + const unsigned char u_n = static_cast(n); + return data[u_n]; + } + } + unsigned char data[100]; +}; + static const CostTranslationTable cost_translation_table; +static const InverseCostTranslationTable inverse_cost_translation_table; } // namespace occupancy_cost_value #endif // COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp new file mode 100644 index 0000000000000..13829b4910d96 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp @@ -0,0 +1,66 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#define FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ + +#include "cost_value.hpp" + +#include +#include +#include +#include + +/*min and max prob threshold to prevent log-odds to diverge*/ +#define EPSILON_PROB 0.03 + +namespace fusion_policy +{ +enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER }; + +unsigned char convertProbabilityToChar(const double & probability); +double convertCharToProbability(const unsigned char & occupancy); +std::vector convertProbabilityToChar(const std::vector & probabilities); +std::vector convertCharToProbability(const std::vector & occupancies); + +namespace overwrite_fusion +{ +enum State : unsigned char { UNKNOWN = 0U, FREE = 1U, OCCUPIED = 2U }; +State getApproximateState(const unsigned char & occupancy); +unsigned char overwriteFusion(const std::vector & occupancies); +} // namespace overwrite_fusion + +namespace log_odds_fusion +{ +double logOddsFusion(const std::vector & probabilities); +double logOddsFusion( + const std::vector & probabilities, const std::vector & weights); +} // namespace log_odds_fusion + +namespace dempster_shafer_fusion +{ +struct dempsterShaferOccupancy; +double dempsterShaferFusion(const std::vector & probability); +double dempsterShaferFusion( + const std::vector & probability, const std::vector & reliability); +} // namespace dempster_shafer_fusion + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method); +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method, + const std::vector & reliability); +} // namespace fusion_policy + +#endif // FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp new file mode 100644 index 0000000000000..6bea5b2a16f72 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp @@ -0,0 +1,127 @@ +// Copyright 2023 Tier IV, Inc. +// +// 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 FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ + +#include "fusion/single_frame_fusion_policy.hpp" +#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "updater/occupancy_grid_map_updater_interface.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace synchronized_grid_map_fusion +{ + +using costmap_2d::OccupancyGridMapFixedBlindSpot; +using costmap_2d::OccupancyGridMapLOBFUpdater; +using costmap_2d::OccupancyGridMapUpdaterInterface; +using geometry_msgs::msg::Pose; + +class GridMapFusionNode : public rclcpp::Node +{ +public: + explicit GridMapFusionNode(const rclcpp::NodeOptions & node_options); + +private: + void onGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg, + const std::string & input_topic); + nav_msgs::msg::OccupancyGrid::UniquePtr OccupancyGridMapToMsgPtr( + const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map); + + nav2_costmap_2d::Costmap2D OccupancyGridMsgToCostmap2D( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); + OccupancyGridMapFixedBlindSpot OccupancyGridMsgToGridMap( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); + OccupancyGridMapFixedBlindSpot SingleFrameOccupancyFusion( + std::vector & occupancy_grid_maps, + const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights); + + void updateGridMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg); + + void setPeriod(const int64_t new_period); + void timer_callback(); + void publish(); + +private: + // Publisher and Subscribers + rclcpp::Publisher::SharedPtr fused_map_pub_; + rclcpp::Publisher::SharedPtr single_frame_pub_; + std::vector::SharedPtr> grid_map_subs_; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; + + // Topics manager + std::size_t num_input_topics_{1}; + std::vector input_topics_; + std::vector input_offset_sec_; + std::vector input_topic_weights_; + std::map input_topic_weights_map_; + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // Timer to manage the timeout of the occupancy grid map + rclcpp::TimerBase::SharedPtr timer_; + double timeout_sec_{}; + double match_threshold_sec_{}; + + // cache for fusion + std::mutex mutex_; + std::shared_ptr + occupancy_grid_map_updater_ptr_; // contains fused grid map + std::map + gridmap_dict_; // temporary cache for grid map message + std::map + gridmap_dict_tmp_; // second cache for grid map message + std::map offset_map_; // time offset for each grid map + + // grid map parameters + std::string map_frame_; + std::string base_link_frame_; + std::string gridmap_origin_frame_; + float fusion_map_length_x_; + float fusion_map_length_y_; + float fusion_map_resolution_; + fusion_policy::FusionMethod fusion_method_; +}; + +} // namespace synchronized_grid_map_fusion + +#endif // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp new file mode 100644 index 0000000000000..306f8988acab7 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ + +#include "fusion/single_frame_fusion_policy.hpp" +#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "utils/utils.hpp" + +#include +#include + +// LOBF means: Log Odds Bayes Filter + +namespace costmap_2d +{ +class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface +{ +public: + enum Index : size_t { OCCUPIED = 0U, FREE = 1U }; + OccupancyGridMapLOBFUpdater( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) + : OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) + { + } + bool update(const Costmap2D & single_frame_occupancy_grid_map) override; + void initRosParam(rclcpp::Node & node) override; + +private: + inline unsigned char applyLOBF(const unsigned char & z, const unsigned char & o); + Eigen::Matrix2f probability_matrix_; +}; + +} // namespace costmap_2d + +#endif // UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp index 047b747c2861f..46bd14b843dae 100644 --- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp @@ -15,6 +15,8 @@ #ifndef UTILS__UTILS_HPP_ #define UTILS__UTILS_HPP_ +#include "cost_value.hpp" + #include #include #include @@ -114,6 +116,7 @@ bool extractCommonPointCloud( const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); +unsigned char getApproximateOccupancyState(const unsigned char & value); } // namespace utils #endif // UTILS__UTILS_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml b/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml new file mode 100644 index 0000000000000..c2391141e9fa0 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp new file mode 100644 index 0000000000000..75e8791a04bca --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp @@ -0,0 +1,322 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "fusion/single_frame_fusion_policy.hpp" + +namespace fusion_policy +{ + +//// utils //// + +// convert [0, 1] to [0, 255] +unsigned char convertProbabilityToChar(const double & probability) +{ + return static_cast(probability * 255); +} +// convert [0, 255] to [0, 1] +double convertCharToProbability(const unsigned char & occupancy) +{ + return static_cast(occupancy) / 255.0; +} + +// convert [0, 1] to [0, 255] vectors +std::vector convertProbabilityToChar(const std::vector & probabilities) +{ + std::vector occupancies; + for (auto & probability : probabilities) { + occupancies.push_back(convertProbabilityToChar(probability)); + } + return occupancies; +} +// convert [0, 255] to [0, 1] vectors +std::vector convertCharToProbability(const std::vector & occupancies) +{ + std::vector probabilities; + for (auto & occupancy : occupancies) { + probabilities.push_back(convertCharToProbability(occupancy)); + } + return probabilities; +} + +/// @brief fusion with overwrite policy +namespace overwrite_fusion +{ + +/** + * @brief convert char value to occupancy state + * + * @param occupancy [0, 255] + * @return State + */ +State getApproximateState(const unsigned char & occupancy) +{ + if (occupancy >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + return State::OCCUPIED; + } else if (occupancy <= occupancy_cost_value::FREE_THRESHOLD) { + return State::FREE; + } else { + return State::UNKNOWN; + } +} + +/** + * @brief override fusion + * + * @param occupancies : occupancies to be fused + * @return unsigned char + */ +unsigned char overwriteFusion(const std::vector & occupancies) +{ + if (occupancies.size() == 0) { + throw std::runtime_error("occupancies size is 0"); + } else if (occupancies.size() == 1) { + return occupancies[0]; + } + + auto fusion_occupancy = 128; // unknown + auto fusion_state = getApproximateState(fusion_occupancy); + for (auto & cell : occupancies) { + auto state = getApproximateState(cell); + if (state > fusion_state) { + fusion_state = state; + fusion_occupancy = cell; + } else if (state < fusion_state) { + continue; + } else { + fusion_occupancy = (fusion_occupancy / 2 + cell / 2); + } + } + return fusion_occupancy; +} +} // namespace overwrite_fusion + +/// @brief fusion with log-odds policy +namespace log_odds_fusion +{ +/** + * @brief log-odds fusion + * + * @param probabilities : probabilities of occupancy [0, 1] + * @return double + */ +double logOddsFusion(const std::vector & probabilities) +{ + double log_odds = 0.0; + for (auto & probability : probabilities) { + const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probability)); + log_odds += std::log(p / (1.0 - p)); + } + return 1.0 / (1.0 + std::exp(-log_odds)); +} + +/** + * @brief weighted log-odds fusion + * + * @param probabilities : probabilities of occupancy [0, 1] + * @param weights : weights of probabilities + * @return double + */ +double logOddsFusion(const std::vector & probabilities, const std::vector & weights) +{ + // check if the size of probabilities and weights are the same + if (probabilities.size() != weights.size()) { + // warning and return normal log-odds fusion + std::cout + << "The size of probabilities and weights are not the same. Return normal log-odds fusion." + << std::endl; + return logOddsFusion(probabilities); + } + + double log_odds = 0.0; + for (size_t i = 0; i < probabilities.size(); i++) { + const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probabilities[i])); + log_odds += weights[i] * std::log(p / (1.0 - p)); + } + return 1.0 / (1.0 + std::exp(-log_odds)); +} +} // namespace log_odds_fusion + +/// @brief fusion with Dempster-Shafer Theory +namespace dempster_shafer_fusion +{ +/** + * @brief conflict modified Dempster-Shafer Theory Data structure + * see https://www.diva-portal.org/smash/get/diva2:852457/FULLTEXT01.pdf + * + */ +struct dempsterShaferOccupancy +{ + double occupied; + double empty; + double unknown; + double conflict_threshold = 0.9; + + // initialize without args + dempsterShaferOccupancy() + { + occupied = 0.0; + empty = 0.0; + unknown = 1.0; + } + + // initialize with probability + dempsterShaferOccupancy(double occupied_probability) + { + // confine to [0, 1] + double p = std::max(0.0, std::min(1.0, occupied_probability)); + occupied = p; + empty = 1.0 - p; + unknown = 0.0; + } + + // initialize with probability and reliability + dempsterShaferOccupancy(double occupied_probability, double reliability) + { + // confine to [0, 1] + double p = std::max(0.0, std::min(1.0, occupied_probability)); + occupied = p * reliability; + empty = (1.0 - p) * reliability; + unknown = 1.0 - occupied - empty; + } + + // normalize + void normalize() + { + double sum = occupied + empty + unknown; + occupied /= sum; + empty /= sum; + unknown /= sum; + } + + // calc conflict factor K + double calcK(const dempsterShaferOccupancy & other) + { + return (occupied * other.empty + empty * other.occupied); + } + // calc sum of occupied probability mass + double calcOccupied(const dempsterShaferOccupancy & other) + { + return occupied * other.occupied + occupied * other.unknown + unknown * other.occupied; + } + // calc sum of empty probability mass + double calcEmpty(const dempsterShaferOccupancy & other) + { + return empty * other.empty + empty * other.unknown + unknown * other.empty; + } + + // Dempster-Shafer fusion + dempsterShaferOccupancy operator+(const dempsterShaferOccupancy & other) + { + dempsterShaferOccupancy result; + double K = calcK(other); + double O = calcOccupied(other); + double E = calcEmpty(other); + + if (K > conflict_threshold) { + // highly conflict + result.occupied = O; + result.empty = E; + result.unknown = 1 - O - E; + } else { + // low conflict + result.occupied = O / (1.0 - K); + result.empty = E / (1.0 - K); + result.unknown = 1 - result.occupied - result.empty; + } + return result; + } + + // get occupancy probability via Pignistic Probability + double getPignisticProbability() { return occupied + unknown / 2.0; } +}; + +/** + * @brief Dempster-Shafer fusion + * + * @param probability + * @return double + */ +double dempsterShaferFusion(const std::vector & probability) +{ + dempsterShaferOccupancy result; // init with unknown + for (auto & p : probability) { + result = result + dempsterShaferOccupancy(p); + } + return result.getPignisticProbability(); +} + +/** + * @brief Dempster-Shafer fusion with reliability + * + * @param probability + * @param reliability + * @return double + */ +double dempsterShaferFusion( + const std::vector & probability, const std::vector & reliability) +{ + // check if the size of probabilities and weights are the same + if (probability.size() != reliability.size()) { + // warning and return normal dempster-shafer fusion probability + std::cout << "The size of probabilities and reliability are not the same. Return normal " + "dempster-shafer fusion." + << std::endl; + return dempsterShaferFusion(probability); + } + + dempsterShaferOccupancy result; // init with unknown + for (size_t i = 0; i < probability.size(); i++) { + result = result + dempsterShaferOccupancy(probability[i], reliability[i]); + } + return result.getPignisticProbability(); +} +} // namespace dempster_shafer_fusion + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method) +{ + if (method == FusionMethod::OVERWRITE) { + return overwrite_fusion::overwriteFusion(occupancy); + } else if (method == FusionMethod::LOG_ODDS) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(log_odds_fusion::logOddsFusion(probability)); + } else if (method == FusionMethod::DEMPSTER_SHAFER) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(dempster_shafer_fusion::dempsterShaferFusion(probability)); + } else { + std::cout << "Unknown fusion method: " << std::endl; + return 128; + } +} + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method, + const std::vector & reliability) +{ + if (method == FusionMethod::OVERWRITE) { + return overwrite_fusion::overwriteFusion(occupancy); + } else if (method == FusionMethod::LOG_ODDS) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(log_odds_fusion::logOddsFusion(probability, reliability)); + } else if (method == FusionMethod::DEMPSTER_SHAFER) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar( + dempster_shafer_fusion::dempsterShaferFusion(probability, reliability)); + } else { + std::cout << "Unknown fusion method: " << std::endl; + return 128; + } +} + +} // namespace fusion_policy diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp new file mode 100644 index 0000000000000..6738286ae5592 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -0,0 +1,458 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "fusion/synchronized_grid_map_fusion_node.hpp" + +#include "cost_value.hpp" +#include "utils/utils.hpp" + +namespace synchronized_grid_map_fusion +{ +using costmap_2d::OccupancyGridMapFixedBlindSpot; +using costmap_2d::OccupancyGridMapLOBFUpdater; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::OccupancyGrid; + +GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) +: Node("synchronized_occupancy_grid_map_fusion", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + /* load input parameters */ + { + // get input topics + declare_parameter("fusion_input_ogm_topics", std::vector{}); + input_topics_ = get_parameter("fusion_input_ogm_topics").as_string_array(); + if (input_topics_.empty()) { + throw std::runtime_error("The number of input topics must larger than 0."); + } + num_input_topics_ = input_topics_.size(); + if (num_input_topics_ < 1) { + RCLCPP_WARN( + this->get_logger(), "minimum num_input_topics_ is 1. current num_input_topics_ is %zu", + num_input_topics_); + num_input_topics_ = 1; + } + if (num_input_topics_ > 12) { + RCLCPP_WARN( + this->get_logger(), "maximum num_input_topics_ is 12. current num_input_topics_ is %zu", + num_input_topics_); + num_input_topics_ = 12; + } + // get input topic reliabilities + declare_parameter("input_ogm_reliabilities", std::vector{}); + input_topic_weights_ = get_parameter("input_ogm_reliabilities").as_double_array(); + + if (input_topic_weights_.empty()) { // no topic weight is set + // set equal weight + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_topic_weights_map_[input_topics_.at(topic_i)] = 1.0 / num_input_topics_; + } + } else if (num_input_topics_ == input_topic_weights_.size()) { + // set weight to map + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_topic_weights_map_[input_topics_.at(topic_i)] = input_topic_weights_.at(topic_i); + } + } else { + throw std::runtime_error("The number of weights does not match the number of topics."); + } + } + + // Set fusion timing parameters + match_threshold_sec_ = declare_parameter("match_threshold_sec", 0.01); + timeout_sec_ = declare_parameter("timeout_sec", 0.1); + input_offset_sec_ = declare_parameter("input_offset_sec", std::vector{}); + if (!input_offset_sec_.empty() && num_input_topics_ != input_offset_sec_.size()) { + throw std::runtime_error("The number of offsets does not match the number of topics."); + } else if (input_offset_sec_.empty()) { + // if there are not input offset, set 0.0 + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_offset_sec_.push_back(0.0); + } + } + // Set fusion map parameters + map_frame_ = declare_parameter("map_frame_", "map"); + base_link_frame_ = declare_parameter("base_link_frame_", "base_link"); + gridmap_origin_frame_ = declare_parameter("grid_map_origin_frame_", "base_link"); + fusion_map_length_x_ = declare_parameter("fusion_map_length_x", 100.0); + fusion_map_length_y_ = declare_parameter("fusion_map_length_y", 100.0); + fusion_map_resolution_ = declare_parameter("fusion_map_resolution", 0.5); + + // set fusion method + std::string fusion_method_str = declare_parameter("fusion_method", "overwrite"); + if (fusion_method_str == "overwrite") { + fusion_method_ = fusion_policy::FusionMethod::OVERWRITE; + } else if (fusion_method_str == "log-odds") { + fusion_method_ = fusion_policy::FusionMethod::LOG_ODDS; + } else if (fusion_method_str == "dempster-shafer") { + fusion_method_ = fusion_policy::FusionMethod::DEMPSTER_SHAFER; + } else { + throw std::runtime_error("The fusion method is not supported."); + } + + // sub grid_map + grid_map_subs_.resize(num_input_topics_); + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + std::function fnc = std::bind( + &GridMapFusionNode::onGridMap, this, std::placeholders::_1, input_topics_.at(topic_i)); + grid_map_subs_.at(topic_i) = this->create_subscription( + input_topics_.at(topic_i), rclcpp::QoS{1}.best_effort(), fnc); + } + + // pub grid_map + fused_map_pub_ = this->create_publisher( + "~/output/occupancy_grid_map", rclcpp::QoS{1}.reliable()); + single_frame_pub_ = this->create_publisher( + "~/debug/single_frame_map", rclcpp::QoS{1}.reliable()); + + // updater + occupancy_grid_map_updater_ptr_ = std::make_shared( + fusion_map_length_x_ / fusion_map_resolution_, fusion_map_length_y_ / fusion_map_resolution_, + fusion_map_resolution_); + + // Set timer + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&GridMapFusionNode::timer_callback, this)); + + // set offset map + for (size_t i = 0; i < input_offset_sec_.size(); ++i) { + offset_map_[input_topics_[i]] = input_offset_sec_[i]; + } + + // init dict + for (size_t i = 0; i < input_topics_.size(); ++i) { + gridmap_dict_[input_topics_[i]] = nullptr; + gridmap_dict_tmp_[input_topics_[i]] = nullptr; + } + + // debug tools + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = std::make_unique(this, "synchronized_grid_map_fusion"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } +} + +/** + * @brief Set the period of the timer + * + * @param new_period + */ +void GridMapFusionNode::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + +/** + * @brief Callback function for the grid map message add data to buffer + * + * @param occupancy_grid_msg + * @param topic_name + */ +void GridMapFusionNode::onGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg, + const std::string & topic_name) +{ + std::lock_guard lock(mutex_); + const bool is_already_subscribed_this = (gridmap_dict_[topic_name] != nullptr); + const bool is_already_subscribed_tmp = std::any_of( + std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), + [](const auto & e) { return e.second != nullptr; }); + + // already subscribed + if (is_already_subscribed_this) { + gridmap_dict_tmp_[topic_name] = occupancy_grid_msg; + + if (!is_already_subscribed_tmp) { + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + // first time to subscribe in this period + } else { + gridmap_dict_[topic_name] = occupancy_grid_msg; // add to buffer + + // check if all topics are subscribed + bool is_subscribed_all = std::all_of( + std::begin(gridmap_dict_), std::end(gridmap_dict_), + [](const auto & e) { return e.second != nullptr; }); + // Then, go to publish + if (is_subscribed_all) { + for (const auto & e : gridmap_dict_tmp_) { + if (e.second != nullptr) { + gridmap_dict_[e.first] = e.second; + } + } + std::for_each(std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), [](auto & e) { + e.second = nullptr; + }); + + timer_->cancel(); + publish(); + // if not all topics are subscribed, set timer + } else if (offset_map_.size() > 0) { + timer_->cancel(); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } +} + +/** + * @brief Timer callback function + * + */ +void GridMapFusionNode::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + publish(); + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +/** + * @brief Publisher function + * + */ +void GridMapFusionNode::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + builtin_interfaces::msg::Time latest_stamp = get_clock()->now(); + double height = 0.0; + + // merge available gridmap + std::vector subscribed_maps; + std::vector weights; + for (const auto & e : gridmap_dict_) { + if (e.second != nullptr) { + subscribed_maps.push_back(GridMapFusionNode::OccupancyGridMsgToGridMap(*e.second)); + weights.push_back(input_topic_weights_map_[e.first]); + latest_stamp = e.second->header.stamp; + height = e.second->info.origin.position.z; + } + } + + // return if empty + if (subscribed_maps.empty()) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "No grid map is subscribed. Please check the topic name and the topic type."); + return; + } + + // fusion map + // single frame gridmap fusion + auto fused_map = SingleFrameOccupancyFusion(subscribed_maps, latest_stamp, weights); + // multi frame gridmap fusion + occupancy_grid_map_updater_ptr_->update(fused_map); + + // publish + fused_map_pub_->publish( + OccupancyGridMapToMsgPtr(map_frame_, latest_stamp, height, *occupancy_grid_map_updater_ptr_)); + single_frame_pub_->publish(OccupancyGridMapToMsgPtr(map_frame_, latest_stamp, height, fused_map)); + + // copy 2nd temp to temp buffer + gridmap_dict_ = gridmap_dict_tmp_; + std::for_each(std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), [](auto & e) { + e.second = nullptr; + }); + + // add processing time for debug + if (debug_publisher_ptr_ && stop_watch_ptr_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} + +OccupancyGridMapFixedBlindSpot GridMapFusionNode::SingleFrameOccupancyFusion( + std::vector & occupancy_grid_maps, + const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights) +{ + // if only single map + if (occupancy_grid_maps.size() == 1) { + return occupancy_grid_maps[0]; + } + + // get map to gridmap_origin_frame_ transform + Pose gridmap_origin{}; + try { + gridmap_origin = utils::getPose(latest_stamp, tf_buffer_, gridmap_origin_frame_, map_frame_); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(get_logger(), ex.what()); + return occupancy_grid_maps[0]; + } + + // init fused map with calculated origin + OccupancyGridMapFixedBlindSpot fused_map( + occupancy_grid_maps[0].getSizeInCellsX(), occupancy_grid_maps[0].getSizeInCellsY(), + occupancy_grid_maps[0].getResolution()); + fused_map.updateOrigin( + gridmap_origin.position.x - fused_map.getSizeInMetersX() / 2, + gridmap_origin.position.y - fused_map.getSizeInMetersY() / 2); + + // fix origin of each map + for (auto & map : occupancy_grid_maps) { + map.updateOrigin(fused_map.getOriginX(), fused_map.getOriginY()); + } + + // assume map is same size and resolutions + for (unsigned int x = 0; x < fused_map.getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < fused_map.getSizeInCellsY(); y++) { + // get cost of each map + std::vector costs; + for (auto & map : occupancy_grid_maps) { + costs.push_back(map.getCost(x, y)); + } + + // set fusion policy + auto fused_cost = fusion_policy::singleFrameOccupancyFusion(costs, fusion_method_, weights); + + // set max cost to fused map + fused_map.setCost(x, y, fused_cost); + } + } + + return fused_map; +} + +/** + * @brief Update occupancy grid map with asynchronous input (currently unused) + * + * @param occupancy_grid_msg + */ +void GridMapFusionNode::updateGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg) +{ + // get updater map origin + + // origin is set to current updater map + auto map_for_update = OccupancyGridMsgToCostmap2D(*occupancy_grid_msg); + + // update + occupancy_grid_map_updater_ptr_->update(map_for_update); +} + +nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) +{ + nav2_costmap_2d::Costmap2D costmap2d( + occupancy_grid_map.info.width, occupancy_grid_map.info.height, + occupancy_grid_map.info.resolution, occupancy_grid_map.info.origin.position.x, + occupancy_grid_map.info.origin.position.y, 0); + + for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { + for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { + const unsigned int index = i + j * occupancy_grid_map.info.width; + costmap2d.setCost( + i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + } + } + + return costmap2d; +} + +OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) +{ + OccupancyGridMapFixedBlindSpot gridmap( + occupancy_grid_map.info.width, occupancy_grid_map.info.height, + occupancy_grid_map.info.resolution); + gridmap.updateOrigin( + occupancy_grid_map.info.origin.position.x, occupancy_grid_map.info.origin.position.y); + + for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { + for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { + const unsigned int index = i + j * occupancy_grid_map.info.width; + gridmap.setCost( + i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + } + } + return gridmap; +} + +nav_msgs::msg::OccupancyGrid::UniquePtr GridMapFusionNode::OccupancyGridMapToMsgPtr( + const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map) +{ + auto msg_ptr = std::make_unique(); + + msg_ptr->header.frame_id = frame_id; + msg_ptr->header.stamp = stamp; + msg_ptr->info.resolution = occupancy_grid_map.getResolution(); + + msg_ptr->info.width = occupancy_grid_map.getSizeInCellsX(); + msg_ptr->info.height = occupancy_grid_map.getSizeInCellsY(); + + double wx{}; + double wy{}; + occupancy_grid_map.mapToWorld(0, 0, wx, wy); + msg_ptr->info.origin.position.x = occupancy_grid_map.getOriginX(); + msg_ptr->info.origin.position.y = occupancy_grid_map.getOriginY(); + msg_ptr->info.origin.position.z = robot_pose_z; + msg_ptr->info.origin.orientation.w = 1.0; + + msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); + + unsigned char * data = occupancy_grid_map.getCharMap(); + for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { + msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + } + return msg_ptr; +} + +} // namespace synchronized_grid_map_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(synchronized_grid_map_fusion::GridMapFusionNode) diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp new file mode 100644 index 0000000000000..e45f3d0686fbc --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp @@ -0,0 +1,69 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" + +#include "cost_value.hpp" + +#include + +namespace costmap_2d +{ + +void OccupancyGridMapLOBFUpdater::initRosParam(rclcpp::Node & /*node*/) +{ + // nothing to load +} + +inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( + const unsigned char & z, const unsigned char & o) +{ + using fusion_policy::convertCharToProbability; + using fusion_policy::convertProbabilityToChar; + using fusion_policy::log_odds_fusion::logOddsFusion; + + constexpr unsigned char unknown = occupancy_cost_value::NO_INFORMATION; + constexpr unsigned char unknown_margin = 1; + /* Tau and ST decides how fast the observation decay to the unknown status*/ + constexpr double tau = 0.75; + constexpr double sample_time = 0.1; + + // if the observation is unknown, decay the estimation + if (z >= unknown - unknown_margin && z <= unknown + unknown_margin) { + char diff = static_cast(o) - static_cast(unknown); + const double decay = std::exp(-sample_time / tau); + const double fused = static_cast(unknown) + static_cast(diff) * decay; + return static_cast(fused); + } else { + // else, do the log-odds fusion + const std::vector probability = { + convertCharToProbability(z), convertCharToProbability(o)}; + const unsigned char fused = convertProbabilityToChar(logOddsFusion(probability)); + return fused; + } +} + +bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupancy_grid_map) +{ + updateOrigin( + single_frame_occupancy_grid_map.getOriginX(), single_frame_occupancy_grid_map.getOriginY()); + for (unsigned int x = 0; x < getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < getSizeInCellsY(); y++) { + unsigned int index = getIndex(x, y); + costmap_[index] = applyLOBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]); + } + } + return true; +} +} // namespace costmap_2d diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 0060754cd875c..b876004d89b0a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -182,4 +182,20 @@ bool extractCommonPointCloud( return true; } +/** + * @brief Convert unsigned char value to closest cost value + * @param cost Cost value + * @return Probability + */ +unsigned char getApproximateOccupancyState(const unsigned char & value) +{ + if (value >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + return occupancy_cost_value::LETHAL_OBSTACLE; + } else if (value <= occupancy_cost_value::FREE_THRESHOLD) { + return occupancy_cost_value::FREE_SPACE; + } else { + return occupancy_cost_value::NO_INFORMATION; + } +} + } // namespace utils diff --git a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md new file mode 100644 index 0000000000000..e1046fa24719d --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md @@ -0,0 +1,168 @@ +# synchronized OGM fusion + +> For simplicity, we use OGM as the meaning of the occupancy grid map. + +This package is used to fuse the OGMs from synchronized sensors. Especially for the lidar. + +Here shows the example OGM for the this synchronized OGM fusion. + +| left lidar OGM | right lidar OGM | top lidar OGM | +| --------------------------------- | ----------------------------------- | ------------------------------- | +| ![left](image/left_lidar_ogm.png) | ![right](image/right_lidar_ogm.png) | ![top](image/top_lidar_ogm.png) | + +OGM fusion with asynchronous sensor outputs is not suitable for this package. Asynchronous OGM fusion is under construction. + +## Processing flow + +The processing flow of this package is shown in the following figure. + +![data_flow](image/synchronized_grid_map_fusion.drawio.svg) + +- Single Frame Fusion + - Single frame fusion means that the OGMs from synchronized sensors are fused in a certain time frame $t=t_n$. +- Multi Frame Fusion + - In the multi frame fusion process, current fused single frame OGM in $t_n$ is fused with the previous fused single frame OGM in $t_{n-1}$. + +## I/O + +| Input topic name | Type | Description | +| ------------------ | ------------------------------------ | --------------------------------------------------------------------------------- | +| `input_ogm_topics` | list of nav_msgs::msg::OccupancyGrid | List of input topics for Occupancy Grid Maps. This parameter is given in list, so | + +| Output topic name | Type | Description | +| ----------------------------- | ---------------------------- | ----------------------------------------------------------------------------- | +| `~/output/occupancy_grid_map` | nav_msgs::msg::OccupancyGrid | Output topic name of the fused Occupancy Grid Map. | +| `~/debug/single_frame_map` | nav_msgs::msg::OccupancyGrid | (debug topic) Output topic name of the single frame fused Occupancy Grid Map. | + +## Parameters + +Synchronized OGM fusion node parameters are shown in the following table. Main parameters to be considered in the fusion node is shown as bold. + +| Ros param name | Sample value | Description | +| --------------------------- | -------------------- | ------------------------------------------------------------- | +| **input_ogm_topics** | ["topic1", "topic2"] | List of input topics for Occupancy Grid Maps | +| **input_ogm_reliabilities** | [0.8, 0.2] | Weights for the reliability of each input topic | +| **fusion_method** | "overwrite" | Method of fusion ("overwrite", "log-odds", "dempster-shafer") | +| match_threshold_sec | 0.01 | Matching threshold in milliseconds | +| timeout_sec | 0.1 | Timeout duration in seconds | +| input_offset_sec | [0.0, 0.0] | Offset time in seconds for each input topic | +| map*frame* | "map" | Frame name for the fused map | +| base*link_frame* | "base_link" | Frame name for the base link | +| grid*map_origin_frame* | "base_link" | Frame name for the origin of the grid map | +| fusion_map_length_x | 100.0 | Length of the fused map along the X-axis | +| fusion_map_length_y | 100.0 | Length of the fused map along the Y-axis | +| fusion_map_resolution | 0.5 | Resolution of the fused map | + +Since this node assumes that the OGMs from synchronized sensors are generated in the same time, we need to tune the `match_threshold_sec`, `timeout_sec` and `input_offset_sec` parameters to successfully fuse the OGMs. + +## Fusion methods + +For the single frame fusion, the following fusion methods are supported. + +| Fusion Method in parameter | Description | +| -------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `overwrite` | The value of the cell in the fused OGM is overwritten by the value of the cell in the OGM with the highest priority.
We set priority as `Occupied` > `Free` > `Unknown`. | +| `log-odds` | The value of the cell in the fused OGM is calculated by the log-odds ratio method, which is known as a Bayesian fusion method.
The log-odds of a probability $p$ can be written as $l_p = \log(\frac{p}{1-p})$.
And the fused log-odds is calculated by the sum of log-odds. $l_f = \Sigma l_p$ | +| `dempster-shafer` | The value of the cell in the fused OGM is calculated by the Dempster-Shafer theory[1]. This is also popular method to handle multiple evidences. This package applied conflict escape logic in [2] for the performance. See references for the algorithm details. | + +For the multi frame fusion, currently only supporting `log-odds` fusion method. + +## How to use + +### launch fusion node + +The minimum node launch will be like the following. + +```xml + + + + + + + + + + +``` + +### (Optional) Generate OGMs in each sensor frame + +You need to generate OGMs in each sensor frame before achieving grid map fusion. + +`probabilistic_occupancy_grid_map` package supports to generate OGMs for the each from the point cloud data. + +
+ Example launch.xml (click to expand) + +```xml + + + + + + + + + + + + + + + +The minimum parameter for the OGM generation in each frame is shown in the following table. + +|Parameter|Description| +|--|--| +|`input/obstacle_pointcloud`| The input point cloud data for the OGM generation. This point cloud data should be the point cloud data which is segmented as the obstacle.| +|`input/raw_pointcloud`| The input point cloud data for the OGM generation. This point cloud data should be the point cloud data which is not segmented as the obstacle. | +|`output`| The output topic of the OGM. | +|`map_frame`| The tf frame for the OGM center origin. | +|`scan_origin`| The tf frame for the sensor origin. | +|`method`| The method for the OGM generation. Currently we support `pointcloud_based_occupancy_grid_map` and `laser_scan_based_occupancy_grid_map`. The pointcloud based method is recommended. | +|`param_file`| The parameter file for the OGM generation. See [example parameter file](config/pointcloud_based_occupancy_grid_map_for_fusion.param.yaml) | + +``` + +
+ +
+ +We recommend to use same `map_frame`, size and resolutions for the OGMs from synchronized sensors. +Also, remember to set `enable_single_frame_mode` and `filter_obstacle_pointcloud_by_raw_pointcloud` to `true` in the `probabilistic_occupancy_grid_map` package (you do not need to set these parameters if you use the above example config file). + +
+ +### Run both OGM generation node and fusion node + +We prepared the launch file to run both OGM generation node and fusion node in [`grid_map_fusion_with_synchronized_pointclouds.launch.py`](launch/grid_map_fusion_with_synchronized_pointclouds.launch.py) + +You can include this launch file like the following. + +```xml + + + + + + + + + + +``` + +The minimum parameter for the launch file is shown in the following table. + +| Parameter | Description | +| -------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `output` | The output topic of the finally fused OGM. | +| `method` | The method for the OGM generation. Currently we support `pointcloud_based_occupancy_grid_map` and `laser_scan_based_occupancy_grid_map`. The pointcloud based method is recommended. | +| `fusion_config_file` | The parameter file for the grid map fusion. See [example parameter file](config/grid_map_fusion.param.yaml) | +| `ogm_config_file` | The parameter file for the OGM generation. See [example parameter file](config/pointcloud_based_occupancy_grid_map_for_fusion.param.yaml) | + +## References + +- [1] Dempster, A. P., Laird, N. M., & Rubin, D. B. (1977). Maximum likelihood from incomplete data via the EM algorithm. Journal of the Royal Statistical Society. Series B (Methodological), 39(1), 1-38. +- [2] diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp new file mode 100644 index 0000000000000..baf94fabfd2a1 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -0,0 +1,47 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "cost_value.hpp" + +#include "gtest/gtest.h" + +// Test the CostTranslationTable and InverseCostTranslationTable functions +using occupancy_cost_value::cost_translation_table; +using occupancy_cost_value::inverse_cost_translation_table; + +TEST(CostTranslationTableTest, TestRange) +{ + // Test the value range of CostTranslationTable + for (int i = 0; i < 256; i++) { + EXPECT_GE(cost_translation_table[i], 1); + EXPECT_LE(cost_translation_table[i], 99); + } + + // Test the value range of InverseCostTranslationTable + for (int i = 1; i < 100; i++) { + EXPECT_GE(inverse_cost_translation_table[i], 0); + EXPECT_LE(inverse_cost_translation_table[i], 255); + } +} + +TEST(CostTranslationTableTest, TestConversion) +{ + // Test the forward and inverse conversion of 0, 128, and 255 + EXPECT_EQ(cost_translation_table[0], 1); + EXPECT_EQ(cost_translation_table[128], 50); + EXPECT_EQ(cost_translation_table[255], 99); + EXPECT_EQ(inverse_cost_translation_table[1], 2); + EXPECT_EQ(inverse_cost_translation_table[50], 128); + EXPECT_EQ(inverse_cost_translation_table[99], 255); +} diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp new file mode 100644 index 0000000000000..2501a361fba58 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "cost_value.hpp" +#include "fusion/single_frame_fusion_policy.hpp" +#include "gtest/gtest.h" + +// Test the log-odds update rule +TEST(FusionPolicyTest, TestLogOddsUpdateRule) +{ + using fusion_policy::log_odds_fusion::logOddsFusion; + const double MARGIN = 0.03; + const double OCCUPIED = 1.0 - MARGIN; + const double FREE = 0.0 + MARGIN; + const double UNKNOWN = 0.5; + const double EPSILON = 1e-3; + + // same case + std::vector case1_1 = {OCCUPIED, OCCUPIED}; + std::vector case1_2 = {FREE, FREE}; + std::vector case1_3 = {UNKNOWN, UNKNOWN}; + EXPECT_GE(logOddsFusion(case1_1), OCCUPIED); + EXPECT_LE(logOddsFusion(case1_2), FREE); + EXPECT_NEAR(logOddsFusion(case1_3), UNKNOWN, EPSILON); + + // with unknown + std::vector case2_1 = {OCCUPIED, UNKNOWN}; + std::vector case2_2 = {FREE, UNKNOWN}; + EXPECT_NEAR(logOddsFusion(case2_1), OCCUPIED, EPSILON); + EXPECT_NEAR(logOddsFusion(case2_2), FREE, EPSILON); + + // with conflict + std::vector case3_1 = {OCCUPIED, FREE}; + EXPECT_NEAR(logOddsFusion(case3_1), UNKNOWN, EPSILON); +} + +// Test the dempster-shafer update rule +TEST(FusionPolicyTest, TestDempsterShaferUpdateRule) +{ + using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; + const double MARGIN = 0.03; + const double OCCUPIED = 1.0 - MARGIN; + const double FREE = 0.0 + MARGIN; + const double UNKNOWN = 0.5; + const double EPSILON = 1e-3; + + // same case + std::vector case1_1 = {OCCUPIED, OCCUPIED}; + std::vector case1_2 = {FREE, FREE}; + std::vector case1_3 = {UNKNOWN, UNKNOWN}; + EXPECT_GE(dempsterShaferFusion(case1_1), OCCUPIED); + EXPECT_LE(dempsterShaferFusion(case1_2), FREE); + EXPECT_NEAR(dempsterShaferFusion(case1_3), UNKNOWN, EPSILON); + + // with unknown + std::vector case2_1 = {OCCUPIED, UNKNOWN}; + std::vector case2_2 = {FREE, UNKNOWN}; + EXPECT_NEAR(dempsterShaferFusion(case2_1), OCCUPIED, EPSILON); + EXPECT_NEAR(dempsterShaferFusion(case2_2), FREE, EPSILON); + + // with conflict + std::vector case3_1 = {OCCUPIED, FREE}; + EXPECT_NEAR(dempsterShaferFusion(case3_1), UNKNOWN, EPSILON); +} diff --git a/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py new file mode 100644 index 0000000000000..c15daaf886b6f --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py @@ -0,0 +1,255 @@ +# Copyright 2021 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time +import unittest + +from geometry_msgs.msg import TransformStamped +import launch +import launch.actions +from launch_ros.substitutions import FindPackageShare +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.markers +import launch_testing.tools +from nav_msgs.msg import OccupancyGrid +import numpy as np +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +PACKAGE_NAME = "probabilistic_occupancy_grid_map" +INPUT_TOPICS = ["/topic1", "/topic2"] +DEBUG_OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/debug/single_frame_map" +OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/output/occupancy_grid_map" + +FREE_VALUE = 1 +UNKNOWN_VALUE = 50 +OCCUPIED_VALUE = 99 + + +# test launcher to launch grid map fusion node +@pytest.mark.launch_test +def generate_test_description(): + """Launch file test description. + + Returns: + _type_: launch.LaunchDescription + """ + # get launch file path + launch_file_path = ( + FindPackageShare(PACKAGE_NAME).find(PACKAGE_NAME) + + "/launch/synchronized_occupancy_grid_map_fusion.launch.xml" + ) + # use default launch arguments and parms + launch_args = [] + # action to include launch file + test_launch_file = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.AnyLaunchDescriptionSource(launch_file_path), + launch_arguments=launch_args, + ) + + return launch.LaunchDescription( + [ + test_launch_file, + launch_testing.actions.ReadyToTest(), + ] + ) + + +# util functions +def create_nav_msgs_occupancy_grid_msg(fill_value: int = 0, stamp: rclpy.time.Time = None): + """Create nav_msgs occupancy grid message. + + Args: + fill_value (int, optional): fill value. Defaults to 0. + + Returns: + OccupancyGrid: nav_msgs occupancy grid message + """ + msg = OccupancyGrid() + msg.header.stamp = rclpy.clock.Clock().now().to_msg() if stamp is None else stamp.to_msg() + msg.header.frame_id = "map" + msg.info.resolution = 0.5 # 0.5m x 0.5m + msg.info.width = 200 # 100m x 100m + msg.info.height = 200 + msg.info.origin.position.x = -msg.info.width * msg.info.resolution / 2.0 + msg.info.origin.position.y = -msg.info.height * msg.info.resolution / 2.0 + msg.info.origin.position.z = 0.0 + msg.info.origin.orientation.x = 0.0 + msg.info.origin.orientation.y = 0.0 + msg.info.origin.orientation.z = 0.0 + msg.info.origin.orientation.w = 1.0 + msg.data = [fill_value] * msg.info.width * msg.info.height + return msg + + +def parse_ogm_msg(msg: OccupancyGrid): + """Parse nav_msgs occupancy grid message. + + Args: + msg (OccupancyGrid): nav_msgs occupancy grid message + + Returns: + np.ndarray: occupancy grid map + """ + ogm = np.array(msg.data).reshape((msg.info.height, msg.info.width)) + return ogm + + +# dummy tf broadcaster +def generate_static_transform_msg(stamp: rclpy.time.Time = None): + """Generate static transform message from base_link to map. + + Returns: + TransformStamped: static transform message + """ + msg = TransformStamped() + if stamp is None: + msg.header.stamp = rclpy.clock.Clock().now().to_msg() + else: + msg.header.stamp = stamp.to_msg() + msg.header.frame_id = "map" + msg.child_frame_id = "base_link" + msg.transform.translation.x = 0.0 + msg.transform.translation.y = 0.0 + msg.transform.translation.z = 0.0 + msg.transform.rotation.x = 0.0 + msg.transform.rotation.y = 0.0 + msg.transform.rotation.z = 0.0 + msg.transform.rotation.w = 1.0 + return msg + + +# --- TestSynchronizedOGMFusion --- +# 1. test free ogm and free ogm input fusion +# 2. test occupied ogm and occupied ogm input fusion +# 3. test unknown ogm and free ogm input fusion +# 4. test unknown ogm and occupied ogm input fusion +# 5. test free ogm and occupied ogm input fusion +class TestSynchronizedOGMFusion(unittest.TestCase): + @classmethod + def setUpClass(cls): + # init ROS at once + rclpy.init() + + @classmethod + def tearDownClass(cls): + # shutdown ROS at once + rclpy.shutdown() + + def setUp(self): + # called when each test started + self.node = rclpy.create_node("grid_map_fusion_node_test_node") + # send static transform from map to base_link + tf_msg = generate_static_transform_msg() + self.tf_broadcaster = StaticTransformBroadcaster(self.node) + self.tf_broadcaster.sendTransform(tf_msg) + + def tearDown(self): + # called when each test finished + self.node.destroy_node() + + def callback(self, msg: OccupancyGrid): + self.msg_buffer.append(msg) + print("callback", len(self.msg_buffer)) + + def get_newest_ogm_value(self): + return np.mean(self.msg_buffer[-1].data) + + # util functions test 1~3 + def create_pub_sub(self): + # create publisher + pub1 = self.node.create_publisher(OccupancyGrid, INPUT_TOPICS[0], 10) + pub2 = self.node.create_publisher(OccupancyGrid, INPUT_TOPICS[1], 10) + + # create subscriber for debug output + sensor_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.msg_buffer = [] + # subscribe to occupancy grid with buffer + sub = self.node.create_subscription( + OccupancyGrid, DEBUG_OUTPUT_TOPIC, self.callback, qos_profile=sensor_qos + ) + return [pub1, pub2], sub + + # test functions + def test_same_ogm_fusion(self): + """Test 1~3. + + Expected output: same ogm. + """ + # wait for the node to be ready + time.sleep(3) + pubs, sub = self.create_pub_sub() # pubs have two publishers + + fill_values = [FREE_VALUE, OCCUPIED_VALUE, UNKNOWN_VALUE] + + for fill_value in fill_values: + # create free/occupied/unknown ogm + ogm = create_nav_msgs_occupancy_grid_msg(fill_value=fill_value) + # publish free/occupied/unknown ogm + pubs[0].publish(ogm) + pubs[1].publish(ogm) + + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=2.0) + fused_mean = self.get_newest_ogm_value() + print("same ogm test: ", fill_value, fused_mean) + # assert almost equal + self.assertAlmostEqual(fused_mean, fill_value, delta=3.0) + + def test_unknown_fusion(self): + """Test unknown ogm and free ogm input fusion. + + Expected output: free, occupied, unknown. + """ + # wait for the node to be ready + time.sleep(3) + pubs, sub = self.create_pub_sub() + + fill_values = [FREE_VALUE, OCCUPIED_VALUE, UNKNOWN_VALUE] + now = rclpy.clock.Clock().now() + unknown_ogm = create_nav_msgs_occupancy_grid_msg(fill_value=UNKNOWN_VALUE, stamp=now) + + for fill_value in fill_values: + # publish unknown ogm + pubs[0].publish(unknown_ogm) + # create free/occupied/unknown ogm + ogm = create_nav_msgs_occupancy_grid_msg(fill_value=fill_value, stamp=now) + # publish ogm + pubs[1].publish(ogm) + + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=2.0) + fused_mean = self.get_newest_ogm_value() + print("unknown ogm test: ", fill_value, fused_mean) + # assert almost equal + self.assertAlmostEqual(fused_mean, fill_value, delta=3.0) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # check exit code + launch_testing.asserts.assertExitCodes(proc_info) From 412b0e77887f5f52277c96fdd52e6bd0b669e85e Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Tue, 9 Jan 2024 11:31:08 +0900 Subject: [PATCH 005/154] feat(traffic_light): output undetected traffic light as unknown (#5934) * fix: output undetected traffic light as unknown Signed-off-by: tzhong518 * Update perception/traffic_light_classifier/src/nodelet.cpp Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> * Update perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> * style(pre-commit): autofix * fix: unnecessary count Signed-off-by: tzhong518 * style(pre-commit): autofix * fix: improve readability Signed-off-by: tzhong518 * fix: index error Signed-off-by: tzhong518 * fix: append the undetected rois at the end so that the order can be easily kept Signed-off-by: tzhong518 * fix: false occlusion ratio for undetected signals Signed-off-by: tzhong518 --------- Signed-off-by: tzhong518 Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../traffic_light_classifier/src/nodelet.cpp | 30 +++++++++++++++---- .../src/nodelet.cpp | 28 ++++++++++++----- .../src/nodelet.cpp | 14 ++++++++- .../src/occlusion_predictor.cpp | 6 +++- 4 files changed, 63 insertions(+), 15 deletions(-) diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index 3a00bb013d867..1ebc14c4e6f93 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "traffic_light_classifier/nodelet.hpp" +#include + #include #include #include @@ -98,16 +100,18 @@ void TrafficLightClassifierNodelet::imageRoiCallback( output_msg.signals.resize(input_rois_msg->rois.size()); std::vector images; - size_t num_valid_roi = 0; std::vector backlight_indices; for (size_t i = 0; i < input_rois_msg->rois.size(); i++) { - // skip if not the expected type of roi + // skip if the roi is not detected + if (input_rois_msg->rois.at(i).roi.height == 0) { + break; + } if (input_rois_msg->rois.at(i).traffic_light_type != classify_traffic_light_type_) { continue; } - output_msg.signals[num_valid_roi].traffic_light_id = + output_msg.signals[images.size()].traffic_light_id = input_rois_msg->rois.at(i).traffic_light_id; - output_msg.signals[num_valid_roi].traffic_light_type = + output_msg.signals[images.size()].traffic_light_type = input_rois_msg->rois.at(i).traffic_light_type; const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi; @@ -116,15 +120,29 @@ void TrafficLightClassifierNodelet::imageRoiCallback( backlight_indices.emplace_back(i); } images.emplace_back(roi_img); - num_valid_roi++; } - output_msg.signals.resize(num_valid_roi); + output_msg.signals.resize(images.size()); if (!classifier_ptr_->getTrafficSignals(images, output_msg)) { RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback"); return; } + // append the undetected rois as unknown + for (const auto & input_roi : input_rois_msg->rois) { + if (input_roi.roi.height == 0 && input_roi.traffic_light_type == classify_traffic_light_type_) { + tier4_perception_msgs::msg::TrafficSignal tlr_sig; + tlr_sig.traffic_light_id = input_roi.traffic_light_id; + tlr_sig.traffic_light_type = input_roi.traffic_light_type; + tier4_perception_msgs::msg::TrafficLightElement element; + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::CIRCLE; + element.confidence = 0.0; + tlr_sig.elements.push_back(element); + output_msg.signals.push_back(tlr_sig); + } + } + for (const auto & idx : backlight_indices) { auto & elements = output_msg.signals.at(idx).elements; for (auto & element : elements) { diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index 257e9fc7ea99d..8037dc5472fbe 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -272,14 +272,28 @@ void TrafficLightFineDetectorNodelet::detectionMatch( } out_rois.rois.clear(); - for (const auto & p : bestDetections) { + std::vector invalid_roi_id; + for (const auto & [tlr_id, roi] : id2expectRoi) { + // if matches, update the roi info + if (!bestDetections.count(tlr_id)) { + invalid_roi_id.emplace_back(tlr_id); + continue; + } + TrafficLightRoi tlr; + tlr.traffic_light_id = tlr_id; + const auto & object = bestDetections.at(tlr_id); + tlr.traffic_light_type = roi.traffic_light_type; + tlr.roi.x_offset = object.x_offset; + tlr.roi.y_offset = object.y_offset; + tlr.roi.width = object.width; + tlr.roi.height = object.height; + out_rois.rois.push_back(tlr); + } + // append undetected rois at the end + for (const auto & id : invalid_roi_id) { TrafficLightRoi tlr; - tlr.traffic_light_id = p.first; - tlr.traffic_light_type = id2expectRoi[p.first].traffic_light_type; - tlr.roi.x_offset = p.second.x_offset; - tlr.roi.y_offset = p.second.y_offset; - tlr.roi.width = p.second.width; - tlr.roi.height = p.second.height; + tlr.traffic_light_id = id; + tlr.traffic_light_type = id2expectRoi[id].traffic_light_type; out_rois.rois.push_back(tlr); } } diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 97fa98ab18a72..6429640ff44c1 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -129,12 +129,18 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( const uint8_t traffic_light_type) { std::vector occlusion_ratios; + size_t not_detected_roi = 0; if (in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr) { occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { tier4_perception_msgs::msg::TrafficLightRoiArray selected_roi_msg; selected_roi_msg.rois.reserve(in_roi_msg->rois.size()); for (size_t i = 0; i < in_roi_msg->rois.size(); ++i) { + // not detected roi + if (in_roi_msg->rois[i].roi.height == 0) { + not_detected_roi++; + continue; + } if (in_roi_msg->rois.at(i).traffic_light_type == traffic_light_type) { selected_roi_msg.rois.push_back(in_roi_msg->rois.at(i)); } @@ -142,7 +148,7 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg; - if (selected_roi_msg.rois.size() != in_signal_msg->signals.size()) { + if (selected_roi_msg.rois.size() != in_signal_msg->signals.size() - not_detected_roi) { occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { auto selected_roi_msg_ptr = @@ -162,6 +168,12 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( traffic_light_utils::setSignalUnknown(out_msg_.signals.at(predicted_num + i), 0.0); } } + + // push back not detected rois + for (size_t i = occlusion_ratios.size(); i < in_signal_msg->signals.size(); ++i) { + out_msg_.signals.push_back(in_signal_msg->signals[i]); + } + subscribed_.at(traffic_light_type) = true; if (std::all_of(subscribed_.begin(), subscribed_.end(), [](bool v) { return v; })) { diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 7daa0849abe5e..6f96913420bad 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -88,6 +88,10 @@ void CloudOcclusionPredictor::predict( image_geometry::PinholeCameraModel pinhole_model; pinhole_model.fromCameraInfo(*camera_info_msg); for (size_t i = 0; i < rois_msg->rois.size(); i++) { + // skip if no detection + if (rois_msg->rois[i].roi.height == 0) { + continue; + } calcRoiVector3D( rois_msg->rois[i], pinhole_model, traffic_light_position_map, tf_camera2map, roi_tls[i], roi_brs[i]); @@ -107,7 +111,7 @@ void CloudOcclusionPredictor::predict( lidar_rays_[static_cast(ray.azimuth)][static_cast(ray.elevation)].push_back(ray); } for (size_t i = 0; i < roi_tls.size(); i++) { - occlusion_ratios[i] = predict(roi_tls[i], roi_brs[i]); + occlusion_ratios[i] = rois_msg->rois[i].roi.height == 0 ? 0 : predict(roi_tls[i], roi_brs[i]); } } From d8767dd354235c78019405ddb42b4d54c266776a Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 9 Jan 2024 12:55:27 +0900 Subject: [PATCH 006/154] refactor(ndt_scan_matcher, map_loader): remove map_module (#5873) * Removed use_dynamic_map_loading Signed-off-by: Shintaro SAKODA * Removed enable_differential_load option Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix * Fixed title Signed-off-by: Shintaro Sakoda * Emphasized lock scope Signed-off-by: Shintaro Sakoda * Removed pointcloud_map and input_ekf_odom Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro SAKODA Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/CMakeLists.txt | 1 - localization/ndt_scan_matcher/README.md | 38 +++++--------- .../config/ndt_scan_matcher.param.yaml | 3 -- .../include/ndt_scan_matcher/map_module.hpp | 49 ------------------- .../ndt_scan_matcher_core.hpp | 4 -- .../launch/ndt_scan_matcher.launch.xml | 2 - .../ndt_scan_matcher/src/map_module.cpp | 49 ------------------- .../src/map_update_module.cpp | 5 +- .../src/ndt_scan_matcher_core.cpp | 17 ++----- map/map_loader/README.md | 5 +- .../config/pointcloud_map_loader.param.yaml | 1 - .../pointcloud_map_loader_node.cpp | 30 +++++------- 12 files changed, 29 insertions(+), 175 deletions(-) delete mode 100644 localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp delete mode 100644 localization/ndt_scan_matcher/src/map_module.cpp diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 2ad4a47b833d1..b4d656cb1810b 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -30,7 +30,6 @@ ament_auto_add_executable(ndt_scan_matcher src/particle.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp - src/map_module.cpp src/map_update_module.cpp ) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index d7a7b1c5c37f3..95a7cebdc01c8 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -18,7 +18,6 @@ One optional function is regularization. Please see the regularization chapter i | Name | Type | Description | | ----------------------------------- | ----------------------------------------------- | ------------------------------------- | | `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | initial pose | -| `pointcloud_map` | `sensor_msgs::msg::PointCloud2` | map pointcloud | | `points_raw` | `sensor_msgs::msg::PointCloud2` | sensor pointcloud | | `sensing/gnss/pose_with_covariance` | `sensor_msgs::msg::PoseWithCovarianceStamped` | base position for regularization term | @@ -193,12 +192,6 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### 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 | @@ -213,20 +206,15 @@ 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) | - -### Enabling the dynamic map loading feature +| Name | Type | Description | +| ------------------------------------- | ------ | ------------------------------------------------------------ | +| `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) | -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. +### Notes for dynamic map loading -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 size: 20[m] x 20[m]) +To use dynamic map loading feature for `ndt_scan_matcher`, you also need to 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 @@ -235,14 +223,10 @@ Note that the dynamic map loading may FAIL if the map is split into two or more 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 | true | at once (standard) | -| single file | true | false | **does NOT work** | -| single file | false | true/false | at once (standard) | -| multiple files | true | true | dynamically | -| multiple files | true | false | **does NOT work** | -| multiple files | false | true/false | at once (standard) | +| PCD files | How NDT loads map(s) | +| :------------: | :------------------: | +| single file | at once (standard) | +| multiple files | dynamically | ## Scan matching score based on de-grounded LiDAR scan 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 1a6c26cd9c351..9bc62d3f919c6 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,8 +1,5 @@ /**: ros__parameters: - # Use dynamic map loading - use_dynamic_map_loading: true - # Vehicle reference frame base_frame: "base_link" 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 deleted file mode 100644 index 79454e89b9ed0..0000000000000 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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_MODULE_HPP_ -#define NDT_SCAN_MATCHER__MAP_MODULE_HPP_ - -#include - -#include - -#include -#include -#include - -#include - -class MapModule -{ - using PointSource = pcl::PointXYZ; - using PointTarget = pcl::PointXYZ; - using NormalDistributionsTransform = - pclomp::MultiGridNormalDistributionsTransform; - -public: - MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group); - -private: - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); - - rclcpp::Subscription::SharedPtr map_points_sub_; - std::shared_ptr ndt_ptr_; - std::mutex * ndt_ptr_mutex_; -}; - -#endif // NDT_SCAN_MATCHER__MAP_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 b8792c20c323e..e9aa265c78f34 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,7 +18,6 @@ #define FMT_HEADER_ONLY #include "localization_util/smart_pose_buffer.hpp" -#include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" #include @@ -216,7 +215,6 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::unique_ptr map_module_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; @@ -224,8 +222,6 @@ class NDTScanMatcher : public rclcpp::Node bool estimate_scores_for_degrounded_scan_; double z_margin_for_ground_removal_; - bool use_dynamic_map_loading_; - // The execution time which means probably NDT cannot matches scans properly int64_t critical_upper_bound_exe_time_ms_; }; 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 655aa8e17ccd9..891d172aaf8fd 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -4,7 +4,6 @@ - @@ -19,7 +18,6 @@ - diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp deleted file mode 100644 index d6088a1b14949..0000000000000 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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_module.hpp" - -MapModule::MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex) -{ - auto map_sub_opt = rclcpp::SubscriptionOptions(); - map_sub_opt.callback_group = map_callback_group; - - map_points_sub_ = node->create_subscription( - "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&MapModule::callback_map_points, this, std::placeholders::_1), map_sub_opt); -} - -void MapModule::callback_map_points( - sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) -{ - NormalDistributionsTransform new_ndt; - new_ndt.setParams(ndt_ptr_->getParams()); - - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); - new_ndt.setInputTarget(map_points_ptr); - // create Thread - // detach - auto output_cloud = std::make_shared>(); - new_ndt.align(*output_cloud); - - // swap - ndt_ptr_mutex_->lock(); - *ndt_ptr_ = new_ndt; - ndt_ptr_mutex_->unlock(); -} diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 446f926a1d3f7..39b92fe248660 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -57,10 +57,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) request->cached_ids = ndt_ptr_->getCurrentMapIDs(); 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 the pointcloud_map_loader."); } // send a request to map_loader 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 7108dca20d8d7..0f6fdbc15db26 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -264,12 +264,7 @@ NDTScanMatcher::NDTScanMatcher() &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); - if (use_dynamic_map_loading_) { - map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); - } else { - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, sensor_callback_group); - } + map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); logger_configure_ = std::make_unique(this); } @@ -347,9 +342,6 @@ void NDTScanMatcher::callback_timer() if (!is_activated_) { return; } - if (!use_dynamic_map_loading_) { - return; - } std::lock_guard lock(latest_ekf_position_mtx_); if (latest_ekf_position_ == std::nullopt) { RCLCPP_ERROR_STREAM_THROTTLE( @@ -380,7 +372,8 @@ void NDTScanMatcher::callback_initial_pose( << ". Please check the frame_id in the input topic and ensure it is correct."); } - if (use_dynamic_map_loading_) { + { + // latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock std::lock_guard lock(latest_ekf_position_mtx_); latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position; } @@ -898,9 +891,7 @@ void NDTScanMatcher::service_ndt_align( // transform pose_frame to map_frame const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); - if (use_dynamic_map_loading_) { - map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); - } + map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); // mutex Map std::lock_guard lock(ndt_ptr_mtx_); diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 65d9594cb7415..fbe019096a3e7 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -20,7 +20,7 @@ NOTE: **We strongly recommend to use divided maps when using large pointcloud ma #### Prerequisites on pointcloud map file(s) -You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules: +You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules: 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. @@ -29,8 +29,6 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi 5. **All the split maps should not overlap with each other.** 6. **Metadata file should also be provided.** The metadata structure description is provided below. -Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023). - #### Metadata structure The metadata should look like this: @@ -118,7 +116,6 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | | enable_partial_load | bool | A flag to enable partial pointcloud map server | false | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | | enable_selected_load | bool | A flag to enable selected pointcloud map server | false | | leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | | pcd_paths_or_directory | std::string | Path(s) to pointcloud map file or directory | | diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index ba4c032d3e514..b4efbec9706b4 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,7 +3,6 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: true - enable_differential_load: true enable_selected_load: false # only used when downsample_whole_load enabled diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index a2d9307084545..5f4b3e311e6e9 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -53,7 +53,6 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); - bool enable_differential_load = declare_parameter("enable_differential_load"); bool enable_selected_load = declare_parameter("enable_selected_load"); if (enable_whole_load) { @@ -68,26 +67,21 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load || enable_differential_load || enable_selected_load) { - std::map pcd_metadata_dict; - try { - pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); - } catch (std::runtime_error & e) { - RCLCPP_ERROR_STREAM(get_logger(), e.what()); - } + std::map pcd_metadata_dict; + try { + pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } - if (enable_partial_load) { - partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_partial_load) { + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); + } - if (enable_differential_load) { - differential_map_loader_ = - std::make_unique(this, pcd_metadata_dict); - } + differential_map_loader_ = std::make_unique(this, pcd_metadata_dict); - if (enable_selected_load) { - selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_selected_load) { + selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); } } From fdeb24c3650901ab7f25223fb0c131afd824bff4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 9 Jan 2024 13:11:36 +0900 Subject: [PATCH 007/154] refactor(behavior_velocity_planner): output path's interval can be designated by yaml (#6023) Signed-off-by: Takayuki Murooka --- .../config/behavior_velocity_planner.param.yaml | 1 + planning/behavior_velocity_planner/src/node.cpp | 4 +++- planning/behavior_velocity_planner/src/node.hpp | 1 + .../utilization/path_utilization.hpp | 3 +-- 4 files changed, 6 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index aa51c38b55742..b31506918a2db 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -2,6 +2,7 @@ ros__parameters: forward_path_length: 1000.0 backward_path_length: 5.0 + behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 max_accel: -2.8 max_jerk: -5.0 diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 5da8df9e70c35..8ecad4d9c1548 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -138,6 +138,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Parameters forward_path_length_ = declare_parameter("forward_path_length"); backward_path_length_ = declare_parameter("backward_path_length"); + behavior_output_path_interval_ = declare_parameter("behavior_output_path_interval"); planner_data_.stop_line_extend_length = declare_parameter("stop_line_extend_length"); // nearest search @@ -409,7 +410,8 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + const auto interpolated_path_msg = + interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point output_path_msg = filterStopPathPoint(interpolated_path_msg); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index ded1d08700a1d..a345f1200f721 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -102,6 +102,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // parameter double forward_path_length_; double backward_path_length_; + double behavior_output_path_interval_; // member PlannerData planner_data_; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 591ef8667b81b..667c915ac1d7f 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -28,8 +28,7 @@ bool splineInterpolate( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, - const double interval = 1.0); + const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval); autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( const autoware_auto_planning_msgs::msg::Path & path); autoware_auto_planning_msgs::msg::Path filterStopPathPoint( From 8f09c8d1fd4e1db7d7afb22a76b5eacc389187a1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 9 Jan 2024 13:12:42 +0900 Subject: [PATCH 008/154] fix(avoidance, lane_change): modules handle unknown traffic signal in the same way as red signal (#6013) * fix(avoidance, lane_change): modules handle unknown traffic signal in the same way as red signal Signed-off-by: satoshi-ota * feat(traffic_light_utils): add util functions Signed-off-by: satoshi-ota * refactor(bpp): use traffic light utils Signed-off-by: satoshi-ota * refactor(bvp): use traffic light utils Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../traffic_light_utils.hpp | 44 ++++++++++++++ common/traffic_light_utils/package.xml | 2 + .../src/traffic_light_utils.cpp | 57 +++++++++++++++++++ .../utils/traffic_light_utils.hpp | 37 ------------ .../behavior_path_planner_common/package.xml | 1 + .../src/utils/traffic_light_utils.cpp | 56 +----------------- .../package.xml | 1 + .../src/scene.cpp | 54 +----------------- .../src/scene.hpp | 6 -- 9 files changed, 110 insertions(+), 148 deletions(-) diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index 081eab28e8833..6ce95acc4ef1d 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -15,10 +15,13 @@ #ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ #define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#include "autoware_perception_msgs/msg/traffic_signal.hpp" +#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" #include "tier4_perception_msgs/msg/traffic_signal.hpp" +#include #include #include #include @@ -36,6 +39,47 @@ bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficSignal & signal); void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float confidence = -1); +/** + * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. + * + * Iterates through the traffic light elements to find a circle-shaped light that matches the given + * color. + * + * @param tl_state The traffic light state to check. + * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. + * @return True if a circle-shaped light with the specified color is found, false otherwise. + */ +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color); + +/** + * @brief Checks if a traffic light state includes a light with the specified shape. + * + * Searches through the traffic light elements to find a light that matches the given shape. + * + * @param tl_state The traffic light state to check. + * @param shape The shape to look for in the traffic light's lights. + * @return True if a light with the specified shape is found, false otherwise. + */ +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param tl_state The current state of the traffic light associated with the lanelet. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state); + tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light); tf2::Vector3 getTrafficLightBottomRight(const lanelet::ConstLineString3d & traffic_light); diff --git a/common/traffic_light_utils/package.xml b/common/traffic_light_utils/package.xml index de05355eafd66..37b4d46ce356a 100644 --- a/common/traffic_light_utils/package.xml +++ b/common/traffic_light_utils/package.xml @@ -6,6 +6,7 @@ The traffic_light_utils package Mingyu Li Shunsuke Miura + Satoshi Ota Apache License 2.0 ament_cmake_auto @@ -15,6 +16,7 @@ ament_lint_auto autoware_lint_common + autoware_perception_msgs lanelet2_extension tier4_perception_msgs diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index e02798bb41767..c8f2ca85b2089 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -51,6 +51,63 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float } } +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE && + x.color == lamp_color; + }); + + return it_lamp != tl_state.elements.end(); +} + +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = std::find_if( + tl_state.elements.begin(), tl_state.elements.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.elements.end(); +} + +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state) +{ + if (hasTrafficLightCircleColor( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::GREEN)) { + return false; + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW)) { + return false; + } + + return true; +} + tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light) { const auto & tl_bl = traffic_light.front(); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp index bf48252a01dcd..6380830806c33 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -37,43 +37,6 @@ using autoware_perception_msgs::msg::TrafficSignal; using autoware_perception_msgs::msg::TrafficSignalElement; using geometry_msgs::msg::Pose; -/** - * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. - * - * Iterates through the traffic light elements to find a circle-shaped light that matches the given - * color. - * - * @param tl_state The traffic light state to check. - * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. - * @return True if a circle-shaped light with the specified color is found, false otherwise. - */ -bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color); - -/** - * @brief Checks if a traffic light state includes a light with the specified shape. - * - * Searches through the traffic light elements to find a light that matches the given shape. - * - * @param tl_state The traffic light state to check. - * @param shape The shape to look for in the traffic light's lights. - * @return True if a light with the specified shape is found, false otherwise. - */ -bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape); - -/** - * @brief Determines if a traffic signal indicates a stop for the given lanelet. - * - * Evaluates the current state of the traffic light, considering if it's green or unknown, - * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet - * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can - * proceed based on allowed turn directions. - * - * @param lanelet The lanelet to check for a stop signal at its traffic light. - * @param tl_state The current state of the traffic light associated with the lanelet. - * @return True if the traffic signal indicates a stop is required, false otherwise. - */ -bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state); - /** * @brief Computes the distance from the current position to the next traffic light along a set of * lanelets. diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index 83e9c39e24dd6..db5dc51150e55 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -60,6 +60,7 @@ tf2 tier4_autoware_utils tier4_planning_msgs + traffic_light_utils vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 850919f539e59..f77fafb42d829 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -14,63 +14,12 @@ #include #include +#include namespace behavior_path_planner::utils::traffic_light { using motion_utils::calcSignedArcLength; -bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) -{ - const auto it_lamp = - std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; - }); - - return it_lamp != tl_state.elements.end(); -} - -bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) -{ - const auto it_lamp = std::find_if( - tl_state.elements.begin(), tl_state.elements.end(), - [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - - return it_lamp != tl_state.elements.end(); -} - -bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state) -{ - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { - return false; - } - - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) { - return false; - } - - const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); - - if (turn_direction == "else") { - return true; - } - if ( - turn_direction == "right" && - hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { - return false; - } - if ( - turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { - return false; - } - if ( - turn_direction == "straight" && - hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { - return false; - } - - return true; -} - double getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { @@ -138,7 +87,8 @@ std::optional calcDistanceToRedTrafficLight( continue; } - if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { + if (!traffic_light_utils::isTrafficSignalStop( + lanelet, traffic_signal_stamped.value().signal)) { continue; } diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index 39c454e59dcb2..33bb471e5920c 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -33,6 +33,7 @@ tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs + traffic_light_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 35d44a22f6cf0..a17fc43b86f23 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -287,7 +288,7 @@ bool TrafficLightModule::isStopSignal() return true; } - return isTrafficSignalStop(looking_tl_state_); + return traffic_light_utils::isTrafficSignalStop(lane_, looking_tl_state_); } void TrafficLightModule::updateTrafficSignal() @@ -349,36 +350,6 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const } } -bool TrafficLightModule::isTrafficSignalStop( - const autoware_perception_msgs::msg::TrafficSignal & tl_state) const -{ - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { - return false; - } - - const std::string turn_direction = lane_.attributeOr("turn_direction", "else"); - - if (turn_direction == "else") { - return true; - } - if ( - turn_direction == "right" && - hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { - return false; - } - if ( - turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { - return false; - } - if ( - turn_direction == "straight" && - hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { - return false; - } - - return true; -} - bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const { // get traffic signal associated with the regulatory element id @@ -452,25 +423,4 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP return modified_path; } -bool TrafficLightModule::hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) const -{ - const auto it_lamp = - std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; - }); - - return it_lamp != tl_state.elements.end(); -} - -bool TrafficLightModule::hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) const -{ - const auto it_lamp = std::find_if( - tl_state.elements.begin(), tl_state.elements.end(), - [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - - return it_lamp != tl_state.elements.end(); -} - } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index c7b2472289bf5..102ddbe2e9fa8 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -88,8 +88,6 @@ class TrafficLightModule : public SceneModuleInterface private: bool isStopSignal(); - bool isTrafficSignalStop(const TrafficSignal & tl_state) const; - autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, @@ -97,10 +95,6 @@ class TrafficLightModule : public SceneModuleInterface bool isPassthrough(const double & signed_arc_length) const; - bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) const; - - bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) const; - bool findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const; bool isTrafficSignalTimedOut() const; From bb4f8d8ce6ba4c92f31754b645e8d697359f6d2b Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Tue, 9 Jan 2024 13:17:40 +0900 Subject: [PATCH 009/154] fix(crosswalk): stopping besides the stop line (#6015) * stopping besides the stop line Signed-off-by: Yuki Takagi --- .../src/scene_crosswalk.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index d10fc35a5a6f4..0dc7b78b6fe46 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -393,7 +393,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return {}; } - // Check if the ego should stop beyond the stop line. + // Check if the ego should stop at the stop line or the other points. const bool stop_at_stop_line = dist_ego_to_stop < nearest_stop_info->second && nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; @@ -404,9 +404,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return createStopFactor(*default_stop_pose, stop_factor_points); } } else { - // Stop beyond the stop line const auto stop_pose = calcLongitudinalOffsetPose( - ego_path.points, nearest_stop_info->first, planner_param_.stop_distance_from_object); + ego_path.points, nearest_stop_info->first, + -base_link2front - planner_param_.stop_distance_from_object); if (stop_pose) { return createStopFactor(*stop_pose, stop_factor_points); } From af08c4089e342239a75ba9a470d311fa502e21bf Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 9 Jan 2024 05:06:58 +0000 Subject: [PATCH 010/154] fix(autoware_auto_common): move headers to a separate directory (#5919) * fix(autoware_auto_common): move headers to a separate directory Signed-off-by: Esteve Fernandez * style(pre-commit): autofix --------- Signed-off-by: Esteve Fernandez Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/autoware_auto_common/design/comparisons.md | 4 ++-- .../{ => autoware_auto_common}/common/type_traits.hpp | 10 +++++----- .../{ => autoware_auto_common}/common/types.hpp | 10 +++++----- .../common/visibility_control.hpp | 6 +++--- .../helper_functions/angle_utils.hpp | 6 +++--- .../helper_functions/bool_comparisons.hpp | 8 ++++---- .../helper_functions/byte_reader.hpp | 6 +++--- .../helper_functions/crtp.hpp | 6 +++--- .../helper_functions/float_comparisons.hpp | 6 +++--- .../helper_functions/mahalanobis_distance.hpp | 6 +++--- .../helper_functions/message_adapters.hpp | 6 +++--- .../helper_functions/template_utils.hpp | 8 ++++---- .../helper_functions/type_name.hpp | 8 ++++---- common/autoware_auto_common/test/test_angle_utils.cpp | 4 ++-- .../test/test_bool_comparisons.cpp | 2 +- common/autoware_auto_common/test/test_byte_reader.cpp | 7 ++++--- .../test/test_float_comparisons.cpp | 2 +- .../test/test_mahalanobis_distance.cpp | 4 ++-- .../test/test_message_field_adapters.cpp | 2 +- .../autoware_auto_common/test/test_template_utils.cpp | 2 +- common/autoware_auto_common/test/test_type_name.cpp | 4 ++-- common/autoware_auto_common/test/test_type_traits.cpp | 4 ++-- .../include/autoware_auto_geometry/common_2d.hpp | 2 +- .../include/autoware_auto_geometry/convex_hull.hpp | 2 +- .../include/autoware_auto_geometry/hull_pockets.hpp | 2 +- .../include/autoware_auto_geometry/interval.hpp | 4 ++-- .../include/autoware_auto_geometry/lookup_table.hpp | 3 ++- .../include/autoware_auto_geometry/spatial_hash.hpp | 2 +- .../autoware_auto_geometry/spatial_hash_config.hpp | 4 ++-- .../autoware_auto_geometry/test/src/lookup_table.cpp | 2 +- .../object_detection/object_polygon_display_base.hpp | 4 ++-- .../src/common/color_alpha_property.cpp | 2 +- .../autoware_auto_tf2/tf2_autoware_auto_msgs.hpp | 2 +- common/fake_test_node/test/test_fake_test_node.cpp | 2 +- .../src/frenet_planner/frenet_planner.cpp | 2 +- .../sim_model_delay_steer_map_acc_geared.hpp | 2 +- 36 files changed, 79 insertions(+), 77 deletions(-) rename common/autoware_auto_common/include/{ => autoware_auto_common}/common/type_traits.hpp (96%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/common/types.hpp (93%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/common/visibility_control.hpp (88%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/helper_functions/angle_utils.hpp (89%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/helper_functions/bool_comparisons.hpp (85%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/helper_functions/byte_reader.hpp (90%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/helper_functions/crtp.hpp (88%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/helper_functions/float_comparisons.hpp (95%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/helper_functions/mahalanobis_distance.hpp (92%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/helper_functions/message_adapters.hpp (94%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/helper_functions/template_utils.hpp (91%) rename common/autoware_auto_common/include/{ => autoware_auto_common}/helper_functions/type_name.hpp (84%) diff --git a/common/autoware_auto_common/design/comparisons.md b/common/autoware_auto_common/design/comparisons.md index f3a7eb5e58429..3d00f77c1b0c3 100644 --- a/common/autoware_auto_common/design/comparisons.md +++ b/common/autoware_auto_common/design/comparisons.md @@ -22,8 +22,8 @@ The `exclusive_or` function will test whether two values cast to different boole ## Example Usage ```c++ -#include "common/bool_comparisons.hpp" -#include "common/float_comparisons.hpp" +#include "autoware_auto_common/common/bool_comparisons.hpp" +#include "autoware_auto_common/common/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/include/common/type_traits.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp similarity index 96% rename from common/autoware_auto_common/include/common/type_traits.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp index 7087ed1e81181..66f382f081b33 100644 --- a/common/autoware_auto_common/include/common/type_traits.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp @@ -14,15 +14,15 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/common/visibility_control.hpp" #include #include #include -#ifndef COMMON__TYPE_TRAITS_HPP_ -#define COMMON__TYPE_TRAITS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ namespace autoware { @@ -219,4 +219,4 @@ struct intersect } // namespace common } // namespace autoware -#endif // COMMON__TYPE_TRAITS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp similarity index 93% rename from common/autoware_auto_common/include/common/types.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/types.hpp index 3f3e444c1aa8c..1c7dfe48c7ec8 100644 --- a/common/autoware_auto_common/include/common/types.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp @@ -16,11 +16,11 @@ /// \file /// \brief This file includes common type definition -#ifndef COMMON__TYPES_HPP_ -#define COMMON__TYPES_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ -#include "common/visibility_control.hpp" -#include "helper_functions/float_comparisons.hpp" +#include "autoware_auto_common/common/visibility_control.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" #include #include @@ -122,4 +122,4 @@ using void_t = void; } // namespace common } // namespace autoware -#endif // COMMON__TYPES_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ diff --git a/common/autoware_auto_common/include/common/visibility_control.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp similarity index 88% rename from common/autoware_auto_common/include/common/visibility_control.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp index 0a988d6407dfa..33834fd9ccfed 100644 --- a/common/autoware_auto_common/include/common/visibility_control.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef COMMON__VISIBILITY_CONTROL_HPP_ -#define COMMON__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ #if defined(_MSC_VER) && defined(_WIN64) #if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) @@ -35,4 +35,4 @@ #error "Unsupported Build Configuration" #endif // _MSC_VER -#endif // COMMON__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp similarity index 89% rename from common/autoware_auto_common/include/helper_functions/angle_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp index 6cca2440d5680..ea974774dd9d5 100644 --- a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ -#define HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ #include #include @@ -63,4 +63,4 @@ constexpr T wrap_angle(T angle) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp similarity index 85% rename from common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp index c6bf09365af4f..45da098ad0066 100644 --- a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp @@ -18,10 +18,10 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#include "common/types.hpp" +#include "autoware_auto_common/common/types.hpp" namespace autoware { @@ -47,4 +47,4 @@ types::bool8_t exclusive_or(const T & a, const T & b) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp similarity index 90% rename from common/autoware_auto_common/include/helper_functions/byte_reader.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp index d9e55c4ecfbfe..3852361caebeb 100644 --- a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__BYTE_READER_HPP_ -#define HELPER_FUNCTIONS__BYTE_READER_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ #include #include @@ -70,4 +70,4 @@ class ByteReader } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BYTE_READER_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/crtp.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp similarity index 88% rename from common/autoware_auto_common/include/helper_functions/crtp.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp index 0e8110a9a3bb9..e75674eb73c70 100644 --- a/common/autoware_auto_common/include/helper_functions/crtp.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__CRTP_HPP_ -#define HELPER_FUNCTIONS__CRTP_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ namespace autoware { @@ -49,4 +49,4 @@ class crtp } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__CRTP_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp similarity index 95% rename from common/autoware_auto_common/include/helper_functions/float_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp index de1f459f21d0a..1a64fe71a1720 100644 --- a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp @@ -18,8 +18,8 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ #include #include @@ -146,4 +146,4 @@ bool approx_eq(const T & a, const T & b, const T & abs_eps, const T & rel_eps) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp similarity index 92% rename from common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp index fb9bdccf32b25..70c29eaf220d8 100644 --- a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ -#define HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ #include @@ -69,4 +69,4 @@ types::float32_t calculate_mahalanobis_distance( } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp similarity index 94% rename from common/autoware_auto_common/include/helper_functions/message_adapters.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp index 352ef7c7b65d5..d3bda57b3374f 100644 --- a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ -#define HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ #include @@ -112,4 +112,4 @@ TimeStamp get_stamp(const T & msg) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/template_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp similarity index 91% rename from common/autoware_auto_common/include/helper_functions/template_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp index b39931a3fd15a..0360908509618 100644 --- a/common/autoware_auto_common/include/helper_functions/template_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#define HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#include +#include "autoware_auto_common/common/types.hpp" #include @@ -72,4 +72,4 @@ struct expression_valid_with_return< } // namespace helper_functions } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/type_name.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp similarity index 84% rename from common/autoware_auto_common/include/helper_functions/type_name.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp index a95834373d552..106cede1f2f5a 100644 --- a/common/autoware_auto_common/include/helper_functions/type_name.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#define HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#include +#include "autoware_auto_common/common/visibility_control.hpp" #include #include @@ -53,4 +53,4 @@ COMMON_PUBLIC std::string get_type_name(const T &) } // namespace helper_functions } // namespace autoware -#endif // HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ diff --git a/common/autoware_auto_common/test/test_angle_utils.cpp b/common/autoware_auto_common/test/test_angle_utils.cpp index 031ae144139aa..810c302845daf 100644 --- a/common/autoware_auto_common/test/test_angle_utils.cpp +++ b/common/autoware_auto_common/test/test_angle_utils.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/angle_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_bool_comparisons.cpp b/common/autoware_auto_common/test/test_bool_comparisons.cpp index 67c1c8ddbf9aa..a84d65e569692 100644 --- a/common/autoware_auto_common/test/test_bool_comparisons.cpp +++ b/common/autoware_auto_common/test/test_bool_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/bool_comparisons.hpp" +#include "autoware_auto_common/helper_functions/bool_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_byte_reader.cpp b/common/autoware_auto_common/test/test_byte_reader.cpp index c83d06c6e8132..a5ab8743f7ed4 100644 --- a/common/autoware_auto_common/test/test_byte_reader.cpp +++ b/common/autoware_auto_common/test/test_byte_reader.cpp @@ -14,9 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "common/types.hpp" -#include "gtest/gtest.h" -#include "helper_functions/byte_reader.hpp" +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/byte_reader.hpp" + +#include #include diff --git a/common/autoware_auto_common/test/test_float_comparisons.cpp b/common/autoware_auto_common/test/test_float_comparisons.cpp index d292dc0a0cb20..37d3afdc177d5 100644 --- a/common/autoware_auto_common/test/test_float_comparisons.cpp +++ b/common/autoware_auto_common/test/test_float_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/float_comparisons.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp index 262599180abb6..2015a85bc2bc8 100644 --- a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp +++ b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp @@ -13,8 +13,8 @@ // limitations under the License. // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/mahalanobis_distance.hpp" #include diff --git a/common/autoware_auto_common/test/test_message_field_adapters.cpp b/common/autoware_auto_common/test/test_message_field_adapters.cpp index 34974c1cda9a6..c35f0ff826995 100644 --- a/common/autoware_auto_common/test/test_message_field_adapters.cpp +++ b/common/autoware_auto_common/test/test_message_field_adapters.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/message_adapters.hpp" #include diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp index 9c9ca9ae4b5f2..a670aaab83cfa 100644 --- a/common/autoware_auto_common/test/test_template_utils.cpp +++ b/common/autoware_auto_common/test/test_template_utils.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/template_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_name.cpp b/common/autoware_auto_common/test/test_type_name.cpp index 1fb60d838f307..4ada59b4fb2e1 100644 --- a/common/autoware_auto_common/test/test_type_name.cpp +++ b/common/autoware_auto_common/test/test_type_name.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/type_name.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_traits.cpp b/common/autoware_auto_common/test/test_type_traits.cpp index 7203ab8d649ee..92d01b3d84d51 100644 --- a/common/autoware_auto_common/test/test_type_traits.cpp +++ b/common/autoware_auto_common/test/test_type_traits.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/type_traits.hpp" +#include "autoware_auto_common/common/types.hpp" #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp index da7121bf4e9b5..e3c2e58c9f80e 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp @@ -21,7 +21,7 @@ #include "autoware_auto_geometry/interval.hpp" -#include +#include #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp index e566e2e43ae3e..ae81c55ad6b55 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp @@ -23,7 +23,7 @@ #include "autoware_auto_geometry/common_2d.hpp" -#include +#include // lint -e537 NOLINT pclint vs cpplint #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp index fd04c0266e065..cd9fd49f71635 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp @@ -23,7 +23,7 @@ #include "autoware_auto_geometry/common_2d.hpp" -#include +#include #include #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp index 17e2789ac0325..54be2c32b1d05 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp @@ -21,8 +21,8 @@ #ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ #define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#include "common/types.hpp" -#include "helper_functions/float_comparisons.hpp" +#include +#include #include #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp index c689638aa2bd9..7b8867ca096ae 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp @@ -21,7 +21,8 @@ #define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ #include "autoware_auto_geometry/interval.hpp" -#include "common/types.hpp" + +#include #include #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp index c160685fb0f8c..8936e2c17d779 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp @@ -23,7 +23,7 @@ #include "autoware_auto_geometry/spatial_hash_config.hpp" #include "autoware_auto_geometry/visibility_control.hpp" -#include +#include #include #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp index 926b170f807c2..24c4d6e879d4a 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp @@ -22,9 +22,9 @@ #include "autoware_auto_geometry/common_2d.hpp" #include "autoware_auto_geometry/visibility_control.hpp" -#include "helper_functions/crtp.hpp" -#include +#include +#include #include #include diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp index 092f8a04f2540..81865656c55b5 100644 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ b/common/autoware_auto_geometry/test/src/lookup_table.cpp @@ -16,7 +16,7 @@ #include "autoware_auto_geometry/lookup_table.hpp" -#include +#include #include diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 9157dffe97785..223611520b2fc 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -14,12 +14,12 @@ #ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ #define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#include "rviz_common/properties/enum_property.hpp" +#include "common/color_alpha_property.hpp" -#include #include #include #include +#include #include #include #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp index ac9c5af4ddeef..57a42c95f9d34 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "common/color_alpha_property.hpp" #include diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index b35eb6e93ce6e..50c16ba8eaf7d 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -17,7 +17,7 @@ #ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ #define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ -#include +#include #include #include diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index d66d0fed3033c..e065f332b75e4 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -17,7 +17,7 @@ /// \copyright Copyright 2021 Apex.AI, Inc. /// All rights reserved. -#include +#include #include #include diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp index 3368c49459a55..a4ff1db77e8a1 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -14,11 +14,11 @@ #include "frenet_planner/frenet_planner.hpp" +#include #include #include #include #include -#include #include #include diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index 2cc33f9b5f82b..fecd3c2bc8be4 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -21,7 +21,7 @@ #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include +#include #include #include From 599aea08ba340371895b1c364e95b9a538127826 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Tue, 9 Jan 2024 15:20:06 +0900 Subject: [PATCH 011/154] feat(map_based_prediction): cope with consecutive crosswalks (#6009) * ready for consecutive crosswalks Signed-off-by: Yuki Takagi --- .../src/map_based_prediction_node.cpp | 167 +++++++++--------- 1 file changed, 86 insertions(+), 81 deletions(-) diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 0091fc6bc38cb..75c1d61e0a19c 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -415,17 +415,21 @@ bool withinRoadLanelet( } boost::optional isReachableCrosswalkEdgePoints( - const TrackedObject & object, const CrosswalkEdgePoints & edge_points, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const double time_horizon, - const double min_object_vel) + const TrackedObject & object, const lanelet::ConstLanelet & target_crosswalk, + const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const double time_horizon, const double min_object_vel) { using Point = boost::geometry::model::d2::point_xy; - using Line = boost::geometry::model::linestring; const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); + if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) { + return {}; + } + const auto & p1 = edge_points.front_center_point; const auto & p2 = edge_points.back_center_point; @@ -442,62 +446,66 @@ boost::optional isReachableCrosswalkEdgePoints( const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto is_stop_object = estimated_velocity < stop_velocity_th; const auto velocity = std::max(min_object_vel, estimated_velocity); - - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); - // nearest lanelet const auto surrounding_lanelets = lanelet::geometry::findNearest( - lanelet_map_ptr->laneletLayer, search_point, time_horizon * velocity); + lanelet_map_ptr->laneletLayer, obj_pos_as_lanelet, time_horizon * velocity); - bool first_intersect_load = false; - bool second_intersect_load = false; - std::vector intersects_first; - std::vector intersects_second; - for (const auto & lanelet : surrounding_lanelets) { - if (withinLanelet(object, lanelet.second)) { - return {}; - } + const auto isAcrossAnyRoad = [&surrounding_lanelets](const Point & p_src, const Point & p_dst) { + const auto withinAnyCrosswalk = [&surrounding_lanelets](const Point & p) { + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if ( + (attr.value() == lanelet::AttributeValueString::Crosswalk || + attr.value() == lanelet::AttributeValueString::Walkway) && + boost::geometry::within(p, lanelet.second.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; - lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); - if (attr.value() != "road") { - continue; - } + const auto isExist = [](const Point & p, const std::vector & points) { + for (const auto & existingPoint : points) { + if (boost::geometry::distance(p, existingPoint) < 1e-1) { + return true; + } + } + return false; + }; - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}}; - std::vector tmp_intersects; - boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); - for (const auto & p : tmp_intersects) { - intersects_first.push_back(p); + std::vector points_of_intersect; + const boost::geometry::model::linestring line{p_src, p_dst}; + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if (attr.value() != lanelet::AttributeValueString::Road) { + continue; } - } - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}}; std::vector tmp_intersects; boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); + line, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); for (const auto & p : tmp_intersects) { - intersects_second.push_back(p); + if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) { + continue; + } + points_of_intersect.push_back(p); + if (points_of_intersect.size() >= 2) { + return true; + } } } - } - - if (1 < intersects_first.size()) { - first_intersect_load = true; - } + return false; + }; - if (1 < intersects_second.size()) { - second_intersect_load = true; - } + const bool first_intersects_road = isAcrossAnyRoad( + {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}); + const bool second_intersects_road = + isAcrossAnyRoad({obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}); - if (first_intersect_load && second_intersect_load) { + if (first_intersects_road && second_intersects_road) { return {}; } - if (first_intersect_load && !second_intersect_load) { + if (first_intersects_road && !second_intersects_road) { ret.swap(); } @@ -1197,48 +1205,45 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( predicted_object.kinematics.predicted_paths.push_back(predicted_path); } } + } + // try to find the edge points for all crosswalks and generate path to the crosswalk edge + for (const auto & crosswalk : crosswalks_) { + const auto edge_points = getCrosswalkEdgePoints(crosswalk); + + const auto reachable_first = hasPotentialToReach( + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + const auto reachable_second = hasPotentialToReach( + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + + if (!reachable_first && !reachable_second) { + continue; + } - // If the object is not crossing the crosswalk, not in the road lanelets, try to find the edge - // points for all crosswalks and generate path to the crosswalk edge - } else { - for (const auto & crosswalk : crosswalks_) { - const auto edge_points = getCrosswalkEdgePoints(crosswalk); - - const auto reachable_first = hasPotentialToReach( - object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - const auto reachable_second = hasPotentialToReach( - object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - - if (!reachable_first && !reachable_second) { - continue; - } - - const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, edge_points, lanelet_map_ptr_, prediction_time_horizon_, - min_crosswalk_user_velocity_); - - if (!reachable_crosswalk) { - continue; - } + const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + min_crosswalk_user_velocity_); - PredictedPath predicted_path = - path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); - predicted_path.confidence = 1.0; + if (!reachable_crosswalk) { + continue; + } - if (predicted_path.path.empty()) { - continue; - } - // If the predicted path to the crosswalk is crossing the fence, don't use it - if (doesPathCrossAnyFence(predicted_path)) { - continue; - } + PredictedPath predicted_path = + path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); + if (predicted_path.path.empty()) { + continue; } + // If the predicted path to the crosswalk is crossing the fence, don't use it + if (doesPathCrossAnyFence(predicted_path)) { + continue; + } + + predicted_object.kinematics.predicted_paths.push_back(predicted_path); } const auto n_path = predicted_object.kinematics.predicted_paths.size(); From f693b04be590402c03ee4eb99b673e9cea56edae Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 9 Jan 2024 17:28:01 +0900 Subject: [PATCH 012/154] refactor(map_projection_loader): make important part a library (#5992) * refactor(map_projection_loader): make important part a library Signed-off-by: kminoda * style(pre-commit): autofix * fix: add const Signed-off-by: kminoda * fix: fix function arguments Signed-off-by: kminoda * style(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../map_projection_loader.hpp | 2 + .../src/map_projection_loader.cpp | 45 ++++++++++++------- 2 files changed, 31 insertions(+), 16 deletions(-) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 16442eb43c740..54e794e2742bf 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -23,6 +23,8 @@ #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename); class MapProjectionLoader : public rclcpp::Node { diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 9fdba288601cc..5966baaed8383 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -20,6 +20,7 @@ #include +#include #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) @@ -55,28 +56,40 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi return msg; } -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename) { - std::string yaml_filename = this->declare_parameter("map_projector_info_path"); - std::string lanelet2_map_filename = this->declare_parameter("lanelet2_map_path"); - std::ifstream file(yaml_filename); - tier4_map_msgs::msg::MapProjectorInfo msg; - bool use_yaml_file = file.is_open(); - if (use_yaml_file) { - RCLCPP_INFO(this->get_logger(), "Load %s", yaml_filename.c_str()); + if (std::filesystem::exists(yaml_filename)) { + std::cout << "Load " << yaml_filename << std::endl; msg = load_info_from_yaml(yaml_filename); - } else { - RCLCPP_INFO(this->get_logger(), "Load %s", lanelet2_map_filename.c_str()); - RCLCPP_WARN( - this->get_logger(), - "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " - "Please use map_projector_info.yaml instead. For more info, visit " - "https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_projection_loader/" - "README.md"); + } else if (std::filesystem::exists(lanelet2_map_filename)) { + std::cout << "Load " << lanelet2_map_filename << std::endl; + std::cout + << "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " + "Please use map_projector_info.yaml instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/" + "map_projection_loader/" + "README.md" + << std::endl; msg = load_info_from_lanelet2_map(lanelet2_map_filename); + } else { + throw std::runtime_error( + "No map projector info files found. Please provide either " + "map_projector_info.yaml or lanelet2_map.osm"); } + return msg; +} + +MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +{ + const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); + const std::string lanelet2_map_filename = + this->declare_parameter("lanelet2_map_path"); + + const tier4_map_msgs::msg::MapProjectorInfo msg = + load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message const auto adaptor = component_interface_utils::NodeAdaptor(this); From 618ad01a21c9bb42447230cc0ad41a298ad77b7e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 9 Jan 2024 20:15:43 +0900 Subject: [PATCH 013/154] fix((merge_from_private): fixed first attention lanelet calculation (#6030) Signed-off-by: Mamoru Sobue --- .../src/scene_merge_from_private_road.cpp | 71 ++++++++----------- .../src/scene_merge_from_private_road.hpp | 4 +- 2 files changed, 29 insertions(+), 46 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index f56d0a4adcff7..b929959e43f12 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -108,14 +108,12 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return false; } + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); if (!first_conflicting_lanelet_) { - const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - const auto conflicting_lanelets = - lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + const auto conflicting_lanelets = getAttentionLanelets(); first_conflicting_lanelet_ = getFirstConflictingLanelet( - conflicting_lanelets, interpolated_path_info, local_footprint, - planner_data_->vehicle_info_.max_longitudinal_offset_m); + conflicting_lanelets, interpolated_path_info, local_footprint, baselink2front); } if (!first_conflicting_lanelet_) { return false; @@ -123,12 +121,13 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR const auto first_conflicting_lanelet = first_conflicting_lanelet_.value(); const auto first_conflicting_idx_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, - planner_data_->vehicle_info_.max_longitudinal_offset_m); + first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, baselink2front); if (!first_conflicting_idx_opt) { return false; } - const auto stopline_idx_ip = first_conflicting_idx_opt.value(); + const auto stopline_idx_ip = static_cast(std::max( + 0, static_cast(first_conflicting_idx_opt.value()) - + static_cast(baselink2front / planner_param_.path_interpolation_ds))); const auto stopline_idx_opt = util::insertPointIndex( interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path, @@ -139,8 +138,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR } const auto stopline_idx = stopline_idx_opt.value(); - debug_data_.virtual_wall_pose = planning_utils::getAheadPose( - stopline_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); debug_data_.stop_point_pose = path->points.at(stopline_idx).point.pose; /* set stop speed */ @@ -174,46 +172,33 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return true; } -autoware_auto_planning_msgs::msg::PathWithLaneId -MergeFromPrivateRoadModule::extractPathNearExitOfPrivateRoad( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length) +lanelet::ConstLanelets MergeFromPrivateRoadModule::getAttentionLanelets() const { - if (path.points.size() < 2) { - return path; - } - - autoware_auto_planning_msgs::msg::PathWithLaneId private_path = path; - private_path.points.clear(); + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); - double sum_dist = 0.0; - bool prev_has_target_lane_id = false; - for (int i = static_cast(path.points.size()) - 2; i >= 0; i--) { - bool has_target_lane_id = false; - for (const auto path_id : path.points.at(i).lane_ids) { - if (path_id == lane_id_) { - has_target_lane_id = true; + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + lanelet::ConstLanelets sibling_lanelets; + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + sibling_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(sibling_lanelets, following_lanelet)) { + continue; } + sibling_lanelets.push_back(following_lanelet); } - if (has_target_lane_id) { - // add path point with target lane id - // (lanelet with target lane id is exit of private road) - private_path.points.emplace_back(path.points.at(i)); - prev_has_target_lane_id = true; + } + + lanelet::ConstLanelets attention_lanelets; + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(sibling_lanelets, conflicting_lanelet)) { continue; } - if (prev_has_target_lane_id) { - // extend path to the front - private_path.points.emplace_back(path.points.at(i)); - sum_dist += tier4_autoware_utils::calcDistance2d( - path.points.at(i).point.pose, path.points.at(i + 1).point.pose); - if (sum_dist > extend_length) { - break; - } - } + attention_lanelets.push_back(conflicting_lanelet); } - - std::reverse(private_path.points.begin(), private_path.points.end()); - return private_path; + return attention_lanelets; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index ab368e6b3106b..a44b99c97457d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -73,14 +73,12 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface motion_utils::VirtualWalls createVirtualWalls() override; const std::set & getAssociativeIds() const { return associative_ids_; } + lanelet::ConstLanelets getAttentionLanelets() const; private: const int64_t lane_id_; const std::set associative_ids_; - autoware_auto_planning_msgs::msg::PathWithLaneId extractPathNearExitOfPrivateRoad( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length); - // Parameter PlannerParam planner_param_; std::optional first_conflicting_lanelet_; From 39e9acc2fa06ef211b2e17a5381b026a03102997 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 10 Jan 2024 01:50:08 +0900 Subject: [PATCH 014/154] feat(image_projection_based_fusion): add image segmentation_pointcloud_fusion for pointcloud filter (#5562) * fix: roiCallback for Image Signed-off-by: badai-nguyen * fix: pointcloud filter Signed-off-by: badai-nguyen * fix: pointcloud filter bugfix Signed-off-by: badai-nguyen * fix: segmentation poincloud filter Signed-off-by: badai-nguyen * fix: add option in launch file Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * docs: update docs Signed-off-by: badai-nguyen * fix: roi_pointcloud_fusion with new template Signed-off-by: badai-nguyen * fix: add param of selectable semantic id Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * fix: launch file Signed-off-by: badai-nguyen * Revert "fix: launch file" This reverts commit 5dfa160964c6db2bcdb402bf47a2ebe30f177010. * fix: reverse perception launcher Signed-off-by: badai-nguyen * style(pre-commit): autofix * Update perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp Co-authored-by: Yoshi Ri * style(pre-commit): autofix --------- Signed-off-by: badai-nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yoshi Ri --- .../CMakeLists.txt | 6 + .../segmentation_pointcloud_fusion.param.yaml | 30 ++++ .../docs/segmentation-pointcloud-fusion.md | 87 +++++++++++ .../fusion_node.hpp | 16 +- .../pointpainting_fusion/node.hpp | 3 +- .../roi_cluster_fusion/node.hpp | 3 +- .../roi_detected_object_fusion/node.hpp | 3 +- .../roi_pointcloud_fusion/node.hpp | 2 +- .../segmentation_pointcloud_fusion/node.hpp | 54 +++++++ .../segmentation_pointcloud_fusion.launch.xml | 87 +++++++++++ .../src/fusion_node.cpp | 54 +++---- .../src/pointpainting_fusion/node.cpp | 3 +- .../src/roi_cluster_fusion/node.cpp | 3 +- .../src/roi_detected_object_fusion/node.cpp | 3 +- .../src/roi_pointcloud_fusion/node.cpp | 3 +- .../segmentation_pointcloud_fusion/node.cpp | 139 ++++++++++++++++++ 16 files changed, 455 insertions(+), 41 deletions(-) create mode 100644 perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml create mode 100644 perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md create mode 100644 perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp create mode 100644 perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml create mode 100644 perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index c021dd92dae64..b13f68b07181e 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/segmentation_pointcloud_fusion/node.cpp src/roi_pointcloud_fusion/node.cpp ) @@ -40,6 +41,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::SegmentPointCloudFusionNode" + EXECUTABLE segmentation_pointcloud_fusion_node +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" EXECUTABLE roi_pointcloud_fusion_node diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml new file mode 100644 index 0000000000000..914ad13519807 --- /dev/null +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -0,0 +1,30 @@ +/**: + ros__parameters: + # if the semantic label is applied for pointcloud filtering + filter_semantic_label_target: + [ + true, # road + true, # sidewalk + true, # building + true, # wall + true, # fence + true, # pole + true, # traffic_light + true, # traffic_sign + true, # vegetation + true, # terrain + true, # sky + false, # person + false, # ride + false, # car + false, # truck + false, # bus + false, # train + false, # motorcycle + false, # bicycle + false, # others + ] + # the maximum distance of pointcloud to be applied filter, + # this is selected based on semantic segmentation model accuracy, + # calibration accuracy and unknown reaction distance + filter_distance_threshold: 60.0 diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md new file mode 100644 index 0000000000000..77188aafb3b22 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -0,0 +1,87 @@ +# segmentation_pointcloud_fusion + +## Purpose + +The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud that are belong to less interesting region which is defined by semantic or instance segmentation by 2D image segmentation model. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto the semantic/instance segmentation mask image which is the output from 2D segmentation neural network. +- The pointclouds are belong to segment such as road, sidewalk, building, vegetation, sky ... are considered as less important points for autonomous driving and could be removed. + +![segmentation_pointcloud_fusion_image](./images/segmentation_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | ----------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::Image` | semantic segmentation mask image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| -------- | ------------------------------- | -------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output filtered pointcloud | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ------------- | ---- | ------------------------ | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 9572e62e0e0aa..fe8acb6746dba 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -49,13 +50,14 @@ namespace image_projection_based_fusion { using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +using sensor_msgs::msg::CameraInfo; +using sensor_msgs::msg::Image; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; using autoware_auto_perception_msgs::msg::ObjectClassification; - -template +template class FusionNode : public rclcpp::Node { public: @@ -78,12 +80,12 @@ class FusionNode : public rclcpp::Node virtual void subCallback(const typename Msg::ConstSharedPtr input_msg); // callback for roi subscription + virtual void roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); virtual void fuseOnSingleImage( - const Msg & input_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, + const Msg & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0; // set args if you need @@ -111,7 +113,7 @@ class FusionNode : public rclcpp::Node /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; - std::vector::SharedPtr> rois_subs_; + std::vector::SharedPtr> rois_subs_; // offsets between cameras and the lidars std::vector input_offset_ms_; @@ -120,7 +122,7 @@ class FusionNode : public rclcpp::Node std::vector is_fused_; std::pair cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; + std::vector> cached_roi_msgs_; std::mutex mutex_cached_msgs_; // output publisher diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index d7de10fed068f..78ae152141a00 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -33,7 +33,8 @@ namespace image_projection_based_fusion { using Label = autoware_auto_perception_msgs::msg::ObjectClassification; -class PointPaintingFusionNode : public FusionNode +class PointPaintingFusionNode +: public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index e54710ad477da..b9471f2f3b78e 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -25,7 +25,8 @@ namespace image_projection_based_fusion const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode -: public FusionNode +: public FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index e11a62c060480..7d7132309fb91 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -29,7 +29,8 @@ namespace image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode : public FusionNode +class RoiDetectedObjectFusionNode +: public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 2b4eb822a9edb..fe685baa0b6e2 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -22,7 +22,7 @@ namespace image_projection_based_fusion { class RoiPointCloudFusionNode -: public FusionNode +: public FusionNode { private: int min_cluster_size_{1}; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..4168c483469ab --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +namespace image_projection_based_fusion +{ +class SegmentPointCloudFusionNode : public FusionNode +{ +private: + rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + std::vector filter_semantic_label_target_; + float filter_distance_threshold_; + /* data */ +public: + explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(PointCloud2 & pointcloud_msg) override; + + void postprocess(PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask, + const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + + bool out_of_scope(const PointCloud2 & filtered_cloud); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..96bf47594bfe8 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 4abd92d7fb063..6bced39bc61ef 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -40,8 +40,8 @@ static double processing_time_ms = 0; namespace image_projection_based_fusion { -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -81,7 +81,7 @@ FusionNode::FusionNode( } input_offset_ms_ = declare_parameter>("input_offset_ms"); - if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) { + if (!input_offset_ms_.empty() && rois_number_ > input_offset_ms_.size()) { throw std::runtime_error("The number of offsets does not match the number of topics."); } @@ -99,9 +99,9 @@ FusionNode::FusionNode( cached_roi_msgs_.resize(rois_number_); is_fused_.resize(rois_number_, false); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - std::function roi_callback = + std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); - rois_subs_.at(roi_i) = this->create_subscription( + rois_subs_.at(roi_i) = this->create_subscription( input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } @@ -147,22 +147,22 @@ FusionNode::FusionNode( filter_scope_maxz_ = declare_parameter("filter_scope_max_z"); } -template -void FusionNode::cameraInfoCallback( +template +void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { camera_info_map_[camera_id] = *input_camera_info_msg; } -template -void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) +template +void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) +template +void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) { if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); @@ -285,9 +285,9 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ } } -template -void FusionNode::roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +template +void FusionNode::roiCallback( + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { stop_watch_ptr_->toc("processing_time", true); @@ -350,14 +350,14 @@ void FusionNode::roiCallback( (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; } -template -void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) +template +void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::timer_callback() +template +void FusionNode::timer_callback() { using std::chrono_literals::operator""ms; timer_->cancel(); @@ -395,8 +395,8 @@ void FusionNode::timer_callback() } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -412,8 +412,8 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const Msg & output_msg) +template +void FusionNode::publish(const Msg & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -421,8 +421,10 @@ void FusionNode::publish(const Msg & output_msg) pub_ptr_->publish(output_msg); } -template class FusionNode; -template class FusionNode; -template class FusionNode; -template class FusionNode; +template class FusionNode; +template class FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; +template class FusionNode; +template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 76b561f677c0f..14783a46ba8b4 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -93,7 +93,8 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("pointpainting_fusion", options) +: FusionNode( + "pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 492f832e30e8b..1c064e4d0f060 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -34,7 +34,8 @@ namespace image_projection_based_fusion { RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_cluster_fusion", options) +: FusionNode( + "roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 333ec7d7ed206..19defc0a1cab0 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -25,7 +25,8 @@ namespace image_projection_based_fusion { RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_detected_object_fusion", options) +: FusionNode( + "roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 233e568ebee0b..421aa3a453451 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -30,7 +30,8 @@ namespace image_projection_based_fusion { RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_pointcloud_fusion", options) +: FusionNode( + "roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..1c9c865b8d21e --- /dev/null +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -0,0 +1,139 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +namespace image_projection_based_fusion +{ +SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("segmentation_pointcloud_fusion", options) +{ + filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); + filter_semantic_label_target_ = + declare_parameter>("filter_semantic_label_target"); +} + +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} + +void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} +void SegmentPointCloudFusionNode::fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, + [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, + __attribute__((unused)) PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + cv_bridge::CvImagePtr in_image_ptr; + try { + in_image_ptr = cv_bridge::toCvCopy( + std::make_shared(input_mask), input_mask.encoding); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception:%s", e.what()); + return; + } + + cv::Mat mask = in_image_ptr->image; + if (mask.cols == 0 || mask.rows == 0) { + return; + } + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0, + 0.0, 1.0; + geometry_msgs::msg::TransformStamped transform_stamped; + // transform pointcloud from frame id to camera optical frame id + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_mask.header.frame_id, input_pointcloud_msg.header.frame_id, + input_pointcloud_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + PointCloud output_cloud; + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + // skip filtering pointcloud behind the camera or too far from camera + if (*iter_z <= 0.0 || *iter_z > filter_distance_threshold_) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + bool is_inside_image = + normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width && + normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height; + if (!is_inside_image) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + // skip filtering pointcloud where semantic id out of the defined list + uint8_t semantic_id = mask.at( + static_cast(normalized_projected_point.y()), + static_cast(normalized_projected_point.x())); + if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + if (!filter_semantic_label_target_.at(semantic_id)) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + + pcl::toROSMsg(output_cloud, output_pointcloud_msg); + output_pointcloud_msg.header = input_pointcloud_msg.header; +} + +bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const PointCloud2 & filtered_cloud) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::SegmentPointCloudFusionNode) From bbde223b1998c6c72a3d1e8d0ededea1276e7cf6 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 10 Jan 2024 10:01:55 +0900 Subject: [PATCH 015/154] docs(start_planner): update explanation about start pose candidate's priority (#6003) * add explanation about start pose candidate priority Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: Kosuke Takeuchi --- .../README.md | 46 +++ .../images/priority_order.drawio.svg | 319 ++++++++++++++++++ 2 files changed, 365 insertions(+) create mode 100644 planning/behavior_path_start_planner_module/images/priority_order.drawio.svg diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 1a730212f828b..f4aa3936bee56 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -243,6 +243,52 @@ If a safe path cannot be generated from the current position, search backwards f [pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) +### **search priority** + +If a safe path with sufficient clearance for static obstacles cannot be generated forward, a backward search from the vehicle's current position is conducted to locate a suitable start point for a pull out path generation. + +During this backward search, different policies can be applied based on `search_priority` parameters: + +Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move. +Opting for `short_back_distance` aims to find a location with the least possible backward movement. + +![priority_order](./images/priority_order.drawio.svg) + +`PriorityOrder` is defined as a vector of pairs, where each element consists of a `size_t` index representing a start pose candidate index, and the planner type. The PriorityOrder vector is processed sequentially from the beginning, meaning that the pairs listed at the top of the vector are given priority in the selection process for pull out path generation. + +#### `efficient_path` + +When `search_priority` is set to `efficient_path` and the preference is for prioritizing `shift_pull_out`, the `PriorityOrder` array is populated in such a way that `shift_pull_out` is grouped together for all start pose candidates before moving on to the next planner type. This prioritization is reflected in the order of the array, with `shift_pull_out` being listed before geometric_pull_out. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 1 | shift_pull_out | +| ... | ... | +| N | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | geometric_pull_out | + +This approach prioritizes trying all candidates with `shift_pull_out` before proceeding to `geometric_pull_out`, which may be efficient in situations where `shift_pull_out` is likely to be appropriate. + +#### `short_back_distance` + +For `search_priority` set to `short_back_distance`, the array alternates between planner types for each start pose candidate, which can minimize the distance the vehicle needs to move backward if the earlier candidates are successful. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | shift_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | shift_pull_out | +| N | geometric_pull_out | + +This ordering is beneficial when the priority is to minimize the backward distance traveled, giving an equal chance for each planner to succeed at the closest possible starting position. + ### **parameters for backward pull out start point search** | Name | Unit | Type | Description | Default value | diff --git a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg new file mode 100644 index 0000000000000..ab48c30b6fdae --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg @@ -0,0 +1,319 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + (0, shift_pull_out) +
+ + (1, shift_pull_out) +
+ (2, shift_pull_out) +
+ (3, shift_pull_out) +
+ (4, shift_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (0, + geometric_pull_out + ) +
+ (1, + geometric_pull_out + ) +
+ (2, + geometric_pull_out + ) +
+ (3, + geometric_pull_out + ) +
+ (4, + geometric_pull_out + ) +
+ ︙ +
+ (N, + geometric_pull_out + ) + +
+ +
+
+ + +
+
backward_search_resolution
+
+ + +
+
geometric pull out path
+
+ + +
+
start pose candidate
+
+ + +
+
idx: 0
+
+ + +
+
idx: 1
+
+ + +
+
idx: 2
+
+ + +
+
idx: 3
+
+ + +
+
idx: 4
+
+ + +
+
shift pull out path
+
+ + +
+
+ (idx, planner_type):= +
+ (3, geometric_pull_out) +
+
+ + +
+
(3, shift_pull_out)
+
+ + +
+
idx: N
+
+ + +
+
+ + efficient_path +
+ +
+
+ + +
+
+ (0, shift_pull_out) +
+ (0, geometric_pull_out) +
+ (1, shift_pull_out) +
+ (1, geometric_pull_out) +
+ (2, shift_pull_out) +
+ (2, geometric_pull_out) +
+ (3, shift_pull_out) +
+ (3, geometric_pull_out) +
+ (4, shift_pull_out) +
+ (4, geometric_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (N, geometric_pull_out) + +
+ +
+
+ + +
+
+ + short_back_distance +
+ +
+
+ + +
+
high priority
+
+ + +
+
low priority
+
+ + From 2a6c16b26db311ea5856691db54abf4b7ae670d6 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Wed, 10 Jan 2024 11:30:54 +0900 Subject: [PATCH 016/154] feat(behavior_velocity_planner): add new 'dynamic_obstacle_stop' module (#5835) Signed-off-by: Maxime CLEMENT --- .../behavior_planning.launch.xml | 7 + .../CMakeLists.txt | 12 + .../README.md | 85 +++++++ .../config/dynamic_obstacle_stop.param.yaml | 10 + .../DynamicObstacleStop-Collision.drawio.svg | 236 ++++++++++++++++++ .../DynamicObstacleStop-Filtering.drawio.svg | 156 ++++++++++++ ...amicObstacleStop-ImmediatePaths.drawio.svg | 103 ++++++++ .../DynamicObstacleStop-StopPoint.drawio.svg | 98 ++++++++ .../docs/dynamic_obstacle_stop_rviz.png | Bin 0 -> 157888 bytes .../package.xml | 36 +++ .../plugins.xml | 3 + .../src/collision.cpp | 71 ++++++ .../src/collision.hpp | 39 +++ .../src/debug.cpp | 87 +++++++ .../src/debug.hpp | 40 +++ .../src/footprint.cpp | 84 +++++++ .../src/footprint.hpp | 55 ++++ .../src/manager.cpp | 73 ++++++ .../src/manager.hpp | 58 +++++ .../src/object_filtering.cpp | 71 ++++++ .../src/object_filtering.hpp | 39 +++ .../src/scene_dynamic_obstacle_stop.cpp | 185 ++++++++++++++ .../src/scene_dynamic_obstacle_stop.hpp | 62 +++++ .../src/types.hpp | 80 ++++++ .../behavior_velocity_planner.launch.xml | 2 + 25 files changed, 1692 insertions(+) create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/README.md create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp create mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index b36b40ed6a980..dd1c0604bc86f 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -42,6 +42,7 @@ + @@ -176,6 +177,11 @@ value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::NoDrivableLaneModulePlugin, '")" if="$(var launch_no_drivable_lane_module)" /> + @@ -255,6 +261,7 @@ + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt new file mode 100644 index 0000000000000..20f4b3aa8f4d3 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_dynamic_obstacle_stop_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md new file mode 100644 index 0000000000000..8aa3415c3f329 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md @@ -0,0 +1,85 @@ +## Dynamic Obstacle Stop + +### Role + +`dynamic_obstacle_stop` is a module that stops the ego vehicle from entering the _immediate_ path of a dynamic object. + +The _immediate_ path of an object is the area that the object would traverse during a given time horizon, assuming constant velocity and heading. + +![rviz example](docs/dynamic_obstacle_stop_rviz.png) + +### Activation Timing + +This module is activated if the launch parameter `launch_dynamic_obstacle_stop_module` is set to true in the behavior planning launch file. + +### Inner-workings / Algorithms + +The module insert a stop point where the ego path collides with the immediate path of an object. +The overall module flow can be summarized with the following 4 steps. + +1. Filter dynamic objects. +2. Calculate immediate path rectangles of the dynamic objects. +3. Find earliest collision where ego collides with an immediate path rectangle. +4. Insert stop point before the collision. + +In addition to these 4 steps, 2 mechanisms are in place to make the stop point of this module more stable: an hysteresis and a decision duration buffer. + +The `hysteresis` parameter is used when a stop point was already being inserted in the previous iteration +and it increases the range where dynamic objects are considered close enough to the ego path to be used by the module. + +The `decision_duration_buffer` parameter defines the duration when the module will keep inserted the previous stop point, even after no collisions were found. + +#### Filter dynamic objects + +![filtering example](docs/DynamicObstacleStop-Filtering.drawio.svg) + +An object is considered by the module only if it meets all of the following conditions: + +- it is a vehicle (pedestrians are ignored); +- it is moving at a velocity higher than defined by the `minimum_object_velocity` parameter; +- it is not too close to the current position of the ego vehicle; +- it is close to the ego path. + +For the last condition, +the object is considered close enough if its lateral distance from the ego path is less than the threshold parameter `minimum_object_distance_from_ego_path` plus half the width of ego and of the object (including the `extra_object_width` parameter). +In addition, the value of the `hysteresis` parameter is added to the minimum distance if a stop point was inserted in the previous iteration. + +#### Calculate immediate path rectangles + +![Immediate paths example](docs/DynamicObstacleStop-ImmediatePaths.drawio.svg) + +For each considered object, a rectangle is created representing its _immediate_ path. +The rectangle has the width of the object plus the `extra_object_width` parameter +and its length is the current speed of the object multiplied by the `time_horizon`. + +#### Find earliest collision + +![Earliest collision example](docs/DynamicObstacleStop-Collision.drawio.svg) + +We build the ego path footprints as the set of ego footprint polygons projected on each path point. +We then calculate the intersections between these ego path footprints and the previously calculated immediate path rectangles. +An intersection is ignored if the object is not driving toward ego, i.e., the absolute angle between the object and the path point is larger than $\frac{3 \pi}{4}$. + +The collision point with the lowest arc length when projected on the ego path will be used to calculate the final stop point. + +#### Insert stop point + +![Stop point example](docs/DynamicObstacleStop-StopPoint.drawio.svg) + +Before inserting a stop point, we calculate the range of path arc lengths where it can be inserted. +The minimum is calculated to satisfy the acceleration and jerk constraints of the vehicle. +If a stop point was inserted in the previous iteration of the module, its arc length is used as the maximum. +Finally, +the stop point arc length is calculated to be the arc length of the previously found collision point minus the `stop_distance_buffer` and the ego vehicle longitudinal offset, clamped between the minimum and maximum values. + +### Module Parameters + +| Parameter | Type | Description | +| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------ | +| `extra_object_width` | double | [m] extra width around detected objects | +| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | +| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | +| `time_horizon` | double | [s] time horizon used for collision checks | +| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | +| `decision_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being cancelled | +| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml new file mode 100644 index 0000000000000..14483093e8bb3 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + dynamic_obstacle_stop: # module to stop or before entering the immediate path of a moving object + extra_object_width: 1.0 # [m] extra width around detected objects + minimum_object_velocity: 0.5 # [m/s] objects with a velocity bellow this value are ignored + stop_distance_buffer: 0.5 # [m] extra distance to add between the stop point and the collision point + time_horizon: 5.0 # [s] time horizon used for collision checks + hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection + decision_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being cancelled + minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg new file mode 100644 index 0000000000000..77beb27c20db8 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg @@ -0,0 +1,236 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + + + + + + + + + + + +
+
+
+
abs(angle) < ¾π
+
collisions are ignored
+
+
+
+ + abs(angle) < ¾π... + + + + + + + +
+
+
+ earliest collision point +
+
+
+ + earliest collision point + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg new file mode 100644 index 0000000000000..08638931b599f --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg @@ -0,0 +1,156 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + +
+
+
+ NPC 2 +
+
+
+ + NPC 2 + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + +
+
+
+
NPC 4
+
+
+
+ + NPC 4 + + + + + + + + +
+
+
+ NPC 2 is too far from the path and is ignored +
+
+
+ + NPC 2 is too far from the path and is ig... + + + + + + + + + +
+
+
+ NPC 4 is too close to ego and is ignored +
+
+
+ + NPC 4 is too close to ego and is ignored + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg new file mode 100644 index 0000000000000..3f07c95c7399d --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg @@ -0,0 +1,103 @@ + + + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + + + + + +
+
+
+
object width +
+
extra_object_width
+
+
+
+ + object width +... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg new file mode 100644 index 0000000000000..0b0953cbea265 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg @@ -0,0 +1,98 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + +
+
+
+ ego longitudinal offset + extra_distance_buffer +
+
+
+ + ego longitudinal offset + extra_distance_buffer + + + + + + + +
+
+
+ stop point +
+
+
+ + stop point + + + + + + +
+
+
+ collision point +
+
+
+ + collision... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png new file mode 100644 index 0000000000000000000000000000000000000000..169cb40875d2dc5172fe2c7cad01dd5bb66990f5 GIT binary patch literal 157888 zcmYg%byU;;_x~tKVT6EmcZf8M?i_+N4h0G6W^_w~NJ)(@C6tgFDcwlt=m0D#5}0HE?=p(2kgbO)Xw->{vO z4P5~M{LX(blw?kPN&tWkpdv5x77E#K_KSJf{icT5bl}$w zQxCPVgVn-kxj|W5+gSB(+mAi@A3@~isCGgT(=!-Cv*=isXd`A;&%}>u6{mX5mPI_? zv&DJ5idlmVj+&}-)tafbRH;Y3h$3M>iF{BOYEntCJERRiI)%s z@;NNk$*yzIU2L=!a#+9;bVN-b+FV$m0+1-YNht)q3v1_Ic&6q38E6)Bdj7>AtN?=j zbY|~Oh(`}-Tl7TuZI9A>Rhag2iXF?21mOg-NZkUoNV>MSORQ9VwqIVY&&e7sSqm7I zrzG88YItF-6w}_usyPN6JzZC~HrtjxbajVM!MrZwo~{CBYh6#1by&k*1NIlIuX@KA zwQ-hrq~3zgeQ)4Nw;sRkmRv3u40#M68{**?WVCn5V~Gfmx+wv31~nO zJ-SDH!Y{}x)6&vrXJ>QQBRq@YD)r}`ty2%&rEY6&k5jyvj~UrzUef1v4n!rb%-{tr zxOY0;ySZ_z@pC(Mku>~_$$o(nLeEfOM@I*0osg_j9ZE4cJ0))=$k2!KClR5ckr77} zD!>J2qa0s%j^5+9EY^^vj%s-4&)0=VU;psN@O6{_s&VR7aZV94hJNQ7C@p3G`Y|u$ zY+Ib{Z={_4!~Q;(8v|#@DHn8juL<=-`s=Xe`K$BVh3Y^%8U&YuWRr)cDj@&>dJAGw z004e^jzHK^0j7wWR?l&7fWhN&FsOXW{57v3z1hL;Y^7FsS4a6_iG07a}~ zh+2QuI^QYYC5jMB4ui!UQ+V??RO#wjDq331z@42>M4y4`A%LHb2+2oR2M7E$2noh# z296#3Nrb9^bWT+P2jrx2!yq!ElRPygrQrOM`HeX-Q^j1P`ySB8ky9SQtYjLY@k&o6 ziwm-M8432yV~disB$o)uIkflNLhXLjgE7Vq$ojcW^!Jlc#sa?QQZ?_Z5dN^X{b=o~{y2oVY=Lel5>hWxpVb@i9!{>t1Z~jIOSm^fi-ISUP^MF@!^S z`mD}Y@3||@2ew%z%a?=Qm|HKyj#Cr|Sy9_&T5kkP-Q*gcgdY#?=B3zpTVHGuD?OdY zE45rGY43C=d;?Rd{xT8$*_NT19)<`tbx0>#-(EL2V$Ag-%@hiCHI8-MNKD%h6y!wL z)ptBx9+J}{=xqvdTbO0y7ApOV=#OzKS|`${Rec{n9i1O_L9 z5%TU3-EK0AfQwI9P&hxtr35)HBubsP@gO6k)hwM4L&~|+ns$}lieihJkcSl{4qK=; z@0VaiCxyCv-P=NVc}?a+@E@i-$xkMb`w9h+eKTkkDhsZ+pz+4aWs3r$J`2KhOG!Mm zLus44*gN2OqoY$T8wzA$VVOG+0%VH0tHN3)*`o5SV0nzoqt;QNb;5l~}}o_RDaI@Cqyk<-$G z*?(N*WyZ3nAYV=-%g zjidEDAHyZ%0i6x>Apkh=`T7sLzmXZ=%Ddy zKcq*j^!Ta0Az=a;8b&`Y=EkrFEDXKD>pL;8j0e$4DvovVtOY4ztGIf)yX(I|?@xLP zu>kin7`!hz`LsxBe1&o zA(JX#M3Rp^ezx>h+zDB%O0Ry=7P0Ir{ni%>x$iC*Y`?Od)zjgP7!cN9%FNbyGo`&Z=WQy-CsVovG z)o_-_8#ab$4UkgAX>x|}9`~_k%QrxukM9i<76}rJV2Ko~)0@GFhBFO((Fi)3=nf(` zG%t0%hJe1}R4s(5H-`lggvk&>_I^U%jj)Akxo_;)i_BjrS3ICE)^2s9pYQB>;++J} z6aFC55EWzbwp=i7c`cDmo?r0|pp{}V`yloa3{M&ZT}u?fxu2dxCa*Sx>AgXSd?&?e6uMBLxm~RhedwQXf<)iFsq#(c1CNX% zGaoafS1Qnwt&rXzU`J;sx|=1HGD`f702?yVxS%IScl@On$`~3Od!vr&&)pN<^qXbb zBA$}a;%|*3PQq_~?k3B-!vUYGX8W61YDO!br7;nSst*rWPw-u)<(-t~F0igR%fFyJ zlB6^qnD6w>BQxX;3uh)<`Rkj<69gEwsRfA_`A;gRDdyx+bCBodD!&ybc-LJM{_*m3 zW8k*znSDBj2#S$I-8{6dvf2B>=EtOJ=b;`2;(KYs$}uZHfp6pMY_pwHRq%UZ)*D6} zUud7tUkxU^vDv|AD%O`8W`G#XSf_0B7&C$Ba{_9yk10X&2IpdpZCu=Qe4U9IDeb9y zJ)ZG1hTqF(>vQMMd?$lA9RY*mTbI+BUSU~5oL3ZS7OM)szb|mnEPoWf_E2wN-$xr! z0T2JVh|NvGK-o`RH!LNXj-HmJZ)Rh85r!-z_}V8x zc6E1jB`|hE^~a%_qYY(U_}rGLqnPguj{So8;< zyUXy?6qgDfxH$q8-hAuR*Kw8WEvx$haGVplxhUjP6Off)^%v^y&2WL&9zUl$<}y0Y zM0xW6B5f)g%S)CXWX9VG3UjLKdoPRl_~8ReQZ_c@=6x8u`mzMt_PQ)6;QMD|>`FOH ze@#MPzpsuH8#}LbdGeBN9O?v2uy-dIiiBsvG3B|D8EJYX1E31JhD|?b(fcx zs+OGI9>g*f<2YrhCg9NTFExul`4yIymP)o9@Y5j+NM*~djeo)G73|5k2$>#IU$!7` z(bc{?%bP!~{wt}H7eAtvqTR#em#*4d#ae{|0&d=kvo<@I`g&7%t!3Z;Fg1QWu~j1m zA9z0{oTW~+9t-cap3iHSdiUe0J&k8So_BV3Zb?0Pd9CVoNAGI^yY&Ui9-)bGju)6q z3WGep8qfbCpo;lp17s6nN$m&epd@}_!;H`@8{LxK8f?C}w+4Ls?D zsD1qE_*_YeQ{Byb7}>R@ovy>OR6`HLalZNlU7|O4^b^TKuUEov46}ZQ)#eGrI`BQq zJ#^IVZrYfbkhWrpSc^Sy{an?CTE=3@JZi5M^J~)P1Y$l%45F09%5t>$0-Si3`$=Bf znmE2TyYckzM&##uHk_H$nP>J#ik%*I*UEpT1 zHj}SSw?4Wn^Ki3?bE=+=ad*>AoJ9%>*3suq8+gn4843Df|8D(b?}ufVWFDi??Ypu2 zzUZ^205SY`eEqgsO=z#%rlQPQBmucHEgY?-N^#*n@8NVmemcii7@BtBBX zRb0+9Dw)r-wg%K|ob7P<&Q*&}$AZ$G1XZ6?DYWS%Sr%+`j^M6T+>*!L}Q_K6PGgF>9$5yFpo zY_eFl96>@I%s0^+<5{0~RM6@QB4QWsu%Vs(df3N>xKhAdm=%nme1En- zN>d;T?bqE{YB4DZcnmh_Jrb27f!m#~LvVbM8;L_f=)=PKxPSu=;Sq`ZRdeAldh{}O z`q1rw-S@!na|Mo?%e_}fBy3c2XDlfA4m$bRtzFx}=zXUZDx4K*NrJ>2ko5PCEUk5& z<}yM^Ot?L(bb4UhU)3o+WQ{XW5Q6FZb7;OqOrvFs9^G$TbpgcDr$Jw`Li{hx+eDdJ zc2WWk8K>8WXObg@(YA-kHwTm;;`jict0l(uh?k7yUl&M%vril8axq4Rqx!=VZxa;h z)+H`Jtx1G7-sR!+ke`s)+ehJCBlwqSE%!bIuV{yK3zLNYd#tW%Urbj-;lIzGm1vu< zKX;TaHdHxJ>GYHJwRx2o>W9+J8PKwSzBA4=VRiFOuaWR-T|$C@Iy7-TW((A(Fj^$Z z3o?!L;avAX~cP)?2V}EAMrWW)J_PTCDz647s8XPpUP_rF)?AH-@&q)JGfk3Wx#gVj*p9rvfgEyYxcP+ z)hSk9;V=a|*X~xO=9Hw+QX8`l| zTl8$1D#z90V-82|U(jnYvD?KX|5V&&>E_@q(W5>MM%|N`#KfLB>NOWI!Y&vPC`>^g z>b^I{&BGHH7pJMNzCT;NHBqXg6i;&?&Hz!}bY1O?{EsG>q>hr6XmQU1q6Uy*KTS_8 zuzAPDF&$5dL~FlGVg8&)`*DW^bF#(-pHK0GE`wb3hw4Wi$Ht%vob(RszfB71Rn?<7 z*jXRG%a7->D!tO^WNuG~sIOT>DZ4VQtY{tYNB?4`X!@Jz7a@GMz2o!k)^V{h@)F^F zTWedVc)oCsP**A85ssBp@LJh#-N4xG(^z{2^5Z$$VRFmPV`0ms{nrj}>+TP}VmbGb z)t!m{gl}BuuyK*vJ->TWzE?AEzb|5b<>p}X?oz-k`3_7t)*5Zj{3I)36*k9tdE4Oyrrk*EU(U3tHQGt}iA11=f-2TF zvRH|UiRFiU{sJ@Q1}J=}!ctNQhdRnwMTr*VMm@_@`oOSFpZbgVjZcx@I*j0{&1 z=pfX*jx5b7P)uR*X{wBfTSoeoZ)lp!l~boFzFw!Q`-?S?lfo6spWNX|S1zioMigLE zl8Y7XNiQ&>j0}6Ei+z#ECIqHNF-DA{K>J zMGh-gkufwdk)0uVo4kx_>|5!sf-0g7IF5Dg0lHc5WD(o^F*nqbDx6qPeOw-+y@M&eXt)3U$s`p(X7rVf}}6%l;P3O@K%#s_w$b>T=SQ+gd|kI~0h zbsk(05uq<5kXI{XX=Sx{)0v)boIGmF5)uE)_L_a)7fa(UTV{fr`PH`nJCZ=ZofKkaiJ{lRPRf)=qn!`0*V6X#e{QUp2$qndT92zRWu7r3JHA7Vq z;r8!8ytyH2v|eT-r(MYSda^4!`P;~?w!v@K3^LQa^r{YJLe#o`HJ#BheT9H+?QNjE z|M=+xGMZ9|XJOF(bUdDlefqy@r($o1$mum;x-2rJ7=M@<=N_YAD`ciFN6OViz zE}#MI&)0zmd&V|k$V$VIeZS5caGp;aIaOv+`(i>Q!i}X8&hmzF2@k3h?}$-+W{fNW z?Hz*9CD!J=I`5P1#HC-JD%g zI4~HAKba*Sa&uzyDRr}6Mj;E=Tzpa#I?a3jjscJqpMfgZboO`naiIHZ(lVxJGmuBJ0wHK*Xh1D7YC)Ytu?A4THv}V()yBb-ch+a z(vtSR9{PgDHRRe~yOh@*L}2j=jpg0b0;$z1$BG@CvRdHoIF{QPW|G# zmeC`eZEPS?=i!T};6B?@bn@MM?4AmHg4z$g;Tgei61UTZrIK|<*3 zlQpq0g%87A&tQpjEni&gzwG%05gB|fd@E}HSCuDi8bjo#r$|&@n@sK4*cf@fNByEh zI1mC|!2lq+R5LA{7oi12<--Hdku?1NTx3O2(dK3`Th#Xv17)Yr4Co)Fl%$k;@{b~% zo;lM6y`q#2T#>LRPR^_>h&gxe`)P3h=jk?AO)A4{?Jl78;vhiKd*rj@Ltk%v3wtsE z@P2w&#TDM4ND#J>eb>9Db41~HFmojMXq;qszku1K>%c_&blr1xxOjKq7;xut`8VD|CiKzV6HkIyW9`Y^BWSQ{w2!RMK#@zr!C_xwkjD zOj{mzvtbi*aLpH~mCe`PN7Pa`#XaE1u9L(7Uu?^{?u;0iUm~};r(O0dOQGhZvDqLz zwZkx5qRboHk?Wu9mN}#4w6fN44C7%W4&W(kwlE zW~gY&mI$2e1)-adrDf5f{cRH z-bDUSxYT$#y_edNdHUEO{gZ4ty9<5j@p1^kZqmBVy>z*S7Tuj|HkiTR7>YYI zMNExr=gaKJ+EyuWsoA41HG!3>LBwBc4&?$7Z|eubOy)$!&Yb*PZ86iHTpHzJ#$-S}wf;T7mdvA81?Uln4J0X;2Oh*nzlHYR zxchLqIEUYbD-;`KL4TCy2hlv3t5u5vb-!^N1s3o!(*4MqGUP6T7EtB(K) zAuqc^GRb%g_F*5_a}ojnf}r1Z5lVWS|6eejE>ja4(~e1RPiZVjHa`n%#VMfW#HPT@ zw@jccSaTcrhE@y9^%3Nx_R#=!33qQdBbcyk4oDDnqamhwYih%;^epMAj7Y8mDxK1N zu1*gAbCl$U`J!3d(Ma?lX7l46oaBbG--inY5a{1GKuONna0tzM=Z)5QC*Zqou$9vt z{YkeWRp&a6ql8LE{FY;+g>|G1KLS5))PtzMb^8uWr9)9grshD0FLol}K>%1%y!|oH8&=!q?{Gx*pM)mxoBpA^5Zj1w(Go|o+cyAv{_5Pqs z_}elCGQm#MH^F@kk^E15*~{_d?g7G)m*9#42*r3DJGvSV1*h2N*M^1$#rysT;o&i;ZxM;Y?T-`RMN~1hH|_0wv>?{MA)wB{ zpr#}-hx7=fysQpy#w`9Xg3LyB)*c`3nmu%+qkky3Zb`JTF8M|9L)p;N zPv<;cK67FzXA4ht$ni=LTVgEysNz!l?TUHiGc-KhO)iI}V0H8fCHs%yIH^ize>mj| z-^HOE)|ZwY5W`y&-tX7iO{6@!WtxrbRMqu&27c!HxVMMllF4G_{mvtROhPnv zAN4de98VqV0K(R#pZ(JstJ#E^I zC*6TB*&SQt#6Jb|$|WRblA9Z*Y-%`rcuc|Nph4;VTF%$#nIxCTnJ4R;Z7<#?P|l}# z_L_QwBOdBfr0(~Yiwnp~zu(Fwgag$Z!i)F&hO7Wi9LQc$nN|l|O@BYeDDp>C9uOTJt@-So!l~6)_QYVhz-(5cg%=q8TY2phwF|%C2bq|)xEu#L< z3jm|?$}t{JUCOvcO0uU;O8(~>u20+$&B5NNOa?!CR=*5@_hC%jjki8%SKBbd|$A9UJb)Q6E(+CxyJaGpI{b-iw7njv;*J#K|P%K|FYpA!oEbA#(t zYsz_io!@SnJ0uchL#_V>b3lMp@|r!8H$#sD6)OtkCy17cpT%*5}2G|)>W6z&U#--4~7SvS0aUM~5Gr#TUe<4LQTB$gf ztd0n>3KeFH<1B%_H%*H|8`n3oAO{gsefhcOmLelJ$D$dIUlh>%1 zx;Nq{B*Ob9nWOzVlOBA|c~Va-Z#fEydCklEeb+}+(_;$O*Jz&R^Cexj-Qv=pfp*lp z1lpVA3TjYBS2gB!{>O@#9u^kVJ(2F(+8PCalM{qhZN)UOVzBw~cR>j!<7r3VC`yeB zHggRi72|O*eBsA3)TAU9tfE94v9@xGY*)N6DvB&4@P1r&4`&RrKsZ&LgrOlX%JrYn zBhwj*ei5aoQGuYqSZERQWb)f@Wm!>*C>QO#DaNR^V7C1H{9?6Ro139uSmf1-B#(7K zJ;bTNf+Kbx-lj0%pc8HSjC2QjO<>kl(4g)?4J}fWN0q)Jy4xDq+JS!BnbYd$y}u{H z4%!j^eI`s@Dau>L=oT>eEqgUm{hRFZ28g*K^AbVyEz!9MozGM2EU8(so6>dl7UoCD ziyx1B%bs*~FdvRNVJ`+frkm`k;-U?>KZ!d;%9P#6e7Do_clLT>|8oVKRC1wKtAdMN)E^C&tB@nqbDt9B>jBp1yEC}r?mWUOz#zwux)ERa=j0a%*B7g7KT?1!)>|dnD^j9*EA}8O0xFR} zGM3~1v;JSI58PQOf`Wp6<2{Lnjc*vC;?{W?!@8gAcF5VCJ=di&hFBz|t%?Bc7ds-( ziOc0w&!Qq#*t?bB*FZ+r&>Zf3`GH(-54=M5DOgS?KMw?r$%W;)sON%2pJzUz!O8xQ zw*%`#-|jyhVw$|`(Ku9}TK*$DquVMSWcAyv?DorP1>9VYWel5s4KPp#@yEx1?UM0s z={MUS%Y?WrPqScer`3dw$=H(h`=~0_y^ZGF-t_cz$zH<&LpNUW!pR)jxKNzb+}yB>>$feXCc5A{nc7*5*;5tU;Xv zbp;(y)avodA2-o*uBk`s*NKa2C92zjS`0C!H*HdlVnUC((NYpkmS`2J0RtV*EoND9 zo(Gwrx}V3-;LaEoCPAF~mym|4gZ`U?EnbHvsPlZ+&FWxBGcN8GkKJ4|(Er%yiS~X> zD)4E~@0Fj;_;j4Z?BIbDXr)WIBEE6O`=*t~j}id)Sb1HM>L@_rx#+InRDC}G10SNg z+bN@Lx}H>u@mlzk*B9!Y{!UyS?=rq5=`Mxfiq5u2IqgoA zBDMKys}XPi-$vXu)d5eBj7k;p$?3yPy=AI?KULR|9EjipeM;%3`LLCc{EyP(@^ONci0}F|{=bi`kHu@(!cToS zHvxQ7PrRGixBK@}pPz05!hBGcv)jX;Xj)jxun#fn$(9$zS&x#fDx6n_n^2bJQvJKb z%Jp!G`T13vsZO*-#_cF74ZMT&l6^G2Z>g#?UCpl2-$;&LbP|KVWbT8IJOtDUi7t@+ z@bOr@KS{u8svR=nJ1qkq)iB*PTFdLY64nCZ-)?iT z$E&8!jEJB*2?*TW@`mH-Zszo~CnfbMyvftw&a0sTzF-2uugtXO;YDk!$VyzT?L6D! zMQuy5zf{j7D;=cTN$V(b)`};N(Awq(7jb*^r;#KQsK=mWkRD6wIlB`Q;;>41 zAufMG(b^^^NDdtOZ4Livdthd^zA&qLhK-T?8NRzqN3QmWV-D1E>M4!~%Mno*?K@!evZ=6u{t` z>b|{hyYV1@+O{+9?%<*3xB8s1LE4X<$^}jYr2@`{Lbw3r4PR^s@6|5Wu5BtD67~r} z671NClu|%9Q{~2EXGq2LBnl{-qlQXJsn}rsAd6L9UHz}>m4;lP*wrXry3#Sa*<-a2 zB&h3sYQ8VW;#obLD|)mR%0M#I)D=VNlnN_cxbNP`cGYIWPS@Ah4;Pz|(!!)e-5V<# z8=HsQ3)tgNy1Ph;B0@qPP`A;i-EXsFdh@!8Eo3^pVa@L2H{YUz-?*%FV30(8q~E$= zE=sJgGEk-V;}2_BQPH|JaX9=#BYBEwZSh}|GVnb)EfUXeU90FK=3RR{trw$luTY1i z(Tbw+qGh5(@t4B^N)-2+q}N6cXEjN)6r5^se0cFo%t5w zhqI^o^uj0E58NJ-@8N5Y&2D%AGO}Tmwv@hi2WPk$2K7)U1gvhc)&9OGmD2fu4NzvE zC2}=wst^`@U_PwFn1IDpoX1f02M+v}xp%bG+%#rAI5Lu&_0lSrR`y$F=DG|2bX%X{ z$V+V?-@&H^E=|zO1W))HHBy~W@H>1O92~rAxsniI*hkW*d2BzM8R!9265em8RU1?0 zaNzJso3uiKM5Gd5ob5kXW^#kHggECXEglMF45`Xt$TSBU>A1RTyE)!XR4*LP*Hv!u z2qyPplT=N9RzS+-w6wxQEb^bOQ z*K_f%c#WVuU}eYsX$PUk9O$#WI$r_Fz=WUeQZKUFUvl$GiEG=tSUP-Mzh$iu^IfKW zIIH8ze%OnRf84t8L~Xkzd$fE=^*82Nq+3GXk>4^Jt%%Ed?>`0FtAgwgM>8i%r5ONd zo{98)W@Wvw3E}UQr_&FEb4*621aG$9~a4b@3X>A;z(^!UgnK6~r2#&~?w))WWS`%Q>#G@PkIaxZx-CK z$dGS}ZDUCJn38ZN3(&?ATB1v(Esn=hBtizyK!ojNc3QklbR|z);%mqVaKv`aOZ%J z!*pZ_l-+JKhaJ6#xYtQ>xliH_*1{gJTUvZ94HcA<8aXOQq8S4b=ey6A)w9+Dx8$oY zBr~L3RWCTSuIljMRmM8}T+n==EYen3)3TjM_}l~w#pnI1kxb!S-J4=1CHn zA+=fh_ghFkOL9``kvHyB6@8Cy7Vxc94D?}L#WfLHo&Wp80BIGk5a7*D+$v{6x1|7k8z=+o&)6o^ymV`JH+3mSV&@Egn9GH@lLKJJ$yYvDh)-|hy zLUB>ELb5I(Ai4^{c`hiJhG)B6{j&(aHuTj|4&;n*LQ55F-LA9ZYmufb5Erb9A2qHVBMNg=Yp&GY*ij~g7>;ptruZZ%dW~y~u18yk5$o%kIU}YQB!z~U=v`4K*A5s4o(yF66DMIYL+H(mY?^D1; z5^Ul=r!T5G_1vGhLC0c>s9;TYuG2AFFi2c>srq%ghdTX75(1f`tW;C~mjL9GrSZT1 z0EozPE}CgqL*GVE&`D_Emq^@r=5lvc$9C83q7&)1R6H`ZaQTrm@Nc@<1d$oCq1HwR zXgXOdrFz(X9*o0nrs}MG{h%Tchk5{T6Swn5)eGBbmJUMdt46Nu7wlq`TsyUwawvDw zeBHo~?QJ%iREwK1USAYs`dCkxj4azS6oITlU8-jSE06m0NviA6{V~<{@{3X!Hy@2r zRZEb>8ZbE3fdV`}pac#WAa$KOH(DIq_{jDx%ct{voY?;~F}u^|h3vPitgbpPq}RPv zB#q1l9<`w5Gft*JSZ1F&?j#7pAFliQZ0qvK(SpUB1$4yjsJxmSlP-+S`4#{tjSBWb zT231>m(MI_@N(I0G(H=Pd&}Zu)@IYwWDJ=MKfoya(0SV(*f7R=e0GvMs3|GO>-zH8 zu6^00HYCb2us2qoYk9$Zy2o#C_vMHwL(%(o_Ox32_g>i_3d!ECx+{DI@9$U1q1h#h z$F2wiuoj!Jk#^(aIF=6Law~DV80f9l2&@Y&H$IH%dJ*-XtMCAtCF`V$?+M%QByYW2 zJaU+=DrAcy@LfhLpqsLLF01B;`lBYr?o!^AXwqc;XZ^#Saa!aOW|FWhe%9)??&tJ* zqDtYzi1zm>Ju6?Y1z9CFL!qXf)AO~Bdt3BOvL-wwM}4}9?qWh z{^A!B^4K2Dt>WS}ZNdAl9_%R3$U!ENxo+GWG?@1z(z&Ij0eMS)`2$Ll2Ke7eSJRCt zDo=H!u-lt_{CDzfOGQrfv54!umSVRn*;aM0r3kfXt!;OvxU)P^ml1KA*QWaCsDf7rRAjfGhTn+C2wM6d%o2GVw+sg=|mc0+HY9 zx17E}g7?SaqkDXgZxHMc+c8ToY(!g9!7x`Q|2xg1V)OF- z@!*SCj5q_RXD3y?%ozSGuw+yFKfeS$_?0YHCEN_Gw$a8Y!pHZ^VX?8Rs|!h} ztqALmi9CfCrdb!AEwg3FcB-k5w^;SO{T%1paHBR5>*K$&%DG0l1ob{|NhmyvM*L!J zITH1B2uUh5kP3JP7%n%gws0ZyrzdQ1Iazft#E(8ZyMnLfX1{|&*JFhN?_2!*L|nIq zvpgMI&bH_3mamAFQb?!#wrn85%5y?#w(PO@ayccdq5T{j;c9c}6K$6`{_#w6bHxvHh)8{T;z?Xpso zrv{yTT#m65J}yD4l~zH%Z+1S{i%lv%>XX>wlsL!Dj%?1@-|}*$<6_5J3qtT=a?O2(l-u3lR$yFaD>`5xwerh)I^2i0>@wXJvt~7F1P^sjA6qox zngMu}_@>_1vijlHOKcO`)u+bycVxk8An|IDJ%O=E3PxB`awPi6QaF`youkivk;Ex1 zv&s_}Dc)&syQHyg>#8Yk?~~-ol^Rm(iEg+C@6D1sGeS+=Dtp2YS-xhcXJq^^Ql%d` zcpQC@svN_7N_xpyPBpnj#s^`ubN?L(R!VURp9uB8EgoLLr1kyD78T6RE=OJ7M_AWZ zGjfA8_fH;d#X!!{g~O(yE{-|TGkE$p;D@-6sE>Auf3&m#_YZa7HzSk^mA(2QAAfoD zkasxbeA5|eX`X>lM*uQ`SZ|Fye+rnxuW zg@3j^6_>H*Ddkfp4xB{?HhiMmk0qs0J3n9kER!QLCO$sH^mB7z>GZ&r;uYd%u5Gk= zp+3HA*w5rEK_hFSAT`cK)k#!U|15n6M^xUVP4jUF?$1Wpu^0ntmkyinaXV%y&o+mm zR9Kka4~0uUETc5a#=5fe(HC$jSuA2=;%yr09IALPY5LG`TK{XL#R@=2x-&93m_s%5 z-9zB7wLqt=`JfJnl>HAMI8FAwe^k;HQGa4Khow{VebEIOY_{I&m8|ErGXL) z$|1Ueo6zPm=W4=pz}`rvm){TtJ*H(SZ=q>swv^}XIXLO@_Z`A(>vVO`_V>SWn(eaG z9vue4V<}Sp#!U16X5&nmDGIMlj!5A z?g-LVqUC#7k#)S$@4F1{apr>ekB29JDpQB|;twB;A~>SV{QdpIBz^UC#uJfBr|bN{ z*h>buq86h-K{nzHGMxwaCRUj{xp}tQ@uY9;f3l#U zv}K?#Y;NN^y_uFcI&}G5xsX01AXN1@rvXg83_)xK9(LxkMfZMP7+_E6fVVh_e%Q>> zDq>(lWo{TPzGcoV#&4dcKT{1(d(EJ=^kLP*#V9wAt-lJ7^9ERpvw(jViS$)2I*eo8 zKQC~)WKkLIVNQdhH3-UK7DIz6!tjS_L^*?al;1M`Pfnt7rS?-{5i5FcI4(v3XF7OR zr(TWcixBts(#)YpSN-y$aLjtaefr%t(a@{k^9Q48UU>8-^-e)|>ODwd=T%}@s3nhe z!zc9n(AOI6+B6RET*%@`w zE;6syllO7yA<@jOR(zYt8OVf$1f-^PsJ_2DrI={$mE1txT^9jF1*mIicoC3tYS5yQ zBR6WKu&h?d`%fhrdTpqxxF`R;*tSxnq4hV)s=CKXJi&PrJc|6P8Pmy{e3b*SE zkciq&J3>e{v}tpO3ShTnSGjj}tUZW?Wf%HJ4y)`0YKP3SZ%7OlJAqC5Cdj{_@meQ* zn~|R0fmW>nl>e=_!m{#v*YShqvheGT2FfUXbX$QRDS8rnH-Xl75L}ED7m5Vc4j0WE zu!tN~%&3A{IgT|=lqa~2M9pLX>~)yNS$Mj06Z)Jw>)%vl<6@3^Oau>riC&`;Y>p$- z*Z00C^LRb8ifafiiS&@~?Nwf=)-C)>Sr;f}6_Y+Eq(M4K#!m9=>ExB3*Ts%D^Wmh9 z*ulg-9|rUxZ&tPkW!W&`+@*uB@i@X%T5R=GpCm3^kCS}*`OyTUQ2 zP`OWp^j(DGQ-A#{ENoCX-Y4l6HR(6v43tmUGn99v>!_>w!2pr4)tWO(4>gCHl!EY9 z`-TP!SsyP7>sqh$$>K@(@B=(oB}OXeL6?sH8XU;!x@N%8h6Ve@Eb33!o84$e+jZ=4 z>Z>D(hrbC@vM1LKZ0`wFwAh zU38RgeJ|*sH-aV&alv&@DWYlHLx5dA4uA-*)78V38an{+uaLE@pQ|asU-tDi9X!;z zx#fu8p)ajhSt%i{9jO^Xq`L!q6Rjt6RO9&9E9oAggdF!UewedEcnapZr_vufvlz?5 zBGCuR;e(%$$L?Ux)AL3+umdLHqnRb9pmrY`oB}W|EYWY$IT7S7lX+F@Mf6;_VX%yZ z#LiFbniG5%f};o)4*hX&INp?E+M)m4M}aSa0tRPQs7(H8-SGbIYWrsm;n&c18V@gn z$Zc22lSnY=YQYr>;^U)VdUu=$04Tfx-R)OHzi1{3K>RO~aR0hchmB{j@E$E*Rj3^x zC+{FMi|*bZ|J)uui2^eB7M?r~GCSAAQ9MlQB};q8G92mWhciWMEn|~VxrRs9n8gnf z1H$J@bfHW95#LFM z1u`{%eLXXT4O0!*eDfy+?7K>Qo#^~AQce?{!ZyAv3Ge$M&25*V_9ZF=)G-+Vmd+y%*eCiPDC0x)aL!*V!e#v_^FfU0SzaAMhBkN_$V^v`L zx?a%I@PiI4%FD+W=J?&KJOmj_Sw+PyV*XKjJQk&ENV`hd`~2w;#Y%V^He^>tiKa{z zAP2lNBtIl3O^V)-mxIsc?Alo4U+gfQppGaYs#L~67mp$$P*ql?NX3PlU@Z)MN3_CQ zMH1I`bhOvB$(&^b0JL{XHD3#0KEeP0jy~)j0z2vn>o_FGNOWoJ1!d8d?hyyMK#6U; zjT+L*9CncLXD)Z|QQ+eENHEBms=f!Sz4Z~4>Hg&Ijt{Wj;uXIqpKJLC&zku((l1R{0*#Di9Zs@aL8plm_n@^{p1$opM{lb2 z+x)U$|QH&1Ap$d_QCr#HsL)DtRHPgti1RIwUxP-#_9h zwqi>xOv?jsbf}E-jv16GPp_opd`%{pgrG4-FCvYeyG#to$PoB4ZSo;s4{#Q7eJ*qg z49tcmd@_e{KK`6cww^#tmgh~!70)!&7{6zhkXrKNUUWdR(lp!fl3_z?N^RwZ4V!VujXphFC@=aI4tC3h+O& zO9u}6F>MZ%iN4Pf3o-_lqWn5(I3W`UPRrydYS!YL-lWnZt2YtK2So=8(OeF;bbuvF zl1A)8HBlZERQ`@7;d*JWGbT=F<#A^u#OLBFcWEBbo6n(winD3x^E-MLRglwyVy#ze(BM%ez}{Wz1) z2yZ>J6L<4+&gsBwF^IIef23h2Z>{Z;^V2_=$DEpkvFy@B@V9i%#Bfjx|Kz)W_qQls z9liELq+RRx;#U{>rhGOe>u(0aS7~SS5+Ru%+-kCs#qX#M6QT^S;7pzp&$}Da!hhQW zNK^6nOOQc*&D$;AV3VDROgGEdD(&uweRXwYSUsBzVssKQpN#wi6KxsJ`FaY8m_+&w;q+_@8w{4DYJHv>Naa!FM?*+Tt%R=^&kt;Cv zIpRxPm}czUJ6zin6voTmQKNp3=;^agcyXQcgqNL#9qeW6>@A~OrhhZ#CgA)%J1e{F z1!9u+)^|pUB=KJt__3w}s8mU|NLGQnFKXc#5~@v#nlnL|6lTLvYnHcJ%Jinb?{Wa3vO@PW=Sv6kIOnv_i8N+s!Y*<4A}Hr zaliO3!Q(6?)f%(+D-H#((Wp)x7L5aMEld&24DT?FI6n?zX9Eb#QTz75h};L&&pL(U z2EJol?W|ifD357i@A%6t0m)=3i6%xgGzom%;4Pp}8PW#ptxRH|7l$ToLRi*38>J}S zBv88e>?u!;Uy_fVo&U9Yc?Q!^5s}u>l3smey60TF^P6Fj$Xh8bCI*X(e$r2_{^UgT z+FGDi)uPnJs_wJC>msL!h(o2Z3eOTEKw@?VCUbtjv9UZ*ZQ*8 zllD`EoV%IcAOCaCe!ns11^Khyw(hef{_f$QeL19~5>t*q7dARq0>K9D$jJ2c(19Kj zgYHxoo$Q~xC%iVtCfXl1>r^$55tRNK~= zmy|VU;b6YJnZ@p)?vm4yt}xkD1JZ}^|IY7k7WgK@eRD)(=3UoIa}YQLmn1mQLv@U|0S1W;)zMcRr-$ z0$FYVAx6eOnFl0M^0U>`83LFmjcaI8sN$Zfz}i+ceq+QMqO9O&&2}IU2Uuq>w1&cp zROkvr8HaGaS`gol2%1WajErd6{`3y?@kx~l%5B3E@{TY@RVW-LwHo^(40(`}LBCbf zZL^40(>Gy20i{*le@&DMXU7$9C0Z}g(i)Ee?7r{9pZBzSfDgwEw;xs$&}^H*3~JP| z;i=iiV>5sTLP|bcw;cnt(!9Q~;PVxj*yqg71_@T9;s!9JP8V`?dP;6o7XKrQ)S^{iQf}bc_w4Ak<)It|2VRb~U#D6b;nv;)v&x zt;9CvG5=4|*46X#*vLT{Ghk%qOiHxu5$!UqYp5f2bvX9jD^&uH%?m2@d|b4l5agsU z+CmsLpG4fnrc`Mz)5rWn!4$V5s2IjVNwPsb3brbkvrGZV7*ug2^c-}qf_Ls(fo@DS zD&{eT-wyxGJz|pw;AP1MfEV#7H1ta$FQFDz{a}vujnS#3W=};0T<<~4F#>-;yOk1| zU#Ni#8qI;;8?5YmxJh7VBCvt@&o*Hr3d#>RcZ6UP9l$I{c$|*pH{(y&f?vrD6qmVw zqayyRRb{#+op>dS)9vQJQnPp)NTlED5xGc_)v@bIU-o|a__1$6i)sSnGv%&ocyqhQ zMR2lhoUsn|CeOdSOdyLoUqiU0{S%%Sj`m6!g5NXBgP|n9VckjCc8XMKnNRxmD1;u2 zJu;(KaAr#SEOFlNcM^#>^nZpTzsJYxQt5O{kbIclu9AX3HpY4zW~0A(Qgs zU%;;FD7a@Q5-9&9-Y?Q2uF`q9CnAeBEfA9jM-l$QEUBe9pt=itVbgK zbJwb%T*Z{$QcIXdYg_9KV9J%u6Qd>AX~~wYy-zw%kqM~59c9_6$v3$xhfmhJt35;q zPue5SMDh{k;B5Q@xx+&w$japMf*VCLkC3P3OS6@$IH^YZLMHvG!gaf}Q-K>GJEtYE znRGc&>&51AfIoc`a(uD9z%9v0Ch>vfB70PN1gPNAW}}l{>pa0qM8O|lmRsQ ziFrx=il<+Hv}0d}qdZ3tB$To+!xTTd4AXb?L`yX1ty}TnQ8{1>1l-l(TS#7&9K_|k z2nZ1m-wiu1xVF#>rDDq)k_7_t99(%BhH@`j99PtK=~$^`!EKC-`jtmC5#P)H@UVMt zFV$lK8v*UHm>mxe`H&xaRw`}ODYof93_D&mzjvV6bRF|yRn8R@G)A~sz^KZI%On-^ zyE?pIxfZ<}h~To-5dr)hI>L$pZH>KdZMcwyz2*t)BnulEj85mX(i&F0^#qG|09T%t zW(q?P#40e>MQzbf1eDs#_?os0j<_we<^;Q&U%SgG^usOjJou4any%BLOn-EUtgBrJ zPxqm^PJ1h4coI#6gce{!UbsITDcmcpgG7dfBbN_2BO^@+DbRMmjPeIl_n(j&IUh}AK2~57$&mY)JjrI0a9L*VGt4s^!lHuWM%|9rJ zPCYkY@)L8x7G@YB68C_K1duxdy|q3+Cr)jj(~v8PJv!AQVkQjzbM(KQiB4bdeEs;i z*gg9(daf|tF@_adIIB4ypy#c?OkdBa2w?)-e^37@RQQ*1+(8R*L}a+?LhF(64(JmL zc(L=V+ZMVxN2o@Vef1>nD?_VVACQ&yO5dCG8K`aG&ibZ$~H;eWX~Ndw}CoA*;;) zmh(H;G5&>za1@4(D=^A+CySJ-z`$b;M6WHuA?4+^W;;&;rA3SCD}{= zw6vHjCq1X~Ad|G@H z<=xYq=~iLN%A%3``uNfXZSk`6dXwKH<}|lM;tRH~TB;JQgMjopo*($*L+##w)PBq1 zB0#=7n})|RSbj=b77{g;I%N|6!?;3pS*reH;scG~tYt20Ba*z9!<(~*GkB}e{LaYt z0>%gdRBHf^E-~5O^E;6cb)Ay|7#%T)+g9@unj#gW&sZj|EAwqOIo5HDPJl1T*#|35 z6fs6Et1t6zW-|QLHh@p7@o~mB#zE-UJ<)n%*!82xn-@-{&f8SVP$P&V>mK?WjKyXCU)0)cUV)RgwLl>U9ZVIH4Lf19UNzw zl+;T;XhXw~0ryuT0LBRd#{7W8zg#}eFO7fy`!Y4MT;-<4tBel@r{Innt+hRk`M2x= zTM;5dgkD)<{+?4t92JW-$GdKJ#(r|UpnuBg>r5e8F%EV^u`BRmRaaLAFJW@#_<60} zY^GtYW!rN7x8cRrrz;Pgxu?_^tZd;RRM2dLa(TcNM^kWm7v!1WY(FH?x)59NJf9J8 z>;MTKzc%~6xiuA*E_J)LF33#Yosd7p-r52~B8u5t!rVfh-Ik zI{h?SmSEC4b3@IC+;ry)8O2C6Yhlxfi&cL$!|Qa0S5 z8KlXpQ%eW{DN8P16cD!q0FbdUG3zSFwWH`1E^7L_5;$Z9A=jOFM--n1A#tnr-lGXH zKR%C#_R~Ngr|<8h zXEkAd>Uk#c`qy^2u`s}1Aq>S7gDj=HG#-jRF#6TI|GRPck@E-F*XM5{83Iq|a&RB? zA3tV**uJOweRbU?`dl^xNC>)96q1O)dd;F+?>ZxwzOr4ZtBcnzgXE37DL1F2dlx{0 z@tF3O8S4DPnfM?b$GO1(ez4woHDpw)u<|7ZTDqLLIfu);au8$Lh^{c_`!(I$f0@i%3yke-_nOe?1}e+g8esS|wz{-gBiyigQ*VwU6kR_wJ2411nyZMtDQC_a70c z$bI?)pj4K}=_cJhr$@N`@&;g=JzKOc{N693XGrIpE%-9+4YVe~)hWdMO2zM~gIYgy zV$Ip-g1I3H)nh7`(<=R~8f25{+1XbV0*HARtdTQ`g-m(()2a^bW>jQDnf4{!rb zZz@t67{9?aAKGGftKMJsIuX9xXL95C$CJ%$ngN8${qRI;wqC7Fzu48y=4U(EpzG1+ zS3#8`Zni)kiaaz@s?QyAbJHi*=5?|HWEU*10@K@8lL2KF|9^1>U?$dt1(sg|x%0pa zxcI%$Fv4CsOcJHhG?!wv*#}jb&~I0j>Y%F5hagwX4vHr!_3W*#*A(YihE}EGvM}s+F>%LFn z3FQNaNC`kzQANqFKmn1&&91CiWZx3rughim4zCxal$2qajdO3Q3^2Ww2%RBfWn71@Os$+PjjaLKy!VNX?uYBPg&{gTw!M2u1aS%-DE} zLKVj7EzYo>S*mY)?qkKC7jhnR)KNhXw^E4!V&$35?_z6?k)!gi2uFaUN{^=>h|HW% zR#^Y;Eb78|^tvETxjFmFk2wurdcyi$i;j%;3%G_K2cQ!Rnu1v-$$8kq6g*!?z2ZGoh)fAxWN8Bn zWUv)Jj=8?@;R^*+(sDfvVz+WWj(@`uanw+#(wGWAt1&gfx5tgF>v`Q33GFK)SXo*B zB}D)loWf^`VlhuzPV!e4VW-VVhNFoKb(m{s%uL*q2jg z!ry$5O>5M?GAKoz8I;oiUs_9pK5sSlGo6&**PlU{v0hfZIAa+UKQFd9g3`g=@*m=R z1;raBqSaWwtPY7!Lhv4jBvHH_x+IhZ!Pxp0vo|+4|2#%PE?P6{>jH;HyB_Gw7%(Z= zpCI^B{Ua1nHg*9oXa$@L5Ma&!2(T%>Zjm8Tx*o32Xr-j$eHMC-X}8O8e?aZ);Cew^nJ^E`&j9T>rb+iHS|kkn6|D>DZ1+ubVp5}U zj2E&J4R^j(+n1c8TT?;P+GG?pe71^to(kY!JrqAh6A9j`0+I=#gh?}7+sJ#FzV_Si z?Q$|VZY`tkQnJ-p!ZgN}K6euA=hTU%JP{FJ*)MQaR?!KOx84swus@SNPxGxjaXf!| z&{!n`KGO&O?sH71tekRmQYSk(U*bI{A&6Io#wZStFJ`T2L-3#9hZ80NuFVFjPb{ zekG;;eep&5KvWtaWpo}Jt%i#TfD9j&bgJwZQw)HWFcTT2hHf7Aj_jV6zZy4l%wy?I; zJHOif@)Qr*VB$iJE!y)s|QG<>wnSWaT>u8lcCIr)n|wa!WZcDDD>NsbOwd=p3w60Y{w zYRUPV_!;Y=wrH-SU*(ySz2_S5*?03t9oEyIv4A)h%@;5{zbhzr-qUPpg$N0?W^@Vo zn)R=^2-NxXJX;39CuS;Pct#Q^f~}7DWoxfeJRYjH`7%%sP3t(+!=aR`X=A-InmL$@P#U{!z|gvr7Lq|{5Wj$|HPu7A zx;Q?c_5m!!$ESlYjRG_%z$>h&&@*%xm6pO{mN6m)Rw!)Y_?`awGK7De(n4=^csNZU zO(=& zlptfFOA?ADpFTnM>-Rugz;24WZe_30TsUh$KK;O&&hW@0^nEP#ilH*+JM_9V*F`=) z0g$qT$`Bsp4i;E{>Ui-BPc^lbyp~taI~cI&sh!d=;jcnxkZ{e$G|n9_oZr5;X;;I@p{w~a#aHK8HWe$&A|l#E4}N`Pg=O4 zX7^wCe`S}(#%QvI7~*ap_*u}4QpiiyYA?TFYlGO6$l$lj8C@y1ZRZ;TafH+ zjOHJz_|o$}zM;pBe%B1IS->4p2RiL{GgCz>Vc6ao&URFr6D*l?JtmV4I)11MYy><{5W@ybk&pd-G zsYnsL&9f3lsQTg!kiMFG)jJ7YN(unHeM$7Gb7OnuFw95j9ktePDUtQ$!Qb6J!y_YR z?w?>Y?nW~L!pDGce$~d^EthT$1t&|E>kqgsVqVIXiufJxeeS%y?HDS#yk5$?GKo>u zl8;5tXaG;;+b_HIB5>|?XQOOG{a2f>A!fZqFc=KM6sV|M!hO;iJw!fzv1?3<#CbE= zRaZWDg7?@et2BL>(huZ|M{CpX!Z<$JC5B$v(d^Q+c%7zA7>`srH{2kr^j+a zg#t!Wet|GDL~_n`N}rv^`4Uaek}T1TUX4g9_(jNDPjo!~_cE?#Gztxs(+(aA6=TQO zeaFA+PbymvW(BT)7)(AsP8?i`-XAX1A8byS2o}Y%a35!y_-FfkaESJob2nd$wh6EbXGAVe>5C zUQ^D0S@~qBmPW&A>E3?R5A#v~t1NHM&kLbmPp70 zTco@$1Eu#G0&HnCOUS6YrTTO)2D+f4E4_dO%~T&auPcI-r&tas;9-_BMz9Cj=N+la zSU=!I_^VB`M<4zM_m*HU6|T<2hfO`e7}VF%?D)TGc6EDl@$>V)d)G4;h3cy1Q6=>V z`@!e({>- z(!m_0J5Yg)>{F+;s!4^YAZBYRFsmLGEu}?;Pu=w17!B?VNHSlbh`+XJC^pdc zFv`($dE(j2TcueeC9~*VkaW~4_9xJ+X@|I5D1I)os;!pJFdM^G9(MRNmQ}lBWe131 zWP1k5%oA>x?hA+0k(^qANpXgn3{L(GvX?R74gC62scJY|6Nz*6b`is+Lv2RvFaEKo zDL#OQw5MT+C9Ax=lw<0+=>g*-d|4O9`px))g`c_9g^}?(hEJbN!EqVi15>S4ApXkn zmV^0fZEX#h^I3)byD{r=JaSgN%N}(-=wh{O^C2~Zs`OE+c(5`{M(Wa`P$Ahz$ zO||nWKQRCUZ%1cS1c$pbp&lN)UCAR*)LTZJ_EZbz1+Z>&CgdT$NR`KX!;(Mf0%j`! zVZWwEwR^6RD4(zK*bC=2D@4mX+w{$>U_m?ljwFK(G98*VDCqyg{mBgYy#`&|;+B|2 zhln{bQ5kH0<5`_7(RW0O2nt3%uXrvx0OzVeD>dNOVJzN+3j#nPfk;F|#YDQXAJ%B7 z8lX&84n|_ICa9_{5Y*+@vAo4$M8#phOFcKvZycYc> zrHYaDvyp%V|2;(5=X`+S4EgeYvBBY@a}t%RMz!L^DL5DEQyn__uE93q3mZ(gSKgK4 z9hq5}YjqjpQM(03OS$xi(FmHb?kW(%2gZjcuLn{Pzt^ZY_<4A#O>bkt$!OHa`6iDh zjIxiCiI@4;3a&&>LN#-;mPp26(}nh#ipwy22*EuLq-~YPCT+jAyumd8ZZ^ zsioxOmhAMOf4_Ee+y|$4ATc9^vb+l)8%BsKA}D4Z0umf&_{$_v2E+fYzQ_Wy9h_fV zq+#$TI=juO(>?D`=5`l(X=`IAQ(|pobwOh>)oYfY9+7Y^*Xah6Vl5dOlsiKSV z^vQ=(B6J{A207Xf`k!8P=m#N=FjzF6UnleW|`K+ zZ!;LtS&Rm7z{Yhz$|BezWvIIogOGZ~Z)q)zvL>1a2#BFTk-Ql-w??Dn9l8)6b9;!9 zJ>%Z|t3ki6%hq!qwr{>g-SbN(=gclUSG!E_3c>W#z+G}G$yFnB^Ya@e_lXa-0>jy} zTj6)j8#ia4;eRM$=IHlMF3yqL4Ko1}j+eM8S?)g4i`$65w(@X#5)#R|o*K9zcljlu zqpI6KCE+!iE$LQzJ0V5njXV$7@A)2#cAxSjf#UDbF++&da?M0;!2%|@Ztj(3FshU* zK2iNm8y!l5H<5mh#0w+S)1a*G_@R0B=g*(%%Gv#@MU=a@+}6uhcoI49lVD9l7nyws zperEz32pB-9|27ze&^RIUsWA+*^{&+FShKOcAdSZ<1W32B}`w@RFi^{-3fg*(cn$d zHxag-x>J*bdMPTXXPqrg*nJL%^oZ&rD~L?U^L5=GA2SXY{js$i0ZEXC7f!IUelp&n z+nJR8H^VSK;pBkwrq=)Nq_)aUm2cS-t-hd#;&va&uNZE(&I>w8nh{Ka z`4p(@&1W9J_6oieEE{$xQVm{4mDaROMk2j&H7aBRH&fR!0DNtdB}gf?HqOyu*S4Ld^F6B$Mp zl$!Qryr>(Zup&EW%X^pF-jetN%JDwCKvFwX&bTa9;n~RP)_nfrb+h>A2B|kfGlZ~^+jq~gg#3|1fFJpe`w6qLL?Q@ohoI@ z*`?G&yFJZg{)2KcOPX~Ea1>uWQ9l!ZeE zDQ?P|433Cn7zT)XL?k3#rlEotFPi^Vy%29?a%~}T7T!W5tbRU^n(wLx9!Ae#Z5E3O zDk;jCXQ28dEvgR3x+!lBksuVi{ITF>GBS0>^0BOCt*tDl`n!4Yg|HMWpRyyULZKW~ z=9`we00rB+us%xklB7%PatSj^t4#|qx8=N{)YN;)n!KD7;NKfbaC6^Ed4~~*a=ohW zzRyn>jAO>RHQMw&?C$4uth3s*RA+V!%TG#|Nx`3kYW*xPWBMh78jAv#oaWloLxD({CsnLn zwx@wJ$CE5i$Ne`l(SrEoq&NjlARp~F>drfNnqMw4v9Y4S;YvT#Er=^M%3w0~p_o$D z)P_kC5u=1wVpjy9n|fGVQ=s>`Sh@e@B*&xPGqjyC8T9_8rj_N-cY_V->5`AVJLrGz z?pH7KRtsczRa8TmceY&id71)j3juxU| zKY6j?`IE`WbL>-_KDyOcFtzgFoN?!0_Ydx7nhI*iBv1r}Lt-IhOcqJL9=fmoe>Q$d z2m@hRbHf))eojWptOC}UUzP`_QG{T{{L}T>*t`=yQVt~W_{?;L)TSk0wh79AtnrWpRs(>C2p6mCJ#46F*)!c*NYAfe);s9O{(nD5maoeY*#>CK=Hzvv%nE8E)||8! zZB&c$cbhbA6iZZZ&kp+K4Tl!16-Wzyy`-b`omYMRbNaEaV;1`NalS7nT^|($?8j*9 zu2$9E5IqNWd}6~KjoZM;8b&QI{SGBu)u_#yA2O(`|LZ_}^;6Sww2sIz{4oF1NcLbf zVO8&{UIWz%?QU5lP3jp^^#1el{4V2H4A;e&*uu*z-gbeiWt!j0Vj(86+8o^?gkF(k zC#^FWdZ`yEDLfTlh{dfWbS{J`$;q|)!k2T zz79ys>Di4yxEI+c+qW*VQlD~EnzO<}8^nG;^?^*ihqOUALs0(#=B~S-cg*{-1p$#+Y6?^waJB9aw`D%aR(2VG=N&5!x z`S8ZW>7g`lnv#NiE=f8^d9#Cn&0bk!a0e&Sv}V@hvTF&oL2>)CpyuK-i#9cG+iE7Q=YjX-RG8a=U0-#6yz& zR~YsIc3f%@`nq$%c8A(k==NFr-Q=j4^guyx$dCIVhpi@>s;9{+L;ae3Crir#$kb}% zh04{*Ln89Z!?)F-&h~}&i>F7Y8+;I%_tA;iiT3ql(Anx(>lN0@+4vFYR5lFCdV58g zb3gE{irCn5?$9dCO2s4?y_S;)eQ@d+%q%r6S&DtwuN$wqZEdL3RpT_EQ4KVBp^&@u zR(_&EAm5(9YefPg6XxPd!r~SaF!t6xn)y9>O$Ny9-;Y!>)j2n=vHS1mNp_#2ChWO1 zuo0m0322{18hYW*QGhboRN-0W;(A3aD&;P@VN!1NkmMZ@QupAA45vM`+LRg$e!rw+ z)m6UCr8!>qQNL$xE%(~^wJ)Ov4!ZEBc3v5TZnCD5OWU+06h&wjy;M79*6u&DM(E>+pU%=nPiXRPAow*9OfgZy^H3 zItCt1oCLxqBV&fz9?F9J`?`xb*lvT2Px(RcoQLC*fs(+qlFKS`3KCiLa1unV@bSDw zX18zUE_of1(fW@aaX(ymQHR>-oCueQ%R8EWZRnYqP;cl;6V>*d7!LUSNKMi9X(%M9m0mSZGD-(ofpJlBmzhk?0!d&R>8g z3DA+~kj>!HauXI@ouCkJh3U_;@F%>9hc&rXYL`XathGn*_Mt(Tp_P)_2zstFu^H4g zuHKGAW!glIoDer7o=3Nr8PchR42}=+Q+?TO)#l!N@LEN$2B)j@^I8TF$kGa7O}6g| z1r)f&Gm}o*@bJ({NvY?5<})-FxSx29MRrF6x+!W?1Kq6DwS7w^Kul&GqPUG(HLTgg z^vTU7S76v(7lSIQp0LcYl%TpWV7cuDGvo^^bs8G*g{jU z0XkbYR2cL5ou-txbti6y;9@{>`v+-IBj=H#CFj8v5(KvjIl}16$ z`2O;nx4%dbJ(42CZm1Smba_9;TM&Ahsg#n_O^GA$#akE+tR?p z*SKbD2Ur0x{ zUrJZBj_LJ>dvrZrJ#5(&>cy_}yWC!7P>U?40tp-4iEr6 z%o{x4dIa6iCU35iHgevs9Xz+UaXc^d3$;D{7P}kE;cxM;@-Q~+lu4$$ULpsxnuisS+zh?;Ei+uXX zMmQr$yOPx_S3ZEFFK5(2iMYOoQRn*+0@S}%b1JSmO+JZ$cbEUtmX0naJHIsk*<(2o zc`umo)3o=!I&t&2%l?-7u@t1$R~s80rzO0Lr`M1Iw~(K`sYP-KpwYIylc1nQD9E%o zU!nc{p_|9f_%1jx=P@FkBYTgMEZa!vW$N8M^60}6av_Dc&T4{qAa!5lPdcc4b#Wl$ zSj{lEx+YK~SOYwQ(d+|;+9EkAvZInSDJa&yjnM$=$&Kd*i8Nmup#6_h`;M_7+b`%g zJA7TbTA6YxQ#0z`vcDzT%r*WyJ->66!c=gO8D*d?VoyHYHnZ-DNmdxnP**M;bL!v_ zK5OtaO!m05u4Lb?$#*LIrkq85S=8~Ue>>Yj6fEa=GJ*M2_6Euf{=m)}rrW8uor0r) z31UfQ&0n2;GI$qZanw$?<^Np4_XyjMVZdB(53z#D2U(t}KOlf$vn!b6VJxF8sUxjn zD4+lki1S_Tj^XOohQc!nxzP4XmfRj@dbNW+fg4 z(_Zv#Qxx&22Vk@xOlFZ3*vQlcBG68bRN!6qA7^Owj z@3sd39I!08Tp&~N13kr=Y&@WRHv5J2@5_cH+(_KYQfTk^F6T*FrrZ9X-2ycs-eMS} zxyyMRPhL>`Q>Pj`8EH-dA+n9SYs6ltx!#{eezuHp4+&bTjj^)yeKEe#EX`y*wN#Bx zq67&Mqw*NTK=Xrx)4tPFZg%J!JeXkj+9dN@^3EE!Gy1%1*UfY77KOemWsWx5D!;YVc zxuwOmLpnMm2?sl|gO?xJny!r;4^#TrKGLIjdO;TW%Ns6@qgg=>#g<_5n! zvFU$lgy7^{(xnT%SDh=FvHeSPar3hW4LOM^gUU8w^db{Era@T*IP0$;Nl-C6tk_tO zE}yxKLI&vERNtom49Go3#qEEh#!QoWZ*Mr*AW@xy(@A9NGAnv9Zufkgn%Cy?usxLO zRW9<&=XSm~pf;5~^E;%o<+*@{2XCAY9b}i%{^@Gtw(aSuP5Px(JA6Hzr+y)9mQDYf zS{UkdHYt5zTxY!;^`@d}QRW10TEqExxbAWBSS-(QN!j`F{A?23? z^c6DlL>Lf~w2fn>y4oy|2|#kVt!J|{V3u=V)8-xjUmYTfHexU8uWe|S0{QMqUL#dF zB!?@qnCpe|HJ%x*#V5)4b~B#i-X@uAaM~Oo&HNL1k|b{8t&9rz`>!OEPwRWA1UQqv z4k$B^8r0eZo#ua^{Ro`4zLe0a`cawE@9tr5YQ64F{-73>^?B7ab54-&rq@{cv5*_t zoh8RGrf{qGMd^wN_y1`DSck8=^XU{E|EdN^25rwKx5LF8{l~89pAQL>2K8l&iTZ_A>Oy+D90`j-lCq! zzXOl!956d&+hVwkx(E=TtxOd^y5-!Hjg?1wxDny{?7kq)N_roX6WspY7agc75g*2A z+WP0_E7=FN?l9w50ugrD1;>P+1nMLe;+S^a)KMy1`kcVO(1a_n*>+wb-;mCxAs z+&iev+H=57V^#2O``P(!%1QXrk3;lw0%ig?F>2ckC=z@8lb7?z|LyM~gr^n;d+&ll zwod3l+4uZZ z3{N&IdOlq5lHI%=a)2&3I1lC%sOSq2Mkua)Xr+vZ9uSl|xV42;yTO*Z?oy=1UIk0I zgmXhU5N>L&dG^N{#@qrYx{#GJ-Gsc3w>qT+OKC-X>7Z} z#rMEVr>x9}3upp;z6{_lKmQx?zN|1@H;lCjvfZGCSIia;)(o&yrCU(ekPu2EBSs}q zKLlc#vfzsXMOVY4O>nFJ%ImXPi7)#Wv-Yul9%~pgKFBNwAy}e}Sgfx%=qBHw(S3ak zUpW6QLlw3tp?lPQ%$@YsoBEH^5gegoZWUaEzo9EfQ;7-kKH1Y=6d-Iff)w7cJ1QB~ z2qKoHfin*c06boc3RJG!KnCg>*`ATzp(6SyauySEvsp+uDk2Zjy?4b~{}Me$)OwWV zg#2ZoMDF2QVikT)$2UTLaNq|=9$PuR;utm-gq{@zJ`S(8vlUzurjARtZwAB&an&r0rkDxH*%$6mn6?1vFtLzw3!Dl22pSzm{0 zYg5C#0z7XIR=hUv13@qb!v9CpS4KtkeSZ%KNJyu2cPbzq0|?SNbc^87-6@TLv~+iO zNOwsOARrAR-2y}S-0$yM|2G4RH_W-~+;jH+)ZTD=Xm@-Z6K&zjd0y!;y{~k&lmH1` zZFtrqV6}z`*W>RkK_0u+t1L?b(D=B*LT-Vg&vCIzxw@!HKy@o zb$b3@0|c~MH0^tH7iV?`0o4D*?^oY7UXi!mEyyT&`r(1HF8rN!&|B6rK!iVo>yGtY zh?UyQ)+~Nzgr~9e3$s_Xu0Pb**K z9B!ckCtN%=cG(jtI99A6Nflu@Z;v!T-oPnQbi~S*ZALi@bIEem;|4A_ZsM@WqbcElW@l$zA(GMfiR#~QUMre+f8c_wqo4=Py2=GAA zMt~yVFu*uJi@C$2$DA00#^)}-v~=o*#{>m*ghrA;c|7berqg+Wfkc4GkeUV6XcMPE zB-zNj13@{iqN(_aF`^??k2&y1lgP|%#I>lMr}@+M`qCkdNfDupK-?%Y zi%TQ`mH1rS^uCP8bdRi)|AP!M{e@ZqgU0CZi<9#(z>QL^)_vDoH~Jlf>15NJpQ_&EftysBFm8Wb>W zsfhWqIBuqK-|ng__~N1P;aU7CV87oA@w1-&3lmSM)nH-_K~aD6HJ7&r<}Z9tH*G#n z2L04_#QdL}6Zu@L*AOp_)>ivffQKrnLa50evaobBd`&@(q^1X28dUjtA6Kfmz3=?? z>1-;Ma#fs$m*37;UdgFCirQ1!Nn5_YM{t6RHp@ljI3E|TOh#$29E$#kV~hxs-U3CIfm5hT*@yroVWnh4gWZ^n&G{^1@+Y|0H-5^} zW1%gIgVHM1RBRhRl7?74)SKl9pkpTg!wDNW0mVVnrix_XfYCR&@V%PhyL_XMBs>_4 z#2T^6F?!}(FO2+%&;paXmjh$)2#rT_)tQ@ux>9Ypa7tcvRz=ldk#awpo&oe_h1FjV zP@N;00yn+dyx2Cm@?W{;{0&Ome_Th4{VsEJt-GmIFmEhJ&Qfp}&e>YKE386C-SId7 zVqf@|9srYz6rrne{wLNCB`1NLV+BtQs@x14ir09`8mf59RL7?AA4wm$Wq9~O(saq) z1B&IG+~)Xs)TiSz%3r+1`G#up{&9PDxPd{;@VC>{7W$ZPF5DZ~wuIt9R&Q}5_}qsO zP5Nh*yPS4P{@d5^nL^&If(J)^JxgXK>a9W=gfZgh5}~Ev-N2(HN21u}f1;e8fjZjM zPOi{q^!i;$38%LPTt`$*;-0Z~m`F+^LhhlM^BZoNn#(MI6=$VhVxlNEMnuCloBrZ}uFvuuf}gLJBF%aD8^oK{lMH5KPse0|-0u zW5P>Q^4`FmW)3l>Jd>s>tMi4~$NWirGI1fJmZL zF+byr|F|WPUW?splm}@&vB&4zt3b9y$ftLd_f14dAN4=|`~lF?a@6D$LXL_RV)Z`! zKdabr#YsJ8iaWcF+AK$KR-iAZ<(}#`3QfR8M_)$X(E)LTy}>VlqT$t3Yn}v47{I`f zgp?Rmi@+86GvnyV^?ZCeegct{b3Z^xa z@H@Ko8N%S(oLsW*_~CzYoc{PIU;$(g0cQWLufSaq&)x3q;-aVsaP)O8iII(v!?l#= zf2mBmG+hzHrTa>SpGc5}aT+hhT1yzYMCFvmp_>(JZ#4NebR1p>cSv;b11b(BI3VE| zq4}@EfgA8>U1{Jz&Z?fhG2(95>q)8QWalkfxblXc(b51#0i z_#Vhdhe{Go)|Z>Y)G6oGBAq9O-}Pqk^rU+g>A&XT{V?Vx-O%D#_*^2TI!A_jHsSc= zq=iHA6?_e#o8=abF3yB25OSIEI(QY%bvLLK+}WuGC=2@BATk{0nr}{Etoq$)(`E|` zz>i59Kk?!O#6XDCSQV-7{!ki-C@n6F^NF_VDJ;tg+J6N-U2A_^rSYF;`2!uYfSxXm zYPrhyIQnY}V@);bXu)*Z|27s@T3x;e(k#01rK=npy68fM+&{1cx-~r&3Y~~&K+kfG zkz3@T1a}VAL?mvwQ6xJjivDKF=$JwE@Se~VVU!5G^Y-zz_h1EeR)OsxG$5?*b4NwD ztEvEy+Y^zPlOexo_iKEK@SbyNE&(4Q{`t63j?lm&EMeCI_3B~AH8;E4^2CLkCCjx3 z`@ApxHC-$H^cSfFZ3Ys8Mf=Cedtmx?XjZQl=R>}P#h78=VRq-Oe(FU>Xa+Fp9?%_3 zyvefL-}OFSZn?y-Qb3k!HqeQ`)McQ+amxWVnEY*?2XW1KWm)x52U-)3priQ)#WXRo z%NKGFo0p(0S0VpcK`ozjw>w$Y?>_lP{KZ{4uG$ZD|4R5?CkS(`;g)(8 zxjRc5(`|@Am-a{z&2DP!(r8r2QHO5%paQY(tAhiwF2xiR2;3euw-vv5{A?A@sV!4Q z{JuM6Y~$JYkN%Sm+S$`f3B>G1z?pBDSXQPqFM7<<8ha=Nsa!9C$OYOHg$QLjL(xy& z^Ny2Y^?pS=eFe;Lf+!+&-fnf~(XRWKms;z;=kaiRi>?5GM1{UH%^L1pn~fK$T;^+Nr_LV}0ER$=a?!Iel# zE)(M_qjkV9o+cV^zcre`z_?o)vo}RlwE*aCbVyRy`lCA$ZepOSIpz*}y_m1;`EPI< z(a_Kqepgpbk|dg1m^Kd6Xya8#EW2Qpb`e>;u2&pPG;H)FfpqkWyf&qA>jp+JgCD+& z{H2&`|IfrEOv9q2yi5mBqmcRH<6i~5zEpV@qC+&5pw2@N_MabHv-lcU!o#y>T zmo*ftujvDb*&Gr0>5nrs--1zk?UpPg!}L$|GN?9wA29>VpvJ?#YQn`U6Jep}Eq|&^ zI4);ihwi2VN}Z`$jjxY;KYzvO3ixGz+fNNbq&>*B@X#bQuSB0(Gzk*sCPi{ox8$+n=n@jSO&MJ*BOo9x%8R8 zmR9JVSYp6DK`UVhj&4lgw`%iFSibEtEJna`&2pX1U{-11N0hn=x^5sn-5Q^8_DqtX zW?BD#f+>&_Fc2Ngtv`Oa8I@dbTTnye7rPu}6cjl{wg4H}CGg?wk-ntl#lD&k zC!1wU6b79`=tYoYd6Re9zdB%~|gdF}>qeFk?MLg){H&;hsKB zcJ6jVGs5V47vhCp50I`mx!(J)Z!9Gi9qw-b-u{DGOL7!@FotC*&igpPqBEIv=K)Oy z1-?r~4e)+^o4-@84uC=3p-@cYp^W%IZ&Fgov(R1+Z{Jj^q~;`r8A?r3!Sq2ji-}6v zQfAmIAHyb1ZanQ)9l8k8Z*9S!4LlYutN+AYwRx`JTH+#+!PtICeYtk$ z3Mlr6WpzKc?P zy0N@zSPKL|yH4>T!8K6xTKHO<~8qE#!HayRDZx)Z1$As;v z8)8|Eipnx(y6G82@N!XrhL};PfBJpcu^&Iv(n70|-xm72hC5B)8Ot>php1WoO=+2qaql z0ep6du1VEX|MaA_V6s9B=**LFMtJ?Q3+g%&~NPAQb6{ z{Vh#jRx`os82?fYDh9e|b$1a=H9o7vD!fCj)~WWat_^CoZL#TknvpO9a;;0Xa~DyQ zhd~~8^%qJ(O242!scV=Guw8ivBQ%gdOD=(n`~UbH7+(^5s2PKnEf@=wg8~Yw zZnb;rT2e_kPxo<-l7GP9XoLWP`fi4J!_!&mtBlIYk+rx&VBu#4=fuKs9diXeN0}^F z8?Nd9#xv#TI{F_OQZ(=Pj%pp^KiIq!xX6D9A3%fLoKpVRb*b|47fIHe<9}Ag!kc}m z&$afNf-+(TIo5r4n{hbWrxjg=cm>DZl}$KSW-3A$8%}B%k=W}}G^fnAjZLvXOyZrR z%T7%0b)mW;Lb?adH!LmF8~>=E90}+S(iQMskNjiiOZWG`4T6+@2Oa?;Y1oad4*FkG zUrzt}kah<=P+s5|{n2c|UI6WQIkiWQgMEZsNjW%OwE9 zD)zkZTrz#T-4iy)9;Tj&sk1#%#Vm`FA;cSI8ejKL%116dg<1sZWFTv+LW z_7+mvTwFBcMfQ(ZV^DEnuC?ws)TPPp*T1OTI}G4hH=p>~Fj?QMVSBesRC@A(1QBj_ zE&Hne1x9RBSpNFk3u zcbahP^kUpB3XUER;Iw397g04%lc9Pq-qcL9^z#0eynFbZ2FRofj)xzN$kJVGe|#f2gDm|2x~T zH@)8Fd*9=o^MF{f;ZS!8g}s6UYpdp!T%xOPN~$EUwC%3RoAX@hBHC`|(uA=D=Sf`Q z|Mi3zHkw%4QE?2BY-tqGt^t4;<5#go2=b&H*)&0Dn8Hkc#YMVcj_tbTW-+Q73!u#d zDqP&kB;Nj$a~}n89S=O;S=OEUxSboT)<0;-pr|m4TjIO{g4QSTk&D~_afq_&YCXww zKY6_F|M_hFS=@IsbHk|9R`*|y_}d?zh6zo(kWj8Cg=)fGZwKO}lrkpV2^_Y=B7oQ+ zHRw^&-pTuy+DgZis8D+DRrzvRhc_&a#JaS8WzrorMM7=>=2=Y?`94l0^>TSxZ@ZC7 zrO$2t2N{;jcg7S&Irnb1E&WGgx@blkP5v8G*2uu>BY8S{;v5`vus-j6@7R@CRsycJdM> z7}jb1Fal+ugH(RTVj3}ouGcVm>9@RVq~e;g4XTUcR=%4It?khou8h7l;dfb}vzla+ zWb$j($7?Te<^6rlJsd*G{Gl4cp&NWqrEm%Yiaoan`Y+Y+R+K5>as1rR=t`LJ-6!^r z3>Y-{KnZ^ZSRFnd0v~tnj5w{1?uzCec!O)~SczCWsS?sI8XiX^0!F3~)2s(hz5wlr zx86pl&IWGV;|AW^dr$EjGy#+FX3|{32S?+IT5lM%c~$b!JOI7EB8!+WD;;ORrYlg& z!=dxyJ$LJzPJME9I7Hjw_p;a7Kv5X}q$4EI{oU7B(h-0BmEXig%rw|`W@1x)H&u*l zt(q})*ZWPvGhxfzsEdkK9j14lj!|X`$J8MfOUMmr<|9C|J(0pYXjzGbdC<2Fw+i69 zbH5~CrA@0$SRE&d6bqzev6B98847#Z_Lzt-b90tO?;R-m9dIbqF zvirtqIklc%X~}{`BM;*Rd>X%gF^|CV_kZLU5-Mw9HeZ9C@%`CZT)=FLL)4y2ZhMp- ztb1{MokGqZv?`Qe)vtrepl&b}O2m{j^o@?@%S7bWm{n8JZXGO(3c=MSmF1eh$l?@R z-n7^U`$n28+r{Vn9-Y#C$=)BsG-29%yRdHHwKofjTTvV-D< z{(ImQyFAb5f98=^`w3Wes*NODZ(anj%x1)BlDoOlIxbiU;KmrG?q0Rss!Cis~YNKIos`_ z-wZT@F+xnu_6V%iB4q1)R?!d2%cd%kc3%)lFI#!ObjiULy)!H;n3wUNM6U${;xp3} zjW1eF3|tz1bQLUS6s=j>rno3UQ9B6gQYt3DP%IGlMqAFCm@B=}6Po8boyzJ-5&M)k z^;;|i8SIBeD|x*$x?v>wZ*D#8>^S`=<42$Y?&R4psp{t4*mk)8$u?_ zIjai)|KEM*5Sc;}MpDV=>g8J3p?UGLDXk zE(+S&%axg4BKhKz%^za%pNNA#`8P9g@M;CCSYpuX;7SIsa_CM{^$-XO$Wn^Tacdx9$bYh;Y&xhVg zzs(6

9`q`y#|ry^sF0B0_N{`~o8Mmnf8>F6_NU`R$+leqR!F@q&Hp5~D}bxv2qPNpv`eEZ!5HTI63(ra;zwfD-^Wet|&+ zelrcead4kV4Nw?1p~QcMi>rj+J3kU1Cc{0LQP{xT%h_%PUsW?srO)J?*es+;(fq-t zk6-w}ieDVr4S2S=be}Me#4N^i7}R++>h@4{>%P9|ohw$#mpCk_&RME4jsXW$q8)3V zcEMdpId`^u7BmW9EOW)Ut}3Koq{0d!+iJaYZinBA16N>7Z*c(tn#%g%^$S~zoBR}I zXrdu6I?dethd}6#DS%xb@yYE-7gI829L|koEJ?s{!=hmcqew<(jY?QslFAnTCd+nP zEaP%d&(n$HNZ{;i(_E^in#j&gFVhUYJbnp6#Niw6WdzDWOv1)h8h6P?Qc}|KDDhn= zAs2a^-LitKwneWjp&5fA1)84j?y!UjxZA_5=I3QP0oLRa0KaG`WZIPog(ehf-O+LM zGtzwadpS>TUO8NS)Xe0V!G?5z1?LiFPjv$)x8O&`&tOgytZLja{5;PK5BZsT>baex*^Sdy$CwaSvOKI>a9~zUij}r z3xg3KRz}ouU^X540fuv?{^d3;{$C1qF=V%BSBIY&0^jLdUH?7Z&O4e#EZAZ!qgsz~ zar$#G%iZ(@J|_~Pp5JpB@b6-UEN*We3?Rk}%Qsj}fyuAWLY}8+B@8#Jh5$WpF4UB1=nYt|cOt}- z`JhCTRdT%_I^wvay~H*5FoJ_W%-1F4o4k*}aA7Pp|GlGv7HB!K{Zp2#NT%$J{DJ3q zbJT7yy`ZScA!Lr&TK^{v?40$}$Y)b&l4LzC8O1E(RO75piQOUO0wUALZBkb|o`|IX z(*g*f2*4NtsPeMKETvh89vt}TWbNfjYmMF;R}KB1)wX~y2&t3~ben2213iQV;D7qN zk>UND$GM#t1>gG-D+v5CQE_SKrWVsX%(k;U{E}r${Z~)0cF$ zz5!V9bmQuuApLws7^B$Dv(4GvdlNUe@`^ipJ=1}vmfF8sr+thDh^hyc^!`j-r_y@B3cBA9w<;j$Xo$7Ec-*TQ?epeDowfqL@8R9^WD!+MEtXBb!M{ zFYli@uCQ_uyH5a9)5J8`{qNp0u;($w)Wl`r3r}a~b$|K0?u*!>4{Hz4w}IyaG~ozq zZ{QD(cI%z5L4uC!RSsN8!!HT3j73c@qIEkDb!tWY?#^G}iqXb@)?;eub)Le;!3ifO zZfx{_@NoZ~SLptOkTJHkW{&=NmdfLF?K4+&GgU=&6jyawz)`XTr;1urgV|WjcF0Ap z;w(N!M?^c8ak0_6pAtlBKHa@`;pu?BqS~-kwJVi~iK$(c0W)FP5Om(R5-7S6cyTo> zAMKEv{hMEuqge3JtCG#H)7|+RSF+I-RTLfM^r*anAa-@2lnkH@tTL5M3)^8}I4ay} z$8+zr$`?ek?JQ=OuUm<;DlO~8(@zCP$+?PlrqP-&^)6K7H769^$i4~|nO`pKDw>?( z=C`SXg^`1+`7Y+bi0k)yrT9;%5;Q-VND6L)5unIT{StN22Q423i$euPYuzR#@NMU9 zMP+w43o^V4$hQ#Jn(@ER|1C|HT7)+LH$IK|o`CoN4M=E-IbD1?4GD%7$MaHXgUAB zT4?KW@nUCW<1Y=(L*|AbHt6xJ=L+`@I~PdnJzZ&zsca4r=yBKirum}Y!>aEqDS+lQ zSWT2ItpJU%Jw`%^j=QMr{O*d_I(-K7SH_B)KYVA&WtB{-_2|Ae>T@PD-U3(^>a=bm zC-xduEa1pwvG7fO81x3<;-CR?ly^PQ$hH}mu|z{6n40{@{28y9T9(TvS`;r<|IVQi zd%sT9m2WXQfBJ`H$pyC>&sW#)hDo`N#}|Go7`Vh4tkE5MaSR1$aWbS@0>vQ!39ta3 zBKvQIv4dxrPGZ*0i!Jkh;xHmY3M3tS>SuT zWIZsNob^5vul{o(qf&Y#zA8p_xbE^nYkQpQlbFOZa^W%SY~6^=d6mK~*RFOXFs5nr zD7PGk7|-QH8HTsbpaDCk0Dh$tmvBmQ$`KJjYAD78`PDH&ULDND^33Tn4gsZn9dQjW zx9~ZKqc42*--hH}m%N&Cz{Oe5HHE3D>w~Vf$@9-4$@{Ho;He+^pZU^!27a*xF4_3N z%@@aZLBopumA3QfG3NZwKs%c%abgn8-&{K$re3|(i>bjcrIfo1`9kcHa{-=irCWax zo6h{p;>P-B897L78i;})1lPej46SDqnOv-ldHMLZvNFGy1!7@JJz{~Zrw?iD+d4My zJN8ly0mBwCPX+h4Uk?I2{FFZ?%C*^bjZ^Z}0o&bk)xa&#PoZ^rTLD8})x<7jQ>_4% zZoi?^EJ2KgP93yaaT-e!@uzT^->ZIZ7QNZP9w1`m<4u08u_{lvfcULcV_x$hnbh$9 zZnT~b|4oa^bt)Vp7e$v1#LZ@B|M&U*`(^_C!}$he9Y|I8ddpbVuNnJdo)r66gXP}s zW#u#McoXmWd8~2c$J)EA<-&NEu-Kz_%Bm_zy=Xy0JEc=@OSyx!Y@1s&9TUp_X%|3uhK=%4cQ-LmvON+Sz<7ReoCm@X8`D|sY5XE zZF9V~G1y>EQ`JW8E(77%wu@C`4aff_|FoxU4-SU#$d^nvwQw zfyVjwDk&IfqVvIDk<(}zMAAn+12dgbTmwpgbMs z1?bt+Jk6s%jFcv1{Y4Q^_%8c-G{8cy^EO9iC5(98XUw!YK0aSvVb);ruG%3-_B$ng z!N>BU`l$wP1E%OtfY+7+Qj@G1TO8|KPM9i|)duFgUo_lh_&btXg&bt~kTOe6HnhZD z^>SSr;qB;5QR;IE zsj8OFFuuvK6 zB^!M`ccek0=oLojw#^v^g{=KvF>>D;$d2@4!>+3d@Snd?)i}OxMR+Tj+S{bQi4ZLh z=XiV@TrD%0vU^PqWD0|pFGm>wtX&!p zU8e>UsrPj&4PZa2kAtZK9@S$*#7V)-kQ+dS97{_+WjeeyZbX47a7a>aoG zQtUi-Da%h>*+N*IV?}A7Fr22wqCre2UAODC2F;j~*+)kkkNGV@rc$_DQ&-`ZDao)M zeV3HThWYoewx$dtzZ=p6Q3Fafr#)6|hEd49=jUs#pFmC@p$?5+3BN{;YTpU^J4%_2 z>$%6g|;E8+no355lHE zN%Yd@4v5WUj7J_90U zug(W>V}w4gU|mhM>r_I@y<6A4TIub^U~YYjj+h&`+v4!pYRS z15Aslc(yHe;?BEHhyc8`xMkds7ENz_h;dvn8r6r8~N^WC`NMi z_@+Tv#DVMa*N;0hE*ctlygzcySXPKIWnr<2N4k9u8=pYTYD{Z+6$bpRm3xW+jO%Bl|kfWXv}l zJ0Wr|G>b0IA2wT1Of!4ZO!@R<1hJ^oeI?T$U($Vma1 za8xT~2B!83AeAX^RqKB?{ft;aUxAma`%WI7&6MfW<`;u*WPnkmz&qaHrcFN#vkA?{ zwO~?onf<5d;Z^XMY$@r1d(v}rd8LNS)pcGi6X7(#_3@+E?qk>$$mvCIY!c!q;_%rJ zj>JAK;o4b?3j#FeyA+<~nO;b}zb>4CI1-B1&byMmCzc0@WWz^d(6yQx*YlxiB2dBp zjZ))8+?e*9^ad|=AL;~N)yE_C7I*oYA1!txJd7ZWux*fx!)xz74pZ;T-7#QDsvb3{ z3Du}-4d8rJ**l&G2j$>7HY z;g_@dOzcLEdRRz6kc7?r_McgS1)tNbfj64*Eolk_e(e3e@x*M_tJs@8si@9as}_y+ z7uR?$hF!XydNhTS{JK_&&{q|i47zr zRZs!Tah|=$1An*r1?1>_>B)B0PZh}iwQbL<`LvIUAI-eOH6qBU{RK6)-G1ePU#Bg+ z9%@Zb7U&%OdG=F$%g)hRnmcwg&J14eyH#j2;KPf;wT*PB9QqVfYIydmIb@}=RK4SC z#=59}B^Pkw(|?tZqZD>OOvVe(f3I5(L_aOR#0{o1_ZyX1(hZ~3uvd_5iZAmhnO2P% z`f;|qffQ64Ph)8HsvYyhxdM~!8-R}D+op1F|NBcL5-OQ0h_*E-gS&;x0bEza^pALQ zdQIdUmkyz7{Slg&!E z>Q`|-(uZ&Kc{3|KKHx9h@ic8cHY+ak<0)~o8Z3rIE+MUt%nsn;wS}%`dlId^7vi&s z+Dx-mvP@CpP`%vV^lW95MUp`3}FWZ!JA)88LN5t%f`***V&~* zOqqZ-ic&KS^bOwbbAf$RAD;9^kn%YBwBx9zM9T;Z1$wsJaZgud-dPOgbn?Q16@+~v zEfOR+C>i*Kr--sR6qHmzcu^rw51Fsq4JonujXI;#Ij0LiMoJeVgp$w%P2CuRX*sQ` zd9CPXgX&8hK)V<&w@eq7)_q;<#a5uQd!cSa7ecIN`zG(3Giwcv%l+^TVER(eH6f_< zU0h~$K4qU8t&=cZyxm>Ku5`!?{EUm`EZyGPLo;GRcAsv)inu-A!h}Q>je6@~FsN-@ zePH1a(SsHK(V1)Dd1F`!QwJmNURZKu;Y;N0@5D<^eX%s&BSqdVh1K)xtI1Z45;%t` z78B^&pymt|2%vqQeP_~j?y$NNIR|wofu>_HRK(HlZ99w4SV((j2zErI$W}W7B17*f zM~`e*^SS>mGT1$3s#tblVO2@^VbLmzjDli^3Q(iHTiqgW9rKv-KXuK05%lA<0}HFb zpxQ$3+)>YoOvF5^)juFwAPN7y3GJMH1XBQ|z$Z!36zXx2Zj9*<8JXdWk3JI-%z6VP^$10uJyNq;vEQ#d< za>1+c%N0m4gnT6_~!Fo%_UYS@)tP#L#7YdcQ5 z3jBDa8{89k((YJ8m3231{rpxmAk3*}z62QQApG&z6w0@_@vkTu3AE;Ogw^7>kz?v3 zaq5W)n%KC4)&w1eKl%Ozc+1#8>lS+~ngU0~c7k@rThVH5_GSYvo;?L@a zN*~HZ`O9nLus>I?mg5CLBG8la=ovqhY2lFfj7+>rvh^{1C4GH2BSEc)#ZhWLy-o+j z$L!l?kiMvSFU7?}y#?7WEYS-oU^3A*4WnlO7rn1?ZQ=+gDV})uRpy7$5YLyPKKkMH zUiMS<(=0+&O>RvQCRM;37-BYN+6hf3LM2oR6_B>Ajw$gMC*l+TS5UY-}a@3u2z z#xY02S$1E_xImQwq`Zqy=~u!kR&q2!Kx6@p|Fs62{kv34R?nMQ``slzSYhg|3Xq}(e*)sR&Dd)BiLWx_cL_NeV z1_{A`GVI%6B9I>>P1I60rmucT{V*c-;QeaB@NSLB-onDd#%7fLv~FdS4IEiS#2WE6 zMzdt2V4k*pMiNnBTC`D1K}PdE-DfvJ9+*8rvET5 ziU2n@9on3cQjSv($esowZsK3dF1OelGUBN0w?+b4&UE#Q@todA{8Z~5h&EA)9QUd6 zTa*Jcrrw?eXOa6C?pE9weXjuhEzIoEC_pj4ykyQ5l*QeO!iYspBHiz!iA{~G%dqxY z+Y!TsBCw3-6=v%|Rw{_5wumHQIl%ChnHbR$C|UXuU+_b+@84jhvW>y{o&#wTfj-Py z00W_{6T#M@p@|lGD~t_lT}L1%IvVx#`N-69=@D8kqGX0;l%v}pysA#bj2#gTuG{94 zs+-cvx)K{XmITU2)?e+e@GL0lF8~ZCVgi7I;A)^Y2)e!?2~79y-aK-^etF1mPv)g@ z@i%RhU~Bg4_x=&McWk_G%=ztX7p;E5boA*o9ZM3N_IhKM*e@SKtoYA_HM^K> zWL0mBj-!azuk_Nozza};v#y)rX4oP)%k^OVLftLhtyN3P7FZbo`40+w;rR9fuyv>Z zrxnz+tzs`yBI+NQxZx5u4+HCjn_)z4dH_c9BqerGo%z#RW#CL3)d1Hotb=zWTbF4VrBB##0_yd3Op3 z@uKFMdtHVBx8>9qI{AE`$WkT!JQIa$T%qfHE$uIq`Re@fd%fD7x86Cqyv+KURV?(c zGsX9N;8?a2^C5o2QQe$(Uq8?pU1q3OLna!kdU|N)*@;997W=dmeGSo1O@5lZaUiXs zvehKXhadFi%6mx-`p^eNm&!!4E6Np;d^6xRH8VA|)MF1I%{!#lpiL%Hmc{E_pX>S& z)oiF23@5!hdzlx&VE}XW21%mtY!%ha@5V>MU*fgqGd6?OiasZZ#v#=$OM@^ zZclCxY#jQ6*A=EwgZ+m~rK!!vV6>HBQ17%8mto7Xt?2`Kn&I#&u-6{7_gJ-Y%MTou z>!ec{+#(k@hM0&SnBZ%GB_U1Tt_43w@$MDDoL>NktzUi#SL*`aS1Hlx^IsXH(%K_! z-*P!V*$%j}ap4aOO|_3U=`a0h!cEYmRouwfj5<7kHL3kf%QODePNn;oD;jJTtu41B z_f}9abzAdCfksA)Xb_sSCd<$rkVDQP2U5p%T&A0GXqs8}A1UM#t54>N*s=Oe-$aAL z(oj&UY81@X%32{Q#?L8-_I9;JKY^3nTgRka8!Srk&H-fr&?Gk({#Uzbr9cbJRyF=x zma}Y3uRK*73ZMnBok<`jWH{&eE3CVV{pzXw;Ta%MKTuX14__of7hQkZ1LBsYE&)^w*mh!>dkgt4i3MZ$yG1tpH>lL z3e6=GOp4SWA^8O}GIBqs4F3J6E^V`mo@yIuFr^Q0!?Wc2CAtl^56%|KUl5qhYPq|+&C6)7^+FWbvd|OSV%6rZ!7bB5u~S9BqxQ` z6~MYkSiMAWPtRbgwp;rF8C1jatnzPd$Li2$o6=5vHu^iDPeQK0u3uSm9-#tWmbw2~ zeGK)|k$3Pg2TtV_>bUjY4VzzqYM%34i=XLP+~pGj*pxG<>{xX7TSqk{M0R>?Z=74b zzP9o&qI{xoz7e;hq^M_(bA??5I8@1Dns4Jtgxo2!-3F_NT`33$j%UL#udj;@l2tC& zeRr;URr(rPjiJb7C|kJk%JZypQ(P|{ncZ@Kx~hEmPr-)GMhXMi@|Y46dTsQF*B6Vk zz?q##JzV#;3Q`F)oSI)=t8$i?HQKTfRf`=Urp0e5QCyXrt1v3eEhV{ZbGats0MkQC zxgi=_(I_`QkpGHXiBwQig#lxUkU>a@;2~`Mj3GP z!Zwi$a{@tdNP~5=>iAR=e*eLdm%;@rv*MndiWC5T#(yO)=GE}Sotoo$iB04+RKj>E zHsXhkjUD#vmR!SB+G3-sh1v%s(OwX}V(3oa%W|Fi?K&;4P7|9@7eF0!VMDrvkI?pm zw8_2%Gua+s2`{y(>WlmB$uKwNDA4flo}ud1zQ$yZpX>^xMbl$>CHxEjMb&`XzIvW?MJ1qK2F6w%>^SB?N=~dv6t!7B z#n2BNl9{Qm1T9p|lEbNs;Z*o&c&$|9S6Mi64Icy|jsQ`&Sk<@4Vwwa)V9eC`ki5-f z-WG8_UvEPmQfhDFJV=R`&l`wK>z%S=$!n_m(dT4FYErrqM~tMT8Y_2A4T+HQSXf7k zuZZ9Tk1Q1FnEz0xav*aIGa+zeEojWkTSFjLF|T=^^G+uV=I0$rqVSpFgg2{k2DyL? zn|p@lRA+EtG(WBY=LwYwEKTus3-{>qHTbhI+{K<;C>c-X9bJ=Pemajq`9<&fJubZCRdLR4U~k`3rh{`?(ZOxK*W%oD%S)IxsWglv zCmPhHPYC{7j_*{!OlNC1Y}4saAugxVf3*+IlYmJXSYmKVj2Pe zPfo-@SF{(IFj0OvE_>$_+*`Ga3-Rr9qhg->cp1rf6!>>USqBjwoRV1<6T&nyvZ=2B z6P(W%tsqC%R6`$zr^^h3pZ3y}k&}^uf`IB*9slF;Sbv>zdj?+kxY?h|j2S&_?e^Y| zkVK=-3r|k37Z~}(?``qboq%+W#u=7yUAP)Yqm>zORCGLh`#CV_2BrU}L;P7FXH@f^ z?3lLl1UY3u2<{!QKe;w&sJBhG$RE7-)k+@Ln#?6k=8<_|5$~-b4PKLt)F5 zl_%+3(!w-e4-t8-Kb!TLKtQG;0YOzS)Y=%i^3dmOmPWHKRS-`b(x{HUh5LGN+{{?c zfNse8kQlaa8pk=oU1uG1zEuI3;zi-SjoZ+C!I$Xt@AVj5luVhJgphBDy8kUs4!uU$ z|K!q-36iVdZ^AT}dnT865&J(az~J5l0B(DP0)mS&PSw5->VR`x09p@q(eMZ4HhU<- zGyItq$PmdKtfjjm?C19a9bKOfbz<`Tq2#)Yggae_&B5=&RoJx&^i;F|Tx1fIV`rbs zpYSoru5l>lz`qyP|Fw^kYnq)d(CZ=_8=yhQMb=qqk!Qx~SokZOeW6gSxT?j2ai6>UV;Q%Az>DOLm_8(uC3dvBn|r&5=vU!8of-q$@QB)D zHbqyZMa6;+3-mq-!%zV}h|MMwgn;c5(#_cpORbLX+bjDTIQIP6@{l`3gcWJ0%hwD6 zb~rHX+k&MmNhKTNgI3LhiCse}qWCF&;5-Xat31(>iGGq787%(a3(y8TL_E;9WPB^Z zM`d*Eiew)IGAr;3TttcxpR<=u2C<@0Zm_)ljfLiZn-ybI;V_w*S}~M)1l=#1bY3V} zWvP0F=%sSC@73K>TGZr$wTL0nEXrQfLZ~eZOPCGRCdzN;0 ztEGR#O2!`%#P2g8>|q(PXh9$RqWgI$Ca9Wms_OTodzGKQOS2HIzUoczD((XwW0hEx z-c*v2o49wG7lcf4{x5ap_X<0ImA-t+JmU$3<-aGJu6;PzXYr)c6fvNC;?E@P@!rxkOoUIX)I^YG6&go<^zs3ouM+a3YDAl==A7`1e%*0 zJd$vq$iIEappnHd1JMoNK3ofa1?|5}ZbgRpbV$aL;o2(6wi7pS#++#o;aFg5*tl_9 zIAEi+0vH9n-P)C+0-l!=Al5h~th0`3j!Q-$%-e*Vdbm? z%aQYlX4AChej9O-mYRm>f0bxRVL)Ll?ev?`i?_O^oE#1$971n`P(<^u=nl25P+&qf z>BMZ|Pr~`#$CR@yyj;qxlw;^G@4(Xe62er+N!1;*xYBLanx9eOl!8NG21Sjmicm}9 zx20=zgR8%5Xf^Ef&E(t_j?uviGYuolBlC=-@1O;#I6WOM)>AC31e{-d~n z05Rlj@_Apn?5mzjN&%n`KKd(@vcqtdlr*jscxZ-2K!PNk+5oJ#EA>v<-Uu+Wpd;I( zT^z4&Oznk|D+4TI)?vyYROpA&<)sG0MPzvTrm-e!ZJ zjkZmDXyf-9VoW*eB2xjC<)&1+Eed~tSf1TxJo8S!>tIjnBlPO)-x4P>-1%@_26I)0 zE$kF7`s%pClHwc+W)-->7h)I=s0~5%solf;zBBM&fCiH$6utEmHe~{lt!xL2xtTB| zca9#aT77wllYkc`1MtN9sdDVNT^`Y*QME?xiD_@9_y7DUTUjEe+OKe*>t}bKh&-gT zLCB}Ap@UjcTm~~-*+T_5K8gD{idO`VtZ*1^x#rxG2J!(MC?F($ioijV0)7hp#e5s1 z=R&MerCkdIWj`BgYZd@nAwc!VqC;1(2w?9i)rE9_iWKPQ5O{ufR6l(F>+bUQKBii2 zF&Aa;%PuEi?Vu2LZ#enu(VAu4PJd7OqRA z4dF?}r*964zWmR+_oF#zi)ER~W-OSg!*QcKG9%333d2B2F|_Jrxqg6Lh~T0qGjXYP zakm}k#j3_k@P8CM*WO$*>j*l&t=HJ(mDpUlCLh2HV}h!97?A-F@yohe!r46Y8e%5f z`sq2io7E_OhiMFi$tSY`F)FmE~Su64Oo0&#e!>yRf=S>wy z;JSSYviZkF>7w6Pub8@tNcche2)*GjxxnNuN>t>;SE>;i3oBqQ^ACZd9F8w)@Mh^I z)k$F$*ves|%~6Xy1FG|0ggo@Z@_6K~G7uh_piv8B)k{ zvOsN0t2iNrUqV7XMxo4`oP#EDao>*XE1(`>%exoZ%KQv}Hq}|;UhR>`A~J>Z$G(xc zFkjW_dP1{4&&nS<8h&rPW7Au%%at7Ro}6~O5*|^Ux|LrMMFYfTQQalji7lxW>_J6m zGa$6uU%<(=|F`+eYS9am8~qcDSFH5@m5=6X;hD7kxFfKy^?m=cv4jK#xg4&J|1M0& z@~tteT&BjP$uFdY=p-HE=N=|bw!GFDPT@$nS4Nhs=i!1?oez4nVey!jK_VI^Y4_XO z7N0;8Zat=y0O93(R!vt{fNHTF+=qQO@g=C!z{rfoJ{RQw>wZcLf(kU(R2SOzR@;C) zi;4u+aw+00;~3r2{G~FHLzeZPk>$J6Ka*R!KTupwI);$H7CR{a$$^1>$N{bF=c4kI zp_2Q^8cohCfH!&?$o6YK(v;Tk>TpW8Z47z z(jy+BQfJUD5(EDJlAb>LEh&Jj>gx-&mD_a_H)Dq?d)KI%;MIpIX|t0%fU<+mz@q30 z!E9uu@Q()n?a8_%w920)TCpwDQkE+w`gZ021m;^44VhjX4|faUqlbTx7lm-&{XKfR z47dbs#n!{Wp@gL!5fugbDZ&d!qx#1_{@#RVLXe9K@Zv0M5HM7Q-dDt#oW3{)@2;7pQwd@PiM)@rB7;$D%z_grpWo11yqV zvrcpC&R0x(l_Q1}Z?$no8h*(FKq1|EQ@p{>FpJVC@XrF=u(ilDzQCdiJvbcW~Q1upf16rzb?!Dqn$ zx^eeCXrGoHi$NH~9VOVK!Y>kwWIHA0BTV&dNm}(MwpdnBQ1C!t(AnD0&|uY2eDe#Z zTvmxK?ESdC7(wUg&rgi;s7FjGDG>jyvNtX*jL+b4#FMwsN4>PCo#HxCq4t3`@;gkW znB)Y_`?Qhhb~qxB$dc?%e@tTv18J(%n-fA5Bm~XOi>fEem}y*19sbah?$Bf|+`}v$ z@h2YsSY4{<5G^EUP@4h1+PAA8Gk8P_yrN?H@|~DZzMPu5-D_bNdzqw(aw#aACi45d zN%`|z+pxtOLq>N>OoJX=VemXLLy1Cr+^8+kN zY6A~0-r%#@cW3q_+7ECNGK|0{od5d4ve(r+t|`!q_(3&r0g=Avx1YjZPPt#7X9v4Z zt;&TUOBp|Yt_+gF8pXQMbV6XTv(~+`_wcXGTB}XRTFNXcQ=Ol2JKU2@vjPt}_@KU` zAov^#P)6A=BN}bKxd&w%mM6}0oag=H8n)&ib!*9#s>ia$kDp5363oqFkzUg|N8hc@ z3xn}!=u<9wjJs)cJXKPfzQkdP9lkx9FVDJR&+o;2yUP(zKWE7GqRmzW-8(3ev_c*l6-L~dBe)~(ZWO=*W1+@0w;)0kRBw4m{Mp( zgV3j6{uEyC4{xM!$b6@sDG}-aK9KTd;ka%3+s_@ipd8c4ePh*GCJ5%8k%`IuwE|q6wrX}HqIamxYz+Pm8 zv>Q`7K7rNQXX+JjD}&r|WK=wZMTqGs#1qxR*VDyOXol^%^ z?o!UwJM9zm{`~fTQ0{|ZxynCxUG64%tFh1db7}(7ZH($itU)g83_YP|MqUuSCpY}Q z>Y*WPj6cla(f~J5kcOlF=OTBmAflnJ^B_)nn!f2XabZj&D!jtF{qoq*B;raJFy!nU z>Wb=HxzH(wGnh7ZItv9Phnw?SU>IVeeMG}da%>LP7T1>-q6TzLiecsQ+~OQj{U zBf#_7m~e3qBZEbLsP8mX z8Y`hDQL$!z=nPSDs$3gE&Dmo+fL(1A84(ddrm+8xVzR#gwU$i+@4Ij!jo3boy%z>^ z=Yat@NN6ICjdJy>By6X;0yWO_#WOB~9|`T@tv;B3j>mM0u%?ZhgC84?w-M`G&v*=# z(7zSV%?-L?kH%Vav~LB-J|Gwv8RW(bT|01jdQ@fo>G1kHy$525q-c(92+LED+|7~zjZe$bV%X(Q%B@p_DbM{(+b>bXHIz*w?x@%Gw z2qI^%mizUdtA;^$oR?fo_0Cyp5vbJoi02_fpKmJX=1Skdt;7c%=vQ}Lhe(z$Dk74o zc;sKw$;jJ`HVH{b@OegAx0Rc3=abd$2`~-;W&`a%!zFOYEEk)-h5IjXrS5NFR)~Bz z?jVdCf#d1qcGYZO$?#m2sKWAB8Cbu^rLlQ7#789=j9O}$)ns`E$e!rq3j?Mp_jya- zC0ennrz0<~&k)HiHtW>vqi%dJNi6Y3Qki(c3+?}VPQD&Efn$ItY)=()(u@nAr4j!m z;;jE&l0?WqQWERn&%3%~9JYVre)?8wu=mxGu6r@UF&TI%FA%74tdwIxiS?&!EOVhf zq!3{G@;2qAIIQ|4`l%}Z)yU%yuk`tiIl-gMbdP*iKF{heKN7^KMS@3rqDHvTYc!0z?`Qy8#l3CapG8DlI7<_ z4DsNvt~+Tf&1e5svLt6aeQ#h?Zw0Sk-A7fuoZZo58GT$1=ZSJ$xXs9T;xc*ZbtOso z_4x03q4n_Zp~_+3n~-jRgkXnV+IFMT@EM|Eq;cq1^|WDPy``+W{Ho}Ep*$uLiK?e0 z)S)w-b5I)VPphoyGn6kpEYXj$TefE9W;;iu!Mut;CiAk&4U@J9Lfds~<|FtD@|5o) zSdM+P`d$ZVh&!lYtyDDUkjGUnjN4-L7^wURH!}WIXGrgRZ4oKdTVU)s{e#bS1$j#= zU8o@7Id;MrPibSIF*YCHVa$)gaS|#ru0y%pMTP$xefhax43x$ZmckWmLk&5ao~(9- z_<>ZJ7r3y=)}QWmV$pUer}Jpylf%NW0Xry3Q~Nr@Wkqv6?zGfDfhUI0{k513qw?`p(3w>!AUnU7@l56ywF~m(0qap1jUq1-e+KCIqpy z;AC{Hlhl;ab7OcIWqo~4O-;>r)*Wm+O=Z~*^1(Pa8z0Ew@QzH@@dgKeB6t)U6n$AD zSg4+n^P4t!mv-fLre4%SkJp>?=ou6{9B&ITPARviW5i=-3MCz%ANv!ze0|q{Px6Z) zs@`GQ2>sktQZ;50XCqlODTZoELb=s0u=Q6268$!rFtetM9&NP4SbYHZN19AkJPb8gag=l?cc{-uAr=dGTDzo4? zXP>19LwR*JD>#&gV{uZpYU!nE{-p377$lqgC%B~y5Bw^xTsNNVgzaHeZTcx1#^$1T zf-_~Tf8sOLsnsGBdeNn(xBWP)CTZ5_d1fZ&TwOWgwI1~lC$LO%5SET}5O(2P-1zN3 zy%Dq9i6p=o-1oYZe!zj2;@rjc{UMTY)*C!9^RC_e*Z0kQ`3&;amB+ySp}g-#F$va7%^pg~F4P7P>!J#O{hG&H!?$6}`!D<=d;wSANRO7nzb?{P?ZrV1 zLHYUlJbpKW9>WJ^Pp$^e=bQtw3OOYIihr>#>HwJC>s2%v!rxMLuIOH`iV$N%%|E>j ziUw8G9zuG)XL-|LpGcH!aG@n8c$=9B=<)h}if&Gu9~#ek%Jv*BRt2wmQky~{PV3~Q zNs5r8wP1w?MfQgQN^E0i*3NhcsI1=6I!2Lt7c zu`w}P+bJAVX019{{rMFq6Q`2N_n%tOlMLtV-ZRoJ2Osg#D-sA6tO`E&l?weK#mbI8Xr|On(<>qb5=X z5^bPT^lwPIIEjW|y|~*XrHJ~8=TiQD7wqoooL7FmJk8QWrQ#H;uQLNt_UdS^znpny zf|2R5;`j8`eEftjZfE&5rOpuU@tVhLt1VLm{Zb37oWvoaWIQ7Z5>xZjjyRXILeh<# z9I`b>9~VeXDqg1tBMct}3Subt+q4e)o1dEn^-7+JE9g;fuxqZTFus&%8>k+7;mM*N zm26puBmVYyiXV76%Bn@!3j#gK22BfR;yexxCWfJQA3i=4e4xc;7c;3ozt4)e)zZc9B12hcy@>b`!V5&oeIX4bkF;MCjyjNYETQ`vxaGRI8#ps! zeqpL=Y8V&8(F2ZN+bK2+he8h-#TKiJi!YvPAktWeU(!Bw{c+DrSc5U}YBD(184C6^ zG4LL)zGrvg6D%o-0U%auglo??k>d0D_W%$TU=B@W4qH!_$rim3tT!z8u+8x;Viv1o zD^^v`HbX$)|Cj(eiXuOId;3v1*!%L|J648zN1-&&<7E?L<4sRHRZnJBh7O)_LgM8b zlwjf(u&}T~J*V)~i0g(D${#NqY7_~3lZQ%7$wi^Y!}5CHcWXP8x`~vmSBK6+qt4m5 z6jx45>wZxHpXk1=EypVxFQwHT+wb3;B+S8r4#lant-;@4rJVd`O(vxBx8Bj${o+C) zLZ>Jh%#DHsN_Na73Zd2$)>W~!>DC+JVCh z@KK+#Ke3jTaUCB>VE;Ud?eT%=J~9;;Ab{o;7t?KN!*ok^R!bA?>i?QXggGuXdH^^& z0id?+cBww)<&Cj@8K6T1A_N^NhnyzU9WyBvD}-qJV|0u>6d6AIL;G?6z+`WXm_11p zADpw&wB{G^W;WOVDx#^1O%Z02f4LSy4_u#Zpxr3?Ii(JPXtm1uUu2Fcwl{xFIet@Q&0x_b}gt;DE$$TO$Qh+Lm`#?3J z`Y|}3OdBF?BYvHUGQJ6`5Y#R>xC>(>1|Pt1ZY!uPRpHNftTeL4(H|V<^s`PHie<2P zTCN!%8kEZ-Ge}K}$N=UYFX{0#VQ6QIm-Q-Q-S}yfW#qu$?_2 zSkqq%cnrz>p2@KH8nTAvKUXu0P@gEn&V4H5KO@>qiM6b{HphKXfPZ(4BkT|rrS7dF zw30q`?V;_+@Q~DzFJ@%rG~yeDsqYp!6}18ITQsz^Y-<>4YJRN3drwT9To3iqHG&&> z2W5%XkY$YurHf~hFsUm0M>#@wgv;ysRGs=O(<`Acrj2-4?R^? z@H>~4YU;FTMjYmPvpe2Y*@9E@j9DCP4dwY&Uz8+94;2djb?eHn)wa1=VQ|He^B@&QTs4^q zgSR3(a7`C*`l|7~haQ1)hOozI+cwPis_ZH2ma0-bb(kfX`sK@R`nUK0(EWFP-irsv z({jvxeHk08h3et^pbyPh710MV-oa5lO{!QFmsDM%7Typu6#sx_3d}X?d{IeVP~ifL zfwK5OF+lGiaJchTRy@Niu92?qKRww`)EQdKjwEa+qFj{;XFaN-6OC=}XUA5pVeMuX z=i5aC1I;FvEZ$$hY@yywm*dVBH(R+1pZPwIjuD|a`rd4{3$NrEaBF0M*7`mADJ=I- zzK_$H<}Ihku?mxtN+SVo1tgdKC{y%T;<*ggnu6qQOgzh~P7V$*@t34>#%@L&byH22 zG6tW+D}ZWyPxxUh@cf(#!#$Uccfh z&n?C9wem9Gl`u{31qz9WEgrv=M}ZS%wcbY!Da+KbvZ^gZ$@s&~ZwX|EpwY8MFI0-a z_vXuvIQT)j0nnDUi!7iVfcF73>^4IsS@>?^ARUGO0S z?)xi85w_jwERBJpB6`mr^v6(p>!QFxe*Oo7c#SmE zgwiMI*=i(UtmDqWXz1LE`hQq}Yo2B;XpG|0A%iKD-;~hTlnn*X*N4szYP7*4=IYi> zgi?`SLy6gpOj^$5Yo$C_e~bdSo`1WUS9=F2U+_gknb-TlxZYZ^MG7nV%;ZSp1+62i z0l@TFeDCL-cotJ}}RDrOqq##wHsggwf)>(L zv5STZW>nj87f46#tF;+w3e`*nFfZ@cg?y|c1DjG+#$0t!zLqcT7&d}L(-i8!+)VP3 z5}yzIn|ZJzBiD#YNn&z=bE+m{F`&k9ojBzx{=xL%U05O$-1tBN3T1z&R@SKKB}o?_ zHS>G8Ad>!Q`6`0=uc>qDbDw&$tOZh5b)@^{P<%+zyt}--yl#)tTc43L_TeNpH(Tx3I(QTJ@}$r`iWL6fO#|sulFwo&mx?&CpbOcUV7Nh zpN@c8KB!O*!reP;bj-8(yiRO6?SK)nq-X4Vl5~kq-rqgLE&5y<58=dF?x;xFbQk>D zR$b-%U3afcvNSU7rO6_+3o#Q&5*xlgvqsEg%8vk{n_&-B!8Z4>8CElamPzVrJ2kj- zQXrC21(ixAf^V>T*39pvAYg34OeNhnrxO2~r$G7*Rs`Run>Tno+%%q>+o2&-LP!J+D?EE9A8N1RY4CVthRS##5y@+F+3o1Dx}N zZ$0z4`HId1EN~9mjRM62Wpqq??f8YA)Ie<%x4tK5TAd}+h$w~XP)F?PuEuWixVa~bR zynrwaAe#yufPS8qc8}@;MpAh-5*b>5WmN=Xz)AMPOl*UBlN_EFFK~{TzaOkws6G_@ zLUA_G%0%~vhI=hnd^x_5(K;Ib0UKzx8va-Rl>Hyy+U;q3W%Va+uj#cjFc&je7-o2&qzdo>!(kNn3e(CKKp$T=&Um! z$|D|D6|eWwD_~(4vdE#M;TGQh(UFGc&B%nBh_0l2X3OZ)P}%XOi$&8?LECxmxGhv6 z!bNo}YvQb5)-ON1jq~t}Sdh8zLDp(T9eUgv^Yf*)GfY`CxlcvN-SOP(u(0|S-==B% z>^ni4NG64-F+F7{r#Q87G(hi?++*e1NqZtt!XFNltQkdJJldPgx6jEoKG0D40vy6M z*A76gPPna1JVKB-Y{liZVJGf(zeAThJ1R@=TRyntE7+CT_pt}t;OQ^{jqHPzPcLyH zxSEvEI^8pneB@oK>baojZ-#x_dR?D1q)!|Gxw~USlkz#=7xJ+W{ zb2cozN8$xKoR|$}WFbxEK#`A^p*HBSC z&GtY0M#W?%Bks11_q@|u3+jr+;Lf7}fVfaZKQ+x?CwDCH5q@NA(}&J7toCV#- zvaY#)2JozcO|4wV*`%F1uo&sJ{1EkE^`$gnU+sQTT+(Z%r4It$5d`1)h2eXBB1%uf ztvADj64zP(?rRh+W>ysTX5r=;Y0*8|0c@|TxC^zKIzl=Coj71K;gbIhrJ6G>9dNWb zHf>rWV2Fs0&h>F}BJc|$lDL~6J*^|3#6rBq@kz@iwbRJ}df^xQG=JhJ^ub#qj=axSz!&OJFaV6{ez;+^+ z5q$A!vq3HMM;e`K_b2#Gus&pp;?Gr6tJrnrV*(KgoEh9;u!w@Z8TESvS{TWvo6+DG z7rWZ9TQy~j=aL9k%KpyFym4E^-BFWqD1HIZt2~>H_UH=#JlxjEnJ?k$t0K@OAWSqx zBv``JyLU;$Afi^1E97iH^1{g`BZ=Tzha{EE*b7}s3_2>)y*ptXGWJY)o|U{Ti2ClW zo8iZ}+3AiMnjl=S-Yg?Eebc$R84yz$Ar- zlM_WMg71rIR{@ewei1Fge>hPi3zwjI+@1U-)i^;!vT_^Yin4yfky0_a6c8~_1F*|k zkBws(SFl3go=y$=nz+Uh1yC*nDJlDJX~PTALu_@8cQ%DS%1mcO?f-UFVdMicQT1I9 zA2GC?e?eWiY_aZ(3_tQ9G8r1{_nvpc9_-*8R)`>5c{OTmXh?!T z2@Ge0yAJL@L{xb@y&zGSE0O1VFaX()1}GT_FK3y{IaNXA~2^-1MpcW z=tZ92NIptgd?r4a`8LDh(YY%KG9@$pTo}SS+IvN+6m<~R`L}-kE-c_CI5^wi|KX~s zr9O8>Nmio;L#m9`YwboX+6PR$5-tl!g@mzQNy1>OVfEixDwYlA1tnc}E(?U8;yaS% z|BLAkPnRpoAe6UjgA-2mzHdtLpWnZ{{KOb3A?b%`d=&YeY!Gki_FgYh~?Hh=teI*O6(A4EtMpqLR5h>*-A@!A3wtcHd^__!aBCD%@~J)h2w5AV$#kFT$ux?5LI`@i0uluBTtrVbKD zSQM#axk%$^o2{ibXmGgZ75-<}`&(cLcB zx7XthB$>g2CAI^h6qlQHWXH!3%g1$p4<+wACF2=xvm8I{ckpimcdZ%1CCOX}0QK6MI3~ z4`#%B*5`m)c7ABW9PkNw9h}IFKWqxbt=jl+Bs2<&b*9PokL{B711Zq(8RJaeW#Tijjwd@;L4F5v*7L zAvDAv>Q*YUslx>#bEhPewgSjLZs*%_3UN}M9y({3tqMajr_%?X zyzlJ%OY7tbV<~jqqP>GWh1%_uf3k!OmH(wv)fJivJevYlm`D6Hl;)^ zCR*eURW$u*T4*JY;|qlx?J-z8Ew1E0=9lT*DL$RLl z?B)f1R8hD#)VFkcVxtgFmWRpHO$DMnNC}8=w0#_xyj8fCvC;6(P0V95Y^B_V1o5T?9E|{{E(WlD?6(vGy;D)UL!v zyzd!mI;9;>cFhOt9(a_9ve4&VBR=`bGK~3LB%^9O*9-ne2p0M25k8uG##BcDpKRm7 zlw#@ho2FX#nl5EaUB)0}H;*r|sFRr*ZELp*Y`6B!{61n=qe-2I$wB6D{vEKxlcOxd zTdyC2O_^FLy{q@#HFq|w+)GKI1PNi7bbHD6wuUviTN6W$fM~$yARTI%WLCwrU25Uv z9};^S_)i?G9q?8J3R<5HqCtgd<@YPe6!K>5E+mn~g`CzKcrC7wkpr}+e+s`SsH5p6 z=`1(vH(^mcn8~>lIOcf<#jgW-`teMrEX(&Z z$?@EgwC{vbK_2oSBcE>eODkO;fa`M2^c5vF7KPC)qrY6=F7J<%0X`|}H``trEI<{D zl)nWdQPmSW=zzFtjFa+vw)S38Kbf_bR5Rra(>@BqI9Xc5l}8go@DkrJ>fcHe1mNZv zOa&hCk*(?yw)d0gB-=a>50N4h&ARdG;U#j z+$9r|;2r3 z;I$aBxSNi`2^Ha+$DyH!OSaKi4DRfd)YNq^ATfrKva&7DntJ2=*^ZvS2;}jJPZJOTYZWX;7j3Jb)9(`77H4Aj@e!=fm-SG*^&%f9H{fF$^#!ULU;p1b&jA+Ak z21Xnxn=o!TzM{Hx6zlO`ZHF7;eAoVi^nxllr+w_sImxb8MY zf_aiy6SjigDPWvflNshA){b7XZ#~idJUa1Q#OBA1Bs-&-X8QWO1~t5&7<(ehzb3;; zGmpC#K*fQi_uM2_G#p8bJZkEI++2uAgm6FEVo%4EEApc>9fi>vc4$I}_uQaL+u9{zh z*Fr38tQ&FYZ9O*D6V27=->hr)U~NoFCMkl4L)j?e^6c)Py&<2_DoHAgcmu`f&u`~s zLiYVY?2FPKH*1;BKIa`#plBckemjTMUs4c>DBcDH27!V)+N*o{i{J{wC7WL5q#j{9QiRDFQYBV z;C`(RKdldn3_@@OHu+tTDng}Tkqk}h$XT@ue-FsX)ZFJ^D$VZmn?V4CDAK5+9^lUd+0nro+2Uo(B#Dp|wxYV|`iN@k>yYz9u`v5>Pxnq7_N2#zio#e8 zyplR!HScQ~T4{y@#P73(4)ou~sESWcX}l7LR(gg)G?2Qq5{pCSav}VH(@z4sMiq_> zfOQ&~F2yqS)F}CNN#IOEa*QFB_wUANIVcgxR-WaKzZoB<9I~*4r6WEA=)y!Z*w)yN zq~F!F9zFX0s=*wGE{{*{I2Ap8(u6wny$+a}j#0e9%8N1BQUgW1yYtLC@__Y5uk}_3 zJdK6|oO;UnE+d32OUGoVpErO?kg9pJ{Kg?y%19(8fR!%NX-jM3lFGk6A!M<;>{*zc zYWwNPK)2T`zLU1!xKq+z2+(=0&1apCf)eT6Z%YV~@wyT+o1GZ+8XrJ6fo4hm_udb@ z>e|})9gzz#$Z8*hB}2M`)+leMF*%8f`)T)bV7$O;bcN&@$hp2r`14jUYr~xA6wjB_;&^0_gb{&F;GaNagpD4#qWuxfhx!xIEtxfaBVh9|;-UXgNGSj-EI^he zQph}?wArlfQl%qr=;(XBCAPJ>4UXk2izkxpDkA27&9Qzhzz=aCFEI@^qMIZv*F6AA zQ$Qh5!EZ(f60cZ{c8cchTB#0OQ|TXsw{GF*A+Wh1M{2gPV`V%@(;>!*O;g&VjUr&52(?h~8t9Y1|E{YXWXp;@~)*4b0^oKF^HW7<5l~=3jSn-Ns z>=}5wjJ|`=O7vLjsP#V3AM6DKJXgk7JyB1Ewdp|ws?;a0U0a)-rsn1J<7<3*l)jGc zfYFQ8VJa(P>8QXM@0YFgAT9Jn@oB&EG1hlOEP*DbxP2t74)EDxf+$r)22OprsdlZ5 zzTC8C3{EFfWI)uy!rH>xQI>v*d#K1~`)n%imF=gHC<>Nmj+;GZ6!p$zd1d7^z; zJ50UWZd8ShCdi^j$Md{d2OeS+3kIL03eSxUOm@}6@@3M}JyJ{ecI5#VENZPYKi`7E zA!=&xllxk7ihAX3&x%JFVpj9=P;fGRi-HU;5UTkaUI;m49+D-OjNA#P$~PCP0+KP4 z!{!--bLZoZI7A5CE*rL?yKJ?Kq!e)LDv3C2Df>^4d7EGC8qCmQ9M0vlT^cz}!*OS5 z-g5w}>6ez9{6+|}YR<_J4cnoB+)zZ*ZI{-8LG+h*Fku`-_q*uoHSmyJ&kpNv?x*c1 zw)_ml?&x(Eu{bO8COaREvu=e4o#t_cF|G$1v1 z`-ffmf@zjtCf5VS4oBGX`YVRb?j7e8X$_<8u)}c5>r8=hD;8hmuyD<>Pl(DNV%3J? zLKF#{cS%w@AKY{31+$YR1C>=ryS&4{Xa9}!-p_{CjHs81kDlb_gHP~m8*3^5a0>oD z0|Wb2TI;R?qU^unZca1@Ytk(jE|a>B_0_v6gk{eR5_S4mrZeH>O=RROgl%ug=!w0Y z9dxA>w3+yN^!FmPq5~R%#oVk#sNe{veBl9EjK}diV0HQ5R0d}7pJ9}qD!|%>bDh00 z)m0a5z@*5VeC;Vj1{^g#l4dz6h*Je`s6XeEkY1kxdx}YCQ&CeWk7-=BG6K1rTRkRY$uwDyyXqwV<_*bz9UFp??8S_f zD^CnvvSRH`UmuoyVSN&8HB$Ry;C;00#BdOU-IsTq@yRD%R!Zl1zp9Y;&*qq&(>LF7 z_a#g^^Y^S0DxB7i7J7#XyFR*%NqP8B=~$F%mV-5*ZtiTXzru(P=!(myTNzYU|4CR*Doonz!}sGo^`{hiV^ zCcL>b&Guilk4=i%`hl0gPW*K@UFxnf9>mp|Z(Pi8ke{IXW-AH!S^;26n(t)|4(2fK zmT74KsWVmbinJx=@%x&$C!v@2T|m@W_SR+rCxZ}!HLC9d#v|Wj7i3S{hlP+rxcW&c zxdj=55N>pEN)?D#8EeIJ)5+1p-El{u@Mq`Ivzz1_Rwb+$%&gqxZyEjjUs1lsSG(j> z=I_ye`vUZ*3)_Ay(SxTrPrrY#pkm0h&pz3DP4@RhQf!P)z9SlmiiRcJ{(&m8!FtKQ zZmiOUO4jv+6(xu$=V|`moCuN59fEtFipkTOnCD>|I$|ayJ|M>}(fD;#Ol-<{_KX6x z&WHH<7k$^RxU?9wwyjbXeOX=u~z)~Oug)-b*2<%#kaz)IB)ZUE|ZS@J^KP7 zwDn9(LORaB3UPG%m>%H$up|RW0toWbIYn6S)pboMJbeSE6A4u_ ze!)ZSU@nXjYCooXUl=x0U_=nx6b{8m_OiP1C&zo@<**<%S5{;Y=GdH4h)PE zc%=;nC+%byg5-1kTVzG+_`~aGiYrgoFE(*;aWywFFE`=Oc9B%4W52niqmU5YLUV4k zRG{o$pPUU69#4QiQe;PdgRTL?2}4+W_>WZ{9q(IkLkIx3kxotbmPp!}`KVJf@AGgm zJ87o>PH_}yv{7HVJNuOhd7b@(>~;kTFHbZMrEzFvu`c->P!9zQWns_Y*MI5c+8(SU1PQDY#UgVu)HQb;SlSV^boqZ~qx_|Ca zIIm^C{gQnldmFUwmI8i$N`dN))1Ks=*&@W+C}fhd0l=cg<^<8QR$d4MNIdVrbzaAq zVeT-~M%Q6n&aKl6Fhu~@8r2=)xZGAB<``ikwNbj>=L4;J%Xo_!y29 zi0E7yYYs`k+-@Citfby-c6=wzUzgnIm7M=a0`!Zj=tK=inxPr9J-nX-i_`UCUv}Gv z);~4`ha>Md=XP4#3;v8L*08l=>bYmn&WyzQDTpjN$?Y=9@^o2cRFtdy*_UKkt3GZ0 zTWS&>tj@K^ktO4>>sjA5*kxJP_3AjguXc^&92|nGF%+HEV;x^~8g4Et=jb2hS>(*h zc&D6FT!=Rm_Qqq@iP4?>9tzY7Ff9*+HtkJJhIiL~&dy_`PFDSJVJbJ{> zk#T*qt&bl6y$J6sCs0jadH0k~PN9f>uls>ccf|WyoN5tzycmu2O8?yt`*x78Dv?Ol z3soga+XI++Uv-1}tXbrT4Kl<&o=0B!+sj@gj)U$pnlzTpNP7R-y_z7#2hlYkSd%Mw z_3!_LlkjU6Er4)N#f882lc94w?p!n~4X>`|$qO)gC7W^lOerRS;qFW$s_&+LP z;simUm*}R2f5Y^){-=P{pJnpJ@o9Xi1njHbkh>5>5!g(L&*Nm;UGt4qz;sqt3pNfT z2@Hf~AXV42&&cDmdcsXE`Kp4seV+RF`&IIpG6V)RzjKZDMT4(b=*|rP=p=h+chlzd zXP!V)Z${+Qdasdq#i&^lBZ>=4QCpJp{J5Z#5iNofDB!uFt}Ua{2j}NW9a@@6i-Tl@ zqbBNo;JT`35B-()yf`l-aww9Jqt$~FZ`m*G2VSvXa5HUFi^GXI0zt*%O8V3$DAQ74 zkTnA)JJ!F8cS^fdB^Tue;Z&i?sn@NDw3ch6#d<82QwE5pVO`kbLs;coJUPT zX!^@;V;siK^^IRIwNUCt65N(f8M6v1kam=j0dwpyK4_9^i-wr{r0WCX#Mi>REyJ`1Xm?Ywu%2 zlrpD^a3+RgKXf)U01Jt2z`)MBdidW*16`t|uwn%nd)Vp+PxgXwTkR$=bGvBU;^HE( zeID!ld-xkjIF+d7x}{4_|DihbP`BaK-KsUC!^6QuBXICvr~0(v zZNNu%@JQd^X-X6|Yuf$84F$8aV{9ba(1q9TApLB=L0Y>(>b`-7|ctIVZ3p)v^ ztcAJ`4L6>Nw2<}VXy@f-@w@hI;m7~x;%mt4gR|Mu%qMcw)MJU#_4&Ii)YX{|aY_=y z$F8sXR8(zA_kw`Yj1qC%g}=Yq??YrrCd_(m|K4nf@cjU)xr?2}+R7sXK!X}r#Ydd< zJgi(t0t@@q4mgyVag!m)r0L62tKW)ufbx_Ue&wI{6_xhbIUH!WY3iHn>x?tOL;;sD z!l~#drZ+)^1tNq0VT*QP6_)33m;wBs^-5%MHDHB&PNq^9V=WpM$i}ERPH+F$=f`K6PwN#XkcrlV+2hq{z}?v5?h+gM5GfS`F}T2C z03s_T3Lx0ZV`1IQ`O1_hLZ{$VrA6Qs1xApL>m(rM%ADFllH6H~gDzNNLL2o`?6+la z6!5|_!QP3IJ31=SNIb2uOS(rq*?KbC#MpsJyo{zGBIYK~<43?^PR<_G28=PSts@R| z0_@L}=*XIWM_YgXJYM@61;8!?B4g^xP1&?yW)Rt8xa;wLXU_8J`cF`8T0(wX$-#^C zrj9Opu$Q}&Qxx1lu0UhXQE0{Y?h^?PlEUO;UPwv#muub9X&K3n*n(fS4WrKVt54M@zpJirbnsuyLT0#~Z>%`ww%RDuG-k^Ad9yM&Ao^Fqin_M3s zExo%x`B#(4eACtScy|>7n8}ns#rS;ui1)^WLjsZ(M7A6XJS$! z1z=zZ;@lgbG(pW;a-tV;-2{9=oSQEwL=iV1rIS7OfqUrY<`yactkuns!gRK1z-_JV z_ou&&&N1hzo?O~d`KOUDD!R1Uf>I2xHc7NP*~j@S;Hc~}sda)VS@VuwS}U{x(=|q- z1s1d4B2B;4eIqb$e20}?acDbi?FNXkPZetwk&4m=!SWc;QII4|V^wKB%pl$ z+*jh)n`M&wW=gZgYVq^dx=YQ0UMFt6-`oUGZVepaHCBUx1nwtUJm49u{wYg{&8#UN+M z%*bd3I1sI?(pj@LK+0HbObh_%w}V1FTwQ_P*K~%~R>nPHgmAPKuO+C=Sss&fbz<2v zoMtdq>{@UM#l`(m1>$@?L*mQF$47r-htTP?=#-+%LRKjz3i;u2$qBa>aWeCPqdv>M z{CNwu*f@D^)%&x$TK8i)`A5nh+l~hFo_7CMEib#ScPK@-MM^I3 zP3SGVLYMxSnk@rM<`x_C`OFtB2kdxSwR+n248J?>3m+k2U+3by9Uky|5@O=yZ%Y>Uy|a5cFV61n*7b`vjWE@w?=uz;x7}DdZC>}xDeE0Qp}1e$ znAxyF2?>w8FcQ#ur+?elmawHItjZuu-0N;~a8NVRDTn~`gFLc9!Gvha*A}xTpa@Zj zB4KjA+Ik{()@@ESmi&l?oAq^{Ox_*vI@9z2d;pUln8Vy{BqR{M~?lMsu(SF*#pH^;B z7WF`aiBi-p(8Ipt$^n$2kXxW$z1l8WGR0bl|4N8n{)P-xbOOF`#Ai8m0cBB_Gp!CG zWk(6+jTKL0tNX#g`%3@5LmJ>!LYZc>jEv_o~FhhnJ@lZTm5@= zPS>8TJqkt2dD!bl(?UZPJ4mJ!v0r}*#sZ!7t=+9mX8CKaWq0u38&Ur4l<>S%emO|jKy|MM5X9l% z#Y0SY3gwyVb;VCzbIAF3-su?&R2EW>;t6(iy6kQul{vfTc{`7ZI(n3$t&|73RQjE8 zGq%!9;UA)J_EJx?=+xGMTtO#op!&OAb#uXFA(Y6~vI7gbs$He?u z{$XIf=Z3xk$cTU|1=UL^&RL7)Ka~Oqtxc4_kkQ7Jg&*@<*&8b&I!*@jXgrkwk}tcK_NP?O#N;ru6x-vtOKJ%a`GMhUV*+ z;&>C!f!ZTJhM58LK<|PC1>wPuCg6(lt>Iax+8Gr`DaAwYLFgKmqXa_)MTZLvqUzAt zeUY8#y^8*&fu^#n(HP770g;P>YNgpd+P~z0zHE=-w6qj(SbsK7IUD7fc-oT@&sS)5 z6>au@97p@OK>M59)q?)GMq& zCqE^fdfdB~LR=txsm^)YS%tOt?nu6z-rnUGa=D5T$vA?JpNQhbv1Hd`nJ$_a!)y=%~npa6Q;WOS?uf1Z+o8E42Qf~jlF=C(W@0vbY+XAA~k*Qo|& zRh>R_k>!uqKx!6b>UgZuJ6vGj0^X@7ytRX)2LZ^x0YuhKTs49t-$KwB>=6vWia-sVJTaOHjP!Mq&@VID zR8gdEgS2YF*+MSN6PF4p?B}sud+Fs|3%*yI*vyLGeLM<8o*hNG4q1O~2d`O|M-u7b zS;u7)4SFTsWn=J&vyL%czgFOBF^Yar@6=CZ4vR+dHdo6HP1UV~b`pto2RDpwC>1ZB zkeCY10fl@_TRZ%^+EQo!+xwcteNuy|lNqa+W+Ip4CCDO8KnMG-@xhcRqlqgl2P*t( zwI}89l4XmtwxrSC)LctrSUWTJi_c@M??&FFw{t}A$n(_qfq2{^*!P7}mHX+L z63C&nQ=MSJ7o-2R3GHgf3io$d5Afe7o)G<(xRsRKsYfm)h`Aqc@KGAl`{7pb4RV7` zVfrTUZ3OP=-uEMhuc(9Y02qr~q&l6Enrvcyg0s+lJjCSh z=o_&wcd?fub`83M8TN1g`>*SCZl#6pHpLCbMBtQY>)a;(i>Sz)fbc%m$_(Dku2z|If_B9p!N2-!Rgj}E zo``%~(3FV5bFP!T53`$*+NFbYz7=k@{$virV&Up1G4#R*#gNx_Xl-qbnH4y@lmGKe z_zewtTR;rl?NpVI0vI3|HEJ7evTjk)IHT~S#D{R1`{YInudg8A3}TR>LB$%8m4<~; zjl{d%MB2okTT~M>rvQxSD z?8OP)#1Yz^(Vz8-vK$@`!mHC*W=jcJ)Tm%iTx^Kd4Gb6bc@m&??h!?ref2;#<}rKX z!{mN>#w3!6ynsR{W@=b5a+$f}*Kg3$K+7<3Ne*1%-sE3P5E^&`o%r*Ke_9jJF3eMr zA&jE-Xv9HE-rZ>l31ndjv#=OvJ9Q5r-pVFl6LWvABgrPd?mdivT8BUnBZv&JA){wd z`FCt?n2=s;!Ew91oq_h}x5mbH-ebQxQ%+X|*@H@$3#EXWt&=TO>p4I{Su?q_A2QNJe<7> z$+c8YLu8{#KMYTg&6)$*1y3c+bSkU|pHjAm45q!DoO%+ouynX8QS1kqns9hAL5v$2 z7OYeL>D@fdbX*^$~^{WL^^{&_I=B;P>hLYEKVm4ipQX+ z(>Kf5fp#6&(2I?#wDB$l7}{cO!QPFBccWQK>@g>J`FQ zim9cTnhn{}0Dg(J4=OsBtbh~KQYRT6q#7+nQ_Z&6<@~rz#$_S6_Fj*~!Lnff3%XMHJ2jd#Y4p6|C;_ zAT-Th**@Dyny|43Q)Kbp9G}|UHw!E1MhS)@zr7!zG(`0i^~|13K?c+}|B()-G!+l@ zVvVx>XIdZ@=0h9cBC|qa`ZTQ7E@7OjK5`)fo5S?o(nLtyOQ-msTD58x{Y3g~chJ;Q zYH7w!+_bvNXyN0IzJa%k&>w!BU; zY~>U8G)DjdyCHi_O}g7SAT8|ssN9~QGrIC-Vh4&z(_ZCX*~YERS5w0+-Fw+{qI@(U z{@s4RGR33+y#Is#xIz~>!r=}lBuHLXVTm{jK4u!`w?3(PQ{RAwe#Pam2RZ`p48?G1DHHTgT?s~$*6S1Jn^AHofu?vc3^ox; zZ8uWNX1LMYgjRU+DL*yCxs;{|vt?|jc@6rk-@ozt@v|hk)Pyrb=_VSw^h?&CyaH$t z0q*b3PxszY4a$FKXJ?z8`nP)}EsvFI`L1k?8Mh#U!*$pl6j?4}V$l~syV8dHQsqg=o(AT4Y| zQ2&JKe7-V>e1Q;V2TkknKm(z|s$c0pyar8zH_feU?a0@Y>jl2~1c0$REBih@qwEH} z9%=<0`y()Q_`C%HQE?64hl71slm<{ZEqjPTvjd^_CdLdHWWTR=2sM1MzJ)&+_UwG> z#yTBpI0vYgQ{NQzTV0<6am5HyRKhh2>11`9KVz3{?xt>h+#k5^1%_om-wG+f9&b-! zSiZJZu zX^(tCZkLGTwruldqlD!Vy#>8&c0iLBHideB{2}JgEFwiGjma7DrDA-=sI+mhJeXf- zyy2cJ=ThvbPDaqrACl+~4I3`qOQ|NJRQhjue1L4y6E+W6R`|-xjr1aOpz?IVe=plC zNsO^ve{?Hzp0;t~o$dg}hf+6}7Dd-j&&Y2mfNMVd{EQeuCX?gEeM4cx+jdX^l1=yq zxJ>6N-XV(6g|#0r-EQk5squMDA3myLhZhtCf zn833OpOhT^?Ep-dMxq<@mokEv#>jp0&sMsnQd!J#Y7^lt+9M{f|I^=k+WcLgSVjMB zdEG0R$+HAj>rH^SyHn_wQpXDPCCv{2JFpO*?20UlA0xlQ3{5{Eh&f4;!nnbN*a_IL z``sv3%T!on4Cy_bcSj-+Ljximkm+{A!>g2JgWSJP=^+=4=`q1<93ZddK2KaW_NE3( zmvkd$@3i%a1&p+Yjyr1oXRx!s(POS|TZL@}k;0_e1K{u6%H!v|IuOj)rn4y~QCCL? zym@yu1;1us%^$i$@7|aEcUU0{oDMtvdbtGOzEfZCxQz754RQrYskHraQUzdh+P^PJ&BKSm#Oa`d;GfGSIc+b$@{=FuLB(fgkQgZ&+8k?G3OVOn4kTEG=jf6aKa+8|>VA;$EgA~IuUDma81z+ZvV zUMWglBl&7y+MuB}X=3sFk1OFI`-52~W_9q>dhNEy=lWI1{w;x^uy6{u$0VVsmcRRO z+wt_fr-qD1hYk^O-*RqnzyV+9+4G}r4AE)K!jaEr7n60S_XZOn!cNj5xR`jvbJ6)L z>|0&|S94VUHcZV{e|gE`dE{>He{w+8qClP#wv+qsZtB+8O9~Jytz?B<*lvI=$0Ffy zPK*${0xj$>H!xWT0P^=a>3E0@Z*`Kt7tS#z<@m_*i!#u86f|2B@xHQBFQIG6PFo5) zQ$)GR7sEJwXWn+lWsPB{9Za@2{F%Tp-h>nK>c7^t!u%ngJ=G^J`I=NHY*Nx7<}7V*GicPyZ*REqL2#A|V06eB@hYY2M3 zHR|eHmp)nlF{~6hHTYA(E(0=fkG|GNzS>4qKfw@PJ51BT&t7RYk^**idwsh3cQq$L zc=n$o+4IH`gZKAZqEclNx1RWwF?gvuJ95z54c_&K8Jk2YC@2&pT339SaDSX#V-WJN zU`h|~jpJy$B9OZ~{7kDa3VD0pU~1u;CHW~gvPrtee-ie}j%vPNi&AK|ed9Jyj?pL8 zmq3*bSzWdw${k8}%4ZTq-S9l+`2Sh}&uXpz++I`AF_fCMUfb!6B|5*>2_Z{_dZ8fE)p0AFg zWCrl2UM>vb3Qr0Axkd3-M4m%SM!>sLH^sM?axlsR%0Kbl?&v<||EyL6)tN$eWTDRHQxv?at1(c%iqfo*$(>k}Pzi7EQ$MNvVuce{JP;TPgMV*t zcq0^ucD$`Gkrk<370xwniUBx3)ZCwY^XbS}BhPP%&#j`koKL5=*EOg|>#%RjowGV# zGrw&F#DOxYAx`D_wWv+-|XNtb8i@f3v57#H79NCWmyI{?-&`5zsQUSn zv2@&0+!2X8jfnVK^MF>gXIqcjE*t+fMNz+%o;3nDTvzyf(pc+B>~X;Oy&h7d-7B9n zcHC1PjA&qqE`X&9owj_j~!rR9VIkJ9)|BYhcKIFtKJFd<{m+axClHnJ1*2Nl4t38l77s3V0(%_z?fs zClcX->DzJF8@r>}D?-n&VU43A(+V@RL2+)UsFl(YW0dOuIlPhezs*0i00!XFM#*n6 zbHE4xnjm?eXN_*>WhZQ6*GwLapv|!l?v75H*av!!+?MjAZlqm?fQ!!D!|6-IKk*CE zYm;3fuMy7+Kw|`KWKv*9J48hwK-qw>I^S4{=(W@ptHqpx{~`X@&fZ=r^bNhW2hFn- zfpFH5-rd!YHA4FXiNQ9E^deX#ePqb0$8Ym~yd(YE)yAtPXkB9kWt;^jHwiB~B=3lS z-z@!o;z~*bh}#t9ZCXUu*)Y@OW<;W=|0HrJ!Xfv&8Gvpioa>(v{9p8G%qLl3 zjJTr$n(FZoZjn6!@FWvjWoUB}p2}eZG74bf_m92lEvg-EZT6^p%>+i z?E7` zXh~SyF(KJSd_Z{6)RmU3o*DjnW_F@a_xeois+MMn#WE&v z3`QIR_4HLzpjF~?`o|o=R`4Vyd5peQ9Y1jJ&i4Hq02(!QF;GD%g;i8;)mraJT{S*7 z26QbOR&5(w7r3vEfz#!zpUiGOHat8W;I06}t}VA$K?Dvops1LW?rSOCI$(6?cD9b7 zOP!v%V(IEAni5CfszuuD|&@{W+$5I@p}M*BkMCWipHa3i)4$|LMC0 zam&tAf*8=?nHpm$Wz$2=@p8-oc89CPZUwW5OwTY9AJ{loZ7Mowq1w<;dv8hiGZbn+ zzmMlIS7D7aA8VF1VV^b`+I+>iyXv-vQrN+uzDj8+nMGS_ELsagzxr8Vq(6$0HRbU8 z&r~JBmzd2!^~}vHb+xg^8J0=ej2pc|V-*CG_Qo4-0ChN5HfK7I?W7^`1rNI?c#ru@ zQ%fS3J-WZ}T!vkHcnZKGInzmF_g7QXmqRdeb&Z*jcIP1u;;%G`neI&Se6Y?t>P`CB zpJ?3zp6=0bcBfnZfSfCQ03pn}AI!fKs*v5G5$B$OxKk7wCfj!EMc6%2O2YZPc1R2urF3DUf{{- z#qqZyeB_l2ucj@XX}tm-1^W;HK-HbGMQz9NHuSL6x^Q~N^g-~>@m00(6+Zfb6nI|K zjQ8OUx%>G6E}01=>08}3_Jh(_r%xzxl|jXP8A>y5!77c9UB2foNmMMA=FQbQ6k0U- zbCdUSk_)fc^Q03{4H%KFL8@mIibsl$GsIQm55L3#tVru7Vis-h(QgR*+97^dhuQF$ zY!}AUxUgW1>Q%|%^X)`*bTlTlKD zWzYE-zETH@zQ2mi&-GL@HJaC7F`Rpbd7KW0#5C4qGwyBboG}GlD<>z_8HFf9&T3vS>$$%Qn zqb6$&(D073IPrBYJv>Hq|Z%6Ynk zm>29it6wyG!ZW%Dm`kjVbckfNS51o>EafItS9IT&iDJIHkBzYJs_Wd^YoL4E)6Q^VZ`-GEFd?HznFhx%9rlV7C zNV=RxHq3qG-;h!@m2~UwPTuYSXI*L{RFv`B?5gMSZ%pI1RKZm)R1N=SJgO<^7d!RU zZt}O7VLfu9QN73zul&dnQM&i7e0=i_wfjdhHwND?e+hx&Uiz!YfVK^S(NhEEzmaOs z+6%&~X7Knc$E{RT3m z^SL-1L3o%Z0IK-Ud9r1gGChhkgG6zk~oNKwn>vSARl>aH^u*EkZ=hUmr*ymGx|7LML(HqNwkid-$vV|9wY9pPs};cqt5 zriaS0+lOA876dAaSbCZP6qs;|I@G?FWZK;pWzJ;Qks23nnz1_U9cpB!$k*ZpM0scE zB#4~=U-;KluC1TlY2PieYBo%8JRulw6oY5AKBAfZW2ryU9oQLy;sri!YK|9}7o{z1 zE(}C_mAa;KMc?vD%+wcuFu@av*RFGbP~7wS4ID3dgJ$9W6b}E}*b(T<{^V7-Zw>c7 z-J5vyOE04p!OEZuka+TSb*+LunSb0=-~N3ZH3A3^00O;gER)ozzdsXldC2Cy_HOZK zRg4Z4GPz2#=u#)hPQzp35Uc$ZK9 z;}$p4v7j-FdZVIk{N2J7gZOtNI1e9L@ugj)&VcEXrm@weNz|80ccb=*uC6opNYVr# zI$1x`3uxCf83JaCs|#*j&}~*P=@kFHn1{_lJJ&Oh?`5!2eyKk-Z{GmFohDKpN5!i2ES){ zgxUmd=|u&sdcp?tYtg4IFnyLy$erP#{Tn) zDdWP*;s^u$Il@s$a~=Pe&Bw(2qfAD=bjW@$!eXJq*8$#tABht-^`51}9yCKax(5)v zRT*}SUn*?ztZE0&;Ux4PjyA1iG`;)n(~D(S75NrEE5;?A_f|Dp+VuWJ)Cc2yoQl`2 z)sxW3#BXQl9+N(0CiK0O#n@$|0ONp*;rr%5=rg~x= zG9WcRKJFn8kwy9PHfX}rQAG*)Pcmut1mgfHfGu-*aAL#A@<7YJ^I0HlW7?e%V7bD&li&l%|$kHNlBim8vuvR z5eURvdiSa9Cqt9I);o~4uI{_$#=x|8oDM#f^je{%YauOUL-e)L5daL_MQ<9T8{bWDX7?|~+%x7qE;`@K^)tqFyk?WGr zY%GDXy#Zh7PKN)d{S8Uj_YmIe<1?3mg8c#)zc6h)y9G88fC9k4zgTM;O5}340*l(x z_*XzUl@UBiCF9ujAM%#)tn# zy4os6`s^wiY##rH{|gte9iMrhkWUi@N$fBa0- z0icNkpd?c3E;-rcAbJMZP1(o^$MD|C{A|)g_s9@YLxm&D54%6oiZ2`K-JEgAuUToh zg@r@QLqYbyng44I8kW4B&DHz_hIuXvodGzY8{m+jz8zjr5P894NbwBsJrm+Guo%0F zvx^KBI$d?GZjf@vnWjE>S+DapdfaWdF92lubgM|GYPXA=~mH|NrAX&6V*Ixo3THPX+wi5 z$cUJ-fuN=7r^rOc7`-`;OSXUmfVzL)Tv~ejxjHRFC zH_7@!1TeU{iEu};f7z!cDr~tkxGq+)2|+8Bn0IV`{D`)OXDF(bE;4J>TIv7cXVMxT zD*7$28IvEy`jShN{)cuCt+Yrgz{VdLAwIT~1_6-@LF9-M&D$Q@47)l+{UDdNSgmc; zahfd#TX6zXzr0V6m+ko<^zfE1$$rL?b-U+eB+B~Y4m#H>`tb;DF{hyd!Aafx8c8Kw zOe~FM@}I_9{L5ymlJ=eo`O~8+J|)XR*V-^vPfkw_t2Iy#^S5oO5=_}c6z@u?i8nU0 z@j>?6gZP`&?M4ZQAx^Ir#Yb89vMg6zCb*JK7HB<~? zohSA-C&yl{1UP65VJSO`DVUTiNGn=$;aCUNt%8B+LAylWmdsfiau|aNmqR# zTPDZMD%YUhmE4%w=#GFHlJES2uo5e~|0!8=j6B+imZ2by@$%EF7oQw0IQ^C!<^DPGD z>?`%kvLru7gZ49rz#L%&gNa|HloAF_7?=WY!=B%k91Lm zuMBzqNJOWR+q|nqo7JGr;ERB1CL*MjIedx^B2CNE%<{};J3V|efP?W%OcS|svlxMt z%Q+lUhyy85EB$S^CN{2}J4T@8-#{|SEQH4FrCh_G*^txXN}D+K(0!n$r@``{59meb zLza9KnFl^)&@O$q`z1w$$nhyi8i%M-fBQHCbE)uS+9%2OPCz5rKD=Fa`k$3Xo-jMS zKsX0nd~}n1hgWwopOtjmCQu;^rq|{P0g|$lA;FG1FS?rtRDag%j6Mkv`Yu9$cD?b| zC&^XquwVGQ%22Y9g<^d*(osHLyQaFK_|WR-ZlW)D78e@TEFUQADRQls?8`12lZ!d! z>O}kPGT~oTLRSf%y3G*Ef%ve~9f}sA6`yf~%R=b2563rrnkG_iYLq73_8$bsU-{aZ zi7+X5twSgUOLRB!R5f&Y1E>Jp=o^QNZ8v^t*XnfjSfrpj#uJ!{t3I zpJ;iH4s<%6ik;Ze5xtU1i@|P;4O-0WYxgaSuw(XmIx)D!siMfooz^A2N`AtO^u*K*0?y+G#o>ZT;!s(Tw3-lzd+Ypd)>buM&EpdaBOHJI=T-U)_a%y*iH^(;* z=d-I?a0QzIuCmOG+Z0E}?ylgL@%oNuZU) zl@FI)n!bYRcVA8(aA#ETh<@7=GZNaTYL?R6_)x38O-%i#Pvd;6V)5#bAQXryWlR}y z&vyYU0bwZUMJkMt%W>i4)pYxD(ThPE;0J(P#gU$AWZbz2Fw3c{t2^pxGAEVP%+sMW zDdb?kj_d>I66+47Uxs0qJC4oTtS5{0p$9#HmYPT8Zj+=V%upQJ&#{@c3gxWgk{rW$ z+tR`!fXW^4{rxPM;kUIpI$q`B<;4N5u4cK%MU|HBFE^g9bBUWfs^RXJc~p6o*s&yIctu0V-`4G=Uad%A&)-|7sC_wKK6k(2H6V!JZ&E(SC z4^ndl8=^`X!a|sYS`6Lxq<>slSL^G7SllexefOhY+b0=%9c{EULX#?}v6=3?A1Rsu zJ>FtH<@$tm|6W#~UhYAcu^^J}^z7_!<#!cUflVIuRs%+ywsJ=A~jh2e};L zFntMYh|K9W{F7^ptssZ^G*yjE8)aWISO6)nk8@E}_(X49v(wB|S;^+C_-Kcr-Bs(3 zrL)6U0}3&7GBbSTLIgBBRZHTKE7NTS4pcRB9;ZFvkueuT!fk5%BHWUNokh#ntAWg;Zm4 z)@$Zu$h^jS(>VIEB2o)}#Od*F31?gUwt2!qEFxd6^lC`ihhURoOCm2$1y^GySwz&z zw0W!6RP+%rrK}@-sh+HZVqKov4{BeK&wCMfwPYc6&iA8s6bDFu@BFn3*ZXe3v(56m zqMazTAs;L2KQfMRH-_&om7a~7O0l|KLqgB;7n*B*66rtwK52M|rH^B2pXy$u$yW_y z23TqRK*KvQf2XT}xPPvPVf-%Qo!E!Cbx@u*YrZZdVhYU*Q=+J&!|>SUZus!vP^&%6 z@0YPZ-%Zu4#CsYT@$8I-<7+&quH-G{>pG#_{+wS$ouR;k6D6i{f*FkXIj&HqXu3;B zL42#bs&IxubsClj77G9Mg*j=&9JV63^_4lu@I(9ZdUzhgU5X=@m{>NPS#sSb(TWQP z2;>DGI!$jd_rt>jFb7j@HdHHA3zyySQ{c)?MLjs;oTvQFST-Yk3YmdIUx6~c4l5f6 zF9H8ee7Ta6()>9` zefGdUmu+l%MZ3E{ONC$P-yH8RLjSZKAIu%j%#3v94M~+`D??O87iI7gqLj|P@_Fy~ z0(S|$on0CyRx$-WZvqQrvb@9`tgM)Q<-X$)1pFFj(EX^+aE9#xu8!yVJ~xQAyHNXSOw_xv>JPgg3VV< zE)XX-cf4BOg-`xcwZ5T1HDL=e?Kiy?zBhTUM&bls=qSH2&kmAQ-j$<@CL(`S3Cjt` z3Jxflhx8__s!)b!I*m6E4ate12Xu1W%6bw;2UQ~EPfr3@Gc`vSp0@**(E$22*Q^A%I zNB_Dv-)a2K4+6X`%6a?9LN=D_{Ojzoctnt&=Wn{pbsT5>in$ZwwN$`fOAnVh}kvIs7MaR%pe)OsjbGaTbU5^W&t~V=-KVNyCw#gLJ zGP3pszK07V^Ok!z>-ReWqD;CO5EVuCInAyXk0HINrgy&I>1z{vdwX|x?K&IQ0beAf zJSzq1aM5_ohA)v6;JrL7l4J(MZ=eblMrUVdm2k)l{Bg%o5IvyU=eqnK%ADHZTABT= zm{%&XqmC>ODC)T3h5ZvAGHg6+1V(mlX?I#RG{g<3`|ph)u%^5+Zz}w`6gqg313J6% z%K1}xGBVg_)>V`zZi?pV#o9kL@$%5jk8oU`Tiqt#AX4^9v||`a$_T?qc2m&if<)fh zS=n(oiJm*ibe>lZH3!QN3?H%C*yPf^Fh5cE6!t6KD{ReA{?d4N@Q5~ex>)Z7 zBB4Ixx8#tC{!z~(=KIob^g5y@gR(4N$Mlxw_X2ey4BzO z??;c2Z~+FhGi>G-6Zc(y2`!zONnm5ksQ%gt6j;$OyD(UR2B^d(?M(C2dvuXf3z}q` zU=`*hS%w2;`&{x^cmJt}9X`AH9ElUu%;%>E2h?cog5Ck<49XauG>Pp@q#y1y2q<>s z*4|yeq&ytPdHML%+Et_l#5pJV_sc}tgNmMq4D%iqz^K8V1BuQADFmt zJdY}+@Mqrnp51l&%iN080T1zQe}B?QGsb#*A>9ytzSH;bA}I+r@7Q-7HP^OhI&MY^ z6&B#|Q{TGTHcR8v>a&7}mxs4-r@7Rejxk@T+ge)+d$ePSJ?yiF+jgVR;Q|1UY`?DT z*ro-ov}cvA`8BzWtms)D=aywRjO$$x=e)L({F$piczb>QwHfFi=Le~Kl}IPuONHgs z&c9XNl#24J55;6VQwKImt-P+#a*AnXq|x0LZH5Y6y@!Wd;#q9`Tk(ke3Ik(HmgWnU z1LlB<=Po_&cwG#&TCfE->LzL)tR0S)me+_Er{^7CK%6H7lLp)(x@W zOjN%q=Y(F!7wFQno}DBLfcyXj(OQd>t-E_uvxmpu;N7r5s`XRJuy=$FSux(UiAAT_ z62~?!o}N_r$T=&Ebwxp}B^O_h^`5VRJi9zeHIR|wfew$~f#PP!{qn7=t4(Uh@k?uC zYr>0uYKN%P;qGW<#@k>`kzC)BvO@FAtE}ajW8v!K6@?di`hLt{HbO#Ect=qY4hcs) z3*7t64tH*D><*ADalYIxS&MfC&d*1``)*?547_0DuS7nqow);lKHgY_ORSUqzZO8G z$;CSU#d95jf%Its+OBbIQ_8DC*Hx7;sIF0rWUUlDX*#4QA|pZ2)p7Q*l{3qUv6C#> z-yh)tIm{OmP3vUSHgNF!36OXaq8lM5(Znh`+!|z3yJzPS=hxNMRjyTSA+%#lHTkgc z^0lj4gp?4bZ*5(!`@tDgJcSt)@BVcIVIejpS%@HTQ1>UYl>g1`ZREl&Ybzx`y4ELY zuQ&)}m{sXqZbt{DcukJL2R`uchw96|8a9$Q6Cj<3K9B%@fJx^0rd%wAsPb$|X?GOSY)K-qg$Mo+pM}@6UT^3vZ+(cVx#+aNpNh zRW%=1s~)!(Q3_n_rx8(Han7q7y~d*AYW<{T4unp-1`=ES(&>(kKk&+Ly>R)RF^HIz zkuhTq35Sxio9kB!N+ds^JK_rw;)aje`P5&`rtHG6PaV_7e6Px%ES0_h9)c&|2j%!m zX^FhYJ3#3;Jw1KF;CsQqP5mpoHQFUD@=kl_o4tKpwN<5uDNexpQiC-q^3R_wdX2XC z^-+|R=xYH-L)3kHucHBvbo0CM?{PM7%3L0X z&sQ8FO~Y=vJ34p*Tz7}yWuPv>g z1b^?@T@1dzPw9x3!xgWCY;`exhz6+dOV1p{;M-H{A5B7yesb3w)uRj#3q=9DyZW1YqjB1M zVidkl=OG~>T87>l^rYa5!&OJ<<_r>B*@*)1<* zU+nbqF==}^lujg#Y>v#jNKOt9FV_Slxwyx?J87?gJLkotoK<7+6 z=KHm~I8V~JBF44>SC_Ga4!_2mIk5wYB=zt3!|de7n;9d-X0I<^z9MX~Ec)82uxQq5 zXJ%&Vd!7(TB*rCTr?05;Vq;@Z&X*!$BzQ}_?njoJ!wiTgc<&VF6)^7d5&XPQr-e|a z>ozB^NiO!!3monHyx^;Z-4g1A`8Vo9c=7ACh|-wcRv`E_164WXL$}$=PZ}AD6_TgT zXv0j=M==trCyT;&slb;i$Y&ZUsqozE^Gi%Dg0I-mIL@5EXR*H7FI|oQjz%OAxleaz zJ-Ek9NssW`@co0bhkjM1-wh4tIc#ifAgWzL@!!%kqVw7w(^P`>hlilX!lh4YO7B#x zG<>aHVv0mq7ODLeyIWK5y=OWVF{WXvcSwXtIU+lE}JKd9% zja+u0?n=O4;yYVen;fxrtgAM4d1nt~PRTK{(Nwk|CTpPk&tlp?ad!W#mt9Z1`3FC>1w$rXmWM6^2Gd?CYjNL zhc8Q>z$~poGcz`yo$=T0O6y)n3T#oMay4U&1V@n%TnYk$uX`WElU2B<#2Q4f460>+3uT5PHA| z=Rb@X_#qpeoghoAe;q!bWTZzFITpiBb-ww1Ui|**3p$$UsM9h-K~-FjNHmOLtY8V! z5bYnCPP2;v2gDWNX~Nl#)92rQY(AWjet*n z4W%;k(&|!ISQlA)Op$$Mh>`s+BE;w6g4t>~K^#)wP~GtHy1upE3WBCpf%L&iiywv& z(BbOi(Y5yHYpbAg7Mp^yLh-mBaA)QF5ub>5V8hGLZ+Lp_@^YojW_&uE2^0ct&V)?DI}(3(?Z0)c!`Bzq$k*_Ap50=h$_0q)8MK7+;PHd3Y}tPC z+{^QzYJL4{+A6tQv{xDF>BvZP=W?%5lr^ktVkPo8-*(vd<`HA=+gVvT?nA3OMJ`a1 zIyW`d0}oKC^O(zI1#Ow*j*xnrOPEmS=&FW+{|a14G9&ZD9Em=u;vxTCB#6BjQt7dd z#>9U0>LSP8mgrLxXO3s&KndDHw`qHg{A4)o=?iTM z+U4ctS#1bPU^~wR@!BUjhKqK+nN-U{u6D)0`#U$io3v|tG9|O`e#evJKbDbK6+?=k z(2P=%1K%-93eyNkb5{^Vt9DF=sC-_irt7deyDJgA_y{j%`l(hbMKh4|@&pUN9YLvG zSyTHeS0f__e@^PbG#s8MSB9DBWW*+}PvHyJWnS@&)7@XiVU~+@{&8>oIc*yN%XL|( z8I)35KbO-?%^dznzxT$EC|II#`bt9~D}Cdvyv->_1fKzQu0C}CGE%_j$-Ud>Wr@R9 zfMoFP%yOM!-Y!vU(M%~7aw#>Xnf!FR)DL$`$+&9on3D=KGj9(?Kv%Y<58C9Q(J?0A zdz>a!^C(U3+0MU zGB#O*a?)Jea+Bj{D8`l^LwIOy7s(isvV74zkAT##I1 zn>Iypu*pvAL&F_)XQSTCTm@AyY0mYWIBK2Bk@akYJ|mim4!TUmKyDEr7)| z@r(2v3K5?x7TND)|HyvD@wJXZJ}bgezN%C~d)0v+GtjQB6xQlV>PrB^{^2fjba=RR zcqye!;$QqL$BOcu=pbQ3%zaI~7X!W~&95btYIB7UjZ#(y4NLpiDQk!FeslbFoIHH= zOl0$&f5%FbFyy>&?9*a})etr9ZJ`j1X1YOGt`Cxp)e0vMnoD%eEXa-TPY{W*F%m2I z!a9U!x1+-9@@J3U-Zt`x3k<mue)3V!YN4H`HupmK%s%@1mXx{h2a|YYZ0kA)Vq8 zn4E#=^&y~ZGB~U)z5K~}7=wmWd1 zLCaA7{lkj~ndh5OONCefzn-hBJd-R|%hG>Zh z^1+mw{JFS}?g=DFv=bYGLrAAvjx*zxGr)=@t8lbEN~2G0$|R}b7YZp<70C$FLI;?m zZet_~Abtn$mdIjH>#N$^>5ZmckHu#VGpj;i6EjuFKdo1ARZ&=`xJSRZF@Jg}W9Oq6 z$zv3I&z$){qEK*rawgI2^!q;|FYnUdc7nF7@_oJsEop2l%>m3q) zwaSn+N624ibrlt{FQhQ6K1TKJiGIJufp7+yg$K}s&DG7Bahg@SM&aMTe?dg3b?25# ziy1vVeE`T9-QL-m=mjQ9(f=GT!RGM0*B+@zFOg5i`ISSd6n(j!@07Dc*lo%zm;9+J z8;R{6k>&hRj!?em=Nui+wr8AELHuD338q@Li(cD_FmH z!QN)P^9LJ8fb=p_wG_rp83U{mZ%xGwOjdkkdk*5Kl#CDzMmKA+m9bM__cME3 zkh$2J0ajMaR2}D74$ZXfr;N9wgqZImA|m|NA;3cVU}uN+k@@X8k7vizHw!?11*^K1 zA?oHZ-$oh*2FiMwWqJS_usqwjG2c>{GwCquam$`JgLw1&r?RxA|5nQme-V)fA5$C3 zM|*(d)ju#m#319hG5V01WZ}+x5y8Rq)_Z?}yFVC4s%IGy0R}FEkt@}jr7r7$A z>On>gqoTf*tN7)&m^76Q^@&MsY1I#EYr)9Y)h7Mv_IUH5PzB1 zr+~ju&eiYddzeNmWwQ{hkk{F>bCvrh2Hzs_f`i?*P2K31KXM_$sl0*%FM_tmpwkdJh*1UGwnOl?09Rn*1xeQ zefD}`O0a%yQDa2`! zxlH!!k6*GeBgezVMt?FGUppN*NBv0L;EcXDiyTq$44|-Qx-Y&tbx1n&OgC%Aju;A- zXwe6q#8P9wuUH$Dv>J{hPYhly0FHm-<@Fb|_GTSR`{h}DhtZ?5B=W<`Mb~gP zxd$xW|L%cNSQM4W@bHv5>IO*(R&K#K5-kIwKwdW67Qw;q1Lk(H@ z_#yc-760~9l@ZImr-7(p3h=Jr%fT}$nv%e7)ayESN;`&*RN-s>S|u4Og=&s=&>Q3Y zx1LaeWwOAecvCG&I(`(U(@eUxzXH~#BIBXOS(UrWaO6t$l;xdHgOsDxpHqz=NZ zcQJ*d_q4Ihc`5Jx^~HHa9@8gyzX^~-fWc*oYD0cgq8Tk>%N3VsNJyl3d5EmpQ*B~Bq%M^cX_%id}IEL0y{6I8m2)$*!rFQ{EIg%PH%oaojIdWpu zb)=HY`xVQ}R+LMy{aG!h+ENzJOHD!acw~93lV+>QIyKs}4wJ+;YxRF7b6KIeT!sb!l^ zNA^4Qfi&}#r==*p?&o^El$H3)f#ffJeSN*WW*nI>#Ny7+?;G#au6`n=rlHBlU5+x6 z_khz&eDC1IjKTSL@}kytEk~8cuK&Xpb}3Ei;`sxR*%Vpd6VBduo#EDz)4M=n{cl5@ z9b)s=JP=rZg*HER&yuRQgFy$*bmgYllBg0Z5PZ1OxH(@>lJB<1^#3q9KwYqc!Tpxc z)6K27==j!J<7w%n_`7$QRB;X?xP$3jGIjU>#to?nr7dG&;MA{&3E^W*7z2AUtF z)jZl-yw#3Nv)}w=6?BP{G#$OXupw1utx$;QZ$ik&>I&ZDhO)T6WW84LN!98~2c}aR z^&Wx}sSYOeHtZ+jFyoEHMos2dt1Ugmek=`CK8e&~voAE7I~mdv5W^x!zl70=a553g zy!lwpT<&M|W!6OyJN}*@a~dz6EKUdgP@#5zu|=*E+Gw5;r7?VRR-)xUR=H{1Pui z*({GONmFD?&@;U06UsgsTH5!lw{$c#?npMjv2e-QVypIZx4^)Tb6z5a2Hd{`9?epY zDmy^Pr%w#cPbc=F{VadF{2<~`={}~(;c>iLc8%g82?5fe6fYe&U>1kuRp?Uhc{)YY z$P-{@hOVRC?^#Qb)y9nEY==!d_FKv;DC&`0H)Z=^$PPIW2Uy@5eKmw%J^_M^n#|g3 z9bFU)Zit5Jw4CAXHk#*KCM#G~>p2iWB+}C8P0jqA-D-bLT+k8_Smxy1U0z=UyEbed zit97Zd4l$bR@m-775g3banU9(oql5cqxv=##<#QF9kg*A4j=kt#z zcNt*9W$}Ru8hoC#F_g?PA-zeLvx53mhi_b|O$Tnz$I>t-g9q_J2T{w2AEfR|$^t%U zOEbbW^Hf%pjVS^)IWv>Cqoo0W?~M4F<~Q3mh4;?c?dS>F?A^Pv`~}SaP@?}CN-DM_ zubJwRPbhz0e#k%}hCs7wPRN(>X1#LHA0{3$DBvV@dU?3NlUYx9ND|HzxRRMcF_l}T z6nt(knzGV({<<2*eKE}wy;Mjl)$OvAi`nu5lT9Rw1#M z&s}T}agWJola5O-v{Sx;KQ;mp0!GJN7&iqz7Hi^wucmKyz~!rkg$d?ptP% zuOllCu7Qq|suJ1#^XudBWI+gcz?S%c7w6{Tl|6>38aaUWFJq$BFy+9eh`iL3%l?CL zF!X^V>-$ZpWlNe!@;-oW9YVz8!yIJ;A5v- zh_)(3gQMlgx@(y!DG4uy_RClpd?35;c@CsCL25jhls@XeTU#E7U;Yfd>sb2c852Ot zdt($8$g1{Ih!qR6mDyDGk_P^-TQgecy9kRYF|=yw=*!AYL!$5hs%Lk(<($STU6uGr zu0vDe+uxs7&8+2#d~Z#yOd_6}%{Cb6Nm61W|6n_w0urRi{_Fedr^25IRo#yUXJiUL zwpODu-_Wa7VFu9UbL#VaTB>2Ek_hhYwF)J&rUJA;?!&Yv% zuRpG5UVZPllKhc@sYD%4E@Uk2`+I4Lq3=deKR)nPZ{l($i*#jWr5?HXW(XrNit3|3 z28J_3!d}Fhg-Lhx3(LTuk2o($_Yp!f{DUsjG);ce@JkSXK6sDpW@q)cJcE zz0BjO$;rne(rqHQD%Rc(vn*}{FYV>Z&jO@~4=>n)3LUdmx&v&jR|4m2$=+FkgsSD= z0y_=8RY(+GFCZTe8(1=1+}suz>Puk)i62JLlb=89pFEL1`>iUaN+XSXU9$#aL3n+_ z44Elm2f@JYF8MejFSeH|F{JVw$4>Qo0|gh$!vkI}_zmf2KQlz^Qve3XskG~pzd zkdS!({J9TK&Zrv)R+#%7)n_Dk|L54NH(!DsQ&M7>6;18bdV}Sz%2)==0mo<6&Bf*5 z&!3kX5&k-dywhKaqANWJLleB5xJlHT9ojtJ^n^b+F~?|c`(8FI-sCjPe_iw-WcPBu zlqK*2JczISi(?gFOusD2xi!ah2umW*#aKP-F2hwRt%nA2Kma4*)G?u@r)M{w7|*%cb?|{q z=U9_GWIfC^c?{Llx-P!NfTV_zheLT0^!OH$Y3Afo&{cb)N@n7!8r z*Rvh>f(mu~nC~ukF5+mZHZ}*^zO!zTP_b7lL|WdWfb9*qbpLQf*5JJaEW<_7=w@1} z-;K~5?6PAc1FeDlH{}|g%l{smlP+4Qc+N-c080V;*7MAakwsC~bDdX6@oU~Un3>X# zScvb5@#rI;;Os(;r1ml-%KowLZZa zHm&j>c2IaSp{~vx^!x?G1jfzQi)X|5yxc+^imye}K5ZvpWpiCe;q=*%n7-!buA&^` zjOI-JuH-G&`ki>3Dd`7g;wo<}{izk`J>HZS>e2Db&DQ%;j z*r&4TJ1)_)j?0aM*3^{v@4J^43pCCKN1-B~V`cCi1LnBGQpwB&#@W*cfJ(Sc z1n+=`?k<_#1KIH@2;kf4Zw>u>r$nNp_vLcSVOT$=Q*CvMmfqH)BtT-bTky-nX;8S2 zmko$$1>n(7>;c==rX`m;z!@+Dj%Xg>=SU$)$1sAZ|H~hh*ff(ORX@ckQ;dtB7zb#F zu?3y*v&+f&Qs2e+bUAI>0nI_yMYf^2D^3Jyt`55-WPyzfrj4Kg z7wL9HH4v|Lmb8+Q*e5D%HchqkxL5~AUNGQ@nT<`6oJZv!#tINfvD^?-SG^l5^!k^e zX^p8-^6E6B-}SMiQ(8xG_K8}0?*btQSFDW{aTjN#(_>R z;Tc8jR?Ks=vYX(R;cUAsG|LzG$rFEVYO>{y=SiX!){cqQ#e{%qo&GE6DN0MDG~8ak zjI4{|h_1nXE#_>%OF1wwu=OsjP#KY)20%Kh_i)J%^oTL56DUzB00ZQ@LXQ7;gejcu1MOiyQf3>9(80{ArcsK9T z5!Y>`LOhrfM`XLk7Cw-&1YD7#qVQUMuCA`;=jPPHojPC5fbU?*m-m7RzoaaZ+@vO@ z+=CKNH!ei_Riy6PR*(CnzBW2_wWEsM=$quMfaTnw&YdXz*~!W7+|SR|)%SwHPB1EJ z2Xc5tz?0K}9aOl*B{k^%TXVqb8O>u1!T?}gF@|zYsQNTCbbN9BAR8NUR9pwvnJuT; zl}~;)(a_MSa92`gNc>5j_as|OA*czJ3m!wE4YKxHJT@U#TVy%Sx2rKfQSSUXIoc$M zhPPngZv;vRQPJ5SKR%3aF8QoI(9lOQdCHSjXq8SsPJSV{KB3r9RBHFQH*7cJp64?) zOrs65thV>Uy^qvo^2oyTmRMATilu!nx~;rzu>`WWNLtk@ZXf%w4X{n<+zt&2X4mCq z+8nf3SL=)kyeiJ zBDwMe2*ZvS6?^&wbVvz;DhUj^^p{Zs`OI$bsa?^*lnF7pM|`v)jGgCVE|F?e4@Ej; z6!v-|+n*A4u)De@z2nTy)r1(k&`4x-$76x_`FUOyMGXvKwu zhK2&YcP{Q422Ei;2fchiJ9?N0zL(|E=fiqGu)mHcveiw4HE#~FN!RvsEpzSQ@L0Em zj1ofpsyejfmhqYu83|hoJj9P&cG3=Kb2;K>-or}q>{aX<->wGucK73S-RRn~ZEkvy)d>r*JhLg^M>5Gn@;D)5$8o~S}xhP{6SQkOt z$z~O^VcIQ5dI$V!?>XH@gNp2gBHOGqlw@Xp8=>9IrEqHU$3ayUM`-;Q=!gy>2gG7T zHYloJjhQdA947&IWE5s|C<(+pksS4Xk_qdM#L#Php2vBw>>dCtNLCICxFTQuAFq0)xxm(n3-iLPG>l2^>Ci&Ff6KRyM8dGFk%^} zi@9rUsm-Ec{s~hw^R6&2Qb4X1zU|FTv28rP_j{n%nDOGX=wYB|I7!(Ryo64)V)I=N zI@NUQY;LCU!Rn*m3vGO?5J>Sz(~+D~>~_mb&R#o9)E~X;SIQPBgn2icy{)Y+XcpN5 zz8lEFltaqsc84VEoQ4SCUKc&oiD`nWMhL986I0yD3 zFEA??b8sb0Ou)xJBF~n9;cA@&*F>|TygZC7kH7TAC-@&d-UI~PW_d4S1O>I_U{#3K zemhiGRH%j1F)(-|Q`efX5JkGyIyyvwzezWB0Xsg+188ah9~Us2kEX&iEMxB>{$x76 zdKHxqpsyvIATZfaOw@utIB>Ml`F;uJh*c?!nMjDLjH8wKh}u$LQRoCC;plBj?@%2) z0{Vg*(C26gA^c{|a)p9oV&|7T)oWcqB)iooF_qw`SxAaa)}x>?2(9@wISyd_uKOX3 z_j>@gW_i6z^)cFp8CNx)4zGt;^IyimVEPCuI5;>w9Q!Tynoi?$Q&TcD;q>trAN=70 z3jQc!sd189l)X|Z_aE|4%fWWd?uotfY)D%|2rxVa8ZE>+mujEQ5onf~$svc!p+q1q zK)<24oTBVB-9(zoi|@nzG4jX-l7mcBIJ~oEZDwZX^3pfS`OTZfI(vc_(Tks+alYkI z=i9GXZlRP1p?R0Kc&6I;Y%one=`y`kXhO&F$=zt+OsdMx;Z``&q8E5Rap{EkK%ITCrL{jVg! zP5pGhXxHJDB

lTLyCD*1HLshGxUrRb-`y9J6mu~{@|OA8 zXeA5ha)1|ZJ_xG}dRw_-sj-?<-3b37bjAZ)-|t1&u&qqz?<$)qk- zW_kocKjPDeC9i92X_@n5>+`2iT;%u(vbSO~Ea)#m(pqa>AP=&~_2OPn$G^?NjHyeD z4;1tC=P_Hn_yrStF6fCKKL2lSaWQ^pMx8?gTRy1VEs^7aJTbt|k+p^^=jnEWS}B)6 z%b@H19nS0J={eEEjDJ|m{|2Zb%z@qyhzr&%#2c}J456r~2ngSPRKcX)LUj4lMc*uN zQre`iF)`5Pb}R1?3rKk*fm}h4x%Jx`^*tdFgM5EKSt;qycJ}_p@~SJatOkB|V}=JNSP%dpuYdN4$7UsDkQp7INxUDD`%fXz z@-T4n-Rt~b@u>k`wWFGDMec5b{w)|=&CBRMWMwpX(;q`Lad6)8^OH;VVt< zRN=ZCM*|EU#jkfvUD~%Y=jSHT1q?TbJTWn2nwq+85{nGE3P0qGs}MX37TL?1J$Hxg z^mGduAq(%#2#p7>e9ja73u~QRKy*`AD2@y7RE{%z|L-%T@j2(Ckj)IBc;b5`1;02c z_CUFVR`0#+Me?xA?S{U~L9f6K>F&Z7(OW&3a;<(t7w+9~^|gkgVvps`wB>7{cLp)> zU;ywNlKOR9(rUP{(`V{wQTcR1F@~yUmvr(^-sg14Pz1 zym7x0PJds&SKs%95CVSUy&%qL*F5Q9RmgG3z*`A#fQc>`;Fsk%>%ZYrD7z*h&>~Im z+$L5g0Tn6ON1y3-PAdLC}@8nW*~ye75?R@RtV=d8z2F zglb$@Q@RC&OxXbHM*nZ#_F0yyYr(Y`Mj3as7Y$KYqy03Up#@v8;cSYo2edI@j)?g# z)P)3iTDuoq7AaenH5v_+!=uw(@!+ke6v|H1*MoyVNUEFuF6=-oF5afwBrvL=`LyQ zy17(NI3t5=%A-$G4Mrc?NC6q8$2OxKaoB}Zn)M(gD1tFM|JmoKE4Ag>SxP7{+Nr!Y zlVw-d>rGY*S{oBxT9+oh{uAL*)zCl@svRfL-q`52rXrC}9L|)D#cKdI&@;(cyD`qz z)Zx~gPyO;SH(GLGFzxqB{CUa@w?6EpDEZDc&~Y)VTXWq$&0Ojt^-N3xr}570e(Drx zDtiaK07K<}n~U@RC`)FBWMy!t3Ck}~m{7~4UZq;nlHWwX?z7x}vBlix$bt)-J&tBx zwAay1!dnocGRC87Vh@tP-Y&mI+x5J6;l{00kGxuN9R&?Zf4w*2PY0ZyL)5*)==lx|JoqKwVhA_j zzaQs|Sg4nT`wm5V&AS+P5yU$xuwgadu?jFdH*gzQ^C#3xY^H6J1^YX(;fCm!hP8U^ z1Lt>6IRhrhkEyA*7ig4Ct(c5Ve9s%oOM7?_(7=tL(96rq zfQH2IA2wvu%-DDqM6FN%3j`?1)E{w1=9}?@jOn=8Sg3qZs1Km0{%Zf3Lgw3CK?NDd z3>x>_c#;fip?pTCqoX5p5_nM(Lz{~h1w1p>6>_Dr)x4Bv{%|miuOGjJQXSZzt|#jR z++8iH7MbyzUcFeUvzOxUdlLRn2?vIjS8P|HL6rzET|WdZpWE(~xHn)|0E^^ICbsuE zR|}i}8<8$VfkA($h2as^D<;M@{0M@7lpm1EM3b=O8MOT)*c)q7hd&$#{yvh5LtluZ ztV6x8{9z*9!iU2+;+hLF(nF5WYfiUGE@{|tA*Q6$v46JCrabB1u z0ng+#BZJbKoSInr2WQOSVSn}8pJhy#?R+`%CBx!MVSL{y4|tSVM+kIy*(iAt!rV`n z%05%)_yx2?xPeF|^LM`0W^Iojqq~ZpeSSXQC@uCF{jo_!=7!rxjrhfN%eQYU3-0MV zHd}LWzfr&eTBzR)zS{rpvVM(YP5ox>>nl)|7=(APFR~tR2G!@Yx{Rgs`LB{)_CV>97z{gqhtOi`uK5(=0@deVPa!OFrjYrbIyKG51-{3Sm96)UIRc`O3 zrFo4K5O@!4z+==a>HD>`>)>YbfaIZ}%TtoxoV89q>>6W?+5qy~n2vLTl0_>TEL|d` zgLQ6?U~i7icl?q7T=vZ@kEP3*Cc#i0sUzq(xo=2|A_fGvT(o{;nFR`!Ao+URu=Kzi zxsu_Zha)Nw`K#Z&4{^t?gP<$IYJLeYfyL zjctKDw1B!y?0}Ncl~e~EY;{4$;z4f|vITfE04fH(a8hjr?g#3JeY>+#=EA18R`+2J zVLV?`$~Eek>qKuEb0Od4yI-2~VG*t?D`Sl#EQ83){u^*4du8f~cgUo&&no-3ng(iK z9I)@Fa+;i5bg9wFn4Bl^Nmm+1#*O#1TH(~LP4Mmq$A!+EAHnLQFJkluyHML@{8;}+ z_kK5{^CwYRSy`zUWm)?SS%{oFSXWQ;+e52GCqLrU#-~m+Bzcq(8l+(K;-J=C{06BX zHf=5BAnR?&wTPuN5+I9B`>%uKIA<&GsZxNQpyaAZFW3#DZjCx;o*rd3H8v(W@Mjad zAzQW^9yxkLBkb}4Kkg zzyV?8aC&=91wfh{|kSoLW1NzD8MuRi-;GDH@(X)7b=UCe;g%Qvt zhQlg;O^zHY1A)N`V&n&Nr}&^A(ha1?&7ke*bZ0)jrCdc0nLm43Tdyhdc!1S?(okT7 z*!V$kimJr`p^mW!^r?&TV#3GqS*UDck8!?68Patzx+dqp_2rVoEG2mB-E+%!8Ce+_ zZU3wZ1=_=eLzoWXqz$jrq@cs05l)rAw?IH0O{)M0X@7q|WI5-lY$lp4x>f013w-N~ z^GAe`lDoNCZfEXf1{O6U0TK%wpK|xr-YEB$g~`e8js&Pm7DhFXA|qnL&IG-R$pQB= zz--X#uiXBMpUq){G%Y3z3z{20K0xErKU`10GPQ0kqBW6Pdbrb(&$$}?2kxUHb>~If z>y#hLDCWM7`xpX5r>;-lb4GvgMuLoif?hA7-2L|+@hTI{f5l}t&8Wk{Z2yuV2Ld+O z@d-w6F999tl!?>NkKr%4{hK*~i+zkT(acm!x=%z5affj`g2#}^)fzH}aQTAZvlU>R z;-mT@U7HvRR^$=*gT$3*9MZ75^W@}Z|D>dNz?Feo6Ecw(O}NOvCgF9^S2&G0uo-94 z$T^FhCP-q;c#8f+sQaWEn{GSw0W~tYrdl->hoxqLlS=n}m(iOGMUu5v-TWC&43Q|c z2`W7SXbp>ty$0&$XL265dJvn7ArVi#BdDe-Q6M8T$35VD#_z`a_Cr8pLjy{5Dogt6 z7Ijox?~s~TFzh=0xdw_n9%IzH*e*Q#`mnd*xQD0UHO={DW#WHh+esJej4*iEPWw^Z zdj}BI2)Y^?dS0n3-bYuun*SnCL3824pVKTU?LUe!-Vu_%Tgw zWunr({A^~m%U2f_|Grzo^{^ka!;LM*D76>v;jg}!xJ-{#dr>Ks*S!(ek+kd>o#0Am z#|JBdtGiJB-l~C}IDH0$e;)QomPC8Wb)Fq>owx*E&HHrXEMAMA=Tr9uLAdw|$=XSZ>{Tu0D8e%eGY)>e_xc*uON&AyY;)c^1r4uP z23IX!VWf4y#`5+%8Y*9#=2gZm-YWVz%Fx@|x|lFe&3pW=wf!!8hk$_ggdBn{))4?# znVllw?uF#DJ-rf zk>z3ZLK&}2$KNTKhVDNQIPVydc<)L0BTT8HJ{3Lkm8(#NU$X3(vVru7OJ17pZL3lZ z$ntwCi)O?z`=wNGKA`}B$u9&aPjwd8<2N3`Ar_aM^UAQJux0gr#d*8Yu(x#8~&awg?l}W zVspR!zSI}`ubybMc*|6Q%W(`yswL&+PS%fRwAlDHKIP(v z{>C(AnW2P98$AklL}as`Sf#blOT_074)Y3h33mNsx4-78yIy-q98>s8 zBigwG4fH^ZqWLvw{~EDoda45qO63WL>n;Yywal2+N9$cb*3J}#t^)KD#ox@@OnvJE z&*ykl^wQY?&RzB+9PdyEqniLoxMp3{;m*AfAM3#bogj0b{9Yo3P^wQ*>ywDq?wa#w z)bx>Wq@yM*uu};1BOi<1M86ABsu1n3#>zI~@uh1|#@#XF!4*N|Ev*`U&xE1)P?%d6 zHnouibVtP{3b2sOO^s&RcC=jyiCXxe3@dl-b*(nAI22s3=WenZ^0wPQre2MswiGJgaylk7mVi{V)IHKexj_*d1?67?n_ zcyI%v)(bd6Bi=$~S#i}b3SkV~{S{vA|9VZd;wnroLt zst>#MAiv9R%-(1_sLK6kkvKk6EOpLYO+K4rcf?cO`sy`^Dk&F=1jyDZ0Lr*~5k->B zb2S-K`(4EI3}`^6b!opSI;MQ|GfQXh(d9tkUxz@IfiOl~-2{(#$+d+13#?GIT{nYi zH!II7x6YgriECgphob{YYv-g+ErL22I+b5MSn!IJUFXA*V%TzFXWk-i%ceqJ*^&TP z$OkIz=N5PteC#C~2(IOm9YBRTDt?~p_~B(o!0eyF)P=^{UE%T;@hb2p2MdV;xc@_rcLQs`5}WfBqO zdN^(D?WpxLnleELQW3U7GbD}qJne2JFE3hIPqtzSA7XBPdDu!LWYx7o(1LqmI{+SJ zeZ#NJuZ~O`omXVt8b(1u<#njo^wR7YE@%DKinYROUK~=a9*8`SF&4(OzAlnV-yCkq z+4jfz7$9rl_}zMl%q2C_2_S?jt+i_vu+0dGu?abo`dE5aurLg>*j+^<8 zXRVIpBv5(yO_VS_?|?-~Y`$6?3#{5s2ZDe8u$KAFCzkWFpo`ejfMPPDFumH>T#L18 z{jZW2=wNvh!g$Ajj&dj~2x4hb)_FWQcS(N3MxpBnt>)x1^SrRD0w8I_YM^p@_POY7 zcCLkFnjrP5qj_uZ&Jbb>Hl*9_-I9q+N#cbuQnmej^62+P+@j5 zCJWy2iww<;i*z#8-C5ED-qDybMo6yNlDMS1#)rs^#Ns$6==tX;5f-gZ&ryMn>&F~h ze99Sy4zmn^I zL+O*oTPSm=RL;N3s!6W=g&6D4UqMO95goiW+QX&mu_ad_f|pd|`MkKBp^U`A{r3W* z@h5xwz{cE!2NycA&bC9{SojtZ@5)!rGd4}FoE4`^pqo5j>8)!M7k%d(ZDV$8Ad9Vjb{G4#{xH; z#SgJJ2Hs0ymPegllanq$Csh9JMTp=q2N_0?z@miv`e1l}oWpq4NnAj&K37;VhSk>} z8#+6RR@j7EmC7-FA_6Z%=(+TMp>|w=MspJiu@Kb?|A(~0gA?{eF+WMsc=DJt zs0nA&3=r5(RSJB9_D#G=ez?;b5xvEbv$ZYwXKrU^Mc#M(<|T8qF(DgV9^S>2gT=e( zh~~W!>3R?q1UIJ)nU-cltPhFu7<{r42NEbf3u`f6Y3}P)+%~>dv47@lfHUuQ1)YBw z13F?aCz~Vg$9No8Z6_r)DW)Wpxe$Eme;V)dFT65rQy5a^CwRBX5Ct(sU)Tk7e43R9 zEl(Vo1s#WIdG4OUy5Y<;O72%c`rX8J|q z=~z1e)=hiz4SpH$C9Ns@#68aU{L^adomPl$x%@5867vO6aXg4S{koa*>34oe5@MWGn;7WLhOYxz;&@OoE{Y3qYAlRrfS?4 z`bA$-385Yq7huGUi)>B*ElBb1opq}!RpE2w?0RKx;rbJMBer01mK1nR?96CIP~iaj z=c;;#pxsb8pq!U|0=RM66HuAd%SZ*oy2|;WlgZa&%Kv}@lpQ8a7L5KkM(QT$vSU15 zT4v3bK2w#-(H9ECJLy(Krbtt5z{c^>+v#=iyB_K#=-D)p*e!!WmKT%SUlmK_HrO40 z=&TQzrfo?j{M-@kkddftSK}uTpW3@&(w| zla0qrr+XZQc@c)~3lV%&Aus%;!br%gtQrm`-$BQ07r zDFm!gnqcI4sbjOhP*^Gp&r6ew3a@FK#uWGn4PDsUYrBP={nI#*Eqk6Dg zG-GLFNw?JrQ)e1z5KLPKQ=XScjgn(DkD!4jO@nuln~UIIA-7a5JH}+~u+V-^j5Meu zh#gWIVOVfFY1hDlNbo0Mbp9w98A-sMNkZ(5$Wq>|PV@K#UAbIz23JYHqlbUYVxXrl z(u!($dAGf)E~RF`4ArDw(*KeJW6md1YW*dpZ)R{?hM-53OPIuA4;5ED70IIVtw`m2 zNhNK(R+OYWb{rd{u7bgHHB__jX5Im)8ATn-JdFp81X;g#@){&dKJQ5MQHVv52l`q?VAro3utcm&Y zZq_3@gofe~sa)k>pn!Z)69yWl%nGjYJQ@4o%HCB&6dvPuuJUIJNkru$gI`!N@Z`xt z0hbn-DGV}XYJJIK3>Ub6H_(R~-&h8G%M#5E53U!aSc}cNBHvWNRS4@tH;3&c@`)Zf zWLWV(l8QWz!t1_0BkS6SnzguNwT?9fr8dJwI{2qWk_CSOwLFYo+8EUEGDNmphMgI~ z)ORVLI34!&dKC>mZT``=l~z<6R+Xwqbj>wPNLV<`MgF-z6;N-}2y2S*45{{*V~E-Br|0%>h2RC=9DN zZ?tdx8rVAc{bnT6Qkzc7LwmWo7|EYsA`GR-tP8q|5xjzd}&J-P3}5$?IM;{2hOiviTj-kCR&|+Ea2?UYN|-;9^6cL zG{PX+51#$JOc`e9hmUAzNEOX+lXS%YGfv+8RJUD*V%=;3vckz~q%xd^+(;V7w8NLE z;lvclap-w^Ix_#mO%o}8*&M7qS<9W-Y062C4|rNhSpO)M_2e__dI)3VbQ;}|R4$1P zq^pe6i}&;t*H1J1ABHS%J#)^BI9pN=rwV2pw?@rH$8wUa4q?Plm6QhZ)uh1L@>Q^R z2Td>Q(SS*u3Y|x+Dtp$gPbbwyf93!yc7VOC>>qPi(AlH#xXM5Em9t?snm1fD1gW^( z&EJJ4pE?A{oECHx<$zeexnB#1celTnFgxuKq(e)VaK_+DigV?>y%i#rr|DHBQSglH z?od>_KHmqaRVq)f4ePzTryu-r(pF zY<;^$nh4dCgFxQ>T}NisYlMYSW&1z9I}6nDQ&9ldwQ5*!ECCH8gQuJj%m z?ms$Rx=@sm@jWTId$Ag5W_E&88(aX|@X(}z84{>^LOqCjlx-2TmRyr1O;|Lf4 zAH$fY6VsB1S{8SF9DHm}DQ8*gcvTV4wVA;#E=_;r6$M_mW0L*UYCUEBTyM-b_>+pV zO<*W=?aP!L@bANnvHDhg(v;QR!-;*x71--vmP>mp|D!bQK;5VQ)ByYrmXAcr?T58U zVA9h%nG_8eD)vQ`=F~Q|k zmNhO`Opnj&Z%4;l{0t1pEV&Z?Si+Z+gZzfBSGNq_j_Qf~J6;FlKp2-i1uR?bE90wU z<5snc&!5|fH;rD+%RxHrTJAfG_@YV$Rxs?hr<*K!(nn7IT8^nj3tYex>Q)%;YCr0m zZIo*gr`sJ7NxHi_{88?^heKE*Dsz|u2zX#a*;<(MK{xB%K7YG5vvt08`~I)Ek7W^q zsM|Epl~NAVS0541mnriabcvQ%1zoarAyum zSKdX;^d@(^Bav#aatS=R?-Uzm8mZu8r=eM4ZY#hg(ej0j8)IlDaN;VA-F}l}-vkI2 zU?-_PzpQKpa28O8b8%Qt_=t1Y6(i19{VfZv#G-1IwNluZ1Z?O2BY^%7zKPBgb` z@lU7C2#g>~m(pJ|jjyf$67ny)Bl-3}#JPett$RNn9#>^^^ha+;Qzl>?92O%K38l;L zjjnfkB+e@-XEY(G*gn(C%5CVJy$#opqj0(3uQw4)_4MMZUklEz4#8V6-&0I5K zJ3%94%v8njUqMkMc$Hlz#)~nMQ_(v8wycKv#ZfmWsxPT2dbzs z$516oU6od^ET@Tl0yx|z(VM+8*!7T7Z=9vY?cis*_8kklg>Tcf(yCuXl1q+QAn*S3 z^^0%oTCC*b6E>w%CfR?haqxWRS*)VerTre)aAE>;bd8~OOSd3kx&rLv~e zFZxxcs;h&JyTL@3!9!Zk^vwq4$~2KIoelNCA_8J(4)MVfNV<;49{jQrhfi}6t%^Dh zsAa17FD(U)YuWswTK9b!UF^42+QLjz-)jTnOyiz(h=o*Hz%=LyG&(mbaZP~}?0cn} zl#)v9FY!`C<%vyUd>oZVa=AtHD}D;e5yY+bv%Z;ffniUKPV0R}N-|8U!i-0Rk1gqp z;vKt(oreiC`%Iq|!!#JOFb z@lTEJNPl&-X8;kD6_QV))3hxRYsPZs!1Z7k+OUl7SoaW0zZdlLsDdq!j|9?4F-Q4g zv)Zyld-^_=%f$Dzi*go7P=O^-I~cj*UTex236bQ;dO5EwU4I6s+4;dqvW~lulBwzZ z>guY;xHJb@Y|`b1Xom@Rs_(mZHxpup^s!EjMaP(wF6Kc-w)qmTMCz?}YOjIt_6Ce= zJuVE0;XDyU`f12I8qc3_v;~z9;iA$m^Hla3d{&|z@;^jxNDKeucsuE^^yUmYplYy^(lu2i7S z5Z6@i%$Mr+;Iq)*8e{K0zntC0m|y@{cFwb>885&{=>#Eu0$oaL|-}- z4)7)y*yZ{wmV^YTCUVKP(u;W}Wu&UHVXpC}ILD`5dsP!w-|%7D^(Y?ogF_K@J|&dF zMH%@Gb`R}NK$pti&7;|PMTM?!2X`G6sfemS1$j=kXW*mZhMwnK_ss zQtu->`y1^@Df>^;ACCe`4QSPh;<&S5`)uF9L{n@NQC6r#G~e(SMqR(9q<-;W^zLAk zDU!pb^fkP7rj<`yJoa<*bwH94L>@^~!rq+SNv4T_H&XQLJ1aS|)mN02_N*GhQUA7A z>N3*Kl`@0Cm-g{vsa$ooqy6t(r&FguoDGegQY^$b_o;r+A-o(3 z+92Ly?_Gr(dPTeQj+v5*Y?YG10t}ht-(Ws@!fY9@B zKs>FlYbd^@X9RM}(+3=Dzy_{;%=+*|IO|1GItiiPbI2?B=Hh~-({8+PQCH$bwm@nY zpejWQqT^llQ5pvQG!EE^Hg#mpjH*`Y%pLldi6pk@ByJz(C@)$!BH1^sFdBa?#=-)$ zQZ+*W*e3yz^(0@c--q7VMQrhSpEp39JIXKq5|_+shkQY(bb!nSIQJzL6n}OZ%{d0= z|JlKHX8-O+=z)tH8E9#~n_#(zA10P*&6g=a^Dd!wSfBKSwI47divk-zcjMjD=99u_8; zV|hkMWd4u@skO-Y6?)Nb@x*$20_#-k<6T&FLNz4+mcTY^ixet5!Tc5i zrkhbT`_lEdOtw}U^5*|GjOs(3!Jr!XV}LM=C!aG_R$gvK6LFA%?QLd3>-1^+Xx>kb zQ%z6r7BciMbyVBHKxuRNq18vu(2$Hmd*_S^`(^w~KqZx^Wp3)1ti}H05d$^Si2^j^ zU^XN9grDJCtS|3vC3dGoKl!3OVeQ7yOgwT4jw|?nZ)u6~@_{^hsnEC1GcnUjc+)|! z4NGVIDcq|L#J5UiuqV=BC4h)+Xc|ERyv3*{sqj$%c9O75f_RGpXl9bRALTCOK^R*? z{Dx)={1%(afF1O-Lru1WUY2A;o?H%bGizG)Pa)u%A@G)lxEz=z{ley?7fN-wOTdn> z4t)LZF%d~gS36m{2m2`K>jT|9k2g;bL+1~M&h(*@SX#OjvYynEX%p&=;-PK`M&~yM ztG?xPdm)}{qf1EK0~l8GQEYA^$%?h=xe)zWrc%h0t!`_x2_vG zW~lsJ8pNr6ybZ0u3d#*P+S=L&lC#9zlvZjj`)f^l@xOdtkPNK$#?rUxV$UwGYIg7` zlz_mp@2v6Do~y7h)Y2KvB%)j|JS4C=gSM$6)@?c(E(T04;8fcOrdx?9{h6McP2!A9 zqBQodWQxIsqlRUT?j0SMmX40*4kg*BO^d;g-#uY7O5Sr9fNHuQNzIQRv8+62ooAw= zRZ-B7adQg`blHpn^s-h8JIko0_SC6A>@;0+j9E&jyiSOQU)b4UA01;>3uH7?NcL$- zXVGbC;bHK!KxFNFCx!kg!jAf= zk^bBv>EK9=UzotQsrzTsF$r-IE&+*>0a5QCrEe(w{st+5 z;seNH`Pq?Z)kZ0;CceIRcXxkxHXUL!TO6^GOR(+O$yzKWZ4_Eq5PBgFN@(VLH~NhQ znJ7{1#{sy>Q#wGBR3O4j>xYp)d|{Qarg~qpph7}5##Vgng%VXfAgz;5P0bSPrZ!6B zk1=c%k%o9K#aIMpj2%(WFL#D;9kMnQk zhC;Lln3I+H`7UA`aBRh|H~@9M&VYqb*q0HM>1MwTVxix)gPhiB8yfw5UhXXmYMN_n zeP+PU1hOwE(i8H1QJceO^zXhRh6F)fE@jR;P(Eaio1Pq=(&k@|ObGc^`|$HHb``U_ z^LJU;I`KHYGJ`BEEad4IZ5v|AMFcrHodMV&3!Tk?@N-Krfx(nT%~Z4VR#}O<$x=hq zGk528%BRI2zYz!sJx)#A=#1T_mx6=gqjmvLo+x3tH*-jY-bZua+uPfpo}K_DWng3g zr%~rZ@2iB3QU($y5^x@8B-%AXzjP)XSuPpjQ_zCn(*G7k#LS{c@#Vz<8Q$j!Q`&k5MbzZp1Juy;ZuKOkKD^{$`o^88U#Z z0O}Dg!>yHlq4cG_iaY?GwCjg}RoW*4@K{$rlQ_5dFqOmk%tPC#vhd zx~F72w&@_wsPrW6oH_``wJeuQlQSR@MosB!p(TD7uBok^9N|f30Z&g|90ZU1+_Dvg z?;Q#~0aZ!kj@G#)^uF72qy;;7#z4T2X++4w@{S={vM&sEKF*RA&{CLNpMIv7{n$vp zOyW0Te1T!A2v@i(G?}f2;ORVj@3#aT??3oVQ2x<~#M%=8@+^1olf;bzCEN~pKRyvV zEi3-;EC80=)&{g9VyIK6lC^z|hH zHPkn9ynwm_l_-D0P(4CBm&~pnHJ^gM@KRaxMEdD{DI17ZIXqJ(B&MczGIqL~!m3?a z!G3sX?g-@#f%ALxFNlZQHx`BtH8XYF+Bw-}@cCynE%s{4t_$TNn@f-VQCR92{>-wa z&eZtP#^!tPCNg43iAuIH>5#HK6}7Ajm)V6(*nm{L%2R1+=>dEdGgCH|^7h|#XqJQ^HLSHB>gCoa zR5xRkUO{`(&Ss=b9a3AHC7kG`*xKMc?Uj)LCo+tzBctouBOHA3=_{&qK>lomo7(n) zJNY4VP@j7Q)(@64+~8URTKOe!_~GF+Pb4kI9n7ud2Oy{-+sXsUtFUU+2sp;^URlCj zxzP3{O6Q}`&;TYRy22>Eaed6Trw(s7d;3g6Y}`Gh*uP+c#IBr#+PvUi)bzzOqsLI;-2)v zoO=7uoOvx}2 z@8sZMSZhAq9I?&nalG<+PvO{02?O8UAPYtGNJYNnN$?d8Z6d49r%nIqm31k@wBX~} z$>opdJE*z$|L*T^&VlILmbkhC@%?I|$eqimy4ek(zPNuEAwQgD#lXgP;&8gan^qb& zZ_CQeT&Gvr=6Qy}X29@{jtTS}t&}Fn%iYc&<}_D2j2|^`E8_9M11hqz$d0~7>F0L_ zl8gxlKVUTw9gXg65)bDNJD9Kw%29^O3i{;DNrdGqqAV8(fQO7w1iFeukgWc*fXeRY z$Wm3JJ+aB&D11wi5Ps`Ien{!t7f2`0`n^db7bPwcc&571jQW>{G9&Va>KR~p1ov@g z&eqb@Vo|9M@MOg!nCDDO4criA;fLh4N>@)8Flex+5%wIt!j&QNRFkVT3m&< zZc`uqtRq+u8~-v=XqXQmQd6U1pQalOu%3=9w}fNvlZy&F-j=k4OG%|9@KY$Uq-l*H z;Y2`L*;FCYKRBsEg6%iHG}Umr$*W}YDeG%)w@vm7YXV?CKQQN|=uE_Cuf|7rl&I&7 zZy&>q;lE!~U0dt%=hyai-pTLgtt}MI5)A$DpoxkPxk2ZCVaN_CilMbKxbG*~-wYG= zx%PdZ4DwQ^z#BOWb+)YNr_iE?od`gD&7RhR-<+53AuIUO$~UX^72r#V?@}8P#3e$- z>d@pRaVMwQ4BHdH&l5DhbF82^F$Hn~ZVlnGq#2_NwUhEKwz+UKqf;nQq+PiV{DNu%u$N2a&;a3N| zxP^@^yOBk-W2cDHN}&lpR5TZVI4NG<5_+XH1qg*|2W=0Va2k|2oi}6luyrRvzz?5* zSP3FlU%xf)d)vs%ONK&ULpnU1UZT!#%)xUGjBrXx$9>CDOL&Syb#E2aq#2}}o|6$=% zUCbec5^C2zA=#r+F>&u_uK{2uGH^3bn#%tU#K$$y4E{P>k|8MQlaP?0np1(I)Ryp$ z4u6%kqO)<(Y3~M}`17ykOO}+ro12MWzm&pzB`5;@0FBdO8TJ)o64iw{a8V(cmL4`d zu6%eXAZ`vacaWElF}deFR3XW>npekQVPYmh0Ja=aSZdZzAhmBOs75D29SByMT92FS z5&3*N{Iz=|=nPB{L1YCO3M5L7G=kE%LtGF_0bIsght@BUEBvyCMK0>CMFBWUkVW2sB&9NS3>;bh zNkGoyLTnDFqGd$!sV;p-+|b@oQ`3m%ck=T`G`Kewl2JS%U;O0a$35O-)o?hhyr9BO z=L}oOAyLhzvt!U-~an6Mfe`M6$aU#3M+0;6}{=Y}-xAc#*fYi{-GBWZ)>jo7^U!OGx16JOxpdh>?!S zKxfl#*@}>D+`Njeju`Iwv!lI(@>1r6UcyH3f_}(?@^gnoEsL-yto~IueNk$S|9zrw z>A=;uCXpD0YH*H9LrXL`vtf0<-*P)i55E4yN^2T(-Ovk^Ax9H}Eb;yL{*t?KA$Hch zjd2s-xA=O~G*SlbZrhB9Y#vWj7cS!6$C>ta`r~3WLK(`-5*%E~c<`dn{WxmcHtX-+ zQG9KTjSULwyjh?_MK7kOU!0peS-zBzl3EL=a6E^ltfnZdY34Ww6lbXr(66qv;_!8q zJhx+R{sARg<*AxUN$G29PX4A0)QJymph_ag#+O~&{x`S;DEF;@*9E^_E^q-=9pAQi zp+ML0v=wx%lcF$yN|!k(dVBw!ynOFQQ6vsfw=+W?yy4ByJlc{gqrc6mCs_RlTv7mN zVjnk`*{bs=0l(+a$jHp2@SPIdJHy>0E=%MweLk<-s+|*QKL8d@0cj@_u>tmwXSL;4v{278sHAR&)4qk5V)Yo*M z&7@0~?#|8^sc>7MDN0HB^^2R@?c>T~=La>lzku6!o!DiL?+@$KzYi~iW#uGVwpI~B zf0}88gxc!rE&=Eh&wB?9AVx-uaL@LTKC|m1N~7-uVENy8_mydCObG$&2>I&fRTUn% z4DLzbPgYnkH>@fhq&J5LZy(m8vm|bEnSq4c?xIDD8W)RQ+K*MGm<80q$q%j3K!`&+ z_i>NwU^BvorJE-GWPfK4)5uitEebE(nx_2@QKF|^2WkqIrHpu-^YXSoJMzZ`?;}S8 z6uLhI7y)B{OLEkk21(6)ha8bn(4iTzpD(5;m6X$AB`TUb`jO}>AQ^mnJSC>0Axtf9 z@bcG^r#einI+M{|7kP?OODqy&dyjZk1%*!}fFw~i+MY*XJ0PsJ;iy`>n#kT_9yEI> zc|XVBW-KA`Ci9{9Zop!+5f86{m$#p&-#COd!h|G699}VcL$ary9JdMziC~^1&L@i z3Lhn67b0$o54wnQI4w`bi0y40*&xHfKuqsI5>@c>5qM0Du;bQ0DW62Qe}mjuX=uv~ zznM5s8k9p`u;Fy&rK*o}B103F8d{w#Jgu&was0WN+G<=;^Xy*fJ|_kLCCC=^Yxv?q zQ4>26rbS^n3HYR|Opc4iaM0F@1k}~iGG3VD;f3`Syu7^8S0)2_cXU>;$eNn=)R>jT zk}g-ZdojjxOF|S=ScZp&N`a4~tZd8gZ!nskY>Z%hP7aT4QeG*;N1erDA0^8Dxy&0T#o^KOlo{w>R=j2AY#DzQ zA}g%M#pCD%*(N5SsUWLD?b}xXRAveQ92;QdR1_x?TKul<1lkrKCoGt$^mE|-W+Is?Drx@$o(G7$5*=m>QTa0YjqAz-K~hy z+n=&q)x=>TmDBxZ>&ths9)!-%%_xb)B;W*S^u$|2UlimFrDBQ6coWJ&!6#~}u&$!C z-D^vVay)Ouf;y8k@h%T=jh8*Aud+tZyZ67Q=6~fGZu7kFp4-R7j^CXcEbhn)V@2j6 z1}OsvUjCzagKM{ezVh17FZ4n}+>FCM4{nCS>`c_o)GlpnL_|ctRKLn!?(rrkrIGtp zpY~#sYy@^a>vZMUnAz;B%spcz?M8ypTDboA#n%2;GNcmTZx&CguE6T=oa;u;doqcBKC1~i9I2qI4=2Z-D%kb#bc9E829$r-B(jYb>T+M>Y9 zVUHLHZst*d*#rv4z(B^LVQ`PTOyG3+dRbZvTn|Tg)h>Lb!SrSrJm0v3nZmr(a|wuH zoSQ+27{fUmuIfdE^}gHZNNeNuYf^d>S_W35jn3~d8$af;ED`_3?7UgAV_CwM#Wp_o zab>=#bFisqe|XS`n7iQ<_N>lEFl7BQy|Lf>;QzsL3H%m-vvCjut~hLC!&0iTvsu`? z>JQy;9!;q2Ld8`M+QgNm0vb8*3!qrCtJc`Hb=)F}148QoxzG22egQ~bR~M)`_mx2E z0Fr(6$5{o6&`DuVU`w@oez*pt6uf}R*nZP= z5nn#NY(+Ft5<@1QJxVeF&qB6{LB2#*aPASbJ`$M$G4l!=uOk%u029^9y8qQufQ;n1 zAj?HzBuXAq0H96o9qa|{xKvpyR0&;1crAvv0IueOxJ+|UyJ$g*AZJy`bOb*k`hj>YlQ5ZHqSq##zB1+H)0+!?%Shp{XcmuVus zPOh7#Xh@e~$W>sX{F52U`+Ax&8ivz&`hVfd$J?#g;5SlStp@VH0|xW@ch8%$s2WZC zTnwa^N?bur;68^1%L2L^rOs+CK(mR?9#-o!Buf6*Y*Gdi`eE?YqhOErS5Il}ubj#| z4j>1!EF~<&HjzH0jmPef0qsxk>IREP{BFKbo%Z!ptDudysHAl~H^+}n|eJ6D|x#A1d|wu?<)LjC}Db>SQxxy_=`f{xDpiXd``>_}>swxJ(&4kOs;(G^u|c``ZCm=g5`ZvywI$SV40yT!}2djrK{ev{=oKG946M9W28{36 zb7=mSh*21sliQMnzd3KsM+OIfIhjTX%Thl&;EKiYkSp!V4@nL?M8MYJL;3`OA4VwV z(JsP6>j(jh)V>{0(ElO|@MXjv;(@p4ORI)fz#Vp?ZV{g-X9fAvr#=cx>R>w5+U`zu0<-l zv1r~{cAV*Iiz_`^% z{;qY+qYvmm0ASr`^9rrcB&uh)veu2ns*qRZzHw=n(CJ}C`D8LE=p}aJwcD1KnoC+G z-_!z+fWS^CODBghaz|RHg}bvPcdyX-+SbfscWQ5Z#RTmdi+0QTYiP+U;8VHu*VNhh zyiB{?E1!8n$z$xjbbeDi{?Rru<73djfjFz>Lh1eStE|mJ0k$tjtju?g+ z)bJqyGFsRROLj}^SUpuyQ4!r%kPoZXZfLit%S4Z$o}Au%m$pJuNm0RF6{xiX6pFr= z1L-GA9<<;upNtb>eyrnVx!fKF^0$t=r!#&p|i3El~?g~UgN*t zyK$y&<<_gV%PT8mVrQ=^C@269^g@NMxA#AVmmGTq$WnUz*7Q3C6mT$8dFmoUZ=rmm z8-g8Mb~;Z{M}jb1v&00};b6Ti(m+U^z=uA$!)RnRNxU}#usim@2rukg;bKplW-t-D zx?E6xyBC9`ABuWA`Mpc=pslUgok=;3kYQGxbcV@s^d}`{G&j$4v3EUIILT6xGFB3G zz1?4NU5)&;6&$6#S=EW<&6DRb=;+owzu(b^-&ksY9zFGr_%3i=^4|U<|dp zb^xr``g5EF^TY`tsD68ar1L}d{0XdO&ixb~riqCe~X4Cz@!eFnWI5@|q)zV)e1RB#SaKX=a7r7&Q zlob@STVNVvKjB62$g=r<2TX_vdZyQbAei$sVHj6>w-1)Y9P`<)HXFvsNLUd1X3?D+2; z(~ZvKp*jIW#**fX+dziZsnp}m8x#3W@vG)pitp=CFsrECZR8>|qJHrVS10S+bHC41 z;$TF58LKy#jdFwvmRMS~h60MT2RoXeW8X%vF+vj># zGPNhVR|bOqydznV|A)eJE}0UD`s%2YYU55ee8>ty(jS?1Fz9IAiJ-qTU;_vmr_3L! z4=)o$0tfOGY=m;K6~u^!H;l~Zfh=}|r(=-k7akuaH}Q86)dl-JLG(RNRd-(>1o#KD z5zJvz)1U`lYF<#47H;?Eb(7Z|odo;Y+wSZH0IB+N63^|3l}~NN?siCN)*)QRVoTab z;}2{)@K}K{N2!_?qd9Cux}EI+kBE^T^e=@#U&YiJI|v!AWFY(KkdI7JP*ck~!Xz|1 z&@`}Oqa!0+wcOKlUc@c+_|nC-SoW-B5T3MbwsZT1_O80R_oQNn3XYuCiTc*X?btYU zN`N4ylC!O!TeYXK(Dui6TY$g-^xdz49!1h{bsB$WShEZ~%UHsUApjslIUe4ALLbzp z!RWX>o`h?5QqR&_V2*qyxC7SO6=PT0{|}axbCCDw5@J-1L4iz!qQGa|RaQN6X+6g# zpdY==GIMlzD)7-;?J=2r*B6RCWf!4mL}DL--jwCuwRt%BjMFQau2x$;rUf#8}wahz~CXP7%E%iL$s`#M5@F?!a zfH^J52z2J&>iAvC2-Np;A7D%1bX@nmv;3{#n&0A-kcfuQ@;hpe(Os=bt$@*+mI$F# zgQlM*5UUEFi~PGJ@X3d9H(nhu1{#9RZ0?-%L#fhELf2*OV#>lDE%?c zzkX{UlnKAUs!W_-*uc(|4`%bVIFMa3Vy|-h`X5k0M%i@--~)}&Z!XstBQEYlIE}C_ z5Fr2?4Si&Xeg1LkzL^yn81#zpjT`y{WX^t`uH(Obm2{7L`-QS6)NQrBCrRu>ZtqUX_IOiN6_aYh%czrprgIkz;Gb+RGo&B zl8iR4#)$k=;mKOreVI>CpZe{TJ_A6fV50=}(d@^IWSC5x7QrP89u&dH6JX+IYmt|; zg!Ld@bUYD%5-xkJx0!u=nS!v#dYtZ6bUQ&Wd|SzSf4lfdvF3A8Tl5m$ULkxv=)mCO z&VX@>dw0o&6DUkm)wepa*P~C-q5eRJ3HQi_nTearEm&3Fblgv1G=->N^k+u)DWJ3c zgIn4_Gqz5^Vp=lm85A;RqG+JDo@Q+FIC1;VYyd01?pxgDQ#K^f%4%XMKj$3h?uQ+L z;p0U{R~(03fW31(zj974G2>Fkj0+_%@AbOxSvI#->=3gm?HQ9sH7!xyvHx=6mpy~a|CO9f<(^Fc9ib?YM_?@#feXZ)ERt2IDdXd} z;xC~(lDMg;kuN1$sIQt_+pGg#ovEHA9^V>c6a3w*2>a@2$k{He^_Q))cIv(p77NVY z@VKepCHkQUWlbk?IUohcern?YA_X2<2p5{AvGk@c)3J=|{PsqR!v**j5Y7CaA(;ho z;>nDipX1}>yZEkm@2&FkCd+F^@1laGAU=Pkz(jEYrB~Qp?;`iL-07aIIM5OOBdgT+ zVThI?|IzhdE!N9Bl$+(cRzacr!Mebw5DMjV;ob74wXv6W$MdcBigrUG(FM9DnaMO- z|DMun+uptL%zyp>gDs`9S3CHF|1k5ctgQYhf~cK~-BB_OBvsS%KW=OZ`nmU4-Y6PP z=JHizV@k4K@ASU{*k5BKAMxSb;y%__B5q4-C(c3}MvPrnMgR(C`z9IRygn!vJYysu zOF7Z-LLmO{K1^|Bso=}@V{O!}zKB~!A9cHkP&)Fh`ay5E*LmPF^+F9-d9Ex!w=W|( zHq}=MDX?%{(hw_=8BaD7fYC%`-BR>N*3*x+TaLZ+&eYxgnmzD-6i_-i`_*ze43;0?TcuAwjZ%c-A%_5cR2Mta((^ zQ|_L%iz5<)PaSd=jjr4O?*INR&amEkE7Yj#s-$DMdwM8v*GFR~wuFt2n8?j#9dUQ? z|F{5E2KQ^uu6ug+5v&fZs!unI;TYPlzuPvtT-K8zJD2yJE~7Et_bRu|5k)w3${9Sv zj!jNW)tkN>k4CDhFd!nl{S)=Wx#)Sd|K3cqa+SpA?0$8SKCpJ_&<(IPz3+N`?4s~F zpemT=Hyel!rwH%@d_V#1TcZx2>rAK4<*6x3LH2%qm6FSUig3HB#*TcMV)r|Vz<^0t zS=kZr3!{2#5CHeCE3&ohOyx<1g@s`X|NZ%Nf9b%d@8EC>2mp6^U+xta76v|TQ>->Q zQHTXRoh;YE1^vw6wIdI>+XSrHEOKQR-)PazoM1L`sXis8 z|36y$Ygnk8=|#euSDblbfdMZ@e2J|S)VU1#B3FvIib8e?LDJKCNyc(8%JD%1z+#&Y z8!PXe-nt%TiGOUj!83IELm$W8I`^sadrUrvJR=jtB=?==G{KX>;Nuo=2s0&Y)0ktYQa zyX(f};^ww`Hg`Y%RkV?r@i>Z>ee3@*FyHC`!*j3Srrvrz3y=iQ+0X+BE5K;ZEueAi ze=$t|Z;y%I{WgBNNd2ezjQi(uWoH49*9W|AUV)OYd;5GWS1j;wrbx%oJ$c&N5BOX} z$R{xa?|LI6BY~X=pi_U=+HQf(%f`TPchl4*VmX%9_NVoD&2z&aD6;_xSrTGmKzi;J zAS=AC0{S8=4R-&wZ&Lmts_a*qHTr08E%TB>zW;0?I9&I5WAK+v?LpR@@i#c? zi1Bm_o2x$FD6J>b8ug2as_0p%Z!5|&1aaam7FW#@D+V7#{AT9WshL4R+zo#B7pJRD zVwYpwkgIMailo%k{oUQ{z?XXf$Fs4qQGMC_zM?q@wa)k53W|I_YkySHQ#g8m_ObJi z)A73Bu`)F<($Zo3^7i&F0*ML3AU6@`SBu#b+N5_xyHKOPJz6s8Vs>yH~=8=fFm{ zYaVTF55vo%!$mgHyO+IR@`(Yr`(pmp{+VLGRjYOFj$(K5cAUIlJAV9etXppHGJSQA zA}FCNc5`!k+MyQ{@Him^=~gZPQup7zUffRudL3}MUp_8BJVxSEKbXu4QLwSIGclD_ zRP2zm8F%`ft~98M-h&_O8XDZkGkAfhDo%+7G+sbJpuS(83;@Jd2gE6>Ek;uojRT+i zB5(q_x=Ko{0N05`RZ-`*wI~Ai#$|iJER8+06|if%SnPVa{z~HLb-U&n(*3R9bh+L8 zpZgPVnaWK}y!;7=2^^&^z^gGq5&KIPs13EZqQf^t>|uH$At4Gz&TGFc>3fT<{`Y?W zd+SQI1sbSER}*MF0j5GDTJM5?P|BbhFJ-1VOGcWJV#tn}FceF;0U*$TU%2{P*jw7a zMd5s7w@0U}4NYBsiba&woD$Ju~*>>lwZEOjuUj zby4MK$3Tfdv=2Gm9l*UeCnF^M?I&!)&ByTP@UMZfoBR^RR#C?<87|B5e)(af^=gA~ zBI^SEg}3eJMRh-~ldZyejGX`1)ke8o(c=IJCc=Vw3cf~~Hl&paLxIllz4`ea5Vuo~ zVX6F(UIX|!?RiYjAJ}Cqf@3Ac)k&qz5k-4O=W;h$+javyEekBZvW)@Ef~pExjf@#8HDF z(7oRa&Ae{~$|;|nt^TUGVx5S{nhB46famP(%ZBGltD}^msu%C zc-gV(a(4~L^|rQUqq}95mG4-wx;(4=U!DN}?0_La-}0X#>2X!p3oyLRvdHUwVV*dg zl#*gux4gts#O{BnXlQ8o&+}$-pN2Z5n*&gJ4tTl~I%@~)sVaf({P{BnH@7Q*@*^N1 z&?O$~Z#;ZRLJA^O`c@b+w3w30PBn7%IQ#UXM!fgCd!@yddoWfRr}L(?kEqZAT>Vp; zsB`JmqlT7fWo4xvg$c3GmA)u`BRDwt;bH0!za%&hsRMgO77S0mkes3IOoVyudf@{sI~uZx{d4848}w4Ow{1z(g`Ba6W6@cKs+qe*Zb`IU zJ9KG!Bs6qi>Y5`hkctQp)!##BvhPCpfaR*FIO)uNf&wD~u8Cxf-6qu=VoMsnTepFF z9arUwuP=Lr1vpM2g0bm*)=oWt~kkZLP@hlx=!`&vbJ`Zg;Yq~){lQc_w!`ov|NH?-t&d!$ScW+aT zxqKJKbj1{F16!SLC)RSuGI)jPg$8ZwVp-XU?ay2VvvUDsL0MT@Kvi5(F>3L+$mFxL z@%+8vxW{EZvk*p-2d|LEN%m;!FHTbk8;S#gg0rA3(Ji@TLRY!mAcX>A<@?J6G4&%IRET z;bi16dO~zT(pcbS*Vm0uWz~m=fj0$+W3{!l>608=n-N%$N=df%+w&pi`o=`*(~E$W?a6N4iQas{*{v^f|GA2Z2R^n@(vVabY3y!30U1*z}HaL7+i@);2KD~*Z{k{jbvH=8ACFGe`QGx|h{nkz$DCJMM9GJcL((eKEGdYrAfZbqZE8=d8aXdC4=H5Bc}7J2@v z_L8@;S)N`YE*}TX+wXq08-#SLseSa4`qX_#Ny|_ACh@!bR$t=fp0U?+Dy;KnaStYz zZxRPBUaAmTUT(PD^DS1+b&y~3xt@m;S%YYCd0)8Yk1IO<^$v$xe=u4%F+MAKzm_CY zY@M=P`y9!<>gwuv4V1;@avSQB2DmmTl@fBZ!G0()&4b&e6c;~~JYgC<|Vb+53f z==n1D)%>2dVA|{T`SxtJ2_|6Dpm&dY2LPnar;V!tw{07|@86@*X5mS)GyO2`x;ZR4 zych-A`dsU`SF?L{0c0BOB9Y=xQ1L4M$e61aj75D%H*^?1S9N$yEjq1y51c&L)Wu99 z`kFshmET2t@vj5%0Uv;vz7-G9zW(=Pp%K6==7!4ZAk@mLFx>-KY6FDl$#U#3v%PFu zkfkMdMq(kPK`HcGGyoosEK#Fgt-z4g9+1xU5-OcJ`1)NcWY;b1MX5L;eTaKp39P3b zpc@u)CZh%gFww_7lRu3P7Z~}QyMO>7#qGsO3aujb3vOIa&ieFpUiR;V+}yy2E1L-O zEtZZ?^`Q$lU}~Djq?rDpCIRA1lEGN=#PsCd%llea7ktBpA=|o%F4F)jPXZ8cBTwMv z52p;Jy>W_m&*`x!?pbbt7R!P^dJl|B&8;s$cI1>ZO?k*ChL*H7{3IlM){}NJwt!@% zcEZ%kv8XG?epJ~W!h_G^MH#8VA=|#Lyt&fsm`!K(Z;Hj91HP3@p?9I`g%5`K+@7|4n@YZ0dHCWvl#&xK z>Rnb+x81Jlc^gY?!9$Ez5%Jg*n3j;UetiBB=kyWdx_rI`Y)qi|?)$Eq|(f`JpKE z^qs)y{;H{ymxCiNF|iJC@E9cbJ^5GTclzD0clhS|-z@9aSpsqM8n{flOG}3ho439^ z922nWx$S3r)=KY$mxE+nT>i~VJg#oMq)4j(krJq>Zl|WE=yUzJfY(h+yPKA!KnUsX zdfH{j6npv%XnM!f$U+YQ3Jy=Y5}*|MNjWt+*}Wf6r~Pt5D3MON_32`igoLDM)$pn? zWXv|F($B*WQrO^9R8yN!4K*^VN>nhqFbH)?{nwk>*c|c(kix~$z#ki`T0W##O)3kDj!b_4`pqJ?=vL%nd> zC>g@@k;jN6w|ufCZfJ(A-ZLAyOei%rUi(0Yrtlv=ph#&X6m@MVD$8EZY6_&=s-FBI zL4KabB0kL@8YVI=%#T)w4sCvhhA{e7D6hJH#7ay^n3?5%I!t5W$WXQ}mQzr0QC00r zV~+~YYVm$ohIwyMY*p{hfN!KKuX1>J8lPXv#=!@Wi;0LFdfxsiK$ThZ9z7jAiY2S8 zWF*#~TUhs4FT4zRn3`OIZr;yYv2;EE*rtzteN2QI;i5-l&RDl~{C+_q4tSP(4T6K1 zPmB>>&ILBMU-{h%kz|XtZs=+K>NwBc8)FHXKXP(X%n`Br8@@r4zq27a+p&}J^U#O7 zcFn_T%m3EXh(lm$!^sUFfWcC`V{bYi&&-Y`=GI<&_0FbtE?RUK*M6@x@-EFfq-*~X zGgxc8$h5X$E2#S!my&U@pZXMD*1p=5t}vjB&qC_;v_aC;*)^RWxLC(NM^SO$XyaHa zy3^_PVdCx_#?tbqD%(L-?7H6WD(zIL@%HLQDU-|pD2cJ@SB=}RK9u)Hx4S6Y16{zj zuv@5LWMTO~G@WyB-0j=NH@4l_HXGYcnlwpcHnwe>4I87e8r!z5#zvwYNTUEGw9UFB;RD~^tv|K?o~hHO1P-^Id^ zCo;V&+_M|X8FS6E?i`gc*oFeZ`&8rHDHIN~&qHtBG{&)!|(Za{A0dGDeU zrin&rL3*gW8;mJ04ilxl8>T@3MgQ|jp~#rCku4-ZP&zE{{puFB*MMcb-Quv9zM3s_ zV$IE3$ocO}Y*Dk&)m4yQby~Qps;aB?re4>JTehEjXyp$}I;L7ieb%daR+x&^@=ylVrB&8qv$UYs6FhGUc*Q`@P|<@@oYvH1@cvy+CwoXyyU zDFT5X?~Zz|+S;qTR#i?)&eRkX1VRqp@2AxYIc$`SUng?n{}V>lN)!bE)f12}xHsMR zdP5NSB$d31SpODv9<`}CmY7j*zJ82MLhzY+zwFKy$=%-E0AT{3cO^o=Vza2EMCfTd z2c!9bZ>9fig$-48_`vJsCmCQ{d(L2vDsNUj z!W_oy;1B#>Hs#`b`bm}_Mk_0Qobe;6cBJ|vV3;I?b|B*L7ubCxo#>MNayUc?&vj4; z!}T6JqE^35wPyjgjXJ5w_)0%UT}=c08uoz47s*KNr=dxT!s0^LM`sKbFCFndOYMOL@-9br$7a}unzsK8^#$&R3L}pP`%SrizD_*(bk>%m z0tCGJvadAlWewx{S~I``+aNZ5F7af%+S+>AhQ+(GfNx`OB{4(KFoK1KxyyMxomFfj z2eB6XE9*W$H0C0E!!t|-L&cKXRDZB-M!phAy z%`qnubJS|gh~7R~FfYtfQom&3K_C=^j!6lz)J-(nPU2f&r`D!sY^Lz4)`@(J}NhX}=Mg>2P93#tb$d4XO%o zCT->A59A}l(Vk=64smjXogr2B#i{<34ligf(QH|ab(FR;FvzQrL$fL_Dx!Vv%pp41 z!xLyii|YmNzx8KO1k9f7u(IY=I_-ZgtP^e!ZC{SoTJO-DpCh7E7=`mx(WiPD$iA=- znsz>+18bAp^No$q?WvjXIoPA$whOk0@9;Jr!ikUgH_?ltnbJ&OpaK_ILYr*invj*? zmY{XS=fy%NBq7jr6SMiwg(bokUT_QzJlFqTvzzO(&9nc#?_dDA2We=n-qP2lb&$h&4+#q zB}qVIqC)oZh8@QAK84CZzMIP@|JD=thkNtvxtbGV>J27OCX_>JcVi1JoCQ4z`tz-%yH>d>3Pz`F^6es2`KdREF|-X|XEj1^690qc0T3_O zDj!ZF=^iq09)9)$Z*QAy_aLt2I)WGXbuT6QTrG6q*+fUnCA@q+V3xYvczs`~Cy(xR z2TsCg$FkMGF(S`@el%Ksv$Q+`>fGN3@$G8dqk2VfW5kDx#_-^8S4&TOZycR?=q#)( zcS$v!2{RV{GkS#pgPH&TNG5@Hvdu`i+H&j6&!%m|_;lHkDFwj|3T8HOahZ|SkKv`L z@Y3$JNi`~!_>9E)MRtN89|4FKtz(7CNDer7Eyo*2ZO#nn z;Liyb2}pRY*)thkAIIQwC={(k^znDGNIsYHjl~TdUbO2l2E>#@%qEriWaX{u;e&rr zvv_G{e6s_C$YjMXB~k`gD_E=)@_^n zb_{5%wo=zef9^?j!!shmO8N?knCs zfb_WTgd)-TM2>73e#@BuC#_n($i&o)P7K@hpRj1l?leknrBMU!3=K-)O$dofFbqxb z9G%hGteLCHGRE2~bht%tY1Te9F6hF+-8Nupj&;n0dd+z*YbA{?J7M*$I$rk}0AO;X+G1L2nvwz(zW+8ST5X ziyjLP7b*#nK|s2swwf~}sf0EyTC&m$JG7?pOlY6ciMj%2P5h>oeY}{wR&-$dHPks+?2XK2b&-Rd6YFP%Y zy8ZkpCO~d2!DL9J0vi;oa?ir~4P31Pf-!($g{h)Xq(TGubSE?ZUdZ`!wUgS!MP_Mb zdD6KX8EQWt{srt>)-RbNh=~v!u(mVAy6f@|KgkbbG)lL;CiJs;JpG7DhzUcz8>A#) z5OyeD{nCo{l?XB?^uJlbT1p{`jO(Bsv+iF<7l4{1Q|yEV>v{E`j)s#MLl(grn%~`! z7gf)T0MbMe&vAnk@HJUh{6W?n_LRg+CikGZaK<$hN8g=PF(IAbQT3;v97+6lb$(@? zJ`7DvD|=-e<-DMN(n|(du6b@tD(e-eY#)W+l2PuAXekI!K-|_vT>Ick7UMxo(a=zUL!6ymXLYr3XTXm$D}aRO_y2nV z6cF&uCVqu%>Z(W5DLK6~>gV#mzZDQV{-PFIS^VCiPZmKVG~lb9ZZyzij8@XOdDpYW z6K87^uDN+bbMeXv(~5wapaByB#I3LcPfk+syUGcTOfMNQctj_6ssgbp036N>_}~B- zO_ID&h>YyBInRw>#dnSx*#wFldd@8PlK~mPp(0BsaXs}(F?S{=8aQZ+oy1}ALtz8I z-XLOBpsan9+m@87@2QzHG0I?!O@i*bke2M#Tu(Y|c5y8({%q>d{iVp%8Sk#NM2&2> zx7Z-pyr3@Eqak0zBA59uGG{B2cy_$;|9fo4v zi)YF6AlHuIJ?XLH-iP;_6Xz1yZ|h7R^g*Lx8C=~)W~)P8sVeZk0aMtXO0vnQC+i~B zNwDVC82#6BoqZmyYNUE+OuVc4;n8w+QH|cyB2cuGFGL~8Xn5gOZhE<95#h~$;NWt8 zlald1tmamVC2x~P?Y_sO_H!SG4->`~NrrJ}?a?4l(< zcvGlEo<~mxhGMpH?bC5l^K5!)lEH;L`R0z?H90&{l3H1om$dPj)eNMr@?I8f2z>kI zwlH7Ayu$){$cPWP>#y(s$h9>M5=5&}Apoaq>tI|p;TQA*dTeXjbGTD0G*ct&$gdhz zKQhwOrNvdlLp5AP`5lR_|ETWnGh%C+nwt@6H-wWFFx_T-?PS!UxhT8K<<|3nPN6U) zijdkduI>sUiHy9)Zq)|}^t*mhvhXEn4)Wos!hQc)qMg8F9PheUjM;f@{ALLlOUQn5 z>XZl1DOmKSu>wiu)wpDKX({IFF%S4ti#;sNVhgl!csgTDGRIhloD0?PLgtUrn*@%7}snTjJ< ze&eReAs$hX?2~0mj0PE7j`I0s=mzUekas<>>Q{UGL%bJs3(S;jrPZ-)lgo!=X5#Ff1GJ5|aUebupFY zyoav!A%A!A_ALd}f>vQ&w2FeH@viZhU5RCjA|T>S=KdjxI6@*ht#Z2aKbD zHJ^l!1_2$WNCQr@3Sw}UIR6dIb!*;|OouXW8Il$SWPQFVRX#~6Q3*V$kOJWxstU_J zqFgk_0Y=c7Lw~OIZZ7nj(z={@(jeq>lS%`k<*f8@i9(j6J|03V`I(_IURac+N}7AUvrg!>SU*;Q%m* zfu0`2m=pPqWD%h z*g_!uSU(!^2sukmOeGqK`SPj}$Db5*xQM4Lk_~Cm zND0^RtZ&q7+n-N*^jJ@&zRL^*23kmzo!ctL%F}R(0mDW#u+XPSnD7Il*Gak2p@IHg zf@D?js8GWeG_dZcrnBm(xbXN^$Z$4KP+oE(d0oCd1~qvPOA4T0PZZUwj z$wbhobSrR~1TChi5^-}l=oc2%vlvUr_k(F^K`M?nutz^*qUYJ|+Cc8@4O&Oa7jTEy zn@>^IoCV(e4yPKh%1uU`-OipnwzMcEH|Vll089l8=X@!US)@qJMMDkBCp6ilF}DQ6 zo9C{Yli3xzAhO#yKWm^3X_-q$H1M5RLTZd&fO?!9h@|?>;rKw~uXbXe03^n$Qp8{| zY>Fy5Cr{4;X#u9rfzfEym|r3E9CJdLcOWC19p*T=ab$uwQEKjP>DkjL^e3&zt6ACMen=Ts; z{Qz(D%eMzwL-X$rh|u}`Vp?i7f!PF#H$u;!*5n!RFJ*cr%T2(+f-j@aB7FZNo_Lof z*%>A-lHxUYjXm^Jjw|`j@(f9wi|z{c12;Sx_y!jpqHNf2J=!KS9R2$?R&2BTlPMP_ zWZ*q85r(BBQ3iCYK%8>_VTaQ4|Au}=7z3O0CJx5-OAcI2ikyrNTP_10Jjc_QXUmEM z2NL>J%qJn-JK-l|T>REe;*7>slVKBA*MOep{x$ZL-EVs^umKvtpqh4#2Rg=&k_+UN z8bBuA&4%iMe%3~v;qc{08^~Bs6rAs4*I}i$MpN;4E;21LwJFi*=UDh-U>`fxyyWvy zG3JN!Q&D_>^Fe56%RAKORF+nTeRn458(iAh!S(cVEyn8Vuks2k2+LOfh(H?l?hm)_ zPT2l@CI_8BTz83|BU4k{^+FiK*VvY{{p`}ujF@vIA5{^?FejEDUem)#^m3Vi{yZ}F z$VEqGgF54}NP?+_1A;Y336m_T>5z*pQ zWDs~s@Zg;<)47I;(c*{Gd$z;gA?BqX!+godrC)ICaM82Q&s|I=g|sej5sRMRau&Io zmvH1|6j^@BOD4@+qCiHpw!Xf7EYBfgC$Vndx^b82%<9a_EkcER z@4#d%D9_^aTmB9ND>p}BBhn2a_Uns|f%h!a<1j$!WCykWjtU>5QtT(ET&>v_Ti;&$ z%%i|J7q-n=KZ610?5q&*u$XJ>t*BK zS99}}xcTIva=e{$BM6Lg^@Xfp_s~r#XZ%lj)$9FN?x}Cyc^N~3HF8_uGJ|swy*UYs zR(*}T@)MC7p&pRGi0Ml33{#)H`J;ihmvkU{3Y!m_hr~9UG+)&9`QkQbx%Iurf&w;G z>CQiu^7`Jtwkx<757Ix4WxVV}SkrzukZ!tJdp!DlE_8YQ6Z`cQwLvt0luob3k&;D^ zWDmA@ekZ~HVm)cbfwL&^n_cyN-wSDPQi>p<@ zOiCMpdhf@$=~02r%!SMAf+a;D_~oW-&NIq7)3XZ}jP<_GcTGzM#Yc_)gHA5kQ8BZB z0u{RwGAFR48xq3VR({d4zF2SVv0}cu$WE#oDex8^c~TpNPw_GP({U77m#j%KEVCUu z1MaQ6!_A?-n9MRGG>MWB1}~&C`oeP8NQ~rV$Pr{)J-oDy`)I1G*vottusq2}+Ox_x z$e{sD{-GX{=zY<@rC8utH1;=A&?E-Hz|3$wq!65uytMufXVSx@s@k;})RI+KcMC|9 z%VrGhN^~~nr!N%9j;gE1yK^$idon%$RO=X^uEcPtJ_&&}51_gN)xnwK6Fx*PH1^kJxXxNjNqJ3K9e zYc#7WnNMv<(~=M%!Q=^S#bIiHZQHuP#m!v4Hn6R_>U{LEaB-O+Z^;2HARelI_Vo6+ zgXf_(pNKEl4|E@!CJmK6`hqn;0Ixl~^lKntOgUcPdJ=nIG^q{9) z$d{$$|Fhcz5@tGI2|naOGLD|DG@R54&2ESg$nYAs=NiL?N(p;zyJiYVOsOa$w)MOj^EY>s4pg-E^pS<$AN% z+jo#^@O#S1(ib&jX5{a7eott-LH&ux2}`9`r*xgO^(gFRh;0%!^PLxuriQ;)PB(Fs z!l+b~B5|0zp)ltJN&XACG~&5{?n~9Gkr=fl0>5|yE!|?+PqlYc+8X{rNb!VAkLjy{=Jcea^ZTHuRJPk9 zG#ANZMkva5-$P29PH#=_mRAOe+O#=4&_8*NrY$j}<@dFTamy zTP{EHu6Rk@1lZqvr%2J^NgXjU8oDiOS(!gGq4X9 zEZZ`RDVj{T@qON=(F;7n`Q-b~{OJqAiMe?OHTPHqy}^yR>WkVVkmz=`Z|ES&DRcxA)! zYuL12r;A~(40Nu4i(YqO{mw4OP34cmJX?QXTiu`92^ZwE4ef7Xf&&WnK1ckLm|#OB z4$s2JyuJ#?enuES`h7(zD!&5-CrIIgdpj_TqcxlGXnsQ3_8%;$bt9mu)o#!1PlJ>F zJRh72%EzzY7;l8d>bx~$`(YCY3c9}1M=-fF=iS&*WTknbpM`7Ue#UE>Q6-s0J*tcF z72-Nk1GDxKR)n|pPIPxqbHwleOk5q^j9+&Lq5_N%x?}9mAn2KkPK$69g`$5i-$X0G zr!nbj>*?j>=HiI{sU&2uDn9#Zy5Hr{mD|$N;@Q#J(13~iPxay6(*xKZI}m_k_e|=O zYQNvSJ-b2t^}H_^VsXHIVwUV}I{%MBsjsh(ii#Q=8{4t}z6mH|M_k+;QI%0vc3-pd37N4DF}$41{+go$3fk^bi<|zBbxf@;#qCwL6wB#7HAn9BpD{FxifSOu|qT9<#99R#1L; zwS1bS&i*B8@<@q! z;}H}O4z^f6FDwe1rlZ@uVCkk^>H4$F*UYsF8~t374{gjYt(=U&wFSRH6mO^8GyYNmtHPTf7CN?w<@8sd;+jXQ$`!B+35%x;t^d3fZp%CLLhCzPx9`-(g7idS54g@{PS{GYvJ) zNSmR*f9>xs$y3Y~yj?bqYteOcGhSmS_Wt?vxbgY%q=?w}Ii+jT`bWpx{7|R!bD~h) z0bZ90N9oe(`-MfgsAQ9TQ1{gRIFHQR4m9-VbJ6NE=L{|CHZk0GY1D> zSJ#_ZG3BrTp=KJxR$3?zHA^=`LqivrZ{lVFOy02 z#%m;`X-zjM8mVUcIfA3QrySF+`~`a)1q3AXvw z86Sc5ZA3{TMp33!b20HV{?~j5l3;JbQ&l_K#JCg2XF3^}V{AIMMmHS+=jh4mh@elc zda(^-US%`fgR8tXNHZbYlopx!pO&sIy5RaY~W|G^6~53sccu8?0YrRv^HF|;{7Qh;aG7Sg`-t7 zoT<`iH8W#YiKt0Q!a-3C)Mznv@~(6#@I$*Qd?uHdY1-$9l@lG((GC}*CrN?g^)su> ztKt2wDUmRigw7umz=u~6fWbMjAk4yMj5)1QU*)ooKWt#Eqs=1xhnN-7U$p^wV<821q zJ>QX5kk4+Oe*Z?kjSmQWvT>Gdd>;&ml;zwfCm>Qim(`^_wR^tDF6)!JUkp%8MCzza zR?91T^UQb~w#zYb&RDos-1ijN&F(59d znnZN6A3O`Rs+XwhFpTLAho#V7D|66wUd9b{t&d%rK9)XTxvi?-7Cw)8gs)&vNEhlO z0ju3oS}N!gB7(>ZOU->Zg8HxB2pC9B?Z4`^WRdEa7$lh-zQ=t4f6&leBJz6!1En!< zEw5FK9{;GXUttx2XSPZwB6KeSG5z=(FHK?pPk8A+8j%?a#N+dFmqM@UENNN&ox$}f zilA@oG=Z+4ozpjZ^G3Lf_G^W1Qxv!DA53^ztb4zclC!QiIZ(;&wN*?g1sK8d(B6`s zx5_m-@L?3W_1UQ}cYScMYxn88o%z!<3FsoCK(=Y4J_g~S>1QKq-aXOobLKq(NY$nPKGGTfM%*VYnH88 zJqES4y2+#|_rpFOSACDlAB`1UMA$z-3og&$aMNopBUxCR1hQ!Yj1(D~9E=n+84_~L z)bxWiIK1Ygke+9~bWP#xvPm}MGo7xTVu(#JSp$u3u|HDZE+lM3T(G<-Rranh_+Ah>&a9&Tmrqk^178eqwiwCwZT8 z;4jbC8}x3|C}-?> zxJ!OnMTHJB8d0y;P*^wxiDoMfKZ-FY*m4{#E$*)ASTkKhf8~b%L$0^KbN+sMJN%@9 z-ym0N#qa;VApo+n0*d9FHee#gl|FkbA3wN>(EXjB2lPMIt4)HZmma{P_tbwY1MP=u zQ*=ave?!(n0x?5dO?bb=jBGC+9$qX|z#IgF%SjMG?=?oPerp-%gKS0iP~!#c&Q&G{ zYhetXl;6^5cU`2@(UaXm`^Gx^x}B}w&2z3#80XxU?yl**A0yssQ6KBVf@D|-zZqnl z*+qF8oiOsef9G5#qGs1Raj?+b4gXWeOi?=)Rg$%NH)oObQC z?fSN*ZwHq!-Va5Z9h(>z$I4oFZ^N>6ZR*o1SA4&8RNm{Wm52W$h$_+<;8N5ai@Eg$g_W-Gt~BN2Y=<;E zGf)q{mDrC;h0L%N=C|*R%i!Fp-LR`c+ECmPYW6kpZu1P@!UKLqbz)U1I6l~Z>E_LC z(303-GrzZivAnn3{mnMVF50fn3C#tkyVUhW?h9>M^IQBXy$azU?-a}bUS#w&onj$K z-n*^gjX?dMGI!Y6qe)3bMF%1k3znR}?ma7iFn!)h34K8sa^r3SB2B(^EuZ-8HhVpG z4vB%=Gj+MYoKEdm>-%*zHBI4`-e{{Pc&FVS`5n2nOPZ}~{E}BtI6pfZcBM#Uz=X%Z zD3dH`>C{_Y!OI9^qFl0pnQjgeoBYT7rqHNeIWeJ7)DT+^FaO{MHQS<3iUl8%qW{|1 zH%9eNX4)-JRI!!pi#?|Rb@Rra+q3na4G_$<`*z1*Z&I?5ot16~0lRSiGW>)KzWfh| z7j2t6^_L&QAIlMlb`uJU1aaSgyZ@p7UZI`ifa=Qk?EaMgPd=XSd>Ycp360u_RD5{# zy^!(n@TjsoC<9+H#|^B7~(&Q8#`4#OR*$vYK+L)|By@(c-a@M3Q6y-g(Qj=l4xw zDhrEfG`)k}ej#rD={+)c;oiM>hqzBQJ#og{5Qg;zN8){C-US94HBM&uPt)?F(aJekt2)T@5PHC7UOZy3_cnMXMuiVF#WTF@|}{}PibyM}>53#x9f z%kN+8?PXm_PfyRtq%Q@V-+wE`DY(^tnN6$G>wq@^BE)*_FVMydT4Kw}XcsyGj2(!b z>v})8OOvG(_*3-e&NS$(M&$*CVdshAtjF8zcbrD$cdv;Bx4o*SH1cQHFw5X>!Z7!P z!|R+A&o_yCfcXc_+i3N_lL=?F$ffss{U@l-_HpJP|A2w1zw4QMw*FnN%h~P34(#t! z?csi@ZItUUP@6@cF+j8(#nz0`)+jg|- z+VSq#^JHYeqaBf9M}>-!isZTX8g96j9B8*wdtxfwiC->9KrJKwW^r#ufR_+$d-Y1f z3uB-PhdAF+q7nuNh13P6k*o&x<}Hp@|2TQ@BB`0?ctU0cCnpmQZ6g^Tb?8K)0@aB< z%=$b^7^9Jp#XJ7QN3FsQhKB4W#jO)i*%h_-M^Lrij3|!LquNlFziu8RY-uT|LlUbm$)aA$66(LuTN-fBDa|uzz5?SLR;|q3k zroMUyFoBKFu7uKr(6kD9kHGSToTroaNy|VEbS^< za}xHSv0$s^wx}^l*wVgU+ETshZ(fe(*5`u$*k9ct6-vJJy9%mUy*dXiE=Ho_vKMYc*{1KAfg)uSHs{|a-_im{ zVn3K3_~opSV9zLZE$w5zNcB954{1$e1S zx;uQ;%I61cl0u~dM@IN2GSwqT1W}7!pyUzx;NdItLf7Ia^YV=xn27+2JDPN1`vQ5;317F`rzQ@2`(7UMY zp2~S=65@pgYu5kqO`IxAs9|u-^&0SJ<{2Tp-t@tdPOz#HsNS&O8DQvGYI1>0%9imM z#pv|Kq@`F^9>0;rkY7^6g_CYQF38?5msY{v)D4zxB9QPpU##q^pP|?(Z{@2y2R*T1 zwcuX8tca;&is@}Jj3TgV*JF~%ia96$QRxsr4q7caK?TERirnlB3kF4DNw|Ib6G;B3 zJkwHHgC=HU>aI1;Jt5L5w^E{}<)>)fX*?ZO)jdhS$-zy1RBz@S{9X-JzB<~86;nmb zw|I)&>5Jj?tkHpO+g!iKH_{=us*@z7l+jTSAKdETJ@oB-BTLh?i(fy>=QlvFC z<&I>hX}*;!>y+RuE|w}-Y`T;$W3nDWut3DAgw`+zdPE^R-hnw;rd6X3NV`&_Yb!T5 zQz&(+fd1rQ)m-ovuj8e(UqI+}%u2~yUL1qy15B^n`&{W@HN1S)E>x~+FWm!}utt0#&(a%=%23Z;mB)XV!!0J=^bKrmt)TfBi^ zYrO0g96JWY**n|@N@>hW6E9-Un+aD?NlrhswZ;$k%{D?Hm zN#ZRws{7dC%xxv)-ZP5nbZTnt&;72Ci+i1@d0VI2x>nm}tE?#3wm#Ji3U}%pI0eNm zc5VUuFK>ow3;`O1*xMk)8aQNwfJD3*MwDJ{#2~10JQ@NV+dUFYe^R>v(-<|a3owdb z=ZgUJegThzC=Q8t5C{rb`5G{rm&lC)f;6=B^mMo;kdnFjnPvlr8V2fyVvv8VnIQe^ zkLb+mfWA)34VM#`X+qZSdTPeSo+dB=TCREQ#|K~TLxJoLuz4eeVJPG)y<^3l+`png z$bAUA{2fSJcC@pQhm|gq>}`O475^d7&bxv8kLzYm+2#{HvW~k1OK`!9U7Z1j66`*( zw3R-C<1QXVwY-w({>|-wLx1Si7y1L{uJn&kOFn5QbxBVsSXh-#=Ee7G#+C-MKEpko z&E67UIiX|YVc94}sO$tb)P4~%#uKcOcI=N4JG@;TrFNJ5&L}?@)Gfyj=#RX4U2*cX z*EQp)Tb=Qg|r+$vYm8b|Mc8TAEdGqIcS6Xc@@K(6q zl~98em1HoXkygOhY=`lTV8+w!JMYyNlKpoq0s27Pi({709Tu`D=x@eMCdIlM(BZv0%Yrv*|`K^So}G*sClOCDJenj%_V9)nm2>h)-L5{pIeX$Lj?j@5TVlFYZOLn)|PFQc}oHj?sm7 z^|cn6RCrPx8~LIW zi_7=TODLxDy$nMA6C_{ttTE=$oM(>#dan>vm4I7}viW9D0ShB5!<}W%>;7ZgF7@xy z@G36_Lh3+Rh>r`Pu0J2;=4NN7oWA{Nbso9YI`2FZxby%xJuYB!yKkL5MMKcF2Fk?G zFD^{$lf1liJim`MEXRJ%?-|yi>|>P~=>q#80N7HddAkY|y2I`BPRO~6?o~M-(%;X% zF87&_`da=@T5XP4b_?by*Mq5t;QUf%d_BG{n)ip5>K;2X;AYr_Segi{)rnW5A+e16 z*M;bK%Zh}XIfku%WVUbDJpgMWVy?f`?1KZHGjmy_4r!G*9b8WJ=CII9P!n?Qsb$}ul%>@p+!wWM2>qi+0) zq5!;)!4NwT?BCvmAXnRyEyC9Ewkl5A+TZ;fS>m>;QmM1D*k$A))rn;iMw+2)j%*ua z9Rl$Uk~OX(-IHNmiE`2Og6aQnb%P9#gYP{c0le%yYtV1rpEzQEBT zr7)t&4+cj!8iM(435>bjPYtraY#9S4jTGN-za_O(y#q}wlvP}UsDI5m-Vjm}62n-c z=TU$n*sihPaL*se5yF(POP5ckAeQ{`&CGtd#@%-JO!igcl>FqtX+hxLL^r~ z@mD&#Y2H>J<}ZxYYBHp;3;1Kq-rRmJ1LXDXcq6ld`&pIhOe|{0BZx2T=|t}DOIE5s zJKU`eeM*axm&*gHO4xC{biZcn;pn`agq^##&Dm~g>ra{10Z9=+sG7%(kCLL2QX2PT zuHVHokQJsuNU6e<_8ca-@^1cV?6*UtgcIsQ3@)X?dGQB*gIT;`!#B3}`8g?0@Ws7R z8%nuSgO)&>Imf=dalWxJB*eI)!j4wRk|P@kp#jPbf#$pADn^FL-k!&g8|$mp4F^-;CK)km)YCk-FRR`$w&f!CtXqJTUp;R#mChcDn9Do4S7w z+|m(B>L%wICkHd~zKe34Qb^%K^Y}~H3t#{J#|eEbVN=W>W#-WZIcqk7AxuI| zy1ru>fk>kDA>uz!t>@)p?uHME-D!?$QgdXegjVQ)?5)2T3xkT>Jee&QMABbbwN`Jf zDGW=&bJ8dJBM9$~AW*!W+A`HE+G)mZgC+Akim9HBujR9nadBb0NHxrGKlA%zJa{;n zK4DdUsj;`JlKH;<@sW$_>N>0QQ7gULJL86=FGJn$#0W=C37lWWgI9KS<**V1@FSqq_Qn z*WFL{&*FaMf3JRS8%Jm}Lr&kPq%66*{d}HsR3d>@I(hEzy!3jTEJzr8f2TsCykKx~ z*WrKOe^q44IcfI%xJSj3$7*89BD^GpkRtwS^^uvn;QjQ@7DYYNaw)$T@VYDHF7Wn} ziGh*P^%V7~?EZ3mTA8Y+;R|5;7`ac7gOl0Z3LYBDDW?iVo1~DP_+i1a%)8-ugUwE} z#CY;JKX(;aR)}4=d`Q}_D}@8FrHzt5=WE?P)0H@&^1(g8l?gwb{v<>`+DUeL#9Pjw z;YvVs*FQ^AbYe_Q-YmMd(EV}&?LSzrS-SBuL9?90>7u`JG=!)1H*fXa6#3ClOvx1BNa)saE>Sk-q4W@)Sru9iXLq4|h zKhf@_-s=Lochh1g*Ig-1DBI;Ey0k!7dLTbQeA3PLh1AVyy> znPk`q26|}FXy;Pz?4+X32S+ad90!#mfX)By3H~dXpY6R?(6b!b;LdE$pU?x2k zA(bxjurtz~^XH4|9Z#TrzUnWBbeePG(&n`tmVgShO%tIsocmuwKP>`BrV_SWr12&^ z7wf7H;>!F}MUm#U1z%%2bp3o_1@f?lfqceS!KAS zZnr~4FFt=GyXxm9{k!l=AeG?;NCN$sIa+UVnjDl2xDDy;Xr?j;0WvE`X604ECBS`4 z>UUUQo}~!A1vHJR#at}d`Mm3n6cR!Gn;cu6rid#L(>Ot-3@lu zBbgBVZ)ri^<2%hgS$M9!H3gY~_z_w*(GSVBony@VhY(bMVK+HvJGk&u&Cbq_HVF;r z(rMw+k%(ImT75~#GB%_zY_}tW7CQLwLkN8-9Z2vB77Q3JLn;5J^C9J&T^CPKw`^G3 zDrsJHkicX^W-~@;expGE!Mq43M!p(JB<>pci*e8hL)|jCH?B4u;iFqHBl0798#Evu zmOXyF=%8b$&!5_jgzg>)VU*(()E~Nd#lfsdtJN}V=e+0;8E4C@)l#~?l)78X-!~_! zTSF@B#F1cUIVxdfd!mwJ}M6U*$aSuGE+IXhG%JySH zcP~0$;E=ItZw4bL5ucP3(ln4yM21l{aTpShyiVJIM^n|cbj%8xc%K4RuD6YFyNu#x z{AV%d9QGVyeO|e+Xfv=YmTi?}jea)+d|xPD4}5(U6LY+=Ol<9Gzy^jtqw=qE;Wca*o*o3@ zMIFc=Fc#-nZp0crIG6&{rDuFqULQ?Fz5He0BhN=Pcy^^asU>>&gx7MToDZ|*w2x!C z$Ha~~mBkN{KVwD&PL5aloFs&7 z0aV|a3TLEg;!Vm^A$LY>{%B0x#biM7SJp#y&+eMD3cyEd_j#-8K_l|M z&*8;MnWTlKK{nct`JD~6jSTu3{`TIS0_iXETWJhOPt@ifRIO*FohR>sYHt7!A7EcI zYEt%tX#_7rs*f1*6IqXItk5z|NkaPR<_jHhdrX#(kUW2+B|({%Ga;Gb2V#1L@r6#) z2s08fZN1r@zQh5|TKwAD_LyNuJRt<+r*YQ`RTlKP&z9&BPy#egDWJY3VJ{{4zAz(C zPiXq+L3Tm?S}T!Wf0He=1LBl)`$)^e3`rB{Ba z!CM!v=`Nbg0?OZ9ycK%sM=|&#Y1j2xxeY$HC->0sb41Xz$Z{Fr_l3(Ot*1W;VewcH zzobgbt_rB2HMv=zzaJQvj&W<|(;dGA>M`fE2zYa51Fkg7`vn5#0wD z@6T1{rn8|f$mj~`+k$fBmr?pP6Xrv-fqc}GeonZq^(iGIN+Xm3uUf-1G%_z5AdPt6 z76=J&aapw5x|tk!vHD)v7`&vPUYqJX0?728)mI*biK;Nj`FoFqs`738^2X2gqv&H$ z=oZVn3_n;j4=u`k|3D$p_9c5=N2fMSCBu1g7wT`12pCa#( zhz{qYnd@sungk`4A%TOt!sLk@p$|aU^;3)cRsNVL35UUc8ix?c4y*VTB8k49|8R^a z<}K9@fJfi|1)`D&`L5OA4S3V}rph00&5L<%;HzBA|M{D7it6_# zK>|K_+4=i4Q)#*N2qp&0F-u|D7%d`8&^lwAOMtPvH^4ou!_WH^*MPB86u2N(B>FMm*32fJN*_qAZUK4NzfBh zEnODIEA>JOV>}J~Kr1&zIG9PH1H&1Y>!BFsqyC@FR@zJ5NjZ7sM16YaKYg6Ua*9Xn__FPwGrDvBvL zIQ0OTrU*AF4KZI@!&<|MZRz$=9ry4T9kX@dvyP{?j2Q-Xb_dT8ZM|-GO|=OK0(ewk zHk61sOf2h3g^)=6X&nv~P)J~(3mAEbPmj&WpEN+t=BuJJ8%yvW);> zpH^GH+5>F=Ht0$C+johA2&N8WB#|>S%B`AW^}hwp;2RRYnv*# z>DT9eNLpm=bujvu31P5;2YXXlv0+ZYg%sGvQQnIG5MlM*M6VjdLtuC>24{Rm?I*KC zRb2_A%q#SRPcQ;P-AmMtZZXqr`7xx;*rZw@fZ$|e!_jt=+*yZloOpJhO%xoo3JYax zVTxu#rUvy3g0oUkfxEOD25p6mFi5|qsU6HHwJbwbLD>jL&eChhL;+8hgfh7Obv0U~ zP=o{Jb9?(WY|%<=U5Ru_) z#Qa=e9%nuiZanK;PNY~wtD_oQ?)-GD9rlBq>jpfIbHDN$Y+&y3D$&c?S>BQDy6Js4 z>^aI3H=INC??&ths@KS?Y&Ng$0&P{cX5z5=q``Ulh~_`j7dyR9$+3Wpy{I}na3{;NJ;kFo+Dm&wva zx-4$Mblvcy3(XM@R29)jRY1C2>B*t%_zJ*=0ymj;`J@xSF(0txJ647xeRgi5mqx@n8Pqrl^ zSl&FS2KSQvx!`zwdinkfUzr0sGfbG#z?7I=&8V2aG!py@2i z3>W6D9?c1_f{dR|H6C1RFr}iJzUhisdRTc_JQ#`y2lo_`|BkpHiq|fY&yo(>=@dWM zZvqRxEKinCYaqj2U;P~~@r_g17h|tmM@c0*(Dki*^7D?b_9MlMG_~ee@qJa;9j+G5 z`|?dCv_Ia)W|2kD^QZ~)iSXlybpoujENjl3^AITwpq&hWm*fjM$OQA$lF!nb2#QT9 zA(5QxPp+ntrKjqDaggSetrreCR|EqBlwlf|nR8B802e)8YCx5NI(TtyO;<1{rPs#F zY8dMZv4cKg=@C6ylE->xT4?RZhT6u}qAJ6$&IBF;i9x#)}+@cqTrdeY@;22;e09(`q= z<)mM;SWt5AV#~)R;6@`3#Ov6=&xUcCtO~94(@XfWIg4yzSmWXPR-hi z7mqEb%aMcFadt!@>cnkora>}6dZXzw?LzJz<2>)+CzS+dW|Jj2=zoy-yzPwoY8}=? zviY_re~k^5Sz{MtuD??wqwcIh3*3~xTv=dX8xZ?!D;PT-O0Y>A9h$$hWCf{f%D5b5 z*fVi;P9z^A^DD-;bq=Tg-W2S#59{8d{XT?Awxjdzy&X_z)Wu&waZwc+>-pVZOKd~y z>K+AEXo>iqjdoJ!U^??7!!ocg0|f8chNhj5W@6E` zsdcQFgWlcUU@<}xtw7B1IBJt7o_vL-AOPbJsJ)5iUB)G0Pp$}tg!tay988Wr7ri^@ z`9;5G>D>#NMs`z9{*A;-zVHOf%Cc;SgShWtQ8r)wZAq28PN2$?g!^BWC8c2t#7?zZ z?7yIQAb`%M(#SLwW4TaytnPGgN|_ltuv4kDTr~%(AonDuz(8BqPZ|6cZRc{PC}=(U ztkd&lg&pq?{QCYM%VKdVNOwX!pbAT*nx^sjlmEXcU*L;3Fq4H!-eY|FM+PA}Fb@Gw z&du9LmofTj_kJhKzFl{N!mzuM(qFwquV&Ha4mdNxb3F@$ls93R{9Kd*_xZB*W_Zc7 zHgdRJL~VRY58i^HC6u$3*=PWmgT%!U@nKx#U##3UGJ8%X4(2T&bE~L7sikb7oX$vD zN(s+XbKy_TMD6Y8a9{8!9w7W^LQA(lC1du!YveZ&_nG~xooCb+5dl;?Bq2Z$QV=F9 zOzuQ|ZA>qfKfs^PDkvd@Z$p$=?#40wGdVVeLKDiq!>a6m`f+CPX`V#9DjiS5&&GNu z-QIiA2vHZN8Us0}Gu*Iq+Lqr&tLBSqlB8HIl_}Z@fe9-{8JT(yKP&T(Jp_X&RLf6_ zUzvF*-}iO6K6Vw}`&7!q{SFh}P+!4WF>jM^{UNVN@+YJa5{J(ZNWXJ`g^+#^ zFUd~lbF01z@q^FRWeEx0FqRExR;i?$LKPpZX=>uY-s5%JA7iZPpzyeR+`#R+guyPK zM&&=2iW$jx&2aA~9e(s64hSjL!@f$-&Sps&E%#morbv=gQUFFP;MH_{oA&F7LQdsf z?!GtzbyuM9{N~Gjd6oN{+7&~a$74l-}Da>i)n zOZuZQhPju}B<;a{Tc+HE*|b5$T&Ysh6G(i{9*nQ$${zj0or2(}<>L*jS?ufL27$T& zwIVypG$j~DM@KX+pcns#9X07^aau<GuR>9(YeB3Mz*_JVq5m3zcD?8E;?a8KgMZ57}plY%owS^Rvw~63p`$b zVp7K%HyT%#v#pa=ntOQjl%c`gn$@wT(Ig~B0Ft2NA-4Yay8iuZohZ*h%l`UiJg>TC z_k=nw{Fmpt;BG6uaT7#73Xjg{)6R#~IV_4OjXozu z_Z4BMYPh<(zlbRS*}~NF)t)GiWVQ|6o4~|FyHZKJ;e$JDb&u0AzKxnpkdF)CuDksV zxI3ELh}#_1Ugwz}zIhMSL_ZabDX4D0H?;!+_%T&adC#Y7Gy0Aov`+mRRfh4&NucST zmXZeOhG;hi}E5=Z_m$q((As&GH8S0{SRv5YPyIl2*gR7M@WHO4?>10B>y}HoR z*@;8iqf-GRiTC^a?MsQV@Q-Wz&yX_KX>5T&lR>Vde01J>`$(O?H6@F0dC9k7h2Fs~ z^o`j{g&~C;l<%H{9VY00?Ow^1=`KUYIYz=;|BfT_6^9N3*WM(yW17ZYiH_3Kj409s z^0Ohj=GSx19>tYzm$xcH-4=+R&{O(zdjx+bG)iHXsL->lY&5TF&}rSx0cIHgfz(8K zE+Y9fCI?Gjl}?dofrx8N=r)(fewgSi@}ft>I?Y^+%5Bo&6>G&@9tou)Ws1BfEXQ5B z_^@y)39WPNIqVyw@7y4sTnF4t41EHgcv~Q1C7V8sZtp=n1dm+zy!~}g^1OVBE716u`ne0XCut%Qa<-}&230>#iPVo6v z!+NmFNy)_cX^matNV|ic1Q-Sly?j3Xq%CYGB9w-vq~tsbwL)*`=K!o!XOX%6vcH(J zV*v7RR?&(HlPh<;&$Yb}!bXY`5VfMhCCmGDbb*#!D{uNWD{eZF5J6<*WCY{IWB_{M z`AHr`w5-19u#AEvF;ggeVQ>R=odkkrrUrglS( z432a>3-#R-rbX}%tn>1?xc(YelVYMGfd?qCjjLIof@^Obx4Onmd_;4GBYX$_y`$&WI5G+xhblgyGC(jTGp8vxgcWy3e$y zh>Oj-+*?%+zTIstie+jSIWfT*9Ccq{z|4}jMo3^7*wJQJ84h7?6ljFnTH%t}9nfwN zh=~M~iivNaG8L`Jyuu*_2HnoqUawX8yMMj3xc5v3*(!+7C)m4*JwNo5XOAs`unw7&P&&XcsrtfL7u3@OI<9Smgn% z7B-lk-4P+*gPpU^)2WRM+Aj-9H}jkEQ3VT|BZ_rIst0eSci{N;ktetVZuAKj8jUXZFsMj74iWN;mH< z%2+^amWKL9xWUf1YBmnp1e$2kO`e=c>q#w}noYLZ)SztZY?F0S|JDA>1*=dEykp8Z zs5rSMtm(sqkS+o>m8)X;lVW@d9ot(aJRu?HfDO=Q0Jyl)#pO+brT)L9B-38qPm|K@ zyE_`!&K?TK7??7b+L~Z;XKlMLdyyQm0A4=CoWh$Oly=0=1M32}RsZB)Rz%Yt54|ua zvpMRocSY3r3m6IlcdpwyAt6j7!F7MCz)-UpKUATgzalgh^zQIq>IM}4Li9N@>%;<} z#l{8+N~dje^s05i6>jlgR`=Jj^>`2N~OWA&JwmSaClm1_zU zwzL}-&YO0AmYleLzXGu!ptW@jrk^!6x9ePBn`>1;g}B_yG=&}En@{g?X%J=bkxv9O z6&IDKVpx9R$o_i#^CY{7da(-3M?qjxT^k(Qv_hD|QsdfQ_k)TgVlYg!88d;Jf>&Vr z?C$=aq4nrpzf46}aNfE*PSP}3FN3?4Wc2Ll6T@9OXAzj6lcbZ{;7xh|%Oxpwf*T2t zW-Yi2NzFK?khQ-zWA7Z@ahdA)-8moWJ4fe6Bqsan_x3>z$(oJwQCl9c;b%Nv zU0fc`T0lrfB~6&RZwj<3wp{OH`^_kl@|zyVDv-K4H53;b8Ok#LfbLyFN6{ve+nN*l zIM8^wKV&qxlHKBUJ6{7P<9$2LuKlR=^*S(ga$eRB{;TxI#dsHLS3Of}(I8KdM&8{=e)Iopj(lwDqY7G2Cl9-6W`mv@3C*{$0u9nI)P7IS6!WbT&UAN2b+)!EAn%dcAFuf(RU^dG0U1)*>|^hc4iH)ykIBGOR!n3 zBFg#SQBMD0uu3{floo62vC^W;LCfHuIgxCZFd>fYTHap&5RhR zRBXhS={|zgX(t@S+2_?v+465vqwyC+s|!bV6GNcEC;sPevZr(2wxR}W#JK4U=hEC< z%{HvSbRC-1eYFouDo%v8z2#l7qqhau19HD=y3o6c_5Bv+%x+*+1?9ej^tnfXbSGXx z>}GzUjmm|{1&wW))Yp_4D&b1W15@^%W&;{fZy}Z3KD=03e+J23u6{g_ezbH|kliJ3 zlI%x(rGAmUCpoazIFK9c^=`XTM0q1%#FRNZKRdfP{S?bK`ptVp?>$ce^04#eFJR7B z5k>MlUpkFm^CiRfWuKUd&BtA`2u99U>d<4r4f0)C$K=?N9vUT@ChFQX_od_HeKJR= zruPd8ae{maV^a9;->uqh$~eh(VzNOwW$`DCSi}LW2sU!X?bJ+Yf|xBi$s%Q77WKA1 z#J`%L)AJi|vNoC8cDioLkjdbL4WjemO}3_PGnJon&mUc< zYEFX-+=3=g#Y^3`>2Q{_WpwGwA1BPzw0XP~_r>j{57e}J)GlDp(|%WMdB#!RF3+7g zUNmexM!`tbob}}^MMUNg=NhAsUi8s=ifP4?Nu~_y0>>y4ihSR0DA<8YX^#*s*MhzK z=d3PGx|Dmu@%;>q%}hyfwXOpXc?kTve$T}}A?Qea$#?XRP|{C@m2sY<-#m~+uBukI zKkP1SO?KKQ_&(O~QFw?Pf3yAt=TTYHh3AXM+s(u?&epG1V*ZusWByDoa{b3Z`2cC! zKAm+L_6dUt&DjW#4$;(2#a4$M;kbAiM>^0YUoErcNIUK{h*2wgw~nbkUtcPm$gvMh z&a*q_%e_54%`^EiAuva*6h(vp5>LpbFjhJJH<4;q?d&~8ctUWcxT-M zeljf`s+xL~`SZHjp~bY{?(Xhr6|$))QeX&JPkx^U;cO)k{i?!4Yjqo-6DlYj^(iP>_MXh zQP0oMnoR0CI$k&XS!s?^OgNMj7iMrPYwd0q>N(kDGWc>S7wYQj>AdLuJ3sB@WnRH~ zc}C{@=PEMLRaI3W_OD;b_dBUx9xl3n1U$KxV8>l!25{o9yaD#HC?xztoWF&OKRhJv zrn90z&OGO6N*4R)Oapt^KIWo(fi!j7A~6csP*q`08o;T`;~MF0cGy%`!=};BnF^m{Vi4L^$1m5_*V5O~t3uIWWnyAtWj;uD z{ovS=cZv4r#fDg_L8nN$nHQ z4fzrFX7Xmy5`QcO13Ab~1GlHwFs|!nlnS6XjOE~g9e#~r=PQGwZ{67>)Z}MnCG|WM zEM^)V@BLi%_u^f=J2Ut*>C-`ZwP$%7KII7mg>@tsh7zDZV*gbF6zm-kidbP2{HiVZ zAET$-118c#8Qo01oN>4s&)3!Blvd;XJh^^`%?J9hqvv=iXu+bnvA=J-NB9F%fg9-1 zY1g{Knz^_{(PMnO0^@@!(wbW3Dx$*JxcFfV$pDLCpudJ?$trv{f3@I0vms@Lj zu&~%{!&E$_dJ9gbk6PE{9|X>sxXfzU@Q4aUZ{qJZ(qMu2eD~Q`A^w*##>JZZ^JT&s zhtv^y*1en23Y+L1RSgXVc{zZ&QnjnmtF|TS(ucsE7asnDTeja=joaJHb>U~`nla$* z;q2<7*XmhRS4WN$#musI{p2lHYwTfFrpY8PD+`#|0hT$-2;3^ODiIM#{l-eVOb#@Z z+>Vc-JKUr`uhVn40(@}COfzvp6~MYh6Ew}+y(#e{A&NJlSYsW*W5rz4GnOi`!_&RT z_4o)VX&|qxV%#7hoz}uHK}DQ}mbDM&eOCyhCTz;bn44e3j7x1{*hSigL1&&|W@2VG zW5Yjt@u9;Sak|;u?oBa1_%_$pj<@NnG5L*pR6sUrW{KxYnFYZrf$^{$_d-U}$<7Xh zrAERhxVVsPXT9?C{Z&^dqg+U}wo^S0CYjUeFB>N(O@x@;fDECOQ!hr=eiNJMZvj9G z{G#I~dXDsSY4P+W9?jL#gWO=q9cWsuVMA1cw9tVUE{9u_4hshtw_G%dnS;3KRZSU*uZdJIjn3T&u$A`tJr?)AJlRz!h^C9- zVCns^cro+LmI>0&%O`hm!1|PrC;AOp8P`Hu>l?N9>HtluV_s2_m>`v`x>B$=If+AN zX3I^*Oz6fVrK2myTC*p5C3$gnmeh?!B~_%HkdUx% zNP($Jo9C`Gg#&swKVte%Fn`nc#ZJytu5S%_l_h;ybXK@hXiL7XtqP$x(&CtT4sQm= zymJrtFm~kEug;UJ-|lK^*w|J^Kl94#*}ehxW1(CNT!7K+;PJ7xj)n3#3oAxrTG4Rnf0X*F$gt z)|8BmR87+TpanV%rDAu9to7+BbsZ{Wj3K1_`mhD@gBFA9uL-!J9DFfD+foJeBnDbq zX;~QsUP$!XH7<>a1?}xGfCWl9Md;uTV5GZZCXgme(_G`{Y95y!UoH>2yGpm+KbH-o z+R)BOOYKH)<6Dv6_q6$Lde`}3Ry|wCF%Ec_j3T!a2i=@$!=9c`rQz z?i}}bcR*!9O`nX_FffcRNxDdx{w&?ISE?Rx>jPN%byD?$rMcv5T52giB3YtsxQdX} ztvTWxm5!Hn>&X8=6zGWoN%}e(FPax7>a?hZ1qo;~=%>T5X|;cX5*p`~+Tl5~vGtsF z_@A*zyhKuT>200(zF*jv;%B#dvC%cI=^b76YMU^k!z}s*4=49jdHMcY$z6w3Sv+>11s?`dRud%ng?p3@AeKa~29D-WaZ?;wgaFz!){ zhQ=K1BCp3Ce3Y-i)=F{Iqs@|gQDXZVa_oAEOe}LDzm~naqlF;&X=;w|8piP1i4a+S z4ljx^P&SOg@u_2G>rG8cUCrCt$=$t*%Fk-m`EnGjcQdK?+;7U#!vpk^Md%w0~>{BO!G9&u?9-%n%2SjN~E z6)8zMa|TwXZuy;tvGVuUVwMxaIXlV2%YomS?$}c>G&-ZjX=~?6aFUsJQtLyP42IFM zI~#wDtDHWC33#A!=hQiUgPzD`Hh4AOT?fpxG4Ca6hh>x)o0klGc|9R~0BxC&zvkJ5 z=ZVP>BN7EAC2kiRJ_!kFy7oVR{)|4a>Rf_Lj<;|^$neJCSC8r;tIbEElPaY2YhQ3DViF&gv6(@zms~ zhm}^>Z{LrUzt&Xhxh|@qZZ1q#f}C^dJ$;YQ(_|M|)?Kz_!1y17+Z^3{`}bPwMN>wj zDC$gQ7Y6f_ePL}wOLqHIg~ws@?HR>&#hxp4eCz^UBbo7P$JP{J+;gi&Uykn}jxEVy z4=gEigVJ&UbC~mvE)WIJH)*mnB)hZ6R#!nw?e4KIPYEO-+de5{R_Uj<^BJd2Wlf@J zKU>*UkdjVf#I-$t2gR*SQMeUr-1#BQOLaD}ao6P^TsW?nTYxlwLOuUU9{OnSKQ)(> z5Tx@{w~*+8SqNfE0R6E;N=O*B3DD87>dd6bZyIP`7%_B`8@ffWmCNOqGfgv_Fs~c* zP{+zmz>m0{E^g9&)lw`tr63snZPBdS=R{##x3t_~5kt(kzmX3eOs=QQCJd|z7fH7h z8%F4gjVmV~CujLFleH6d1`EET{TTEZQui31+myYl5ckXdnMs21FnOoXAm_oMhKrhr z9<6mvSx0M6COSvwZU+(rB_~XS^-mFI^)-5}V#`p1eqyKj0W-T$Jq*#Nh~gHqa-4e% zy#V#5L{Kslb(r{HQf80mbn?)--@f%@eO^L;pF-^lPFvGXeVV7}c>GBuNP@Mtj48@1 z^cXQ01>JK(K~cdI)S_Gm%)3SgUNU2PWwbC?+Wg-czwoxP61@Gem*-cIc~6pe!M!?6Pcde#>s0ku69?!W@PQxN<`dk zYg|{U(9rEWjy?gnBt7i%iZE_)ja5FCBD$tt+ks_7a=0md0XEBid@LD36~J!bvcYZc zi{s@xGEQbX)s5(5(%J@e%A%k2UViC*FqoKhP82&JM3D{|pVCRbP-nO}o-G3G?&w#i-Veg)Ae2X@uGgo=!)P&uVTz5FfJeiwwSBaLy!9kj7MapT1nn^bNHQlW;IQKdPkDQjEf=sTvtO(TIJODMiT&n)-=50+IJ0xZue#$H$Z|ro^DY09wdFHA`FmbXhnHrP+bR0%wUctc8WLO$TT^_raXiAR zD^J3t-N6sG68hcpbquo9)RRs4%`6)beQ)_lVpi9f5;A;#&aS9t`C1V_%A74Qs#>al zsodor*6KGhhqzM2<$p8o;H$F8BqAf~cK(C2Saa=biKeb$iKgc69@7dI67Mc~XJzhA z%T#~eJYtnBk0P$%q@FSjx`Z9$$j{J^Ao6O@4LloSdcArPeGHH|x|Z^T`)CH=p;ORp z2y!zDWvmb*i6#7j`hskhF#k;8>`vD7R8k!JMUmRD_O7q+e!ITM-sb!Hg{qit)g8dx z`mpp^=JC~i`QDfw_&U8D9hLH@o`eneT~!maH>%uom}UxuHDj0XH9|>*EzQ<)$tYQO zeO)gF=o&7jje&-rm^n$YB89IidA8hBCh7$xEgXo2qbM+gj&?e(M;KSCCaC?WKFwc5 z)Vvw!dH$;nVZf=l?$0z3(M)2}M@eVN9gh+0LB*!?a3aOdb=S#)fhkbnkgi|j&7fs$ zGc=RuCOH}!l(rD-efFWD{D|1r`Y>hV5fXCnU5E{v+nXf{+GurJl(bJ;i1~JnH-lf+ zcHR9E;$EeEz8*iGgFQU8*L3;c6)UrsvoM8&`JW|YCj{ja00M1n)$;Ok0nnRd7tB^gj@Ozgb$p>bbsf9&Mi=)gu%+`;8XW$E4O6*mts_m*@ zVJC?_)}%C*l!_g5O8&wfj0u}VH{hv~&Nm$)<8Hc&3Qp^-Yg0fQS!b*X^s+rqYVK&N z)jrj^7jd>P!h2i^fXHPLb1_^+{uv*sc>js;AeY7>MSf|Fr(E~lZu(p*bThe)1)_e+ zb=>pW%(t6(oq#t+sbL|%IVr06I?c{|Ra#7sx{hYg7EbO%$9aB&)fXbQ?&|1=XxppT zryGDRWsEA_68FoiH@|eAy<4FN93w>I^L5a&eo>AfyU){ z4uMlYS*uU4#0*EL5@8Eld`_#-Z~t4)(dp@&JqhR!T^iP79kCm!lO6g~0rTE@O@;6x zNXxMdQ%_$VLn&U;B7yv0%rA7hY>o$|l(TY#&_#1;`Y)&iQ||`q@xz5I=;%re{RV}` z6|24bfn#wK>1fg5c_80ON&p!42m^h~)Yc^twc^ECbngOdX5M=awGc~<|9A$@1@TFf zy6gx;PW1#;Erm=^M!Y#99@NHJZtFxMq3@%SDThib$b_xTn;~5^7Z6X2w;RJ-v6Wpy zlMh^^(d4fni4DDrqocR1O%HK>h(t-M`P~R8D4W1Q&@Pb>$?Jq$=FVW&&S3xi6~kK~0va@Ia288%gpvQQXqJa;->sso-p z3?&VX#pwcxB75U@vpCcWo9(5g(iZGE=4N;|#54K-?#&eae|wAW3HiiwcOJUWrff(^ zs$^DlPBt?T4mJOGm0>p}MZCBG9UVBjHXNvCKm!kxN^X~LsXJ9Xc%32!x>O~Gy;Nld z*Rs(q?8~j4R|K+&pqzgwi$NDRC#SUgx*}Qif@2IlChFhyU&aO;v~44D!+{xXa(l(M z@ND3DbUKnF__=FK9pm3yIjgFKbb%13a_*LzfYdVp=Y#oqFm?n z*bQ`Mj14=(5fQX(gXz}^kd2Mz4UYfs%s{t+t5~wDz!p*2SOd65Dw$^qc;Qe_&dl{~ zB1Xwh!RNZOQ-^uS0(--KRo<#Cv(nT4Qt0f3^<$o?+4XdZ#OFS(>5qt#d>8`rHZSAt zgtPWQsr#5jPdK#mPZw{_42}rqZ&IK5;tL-ekkd+yj12^UzZx2nWWq_n)Wevgn^C$> zXF;_eqD9lhY0ym@>jUojT<5J)y3L>FwmuX78R`}LNfHI=bTU0cDGw}VLbXZyV;kjd zm9n|@6L5U)^LJjz{swAVMhvBJMshu8=x_;MNEgH-rcxyxVxJg^tFFV+5?^YQlbCHS zzY0TAm(|_4iC@3oW|T^Rwb4?n1|k3_*Q`J^bqm>{tg9)QY%ztVw@4l%7@3$s>miOF}yU;K+29gWfclT9ViQ2 zfl`UB3u7$8%=jjcVRec{yTtp^>5msNMA`h6U-1&2S|XIc81$wHpRB!_V0H zDDqdV(l}JS#1}G`s`yd|0;}uIzKbp$k#YbrZ{Q6rE+MfjK6}J&oft)1x%!;zt}a2- z%xfbr-oY=JTSO;C5yyXf{xmT`qGZd#H5g{;v6+1uk<(UNRdxI~=0xFD8ApepRH?84 z2n8-^(If)Trwx;BSUM}A!=_V`);aWzHPW%wD7|$igZh4NcX4rX)z#ID`d)G@tmPp; z=i!JqmR0z+;!>{Cqgm!tEU&KFgSI=VV*a~WAlwB8-o}@DL9&UoF*O&4t=@obWt3D| zR|iUjn?jv&up;9{c(1R-B%+>+UO0N2d6A@N)FpB!C`fhI5!u;%k3#EdVuYjW1?iSp zN6s9O_~22ow;%MAAI{LRlibNQP#ai%%isH0n}q5*C)&J+teCGlD#qJ*HxSvUn_#vk zYvwS+sL=b3?>IP`=I7&pdM`k+-WlLHtNSrz;bgRc38rI2J@BpV$DyWgJ+c0D-JQQ5 zn!EiQ0FlE0q71W$jJ@67TK@T zGZIsW@fHCG{VwrgFI#Y7b@M3fRGU5#-qOQvLIN@I-1^yPRLr=wJw*#3Tn{`ym$tV9 z{a1M>r*m2%`eNk>Krur=AOHE$;)6B;f6{bdaIgjiwS+dFn9KJTRZlPL@81-};3i#r z@)RAid|C6%$Ukwp>-u8vdC7%y(?cc7Wq<-qKtRC#{k<*cO9epur~+@W?M&l<+N3Gs zLd6SiZf>3lVJ(9=Ousvt&Kx=G=;+AF$(fj#=;&k+6m%`bGx3yL3j$)IVW9l>cCW>1 zJ!E9$JVm;cQ8Tu*cq!||xoMm^;VW%H0fEl5Gd@Cs^zeRRDrG`MV9rR9ogiv(r>Ut4 z8#~VsaBzk-dmR}WS#IcNST2Qq*`lVzN4Pd}QKw2daGEwIIJUue^0)iq8w>GziMUEhu`aQXy3 z$-H}$M}*Q++y`jxZ>FOLb4gKQtgv1tXIxhehDqff1Lx}zQ#M>y{W;3l;_hytILXHi z;wDN4)M6DvaT0bc-~g z+xK=218m(R)yOEY;u3rn6%yJy^UBBrz=-KOZu>`$*S0Y zHN`}#a0ySgw2t=dROs0i-y)S0*#=1Nq?cpo-Pawq^B;>^*pNwig= zN@Mg>`r%+zaxj=QiWk{uP!Yz&lV2v&j4Yb}0NjsHPHZeKFXKBRJ+At3CB(%GS#X)= zkH(t!+Uo`0>;;g@&&~byKFmGd>=StG_fq{WU?BcQ6L-_ivnu z9Nb|WDe3QJz?;%`zVVIDWg8O(-IWICro@~ zEVWmn{GPX#4h|07L?rn59y?K7fEww9?;(86{?G&`2M2JD6)RAxTToY0GSb&5aFr&6 z=9CJOsr6fC24iq-Z0JecXlN|0!bJ0oz4+F<&P*FXs%u?Y3kyWGbj8)nX^i&G zp%&rgG=%r6ABsyj@x%Yzx+rN%IrqbaPgEdTD^wpEynysl{~AWpFb$M;Bl;=g;8#PngxJPom}|$<^J7u+%`m#rPiEjorL|C<7%; z@QkLt~K_Q?|5B z-{Z0#RNvh2iOb7s4gIvIe^6>zrNdZou%+!fuP2_*M`e@};dgiPPB+AHLp;9TAHBVE zbE?2Ud+bG;n2Nm>2^>4NE;#O!^77(aEdv%JGA71?4PQ>;Cf8CL3es?xkb?z`;5tR4b+i{n`{Ut zk1Hj`OQ=W)*DuMI_6zdg^E@5ZT;>f3R!CMFsm$!HaU&)c!!O~H0fze55` z6mU_A5j$>Bn3-WFvYht!X;MPaAgy<0JCi^EiH83 z`fj!tC;v_!Lad{%N{^qLH{kGm!2S{C5cGaJ8ESbqYEb=n(w*$NHI>(E5~KF|wuzhL z*X4d^{c|$AC+*ur5~He0!~L9!suuq1@?rFbkLD;^JaOy4|9{^DMRsQa8npl->E4uY&^v9ZV%a zGZk%mqtmP3IFl-6^uJ>#M|eEQleC2Z?n&opNS6vo5VLC?POHt)Nl8W1(@27@h)+w$ zO!_BP%v1Y-G;GDV2@;zGFK=M}V@SCQd}3i$^h(o=R$Q(9`)+a%LswZ@S!-+BS>oDu zxe-GW#kmIk)DRgl^^x!4oLVK=_pSod?+O=`_%SPl+5T_@Mr>nT$ zJIE9xemjKNACECrewT125fL`)>v`Z!7H4N@76TUB)n+d>Q;;*PUerl zg=+Y>2CZwpkZ$CKBz9)$Kd8Vq-tvvs=gpax(aC)BXOT4ZX92?$vA7Y22L04mYI4PR zjQ4b>kZ^X+gyK8iq_bTSNgYdIxHgSY!lqk(_Vbh|A-&akJQgHz~>~=lLYx3c9#V+RUKeAt4nu(+m+%rF3~1-7MKn# zQ(J$iwB`q@Eo;q;cg9MYFU{M*c~DYPZx=F}maamfIyA95x^X*h%bOFYPUs!mSTXF| z?aJo)l&LCxzTao6y)(%UM)kU^_M>Sx_}k5GC2s7?*V5AMV%xVA{0u!X?WBR8o6T2+ zbrT^!>wZ6}arW(3v(d=}(rd@}d%6oLU9KNXXR*I#XVWq>NpA(M#!L!-Z;8W1<+`Q3 zOwqm6&us|3#Zu_O+5P1=YPVM+MZCj)8aU6XFK{bs5pNk=XXIW<5F8-A8`7io^1IDv zGYgP41MFWAqj&R1g*B&UGWtJ$JUAD=RZjF>lg%g?WW3R6>zgwHidvGZX`5p3EM*~& z(@mSZ+wA`UX|4nVXt$%3|G8JXuYjBP<3L1C~_qz#GC3_mEL9+-cK+okFl|#Yq z48B5Wz{a4H0&kIeYy@mBJdEHolreuHf=Ky_*HQJ+HWGMVsqI~0RX6SSboty$^@7@% z@}u%7#Mk4};tuICWBd_N4HNflgRA5pc;CsjV7q3mmh-2pO6#>7gX;~HA(d%8hhITv z4+U8JbY?j_&ep9ALrMLRq8Nu?WU}Sg@w9cWK+{# zWMM)40cP@-1B(zf*m(m34WB{f0?(@Ts!hYm(!oODdjubX!K#U`tT$~mRlJWBY~9`2 z)O6hNw74S^R8nh^C!IUJJwV@|V(l2tK<58HI;5bWjI;^MQ07OJ-E~xiTIZv;n9dWk zW&8nu$+@9hruc+cH1Gh-b~YG=ie8X@gel+mQ6b4 zI!|?nSC6=8+oOt8?ovBX~qvbm#2dO-JRatq?%Mi|@?BMml z4gw8jHV#x$$i}ib>nnq%9Uup;jX&rsYe!wC*bDdg=I5LrrmO;r|iL|Lo^cn7M z&Ge+qgrM!jDA+-$B%aBx-|WsGobilrXh*rzzNq&qSz`Qi^HEXDE;3{2OGE+{ zzEA_xbO5|_b`{|p@!okoXK-U9@Qr~gsjlxZSd$VwIsr9<1j%|39{xEGJE!kfd~+)p zi;6od0C~E# z&+6$Hy*mut_A_agd8pZd__TL62EK`umD4P0wC1od#97gJkdT_t4Fq@e?}-)XQk_<# zA08h}U~JsgrrUCAnD@$8Xvm%@s$(tNvZ6_6M(vmJPM z&lE+3U2RHDx#&K|fUluRED^*iME)<_ zm$~QRCvQdO{(L&B@8$J_FuGhRP$blvLZCd3f1K+FUcGv?K?8+-_D`R|Vgh^r@6fNJ z_C@%8p|OUbS5y=CynFX~^bLaRcOmeze_>8o0>wy%R-5Al{CbFnm=L4kgJ5tf@%F_k zePdt&R2mK;Tff(70XsM`%d(Id^<@5nTj{0=4wntohP$FZ;-S7)L%7u1>^%!DvaT+;5bp}@ws&S^rgwiL-pL7F7r z`9Z)VCMOXQF8lpz_RSA*q_xHg()Q{wXy5@aFopv|A9!$^qr+K2Y>RtI5GE+ZF>rm^ z9Cg}|!lMhG4COWhwO{ZY#mbG&Y3&tq=D;y2DbT52%7I;EWRz#|T$L{cmAO0s(v2;@ z%FK5Z!p)@D$J&CW;9VdhaU3}NWK%@PdrI!>z1zPh_xKz*#LX0C8?z{~A%Mw2t|cA% z2F4SS0WXHjEzm>+~ zE*SF_U%$qY4_dnO{0s6wagMXD&rG@F7nP+$mqCYeFaJo2A|2#F7w}HEzYOUxs-lXhU>XAvVtnXDK^@~?C zMm~+kosMp`!%`xqkOrs*#pg}g0tDq`JvNJcaDef(%SvxFbJ|lqpk%_mu}lN^du?s$ zbM?h9_dnC=LI?S3N9MtWj+vyoI{)~t&NvUC3_dk(OglRGc+mJ3B3w_8!B1MIFBXOJ zMfM_W%wiE1mRPGx^4{AUt3()yIjGFCG7dE|+H$l9~Y-&F9%rZ*u<0G5@Yl>Pf z#kCYxF&5}CgZjN*g+?W*Jd6w+%bdFEY^=E7H7*DgW{GxadU!5C~-QpTF#x9=Ge~%~sMWRhe>n zY|4V1j5u{&o&OF10l`4L9E#*klYi%+@A`>P1eoalGixRnETrs3*%e-T-z~^hTO87v zexxXdphWb((9N%Y=?D&nmTN$WV<0Lc z`-QGLo3>d5#viRkb8bz*E?fW}*=g@daM4aCOV?XbBDUL~k+AA+di1Mx2bRiqHVy5; zD1tmO+EikF44hHu-d5q|;V`#{5*fDOi(tCo{(0KvB3m$f8;o@amwJt4`@xN>7+U+_ z@Y}a<7r!=wc1q>hrR=2$(AbVvY@9UBbTRC?ZZ~3^sL4*(O_zV3qu$x!ndikhVg(LR z7)Y{P=3{45_Nn8bw3&X-?BoHdb;K{40sfGWPyL#&4HEiGlAkGRIgWDQ?v>}opC0)D zqP7o&?tp8UnJ~@pN*9pSEJXVbAJ$|EOz#5C-p<-OwH6KDH(rlze2s?TZG3oG6tLMt zc$VisIuBn@$2_Tve868bf>%rus-$MED!gvY@@O3CUkn5L+v)qrMY2Fy7vD1N!%>ko z19Jq1!>Wi;5)Z=?HS8Om%)0%0fxP4*!2Ux|P<>pWCLN_YH>rot1216GO6o@p|D^Yg zH-g{Qps6+hvklUl(ll zGHul_j`Z;{bSK%{X=5esG-$yM@cFLt@q9L6zLMHmHa|92=YYI!2`UpJpw=6JQkOJs z>~zFRh8oG9IP5jig5AoXH1XkXJP!H0Jc(`e?W#8~mG8RP)X-NNa;@g_quxXf(ntBU zxtYr#E&KQhXWRo-w!85=Ah^kbcMaA_8PsXQ#k39Ny5ibmEQ+d~^29JX>r~6%DbAn+ z+0Du*JlS0Idm?CfpPCDg|mQJ_*j+KYbb1Jy?8=4_tAVl9B@7-ZT+a zI*7ckpHYjR;UbAspfdNx$WL#d7OM|SoFj)-SD#_Exgo@o>QS9#mU4Gxm*eL1MWW#N zHQh0P3|=sz#p!{^Sh74JG4uWsw(C&$&KtB-wR12MwbDB!GE$;`B$#j)ta0DmgeU(k zJXSE_5a)lQz+eM4tTOX!h#a%t@GLHlp&{W7uHl4Mx$g*t@WNlG+04eKx2-JNxTbciaT>v1gk4}iGJFA*`L7XmwZN8QB_t(_ zii)mAFnZ<8E?(gpZHl=jff6(J^qgyNf23ROiZKH|mS*6}4+ua5eSc`5>D3|?iBZ>M z5{wtWpI!HV7-Kqaxpjq^k((Q#NnJTU8fShMM1f>gX4}DYtlZ)B z{M`-^b+8VkZ1m-%)^qx}CB4Krdw6rEvST-7*`oio8SZj67kDg8ZQ&IrPGSE8*#+@% literal 0 HcmV?d00001 diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml new file mode 100644 index 0000000000000..7e9599c49bc2d --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -0,0 +1,36 @@ + + + + behavior_velocity_dynamic_obstacle_stop_module + 0.1.0 + dynamic_obstacle_stop module to stop ego when in the immediate path of a dynamic object + + Maxime Clement + Mamoru Sobue + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml new file mode 100644 index 0000000000000..2f5662c7998ac --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp new file mode 100644 index 0000000000000..46c58d6be8805 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "collision.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +std::optional find_earliest_collision( + const EgoData & ego_data, + const std::vector & objects, + const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints, DebugData & debug_data) +{ + auto earliest_idx = ego_data.path_footprints.size(); + auto earliest_arc_length = motion_utils::calcArcLength(ego_data.path.points); + std::optional earliest_collision_point; + debug_data.collisions.clear(); + std::vector rough_collisions; + for (auto obstacle_idx = 0UL; obstacle_idx < objects.size(); ++obstacle_idx) { + rough_collisions.clear(); + const auto & obstacle_pose = objects[obstacle_idx].kinematics.initial_pose_with_covariance.pose; + const auto & obstacle_footprint = obstacle_forward_footprints[obstacle_idx]; + ego_data.rtree.query( + boost::geometry::index::intersects(obstacle_footprint), std::back_inserter(rough_collisions)); + for (const auto & rough_collision : rough_collisions) { + const auto path_idx = rough_collision.second; + const auto & ego_footprint = ego_data.path_footprints[path_idx]; + const auto & ego_pose = ego_data.path.points[ego_data.first_path_idx + path_idx].point.pose; + const auto angle_diff = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(ego_pose.orientation) - tf2::getYaw(obstacle_pose.orientation)); + if (path_idx <= earliest_idx && std::abs(angle_diff) > (M_PI_2 + M_PI_4)) { + tier4_autoware_utils::MultiPoint2d collision_points; + boost::geometry::intersection( + obstacle_footprint.outer(), ego_footprint.outer(), collision_points); + earliest_idx = path_idx; + for (const auto & coll_p : collision_points) { + auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y()); + const auto arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, ego_data.first_path_idx, p); + if (arc_length < earliest_arc_length) { + earliest_arc_length = arc_length; + debug_data.collisions = {obstacle_footprint, ego_footprint}; + earliest_collision_point = p; + } + } + } + } + } + return earliest_collision_point; +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp new file mode 100644 index 0000000000000..ccc3fa94df603 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -0,0 +1,39 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 COLLISION_HPP_ +#define COLLISION_HPP_ + +#include "types.hpp" + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief find the earliest collision along the ego path and an obstacle footprint +/// @param [in] ego_data ego data including its path and footprint +/// @param [in] objects obstacles +/// @param [in] obstacle_forward_footprints obstacle footprints +/// @param [in] debug_data debug data +/// @return the point of earliest collision along the ego path +std::optional find_earliest_collision( + const EgoData & ego_data, + const std::vector & objects, + const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints, DebugData & debug_data); + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // COLLISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp new file mode 100644 index 0000000000000..cc5dafa218654 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -0,0 +1,87 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "debug.hpp" + +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +{ + +std::vector make_delete_markers( + const size_t from, const size_t to, const std::string & ns) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETE; + marker.ns = ns; + for (marker.id = static_cast(from); marker.id < static_cast(to); ++marker.id) + markers.push_back(marker); + return markers; +} + +std::vector make_dynamic_obstacle_markers( + const std::vector & obstacles) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = "dynamic_obstacles"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0); + marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 1.0); + marker.text = "dynamic obstacle"; + for (const auto & obstacle : obstacles) { + marker.pose = obstacle.kinematics.initial_pose_with_covariance.pose; + markers.push_back(marker); + ++marker.id; + } + return markers; +} + +std::vector make_polygon_markers( + const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = ns; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); + marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.8); + for (const auto & footprint : footprints) { + marker.points.clear(); + for (const auto & p : footprint.outer()) { + marker.points.emplace_back(); + marker.points.back().x = p.x(); + marker.points.back().y = p.y(); + marker.points.back().z = z; + } + markers.push_back(marker); + ++marker.id; + } + return markers; +} +} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp new file mode 100644 index 0000000000000..02f550d314d39 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -0,0 +1,40 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 DEBUG_HPP_ +#define DEBUG_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +{ +std::vector make_delete_markers( + const size_t from, const size_t to, const std::string & ns); +std::vector make_dynamic_obstacle_markers( + const std::vector & obstacles); +std::vector make_polygon_markers( + const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); +std::vector make_collision_markers( + const tier4_autoware_utils::MultiPoint2d & collisions, const double z); +} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug + +#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp new file mode 100644 index 0000000000000..6a0c61963ac81 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -0,0 +1,84 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "footprint.hpp" + +#include + +#include + +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +tier4_autoware_utils::MultiPolygon2d make_forward_footprints( + const std::vector & obstacles, + const PlannerParam & params, const double hysteresis) +{ + tier4_autoware_utils::MultiPolygon2d forward_footprints; + for (const auto & obstacle : obstacles) + forward_footprints.push_back(project_to_pose( + make_forward_footprint(obstacle, params, hysteresis), + obstacle.kinematics.initial_pose_with_covariance.pose)); + return forward_footprints; +} + +tier4_autoware_utils::Polygon2d make_forward_footprint( + const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const double hysteresis) +{ + const auto & shape = obstacle.shape.dimensions; + const auto longitudinal_offset = + shape.x / 2.0 + + obstacle.kinematics.initial_twist_with_covariance.twist.linear.x * params.time_horizon; + const auto lateral_offset = (shape.y + params.extra_object_width) / 2.0 + hysteresis; + return tier4_autoware_utils::Polygon2d{ + {{longitudinal_offset, -lateral_offset}, + {longitudinal_offset, lateral_offset}, + {shape.x / 2.0, lateral_offset}, + {shape.x / 2.0, -lateral_offset}, + {longitudinal_offset, -lateral_offset}}, + {}}; +} + +tier4_autoware_utils::Polygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +{ + const auto angle = tf2::getYaw(pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + tier4_autoware_utils::Polygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.outer().emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); + return footprint; +} + +void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) +{ + for (const auto & p : ego_data.path.points) + ego_data.path_footprints.push_back(tier4_autoware_utils::toFootprint( + p.point.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); + for (auto i = 0UL; i < ego_data.path_footprints.size(); ++i) { + const auto box = + boost::geometry::return_envelope(ego_data.path_footprints[i]); + ego_data.rtree.insert(std::make_pair(box, i)); + } +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp new file mode 100644 index 0000000000000..c28e89ac6c9f6 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +/// @brief create the footprint of the given obstacles and their projection over a fixed time +/// horizon +/// @param [in] obstacles obstacles +/// @param [in] params parameters used to create the footprint +/// @param [in] hysteresis [m] extra lateral distance to add to the footprints +/// @return forward footprint of the obstacle +tier4_autoware_utils::MultiPolygon2d make_forward_footprints( + const std::vector & obstacles, + const PlannerParam & params, const double hysteresis); +/// @brief create the footprint of the given obstacle and its projection over a fixed time horizon +/// @param [in] obstacle obstacle +/// @param [in] params parameters used to create the footprint +/// @param [in] hysteresis [m] extra lateral distance to add to the footprint +/// @return forward footprint of the obstacle +tier4_autoware_utils::Polygon2d make_forward_footprint( + const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const double hysteresis); +/// @brief project a footprint to the given pose +/// @param [in] base_footprint footprint to project +/// @param [in] pose projection pose +/// @return footprint projected to the given pose +tier4_autoware_utils::Polygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); +/// @brief create the rtree indexing the ego footprint along the path +/// @param [inout] ego_data ego data with its path and the rtree to populate +/// @param [in] params parameters +void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params); +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp new file mode 100644 index 0000000000000..99981007b20c2 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -0,0 +1,73 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "manager.hpp" + +#include "scene_dynamic_obstacle_stop.hpp" + +#include + +#include + +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::getOrDeclareParameter; + +DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) +{ + const std::string ns(getModuleName()); + auto & pp = planner_param_; + + pp.extra_object_width = getOrDeclareParameter(node, ns + ".extra_object_width"); + pp.minimum_object_velocity = getOrDeclareParameter(node, ns + ".minimum_object_velocity"); + pp.stop_distance_buffer = getOrDeclareParameter(node, ns + ".stop_distance_buffer"); + pp.time_horizon = getOrDeclareParameter(node, ns + ".time_horizon"); + pp.hysteresis = getOrDeclareParameter(node, ns + ".hysteresis"); + pp.decision_duration_buffer = + getOrDeclareParameter(node, ns + ".decision_duration_buffer"); + pp.minimum_object_distance_from_ego_path = + getOrDeclareParameter(node, ns + ".minimum_object_distance_from_ego_path"); + + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + pp.ego_lateral_offset = + std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); + pp.ego_longitudinal_offset = vehicle_info.max_longitudinal_offset_m; +} + +void DynamicObstacleStopModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + if (path.points.empty()) return; + // general + if (!isModuleRegistered(module_id_)) { + registerModule(std::make_shared( + module_id_, planner_param_, logger_.get_child("dynamic_obstacle_stop_module"), clock_)); + } +} + +std::function &)> +DynamicObstacleStopModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { + return false; + }; +} +} // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::DynamicObstacleStopModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp new file mode 100644 index 0000000000000..e461cc9c16445 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene_dynamic_obstacle_stop.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface +{ +public: + explicit DynamicObstacleStopModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "dynamic_obstacle_stop"; } + +private: + using PlannerParam = dynamic_obstacle_stop::PlannerParam; + + PlannerParam planner_param_; + int64_t module_id_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class DynamicObstacleStopModulePlugin : public PluginWrapper +{ +}; + +} // namespace behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp new file mode 100644 index 0000000000000..0411ab2d01cfe --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "object_filtering.hpp" + +#include "types.hpp" + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief filter the given predicted objects +/// @param objects predicted objects +/// @param ego_data ego data, including its path and pose +/// @param params parameters +/// @param hysteresis [m] extra distance threshold used for filtering +/// @return filtered predicted objects +std::vector filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params, const double hysteresis) +{ + std::vector filtered_objects; + const auto is_vehicle = [](const auto & o) { + return std::find_if(o.classification.begin(), o.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + c.label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + }) != o.classification.end(); + }; + const auto is_in_range = [&](const auto & o) { + const auto distance = std::abs(motion_utils::calcLateralOffset( + ego_data.path.points, o.kinematics.initial_pose_with_covariance.pose.position)); + return distance <= params.minimum_object_distance_from_ego_path + params.ego_lateral_offset + + o.shape.dimensions.y / 2.0 + hysteresis; + }; + const auto is_not_too_close = [&](const auto & o) { + const auto obj_arc_length = motion_utils::calcSignedArcLength( + ego_data.path.points, ego_data.pose.position, + o.kinematics.initial_pose_with_covariance.pose.position); + return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx + + params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0; + }; + for (const auto & object : objects.objects) + if ( + is_vehicle(object) && + object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity && + is_in_range(object) && is_not_too_close(object)) + filtered_objects.push_back(object); + return filtered_objects; +} +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp new file mode 100644 index 0000000000000..22857f6db1bda --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -0,0 +1,39 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 OBJECT_FILTERING_HPP_ +#define OBJECT_FILTERING_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief filter the given predicted objects +/// @param objects predicted objects +/// @param ego_data ego data, including its path and pose +/// @param params parameters +/// @param hysteresis [m] extra distance threshold used for filtering +/// @return filtered predicted objects +std::vector filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params, const double hysteresis); + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // OBJECT_FILTERING_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp new file mode 100644 index 0000000000000..98b7984bab1dc --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp @@ -0,0 +1,185 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// 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 "scene_dynamic_obstacle_stop.hpp" + +#include "collision.hpp" +#include "debug.hpp" +#include "footprint.hpp" +#include "object_filtering.hpp" +#include "types.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +DynamicObstacleStopModule::DynamicObstacleStopModule( + const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) +{ + prev_stop_decision_time_ = rclcpp::Time(int64_t{0}, clock->get_clock_type()); + velocity_factor_.init(PlanningBehavior::UNKNOWN); +} + +bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_.reset_data(); + *stop_reason = planning_utils::initializeStopReason(StopReason::OBSTACLE_STOP); + if (!path || path->points.size() < 2) return true; + tier4_autoware_utils::StopWatch stopwatch; + stopwatch.tic(); + + stopwatch.tic("preprocessing"); + EgoData ego_data; + ego_data.pose = planner_data_->current_odometry->pose; + ego_data.path.points = path->points; + motion_utils::removeOverlapPoints(ego_data.path.points); + ego_data.first_path_idx = + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); + ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment( + ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position); + + make_ego_footprint_rtree(ego_data, params_); + const auto has_decided_to_stop = + (clock_->now() - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer; + if (!has_decided_to_stop) current_stop_pose_.reset(); + double hysteresis = has_decided_to_stop ? params_.hysteresis : 0.0; + const auto dynamic_obstacles = + filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_, hysteresis); + + const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); + + stopwatch.tic("footprints"); + const auto obstacle_forward_footprints = + make_forward_footprints(dynamic_obstacles, params_, hysteresis); + const auto footprints_duration_us = stopwatch.toc("footprints"); + const auto min_stop_distance = + motion_utils::calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, + planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, + planner_data_->max_stop_jerk_threshold) + .value_or(0.0); + stopwatch.tic("collisions"); + const auto collision = + find_earliest_collision(ego_data, dynamic_obstacles, obstacle_forward_footprints, debug_data_); + const auto collisions_duration_us = stopwatch.toc("collisions"); + if (collision) { + const auto arc_length_diff = + motion_utils::calcSignedArcLength(ego_data.path.points, *collision, ego_data.pose.position); + const auto can_stop_before_limit = arc_length_diff < min_stop_distance - + params_.ego_longitudinal_offset - + params_.stop_distance_buffer; + const auto stop_pose = can_stop_before_limit + ? motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, *collision, + -params_.stop_distance_buffer - params_.ego_longitudinal_offset) + : motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, ego_data.pose.position, min_stop_distance); + if (stop_pose) { + const auto use_new_stop_pose = !has_decided_to_stop || motion_utils::calcSignedArcLength( + path->points, stop_pose->position, + current_stop_pose_->position) > 0.0; + if (use_new_stop_pose) current_stop_pose_ = *stop_pose; + prev_stop_decision_time_ = clock_->now(); + } + } + + if (current_stop_pose_) motion_utils::insertStopPoint(*current_stop_pose_, 0.0, path->points); + + const auto total_time_us = stopwatch.toc(); + RCLCPP_DEBUG( + logger_, + "Total time = %2.2fus\n\tpreprocessing = %2.2fus\n\tfootprints = " + "%2.2fus\n\tcollisions = %2.2fus\n", + total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us); + debug_data_.ego_footprints = ego_data.path_footprints; + debug_data_.obstacle_footprints = obstacle_forward_footprints; + return true; +} + +MarkerArray DynamicObstacleStopModule::createDebugMarkerArray() +{ + constexpr auto z = 0.0; + MarkerArray debug_marker_array; + // dynamic obstacles footprints + const auto obstacle_footprint_markers = + debug::make_polygon_markers(debug_data_.obstacle_footprints, "dynamic_obstacles_footprints", z); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), obstacle_footprint_markers.begin(), + obstacle_footprint_markers.end()); + const auto delete_footprint_markers = debug::make_delete_markers( + obstacle_footprint_markers.size(), debug_data_.prev_dynamic_obstacles_nb, + "dynamic_obstacles_footprints"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_footprint_markers.begin(), + delete_footprint_markers.end()); + // ego path footprints + const auto ego_footprint_markers = + debug::make_polygon_markers(debug_data_.ego_footprints, "ego_footprints", z); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), ego_footprint_markers.begin(), ego_footprint_markers.end()); + const auto delete_ego_footprint_markers = debug::make_delete_markers( + ego_footprint_markers.size(), debug_data_.prev_ego_footprints_nb, "ego_footprints"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_ego_footprint_markers.begin(), + delete_ego_footprint_markers.end()); + // collisions + auto collision_markers = debug::make_polygon_markers(debug_data_.collisions, "collisions", z); + for (auto & m : collision_markers) m.color.r = 1.0; + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), collision_markers.begin(), collision_markers.end()); + const auto delete_collision_markers = debug::make_delete_markers( + collision_markers.size(), debug_data_.prev_collisions_nb, "collisions"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_collision_markers.begin(), + delete_collision_markers.end()); + + debug_data_.prev_dynamic_obstacles_nb = obstacle_footprint_markers.size(); + debug_data_.prev_collisions_nb = collision_markers.size(); + debug_data_.prev_ego_footprints_nb = ego_footprint_markers.size(); + return debug_marker_array; +} + +motion_utils::VirtualWalls DynamicObstacleStopModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls{}; + if (current_stop_pose_) { + motion_utils::VirtualWall virtual_wall; + virtual_wall.text = "dynamic_obstacle_stop"; + virtual_wall.longitudinal_offset = params_.ego_longitudinal_offset; + virtual_wall.style = motion_utils::VirtualWallType::stop; + virtual_wall.pose = *current_stop_pose_; + virtual_walls.push_back(virtual_wall); + } + return virtual_walls; +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp new file mode 100644 index 0000000000000..c7bca149890a0 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp @@ -0,0 +1,62 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ +#define SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ + +#include "types.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +class DynamicObstacleStopModule : public SceneModuleInterface +{ +public: + DynamicObstacleStopModule( + const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock); + + /// @brief insert stop or slow down points to prevent dangerously entering another lane + /// @param [inout] path the path to update + /// @param [inout] stop_reason reason for stopping + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + motion_utils::VirtualWalls createVirtualWalls() override; + +private: + PlannerParam params_; + rclcpp::Time prev_stop_decision_time_; + std::optional current_stop_pose_; + +protected: + int64_t module_id_{}; + + // Debug + mutable DebugData debug_data_; +}; +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp new file mode 100644 index 0000000000000..74fd5ca818fb0 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -0,0 +1,80 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 TYPES_HPP_ +#define TYPES_HPP_ + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +typedef std::pair BoxIndexPair; +typedef boost::geometry::index::rtree> Rtree; + +/// @brief parameters for the "out of lane" module +struct PlannerParam +{ + double extra_object_width; + double minimum_object_velocity; + double stop_distance_buffer; + double time_horizon; + double hysteresis; + double decision_duration_buffer; + double ego_longitudinal_offset; + double ego_lateral_offset; + double minimum_object_distance_from_ego_path; +}; + +struct EgoData +{ + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + size_t first_path_idx{}; + double longitudinal_offset_to_first_path_idx; // [m] + geometry_msgs::msg::Pose pose; + tier4_autoware_utils::MultiPolygon2d path_footprints; + Rtree rtree; +}; + +/// @brief debug data +struct DebugData +{ + tier4_autoware_utils::MultiPolygon2d obstacle_footprints{}; + size_t prev_dynamic_obstacles_nb{}; + tier4_autoware_utils::MultiPolygon2d collisions{}; + size_t prev_collisions_nb{}; + tier4_autoware_utils::MultiPolygon2d ego_footprints{}; + size_t prev_ego_footprints_nb{}; + std::optional stop_pose{}; + void reset_data() + { + obstacle_footprints.clear(); + collisions.clear(); + ego_footprints.clear(); + stop_pose.reset(); + } +}; + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 22220ccd182a1..c1631fdb739de 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -26,6 +26,7 @@ + @@ -69,6 +70,7 @@ + From 27d866807e97daa7e3cbc6bd2854614913d5e591 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 10 Jan 2024 13:25:30 +0900 Subject: [PATCH 017/154] fix(map_loader): show traffic light regulatory element id per lanelet (#6028) * fix(map_loader): show traffic light regulatory element id per lanelet Signed-off-by: satoshi-ota * feat(map_loader): show traffic light id Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../lanelet2_map_loader/lanelet2_map_visualization_node.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index e14defcb47f26..a0c57759d51a6 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -215,6 +215,12 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + road_lanelets, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + crosswalk_lanelets, cl_trafficlights)); insertMarkerArray( &map_marker_array, lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); From a57eb0f016cb41b6c7c424c5292a0be132f9331d Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 10 Jan 2024 14:10:43 +0900 Subject: [PATCH 018/154] fix(start_planner): don't update start pose when backward driving is finished (#5998) Fix conditional check in updatePullOutStatus function Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 9870a324caf64..66e04d7e9bb7a 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -839,8 +839,10 @@ void StartPlannerModule::updatePullOutStatus() return {*refined_start_pose}; }); - planWithPriority( - start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); + if (!status_.backward_driving_complete) { + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); + } start_planner_data_.refined_start_pose = *refined_start_pose; start_planner_data_.start_pose_candidates = start_pose_candidates; From 142bc49b57f6ca002c99efa6406b22ce9012448d Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 10 Jan 2024 15:16:13 +0900 Subject: [PATCH 019/154] docs(start_planner): update Purpose / Role of the document (#6002) Update Start Planner module to stop in response to dynamic obstacles Signed-off-by: kyoichi-sugahara --- .../README.md | 20 +++++++---- .../images/start_from_road_lane.drawio.svg | 34 ------------------- 2 files changed, 13 insertions(+), 41 deletions(-) delete mode 100644 planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index f4aa3936bee56..2f90cc2ee3cf2 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -2,17 +2,23 @@ ## Purpose / Role -The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and implementing safety checks against dynamic obstacles. (Note: The feature of safety checks against dynamic obstacles is currently a work in progress.) -This module is activated when a new route is received. +The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and stopping in response to dynamic obstacles when a collision is detected. -Use cases are as follows +Use cases include: -- start smoothly from the current ego position to centerline. - ![case1](./images/start_from_road_lane.drawio.svg) - pull out from the side of the road lane to centerline. - ![case2](./images/start_from_road_side.drawio.svg) + +
+ ![case1](images/start_from_road_side.drawio.svg){width=1000} +
pull out from side of the road lane
+
+ - pull out from the shoulder lane to the road lane centerline. - ![case3](./images/start_from_road_shoulder.drawio.svg) + +
+ ![case2](images/start_from_start_from_road_shoulder.drawio.svg){width=1000} +
pull out from the shoulder lane
+
## Design diff --git a/planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg deleted file mode 100644 index 7a89cc2183acf..0000000000000 --- a/planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - From b20c2bf64d6554393e132f1dbca345013a50a924 Mon Sep 17 00:00:00 2001 From: Zhe Shen <80969277+HansOersted@users.noreply.github.com> Date: Wed, 10 Jan 2024 21:23:04 +0900 Subject: [PATCH 020/154] refactor(motion_utils): clear the repeat definitions and correct the dependencies (#5909) * change .hpp name Signed-off-by: Zhe Shen * change .cpp name Signed-off-by: Zhe Shen * correct the #inlcude and #ifndef Signed-off-by: Zhe Shen * toPath(): move the prototypes and the implementations in .hpp and .cpp Signed-off-by: Zhe Shen * correct the dependency for /home/shen/autoware.universe/planning/behavior_path_planner/src/behavior_path_planner_node.cpp Signed-off-by: Zhe Shen * toPath(): deleted the repeat definition in planning_interface_test_manager_utils.hpp Signed-off-by: Zhe Shen * toPath(): corrected the call and dependency in planning_interface_test_manager.cpp Signed-off-by: Zhe Shen * convertPathToTrajectoryPoints(), convertTrajectoryPointsToPath(), lerpOrientation(): moved to conversion.hpp and conversion.cpp Signed-off-by: Zhe Shen * convertPathToTrajectoryPoints(): corrected the call and dependency Signed-off-by: Zhe Shen * convertTrajectoryPointsToPath(): Corrected the call Signed-off-by: Zhe Shen * lerpOrientation(): deleted the repeat definition in longitudinal_controller_utils.hpp and longitudinal_controller_utils.cpp Signed-off-by: Zhe Shen * lerpOrientation(): Correct the call in longitudinal_controller_utils.hpp Signed-off-by: Zhe Shen * lerpOrientation(): Corrected the dependency and call in test_longitudinal_controller_utils.cpp Signed-off-by: Zhe Shen * name of conversion.cpp: modified the CMakeLists Signed-off-by: Zhe Shen * namespace updated, but maybe not correct, will be tested in the next commit Signed-off-by: Zhe Shen * Correct the dependencies the test_trajectory.cpp and test_interpolation.cpp Signed-off-by: Zhe Shen * style(pre-commit): autofix * [namespace problems fixed] conversion.cpp & conversion.hpp: The dependencies have been added. Also, the namespaces have been corrected. Signed-off-by: Zhe Shen * style(pre-commit): autofix * correct all the dependencies in #include Signed-off-by: Zhe Shen * style(pre-commit): autofix * avoid using the ../../ in the paths Signed-off-by: Zhe Shen * dependencies fixed and package added in .xml Signed-off-by: Zhe Shen * style(pre-commit): autofix * change toPath() to convertToPath() Signed-off-by: Zhe Shen * lerpOrientation(): move from conversion to spherical_linear_interpolation Signed-off-by: Zhe Shen * style(pre-commit): autofix * fix the missing relative path definition. ../../ will not be used. Signed-off-by: Zhe Shen * the unneccessary dependencies eleminated. In specific, the lerpOrientation() related dependencies in coversion.hpp and unneccessary dependencies in conversion.cpp are deleted. Signed-off-by: Zhe Shen * change the function to template in .cpp and .hpp Signed-off-by: Zhe Shen * style(pre-commit): autofix * change the corresponding calls to template Signed-off-by: Zhe Shen * style(pre-commit): autofix * change name to convertToTrajectory() Signed-off-by: Zhe Shen * style(pre-commit): autofix * change the template of convertToPathWithLaneId() Signed-off-by: Zhe Shen * fix the dependencies Signed-off-by: Zhe Shen * refactor(motion_utils): specialize class Signed-off-by: satoshi-ota * fix(motion_utils): remove unnecessary header Signed-off-by: satoshi-ota --------- Signed-off-by: Zhe Shen Signed-off-by: satoshi-ota Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota --- .../spherical_linear_interpolation.hpp | 4 ++ .../src/spherical_linear_interpolation.cpp | 11 +++ .../motion_utils/trajectory/conversion.hpp | 71 +++++++++++++++++++ .../longitudinal_controller_utils.hpp | 12 +--- .../src/longitudinal_controller_utils.cpp | 10 --- .../test_longitudinal_controller_utils.cpp | 12 ++-- .../src/behavior_path_planner_node.cpp | 4 +- .../utils/path_utils.hpp | 2 - .../src/utils/path_utils.cpp | 13 ---- .../utilization/trajectory_utils.hpp | 5 -- .../src/utilization/trajectory_utils.cpp | 43 ++--------- .../planning_interface_test_manager_utils.hpp | 13 ---- planning/planning_test_utils/package.xml | 1 + .../src/planning_interface_test_manager.cpp | 6 +- 14 files changed, 111 insertions(+), 96 deletions(-) diff --git a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp index 516b3ab09e8b7..8c1d49fb5f607 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp @@ -39,6 +39,10 @@ std::vector slerp( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); + +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio); } // namespace interpolation #endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/interpolation/src/spherical_linear_interpolation.cpp index c3595d212f349..e44626498a80b 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/interpolation/src/spherical_linear_interpolation.cpp @@ -57,4 +57,15 @@ std::vector slerp( return query_values; } +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} } // namespace interpolation diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp index 6e77c1ec4a186..7d4be216e89fe 100644 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -15,6 +15,8 @@ #ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "std_msgs/msg/header.hpp" @@ -23,6 +25,8 @@ namespace motion_utils { +using TrajectoryPoints = std::vector; + /** * @brief Convert std::vector to * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to @@ -45,6 +49,73 @@ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( std::vector convertToTrajectoryPointArray( const autoware_auto_planning_msgs::msg::Trajectory & trajectory); +template +autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); + throw std::logic_error("Only specializations of convertToPath can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::Path convertToPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + autoware_auto_planning_msgs::msg::Path output{}; + output.header = input.header; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + output.points.resize(input.points.size()); + for (size_t i = 0; i < input.points.size(); ++i) { + output.points.at(i) = input.points.at(i).point; + } + return output; +} + +template +TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used."); + throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used."); +} + +template <> +inline TrajectoryPoints convertToTrajectoryPoints( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + TrajectoryPoints output{}; + for (const auto & p : input.points) { + autoware_auto_planning_msgs::msg::TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + output.emplace_back(tp); + } + return output; +} + +template +autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + [[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); + throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + const TrajectoryPoints & input) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId output{}; + for (const auto & p : input) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + output.points.emplace_back(pp); + } + return output; +} + } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 9c01f7bc26c4b..cc1c9c08887ba 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -16,6 +16,8 @@ #define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #include "interpolation/linear_interpolation.hpp" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" @@ -74,14 +76,6 @@ Pose calcPoseAfterTimeDelay( const Pose & current_pose, const double delay_time, const double current_vel, const double current_acc); -/** - * @brief apply linear interpolation to orientation - * @param [in] o_from first orientation - * @param [in] o_to second orientation - * @param [in] ratio ratio between o_from and o_to for interpolation - */ -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); - /** * @brief apply linear interpolation to trajectory point that is nearest to a certain point * @param [in] points trajectory points @@ -107,7 +101,7 @@ TrajectoryPoint lerpTrajectoryPoint( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); interpolated_point.pose.position.y = interpolation::lerp( points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio); - interpolated_point.pose.orientation = lerpOrientation( + interpolated_point.pose.orientation = interpolation::lerpOrientation( points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); interpolated_point.longitudinal_velocity_mps = interpolation::lerp( points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 9791a1f0b5e3e..33a54663ccaef 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -141,16 +141,6 @@ double lerp(const double v_from, const double v_to, const double ratio) return v_from + (v_to - v_from) * ratio; } -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val) diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 5c7698180f82b..58aa867b2deab 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "gtest/gtest.h" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" @@ -303,7 +305,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) o_to.setRPY(M_PI_4, M_PI_4, M_PI_4); ratio = 0.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -311,7 +313,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); ratio = 1.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4); @@ -320,7 +322,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) ratio = 0.5; o_to.setRPY(M_PI_4, 0.0, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2); @@ -328,7 +330,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, M_PI_4, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -336,7 +338,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, 0.0, M_PI_4); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 12715520e1146..9b3fbedc37203 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include #include @@ -700,7 +701,8 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = utils::toPath(*path_candidate_ptr); + output = motion_utils::convertToPath( + *path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp index 4ac6f01da65fb..f109bb2cbb213 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp @@ -58,8 +58,6 @@ PathWithLaneId resamplePathWithSpline( const PathWithLaneId & path, const double interval, const bool keep_input_points = false, const std::pair target_section = {0.0, std::numeric_limits::max()}); -Path toPath(const PathWithLaneId & input); - size_t getIdxByArclength( const PathWithLaneId & path, const size_t target_idx, const double signed_arc); diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp index 2eceb0ffb81c8..1d51426793de8 100644 --- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp @@ -155,19 +155,6 @@ PathWithLaneId resamplePathWithSpline( return motion_utils::resamplePath(path, s_out); } -Path toPath(const PathWithLaneId & input) -{ - Path output{}; - output.header = input.header; - output.left_bound = input.left_bound; - output.right_bound = input.right_bound; - output.points.resize(input.points.size()); - for (size_t i = 0; i < input.points.size(); ++i) { - output.points.at(i) = input.points.at(i).point; - } - return output; -} - size_t getIdxByArclength( const PathWithLaneId & path, const size_t target_idx, const double signed_arc) { diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index c86272cff1c2e..2184be4fbd2fe 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -35,11 +35,6 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path); -PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory); - -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); - //! smooth path point with lane id starts from ego position on path to the path end bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 7fe8612cc6858..314281a954b49 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -13,6 +13,8 @@ // limitations under the License. // #include +#include "motion_utils/trajectory/conversion.hpp" + #include #include #include @@ -41,41 +43,6 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path) -{ - TrajectoryPoints tps; - for (const auto & p : path.points) { - TrajectoryPoint tp; - tp.pose = p.point.pose; - tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; - // since path point doesn't have acc for now - tp.acceleration_mps2 = 0; - tps.emplace_back(tp); - } - return tps; -} -PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory) -{ - PathWithLaneId path; - for (const auto & p : trajectory) { - PathPointWithLaneId pp; - pp.point.pose = p.pose; - pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; - path.points.emplace_back(pp); - } - return path; -} - -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - //! smooth path point with lane id starts from ego position on path to the path end bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, @@ -87,7 +54,9 @@ bool smoothPath( const auto & external_v_limit = planner_data->external_velocity_limit; const auto & smoother = planner_data->velocity_smoother_; - auto trajectory = convertPathToTrajectoryPoints(in_path); + auto trajectory = + motion_utils::convertToTrajectoryPoints( + in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); const auto traj_steering_rate_limited = @@ -117,7 +86,7 @@ bool smoothPath( motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } - out_path = convertTrajectoryPointsToPath(traj_smoothed); + out_path = motion_utils::convertToPathWithLaneId(traj_smoothed); return true; } diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp index 4d8d7be96b44b..371a6316ce993 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp @@ -492,19 +492,6 @@ void updateNodeOptions( node_options.arguments(std::vector{arguments.begin(), arguments.end()}); } -Path toPath(const PathWithLaneId & input) -{ - Path output{}; - output.header = input.header; - output.left_bound = input.left_bound; - output.right_bound = input.right_bound; - output.points.resize(input.points.size()); - for (size_t i = 0; i < input.points.size(); ++i) { - output.points.at(i) = input.points.at(i).point; - } - return output; -} - PathWithLaneId loadPathWithLaneIdInYaml() { const auto planning_test_utils_dir = diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 23bfb2f44cb96..278813818de0d 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -23,6 +23,7 @@ component_interface_utils lanelet2_extension lanelet2_io + motion_utils nav_msgs rclcpp route_handler diff --git a/planning/planning_test_utils/src/planning_interface_test_manager.cpp b/planning/planning_test_utils/src/planning_interface_test_manager.cpp index a9d096bf00fd4..0a6b4246348eb 100644 --- a/planning/planning_test_utils/src/planning_interface_test_manager.cpp +++ b/planning/planning_test_utils/src/planning_interface_test_manager.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "motion_utils/trajectory/conversion.hpp" + #include #include @@ -289,7 +291,9 @@ void PlanningInterfaceTestManager::publishNominalPath( { test_utils::publishToTargetNode( test_node_, target_node, topic_name, normal_path_pub_, - test_utils::toPath(test_utils::loadPathWithLaneIdInYaml()), 5); + motion_utils::convertToPath( + test_utils::loadPathWithLaneIdInYaml()), + 5); } void PlanningInterfaceTestManager::publishAbnormalPath( From 4158eeade350c50208722a4dbe73ec94a26b9987 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 10 Jan 2024 22:57:35 +0900 Subject: [PATCH 021/154] fix(static_drivable_area_expansion): fix bound extraction logic (#6006) * fix(static_drivable_area_expansion): fix bound extraction logic Signed-off-by: satoshi-ota * refactor(static_drivable_area_expansion): define as anon func Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../static_drivable_area.cpp | 66 ++++++++++--------- 1 file changed, 35 insertions(+), 31 deletions(-) diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 1aa55e5f350b3..44c3025a8c5d5 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -148,6 +148,40 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( return offset_point ? offset_point.value() : points.at(nearest_segment_idx + 1); } + +std::vector extractBoundFromPolygon( + const lanelet::ConstPolygon3d & polygon, const size_t start_idx, const size_t end_idx, + const bool clockwise) +{ + std::vector ret{}; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { + ret.push_back(polygon[i]); + + if (i + 1 == polygon.size() && clockwise) { + if (end_idx == 0) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = 0; + ret.push_back(polygon[i]); + continue; + } + + if (i == 0 && !clockwise) { + if (end_idx == polygon.size() - 1) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = polygon.size() - 1; + ret.push_back(polygon[i]); + continue; + } + } + + ret.push_back(polygon[end_idx]); + + return ret; +} } // namespace namespace behavior_path_planner::utils::drivable_area_processing @@ -1261,36 +1295,6 @@ std::vector getBoundWithIntersectionAreas( const std::shared_ptr & route_handler, const std::vector & drivable_lanes, const bool is_left) { - const auto extract_bound_from_polygon = - [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { - std::vector ret{}; - for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { - ret.push_back(polygon[i]); - - if (i + 1 == polygon.size() && clockwise) { - if (end_idx == 0) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = 0; - continue; - } - - if (i == 0 && !clockwise) { - if (end_idx == polygon.size() - 1) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = polygon.size() - 1; - continue; - } - } - - ret.push_back(polygon[end_idx]); - - return ret; - }; - std::vector expanded_bound = original_bound; // expand drivable area by using intersection area. @@ -1336,7 +1340,7 @@ std::vector getBoundWithIntersectionAreas( const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last); intersection_bound = - extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration); + extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); } // Step2. check shared bound point. From f2936f08b46b55559749836f416ec1ca6610c531 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 11 Jan 2024 00:32:30 +0900 Subject: [PATCH 022/154] feat(avoidance): check traffic light info in order to limit drivable area (#6016) * feat(avoidance): don't use opposite lane before intersection Signed-off-by: satoshi-ota * feat(avoidance): check traffic light info in order to limit drivable area Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 2 +- .../behavior_path_avoidance_module/utils.hpp | 2 +- .../src/scene.cpp | 6 ++++-- .../src/utils.cpp | 12 ++++++++++- .../utils/traffic_light_utils.hpp | 15 ++++++++++++++ .../src/utils/traffic_light_utils.cpp | 20 +++++++++++++++++++ 6 files changed, 52 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 66d11d044966e..b76a9e5645b48 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -166,7 +166,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( - lanelet, planner_data_, avoidance_parameters_)); + lanelet, planner_data_, avoidance_parameters_, false)); }); // calc drivable bound diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 4508cbc3c18f6..dc602edcc8b86 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -163,7 +163,7 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters, const bool in_avoidance_maneuver); double calcDistanceToReturnDeadLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 31849c716971c..9d7f5cc459e9e 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -223,10 +223,12 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); // expand drivable lanes + const auto has_shift_point = !path_shifter_.getShiftLines().empty(); + const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted(); std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back( - utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); + data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( + lanelet, planner_data_, parameters_, in_avoidance_maneuver)); }); // calc drivable bound diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 493ca35f09a3b..83e96d83b995f 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1953,7 +1953,7 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + const std::shared_ptr & parameters, const bool in_avoidance_maneuver) { const auto & route_handler = planner_data->route_handler; @@ -1967,6 +1967,11 @@ DrivableLanes generateExpandDrivableLanes( // 1. get left/right side lanes const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto next_lanes = route_handler->getNextLanelets(target_lane); + const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); + if (is_stop_signal && !in_avoidance_maneuver) { + return; + } const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_left_lanelets.empty()) { @@ -1977,6 +1982,11 @@ DrivableLanes generateExpandDrivableLanes( } }; const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto next_lanes = route_handler->getNextLanelets(target_lane); + const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); + if (is_stop_signal && !in_avoidance_maneuver) { + return; + } const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_right_lanelets.empty()) { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp index 6380830806c33..62fd87bbd91bc 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -92,6 +92,21 @@ bool isStoppedAtRedTrafficLightWithinDistance( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data, const double distance_threshold = std::numeric_limits::infinity()); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param planner_data Shared pointer to the planner data containing vehicle state information. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::traffic_light #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp index f77fafb42d829..ecd02bfd63485 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -127,4 +127,24 @@ bool isStoppedAtRedTrafficLightWithinDistance( return (distance_to_red_traffic_light < distance_threshold); } +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data) +{ + for (const auto & lanelet : lanelets) { + for (const auto & element : lanelet.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); + if (!traffic_signal_stamped.has_value()) { + continue; + } + + if (traffic_light_utils::isTrafficSignalStop( + lanelet, traffic_signal_stamped.value().signal)) { + return true; + } + } + } + + return false; +} + } // namespace behavior_path_planner::utils::traffic_light From 3db20c0198fdfef0df0cfa9752c6b75a494c96fc Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 11 Jan 2024 01:39:06 +0900 Subject: [PATCH 023/154] fix(avoidance): return shift path was not generated expectedly (#6017) fix(avoidance): output return shift path properly Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 3 ++- .../src/shift_line_generator.cpp | 8 +++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 9d7f5cc459e9e..61941eeee6f00 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1390,7 +1390,8 @@ void AvoidanceModule::insertReturnDeadLine( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); - const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length); + const auto min_return_distance = + helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 8ed56ce9aff4d..dabb0d7257555 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -1101,12 +1101,14 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const double variable_prepare_distance = std::max(nominal_prepare_distance - last_sl_distance, 0.0); - double prepare_distance_scaled = std::max(nominal_prepare_distance, last_sl_distance); + double prepare_distance_scaled = + std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), last_sl_distance); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); - prepare_distance_scaled = last_sl_distance + scale * nominal_prepare_distance; + prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); avoid_distance_scaled *= scale; } @@ -1219,7 +1221,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( const auto & candidate = shift_lines.at(i); // prevent sudden steering. - if (candidate.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { break; } From a0da8c43be7567736860756710d808d207c14b67 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 11 Jan 2024 01:58:37 +0900 Subject: [PATCH 024/154] refactor(ndt_scan_matcher): fixed ndt_scan_matcher.launch.xml (#6041) Fixed ndt_scan_matcher.launch.xml Signed-off-by: Shintaro Sakoda --- .../ndt_scan_matcher.launch.xml | 7 +++++-- .../launch/ndt_scan_matcher.launch.xml | 16 ++++++++-------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml index e4b04d3d4a32e..bee300c587816 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml @@ -2,13 +2,16 @@ - - + + + + + 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 891d172aaf8fd..cbcb0a9f74bc4 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -2,27 +2,27 @@ - - - + + + - + - - + + + - - + From 12fb78878671a964e1376b0364ee65167ca2d3e7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 11 Jan 2024 03:35:07 +0900 Subject: [PATCH 025/154] feat(behavior_velocity): add the option to keep the last valid observation (#6036) * feat(behavior_velocity): add the option to keep the last valid observation Signed-off-by: Mamoru Sobue * keep_last_observation is false by default and intersection/traffic_light is explicily true Signed-off-by: Mamoru Sobue * for intersection Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../src/scene_crosswalk.cpp | 9 +++--- .../src/scene_intersection.cpp | 31 +++++++++---------- .../src/scene_intersection.hpp | 9 ++++-- .../behavior_velocity_planner/src/node.cpp | 16 +++++++++- .../planner_data.hpp | 19 +++++++++--- .../src/scene.cpp | 8 ++--- 6 files changed, 60 insertions(+), 32 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 0dc7b78b6fe46..7175cd0895fc0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1055,19 +1055,20 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - const auto traffic_signal_stamped = + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); - if (!traffic_signal_stamped) { + if (!traffic_signal_stamped_opt) { continue; } + const auto traffic_signal_stamped = traffic_signal_stamped_opt.value(); if ( planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + (clock_->now() - traffic_signal_stamped.stamp).seconds()) { continue; } - const auto & lights = traffic_signal_stamped->signal.elements; + const auto & lights = traffic_signal_stamped.signal.elements; if (lights.empty()) { continue; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 292a6e6a8843d..b6201057f5e23 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -940,12 +940,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } -static bool isGreenSolidOn( - lanelet::ConstLanelet lane, const std::map & tl_infos) +bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - std::optional tl_id = std::nullopt; + std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); break; @@ -954,12 +953,13 @@ static bool isGreenSolidOn( // this lane has no traffic light return false; } - const auto tl_info_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { // the info of this traffic light is not available return false; } - const auto & tl_info = tl_info_it->second; + const auto & tl_info = tl_info_opt.value(); for (auto && tl_light : tl_info.signal.elements) { if ( tl_light.color == TrafficSignalElement::GREEN && @@ -1004,8 +1004,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // at the very first time of regisTration of this module, the path may not be conflicting with the // attention area, so update() is called to update the internal data as well as traffic light info - const auto traffic_prioritized_level = - getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet); const bool is_prioritized = traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); @@ -1259,8 +1258,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // If there are any vehicles on the attention area when ego entered the intersection on green // light, do pseudo collision detection because the vehicles are very slow and no collisions may // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = - isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map); + const bool is_green_solid_on = isGreenSolidOn(assigned_lanelet); if (is_green_solid_on) { if (!initial_green_light_observed_time_) { const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); @@ -1413,12 +1411,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } -TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos) +TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(lanelet::ConstLanelet lane) { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - std::optional tl_id = std::nullopt; + std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); break; @@ -1427,12 +1424,12 @@ TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel( // this lane has no traffic light return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto tl_info_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { - // the info of this traffic light is not available + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto & tl_info = tl_info_it->second; + const auto & tl_info = tl_info_opt.value(); bool has_amber_signal{false}; for (auto && tl_light : tl_info.signal.elements) { if (tl_light.color == TrafficSignalElement::AMBER) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 6a350e66706e6..d533c7316b264 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -631,8 +631,7 @@ class IntersectionModule : public SceneModuleInterface * @fn * @brief find TrafficPrioritizedLevel */ - TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos); + TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane); /** * @fn @@ -738,6 +737,12 @@ class IntersectionModule : public SceneModuleInterface const std::vector & lane_divisions, const TargetObjects & target_objects); + /* + * @fn + * @brief check if associated traffic light is green + */ + bool isGreenSolidOn(lanelet::ConstLanelet lane); + /* bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 8ecad4d9c1548..e2c29ef868257 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -327,7 +327,21 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + if ( + is_unknown_observation && + planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + msg->stamp; + } else { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + } } } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2ea9f6ed2648b..606e41ad4b1d1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -76,7 +76,10 @@ struct PlannerData double ego_nearest_yaw_threshold; // other internal data - std::map traffic_light_id_map; + // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the + // last observed infomation for UNKNOWN + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -125,12 +128,20 @@ struct PlannerData return true; } - std::shared_ptr getTrafficSignal(const int id) const + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ + std::optional getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation = false) const { + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; if (traffic_light_id_map.count(id) == 0) { - return {}; + return std::nullopt; } - return std::make_shared(traffic_light_id_map.at(id)); + return std::make_optional(traffic_light_id_map.at(id)); } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index a17fc43b86f23..9982ec34c4bca 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -353,15 +353,15 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const { // get traffic signal associated with the regulatory element id - const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); - if (!traffic_signal_stamped) { + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal( + traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */); + if (!traffic_signal_stamped_opt) { RCLCPP_WARN_THROTTLE( logger_, *clock_, 5000 /* ms */, "the traffic signal data associated with regulatory element id is not received"); return false; } - - valid_traffic_signal = *traffic_signal_stamped; + valid_traffic_signal = traffic_signal_stamped_opt.value(); return true; } From 28c871a6dd21f4141b750c61b1a901cef06e1df7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 11 Jan 2024 04:56:03 +0900 Subject: [PATCH 026/154] feat(intersection): distinguish 1st/2nd attention lanelet (#6042) --- .../src/debug.cpp | 16 ++ .../src/scene_intersection.cpp | 137 ++++++++++++++---- .../src/scene_intersection.hpp | 44 +++++- 3 files changed, 159 insertions(+), 38 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 83e218185a5ad..5f103e0ecd3b0 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -188,6 +188,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } + if (debug_data_.first_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + if (debug_data_.stuck_vehicle_detect_area) { appendMarkerArray( debug::createPolygonMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b6201057f5e23..1bc7bfe04d271 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1007,7 +1007,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet); const bool is_prioritized = traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); + intersection_lanelets.update( + is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); @@ -1018,6 +1019,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); + const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); // generate all stop line candidates // see the doc for struct IntersectionStopLines @@ -1028,16 +1030,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( : first_conflicting_lane; const auto intersection_stoplines_opt = generateIntersectionStopLines( - assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, interpolated_path_info, - path); + assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, + interpolated_path_info, path); if (!intersection_stoplines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; } const auto & intersection_stoplines = intersection_stoplines_opt.value(); - const auto - [closest_idx, stuck_stopline_idx_opt, default_stopline_idx_opt, - first_attention_stopline_idx_opt, occlusion_peeking_stopline_idx_opt, - default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines; + const auto closest_idx = intersection_stoplines.closest_idx; + const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; + const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; + const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto occlusion_wo_tl_pass_judge_line_idx = + intersection_stoplines.occlusion_wo_tl_pass_judge_line; // see the doc for struct PathLanelets const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); @@ -1146,6 +1152,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.attention_area = intersection_lanelets.attention_area(); debug_data_.occlusion_attention_area = occlusion_attention_area; debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); // check occlusion on detection lane if (!occlusion_attention_divisions_) { @@ -1659,6 +1667,7 @@ IntersectionLanelets IntersectionModule::getObjectiveLanelets( std::optional IntersectionModule::generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, const util::InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { @@ -1680,15 +1689,16 @@ std::optional IntersectionModule::generateIntersectionSto const int base2front_idx_dist = std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); - // find the index of the first point whose vehicle footprint on it intersects with detection_area + // find the index of the first point whose vehicle footprint on it intersects with attention_area const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - std::optional first_footprint_inside_detection_ip_opt = + const std::optional first_footprint_inside_1st_attention_ip_opt = getFirstPointInsidePolygonByFootprint( first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_detection_ip_opt) { + if (!first_footprint_inside_1st_attention_ip_opt) { return std::nullopt; } - const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); + const auto first_footprint_inside_1st_attention_ip = + first_footprint_inside_1st_attention_ip_opt.value(); std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { @@ -1716,7 +1726,7 @@ std::optional IntersectionModule::generateIntersectionSto stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; + stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; } if (stop_idx_ip_int < 0) { default_stopline_valid = false; @@ -1732,7 +1742,7 @@ std::optional IntersectionModule::generateIntersectionSto // (3) occlusion peeking stop line position on interpolated path int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the detection area, invalid + // NOTE: if footprints[0] is already inside the attention area, invalid { const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( @@ -1744,32 +1754,32 @@ std::optional IntersectionModule::generateIntersectionSto } if (occlusion_peeking_line_valid) { occlusion_peeking_line_ip_int = - first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); + first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); } - const auto occlusion_peeking_line_ip = static_cast( std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; + + // (4) first attention stopline position on interpolated path + const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; const bool first_attention_stopline_valid = true; - // (4) pass judge line position on interpolated path + // (5) 1st pass judge line position on interpolated path const double velocity = planner_data_->current_velocity->twist.linear.x; const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( velocity, acceleration, max_accel, max_jerk, delay_response_time); - int pass_judge_ip_int = - static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); - const auto pass_judge_line_ip = static_cast( - std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - // TODO(Mamoru Sobue): maybe braking dist should be considered - const auto occlusion_wo_tl_pass_judge_line_ip = - static_cast(first_footprint_attention_centerline_ip); - - // (5) stuck vehicle stop line + int first_pass_judge_ip_int = + static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); + const auto first_pass_judge_line_ip = static_cast( + std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( + 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); + + // (6) stuck vehicle stopline position on interpolated path int stuck_stopline_ip_int = 0; bool stuck_stopline_valid = true; if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching detection area and already passed + // NOTE: when ego vehicle is approaching attention area and already passed // first_conflicting_area, this could be null. const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); @@ -1788,14 +1798,37 @@ std::optional IntersectionModule::generateIntersectionSto } const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + // (7) second attention stopline position on interpolated path + int second_attention_stopline_ip_int = -1; + bool second_attention_stopline_valid = false; + if (second_attention_area_opt) { + const auto & second_attention_area = second_attention_area_opt.value(); + std::optional first_footprint_inside_2nd_attention_ip_opt = + getFirstPointInsidePolygonByFootprint( + second_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (first_footprint_inside_2nd_attention_ip_opt) { + second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + } + } + const auto second_attention_stopline_ip = + second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) + : 0; + + // (8) second pass judge lie position on interpolated path + int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds); + const auto second_pass_judge_line_ip = + static_cast(std::max(second_pass_judge_ip_int, 0)); + struct IntersectionStopLinesTemp { size_t closest_idx{0}; size_t stuck_stopline{0}; size_t default_stopline{0}; size_t first_attention_stopline{0}; + size_t second_attention_stopline{0}; size_t occlusion_peeking_stopline{0}; - size_t pass_judge_line{0}; + size_t first_pass_judge_line{0}; + size_t second_pass_judge_line{0}; size_t occlusion_wo_tl_pass_judge_line{0}; }; @@ -1805,8 +1838,10 @@ std::optional IntersectionModule::generateIntersectionSto {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, + {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, + {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, {&occlusion_wo_tl_pass_judge_line_ip, &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; stoplines.sort( @@ -1840,11 +1875,17 @@ std::optional IntersectionModule::generateIntersectionSto intersection_stoplines.first_attention_stopline = intersection_stoplines_temp.first_attention_stopline; } + if (second_attention_stopline_valid) { + intersection_stoplines.second_attention_stopline = + intersection_stoplines_temp.second_attention_stopline; + } if (occlusion_peeking_line_valid) { intersection_stoplines.occlusion_peeking_stopline = intersection_stoplines_temp.occlusion_peeking_stopline; } - intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; + intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; intersection_stoplines.occlusion_wo_tl_pass_judge_line = intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stoplines; @@ -3249,7 +3290,8 @@ getFirstPointInsidePolygonsByFootprint( void IntersectionLanelets::update( const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr) { is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path @@ -3262,13 +3304,44 @@ void IntersectionLanelets::update( } } if (!first_attention_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( + const auto first = getFirstPointInsidePolygonsByFootprint( attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); if (first) { first_attention_lane_ = attention_non_preceding_.at(first.value().second); first_attention_area_ = attention_non_preceding_area_.at(first.value().second); } } + if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { + const auto first_attention_lane = first_attention_lane_.value(); + // remove first_attention_area_ and non-straight lanelets from attention_non_preceding + lanelet::ConstLanelets attention_non_preceding_ex_first; + lanelet::ConstLanelets sibling_first_attention_lanelets; + for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { + for (const auto & following : routing_graph_ptr->following(previous)) { + sibling_first_attention_lanelets.push_back(following); + } + } + for (const auto & ll : attention_non_preceding_) { + // the sibling lanelets of first_attention_lanelet are ruled out + if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { + continue; + } + if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { + attention_non_preceding_ex_first.push_back(ll); + } + } + if (attention_non_preceding_ex_first.empty()) { + second_attention_lane_empty_ = true; + } + const auto attention_non_preceding_ex_first_area = + getPolygon3dFromLanelets(attention_non_preceding_ex_first); + const auto second = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); + if (second) { + second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); + second_attention_area_ = second_attention_lane_.value().polygon3d(); + } + } } void TargetObject::calc_dist_to_stopline() diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index d533c7316b264..38feada725872 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -53,6 +53,8 @@ struct DebugData std::optional> occlusion_attention_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; + std::optional first_attention_area{std::nullopt}; + std::optional second_attention_area{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional> yield_stuck_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; @@ -82,7 +84,8 @@ struct IntersectionLanelets */ void update( const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr); const lanelet::ConstLanelets & attention() const { @@ -131,6 +134,14 @@ struct IntersectionLanelets { return first_attention_area_; } + const std::optional & second_attention_lane() const + { + return second_attention_lane_; + } + const std::optional & second_attention_area() const + { + return second_attention_area_; + } /** * the set of attention lanelets which is topologically merged @@ -178,17 +189,25 @@ struct IntersectionLanelets std::vector occlusion_attention_size_; /** - * the attention lanelet which ego path points intersect for the first time + * the first conflicting lanelet which ego path points intersect for the first time */ std::optional first_conflicting_lane_{std::nullopt}; std::optional first_conflicting_area_{std::nullopt}; /** - * the attention lanelet which ego path points intersect for the first time + * the first attention lanelet which ego path points intersect for the first time */ std::optional first_attention_lane_{std::nullopt}; std::optional first_attention_area_{std::nullopt}; + /** + * the second attention lanelet which ego path points intersect next to the + * first_attention_lanelet + */ + bool second_attention_lane_empty_{false}; + std::optional second_attention_lane_{std::nullopt}; + std::optional second_attention_area_{std::nullopt}; + /** * flag if the intersection is prioritized by the traffic light */ @@ -219,16 +238,28 @@ struct IntersectionStopLines */ std::optional first_attention_stopline{std::nullopt}; + /** + * second_attention_stopline is null if ego footprint along the path does not intersect with + * second_attention_lane. if path[0] satisfies the condition, it is 0 + */ + std::optional second_attention_stopline{std::nullopt}; + /** * occlusion_peeking_stopline is null if path[0] is already inside the attention area */ std::optional occlusion_peeking_stopline{std::nullopt}; /** - * pass_judge_line is before first_attention_stop_line by the braking distance. if its value is - * calculated negative, it is 0 + * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value + * is calculated negative, it is 0 + */ + size_t first_pass_judge_line{0}; + + /** + * second_pass_judge_line is before second_attention_stopline by the braking distance. if its + * value is calculated negative, it is 0 */ - size_t pass_judge_line{0}; + size_t second_pass_judge_line{0}; /** * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with @@ -650,6 +681,7 @@ class IntersectionModule : public SceneModuleInterface lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, const util::InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); From 93ad31153588ff32cc16d8ce9020f129fe946905 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Thu, 11 Jan 2024 02:08:30 +0300 Subject: [PATCH 027/154] fix(motion_velocity_smoother): make stopping_distance and stopping_velocity be able to work (#6037) Signed-off-by: Berkay Karaman --- .../src/motion_velocity_smoother_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 90f7295c4cf6f..5138da6a43c0c 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -453,6 +453,9 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar return; } + // Set 0 at the end of the trajectory + input_points.back().longitudinal_velocity_mps = 0.0; + // calculate prev closest point if (!prev_output_.empty()) { current_closest_point_from_prev_output_ = calcProjectedTrajectoryPointFromEgo(prev_output_); From 17a3408dcaf515d6cfaada7e85b03bd640c68480 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 11 Jan 2024 08:43:59 +0900 Subject: [PATCH 028/154] refactor(tier4_planning_launch): remove duplicate arguments in launchfile (#6040) Signed-off-by: Maxime CLEMENT --- .../behavior_planning/behavior_planning.launch.xml | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index dd1c0604bc86f..1c7643359c496 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -16,17 +16,6 @@ - - - - - - - - - - - From 49d5f2245f4f9af6c31923cc7e2c6c58594bed76 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 11 Jan 2024 11:08:40 +0900 Subject: [PATCH 029/154] fix(lane_change): check able to return to original lane in abort (#6034) * fix(lane_change): check able to return to original lane in abort Signed-off-by: Muhammad Zulfaqar Azmi * fix cancel state Signed-off-by: Muhammad Zulfaqar Azmi * revert changes Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- planning/behavior_path_lane_change_module/src/interface.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 22d85a2a1fc2a..448f83ecaf15e 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -276,6 +276,11 @@ bool LaneChangeInterface::canTransitFailureState() return false; } + if (!module_type_->isAbleToReturnCurrentLane()) { + log_debug_throttled("It's is not possible to return to original lane. Continue lane change."); + return false; + } + const auto found_abort_path = module_type_->calcAbortPath(); if (!found_abort_path) { log_debug_throttled( From 9b736d52274e85511c608923955d82a8cb50d001 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 11 Jan 2024 11:39:43 +0900 Subject: [PATCH 030/154] feat(start_planner): keep distance against front objects (#5983) * refactor extractCollisionCheckPath Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: Kosuke Takeuchi --- .../behavior_path_start_planner_module/README.md | 3 ++- .../config/start_planner.param.yaml | 1 + .../start_planner_module.hpp | 2 +- .../start_planner_parameters.hpp | 1 + .../src/manager.cpp | 2 ++ .../src/start_planner_module.cpp | 15 +++++++++++---- 6 files changed, 18 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 2f90cc2ee3cf2..80ad8d6675e9a 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -75,7 +75,8 @@ PullOutPath --o PullOutPlannerBase | intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | | length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | | collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | | center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 514d61e225ecd..d04728861a52e 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -7,6 +7,7 @@ th_stopped_time: 1.0 collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 + collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index a451a92c16ff1..eb1827ec0045d 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -174,7 +174,7 @@ class StartPlannerModule : public SceneModuleInterface const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose); - PathWithLaneId extractCollisionCheckPath(const PullOutPath & path); + PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index 783aace0983ca..2df75107d872c 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -44,6 +44,7 @@ struct StartPlannerParameters double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; double collision_check_margin{0.0}; double collision_check_distance_from_end{0.0}; + double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 58d0fbee7921b..f719718a3389e 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -46,6 +46,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); + p.collision_check_margin_from_front_object = + node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 66e04d7e9bb7a..fd23adf654502 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -645,7 +645,7 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, extractCollisionCheckPath(*pull_out_path), pull_out_lane_stop_objects, + vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, parameters_->collision_check_margin)) { return false; } @@ -660,7 +660,7 @@ bool StartPlannerModule::findPullOutPath( return true; } -PathWithLaneId StartPlannerModule::extractCollisionCheckPath(const PullOutPath & path) +PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) { PathWithLaneId combined_path; for (const auto & partial_path : path.partial_paths) { @@ -912,6 +912,10 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, backward_path_length, std::numeric_limits::max()); + const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, + std::numeric_limits::max()); + // Set the maximum backward distance less than the distance from the vehicle's base_link to the // lane's rearmost point to prevent lane departure. const double current_arc_length = @@ -924,9 +928,12 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( back_path_from_start_pose.points, start_pose.position, -back_distance); - if (!backed_pose) { + if (!backed_pose) continue; + + if (utils::checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, *backed_pose, front_stop_objects_in_pull_out_lanes, + parameters_->collision_check_margin_from_front_object)) continue; - } const double backed_pose_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length; From 8785bf829c23836b63f044c0fb275d131835b41f Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Thu, 11 Jan 2024 05:13:56 +0100 Subject: [PATCH 031/154] feat(ekf_localizer): tf publisher as an option (#6004) Signed-off-by: amadeuszsz --- localization/ekf_localizer/README.md | 1 + .../ekf_localizer/config/ekf_localizer.param.yaml | 1 + .../include/ekf_localizer/hyper_parameters.hpp | 2 ++ localization/ekf_localizer/src/ekf_localizer.cpp | 8 +++++--- 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 87cc510c5fe14..02f14a86cd7df 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -83,6 +83,7 @@ The parameters are set in `launch/ekf_localizer.launch` . | show_debug_info | bool | Flag to display debug info | false | | predict_frequency | double | Frequency for filtering and publishing [Hz] | 50.0 | | tf_rate | double | Frequency for tf broadcasting [Hz] | 10.0 | +| publish_tf | bool | Whether to publish tf | true | | extend_state_step | int | Max delay step which can be dealt with in EKF. Large number increases computational cost. | 50 | | enable_yaw_bias_estimation | bool | Flag to enable yaw bias estimation | true | diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 1db45db411b0c..9de7f56f5c006 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -4,6 +4,7 @@ enable_yaw_bias_estimation: true predict_frequency: 50.0 tf_rate: 50.0 + publish_tf: true extend_state_step: 50 # for Pose measurement diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index e0e34f382e3dc..5139f900a339e 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -28,6 +28,7 @@ class HyperParameters ekf_rate(node->declare_parameter("predict_frequency", 50.0)), ekf_dt(1.0 / std::max(ekf_rate, 0.1)), tf_rate_(node->declare_parameter("tf_rate", 10.0)), + publish_tf_(node->declare_parameter("publish_tf", true)), enable_yaw_bias_estimation(node->declare_parameter("enable_yaw_bias_estimation", true)), extend_state_step(node->declare_parameter("extend_state_step", 50)), pose_frame_id(node->declare_parameter("pose_frame_id", std::string("map"))), @@ -60,6 +61,7 @@ class HyperParameters const double ekf_rate; const double ekf_dt; const double tf_rate_; + const bool publish_tf_; const bool enable_yaw_bias_estimation; const int extend_state_step; const std::string pose_frame_id; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 0ab88202e0c0d..b3ee3665cf45e 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -60,9 +60,11 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), std::bind(&EKFLocalizer::timerCallback, this)); - timer_tf_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timerTFCallback, this)); + if (params_.publish_tf_) { + timer_tf_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), + std::bind(&EKFLocalizer::timerTFCallback, this)); + } pub_pose_ = create_publisher("ekf_pose", 1); pub_pose_cov_ = From 7597b28b64cc6f53245a5e0c23e39702b7cca974 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 11 Jan 2024 13:18:56 +0900 Subject: [PATCH 032/154] refactor(behavior_path_planner): remove create_vehicle_footprint.hpp (#6049) Signed-off-by: satoshi-ota --- .../src/utils.cpp | 1 - .../pull_over_planner_base.hpp | 3 +- .../src/goal_planner_module.cpp | 3 +- .../utils/create_vehicle_footprint.hpp | 47 ------------------- .../pull_out_planner_base.hpp | 3 +- .../src/start_planner_module.cpp | 7 ++- .../src/util.cpp | 1 - 7 files changed, 6 insertions(+), 59 deletions(-) delete mode 100644 planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 83e96d83b995f..f72f8279d4351 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -16,7 +16,6 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index ddd7c93998654..fe24d66ddc850 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -17,7 +17,6 @@ #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include #include @@ -114,7 +113,7 @@ class PullOverPlannerBase PullOverPlannerBase(rclcpp::Node & node, const GoalPlannerParameters & parameters) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } virtual ~PullOverPlannerBase() = default; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 493e7ec57f063..7bd97b3775bad 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -15,7 +15,6 @@ #include "behavior_path_goal_planner_module/goal_planner_module.hpp" #include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -94,7 +93,7 @@ GoalPlannerModule::GoalPlannerModule( // set selected goal searcher // currently there is only one goal_searcher_type const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - vehicle_footprint_ = createVehicleFootprint(vehicle_info); + vehicle_footprint_ = vehicle_info.createFootprint(); goal_searcher_ = std::make_shared(*parameters, vehicle_footprint_, occupancy_grid_map_); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp deleted file mode 100644 index edb6d32bda205..0000000000000 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// 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 BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ - -#include -#include - -inline tier4_autoware_utils::LinearRing2d createVehicleFootprint( - const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) -{ - using tier4_autoware_utils::LinearRing2d; - using tier4_autoware_utils::Point2d; - - const auto & i = vehicle_info; - - const double x_front = i.front_overhang_m + i.wheel_base_m + margin; - const double x_center = i.wheel_base_m / 2.0; - const double x_rear = -(i.rear_overhang_m + margin); - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; - const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); - - LinearRing2d footprint; - footprint.push_back(Point2d{x_front, y_left}); - footprint.push_back(Point2d{x_front, y_right}); - footprint.push_back(Point2d{x_center, y_right}); - footprint.push_back(Point2d{x_rear, y_right}); - footprint.push_back(Point2d{x_rear, y_left}); - footprint.push_back(Point2d{x_center, y_left}); - footprint.push_back(Point2d{x_front, y_left}); - - return footprint; -} - -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index de3d376fa4b3d..e345e3dc91337 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ #include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/start_planner_parameters.hpp" @@ -45,7 +44,7 @@ class PullOutPlannerBase explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } virtual ~PullOutPlannerBase() = default; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index fd23adf654502..6702e37f19077 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -14,7 +14,6 @@ #include "behavior_path_start_planner_module/start_planner_module.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" @@ -623,7 +622,7 @@ bool StartPlannerModule::findPullOutPath( const auto & dynamic_objects = planner_data_->dynamic_object; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - const auto & vehicle_footprint = createVehicleFootprint(vehicle_info_); + const auto & vehicle_footprint = vehicle_info_.createFootprint(); // extract stop objects in pull out lane for collision check const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *dynamic_objects, parameters_->th_moving_object_velocity); @@ -902,7 +901,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( { std::vector pull_out_start_pose_candidates{}; const auto start_pose = planner_data_->route_handler->getOriginalStartPose(); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); const double backward_path_length = @@ -1356,7 +1355,7 @@ void StartPlannerModule::setDebugData() const // visualize collision_check_end_pose and footprint { - const auto local_footprint = createVehicleFootprint(vehicle_info_); + const auto local_footprint = vehicle_info_.createFootprint(); const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( getFullPath().points, status_.pull_out_path.end_pose.position, parameters_->collision_check_distance_from_end); diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index 73a1e94399503..b34398083a95c 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -14,7 +14,6 @@ #include "behavior_path_start_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" From 6928fff0d9055645a6f31c36ac0fd0381f5d7459 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 11 Jan 2024 14:53:37 +0900 Subject: [PATCH 033/154] feat(start_planner): define collision check margin as list (#5994) * define collision check margins list in start planner module Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../parking_departure/common_module_data.hpp | 2 + .../README.md | 26 +++++------ .../config/start_planner.param.yaml | 2 +- .../start_planner_module.hpp | 2 +- .../start_planner_parameters.hpp | 2 +- .../src/manager.cpp | 3 +- .../src/start_planner_module.cpp | 45 ++++++++++--------- 7 files changed, 45 insertions(+), 37 deletions(-) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 91683f4547659..3dc230f0e92ed 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -41,6 +41,8 @@ struct StartGoalPlannerData Pose refined_start_pose; std::vector start_pose_candidates; + size_t selected_start_pose_candidate_index; + double margin_for_start_pose_candidate; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 80ad8d6675e9a..aeefd18357a5c 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -65,19 +65,19 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | -| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :------- | :-------------------------------------------------------------------------- | :-------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | [double] | Obstacle collision check margins list | [2.0, 1.5, 1.0] | +| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index d04728861a52e..d174b54b9ccbe 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,7 +5,7 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margin: 1.0 + collision_check_margins: [2.0, 1.5, 1.0] collision_check_distance_from_end: 1.0 collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index eb1827ec0045d..41a80e59d56bf 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -172,7 +172,7 @@ class StartPlannerModule : public SceneModuleInterface const std::string & search_priority, const size_t start_pose_candidates_num); bool findPullOutPath( const Pose & start_pose_candidate, const std::shared_ptr & planner, - const Pose & refined_start_pose, const Pose & goal_pose); + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); void updateStatusWithCurrentPath( diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index 2df75107d872c..66dbddebf99bb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -42,7 +42,7 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; - double collision_check_margin{0.0}; + std::vector collision_check_margins{}; double collision_check_distance_from_end{0.0}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index f719718a3389e..8cc0b34082e44 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -43,7 +43,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); - p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); + p.collision_check_margins = + node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); p.collision_check_margin_from_front_object = diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 6702e37f19077..d523f57125679 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -584,9 +584,16 @@ void StartPlannerModule::planWithPriority( const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); - for (const auto & [index, planner] : order_priority) { - if (findPullOutPath(start_pose_candidates[index], planner, refined_start_pose, goal_pose)) - return; + for (const auto & collision_check_margin : parameters_->collision_check_margins) { + for (const auto & [index, planner] : order_priority) { + if (findPullOutPath( + start_pose_candidates[index], planner, refined_start_pose, goal_pose, + collision_check_margin)) { + start_planner_data_.selected_start_pose_candidate_index = index; + start_planner_data_.margin_for_start_pose_candidate = collision_check_margin; + return; + } + } } updateStatusIfNoSafePathFound(); @@ -617,7 +624,7 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( bool StartPlannerModule::findPullOutPath( const Pose & start_pose_candidate, const std::shared_ptr & planner, - const Pose & refined_start_pose, const Pose & goal_pose) + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin) { const auto & dynamic_objects = planner_data_->dynamic_object; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( @@ -645,7 +652,7 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, - parameters_->collision_check_margin)) { + collision_check_margin)) { return false; } @@ -666,23 +673,21 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat combined_path.points.insert( combined_path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - // calculate collision check end idx - size_t collision_check_end_idx = 0; - const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); - - if (collision_check_end_pose) { - collision_check_end_idx = - motion_utils::findNearestIndex(combined_path.points, collision_check_end_pose->position); - } + const size_t collision_check_end_idx = std::invoke([&]() { + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + if (collision_check_end_pose) { + return motion_utils::findNearestIndex( + combined_path.points, collision_check_end_pose->position); + } else { + return combined_path.points.size() - 1; + } + }); // remove the point behind of collision check end pose - if (collision_check_end_idx + 1 < combined_path.points.size()) { - combined_path.points.erase( - combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end()); - } - + combined_path.points.erase( + combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end()); return combined_path; } @@ -949,7 +954,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( if (utils::checkCollisionBetweenFootprintAndObjects( local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes, - parameters_->collision_check_margin)) { + parameters_->collision_check_margins.back())) { break; // poses behind this has a collision, so break. } From 5a0e1411b0d5e6565185459ac15351cfd8a81698 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 11 Jan 2024 15:09:38 +0900 Subject: [PATCH 034/154] fix(map_based_prediction): yaw rate can be overwritten by max function (#6039) * fix problem with negative yaw being replaced in max function Signed-off-by: Daniel Sanchez * add check for delta time value Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../map_based_prediction/map_based_prediction_node.hpp | 9 +++++---- .../src/map_based_prediction_node.cpp | 8 ++++---- perception/map_based_prediction/src/path_generator.cpp | 2 +- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 02ca62a61717c..02db91b989116 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -316,8 +316,11 @@ class MapBasedPredictionNode : public rclcpp::Node }; inline bool isLateralAccelerationConstraintSatisfied( - const TrajectoryPoints & trajectory [[maybe_unused]], const double delta_time) + const TrajectoryPoints & trajectory, const double delta_time) { + constexpr double epsilon = 1E-6; + if (delta_time < epsilon) throw std::invalid_argument("delta_time must be a positive value"); + if (trajectory.size() < 3) return true; const double max_lateral_accel_abs = std::fabs(max_lateral_accel_); @@ -343,19 +346,17 @@ class MapBasedPredictionNode : public rclcpp::Node delta_theta += 2.0 * M_PI; } - const double yaw_rate = std::max(delta_theta / delta_time, 1.0E-5); + const double yaw_rate = std::max(std::abs(delta_theta / delta_time), 1.0E-5); const double current_speed = std::abs(trajectory.at(i).longitudinal_velocity_mps); // Compute lateral acceleration const double lateral_acceleration = std::abs(current_speed * yaw_rate); if (lateral_acceleration < max_lateral_accel_abs) continue; - const double v_curvature_max = std::sqrt(max_lateral_accel_abs / yaw_rate); const double t = (v_curvature_max - current_speed) / min_acceleration_before_curve_; // acc is negative const double distance_to_slow_down = current_speed * t + 0.5 * min_acceleration_before_curve_ * std::pow(t, 2); - if (distance_to_slow_down > arc_length) return false; } return true; diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 75c1d61e0a19c..6a6f24081655e 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -613,16 +613,16 @@ ObjectClassification::_label_type changeLabelForPrediction( case ObjectClassification::BICYCLE: { // if object is within road lanelet and satisfies yaw // constraints const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float high_speed_threshold = + // if the object is within lanelet, do the same estimation with vehicle + if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; + + constexpr float high_speed_threshold = tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h // calc abs speed from x and y velocity const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); const bool high_speed_object = abs_speed > high_speed_threshold; - - // if the object is within lanelet, do the same estimation with vehicle - if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; // high speed object outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; // temporary disabled if (high_speed_object) return label; // Do nothing for now diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 5cb7e186b7cc4..d3f610583577c 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -162,7 +162,7 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) { const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; - const double ep = 0.001; + constexpr double ep = 0.001; const double duration = time_horizon_ + ep; PredictedPath path; From 73c9e1727eaaa959f2af10e1649577dee85baf0b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 11 Jan 2024 18:27:54 +0900 Subject: [PATCH 035/154] fix(intersection): fix bugs (#6050) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 +- planning/behavior_velocity_intersection_module/src/manager.cpp | 2 ++ .../src/scene_intersection.cpp | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 6fc136c512689..4e9eb50f2a462 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -72,7 +72,7 @@ enable: false creep_velocity: 0.8333 peeking_offset: -0.5 - occlusion_required_clearance_distance: 55 + occlusion_required_clearance_distance: 55.0 possible_object_bbox: [1.5, 2.5] ignore_parked_vehicle_speed_threshold: 0.8333 occlusion_detection_hold_time: 1.5 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index e859b15b345b8..c92f16dd7b474 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -152,6 +152,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); ip.occlusion.peeking_offset = getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); ip.occlusion.possible_object_bbox = getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 1bc7bfe04d271..dad0c194b5c9f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1195,7 +1195,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } - const double is_amber_or_red = + const bool is_amber_or_red = (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); auto occlusion_status = From d9571352593c4f73ac361b20ed27ab86b9c39d52 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 12 Jan 2024 00:36:03 +0900 Subject: [PATCH 036/154] feat(mission_planner): check shoulder lanelets for check_goal_footprint (#6053) Signed-off-by: kosuke55 --- .../src/lanelet2_plugins/default_planner.cpp | 6 ++- .../lanelet2_plugins/utility_functions.cpp | 50 +++++++++++++++---- .../lanelet2_plugins/utility_functions.hpp | 3 +- 3 files changed, 47 insertions(+), 12 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 5d1c0c68d8208..e98084204ba78 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -292,7 +292,8 @@ bool DefaultPlanner::check_goal_footprint( lanelet::ConstLanelets lanelets; lanelets.push_back(combined_prev_lanelet); lanelets.push_back(next_lane); - lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets); + lanelet::ConstLanelet combined_lanelets = + combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_); // if next lanelet length longer than vehicle longitudinal offset if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { @@ -351,7 +352,8 @@ bool DefaultPlanner::is_goal_valid( double next_lane_length = 0.0; // combine calculated route lanelets - lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets); + lanelet::ConstLanelet combined_prev_lanelet = + combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_); // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index 690a864fcdacd..e3983b7f3b962 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -54,7 +54,8 @@ void insert_marker_array( a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) +lanelet::ConstLanelet combine_lanelets_with_shoulder( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets) { lanelet::Points3d lefts; lanelet::Points3d rights; @@ -70,17 +71,48 @@ lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) } for (const auto & llt : lanelets) { - if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { - for (const auto & pt : llt.leftBound()) { - lefts.push_back(lanelet::Point3d(pt)); - } + // lambda to check if shoulder lane which share left bound with lanelets exist + const auto find_bound_shared_shoulder = + [&shoulder_lanelets](const auto & lanelet_bound, const auto & get_shoulder_bound) { + return std::find_if( + shoulder_lanelets.begin(), shoulder_lanelets.end(), + [&lanelet_bound, &get_shoulder_bound](const auto & shoulder_llt) { + return lanelet_bound.id() == get_shoulder_bound(shoulder_llt).id(); + }); + }; + + // lambda to add bound to target_bound + const auto add_bound = [](const auto & bound, auto & target_bound) { + std::transform( + bound.begin(), bound.end(), std::back_inserter(target_bound), + [](const auto & pt) { return lanelet::Point3d(pt); }); + }; + + // check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist + const auto left_shared_shoulder_it = find_bound_shared_shoulder( + llt.leftBound(), [](const auto & shoulder_llt) { return shoulder_llt.rightBound(); }); + if (left_shared_shoulder_it != shoulder_lanelets.end()) { + // if exist, add left bound of SHOULDER lanelet to lefts + add_bound(left_shared_shoulder_it->leftBound(), lefts); + } else if ( + // if not exist, add left bound of lanelet to lefts + std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { + add_bound(llt.leftBound(), lefts); } - if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { - for (const auto & pt : llt.rightBound()) { - rights.push_back(lanelet::Point3d(pt)); - } + + // check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist + const auto right_shared_shoulder_it = find_bound_shared_shoulder( + llt.rightBound(), [](const auto & shoulder_llt) { return shoulder_llt.leftBound(); }); + if (right_shared_shoulder_it != shoulder_lanelets.end()) { + // if exist, add right bound of SHOULDER lanelet to rights + add_bound(right_shared_shoulder_it->rightBound(), rights); + } else if ( + // if not exist, add right bound of lanelet to rights + std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { + add_bound(llt.rightBound(), rights); } } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 3ea6237f38501..3287267a00dfe 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -49,7 +49,8 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); -lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets); +lanelet::ConstLanelet combine_lanelets_with_shoulder( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets); std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw); From 068f58986ab5cfb015d951e1bd37bc4843cef82a Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Fri, 12 Jan 2024 05:21:58 +0100 Subject: [PATCH 037/154] build(probabilistic_occupancy_grid_map): move header files to the probabilistic_occupancy_grid_map folder to avoid conflicts (#5839) * build(probabilistic_occupancy_grid_map): move header files to the probabilistic_occupancy_grid_map folder to avoid conflicts Signed-off-by: Esteve Fernandez * style(pre-commit): autofix --------- Signed-off-by: Esteve Fernandez Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../cost_value.hpp | 6 +++--- .../fusion/single_frame_fusion_policy.hpp | 8 ++++---- .../fusion/synchronized_grid_map_fusion_node.hpp | 14 +++++++------- .../laserscan_based_occupancy_grid_map_node.hpp | 12 ++++++------ .../occupancy_grid_map.hpp | 6 +++--- .../occupancy_grid_map_base.hpp | 6 +++--- .../occupancy_grid_map_fixed.hpp | 8 ++++---- .../occupancy_grid_map_projective.hpp | 8 ++++---- .../pointcloud_based_occupancy_grid_map_node.hpp | 12 ++++++------ ...upancy_grid_map_binary_bayes_filter_updater.hpp | 8 ++++---- ...ancy_grid_map_log_odds_bayes_filter_updater.hpp | 12 ++++++------ .../occupancy_grid_map_updater_interface.hpp | 8 ++++---- .../utils/utils.hpp | 8 ++++---- .../src/fusion/single_frame_fusion_policy.cpp | 2 +- .../fusion/synchronized_grid_map_fusion_node.cpp | 6 +++--- .../laserscan_based_occupancy_grid_map_node.cpp | 6 +++--- .../occupancy_grid_map.cpp | 4 ++-- .../occupancy_grid_map_base.cpp | 6 +++--- .../occupancy_grid_map_fixed.cpp | 6 +++--- .../occupancy_grid_map_projective.cpp | 6 +++--- .../pointcloud_based_occupancy_grid_map_node.cpp | 10 +++++----- ...upancy_grid_map_binary_bayes_filter_updater.cpp | 4 ++-- ...ancy_grid_map_log_odds_bayes_filter_updater.cpp | 4 ++-- .../src/utils/utils.cpp | 2 +- .../test/cost_value_test.cpp | 4 ++-- .../test/fusion_policy_test.cpp | 7 ++++--- .../test/test_utils.cpp | 6 +++--- 27 files changed, 95 insertions(+), 94 deletions(-) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/cost_value.hpp (95%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/fusion/single_frame_fusion_policy.hpp (88%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/fusion/synchronized_grid_map_fusion_node.hpp (87%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp (84%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp (92%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp (92%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp (75%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp (78%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp (83%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp (75%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp (71%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/updater/occupancy_grid_map_updater_interface.hpp (78%) rename perception/probabilistic_occupancy_grid_map/include/{ => probabilistic_occupancy_grid_map}/utils/utils.hpp (93%) diff --git a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/include/cost_value.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp index e0f18cedcdff1..880297a1210ed 100644 --- a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp @@ -48,8 +48,8 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef COST_VALUE_HPP_ -#define COST_VALUE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ #include @@ -103,4 +103,4 @@ static const CostTranslationTable cost_translation_table; static const InverseCostTranslationTable inverse_cost_translation_table; } // namespace occupancy_cost_value -#endif // COST_VALUE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp index 13829b4910d96..1f0553878ef5a 100644 --- a/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ -#define FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include #include @@ -63,4 +63,4 @@ unsigned char singleFrameOccupancyFusion( const std::vector & reliability); } // namespace fusion_policy -#endif // FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp similarity index 87% rename from perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp index 6bea5b2a16f72..8f09764688055 100644 --- a/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#include "fusion/single_frame_fusion_policy.hpp" -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -124,4 +124,4 @@ class GridMapFusionNode : public rclcpp::Node } // namespace synchronized_grid_map_fusion -#endif // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 84% rename from perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index 3a0f1f5ea7433..a8adea6e700e5 100644 --- a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -102,4 +102,4 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node } // namespace occupancy_grid_map -#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp similarity index 92% rename from perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp index a9bbc3fda6ab4..d2210cf9c348a 100644 --- a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ -#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ #include #include @@ -91,4 +91,4 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp similarity index 92% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index 8f8e1e503d388..f01b2d74e160b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ #include #include @@ -92,4 +92,4 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp similarity index 75% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp index 5755cd2c889be..298f327d632d7 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" namespace costmap_2d { @@ -44,4 +44,4 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface } // namespace costmap_2d -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp similarity index 78% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp index bc278772e9737..67b51d6184c8c 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" #include @@ -52,4 +52,4 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface } // namespace costmap_2d -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp similarity index 83% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index e34899bb98b0c..93486e0ca56af 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -95,4 +95,4 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node } // namespace occupancy_grid_map -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp similarity index 75% rename from perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp index d8f3cb6556bf2..3a921035ef219 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#define UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -39,4 +39,4 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface } // namespace costmap_2d -#endif // UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp similarity index 71% rename from perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp index 306f8988acab7..f9f100f285d38 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#define UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#include "fusion/single_frame_fusion_policy.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -45,4 +45,4 @@ class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface } // namespace costmap_2d -#endif // UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp similarity index 78% rename from perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp index 9305313ddc2fc..e85edf2245ef3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#define UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include #include @@ -38,4 +38,4 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp index 46bd14b843dae..0950272dac470 100644 --- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS__UTILS_HPP_ -#define UTILS__UTILS_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include #include @@ -119,4 +119,4 @@ bool extractCommonPointCloud( unsigned char getApproximateOccupancyState(const unsigned char & value); } // namespace utils -#endif // UTILS__UTILS_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp index 75e8791a04bca..6ac681eff4916 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fusion/single_frame_fusion_policy.hpp" +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" namespace fusion_policy { diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 6738286ae5592..a88e65e712ac1 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fusion/synchronized_grid_map_fusion_node.hpp" +#include "probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" namespace synchronized_grid_map_fusion { diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index b4505eedddd21..777c180fe1a3a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" +#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp index 59f562cb58f35..3d02be9ca7156 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -49,9 +49,9 @@ * David V. Lu!! *********************************************************************/ -#include "laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index 8652cfa34d96c..3d176e41583a0 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp @@ -49,10 +49,10 @@ * David V. Lu!! *********************************************************************/ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 56aeea30e0773..00b3e42fdf392 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 20a5770e37fdb..64b76a2488e24 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index f6369602b8890..82da92da7f146 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" -#include "cost_value.hpp" -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp index 16de17b1c2e61..40f538a64f6e9 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp index e45f3d0686fbc..f3e306f262bf0 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index b876004d89b0a..8009a503167ea 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp index baf94fabfd2a1..c41c809a92b0c 100644 --- a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "gtest/gtest.h" +#include // Test the CostTranslationTable and InverseCostTranslationTable functions using occupancy_cost_value::cost_translation_table; diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp index 2501a361fba58..6b3dc8a2ebcef 100644 --- a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "cost_value.hpp" -#include "fusion/single_frame_fusion_policy.hpp" -#include "gtest/gtest.h" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" + +#include // Test the log-odds update rule TEST(FusionPolicyTest, TestLogOddsUpdateRule) diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 367dcee807e7b..4bc3dca0a67bd 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +// autoware +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" + #include #include @@ -21,9 +24,6 @@ #include #include -// autoware -#include "utils/utils.hpp" - // create pointcloud function pcl::PointCloud createPCLPointCloudWithIteratedHeight(const size_t width) { From b51759fa17f40a230749217e0113eea2b5d67657 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 12 Jan 2024 13:33:30 +0900 Subject: [PATCH 038/154] feat(crosswalk): change the TTC formula (#6033) * change params * change the TTC formula Signed-off-by: Yuki Takagi --- .../config/crosswalk.param.yaml | 6 +++--- .../src/scene_crosswalk.cpp | 7 +++++-- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 240963309e9a2..273db320f1027 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -16,7 +16,7 @@ # For the case where the crosswalk width is very wide far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). # For the case where the stop position is determined according to the object position. - stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + stop_distance_from_object: 3.0 # [m] the vehicle decelerates to be able to stop in front of object with margin # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false". slow_down: @@ -38,7 +38,7 @@ # params for the pass judge logic against the crosswalk users such as pedestrians or bicycles pass_judge: ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_first_margin_y: [6.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_y: [3.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) @@ -62,7 +62,7 @@ # param for target object filtering object_filtering: - crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + crosswalk_attention_range: 2.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk vehicle_object_cross_angle_threshold: 0.5 # [rad] threshold judging object is crossing walkway as a pedestrian or passing over as a vehicle target_object: unknown: true # [-] whether to look and stop by UNKNOWN objects diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 7175cd0895fc0..acb88a214b0c0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -766,7 +766,6 @@ CollisionPoint CrosswalkModule::createCollisionPoint( const std::optional object_crosswalk_passage_direction) const { constexpr double min_ego_velocity = 1.38; // [m/s] - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto velocity = std::max(planner_param_.min_object_velocity, estimated_velocity); @@ -774,8 +773,12 @@ CollisionPoint CrosswalkModule::createCollisionPoint( CollisionPoint collision_point{}; collision_point.collision_point = nearest_collision_point; collision_point.crosswalk_passage_direction = object_crosswalk_passage_direction; + + // The decision of whether the ego vehicle or the pedestrian goes first is determined by the logic + // for ego_pass_first or yield; even the decision for ego_pass_later does not affect this sense. + // Hence, here, we use the length that would be appropriate for the ego_pass_first judge. collision_point.time_to_collision = - std::max(0.0, dist_ego2cp - planner_param_.stop_distance_from_object - base_link2front) / + std::max(0.0, dist_ego2cp - planner_data_->vehicle_info_.min_longitudinal_offset_m) / std::max(ego_vel.x, min_ego_velocity); collision_point.time_to_vehicle = std::max(0.0, dist_obj2cp) / velocity; From 08a02ba95895f1df13e5147e9f24d74c4f87989f Mon Sep 17 00:00:00 2001 From: Phoebe Wu Date: Fri, 12 Jan 2024 17:05:03 +0800 Subject: [PATCH 039/154] refactor(shape_estimation): rework parameters (#5330) * refactor(shape_estimation): rework parameters Signed-off-by: PhoebeWu21 * style(pre-commit): autofix * refactor(shape_estimation): rework parameters Signed-off-by: PhoebeWu21 * refactor(shape_estimation): rework parameters Signed-off-by: PhoebeWu21 --------- Signed-off-by: PhoebeWu21 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/shape_estimation/CMakeLists.txt | 1 + perception/shape_estimation/README.md | 6 +- .../config/shape_estimation.param.yaml | 7 +++ .../launch/shape_estimation.launch.xml | 10 ++-- .../schema/shape_estimation.schema.json | 56 +++++++++++++++++++ perception/shape_estimation/src/node.cpp | 10 ++-- 6 files changed, 74 insertions(+), 16 deletions(-) create mode 100644 perception/shape_estimation/config/shape_estimation.param.yaml create mode 100644 perception/shape_estimation/schema/shape_estimation.schema.json diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 5ae6002cd7c3b..8eb7a15b5a885 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -64,4 +64,5 @@ rclcpp_components_register_node(shape_estimation_node ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/shape_estimation/README.md b/perception/shape_estimation/README.md index c50d66b546213..b635631381cc3 100644 --- a/perception/shape_estimation/README.md +++ b/perception/shape_estimation/README.md @@ -36,11 +36,7 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull ## Parameters -| Name | Type | Default Value | Description | -| --------------------------- | ---- | ------------- | --------------------------------------------------- | -| `use_corrector` | bool | true | The flag to apply rule-based filter | -| `use_filter` | bool | true | The flag to apply rule-based corrector | -| `use_vehicle_reference_yaw` | bool | true | The flag to use vehicle reference yaw for corrector | +{{ json_to_markdown("perception/shape_estimation/schema/shape_estimation.schema.json") }} ## Assumptions / Known limits diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/shape_estimation/config/shape_estimation.param.yaml new file mode 100644 index 0000000000000..253516fffe0d4 --- /dev/null +++ b/perception/shape_estimation/config/shape_estimation.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + use_corrector: true + use_filter: true + use_vehicle_reference_yaw: false + use_vehicle_reference_shape_size: false + use_boost_bbox_optimizer: false diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index 8e90e3ea57cc0..65d1944417cc0 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -1,18 +1,16 @@ - - + + + - - - - + diff --git a/perception/shape_estimation/schema/shape_estimation.schema.json b/perception/shape_estimation/schema/shape_estimation.schema.json new file mode 100644 index 0000000000000..d81bfa636a923 --- /dev/null +++ b/perception/shape_estimation/schema/shape_estimation.schema.json @@ -0,0 +1,56 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Shape Estimation Node", + "type": "object", + "definitions": { + "shape_estimation": { + "type": "object", + "properties": { + "use_corrector": { + "type": "boolean", + "description": "The flag to apply rule-based corrector.", + "default": "true" + }, + "use_filter": { + "type": "boolean", + "description": "The flag to apply rule-based filter", + "default": "true" + }, + "use_vehicle_reference_yaw": { + "type": "boolean", + "description": "The flag to use vehicle reference yaw for corrector", + "default": "false" + }, + "use_vehicle_reference_shape_size": { + "type": "boolean", + "description": "The flag to use vehicle reference shape size", + "default": "false" + }, + "use_boost_bbox_optimizer": { + "type": "boolean", + "description": "The flag to use boost bbox optimizer", + "default": "false" + } + }, + "required": [ + "use_corrector", + "use_filter", + "use_vehicle_reference_yaw", + "use_vehicle_reference_shape_size", + "use_boost_bbox_optimizer" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/shape_estimation" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 987cf8106c99e..624ca604d8fbf 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -42,11 +42,11 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option "input", rclcpp::QoS{1}, std::bind(&ShapeEstimationNode::callback, this, _1)); pub_ = create_publisher("objects", rclcpp::QoS{1}); - bool use_corrector = declare_parameter("use_corrector", true); - bool use_filter = declare_parameter("use_filter", true); - use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw", true); - use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size", true); - bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer", false); + bool use_corrector = declare_parameter("use_corrector"); + bool use_filter = declare_parameter("use_filter"); + use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw"); + use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size"); + bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer"); RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); From d1a86e644adc635a29f1044c987081d9037906dd Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 12 Jan 2024 18:05:46 +0900 Subject: [PATCH 040/154] feat(landmark_manager): add get_landmarrks func (#6063) Signed-off-by: yamato-ando Co-authored-by: yamato-ando --- .../landmark_manager/landmark_manager.hpp | 2 ++ .../landmark_manager/src/landmark_manager.cpp | 16 ++++++++++++++++ 2 files changed, 18 insertions(+) diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index 7e78ed713dddc..c2b0751d91f9a 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -44,6 +44,8 @@ class LandmarkManager const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, const std::string & target_subtype); + [[nodiscard]] std::vector get_landmarks() const; + [[nodiscard]] visualization_msgs::msg::MarkerArray get_landmarks_as_marker_array_msg() const; [[nodiscard]] geometry_msgs::msg::Pose calculate_new_self_pose( diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index 7ddd66efea0a6..57bfcde461af6 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -83,6 +83,22 @@ void LandmarkManager::parse_landmarks( } } +std::vector LandmarkManager::get_landmarks() const +{ + std::vector landmarks; + + landmark_manager::Landmark landmark; + for (const auto & [landmark_id_str, landmark_poses] : landmarks_map_) { + for (const auto & pose : landmark_poses) { + landmark.id = landmark_id_str; + landmark.pose = pose; + landmarks.push_back(landmark); + } + } + + return landmarks; +} + visualization_msgs::msg::MarkerArray LandmarkManager::get_landmarks_as_marker_array_msg() const { int32_t id = 0; From 39642d22e44faa7b64a00eccd7c5d476d4363ced Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Jan 2024 18:41:54 +0900 Subject: [PATCH 041/154] feat(perception_rviz_plugin): visualize object heading direction on 3d bounding box (#6035) * feat(autoware_auto_perception_rviz_plugin): visualize object heading Signed-off-by: Taekjin LEE * feat(autoware_auto_perception_rviz_plugin): visualize heading only it is available Signed-off-by: Taekjin LEE * feat(autoware_auto_perception_rviz_plugin): 2d visualization implemented Signed-off-by: Taekjin LEE * feat(autoware_auto_perception_rviz_plugin): integrating repeating codes Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 18 +- .../object_polygon_display_base.hpp | 11 +- .../detected_objects_display.cpp | 4 +- .../object_polygon_detail.cpp | 259 ++++++++---------- .../predicted_objects_display.cpp | 2 +- .../tracked_objects_display.cpp | 4 +- 6 files changed, 142 insertions(+), 156 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index b661010c19a4d..deaa6be90b5f6 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -83,13 +83,15 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::Sha get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true); /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param centroid Centroid position of the shape in Object.header.frame_id frame @@ -139,14 +141,26 @@ get_path_confidence_marker_ptr( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( + const double point_list[][3], const int point_pairs[][2], const int & num_pairs, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 223611520b2fc..240cbdc9efc5b 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -170,14 +170,16 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const ClassificationContainerT & labels, const double & line_width) const + const ClassificationContainerT & labels, const double & line_width, + const bool & is_orientation_available) const { const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); if (m_display_type_property->getOptionInt() == 0) { - return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width); + return detail::get_shape_marker_ptr( + shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available); } else if (m_display_type_property->getOptionInt() == 1) { return detail::get_2d_shape_marker_ptr( - shape_msg, centroid, orientation, color_rgba, line_width); + shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available); } else { return std::nullopt; } @@ -187,7 +189,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available); /// \brief Convert given shape msg into a Marker to visualize label name /// \tparam ClassificationContainerT List type with ObjectClassificationMsg diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 5dcdce791c585..db57e0e59a1ce 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -38,7 +38,9 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + get_line_width(), + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index d87541b7840a9..84321559f3541 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -262,7 +262,8 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available) { auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); @@ -271,6 +272,9 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_bounding_box_line_list(shape_msg, marker_ptr->points); + if (is_orientation_available) { + calc_bounding_box_direction_line_list(shape_msg, marker_ptr->points); + } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_cylinder_line_list(shape_msg, marker_ptr->points); @@ -294,7 +298,8 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available) { auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); @@ -303,6 +308,9 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); + if (is_orientation_available) { + calc_2d_bounding_box_bottom_direction_line_list(shape_msg, marker_ptr->points); + } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_cylinder_bottom_line_list(shape_msg, marker_ptr->points); @@ -323,163 +331,120 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( return marker_ptr; } +void calc_line_list_from_points( + const double point_list[][3], const int point_pairs[][2], const int & num_pairs, + std::vector & points) +{ + geometry_msgs::msg::Point point; + for (int i = 0; i < num_pairs; ++i) { + point.x = point_list[point_pairs[i][0]][0]; + point.y = point_list[point_pairs[i][0]][1]; + point.z = point_list[point_pairs[i][0]][2]; + points.push_back(point); + point.x = point_list[point_pairs[i][1]][0]; + point.y = point_list[point_pairs[i][1]][1]; + point.z = point_list[point_pairs[i][1]][2]; + points.push_back(point); + } +} + void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + geometry_msgs::msg::Point point; + + // bounding box corner points + // top and bottom surface, clockwise + const double point_list[8][3] = { + {length_half, width_half, height_half}, {length_half, -width_half, height_half}, + {-length_half, -width_half, height_half}, {-length_half, width_half, height_half}, + {length_half, width_half, -height_half}, {length_half, -width_half, -height_half}, + {-length_half, -width_half, -height_half}, {-length_half, width_half, -height_half}, + }; + const int point_pairs[12][2] = { + {0, 1}, {1, 2}, {2, 3}, {3, 0}, {4, 5}, {5, 6}, {6, 7}, {7, 4}, {0, 4}, {1, 5}, {2, 6}, {3, 7}, + }; + calc_line_list_from_points(point_list, point_pairs, 12, points); +} + +void calc_bounding_box_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + // direction triangle + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + const double triangle_size_half = shape.dimensions.y / 1.4; geometry_msgs::msg::Point point; - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - // up surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - // down surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); + + // triangle-shaped direction indicator + const double point_list[6][3] = { + {length_half, 0, height_half}, + {length_half - triangle_size_half, width_half, height_half}, + {length_half - triangle_size_half, -width_half, height_half}, + {length_half, 0, -height_half}, + {length_half, width_half, height_half}, + {length_half, -width_half, height_half}, + }; + const int point_pairs[5][2] = { + {0, 1}, {1, 2}, {0, 2}, {3, 4}, {3, 5}, + }; + calc_line_list_from_points(point_list, point_pairs, 5, points); } void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; geometry_msgs::msg::Point point; - // down surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); + + // bounding box corner points + // top surface, clockwise + const double point_list[4][3] = { + {length_half, width_half, -height_half}, + {length_half, -width_half, -height_half}, + {-length_half, -width_half, -height_half}, + {-length_half, width_half, -height_half}, + }; + const int point_pairs[4][2] = { + {0, 1}, + {1, 2}, + {2, 3}, + {3, 0}, + }; + calc_line_list_from_points(point_list, point_pairs, 4, points); +} + +void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + const double triangle_size_half = shape.dimensions.y / 1.4; + geometry_msgs::msg::Point point; + + // triangle-shaped direction indicator + const double point_list[6][3] = { + {length_half, 0, -height_half}, + {length_half - triangle_size_half, width_half, -height_half}, + {length_half - triangle_size_half, -width_half, -height_half}, + }; + const int point_pairs[3][2] = { + {0, 1}, + {1, 2}, + {0, 2}, + }; + calc_line_list_from_points(point_list, point_pairs, 3, points); } void calc_cylinder_line_list( diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 438d70f052b4a..aa9eded82a88e 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -77,7 +77,7 @@ std::vector PredictedObjectsDisplay: auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + get_line_width(), true); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 75b094b49a762..fb53dbe1c2b8d 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -59,7 +59,9 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for shape auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, - object.kinematics.pose_with_covariance.pose.orientation, object.classification, line_width); + object.kinematics.pose_with_covariance.pose.orientation, object.classification, line_width, + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; From 0aa39277fab0a9a83e8b1283b910c65f3b98c2eb Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 14 Jan 2024 23:37:39 +0900 Subject: [PATCH 042/154] fix(motion_utils): add guard to calcCurvature (#6070) * fix(motion_utils): add guard to calcCurvature Signed-off-by: kosuke55 * style(pre-commit): autofix --------- Signed-off-by: kosuke55 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/motion_utils/trajectory/trajectory.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index d81f75815ecc6..b9136bc4002e3 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -952,7 +952,10 @@ calcArcLength>( template std::vector calcCurvature(const T & points) { - std::vector curvature_vec(points.size()); + std::vector curvature_vec(points.size(), 0.0); + if (points.size() < 3) { + return curvature_vec; + } for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); From a4c92079cd1fcc80e4a9b78032a2be11ffdbb606 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 15 Jan 2024 09:40:51 +0900 Subject: [PATCH 043/154] feat(geography_utils): add mgrs code in projector (#5990) Signed-off-by: Takayuki Murooka --- common/geography_utils/src/lanelet2_projector.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp index 3e982ac2ccf4d..532606248cd1f 100644 --- a/common/geography_utils/src/lanelet2_projector.cpp +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -34,6 +34,7 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { lanelet::projection::MGRSProjector projector{}; + projector.setMGRSCode(projector_info.mgrs_grid); return std::make_unique(projector); } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { From 5ab18044145d8c7d243015e01657b0d1e37f024c Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 15 Jan 2024 18:10:15 +0900 Subject: [PATCH 044/154] feat(mpc): calculate mpc predicted trajectory in the world coordinate to improve accuracy (#5576) * remake Signed-off-by: Takamasa Horibe * remove maybe_unused in decleration Signed-off-by: Takamasa Horibe * support two predicted trajectories in world and Frenet coordinate Signed-off-by: Takamasa Horibe * fix test Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../include/mpc_lateral_controller/mpc.hpp | 30 +-- .../mpc_lateral_controller.hpp | 2 +- .../mpc_lateral_controller/mpc_trajectory.hpp | 6 + .../mpc_lateral_controller/mpc_utils.hpp | 15 ++ .../vehicle_model_bicycle_dynamics.hpp | 10 + .../vehicle_model_bicycle_kinematics.hpp | 10 + ...icle_model_bicycle_kinematics_no_delay.hpp | 10 + .../vehicle_model/vehicle_model_interface.hpp | 46 +++- .../lateral_controller_defaults.param.yaml | 2 + control/mpc_lateral_controller/src/mpc.cpp | 64 +++--- .../src/mpc_lateral_controller.cpp | 90 ++++---- .../src/mpc_trajectory.cpp | 16 ++ .../mpc_lateral_controller/src/mpc_utils.cpp | 26 +++ .../vehicle_model_bicycle_dynamics.cpp | 44 ++++ .../vehicle_model_bicycle_kinematics.cpp | 81 +++++++ ...icle_model_bicycle_kinematics_no_delay.cpp | 78 +++++++ .../vehicle_model/vehicle_model_interface.cpp | 8 +- .../mpc_lateral_controller/test/test_mpc.cpp | 205 ++++++++++-------- .../param/lateral/mpc.param.yaml | 2 + 19 files changed, 560 insertions(+), 185 deletions(-) diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 186481e92cdc4..2e83c5ab847c4 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -221,6 +221,7 @@ class MPC double m_min_prediction_length = 5.0; // Minimum prediction distance. + rclcpp::Publisher::SharedPtr m_debug_frenet_predicted_trajectory_pub; /** * @brief Get variables for MPC calculation. * @param trajectory The reference trajectory. @@ -333,6 +334,19 @@ class MPC const MPCMatrix & m, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered, const float current_steer, const double predict_dt) const; + /** + * @brief calculate predicted trajectory + * @param mpc_matrix The MPC matrix used in the mpc problem. + * @param x0 initial state used in the mpc problem. + * @param Uex optimized input. + * @param mpc_resampled_ref_traj reference trajectory resampled in the mpc time-step + * @param dt delta time used in the mpc problem. + * @return predicted path + */ + Trajectory calculatePredictedTrajectory( + const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & mpc_resampled_ref_traj, const double dt) const; + /** * @brief Check if the MPC matrix has any invalid values. * @param m The MPC matrix to check. @@ -352,18 +366,6 @@ class MPC : m_param.nominal_weight; } - /** - * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result. - * @param mpc_resampled_reference_trajectory The resampled reference trajectory. - * @param mpc_matrix The MPC matrix used for optimization. - * @param x0_delayed The delayed initial state vector. - * @param Uex The optimized input vector. - * @return The predicted trajectory. - */ - Trajectory calcPredictedTrajectory( - const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix, - const VectorXd & x0_delayed, const VectorXd & Uex) const; - /** * @brief Generate diagnostic data for debugging purposes. * @param reference_trajectory The reference trajectory. @@ -424,8 +426,10 @@ class MPC double ego_nearest_dist_threshold = 3.0; // Threshold for nearest index search based on distance. double ego_nearest_yaw_threshold = M_PI_2; // Threshold for nearest index search based on yaw. + bool m_debug_publish_predicted_trajectory = false; // Flag to publish debug predicted trajectory + //!< Constructor. - MPC() = default; + explicit MPC(rclcpp::Node & node); /** * @brief Calculate control command using the MPC algorithm. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 441101abfc243..92b01247105c6 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -91,7 +91,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; - MPC m_mpc; // MPC object for trajectory following. + std::unique_ptr m_mpc; // MPC object for trajectory following. // Check is mpc output converged bool m_is_mpc_history_filled{false}; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index f77ef72608edd..7e1c7ebdf1348 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -85,6 +85,12 @@ class MPCTrajectory */ size_t size() const; + /** + * @brief get trajectory point at index i + * @return trajectory point at index i + */ + MPCTrajectoryPoint at(const size_t i) const; + /** * @return true if the components sizes are all 0 or are inconsistent */ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 036e6a32a9b83..9062ff1ea34e3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -92,6 +92,13 @@ Trajectory convertToAutowareTrajectory(const MPCTrajectory & input); */ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector & arc_length); +/** + * @brief calculate the arc length of the given trajectory + * @param [in] trajectory trajectory for which to calculate the arc length + * @return total arc length + */ +double calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory); + /** * @brief resample the given trajectory with the given fixed interval * @param [in] input trajectory to resample @@ -194,6 +201,14 @@ double calcStopDistance(const Trajectory & current_trajectory, const int origin) void extendTrajectoryInYawDirection( const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj); +/** + * @brief clip trajectory size by length + * @param [in] trajectory original trajectory + * @param [in] length clip length + * @return clipped trajectory + */ +MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length); + /** * @brief Updates the value of a parameter with the given name. * @tparam T The type of the parameter value. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 29cab73826f31..488a3c7ab8be2 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -103,6 +103,16 @@ class DynamicsBicycleModel : public VehicleModelInterface std::string modelName() override { return "dynamics"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_lf; //!< @brief length from center of mass to front wheel [m] double m_lr; //!< @brief length from center of mass to rear wheel [m] diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index fb113d5e30df7..e2f66e95ab0e3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -91,6 +91,16 @@ class KinematicsBicycleModel : public VehicleModelInterface std::string modelName() override { return "kinematics"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_steer_lim; //!< @brief steering angle limit [rad] double m_steer_tau; //!< @brief steering time constant for 1d-model [s] diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 93d84caa89992..b503ad8eb1d13 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -87,6 +87,16 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface std::string modelName() override { return "kinematics_no_delay"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_steer_lim; //!< @brief steering angle limit [rad] }; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 808ec1b7def2c..68806ff5a00dc 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -15,6 +15,8 @@ #ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#include "mpc_lateral_controller/mpc_trajectory.hpp" + #include #include @@ -55,25 +57,25 @@ class VehicleModelInterface * @brief get state x dimension * @return state dimension */ - int getDimX(); + int getDimX() const; /** * @brief get input u dimension * @return input dimension */ - int getDimU(); + int getDimU() const; /** * @brief get output y dimension * @return output dimension */ - int getDimY(); + int getDimY() const; /** * @brief get wheelbase of the vehicle * @return wheelbase value [m] */ - double getWheelbase(); + double getWheelbase() const; /** * @brief set velocity @@ -109,6 +111,42 @@ class VehicleModelInterface * @brief returns model name e.g. kinematics, dynamics */ virtual std::string modelName() = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in world + * coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in Frenet + * Coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const = 0; }; } // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index 59d3f759c9c2f..182458d532c8a 100644 --- a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -83,3 +83,5 @@ mass_rr: 600.0 cf: 155494.663 cr: 155494.663 + + debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 153c2ae61d076..8fa95f4b3b87b 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -17,6 +17,7 @@ #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" +#include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -28,6 +29,12 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::normalizeRadian; using tier4_autoware_utils::rad2deg; +MPC::MPC(rclcpp::Node & node) +{ + m_debug_frenet_predicted_trajectory_pub = node.create_publisher( + "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1)); +} + bool MPC::calculateMPC( const SteeringReport & current_steer, const Odometry & current_kinematics, AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, @@ -97,9 +104,9 @@ bool MPC::calculateMPC( m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = Uex(0); - // calculate predicted trajectory + /* calculate predicted trajectory */ predicted_trajectory = - calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex); + calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt); // prepare diagnostic message diagnostic = @@ -108,30 +115,6 @@ bool MPC::calculateMPC( return true; } -Trajectory MPC::calcPredictedTrajectory( - const MPCTrajectory & mpc_resampled_ref_trajectory, const MPCMatrix & mpc_matrix, - const VectorXd & x0_delayed, const VectorXd & Uex) const -{ - const VectorXd Xex = mpc_matrix.Aex * x0_delayed + mpc_matrix.Bex * Uex + mpc_matrix.Wex; - MPCTrajectory mpc_predicted_traj; - const auto & traj = mpc_resampled_ref_trajectory; - for (int i = 0; i < m_param.prediction_horizon; ++i) { - const int DIM_X = m_vehicle_model_ptr->getDimX(); - const double lat_error = Xex(i * DIM_X); - const double yaw_error = Xex(i * DIM_X + 1); - const double x = traj.x.at(i) - std::sin(traj.yaw.at(i)) * lat_error; - const double y = traj.y.at(i) + std::cos(traj.yaw.at(i)) * lat_error; - const double z = traj.z.at(i); - const double yaw = traj.yaw.at(i) + yaw_error; - const double vx = traj.vx.at(i); - const double k = traj.k.at(i); - const double smooth_k = traj.smooth_k.at(i); - const double relative_time = traj.relative_time.at(i); - mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time); - } - return MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj); -} - Float32MultiArrayStamped MPC::generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, @@ -794,6 +777,35 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( return steer_rate_limits; } +Trajectory MPC::calculatePredictedTrajectory( + const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + const auto predicted_mpc_trajectory = + m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate( + mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, + dt); + + // do not over the reference trajectory + const auto predicted_length = MPCUtils::calcMPCTrajectoryArcLength(reference_trajectory); + const auto clipped_trajectory = + MPCUtils::clipTrajectoryByLength(predicted_mpc_trajectory, predicted_length); + + const auto predicted_trajectory = MPCUtils::convertToAutowareTrajectory(clipped_trajectory); + + // Publish trajectory in relative coordinate for debug purpose. + if (m_debug_publish_predicted_trajectory) { + const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate( + mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, + dt); + const auto frenet_clipped = MPCUtils::convertToAutowareTrajectory( + MPCUtils::clipTrajectoryByLength(frenet, predicted_length)); + m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped); + } + + return predicted_trajectory; +} + bool MPC::isValid(const MPCMatrix & m) const { if ( diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 3d19104112a71..7eca1481ba921 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -42,7 +42,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto dp_bool = [&](const std::string & s) { return node.declare_parameter(s); }; const auto dp_double = [&](const std::string & s) { return node.declare_parameter(s); }; - m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double(); + m_mpc = std::make_unique(node); + + m_mpc->m_ctrl_period = node.get_parameter("ctrl_period").as_double(); auto & p_filt = m_trajectory_filtering_param; p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing"); @@ -52,10 +54,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) p_filt.traj_resample_dist = dp_double("traj_resample_dist"); p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control"); - m_mpc.m_admissible_position_error = dp_double("admissible_position_error"); - m_mpc.m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); - m_mpc.m_use_steer_prediction = dp_bool("use_steer_prediction"); - m_mpc.m_param.steer_tau = dp_double("vehicle_model_steer_tau"); + m_mpc->m_admissible_position_error = dp_double("admissible_position_error"); + m_mpc->m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); + m_mpc->m_use_steer_prediction = dp_bool("use_steer_prediction"); + m_mpc->m_param.steer_tau = dp_double("vehicle_model_steer_tau"); /* stop state parameters */ m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed"); @@ -70,7 +72,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; constexpr double deg2rad = static_cast(M_PI) / 180.0; - m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad; + m_mpc->m_steer_lim = vehicle_info.max_steer_angle_rad; // steer rate limit depending on curvature const auto steer_rate_lim_dps_list_by_curvature = @@ -78,7 +80,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto curvature_list_for_steer_rate_lim = node.declare_parameter>("curvature_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) { - m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back( + m_mpc->m_steer_rate_lim_map_by_curvature.emplace_back( curvature_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad); } @@ -89,26 +91,26 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto velocity_list_for_steer_rate_lim = node.declare_parameter>("velocity_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) { - m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back( + m_mpc->m_steer_rate_lim_map_by_velocity.emplace_back( velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad); } /* vehicle model setup */ auto vehicle_model_ptr = - createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node); - m_mpc.setVehicleModel(vehicle_model_ptr); + createVehicleModel(wheelbase, m_mpc->m_steer_lim, m_mpc->m_param.steer_tau, node); + m_mpc->setVehicleModel(vehicle_model_ptr); /* QP solver setup */ - m_mpc.setVehicleModel(vehicle_model_ptr); + m_mpc->setVehicleModel(vehicle_model_ptr); auto qpsolver_ptr = createQPSolverInterface(node); - m_mpc.setQPSolver(qpsolver_ptr); + m_mpc->setQPSolver(qpsolver_ptr); /* delay compensation */ { const double delay_tmp = dp_double("input_delay"); - const double delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period); - m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + const double delay_step = std::round(delay_tmp / m_mpc->m_ctrl_period); + m_mpc->m_param.input_delay = delay_step * m_mpc->m_ctrl_period; + m_mpc->m_input_buffer = std::deque(static_cast(delay_step), 0.0); } /* steering offset compensation */ @@ -120,7 +122,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) { const double steering_lpf_cutoff_hz = dp_double("steering_lpf_cutoff_hz"); const double error_deriv_lpf_cutoff_hz = dp_double("error_deriv_lpf_cutoff_hz"); - m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + m_mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); } // ego nearest index search @@ -129,8 +131,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) }; m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold"); m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold"); - m_mpc.ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; - m_mpc.ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; + m_mpc->ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; + m_mpc->ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; + + m_mpc->m_debug_publish_predicted_trajectory = dp_bool("debug_publish_predicted_trajectory"); m_pub_predicted_traj = node.create_publisher("~/output/predicted_trajectory", 1); m_pub_debug_values = @@ -144,10 +148,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_set_param_res = node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1)); - m_mpc.initializeSteeringPredictor(); + m_mpc->initializeSteeringPredictor(); - m_mpc.setLogger(logger_); - m_mpc.setClock(clock_); + m_mpc->setLogger(logger_); + m_mpc->setClock(clock_); } MpcLateralController::~MpcLateralController() @@ -244,7 +248,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_is_ctrl_cmd_prev_initialized = true; } - const bool is_mpc_solved = m_mpc.calculateMPC( + const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); // reset previous MPC result @@ -253,7 +257,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( // After the recovery, the previous value of the optimization may deviate greatly from // the actual steer angle, and it may make the optimization result unstable. if (!is_mpc_solved) { - m_mpc.resetPrevResult(m_current_steering); + m_mpc->resetPrevResult(m_current_steering); } else { setSteeringToHistory(ctrl_cmd); } @@ -284,11 +288,11 @@ trajectory_follower::LateralOutput MpcLateralController::run( if (isStoppedState()) { // Reset input buffer - for (auto & value : m_mpc.m_input_buffer) { + for (auto & value : m_mpc->m_input_buffer) { value = m_ctrl_cmd_prev.steering_tire_angle; } // Use previous command value as previous raw steer command - m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; + m_mpc->m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; return createLateralOutput(m_ctrl_cmd_prev, false); } @@ -323,15 +327,15 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_ m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; - if (!m_mpc.hasVehicleModel()) { + if (!m_mpc->hasVehicleModel()) { info_throttle("MPC does not have a vehicle model"); return false; } - if (!m_mpc.hasQPSolver()) { + if (!m_mpc->hasQPSolver()) { info_throttle("MPC does not have a QP solver"); return false; } - if (m_mpc.m_reference_trajectory.empty()) { + if (m_mpc->m_reference_trajectory.empty()) { info_throttle("trajectory size is zero."); return false; } @@ -354,7 +358,7 @@ void MpcLateralController::setTrajectory( return; } - m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); + m_mpc->setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(m_current_trajectory); @@ -504,12 +508,12 @@ bool MpcLateralController::isMpcConverged() void MpcLateralController::declareMPCparameters(rclcpp::Node & node) { - m_mpc.m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); - m_mpc.m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); + m_mpc->m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); + m_mpc->m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); const auto dp = [&](const auto & param) { return node.declare_parameter(param); }; - auto & nw = m_mpc.m_param.nominal_weight; + auto & nw = m_mpc->m_param.nominal_weight; nw.lat_error = dp("mpc_weight_lat_error"); nw.heading_error = dp("mpc_weight_heading_error"); nw.heading_error_squared_vel = dp("mpc_weight_heading_error_squared_vel"); @@ -521,7 +525,7 @@ void MpcLateralController::declareMPCparameters(rclcpp::Node & node) nw.terminal_lat_error = dp("mpc_weight_terminal_lat_error"); nw.terminal_heading_error = dp("mpc_weight_terminal_heading_error"); - auto & lcw = m_mpc.m_param.low_curvature_weight; + auto & lcw = m_mpc->m_param.low_curvature_weight; lcw.lat_error = dp("mpc_low_curvature_weight_lat_error"); lcw.heading_error = dp("mpc_low_curvature_weight_heading_error"); lcw.heading_error_squared_vel = dp("mpc_low_curvature_weight_heading_error_squared_vel"); @@ -530,12 +534,12 @@ void MpcLateralController::declareMPCparameters(rclcpp::Node & node) lcw.lat_jerk = dp("mpc_low_curvature_weight_lat_jerk"); lcw.steer_rate = dp("mpc_low_curvature_weight_steer_rate"); lcw.steer_acc = dp("mpc_low_curvature_weight_steer_acc"); - m_mpc.m_param.low_curvature_thresh_curvature = dp("mpc_low_curvature_thresh_curvature"); + m_mpc->m_param.low_curvature_thresh_curvature = dp("mpc_low_curvature_thresh_curvature"); - m_mpc.m_param.zero_ff_steer_deg = dp("mpc_zero_ff_steer_deg"); - m_mpc.m_param.acceleration_limit = dp("mpc_acceleration_limit"); - m_mpc.m_param.velocity_time_constant = dp("mpc_velocity_time_constant"); - m_mpc.m_param.min_prediction_length = dp("mpc_min_prediction_length"); + m_mpc->m_param.zero_ff_steer_deg = dp("mpc_zero_ff_steer_deg"); + m_mpc->m_param.acceleration_limit = dp("mpc_acceleration_limit"); + m_mpc->m_param.velocity_time_constant = dp("mpc_velocity_time_constant"); + m_mpc->m_param.min_prediction_length = dp("mpc_min_prediction_length"); } rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( @@ -546,7 +550,7 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( result.reason = "success"; // strong exception safety wrt MPCParam - MPCParam param = m_mpc.m_param; + MPCParam param = m_mpc->m_param; auto & nw = param.nominal_weight; auto & lcw = param.low_curvature_weight; @@ -587,15 +591,15 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( // initialize input buffer update_param(parameters, "input_delay", param.input_delay); - const double delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period); - const double delay = delay_step * m_mpc.m_ctrl_period; + const double delay_step = std::round(param.input_delay / m_mpc->m_ctrl_period); + const double delay = delay_step * m_mpc->m_ctrl_period; if (param.input_delay != delay) { param.input_delay = delay; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + m_mpc->m_input_buffer = std::deque(static_cast(delay_step), 0.0); } // transaction succeeds, now assign values - m_mpc.m_param = param; + m_mpc->m_param = param; } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/mpc_lateral_controller/src/mpc_trajectory.cpp index 3c799722494b9..b7fed942375e8 100644 --- a/control/mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/mpc_lateral_controller/src/mpc_trajectory.cpp @@ -58,6 +58,22 @@ MPCTrajectoryPoint MPCTrajectory::back() return p; } +MPCTrajectoryPoint MPCTrajectory::at(const size_t i) const +{ + MPCTrajectoryPoint p; + + p.x = x.at(i); + p.y = y.at(i); + p.z = z.at(i); + p.yaw = yaw.at(i); + p.vx = vx.at(i); + p.k = k.at(i); + p.smooth_k = smooth_k.at(i); + p.relative_time = relative_time.at(i); + + return p; +} + void MPCTrajectory::clear() { x.clear(); diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index d3fc5fba0b014..01a81d9015b47 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -89,6 +89,15 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, const double ego_offset_to_segment) @@ -458,5 +467,22 @@ void extendTrajectoryInYawDirection( } } +MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length) +{ + MPCTrajectory clipped_trajectory; + clipped_trajectory.push_back(trajectory.at(0)); + + double current_length = 0.0; + for (size_t i = 1; i < trajectory.size(); ++i) { + current_length += calcDistance3d(trajectory, i, i - 1); + if (current_length > length) { + break; + } + clipped_trajectory.push_back(trajectory.at(i)); + } + + return clipped_trajectory; +} + } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 576907e79da4b..2467b1f0c2311 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -86,4 +86,48 @@ void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase); u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature; } + +MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + RCLCPP_ERROR( + rclcpp::get_logger("control.trajectory_follower.lateral_controller"), + "Predicted trajectory calculation in world coordinate is not supported in dynamic model. " + "Calculate in the Frenet coordinate instead."); + return calculatePredictedTrajectoryInFrenetCoordinate( + a_d, b_d, c_d, w_d, x0, Uex, reference_trajectory, dt); +} + +MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // state = [e, de, th, dth] + // e : lateral error + // de : derivative of lateral error + // th : heading angle error + // dth : derivative of heading angle error + // steer : steering angle (input) + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 2); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 35bef667125c5..cd52dd4f79314 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -68,4 +68,85 @@ void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( + [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + // Calculate predicted state in world coordinate since there is modeling errors in Frenet + // Relative coordinate x = [lat_err, yaw_err, steer] + // World coordinate x = [x, y, yaw, steer] + + const auto & t = reference_trajectory; + + // create initial state in the world coordinate + Eigen::VectorXd state_w = [&]() { + Eigen::VectorXd state = Eigen::VectorXd::Zero(4); + const auto lateral_error_0 = x0(0); + const auto yaw_error_0 = x0(1); + state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw + state(3, 0) = x0(2); // steering + return state; + }(); + + // update state in the world coordinate + const auto updateState = [&]( + const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, + const double dt, const double velocity) { + const auto yaw = state_w(2); + const auto steer = state_w(3); + const auto desired_steer = input(0); + + Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4); + dstate(0) = velocity * std::cos(yaw); + dstate(1) = velocity * std::sin(yaw); + dstate(2) = velocity * std::tan(steer) / m_wheelbase; + dstate(3) = -(steer - desired_steer) / m_steer_tau; + + // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation + // in Eigen. + const Eigen::VectorXd next_state = state_w + dstate * dt; + return next_state; + }; + + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_U = getDimU(); + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + mpc_predicted_trajectory.push_back( + state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), + t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // Relative coordinate x = [lat_err, yaw_err, steer] + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 1); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 1536beba9cd00..8f4510510aca9 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -59,4 +59,82 @@ void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } + +MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInWorldCoordinate( + [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + // Calculate predicted state in world coordinate since there is modeling errors in Frenet + // Relative coordinate x = [lat_err, yaw_err] + // World coordinate x = [x, y, yaw] + + const auto & t = reference_trajectory; + + // create initial state in the world coordinate + Eigen::VectorXd state_w = [&]() { + Eigen::VectorXd state = Eigen::VectorXd::Zero(3); + const auto lateral_error_0 = x0(0); + const auto yaw_error_0 = x0(1); + state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw + return state; + }(); + + // update state in the world coordinate + const auto updateState = [&]( + const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, + const double dt, const double velocity) { + const auto yaw = state_w(2); + const auto steer = input(0); + + Eigen::VectorXd dstate = Eigen::VectorXd::Zero(3); + dstate(0) = velocity * std::cos(yaw); + dstate(1) = velocity * std::sin(yaw); + dstate(2) = velocity * std::tan(steer) / m_wheelbase; + + // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation + // in Eigen. + const Eigen::VectorXd next_state = state_w + dstate * dt; + return next_state; + }; + + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_U = getDimU(); + for (size_t i = 0; i < t.size(); ++i) { + state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + mpc_predicted_trajectory.push_back( + state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), + t.relative_time.at(i)); + } + + return mpc_predicted_trajectory; +} + +MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // Relative coordinate x = [lat_err, yaw_err] + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 1); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index 97e3f2742f546..f54a5e325bead 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -20,19 +20,19 @@ VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, do : m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) { } -int VehicleModelInterface::getDimX() +int VehicleModelInterface::getDimX() const { return m_dim_x; } -int VehicleModelInterface::getDimU() +int VehicleModelInterface::getDimU() const { return m_dim_u; } -int VehicleModelInterface::getDimY() +int VehicleModelInterface::getDimY() const { return m_dim_y; } -double VehicleModelInterface::getWheelbase() +double VehicleModelInterface::getWheelbase() const { return m_wheelbase; } diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index dade035daf26c..ba145b5dd146c 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -19,6 +19,7 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "rclcpp/rclcpp.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -48,6 +49,22 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; +TrajectoryPoint makePoint(const double x, const double y, const float vx) +{ + TrajectoryPoint p; + p.pose.position.x = x; + p.pose.position.y = y; + p.longitudinal_velocity_mps = vx; + return p; +} + +nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) +{ + nav_msgs::msg::Odometry odometry; + odometry.pose.pose = pose; + odometry.twist.twist.linear.x = velocity; + return odometry; +} class MPCTest : public ::testing::Test { protected: @@ -86,16 +103,7 @@ class MPCTest : public ::testing::Test TrajectoryFilteringParam trajectory_param; - TrajectoryPoint makePoint(const double x, const double y, const float vx) - { - TrajectoryPoint p; - p.pose.position.x = x; - p.pose.position.y = y; - p.longitudinal_velocity_mps = vx; - return p; - } - - void SetUp() override + void initParam() { param.prediction_horizon = 50; param.prediction_dt = 0.1; @@ -168,259 +176,268 @@ class MPCTest : public ::testing::Test mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); } - nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) + void SetUp() override { - nav_msgs::msg::Odometry odometry; - odometry.pose.pose = pose; - odometry.twist.twist.linear.x = velocity; - return odometry; + rclcpp::init(0, nullptr); + initParam(); } + + void TearDown() override { rclcpp::shutdown(); } }; // class MPCTest /* cppcheck-suppress syntaxError */ TEST_F(MPCTest, InitializeAndCalculate) { - MPC mpc; - EXPECT_FALSE(mpc.hasVehicleModel()); - EXPECT_FALSE(mpc.hasQPSolver()); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + EXPECT_FALSE(mpc->hasVehicleModel()); + EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, InitializeAndCalculateRightTurn) { - MPC mpc; - EXPECT_FALSE(mpc.hasVehicleModel()); - EXPECT_FALSE(mpc.hasQPSolver()); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + EXPECT_FALSE(mpc->hasVehicleModel()); + EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, OsqpCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(logger); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - EXPECT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, OsqpCalculateRightTurn) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(logger); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, KinematicsNoDelayCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init filters - mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Init trajectory const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init filters - mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, DynamicCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, MultiSolveWithBuffer) { - MPC mpc; + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); + mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); + mpc->setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); - mpc.m_input_buffer = {0.0, 0.0, 0.0}; + mpc->m_input_buffer = {0.0, 0.0, 0.0}; // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); } TEST_F(MPCTest, FailureCases) { - MPC mpc; + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); + mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); + mpc->setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); // Calculate MPC with a pose too far from the trajectory Pose pose_far; @@ -430,10 +447,10 @@ TEST_F(MPCTest, FailureCases) Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_far, default_velocity); - EXPECT_FALSE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); // Calculate MPC with a fast velocity to make the prediction go further than the reference path - EXPECT_FALSE(mpc.calculateMPC( + EXPECT_FALSE(mpc->calculateMPC( neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag)); } } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/trajectory_follower_node/param/lateral/mpc.param.yaml b/control/trajectory_follower_node/param/lateral/mpc.param.yaml index 4222082d40deb..9998b6aadf656 100644 --- a/control/trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/trajectory_follower_node/param/lateral/mpc.param.yaml @@ -74,3 +74,5 @@ update_steer_threshold: 0.035 average_num: 1000 steering_offset_limit: 0.02 + + debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate From b60975f8b554aa485e7b11fe2cfd8c317ce9cdea Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Mon, 15 Jan 2024 11:09:27 +0100 Subject: [PATCH 045/154] fix(autoware_auto_perception_rviz_plugin): move headers to a separate directory (#5921) * fix(autoware_auto_perception_rviz_plugin): move headers to a separate directory Signed-off-by: Esteve Fernandez * style(pre-commit): autofix * fix(autoware_auto_perception_rviz_plugin): fix include Signed-off-by: Esteve Fernandez * style(pre-commit): autofix * fix(autoware_auto_perception_rviz_plugin): fix header paths in CMakeLists.txt Signed-off-by: Esteve Fernandez --------- Signed-off-by: Esteve Fernandez Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 14 +++++++------- .../common/color_alpha_property.hpp | 9 +++++---- .../object_detection/detected_objects_display.hpp | 8 ++++---- .../object_detection/object_polygon_detail.hpp | 9 +++++---- .../object_polygon_display_base.hpp | 12 ++++++------ .../object_detection/predicted_objects_display.hpp | 8 ++++---- .../object_detection/tracked_objects_display.hpp | 8 ++++---- .../visibility_control.hpp | 6 +++--- .../src/common/color_alpha_property.cpp | 2 +- .../object_detection/detected_objects_display.cpp | 2 +- .../src/object_detection/object_polygon_detail.cpp | 3 ++- .../object_detection/predicted_objects_display.cpp | 2 +- .../object_detection/tracked_objects_display.cpp | 2 +- 13 files changed, 44 insertions(+), 41 deletions(-) rename common/autoware_auto_perception_rviz_plugin/include/{ => autoware_auto_perception_rviz_plugin}/common/color_alpha_property.hpp (84%) rename common/autoware_auto_perception_rviz_plugin/include/{ => autoware_auto_perception_rviz_plugin}/object_detection/detected_objects_display.hpp (76%) rename common/autoware_auto_perception_rviz_plugin/include/{ => autoware_auto_perception_rviz_plugin}/object_detection/object_polygon_detail.hpp (97%) rename common/autoware_auto_perception_rviz_plugin/include/{ => autoware_auto_perception_rviz_plugin}/object_detection/object_polygon_display_base.hpp (97%) rename common/autoware_auto_perception_rviz_plugin/include/{ => autoware_auto_perception_rviz_plugin}/object_detection/predicted_objects_display.hpp (92%) rename common/autoware_auto_perception_rviz_plugin/include/{ => autoware_auto_perception_rviz_plugin}/object_detection/tracked_objects_display.hpp (89%) rename common/autoware_auto_perception_rviz_plugin/include/{ => autoware_auto_perception_rviz_plugin}/visibility_control.hpp (89%) diff --git a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt index dd5f88d4caae8..8d0469e78c3ac 100644 --- a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt +++ b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt @@ -13,18 +13,18 @@ set(OD_PLUGIN_LIB_SRC ) set(OD_PLUGIN_LIB_HEADERS - include/visibility_control.hpp + include/autoware_auto_perception_rviz_plugin/visibility_control.hpp ) set(OD_PLUGIN_LIB_HEADERS_TO_WRAP - include/object_detection/detected_objects_display.hpp - include/object_detection/tracked_objects_display.hpp - include/object_detection/predicted_objects_display.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp ) set(COMMON_HEADERS - include/common/color_alpha_property.hpp - include/object_detection/object_polygon_detail.hpp - include/object_detection/object_polygon_display_base.hpp + include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp ) set(COMMON_SRC diff --git a/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp similarity index 84% rename from common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp index 8c43192a26bd6..10dc46e55ec70 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp @@ -11,13 +11,14 @@ // 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 COMMON__COLOR_ALPHA_PROPERTY_HPP_ -#define COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ + +#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" #include #include #include -#include #include @@ -55,4 +56,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty } // namespace rviz_plugins } // namespace autoware -#endif // COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp similarity index 76% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp index 67dac25c796bb..97479fb68ca9b 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp @@ -11,10 +11,10 @@ // 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 OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#define OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -43,4 +43,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp similarity index 97% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index deaa6be90b5f6..b3ee4a33b28c9 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. /// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class -#ifndef OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ -#define OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ + +#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" #include #include -#include #include #include @@ -252,4 +253,4 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp similarity index 97% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 240cbdc9efc5b..a0a21406ba416 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -11,19 +11,19 @@ // 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 OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#include "common/color_alpha_property.hpp" +#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" +#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" -#include #include #include #include #include #include #include -#include #include #include @@ -449,4 +449,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp similarity index 92% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp index 2896286970217..775c18db6ba5c 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp @@ -11,10 +11,10 @@ // 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 OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#define OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -153,4 +153,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp similarity index 89% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp index a4cf8a703dff1..4e86a5ee93fd8 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp @@ -11,10 +11,10 @@ // 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 OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#define OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -114,4 +114,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp similarity index 89% rename from common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp index bce7351572375..47cb8383fdada 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef VISIBILITY_CONTROL_HPP_ -#define VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ #if defined(__WIN32) #if defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ @@ -40,4 +40,4 @@ #error "Unsupported Build Configuration" #endif // defined(_WINDOWS) -#endif // VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp index 57a42c95f9d34..b3e542a02243b 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "common/color_alpha_property.hpp" +#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index db57e0e59a1ce..932ea87ccf5ad 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 84321559f3541..97d7212d4f0ce 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License.. +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" + #include #include -#include #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index aa9eded82a88e..a9074f9e1bc1a 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp" #include #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index fb53dbe1c2b8d..54dfb9edf889a 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp" #include From eeed8466ab33d316c9e0c2761ccec39f90469e64 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 15 Jan 2024 19:59:36 +0900 Subject: [PATCH 046/154] perf(bpp): reduce computational cost (#6054) * pref(avoidance): don't use boost union_ Signed-off-by: satoshi-ota * perf(avoidance): reduce frequency to call calcSignedArcLength Signed-off-by: satoshi-ota * perf(turn_signal): reduce frequency to call calcSignedArcLength Signed-off-by: satoshi-ota * perf(static_drivable_area_expansion): don't use calcDistance2d Signed-off-by: satoshi-ota * fix(static_drivable_area_expansion): rename and fix return value consistency Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../data_structs.hpp | 2 +- .../behavior_path_avoidance_module/scene.hpp | 13 ++-- .../src/debug.cpp | 7 ++- .../src/scene.cpp | 62 +++++++++++-------- .../src/utils.cpp | 45 +++++--------- .../src/turn_signal_decider.cpp | 18 +++--- .../static_drivable_area.cpp | 17 +++-- 7 files changed, 84 insertions(+), 80 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 1d1494b7719c3..86e1608501831 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -588,7 +588,7 @@ struct ShiftLineData */ struct DebugData { - geometry_msgs::msg::Polygon detection_area; + std::vector detection_areas; lanelet::ConstLineStrings3d bounds; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 889d48c481b6f..0bb480bfa26b1 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -112,13 +112,12 @@ class AvoidanceModule : public SceneModuleInterface */ void updateRegisteredRTCStatus(const PathWithLaneId & path) { - const Point ego_position = planner_data_->self_odometry->pose.pose.position; + const auto ego_idx = planner_data_->findEgoIndex(path.points); for (const auto & left_shift : left_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position); - const double finish_distance = - calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); + calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position); + const double finish_distance = start_distance + left_shift.relative_longitudinal; rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -130,9 +129,8 @@ class AvoidanceModule : public SceneModuleInterface for (const auto & right_shift : right_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position); - const double finish_distance = - calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); + calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position); + const double finish_distance = start_distance + right_shift.relative_longitudinal; rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -399,6 +397,7 @@ class AvoidanceModule : public SceneModuleInterface UUID uuid; Pose start_pose; Pose finish_pose; + double relative_longitudinal{0.0}; }; using RegisteredShiftLineArray = std::vector; diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index ee97efe37d440..f088b9228d964 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -608,10 +608,15 @@ MarkerArray createDebugMarkerArray( addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); } + // detection area + size_t i = 0; + for (const auto & detection_area : debug.detection_areas) { + add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + } + // misc { add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); add(laneletsAsTriangleMarkerArray( "drivable_lanes", transformToLanelets(data.drivable_lanes), diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 61941eeee6f00..c94243451ed2b 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -703,18 +703,6 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const bool limit_to_max_velocity = false; - const auto ego_predicted_path_params = - std::make_shared( - parameters_->ego_predicted_path_params); - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); - const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, - true, limit_to_max_velocity); - const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, - false, limit_to_max_velocity); - const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { @@ -741,6 +729,22 @@ bool AvoidanceModule::isSafePath( const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug); + if (safety_check_target_objects.empty()) { + return true; + } + + const bool limit_to_max_velocity = false; + const auto ego_predicted_path_params = + std::make_shared( + parameters_->ego_predicted_path_params); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); + const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + true, limit_to_max_velocity); + const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + false, limit_to_max_velocity); + for (const auto & object : safety_check_target_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); @@ -793,15 +797,16 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; - for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max( - max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition())); + auto lines = path_shifter_.getShiftLines(); + if (lines.empty()) { + return max_dist; } - for (const auto & sp : generator_.getRawRegisteredShiftLine()) { - max_dist = std::max( - max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition())); - } - return max_dist; + std::sort(lines.begin(), lines.end(), [](const auto & a, const auto & b) { + return a.start_idx < b.start_idx; + }); + return std::max( + max_dist, + calcSignedArcLength(previous_path.points, lines.front().start.position, getEgoPosition())); }(); const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. @@ -1029,11 +1034,14 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) const auto sl = helper_->getMainShiftLine(shift_lines); const auto sl_front = shift_lines.front(); const auto sl_back = shift_lines.back(); + const auto relative_longitudinal = sl_back.end_longitudinal - sl_front.start_longitudinal; if (helper_->getRelativeShiftToPath(sl) > 0.0) { - left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end}); + left_shift_array_.push_back( + {uuid_map_.at("left"), sl_front.start, sl_back.end, relative_longitudinal}); } else if (helper_->getRelativeShiftToPath(sl) < 0.0) { - right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end}); + right_shift_array_.push_back( + {uuid_map_.at("right"), sl_front.start, sl_back.end, relative_longitudinal}); } uuid_map_.at("left") = generateUUID(); @@ -1150,15 +1158,19 @@ bool AvoidanceModule::isValidShiftLine( const size_t start_idx = shift_lines.front().start_idx; const size_t end_idx = shift_lines.back().end_idx; + const auto path = shifter_for_validate.getReferencePath(); + const auto left_bound = lanelet::utils::to2D(avoid_data_.left_bound); + const auto right_bound = lanelet::utils::to2D(avoid_data_.right_bound); for (size_t i = start_idx; i <= end_idx; ++i) { - const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i)); + const auto p = getPoint(path.points.at(i)); lanelet::BasicPoint2d basic_point{p.x, p.y}; const auto shift_length = proposed_shift_path.shift_length.at(i); - const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound; const auto THRESHOLD = minimum_distance + std::abs(shift_length); - if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) { + if ( + boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < + THRESHOLD) { RCLCPP_DEBUG_THROTTLE( getLogger(), *clock_, 1000, "following latest new shift line may cause deviation from drivable area."); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index f72f8279d4351..4c09962840907 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1892,11 +1892,12 @@ std::pair separateObjectsByPath( max_offset = std::max(max_offset, offset); } + const double MARGIN = is_running ? 1.0 : 0.0; // [m] const auto detection_area = - createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); + createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset + MARGIN); const auto ego_idx = planner_data->findEgoIndex(path.points); - Polygon2d attention_area; + std::vector detection_areas; for (size_t i = 0; i < path.points.size() - 1; ++i) { const auto & p_ego_front = path.points.at(i).point.pose; const auto & p_ego_back = path.points.at(i + 1).point.pose; @@ -1906,41 +1907,27 @@ std::pair separateObjectsByPath( break; } - const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, detection_area); - - std::vector unions; - boost::geometry::union_(attention_area, ego_one_step_polygon, unions); - if (!unions.empty()) { - attention_area = unions.front(); - boost::geometry::correct(attention_area); - } + detection_areas.push_back(createOneStepPolygon(p_ego_front, p_ego_back, detection_area)); } - // expand detection area width only when the module is running. - if (is_running) { - constexpr int PER_CIRCLE = 36; - constexpr double MARGIN = 1.0; // [m] - boost::geometry::strategy::buffer::distance_symmetric distance_strategy(MARGIN); - boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::model::multi_polygon result; - // Create the buffer of a multi polygon - boost::geometry::buffer( - attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy, - circle_strategy); - if (!result.empty()) { - attention_area = result.front(); + std::for_each(detection_areas.begin(), detection_areas.end(), [&](const auto & detection_area) { + debug.detection_areas.push_back(toMsg(detection_area, data.reference_pose.position.z)); + }); + + const auto within_detection_area = [&](const auto & obj_polygon) { + for (const auto & detection_area : detection_areas) { + if (!boost::geometry::disjoint(obj_polygon, detection_area)) { + return true; + } } - } - debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); + return false; + }; const auto objects = planner_data->dynamic_object->objects; std::for_each(objects.begin(), objects.end(), [&](const auto & object) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - if (boost::geometry::disjoint(obj_polygon, attention_area)) { + if (!within_detection_area(obj_polygon)) { other_objects.objects.push_back(object); } else { target_objects.objects.push_back(object); diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 44358dfa84e4e..6ddc0df4eebf4 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -415,15 +415,7 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const double dist_to_original_desired_start = get_distance(original_desired_start_point) - base_link2front_; - const double dist_to_original_desired_end = get_distance(original_desired_end_point); - const double dist_to_original_required_start = - get_distance(original_required_start_point) - base_link2front_; - const double dist_to_original_required_end = get_distance(original_required_end_point); const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_; - const double dist_to_new_desired_end = get_distance(new_desired_end_point); - const double dist_to_new_required_start = - get_distance(new_required_start_point) - base_link2front_; - const double dist_to_new_required_end = get_distance(new_required_end_point); // If we still do not reach the desired front point we ignore it if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) { @@ -435,6 +427,9 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( return original_signal; } + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_new_desired_end = get_distance(new_desired_end_point); + // If we already passed the desired end point, return the other signal if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) { TurnSignalInfo empty_signal_info; @@ -445,6 +440,13 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( return original_signal; } + const double dist_to_original_required_start = + get_distance(original_required_start_point) - base_link2front_; + const double dist_to_original_required_end = get_distance(original_required_end_point); + const double dist_to_new_required_start = + get_distance(new_required_start_point) - base_link2front_; + const double dist_to_new_required_end = get_distance(new_required_end_point); + if (dist_to_original_desired_start <= dist_to_new_desired_start) { const auto enable_prior = use_prior_turn_signal( dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start, diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 44c3025a8c5d5..03fae6864fe50 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -208,26 +208,25 @@ std::optional> intersectBound( return std::nullopt; } -double calcDistanceFromPointToSegment( +double calcSquaredDistanceFromPointToSegment( const geometry_msgs::msg::Point & segment_start_point, const geometry_msgs::msg::Point & segment_end_point, const geometry_msgs::msg::Point & target_point) { + using tier4_autoware_utils::calcSquaredDistance2d; + const auto & a = segment_start_point; const auto & b = segment_end_point; const auto & p = target_point; const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y); - const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b); + const double squared_segment_length = calcSquaredDistance2d(a, b); if (0 <= dot_val && dot_val <= squared_segment_length) { - const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x)); - const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); - return numerator / denominator; + return calcSquaredDistance2d(p, a) - dot_val * dot_val / squared_segment_length; } // target_point is outside the segment. - return std::min( - tier4_autoware_utils::calcDistance2d(a, p), tier4_autoware_utils::calcDistance2d(b, p)); + return std::min(calcSquaredDistance2d(a, p), calcSquaredDistance2d(b, p)); } PolygonPoint transformBoundFrenetCoordinate( @@ -238,8 +237,8 @@ PolygonPoint transformBoundFrenetCoordinate( // find wrong nearest index. std::vector dist_to_bound_segment_vec; for (size_t i = 0; i < bound_points.size() - 1; ++i) { - const double dist_to_bound_segment = - calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point); + const double dist_to_bound_segment = calcSquaredDistanceFromPointToSegment( + bound_points.at(i), bound_points.at(i + 1), target_point); dist_to_bound_segment_vec.push_back(dist_to_bound_segment); } From 6e550fc94db67b8362f3f6d57aaee103c1bd5bf6 Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Tue, 16 Jan 2024 00:09:14 +0900 Subject: [PATCH 047/154] fix(image_projection_based_fusion): re-organize the parameters for image projection fusion (#6075) re-organize the parameters for image projection fusion Signed-off-by: Shunsuke Miura --- .../camera_lidar_fusion_based_detection.launch.xml | 9 +++------ ...a_lidar_radar_fusion_based_detection.launch.xml | 6 +++--- .../detection/detection.launch.xml | 11 +++-------- .../launch/perception.launch.xml | 14 ++++++-------- 4 files changed, 15 insertions(+), 25 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 227aac50d6d90..10d1ac034d457 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -12,9 +12,6 @@ - - - @@ -113,6 +110,7 @@ + @@ -167,9 +165,7 @@ - - - + @@ -305,6 +301,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 65caad54876ff..c227298c932d9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -139,6 +139,7 @@ + @@ -193,9 +194,7 @@ - - - + @@ -344,6 +343,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 0649d8c5d5116..68d5ea944e6cb 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -34,11 +34,6 @@ - - - - - @@ -65,14 +60,14 @@ - - - + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 1b5753fb4d5c4..87616b9ccb122 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -10,6 +10,9 @@ + + + @@ -99,11 +102,6 @@ description="options: `ped_traffic_light_classifier_mobilenetv2_batch_*` or `ped_traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6" /> - - - - - @@ -177,6 +175,9 @@ + + + @@ -190,9 +191,6 @@ - - - From 7b491e064663fc3a88843d4d848667f637d1e751 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 16 Jan 2024 10:09:47 +0900 Subject: [PATCH 048/154] fix(ekf_localizer): fixed timer in ekf_localizer (#6066) Fixed timer in ekf_localizer Signed-off-by: Shintaro Sakoda --- .../include/ekf_localizer/ekf_localizer.hpp | 4 +- .../ekf_localizer/src/ekf_localizer.cpp | 46 ++++++++++--------- 2 files changed, 26 insertions(+), 24 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index ba18b7dadd599..6925e8b63c66b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -199,7 +199,7 @@ class EKFLocalizer : public rclcpp::Node /** * @brief update predict frequency */ - void updatePredictFrequency(); + void updatePredictFrequency(const rclcpp::Time & current_time); /** * @brief get transform from frame_id @@ -219,7 +219,7 @@ class EKFLocalizer : public rclcpp::Node /** * @brief publish diagnostics message */ - void publishDiagnostics(); + void publishDiagnostics(const rclcpp::Time & current_time); /** * @brief update simple1DFilter diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index b3ee3665cf45e..f77481d45a534 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -104,14 +104,14 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti /* * updatePredictFrequency */ -void EKFLocalizer::updatePredictFrequency() +void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) { if (last_predict_time_) { - if (get_clock()->now() < *last_predict_time_) { + if (current_time < *last_predict_time_) { warning_->warn("Detected jump back in time"); } else { /* Measure dt */ - ekf_dt_ = (get_clock()->now() - *last_predict_time_).seconds(); + ekf_dt_ = (current_time - *last_predict_time_).seconds(); DEBUG_INFO( get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_); @@ -133,7 +133,7 @@ void EKFLocalizer::updatePredictFrequency() proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0); } } - last_predict_time_ = std::make_shared(get_clock()->now()); + last_predict_time_ = std::make_shared(current_time); } /* @@ -141,17 +141,19 @@ void EKFLocalizer::updatePredictFrequency() */ void EKFLocalizer::timerCallback() { + const rclcpp::Time current_time = this->now(); + if (!is_activated_) { warning_->warnThrottle( "The node is not activated. Provide initial pose to pose_initializer", 2000); - publishDiagnostics(); + publishDiagnostics(current_time); return; } DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ - updatePredictFrequency(); + updatePredictFrequency(current_time); /* predict model in EKF */ stop_watch_.tic(); @@ -175,7 +177,7 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = this->now(); + const auto t_curr = current_time; const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); @@ -210,7 +212,7 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = this->now(); + const auto t_curr = current_time; const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); @@ -228,15 +230,15 @@ void EKFLocalizer::timerCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); const geometry_msgs::msg::PoseStamped current_ekf_pose = - ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false); + ekf_module_->getCurrentPose(current_time, z, roll, pitch, false); const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = - ekf_module_->getCurrentPose(this->now(), z, roll, pitch, true); + ekf_module_->getCurrentPose(current_time, z, roll, pitch, true); const geometry_msgs::msg::TwistStamped current_ekf_twist = - ekf_module_->getCurrentTwist(this->now()); + ekf_module_->getCurrentTwist(current_time); /* publish ekf result */ publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); - publishDiagnostics(); + publishDiagnostics(current_time); } /* @@ -256,10 +258,12 @@ void EKFLocalizer::timerTFCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); + const rclcpp::Time current_time = this->now(); + geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = tier4_autoware_utils::pose2transform( - ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false), "base_link"); - transform_stamped.header.stamp = this->now(); + ekf_module_->getCurrentPose(current_time, z, roll, pitch, false), "base_link"); + transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); } @@ -340,15 +344,13 @@ void EKFLocalizer::publishEstimateResult( const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist) { - rclcpp::Time current_time = this->now(); - /* publish latest pose */ pub_pose_->publish(current_ekf_pose); pub_biased_pose_->publish(current_biased_ekf_pose); /* publish latest pose with covariance */ geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; - pose_cov.header.stamp = current_time; + pose_cov.header.stamp = current_ekf_pose.header.stamp; pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance(); @@ -363,7 +365,7 @@ void EKFLocalizer::publishEstimateResult( /* publish latest twist with covariance */ geometry_msgs::msg::TwistWithCovarianceStamped twist_cov; - twist_cov.header.stamp = current_time; + twist_cov.header.stamp = current_ekf_twist.header.stamp; twist_cov.header.frame_id = current_ekf_twist.header.frame_id; twist_cov.twist.twist = current_ekf_twist.twist; twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance(); @@ -371,13 +373,13 @@ void EKFLocalizer::publishEstimateResult( /* publish yaw bias */ tier4_debug_msgs::msg::Float64Stamped yawb; - yawb.stamp = current_time; + yawb.stamp = current_ekf_twist.header.stamp; yawb.data = ekf_module_->getYawBias(); pub_yaw_bias_->publish(yawb); /* publish latest odometry */ nav_msgs::msg::Odometry odometry; - odometry.header.stamp = current_time; + odometry.header.stamp = current_ekf_pose.header.stamp; odometry.header.frame_id = current_ekf_pose.header.frame_id; odometry.child_frame_id = "base_link"; odometry.pose = pose_cov.pose; @@ -385,7 +387,7 @@ void EKFLocalizer::publishEstimateResult( pub_odom_->publish(odometry); } -void EKFLocalizer::publishDiagnostics() +void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) { std::vector diag_status_array; @@ -421,7 +423,7 @@ void EKFLocalizer::publishDiagnostics() diag_merged_status.hardware_id = this->get_name(); diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = this->now(); + diag_msg.header.stamp = current_time; diag_msg.status.push_back(diag_merged_status); pub_diag_->publish(diag_msg); } From fe21159b8c329389b37759fcbfccec606c354b32 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 Jan 2024 10:28:36 +0900 Subject: [PATCH 049/154] feat(goal_planner): output objects of interest (#6077) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 7 +++++++ .../utils/path_safety_checker/safety_check.cpp | 15 +++++++-------- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 7bd97b3775bad..a13efd543d61c 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1957,6 +1957,13 @@ void GoalPlannerModule::setDebugData() } add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation")); + + // set objects of interest + for (const auto & [uuid, data] : goal_planner_data_.collision_check) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + utils::parking_departure::initializeCollisionCheckDebugMap(goal_planner_data_.collision_check); // visualize safety status maker diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index bf93e71ab3591..e209e8dba36be 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -453,16 +453,15 @@ bool checkSafetyWithIntegralPredictedPolygon( for (const auto & path : object.predicted_paths) { for (const auto & pose_with_poly : path.path) { if (boost::geometry::overlaps(ego_integral_polygon, pose_with_poly.poly)) { - { - debug_pair.second.ego_predicted_path = ego_predicted_path; // raw path - debug_pair.second.obj_predicted_path = path.path; // raw path - debug_pair.second.extended_obj_polygon = pose_with_poly.poly; - debug_pair.second.extended_ego_polygon = - ego_integral_polygon; // time filtered extended polygon - updateCollisionCheckDebugMap(debug_map, debug_pair, false); - } + debug_pair.second.ego_predicted_path = ego_predicted_path; // raw path + debug_pair.second.obj_predicted_path = path.path; // raw path + debug_pair.second.extended_obj_polygon = pose_with_poly.poly; + debug_pair.second.extended_ego_polygon = + ego_integral_polygon; // time filtered extended polygon + updateCollisionCheckDebugMap(debug_map, debug_pair, /*is_safe=*/false); return false; } + updateCollisionCheckDebugMap(debug_map, debug_pair, /*is_safe=*/true); } } } From ea7cbbed6f50f8a8514b890288bedc3c1917b7ad Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 Jan 2024 10:29:04 +0900 Subject: [PATCH 050/154] feat(start_planner): output objects of interest (#6078) * feat(start_planner): output objects of interest Signed-off-by: kosuke55 * style(pre-commit): autofix --------- Signed-off-by: kosuke55 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../start_planner_module.hpp | 2 +- .../src/start_planner_module.cpp | 9 ++++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 41a80e59d56bf..7b463f4fadf80 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -254,7 +254,7 @@ class StartPlannerModule : public SceneModuleInterface void onFreespacePlannerTimer(); bool planFreespacePath(); - void setDebugData() const; + void setDebugData(); void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index d523f57125679..9ad7f72d6af4e 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -1325,7 +1325,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } -void StartPlannerModule::setDebugData() const +void StartPlannerModule::setDebugData() { using marker_utils::addFootprintMarker; using marker_utils::createFootprintMarkerArray; @@ -1432,6 +1432,13 @@ void StartPlannerModule::setDebugData() const add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); + + // set objects of interest + for (const auto & [uuid, data] : start_planner_data_.collision_check) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + initializeCollisionCheckDebugMap(start_planner_data_.collision_check); } From c52d0911079ce6a536ad7704ebb5eab9128fe02a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 16 Jan 2024 10:39:46 +0900 Subject: [PATCH 051/154] feat(perception_rviz_plugin): rviz object covariances (#6074) * feat: visualize yaw rate, yaw std Signed-off-by: Taekjin LEE * feat: discrete yaw covariance marker Signed-off-by: Taekjin LEE * feat: add yaw covariance to detected object Signed-off-by: Taekjin LEE * feat: visualize twist covariance, yaw only Signed-off-by: Taekjin LEE * feat: discrete yaw rate marker Signed-off-by: Taekjin LEE * fix: property order Signed-off-by: Taekjin LEE * chore: parameter order change Signed-off-by: Taekjin LEE * feat: position covariance as ellipse Signed-off-by: Taekjin LEE * feat: twist covariance Signed-off-by: Taekjin LEE * chore: pose covariance marker renamed Signed-off-by: Taekjin LEE * fix: velocity covariance minimum alpha Signed-off-by: Taekjin LEE * chore: refactoring Signed-off-by: Taekjin LEE * feat: add option for interval coefficients Signed-off-by: Taekjin LEE * fix: confidence interval (1D) and confidence region (2D) Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 36 +- .../object_polygon_display_base.hpp | 131 +++++++- .../detected_objects_display.cpp | 53 +++ .../object_polygon_detail.cpp | 309 +++++++++++++++--- .../predicted_objects_display.cpp | 72 +++- .../tracked_objects_display.cpp | 63 +++- 6 files changed, 592 insertions(+), 72 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index b3ee4a33b28c9..5f9da8531d2ab 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -17,6 +17,8 @@ #include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" +#include +#include #include #include @@ -113,8 +115,14 @@ get_uuid_marker_ptr( const std_msgs::msg::ColorRGBA & color_rgba); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); +get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & confidence_interval_coefficient); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & confidence_interval_coefficient, const double & line_width); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( @@ -131,6 +139,23 @@ get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient, const double & line_width); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, @@ -142,10 +167,17 @@ get_path_confidence_marker_ptr( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( + const double start_angle, const double end_angle, const double radius, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( const double point_list[][3], const int point_pairs[][2], const int & num_pairs, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( + const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index a0a21406ba416..4290fdff49bb3 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -64,14 +64,22 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this}, m_display_label_property{"Display Label", true, "Enable/disable label visualization", this}, m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this}, - m_display_pose_with_covariance_property{ - "Display PoseWithCovariance", true, "Enable/disable pose with covariance visualization", - this}, m_display_velocity_text_property{ "Display Velocity", true, "Enable/disable velocity text visualization", this}, m_display_acceleration_text_property{ "Display Acceleration", true, "Enable/disable acceleration text visualization", this}, + m_display_pose_covariance_property{ + "Display Pose Covariance", true, "Enable/disable pose covariance visualization", this}, + m_display_yaw_covariance_property{ + "Display Yaw Covariance", false, "Enable/disable yaw covariance visualization", this}, m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this}, + m_display_twist_covariance_property{ + "Display Twist Covariance", false, "Enable/disable twist covariance visualization", this}, + m_display_yaw_rate_property{ + "Display Yaw Rate", false, "Enable/disable yaw rate visualization", this}, + m_display_yaw_rate_covariance_property{ + "Display Yaw Rate Covariance", false, "Enable/disable yaw rate covariance visualization", + this}, m_display_predicted_paths_property{ "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, m_display_path_confidence_property{ @@ -96,6 +104,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this); m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); + // Confidence interval property + m_confidence_interval_property = new rviz_common::properties::EnumProperty( + "Confidence Interval", "95%", "Confidence interval of state estimations.", this); + m_confidence_interval_property->addOption("70%", 0); + m_confidence_interval_property->addOption("85%", 1); + m_confidence_interval_property->addOption("95%", 2); + m_confidence_interval_property->addOption("99%", 3); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { @@ -238,11 +253,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - std::optional get_pose_with_covariance_marker_ptr( + std::optional get_pose_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const { - if (m_display_pose_with_covariance_property.getBool()) { - return detail::get_pose_with_covariance_marker_ptr(pose_with_covariance); + if (m_display_pose_covariance_property.getBool()) { + return detail::get_pose_covariance_marker_ptr(pose_with_covariance, get_confidence_region()); + } else { + return std::nullopt; + } + } + + std::optional get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & line_width) const + { + if (m_display_yaw_covariance_property.getBool()) { + return detail::get_yaw_covariance_marker_ptr( + pose_with_covariance, length, get_confidence_interval(), line_width); } else { return std::nullopt; } @@ -286,6 +313,44 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + std::optional get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const + { + if (m_display_twist_covariance_property.getBool()) { + return detail::get_twist_covariance_marker_ptr( + pose_with_covariance, twist_with_covariance, get_confidence_region()); + } else { + return std::nullopt; + } + } + + std::optional get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_yaw_rate_property.getBool()) { + return detail::get_yaw_rate_marker_ptr( + pose_with_covariance, twist_with_covariance, line_width); + } else { + return std::nullopt; + } + } + + std::optional get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_yaw_rate_covariance_property.getBool()) { + return detail::get_yaw_rate_covariance_marker_ptr( + pose_with_covariance, twist_with_covariance, get_confidence_interval(), line_width); + } else { + return std::nullopt; + } + } + std::optional get_predicted_path_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, const autoware_auto_perception_msgs::msg::Shape & shape, @@ -408,6 +473,46 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase double get_line_width() { return m_line_width_property.getFloat(); } + double get_confidence_interval() const + { + switch (m_confidence_interval_property->getOptionInt()) { + case 0: + // 70% + return 1.036; + case 1: + // 85% + return 1.440; + case 2: + // 95% + return 1.960; + case 3: + // 99% + return 2.576; + default: + return 1.960; + } + } + + double get_confidence_region() const + { + switch (m_confidence_interval_property->getOptionInt()) { + case 0: + // 70% + return 1.552; + case 1: + // 85% + return 1.802; + case 2: + // 95% + return 2.448; + case 3: + // 99% + return 3.035; + default: + return 2.448; + } + } + private: // All rviz plugins should have this. Should be initialized with pointer to this class MarkerCommon m_marker_common; @@ -419,18 +524,28 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::EnumProperty * m_display_type_property; // Property to choose simplicity of visualization polygon rviz_common::properties::EnumProperty * m_simple_visualize_mode_property; + // Property to set confidence interval of state estimations + rviz_common::properties::EnumProperty * m_confidence_interval_property; // Property to enable/disable label visualization rviz_common::properties::BoolProperty m_display_label_property; // Property to enable/disable uuid visualization rviz_common::properties::BoolProperty m_display_uuid_property; - // Property to enable/disable pose with covariance visualization - rviz_common::properties::BoolProperty m_display_pose_with_covariance_property; // Property to enable/disable velocity text visualization rviz_common::properties::BoolProperty m_display_velocity_text_property; // Property to enable/disable acceleration text visualization rviz_common::properties::BoolProperty m_display_acceleration_text_property; + // Property to enable/disable pose with covariance visualization + rviz_common::properties::BoolProperty m_display_pose_covariance_property; + // Property to enable/disable yaw covariance visualization + rviz_common::properties::BoolProperty m_display_yaw_covariance_property; // Property to enable/disable twist visualization rviz_common::properties::BoolProperty m_display_twist_property; + // Property to enable/disable twist covariance visualization + rviz_common::properties::BoolProperty m_display_twist_covariance_property; + // Property to enable/disable yaw rate visualization + rviz_common::properties::BoolProperty m_display_yaw_rate_property; + // Property to enable/disable yaw rate covariance visualization + rviz_common::properties::BoolProperty m_display_yaw_rate_covariance_property; // Property to enable/disable predicted paths visualization rviz_common::properties::BoolProperty m_display_predicted_paths_property; // Property to enable/disable predicted path confidence visualization diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 932ea87ccf5ad..53e935fa1850a 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -59,6 +59,27 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(label_marker_ptr); } + // Get marker for pose covariance + auto pose_with_covariance_marker = + get_pose_covariance_marker_ptr(object.kinematics.pose_with_covariance); + if (pose_with_covariance_marker) { + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + // Get marker for existence probability geometry_msgs::msg::Point existence_probability_position; existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; @@ -99,6 +120,38 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) twist_marker_ptr->id = id++; add_marker(twist_marker_ptr); } + + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw rate covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.3); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } } } diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 97d7212d4f0ce..11a0bbbe57d53 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -101,17 +101,16 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; - geometry_msgs::msg::Point pt_s; - pt_s.x = 0.0; - pt_s.y = 0.0; - pt_s.z = 0.0; - marker_ptr->points.push_back(pt_s); - - geometry_msgs::msg::Point pt_e; - pt_e.x = twist_with_covariance.twist.linear.x; - pt_e.y = twist_with_covariance.twist.linear.y; - pt_e.z = twist_with_covariance.twist.linear.z; - marker_ptr->points.push_back(pt_e); + // velocity line + geometry_msgs::msg::Point point; + point.x = 0.0; + point.y = 0.0; + point.z = 0.0; + marker_ptr->points.push_back(point); + point.x = twist_with_covariance.twist.linear.x; + point.y = twist_with_covariance.twist.linear.y; + point.z = twist_with_covariance.twist.linear.z; + marker_ptr->points.push_back(point); marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.999; @@ -122,6 +121,168 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; + marker_ptr->ns = std::string("twist covariance"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // position is the tip of the velocity vector + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double pos_yaw_angle = 2.0 * std::atan2( + pose_with_covariance.pose.orientation.z, + pose_with_covariance.pose.orientation.w); // [rad] + marker_ptr->pose.position.x += velocity * std::cos(pos_yaw_angle + velocity_angle); + marker_ptr->pose.position.y += velocity * std::sin(pos_yaw_angle + velocity_angle); + + // velocity covariance + // extract eigen values and eigen vectors + Eigen::Matrix2d eigen_twist_covariance; + eigen_twist_covariance << twist_with_covariance.covariance[0], + twist_with_covariance.covariance[1], twist_with_covariance.covariance[6], + twist_with_covariance.covariance[7]; + double phi, sigma1, sigma2; + calc_covariance_eigen_vectors(eigen_twist_covariance, sigma1, sigma2, phi); + phi = pos_yaw_angle + phi; + double area = sigma1 * sigma2; + double alpha = std::min(0.5, 1.0 / area); + alpha = std::max(0.1, alpha); + + // ellipse orientation + marker_ptr->pose.orientation.x = 0.0; + marker_ptr->pose.orientation.y = 0.0; + marker_ptr->pose.orientation.z = std::sin(phi / 2.0); + marker_ptr->pose.orientation.w = std::cos(phi / 2.0); + + // ellipse size + marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; + marker_ptr->scale.y = sigma2 * confidence_interval_coefficient; + marker_ptr->scale.z = 0.05; + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = alpha; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.2; + marker_ptr->color.b = 0.4; + + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_ptr->ns = std::string("yaw rate"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // yaw rate + const double yaw_rate = twist_with_covariance.twist.angular.z; + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y + + twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double yaw_mark_length = velocity * 0.8; + + geometry_msgs::msg::Point point; + // first point + point.x = 0; + point.y = 0; + point.z = 0; + marker_ptr->points.push_back(point); + // yaw rate arc + calc_arc_line_strip( + velocity_angle, velocity_angle + yaw_rate, yaw_mark_length, marker_ptr->points); + // last point + point.x = 0; + point.y = 0; + point.z = 0; + marker_ptr->points.push_back(point); + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = 0.9; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.0; + marker_ptr->color.b = 0.0; + + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->ns = std::string("yaw rate covariance"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // yaw rate covariance + const double yaw_rate_covariance = twist_with_covariance.covariance[35]; + const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance) * confidence_interval_coefficient; + const double yaw_rate = twist_with_covariance.twist.angular.z; + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y + + twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double yaw_mark_length = velocity * 0.8; + const double bar_width = std::max(velocity * 0.05, 0.1); + const double velocity_yaw_angle = velocity_angle + yaw_rate; + const double velocity_yaw_p_sigma_angle = velocity_yaw_angle + yaw_rate_sigma; + const double velocity_yaw_n_sigma_angle = velocity_yaw_angle - yaw_rate_sigma; + + const double point_list[7][3] = { + {yaw_mark_length * std::cos(velocity_yaw_angle), yaw_mark_length * std::sin(velocity_yaw_angle), + 0}, + {yaw_mark_length * std::cos(velocity_yaw_p_sigma_angle), + yaw_mark_length * std::sin(velocity_yaw_p_sigma_angle), 0}, + {yaw_mark_length * std::cos(velocity_yaw_n_sigma_angle), + yaw_mark_length * std::sin(velocity_yaw_n_sigma_angle), 0}, + {(yaw_mark_length + bar_width) * std::cos(velocity_yaw_p_sigma_angle), + (yaw_mark_length + bar_width) * std::sin(velocity_yaw_p_sigma_angle), 0}, + {(yaw_mark_length - bar_width) * std::cos(velocity_yaw_p_sigma_angle), + (yaw_mark_length - bar_width) * std::sin(velocity_yaw_p_sigma_angle), 0}, + {(yaw_mark_length + bar_width) * std::cos(velocity_yaw_n_sigma_angle), + (yaw_mark_length + bar_width) * std::sin(velocity_yaw_n_sigma_angle), 0}, + {(yaw_mark_length - bar_width) * std::cos(velocity_yaw_n_sigma_angle), + (yaw_mark_length - bar_width) * std::sin(velocity_yaw_n_sigma_angle), 0}, + }; + const int point_pairs[4][2] = { + {0, 1}, + {0, 2}, + {3, 4}, + {5, 6}, + }; + calc_line_list_from_points(point_list, point_pairs, 4, marker_ptr->points); + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = 0.9; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.2; + marker_ptr->color.b = 0.4; + + return marker_ptr; +} + visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, const std_msgs::msg::ColorRGBA & color_rgba) @@ -164,47 +325,117 @@ visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( return marker_ptr; } -visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) +void calc_arc_line_strip( + const double start_angle, const double end_angle, const double radius, + std::vector & points) +{ + geometry_msgs::msg::Point point; + // arc points + const double maximum_delta_angle = 10.0 * M_PI / 180.0; + const int num_points = + std::max(3, static_cast(std::abs(end_angle - start_angle) / maximum_delta_angle)); + for (int i = 0; i < num_points; ++i) { + const double angle = start_angle + (end_angle - start_angle) * static_cast(i) / + static_cast(num_points - 1); + point.x = radius * std::cos(angle); + point.y = radius * std::sin(angle); + point.z = 0; + points.push_back(point); + } +} + +void calc_covariance_eigen_vectors( + const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw) +{ + Eigen::SelfAdjointEigenSolver solver(matrix); + Eigen::Vector2d eigen_values = solver.eigenvalues(); + // eigen values + sigma1 = std::sqrt(eigen_values.x()); + sigma2 = std::sqrt(eigen_values.y()); + // orientation of covariance ellipse + Eigen::Vector2d e1 = solver.eigenvectors().col(0); + yaw = std::atan2(e1.y(), e1.x()); +} + +visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & confidence_interval_coefficient) { auto marker_ptr = std::make_shared(); - marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; marker_ptr->ns = std::string("position covariance"); - marker_ptr->scale.x = 0.03; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; + + // position covariance + // extract eigen values and eigen vectors + Eigen::Matrix2d eigen_pose_covariance; + eigen_pose_covariance << pose_with_covariance.covariance[0], pose_with_covariance.covariance[1], + pose_with_covariance.covariance[6], pose_with_covariance.covariance[7]; + double yaw, sigma1, sigma2; + calc_covariance_eigen_vectors(eigen_pose_covariance, sigma1, sigma2, yaw); + + // ellipse orientation marker_ptr->pose.orientation.x = 0.0; marker_ptr->pose.orientation.y = 0.0; - marker_ptr->pose.orientation.z = 0.0; - marker_ptr->pose.orientation.w = 1.0; + marker_ptr->pose.orientation.z = std::sin(yaw / 2.0); + marker_ptr->pose.orientation.w = std::cos(yaw / 2.0); + + // ellipse size + marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; + marker_ptr->scale.y = sigma2 * confidence_interval_coefficient; + marker_ptr->scale.z = 0.05; + + // ellipse color density + double area = sigma1 * sigma2; + double alpha = std::min(0.5, 3.0 / area); + alpha = std::max(0.1, alpha); + + // marker configuration + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = alpha; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 1.0; + marker_ptr->color.b = 1.0; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & confidence_interval_coefficient, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_ptr->ns = std::string("yaw covariance"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; geometry_msgs::msg::Point point; - Eigen::Matrix2d eigen_pose_with_covariance; - eigen_pose_with_covariance << pose_with_covariance.covariance[0], - pose_with_covariance.covariance[1], pose_with_covariance.covariance[6], - pose_with_covariance.covariance[7]; - Eigen::SelfAdjointEigenSolver solver(eigen_pose_with_covariance); - double sigma1 = 2.448 * std::sqrt(solver.eigenvalues().x()); // 2.448 sigma is 95% - double sigma2 = 2.448 * std::sqrt(solver.eigenvalues().y()); // 2.448 sigma is 95% - Eigen::Vector2d e1 = solver.eigenvectors().col(0); - Eigen::Vector2d e2 = solver.eigenvectors().col(1); - point.x = -e1.x() * sigma1; - point.y = -e1.y() * sigma1; - point.z = 0; - marker_ptr->points.push_back(point); - point.x = e1.x() * sigma1; - point.y = e1.y() * sigma1; - point.z = 0; - marker_ptr->points.push_back(point); - point.x = -e2.x() * sigma2; - point.y = -e2.y() * sigma2; + + // orientation covariance + double yaw_vector_length = std::max(length, 1.0); + double yaw_sigma = + std::sqrt(pose_with_covariance.covariance[35]) * confidence_interval_coefficient; + // get arc points + if (yaw_sigma > M_PI) { + yaw_vector_length = 1.0; + } + // first point + point.x = 0; + point.y = 0; point.z = 0; marker_ptr->points.push_back(point); - point.x = e2.x() * sigma2; - point.y = e2.y() * sigma2; + // arc points + calc_arc_line_strip(-yaw_sigma, yaw_sigma, yaw_vector_length, marker_ptr->points); + // last point + point.x = 0; + point.y = 0; point.z = 0; marker_ptr->points.push_back(point); + + // marker configuration marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); - marker_ptr->color.a = 0.999; + marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; marker_ptr->color.g = 1.0; marker_ptr->color.b = 1.0; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index a9074f9e1bc1a..24b21a15732c3 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -79,10 +79,10 @@ std::vector PredictedObjectsDisplay: object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification, get_line_width(), true); if (shape_marker) { - auto shape_marker_ptr = shape_marker.value(); - shape_marker_ptr->header = msg->header; - shape_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(shape_marker_ptr); + auto marker_ptr = shape_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for label @@ -90,10 +90,10 @@ std::vector PredictedObjectsDisplay: object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification); if (label_marker) { - auto label_marker_ptr = label_marker.value(); - label_marker_ptr->header = msg->header; - label_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(label_marker_ptr); + auto marker_ptr = label_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for id @@ -111,14 +111,25 @@ std::vector PredictedObjectsDisplay: markers.push_back(id_marker_ptr); } - // Get marker for pose with covariance + // Get marker for pose covariance auto pose_with_covariance_marker = - get_pose_with_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); + get_pose_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); if (pose_with_covariance_marker) { - auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); - pose_with_covariance_marker_ptr->header = msg->header; - pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(pose_with_covariance_marker_ptr); + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for existence probability @@ -181,6 +192,39 @@ std::vector PredictedObjectsDisplay: markers.push_back(twist_marker_ptr); } + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance, get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + + // Get marker for twist covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance, get_line_width() * 0.3); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + // Add marker for each candidate path int32_t path_count = 0; for (const auto & predicted_path : object.kinematics.predicted_paths) { diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 54dfb9edf889a..504b51f850444 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -55,11 +55,11 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) for (const auto & object : msg->objects) { // Filter by object dynamic status if (!is_object_to_show(showing_dynamic_status, object)) continue; - const auto line_width = get_line_width(); // Get marker for shape auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, - object.kinematics.pose_with_covariance.pose.orientation, object.classification, line_width, + object.kinematics.pose_with_covariance.pose.orientation, object.classification, + get_line_width(), object.kinematics.orientation_availability == autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { @@ -95,15 +95,27 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(id_marker_ptr); } - // Get marker for pose with covariance + // Get marker for pose covariance auto pose_with_covariance_marker = - get_pose_with_covariance_marker_ptr(object.kinematics.pose_with_covariance); + get_pose_covariance_marker_ptr(object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { - auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); - pose_with_covariance_marker_ptr->header = msg->header; - pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); - add_marker(pose_with_covariance_marker_ptr); + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); } + + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + // Get marker for existence probability geometry_msgs::msg::Point existence_probability_position; existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; @@ -150,13 +162,46 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for twist auto twist_marker = get_twist_marker_ptr( - object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, line_width); + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width()); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; twist_marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(twist_marker_ptr); } + + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + + // Get marker for twist covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.5); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } } } From 2a6dc0e62bd6a753662c7bf35e66ba03b3cf5075 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 16 Jan 2024 11:10:14 +0900 Subject: [PATCH 052/154] feat(static_centerline_optimizer): get behavior_velocity_planner's path interval from yaml (#6071) * feat(static_centerline_optimizer): get behavior_velocity_planner's path interval from yaml Signed-off-by: Takayuki Murooka * fix test Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../launch/static_centerline_optimizer.launch.xml | 5 +++++ planning/static_centerline_optimizer/package.xml | 1 + .../src/static_centerline_optimizer_node.cpp | 7 +++++-- .../test/test_static_centerline_optimizer.test.py | 4 ++++ 4 files changed, 15 insertions(+), 2 deletions(-) diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml index f8b96ca0db146..37a9abc47bfeb 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml @@ -21,6 +21,10 @@ name="behavior_path_planner_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml" /> + + diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index 51bd9e87d6ba2..17191868b7418 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -19,6 +19,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs behavior_path_planner + behavior_velocity_planner geometry_msgs global_parameter_loader interpolation diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index f2d9dc68a8cdc..d98341ecb2e23 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -425,6 +425,10 @@ std::vector StaticCenterlineOptimizerNode::plan_path( const double behavior_path_interval = has_parameter("output_path_interval") ? get_parameter("output_path_interval").as_double() : declare_parameter("output_path_interval"); + const double behavior_vel_interval = + has_parameter("behavior_output_path_interval") + ? get_parameter("behavior_output_path_interval").as_double() + : declare_parameter("behavior_output_path_interval"); // extract path with lane id from lanelets const auto raw_path_with_lane_id = [&]() { @@ -439,8 +443,7 @@ std::vector StaticCenterlineOptimizerNode::plan_path( // convert path with lane id to path const auto raw_path = [&]() { const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - // NOTE: The behavior_velocity_planner resamples with the interval 1.0 somewhere. - return motion_utils::resamplePath(non_resampled_path, 1.0); + return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); }(); pub_raw_path_->publish(raw_path); RCLCPP_INFO(get_logger(), "Converted to path and published."); diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py index 141743deb007c..36bdfc732ed79 100644 --- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py +++ b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py @@ -56,6 +56,10 @@ def generate_test_description(): get_package_share_directory("behavior_path_planner"), "config/behavior_path_planner.param.yaml", ), + os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config/behavior_velocity_planner.param.yaml", + ), os.path.join( get_package_share_directory("path_smoother"), "config/elastic_band_smoother.param.yaml", From 3acbdd76ace59d25ee55ccf5a14bb73d7c4b4833 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 Jan 2024 13:15:48 +0900 Subject: [PATCH 053/154] feat(goal_planner): add scale buffer to calcModuleRequestLength (#6068) * feat(goal_planner): add scale buffer to calcModuleRequestLength Signed-off-by: kosuke55 * style(pre-commit): autofix --------- Signed-off-by: kosuke55 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/goal_planner_module.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index a13efd543d61c..9403ed7529678 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -440,8 +440,14 @@ double GoalPlannerModule::calcModuleRequestLength() const return parameters_->pull_over_minimum_request_length; } - const double minimum_request_length = - *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; + // The module is requested at a distance such that the ego can stop for the pull over start point + // closest to ego. When path planning, each start point is checked to see if it is possible to + // stop again. At that time, if the speed has changed over time, the path will be rejected if + // min_stop_distance is used as is, so scale is applied to provide a buffer. + constexpr double scale_factor_for_buffer = 1.2; + const double minimum_request_length = *min_stop_distance * scale_factor_for_buffer + + parameters_->backward_goal_search_length + + approximate_pull_over_distance_; return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } From 39282953ed6407ab0a49ba140b7a6eb44cbedb47 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 16 Jan 2024 14:26:04 +0900 Subject: [PATCH 054/154] feat(avoidance): improve object detection area in order not to prevent endless loop (#6084) * perf(avoidance): reduce heavy process Signed-off-by: satoshi-ota * fix(avoidance): filter objects by precise lon distance Signed-off-by: satoshi-ota * refactor(avoidance): remove unused function Signed-off-by: satoshi-ota * feat(avoidance): improve detection area Signed-off-by: satoshi-ota * fix(avoidance): return shift line Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 6 +- .../behavior_path_avoidance_module/utils.hpp | 19 +- .../src/scene.cpp | 32 +-- .../src/shift_line_generator.cpp | 8 +- .../src/utils.cpp | 203 +++++++++++------- 5 files changed, 143 insertions(+), 125 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index b76a9e5645b48..6107314bc2824 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -187,7 +187,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( } void AvoidanceByLaneChange::fillAvoidanceTargetObjects( - AvoidancePlanningData & data, DebugData & debug) const + AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); @@ -227,7 +227,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( [&](const auto & object) { return createObjectData(data, object); }); } - utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p); + utils::avoidance::filterTargetObjects( + target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance, + planner_data_, p); } ObjectData AvoidanceByLaneChange::createObjectData( diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index dc602edcc8b86..cbf68ada44abb 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -77,11 +77,6 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); -double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); - -bool isCentroidWithinLanelets( - const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); - lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); @@ -128,12 +123,7 @@ void compensateDetectionLost( ObjectDataArray & other_objects); void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, - const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); - -double getRoadShoulderDistance( - ObjectData & object, const AvoidancePlanningData & data, + ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); @@ -157,9 +147,10 @@ std::vector getSafetyCheckTargetObjects( DebugData & debug); std::pair separateObjectsByPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const double object_check_forward_distance, const bool is_running, DebugData & debug); + const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, + const std::shared_ptr & planner_data, const AvoidancePlanningData & data, + const std::shared_ptr & parameters, + const double object_check_forward_distance, DebugData & debug); DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index c94243451ed2b..bc1d8c6d75424 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -287,21 +287,18 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat void AvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { + using utils::avoidance::fillAvoidanceNecessity; using utils::avoidance::fillObjectStoppableJudge; using utils::avoidance::filterTargetObjects; using utils::avoidance::getTargetLanelets; - - // Add margin in order to prevent avoidance request chattering only when the module is running. - const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING || - getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + using utils::avoidance::separateObjectsByPath; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. - const auto sparse_resample_path = utils::resamplePathWithSpline( - helper_->getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); - const auto [object_within_target_lane, object_outside_target_lane] = - utils::avoidance::separateObjectsByPath( - sparse_resample_path, planner_data_, data, parameters_, helper_->getForwardDetectionRange(), - is_running, debug); + constexpr double MARGIN = 10.0; + const auto forward_detection_range = helper_->getForwardDetectionRange(); + const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath( + helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_, + data, parameters_, forward_detection_range + MARGIN, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; @@ -316,11 +313,13 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } // Filter out the objects to determine the ones to be avoided. - filterTargetObjects(objects, data, debug, planner_data_, parameters_); + filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { + fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); o.to_stop_line = calcDistanceToStopLine(o); fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); }); @@ -380,21 +379,10 @@ ObjectData AvoidanceModule::createObjectData( utils::avoidance::fillInitialPose(object_data, detected_objects_); // Calc lateral deviation from path to target object. - object_data.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT : Direction::RIGHT; - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, data.reference_path, object_data.overhang_pose.position); - - // Check whether the the ego should avoid the object. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - utils::avoidance::fillAvoidanceNecessity( - object_data, registered_objects_, vehicle_width, parameters_); - return object_data; } diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index dabb0d7257555..c20a8a73c955d 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -1101,8 +1101,8 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const double variable_prepare_distance = std::max(nominal_prepare_distance - last_sl_distance, 0.0); - double prepare_distance_scaled = - std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), last_sl_distance); + double prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / @@ -1122,7 +1122,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.end_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; - al.end_longitudinal = arclength_from_ego.at(al.end_idx); + al.end_longitudinal = prepare_distance_scaled; al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; ret.push_back(al); @@ -1136,7 +1136,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.start_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.start = data.reference_path.points.at(al.start_idx).point.pose; - al.start_longitudinal = arclength_from_ego.at(al.start_idx); + al.start_longitudinal = prepare_distance_scaled; al.end_idx = utils::avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 4c09962840907..c3d5467d680c6 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -175,7 +175,8 @@ geometry_msgs::msg::Polygon createVehiclePolygon( } Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, + const geometry_msgs::msg::Pose & p3, const geometry_msgs::msg::Pose & p4, const geometry_msgs::msg::Polygon & base_polygon) { Polygon2d one_step_polygon{}; @@ -183,7 +184,7 @@ Polygon2d createOneStepPolygon( { geometry_msgs::msg::Polygon out_polygon{}; geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_front); + geometry_tf.transform = pose2transform(p1); tf2::doTransform(base_polygon, out_polygon, geometry_tf); for (const auto & p : out_polygon.points) { @@ -194,7 +195,29 @@ Polygon2d createOneStepPolygon( { geometry_msgs::msg::Polygon out_polygon{}; geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_back); + geometry_tf.transform = pose2transform(p2); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p3); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p4); tf2::doTransform(base_polygon, out_polygon, geometry_tf); for (const auto & p : out_polygon.points) { @@ -629,7 +652,7 @@ bool isForceAvoidanceTarget( } bool isSatisfiedWithCommonCondition( - ObjectData & object, const AvoidancePlanningData & data, + ObjectData & object, const AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -656,7 +679,7 @@ bool isSatisfiedWithCommonCondition( return false; } - if (object.longitudinal > parameters->object_check_max_forward_distance) { + if (object.longitudinal > forward_detection_range) { object.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; return false; } @@ -741,6 +764,9 @@ bool isSatisfiedWithVehicleCondition( } // Object is on center line -> ignore. + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; return false; @@ -817,6 +843,64 @@ std::optional getAvoidMargin( // Step3. nominal case. avoid margin is limited by soft constraint. return std::min(soft_lateral_distance_limit, max_avoid_margin); } + +double getRoadShoulderDistance( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data) +{ + using lanelet::utils::to2D; + using tier4_autoware_utils::Point2d; + + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = + findNearestIndex(data.reference_path.points, object_pose.position); + const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; + + const auto rh = planner_data->route_handler; + if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { + return 0.0; + } + + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto & p1_object = object.overhang_pose.position; + const auto p_tmp = + geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); + const auto p2_object = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; + + // TODO(Satoshi OTA): check if the basic point is on right or left of bound. + const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; + + std::vector intersects; + for (size_t i = 1; i < bound.size(); i++) { + const auto p1_bound = + geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); + const auto p2_bound = + geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); + + const auto opt_intersect = + tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); + + if (!opt_intersect) { + continue; + } + + intersects.push_back(opt_intersect.value()); + } + + if (intersects.empty()) { + return 0.0; + } + + std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { + return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); + }); + + object.nearest_bound_point = intersects.front(); + + return calcDistance2d(p1_object, object.nearest_bound_point.value()); +} } // namespace filtering_utils bool isOnRight(const ObjectData & obj) @@ -1113,11 +1197,6 @@ std::vector generateObstaclePolygonsForDrivableArea( return obstacles_for_drivable_area; } -double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v) -{ - return v * std::cos(calcYawDeviation(p_ref, p_target)); -} - lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset) @@ -1517,67 +1596,8 @@ void compensateDetectionLost( } } -double getRoadShoulderDistance( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, - [[maybe_unused]] const std::shared_ptr & parameters) -{ - using lanelet::utils::to2D; - using tier4_autoware_utils::Point2d; - - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = - findNearestIndex(data.reference_path.points, object_pose.position); - const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; - - const auto rh = planner_data->route_handler; - if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { - return 0.0; - } - - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto & p1_object = object.overhang_pose.position; - const auto p_tmp = - geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); - const auto p2_object = - calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; - - // TODO(Satoshi OTA): check if the basic point is on right or left of bound. - const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; - - std::vector intersects; - for (size_t i = 1; i < bound.size(); i++) { - const auto p1_bound = - geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); - const auto p2_bound = - geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); - - const auto opt_intersect = - tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); - - if (!opt_intersect) { - continue; - } - - intersects.push_back(opt_intersect.value()); - } - - if (intersects.empty()) { - return 0.0; - } - - std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { - return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); - }); - - object.nearest_bound_point = intersects.front(); - - return calcDistance2d(p1_object, object.nearest_bound_point.value()); -} - void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug, + ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -1592,12 +1612,16 @@ void filterTargetObjects( }; for (auto & o : objects) { - if (!filtering_utils::isSatisfiedWithCommonCondition(o, data, planner_data, parameters)) { + if (!filtering_utils::isSatisfiedWithCommonCondition( + o, data, forward_detection_range, planner_data, parameters)) { data.other_objects.push_back(o); continue; } - o.to_road_shoulder_distance = getRoadShoulderDistance(o, data, planner_data, parameters); + // Find the footprint point closest to the path, set to object_data.overhang_distance. + o.overhang_dist = + calcEnvelopeOverhangDistance(o, data.reference_path, o.overhang_pose.position); + o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { @@ -1877,9 +1901,10 @@ std::vector getSafetyCheckTargetObjects( } std::pair separateObjectsByPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const double object_check_forward_distance, const bool is_running, DebugData & debug) + const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, + const std::shared_ptr & planner_data, const AvoidancePlanningData & data, + const std::shared_ptr & parameters, + const double object_check_forward_distance, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; @@ -1892,22 +1917,34 @@ std::pair separateObjectsByPath( max_offset = std::max(max_offset, offset); } - const double MARGIN = is_running ? 1.0 : 0.0; // [m] const auto detection_area = - createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset + MARGIN); - const auto ego_idx = planner_data->findEgoIndex(path.points); + createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); + const auto ego_idx = planner_data->findEgoIndex(reference_path.points); + const auto arc_length_array = + utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 1e-3); + double next_longitudinal_distance = 0.0; std::vector detection_areas; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & p_ego_front = path.points.at(i).point.pose; - const auto & p_ego_back = path.points.at(i + 1).point.pose; + for (size_t i = 0; i < reference_path.points.size() - 1; ++i) { + const auto & p_reference_ego_front = reference_path.points.at(i).point.pose; + const auto & p_reference_ego_back = reference_path.points.at(i + 1).point.pose; + const auto & p_spline_ego_front = spline_path.points.at(i).point.pose; + const auto & p_spline_ego_back = spline_path.points.at(i + 1).point.pose; - const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); + const auto distance_from_ego = calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; } - detection_areas.push_back(createOneStepPolygon(p_ego_front, p_ego_back, detection_area)); + if (arc_length_array.at(i) < next_longitudinal_distance) { + continue; + } + + detection_areas.push_back(createOneStepPolygon( + p_reference_ego_front, p_reference_ego_back, p_spline_ego_front, p_spline_ego_back, + detection_area)); + + next_longitudinal_distance += parameters->resample_interval_for_output; } std::for_each(detection_areas.begin(), detection_areas.end(), [&](const auto & detection_area) { From a3f90f0fce60791e298ea59108ce6de1e4098d4d Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Tue, 16 Jan 2024 14:58:23 +0900 Subject: [PATCH 055/154] feat(ndt_scan_matcher): change coordinate of output_pose_covariance (#6065) * feat(ndt_scan_matcher): change coordinate of output_pose_covariance Signed-off-by: yamato-ando * style(pre-commit): autofix * Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Co-authored-by: Kento Yabuuchi * style(pre-commit): autofix --------- Signed-off-by: yamato-ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kento Yabuuchi --- .../ndt_scan_matcher_core.hpp | 2 ++ .../src/ndt_scan_matcher_core.cpp | 32 ++++++++++++++++++- 2 files changed, 33 insertions(+), 1 deletion(-) 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 e9aa265c78f34..71895c59ec37d 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 @@ -127,6 +127,8 @@ class NDTScanMatcher : public rclcpp::Node const double & transform_probability, const double & nearest_voxel_transformation_likelihood); static int count_oscillation(const std::vector & result_pose_msg_array); + std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const; std::array estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); 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 0f6fdbc15db26..942df03410165 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -486,7 +486,15 @@ void NDTScanMatcher::callback_sensor_points( } // covariance estimation - std::array ndt_covariance = output_pose_covariance_; + const Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond( + result_pose_msg.orientation.w, result_pose_msg.orientation.x, result_pose_msg.orientation.y, + result_pose_msg.orientation.z); + const Eigen::Matrix3d map_to_base_link_rotation = + map_to_base_link_quat.normalized().toRotationMatrix(); + + std::array ndt_covariance = + rotate_covariance(output_pose_covariance_, map_to_base_link_rotation); + if (is_converged && use_cov_estimation_) { const auto estimated_covariance = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); @@ -766,6 +774,28 @@ int NDTScanMatcher::count_oscillation( return max_oscillation_cnt; } +std::array NDTScanMatcher::rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const +{ + std::array ret_covariance = src_covariance; + + Eigen::Matrix3d src_cov; + src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], + src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], + src_covariance[14]; + + Eigen::Matrix3d ret_cov; + ret_cov = rotation * src_cov * rotation.transpose(); + + for (Eigen::Index i = 0; i < 3; ++i) { + ret_covariance[i] = ret_cov(0, i); + ret_covariance[i + 6] = ret_cov(1, i); + ret_covariance[i + 12] = ret_cov(2, i); + } + + return ret_covariance; +} + std::array NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) From c3f8e0a987b5bd2140995c17eaccb61d607933a6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 Jan 2024 23:32:19 +0900 Subject: [PATCH 056/154] feat(avoidance/goal_planner): execute avoidance and pull over simultaneously (#5979) * feat(avoidance/goal_planner): execute avoidance and pull over simultaneously Signed-off-by: kosuke55 Signed-off-by: kosuke55 * use utils Signed-off-by: kosuke55 * fix overlapped Signed-off-by: kosuke55 * reafactor(behavior_path_planner): move isAllowedGoalModification to common Signed-off-by: kosuke55 * fix readme Signed-off-by: kosuke55 * add goal modification condtion to avoidance Signed-off-by: kosuke55 * clean up * revert param Signed-off-by: kosuke55 * fix param Signed-off-by: kosuke55 * move dead line process Signed-off-by: kosuke55 * fix condition Signed-off-by: kosuke55 * fix crop Signed-off-by: kosuke55 * fix crop * fix typos Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../behavior_path_avoidance_module/README.md | 3 +- .../config/avoidance.param.yaml | 3 +- .../data_structs.hpp | 12 +- .../parameter_helper.hpp | 2 + .../src/shift_line_generator.cpp | 97 +++++++++-- .../src/utils.cpp | 20 ++- .../goal_planner_module.hpp | 4 + .../pull_over_planner_base.hpp | 7 + .../shift_pull_over.hpp | 2 + .../util.hpp | 17 +- .../src/goal_planner_module.cpp | 151 ++++++++++++------ .../src/manager.cpp | 6 +- .../src/shift_pull_over.cpp | 84 ++++++++-- .../src/util.cpp | 134 ++++++++++++++-- .../utils/utils.hpp | 7 + .../src/utils/utils.cpp | 82 ++++++++++ 16 files changed, 531 insertions(+), 100 deletions(-) diff --git a/planning/behavior_path_avoidance_module/README.md b/planning/behavior_path_avoidance_module/README.md index 20c0985f0f333..122ad7adcce9e 100644 --- a/planning/behavior_path_avoidance_module/README.md +++ b/planning/behavior_path_avoidance_module/README.md @@ -833,7 +833,8 @@ namespace: `avoidance.target_filtering.` | object_ignore_section_crosswalk_behind_distance | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | | object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | | object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | -| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | +| object_check_goal_distance | [m] | double | If the distance between object and goal position is less than this parameter, the module do not return center line. | 20.0 | +| object_check_return_pose_distance | [m] | double | If the distance between object and return position is less than this parameter, the module do not return center line. | 20.0 | | object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | | object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 | | object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 2d38cebd591f5..b300de2236fcf 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -122,7 +122,8 @@ motorcycle: true # [-] pedestrian: true # [-] # detection range - object_check_goal_distance: 20.0 # [m] + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] object_check_shiftable_ratio: 0.6 # [-] diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 86e1608501831..9b5ae3cb4db9e 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -164,10 +164,14 @@ struct AvoidanceParameters double object_check_backward_distance{0.0}; double object_check_yaw_deviation{0.0}; - // if the distance between object and goal position is less than this parameter, the module ignore - // the object. + // if the distance between object and goal position is less than this parameter, the module do not + // return center line. double object_check_goal_distance{0.0}; + // if the distance between object and return position is less than this parameter, the module do + // not return center line. + double object_check_return_pose_distance{0.0}; + // use in judge whether the vehicle is parking object on road shoulder double object_check_shiftable_ratio{0.0}; @@ -462,14 +466,14 @@ using AvoidLineArray = std::vector; struct AvoidOutline { - AvoidOutline(AvoidLine avoid_line, AvoidLine return_line) + AvoidOutline(AvoidLine avoid_line, const std::optional return_line) : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)} { } AvoidLine avoid_line{}; - AvoidLine return_line{}; + std::optional return_line{}; AvoidLineArray middle_lines{}; }; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index b9af50ce76eb5..9f2fdf7ab96d9 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -122,6 +122,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.object_check_return_pose_distance = + getOrDeclareParameter(*node, ns + "object_check_return_pose_distance"); p.threshold_distance_object_is_on_center = getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index c20a8a73c955d..6196b3e209d11 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -81,7 +81,9 @@ AvoidLineArray toArray(const AvoidOutlines & outlines) AvoidLineArray ret{}; for (const auto & outline : outlines) { ret.push_back(outline.avoid_line); - ret.push_back(outline.return_line); + if (outline.return_line.has_value()) { + ret.push_back(outline.return_line.value()); + } std::for_each( outline.middle_lines.begin(), outline.middle_lines.end(), @@ -341,7 +343,30 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( al_return.object_on_right = utils::avoidance::isOnRight(o); } - if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { + const bool skip_return_shift = [&]() { + if (!utils::isAllowedGoalModification(data_->route_handler)) { + return false; + } + const auto goal_pose = data_->route_handler->getGoalPose(); + const double goal_longitudinal_distance = + motion_utils::calcSignedArcLength(data.reference_path.points, 0, goal_pose.position); + const bool is_return_shift_to_goal = + std::abs(al_return.end_longitudinal - goal_longitudinal_distance) < + parameters_->object_check_return_pose_distance; + if (is_return_shift_to_goal) { + return true; + } + const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; + const bool has_object_near_goal = + tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) < + parameters_->object_check_goal_distance; + return has_object_near_goal; + }(); + + if (skip_return_shift) { + // if the object is close to goal or ego is about to return near GOAL, do not return + outlines.emplace_back(al_avoid, std::nullopt); + } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { outlines.emplace_back(al_avoid, al_return); } else { o.reason = "InvalidShiftLine"; @@ -637,35 +662,43 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess( auto & next_outline = outlines.at(i); const auto & return_line = last_outline.return_line; - const auto & avoid_line = next_outline.avoid_line; + if (!return_line.has_value()) { + ret.push_back(outlines.at(i)); + break; + } - if (no_conflict(return_line, avoid_line)) { + const auto & avoid_line = next_outline.avoid_line; + if (no_conflict(return_line.value(), avoid_line)) { ret.push_back(outlines.at(i)); continue; } - const auto merged_shift_line = merge(return_line, avoid_line, generateUUID()); + const auto merged_shift_line = merge(return_line.value(), avoid_line, generateUUID()); if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) { ret.push_back(outlines.at(i)); continue; } - if (same_side_shift(return_line, avoid_line)) { + if (same_side_shift(return_line.value(), avoid_line)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) { + if ( + within(return_line.value(), avoid_line.end_idx) && + within(avoid_line, return_line->start_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) { + if ( + within(return_line.value(), avoid_line.start_idx) && + within(avoid_line, return_line->end_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); @@ -686,7 +719,10 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( for (auto & outline : ret) { if (outline.middle_lines.empty()) { - const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID()); + const auto new_line = + outline.return_line.has_value() + ? fill(outline.avoid_line, outline.return_line.value(), generateUUID()) + : outline.avoid_line; outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -701,8 +737,11 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); - if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { - const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID()); + if ( + outline.return_line.has_value() && + outline.middle_lines.back().end_longitudinal < outline.return_line->start_longitudinal) { + const auto new_line = + fill(outline.middle_lines.back(), outline.return_line.value(), generateUUID()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -973,6 +1012,20 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const bool has_candidate_point = !ret.empty(); const bool has_registered_point = last_.has_value(); + if (utils::isAllowedGoalModification(data_->route_handler)) { + const auto has_object_near_goal = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + data_->route_handler->getGoalPose().position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + }); + if (has_object_near_goal) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "object near goal exists so skip adding return shift"); + return ret; + } + } + const auto exist_unavoidable_object = std::any_of( data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; }); @@ -1027,6 +1080,21 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( } } + // if last shift line is near the objects, do not add return shift. + if (utils::isAllowedGoalModification(data_->route_handler)) { + const bool has_last_shift_near_goal = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + last_sl.end.position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + }); + if (has_last_shift_near_goal) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "last shift line is near the objects"); + return ret; + } + } + // There already is a shift point candidates to go back to center line, but it could be too sharp // due to detection noise or timing. // Here the return-shift from ego is added for the in case. @@ -1070,8 +1138,11 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( return ret; } - const auto remaining_distance = std::min( - arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); + const auto remaining_distance = + utils::isAllowedGoalModification(data_->route_handler) + ? data.to_return_point + : std::min( + arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = data.arclength_from_ego.at(last_sl.end_idx); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index c3d5467d680c6..80aeb2bf0dccf 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -700,11 +700,13 @@ bool isSatisfiedWithCommonCondition( return false; } - if ( - object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > - to_goal_distance) { - object.reason = "TooNearToGoal"; - return false; + if (!utils::isAllowedGoalModification(planner_data->route_handler)) { + if ( + object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > + to_goal_distance) { + object.reason = "TooNearToGoal"; + return false; + } } return true; @@ -1686,7 +1688,9 @@ void fillAdditionalInfoFromLongitudinal( { for (auto & outline : outlines) { fillAdditionalInfoFromLongitudinal(data, outline.avoid_line); - fillAdditionalInfoFromLongitudinal(data, outline.return_line); + if (outline.return_line.has_value()) { + fillAdditionalInfoFromLongitudinal(data, outline.return_line.value()); + } std::for_each(outline.middle_lines.begin(), outline.middle_lines.end(), [&](auto & line) { fillAdditionalInfoFromLongitudinal(data, line); @@ -2169,7 +2173,9 @@ double calcDistanceToReturnDeadLine( } // dead line for goal - if (parameters->enable_dead_line_for_goal) { + if ( + !utils::isAllowedGoalModification(planner_data->route_handler) && + parameters->enable_dead_line_for_goal) { if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { const auto & ego_pos = planner_data->self_odometry->pose.pose.position; const auto to_goal_distance = diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index e90162c958be5..9fb9c17f1ca70 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -472,6 +472,10 @@ class GoalPlannerModule : public SceneModuleInterface // new turn signal TurnSignalInfo calcTurnSignalInfo() const; + std::optional last_previous_module_output_{}; + bool hasPreviousModulePathShapeChanged() const; + bool hasDeviatedFromLastPreviousModulePath() const; + // timer for generating pull over path candidates in a separate thread void onTimer(); void onFreespaceParkingTimer(); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index fe24d66ddc850..9700c44445a65 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -118,6 +118,11 @@ class PullOverPlannerBase } virtual ~PullOverPlannerBase() = default; + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + void setPlannerData(const std::shared_ptr planner_data) { planner_data_ = planner_data; @@ -131,6 +136,8 @@ class PullOverPlannerBase vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; GoalPlannerParameters parameters_; + + BehaviorModuleOutput previous_module_output_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index bd19dc2e87de0..892ef7d5dd303 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -43,6 +43,8 @@ class ShiftPullOver : public PullOverPlannerBase protected: PathWithLaneId generateReferencePath( const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; + std::optional cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index a7aec66f64363..294a5125a0d13 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -49,8 +49,21 @@ PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); -bool isAllowedGoalModification(const std::shared_ptr & route_handler); -bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path); +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_thresh); + +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose); +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const double extend_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const Pose & extend_pose); // debug MarkerArray createPullOverAreaMarkerArray( diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 9403ed7529678..3f765c99cffa9 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -136,11 +136,34 @@ void GoalPlannerModule::updateOccupancyGrid() occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); } +bool GoalPlannerModule::hasPreviousModulePathShapeChanged() const +{ + if (!last_previous_module_output_) { + return true; + } + + const auto current_path = getPreviousModuleOutput().path; + + // the terminal distance is far + return calcDistance2d( + last_previous_module_output_->path.points.back().point, + current_path.points.back().point) > 0.3; +} + +bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const +{ + if (!last_previous_module_output_) { + return true; + } + return std::abs(motion_utils::calcLateralOffset( + last_previous_module_output_->path.points, + planner_data_->self_odometry->pose.pose.position)) > 0.3; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { - // already generated pull over candidate paths - if (!thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -149,16 +172,30 @@ void GoalPlannerModule::onTimer() return; } - if ( - !planner_data_ || - !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!planner_data_ || !utils::isAllowedGoalModification(planner_data_->route_handler)) { return; } - if (getCurrentStatus() == ModuleStatus::IDLE) { + // check if new pull over path candidates are needed to be generated + const bool need_update = std::invoke([&]() { + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + return true; + } + if (hasPreviousModulePathShapeChanged()) { + RCLCPP_ERROR(getLogger(), "has previous module path shape changed"); + return true; + } + if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) { + RCLCPP_ERROR(getLogger(), "has deviated from last previous module path"); + return true; + } + return false; + }); + if (!need_update) { return; } + const auto previous_module_output = getPreviousModuleOutput(); const auto goal_candidates = thread_safe_data_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose @@ -173,8 +210,9 @@ void GoalPlannerModule::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { planner->setPlannerData(planner_data_); + planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path && isCrossingPossible(*pull_over_path)) { + if (pull_over_path) { pull_over_path->goal_id = goal_candidate.id; pull_over_path->id = path_candidates.size(); path_candidates.push_back(*pull_over_path); @@ -188,9 +226,21 @@ void GoalPlannerModule::onTimer() } } }; + + // todo: currently non centerline input path is supported only by shift pull over + const bool is_center_line_input_path = goal_planner_utils::isReferencePath( + previous_module_output.reference_path, previous_module_output.path, 0.1); + RCLCPP_DEBUG( + getLogger(), "the input path of pull over planner is center line: %d", + is_center_line_input_path); + // plan candidate paths and set them to the member variable if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } for (const auto & goal_candidate : goal_candidates) { planCandidatePaths(planner, goal_candidate); } @@ -198,6 +248,10 @@ void GoalPlannerModule::onTimer() } else if (parameters_->path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } planCandidatePaths(planner, goal_candidate); } } @@ -213,7 +267,10 @@ void GoalPlannerModule::onTimer() const std::lock_guard lock(mutex_); thread_safe_data_.set_pull_over_path_candidates(path_candidates); thread_safe_data_.set_closest_start_pose(closest_start_pose); + RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); } + + last_previous_module_output_ = previous_module_output; } void GoalPlannerModule::onFreespaceParkingTimer() @@ -225,7 +282,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } // fixed goal planner do not use freespace planner - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return; } @@ -354,7 +411,9 @@ bool GoalPlannerModule::isExecutionRequested() const // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const lanelet::ConstLanelets current_lanes = + utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); const bool goal_is_in_current_lanes = std::any_of( current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) { @@ -384,14 +443,14 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (!utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) + const double request_length = utils::isAllowedGoalModification(route_handler) ? calcModuleRequestLength() : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { @@ -402,7 +461,7 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (!utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } @@ -591,7 +650,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const { // calculate goal candidates const auto & route_handler = planner_data_->route_handler; - if (goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (utils::isAllowedGoalModification(route_handler)) { return goal_searcher_->search(); } @@ -609,7 +668,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const BehaviorModuleOutput GoalPlannerModule::plan() { - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOver(); } @@ -1100,7 +1159,7 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOverAsCandidate(); } @@ -1159,11 +1218,15 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return PathWithLaneId{}; } - // generate reference path - const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; - const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; - auto reference_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + // extend previous module path to generate reference path for stop path + const auto reference_path = std::invoke([&]() -> PathWithLaneId { + const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; + const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); + const double s_end = s_current + common_parameters.forward_path_length; + return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + }); + const auto extended_prev_path = goal_planner_utils::extendPath( + getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length); // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible @@ -1171,7 +1234,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); const auto decel_pose = calcLongitudinalOffsetPose( - reference_path.points, closest_goal_candidate.goal_pose.position, + extended_prev_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); // if not approved stop road lane. @@ -1199,7 +1262,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const } // if stop pose is closer than min_stop_distance, stop as soon as possible - const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, *stop_pose); + const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, *stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; @@ -1210,55 +1273,43 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const } // slow down for turn signal, insert stop point to stop_pose - decelerateForTurnSignal(*stop_pose, reference_path); + auto stop_path = extended_prev_path; + decelerateForTurnSignal(*stop_pose, stop_path); stop_pose_ = *stop_pose; // for debug wall marker // slow down before the search area. if (decel_pose) { - decelerateBeforeSearchStart(*decel_pose, reference_path); - return reference_path; + decelerateBeforeSearchStart(*decel_pose, stop_path); + return stop_path; } - // if already passed the decel pose, set pull_over_velocity to reference_path. + // if already passed the decel pose, set pull_over_velocity to stop_path. const auto min_decel_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, pull_over_velocity); - for (auto & p : reference_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); + for (auto & p : stop_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(stop_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { continue; } p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); } - return reference_path; + return stop_path; } PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const { - const auto & route_handler = planner_data_->route_handler; - const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & common_parameters = planner_data_->parameters; - - // generate stop reference path - const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false); - - const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; - const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; - auto stop_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); - // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return stop_path; + return getPreviousModuleOutput().path; } // set stop point + auto stop_path = getPreviousModuleOutput().path; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { @@ -1887,7 +1938,7 @@ void GoalPlannerModule::setDebugData() } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green @@ -1900,6 +1951,14 @@ void GoalPlannerModule::setDebugData() add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } + // Visualize previous module output + add(createPathMarkerArray( + getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); + if (last_previous_module_output_.has_value()) { + add(createPathMarkerArray( + last_previous_module_output_.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); + } + // Visualize path and related pose if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 3079361253c31..8608d6cce2c81 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -419,7 +419,7 @@ bool GoalPlannerModuleManager::isAlwaysExecutableModule() const { // enable AlwaysExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -434,7 +434,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -449,7 +449,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 712da5aa03a4e..b168da61e6239 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -46,10 +46,10 @@ std::optional ShiftPullOver::plan(const Pose & goal_pose) const int shift_sampling_num = parameters_.shift_sampling_num; const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; - // get road and shoulder lanes - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_search_length, forward_search_length, + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + previous_module_output_.path, planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, backward_search_length, forward_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { @@ -99,6 +99,31 @@ PathWithLaneId ShiftPullOver::generateReferencePath( return road_lane_reference_path; } +std::optional ShiftPullOver::cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const +{ + // clip previous module path to shift end pose nearest segment index + const size_t shift_end_idx = + motion_utils::findNearestSegmentIndex(prev_module_path.points, shift_end_pose.position); + std::vector clipped_points{ + prev_module_path.points.begin(), prev_module_path.points.begin() + shift_end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected shift end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength( + prev_module_path.points, shift_end_idx, shift_end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_prev_module_path = prev_module_path; + clipped_prev_module_path.points = clipped_points; + + return clipped_prev_module_path; +} + std::optional ShiftPullOver::generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const @@ -106,33 +131,55 @@ std::optional ShiftPullOver::generatePullOverPath( const double pull_over_velocity = parameters_.pull_over_velocity; const double after_shift_straight_distance = parameters_.after_shift_straight_distance; + // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); - // generate road lane reference path to shift end + // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval); + const auto prev_module_path = utils::resamplePathWithSpline( + previous_module_output_.path, parameters_.center_line_path_interval); + const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; + + // process previous module path for path shifter input path + // case1) extend path if shift end pose is behind of previous module path terminal pose + // case2) crop path if shift end pose is ahead of previous module path terminal pose + const auto processed_prev_module_path = std::invoke([&]() -> std::optional { + const bool extend_previous_module_path = + lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length > + lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length; + if (extend_previous_module_path) { // case1 + return goal_planner_utils::extendPath( + prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose); + } else { // case2 + return goal_planner_utils::cropPath(prev_module_path, shift_end_pose); + } + }); + if (!processed_prev_module_path || processed_prev_module_path->points.empty()) { + return {}; + } // calculate shift length - const Pose & shift_end_pose_road_lane = - road_lane_reference_path_to_shift_end.points.back().point.pose; + const Pose & shift_end_pose_prev_module_path = + processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - tier4_autoware_utils::inverseTransformPoint(shift_end_pose.position, shift_end_pose_road_lane) + tier4_autoware_utils::inverseTransformPoint( + shift_end_pose.position, shift_end_pose_prev_module_path) .y; // calculate shift start pose on road lane const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( - road_lane_reference_path_to_shift_end, pull_over_distance, shift_end_road_to_target_distance); + processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( - road_lane_reference_path_to_shift_end.points, shift_end_pose_road_lane.position, + processed_prev_module_path->points, shift_end_pose_prev_module_path.position, -before_shifted_pull_over_distance); - if (!shift_start_pose) return {}; // set path shifter and generate shifted path PathShifter path_shifter{}; - path_shifter.setPath(road_lane_reference_path_to_shift_end); + path_shifter.setPath(processed_prev_module_path.value()); ShiftLine shift_line{}; shift_line.start = *shift_start_pose; shift_line.end = shift_end_pose; @@ -140,7 +187,9 @@ std::optional ShiftPullOver::generatePullOverPath( path_shifter.addShiftLine(shift_line); ShiftedPath shifted_path{}; const bool offset_back = true; // offset front side from reference path - if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + if (!path_shifter.generate(&shifted_path, offset_back)) { + return {}; + } shifted_path.path.points = motion_utils::removeOverlapPoints(shifted_path.path.points); motion_utils::insertOrientation(shifted_path.path.points, true); @@ -208,8 +257,13 @@ std::optional ShiftPullOver::generatePullOverPath( pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; - pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); + pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); pull_over_path.debug_poses.push_back(actual_shift_end_pose); + pull_over_path.debug_poses.push_back(goal_pose); + pull_over_path.debug_poses.push_back(shift_end_pose); + pull_over_path.debug_poses.push_back( + road_lane_reference_path_to_shift_end.points.back().point.pose); + pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose); // check if the parking path will leave lanes const auto drivable_lanes = @@ -218,8 +272,10 @@ std::optional ShiftPullOver::generatePullOverPath( const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); + const auto combined_drivable = utils::combineDrivableLanes( + expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); if (lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) { + utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) { return {}; } diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 1a1e66f85b403..dfdaa2a3a18c1 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -196,22 +196,138 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -bool isAllowedGoalModification(const std::shared_ptr & route_handler) +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path) { - return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); + double lateral_deviation = 0.0; + for (const auto & target_point : target_path.points) { + const size_t nearest_index = + motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); + lateral_deviation = std::max( + lateral_deviation, + std::abs(tier4_autoware_utils::calcLateralDeviation( + reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); + } + return lateral_deviation; +} + +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_threshold) +{ + return calcLateralDeviationBetweenPaths(reference_path, target_path) < + lateral_deviation_threshold; +} + +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose) +{ + const size_t end_idx = motion_utils::findNearestSegmentIndex(path.points, end_pose.position); + std::vector clipped_points{ + path.points.begin(), path.points.begin() + end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_path = path; + clipped_path.points = clipped_points; + + return clipped_path; +} + +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length) +{ + const auto & points = path.points; + + double sum_length = 0; + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + const double seg_length = tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (forward_length < sum_length + seg_length) { + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.begin() + i}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + + // add precise end pose to cropped points + const double remaining_length = forward_length - sum_length; + const Pose precise_end_pose = + calcOffsetPose(cropped_path.points.back().point.pose, remaining_length, 0, 0); + if (remaining_length < 0.1) { + // if precise_end_pose is too close, replace the last point + cropped_path.points.back().point.pose = precise_end_pose; + } else { + auto precise_end_point = cropped_path.points.back(); + precise_end_point.point.pose = precise_end_pose; + cropped_path.points.push_back(precise_end_point); + } + return cropped_path; + } + sum_length += seg_length; + } + + // if forward_length is too long, return points after target_seg_idx + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.end()}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + return cropped_path; } -bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const double extend_length) { - const Pose & goal_pose = route_handler->getOriginalGoalPose(); - const auto shoulder_lanes = route_handler->getShoulderLanelets(); + const auto & target_terminal_pose = target_path.points.back().point.pose; + + // generate clipped road lane reference path from previous module path terminal pose to shift end + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); - lanelet::ConstLanelet closest_shoulder_lane{}; - if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { - return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + PathWithLaneId clipped_path = + cropForwardPoints(reference_path, target_path_terminal_idx, extend_length); + + // shift clipped path to previous module path terminal pose + const double lateral_shift_from_reference_path = + motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); + for (auto & p : clipped_path.points) { + p.point.pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0); } - return false; + auto extended_path = target_path; + const auto start_point = + std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { + const bool is_forward = + tier4_autoware_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose).x > + 0.0; + const bool is_close = tier4_autoware_utils::calcDistance2d( + p.point.pose.position, target_terminal_pose.position) < 0.1; + return is_forward && !is_close; + }); + std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points)); + + extended_path.points = motion_utils::removeOverlapPoints(extended_path.points); + + return extended_path; +} + +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const Pose & extend_pose) +{ + const auto & target_terminal_pose = target_path.points.back().point.pose; + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); + const double extend_distance = motion_utils::calcSignedArcLength( + reference_path.points, target_path_terminal_idx, extend_pose.position); + + return extendPath(target_path, reference_path, extend_distance); } } // namespace behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index 01b2880f4362a..dcee7696933dd 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -221,6 +221,9 @@ PathWithLaneId refinePathForGoal( bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id); +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); + bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); bool isInLaneletWithYawThreshold( @@ -303,6 +306,10 @@ lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data, const double backward_length, const double forward_length, const bool forward_only_in_route); +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route); + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, const double forward_length, const double backward_length, diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index e6f9ce21cebf7..308cf9c4fed2c 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1450,6 +1450,70 @@ lanelet::ConstLanelets getExtendedCurrentLanes( return extendLanes(planner_data->route_handler, getCurrentLanes(planner_data)); } +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route) +{ + auto lanes = getCurrentLanesFromPath(path, planner_data); + + if (lanes.empty()) return lanes; + const auto start_lane = lanes.front(); + double forward_length_sum = 0.0; + double backward_length_sum = 0.0; + + while (backward_length_sum < backward_length) { + auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for backward_length, + // the extending process will not finish. + if (extended_lanes.front().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + backward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.front()); + } else { + break; // no more previous lanes to add + } + lanes = extended_lanes; + } + + while (forward_length_sum < forward_length) { + auto extended_lanes = extendNextLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for forward_length, + // the extending process will not finish. + if (extended_lanes.back().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + forward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.back()); + } else { + break; // no more next lanes to add + } + + // stop extending when the lane outside of the route is reached + // if forward_length is a very large value, set it to true, + // as it may continue to extend forever. + if (forward_only_in_route) { + if (!planner_data->route_handler->isRouteLanelet(extended_lanes.back())) { + return lanes; + } + } + + lanes = extended_lanes; + } + + return lanes; +} + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const Pose & pose, const double forward_length, const double backward_length, const double dist_threshold, const double yaw_threshold) @@ -1551,4 +1615,22 @@ std::string convertToSnakeCase(const std::string & input_str) } return output_str; } + +bool isAllowedGoalModification(const std::shared_ptr & route_handler) +{ + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); +} + +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +{ + const Pose & goal_pose = route_handler->getOriginalGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); + + lanelet::ConstLanelet closest_shoulder_lane{}; + if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + } + + return false; +} } // namespace behavior_path_planner::utils From fe8f28396f655defb4e67f1f33d01d76a8e2d668 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Wed, 17 Jan 2024 11:40:35 +0900 Subject: [PATCH 057/154] feat(yabloc): add yabloc trigger service to suspend and restart the estimation (#6076) * change arg default value Signed-off-by: Kento Yabuuchi * add yabloc_trigger_service Signed-off-by: Kento Yabuuchi * fix misc Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi --- .../launch/image_processing.launch.xml | 2 +- .../yabloc/yabloc_particle_filter/README.md | 6 +++ .../prediction/predictor.hpp | 9 ++++ .../launch/yabloc_particle_filter.launch.xml | 3 ++ .../src/prediction/predictor.cpp | 41 ++++++++++++++++++- 5 files changed, 59 insertions(+), 2 deletions(-) diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index aa6fc10ee6667..a0cad16302c2b 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/localization/yabloc/yabloc_particle_filter/README.md b/localization/yabloc/yabloc_particle_filter/README.md index 41ab7b1b10039..d42ce4647bc95 100644 --- a/localization/yabloc/yabloc_particle_filter/README.md +++ b/localization/yabloc/yabloc_particle_filter/README.md @@ -46,6 +46,12 @@ This package contains some executable nodes related to particle filter. | `prediction_rate` | double | frequency of forecast updates, in Hz | | `cov_xx_yy` | vector\ | the covariance of initial pose | +### Services + +| Name | Type | Description | +| -------------------- | ------------------------ | ------------------------------------------------ | +| `yabloc_trigger_srv` | `std_srvs::srv::SetBool` | activation and deactivation of yabloc estimation | + ## gnss_particle_corrector ### Purpose diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp index c7794a2be9f51..e02393ee14db6 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,7 @@ class Predictor : public rclcpp::Node using TwistCovStamped = geometry_msgs::msg::TwistWithCovarianceStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; using Marker = visualization_msgs::msg::Marker; + using SetBool = std_srvs::srv::SetBool; Predictor(); @@ -62,6 +64,7 @@ class Predictor : public rclcpp::Node const std::vector cov_xx_yy_; // Subscriber + rclcpp::Subscription::SharedPtr ekf_pose_sub_; rclcpp::Subscription::SharedPtr initialpose_sub_; rclcpp::Subscription::SharedPtr twist_cov_sub_; rclcpp::Subscription::SharedPtr particles_sub_; @@ -73,11 +76,15 @@ class Predictor : public rclcpp::Node rclcpp::Publisher::SharedPtr pose_cov_pub_; rclcpp::Publisher::SharedPtr marker_pub_; std::unique_ptr tf2_broadcaster_; + // Server + rclcpp::Service::SharedPtr yabloc_trigger_server_; // Timer callback rclcpp::TimerBase::SharedPtr timer_; float ground_height_{0}; + bool yabloc_activated_{true}; + PoseCovStamped::ConstSharedPtr latest_ekf_pose_ptr_{nullptr}; std::optional particle_array_opt_{std::nullopt}; std::optional latest_twist_opt_{std::nullopt}; @@ -92,6 +99,8 @@ class Predictor : public rclcpp::Node void on_twist_cov(const TwistCovStamped::ConstSharedPtr twist); void on_weighted_particles(const ParticleArray::ConstSharedPtr weighted_particles); void on_timer(); + void on_trigger_service( + SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response); // void initialize_particles(const PoseCovStamped & initialpose); diff --git a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml index f38264b828d34..5dd483f142174 100644 --- a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml +++ b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml @@ -16,6 +16,9 @@ + + + diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp index b16905ba19b41..764ef88bbde18 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp @@ -52,7 +52,12 @@ Predictor::Predictor() auto on_initial = std::bind(&Predictor::on_initial_pose, this, _1); auto on_twist_cov = std::bind(&Predictor::on_twist_cov, this, _1); auto on_particle = std::bind(&Predictor::on_weighted_particles, this, _1); - auto on_height = [this](std_msgs::msg::Float32 m) -> void { this->ground_height_ = m.data; }; + auto on_height = [this](std_msgs::msg::Float32::ConstSharedPtr msg) -> void { + this->ground_height_ = msg->data; + }; + auto on_ekf_pose = [this](PoseCovStamped::ConstSharedPtr msg) -> void { + this->latest_ekf_pose_ptr_ = msg; + }; initialpose_sub_ = create_subscription("~/input/initialpose", 1, on_initial); particles_sub_ = @@ -60,6 +65,7 @@ Predictor::Predictor() height_sub_ = create_subscription("~/input/height", 10, on_height); twist_cov_sub_ = create_subscription("~/input/twist_with_covariance", 10, on_twist_cov); + ekf_pose_sub_ = create_subscription("~/input/ekf_pose", 10, on_ekf_pose); // Timer callback const double prediction_rate = declare_parameter("prediction_rate"); @@ -67,12 +73,41 @@ Predictor::Predictor() timer_ = rclcpp::create_timer( this, this->get_clock(), rclcpp::Rate(prediction_rate).period(), std::move(on_timer)); + // Service server + using std::placeholders::_2; + auto on_trigger_service = std::bind(&Predictor::on_trigger_service, this, _1, _2); + yabloc_trigger_server_ = create_service("~/yabloc_trigger_srv", on_trigger_service); + // Optional modules if (declare_parameter("visualize", false)) { visualizer_ptr_ = std::make_unique(*this); } } +void Predictor::on_trigger_service( + SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response) +{ + if (request->data) { + RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is activated"); + } else { + RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is deactivated"); + } + + const bool before_activated_ = yabloc_activated_; + yabloc_activated_ = request->data; + response->success = true; + + if (yabloc_activated_ && (!before_activated_)) { + RCLCPP_INFO_STREAM(get_logger(), "restart particle filter"); + if (latest_ekf_pose_ptr_) { + on_initial_pose(latest_ekf_pose_ptr_); + } else { + yabloc_activated_ = false; + response->success = false; + } + } +} + void Predictor::on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose) { // Publish initial pose marker @@ -181,6 +216,10 @@ void Predictor::on_timer() { // ========================================================================== // Pre-check section + // Return if yabloc is not activated + if (!yabloc_activated_) { + return; + } // Return if particle_array is not initialized yet if (!particle_array_opt_.has_value()) { return; From bdd8d65d084b4505a092d97d0b05225e7303eef8 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 17 Jan 2024 14:21:43 +0900 Subject: [PATCH 058/154] feat(goal_planner): exclude goals located laterally in no_parking_areas (#6095) Signed-off-by: kosuke55 --- .../behavior_path_goal_planner_module/src/goal_searcher.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index bf359be3de18b..31f99e8ea5aa1 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -170,11 +170,13 @@ GoalCandidates GoalSearcher::search() transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) { - continue; + // break here to exclude goals located laterally in no_parking_areas + break; } if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) { - continue; + // break here to exclude goals located laterally in no_stopping_areas + break; } if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { From 67d7acf6f9a0f8343c9e1ba0e644206ca92914d6 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Wed, 17 Jan 2024 16:12:32 +0900 Subject: [PATCH 059/154] chore(mission_planner): add logs for reroute safety check (#6096) * chore(mission_planner): add logs for reroute safety check Signed-off-by: Makoto Kurihara * style(pre-commit): autofix --------- Signed-off-by: Makoto Kurihara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../mission_planner/src/mission_planner/mission_planner.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 9ee874928d7d1..e0181065150fd 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -731,6 +731,7 @@ bool MissionPlanner::check_reroute_safety( const LaneletRoute & original_route, const LaneletRoute & target_route) { if (original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ || !odometry_) { + RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Route, map or odometry is not set."); return false; } @@ -803,6 +804,8 @@ bool MissionPlanner::check_reroute_safety( return std::nullopt; }); if (!start_idx_opt.has_value()) { + RCLCPP_ERROR( + get_logger(), "Check reroute safety failed. Cannot find the start index of the route."); return false; } const size_t start_idx = start_idx_opt.value(); @@ -838,6 +841,7 @@ bool MissionPlanner::check_reroute_safety( // get closest lanelet in start lanelets lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) { + RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet."); return false; } From 0466378fed980ac8508b9929b5cc0add3f32971d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 17 Jan 2024 17:43:45 +0900 Subject: [PATCH 060/154] fix(AbLC, lane_change): fix module name inconsistency (#6090) * fix(AbLC): rename Signed-off-by: satoshi-ota * fix(AbLC, LC): fix duplicated rtc setting Signed-off-by: satoshi-ota * fix(bpp): fix module name inconsistency Signed-off-by: satoshi-ota * fix(AbLC): rename Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- ..._lc.param.yaml => avoidance_by_lane_change.param.yaml} | 0 .../manager.hpp | 2 +- .../src/manager.cpp | 2 +- .../test/test_behavior_path_planner_node_interface.cpp | 2 +- .../include/behavior_path_lane_change_module/manager.hpp | 2 ++ planning/behavior_path_lane_change_module/src/manager.cpp | 8 ++++++-- .../config/scene_module_manager.param.yaml | 2 +- planning/behavior_path_planner/src/planner_manager.cpp | 8 +++----- 8 files changed, 15 insertions(+), 11 deletions(-) rename planning/behavior_path_avoidance_by_lane_change_module/config/{avoidance_by_lc.param.yaml => avoidance_by_lane_change.param.yaml} (100%) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml rename to planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp index d996555365166..75fa67e7fe1a3 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp @@ -35,7 +35,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager public: AvoidanceByLaneChangeModuleManager() : LaneChangeModuleManager( - "avoidance_by_lc", route_handler::Direction::NONE, + "avoidance_by_lane_change", route_handler::Direction::NONE, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { } diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 3f810710ef37b..4bdcb51f1eab5 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -37,7 +37,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) initInterface(node, {"left", "right"}); // init lane change manager - LaneChangeModuleManager::init(node); + LaneChangeModuleManager::initParams(node); const auto avoidance_params = getParameter(node); AvoidanceByLCParameters p(avoidance_params); diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 581aafa6c860c..1167fa4414752 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -70,7 +70,7 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + "/config/avoidance.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + - "/config/avoidance_by_lc.param.yaml"}); + "/config/avoidance_by_lane_change.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp index b34f691bd2b0d..31561b6210591 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp @@ -45,6 +45,8 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; protected: + void initParams(rclcpp::Node * node); + std::shared_ptr parameters_; Direction direction_; diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index e4405a31a360d..0a56c17d89fd0 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -29,10 +29,14 @@ namespace behavior_path_planner void LaneChangeModuleManager::init(rclcpp::Node * node) { - using tier4_autoware_utils::getOrDeclareParameter; - // init manager interface initInterface(node, {""}); + initParams(node); +} + +void LaneChangeModuleManager::initParams(rclcpp::Node * node) +{ + using tier4_autoware_utils::getOrDeclareParameter; LaneChangeParameters p{}; diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index 91e34c0e91931..439e08fd7e94f 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -67,7 +67,7 @@ priority: 4 max_module_size: 1 - avoidance_by_lc: + avoidance_by_lane_change: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 262de4764fcfe..a1d0a82be38b1 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -859,11 +859,9 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) // when lane change module is running, don't update root lanelet. const bool is_lane_change_running = std::invoke([&]() { - const auto lane_change_itr = - std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) { - return m->name().find("lane_change") != std::string::npos || - m->name().find("avoidance_by_lc") != std::string::npos; - }); + const auto lane_change_itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->isRootLaneletToBeUpdated(); }); return lane_change_itr != approved_module_ptrs_.end(); }); if (is_lane_change_running) { From dc3349b8974648b6cf136a910e7707388c907fd4 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 17 Jan 2024 18:14:36 +0900 Subject: [PATCH 061/154] fix(avoidance): fix detection area issue in avoidance module (#6097) * fix(avoidance): fix invalid vector access Signed-off-by: satoshi-ota * fix(avoidance): fix detection area Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/utils.cpp | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 80aeb2bf0dccf..3a9b5db283304 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1913,6 +1913,10 @@ std::pair separateObjectsByPath( PredictedObjects target_objects; PredictedObjects other_objects; + if (reference_path.points.empty() || spline_path.points.empty()) { + return std::make_pair(target_objects, other_objects); + } + double max_offset = 0.0; for (const auto & object_parameter : parameters->object_parameters) { const auto p = object_parameter.second; @@ -1925,16 +1929,15 @@ std::pair separateObjectsByPath( createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); const auto ego_idx = planner_data->findEgoIndex(reference_path.points); const auto arc_length_array = - utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 1e-3); + utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 0.0); - double next_longitudinal_distance = 0.0; - std::vector detection_areas; - for (size_t i = 0; i < reference_path.points.size() - 1; ++i) { - const auto & p_reference_ego_front = reference_path.points.at(i).point.pose; - const auto & p_reference_ego_back = reference_path.points.at(i + 1).point.pose; - const auto & p_spline_ego_front = spline_path.points.at(i).point.pose; - const auto & p_spline_ego_back = spline_path.points.at(i + 1).point.pose; + const auto points_size = std::min(reference_path.points.size(), spline_path.points.size()); + std::vector detection_areas; + Pose p_reference_ego_front = reference_path.points.front().point.pose; + Pose p_spline_ego_front = spline_path.points.front().point.pose; + double next_longitudinal_distance = parameters->resample_interval_for_output; + for (size_t i = 0; i < points_size; ++i) { const auto distance_from_ego = calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; @@ -1944,10 +1947,16 @@ std::pair separateObjectsByPath( continue; } + const auto & p_reference_ego_back = reference_path.points.at(i).point.pose; + const auto & p_spline_ego_back = spline_path.points.at(i).point.pose; + detection_areas.push_back(createOneStepPolygon( p_reference_ego_front, p_reference_ego_back, p_spline_ego_front, p_spline_ego_back, detection_area)); + p_reference_ego_front = p_reference_ego_back; + p_spline_ego_front = p_spline_ego_back; + next_longitudinal_distance += parameters->resample_interval_for_output; } From d88f7d2f2c4e2778fc6639159f5c0bca55f8e7fd Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Wed, 17 Jan 2024 20:21:13 +0900 Subject: [PATCH 062/154] chore: add localization & mapping maintainers (#6085) * Added lm maintainers Signed-off-by: Shintaro SAKODA * Add more Signed-off-by: Shintaro SAKODA * Fixed maintainer Signed-off-by: Shintaro SAKODA --------- Signed-off-by: Shintaro SAKODA --- launch/tier4_localization_launch/package.xml | 3 +++ localization/ekf_localizer/package.xml | 5 +++++ localization/geo_pose_projector/package.xml | 6 ++++++ localization/gyro_odometer/package.xml | 5 +++++ .../ar_tag_based_localizer/package.xml | 3 +++ .../landmark_based_localizer/landmark_manager/package.xml | 3 +++ localization/localization_error_monitor/package.xml | 5 +++++ localization/localization_util/package.xml | 3 +++ localization/ndt_scan_matcher/package.xml | 4 ++++ localization/pose2twist/package.xml | 5 +++++ localization/pose_initializer/package.xml | 5 +++++ localization/pose_instability_detector/package.xml | 1 + localization/stop_filter/package.xml | 5 +++++ localization/tree_structured_parzen_estimator/package.xml | 3 +++ localization/twist2accel/package.xml | 5 +++++ localization/yabloc/yabloc_common/package.xml | 5 +++++ localization/yabloc/yabloc_image_processing/package.xml | 5 +++++ localization/yabloc/yabloc_monitor/package.xml | 5 +++++ localization/yabloc/yabloc_particle_filter/package.xml | 5 +++++ localization/yabloc/yabloc_pose_initializer/package.xml | 5 +++++ map/map_height_fitter/package.xml | 4 ++++ map/map_loader/package.xml | 3 +++ map/map_projection_loader/package.xml | 4 ++++ map/map_tf_generator/package.xml | 4 ++++ 24 files changed, 101 insertions(+) diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index c4de9c04dcaf2..9b4f727c9ce52 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -8,6 +8,9 @@ Koji Minoda Kento Yabuuchi Ryu Yamamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda Apache License 2.0 Yamato Ando diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 4101bae88b6e2..e9d756e547f26 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -9,6 +9,11 @@ Yamato Ando Takeshi Ishita Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Takamasa Horibe diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml index d424ff14b9c99..c0e2db59deb64 100644 --- a/localization/geo_pose_projector/package.xml +++ b/localization/geo_pose_projector/package.xml @@ -6,6 +6,12 @@ The geo_pose_projector package Yamato Ando Koji Minoda + Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Koji Minoda diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index ad4a865a2014a..a0a982ddedc16 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -6,6 +6,11 @@ The gyro_odometer package as a ROS 2 node Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 857ec4f16f051..072479cc7aaf5 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Yamato Ando Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 ament_cmake diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml index 2e0e252386d1f..be19de9334e84 100644 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Yamato Ando Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 ament_cmake diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 266ae6c848834..a1b352d911a0d 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -7,6 +7,11 @@ Yamato Ando Koji Minoda Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Taichi Higashide diff --git a/localization/localization_util/package.xml b/localization/localization_util/package.xml index 20f9b160212b5..fa8da2aa1231a 100644 --- a/localization/localization_util/package.xml +++ b/localization/localization_util/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Shintaro Sakoda Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index dcfdd77cb5a18..0913ee04174f5 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -8,6 +8,10 @@ Kento Yabuuchi Koji Minoda Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 83bce5a718334..16c49bb51c218 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -6,6 +6,11 @@ The pose2twist package Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 1a1a8e48b7e3b..85c9c26bd6c8c 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -7,6 +7,11 @@ Yamato Ando Takagi, Isamu Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando Takagi, Isamu diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index 7774407a7990d..bf57e5589b747 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -10,6 +10,7 @@ Taiki Yamada Ryu Yamamoto Shintaro Sakoda + NGUYEN Viet Anh Apache License 2.0 Shintaro Sakoda diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index b17809aaa9948..83eaf1b9fa821 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -7,6 +7,11 @@ Koji Minoda Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Koji Minoda diff --git a/localization/tree_structured_parzen_estimator/package.xml b/localization/tree_structured_parzen_estimator/package.xml index b699d5c69e610..96d2b9ecf54cc 100644 --- a/localization/tree_structured_parzen_estimator/package.xml +++ b/localization/tree_structured_parzen_estimator/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Shintaro Sakoda Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 Shintaro Sakoda diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index c44cd9d144566..08dacf9185769 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -7,6 +7,11 @@ Koji Minoda Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Koji Minoda diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index d7e94ebefa24b..710ec0aeb75f8 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -5,6 +5,11 @@ YabLoc common library Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index ffdcea25b4b6d..209f09fdaa7ac 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -6,6 +6,11 @@ YabLoc image processing package Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml index 8c92c3c6a4303..22a5d0eded6b6 100644 --- a/localization/yabloc/yabloc_monitor/package.xml +++ b/localization/yabloc/yabloc_monitor/package.xml @@ -7,6 +7,11 @@ Kento Yabuuchi Koji Minoda Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index aabb6009c2148..5db4aa0c29e88 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -6,6 +6,11 @@ YabLoc particle filter package Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index 4e3ca1c1e61d1..e9921db50093e 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -5,6 +5,11 @@ The pose initializer Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 2dc864b0cb925..85258a8e54b22 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -8,6 +8,10 @@ Yamato Ando Masahiro Sakamoto Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Takagi, Isamu diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index c8fcce6f7002f..0103c7d2b74a7 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -9,6 +9,9 @@ Koji Minoda Masahiro Sakamoto Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda Apache License 2.0 diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 76e40d4379de4..475881577bd58 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -8,6 +8,10 @@ Yamato Ando Masahiro Sakamoto Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_auto diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml index 6e266b0ad82f0..6f08da169309e 100644 --- a/map/map_tf_generator/package.xml +++ b/map/map_tf_generator/package.xml @@ -7,6 +7,10 @@ Yamato Ando Kento Yabuuchi Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_auto From f78ea5985d07bc6a333c7f155639cf4b52050c41 Mon Sep 17 00:00:00 2001 From: Ioannis Souflas Date: Wed, 17 Jan 2024 15:07:15 +0200 Subject: [PATCH 063/154] feat(obstacle_cruise_planner): add calculation of obstacle distance to ego (#6057) Add the arc length from the ego to the obstacle stop point to the stop_reasons topic. Signed-off-by: Ioannis Souflas --- .../obstacle_cruise_planner/src/planner_interface.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index f00ad01efb0d9..f7ce218cf3ccf 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -37,19 +37,21 @@ StopSpeedExceeded createStopSpeedExceededMsg( } tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( - const rclcpp::Time & current_time, const geometry_msgs::msg::Pose & stop_pose, + const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose, const StopObstacle & stop_obstacle) { // create header std_msgs::msg::Header header; header.frame_id = "map"; - header.stamp = current_time; + header.stamp = planner_data.current_time; // create stop factor StopFactor stop_factor; stop_factor.stop_pose = stop_pose; geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point; stop_factor_point.z = stop_pose.position.z; + stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( + planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); stop_factor.stop_factor_points.emplace_back(stop_factor_point); // create stop reason stamped @@ -359,7 +361,7 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; const auto stop_reasons_msg = - makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle); + makeStopReasonArray(planner_data, stop_pose, *closest_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); From bc1751ee1f2fd22c862f4be3c8b1de8435b74c31 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Wed, 17 Jan 2024 23:14:07 +0900 Subject: [PATCH 064/154] fix: rename `score_threshold` into `score_thresh` in config (#6098) --- perception/tensorrt_yolo/config/yolov3.param.yaml | 2 +- perception/tensorrt_yolo/config/yolov4-tiny.param.yaml | 2 +- perception/tensorrt_yolo/config/yolov4.param.yaml | 2 +- perception/tensorrt_yolo/config/yolov5l.param.yaml | 2 +- perception/tensorrt_yolo/config/yolov5m.param.yaml | 2 +- perception/tensorrt_yolo/config/yolov5s.param.yaml | 2 +- perception/tensorrt_yolo/config/yolov5x.param.yaml | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/perception/tensorrt_yolo/config/yolov3.param.yaml b/perception/tensorrt_yolo/config/yolov3.param.yaml index 5ce50f3acfa96..d97505dae5020 100644 --- a/perception/tensorrt_yolo/config/yolov3.param.yaml +++ b/perception/tensorrt_yolo/config/yolov3.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [116.0, 90.0, 156.0, 198.0, 373.0, 326.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 10.0, 13.0, 16.0, 30.0, 33.0, 23.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: true diff --git a/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml b/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml index 2ee53ee68f764..8071da9dcac06 100644 --- a/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml +++ b/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [81.0, 82.0, 135.0, 169.0, 344.0, 319.0, 23.0, 27.0, 37.0, 58.0, 81.0, 82.0] scale_x_y: [1.05, 1.05] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: true diff --git a/perception/tensorrt_yolo/config/yolov4.param.yaml b/perception/tensorrt_yolo/config/yolov4.param.yaml index 6122af667c470..9edc0fc6ce708 100644 --- a/perception/tensorrt_yolo/config/yolov4.param.yaml +++ b/perception/tensorrt_yolo/config/yolov4.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [12.0, 16.0, 19.0, 36.0, 40.0, 28.0, 36.0, 75.0, 76.0, 55.0, 72.0, 146.0, 142.0, 110.0, 192.0, 243.0, 459.0, 401.0] scale_x_y: [1.2, 1.1, 1.05] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: true diff --git a/perception/tensorrt_yolo/config/yolov5l.param.yaml b/perception/tensorrt_yolo/config/yolov5l.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5l.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5l.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/perception/tensorrt_yolo/config/yolov5m.param.yaml b/perception/tensorrt_yolo/config/yolov5m.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5m.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5m.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/perception/tensorrt_yolo/config/yolov5s.param.yaml b/perception/tensorrt_yolo/config/yolov5s.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5s.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5s.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/perception/tensorrt_yolo/config/yolov5x.param.yaml b/perception/tensorrt_yolo/config/yolov5x.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5x.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5x.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false From eda87bb0de6247a57ee9cdbde7427c00ae4f9bd5 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 18 Jan 2024 02:20:37 +0900 Subject: [PATCH 065/154] chore: update api package maintainers (#6086) * update api maintainers Signed-off-by: Takagi, Isamu * fix Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- common/autoware_ad_api_specs/package.xml | 2 -- common/component_interface_specs/package.xml | 2 -- common/component_interface_tools/package.xml | 2 -- common/component_interface_utils/package.xml | 2 -- common/path_distance_calculator/package.xml | 2 -- common/tier4_api_utils/package.xml | 2 -- common/tier4_datetime_rviz_plugin/package.xml | 2 -- common/tier4_localization_rviz_plugin/package.xml | 2 -- launch/tier4_autoware_api_launch/package.xml | 2 -- system/component_state_monitor/package.xml | 2 -- system/default_ad_api/package.xml | 2 -- system/default_ad_api_helpers/ad_api_adaptors/package.xml | 2 -- system/default_ad_api_helpers/ad_api_visualizers/package.xml | 2 -- .../automatic_pose_initializer/package.xml | 2 -- 14 files changed, 28 deletions(-) diff --git a/common/autoware_ad_api_specs/package.xml b/common/autoware_ad_api_specs/package.xml index d4d82faf4ee78..0d14ccff758b0 100644 --- a/common/autoware_ad_api_specs/package.xml +++ b/common/autoware_ad_api_specs/package.xml @@ -5,8 +5,6 @@ 0.0.0 The autoware_ad_api_specs package Takagi, Isamu - yabuta - Kah Hooi Tan Ryohsuke Mitsudome Apache License 2.0 diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 93f77c651869d..67279d0ae2b7f 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -5,8 +5,6 @@ 0.0.0 The component_interface_specs package Takagi, Isamu - yabuta - Kah Hooi Tan Yukihiro Saito Apache License 2.0 diff --git a/common/component_interface_tools/package.xml b/common/component_interface_tools/package.xml index ebf0684d0066c..cff1829473e86 100644 --- a/common/component_interface_tools/package.xml +++ b/common/component_interface_tools/package.xml @@ -5,8 +5,6 @@ 0.1.0 The component_interface_tools package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index dc82fda3f5c64..7a6a6d12880ad 100755 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -5,8 +5,6 @@ 0.0.0 The component_interface_utils package Takagi, Isamu - yabuta - Kah Hooi Tan Yukihiro Saito Apache License 2.0 diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index b5e770a0ea624..e796bbd0d20f7 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -5,8 +5,6 @@ 0.0.0 The path_distance_calculator package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake diff --git a/common/tier4_api_utils/package.xml b/common/tier4_api_utils/package.xml index 091f512480150..d05b810caf904 100644 --- a/common/tier4_api_utils/package.xml +++ b/common/tier4_api_utils/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_api_utils package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_datetime_rviz_plugin/package.xml b/common/tier4_datetime_rviz_plugin/package.xml index 6dc0c09b32a3b..82e1e82c61ba2 100644 --- a/common/tier4_datetime_rviz_plugin/package.xml +++ b/common/tier4_datetime_rviz_plugin/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_datetime_rviz_plugin package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index e103db19f1d43..f2482b2d45a3b 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -5,8 +5,6 @@ 0.1.0 The tier4_localization_rviz_plugin package Takagi, Isamu - yabuta - Kah Hooi Tan Takamasa Horibe Apache License 2.0 diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index 4d41ad0ac4a83..c55f1c8038e83 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_autoware_api_launch package Takagi, Isamu - yabuta - Kah Hooi Tan Ryohsuke Mitsudome Apache License 2.0 diff --git a/system/component_state_monitor/package.xml b/system/component_state_monitor/package.xml index 60d0fd0577ef0..16903fa216174 100644 --- a/system/component_state_monitor/package.xml +++ b/system/component_state_monitor/package.xml @@ -5,8 +5,6 @@ 0.1.0 The component_state_monitor package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 6a9a22f39cdc4..4e691f7ab3192 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -5,8 +5,6 @@ 0.1.0 The default_ad_api package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index e54a5faa6e1b7..785ff58db1f81 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -5,8 +5,6 @@ 0.1.0 The ad_api_adaptors package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/default_ad_api_helpers/ad_api_visualizers/package.xml b/system/default_ad_api_helpers/ad_api_visualizers/package.xml index 95961769813cd..75fd09a1335b1 100644 --- a/system/default_ad_api_helpers/ad_api_visualizers/package.xml +++ b/system/default_ad_api_helpers/ad_api_visualizers/package.xml @@ -5,8 +5,6 @@ 0.1.0 The ad_api_visualizers package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml index 10f4abfe446fb..be192c3dade4b 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml +++ b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml @@ -5,8 +5,6 @@ 0.1.0 The automatic_pose_initializer package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 From fe11f2339fdda073c62f356e24c6dee870c6755e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 18 Jan 2024 05:55:13 +0900 Subject: [PATCH 066/154] feat(rviz_plugin): add string visualization plugin (#6100) Signed-off-by: satoshi-ota --- common/tier4_debug_rviz_plugin/CMakeLists.txt | 2 + .../classes/StringStampedOverlayDisplay.png | Bin 0 -> 18815 bytes .../string_stamped.hpp | 107 ++++++++++ .../plugins/plugin_description.xml | 5 + .../src/string_stamped.cpp | 198 ++++++++++++++++++ 5 files changed, 312 insertions(+) create mode 100644 common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png create mode 100644 common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp create mode 100644 common/tier4_debug_rviz_plugin/src/string_stamped.cpp diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt b/common/tier4_debug_rviz_plugin/CMakeLists.txt index 05cf35b93ef9f..02e65ad759ed6 100644 --- a/common/tier4_debug_rviz_plugin/CMakeLists.txt +++ b/common/tier4_debug_rviz_plugin/CMakeLists.txt @@ -12,7 +12,9 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(tier4_debug_rviz_plugin SHARED include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp + include/tier4_debug_rviz_plugin/string_stamped.hpp src/float32_multi_array_stamped_pie_chart.cpp + src/string_stamped.cpp src/jsk_overlay_utils.cpp ) diff --git a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png new file mode 100644 index 0000000000000000000000000000000000000000..6a67573717ae18b6008e00077defb27256415663 GIT binary patch literal 18815 zcmc#)ig}7!VL~0BI3P>4u>jX{5WQyQGE==@gXiZcsv`yJ3)ShVHnpzyIKV zx%UIl!@PCQ*=Oy&*IwtGaAid)9L!gkAP@*gMjEOL0wKly`$K;YTnXsp8Ulf2BxRtY zY95&f%V-`irs2ZJD-E_>W|QJ#$f(lDGUzn9FhfdJ6ls+f7g13tl1h)8=nGVpD&-eU zEXJbkUMvxiN-bt%sdy?0Ws#;2U}Tww$D9k?tJrv3b90N4w6q7mY>Vu>j)cq`Gq>&g ztxVotd7x=_%0QrfCTU6#h!Tqc9R&LG1r-?ts`)>?Y`h5pRS_lWag8OXawfXJHk9@r zwSCxzbPRm-g9n1hq$!Q)8a9)#!i(M_cz&h|6vnpsyKVD7twGSL0-4KL6Q=BD1ZeP- zzfzYzWVM-q+?d)>KwJl;5;rOeaVP-p8?mb$R~c z!r4STVAB1bJ?W`Q1N^wk6a>;QVI%JxOu}Vf{^G|)Co3MGH{(9RMV<;nHU4Za)%KSC z`;I=`QN*K~)?CjlT%q3CJ0d$G%J_7^;$JI4Ms3f{6oyJDbjK_N!4ju2 z&$?3P$rm$WTureE-aq`r%H%qf&xx}1+^E65XE7*U*@ASHv#;8ex#{Xeuo?I&+x*og zCp;~E10-qE582|U!y4K~`SF#B31Gjpj3?LDSx6hf<#=NRr5x>o;P$^l9osYrEfL{WYjQydU9;g?K5#3k=W*FphTCuCHewl5&`Uu9F)(f>MMOxB>pE z88huBN|GrNq|eH~M(ELpl-tdZkMhs%;OV+^2?6{7^3=m>h4(Sn1AF4SLTeDL;O9g! zBWaM6c%X0DechlP8B_BUQ^1_B5ImG0q$&AGba@Q1!Sx#uv;b)FNlKJ%FF!gu4goAi z+jugMiX+lt3%Db%$x}1fa)yh%A&xxGW-Lle9BaRZcr(Jl8YJvmXVoM8b*d?nDn zr>xqjT|4gjOmgjyb!-I_L=1WSO(};jF5)F9(KK9k$6juv0YKyrQRHz_DK~8jI2)+w zm-Yt$QfvYLhOutzxp@d+SUG4qO=vJ%C2^D=7atD;N#3;^c;DQzmR&&!oPfkcBJT}x-4FY#kT02m@DxoqS-{xT|$ z3PAj4Y06h5S$bR{FjZhmf2ImPCOB3Cm{OLqda0jhqvfLSxmZ+?A2<^h+z64sYjMk*OaM^512bTNz1yJX#3oL;p>F~$69vB)J^K&J0kyCuHxo2qc9?N7 zpx^)Vh!`XhEMW3-8hQ#=jbwx3L_Yk3cspR5f1h~;US}c=&En=n$AFqJRa-Trw&}AF z#AgA`>wx2@JWAg4l9l5}1GY|0Z9CNsXNdgw(z**Vj@S4BOp0I}AA5Z|Zl~>Z3kVbj zqQ+>~?`j3P7S~|HAAK#cX8I@4)zn@Yx%DiDRJ@~c4bTC4ySPXJL&So)v8RA@6#{J%M_y$DCRzdd0*FPR zEH--gPG5+|{w5Yegr~;hMSkzb)vYj91mGV5xcRrf*}fIu`8(Gk(oJHQTMWSTlpe3W z7U|f@97Ozui1Pqn>xtK!34&Gp1$<#-lg>4RGs7K!6%!+v&|>f%Vhw^400u&sGY~q5 zJ^R2hK)+x%+?N_ujGMppd5=D@{V_pS_0HC2*7x*eApoKv>S`$MhIW>f zMXBEn^5P16FfqDGOxdOTX$;a&7%;G-28$P)$1&8Il!2^?K;(-9A|F432ZYVYM^|}I z3>OV1q`?mb9wc7}dJzwJ)_0FSe8o7Cm(><8hzS0{vBcWu7VZ9!xVd$!*%0qZqPEJb=%hJ$Z_L zz~Miu9bQOq9)39Df;+w>C4k8SnA^IR``!NH4|SUf61t|;@Y}Cs7L7w2kT8`ufki#> z(|*WcvwwZbRQK{xx%^U^9it@ui(i`PZ(a!{U!?$lf%er!QwQi?Jq@BTnXsQ0^KiZy z)@H)?s!l`=|Lg4jvo;>IFs7CP;9l8V4UpAo>sjIq#`%v=kuRv~Dh zsiW)pz26t!@MhZgf>?_yj35jsQu16~;-8_l{?rj9%$d(V-9L6Mv^t{|GGZ5-C`W&WqNh29e6d|hsvmvzE$*g zQmyMXe|TD_9?2}u%kdXf_x#(4xd=B4sAmzB{R*=+ZDMv*{0=ZZw`U z1zHnRLzGA%_kVvk=&UnLH`*nK6OLz)IZ+1RaP z$_a6)pzGO}t0_(S2EeN@?u0pWGf^QOT;Fy&fIf+061Vv&k6#eQ9E?CJbN7 zR~KK=eS#kf?q9C)BZqF)@T7l`iF(-E2|R4|?91b}4Ez)Kk2FyYsGq$iY-#*_u@~1} zergZr={EWSme9lc1Lvw@G~X6iq*U zJ>6TJJ1l)Wu8Wk+%I_jO@X>ttg#2Gtvr6H&5?wENhU3wfOD|1HK$EE0JWPIo?o%@w7QX>1z=lRIcDYmm^RdaH!J$U0qdKjR{v?Nj@k8=mLg z!QaffX+{G^hFLx>yzv{!{4I7+*?C}Mw;W;Akw&+^oGNB zKbo1D;RxOCe8*HY+Wq2Z{a&$bq>kL0E*5uqV%bZU97(Rgg}axR>R3#V{3&SGo$cMU zo>il+6}(CD_7>Sn+RG>LD*ZH>q}A^P_|jLN;2-ZSZ)e^-CR7g&UCtF)C)#ml2sjLp zna0G_?3W<_14PCTp7u%y`1jJLdPCb;WW==}ZA1fH$8?tCsR>bJF?vJ$I|J{? zlTYZ~d#P!IC7nc5MI zRsbBkP4l%D7PaBB{&6DKt4yR^NwBqPM*lr^I~U=AH?+LJ*&C$ZAXr^~8P$6rvc|OQ z@5*T^JLt$TP5T0t(=wiG^sXg zEd~AVuZVrio-s!T&LrqYTve>D*P81_=>7;qF3oJb%9-_$ zl7tF5{GK`$Y)Po@y_Df6U|SUSg7!xbv_*4yVe!Q#Ebc;ZcwoSK)rHUB2ho1zr!2(h z%dVk7oadz}SQ849EKz@)p|3b8py_+bZq+KNZ|#0KdDLC_ZhWXy#9XXyyOh3&D_KBd$d5r~dZ&@?gHCJhG5mniAQ@R>y{O zFvmlJ?JS~a-%K~9k;T?h@u&=-btl<6W(Mr?#p_#Nvx_0v`ilrR<$^+%&Dej}o4vNPn* z;owRb6rB0y*XFvkd6OhCQqGB6I?r)a!X@j6i&CJJ1K0a?>M~fR$7l@HLqkBtR{DNi z#>_e~=e6CHkiV3$L3^TB!NYiWD6Xr+?LjRJ218fc9?44c9VrfHEASeRf4u5=;4Pz2 zbmAarlXGC+<}+MNXuEB2%{j_j84W=YGt*J6aB$pdMP%f^yA?e@n6EWm{~fLLbbmH# zB#6dX=5Pv<(zsg%*jkAKL5r0knZM%tivqc1CatP(x82cXPdwWwcmjs-W@q|ImYY99 zYwlE~Y-hze~lk`x3mjPB!27mzk`xg^y;8^_L_ahVv6~)|t~mfqeU``w(TZ zxrTjOPssfK`oySR$6>Am1>m)tK+)!_P=0!6FfqlwTf!xubMJn*u$lh(uwJ8c<*Bgn zDIx;XIk9`S^Zs-&ZN{2BW%h21L^x#Hy{t&3#$u%2EIrNNs9uMGjS4;G4WkVIc79S?9<_)}5sGh^o2GGu01pfp6?z7wr_`sf6M}tS@hT zNCXk5MDOIEc)b#>BL5Byp4r;f?uV$qGqs%c8FpR1U>>e-v`u`iQ65*US#Q)pH|_z0GX?kiG%%(LOU!%Ckq@v`f@JK|pfX2{xiK)~<5soB5N zZ^n6hgiX)Z0q$xxBO+$zshAz%JT;G?R^ui!yaQF=CYolEefA_D418x5EBtuv6S_T# zt>8C5ZK^P2{Tk3jP{74C9i5Z*XB90Em|L^1Lp$Ty?ZVFx%zQ38_!1Hlo_wx*Jxm>5 z;cSTnM62^vnW0! zv(<%*u%dQ#ZEdDm`^UPysNb89-plAwdG@K0)y_jBD-qUQUN%)_5(TsN78%{K)f*=5FXVTI-5jC-M0r)_e2{Uu>Io;LFZ)WTSayCh zH$y%7yq?2_WZ>t-lJJ(ErM5;8)CMsVjU&indTZFQ!V%ji9)yN-+O%Xp_zMN?x2c&~ z$h26bQIZIOY$0|Hl-A{A>HW>D%Vl99Us^*}%6q+6(o_lL&Dq|Mo_&23{zG+CyVGoa zZdPuB-3?-rE;hgTQB^{1j%hnpE81vUZxe{q7+r62)9R zQjuMBfm(#Ms?@4Z$;Ik^iw9oWqRW`6isW|}$3~+7uuRp_uSRo5+uoV%o;%fYOqe&( z|2-;yrIE5RJ-g8wo6hVoIj1wlF#nl)eXp0d4XYB-b^eG|Cv?<)1s7iS_r5%WWTvIf z2b#SW2vI-|e@Dud15=%5ScGn5xI-4JpMXeprsukuG_&3pJ9vw|;{D{iSX$ZsyN-QS zIkazCCe z5tjTIqBD|W;A`f4 z(o2%2vBi}U($ZixbedSg2KMZ`WSTOQSc*AmMXpEHQN>d_Ts3UpJGs9*!n#G=T_Pyr zRXRI_^rJ$49%InZRr>IOdrQ?H&qWVYSv0t<)_MjA=@l6!>}m{uAv-nCbu4CkNL1I0 zszx2HgbeMhEPPFWT{;-iq%i3PmyXoFdjD&`g5k23{NZw;w9<>q|Iu^oZgl-bE6dSw z^mlKsUR{#$i%`XX%3&%n7Tqbvr-=iv3Vn!kYG;%8(aknQHmT914|hK(l9+ATj)ON8U+A7k}qtQoh z(o1n!2r2de-qqvX8LPa^T2)TVwotK{{5PUlaF}h}t$?CyJIDXX_j2AsLij`a@Rsi6 zT9`WmSJ@(5~J3(YDA@J|o3An#c=nA#EWWa`laHX*T$SjobapA&u?(UWP+Q{C%@7Q}QA^ zJNv{`cm#_$-9Pc`0QTcIA;7WaQ;?NPKQ)wA2wg2XAF*`a%1+DL{Ei|8qxs#0>_+qE zIygdQ6ZJGXrnY0=Ug)~OeO>jx(RvJib8E2<`5+hRj<=UQ`XSK&;bJyBjKUK-vv>=H zGCKa(@!Sj)ME<{YC-0A1b(Bqx63NF8LaQ+pwr7++?Y;83W-@6>tv_S?kk#VM|MVs2LY-DySvyPfa--K5g8|I^)y zxR)i5=c%cxpXe{y&Af@{;>*~{b02i&{aB_$54p;>&#@87qjme%iw$;g@1y3hdBV!k zZH`JGW7Tng^Ak~%}#;Q-2Kv@J$5 zPoiIQ%EmkU-JkTeJJ;0SF{mVKykranq(ebDsHb&e)<5FL`@L~x6=ryQdf$-7vCQ0< zVKw1(9v>zMv~Hr4MzS?k%k=W&z-eNFmOLHeG{ZZM>d@B7MqQoA_xwa^NGPaDE9_=G zqs6AdhWYsONzd@+OaTr-8XOBq(wvBuPJ?Raqs7jrj-Q(vB?55luOh~nXxNd`{VH~I zM;8yWZJr7r(}dDk^xNhxwsalXR!sA1N(T6xx8%3 z9i?;He4?%(U&RqXs^zm7y_}z_XZ`rKJWdqi+!lDPbqjFCc03mCyq$|H4*?12d18Xu zsc9-IL6-9k#5yU9z|37?M|bFc*A_tKQa1XE*M2$dVt-b%RtQm5BcL!+`EtBIz{=SB z>EVhR7s6-iy&6K2n(c0w;sbP}fDO(N@z^>W*1LXDq9TP$lSj^&IGFi{KpGIP-++mV zR2tI$TwO=$)6FcJ+Rs%5-rd7wzS5;BWefCwh}pkOiJM7&g&1R6o^Ft!tBsfjK$~LV zchl!;(BZ>dy(Cy;~wVJ*0yFzq6!EcXR5$rY?4gI54mUi2JbS2$QJGSQ5Xyxt;M>AF4Sv@28`bN@} zlqwK4bFtGi`_*QHwN9+3#N)ub$|i36r9T2zdP63$?>Q-i9luzG2$gB(w!3~+Q&2P3 ztg~7hwvPLAu**HR*1BKO`siW+cbQb!0_qL(aGzg2%;p*MKf-C??dsxN<>Sk(?%vrz zzc_PV+EeGDFeZ;i84*ZtW=H3=#2j|W^|ND<(N6Ys^vYL`H`p!um`KLX43QJ|#XvZ& z5VNDI>gv4*nZ&heTY8$rMpSG1;mfQ=IFry0%W*2X;r!dHgLxC(0(fUSoX7 zwNjEXD0t!s_`m5$=5BFW$;8yBAcYLBacVr_p>vrk2KwkD((ynWWJf^m6M0>Rs^j&K zcuG{Bb;k~FsXL?edF#}7S zHZDLmq@^Y_Y4fI*ecn|MwalQipJKC!`&+AEi=Oxumg^tYvS0s6v}&dED>#4Eap zAJNq9yf;-G>9>swxt+fKLmkVyKVy>_Z%8^@94F5{sE|(%yu=RF} zU|f+?>1X;0!Jp9#;p0r9dszlE)NBFz%Xb>g4B!|F;iIe%Rte_9u|W9MNnz3b-6obM zRP{c^sD1ZD1@8DThqDat@NK>7r_aG&(BpD|3!JTXho#kHn#b%IMn={-%208He(I{j z8s5K{sLB<8*d9?YNvdlAkE1Xq4M&^5V`}zED!eajyno=M;-hVhny2RT;+R}Z9r4hs zHg0|z%N0-eaMaHz%=Q`>bdt9ENtBg7zrZD9*0FRwt#|ss^{%z@XNh|0y-*Iz&V$F+ z5b1tlj^6+i1CSkW24b4a6t9GRb-61JetIWG3TakcE8lU{iJ1*WfY zhT3M?F_AEMwx8^2o>J9?B73d2M-Ao0yblIV=w|pGk7+ zChG0Dy~rHOK1Z}(>0iZDOF^?7HV2aYQZklSUv5SgTzl!7E#o4p(rOC24ScRx|^VD=~3JdRynp4w?)W(5A0 z)_C3;5w{#ut;AeK9af+D3$6?#Btje>?{BQL*L-vD2I8Ik7Vg!@QnLe zppa1pgsr!=`wq1_mTHZ}C3xl@K9Vf_gQQRAD_36GdaZ87H=x1+REu&}7i5gjPv`MP z?=NN3^VvFySA4Is?wUDmnv~`eOyF%d=CNUipe$ZOJ}FZgXz3+QW*Yt>xpMko-JL05 zks6_=q<8Zb!SRc%GSdhvd#^TbuH?h6Nef0sA3Wjvs`Q+zGw6AI#HO>wHLBBX;iyJI zFZ5A$b~Ji_KEX=v=l0q7@oKkklDfM5JEnZw<(BE1>)c*>-8TRsJgfKBDNYu#C!HGU zbLvbP6<#?=;4iU0T&Pb*LBnMo{A)%V&E)8oVNDoVfNMGIGqDC~Tscy8s*HW5h(C^+ zG%$nRO&>=ccGarn_c)}SF|TH0#$7Cx8+pN%20+veAhSU?RF1MOcSo&5t!{_L=^j!S zwpKQodunYpRvLi#(OCA4LtJoKXSCHK;j8k9qyr5?e~KQ|t>1+SXe) z>)&;E*Mlsb=gfL7YJkKP1nqI#dXEOw6i8@(OKrTvK3jR+J9~Xn-iY+EwA~bLWlsRR z23NE2O*dU)=1+e<1VW`GQj_6h3MbAnyDHL5QY*f|gl9W++|d2#xN2PS<3Ii9G|AA= zOQyQ5g5I}YLVzr($N9aHm+H&mo*)(pv+|j@cP5Nnz1nAZ%$JsKX2hLXNhUIS$$QnG z1AL<~%s^invTXLbb=gI`FD6^i`_j_7{ay3FG_nui=Feq_SET$RM11j`*GRQ*M5-@2 zd2E<{+liz^%K)8H_K-`#W19`%J018J8$M5t{XUL%67!Kv?T5Y6%HoSPfj9xo@SRNA zO&i%cnz)EbdPRxH)73v%VP9f)rk1ApgoAeZVYB+))9p8Bqd4&bBR~*`oNY>09O~N2 z!Rp>7KnHX=U8>;-697~ZJ_sN<+Irs|Hl_`;G1z694OixT`*V|Cz_YBGLc(tM@Kohm z>JD&MYd3Xf+B|B;lk$xwEPD9_~%=V(vx(C@a6Y-H$#=bfVOn57$&>rkwrVs4#bi zIXh7e(IM6w&y9tw+~I(;y6HV5=yM_(oXxC~r9=|;e>##2d}?n{dgmZtm$kdn2_X2$ zZn1&1c*eQwvMIIRLX^A-fxxJ(8WBPe?||LSuy z{t{iR`A!>-41uqe*4_8|FWHQLq|ffyeq*aJzDYNo!z||t!@KwR;k$ICGOlv)`wp!Q zFe7=&V)-C1g>f{5f5E>X@5^pc*;|x_)H3t0Wj(Hdf}t6f7dj;Lm3y|b61$Fgis3HI zZZTE=iadp@o=Y|={({-`cL3Mp!SitW{cQ0t++xA?d#mq<7jHsy-_FH#^te&BG6e9=TM4K?yE4$gLtJB{QzeetB zryUa6*x1vKi=Ur?^*FjI-@8jse%C$IlbLGM)=y!06b)*c>gqrMkCt)IoHxxwf;h>K zI$ZxUI0Qd&(@HM3+nD1SE<}@A-dyZ1FM9v|iBvCi@E-ah@k7-!4x7-5u`g+?MqO*l zm)P-Zzu$iR6iAlY+w9>D6kT>tx3it8Zf2XNw7OwQn=-dqz_;&l(OUI~)JrvPrtJfn zwVGJjkAIYJ@iHeSyf)eOw~G0ZJld2UGPIIZu3h{4455%Qbz6lZW74FexmYeYM;8aI z$K-h3Y^GFFCuw>-YX)_9?Ig^9A4wx1xQ2#eYVG!-LK}|bv$R8@eqlJgC_fD*}q9KhIE1f_i#O7_ zT)%x`)T#K2OJP6!v6Vb?t~cc+MQq6?;as!glC6mA<(e z<%lFRX`h0@1C67z0NHWh|ZjuVZ|V8t@FP@kIUYdtPp)2I9s3B_BXeBp}-ZP z5|l~AJ-|-~e7adHXKm6rZHQ~nBqb!qRwUhuE_?tsh^Kl=Hlz{jJv5 z%a`iA?S8PdtMf0~>M^6t2;7z3^z?rn(s|R}N+Jo!JO1-*rxQyqqw0F6X>RM!ACLOT zz2%2LmeJ%LT4#%}#EGCHI_{2UWy@EDyhONJs|(RvT?Z?St5 zJe~uJKn9oH8ehO#Qs>h$z{xbsOPOK-DR@vG_GmD%r#DqpIINVFeAg~=yeiV@S@ycUgHP|m(|5vgek*ZEH4RdwK{d$6jL>+4X z`4ChMuWjS9nJF{!ij95ygn<6)v21l;z0gZ@UjZab`9s9KK++(ZU}hS#jlPp@YHsdX+N+j<%}bAm_k9)T8@LAgyH4JzSyw>)>XO5eYZ-f(KZq4CZ8KAE}&p33a#nAw#ZL!zhkYZa0_fy&Jh12UfTAj4W?< z)O2(X-CC!M1+7n}8-n0j-m&cbyUhc#29lDYkyrH{#~wfcNz*@%?SyYHR3jK#qYkg` zfOxt;Xp>!NetI3?-j2$*c6E0peFJ1k0qtRyN~0J}eb0QIcI)6yPMaCaqc?e?=5%L0 z!wWTLgHwtNdLFYf->C5f&&TSGJKKO9egTz|q7H=dL_|bSuP9CLa~>76 znCtPqJkWLkR->ldp7iOcx({hE83#s3x$hnxA+ElF{8rgnRi*qnM5H$vfR4=(~l-hR-WQi)59iT6|TZNDzuRd5R$FCqNF)uguYRz#H!g{_d46@8|;6d)C31g+?wwl9UWUN!_7@iU(y8@`IP%@=plBH%=02yJ&ynGhwto!JlUOX z4ZlnR=UHTX(m73bJ`MY1G35^fepmv;GCh*P+1Gq7e{QeWW1q~jSlc~K9=z&1x!R5Sw+>Pg|4d7B*QMP1eov^xZ{boH28L>~l{Zhb#6){LZ zm@6K(L>wtG?EU!}!s|J9nZo{zoX5(&q`-6B*F~kgn1RWbz(S1+KjtWyba%eqCgn)S zFN@z*^(@_A3M$`75z6`%5E_kQVPm~8vUW(0-)Xpgi@MOw2BBVq0JDHsQp!G~mGIp` zt+g8jO^5Tn$1Dr}bH1cx#Vy}6vXegp0~5m=T_@{(WO?uY<)bNy2_cW;Z}e~9$_)RA z&R+=3N)8DzIPSQK_PD=ZR~&XP^$^GwPJke6@_WV`6v&1R5CO#>3rm*Q23Pe8MP7zS zN4FFUff1Z$-rRg+ zUOSJkl&izPHIn6>?4GZlL7(#}f01PSSIocavf%MkVJQEWW_pySvMFl!k06}^LD&vwY0rFM7Sdo{5 z!^6JOK>wyp&Wqx=>r``x(>dQU>14zTUKY_wnmZX{if`ugWtEtf5;FbfaR!RPMaJd=iiHkN)+{o zw=7_sEj@pbvWiNnS(}3oHAP5B$P(ALz1hl~T>aEY-XZhaZD8r8xs7nX)xEONDCjv; zOUTNmo&)Iqji8`liKzLOKAgKFJ{IZWuJ!WaoUryyAsQa}xqK??8(ZCEx7KBV%c+g} zKb}#TJ3DI#oj_gNy)Lv4->#&&w<8@3lV0qv{JbmRz6E=*>$kXAjlB;|IzAm_X;uCB zv75{DlZUCPX_L_PUzBOWE8nZ-zMA|iD#R`FU!}FRHDgRnO#Q#U-C^A-YHAzgg4T%^ zfT5UUrP9zgG!1;~y?UlKy%*}N2#Hw+^T|{6+|stQd7Vod9abu&XJ-I|3zTY{NPshIIhMP4(e)?%0Uv@SDj|pppr_Jb?SU%^oes09s*d=hLG% ze`5g7Pg|}#S=k6d+Fsg@%%xAxPEJlx7nk#=$7MiN?f*(I2$fs2BE8qna6zlDpw#AvWn%g%t!02^>Yj~m`Jc$>iH%XJ+e zZKMxoZxEm;V0$ox2?Us2H~)s3I>H<)HIzL9C2*B2%2% zuoz$qGG)~AC_jk0_89l)-w}Kjrep=sU*p1KA%bo<^)Jj!7x@I^8msO> z4>V)boi5cC6{?m%cYKi1zpQXO7Sj9MjcR~0ZwA;qj*h|~Zix)e+JeBwbFm_`UgUBj zgZ3CD_k(a;jH;dGQ*p3Bd%O`FzipXyFXu?vx-t zRRt(cFcEnGVV%fzDOH|TTM%e0EhS#yS{w_?&t<&+7NqhCIwh;+O2Q)sTsc~YfC3bU zq)x}416#3Ue7ioXI|D~PEaz~kUWr15fPZx;b=F9Tzwr0+<&|tGXq@ z-!p+0U!6Y(5C2L=UkFUuSvc=t6~zQS+H_VW)Q7oK9zW$`$z@cCY7+mxgHfQ2uQhZ-Bg6F)mmEz|j=0p4I}11k?as_b+mA%#j1I8iXEwQXk2s&yQGt+}5bJPGb zYs;@L`=D=*mVrBcjz(@?+%AG&0^6ZbXHPVwwQtYzgjukZEJ2bVC7?Lfli&RQ%4ql> zZ3Bjstq?yQBdHTP-^B*^9^)d5i0V_-08a5k6bqU|cZ>@XF)H;$15M;U%QGN@6}Z0a%$-dB3&%`n1N7*~lrtIJ9})K!Z&`SKbd?mA;INup)SGh_}|FuM4}K3U^9Fb8+f3I)=dOLQaPRBqE3YvwXIT1%t;9*xKkQh>Kn}+ z;Q<4<$i;f+^O`#E1n{wLZ`^)rpKs3y{DkQ8RNlH1vu<1qB`m4Ar|D>VgG6BuM+f>~Dm_|t=53>L+p^5?Ap9utF zppJ$KnZyZs@R1KmMUXW&i^`seS3FLP#h)k*_YGfPxP+Lf^-x z_sPQj+kVNDcrI(6;l2Q!R|cD$^7tp7bf7;!t@&f~h=LG73c#aHVNd&EsO|JDJ9OOx z4GDygnsW{dr3{vF5xpd#k62wqCZIlHJHXZfU5XlU_yV<>Rk0;H^GfkD(N zC8;^cd!U6lfJ^KX0@2Cm4 zd=8d)ev_63t*c7*w-JRN1{3v+Dme+gIiEr$x928QNy2pGAlduak_Nks{AB%r4d$fH zGhrW1qRbq8d%BIF&*L}E-fw%oD&cP&m%}Rqb%t5mz_v?@_2O7|JOgdKNWi}S@&06z zP{QC}7)~36DkZQSelp`cI3;!}dXmDmA1C4Sk2IJkXca2?Rq1CeU5pr=Z$Eb|MA79r zy57rE7PN*8KqGRYA39>(eT2c|(NGsfr{%mU54|>P>rf=dU$a zVl0M^67U>rcEgp*DN>+P_xfhO3&BA|ja^IR9nZ)3c|`h+cLeVW#xuq+3uv}nG%A6$ z#JM=Sf#pn?8f_ORN$Xqfs`b>*IN!zY+eSWe1i~INHf-G~8xDL(&1R?cv=sOgWv}k5 zvteF@Dw-i34yuxg_Ahk2WZ_t?;YIDiHl87$aT0m1go-04L)r0u>_1x^n=umxKkx@T zXKB)YVqzl*IYMPkmI~jo$xVn-{?=xiJ8`YE6AE&DmVLZJomVQhqu;I!nEhjZSW}A& z9aUJDnK_|~5?EL>2L>l;*|=qJIbzC{IPFmO%Mz2JRMpR3A?xzA_EP8?g@56F6FM$q zslHsKdNUki-|0=HpNsVPE(Hds%j;8TT#?q*MZ~(-<~C%b_A2|X}`fHdwvLeljj~n5@a2L|D4uGt&g#XQX&30uL+(&sRQi{`=)W|ELz*%Z{nK;cEHSxzI zsrA`<4Rz6Z8InRbWPpDMuP7)CHtM(&gT2jMVmcA6EvW4pd-}Qk^8M1f ztkSjGqJ~jOXiyi^+0T{nVA#S8`(S%RprmWLq)G}RWd2UQ3q_lH)oTEP+*uVvg!LsN zp4D^TqM1J4uq;@HeYBs!V3ZI;JIi2~d7$Bqe6W{zYP=5{*J{>)N}3r95vhNZ5{>PP zuI(%1IF+xoU4$(JU!{WLTR0_#*HMh#ml-ls=i#wjLt?A2l@z+gU?w4~g&W3HA^s`4 z4F}UB7IHlO-A+nWfeRdy@gjRdiHelQl{X=Mi%^&FKd0*<@l5nI+3Vc#h{rC$Kmoe- zvpZ3bMn*|-qZj5396y5q;CzC9kn7s`b9To2s502|i^nLOu-LIIk#*-sAS;X19xv|T z{0*ls}liCZmq|f?Z;8zxgGkM?Er0=gxe!}5B)Cfts%zx+KRG~>%xK$^Qwgd zxLTF&cv9bZ<-o^?!iTp33(@-}y0vl_)2I~G)HF}92jl3v=#<)O=E7Rmdy2oglf|Hk z9aQ(|WOUn)IJ?Ev)&Fx6bF6)kaLn)DLr)thofgM!&@~!rT^O7bt@e5Or7|3fA$%9Z zdcgKVh^j?@4Sr`l^A=M|1^Pj?Yjm{L@U2TTmvgF?=HwY>`I&p}76Dqk08?8TUT-A< znh@%m4Kr8Z2eAG|xkt@vbVWTWWo?7J*w`48u>8$6i$5I*42V{tPKX%OlI`)B z()!3ib)k>8#b55^e=w>k^6=B1ZbJ{JnSv5WmCH4hd6f6{<7;|@XRLCWiDm99yGhA{}Wd_r0D9El$(X5~ZD@6Yeq%JJ_WnN-%QoT_^& zls{`|J%9EnfAEk~Gyx};)G-ocS*iS3?;B@s#A40HNR)|D9E{SnkxmA^FH@JMsvova zX8&ayT=lAu;Am_z@Z2<%$ z6{BjEfS{w}t2VXwOJOAH%tlL96JNh1#=Wf*>*+C=wXQ*vNIs(oaU1*ElJg~AWBoz3 zYsEM6%XU&tu#eu|7g)PJi(C|&tXR}Xn&hnaxd#gx>+j>0Lv??-YpZnq{-}DCX85D* z_dAPe6Sn6vuwUIAo->ylZ}KI6nQVFL;mT@J6n=}jh-Pjuf%pEv++Pdkdj*Zwp7DBP zrhTtVw6{gZP)&6$66l-|HJ4w8%;eg~^EPs4nrqIew2C|SrN%S*vk{KJr_g;`Y{qKK za@>)WYnnH1$Y7g~H208STe0$q*DizuUr(^Zw@=B+o)Nq~aS$e4yGfT@>daH^DKIsz z)cwow6@MRvpi+T4eltCU8Nd3QQI$>-+ zFgW?Ai;?m(M6`3~3t0MUNoj|7dB1(@$eb{5jiAiUXxRcsX%N7syb8a-?5DbXQY&dt z%QtBuucYrm*1Dmz5?dn{A0>WU5x4YjETQ|M(8cAmSNvkb`2Ku7q`f}>zYyO<$8g@x zUzs*Gk#M%Gr7AVQ9ITO)uwuolOW^S4&Y0P4a>%SIVE2Gjj|O|=mQu{^v#Iu?g_u(5R=+V-wf&8^chyn4kgY80C{bYAldhLKCoo`h9f)w_=Um8!5STwbwF{_U3Tizp{AyHC84h8bs|GATle zy3F*$Rqlvr6MURu^<;ctoJ-C-Z#8oO_$tvow?$;EY1DOi-(^S-2TIzlGbRuwb&;-U z7yZw0dus6-cg1D}!{`%wA1C;jA;~pKJ=T-XVnJJKHRncn?@V?ZaEO-)De!(_{cb?PQZM`vF>be}<$+yB>x!n%iMG>Ka&r0;UkQJwE7Mk z$`Z}Z4DInTXYHvlPzHtdM7q)!O@~4(CDCwL6=&P!6H8`ZIu8IG){ebnFOOsgJi5#J zo*23$q%3OfclD3VJx10L$%S6mR#0O{Z-fm;yH{W~(%~1?#@9nDw|X-FV4WFNNE?u= zNsUpZB9>X39R6ko>B{E@palselu!fi@;2{*99+kw!vd7==a0k1U~mtD?VxZfC+5?F zS*Nm{8|Z%mvGF+lu?k#)zW_q@WO_h`!~-H-wk-C(55!&CS9;0~tCO{qo!|0Ld`A4R z!?{Ezel$sA7!Q>QFASC6a)j3Q9elimCP?JOd_Bc7+iW%D38BRl_}=XI4MruG!$~Y! z)#3G0Ing0FLqmnq)N|04xKy0c__kp$%3zYQ=jbnv1!pZF??**9r5!AA6QQz8HaBac z-)DvlmLg^;&QlPoDNzVPj2csI+Sp_GZ>Y~5tTw?OOsB`~G>Igi6#!z}vZxRn z_mZ3%^Wj5E=1-Gbo9g3YL(lz--KY}z4P9;Rev`lWf1j9yQ{VAp+Sf5=7Hy=={c`2yJadWY-BAQd=Lk{ z_R5BjK&)=KT9B)gLV{1>rb2!28ThgAIW#%qO5B*yF`N0*A1srvHd0v*yWIk#^?)Ts zM5jSs*i*xFz2D7|(i500ZVx1J5^<9kp}e$hGHAC2LutRtBH%ytYf07!h%N|p$O_p@ zM76*Ze2pGv`4>e77PC@W+j}V~VvZ$pJA7_u0J6SUt6#Bso7y)~k6x^LN|Y_7g&3t~ zc23kDyJ8C;xg3Hmb~#>n;^|83;U1180$1Uh7VPe5U0Yj3=xI`|HIDic);1 z%Zx)#tB~cm_w^{1mrymqhd`i6ceMi`P*4dL1i~7qi~&@l{y)Y0VJg~EDlzW{YoEjk QxT*u><#`b=z=h}h2b9~@i2wiq literal 0 HcmV?d00001 diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp new file mode 100644 index 0000000000000..0960875d7eee2 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp @@ -0,0 +1,107 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ +#define TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace rviz_plugins +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage(const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + // QImage hud_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + tier4_debug_msgs::msg::StringStamped::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ diff --git a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml index c1158465e1cf1..e18900afc8d84 100644 --- a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml @@ -4,4 +4,9 @@ base_class_type="rviz_common::Display"> Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped + + Display drivable area of tier4_debug_msgs::msg::StringStamped + diff --git a/common/tier4_debug_rviz_plugin/src/string_stamped.cpp b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp new file mode 100644 index 0000000000000..538dc0cbe7069 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp @@ -0,0 +1,198 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "tier4_debug_rviz_plugin/string_stamped.hpp" + +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + std::lock_guard message_lock(mutex_); + if (!last_msg_ptr_) { + return; + } + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + last_msg_ptr_->data.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::StringStampedOverlayDisplay, rviz_common::Display) From b4b0002b6ea505ebe006b4557dee9306ce811434 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 18 Jan 2024 08:14:33 +0900 Subject: [PATCH 067/154] feat(behavior_path_planner): output manager internal state as topic (#6099) Signed-off-by: satoshi-ota --- .../include/behavior_path_planner/planner_manager.hpp | 3 +++ .../behavior_path_planner/src/planner_manager.cpp | 11 +++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index f6f95ae3b5e82..5e773eba96aee 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -46,6 +46,7 @@ using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; using DebugPublisher = tier4_autoware_utils::DebugPublisher; using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; +using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; enum Action { ADD = 0, @@ -456,6 +457,8 @@ class PlannerManager std::unique_ptr debug_publisher_ptr_; + std::unique_ptr state_publisher_ptr_; + pluginlib::ClassLoader plugin_loader_; mutable rclcpp::Logger logger_; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a1d0a82be38b1..cbcec2e3095d3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -40,6 +40,7 @@ PlannerManager::PlannerManager( { processing_time_.emplace("total_time", 0.0); debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); + state_publisher_ptr_ = std::make_unique(&node, "~/debug"); } void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & name) @@ -908,10 +909,6 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) void PlannerManager::print() const { - if (!verbose_) { - return; - } - const auto get_status = [](const auto & m) { return magic_enum::enum_name(m->getCurrentStatus()); }; @@ -961,6 +958,12 @@ void PlannerManager::print() const << std::setw(21); } + state_publisher_ptr_->publish("internal_state", string_stream.str()); + + if (!verbose_) { + return; + } + RCLCPP_INFO_STREAM(logger_, string_stream.str()); } From e3d4e759f8e6a775ef4a956f9806dbf1fd3a0c75 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 18 Jan 2024 11:10:42 +0900 Subject: [PATCH 068/154] refactor(goal_planner): remove duplicated execution condition (#6087) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 3f765c99cffa9..a13344ee87e7c 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -458,13 +458,6 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } - // if goal modification is not allowed - // 1) goal_pose is in current_lanes, plan path to the original fixed goal - // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!utils::isAllowedGoalModification(route_handler)) { - return goal_is_in_current_lanes; - } - // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes From 3239b9912401ae4d663a3acff0aa12239c69ae63 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 18 Jan 2024 12:58:32 +0900 Subject: [PATCH 069/154] fix(behavior_velocity_planner, behavior_path_planner): refresh raw traffic light id map every callback (#6061) Signed-off-by: Mamoru Sobue --- .../src/behavior_path_planner_node.cpp | 1 + planning/behavior_velocity_planner/src/node.cpp | 14 +++++++++++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9b3fbedc37203..3eb5eca5954fb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -784,6 +784,7 @@ void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSh { std::lock_guard lock(mutex_pd_); + planner_data_->traffic_light_id_map.clear(); for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index e2c29ef868257..9fa5634c6dd65 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -323,6 +323,11 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( { std::lock_guard lock(mutex_); + // clear previous observation + planner_data_.traffic_light_id_map_raw_.clear(); + const auto traffic_light_id_map_last_observed_old = + planner_data_.traffic_light_id_map_last_observed_; + planner_data_.traffic_light_id_map_last_observed_.clear(); for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; @@ -334,9 +339,12 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( }); // if the observation is UNKNOWN and past observation is available, only update the timestamp // and keep the body of the info - if ( - is_unknown_observation && - planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) { + const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { + // copy last observation + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + old_data->second; + // update timestamp planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = msg->stamp; } else { From 1f9cdfee48f3ca14628001f6d9fd18a9c56e8198 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Thu, 18 Jan 2024 13:23:40 +0900 Subject: [PATCH 070/154] fix(radar_tracks_msgs_converter): change default parameter for twist compensation (#6080) Signed-off-by: scepter914 --- perception/radar_tracks_msgs_converter/README.md | 2 +- .../launch/radar_tracks_msgs_converter.launch.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index fb8f117c82245..fa08eb08f521a 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -23,7 +23,7 @@ This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-p - `new_frame_id` (string): The header frame of the output topic. - Default parameter is "base_link" - `use_twist_compensation` (bool): If the parameter is true, then the twist of the output objects' topic is compensated by ego vehicle motion. - - Default parameter is "false" + - Default parameter is "true" - `use_twist_yaw_compensation` (bool): If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle. - Default parameter is "false" - `static_object_speed_threshold` (float): Specify the threshold for static object speed which determines the flag `is_stationary` [m/s]. diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml index a6a3f394b1f40..cc2bb80c6f4f7 100644 --- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml +++ b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml @@ -4,7 +4,7 @@ - + From dcc29f2887eff1214d42fb326bb6af0a4aa342fc Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 18 Jan 2024 14:28:40 +0900 Subject: [PATCH 071/154] fix(pointpainting): fix param path declaration (#6106) * fix(pointpainting): fix param path declaration Signed-off-by: kminoda * remove pointpainting_model_name Signed-off-by: kminoda * revert: revert unnecessary change Signed-off-by: kminoda --------- Signed-off-by: kminoda --- .../detection/camera_lidar_fusion_based_detection.launch.xml | 2 ++ .../camera_lidar_radar_fusion_based_detection.launch.xml | 2 ++ launch/tier4_perception_launch/launch/perception.launch.xml | 3 +++ 3 files changed, 7 insertions(+) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 10d1ac034d457..d269144067e0e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -267,6 +267,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index c227298c932d9..4838187ef8fbe 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -296,6 +296,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 87616b9ccb122..4e9f0daafa736 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -34,6 +34,9 @@ + + + From fe9e11549653d926448fb8f5d00662fe8181c857 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 18 Jan 2024 16:14:22 +0900 Subject: [PATCH 072/154] docs(crosswalk): update the document (#5583) * docs(crosswalk): update the document Signed-off-by: Takayuki Murooka * fix the spell Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../README.md | 359 ++++++++++-------- .../docs/crosswalk_attention_range.svg | 229 ++++------- .../docs/crosswalk_module.svg | 98 +++++ .../docs/debug_markers.png | Bin 0 -> 220365 bytes .../docs/explicit_stop_line.svg | 71 ++++ .../docs/far_object_threshold.drawio.svg | 114 ++++++ .../docs/limitation.svg | 164 -------- .../docs/no-intension.svg | 193 ---------- .../docs/stop_line.svg | 149 -------- .../docs/stop_line_distance.svg | 170 --------- .../docs/stop_line_margin.svg | 204 ---------- .../docs/stop_margin.svg | 144 ------- .../docs/stuck_vehicle_attention_range.svg | 174 --------- .../docs/stuck_vehicle_detection.svg | 93 +++++ .../docs/ttc_vs_ttv.drawio.svg | 210 ++++++++++ .../docs/virtual_collision_point.svg | 157 +++----- .../docs/virtual_stop_line.svg | 73 ++++ .../docs/with_traffic_light.svg | 97 +++++ .../docs/without_traffic_light.svg | 67 ++++ 19 files changed, 1141 insertions(+), 1625 deletions(-) create mode 100644 planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg create mode 100644 planning/behavior_velocity_crosswalk_module/docs/debug_markers.png create mode 100644 planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg create mode 100644 planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg delete mode 100644 planning/behavior_velocity_crosswalk_module/docs/limitation.svg delete mode 100644 planning/behavior_velocity_crosswalk_module/docs/no-intension.svg delete mode 100644 planning/behavior_velocity_crosswalk_module/docs/stop_line.svg delete mode 100644 planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg delete mode 100644 planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg delete mode 100644 planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg delete mode 100644 planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg create mode 100644 planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg create mode 100644 planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg create mode 100644 planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg create mode 100644 planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg create mode 100644 planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 5f82fe839fda6..37c3fb58049a6 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -1,204 +1,169 @@ -## Crosswalk +# Crosswalk -### Role +## Role -This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage of pedestrians and bicycles based on object's behavior and surround traffic. +This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage of crosswalk users such as pedestrians and bicycles based on the objects' behavior and surround traffic.
- ![example](docs/example.png){width=1000} -
crosswalk module
+ ![crosswalk_module](docs/crosswalk_module.svg){width=1100}
-### Activation Timing +## Features -The manager launch crosswalk scene modules when the reference path conflicts crosswalk lanelets. +### Yield -### Module Parameters +#### Target Object -#### Common parameters +The target object's type is filtered by the following parameters in the `object_filtering.target_object` namespace. -| Parameter | Type | Description | -| ----------------------------- | ---- | ------------------------------- | -| `common.show_processing_time` | bool | whether to show processing time | +| Parameter | Unit | Type | Description | +| ------------ | ---- | ---- | ---------------------------------------------- | +| `unknown` | [-] | bool | whether to look and stop by UNKNOWN objects | +| `pedestrian` | [-] | bool | whether to look and stop by PEDESTRIAN objects | +| `bicycle` | [-] | bool | whether to look and stop by BICYCLE objects | +| `motorcycle` | [-] | bool | whether to look and stop by MOTORCYCLE objects | -#### Parameters for input data +In order to detect crosswalk users crossing outside the crosswalk as well, the crosswalk module creates an attention area around the crosswalk shown as the yellow polygon in the figure. If the object's predicted path collides with the attention area, the object will be targeted for yield. -| Parameter | Type | Description | -| ------------------------------------ | ------ | ---------------------------------------------- | -| `common.traffic_light_state_timeout` | double | [s] timeout threshold for traffic light signal | +
+ ![crosswalk_attention_range](docs/crosswalk_attention_range.svg){width=600} +
-#### Parameters for stop position +The parameter is in the `object_filtering.target_object` namespace. -The crosswalk module determines a stop position at least `stop_distance_from_object` away from the object. +| Parameter | Unit | Type | Description | +| --------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------- | +| `crosswalk_attention_range` | [m] | double | the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk | -
- ![stop_distance_from_object](docs/stop_margin.svg){width=1000} -
stop margin
-
+#### Stop Position -The stop line is the reference point for the stopping position of the vehicle, but if there is no stop line in front of the crosswalk, the position `stop_distance_from_crosswalk` meters before the crosswalk is the virtual stop line for the vehicle. Then, if the stop position determined from `stop_distance_from_object` exists in front of the stop line determined from the HDMap or `stop_distance_from_crosswalk`, the actual stop position is determined according to `stop_distance_from_object` in principle, and vice versa. +First of all, `stop_distance_from_object [m]` is always kept at least between the ego and the target object for safety. -
- ![stop_line](docs/stop_line.svg){width=700} -
explicit stop line
-
+When the stop line exists in the lanelet map, the stop position is calculated based on the line. +When the stop line does **NOT** exist in the lanelet map, the stop position is calculated by keeping `stop_distance_from_crosswalk [m]` between the ego and the crosswalk. -
- ![stop_distance_from_crosswalk](docs/stop_line_distance.svg){width=700} -
virtual stop point
-
+
+ + + + + +
+
On the other hand, if pedestrian (bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `far_object_threshold` meters away from the stop line, the actual stop position is determined to be `stop_distance_from_object` and pedestrian position, not at the stop line.
- ![far_object_threshold](docs/stop_line_margin.svg){width=1000} -
stop at wide crosswalk
+ ![far_object_threshold](docs/far_object_threshold.drawio.svg){width=700}
-See the workflow in algorithms section. +In the `stop_position` namespace, -| Parameter | Type | Description | -| -------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `stop_position.stop_distance_from_object` | double | [m] the vehicle decelerates to be able to stop in front of object with margin | -| `stop_position.stop_distance_from_crosswalk` | double | [m] make stop line away from crosswalk when no explicit stop line exists | -| `stop_position.far_object_threshold` | double | [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object) | -| `stop_position.stop_position_threshold` | double | [m] threshold for check whether the vehicle stop in front of crosswalk | +| Parameter | | Type | Description | +| ------------------------------ | --- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_position_threshold` | [m] | double | If the ego vehicle has stopped near the stop line than this value, this module assumes itself to have achieved yielding. | +| `stop_distance_from_crosswalk` | [m] | double | make stop line away from crosswalk for the Lanelet2 map with no explicit stop lines | +| `far_object_threshold` | [m] | double | If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object) for the case where the crosswalk width is very wide | +| `stop_distance_from_object` | [m] | double | the vehicle decelerates to be able to stop in front of object with margin | -#### Parameters for ego's slow down velocity +#### Yield decision -| Parameter | Type | Description | -| --------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------- | -| `slow_velocity` | double | [m/s] target vehicle velocity when module receive slow down command from FOA | -| `max_slow_down_jerk` | double | [m/sss] minimum jerk deceleration for safe brake | -| `max_slow_down_accel` | double | [m/ss] minimum accel deceleration for safe brake | -| `no_relax_velocity` | double | [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | +The module makes a decision to yield only when the pedestrian traffic light is **GREEN** or **UNKNOWN**. +Calculating the collision point, the decision is based on the following variables. -#### Parameters for stuck vehicle +- TTC: Time-To-Collision which is the time for the **ego** to reach the virtual collision point. +- TTV: Time-To-Vehicle which is the time for the **object** to reach the virtual collision point. -If there are low speed or stop vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle (see following figure), closing the distance to that vehicle could cause Ego to be stuck on the crosswalk. So, in this situation, this module plans to stop before the crosswalk and wait until the vehicles move away, even if there are no pedestrians or bicycles. +Depending on the relative relationship between TTC and TTV, the ego's behavior at crosswalks can be classified into three categories based on [1] -
- ![stuck_vehicle_attention_range](docs/stuck_vehicle_attention_range.svg){width=1000} -
stuck vehicle attention range
-
+- A. **TTC >> TTV**: The object has enough time to cross before the ego. + - No stop planning. +- B. **TTC ≒ TTV**: There is a risk of collision. + - **Stop point is inserted in the ego's path**. +- C. **TTC << TTV**: Ego has enough time to cross before the object. + - No stop planning. -| Parameter | Type | Description | -| ------------------------------------------------ | ------ | ---------------------------------------------------------------------- | -| `stuck_vehicle.stuck_vehicle_velocity` | double | [m/s] maximum velocity threshold whether the vehicle is stuck | -| `stuck_vehicle.max_stuck_vehicle_lateral_offset` | double | [m] maximum lateral offset for stuck vehicle position should be looked | -| `stuck_vehicle.stuck_vehicle_attention_range` | double | [m] the detection area is defined as X meters behind the crosswalk | +
+ + + + + +
+
-#### Parameters for pass judge logic +The boundary of A and B is interpolated from `ego_pass_later_margin_x` and `ego_pass_later_margin_y`. In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `ego_pass_later_margin_y` is `{1, 4, 6}` for example. +The same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}` for example. -Also see algorithm section. +In the `pass_judge` namespace, -| Parameter | Type | Description | -| ------------------------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------- | -| `pass_judge.ego_pass_first_margin` | double | [s] time margin for ego pass first situation | -| `pass_judge.ego_pass_later_margin` | double | [s] time margin for object pass first situation | -| `pass_judge.stop_object_velocity_threshold` | double | [m/s] velocity threshold for the module to judge whether the objects is stopped | -| `pass_judge.min_object_velocity` | double | [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV.) | -| `pass_judge.timeout_no_intention_to_walk` | double | [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. | -| `pass_judge.timeout_ego_stop_for_yield` | double | [s] the amount of time which ego should be stopping to query whether it yields or not. | +| Parameter | | Type | Description | +| ---------------------------------- | ----- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `ego_pass_first_margin_x` | [[s]] | double | time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) | +| `ego_pass_first_margin_y` | [[s]] | double | time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) | +| `ego_pass_first_additional_margin` | [s] | double | additional time margin for ego pass first situation to suppress chattering | +| `ego_pass_later_margin_x` | [[s]] | double | time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) | +| `ego_pass_later_margin_y` | [[s]] | double | time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) | +| `ego_pass_later_additional_margin` | [s] | double | additional time margin for object pass first situation to suppress chattering | -#### Parameters for object filtering +### Smooth Yield Decision -As a countermeasure against pedestrians attempting to cross outside the crosswalk area, this module watches not only the crosswalk zebra area but also in front of and behind space of the crosswalk, and if there are pedestrians or bicycles attempting to pass through the watch area, this module judges whether ego should pass or stop. +When the object is stopped around the crosswalk but has no intention to walk, the ego will yield the object forever. +To prevent this dead lock behavior, the ego will cancel the yield depending on the situation. -
- ![crosswalk_attention_range](docs/crosswalk_attention_range.svg){width=1000} -
crosswalk attention range
-
+#### When there is no traffic light -This module mainly looks the following objects as target objects. There are also optional flags that enables the pass/stop decision for `motorcycle` and `unknown` objects. - -- pedestrian -- bicycle - -| Parameter | Type | Description | -| --------------------------- | ------ | ----------------------------------------------------------------------------------------------------- | -| `crosswalk_attention_range` | double | [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk | -| `target/unknown` | bool | whether to look and stop by UNKNOWN objects | -| `target/bicycle` | bool | whether to look and stop by BICYCLE objects | -| `target/motorcycle` | bool | whether to look and stop MOTORCYCLE objects | -| `target/pedestrian` | bool | whether to look and stop PEDESTRIAN objects | - -### Inner-workings / Algorithms - -#### Stop position - -The stop position is determined by the existence of the stop line defined by the HDMap, the positional relationship between the stop line and the pedestrians and bicycles, and each parameter. - -```plantuml -start -:calculate stop point from **stop_distance_from_object** (POINT-1); -if (There is the stop line in front of the crosswalk?) then (yes) - :calculate stop point from stop line (POINT-2.1); -else (no) - :calculate stop point from **stop_distance_from_crosswalk** (POINT-2.2); -endif -if (The distance ego to **POINT-1** is shorter than the distance ego to **POINT-2**) then (yes) - :ego stops at POINT-1; -else if (The distance ego to **POINT-1** is longer than the distance ego to **POINT-2** + **far_object_threshold**) then (yes) - :ego stops at POINT-1; -else (no) - :ego stops at POINT-2; -endif -end -``` +For the object stopped around the crosswalk but has no intention to walk (\*1), when the ego keeps stopping to yield for a certain time (\*2), the ego cancels the yield and start driving. -#### Pass judge logic +\*1: +The time is calculated by the interpolation of distance between the object and crosswalk with `distance_map_for_no_intention_to_walk` and `timeout_map_for_no_intention_to_walk`. -At first, this module determines whether the pedestrians or bicycles are likely to cross the crosswalk based on the color of the pedestrian traffic light signal related to the crosswalk. Only when the pedestrian traffic signal is **RED**, this module judges that the objects will not cross the crosswalk and skip the pass judge logic. +In the `pass_judge` namespace, -Secondly, this module makes a decision as to whether ego should stop in front of the crosswalk or pass through based on the relative relationship between TTC(Time-To-Collision) and TTV(Time-To-Vehicle). The TTC is the time it takes for ego to reach the virtual collision point, and the TTV is the time it takes for the object to reach the virtual collision point. +| Parameter | | Type | Description | +| --------------------------------------- | ----- | ------ | --------------------------------------------------------------------------------- | +| `distance_map_for_no_intention_to_walk` | [[m]] | double | distance map to calculate the timeout for no intention to walk with interpolation | +| `timeout_map_for_no_intention_to_walk` | [[s]] | double | timeout map to calculate the timeout for no intention to walk with interpolation | -
- ![virtual_collision_point](docs/virtual_collision_point.svg){width=1000} -
virtual collision point
-
+\*2: +In the `pass_judge` namespace, -Depending on the relative relationship between TTC and TTV, the ego's behavior at crosswalks can be classified into three categories. +| Parameter | | Type | Description | +| ---------------------------- | --- | ------ | ----------------------------------------------------------------------------------------------------------------------- | +| `timeout_ego_stop_for_yield` | [s] | double | If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. | -1. **TTC >> TTV**: The objects have enough time to cross first before ego reaches the crosswalk. (Type-A) -2. **TTC ≒ TTV**: There is a risk of a near miss and collision between ego and objects at the virtual collision point. (Type-B) -3. **TTC << TTV**: Ego has enough time to path through the crosswalk before the objects reach the virtual collision point. (Type-C) +#### When there is traffic light -This module judges that ego is able to pass through the crosswalk without collision risk when the relative relationship between TTC and TTV is **Type-A** and **Type-C**. On the other hand, this module judges that ego needs to stop in front of the crosswalk prevent collision with objects in **Type-B** condition. The time margin can be set by parameters `ego_pass_first_margin` and `ego_pass_later_margin`. This logic is designed based on [1]. +For the object stopped around the crosswalk but has no intention to walk (\*1), the ego will cancel the yield without stopping. +This comes from the assumption that the object has no intention to walk since it is stopped even though the pedestrian traffic light is green. -
- ![ttc-ttv](docs/ttc-ttv.svg){width=1000} -
time-to-collision vs time-to-vehicle
-
+\*1: +The crosswalk user's intention to walk is calculated in the same way as `When there is no traffic light`. -This module uses the larger value of estimated object velocity and `min_object_velocity` in calculating TTV in order to avoid division by zero. - -```plantuml -start -if (Pedestrian's traffic light signal is **RED**?) then (yes) -else (no) - if (There are objects around the crosswalk?) then (yes) - :calculate TTC & TTV; - if (TTC < TTV + **ego_pass_first_margin** && TTV < TTC + **ego_pass_later_margin**) then (yes) - :STOP; - else (no) - :PASS; - endif - endif -endif -end -``` +
+ + + + + +
+
-#### Dead lock prevention +#### New Object Handling -If there are objects stop within a radius of `min_object_velocity * ego_pass_later_margin` meters from virtual collision point, this module judges that ego should stop based on the pass judge logic described above at all times. In such a situation, even if the pedestrian has no intention of crossing, ego continues the stop decision on the spot. So, this module has another logic for dead lock prevention, and if the object continues to stop for more than `timeout_no_intention_to_walk` seconds after ego stops in front of the crosswalk, this module judges that the object has no intention of crossing and switches from **STOP** state to **PASS** state. The parameter `stop_object_velocity_threshold` is used to judge whether the objects are stopped or not. In addition, if the object starts to move after the module judges that the object has no intention of crossing, this module judges whether ego should stop or not once again. +Due to the perception's limited performance where the tree or poll is recognized as a pedestrian or the tracking failure in the crowd or occlusion, even if the surrounding environment does not change, the new pedestrian (= the new ID's pedestrian) may suddenly appear unexpectedly. +When this happens when the ego is going to pass the crosswalk, the ego will stop suddenly. -
- ![no-intension](docs/no-intension.svg){width=1000} -
dead lock situation
-
+To fix this issue, the option `disable_yield_for_new_stopped_object` is prepared. +If set to true, the new stopped object will be ignored during the yield decision around the crosswalk with a traffic light. + +In the `pass_judge` namespace, + +| Parameter | | Type | Description | +| -------------------------------------- | --- | ---- | ------------------------------------------------------------------------------------------------ | +| `disable_yield_for_new_stopped_object` | [-] | bool | If set to true, the new stopped object will be ignored around the crosswalk with a traffic light | -#### Safety Slow Down Behavior +### Safety Slow Down Behavior In current autoware implementation if there are no target objects around a crosswalk, ego vehicle will not slow down for the crosswalk. However, if ego vehicle to slow down to a certain speed in @@ -207,27 +172,103 @@ it is instructed in [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) document. -### Limitations +| Parameter | | Type | Description | +| --------------------- | ------- | ------ | --------------------------------------------------------------------------------------------------------------------- | +| `slow_velocity` | [m/s] | double | target vehicle velocity when module receive slow down command from FOA | +| `max_slow_down_jerk` | [m/sss] | double | minimum jerk deceleration for safe brake | +| `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake | +| `no_relax_velocity` | [m/s] | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | -When multiple crosswalks are nearby (such as intersection), this module may make a stop decision even at crosswalks where the object has no intention of crossing. +### Stuck Vehicle Detection + +The feature will make the ego not to stop on the crosswalk. +When there are low-speed or stopped vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle, the crosswalk module will plan to stop before the crosswalk even if there are no pedestrians or bicycles. + +`min_acc`, `min_jerk`, and `max_jerk` are met. If the ego cannot stop before the crosswalk with these parameters, the stop position will move forward. + +
+ ![stuck_vehicle_attention_range](docs/stuck_vehicle_detection.svg){width=600} +
+ +In the `stuck_vehicle` namespace, + +| Parameter | Unit | Type | Description | +| ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- | +| `stuck_vehicle_velocity` | [m/s] | double | maximum velocity threshold whether the target vehicle is stopped or not | +| `max_stuck_vehicle_lateral_offset` | [m] | double | maximum lateral offset of the target vehicle position | +| `stuck_vehicle_attention_range` | [m] | double | detection area length ahead of the crosswalk | +| `min_acc` | [m/ss] | double | minimum acceleration to stop | +| `min_jerk` | [m/sss] | double | minimum jerk to stop | +| `max_jerk` | [m/sss] | double | maximum jerk to stop | + +### Others + +In the `common` namespace, + +| Parameter | Unit | Type | Description | +| ----------------------------- | ---- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `show_processing_time` | [-] | bool | whether to show processing time | +| `traffic_light_state_timeout` | [s] | double | timeout threshold for traffic light signal | +| `enable_rtc` | [-] | bool | if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. | + +## Known Issues + +- The yield decision may be sometimes aggressive or conservative depending on the case. + - The main reason is that the crosswalk module does not know the ego's position in the future. The detailed ego's position will be determined after the whole planning. + - Currently the module assumes that the ego will move with a constant velocity. + +## Debugging + +### Visualization of debug markers + +`/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk` shows the following markers.
- ![limitation](docs/limitation.svg){width=1000} -
design limits
+ ![limitation](docs/debug_markers.png){width=1000}
-### Known Issues +- Yellow polygons + - Ego footprints' polygon to calculate the collision check. +- Pink polygons + - Object footprints' polygon to calculate the collision check. +- The color of crosswalk + - Considering the traffic light's color, red means the target crosswalk and white means the ignored crosswalk. +- Texts + - It shows the module id, TTC, TTV, and the module state. + +### Visualization of Time-To-Collision -### Debugging +```sh +ros2 run behavior_velocity_crosswalk_module time_to_collision_plotter.py +``` -By `ros2 run behavior_velocity_crosswalk_module time_to_collision_plotter.py`, you can visualize the following figure of the ego and pedestrian's time to collision. +enables you to visualize the following figure of the ego and pedestrian's time to collision. The label of each plot is `-`.
![limitation](docs/time_to_collision_plot.png){width=1000} -
Plot of time to collision
-### References/External links +## Trouble Shooting + +### Behavior + +- Q. The ego stopped around the crosswalk even though there were no crosswalk user objects. + - A. See [Stuck Vehicle Detection](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/behavior_velocity_crosswalk_module/#stuck-vehicle-detection). +- Q. The crosswalk virtual wall suddenly appeared resulting in the sudden stop. + - A. There may be a crosswalk user started moving when the ego was close to the crosswalk. +- Q. The crosswalk module decides to stop even when the pedestrian traffic light is red. + - A. The lanelet map may be incorrect. The pedestrian traffic light and the crosswalk have to be related. +- Q. In the planning simulation, the crosswalk module does the yield decision to stop on all the crosswalks. + - A. This is because the pedestrian traffic light is unknown by default. In this case, the crosswalk does the yield decision for safety. + +### Parameter Tuning + +- Q. The ego's yield behavior is too conservative. + - A. Tune `ego_pass_later_margin` described in [Yield Decision](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/behavior_velocity_crosswalk_module/#stuck-vehicle-detection) +- Q. The ego's yield behavior is too aggressive. + - A. Tune `ego_pass_later_margin` described in [Yield Decision](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/behavior_velocity_crosswalk_module/#stuck-vehicle-detection) + +## References/External links [1] 佐藤 みなみ, 早坂 祥一, 清水 政行, 村野 隆彦, 横断歩行者に対するドライバのリスク回避行動のモデル化, 自動車技術会論文集, 2013, 44 巻, 3 号, p. 931-936. diff --git a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg index 270a61264fe66..26e6118756bd8 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg @@ -1,213 +1,132 @@ + - + - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -
+
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%22%20style%3D%22endArrow%3Dclassic%3Bhtml%3D1%3Brounded%3D0%3BexitX%3D0.25%3BexitY%3D0%3BexitDx%3D0%3BexitDy%3D0%3BentryX%3D0.5%3BentryY%3D0.442%3BentryDx%3D0%3BentryDy%3D0%3BentryPerimeter%3D0%3B%22%20edge%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20width%3D%2250%22%20height%3D%2250%22%20relative%3D%221%22%20as%3D%22geometry%22%3E%3CmxPoint%20x%3D%22851%22%20y%3D%221780%22%20as%3D%22sourcePoint%22%2F%3E%3CmxPoint%20x%3D%22890.04%22%20y%3D%221780%22%20as%3D%22targetPoint%22%2F%3E%3C%2FmxGeometry%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - +
+ target
- %3CmxGraphModel%3E%3... + target - - - - - - -
-
- crosswalk_attention_range - [m] +
+ target
- crosswalk_attention_range [m] + target - - - - -
+
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - +
+ NOT + target
- %3Cmx... + NOT target - - -
+
-
- Module don't respond prediction path without attention range. -
-
-
- - Module don't respond predict... - - - - - - -
-
-
- Vehicle +
+ crosswalk_attention_range
- Vehicle + crosswalk_attention... - - + + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg new file mode 100644 index 0000000000000..18225f188fddf --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/debug_markers.png b/planning/behavior_velocity_crosswalk_module/docs/debug_markers.png new file mode 100644 index 0000000000000000000000000000000000000000..15645a340dca83a14ba136c0192849ed56ba887a GIT binary patch literal 220365 zcmY(r1yGdV`v+f|(%rcrAR)N2EG69`-5t{1CEa*m zzQ6yyGnZio*kOQk&htLcrw-w2DssAEwGV-0CV$hqy}&atcQhg3vA{k zU^kUofk0r80z?w#{%x<({xgwf=HX38GW+zi9Z)IxaJTR0P5SF{1b!HKKrpwwvDJ$g)#l4LQX>0uMR|z(lT-2SD;Oexz4lDU`rZ8E{e9D- z=D)6gR|h^FsLZ5=cEi z>Adoudr#ziFhiJ|hsUI5z%|&1gG5GBEUOFniTaC1vzT{2P8-4 z`m+=fTyNeWz2}REQhb_04LALLW<2t*6W@59v;EA=Gw>B*{Gq7oA}KptpD+=t{>O2LgS&J|)JHvAK>+8OnH-(bIdK(+H)XoXW&?fyylVtU4`s z*re-ZNmK~pfSBeuB@E1LqCC5g@^YApiw1D=RK7_q+pRkNxnuM_&I<))>vQC)=abtl z=^gMYOJMpPf5BF7j#^k?`f0zZmXC}xf3fXMQL58_E`|0FW4v7g{k?$w$3La2-kvyu zb_>5MidvP>5TPsW()fmi;TRj6n@~2=JHv<;bJv5paPOP*k4Bp(9#OHe_19ubVw@mw z`oLo?pU}^Zf!Mweo2;$J%f1|Em3bnqRzp`DynqM2(Gfm&5GqcMS|!_W4ZWV)X6c8gIwI zoyouZyRYf#Z9R#M+=7CeGpm1dV4ffay+@$9A8$|-3=moZXBU^;f&$8Tc|4O+v3;+J z9YPS8v>?2wtl{~H6}46ox`l<^^;N+o`gBUS*d+6zxYma|(S(2XS0dY(Fwl%;Fbbs~ z^XwghP!T~$7#_50i#8ln;{GZtasLhf48hhJe6#OQnFlT?Tg0e}Ehkx#VgF<{KSpL! z_MG+ZM4O3JjfoVjKuE8E4pIgQ=vBj~s`QG`K_(;QwDSb*IowIqDU_34dg%r_CHv|w z*y_If7m*O!XEt24q>&$+8Dr1lr?i@zw3s3S!pDe|&`3aa|L&R>y|<25yJkWOA{}dk zY-(i%yQL9fz^BJw@(&b={v~&_}*%@qhGWXQ6)R+uK0kw5DVReuW_s>OIN$d0n=i)R%W!nCfDZ=EsT{J_*v@PV>%#>U#RmXZ?%DkSQv2nQm>4itU((6*o zAu1(R)z!V}a{cOk0aTsend&Q8sSb5=N>=toeqy);E((MnermP(3#BKV#h^h3oF!uP z5uWcO-w~s3G@;eIuII8j<&N7(d_37LjcQfCt3Q(6Wveggi{ zS0hb%Mzc#xV4xnVr;-idSLzkL53EC4qnEt6B|gG+!+pc;gT7u?`uNY?zJ0TgmB&^1 z@~dv7k4NSmE{ugmJ)bxvBt(w`NizLOYu2N&y<5xsyrD7>Z{0s&+N&JNk-8{}5dh{9 z5s5M>Wzv_ojfzhkDoE7TgD~V$N(P69?FbafUct^B=HK|zmOd|Qpa6ZoawH9IHJF;T zkb%rs*-}>7&UQL~C+zw`GO~#BE9mMYVlVkno7~ma1ssGlP}0vM}nFrqLKB>~{MEBjI>d!0f*!s0A3&Gsg;4%DmgPDnmXW6fl%EBO? zI~Ws=9xAgTY$btgQyj#u32`&|WR80+gc%-n?L?G>-FyAu((h-OKf(24^Z1?~5pXl_ zb;lRGQ^ilzcw2S3e~;8Ge}$W;<~tBiyFdAH~5fws{dUTE69q(O|G*q#?s!FF0M)a07L><@a`D?vz99}2>e#hAuM8v3qA@;B01>|qij~anVJaXLp zAOr$vNup_=DWcD5Qf~eU@lccCuijZWymMY?G8J|;bCu9zPrZL3h(Whd3@bCB`b)}6 zCrY5qmqg#=NSy0qE0R^O!#3&MkhP^!!3v+7{N!D<@?>0QA}MTp(x%??%v#P4COA2z z)IMa`yMmW>_j}wmEdnWPdW&%tql&p7^7(s}pilH+ zIaYcYMY~HG1}vBVHKD@Ku$?M*3oVE_Tw&TFYqWjUVZUW!`*51$K#C^Y^BrSttIiej z`Xm4G#q9R41=Yt-o_uGDSRY!wySbpFid|ie6@M;s|B*FqJ-19p$6Y^V`&Hyb88rOw z%tzBZjOJtUHZ0YVw|4`4h7>+M=XwEbT8|C3Fs+Nvp2hmRQg zKKdvzk(TiR$fWeWoe6m*f6t74f@v+hIelWO{Dc7aB0t;U1r-%(u)1y7;_3}sn6%9V zO4I*UqAEh&rML~8;XWKt^Fpu`#bH&83=E{ik36Hz8mhmaOLqx8n5`}u3`V(Sp-*1u zdg_IAVKvRJ*4X7Xhpk+AN#wO?aN!C$i&-<0$WQxgnpk$Mt88Xvig=9fuX{$Met~5} zB^de#N!5xtfW2UK5Gc@Rg+67;IK5xRpvrODsU>!8sA@aXo9m`!m|d5Q3pQM7aNdz+ zsEJ>lK{-ccCI~np6p!9C-QQ}pPqssA;DTQcw!Vy8nBJXU>)4d{_4hvo8kc;+T6Z)K zpWEq-KUQOH!P~jZ+)8hR*Y5DU&})-uEvH;=AMc;>2DfZ+qnlZz5tsl*N!l(T*$Ief zRgwhi!nUiW1qAeX@^yRO0IsH?){lk=vSG$bNdm7k+Z_uz({0E6@84&JJPDx2ozE0| ze}v&`a-OfBcr;urcqQYdSrLS4`=`DKwxxB(GT<-Ou)>z=a1oBh2mfz>W22&+&HR%z z_hN`bMgLWhpsvXyjzMJ&Dt&TATCC5ctZV1C8Z?u1`-cR5jFP8l;^k@KGcyjR_t&_7 z``7nvkLq)rOia<^Gt*`{!*qYxMO~b@K2EdE77XCvO`yjf(IGxLIf)6jT6kN0>Z*g# zC_K7vmoPy@v0S-)xubkPLRr*EJvi69daYqfYvb)KkU!3uN~rD;ak+j~_WG%?L+>fySgab#agZ| zIq$BI<__Cd4!D8Ms_0b7F)*Qt0^m6Lix(p*LBYoM&skU7_Y-_)a8}3}u8^vk;}LN^ zU+-I6P1$+j*-Yh3aEo+1!K@*dHFsZfPSABQNs2okX~(tySy-pFUc_{RVmZe2S`Ikb^Se9%;?s1jZbf(P9?VikFE2OdEMKE0P}^T%=&A(GB1Rh< zvJ7vcsXtps?D=?H&W5s(kcc)yo!#WD-y)5CLKG2(E|ZoN(+ueYj`b!VNONyBSTWTh z`C4QR19l`>4G?0*N<21h7AqAn6++xDQ6;Yu?_+ZY67rPp4*?m_egFJg)}<$`JdOs3 zA=`tmZ%>A1Sz@h23^6v@U+(k;25cNGmkwcFniAvwrgLmnJq>8gVY>X=h9`X|>=Ax{ zhharrJHrlCk?a*RGBUEi^wy-b7Wr*xQwVrO<&soV+oIlyP()6?M8?av4V^f1)WNl< zW289uWf=nIcP0vIi-rzI^DB>LOp}>It?c|g;IXYnd@ zjLzceI==D=W^8Qzxp{&SyS@!Up^A;T%2^-HQa%ab%0OS*ci|M?YP9J2Co#nmI-}>P zp}e`QdGKa_a}SMDS7(d=yLIm0p?k4sfeUn40Q!a_iHL}babFHV#sQM^;o@L^Yk%LR zgIhT=Eh4ApToAwg7;JbG)$&TG_!k)auhxfs`>vQ*Ck`^p!Dl`RDtSly_P&%;B}Vr3 zw_{qwk=3Oi$DBacS4iLojm#nWB6wzb4o{EQ6|^VG1^Thh11My3oC-M~yX4nmk({!r z3kEHy=eM(WO%&@W)I-CKH$(7kEp27V8>`esVix!vPGxiBG9z--*+PPDpZbw-SBhv$ zt#3C+uRJN-$Q`wDx}MvfN7nAXWz$Tl3BR<+Wk`C0lahaYAeMiD50X!ay16(szIMHv zcuR$j%3-Scyo{bA&csT-o|VQPN4O3RP%)V*HjskYJ_-ODqVTie#o&S+ED7vimVhdu!7nXVV=@6&Rck06j}t#0x26Bvq>~wQT4e4-?8L#tp6x z%wcN!<}A=B&aVMxn9Bne@9s_>4v~sF4kgcJ>&edQrU#-@v zb#I=;P4&72i?{zWmsP?P%5#qRl=8$iQyAC{L+TQBHbG#&d_;pSo6h-9yLPL!wKRd} zI^S*vjswDbhj>gtV)ey#yVU2{mo@iH1@dJknni&Srvo5Y77v)(YLT^{xQ2qR{$X#h zBn%#-*!svRC>(G59wzif1#>^^w~Z_uU?>wQ&=a->Q;K|C$?uf-T z5<}FL?ueyDS-hL4j}|45x_HW zf+yGYOro0KO4fkLo#p+<_X5*aQzt39n+LG+8!{rz>P42APwU{TidFLw7IrW{UuM#h zO^!OxXV;m!k^HKduw=D-H-us-c~d6x1$Fefq^CDM?u4}%%kS_o)p{s{LThxopgzG- zcy9HNB>ipvQ3&i>a%{U|ReUc$pLuA}Ib$PB$ab3$t2Kft35W9o#o`pvV*+@R5!yn; zXS;|GYi_-pCz-cmr@L%@Jizw+a#)q^9~fAkt92^Iy@!12wkXe}N^#%j7#ALy|9pni z;Lp&4_x$;_o^>80TyZ(WZ6Uw@v)kzww6xhF2n$K=d&}+D$-|TFB*U?y2w!)(T#1YX zS;xTW%BFjgb@Kx#BqI>mgxBcITFbGz+AQ{)Ig}>m^;nGqb4a_w*idrv?PyxUXmn&a z+)UV$8(yQ1(0SUZdeNt4B6c@ZyrXB%N)7KEbn0@-SNaRJ?j_FkA_~Y17e=k zryxFJ`O@BQAx(L9IF$s{dJv{*AVC{%w!oP+^j)W6|7fQ*`W_uTTzm5Z)SeY?de`E) zVGA++wX293pewWegZ2wKpxgL!&4yesW?EmAko#{enIV`ksQ6jq9|O<~Bd zC^DFcv8#*@mlRd#m1V>#{-RqcpF?68()ygMlUn+EO`Z=`OA$dmh~?2L6M3kv2(^sY z{||$~`ffFNnucaqSn-2`4eDK&kJtOYW|0w)exNA?Q0gslO}_+&T#GjWeEqev^wX2; zx2kz#E?s1$I!}Sg&}Q#*LODe(3|I8fVw-qM=k|T<_&JSLrJy8h1|g>ULAmcgpNBfx z(_KvzUnVV-oI_Ob!e!U8XS%k~jfeG*?1+3{KliY8qUjH3ohTY*Hq=v!bJ9oqO6WOI z1I2D|ve=LCpXiWbW9P_W9gQx|TRm@wqfpUOTm{Y%R;B=fdpChd!?)!Hjyoc3u|FE; z4cSq7N?Es|au`Iju2VHwzUt*ZuPq8@vmu&sfZB%DbpHcN|zWUk&#l@jVg=`9(Obwl1~ zu=S%s8cQ$3|2=wxX8Zky7%+RfPEVaR(BKjas@-1LOU>N}jDSeczPh{9c`frC5&oE2rceF`@*MeQ_k zd#k-Y$c8q`koYwc717h906gMd@U96WdIES$n*kF>(O^I|$eu(?)`^^vIv|P!4c^(! zzE#;xg%Qy)n--Tgw5i4-`qxXrRx&SH#YL`;V7Gh&%;$)xa>PX6hs(_1l)TF;tbxw! zWsS_}K|S=x3Y20mQWV|YORRcGyjM4#Da$hy!|J6_x&+N|O1QCByEZWjZMv)f%bc z9sLHXLrW6*@K&BC`a}gl8R|;*Q#7u{Y$Toz3_@{8Xi`UV`;c)(!~=uO_9`=*1F1#^ zdj~+bcvgPyI0=uYPg{y42#rB5Le`=>=)6uAkz5H>g{8n5B}gH2ev$QEjm-+3VR#0v zC@h_UlkZo%IT%F=A65$Em}6fOY`%V`7~ys(JwIDzFKl)Z+9t%j2(W(R}3bZpb3TN#-6Tu?<=?@eh`?;wEo0l7Nhp=oUnX0&Z) z`3{~rq)Q6hmrsAnzPH;;YTtU-1$@(^3pcvYWZv76@1;VzXI-+)c*3g}JaHLSzO}$v zwO`s6Xv_zA!p=%RXV+l?3&_B{*-_VI&D>a4gwyv_u#(^-G+em|J^Nz&;-cZnxYo8O_U>pd3+b7X;LnxT$2NQS_FS zsiyY!ZS0|E301%N@12S57708m!rG9EA?uFIm~TmNMj6lPMtJ6xTqw8<2XX#?k=M;}ntH9V3qXksH!6NbZBf?+I}GEk9$6+Bvizv7}z zcmo5<&~uyN+Z%^~g8yoJ)?kg$T{RE;93bVRZGL7 z#+sx$*w6)9xi0Hs>h1cfv2TK9T+ZoPutvN*glIbjjrgbi$Lbg6X9=@br#S@~o8iIG z?IcY>78+mP*jwd)3zlr*|6fNYOMwjmriEbC;iDqlAKjN?wT|@oSXzL8(s_H^fDe5n zFC2R~HoT&RZ-A7zqd*gDrOU9~?s_9IU0pU^xY{)lc?X(>*4xesi2S=faX;OPFQK&_ zmWz*`6J_(S)1X0TQDc=Qma;Z|S5HJEy|!Pc`Z`gPC$+tc+Bd4U_U#l^*VY~6(2)%5 z+i*BH;kq2Ze^)Up9IpAX6Hd{|rY`JGb2{UYTURUQX>0L;LRYIr>VHjbfGd2L)V3*Cm$)k9biz;in8t1{ybDAu6oHaFusDK_OLu&J3kuz$fPg z+$(1xcG;^(vW!uOV{y>?>^e1nS%QiDzsRBTQ`48}q}t!^IZm5ql<*g-?Xz6tcJjg~ zlA9SC>qCbJ51&|kzjzA@lg(W*S-vi!Z;5?+Rm5au5u^}ZAH z_r)-PPXLnf6q~wG$tboRc4KMNS2{ry~*LKjz1>S+AOjXmJbdJEL76Mu}BVGW+{>Cf^o7 zH+Z&Mw=eJjo{pcCszqdzLHCO%n1)7ewsP3&mCrm&F|7`g7XYOg(Cq_(K1AhrQ6kvm zhuKO86zUDt@clui)(`dk&QIk!PmkR7zhsJbC^Z#R9bZXrIaimzzof|~6dGY?`U||G zVzg(yIm*nlfcM7@Q6^hBP@|6KgV1Owm-mRhD3@*7mkL_YVSD_RW7hTI;K~fj$+7cQ zQJSY0=i|01YMGq87me)_IfF>|)s1K&8vd-ITs5{D+Lda@Tof*g#WAlxw?Y*$w2>oD zvH9ENf{2yg`U6jpFn$gU7q%#hxY?s#QGeI(k9Gg7&;V`6Q5+K|Pxq zj2~zd^Us970h-;E%O8SiOmx;HMMMnIUPg_&e^0r?+o7K&V>I%zA})BME*rEo5U8yPil6B+G42 z1G*81VMCa(+rcGA&c?}A3oa&G7Y{-HM>o~_6t`c0>RfEb60sK;{q`jaMwy}D^=A)Q zXm0N1V5Y5M&@RcT4={@3frfi<2U!vWN)%}5VW$wb6cx1JpAoKl+r+;TuC`qtHEQlb z1b^Y=vBYkc>K6fa)C-}Qfh3g?4k+yp!^=H%Gu@4}k@Wh^0-{glS(f~{X2V>>P{6wr zv~gtJ+sGo+)B|Khbf>+U%6DN%wCu6Yi;r8SBiW>i1Xg5p#>$|lIT9ZjsmDKgBvJ8n z`W~&wfNsllilhvssSw6?t)8mcyiNdnkp`;yC(mS1+)S8HL-HFufF&`-9s2FpT=yBpI$Gqd zY#&-#Wr5HD%ur+>RQV}AV9=w0WTEHN;;S)h19HfZ#@!B)@;9|~B@ z94P>Q&twC0$F%?rlJ`V)C{x_#G6edS`EsF`0Y4~wdHDmVWo_-p5bO3QAqS!kH6U;B zzbwF)EQxxxKz$~eKlN_*^HUa>EnpmEOGNo(6l>zT7(YvTc-QKmCT}kTjq4`wo1M90 zpK#E!jgJGEh^mc*xm62$VgGC=3d6!k)9&K`Bv zdlA<{Eo3Xc%DrGe0pHOgWwybXd)^RiX!y3c{lg27ojDlU)l1<^10fklb*~!+Wb6jT zqy_z{0MXHzxfNg$OMcfG?)>W!eIe&*&7c*35av2a*nSZ%M4$;JHY?X1dM>8mpOh(8 z>Zg&FX+kO=f8>4qm;oQF9}pS1#Kk%FkzynH01&7k;@RSWfn21Rj8#D6xJXGycLGW7 z^Z|i8(bHo2?Al)`^7P=ovdJVBeWcsL+{nRdN|+**Ad(i9(fwrQ;5%liEBm1_T%nRS zULK2P0@Mhe-)WRE&iMahpj$(gcimn*?8Vi)H{mVfr&27#3pX#AGa}A9K1E+VIycA% zHM5Oo0~I|uDO=szmBX!cYuBnynm@anTY5c1NJF9%Y%UA)AGuD8zSwP6#mRP;6(l|& z{kvYtS!&)#YU?E_;xh#q2gvxE42%sIC|0F1qx z=Ki_j+V|nQAgl#1$VJuu>~jxY@C4hxaioM|k{mY1Abzm={YVdRdeer#3mb0uS4d^n^za3;o5?J{VA*QjFQ76(=|0;5W`QJ{!&r)K&+Im@VHHD)p4cp8_*6 z0DL-X2UEBBLG~!O5qf%litbm0&I0JtX&-(%M~H~GC{nSgIpvfE!C0j@X}p*t%bN|j z)i+K&_PsxR{>=3`F@1jb$t3l$x`g?97JBFZ0Qnoa8nrur(n-HT%EdgI)L6^GZ`i?) zQb=E67dp6VK5fg94+c%Pbsuoccw-)8&3nDXgmt;-Mp>D(DE{A{KBfGx9%!^rf zb-wuXVl|KEMXBVC@F~S)lk9!5uGHf@Z+{N}uO;-*t9*Mypoaz@jyYruSo$7|K%4Or zr(3-P`D(K`Kq+d0Vdw#GFvz#VYIIf1vl3)c^WxzDF34dzKjQ+*HqVdigdc#rs1wN8MMELT%1|1(PdXl4i{;0!QW^+c@V=T2u9!eA2zF< z6vrnNDs5L;5-lXhW<_Y^=H|NJ>{mNnpMKPy>cJH>$Pf3&=sj6~tQ>4<$T0mFXt;oX z`KQOhycUWkv6J%aJhc*(UCTl@PXNfpzZx3$rmv$uuU1y73(2(g?((0^2oR~ATzxUP z_l;Bh`JaZ`z5hf3nE7SGR#C^Q6PP5ysR5s}a4=qi?I)!mXO9~TN;JBFnX+-jMXZPx zO^a$8Sy0Rs;#AMKmq?zHOrAp1!PNBrv~0P%O009B!P*>XXiP;x(F%-?qxBRc>&4vQ zaTPLXG=|&4A?|g;SFQ*X*Urn2M-^kZh`l|VecBeUXY4?HV(l*IR=LV4JflT?FIgoz zCdTB;mzTiu$zQ$d7#}D8`Sa(T0o#*t%m?pGUbP*%O0;0mEk^Ahg)J zhM^GJaF{jg%8^T0$2GWb7vTR~x6|?Q@pbWU8znwAOwQqg~`u-WAm4b?Smj|+95T{Qk)Nw#4CX9oBK4y+g3eG}k zcnY$B9`>PHyFk-c7!a-I=jW3ne|{@0A#p92j|P*i_UMQy^J=s7D4D;_>2ZBXQ{2p! zGtJq~Rb>Ln33)-34Xw2a*k641!t&>dYR{!uX)P`W`eqo^$KE(vW|lqTqhRZn6y{Dw zdP{`PYMxrAJUv0=*@Sm277|E_N;sWk8QN_(uzhTec;-0czyGpPypJ$M=tJ=vElgEZrWrY6zi?bX@<2)ldkkS5O~KE zyKFf`xXb3j(XelS_xlJ42!xcDa)bEM&X$a*+GGFC#-f45FBg45;FnS?EtU;{sb<$* z_#;>%nU#MFz{(vGW%F9YcCdV-R_XN*ZQkA7a=u35Ag?Q=>(mqupu9g!Da%I zkJ}+bPcoU|#~3G=^!*Ekvh=ZZMMT9@HYrvzmhWQ_*g{USMgT_yKjkVJ-A!kWuGY_? zwhFz=(HKB>YB-yGfB8RIV*kSN%WozQ`fd(4$;;tG%XC5ePNTaCqviLDCK$M!xQbMzxo0VrD9P{fX+f$U@n2~ui4qz`Pa?&!VU0M^B7e#+X01og6}XWaq)L)IZ$G= zeUjMb5XM4K7qy{a(lHJqm`fSzV(G{HzfASJ>dYouGhb{9yMha2$U zA)$HB!Uv$PZg9CLkY}swASUf8s7e}4?kNvI$80t z^vZ3fIB0SlLtIu^j#Z~ID%Eh1>)!>OoO+LievB=)4i8)Gdr5i}<&HG{q-Al)+UdI3 z)>jiQ!7TW@c{XKaY4Ls3gxp(gJv2-aciE20ESR5CmouWLY%lU}eBU|{84Y%~4gj0& zmmPEYpA+{5`B_tW>tg{-A*96skXchP)n(Te)&%HU_5Y6o0&je{rXX$f<> zg)usM_FhX#RI1>wN?|OUI6D&q6Z!Yn787XclCkS(ds`K)FpRPA_y-`c6t{0+=4Vy139xl1Y0sMR{Eud_->qG(yn6rTrI|z}f4x zTCx2lt!-F7OlnyLt^7jkDQV&Dr#!N~Ks6q7U5G8*SCLFU{FuqV#jEPmG_5}Jn{o73 z6EF!(zBFS?x_(EG_CWH7>`!F&e+B=oWdy_<#caRVi#+viue+A+P2 zI>F)+jkptgQJz3#K}Df?DS$1xNSWvt!>mGSI)_=KJ{;k7q~v z-`(9YrSx?h3_Ym-gJYFLh5VX4GT^tUi-X10DgjSn$^KMKzt#-&V?oNr)%otB^Ln%1 zgs>U5e@Pq<0S0t`UqIc!P2n`^yluF=agx%JbG{4VIKj@uJ);cCz0Dtg{Pek<@q!jp z5&5VbCWUu3gjNJ4OWXSIE!*I`TuTGMA*zWM5K!O7o zR2+tsBrn)_whPv-u)TRlE*3gd>TLZGdCtu2oG!O~zo^?1)G$SK@nV0tNOgw zO#o_{u+cuc4?SGnsVs20_^@t6XvvUaIX#Hb*a_&ZbncB{tt5Jv2Uzo5-d=jM&HYJA z1Yu4S>{E>Rg`@erc|%|~_Nh}bSq-*`9G!cETaMHnA9ZiXZlyulf?ExfEHc2`X%X*c z`Ul7zi|?@cJ^ktXpYw9k(B(;R86ai%&ppvxtat1(%6Zcp1Nu?|j|62Pu)E8q91$sw zl+c`{%1WVdO>nR{UqsGQ`r_?tpNsm%*bt@NjEKK)*);VSROKNJo#N>>p=7u!IguQ^s{qe#|H-X2X@s-KBObj|qa zDB+s;a7HUUUj2F1x9Gt z-(RKAq_u*70IXxK^9IdS`+*!Bf2mb@x|cM{OgjkgGcPo%rvmZOkocW9kYl6P7WKA0 zv0gS>7|wE*c!vX_`j}I3s;g<|o}?7hyR`z}fX{1=bA*PeRv zk6Aq~eZ%*RSV=-E>}!#6=W7pTCe0+dZi|0dnnmU>dycL5tH5up!+)tCKKBm7C2Ja< zd@AAe9sQR9kk&!q$97R3t5n<0K5n^J2%e&~W`U!ee!ull(&8{z>q5=@S%$z{zGjok zPt$x-u>c~rq3+@Ye85%&rj9bxX)_F*?r+x}-K%h9jN*jOZ2jQ zaNq$PE-!bB73@|_1F(zT@yq+=dEb_d`tul8t0=`hoQ{CkCTJN@A(Gw5%Dp=7z>$+K zN5nDHYuZcEoQ0>rJsCC%=^w%Yz##j$VFw@&j0r%B1RC~46OaXdCdc{8X&GbECcQC9 z=A3Xb-XpII_f$vxs(IG*?inpgEHqpK=LhgRfK(n9dKj?C$9-o+Ri!=IOMJd#xTD)s z;6B39ZF?(^95!yA^b82wB;rmCh~_obn(7b`P4A^J!AU6&`r3kV{5*y$|K}LB=y3z{v&WDu?vA-Ubh8zKT@-%X*6vPhU zkNY&FQtVlEj)>7AWTFIbX>)6$OaG0(k1Z#_1u1>`E^cOi_qaFv6fX7;y^#cLj$rI- znY)dCdoE#N*4|u<01&P0RndpFYl2)eR{Z66(4^!iToqPi?TP=~zr3ol|K~okl47|_v7Y#xFh-uGqbF*dRwsmPf_8IO!nhTZ z4ddU(brL&&RiB&gg__RT+L=LiPWaDO?_^iE`NpdA9QG``KtbisI7@Ut&^wD^-VpRd z0+;G`3C7(5t%5X4&4eF6?O?1Jjm|7?{Fb#6z0XxRevO#k1qKEd`@}e3oSwcM#l_)+ z=z~ce9vXN+DEYcz?EMdZWNX%9+$Ix6w7`7H9R9w0x%Z7g!*TDsnJM5Dp%zEemd*Ys zvB!Qw+Fz$`9$$(4#QVE7Vo3N-B5}xQYI3lCt}p-pU9z04Y=xf(5YXRO+grtKm6=Ra z&kq8eT?p;>eN~BlNU_F6dG?#sU_S~rom6+A5=4i|F-s?;{Z z)!ZhYcUetlh2IOZ|2TFBE#2qX0GNQ1aIk1jkU@VS$*ZtNK-#eS;6auSLJ1@u4nV4egJp3mlpnYqm z-A9pB($7%jMbvOWw5ETTG?yh->L#p_H?608gEWK;Y&tQK+S`Z^{co8%1bk?RknKjV z3*Z`_NQFjetTu)ROc;weQU-GEp09Uv+w<9QD~-cf3slhIamL)B zxyt(?u>%MouGg8plaz*STP@Yx2SW?dxKWWonqu59N=5x@9Ly`q7MDsRF92bf5l>wAIp zj$m9ep@VYyL$h;~jyx9fw_JO0#gpsXu~O=o<+-)DW8#M)Ah}JOgd=C3QM~MRcX9lK zhjWwp!Pts$#<3#K-S!=mvW|O~_pL?@AoMzXAv#7+N3Fwc@)H4ZtoM>dCQZGLc#dhtg{QYRATihpy zkQ)rtdEG&%Mq=LaL1Hs(bhuan2ogUDtH+SSVGLzLS1Nv1;t} zXnr;p5R>Bg<-t8^#F&HQAPL5zHc85Q);G`i@_b%8LsDa?ZVh`XYdT6uY1MIiHShdcKW%QMQ9WqC+s4ii3M{^tCviN@p zJy<<%?PAYtXi;w*@UdFu$mOoOSV+Dd9Jrow>r97asLd>6sQZmZ>9BT|#Mp2KQt=JD2 zo>Xe2jWl+aK=;8p|K0;ZtXlo&5jlK!ZxgZkJ+!@>ljjYg(^3ZsCrE0gK8Z+y&FS`NgY2x98zRPyE5( zf<7ksR_JxacgQr{@kPzD(@ZeU{+d^n%4q489~Y?kt&W&{dS(#nd7e>j9nrMI1VF zYrVK98Bucvmi~)4szy+kb(ZQ=`Z&Om2eepSp|hESl4T%@_R2sA^Q#l$agl#!Cu zhzE20Sbh8KNRP7dRVG3sxE^z@xi=IgzhD&+JSV#tI~TgC<9h)d>ho|s@WU3E|F;D- zsFm9F^_eNvOk-d}#!;GIcXn3J-*{T4e*Tjl=rfoG*zY@#-fWR-NFg|`5~0562VA7f z>8DNU%Yj)ha4BqlN57R3pS*0!cxcQ*b4tn?D zt32%zgARZ=!~zs*a-LD%T*6bwT-cL9ErDJGq0!~z`c&c#AmZOkp+- zVq%=&x*XgobSp3zhF{m=7M+X=Opfa3Q zI3lj+B^Gg5Z&F8e#A!$)9bEeL45%ajbL?8PxaW7!DKlbwZ3>&~^YS+1#)8@7Ueo=* zSpdMQIK*ya`lo{kC{@JV{NeL*?pv#~Jzk8chF7kyZIdfV@Olj!bRz|*d_;phCx2e8 z4y`+?X=<{j9Y<=JJlvEo{*wRVl>E&!X)F=X34b^8S8G)o3Q^PF=1oesbmZ4l_eo%T2fV3DkDZ*HZ0WMm`2lx7DP+&ln?{!r z3ncfxz^RXJ|2>QjCF% z0#O%_a-4DgF2HxDd@Nu*h_0g1e62=O&%tkEfpe8FQ}{8^2ydiP6h_GY5X&qJO#(y7 zAN&KuJ1Gkb8Tw?>l&!Y(ph_X1TB0#dNGrcG?>&KrE^QjVY`>{%lz4`U4dV?EdYXBI zbCpTW`008olR=9Qkc6dU{T~e)WSD)@7jGH@cEJGI05Qr0vIjFZw*(xHm8Ehr}G+~pU9$F-5SPPxyyXH$s9yW?C-rLOV2OS638yP4S zaD|;!J-?u{|G9n`@$x~luF?AW z^iHl87MIjdC2%|IgT@nPATv)X z@Cl?wq}KD~`N!0AQ))A8qzWdcHx-IVJJ09S%g=I4d32-!TT=Y*kK1vzs8A26Ot`jN z$AQO!Bl*vO+&(>!6IR-W{V#q8+@5bV%9i^Y9k@j-3D*>2=)8F^-FZb6PEQz45BP{P z&1hBG)-V9cH~0DJ{btE16J8Ma@FDC`?oorQXr`c;#8J_?_4|qlUdxl}V?bofdH1vX z-JT%hX-I6NZNK1#Lbf>g* zOLup7y@TI-f9t+$xfXx9U_LW*&e`Yey(c)e>hr!4x=!_n(;ZU<;4F zlHP@Vi1T`atPhVjpWA`F?MfYf97zQ~GUZ=S5ztc+>@FpvpGy>QvY!_#ktOFSI`$}K zb7y)Eg;YiDN6wjQY%u^DF&OaXF|vuGR8olIPD$oOUnJ*_nay?~{rdoK0<-S2CTMxV z{yX&bany498CVQ>AIV7GC&b)*ij&1pDfM6f#{B{m-YBT3iSVUm)P9YJVFtt|)#?8FkA8WZXE>LbwMEV_bZu_=?!f z{C9}`)~y7GKtXUd#&@rHx61cG0dT8_cE&Bd-DF(dvFs#L$22D6dqDd$L8`8cV?8vy@(Gd4F5?|whCrm$W7By>dE(3w<2>1 z0^YtdkM7e08~?5e%NLGbK8;`cn#y*vJKbt0795x3TIPN#V6Lp1h}T!u4IC$A=9yw8 z%}80j_$GzZ0RgFTcr!x-n=+_Y&ih@%zB@|&Qxa8}AEf>1XY08TMV*AqtLG4P93J#; zm40uW*^6S!(-^>uSV_1V>-n98%|F&e&rvaz0L3gloF;`#GPelu64h!LeVdyn)T)rS z4t2uRiqJpRpslpr#UJXqUWq_{m!;jqwp@A3W-+~8?)h#scU7Pwxf-Xo{hUcM_Y9V7 zKbwB2LabI~TZuxnSZU>GKx?xh%}s$z`IkxtV?k0RL6C4#`1vbMyPSpo8fD)YVvt!) z4b`iid^G~|UTAL+cLV%9kB~K3Alt0{0d+i@*GFMpz-8@}hzqt-&F6_;s4aR&#mZO0^P>UnzTr znpb@RT?F^fSrDMfxr2Y$_5K)WlBR{Ai|utBGRATJ_<+{bq)_B(3;|vxRYPy&X=x&F zjD+1)?9`?3o_Mj*7nhl&hq);+`#X}#xJu#{Z;EG)>Y&@+o5h=gs!yMWA?$q2V(iew zS%HL_dI8m6dx^aW+44EJ!M&g`_6tGLNYSUFJ5-sLgQt|lpeJmuT1qk(2%%AM4`Co5 z78}(+_v<{AQ^Fcctm}(8>soo=*v(j8TRUVGuM*pWfC#dNKWcE$!=}qrDyrLkYWrid?>)MDdN7YC zOpmZ+9#I%PVDRRjwr>p72)=dHLxi4ZLh?-g&wZiWFhIcuLh4kDH&gRv)1yeke#_%! z*419c(8xTofQ_7t;r@P5MvFIFs@q!_p6#x=IammP^h%xzF7YqF+`#8WF)-6XExH*l zEakn?MKQ8X*PAv^lOI4xx!RWV?llUthH4em13oSuUp#OD0kg;HzN*fQ1YtPM7*5bjbKFjY%m^p0qBKQ>k;;$-AM$&;qAIyY)|q4mB&3+YK-mTMB0yI8 z?fo$HNcb@3eWL>dnJigs-1`- zHL`faxoFte_CHizn<7T8^HRv|)hYkeBChp(cUqi0e4nzyQ~4ueMxso-s1}0{oR%nH z$MTDR$NO%vrA^NJ=Djfa07ZQucU8lan+;Q3TmjhIs23R9;r4bpu&w_%f-#2@|ApC{ z0LqRV{U-`V&3u2X25qeEas@Kviw>7aa;{+|={xo(4}~HF6)kx{VNI`J+k+L&U0Q?v zfuR8R1q1V&!o#s2Tqv*Gm#;q*IjZA^l=*>2if?~mV*Ru5ip`;w%o9k9yET}X!ZHnx z6FE3;$NWwJoiA6OLC2JxLox1OcfA4=$T-&AJZ1{zi4}@`y;28_B(YoiKd#>PiZ7q$OR;oScX=DdC>3-Q1EcwBI?}Hceu1c~p(QU*LY>gW zzH2!`{G#r+X#7}G-4*Rq$@=E%<5T)sIrW2^+dpjomwzfD!MK3W;0%sQgZ(GKB;FZQ z1cou7VtEHf@r6Ik>k2EMs@f$-RotKT^|i&hDY|vC=fkVpjRcFo+Fr`fYa^sEnXlc9 zMx5LzTZ#mQy6pm8*+-8qlyZ_EHi@%p%SR9)(FU;fdcClNG)bm?i zUER;i_rc$6xzncQf;%&5URR9~59&91`;Ck!01Kzpy3N#Q!00N=<+vj4k&uKZ!Ec;t zvg^E|e#Gbm?{it^-;VJGz-oRL(TiTYmJNHSm48!aK~aG*xHBH*D2|%FVx2?oTbwI4 zG#O+ZZPvMGV07%R7aizybL`pDTOGJW=!i=z|LD@^;7x91;OLBbf9Q-O&PA-^A4|9r zFbfAY#)LJec%r<6(Xj1$FSg<(xLoD*HDGQ;jG*_;mLp`827*UO*d9e827&w^AS@VX z6w^n^<5eUWy`rYA{TC@GsZ32lA-|~6CU%2F*Uk=cy&Q>?*GD{i_BgQRhry(*9w&a# zRXm)u`9&)|@QT_m^ZMe&_X-YrS8DGo^Audfk1S%mi=eHb zM?yrR7E!h)dN3;oIF7~ruxv1~npV=eV^gJi&rUbhik0|Z#reIcFX?Og8Ca4xQedVFe57ZlyM&eK+xq^wG*LlD! z3ux&pOZwJd;*7iP>;i&PR^VBY{hp)~;PnH2=wDZFkR^c5yZ6*mYz6QO6u3Im!OfDV ze_jnb^B;2&cbvO#wyU%_Tm-Ph?Yb;FZVrOE%nz@6V=X&`{VMYGROks@1jhtXJX zMp0~iM<7}}!D1)89pkiR=Umc#r5;`a)ZFeDSHfG*cHnB$pVT^&K}DU2)(!G7x`zu* zkSoL=qSG;zwe%Ssvrv$Pty^@qLsrn}lgKcfW{Ypv;l5 zxJ|_DQn^0vxRjg-=Ah;IUx&BJ5$TsPG?mGsq~Qtu{keH~nz9};-H}vO zkUyJX&@d`Q##API0Rs#T64KHx&z2@-U?%eTgEc?Sl^FzVYyP;k_{$13VX6h3rZBx0 ztv(zd5xqE?1BVgDpL3|k5q4en)Zg($W$I9oWM60j1X+9SOAZ+?6%7tVg4xV}#7?oPgS5Uy{Q z-`g*yRg~v?V%RBjDR0K3N)s=Y(Cj&nJxO&~whLxb8qGhWLZau4pVOqcpAIc{Y;BpE zYUWSm6zl?0Q7zP<46o)slp~_Y-C!gk$K#2xD4W7D5r^yg#b1HMn>X!WF%Pf(d7n)V zbV_k?j-0E4|0mI48wggw(%hs^&;rwz5p!(CLY1IV8u&`n-|h3J=Vj*<723!+fGQ0r zvb|Mz8o0Al6Z$lDrYC3zzB}0yFXOwHtJ1K8 z;jD@lC3(&N>Afwy$%S-@RY<(KQgEKozGEZNI_WPJJ3~9nk!HNEXq|S>{IuPRPuSw9 zsHmJX4-+zkcCNY~wUg_<8s>c2jEfi9#(D-#5I!0;!^i5zO$k6sKUfsNTcp~WZ_~5Q z7#;fc9rLrN3S*(A^jZcI#MxJ#4j12z)Uql9@$Pk^wEj@)M-ID{=K$H&+AM~N2c`>@ z%LO&a>V`YdvWFsN7abYa1s@!c2xob!d6kDM7uF|m<`pqN2r>^j=GHIYtZ_OIG&lyc zE$BQs*+o;QtU&r=fxE*8Ppela*F5a1v`m-pKmN6OOUee=`FoUlI7d&DnNdlllw4d&XJV)iLOlczw9WxJO+6R>7+8UYh{?Des7^`t-XeQJ=O z0H(Yz*ytoOaMg#s3Bf{P2gUZ(S8xg~LtUoh$nFxHqP9a;8|f=wC9K{qKJpHxehi^g zEv~)5AvY(XR!uMsk*cjE;}Ul2h&Er7mdEnzu!L94NI=fyd9!unQVNDU2fgwA1G(YJ`m`FoAIKdre;xI~`3q5w}GD$7c z&Pkhb956&yK!5Ys;U0useipxs{bfz)L7$W^P9=I#G~SFzSMV!LxB6CcfJWTiXB47h zuxFgUeOe;Qs*QZt(LD?Y4e++d4}sreh92IHFHgwJPLEO9Y%S zs_3kqEFkzgutWqt{Fp!!Hhr7pweRw>cwF@L7~OJEEa%o{4-La8FiXe=lrkn;YvR;_ z(+=NsWE zv0Au0-A=#fZj1jrh?N@4$5Z>om|4#Hcvfg6ob^)4iDdn{E2P3tKb3RA<{M$LoO)bF z0qrSKD{aSwjJv>TAEfcOFUm)NQ2eyaTl09OMPp+%dU@!fZ!8Km6R zd!R5IetrrZIDtiu$8jevP^3#Ir}EcXHUmL3@l&w`8#zoOz21+JoDOowaLfCmvZZBsw&;bCvY8?aGQ!*M%i3>@;Fcw^ z@i6^8g>=ZAj+oHRtwp|!(T{>OKr3|$4h8rm5ep02V8~0)z+jdb4*1_cTDe1%m{hm~ zl$BMxZ(&wP^99S&!+drDiGg26ni9=$S<Z^f9^IJ?DF1Zeh-1)_r0~m5WNuiG? z&P!^Gq?XYBTlbHwJ7NBh|GHe)p+gwprtNtd)HqL+$kQ*4=K|V3lLJj8tT}8!>@(FB zNl&YXG8L`~iPig?FXT1zs-ksqUKuW&FTZfX557PAUMtU}-T24C&3`xtYMl0o8@Aw& z5Vo2xN3nqeY>)Y8iKH5j(a6#wvS!lAUsVpd29_D(-s&Dtx^LmRW$SYUB4;)OZ{I0rQmVn z#@KPf1hf@Dqq@~YxAWlN4`1g+zY)-q_w)ut9hOoU!IU7XSnrj~@QA2R~_6IGn7+sCewp{ldl{rrem>4-bjO#1}RH|}ohK-=)x zHNaC-l!x&CfIljL_T4i&gZMk{K*n-e|F!gbb+E?2FWC3XaUB^&?OywM-@^aLxIueZ z7XW$7`r1LZVO_V||B^dSj-t#_Z!A*6;tSz)W$~$#ZyH0 zK|w*$?Tcg36uSm13@xNRxVT&fjkucqmms0~%mg0#qY@HX=_p`n*4nn(aWvBRO_doS z6kE>(uFe9h<+neZ?^*luQ#AA4#qxgO`_PZHG5Jch;E<2f1i!puwoPU1ds^p?ARFjK z;nqlue~o=b0M*7;`nODS@-s9cNSodkg}@&HE5DH!H5iL?LX(*v)PZ4kRQsci;g*Q(g%dc;OLxp5_LZ)i- zt8)(Q{_AX@DSx%?sA0k6qBvan-C`U5O^EX1?~lB^;lOOnNqPZwYcncFD-2G*2?CZ`$QrK3b76c;Y~y3(P*~C37uTa_N-+r@Nsv(HuXt35H!n z@y~Jp)g^|&csHX&Hz!~l$uQl+k!)?+7Oi$~qP%jFdKPiY@MIYTeIwDU8p zw*J_{)%gH3)$>f8@DQPm?&qk!X1!Ijt4kWsqXJw9H>4Qr-->ILeo$@D zH@s-(Wkym-LJz1{E~-PmF+;M7XRwid$*#?$)eVdkSex-`Pc$n zwjkuOkZvybvpgoM?>9~uFn%}=Uk`NG68(_IHS=(pv^xOia-}b!95|78gm{Xk2AWtk&;}k*ror{3ovb;{8ZNt@k6Z&)`eCd0WpJGiCNK9PU{o zrespN3wNjNRmWarSNi5o`8(6DEk8i}+iP&KKg)H}btYzSk(`vR_Qex6!fO=0HiJ1& z9v?DuVr!(0B-h)QEluj~B{&3yGjo->GBq$~BMe#knI{o3puqsYB2swldGGmkM>ktU z^hA@(w^rV*wst_UBy9oRl{@z9!CM?xDF7_!T-V88eWO5_KoRu7#as3_NMwxU0!)a; z$K~7p+q%~(Czk2x<~TIECCc+Jl$Et;QZk3afa-5#^_^rLC*d^o=0M{cG2AhOeyZxZk&J! zRn#a^)3tLxu%i;<>-w8u(dO-N)TX~-2Rs~mayudWUq*OWwqCgg(`_Cl72cfh-~XgH z1i>w|afeJ>;U=bws@cmAV8r-43mh$B<7P7bTME(Rn(J8FGK0zq*0cFPU)gWpOYyh+6jY#fEDOsL2*aURcM=a^fG#Sxb^9O8XZ$ z7To2jWUuGaVM7Wg%-e@pEmy5!TCOK-0J-MV0LQ^f=7|L{8!~Rvxf(Yh}?KzF;=}9m==hk<|HWe3WJef9I!< z0QNxWlcwmc^ zr9@^bx&AK~K+bI4?%P=5dpoK0$@Z5}zjqs5tu1VF%}*yD&(hl7INRMp*--jffk zvn23=b?3HTubm-2=Bt#hJO^jBa+58vcpNZU%t?!_ zk@KF{kmTa40C_ENm#a)W)wp77lSMOUodM!fMD>Tq>Ccz79|pM>A1M%h`PE>buNWg* zqD;YC2Euf6(1*Xu?Q~9=wU%}MF&t9kfBF-0VYx zOvC)!LBEZS8A@mU(p5qZcy4Ag%dfRl(NX*_BwG0VP4L2uv!hs};S+{-JCO(O^;r9P zhCcHy9 z==n+@`R3aXb#?W@Uv$VX=*4mR%liAhdl|{FgK%CJg~DxNb4L`;Tjy%EjRfz1c>8-O z!618pDa!xy*=Ruin?}+=bm=OzPZ<;D+YW^iBmCbpTPsbte#g$Z)xXN+<-|U>OT1Qq zsT+>LdLyKyUj8OiTW@O3P7l?~AZUw=S-A z5Ig@m(&8epmRUk`ad3Fwl!C=YvInaxc$!WS1~Z^+&CZ~xuR{fO zdes*k(LoFTbi)9>vm`k;d7&Qbn!Y+Gqhne%S8oFYii?AJsRo!dw+sBcy-mLF^hH?Q zxcN#UvOy^Xt?TxktN_Mom3-NjWZS@7w&hx~<>I&;vnvu}`7?yhtS(cXrNH2B`{gLIew{$(OmIJnjme&L*(Uh8v z!op+{5)yp+)|XMvBm8O)5Xx$0v$x*HVUtqc@IDVzePjIEoOeS2n;gHnZ{^iw)$ux$ zH-3Z5;ZJ1okEeU@r%vZm6E?>}MfEqyV?$&`7#VArxf^u~Cq}CnHY`SHCbN+xcCr(2 zQedw@e_O_~E( zd3|ga(x8t1Zh`}$z`j_d4)pBuj5a_r)b~PRMy`4Bk$22ZG`V`YTEHBw%l0sf$3 zJQ?xZkXG2l1l=ajo4!N(6w-0P_1by`2bjufXe5e)QBSlFEiEks0>my~{@SD@3riNjgMF)wDZ=?h z+(kt3_wTRs&$T2UonyTI1TSBqV6!kOLH~63!a-LR9D|9GNJ5f*G$Cx-&v;KR?biQ> zt{uiie`TIILvAH2XpICThC)2ThMgKOdOh0re3TdsyWbE~5o02Q9l27dGXl!*uuEW} zQGq4mp~;t{I>?mh=F~k$S}O16y&vyuA0KXE~;-{`zb5cVbGkmlm znP0V?&A*9<$o4GXXBZJzuK?1`a_vE&6tiUAu+j?UzZ*uDHS!{N44);YWn{H=A)h9h z#V|AHozC2cHN3JiGYaBpUGN`&XFRjzARgX5&-?WY8%_-gF%g}z?4&F>N;W;0jP#Io z?nRjB*AqG=l3ulXXdMzy(0;EzSIY`2V4SE3v zSV;MlK2D_hp=o!;&`0E&Sz;{PPV%&$N)AyhmlVpwg_FA?5;E`|yb*MW0w$t_jNdX9 zlayqn+i$hLGpd!SJv%t=AJSjg1X~*v|KHYHH(wirr!88y9Z!nKwvN*$*4qfD)tPC? zs;aFSKF+&UzY>d3$Z**+pN*0<2SG@Zihr(j9u8~V_{+Fx1VTyR-LUcIC>A%<(i6LQ z^DdaG7K6?stw}DQ1CL_hYj4bm+8;y(DJdyf%{*b8J~%b{G(S&zUOQiFvT8=1mA7${ zdhf{XZLnse$|Ga{fSneHH<;7N17|r%D?P2?wlj; zn!#fdL5xoGm%U>*fs90)C93rkhjQlhW>kRwVoFL%IRzUmv(}Lh$0f|r=uUcH zla$3(4Fh5`!{UX^ptYs##qgNbS6L!yI^?<%+w7aYH}~q-oaf03o?g*5=d}f;r5G-Y zp6p%^r)gC6G!BA$w*{U060SS=>Sc;jQpm(esPb^xGu$(7ljz2dx>7}6+)-+t#{4r@ z&pK?<)C@>{VAJ>Ft49KN=gPXjwidt7Oj(=?5+iksNnNP2zZy_?&Lu3te=gtZvrJLOO^u-y(ghP*zZA{Txv_ZHZV&!iQ_bl340> z@7}^nzIO{D^ndBv^E{>1$F#8%KEKKQnf54q$NChjl9BIUSAoC4-Sr0tKx=z~sEQ5~ zehgBz36QFWT*3zL@=x-Mj?F$w{K>~~QEICwgS``#fJ~NPstAoWt-CO-**mH0_O{phD_uQ4nHCDae=<|L%6_L5=0 zu+*uY#2?Xg%K2#uO&=Pk5cy7S>IvniCFn%@*=65IO(*|vnZCU}Jpv*kaA+hvTkI`P z_Ahf$672@Ox#Ee_THnBfCC0pc8QJSK9E@1-l1(kd>56x;?B%j97U4>gH08U*xa)yc z`;0E6^m5`ahU)a(7X3B5PzVxNitLdm%ZOk9OUf03b^8#O&5HS?vge`Fkv%F=pWp0R z)y7>Gpu+5)6`*=~Q}ExXc$y!Wa8L5S%n+X|V&TUYvlPQ5YY5&b7pFYcg~Xb^u7l>x zebCjbiYT;N)-(}5cRHWC4ok@^DMWbZ!cR-qA4@?0XjU7HARa52=Ef`KpI2^3DZeNG z03terwaXk#ff~924UAHLou1VP!qM<*gUuuA9b#E`UQ+2WBUAX>W#SST0@W)o`o);H zqpf%$>}{GDvHOBm(_O<8ojTdFS7mo4e8nxVN%CJNY$_m;2?Qd?=3k>h5#Kr4xI=tw zH%&Ppv8Z&3!{3|ev96K}_1lzB5`CtGeN@jsGmWMpYTj?Kx6o_~U@MfY4&dO1!3C}fr#xtPJ5IAEo}7;zW=3~CHRs>;h>0uF@2z7V~SIZL9v zh6cg8*Ujy%-Ete8rliVh70M2b)?qRP!s^}xS&FBMEgb%?meKk&LqPRoz(EGb%K*(y z`t2(cuPeP4*}LWTfO~k{GLE;`+^bq6^M)+f%B40|R@gbik=ki+O}rtgj0_AwU#}b* zrj@hldD4@X#fvt1-u+NaKtOOu>)W++$8soF)~Nlhnv%!|DNyGZyTS?mFMp=@=&5W2 zw%`knRShK6=fb7V7`vMPSnlf&V0zED#Tz`I_2)=->Eif%o-^w3DYzttu2k~-<->d~ zp=llPCg>kHKHqJqHqe7hG|Gp+fY_3zW-kBT?>Kurl5cx#rRM83ySWmtk}S&b=GWw0 z)Ik!g1~G;Ga&o%qq>s(>pdbjVx~DqTXS~O`3{L zS?}$0kYZq9048-zADeEe8tm4xhTjQgjeINOw;`@>4=9OWb-glnbcg73r8m2?Wm`3Mx+1o|& z(P2R4bt1~rUYnep$jZxmaXIfo3K1vCu79h=NWw)J4O!{R%2Or`SoAd+>X+yb$#qo)?f#m;z2QCA+V`&jDKZMJORWjcm|I~w<1QtIxOBEV zlgnTJQnB$gLvq>5T}Wun&zFmvdsCPy4nJ6~?Wny+N-{YeWVV)VH4FLg*c?o3wj>Y~ zv#XRJ9<#!QjX~3oeRDkuN;*wTHFt&^EQ3zlZr?}lb>5347fK^ouonMyavNW|$r3== zv^Rd3aK))jVhUD8vr}IQD)do$qanV0ug`6at3fj98gN;$Yj$K-jGI!fUFNUFPYVs& zmNfeDIvX0}#4GXd+KXdjWB&m`t^M=i@?S%?i#0r9xV*Ii*3(~l^hn}CBN9;>yeehU zy~w_OmI*f}qd8Z1py&?#R28Ll&-E7)Yq5mOWNX2aI6OBe?HzEu$J4<=svsglyf4g# z$6~h{=Hy7-g6AgRj!0_sWkoGThh|+Uti!K2Zk)fs#LQ{DwDQkKLGN&!gmMAuSEl8y zCPlg?)HDc#LFJ@bt$`D&L$|FX-xK?6X%*Ghp27U8`NqH)^pdH4;s_K@L#TdFkXG=Vvi(;9$v@PCLWk>a{>@Ltkrs1>kXuBbHwsy%Q{`Vc z7bwWf;}4k)yF2|jrA;)yl+0idj!F3A!|0f_53LFne}AM~eyyHw$8Vz#;vc8RziF9Q znL=WTlZbbHaaHv~UD2)*hyxxTyofmM-!RIKtGTEpV8m_Ni$vh0 zN8bu$Ld>(>q=LW|)oX{s3L_|1M6xsEK#&7i;YS3hahH_P936@akzjUZ+z~ z&=!z$adOEhytVwZ-x!jASC-n@_iV$>WTO+Ql4aT4{n$Q~X3SM9FDejGb>grEPk-N! zviJ|0n=UWbA2Hm+XIE zf$QhkvS$Q$@wHse#uZkfPfW~QX;j<#FSG3_PM+YE1#Nc{oe`Q0$>;nsN>{v5J%%TJ zlM=-fu2@%kKlK7dkbM39nTFFO1ZbEcv<*wDdDY#8H&kY-nF0dYdUruSxg!w8>IS8} ziWrFEN~U4k$kVp^6$h+VKfi0GhWTL$Az)g`#YZ059pBBPq293!Qbb6#Fd3?Q_+@+s?p6DcZuGeBnny(=JIye>b@Ib0~Zt7nt3EqtY?S+k-bM%$?BbfoTnncbE3 zU672{E`x;ouWxa2c;IDs?1sRLBW>|(c?!FXF67km(|CVm=`6eW=%X85>rp06-W`P1 zg&v)d*6|`pcb}7D7T_(;ubEmT7$pkUzrtpg?n8 z8$XO)ggfuKS)-%M2d$vaO^ulH-)ZLCGmieDRQX041&}=-)-SqPtGpnWl)!6R{LJ2H zY%w->9#f~fqk|i#sqf#zQcpi-?sJpCWoO4Yuqu7hc7LF3+1^LD-qd6o*?bG}NZakW z$K~y=UZvV{vy!AtVk0OX;KV*`#h!e|i-~Twf@i3$6M#;7@req@jb?Sd{|$yyU)=2Q z*eU+QQNW_Grg-ptK2rBSeuPI}5Iu@079&69vu?HDW8%JM^m92GG3?A6@rR1^ChLOo z26JH`1GaBY4ZT*SxWBkBB-~6>9O#*B8-LB8- z!1p7sX=cKd70~v2A^xkRuzS#EPuNrIBLqJiNrCkp+gyxRx!}K2rc>g1x;4{>Weqkq zGMs0b_Slj7Z)4)2g9-q&BW>%JzAjinaVnnuOdY@Z-sZ7p4N~AVjx;%LiTwuvovT)* z*Tv(meAGECfd;Tl#=J_!&Vh)~X>aeM^vr)XIadsL$Fr#DdCOI21!lmp81gi(B(KtI zy}L5ol&)6^BJBcP~t*M(AFJ>4Q ziid0q3%Uu<{K1j$^+R(vQZ@H@^HphE{3)Dw3IJqhEu&%}7Y?&O!)iHSj&0L?ZajXA zs=|*XE*D#?YxX47R_%h3F zLxomND=*0l{Wh5VV-J4G*B)s!u#E}Cow}33YGiDG-Q_E|`JGXD5;)T!pI(3hU8Qh_$+v~jDv-C`CK@B_ z9@IaTxcoEum>n$T7-d6RpnQ))fwrq=zhnCgXNw#C)Miu(6@lzr=$rFfzr&MP(eRJp zeV{J5_opSJ?D`5Vy5l*vZ|AMQW*m?I^;^~JLg_b4^?Rp9OeE0UXyM%U@s-|MDHfjh|&)py??8oBme(~*`3#QL)6*9G+I%t|4%r~t!$4xK)0 zEQf-Yz6{tWpQJzJ5|%;A{Ip64o0E$Vz;%?j6Wj@x-{ptihqr3l&2u&j&A1mZklrvj z>LCXH%V8tOPfWF>xVte89$~?&I{SK*4VEW;;v#kW1vrPHT8;KHy z-zp*!5LgS!4VeKydh#=P`k-~_TeJ~1ufrg;^%79F;U$ZFKaBad*}(o0<7JZQzhEh< z(Jx`1ZqrQD9r-o*1PQOgVaqLuqx(&0%lSNCaJ>$1tt66$vQE?Q28Q<~|Gvn&VyssF zij+7^zBrc65{|X96Ad?A4jLV#_Mn13L=Rdm~<~Ve6poJ51Z|X724F|-kWMx-0 zRQyT7+qdQAEZ~V>T}Np(U-arJ^qyV#dZ}k+i&U2KmXp?~_}N-`wxf z;`ASS(5>qG0t){*X0MkMjK~FAIPUn0Uz6xVsd83q2WdU3O_|XM^B2{j%ngCN?_VF79;+_a^0IXm&+F2Emqp8_D#ksZcvSJu~jum^YUPMpImK z;00yKr$Iz2cZMbi#GrcQ(1j<&iEZ&hq>}g=V>SHffG;DvG(wPV7^=~A(a2w{)8s@`eVzBDvUWUD>T(uyy zgM&@K&6Em`AMUH2>7KJ;9W`Y=J5?@nKnNWBu;JCwYiC`haeqM=cIN)^-kbh!9mVCd zvKg@pWLq7R{uyGA9ge@g;ReKK31Qg-k;M6T`iqB&XqStS;vQ4wovTUhg$s>V<~s1a!Yg%@GVj=ld8&dOz9HzTwE9qm+5N z$+TQxK$Mhtm;UNKT;kuyc3Em{B#--$n$?Rc_SVg3qpSoT=1rmFz4lv;NLW+(PPk^! zWw<3}h;|L(VJf#@W)V@0;b!Z8;?J31l7fpGMYO314hdht0^Tyy8 z59aE$l{}t_-HjdUWNbceJp`2H*W-APoZuxFODGi=3E%G!9@SXw`Qs72P`b-t;fXrK8 z-=L4Cn1#5CD|A3;gYa%w33~+!XDswcK?BY4I_z{?mN()GhnVp+Y(^)*WnZ+^k%DfK z9Mku5Zh3K5S&rrVvcX*ZU)g(-vG%i&*|sf-4i&3UX5-r2^3DirZ22CjoiQ!$s$0^V3eA) zBW7kawnoRwx-`EWHa#4=Ue3`pPI|&y^h@dW_IFLU!dTMk_?gTuw0e^~VjOz7M?c^@ zg$m`%FG*or*p6M2wl=#g{_aCq6fwRb=ee)4SF6?4^5wtVwEf8CCg<&@>7l8entBeQ zKFHG4DJ;>3gS7DE>*r-;zyL4Qi(QUK&HXB>kwsv6#82e^?AbHDI%Jp~ zk9Q8mUAsJnMn<~8O!+IU0OvLl^Wr-NbDyk61HNye^JcbY#zq|;PW3RFzYfk7LybD}M2kExYXet+D5Rz6Nm6LRIA~V<)zU ztJGS(yXC-rC1PndT~|jus)Fj9CR5!_C?(^>4t@=L;$$wGN9zF2EFfF=7M8gw;7diB zWb8?z%7ul6J-sF$92^XJ4y&YFJtROBFd}I=X+pi3=`@A3TN>+}8QsQjh_A>PFEz8- z!~efrfRjPVavkP3Kk3+TishQL(c}BDok`=Q^wYSjXA?YAU|Q#ggo(TJyhJ>SCl({M z@-wi*>0&kG-#}%&K76Bk0u7YW;P~<+W(!K`k<3)h=^4U*eGM9ey|J-spK9V#4|ryS zvbw_u8B2o+e ztQXqOop2U&JrjDygo?7%he}v>IDc>Zndj)Hacid~uznQ#oyq3Pl%IoZ_O)!|QS0m< z$;piSyIil?9}f0*W4+28obLFbNq2PPakZt7ep3>D^~)v8jAm{|Nt)J&jm_fd`WtZ_ zFSfhbFTC0hx5O!XJHif5#M+!^Bviup=QWSG)sG&yAAS{!EFNJnMksXGIXN*rtPUOO zZN@d|`5{q%?e|`Pce*6p@x1GyUr1PCqqi6|k7zz3fHqseg zEarY1P4QhWcxOI@ZffO~>cV0{0WDr z%|RWK4g3pg-7Nj7h~}#)w1R(pIw~p&n)@3ew!K)0ohDxu$&J-`VSc_L;=_7}%}vCo z*#i*yYgp-3%!NgdSzNo7aS0SO2?Vm2dA%t2RPwP*XQVzlTizV9dOVrhSYlbe`9Z9dC)*X zn2?y#wrtBWWIJ@<{rgL3=$ZsnR7b__N_nu*oSYm5zP(I_XyJ~*?q2^q$PsRzitZd+ zkYfBzX}$-x3NFs_&z!!IN=wS5xL?9rBP@HLS1-YvwXGBbF2fkHf0mOvHrOo6TJGPG zm^Ht0DYEL2bYQOj`(ep@t`gHJr|)>7#~=ZEWjk5__HJP2ZPA}kD3fQb#Zx74mDd}i zNGLKg{86H)wKxnK-za(6xJuOC*;-+NDt$6Q?0CG1+xF}(_sU^19};Lj*wYy|LwHt51kCe^dCNV}e%#X%1ox4D$*XrHJ_|8!IUKA= zx!V7Ok@6vPfzy{f4gGQ;xdNzJp#9#G9oW#Wc=XtmNfsz2IXbO^j0`Gx=Nhg~-auXc z4av1UV%|qk{TVL!zpB}Gv~n1}`&lyt83R?AtgYnS&)jlJvv`{Lm}i@&E;NJ*UAA!(*wLR~@NnfMjdHH{XTRMf1j(-4 zlo)C514ndKZSnYXzXHM2{6XdPUJ|w4bN4}kb3t70$L$cNroe;LVH$;w!CacRwiLxf zx^+#6fAnD)K~oGZ_iMPjK{Z2aIy+I&enJ>rQK$*poGo_9Zg`ze@ownQeLs*lw7~n= z=ip#tFE`&FJR@s>ewE0hi}hePR5vopL4m?i(~O`-Qd$%jcJBK#3I*&FUCY(6a?x5s zh>+6lIgi_vi;%_fE4QzDQf6Twv^g)ww9&n;HY~-DZFo6DL$bz&#;j`$baf$M>m+!N zm?Uz(O|t*!x;~kDLE`RQ?OxNp>!)4PeS%GQG(@^5W^T{Y8+r|LS4hC*VlZjbCBaSC z9YVZ%qIzxcv?tfkw`Eq!#2@HCA?A(+VgDreST~VdUK2T(v-nK%fJWkRlLiAZVQ5mT zC?XQ%L(83p0h1qrrZ}kzFcxGgXCvT7 zGur$?@#mD@9VB)r`j2&uSn_1F++Od+u@cyPZP#VL$lcDD=^t2G1IyEdNrm#ny%d}0 ze*U)n_v33)Q6#5K?6Xb%)D|TaE7PeZRpJM^0Q9b^yeN8W)CjcInp^Kpgxa1%Qy6x{ zRqx|}mGN?w=@%Cn@gYl`b&)P;2pO zTEz3c*Q^D=XjSD@uUPV_titwt!M{#R)~TXyhA%262IqHTV>z=G&%FW28MRX`nEdIh z@1a2x3d>Sd=?aBqT}n@d6w5+l55vttouch$n)tP!C(2i~mpuN(I_*bO)V!Q-FE0`c zR)J^0SkMi+izj}X$HLduG^f?54Q{*Zofl831MGT&qugtDzM;dp+K(hCGbPKzeie9) z*6oW$>JfY?X#;w0E}j&7w7O8{*@HGS$_vjUoaW2B_gC{#U#WZOgw=(ej$7Iwv4vG* z9nZehX$;47%=pAE0%i5r<)XhsTjEk&kA5&+BVV_!a7hmPil-!VDBnvNog0+k-K^nh z#@9~i$l+_OdC97FnOmY9&`OSc9e+?W+3+g=l!|5ATl^1(&HW(p$;}2?&O-t>yIhSP z=5;cd!+iK*)mk3`24`k2vk&Z-tDW)W2rK^|Q)d|!W!Sat8B!WaDM6Z{1*E%?W=LtJ zC8Rs0MY_9(?rso}2I+2)?ymRtd7t-N-&+3AKeCv)@9Wz8+{bxtV?IJX7R@ES%rC9) zI9pe38YX=l1QzI^0Q4Zo8(A_pn?A>Dmt6E3D4Tbk`gXl(Dc7Gs+j7=lzvdYgAIfbL zHtlj)Nmrgc91>!=x9Kb1;Yj>P-xm9Ct54yJs(hymMhJxc7h5{0Bn7ag^f>wPT zU-`Z?xs?_XG1?Pu4sQ$Hg6Q#3m$W?4YUOBt>Lz(neAQP!KvubFtHc^%v^wqoN%gl~ zLQsBTG^6jhLdaj(^U;ilmhOpSe{_eR{Yy)j^vx4`1(|h^mGq)r?XH@q*`i=09M%k8 zeCK}SPyGY@x&EWZ0h597co1Q=_WYnk+X-0 zt1zH#4|x4zp}0C{WZhi8S&eY-a9|!zVQ#OqI;iBz!Xic+XPnl}8-9|(zl?bb%huZc zW@2EbQ}$saN6p&W+CF27qI&TN9Edc3<3%e&bAv*ZXa5m%H<5->{-5mqt>ui-;him* zfh9gZeto22WlK-7+cX3?iAZ@IeKX2SOYvx4ih7Mh`If{>9pMZHhbbp30!>)=8(KMQ6SXn}%xge#{!Jvy3 zt0S`Cwv9}<T6&=PZj0S+ zu{%$pmi z#bwqFXLfe=Yz15D>%Tp}xNvirz}aPs27Z0FVm~b><>XcCQvR-Nv8%xEd+?n_!+)}j z?)M`NGVhp1u%8T5uJm3XWnPEv7X)?4P7G+f3|@Kg}q);mxwi% z=werzYetSF{Q+t9DGxde{Wf_h+Hx8?Tyq`BceaQx0og3v+WV?6Cm5e3_3QH&57s9%x7jsPuL8SO7jM z5K$Qe34Ty+=yal={2L2a!V4ZYR#uq0h}@50b6D_0Ob*SsWnJT(NA#?lwm~x-Z4F95 z?Bl3>eXXRr@`1xQr6W~PXNYPrXC|735MjM=BU#7uHP&f-rXV7SZJHAdVxq%DS!mF& zknV}Va6DFBbRxk}?wQ#7MJ<;&T0paQP#J`CSN?wx)`d&a^jyBi#g1^DRuBk8Fl1&# z&Y@x4qMU3ipIqs1b9`!_ggChwDXi$DC}ES8LX3Kz4(P6iL?l^#e`tmTI;!}v&UmEQ zTy+lLN=AEp52E^tVOhUaN1C>0r?B)HmYCTjn5TlVVM4k%|RhA*T=S(?}~rtHZW-saY{!6Us~YY|Ke6W?2>cZi{ zXJ{-RHxAuJ1sOgBES~+{X#s)8TAx-g?+a3B=gKd;h2{tyJ7{B30G-3acR$6xi;viQ z5TObT0sdDf4$?OuhNL+3*ihmxI!uJrU{w^6ZAJn~!@?xRVTs@~_|BhX{5)uKz16;W ziN=UU$;QqzzXYV{?T8hO=(pwBVRsukByp^zo@sr{Xl=G1sR2)QreJvoLV8Vl}zB`On@EBy2Xz!KYeq zVL-wEUO?cxLHnIQ7#Fnx-M`U~!;_D=xuAJ%X>BbDw1ObiEE@nMcG3Hjs5&BX>iWj* zz9(L5!tk&@G++5MqVq3O?AAz#i5A0lJ@_nmYdz#ghcV(npspEzM^!8f8{%&em(E!i zbVG(_^5RGvJK{eKIH<01xsgko>@}dkJ8_ry0QP^71Pwl0XcA(;4wVT0ImK*&#Z#Ci z*tds0)tB^Cn0vKl`?`{1U!4Cf97qc)B`+Q*DTG|z=KR&ij(`87W)`3qfET~R6@3NN z_5{ZDOb5N@QCo6jFf`|UNVD?j7A4Lgd8ERvvIylxUsbF}I=1M05X_fT#1bI7=mZcS z$&!r)*YZICAPEb7v(^WvdO;@sqa)Z?4Fo=o<54tY{Pqy;J9$T3Fjhz^jtLoLH21Y6 zBa+f|Ni+v>5%@ww^q=K~k6-FecQw3JD0H{s%dL8}xZk28nU{FtgK`^uq$YY%J~N^@ zZv;k~`ehjZ64nEGxR*n8qvW=jnfgtfkXi$`!6Nvt&>zeoaUa#6R>GmbI}pwbM>V75 z!p5gdzmFn1v_i5(jVKnMc*>zf8dBwf&c|~T zF^2~~^0;EbmS2IEU3N}*!qbf)il8G2gd0eBLEj2I;hz&bsniZ`HSq&?3B^}W(Bd4^hMUt*`4NVRun41ask0R}GF2M?9k#j{HB7g%)vXoV};)@G+DHlQjAc z6ysH4FP{EIfEed}*Bb~2EhyI1q!PBRzoppQm4t)lqu2V5A8o?v4u6wpT5K3M+!j-e z=xiTNNlB?R>OpeXL4s|BqZPc554ki|jr&K4qEfPhEP?(79SJ@iiwg=aat$0ti`IHa zB1XP&$o33k0C`CneyUfWh67z^@&>*CEY#lejpTXuQ-|Lb-_=cFaqQWzHvX&&$QryI z!$7#wbU53&1^@Nqy>O*iVQ(y!H_7Jg(XDX55+LyEeRlmiX$p&z^PKZ|{c;(+3UnZh zj11T|oWu3Jy1te*^H~OYH}Eid>1yS``*NU?0RCNwrp`qs@E2KiF-{IxBiMza0^ak7 zvWt(*yGeb;y9$qA%ngC zRdP0Pn08;{m~XlpQ)Tie2_p&C&l!nlB&8;Y+gL=bdRzi@n?$XCHiuxwb;b=rcM{D6e4BcM1TCt1Vpg-el6i8vS-7SrzE4@Qc z3DE+@!<8@kb$;!1fv7i$V!&4hD)I!7>WS(Y6M8)jVm4vbEQqtvh*prcLnxUFw>bwcw_#|j$uJ(1L&&@YHLc+<5l+@ITnHkzac9Xl0NJZKIaSFIj z@$rTlB%}2JJt&PSP$uI?;cQ}9w@v~r!x=~Z(2|pCVv-;e~dh&4kx3LOi7}nD7+|93O9<>0%>!}Gxx~k$ZOkef z5?oh1gl!44c#6F*doQ;n;IJWh5Bc^6zccc(xI9r!u$Gz+3>T# zjLACzxt`eZOlX8T?`Nq{33GyK-d;G^EJA=}%&-j2=}J@&)+am-#Q65?82DddDKiw9 zRFQ%n4yT9hX;}vn#4}2qoID}fU7{Usz$j)Hp%}ESy6pf9vM;2?DX2!CI`hVq%msjL z%87mTl06tLOH+Za<6H-}ERi`%C?y(QtMF?;g6hT9u$*+HXKVx8G@#8Gp_TT)$5?!# z#%b~Tg$^~)jQodi14aOlh^;LvCK=!N`ucia2lA8>R3tS*KAHG`6LF^hZ4)HHFcALW z3sBv?6Xq5+2Q#n(_9gKhafa7>2)Et0;7)T+N*bEL3}<;B9{xffH4j`;7Q*&Q<2rH) zuS36JwldIf0^l2c?(cXau^E|J?>k3j`lS0K(>c9AK^~+1e3S09qTz)rYjX1w5b6iY z`l!&8s`QBP$>*Mmp0+&O^odz!l#OzESE@P{;)>5Aa&&Scz4u3TB@R;zf4{|eS9=@c z%B3kVauvDWn<-6B0=*#*kaM-lr`mxv z;&kRWB~c&{>;JFdAP@(t`V4thNnMe!kDnU8N}lwtWPzO8@uT3nKhkv)_GQMzz_sb0 zAS;L7`fk@3>v$Gb6Ck_uttX}Ydgkc=Ype>2I#$n`D*N@I$R#9v3L(~j8m|^9qTi&6 z40##!bL+bj__$m!S9(Bi9hA8F6rYJuK}W(5+)FP89pbX;%%omsQOR2Yw2b7a)&ySz z+U(?c*6T4)?me175RUwSf9EjJ)RDDgod&E7ppYkMm#>8e_*|yGEOSNhq!YpQ&jpGM#&klccObcPdM!OW5uzcDd#N!(+Y3 z`npc&h>1d?$L)L=THpy)Cm5>8YP!n#KaU!hmVf;&6&u9B?c}A~@_`f&CU>_4-l(-* zoUxv(cOnrS8TR2LJQlxD@C+;@fXJx-Kh!%f3vgm!6~_WBt4Y2IJu9=c#?B$dGawc^ z=DWoXeE;*Ob4hMpP7X9}TC?6Ai001^Ti|2{G#|{Qe@{W~L`Ys9_2>~4g19U)HGoTV_4WpSY4t{tT5XJSLc7b|${Gj<}LBImc{$>DuA=)ktmq0*6uK(0q7 zEv~|@c$3!kON9KHmWwQ0uH>7Q0tTiNnV(#)Wc6nR;0LmC`B?=J8j+BQS*`#+09QS~ zW1<3DI$D(|O>8yal$bO)@gPxbk)y-I?>+1&W{U5;HAMquHPO}9LmTw=kU?pqm*kH7 zSK8xy)ocQuyob)=MOO#2lVrN4T_JYVT)sMLY61Bl6La$NzGo!DoYgfn{>;t+d3A;j zXf8Tv_)JXIMIP;udkXw(L&GHZ2LYL<7_EiP%;+6~jY_e7rKgPEw$b=#(3Eto>+c4u0>Q&xwKelqw;F@#g!ZYK9^m6-47!G7FfV!o` zSoO^BgH{+0shqgF9D3u@vhJ)vA)7CzBZsQ!>HT)MT*=UKDTw^#8ZpT-MY-du@6(+rAJYXLJI7+K> zNd6mkxF_suq19p*hk2uSw8t+jCcvk8@B4ldxdP|3G^^S1FzOLvfLQEVbDYHyrkJMU z80dtqw(}#Ea}q7q&ps*zgF{bda#JI{(h#TL8rNN?Z zI_CpZ_G*vvW)pJYRq3iSVB3jZt#Zu40U~le_fr+NV`l5akYJ#Er9JWgV*$41+J4#R zOHC3y1k2rmWsOsXXQ zz>eC8%FNjtkIaSBWTsp6)n#87^(rxilw<9 z`U|HQFo(1EhXAC2{R9K%w>pK?0`cuZr{BMSi&pco-T&NECTsKr&z56Mig0N($@D5y z&+5_p4&t@gfTC_6#>KD`8J)7zT>xi>RTrZM$m#Q>6jWV(eWN#=9Uca_1TRzn$mGu+ zr~vL9r~cx9Xr2ES@@5S1BZq&;cet%Pip4^wKKVHBI(+QKxE&Mjh^*Q5I9+(2wY%8Y zd2@(sejhBomXT2~>rsD&<=XF#!P@#lfcaL~-}v*}x3_nZY@9s42_28`S4gTpPxtXx zaaoXhZO*dCrGz@xp(f>|*=m+*KK?wY*ZwEzF7CXf&wc$cXMGL(*WJqf!v%Uolv)aO z8;#p=?7UwcIydQ#TRM$fKHa#?Bn7XkLvxK?v88r-Gz`ePo-)02y$12Ub$#)U)DnadNIzJnvVGPfo)7%BhS`ZBO^(0?-@Xnf7s=YFH)~UH`J^$DQSGuIbLG$(TcOGUeWV$8&r`NIwfHgy5G2FIcn>&zJ^y+wRtRo# zjH(qd;=ekU!H{M!9ZXKq*bY3VWsMmTqTch8`S1V=gtK~I^m1Z`X_DKWQNFGeysh4l0jI22YB% zC?%IeL1|4dp*1m&BULsAJYj*K6CNu78FHBgs7=rV{Or>q@z2Lz@sLnw7%}>tmTo0F z?|h(Li(aF>iAZfdIJE8f?Mr6l=ci_Eo>`!d*VK0lbn$9gR!ntCx_MVK&tzGysr^j( zf`uCB|E!Pe_M-G1C|j*W83{mj_9ird&mQniNt&9{Vv-8@{(le-wEq?NmkcFpa;w^G zis;s}ZPZPy8)hN5042Wk)yBOKo<;T;?>Q~Bto8jpW>$Z2FJNtE14nU8+K-mmC&cIG zGlEB_O=ceZa@-Ge_d-{>?N)ur}$M zn905&`}W|w^{0{w)$**#WZug8KD%`>vu?5bdmxMnLq=v|Q>xy%{-8_4?^DWQPUHN=a@Fz$q7 z_Ji(F@c0Aca#Va_Kf!gt?XTt-uK-k+~1UU(emTVg>{;v`E zApPM%Z2FwvzkK3T{kk`PwVPwtmu^XjQh-x*iKnIT;UP+AyVrpx@s!yG3`MWJKeJ$d zwv!t}fzA@}SJfxM6}NxQx(>(mOp{y2z0I<$RDL-!D99Qcv}Y7L!aOWY=l9T<>c|X1 zQHzIPK4sB=`+h33b4nuk{u2`c;8Q{{tw|0NKf^2m8l_&Zn`LBQI2d63CCSb0K6#lC zqYCa3C4tjrA#BUEyPr9>;5G|`6a{K?^diE~Z=jvH@Q~T@JgE&d>at82<^RW|7xpHG zgJ!4c^KmeWDjgjVlTZ^K6EoP7E*JkjkD7}!g>!3%;GJ;An^T%DqeNXxgdOkO z1r2?TpJast6P$rgmBHV?SMC6`-Dg~|ZVdl5btK{Rn|dit@Wqf$ z_6_1CjA`-^n{H9~Ve2a;qcc#{i zxSW9ys}sFLu(_3T4*3B{LJ9XG+HXhbjdzf_Qf2_fH4WKKJLA8t2J(f2 zrKRX*V;Q|b#QNm-x?p-}SD}m~s${Z)X)^^Lib_PSOq_idNpbS3Ds-6`*;z>7P*b~g z)0MdWUb!mksK!eu?!ognk~-9a62mSJJX)JPrqh`$H6rBL@}#_mv9#EZz}z zd48toASJ4QGT|Lu&+eVr(OvgZVEGtfGb`s-W|I;^(E3H-1tG0MmNJ)LWMMVT?FT2w zFz_j$PT2W(dsLKn`L<_Zfmq;03kart@e6%-wZI*_!YyN0x9?H66ZM4L@cWJQBEhx$ zqB8S&c+WT0N4qEYwDqweH#5~Mu}*El?>Z!IN_ zd>QNC+pDktz=s3DVSl(4*?71^6GdY$^9RuCy0s}_$Rp~6qjM3F12+|+KvI?GI&;Qh zX95CNHNPPv!Jj@OviU8Lh=ZY=Z1l*$z(7w=|0*SlJc}e2(dU?YQ5H-n+{>FHlzzL{m1&k7un)o)O0f-aOyw_`UcNWxv1c^plBx5?by!Snw{QUeq`%AzTKkh3q zf#nJpASD2gmJ$uehc<)a+2ahZ%s8QsGD$QyC4 zab}CzJu-vp>`KEKqp5EK)&(#HVip5g%()MXyW$aJ$TefZB%VS^>?GM0pfwe626bnu z1h0}Hq%*f*2jARFDqa*9yuOSZjKY8gEYpA|F(0Tb`GR40IYJe=@q;roYwBUA|SV;{s(B)h+&m>5A z{!+@E;dNZb?7UHKN%Gc;*pD^BJ|FY%^eIcB&M)Vp^CmS-mWB;hTVeSATb=}FqLAN$ zZ$sw-VZTtXkU=MZQj(g~nz|o8<7sVvz&t$)9TGG#&HLgDa90Y)k*ayZMM-%(GEl2#+6IFD6H#NYZT;v}a5fUdy zLv~yN&7+Vk6SuTP@@%(o6;M-Yh^$pWXRm0n{?uU9e@%8-4J1~s9~iv2LmzhSrR9ol zmp$;ba`>x0`hv7bY=%2YvSIwWF){HC$=Q9AEj^OR)&?JN!U`gv!<}f2D*X51lpUqO&2Fd-X+W?}PJgzVwgl_nk}v z%Xg{!C!T!`NC-R+t+P+ohQmhm!1b_UE?zK7-lU*cW*LzBe4u$!{@x; z#mypSk=VW;sl#;OQw@O>B=ys4P8l|xBaxbJiDA%qOkf`gO<(O&_9pyH&uUJkvpI(ATgW=)V}x39lZk07=s??P6siwmZ~;u*}D(B=UL)8^NiAc=of-3+=(v zKR0zomx`uK$|suC2HekLlCq!Aey7THKY+*IJsSs4*Voqmi?P_WFSS<1P{Gph%Lrj?%}l~*G_+(gGN}zSarLQugk|Tc(uig& zcOPX0iDmn%qNs=z&&@8^mo3`8B%T!RA~I6omp4KNIMkp6VG)h`c_Xv6r~vj~^wJY(%kO=5a5%wV!a z76acQFS70Je1NO`%ko>=#rth_MI@DnCuxK}tIpCCh~h08Xo@&9#uDhme6Y%kY{3$En|Sty2$n03BS58*{o3phG=96w1v!xZJ4Xd|Vq~Z1iTr zZ%Ul@wowyG_7wUH{+fKfhBolFROZZBGKFvfk3tU~9j0OAYWd+($BYLHJ_AZp9g@&VIy$}Bu#;6U@()n;o;~~Z{)&z;VQ+gp?lM^S zSjRQ9qrrZEL#7W>hu5Pl=hIr1?-O=o>C7%AZ%;_=6lWg0{c8nz52GW2@2MXd*|Nui zyZBMc#sXF|lAIXdd%HT;<3wHH%!E0~vufjGumEMlnmNBN*;7jco$!^_Uo)Rtub z4F}$6$?2G_+%B#9@D5j*(%S$fu%|%$cEb|M7n1f?*kM`$b!Jg_QLuaCE7Zcbn~V!Q z4tei==qTwRce>>;KP-`k6ND3IATnm=(^O`7e8tyutKjW|$-lM&%&<1y{<(7E9$5p3 zqPv>p5O_%UIm!}U2!utahxiGo=6Dtor3lO6M0SS(c z)PR8<*6-95ZuX$1fr*gw0*J8iJ3eI-Li<`Rrq|Luwe|IX?v(!J1^zRtpRnU5E5$Mj z7q6Zt6N7ZU8nhai<2JmG=RPyEdpj^K05P6`=9%;M^XuM?rF=_!%E3s!J^SwU%AWPS za`uBt#J;5;N!#CmrR)H`wSTY=25KpnqlG}A=yT^?<)tp-7c1+hF(qcp868!;yt%m` zsMtS9B`ZW>wxby`I2(;6Qv%%nGp(AffPCutv|f>0QGs=R5bYB6%e^uDN;$GVw?JXi zSoSQ&7fy9D1GvmHDp&hB*3Z>$yf8il!@rydzxb+T`pvwwkthh?(iSH&^|VDatW9_c zi`z(PfF!oz5`Ou~Nl@hLWQLR5F8JF%Jxh98EE7iUIBni!I{$lWqn|I{+`y_VH_2|1 zKSQ3UF?HQvXh;v9f8ZwUpNC}ut@ie>cO7@xpyG+ORfdM2I=o|_Do-GIB0x%{Az6XF zI%<>i97UZ%kwnd_yOP_>+s&GR-9UjK^YD!l8f|8DEp*mJMcJL3p}2InA0kSUc6sn5 zwgU>gIvFMghD~Y%!esVOd2q*&a z_*s^QDNAxBnG#0*O93+0d!&n->s6b#=OLq~XTr|40R0LsGL|TR9cW2{vX{v!S@?^s z3|r+2pCGp~3t?b?yw?TXxnBEu@Tj}aYD#c$y>ipofYA3>Mhh?@ZoTaO=)EX(HKhi~ z>rI!c6i$~5Y57@j)UtLMi%dfAl20G(uu6fXt$l25Zzx_@f zkfP>DxD%=`5_Xp*Z1)#L<}Jo&-C*5}A-xzW<@v*0d?yq~Pb92}M)G6jmjN8&te^uv znhpPZdb-^xDqQGi{fUVjmFSVvQPSXzpI&5s;N`rB(%|FqwqANvt*(uUJc)UX6xxP1fh4nLm7C~ z1Nc+Z6}R19L(uTjg-3>&FfMP-M=m&DG3ulrh%#5T4cM4GpGD_gC#wtrHo!_$$<6y! z91x#3jz65m7{v}Rp@14?p;}sjn@bdb)uYe?7H1PY2Pn*F?DjZiK$h=t+`Jrp*Q7J za-ucNNw_saBoc3bPY>R|zdG3Vge@;G%Ubuqgzc%GsWBXSSe&Z7(_M|@m;~bca4}d4 z7Y`rj1r4%VvqBn#AANmoPZzKWlv?j$-8ATDAt*wx4I> z@m$=QZl<@|mj`k$rZj!t)+vCSJbXBPQ{F0eUAj7qh1u4Zkt!tVrOoT(H8W!Ff%amz zi0M7y9h&j1_`F)yVBeFA_1i{`qWc>vr3B)eqoa^JZG1w!1Ju>v+saR9S;gxN)F&cI zR#7SD^c^_5EK`|v8uYpE(?u*S7+Rc<%ynr>b6hI0`1 zyx15d{5ge29zgA*XyFu%`m8NWG7XO=m(eQ>qQ~b}3^a$L>o`s=d(Amac1&sQC4%=G zaS=$)#E-8)!&=H}Xw7|aSvuJ2^NLF3)R-zJNkv*%vBvtOA&~ED45+TC>vlGjq9Mf@ zg@?1|AV8>4o40RL%gb@j`_~?xR-JM?$}VN^ZO5Dk zYT`66HUx)`7zAZ&*rjU6e5(C1lsO5N8h_swPu{6X_v>Ok^DqX%(TjHS3*Ij{Rq529 zv~?qazVWIE392|E=_O}GCf!lBA{-I*;4?NeY7U|aK`0{`oZ+Ba*XEiD`85%~earn&&+&B?+D;*Sd zLO5F1@1|&#YO>*h`2TaJIyQ*a&QqoD%{U>apO zJ*p1&_Mk@hE6eUh_G({fZH|OHO?t>kJ?dt4u?bXDlVd&A@DF#W&jNXkGL*BIk%EST zO^CP4k>Yp8z4hZ`TR~dYI+kJ%QX@nc_oONnzNKS-Zp|=R5lGy4bXk~k-$A7%2k*g_GiTY&t zMg-B0$KH;|bj|$5H}yCJ?e=NI?9h=9=%&I=5w!)wG#vM%=H<=&rzPw1zDNJ7HHYiD zmmOnCRpTL`tJC!!qo2gS>LnTzi;Jo6S)kjpS7@xQ6ojEE&AaSypPVfT!kT+MelD#^ zXbG!k!_@9d{tnR_4kx^8&K6UI&D3j@lir)i5r(lm>OH7PoS46*5=maTg=gk>xHP0&-CoQmR zaZngTImjy7GU|uzy#$Y@7AZZw!1N9BY>>G(_`jYY=C%0HrzDC}^LUFaxFJKDgf6+$ z{%1Y(1zHpyQ}2MQ&VKWsAE`cGPABcZ!C~QulVE3P+plN~>6EWx*qg_N@X=r&i>dEF zT)%yG=5v3RVIB#5*i4G*Vt*IlgDG>DtPq!40Is5lhsL31We*NbU{=w`D}(hENfi3Y z9xpaG9)KuAsOoEogCR{#bN3ulDezU2$Ka-Of6(Swul7-aRUS$O7_I&BGnVKI_5eh1` zhVu&f*^`G~&#L%!a;mAtHiL^fg+kg-P*G8T4-W(QxTt~?29Ocbtl;#;)KpbV`SCw%TIU_Uwy6tY5(r{Vyx z_A6UF_=|5!Dx*Hy%*@RBfRcAiU!N2ZU-)}uB;Kz?h1Ww(RJBT{$T#KvQ*RxTD&f2R zoN`7kB)G%TfsFy0{Gh8Q?<4p68Qb{6Pl^q8TXXWT_It&Z1uiRIhiiG@C41g!Yl=oA%*Lg30vb<1Y_+WMHC5xISz*zBJjjThDoZp|-KBpOmch zb3}|{(T~kLFyf=8UEUK;MaLPALAy1#QpZ`CK>5T9W-;+`XF>)MLHh~GMWxLoLG_Ru|8+WrP4 zI}WS8qhDOZ?^@UuFW(eT*qZRIHuh|7Nf^M(e6It%VYtAlA5)kc@fiq=1R`nttE#H* zNcEwzO_ioHvD7r?!mR@7_wf-FLP`A0GzB zAt-h{5DFf)E#e|6vA!MJYVoQ1e)FvKjz;hUQ3`Nf*kU4?*cEF4FIJy;k9=DanM;~* z%eM0_)t>OMz+|PDaYO24{$6oTa?eRFZI4OPA&bcSb(7Ap|_NG>QXREi%cg zj%3(v+^I!jsQEt@fX|>-cl`@nK&)8M&UxS;;H4rAm`8!1UbFNo*u?Zcy`Yp2piTM7 z|0S#Vd(l;D+XWJoS)`oY2-~;)T^{#;{QoC>9=gNtNK7gRB9Pv?dTX+TU)CQ&u!s()|U{n zfq!%fxVn(2zEqQ)U4(XjY=VJ?p6{+FD1ZJ?$@XEOczVKLs0-ma0v* z;+gy7;v?C-5m)0Phn^C-CdsMcTF8Eqc1pO6x!oi8oX9h#Pt`=9^Vp!~Peh4WepVKk z|45JriKN?8EJ%n0sW-n#wcQ(eP}9*FELJb+E{HgLy1}hy^yfCH)jj8F<>m$_F*S%-MBf92WEnXEEU1<>%c`w=Lb3n=t^3H78yoj^t(JH+YE;6OC3QVMm{|Xj`*Rz zmU|-P4P+jX8zo){)~LwEUPuLsvg85!4RqfYte7(~L?Y^bV$v1koQ0DEbi#MC&&MavKY{g3aFr+9 zUail42SNOd($dllEQ-TVqzdl7iy>yzhrB7CLYYRL3j4V^IsOw96WY@8U5^3!%ux`% z*pWI$byn(G#@xOSWJ%qYBb}C20~Ah!Csc3H+1#Cy43%3ocwC+XlAhvN8O%oZxj3h| z?Jnw5&mNw(c6R33% zmqH7Yj|+OTyUTscE&O=zV>Y`(a{F?<5I`Ir zZ*@9lw$b}E^eOlTmtJ?!%*yI+_K7t(f|Z$369|v@8G1YOwLNs8J$!3wDP(nW?8~Yo zaGji&MhOW5w-KT|TXJ;2??c*-hvHU^{g<7P&>iHKt#L0|sqIlgpJz{r-?Fh(YR`%v zTy=6!X_EEN7nskkh#|kstA=%c{C8;*exV{q;>#uz8A1}rx7Z!?}^ozfre|+}< zaD0hii+@{UT@Uybe;Vxf-QB7ec&zvq74tYK*sMumnA-ajFoi*)cz%i=;%o|JGUe${sU zsn<*t@<=UOagf(XC?0>&nA^>N9p}JfCj9FlDA{={RZB1awgM`zJr?T zB3tS}sf_kr03F;NDgOJvuS?neAR>t)Qg(Hn6F=p>PpKx-E#JR;*)% z_;tWO<9TFd5Np&Gt0XwEh`gGx_+}6D^3F4epD$;Zlz3Hy=mhs z9tyRDp1b6dX1d#3_Qk49zmI^JUnMIYuVq*_oGW$QIn8-G)jeo6Nf{w&$@K>|eW7V% zJ~^N1YUqAoF8lJ5>6yCta?Ed({ZQU~)Tl%nCtTux-;#W2Yu36cE;8)1KkO{EC7fOv zVmNq$;&S>1r6uCF^pdn6>jvxmcju1)_UIfn_nwRV;&Y1bp}Sl7(SZT8r+atMOohSE z$>e+w^qW;5;fmJtL4Ofw8Knvi8{+F6DMmcNm5<*>boY8_SkH_LO%fj;@7qEBC(vJV z(zVVOWONU5cl8dpwEGc#;`$%bcwx=1OtIXVOmX27d(qH0#l{2{a#Mp=O`X#Z32e)p z@OjgBN)7cN9D4Rl-P|DxUwqH0%y!%a2n$FXJjK;6VQ!)TSXg?pl^cho9)5oTH-J znSZG>pQ&L~#PBOeHIz?#`siA#eRzG15gj$LXQ5w5$(H|$$6sZEhyNo?0Ls_{^}S9_ zwXprV6Hf5@k07qSt5m?)zT3bt*42mc2^L~4_0bY;C1u-`wbv2;Pzr*4zJ-i|3Cy#b zUe6hcr=4&0e6*?s*zv+pU%3+Oc#YSP5d9EHG9`5IvGC~Vh*ZEe>iXK*Q%wlCt8WWT zJM&a_gJV>i*j~TkZ->4mq1Jbl(_3?4=})xm*OStD`*YY>o|a=0Os-je$-&15*%f65 z$`;_T1#NF{JG!_`E>AKGkM-VOFVzJLx*SCps?oey* zN^_<(#7%pwW~Pm=_$ewZ4E5_M&ill2qr5u|(s&XXrH_T3>*K>d#Fr7VD$S&9uP;Mc zkaAUphh2`BXr*Zte##4RP#z!j8AkPq_$tOS*i0}uRZ%f7dz`eLKkx#f1JIko2?&e6 zm}FqMdWr6Pkt^4}TVVgr+Y4jEY_U6<*&@DKhR#HRlYt7eO+IxiZOQmUF>9BGF)Ml> z%JNCAFpW2ZGV}X%Rntm?HdC%3e2I*|n$5~b9CykA7HhME;bEh0?xe+MO9k%t3V$Ms z_axLWYx*N+%^Elo#@QO$Qi@F7}{nB%IMV0Oq=4G$={m-cE@|4MbE@UP0d5iDB6an6L8L!b+3T)Q9(?XJZn{P{4PGKXw8Stz;|(SQaF{5 z;csK~B)8;=azXp+PNiYP;$2$aK+4%WrAALaK8Dk3$c{7wlKv}?%|jJlU4UC!UJc!i zjuRg9FHe;1ki@Ivc4&4U1fFm7^aBKc83>B1xi7GIR0A{}nwg$GI|8Z~blC zJj(QEB3`v`?2;Er!#AF4ADs->9JkHh<={n&CV5KL`{4N#Yt|7bXo1I#r<(V3)}wdq z`aVvQQiY28Z)1zvEw(aA|6V2 zA=+vUT{G~L@fKJA|oV3o|pNi z*`43e&MX>-TK!}&4HaQJqcGLUEaRK`jz*YBxUjK_V>RsqS$8}Nq8q!SMCS5Ro_$4b#;H!X2IUj*! z1hOPPUUtyypo{a(hI8-_#;Cowd3oxe^bb7)E~rrz6n4w=ncB$UGJzqSvb*R%P_8mis2ZI$B!RPzfe% z52wQx>%Pe~>2W2I-R_FdxXWh6Q&g>22|D?fTN{O>ls6iVH%p4zjZK{UmOSLa4uEgZ z58r+aLu!-9GL-)+vhmbpNF&fM1lChRZts@=82vUx5kFUCy6PohK?n?6%n`cgLW^&~ zCdsXbsk&milcU8I|GM7wtVjR^^M!Gw4dg}g?x~LX;QpZjQ;d*GSbG;Z%c@C`PZ`5 zy^fKp4J<%D_3r&@ii$8HZW5@dZ$A7~Hw=M>bf?a`^Gk^DCvt6024n9qn~KL!4|FvI0&-?$uh-{q;- zb)_r8IOXUBNHeg-KbDR^3s~tj#L#wR6^8Hq175MRRBgnmqE_qi!x#MftTw63N$u*3 z;>Ii1gi~0f3db&6opA=Jd7tD*7&9^+^)A)bX)}qz8@->x~9`7rC)a1k&9_* zw{o(cP=WH%yfX1$G~~of5_#0QBT2mua4i_eeMQ-2Rd>0bS8v<#XQM7KoZB7t_5u2ZR;R?t$qC5& z4$=`NpnbKz9Sr1;4?~nsaCg2Rt2+3;UXtJBoI|j|tz%{fDk&`v*wWMLyE#U7yIEiw zLd@SZU&Qk-YFZyf4=!2NP96~!nEr{xl_#X$*2$JSmGI$c3_JHk^{bK)EQ1`YzKU~6 z5Pd5|U{zZdTi@8^*%i}?d+JBSFD|0opvBL}qN+ay2(;sO)zc1(Ieh%Rrd|nVsbWma zZwu1#F?68=2k?{N%a_&Jo^YRg>Wiy_w6v!Ad7_D|Rcleg(g_&VHf@>n*?Wt3*$*8e zWY<~fc!mT4!9E_*guhy`I;?>+px95#MdpvGqD zsP&}9XVUqZe0uKs)%&TDoQq!_fGSJ*J2eJO0n^h{kbS&WM@cmFm)^4J}BR5mV z*Y2d#M%1{O?wyG<)VPjiFD3y2hu8+Dx(R+r;F`|2fl7X>O&pn|^E6ab*S$^6rt4rAQjMJ;l9PE~ix#MlG~MX_S25t~>5- zLCO)xSXm>gtH1Q<6f3t+btazSEP!4yvq*Rb8CD$(n&dsEF1fOF#!6aM%ZPA@uc$B8 zCXE_INfrF23PuSYplAR6z(8?jB8G-y$N67@2JkA7w@h_~|;ZRUCSuHw-&73+< zRtb1}Tf@Y`{KNUfxRi>lH!1Xkvxy5QzkEh6A-A45*E;0@AEkk(O0CtoSTl%c)fD+; zu&wy*T**{CQJTCE!nohyIeq%di&8J;pF#q>5M+~uo4^Souqcb3vU_7scxGJ!1+}}B zaX6|35`HB=g*I43gl$FP7#CSW{pg&Ec0iSgzjmIA-7+ZYF#;bj z)Wu&Jbj;=?T91$H3rTQUzJk7=b%+3WA_D6?!B9?f-O%tXh)HjSKP381@_L$W{DdQK z+zAOGnx$CsVpShuB2V2xBgH*ufl6WpM_CaD{KRD;)S*y%TzPC7kZdrvUq80MeqD5o zA85b+a7=t`?;t#MuQ)Z?lbH}X4IUE%S~Py@CQ1Cu#<19n9BC)icqfM1eiKI$^`a@R zBQ3;OAdZffiD_@wnsW=%L_<%LUzHtQ?_doelGrXuX`!K^cf?;@?tc&-65h=kt>sn; zKI!@p!GJ_}|CSXOOCF?Wgh~z-fx4!am$gsYA8kHD8Q;qSTj>&P1!-@2=%4uY{-G^Y z|In6iZnPaEth+4g3a}*%-|F}Ixqv@xdUa?f+I<2Bks+3-#+%hEo{amP%J5$9(#dMc zm1)<7O$@%V(NgQF9!se@VDlizDkuAJ>uZ zJwxzfHqMv7@VC|L&Z+&iKwulcC~68I0UG$PTUw_f>8Q^-+Gq07>UAVtF$0TFa9Ubx z1;;%uchiSjy6&e=Pnq7pn2}6yFc5pvxz+H;dD9CbO(!)z)~Bf!Xlo@JRG8OBoNv6N z+-iX{_6u4puS%ic5YyLQ8@W|aY3N5mDQW!<0TV=rC7eK2AbP4S@DJvn1G2pK(0J~n zsM!Ur&6@;$I-FHa7`QU-m)-V=pC0BaH!cr&up*1h`f{O9BA#A%avb-|SZ81m_Dqse zXIM4k{ich)nJ-p(yfBCy) z?0FWef|Dt|?&!t*d(=Vk14y$3RnpjJ+vxIS3*uMe%0dn|ZC;WMM#)FA`lOv94=+Sf zv|rW*jU_ug(8G{b_2rLyBfI;DUgb_iOH8qh6sc5}JOm1&rz~Ewh!C3*ue!%1BJt0W zAZv&^$dy0Gf-Ccr#Qa2QpGFcFs?fy z9kNcI_S+_fOCVgfTgZYwr z5lX_9kMa&_tifSYz4iJMnv9??#omhNNlMbe$%#po952#lhRQ>s=Ut(vM!8xU9^HZY z!7mh1aIOU}#v z)*ViS!ixIKJo2`BOnYM)GAQX5gTJ>fCs@^C*~^ceQpPxhfGBHSCaX20@@fC|&oEnyC5K&W(GLNy z2jr`FR}My_+~~FG>o{~!shS1Xt!evA*E7PW1B|R~y_=2^pQl~45h|RIV?_KOTh6-X z&oD5hVy*Ad7ysselTEfVryq6xwU9E3YN~}I8+l$*mv(pkvD&|UZAwA>05V48lT@E) zy9Hg67n&h>!Z8BCq@Q*fAe}06%_f842>+E!CyH!qsgI8w;h5RG5T*qG&KsFry4E~s z7{0EnkWGu_9OJzgF~4UrKrm`XDMXzB3p}gVf)W!^U3s_OTvuDnysgka*qX1tHRa3H zI0Hj6rInlI6@WPp;F+1|GFm_BU1i!lpy7D+r<1KTALHF$`?+9Httha~9rPKccvg)M zrYYcwI{|kOEOiwfW^|W1v{S`5YX!n%3RU|(^>6+vT|4eQU-2$XV?hzOpM>JFjeb=q zI{qYxHcuXn=U$IZ2=S+q{Hb9R`O6#5D`n)ZAQY+4U}4z36>V00 z?ON?2y&v9jk`V3$zfr#Ak=ZYa`1vd@zYYUOq_1U-o9uW!*zE27Q zFLztKz`KTIsjd)Uy1CMMXO?5cW!``t92{Wr-W4~moWw7lhGRM@(Ut=OpT8~)IZG9% z!YrAyl{(|I59Jq*!v}bgL}|3UaaO;d{^N?BsjeUu;hzC{h)#fb0S+_Vkk|dqO0Die z4qxHaSpT-;#q4b?`_9<{eWeY>IPrfcX0m-Ec9l*t|r|*;_K;8^T?OBWW3&O3GDT!I;9`kvu32o z3n`Iza0#e<-J3ik^B)yYb#!&mJotvC(aQebXFI@OIMignOUD$s?U_)2 zr zB@2#;WqwC+XK-u@q7@55*DB0t_>KZ|su#_;B$fP>3Vwi5^)1>y@)=PO<$~sD8)Ox2 zGJYP4&S+6%g@E`_S>-5vH$8=?EmVkqbng^1PK)MWXF5p%@wf2P5xZ}s<)&Kk-U(C$ z(>;DD`Ae}9CnmmM{Yl!jW8z-1Zg&*%v)p%LcEyD5X9J{SoAL|DcWs0cxq)tjL(iax zZ_SHUZLgmMlj-W1@_Y%UA$O-3uQ{IQUhAE(Vovi)hJRdNg)est^q=&Ry^j7g`|xD1LP zQzw}xAtC)>zikWOW`{lbOpg;Tk$%T=5bjEhL&@0KSUe0=>#+@>%c2HYZIwc+{vGVy ziy3i!rN({dgoohP-mSr)k-hJGfbLP$r6F16PjU3U=;Rww6(~@r*h4c6-$Gr6BpM_f zxf?Iu(u|Gm^sH7?SyXVUm#yznufcZ)gnIDXQ#|tX_NV5uq{=t^@!TMCIc@ymBm=rz zhfqJmKmY5FPCM@4_&c%^jAb-+e+4{Kr@3CSc8tHiKFG)-!(Djwofmaa@Z!L-f~}B=!yEQLoq-J+TRT^!PrXk(e+%8Ns4kj$Aw{Z*vYGvrh2+3fV?81f?NVVZowk(-+~&9w0U$X8F{ zwJ#=caEU4D#jt7JfvHF@Y=%7I*WJo6H9_HQSBgP%!F;8AlqjZo!ecZ*Hz|g!=R!u& z+LrmHWC`bmo51XwWQcp7G3P0wo|IkrJj3YDHRVa z*xwky=F&a71TP1?$x2S0PESA8x=56dPex=}6v9X={_JqY(toay6>x>fue7TMJ2Iq` zx#v#6?6S`f{g9WtdiRA-FMn&sA&2p<=qeUyzrN@^MM^WP{$`3Z08!yI-OR7St;H*} zb7lUA!A78*cHrq3Ojlw_*I_yp{8J@tJfjDTcuN8iHN>kJqC_s(Uv+BS(E!Z;-`SgH z+cvmRUxT#{wzLv25|`XY>Zwr>7&g7}M#8+z;=fz}LB#E7HoMhG^0|qE4g}gZ+H-d* z0O6t)OS3?eHQ7q0Ks0$FdzKwcfViL_od6z|WXSmpqU2}b1%-vG!Ipd%*+mV1`>~B% zT3tOUhy1c8L8+4^0S1aabR8LawnOMr^=idx5*Ny3-4^JwBp_T>RQ;JJDb!q`Cy!#f zCDFclZd7&cbHy^;Y(mrB!Gs8S4|j@(TNqHCmC;w~yXlg5*zos%LtfjA(diiN`0K;< zF88JI`g2y>f!fP41C}q}ug8Xi4fq7*B4elCx-OaS1)&rD%&dtl>mCL7n{uUvBq#@P^Y>>(vhn(`?=jx4GY) z8Mw7D&Ph*BQ3wgLHLdK?(!Zbb{_JklBk2kqbAmelB#3DMuk0-aW2|SK^;-#in=5G+ z1VaH4uD?hc;oWa86(V~}OI8=QwxWeqHG$Bk<#ETj_AS|=aDrMoeG8ga0l><75Vgav z+APD~eTqmv~N*Gt3*p%>*&%4mmtuoSIIZk1!{5>z#mQ)Cu(}8TLwBd{AYnp}@RJ*sv-oA_A{oshglv zblzE#_|tLZu&L{h;#%jH&+}KbYqU;T*pk4C`OL6MkasQS$?w2u>|!-FH3+abbv&HM zDQ5~q|F0_JfvRjbsyowg(|qdZt}}W>zdxY}fl<{2LK)pCk`kGW_4R#S);u^1gm+aa zN$Ez=;xA?0?RRj`njXZR%d`{*&M-$sHaXfS&!i&jeR_B3;E4{tj+34?rYTft_e2a^ z+||5#1{oE}L*{1F>=P4N46uI>|IVjh>2mR*QFER1FZQD7slC)RqCW>^pn~NmbUKFX$Fec{AgirZm6Ff3@i%+=sCyW3qn|h^3x8-KYr(|B&jb=O&AEd*1bND9Fxc+94nu}!(?x4}`RKA;GuI(^7l|c6E?`13 z7gd?rx5YCeXrKY+Ls)-;8K zsKYNX`j)%Y!5h!+J7?V0k5hw$v|Src%DfsdLj|PIcI0hXh(xXn~P#=%Ok2kAr_`Y!rMR$3pK3T*5jS*Ni5i)q^ zJRdO%4d|mkIt%x z;<7DFm&)VBXmBW8pvr7)Vxn6q#I9G9gsuN+^lBy}%7;%m@QJ5kM9@X?;!|@iH zU04|O@jf1J+!J_#zYVqX-GMHlV}8-oN>tGbee4wiuJ`TbrpO_*pDvy>4O5FIY*I}Q zrlkuc16-P15J{2{PCjR=HQ3G@c;v>N^^q6XWs@!Yv@iFu31P^JIu*E$+H_9WGn13O z`^CwS&6mg1y@R5x$QF71d#a~~H3>U6o%tsKBm@*EkUAsGjROE2yfO(0_WA}adO$D# z`HvyP^L^=pMQ`;Wvw3vYQm;vLVn2fU!Y63CbWpW(oi^YGKpKaf63r_Abd96bs7;Ra zze2KQFz4}56v?$cis<(&EZtl**-0Kp`Zj~;ynxO&ubGQm6!EurVQ?t*g=3QvB93du z`Y2Y`fJ0{*DvsOw=#O(N%=}F`>^_!ojFL|+Y!R8}*DR*k70+7p%2;_ivM$RbOMW9p153Uo`B<#~vDN_8T6 z#GN1&Vz}Q;ih0)sGnMYTtfrBmf>B5~;i3 z5NK{ni6Ss<=N#f^Y1{OX*lolK=ER4^%7iY7dz=m(tahV6gmLFp6xGSsx(y0Xi+Ud; ztXt-IJvklOZ41Xa2Mf3=(~sTvs))U@Vo<(LI|4qm;NmsJs%W7J9D}D zntWrwH;KqpJ-ZpH?`@+w9!*}BN}ogwUOlSw`Gfy(ld=t&kqs7Kx-g$;%f`kS{jl^| z>ytSy1n15P|9dvT;RCKqO&mc%-BAnRRTpN~_CA0nRc}2CiHZaE^O##%#j6O87=uuZ zSKcKrJ-6?RW(3M2NY)@$f9tYA41Yt9=O?Hr7$RBJLBsdpxgTlYv^GuwznP^FM{(iL zof#G$wovVbj2^s?!Upw?*8*aewzs$KPVyYi2?wIFA{C+rjLf+G7FLNleq|@b01;S> z_07faOBZA>7p92#y&cuIl=cJwIl&$hQO>jJBc?#yAUL*+ z;@JGN7u9d|HC8JoELcp!f14vGo6a~IKkyvxuLAS@`YKj62g!W1I zF?bf96I_12_<0kw=F$~>q0xnq{`%mub-3zriq_I}@juBG!K}CfKN#Qv_r@E3!_-v( zCuH?aO^m_g>{Rep`&_R7r$kjS?4J`G&d>1zsWt40M^{B_j9WjF(y=P0EGO)t)2c;r zyJ&Tuiq*nM#bYH>P(G2bMN}!$3Z_!&Etg#+bRRL$jv#rdm?97G^@=DR#Bc}X_!hq- z2!8G&MeLHzXqiM#s#Pvc7YF~+>)tD`54Ry@pax+kwx}M9qNOQ(^+_$qQEkh_`<(V^ zx$EP`BG`-(l0!vNeoIaan*8Ncb$f>9<0k9{qg}FZ)c8xmITz0{u?hrs1f^l8XJRL6 zR}`96#cC_OX(Hz=8O^S--%5~+M_IL;1 z^}8LJ-tpVI2E9GIsg|D+QD4~E1I-;JBLgrYCX*>RWln<6yv4_%J;$av?9+2HY+6)lb{c7M$(g?fWrKn2*bguF=Dvs z=|F>kuFsvFtP#)XlsfvWRMwvYn_dbbUpiVgRtd;{?O$4u5~rayR99D5_WBSb5Azc- zQM_yC-WTEy3yqf-bVFV|%F2C&a9ZQ@=k5(JygUKR=EvnjmqEJ@6 zdzOO1<)r2~LnCx(W*huY*~=&fA0l5A7uId{O9u4aHMQZ~83rzHOZGn<80S9?CT-u`ZwgGDTw`hz6KNI(y_$;3url&gQxW?}LO%Oywr1uwg6)5!A zqXHoGZDJXB*6qc^aR(0kvvKcnip86Fr3x`;M11ZrqkQ&SF0L>#UrbZwN#Mx@^p|SEI-Jq2H!<#^jgC5k$rfyj9!#-u=573&9}_1N#E#!L_ES3IY@d_Fc%YnT`vA})Tr4|` zk2;5tev|5p`s|8M3aF+gKJUmM@9&dwa;gWF02@Do;8e?N-#Wln;geg$Vk5w|P=@rq z!$W1g??FI8AjdSB(Exvn)i#!ne)!#q38US z0m*~R5ZF@)68jg$KX@!#H=GY^SC>2Hi9xEVx1<3}7w=go{DlV~UFUs$x>QoIGYY(r zfl)07;zq>Y_h`|$g3NVBx$o~H5?a8OpYbhJRe3ya+ds@WJ=8>BUbzF|WRyDss(8`R z4}>|3IQRFm1w|@<@OgH`={ccz;pz2(%f)ZiEjrP=*v^*N)k2UJ>k>;<;6A^VMT6v~ zqPQeTGbL$I@4xvD7=BC$pw8MXn5svbO}x5|9<}@l$|BQd%*Xv7niThBss*!1qYYX4 zjqx(Nutg&pDr&V?#D{`Ge$zj@94C>dK9(}Kug80Ywxi^I7rr({mBcw#Km(_t8&}s2 z5#kgc+86O(1!J6_!DGPMy-Xr{KZWBr{_!R+lT%Z}V}@52A}owdwq}u4-IHOcL$Xgs ze1O9fr%(Br%qofhlux%ErB0>n z2yJamPit5iIXs+krEX3C>LiTMJ428)eQISaioPt-abBDrw*Dtagcq3V9SEGC?I-s4 z_gkAODiPgyGnrig(FKJ*y{uEqii57B1y(K@55~aQtOFoS1B<$t0Mt z++^VkrDrTO<+Ye+2+WWe_JI}v&Y;va0ZDoDg2I~jWDImkW!)@Io$o{JW>gz=oQo~I z)IJsre>C@o|K{BOS305)D8;mzfZZ(#Wze;VSkuipKAm2|)i>s}GjBi;2q67*3Z(p= zU*VjC8uYN(6}C?vxXB`g)yP^-$ZIfm#h5jQd|%eHwD{2J!J3a6)4#pc|o5z;#I(4dPOUH)ke( zdxQ@rW1-zME8D4_4WBdh$HWbxd5`!>w2h!PP-wd985c1(Rd(a~Gn~@QasFIQV6hgI zW9G|eZ4*0mo#Gl;KUfpxdEqC;7g09w!hl4?=(bB@^ub{FZe6U4+Ecq6-os|3T#3eU z?M03*hP&SH_2+_vr_&#SD7bJY4Jj9`XMmDZu-PoII36*7Os}~UXZ(q^hXSfyMh>i! z1u`Yy?*Aw`#C)Z%!Nl7XFt4Am#C`qp^X;j^4zndiMmT@gxZPaQ#N(LcA41zTF*@!U zfj{GoF1x$3_puP0C40gJ%>_@pO?bw7z221_`49ijtw`Mxy*6k`#fFdPt&gGxnfL2I zUl(9T*{j9;GoW<26WKb;M#_x)X$sW1a-!+8hleUNDy%lecEAA}C-rU*9}s)@zDM}d zK0i31I^?ec^9lxcVuYCz@dHJqCExJd=chQHl=2#Tv*oQ$>r-hG&LK4}ah79&&Y=XH6-wyw5xFVQz-`!b+38#{B;@HEI>w zQu1RXrpDt8CKjTeOvf$5=V^*dgwiELik?>`r z?w_8z?!zx?Gjml2KYK^1frpOyqn5mcj3QDziwAhS%TUErTE+}LUd8UaSvC9UU<%YH z-iql|isj}W00{ZckhIn?og+M0YJo&y1WtvD?3sLGga2OCTf2B`7zH;uHcWPhmSln1 znA)AZ$ZY&POvW5HWKv6gg=+CaV@U;27E8g>iBvY^>*aWG!@DqxXczkCDwCJ!18En_ zgKRbJU^9@~In6c8^@Z8-%7tTmft=?oRYe8j9c1{oga zWwSKmZMU90f$#iA5YujjeF^NJUZzD@0F#~ zHK}Ly^#HK+FFVB6LrD>QVAbFpOK0#ldmg|kL&m3AonkU zQ-H?Z@z#-$pQCL{M%2AuT}7uSZ_O2)sS8A53#5*3O3bBxjt1f8vJ9aeLz?F6ti`k_Kk!>fi?2jjxi@BV@|(40lky3Sb_5B zxjC4Bc55JNT!w#k0S>%Vony*-2 zf^x11s-@y~6h-}<+?j{GJx9CNA7|)~J&9f^@>UQ4UQKlkSjj{fsK-f>TeX#4VEwgyPgrrK#DevDE3Tt2ZQc0Z_n`3I zudD-u&Cq$oQNaKb2s${BP&Jj)ngULAY>@#Z1rH(E<&F5Yq9H$B>Ms{m8rw}&TzKYI z{1Ar!tJ1V-LLNLA$c^8m7|~juGZOsPP8w^day2va*rM*^B+9AYgAk<%uGhPGz|y}P zR5u5&2pg2)g(X&S1vl-=@-Q2N_521G?vzPxZ?7o*X?Q(>qRP0eKLS}P{{S+OjY%0V4h3;oOA5vaSNc%+IV4F zWl~^*o4$`zbL~(%`E9AAyb6{Q$IS^#RD+Fmfdi0f&EMZy$o0_!Pt#CP@ZkK#U=dZSdv;*`VPEdv~R_Ekz zW-AIv^S&P>6lQue*ATFxM!*!;d{p>!uRwWVzt<0!f-P8Xk86~9^;>ZI^uY8`+Qn>9 z)O`>_^~e}h&a>bW15^;iwd3OJO@tepL-}FM1SecPBLw(FZFg>=0JCjvTwujRy?SQ$ z4$1r-!aiF{cbGgSsvN;I~ zD1yRFll;Cw#_`KUa!(V zMRM@$lT=qbrKmfs?d)(k?3JIwRzWoJ@fURn%0C%5EK!8XOR+?t5578S^%YIuE5mZB zgFfT0W@RQa!mA|{YM6u1NihdZ!I|LXG~Nn3Yk%Fe(f%=o5uU2TZN)xO7R~t`vV$@j(+QSF0YWZY{M_Si~6>g zXYHH}8WX9I=Ubp;w-Dm5=Du+eyGeOr$>0kTYTC@*WOn&QP(jU|`TZh`0I@}Vt(8zc zlCtbWyq3_~R}>-H+1d~e;LN#M#^&`#wWeOh_(mo4-(APyoqF~6?gUI>gV6a-TWt@T zkd$TfX4tf>z-+BF=daa`HD1q-CF89!EsB8{!1n{vh;JbCqutV0G!1xNXngXBTIO7~`r6K6w_qYZskrKVSorJJ~F2 zayzxb4kME*rT<=y(vS&1@^LpQA@ z2_r5=qYmjQaC_l5_8D^M`?r;KR*X8u)$ueR;=`o9x*&lnT|b%eEq+x@xyEXjU`sOm zr8nT65*bK!_{qb2XC3z)nqH!{goAmR^I+W%12tPpkxOezR3yh-#?1HV?Z+bjQOVHZ zFLk~X-Tg@EayVqHQ&xN!>j%|MhN?PB$F6#oSg0R{aQ&DRw1U}5yBOTF1B5;r3~EhP zJK9ak5i~D(U>~0j15DhX<6EhRh%M7@+6C!F0CD!$!<@9k^&DSTUY-g-G;oRA4v8B7 z*|F+Zz8Za+Z*tf(ge^gw^n5SSJj;&HJX8GKj-s?DNF@W~p%#5*Ri)7vyZ~&~6YMOi zwWSArOz=5zNT06>25JQ&A^w~A<3?oWu&;)hNxSfFX=&-f;YnBu*t25l&1a}3JE^6; z(3jchVHv<0*Vq2p7euS3^r@~kJAK&6kyud8)dE{l!Z6cggZkGu`6!A;Co~0%){-N+!>bhA@auVzz#g}{EA0i1}@6>K$ z4hBC>#fcf1zA0TR>9f6@sNN-d&H)@=%szF^iRq|3B+4@}+;dBqAcPmAK9F`{fgrNJ zFkM0&V1CU8b%A|1V&-2FCc@+gy{J)CsO2~!llxvj=Y`1&Sp*ilI6D*I-IPz)k>qBE z0J#gajEsfQP~fp8ZUzW2s4VkTe5C!?Qs)s3lv=7?j!JY$$<-x0+yN6B zux-Ys+86xxx1`JVHMGgU1hah&AYad1d7HgYfm{Tl--%egnQ!n~P!vWJqgg+rbeUuN zhuchlFOcnjf&kKM1_pzTXZa$Pv54fdctyrU4(sJ!z&@DEn04d`7dj+~pM!O;(O|WZ zi^@oc3Tp;CotTK{$xnJTS78b`uu|-)P!wkr8B?B$#Jf+}BU*aGMEc-Lfb%DENb6&x z@V0$fK>q*Cem{TrKu2akJ4P9$(->L?PwxCRqV4Jczdzs56q%Yi{5~?G+TJz!8#)QX zdi9*1!dz^6z!s$G*iL5XXG2bx%KzGPesJ+>{7VVVbqV?pBL<3TpU;U>(&0B7NA8bFdAou ziod)2!FpYNPfTvsi@@5g_4;Q(F=o4*ejR!Uw5g)slVP7L2~%|_6slV<9gOP4e`ziL zt#IT3iaS&}qwuK&u6CC;t8L7Fxz5m?uz(I%ze;(;X;FPT6z02_U&$mylVBA&50XEY zgiXApcB$5G`b@eEL_s2CZuIIiR61uCR4`v~9@aJ3eiuvlfL0h#f|m7lbjaA|!a-uI zS;eSH+3)PNkL`=2Pp_3x?{}l_@901*7)7FN{|_pif1py_D^9`L%Ry(YXFI1l`JwV! z=X$}(0R2G)QPT^u!`pCWH7 zJ}vxh33@=v^m@<)%%W+SQ35YtG5GJY$+y16e}S~D{7Td0cE-~wiD4lqgbG1_bvm{F z@3b=UfbpP89@z{<2+3g*#)RX?5!MGl0T3{2!(;H0y;cE-KqjC@4LN&>P!C#gctT*_ z5|t5^roop1Q2xfm=3Kl4X+WTuy>|Xl#(e|AO9|HnY{;Oj0tW|D!ztGF$H~5^zr4+zd z_e@ke0j%dAC6zcZEbX9i>h7h|pSgk9G5NO-4>bEnd56nfiH^?wh{f4!5f<2FW*M}f zalv?rZ<~|?HOQa$wG-b3MAY*W3_HrEDFz(Tky)e(@EG!cMpkn0W1HzAaW5`9_PrWT zqZggwmTB$!#bX|lN^>>^&z@&afR$?zctQOOt<5^uT#=%+qqrvEeA&J~V~a&L(9ZNw zqcC`5`9p^>fuL$sMS`L?FkVMl3mBFhn6Kkya?zb}lJbrZc4;#U=_uXV&ICH+EATpC z!0a(KX(-K5>b-97Mf?JKsjNjTfoTD?30jmUW#ryV`oRQla~6s1e^`{J0S2?-*q(mG;v-e4uAFScusmSPskTAG?t0A8WY%)W5utzu$g z0(~;oK=x1NYqyuuLwG0e?(Jo=4-(AZC2i_G@##EK zGL37f1&myX#b?0`hAQx?QcA>bjc zbO+M39Y-m#6U0mnRR$b4-U@oXydE}R;NgFy&%aKEI00&H;C75r(ULkU73fuMARY%+ zbyj&5eCGT+E$P!+j~z36D@})gcq!n;Zz3?bjPiSMDCRO9w(Ne;2Qq%ZdT~Wwlae8& zON5YU+wJq{g^(2d+V44W$*Ag$6=bv8ngWcXQ)!vw|IwKv+bZlX8GSb|;gIA_PXPDS znw=@jgF-WomoSu7XGkV?fn14I&tK6p1D{}<|1Wci@>DtQt(*CJo%LkK7~HmgSW>+H z7fQfCl4XC%Q$)33SD+A-wLoDgR8HS#>qo+zTKvy5$jdeYxSo5h_J(09aj4RnGm%$$ zlYgVNId_7&H`f1@xh5O7MmCJVVfnJHKDaMIi$vb0W(#5(Kw&&NZsA~7{N6Pe_MJ9z zM*F#N|9~-U;U)W^r5>Fr_lX%T*`yF&nWvAUbHsC}CVHC%CAh5X&!y}Bf-$t`tv06o zx-rt2Jk9Jm_M45`PHX0V7NK*+hKa4Jgw;F*AjlhSn{c+_p{>uN$g1N(kpZ}%6N(hp zmPb+)zko1rr3M8NDSsqf;W`I!DdoYLseJ#@c+{)(3IDsp%*|0^<%QGHobQk1`7ZUf zdk{6Uf_QC)$QwO!rxjXe`;Yl-|LHe3IH zUbB{kn*;R9IS~8MDs)8jXX||3xy96Lm0Iew>A|y6vF`h{Q^aSAu@uUZ@r=BZ>}=oE zLxQ)cZ@HdU4T6GdqXJzYHIAOxXPts49vNed*8e{}D=?;7N?6>$hT?WTH|EWm&8sj>+33Hm|P*m%v zW)iO8Jn=U2q48|v3)idpyarQvr(<9JiuTthI+&)6$KI~0`k`?U-be&>InIu#f?(GN z-@_AUhr6~O7~W-p-xTny)PJIutaxpvZkLGAGHq__?ZaJmZ?l0d0puO=rj_$5aGBDJ zUPV8GU+e(4A_wvKb|7{52kW71MI$~+UPM-uO?X2zswwuFBgZJ|F%u{DgIFgEuW!5$mtKD>x-rUVLGg>}Igb&Pe(4 z-(hi5a@I6EMG4lW#J*RQ5MfeA#Z((cg$l`k(T8H??oMtza=}2lcm>eNs1X!sFW|Gu z_F}h>Sg<7hc#_`{2xi%mw4nQyeE-&k3FNDe9|RR|n`RuO?dQwq{;R}8XeD99dwxvDR$W}>3&%S}*mb49mTu^eTGHR=3=BYfp#jnHs9^KZFNVtS z%CJt%*A|^2-^W8X#q0&25uMs{3YU4^3SmGKvfYC0u}d9JmUGo(Q>gd}d%6~5JsdN( zn_|j?s+}c>Z59Q)bvSU-qVOdPwB7=fkuCs&=k-*$=mhWS~eLX?bPOD(DvS=X{&L&W&c_|IhYTekdb zKkFkoCXPiPT__QXspT^|OIQu4+Gnz`fdFhqIu5YW&l53qlf^CEV^g@8+-|s3J>u<9~j}vz7H(s$G8`U6Jth zE5bnI3mCW{?eU_$-B2*{=@y*q{OO$u)5%27WY}_W@~2H=g6)p5~oziZ36ywVm_AN~hZsoN4o z3yAh$tE!w^$K26O+d=$_P4L_iE+6v3>tFBXx?@HdS@@Du@7tC)zT7gDGmg;GQkjO1 zjHY~mv?Aj7`~Yk&P}(biMyN^(PM2Vh0@Xqlk?l$K$FGWk!tBi_GgN=+T)`hma#PHc zq3PfX0Bq6CWIa{yc)$h7k|K*0`8+J9l#g~o>nI0*7!XP(Od=s^(xWB=d*IWk*@_co z7*&*=Ms?pef~qY7!Zi5SFD1HF-wrjDbU9;LE@3{1vMLUK2 z_Yd$iSC7?xxo1({0_cPh`)ol1Q3dPfB|az^k*)Z!3Q^mP0|##5Rb))a)<$u5$TZ1O zZISsdnHh;AQ@<`C^OKXzEF-PKmTE2MMFRD?Rh{D|({&9=o1w2QC{cV?5(9+Ja%b=L zwTsn>-i0I|X%>^(6Ez4vHQK;O&{pyBlP&2F!T7QS)+jD$gX!YJ31s1DO0>Zz)?9)h z$8>2c>bU{INkR#~SgGB;e*0{QKWq`DE>h|)l87Mgh^%{54JeY?|q}^ z!H4saAJoxsq{czi$^Ky0j%j?DNx3*lPPtD5yph7gC@ivZk{oviobG~zFc{Vo#JT=8 z?^te=QcpTRtb|)Cal|Y8J5U8U=%|PY9O!3*zR=-xf}AvCp0Xq}E$d=n!P1!Q1il0j zzd4YkD>k2yc2AQ!!4mtN`1yxaV{2U$)=}#p_gEX5cKH7fRbLquW!QB+L#LEMBQ4!9LxU(KB_V?}NOzaCG`zRZ^S$d`-;Y^~AH-T**LBW5 zd+&1&?|=#V!Am>SkBeGuV_#m%nwA!ZjMHI`rMWMLZ^E-qNEiNi9D|UVFNj~S?7zVA z(%+00X7Q94C)5$Br?x)fd+s}p#6qPAY$?CAr+#1c%3>}3!bA(X{g4!`My%&Un&vlP zCtnruS;y@}TTPp`9b#bL6=;HA_(Exwy?K6cP!HI{vWkTP zo`k(Nv)YVw$Y3_D8hs?7SHAsF=p0a0IMA0R#`2D&x^&%BSn8k~kPO1Nljxb?vNokr zX>(JP%i7ORrE=_H>$jl9I~axVCnKZFwJzfVxv;y+%Jg8?$hn&%}BsO32D_=eqm z!kbS<=;5E#Imd#cOM@@Og$=!9yC*BF#c_I!Jjmm_zJnX~{5Ngma$#2ygg`4xny=rY z6|Py(-DWj|zv))l212bMz2T@AC%ToNF3v zV4bIY~%72gF&qMxsmWfJz~z0JhL&jyh-m9y zSJ>t7q^_RWC_;Tc2k+Esm*gule$<5Lr-p>7R89V=6cBvy>u;=A;f&SWXjq|#Sz&!g zMfMRMVB7oYyI+7*`Ics@YMO|~ zk3M?)yee9=A`z1tfzHzH#(;Ozsn`$5QU1~79VKYSpIa04g$;Cm^1g46B?~REf>c&+ zzRHbcwPgO%fJU+XVR=1iaMuXm^^(-^=|z6+cxGn z=f<4x0yvuBC0%{V7q&;MG%57GmQVjmJ-w`x`tXHm1!Fw;?piL-O_j!r`Cfuj^Ol!7 zmI2pmUJh}4?cIJoQ#IW?(WztMY@0{!Gn19^VbN`cAQGptr)X7QBa|CWgHfDkqU%ky z#M)RY)}HlVMI4!4u7epb-8VP%*#==>NgY4~!O8(57rM8L#bJ!rT4MEK`}Wwc;CL~R zt>%Yi4&tR7ZZ;;7rb#svR7iL(f1aWu++;e#t&TBg!62W51b%m$mZ zz`2*jPb8?_%Fs?~4xb;kS9ln##Q47bSWYanc#OehdRV$;ZKdtM!YLj*AV*B>w=;d; zS@c)!;)o{j1FhtE~RP9V9%t3grS&1e)_jpNm=BG3w>rVlw62ia(WZAOEb z)MISp=?+FIaC+Px0OM*uHh-NDUybQ?Z8?%aY!9(fzSAu-Bli35WqdFfS>f&c>VONh zGdJ>sM6TSy1fv+jqn`%BLgIgviXljUm%E{*KJak4NF?nLiQg{lb}N>Z9=l}2sv_&G z)*Y*T#K0-ZXZD4Ll9G~!>65^v{-~}PN2FKq=}(h%0tD)epla?8kE;XNy?FZ!{prCG zs5ye(!@`0zbx=$hs>7->;F8jwZH}gYGBf+YKZv_2oz5mMxx$8aPfgtFbWVP{N1Mu@ zsi{fIb$hQjKaLO#VVPHfRpDq4Y!*oC1H&_q+S^J0GWf^6v;Z;9A3J@iD$~Z+R2?gQ zwXObjr%`iUI?8I|IGLyBme3aX+*qH@sSzITwQ&}_E7oe*@1Lr%mYaF{qPsaxCw+-~ z#Kb+%ZCB1t5OhWfhJL@cxOv@*axL&1rHw~pjH|cHlBB8N&WRz|T=y}abDsxQ(j)L2 zEJ@2}lW@fc;=SW{Kw243g=*t+WNe{hl8^jB8H4H??Ail6g6evh`t+KGD;K?Df4z7u zJlBttLAm(34qC7AzRr|AYcm+Bb_em~GUkth{)iD!gy{$riMepkbWV$(3#|PCG08TU?`(HWyZ7E z*f?dAu(oWA9d)rR2=8N?{>eCTrrzXi9-XFO*tIh3szjBHUVae%>Fxl57%!gjNfxP#?RC@EN_ushk|+pOy*qqmih z4~_Ne$maR4ht)}=>gzQyur7afGe)%*bsM-Pv|_XHxe;MWQz~n3O1;ttEa4R zCWjzVeTQ=%``6C)k?8LFw7s{f;ibUR)Sre8nV(!zc!UFX21V4jm&TqH%JFsQTr@Z#aLO1@1mFsct&oHklZrh#}O| zd{v$w4@?N4&7%o9)l0Ivyq_<&90s*^fsvrYT_}|q!Ju{jeIqeswG~rxV4s2b=mT5ZF+KzcF^C6|Ct-9r-Xpa=)HTs++ z;P3%Luxl-44zngyS9_+dswm90$ju8zV&Je_q~D;J&K;?$5z+SWAthuP7L#Q5$qLal zSqVU^dZI8=d1C90MP zd$DGC5wNf24OZ#)wVy?$Wv$F}6q>zP>velFEk?;p1lJlY>!;0VIMsuYkRtxWj^eat zU9>EOL2a%g&s8Iff!e+qtdSbS+u-C7BcMVTa~caybD(x8hnXv}R!A|>FWu<%;hQCT&pw*d-dQsCp4up%>*c$Ti?Ytgs9FTmwpe173J#am5XC4Zn zixAD4(K3o&Ws_f9i{g7Eiy0mseo?rdSeot5k1=j%5@v#cp|``^g{csbH!PxwmnD9T zJoT;^Sg+Bd@fa5tNpm-MGm(SM$Rv^lcGVe!P#T1?&Md)xH+V7U2CaFV9 zK-F_Qy};4Goz}oz{82&7EbL8$b7M0rev~7=@DQvSk+J~Cx7bm;^bWRV>BK(YaMY8y zFrp;Mp0TGpv8T(r+r`AgjaTldGErkJbAX(xq3y1D8Fl2ELSafrgg`Low zAO>Peh)Bn8$N7aBKZu=Uby=j42ARZ)Oqx3vayk86z=;#@H5de?i?NCUA70bq!det` zxu^BDs>7EY_;Je@9%1MN)Bdyy!riP{eH@nOK&pPA$LFla9gpPT1X-9MtylnaF9B%w z`rB&DzRciH2+P^<*Jo3c+MSNN_3o;;iJ#L4 za>iUNk@^M>)>%< zPK{#YIevG+_(m>-8>~HHe|K{^Rc(nWB`qx)^6z+Af#V%krsJH!de)xX(#ZfIgZM~g zSf%ctR3}SIR?rffa^HOJ$@)c7f*Ea&k!jc*&$%`zS$Y{x@Ol$gbu;E8rEg?H)ID+5 zQtQV*^-x;>Yr?C9qARPoX;_LSfFU#v?!>HQeEGQ%kW3L^kJR_40_j8IWjLiqOZJEd zoofDMxuH`5TaU$<337hPuK(&@2so;pi=h{@vZxM5v!K$hPfemQiL3PT+PcYvf@qaG z9xVL`7YE^C!rfzVfmdt87;WOp>LAk$pLnN(=##s(+jgVM?Qz89W)t$Evs2f?1~?rA zD`ho1rV6G7;jP%Sr)|0`USptD-oRpREH(d1O+%EDkrjvt2QxM}lU$Do+QPk=@yph6 z2SR6FMcb8%EqF%Fns!BwTkO_R$mJmo!YPNX*Sn?4chg7$$Jvt6LGtb=`nrvst3d zDs^Qy=Id1bI??b@navVNARG|!tP$JY?&EqEfWLBg!q7Ll>pMMNXHn1X1ZV^*3RLho@+_D6tSe=;csK){{ zd>9IR5Af!h0CWg2oK-u4yx5v2zaHuv-u`^@A@Qi;Q1`K@peO1W-o3=Ffr)Egrto8T zlv;RCgxLSJm{eWbt;x*v`=NlYweP6DF|3uva1?eX`9Lv_c1^pEzOvv_QoGKvH2%t; z(fI2x_F=9m*$l$D_JT=TGdZAQKh*;o#B^?w-b1h7M3(2bm^X4}AOY;!Lo2*u3?f0w zcY5!>ejL(N<8DhiQq;pR*m?Qs_|6W5nh_$iCho)@wSI{p$hBX3Xyzk)jFDzUje zN2DFp=;|iu`L#hi8MnY`3Y!3Iap{}S!ZUpM^hesdbSGmqG zXeu$(6DstEIEt@zRx%+O&aqJ30{auNk#tttv}C@)7LjcM|{>f{=Z&;j*t7A?(R=o%sj6J zijbhvSQFeGhaD@N(%Q5{&+TWa%%`SyVd7EZmh#;7`tCx;j*vc zvZFD?jn%IH&u$RPqj0m`0)0E0i zJ>>e{+rsTYX4;yHl}5zL{LkxoLlMN9{lkIi4dn8 zW@1xa*8-NsA&jZ)g~HDlZ|$^mClSEU_L!<+)-(9D%t=I4z0X9I*dGyMaSFbjWfy#F zNk?=Z*Rn^M!m#%+mJHPv);PSiWz+iYSn4Ex^7DtU>#484ifJTVy5EGaE4>k)Ck9PT z9y8n(ZQp}Hqo&&}uFY*n!hioXKR?k)+>b7aQ;YRI28vYCvdSu!%V}O>&A5&8_<@#kC zzOKM~0S4E^RsP}6lnNwZiu)kS6&<6rIA_oh4Dvu8*3ItkHGZ7B`-GMP?KM6)&wT!7 zv8k!)u)ZpQ`1}pYeT?&TEw;e-Q|({Z8StA@K$`is804=$E8!EittKQS6yi>u=Fb28 z!O0!lL4VjIJu_fnxP<>-UKk8^I8h}W$O0rVrC!d=Qhbf5Fpy|=#kzvQ4t-hKQO}M8 zXn0)LK}?Jz9d%0v$TK|0o#ZZj8t)onhwM%?OZu}i(i0-_<7#Ig&scN3`O_8X)RkG} z_Chx*OVaW*&MetO(?;TRBGcwOdbt0|YpkjubdzSp&zkauW9mcitPvS4N-624N{{8A zBab3?fwzIBr$1oKxdFgWRWzYMML@oi?(dQCd4aI`;uw3R4@8cdLR1uV+W)NL<$jO5 zrgwY0q*k%L#o|3m$@W~qlfLg4j_~Ki&dW!bX>!7Npq~fZ^73y5eu**_?$LenDCo%` z6s4QwHmJ)Bc__(QX#bEj{6tqhPy3EDicAG3n^sgvBd3RaaMb6M;WKVNArMH;-1BP} zXs=05c$F4phWC1TGe%OyNGU=}HC%2@Co`jnPJ_EGsqXZ5sBeT&**YqEXuE+$U~6gSxUN$PtmYOvh2TyJ}}X_q>zNZbBM5I&#K>k~)Jb z2f2JZ^mTWB|6jT0oPiJxk@)mxae4Q|X6#y%xJ}nA}u)8!YfnUNRVc+UQ=P zs?YwCCN+NUDbQcNF7+(j72|FsXbP5cH23e8pm*O3aNq*V*tXi(f+z=f>B42ZBV!it zp4zqH>S_|+-;+zFcr`PId-05(z@WbKsCzjpE>>V!^v}v?wvuG!GoU65vSxL(uav6q zatdEJ&y>sR=UIK zDt42~48#`RYr=leR(mrCP$x6GE^!h@NNDPK#k3p#l*q~V%61#K5m|AqqkT)K?lWiG zk-UzRVj#U18EnKEb2EvhTgP!FRB&VZ~VHpELNX!)_Hnqtv(_kW-}e z8;Q48rYq1=I$$%KK7v(tFDhc|o8Vt|GM@+zR39bis|FLej6DWzJP9R_{sdXye9B)R z-Jp3QQK?I3`3*HEX7y=ie_tq=L{9i#FyQ`2ThDCP+vm~rn#{d_(_b3~{!RmyiLMc? zSI=_HG!|mM^_HSEEmg>(Blj*Mx0~^C;HJ~$dzG5F_^Qa1rZt6i2Zjwwoh{nABsL=< zvL^ZxHIa;F-2Ca;{%IF#{5%77n--x=`)YH+ zuTgv7t9dP;6_^PQ3?!uJdtLRr!nmZd4bNcH zXfW@4*|XD{kUa1y32}RhLnvQzV?(-25o&B}>oE|uI82fFTn-94#Q^tPZr+yty;gXP zxG;ajI5Ck|9UBq)ZG5#`={Ux4rJ}5v>6&7n(ntt__S__>>T&>=&Ix3G9+cB_FBcva zo|-kwG4NPn+|@e+*cUf7svq_#?|r&HIsB6x5@L$*OsJZS8_~wqSl<>}8Nc|`;LOT^ zK%U8sKF}4Ku&S8M>*Vv`k*$@FU+s0jID8)`NbL`2q@Drxb8Z#>1j!_G&3nLNvi+MF zMa#GVilsjWc`-KRHvHEWqW*&G*Nc)`($Pg>-j^~aZ6}eAAgxC<0~nn zDjpF{K`~i@;xtd*)sN{7&`Y4?@hEwE_5_D*wv`q{%yMQdFD6b&>Ro~n{VW+r=eUZ4 zMDKz|Yhrobu%}NI8zC3CZfcShtmj4#n~nC|V{*RRUgG}eFDQBcLbWo$ZkBcKsFU&W z79aG=r|E(vW@V$0X~qi=a34H8fv@Wh>mah!q-;*$^&;7$z~x~((XMaOj$@D{KQj@} z)-^GaS$|mwVDb2@EWX{Rl!<2UzKwxVN=By0N_^^|(;v(==M#G2345&lZ=j|(uG`|f1>eR_!T8#nI&~fE%9PdCOG3HZ;5lQE}MV`is9YAmxD7vwN zF>Cf_D{ykE5EMJu%M{_N{41>hIJ0%}_yEHf5ZwW#M$q)BXqi0ZbH4^OnLO|PQF!}_1UW7e)6r=Goy2I^#e1O&IQ@GsZQjgE!ie4`a8`RBf?wU3YC z`JRbnDzZDfw@ItpqBwo>J$;9TN(S|((3yR;ce77I#v@c%Sa7w%;d8quo%dXFuo<<( zq*d+(Lv9~PiocYV;3IB{VuI}SXeuo3wk>P^RKkkXE^8^ORKA|9-lxBbv;l3q=2{w4 z=A4^*K3HJwDMxLk39{i{F{vNhdcxsv>v_HT>H-vWV!vG#P)YCEWLoM6u$z&SH#*!t zJJT8Aecw-XoeKKwx>Q5K8p^Ag?~AcaE+zv)>fT&gl#V^&z4losY%qp30FQUg^?N!) zY!340P)7p?B^T~^GJ_+T>vy_l0Sj+D*l$TqD~$!!ZqJ-6+K2~fFTbJa(qJ~XuR+Cn zBGF}O2O#8)E&_~#(v+eG#MYbiQyI--*XiU@*`#OGDb!lF<(Mql!Dt=4V&Hu~#&Fre z*ABi3WiGHEj>U~D2Z3xf2Y@_DKS6+9Pd)GT^9#6lS_XE1kb(0tTh zi92d zS1_x7ez5QsLns`Y2<-33oN zSJ3tBdBJbfsGNIX*BpV7r=Gr zvP5!o93-ew(%M=oV%0(;gUvQyqUc~+n$#a|Va`mX#33kPxlj<2@*2x{FfXmx)trT; zv9hrz6tI^0>}EF?6X(9bmcd@3$nC+<-1LiRB~97EhJ_0z63X^j2XdcOyRrWzIx+m> zJTE08F8577ky}P zpBhFj2APCs-!QSi`qm`CDaB--Ta*{^|t?$IJO*Tba_JcAuqV@Ni7pvTo1^^0naQuDF|dg_V+ zgzumq*Db2QimxoiM)Mq&V>^?l+Bwj~r<&+~MVMpFfSDRLM3#h|J*5$P&$nMgxVpgxe{e z*ZDyYCg!p#2S}!I125aeJ?y{AZJ{J17z@6vcW_v3_(r8REU$fS4ewo&`H>=zznR$Y z-{sxR4{kzCIm3`91`uity!n#*ll`CPDdf;16Mm}f+Fjn4Xa(H^$F|q0JdIk3-o&dr z`(85Nf^iYZ&^W)8J;F0|&+OEV4#NIFl58LQeF+ zk=Q(hpI=#78RwsqXjTkAfWsoW!E6Gd&uTGQ8jm-K_GE?&x?qs=tNlsV0kQ73g zx_>4X4UaW#JF|Fj;n&yK%V+PiJ~@9gcJI}fEnvRtX{n|b=qY*>oW$_WXMeLAB{=sg zRsC=3!~$^ayt_CXJ%sVIfY3U}X#0u$wEBSF+DuZaS?Xw+ey9#~*03+(mGq~*vfcV( zA$%W%;F=8h$Ti8(Ve7Pj=i@h>6fLGhKyXv=7|$+aYF3+(tY4~GAg^pUVb3qBpJsBu zTaDVcKb*z$;Co}~rSUh^XkcX_`T}2~ND)R}G22SSvZuhI?N-24W)jPB{|%p*`0OXJ zBDNCbi83isJo*iV5oF`g>G(|7nUJ|i&E_GQszu(qef++k$)Ie0EHe-0=KiKcX{dWxYyv=UCVZ)%Ww3St+f%Jji5ratYAu8vCNTi57>o?iR*uqRfdZtMiU3y z)`EtZtgbHrfu4@D50E9am$@wBqJc7ul+?-eW%y^S%r`U@PD)eF2TTjcKtQ3{S1!c4 z@Ev-lzwy0-vJ~B@Z9E(4g3OZS`s`o;$@)Yr_2_0gw>vpqZXpzfhIly>b&;q~2ro{! z-Dw%?M0LJXV0bGN%Oh)`*t<1Bt+3mQ{FKd7Z{>=;BR+lt+DYLv_Oae@3*S6LZ4b)p z6)5_)d2s+ZLP&KAxJD@qZ{`}vFoEqTFUazE^mtTaxDa$|nF!KcH!$+iWJd-&DvQPK z+_v~c6TIIGE012>J+lHP$0;jn!fP z`82gV`vaiE3KrSq-Vdw&awo8&{pnxO(8P4makYX$#EZG@oN-*ku{4+g*;)H7$X|f8QP6UtM z{$}o!gxI|Z7X19rb=60q)$P@WYKdCOIBi-v9rxaKRar$@ z>UPez44cp#$;>GI?6`Oq-t1DeNiYFtn?B z1n&_jQXJ!U`jOyKHz{k%Ci*IbBM|Lx%m*k7<~Jmd}AL_jO8!K{%_uct@ALnfU_n;(z8 zjKzbovllc0Z;v3c=V2_5IK8*`z4MLIj&n=ey{*~yQ&50E3-kTq=p=w^#Ko2t-^zxN z70^OG01Yvr;i%Tw7HS~%=(c5H#97nf=$9P=e0 z3w5>7qL$3>Ralx49|YH1hJXi?#n~*Rw6vvDUL$y=>|8X;k%)tgn``nl|FXq=@_5of5^zg`tCQr-%8n z$fV8W95vhsp~D1~VwCyyfD{RV*MF$7;XcytT8eQqBuKDQbZ2$126$U4^0$+w{_aG3 z`V61SQspuMum0c5pFRt()UA1~UAxk*ThF3bo@D+wiUWVaH!*=FS%na;hp7WR>*~vt zg2(2=1IGMe$PuwD44_n?GT4b+gPI`54FyJ{2AY8dJ;19jqst{9(2H+68?2b}+tmNt z$^cz|O8sqzB5iha-d*d62;Ml;5cG&aa`^xa zgD$4eMe!}-o`&|1F!1CNCT9er*d^noCjOwR)1R#M}v184d-&IGe z!S}%*z=!Ud#GtL`U>Smz)YQ=POf>&E)SOx1K=%cVUzbftd`GkORPXQKP=U6O05Fuf zIL~V)7oT3^a?vrZ70_(~>2ml;(Vu99b6k#Kj2T*N>msyS*F;REiX1ZG#r-S0_9N#^ zx2$*8_X58CT^5ejyuzc;-k69);?xpwP!SKO3cpph23dt5K?qWK_cyj#jS;-mOP$PD zR_NVdKb!>SdyI>5iWBy0hzk`6$3ePm7}|Qx@b0b0nC2uo z`{V)vHv~ujd2%6_`UKBHlnReWEKaVQ=l87OAxeB6?|Rq}gRUrfllG>Su66mpmo4JR zFCI@m24KQZ85vQbxFnERN=K>s0+Aad4JBGH-gF3p&dHv9ItR%4A)f&X$F$T41kp0~ zr=Oz;uuU*aR9SX>6jlnQDRgsx(u(rpw7$+MJ>EK0|k^>)(+ zW)Tl^-6*f(1-7`M@|uM_|JdkK8%F|lPKutfUSJuUB8(UrCRwQs$k(fL>z%i;RD~FI zW67vVNrLuYfjT8n&oHh={dKSgHdDZ^cw@-$p1|AnnLYv zTm8qnAuuD4&=XR?+{FR`y}3Sq!H!EiiTkNN4N3P2ZOMSW4xPlkqeM<&nF%)yjDnC;o#23FS{E)iBD zEE6d^aBO&l3=_4Vp8%%CF0{n9E3lb8QO1WL?6O^FC@5mnV>)1~luac@S(~`1 z>-0R6@Tmgz&WArR;5^kNgpWiQq~0dIS#Q*`#mMe?_K(N$yHSL2o)7*QdQsdhD-l!7 zD@SJmTe55B$Qa<%BUHyH5;c{gY-Ri)C}I?N6lLC(%wu5nzSKd4GB=sfupp$2-CfkT zPeKj{ii5%!=>=^8*nTMkJX~gHLCHa&(|F#)0MA86TraLA;_tMXN zjv4@Jc3?9@Z=l5blNt5iWQ8d%=whPk?8(g3zE@Ie@{dou7DM<}VAL4CHgnLHr7rL5 z1;P&`%soJXPeut{2Ru<(jOl!@ZnR+{S>y&7`J9EYNj2afp(>iJoiLny1;|M)HBw>* zAI*phts*Ff>2uxAxl({!4M-1d(QR#Q=3hdR;LhLZ0fLxpPzSoWg^Qe2Cdwzr{imM~ zGRbvei~~@S<^(No>ZC{f6};iu_sbjv$~FB#RvFpJC+*t1#dwjCW3b#O%De)^bHMBJ z+*6g?_*l+LaJ4YpT-i{<1OE~k9c4X97#w~aD-41<_Hpdp8rl3H7JA7m)?i`=XnK09 zjXT?oVaB^}<-L9f137%T*uvx-naNX9UO<>Z!l>knySn#1GbQr;*ljq4Gb<6oRbOH@C%Rq7VuM~KLNrDyi0z-t*Bjj&-^<1?l0{%a zN63ra**<>!=PAgYFeMUMT}|q}7$}|{aNPHKfl^;#n2y(&meL{yr;_}egAGx;YI&bD zvY|DF1~hL00ZC!$ZtD>}rF>m--19zY({sbJdwKomE>ZJVBOn4~;%I}61fs;_wI6B! ze$g5lCW)+xB-EKmId*?l!_L~5(6zvfW(pfZF-8)<{J>^`GK?)VN|TzX1k6qdYOdt^&?3h$3>TCbI zD5bl%u+(%JU2eb!bv2S@?2^Y~e)9Eo6JUE|%0+#*8l3+Z_*Xl!)J#gvs;GhDKN6(( zARA*0=q2<}5A@!Q}lm<=4Sn`JX0>NlvtJY5nJM8yiuTS!Z ztbkALd9n!-U<#?!Uh4v&&%q$ZvcKX3T(=!zRx=uCIhdYy>Q)31O(I9771yxxWZ-fz z2V5$F+Bau|-#w!6{#bc_ols#|6(1u-ncjD!!zxqM0m|@sU!toO4k@uOZx^96lroqv zawsz3zV~?C;PUF11?AF@i5Lp%SH0w^kPy_HF$i6$J0w)EUI=8>h{XEo07jic2s}iw zVemhi&T67rMy!tyG?AJeOrubO_W!3mcJpd;e>(QdI}HPx`LL&6j^rVeasa9|=65ng zZ$%(r|DWa<-VY_azKv$Ph^t; zlD}`q54RLlZQc7|Z?R3T*r{s=HetO&<{0sS+KqtA#pIAPtPFNd;dp1?0v^8$@^vZ4 z(hNb`vv3pvN)ABeZ#g)S4nVOH*R8FDUmR(AT8>LWkY>3bs~0EN!C0hHLIP#Vrsn}I zr?b}}konDrUbv|?I+i(`=2-?e_795X%_muM@01!}s;H@ja`WNJQ@06)8^r^xzo3Xr ztnW)-vN&#JehYohVgM24`g{xFy2D(FcGLcVclEl>#akGU%EXQWhY?&WBAY%1y!5P( zWIkEgd7S$2di1&8K?YMgH$rN5i5py^Jv_cm`SqkET7INb*fo=9(4g^c@m}smuKUj?MN&Hv+(FU%ot)_S&;} zqKyYY6?c7fgal7fYt$(gQm%1Z1XcI^k(J|kzDMB9 zlih1vEXL16h0Kc6=dFYS&IEpw78U|{#A$k+v>T(#npFI!PpnejTr7HOjcwU1p6BNK zJ($f*76+O@3UK(ET{q@I+DAn|MPVT#$Q&Y1hld3=Z^t9hA=EagYNahRcrs$e^+F^p zs23PpCTsJ>fzu5>@mOnN=rLG}B4PZ^t>AARwN(CYbTW_Y)^J{0n_&SMc?8dY@KYLY zja>@(UNMfA2eH#8pL5^q63bv0Q|FSul2H5SE?9&Ax^o9ZBwIIXrHHcpfIXs~Hqt&8 zH@EN4h3sMS%Jv*Hn9+okAhWCez|!69xX?4)k?JEBPo4s&4qNA#q7o6WdBny_$yh)@t00#*d}1GYZGa2jt2pzoSy)nu5wFJBH&&(x#_vqEp zsq5+5xrIr=iB9RWS(+u6g9U%l5HmLCr;yk?GYXx>}KUjvGS%Q8cJh z3=idt=0LG#ZPmN&oH&&9^GyDDSfQw#=xa!m337BO)H8cYsKW) zZM3A}NPqRkAK7qBB(UCHL_{R_`*+6FEGIj6V2HOGelf7}BRsM@xd|Zfy!f|))Ih%H z$UU-`#u15Ah;q#%pd?h!0*E!X&lN>XKR>DA+1b2?ti}ibSM7KC1m$d3U@2;RPgj(?Sc{TG+%lytCo39ku>g}v}+(nD;;MG62! zrlZTPWc8sQE-%Z&+8kE(lWc5R0BiM^C`ozN2bNK?ak$$5)qO2$HaoUxm6~E)BeV^S zeTQXTR;hjmO10Use!UfA{>D`i+E|Bq?Y;K_z@b-9G9{kS;CQ^Rk(Y>}vYFOqJJxI@ z=h@{Ve>Z*`mD76{xvBR|7jV#Dfb<+65woF1A1s!PK1f%3HhC!C+lkn5GlJZeMBfLA zQd0krX`@jY0KNI;|C~>8y$MeER5D6HG$+=MN(pu~S<;FK{uxLsK0Wy8f7=bCWu_-| z#^#|gqHK&Bc?#L0Vs-OAry1uy+#%+etL&=tJa1P>b)3w zr@Nvsf3obBN%u>)K3JQ4qk#M}p@l}1c~dki5+@h3KdauL9iozk>Pb$wEuOmZ$=`O6 z($mx9m;ns!-F~gsYW0@#+Zcaa6L^w&IC4>*FZi%Z{r*{^>x%Nn?Q$(eaV>`2^QReNPMy~WANt3hKY#A}CsQglHK7owis%n}Z#oK|8+Zd88{!iVm1#K3#1Tv{s zo#ru>iejGoFM!(o*+}5`?Sw_xGd`vzq>&4%#|eV_f@1w*tfbKd&0LZ1ib4(OP-3D$ ztJ>HgwXO5YYuYr(raonV+rqU6zww_cCDAg+>TBs{cwG~Ypx|Z6Q24Q?J&5oMR#PoN z^u7@4aOERK;77Xn95$$!1V!$!J1V8@7&9Px;J-7qi!QpW%Lz5!t1g% zzSo6qG6D9Wsrxq#-aQ^`IjD&@+y~6*znDv zoVb-6!KB1@bAUr1Xa&V4orXrf5pcQ{i!OX!(;wl!l1(CRVf{mypRQ>R>oWF!+XkN}vxXr6B) zK1ghj@K_41r=h>6XSg+Fs77-S&qhIWzPkAc#a}JY8-JD-YJej(O$ zgZuT9Deq-T5Xh9#i;{nfryJve3ZrWC`^0sP{8{#DZAaR+q;Y^T<8DhkmOat>T}J~V zJr!~{MIStaTd=VtKz6z4G?{<&N^R<9A4S#05sLcZR z^#HBeln7t3X5i$Kg%ZT@02gr2cG0aIU$x|Ka%f*u4S|x&rd_h%3S(IcYv=)icb-qr-mguz%6+Uy!&_9zt-x3 zos?G5OqZo5+t}J?E`})VQirB@JD-9HwlLup`aFAH(&hR*Ev-U6fg_^YEU`lij~D5g2s_WR^lKSu)Vsn*)}2uO@QqGOKNEEIrq>t3h{noI<+GX!4V!|5%*cK`}VOLsZ%L{r~mZxdN(Dvq_u;B;Ro3 znjHu>e3j!+K?HtWfU~goeHd}!3eacUaTNKP1{|8SvzVX1aWGxSH@j?MuVNl~8N9c%bW{degY=2NFAv zQ^Pv+U)HdEgJ*AVZ)kEZ2@f63S`KHK*fDvBaE#y7Wpu$^dLrE0m+#?qogy-lhj%moL(^49Mb&=Yp`}DhLP|QMyFn@G z7?2K0hwct(q*FSiTRJ2}K%`rwySp2{hxfNW7X0Co8SdQsoPGA$r;3OL-@ixGz#(9y z(C=eSAd$h3@3rucEfBYFi}#8rlXuwA^YDj4iW)UvYcF6~IOhLGQwck>%ilp#wX}q( zfGPyg>+KFnl&N%QR^d9;F%n^n)s88l(YD&n&~g8-vUGd_+yKIJo%EGi(#3_^ge{FwlLX@iY8@_^b9#G!B$x1w;eBP88R&Sq zxt;vs4kz+74@|nhkQtO_$I^p;><|nFSN}U@b3f-GnSTCH)jE5=lg$i&#m$p<=ryL3 z_!TwRu>A5xo~4YOBC;2mQ-aG<>Zm^rDb2qzC@O@{*qyvUuF&wvBPJ;+_@vr_Fj9&z zURC_IixjTo1$~{s*Pp4r2o{To^oxjhof+BQxLdos?`)?`Go;mG4uIhQUY za=&L|K#n&1q#$D5i4LF@WN2DlUgyT6%@H(bKa0M~nde*GNrE1^dN#MxI` zdH^z+oD^J}D&VBfBJO$p7Rt-99+6Kv@ZL(=NMz~qmqkMW*()UGlO_{=b@X>zhwLL` z7$r3smsiOhe&=}jRhh{}WawqHzs1j!#m^-c`4hQfh|#&JsW9_6zO{&r{;y0sTIY@1 zxXIhB4nHsrVX3K5P}$S?>VCqF694h+9)8bz=5yx4A1PWj8W0h;4g27B^qoH|n?@F( z@`@eUCi;EznX~cJ(wjcL4>2#7c$;7iCadRXu$Ig=oWl5U`@w`Smb4G90mlxwaXzQ4 z-`nn3*|OYT{f_;ow1)dd4-8akbXZ3dxn7G;SxD4kUX>x2|EkpCbdC4McLrs3opwXm z2C`M=HR>$zhJWk(+7XMuGt!cwUL!ubkSd2fe8pk5F?cdUL%;qw_!0|Lc*{1Rh<{1? zIYDakZTx?YI3rwkcgATZ)9oFG&+fT?8d^ZUlV|-nC)9ZJ>hm~zSfaZ8=~(G~x<#4@ z0){s-VOm6Ek_)DjNaPy=-01lAzF)uEz+_Js%=q^MBy|Gjc%a6YjcmuH@}&l2m{o|? z*CGM)1aC!t0>;>$Eq~VD5JO_jQdZQ0Y_A%^)B+=pG{ag=L#{L{`>2 zU?Toof|jFo`q_+yTSS#X9!a0s0>Q^h(y33{ZDM!uuxo!CtG0D|phqh5nukS=Qg#y7 zh^~3Aq>ldjO>R1hLGPT(@G?~&Gu4jU;^S9@We(TNK*1gKN5t&5o10QXaspoR7iwYp zQP65qW;6WuMXZL|$TIl{%foYz>HQc-p_~?x%B}Pd^3lRKgf@?J`;cU0)y4Y*S92sW zATGuGy>Vc9>!ZYt@LiLhn&xv*krdTG3~2iz#PCM$a(!Za{PUMD_+M7b{z3-KoE^>C zGc5T-KEGb44XJq#*6mo;Wm1LkAEsAaPCGHcsZ_p;EmdAa|H2hBHm=V9N?liXz>x`T ztYpkQ0-NSBFt%0>dhLMWvaLLXz-!#VIp)!>`^^X* zEaUK}Q;bH01Vk9!%d9S}OiVXBVcGwzNrmSFm%hyUC2+a54xr9El#=hN5sg?4d`y+{ z{#d^p!0}s8b6z8Gr#}eD;mb}#pZHGG_XytXr-_ZV99i?{CzVSY=Dm7YR)nmWI{6Iq zafCA&<-^OoH!{?dbm#pmd}r{dmnss zS@o7?`X2rlW}ZcsfHEsh4JUaq?;At{`5O(yrA$We)rc<(Tbl&YMF^HIaQ8 zvE33&+cjpKH*e1<$(DiQ`titVy-q`YDS-)C8KKk-&Jx#>|3sAiQ5nKtNEv*>s8~n3?Aw09p(ppiV5oj(WYZZ9mRK`?ZX9fVGa+w@?m#s_V)c(a1@-J zxacq6h^fW9g^GUg*vxe}|GP#FN>+T_+sX9i^C2ZvQ=b9Xe&)wWH%9%5_Ef2fkyYo5 znzsBk-lUq#oOj3&9_;FB-q^idsgKuj_!Iuh;rBX}D3 z@3ka)tPp#ks;%e=-v?7cw4@y8n0QVBo>1Es?V)~J;s28Ur-Ch;oI%wJL6LcXjQ`%f zdO<>ND0e+fdX1pV5jDKd>q{irdnmP*S$&J&jHW1h^X$3TciNA>kn{Yzs*Nn1l#FW` z1}1YL500*ejN*#jcLeqYBg4u z(zH+Q`i*SBv9Gq5R zDrZ&o4lw|9BfTPrl?vBpesr4aQT3xVd9*{TKMkpTHdTbsmDdh%o zV^hnZeg-S79~OqoBXH6@r~XdBL!x-erdldF^%hu@qGh}GVVrzAw$apOZ+*w0wtI_yRTiSC9{|SiOm~ zTMP8W8gM}dDWIuX9EiF$$O^2(vE&Oua|&?szJRI$dljt#z4>*>4k!@D?q8=y0$bs; zcDAqHu#`>IM$K}D8nSuOc!2m_PfxEIGNBob|Ki><7g4#XM;pqi8|kDV^YT%OCiL7~> zUu$cGoX{3=;M1(De2q1hiq0-&-nn@6B-fKwnA836LJU9 z#GwL{5PU<5k`ASg7uhe8zxvzWoo#B4co1cOswLnXRaT2sk`5M&jOu9K@89$IMFM62 z^ob3LO`+gv*w~ftUWU6{ylLBm=yWQ}`%7rSMi-{? zX?kzl+*4Z=GB-~b77x9vHziEv^ep`ntSi;TFW%+`;|iUB;9Z(hiEeMs`QSE#%SVbD z+|135L+^Y_bEY~e?Q#y?S7;4Vo8HZWp{tc6pIb$`iJ1_>ip>ATHM3Z z6m}iWh+DCn!z(yE09!$MxW4UTsTQ)%cIw5mx_D-OmVp$DwHv~0VKsIi>gjcgo)r<| zd|*5~tmEv2>QK!)Ly8UCKa_-so3G2)Ar||nA{?J6mWI!MMQ2^znrN-nOSW8SyL0Jd z>O(`|HEv!-C}3$c>$@Qc*Gt4nfzG3)v0(uj7}>0rhAbi10I(Pt9( zVSg?TU_yMStk=(2&rd7t|~nHeWw99Qua%e?M41!3kmt-?)Rd>UHF&B_K{_JdJ-13SeYmNF22&QUr<(4$2pS=o82cP$!8>S zB^np{eD}m46!6=5K^i&Gi7S11ukCoE=7gD^8p;S5PEU?0Ne+=si61d}f@fBwKeA1CjW!`Y7ay3Bn5*=4e z*p)qAAx)I}^dqh12mC4*=SzN5B|m173iN`R*-B|Gzm{DjH3QEBZb%y+evd&vibjxO zsi7X;kW_cJc*)Y+x=H0`)kJ1lcqRl-j0~7ceT<;M6r&Y-MJ3bZ)=toCzTYL>17do` zfeEfO|6dCrJUE_9ncL{JJ;n_OmBY+ua;88IX%mAfSYtX!yH^_DGC-i>^<|LY1v{E} zqL|(Yv(S>M01ZJey&`{j_+O3t^%Xl!u^bN_{V(%=m4DiMwHS_d^s3Fz0l>0UPr9Z@7ELwNY>@n63>Dj0uWMp3rhu`Ii(n(-g)Bfec4 zcKI>zoPkat;~6GdP)QTEXz^zvMQx^#&QA(-0+oh$UY(yT)pA(-xI(1i!1;Ll?yU;F zJcS0NK!v7Yq!3KF(V1ucQ!fg0{1NQ0Ye`QZ_`b;v3Hz0M(BsWH4Vk81M7u2ttd#(K zD~_=wC%SfIFbmqOuJMl(c;tHK5+$RpV*C94J-M>GKc0)O?#Ykk9dOTe{vcbXOo5L6 zu1tAlZ{6DURqW837?K#}FM3YB4%R4QBryqb>`)1b30_JEmmF%LmPH7@Je=BVDXRPP zC_+Y)&;Ws0MUBfXDN-xHgw||&@p#->w`C>#GSAwmY_W04f>+b0PTyavyv-Vy?|H$I zZtQ^(;&Am45KC6r&=@FI9IIc{DTjbsSdCdw?Noqf1U1}AyE%T>;3y44gP2z)oX*mb z=QjU#ilsmO`1KUJmBMhRDiE;6DSuAlfQ@Vzjvz3(?z6q zD^1>-4{Waqc62^?ha*sDIK=MAI^x@M%bD%DH6b?f#^z^l(a$Z2b`x^3{+RhzLcRd> zLhpw#(nm}w?WX#vkz({!R!G6ack|u3BABbmH&=o+Ts`l1i1Z@Vt*YFxuDXJz9Mj4P z+E@(fTsS&;N5onE;-pLdJ_SXn9Nos*r}6Ert?b4;5Oi}=FE_m-5?J5bvKXq|V(QOr zcqrno8|V`=Fw*2?8{3_104!8ROQu=tIhL^Y4S9uucsy+ zor+{Pm_O&wiSyGFl(1!PfK78>YAh_t?`{R55a;jrn#^z|Ql-ld%ciBhIs5L)Y6iv) z>AfyLgsmM)oC|uR(n?WK^(z8-oZvR4ViPzy#^F1=yE6=f=n^+C$Mq9FElA}kzrZJ& zT>ZzaSA5a8jRfMN99Q%jhjeY&ehH=78!~jUgjq$%iF$aL!|eM2+SK4E3z55cbn7n` zT_21ptPj<7LiVa?MD46VMFvGlP92h-DNV5cP1S|;#~}EStx*VH^AD<6=McXn7HB{` zUVz%=cm8*B=#G~p6#Glqr{13TEz`TUa%x;WzVSF>686e4ru+`&aMKoaPd{lDi-`$=r-#9$Q?$1?U*1d zYOk2c)GewfmiY8GyNdAM*!_}?px$-^(U*AjpnO_rk{$AQTI`+qC@ZWbn6O&KqVk*bg7y}!55nuS+|&QcX9P5*4AdLtzV&cJ$xy^tsEmTvQTuT`Gx3z2a$1)o z8mIO3vj$9MB=j=YyQUSe(p_u;q%-${1=+h{Jty<=jnLQj1#d6)i^-5PiVaFeeNfxi zz1UvA{(SPqVaOzN+huMULp8^DwMKXI84J`imDiPQm)zhn9=XWhN(=sM5I2I5o$r+x zawvPo5%Rb%W#qFev#TG9p7GYtro_(GTytjPqZ_bRKmML~0ATXsT)HS0QrtF)D~7b| zQ{>O7a>YMqvMalfn)jSWOd&mi@6Aot+sm+jhOc2N_9FTD*Hef}Atj$5>VB@bK}xJy zbv+N$=EQBzA2fEm_CD=;RNm(ykLfjMm%bpIq)$q{&6;6(=`BUK$~c~ruoFw)63f_9 zZkRvki;-GFa;o$lOSL`y3}1axy;hq~M6bo)qLh{QQh1*shN{$_`$M(l+TNdnZ2HDm zfCSFEs*m^wY`+nzg{-OKYp@Sh%odZPptt`oC(<}$57gDQk4F= z9coCeywj5hW=+4!kej29*ZS3Xs3<-QL0ma`o1WF&4*lO7(^_HdXEXKP_ zr)#j10}2tdVd$#XG(S~9Kba}%b93=2ii=VJSJS{A=PBsQ}HH^gif8O%V!GIJD(hd|C;^SJ~Sq|SA^Sv5qko%nN+C`Tk@;-R>Z!-q{Ankd`d5r7gx z!&j$nkfSCH2c6AKx#ievV0~dS{Wzg2%unPwrmXPGiZm+#t_Ty8TX#Uf5sUf_JA)+I;;evL@;T-zga}t3zmkvy1m9tih&X9s1M(p?enWK8}0B{ z%JX!4$rOAjnDjYp)p_S6M&LH|V`<1DJk#;Iq>j%pNDy~K(# zAy0xTosT~A3+hpAYI5djpRkLEhJKCAe33IG z{PNQdqRsT>Qz;wsOwt;D&6-2iwcEXO{zXsZ&XpU2Vl;26?XIH0aYsR&2IPDQVbK{q zh8P1ZxwsW3=ee`C^NzmFjdi%k#tf2z&-d}Fesg{ZUt}lrh<0iH55ws(6VC*zK>GoY zD}jys4MoBcMD2S5jXbxU3FC3rR(UnL8WJj-I;7|VV0j;DhM0SO4_$%7B;N^*!tzFulqlR1smm7~w!p5(HwC(pi#==G+Q~+MtHo_+V z*;hMHwu<*8!QMw0%2;g*N z4O-rq1vm6B^xM?#t?=KRWUr>Crr3mpKv1eXd5?e^Keo*col3B~8DJL>tStZJBO~){ zbad3)_FqT^uUH+%V>K^-Jtj&2UW^0OdZ(8mP+C&JsdXv{N1EmR55KI&H&cv4vdT~s zyv)o@nM!!oPgsgdY)I1lZo%5Z`Y$xZEG?O;5{-{P+}3<|`uxdjp`dp4@%B`!C4qab z^yPSV*v8d%ue^c)su*Ab2t5SVtE8xj*8&3SA7Z2<_MI^xA5qM^o)2k=Yltj}tcBoz zcU~coNRn77KIxL%ACBv{gln`yaJi)l*|MP4FPLiK^+TihyE9;B0>3Erxvp+(ZLw$1YZw*ePzY& z;ig-l6jF@h>SK*hK@%w1%9^N+B#By0K!`-^cC(bbJT>Ueh$uN7EFu0u8TwYOZ9b;R zHUZT{Aoz?Jha2a``FQNo+g1?KD3JAr2CKfMn7nM)>rMT%3c>cB_fCGCsUB}psMG(b z`0lddneG|e&VY>=R&JD(t8YZbg4qa-u2(Haynn?D#(qdiU}J$#qF-r6a84uBJOcwN z3Wd;Iv(jdJ263t+y@Y9OE*LCvF)RO=ajHpNfVihBk7-v zQNHGtZy%^Ql7~c#|8NmRe2Ms_avmo&i5jGB-H5pW11Ngz3DM05RE5 zF`V(RW0{4YAoui}Fr*Gt3t6Tx69cPpJ3oXk)MZETU)Xzlk&xl zUP@7MgeYFS-L9-yrS4(@ZjMd26D@WDpBF{K&#|Xizf9q2dRO172r`w1J9}UfTcb-#eyx-f z{M@l41%c)WwbS{*t{==?WCp{3F7#a@y|0dTa?E1HL%X}bQ_;n=#*sY_a(GxEDNZuI z2mBSP{-KJ;71Q$cuT$jl5O3(>O`6RuT8>I))K~ZWZNwdxDazC5QfteBnnv1u)0PZ| zOgg+fjmer&=0QBOz0Q%DzPOe;Ge5NNR35m3QnuBuMK(B0crpm^&A+MQm$z(uNAnIR zC^dFtw&ih$p9dgEj@D+^?)U6`<(j=a{5>aM%Y32p3S%PiMo=(ZecOb)FY>QqCIX6? zBL&r@*KEDlV-^nTp`53pUK`QSIQG*#3Wk+*Tw%E~vbmZwwyq_@=8Pk(o8Nx?iu~@!HdpC0JnNWmGk|JrY?G{Rr)uKH zpn_dM(T(I76>ESu5FE*L>m?$m7rr~*TSOsYPAXAsp+d=`dilNQr*n;Nv7z3ItxLZC z{o=V@KC(;6vx$_AWn<~c4sq%lm}m}6C1(^?#ux`k+qI7lruly~7gZSxtSv`pZTXq= zE+p6lLsVnBnZvp%Kpggy{s-|J8ssy-=Uv3?ls3Yl0xNG@`N5Na&fCTfuVQOI&Ew6& z%nbJ0GxOR@bm}CT-d*^A^YZc{}W4J_(Do(DE5^`5@^KgnB{7jrT2=x z44xvJ>AIsTq&V--P$N&cY_u=YmTQkBNv}N)v`)Q$i{6E`2rhhQTpR`-aCev*LTktn zo#}_Y>tkr>UuW3f?YEy?7^Sqd^dZam`&JL&4W}(bRa72om7taL?Pxi9X}HpqzvSX4 z_`D2Iep~rVj`#fsqFh;9hoLP-rt+IAkFKt?+idNmr?A{ z`f*o91F<^M{0BqP&>d}~5>t0yH#c@8qwahE;&(Y6zhre{v{@_O>NAtQG=9Z69aKD3 z`~l_{ACX`?VwmOwx5S1cY6P=G{ug>ScdCDZyb%>zcz0{1F*d`JIX%yCaWMX@91hZ+VSS> z)6+^*aZ)M6@rAXUr+B&hp~zfy^8Q~aQsV4EqU!=#rCW6zC{fVuMn48zAJ2q zuzB(j6&H&wLM8_XBY_C_OZSa>ruk5EZ&^xBW3qQ_ zL;wQo_|~xH>h^YTNn+`d-R08iAi0|A7}D=CvqE+e8=>Rl2MTCS3uy|E8Q$m5pZNp@ z$F1;GCcbS9ByuReOcMJPH3_do8;^~TEjd%h9~ct+8Ylddjm?IGipV;tYHFqK_nUbS zgkqc^UQ^S2=vODwMcDDxTutc1*_(kOKhZl>;~lu4h)S+j&o@tyaOLs*@>}&w{XO7| zQPsxZKf}tdsfmw>FSK{OSUKBhXx=~k=46_)zDs_KkuWT1ebN_T&c3^82rwpYotm>T*1TZ^Da=)w@-BXp2R>~BH zi~Bx3Ikd8poDzFtYRd88E$uBjqY6!4T1Rn+ZlBz^6dc8rrTwl|=V*4Fzyj#i`^+Xb+>@xQ&AChx;gB)HS!uH>DbQ3 zXuq|`$>z75jmaURptyPtSa5yQC@n53TDm%3B@u86Eug(Uuo4Hsq|nt;K;w@eHK1Qc zFdGM-=@0{1$cEg2M}2_ATfNXOPXa3ts%vVd>nsVt?&Xc1gQiWgjp3}} zdh6ML_0~EK-qvtCGa-K+R@P8|3>%*v^(-xmS|p>LZ7bHHsO9*M4C3VZdw??3$*>3oH8hen-VCjn6d`J2@?nSgU{4-X!Q8FJ7I4i-cQcfOq%avKe) zVu-w+z%xnHD|Uzt&;#%M1I@Zr`O^RyXO>e$~yYOpkYW9PQg8 zp1a>ZH$U$VvU_$84l7NYnKc5eQ3GiqD+fQ`-)y|PIuKOl(gumdKC>ta}x%_Z@Kqn-W zE-G@_0~gut@bQC&ey}c!`LTaDXfLo^UI`MpI8X&1tfq!jj)2v4+PeSW4_nsrb3L2k ztK;K=WRU=H_Hl`czv!}T{tGh>4R3y^e*;${PfPWh}17-!-DW#}LMQmMZ_(1C~ z$cc?FLaaG$?vyWr-!p7?TW0f~LL)PRB4NW%O1~WY|C4~;&E-&<&d3+g4*#45TU4D< z%%e$itB(*B+4U6VoDb|Q*f3BXheqT?UYH~Z_6VNk{ zUfpg)bDq58vqa&#HaD6nHFA8hx)gfG%Em&^7V3GR-8(nY)Y#lS_V;hF%!Z%pcHf>PlT4ZV=~xO=t;KmLL`jd-(dHq()^>aFG%N0@!`&sCIZ^h# zygWG9WJY6o@4@fBei;r_a52hQ3Q!klV^dI87UBDFn&owWJ~f4i(t-HQd^mFuEH+_a zWo@{5NRrJ5ZuE7mjApe7s^{f_X=gAd-piL4ZY{I6?HTL+Usu=GBrGiG0q*3~ge-(} zq(r1}nxFhLj>aY+2!S$k`}X`gLAxc)$3l zpsI=^n~#$yD+#7uaim}{4^@6sx1z0A-%WxIO zL_U80(Yd*SC-ze_EYcr_OZy`A>(?Hz@@AmfdAF#dq8`?1h^nlVF4O^FbSzB*Y_%gU zjls)&C(78tA}?GSYp+kWtFdEykOCV?k-HDdESCDfDlvE-+V3!3+l?q^ZL~8pSuoq$ zF%j3tlmOLd@0^H;fO2rCmK%cqkSyzVN@qP;J-WOk$I&FxBnquoe=jN4?ROiS^#0VI zgHbtL-Z9{%DSA`i+-t`HWR27)#~Q<(-|1{T8-dON#gy1(tqsq7eSLYXqZ(;rh{*}C z>hg8@yay&OXNR-IGWsSc|M=m{IIA@Svah0M5Rf*8*%Uy+o1X><6PeSSENX*siESvW#fCh zQ#1-^zI1b-Un2KUO?~Yl_e}?$8H^?6r4Ma}O){3YWrD*)#B={Txb#NbYaoR)WV!}S zpxo6c?X3ZxGn{~xth%)H9Wwy|!AoXs32*P#Np}@(?JrP9nx)SuR|0G=T+9FN$$GD^ zefs6q;x^mhxqYB~E=pPgVR(%w<PS9mo9v{p~%~?sv zV<>B}qya3W2+FwgY6tbGA=ohqqWC7IZ;-V4} zM5{@@!c8qn-g#}jGf~{t)fJ=X5EQLwkXN6NA0{KwQYjeCoZj;-g+X^nq-PYD*3}Ry zL2aoq9~~dhi4sjl-9SOmy5QGdN$zMiWc2$=lQF!y$>|zL0NuQRSq7Y9nsy3uf=Y^& z)zn6&`v`8K)}^6!^aF`#TS|}nDZ&n?+YgVF0>(V;9TRCzmu12lKUEjCL*{(tD~(iv z!t0eAm(1&q!=59qOHcpXc#d%7(v|p@@3O;^~+CX#Vz6Sy$VCg8ubC z)=;e)Gj^9fO*Gcb<)7@kz5Xw#^Ofn7`HbMRh~NOR+wqlyK$yYd$m*7_%|H*R4&Fcs2;p3RT!XAT{Mh)R}~ z3nz#qqq{oScm2NArn~Qd+(Iv@f~Kc?Q;<%_njEh)5|X|iH&nXXiDL&Zu%>H9^Z3NS z7^r?`UQImn&sQYm>bDn8og!hRJyc!eyZ5aNgfZ_R-3g=cEzDnxu{@Kw4%KPSYU-20 z%~Al?6FmJ8XO!D^5-2JCG1rR|7TDytd2+k>n3cO4HYfH<< z7V~7K2?==mUdNILjjPXhcI16^(1QFIA1)f+=r+ax{`)TN6#D&SP%L~0 zj9`7e`-!{)PV08kOUJ;(v^tXW?%m1)X%V2~q0!Mj!^6=71M)TFZh*r=H8ixX(^67; z-(d*{PJFwp0{jPHJBz-)e#^tnuGi!JrN%7?A#%sJV`Q3D(|B#>{#@+OmS;DXTMe6v z6A=+v{rldr^NT83#Jsb!(|xb1Ct1Lmv9k4o7yw5>Q&Y;*?ey4X*7w}!eqzeNNf3ev za9HRRgMooT?DFLPLFU=Fv1r+!TSR`Noj0Ch78z>gUzg}c(~L?&R6;@)@FZvwZeHoN z4vItH7C+Ygg8tl-9c-K#o*7GIBylvhLI14&`RejS_^$zjPC{307Y3RJ8e~%S7kh9Y zI}^#KS(ECI4rnfF9*ex-Z-4%ttCQJds~H@2Ltb|;T_H#c0jGjpeG@46r(uiusfbU= zWpSp>^r3_;8d_JhHR6b z0=6mo-+Y*uc+*mEw10O;bWY7q?ChFl{)fZ6VCa_-z#y;V zZh~|H=RP$0GwJ$lD?aXjt?D$fGNF-?-5nipwzjq~%O4_V;9vn@ANYNj`apaqsAY9D{JWgJv=^h;%;eaNpHQqs0K?d0WhzJSGH}P^W6va z4-E|oo&9_#LygmRwS#_oNJ{Xk=dS5AEId57tR*7jhikP(2PIP|V1u4$V#a*4zve;YA}aI9TxQ*W!6oPWsXy^z-u!O;)X0ApxK5 zImbpUCP)?=(gw(*>{9QoP~8L8J;}_Lw{8t-gxD^K;IX`W+V5R2uUg0s$ReV+&fg0= zA8^qPXS|*GhV!}c6df^~~h+Ij+C9U24f%V&Bz4cqIrVpMX=dUtToeo-1%F7qJ zBL3Q)g}_({)-D@;4bZUAjr-P21R|yCbw90JW|;__AzN4#7BvOD$WNF4edE~Pju$dd zdzqpuf|fff1HuS8;`0KZLS)tvc&Cn6)1bx$3jshXh|nno1wF;WcmMs<1kI~ioHlcO z^)~Z;I<~En_V(0RIFYai7l4ty z*@ozb1&{ES7NNMfxF>aNdU|?oeI0<}02r?L!~xU_6}dY-TwPuN!_(be_vIi0K7iec ztLl`o%9@&?s>N&dvo^+k2@L6?zCbK!W1YaPySu-CFDIAV+??urziR+;+fV9;m6a85 z=*-NFtYXx;dA@3KUT!WzhN85VmR7s=9RSogaCZn?F7pctXirW~_Tc{l&I|^D^#j}_ z67>-ReNbl7XIP?lKfz{;{%?}uZac+=nhiD??(H2AsN8$B;STj=w0Y1!xZ4HY?J{#M zF)edMN>K=Dz)Xmx&n8Ds}sw-kI{u<~2l za8t>(ax_)xo9yQXlYb{2vJd>!-5&=9hNUQWFrk{7gxlNOp%RpBcPDbCy+==O>EU`4 zG>9m<0q1f(S?SZgH(g`znUK(>rX>6%4Pg0R1(`rEo0sj6!lIAY!$7j|e2URz^W%Wyc5rau zbKb#gznQGGuwX-q0>{_>cE6#&)b=7xGzBAKy|cDk{oniDZoCO4k4mEUScJ1<&=uZ|C4ZQ5#0=IKF`cBn&8J{`*M@ zuwgrefCI>GKMlF;4of^!xa@Jjs{@0Aj{CwZfp7Tz;c-UN`yB#E1)&8|snIG?v9LTR zwK?VCl_vY&2t!EoX$3CVairyP2)lhiDv9Pd6NJ>>b{aP>&f#3`=M z%gbw)Kk%{R4hg*Z>#zO50+!$YMT#O9Rdke_3LLfIayU8?A5$Gq2A6<>MV?piyUntq zy)shsjoGg&RQWW4$Z>PUn#Z3$z`fX4*>`pS9tC9b7`KPteS?}Yv1E3!)f51iWKdQ+ zIq-G5L+w^z4AWn zMgtRwnQlTTTiS>=^CVF<*a!7~g-IPM5J9Ol*_yzZLL`)NEjzZ1{}r0 zFY_q0X({g~uL+%{T!yQWfA~+pxV4(J@RINxnvOWM%m0$l=8tbTdfl=0g7p#GtKphf z4wg-b!OPiB($qN1MZ1OdW>UW&BM>qh4u$V zUBTW%#o8TT7;=&DAAO67-tnQ9`%J7v_1L>j4|4$lfsqSkj(Kt-f8(OewlAdLDB6C^ z4X8wNl78s_7!*TK-tt0nF{-i@oig?ra53TGSQ%1$EGNeCX?44XjXxAa-a0g;=jBm9 zDWk`$J=3+Ha6w*uzRi)F7!w>Qq02c(`Bbhje}DJ{+5Aa+TE+A~{z&M5fQjw_{3BR7l-0@A7J4VZjWgm;#7(LPA1zE-nT2^+{u6V>XMI0vS=@5_~Rd zvT}gmBCwF8#0G45gKXd3-F;pg89)mo6Vvr7`Qsmuegg`4#B|iK+zf3M`Ppbo83ydCy+6Cydk(; z@MPVcs^smwTDZx24*j*+yCionSawr@mj3{+To+h8vQE8^S-?|Q@`umyPssxNZ&`Nr^f4j ziBHkn?O}TsE}f!oLsb^=0%5pxE1R3rfRH_XJRtm)={Cj9%xHUgdmmq1YybyD0kRw5 zCUFsCUNR*9Ic$4%$h#PL;qrItFYeUDTjYXEn}KBZEvIIadB3dRHXFQ{n|eTpuo;2T2Cu;S(&EIMFb$ht>b(QOpunP*dY%EQ9oJ}Y-*R;l4hH%=F0UZ92(@ZfyP*QJ z+q?F`0y&qBlc}FS#mK-s#iYgh^xp+iz)}WHiYBy8H%ko<{! z`k^T+Yl>OFV@3|hnPvAD`Vlt}^xj(52@N^mbKe(Ppp4hN>vNBcaQgcEP6D|ia|}AHRd?0T~;bOX=w~81X5DiH_BYQIKh?GeYtnW!sqAn%O=KZRLtef zfyg0Cq%RcJ6%r>K!TP(S)26ON4FYW9{dXFjt$B!dVU7gz45EjMcUQDh(Y(wnRsp$) zVfbimIt~e~U(lZl7Y=fJ{s~(~6Y1|%zJredVYWFHHP%rm@6=m5gb&e z9P2gkR$t-y$-Md2nvWDPYIqQUMscu5VSP*qY;Do|oXDDaK4bNM9}E2jk-qF&Kt3VW zn_tWih;$2%%Q8E7slT1MT@Ep`FOZd#GF{k(KLnCA1*8M0B)e6-pnYeDk)ih0_!-%| zre_c876OlFQn^H#H?GMnZoMW2NbTC?1)3X@!Q&Pi9uZQuc%x-?TNzileue&bcX!D` z?i`e{p@9B7f{uFq_DUdY$S5iG7_w=8=ETCnQdO1D;(R*iYrXNUWQDD*8BZkW$tcdw zoWSe%nvp|PVRfm&!N{Nh7o>uAF5Da(I8;V8eH52eT9Q%>%i7}8*`3kcjypz=v)wAZu3x`bI6t*9_*+N`mfnd2(cD`xIXdX1jZOtgMqA2l<7TUh1!$n|o7AB2AJLf@N1>qtv;$Bi zN_VDpx)VLhA`7AEml zRu=7t>!BIQnS#vdLGvyP7~AlE|pF{unsx=!V)^}e|+&uwx8eKF_OYirMB_l@3Ay}sua5y_mJ(*?B@>IF(! zQL_0fXWQeo4Go;G2f9y@aAB6umr;wZ(a~7o0&s9}{&CrmSYnYQ=f+M%U{Ln;{qZ`2 z1B%L@wuJdZhs%zHJ$d=$KM~uUd|%z89z9nd8xtJEfCLW?xJ*jyFFn_}?r%up|Be=r zefS;_0E$v8%I1N}_ThRT0D^_1o;s&Xczs%$%%nx zn-%|}*DWTUVy0ow5Gf$uH9*EGu`F@lUjI7K10s=+4|CUZ?zb5vd~SYvj|-B!>U{QkdB`-A%Grf+^k?q+-y2_46&;Gz_m#+UI>q#w zIE zd9wGD_5SpqVNz9i5mFc)txcP$`QZM4su=^hPUr+wpR^LeM)m_^DU(Ibc=)&l*BD5p z>KFEOPU1H`5(?DB5*maPagCmqc8{WW5M zr~#Z6Wfm`?$f-2|>N;qI*kAAe(3xBD7McqeJ1Ul6aNAAMsu-eJD#_w1cJL3$6w$DF zYIjt&j*v8hq;;QuT$GKU)VibMh*+!n9|?^fdQ$xtrmls05`uiDLXL}^ahN>;q(pOI z>qz-`7hPP{s%7<1I`x~r8Eb(TU`6u?q8?1?(~ z&DJK+0L5uxVa;K;`2l1kO4Xs(y6ec6fahAFa&o%G~y*qc}f8 z>IhcZQ~7^Podr&doyHjcDmPU{eknZl-ASI0miZlXJ(k0#9 z^{svGy))m;Idk-kg6_Tl>s{-4e$RTd)BM`DtZw=m&PQ@}jOlzaiC$M}A%8?-Pb>CA zcCf`FSSLk z8o(m5q3MMN@2Ci6z9k3;PWAb zw;j^e2CtqVA3P9ZFtB)bZfrN~hv9X+di(%H7dt3H(7m-HXLf+oI2>0VVh6RXy)L^e zkerl+NW_~1A~Jw zme(ylTdTF32W|AVKF`BSixDOzC8gi{4RDc~4*Lgl#Jc7h^N%4olqEk|tl{LLWh(FX zDU48W%8Y7%Kx(4}|h;wZC>ABXcn6f=J%+xij-$!o>C2nI)YTqXR zqJy|<^m_g|-1K-lP5b`Xpnwh{&R)5Gj^@3{aF$T^rg<@t2LZ{+6A|&p$BR;-gv7s; z<#~)7c?Yf!+Pd{zz0J&4%B(~|T5>GWEY&ySNyEo^K|mwQu@t^^*v+6%9Xll41_{+(Yl zpw7Mg{nhjLfaXiD3zs~(R4Fwze9(Dc9izT{`~Xg2AJGu}x$SLv2?>dst2ZNbnM2Pw zI7ZDs59Z0fygmon&&9k<1&PFf#}@>WI5daZ{T*}JfwZMG`MJ>d{CtB)W!!ScYiH;D z6Rf4A@h5WWoIS@g-ZqWnsZcHIemfxjm$w(UdsBD~rFd%WZr?}4z%X9=oEQ@uADNym zFKsa*^sTr!ly_#EjGV#uUFYD**7^4Dhr?#Cl=SrE@~d&+q=hcGz@~tD{>>dy@4ej* z`pGK=W}p+~8t2mJ#(aJ{v3OC0*yemAPW9S{FI3I&PC=tyBm8xr;fDl$`t*#4E1)qC{Ii;uU>tZD;2zb~f9Cb6t+W)&$b`1Mka>7Qe^f>zN$?m6v_qFNAGllXKR19Yz6h zo)^oH6(8vm$oaJ6BCnzgX0C8$pi2&5QPY; zrm`FC>gu4ExgIQsv)c6XkZ2yG7cP)E&(Z$raWY5H+mklnn7%Y0qCwN*a^=j&`1q)W z+}5I~RTKM1A5~};<#-?r^5zy6_pz`{Pxlt#x!M_!X5O6(B%Q{l?1oE*v6&lB48p_T zJ3D`MSe8Rhnm=yi!xgS@yt3645Pl{iBHG#8%liI(sxaThW?=m=CN2(XU??ak%y2(Y z5~>{nF*@PJTa@;2((R*EB7A%?0|Rm(qu|bmBxa8^4m)31Hx8e6h#=m9?ef**+aTjL zocs&vFk%Iz`cqKQ9Qu5Jz}DfKQy?g!R`mo~G=TZ?3N{g)tRD4+6^p+GL;! zVOFkkn}lkitsSC-c+SD`I8lbJz`k!iHzVU2IXO8Tei5p*cM=2^8(jBGy7$pY(R`V7 z>u)CziTYMsjtPOmj84E*>HZlU-Heg-L>Qj{|5-e(d?n_2@TiX_w#=4nuS=?-=>$8b z*ZG*hkx@N%eQgasB9zdl!3Ry6Gv3U?Mi~BPXB~L>2c8cz>T(l7mMwSfHep)U7o&=u zTb+A{Qc4b1CVT@$8eFv(f776(eF_WClN|P|7Y;2dDjJ?IE-r@A2<(pCgM%E&;n*jo zgX{2JTKhW}oVnb$HZt^+6|==M2iFx|BkFIebQhib-gYssOMS>h`96Xox>Lq``;nsM z^_DvtYaa+Hk)1{-vI!kSXJ{HYRaW3zhLU!YPF>t}zWGyH`$J(7a}8IR#PclC;ZI!b zPxBLua`XfX9Kw}#^|}_n?EelYY`3}}Ubfdl>f$ghcjH30<#$@tCmp6Hw+H2f8%!Jt zaw?KJx@8ut2Y-g*aBv8JMh#m;NElnXt}{D+TI8UeD&^Drxh77Utz5LYj&&s#7>cg3 zvGKWO;%=e4?=Pycdik5vUf0%W!NR}iJ0)Z(DPG4`C9^%!)A>C`P;pItsvE!LBetfap~(O_U7WfA`ua6RCj1b1 zKbQWHY6C6{%;!e+V*je6$qx8$c5tE{R#(|79#LlgYt_@v_}TI6*DZa0{g+=r!v)ua z1vE4gw-qu#W6zH^=bQb})y{hWF@m%(4A#O&m=3}MFbXEUDV;Fs zzFc@%Rq9YzQBl$EbzY=a4E*VKb-g?TelqhB_+VvL6KvqTTzv${PAd^(dVB|8JdA{l zjPIs;EBJvh*m!E{yI6L20--RziQ(X6HpT1*v(L(vjY*lyu5#Mdr+x5!uG$w0C=LI z?9(fc8e#ec(9lu%S)m5Q7CuRTw2rQ>or6PmUEOJY?D|9(+_A;$76VOS;@q2e?+|14 zTZ@av$B%`~;8Q?OLj!g|2l%iN99-DEuB`O8ZoNHsHt{~o9$sF^AiTYtUw8M|!ju)S5rvI$0>fW~G8HoV z%}UZ_=LDd9uMt@VE>@bPp7-z0H=vhVxQ-<`8>rGw*a0$Pc^a`wiooc8|v zD=7X#K{8a?RmHK~)}Da}?~bAe{x#h2y514ElbgYJn5jk*D6fUPdA_mV5Ht5kL)FBs zNN&_Ph3l8?k(Vi&im`1@4jXDj=b~dZx}U53+*=hgMHforxnrl7Nxv#qKE>j&dXs;3 z936@bYwiAA+7>_AEYvWpeD6U0?JG2#iGfk2w7OHx{Fbef#O=%j^jo)4M~7AiHSLGF z7}U^huFq-BleBiwLa@VJ)$eL15}M&eof=Kh#}ME@_&hT4w5|s0>vZJ(`1F~h>eRE! z9G+lPy?V?tYR8+7PI!}iR@lirMwWDR)P`#8+Se*RPP;#@#H7=LJGf)}JiTje{%pn- zJP(uq9u;Vz%9Cz17u@ogYN2Z2&lLNYHI;lclcx9Bc!h6Jcw;XZDE+weWHT!Int5$J zdpjA!|0z1+-oV)bV0IV&#Tb_dBo#U&q~thh`wIg$MeKsj>rHL`qkI;AU+2&nS>d^o?eTc|VNv-ob9v)_W6h)Y1Atg0%iGxIOR+xF$*4OLKx4t1$ z7RYiAI9puxBlMg+nM9#)jl|5%%<+#dmV+It&L+k$tk4T?yvuatV0~?+^|a2_$8w5F z@ObP<|(A$c!bXe%|Lv6wcrBF|?P9KUmInHlhPPufdr6VBTg0-|%l0LIY zrNC^U^Cedqpn<2Z*>7#ve&^!7+xKkur%SbuE+Du^LV|)YeDx$yN3%w&I0}_0VtQk} z1c#~$Kg3uinga{VM};uhu~LH}L52#3LHjzQT%{l#rglpAT6r?=R=sj^-08_r|GKncQGcd^`9UOS?a(d$cFWy6s8QZr>1pb2 z39k+DA#4H}E>I;kiZr@|CUG`K>kM1$i;k+D;~2D>W!Wc1Oiih&_vMsI=<^}(bamy0 z&#Wb*w$wql2D?~6~3)if4?&B~JBs7%s@=j_5R}te*ch$g8MRBv%M8>Xm!&?Hi(~ z^p~X+nS^zl@Sd;8WrKIpJy#qaihWP^q`%3MWSn7g^w}(T$3ejThkf{3BG88OHI+bw0 z+Y3oPLB$#mU*iU$JwTvLC6>je25E2V%S(s$?01%Nv3+J&-yiy`POE9=ll@20kdj91 zj`TBIp&O##A%5@pA!-jhPbSH4*dX)nxjmg}i?xz5dFkXC8Z_LilF^BYk^YUv{^9!G ziT@gOdr7yO>YZJ;vq&0bIlij%DuYs za(+GYdG~~0`9hiU-m3}jNHo=ZT&gcv2}LY;YN;7xFtjihP389UXReY!_XkaStHa(vG;dAR_Ob*P&D~?>4Bs>z<)>|1f>!9PnW9{2&F!#Rc zoVe+HYWvP+O_Tm7bIls-sSICQap=Azo%SJc5Dh^2KF}<&@LvZg3lmk1+*$egfq-iP zAmsujQBp!;*86JNyLKtbqINYexud72ivA&QIuf=mEGlUp9E^s5f%C>Fi`mmeng4=2 z|5^bhQ`X|*y4!i$24k}ez~~!YUEr&aeKkf0Fx4+GkRraD(tR6@VoDUe9|qNbMn<@#p{ zPp;_s!j2TOS_3vK25QEt&|;gm$YfHElJa5pA?2by>bF8S8u7E0RSP9D#tk^!o9LzU zN!)hGSYNWV`nj~}DhVnjM83QgW8~_rE@tFY=y+rerEzE_Y5fB3=>!$^y<`t^ z^LeKCs(aBs|GA-(QdDY^BadaBUMV#Vjf9fxsL*rfVe=}ZIh;;X$YZbSXwTy-^H|5$~D;6#NJ*rXOQ$$kr_P zK!u*|Vm;Bj+;p3SyP21YDqyU_1dBUuXh;R7O_oGi@Mz#1?3hlMwVGOC4(8_Kdhz&y z#+O^@#Dn>=%g4#yeSQ`C)4wvazWQogfAc@7brJFvzA;zv8rO$z{Lm^>sUYFuD?zu* zy6;lWvG_8ZJsn1N5DfT6rT-B3PTVuX%e>a+Q^%uuhUm^eZ7ueFecNPgbO9+1I2)4{ z(InJT*-&f74>w&3d3T3T##@O#2>iKwlBh^W#IJdK&fDXjZ|^=?JvTSNC85JB-Yh;S^00TtvXUp&Bq_ZC!gk83ZSNHJ4QKwOsgDXW1CWtb1U4r zk`iAOoa^gW*>OC%ofh=?)T72lAvq;Ti#-t(5cyZK@;sqt!%do3%G_6 zcT|p9pQ+9N^o)+x!6TcWVCI{LbkiJY*K$qb(EdP@D7tguz;kPRJHM%EW~=7({C4?0 z^VNY|5a1p@ery&WS~#+Px@qHWas0U4e;OY~5m3J>ARCCZ+nSoLKSloovo5RO3{pc2 zARaTh@&Z&^tK#+Q3B0rLI0vc~Awd*pXLs$BlM~x1rxCkhkQ0z9`qWgY5~5{hh7_bL zaq+t_?D8BADLaQ(XfSvHfrp%YkU@H`gC@@|M+q2~xQRR~M(hQ@P_Mqp30nSH)E6j&w7!vV2GM+(S=J zT)fUFW{@_ll$2D?mzJJn_GlLFkNN4ITI7Ln--25EgUFB@)oR|4zsT{vQ}*M~W5wHW zP_FS#k+T$GxY>ARMq$twg<$y@T%^XpDbi3KtfX5h-7yq0HNCw?G~z(0-@rz4Z=vTn zJSy@k$kE-+oO{)~^U<_EL4lHV5t#>Wp^Ekc~De1(pQ% z{IJ;l69Id);LDfFZ~PP#w?f5)_4K9-hKqY=B{G6*f@o-HewbHbBCcXPjiBY--EAkm zbt~=~TwONECjcTODJx3}VGl@d@SShG1XI{mBllm1PVW-5I~oknhllqSbb04+d|kO- z8R?W#Q&HWqVYUINZi;sb>JfC#w6u(wIhB5s(RCMVTl$fFyu?-Gbzc=f<0SA|aYf=# zoRXiT@9phx+*1h^`^ct7Pg_BpsX+4_1<$X~42tm8)fHIfnBR?0QL`$%2#yUr4Du!WX9ICQoaNdJ}PPh(3fp@=sD5 zj)NDY)yRwv<3d^2ZG!>)!U!77YjxhtP_?XNquX@GF~!vnZKcE%{6DYW%_~2S>yu20 z`Jh$2Oq$@A&QY^%qR<$KBxfR*F(uVLwz>@Cc|s<8pIz4`2*O@k60a z)FM}y;679tQhiu7yes~6PsML^tpQP%B6|;KnS0$|DYG9NG53-nqISp+2Up@BYDkM6=+nk9twA4`=C(!)dfEJe z=MkiwDJVWdH^ln#WoK_M)vH%A7*E+AVq*&dpr+gXjpDOdcFcW@W}CjM5j|y0lExVHjRcTG)zL1cJ0K-!Ulb0Xi>(7$2{1K zZ@@Xx0Yc;AVwoTLYX<79i=u6GP=T!w5^ zD6PL)ua|{;P58e#b$vIZs07 zNHhs17H#rj#n`ob{nOiB1BlKPQsiCR#0<$dmuz3!n=;x2GGVv^u0s#^YaIHH5u@QgoOdofiF(mG3D&PjePGw z14X`ffY$sO9&UvILhd;h9qtz;nv-?E`xge|uU6Lnl{D~}S@Kbnt3GE~wC4l~Kt?i6 z=nCP{ihn7k(SEXMV%cO!3dT-E}GBN~|ID7osppejq{hE)D55!-|uEgpiHb*?Jfx!XEgcL}Kpaw7rlI1ASfKU8pYTOAR#NGbh z@RiNYShc=ew{F29k^wg-o9F*kU2(bcUlxCFnU@#E^&0PC*FWeiu5NyeT?oVXD#shf z$DrbT+jW!tSyO#j-I-kFT&7WL``%%Bb^^Uz=7zbc{Xg71D*4>+*{N&e=4a>U{U>I% zcE2#5Q$KgOSg>n6p>rQ4;uDVd`5MfMO(jdk!iW43w1HsE+n*ZGe07`)|3LHRshS1) zJ2s=qUiQij4`uWpIIgS6LRr!v*xt)tr4ib^EJ;Y>!%8kh{%m-z)31Z;KIVfg*%>)- zHQ~itd{I~MM*msWOU1I?nf)Gq2}?=pa>`UwXD@5h?QL3~2ha_Kp!P|wx}1tr3WK1%A!Nw(=mFd~h{k6hozD7FzZ7ta~^GlmAyG0-?cxFPGN)%aScy;KW+`v%E;I zic)O6w5KOAN#jiqZt8RzO>$*+8UL3Lcb7H(#$Tl@mwKY~h|HZFI8_g-(&M75;Swk* zGRU|P&tpJ)9+dW2R^A3{hKN^2S9q#yQSsTs0=~VQ_Fuj$%UkbC21+_UfAMiD{d{jy zns=p-$1jIj4<`2I728rC1Zbf#*q{{U}cZ*V>cd(>Ua*y ze$o9F2hAXIYIKl5!Mlw&1v@?D0ZEbP*}V-p<;YK3F#W)aCZNPXgV({P1vE4f5{GWz zy(a4hNN0bZea+a49id!xm`DJKwF9IB9X*z zaBwhL&>n~+d?&NFkc&M)E9-`NbO#DGpx&(^1P&J`cJ=l3$eETRW7zyP3kwUF8))$N zpiP!%iht>}rea;Uh!5=$0)fcP$oL@_S*SvXG_804wf--|@lC^j|2|Y)=x-!Iz=2Hw z#B5L2kTwa#Rp?7_la7P38`IyDr@p8 z#=!VESBIt@3$k9rs9SmRYZj$2vP0#KLZpiG5&6-!{)2lW<{zj&Y?7X1k-ur3AywDB z&@GnQJlMqDQ|E3kJ-A}}G?ei{fYrpv_WAOsN)0*^(_I^Fa;L#I`}hK-^`~nHn}YX= zbWR|zK?6$t{_=up_1YTcaG>SAD%WDK;N{yn4;X`yJtr(i(bCcaIg%XCG3YYCe*O9! zMOCG~Hp}xcTAptXj!9DhGcWeyn~*@cqm$F4$B#Q;P)&q`6Qs>$&?asGI1YCf{dzs3 z^9wSOG71V2IUU+H*2JJ^4A7C?6TN3(=51m^g?t|=DJemfd*Oa$Nlrl_ofDZ?= zVM*heInn{L@!Egtyg4okFR`GYAcKBm3V0g3tNH2Ux-l~@^P$Mk6qv#B{1o0_>wyZQW=K`>VAdS4%VQ$!U|JSv!|w#o*$96sz^#51bux|FRi zpq$%+6ar^gMEBx=$S!#P+3-D)%nrm2AZ&bR?A6W2w3mU}VC>yU#a!R^?$g!x?$MQH zys(HVRDt8^+mcMta4^h1Fl1BnFr@tWAfTn?A5^VXOvOU>h?JBR4Gn!|Z3}v#f#G3M zko%BOKstLmyk8J_eP?EtQTZiiA;(CqM5A*zWu7Kh3L6JIF_P~iyb8@iza*q&$T?{f zp?!97cQ<5B+!;QJ;^dA7jM;qM{GE*rH9IqMdSbCyVvwQ2S**7Qc?=#*9ky-Oi;K$w z8{8)!ThV!uHvF51nK(koZoEFv&5yzWOB)4mR;Hg2Oa*v@oY8T;p78sk_};_Bq@tnu zw3TKJbp}U>3U+HC17s!@iJhkR(NpExSG@{YY44w~1!^;QFXU+=ZvQ3d&yo9~o=s-q zjK4cB>EUqj0#($v+2M3x@pnQTQ;hTKM9_$wJ(sniKB;j%E2v|Xi@DER*CjqB%gh*i zE)b*`v_{=(Y1Kp3xjXcxsuANJj*_A$ok@D|=xJV@ekx?v-U6A4h(q4Lv2)^1lSjles_0nEftBX6In2W|OMI3#B85Y}{iBE|- zW=c|<^Rn#Kh&V(7GLz#&?oOPaFMnx$Wm8X|J^$zs|$l0I0-@F;o?QZ zT)b5J9=oV893g`z7pHyROAz@N&s4C5g=cWIC&C*I6FQK{dr;l%H%80fhVPn{zxj!R z1bP6{I2}yCdR0K_)c~&WA`r$z<+J(4#j#?~8fTv2X90p-g2~RN&+) zNx~!mH^01@Zg7Q;?>|?`-cE<+-t$Wa_$|4QGXu))+e*N01W94(X0SxtU(wMY3)PWF!B#4I2c>(O)Zo1|} z?r|DzaD52}y>sHlzw)OB2($tt;+_sJ zzm4PKiH%OSNFv<^7yb}}?f8zN8}MoM$j>e+ieObC4$1%FRH&l=k=Z}UpTdR*h0Bwh z_JFa$X!8;>OEEF=1LM@+2(YsyfPwQ13xh$xx*aO^3#_{ySfvxxZx+Ka<$km=hsaN}=Tanm z=gTq~|1E)nA`8wNxVifUlHG8by$KpU+$JhkI-;ztEe-Z4^e>i^wU3Z}m?s0@Q8#nb z0st9p2IWY6Um{l`>`%8*N=!FxboZF!QT4YZKlF{cPsl?DB&1DioNOXwPz0f@KIo_tPcxaWo#P3DJ^YRpO?0jVL&xSdyZNNWCJIsCS14DX%zL<^Xy<1c6;E#p^EA< zo4OgpUhUJ`+&CzD@}BJlKVxb#+vKNyGZAp|W@h>_DFRQ7`j`%g-Kg5p&6V^?^<(Jp zq?xo|8Bys-Ytu6~o?_=&M@Mk)AKqLt{%vd?71#Zh3PrllTGMa%48<1v#TcGAXBG$Z zhj#Y-{h~m5h5Sf|GqaWqtKR`8zjcd0JO~NxvZdkn*0*~U_jbHXba6GdO}68ThJwPG z_T>Wl3%7gaXe4QAI8jz{5R@e897^3~GgWO*ps%N`4@F zOo*_HO&){c{Tv%_7KQ?zqftXBBV}g&IC~f;0Kx78R%Q%xFCg#0n!~SYKXn=pX*K;9HD8>ihZ z4LOC}hT;lBN)IGSYoTVJ!mtDbROECSt#aN@P1B_;Ihz%-Df z9TrJC$4x*4!4_5-0K!QGJ^#+mPR6%y$zXuN%l4mw&A|&aL{Cr8@1)luaL_}m3vV?M zhwY7ArOxbdKJ)xE*eJ08MVOqLTAYs49DcIWpABgYz!iWBQQ|g-Gg_YaFeX*>k0qNl zS_Fh5V0ziz+4=G2g#sMhtSSa-*yVK|&pkqsa zwQY#muB{ku)7`^KK*EB^Q!XXtb5m1O&Lr}zM^Y@vwLi~fQiwa3VG}|ah@{Y(AtNFw zd^WIY02T`iWcTlo7R5eE9zc9c((2gQdK>L*OHq;Fz4pLr(~9U~?-`(ceu4h;^Yb5d znDW7)hM;IySGSqg7mr}+HKh!rfM98?U`mDo;1w7bGEMT7X)FcUX<@1ApW`Q8O$@u$Qg zaIM5;p4Zybby^0d=08@?yp(uu7~?I1(3?-`P`$FX+BU>2(=20xV~l4klb$52@ZrhN zGsbpvtJ`5HrZipq3qd_0;yi4vt~WQX44Ar?Jf-FmG2m7QQ>oxTViqh~er$G5wGp(N zwVM*R3qJw+a4NLVSF?T1OS{An6wQu^Y{X0vZh^I&>ObKfZ!_tVmh`$JXEv+Rb1aRJy| zsy8=gcKP)9niqs$vczfVGK`}!-bi!lU(C(t;E1=z3V(ZlD6AKg>w!&Fl;e|!60Yrg zG-b>YT9q`(t-=xc2H7?;DtOd;4!e5?l@-+!TBkRhnNzyA{2esyW*^dH68!_MY0G2l zQQ8-qf1MUr=pQPp4F)Fs;&uzw$eM*7#Y$+Kp_nl9GyG+G;wH1e<`g|K@G*CRV~v+N zlZIQcrj!|!BS8q0oE`k|w}giYx$sW)G%TUq>cwN1r9@K>ctAmW0n6s!+#J$21epLt zg@I8H4O)j14%fZs$PA=dc0c9rjb{-;=^lHXWl~b&9M<$ven`0m&{hgg&P0G(A@}zp zQ3hlRMd+2FhlWq0aM+YVxdKp_&@lEunU$21B4=QD&{wf615Od_gH3^$0GMd;4?AIA z1q_!zD%qfNWp#B96kMR@W0mv8su!K!zu$+QDN$)@OsP1K*@T5N@K{krOP=)X>_P%7v$j?vSPAXH186Y~;X)9fGvL_qO5bKG&Q7 zyp+Q2c0dD94$s+2w#VsWe70LSQUkHEVFrLruCyCwUvMOlu?AmXU$SEgu9&tgIedbb zt!BZh}IX|TDK|8^)Um7*ZyNZC$KZO551T^Ld#>D!2 zuq2^8M~XL+^o_a<4Yn)gqM~z?^s+C|U0oxy(pG*CpUwvF2JPNj{ke+mgO;s)wgsPa z_*J(~SJXxjN}zI421E%SUG*0z{d8R&g22P(#-+CYHCwG?%7 z>%)4-XT}KWSr<~zO?UuUgCF8WQQpiItA|aszBE z!TO9b#L5MgRUKoAXM~~7zuTzvxAGaP6Dl2J4`x13>pa1yJ-@j}d zUI0TRvYV#qRwE=bd^4CrKYv~zcz@xGWWsD__}1)M?e-{|?e^v$ggqavGw@m>@p2sg zz^j?!j%7Uc{!X3PM^=*7e>#gQg;WrPoAdnmw5SOBcJA;RT+Yp}RV-IO6Uh^+S1R;#TvZ(h z2d_t)v_V!^m^XT!8cREc3L9~w!>FO^pH{EY7y%xpI&LnPbc&aLh7zI{+MeCLJtUD> zRq^PX-8&?8y#xp1SxMkr06NTD$fNlL`1Rj_tG}`|ijl}8;dJw}3HNaK5qGn;70W}B zQAfZ1-O7aS^QK#KxgXjyZudveB8u^xVuh`j3*nZ}<`$v&h}aL^pHv=|DE&ss3CxK$ zlgqH&uA}!Lt{4bB+vF$U_*Oy`CV`qMp*n+juT`+NxZ2voKE?2;VC!O#^iE1oNc+ns zr-_p(+dJCisx4-{Fa4!*-DY{hx}$kA`cwY()a0F3l<%Q03Y zRsfHjjtRIcl7z8O7IiXJ=<0xWLS`TnP16Wfh8#+6n?pxiq+VK0p-=;#z=08@`TzqX zqOkB^2y^2PBUVM)`291@+$@P46K8ZkXL&U$AVcT9rlIGtEBb-lRwW@}d*3H1J)M8W zQ@Lo^veD)x+|R{xxiPq~h?Q>ln3%Zd3t>gTa|O|ZqoaRCyT)+rYcRwKcG-zncE;e+ zcorz%BPf6a3pbauBRPONLsu{ZqVHaZGo<#KYC(?497Cx#X0C^wMp{pK#V5mcvVT&L zJxUDRc_QW1&yeDI!`KXM81qqcw86ZDoWws7JK9uyESUh4FP^@`PCU2i1nkN(gyhX} zsFI`U|7qI2;iN3jtweqBaN%#GHIJ`BjyLzmu|G0>h+hR4h6<>ne>|niQi5*NJ-`)9 z)H2^m=D|1|J979RL--v}Q1R-3%MB^RO{}h9%D~#UQ`WvHsISE(U2$!^%6uuP?=U@R zK|x5`#e%Dl+8T0<^Y9tXN0>8qR-%<%j3=HydGg&BLgzuhyMZoOrtqf}1+^VhIs?t{ zbKY~d;Aft91viOv;_lH^q7~z((j{y*z8u|775Qe6F(joI_pyD#yVY>JyJMm){iZ7m z85{sGP(ZA+Z|+y1?i1r)3~=ySX>} z^W(-;MZCp`UiW|NZaNk^`Y8x(TMPpmiuF$9Hd)?aJo(i1?ghQs3-l5{+hnCKfg;H+ zS*!S>vNQitqrB?TO>TC61*L*Kc^oh0%5gOK2j$gOj=aV4jvaIUglt$*Tt~xhp^~tc zU#qP8Lquw0s^gin)HxBe2S($7S9^#m3;o#08E1gktyQwk$ujHXgifTx*fMgz_Wlql zHhj_x3JaTOX7rQVL#bYl5Ki7Md@7uY$PH?Xqw;9*%-PQ9PcgNoTtod(ELE17=>vNf z*epkX(3dte^Iz?TzqJy#9yJ69w56>L60DJAA&=UE@8eKKL`B)_ss4l!9X9B|lCV3A z?=g;B`E)X>=jY*j_R>CfeGZYW=$IH-Q_E{?Z2WO+W^#sk>|FH1vxg3i8nd^82t%aV zzQH!G-Hqw79ESV8EW1PB)bY;GBP^GT&i24d2DrB}n(MWb2$)aq$~u-rNEHkOLJ$Tq zI5W8k4ryxx1X79ATQ5HI4jA3LjpD?xrhLQL#ZT8KhSAGwVnXFu?=3s$y`y7_KYj6g z11R;{sK$XpIKJf^HVQs5bkC)fyum_>X)j zk~TrhT;UEn&WE(-Uw%D$&#eY+Rm%E9S_)-wL`?hb>do(d=_I*-{|i^+v^B#G`I!p_ zhAt9CPc&b&=dMaq1+n>IJIDLfRCED)hFE+Q5&6}9v~=?L#-PAG337!36zEYH7@-@QOkgz=HKwzb)VS=49w@&f`UZo@1LKXw zE`eg+PL6I~mMl|*{@$9?^M4&@ZzH-rR|L;DCRr`$ChCMMOj!i3l%rxAxN&6DB= z#ya1LC_0z3R*YpCA?`w@8tbRta)qB96mx?)5$dINNkt<%j8j`w3f{Gkw^QI4*(M&@ zZ~A_jYR5YrDt#-gaA?lY5$zx(cwP!h+C;ai#WB zwxW{5k~?5d3CjiE@|0;6!{Sq{uCph>0>y=e$eeXH%#vW$KzBrGd|8Lh%gd{xs*3SW znZH0`r%Vd3OBVTE^Dn%*KwCz2UMoQp<V63 zHhu9x#FwdV+{Wfj{SxvPFXOf_7oK1gj%qAC z(U&rmrbJOE5?|_kz-pRCwUR_U;F9KL_c8Sh_Ze+T*m~)uqfD2wI51WV_NN8CPpf|4 z3T>W{vr>A+;#-o&6t%W?lQ2X&?3S$p+4VG?JaIpmPsYZJuBT~Ne6!#MisveYp7lW< z<0`e@6)9CrdtYCopQSh~_fj%3Sr4_-Ff;ct^-W_e*Mf{vcX>2{l;&aC?C%6L^0xws zNr{oJk~GSTR)-xH{_C%_Rq5_^4MmI1CCRDe-O-X2(ePWhR*W@IeMaMy0vD;_=KpIF z{pYw|UAvzrb@=*3Dkr~41@X^I*T+FMH!m>9aD{yor=SzBLFIAxG9}A051+WIIS_ft z*0|Hu>IF(SxtD_rq*S|NI*j)O!*^BO?GKZR0E+c@oPe8lU0~&V`)ho5Cf!dCEQ(}i z^f81%5>v3`pq3~ozc(cdk-M5gy4{w0Yu6JK<36h2zU+cFQQylN>_Z+w%QNh`4x{$A zHrT!*3MsnoWbdiRPnDEzH_v&DpQgU$Kp8F(aRM*dorCgKUY51kOec|=5?VYOvlon|nOSu`!J zx{GAtLrw90P2*1vs-NzKiW!cixs_;?FFd4YCRJekv^Gf1hSkeVj0u({`M)YvSUAEh zOvh-6o&!Om8^VS1s$`0A-{2~3rF!_-Qn{Ec?vbJ{{RTxNc^bF=_g~iMnO`N1T?`5f zW71~S?$bx*?v-^ORI?2@o81Mr;WkySWVj7SJ*lb$?$Y!T8G4Qykw$OV$6JLe>**vp z3rhrdV#PVTC3r_BEBa~TQxVcc4!w?rZ%X*$eof76-16%VS2cfqH;`7^VLs3YoX602 z=Rga_GZloPf?{%mK*b;J;gcs^dE46n+OWUlM{8+qg$;6oPELmj!Nmd1xdk##aq&;` zDj(*(2&ZSp6I096lPVm_$$IX=tt*`8eCDi_z_HiNe($!q-+PX4bo{Z_+_pV8Z{T~? zKC}%rI5JSkMP6wOG{CKlqJr6`Y2M%L-t|+IhPLwqfiKDW70d-|LT5r9S|tiq00bv_ z5rGAA=#i$g-h0{<{6vQ*PR|FK3e(Cz3P7c2kIOG4K zBpK|?K3O|z2PuGksMdA+3R92qAtkh=-**uLVxazjQQ^xF?_?RtWF1ZuR;PZQBHf$^ zDc-DA|C9QWu6JCkUeF#>J*7RIIyAlZR`YkGii_`Usdw}U6QTQBu^oxtp<+6MQ|%w0 z%w{E2G>nolxH?+zL_aniv~esaRJBa0;;#SX`0-@z!JH+@CJmiEPJ#7jf-sRn#^Zn0 z``;cTK?Pw5J8`92%bcpu{{%fZ)x;^xUw}9L=yEqpiHX!`n}DIAck{A@CIUrpmvdT( z{=1&Mq-2ovJz&RF;EHNpsSFbJGpR+z8^MG6p&BHp&7$iror^A3cg4*ckuvLV>RbSg6wP z@51#A{xzib{G=-7THnrN&VjRtZ zrD|vv=kW)(HS(REtcnwk?~E8UzbJO3k|-ynRIV|Hf%lxJGB0}Xw?%=zzvvGqoH$F3!YESV81edeFt8F%3f(0iPm?=yrXCCja zIEotZvRxr2Rp771?5A*cagT<&YTd{AC%SWLIrVns6aeHRl=Hv7-=5xfqp{XmK&mcn zdK8I-i8->;hTyFceD8s{g)>9CedUS!Te3fN4~sVK=Njj_@&(EQ(mP}+1m?r#@p@Uu&o)XQ(~4baxw9$i6SOZzuoA&Zn|$7Ahl z^bKIYD+MRQ-+RET98_Cfb@HvyvFnfV4~Q(oZn7H;8bLv!Z+}(sN8KBOwGA+LV}7V^ z@^76IO_(1kCMIR0>2HNo{I&ND+qrS&Sm?N`G-nbDdV8LVw+ydL9BB7qXk+H zJj*Si>vxl8LiSKx7K4ls9EG|4(b`*_!iru$$}|hI{wXi9IA;E^r5P5DW|_ymS@;C) z=wN&*A1z2|YtuCTeIZ)VlPHBM$M+d&F@BTYJLTbNk3|u^u3>N9S<=F2c6;^Xy@9(d zb>wMycjF9M9@Aj<5?6k2p1_(p!Np}2WHL|wxQ2%vmRo$sFg?gCHNMEfkh*p8kui<ZhBle-9aYHAtV1YP-Sf?XK!&ITQHSTE9EoWQ2NwH1fkS*?k%#vVqsReSbYhc@CV0Rcq2IqVlM^nXM8j*DBpMMpQ1 z2)D0DIw;{iLn<1}V;HlK^at06t-Q8`n8o~5JX=&N?EKkrtD+Jr>bHm)dhu)R# zLK%hP&v3hO(?A4R;eFc3T*@S(=!TM5ZaUiTvwlxy;$kp-=FeUi&QYu4zoun|;^kz| z-gJ3F5*CU7N7Yw`MHzkHlG2JGA+3N&gQPS_4n52O(jh6`(jnb3bi)8Mlqj8|gn;w_ z(%lk6ND186-#_lX&x4=%gy%i`ti9ISYokB#h}LsP!8j8pCwA;a9%viet7aRtibt@3 zVXFWfu&nzt%bPE+9w+zUyNN^tZoMnZ!ArhEMRf;?HDbDGNNuo8J6oTu=_3GH8aGgc zRo&9#F0VLm9M`Y`pAyrk#ohJZdalHl5-=q} zTeCR4F>iub{qU(x2_}PoaIoi*jyAVtlf&rF!zz-V9XxH;=k#B`06L2iH%yjTA^+Rg z?ZIRT&z{GO&N58k?rM`darINp0Hyr^RAkg`K`ZH%N;1b zSUlXz@H7QoJhEB$mM>^cOkYWNjjx4JsRK-fEG9h;t{3U5<2$Q#7n5c=SnD0q(_zjh z)+J1>kE9@PHOpYNo$!d>=oj+9`I&A>F{``eftzy^$5gy5TnA_y)?a8X3I~eNt?G+y zzR>O`u#uku=t~|@P6KTiKVSy~I+_u2{@u9$^e}*nL}L{8kCXB}x8J>|`VtT%=u!p> z$2{3EV&ahMC(_%l6XQ_re^Z!W8Ne9PWS&Ury>us>C4k4nA{F>%2r$BGG;xnSqMn$R zp2I>Un;|Q|Lrj%aOW3-6FMF6aE5FS30v4rSmMC19!M zju{e>PNL@OzfR3q8Dv>vl(Eu$FJilvT&APnO@pyfyC5B}&#n#ab% z(w$t%jc4S@LZs6#>Q5k$!z~gm#z3s#(c-!bOy1xDo;?q-#SeZDZE*Hnwqq>0df*nz zj06et_SW8s5H11TS5q)taLs=nl~I_CV=DXA+;>xOK|*h*bS{Zsy~|H&+{^x>iAYFR zjEnWYH$}*tp3%n^Xmh5M<5}rT>0nOhvDtVPddc3A5neeKaI)d=6aBp8igEH1MHX$x zQ~lU5_t~sw*vDzXP`_^khXmV$79l+j`69=eTh7woMO*1@|$uGh3R(gPdEmJ{m=H=Pw~=O@q!%ZGicvmn5d+UV2)18!JLH=CWU z9aD$G4;58s73ONz_{DK-Tzw5w9SGSfe`;pMPKJ}aVm{W6_<8-FVO~l(U+3slB<8nX zXKU|Ab-JAQ%d^`qmEgr@>XpWmwpEb7=>V#T>u%%+q(fFy!I%{@8$)6$Ut<7b;{xND znpxpQ2=!y@RY*Cye)vWv79)nRN2Vxi8PeV>b3l}up9g*&Ut(N!+?Zw+Gy9E11^wN; zuXEca#7LkM|8`QTtEKmnv?u+gIc9S=XXb*8EEj(o#}o~<#=xE*<^mGMC<=JY09zS7 zRrIyvNHWoM@>(A}8j!|4pkhD+P~$?S93V|fMWy8JEuyYoGb-sC_kolj)j|d^xTAnQ zNOi7LnvCw?md|XJo;!IDgf#{GP}SH|0bDd=%RHSFJHSR;e-uj{8xZu8_K!YkZS`6W zW0z${Q0vr-dk;~M^Zpc3_0f$D#m-qO9(JjI?AW5zyBI>Q2Y#wP?@QRai#m)xtYaht z0KpAZjK$QnB4o+H)Xqkzjt^3gf~>GuxEV>5N7kWRL z*4)?}hD>m+OSLv|ulE~8&+>fPX{Vm{Rf3=w2wpM2pmyQ>{3R2BmGX+2T^%gtL0v|0 zXMasQPcm;x&A9WLob2$YXONjhBCBWN34QY*GGfC1qPyrLqxn(#^$NPi~Y<2R2h z5|CR*eq0{~pupXI)WAU@#DILp$OhDB><*LBI?H~e?EXFFLg7YY-BpAn3I`A8k!oQ= zekcG z*VkUVtyL`ka#F1^b2B>sWCgZ-y4(^sObIZEPoG^s20|$ib`p0P_ewq$pN?ma9Rx=HIjzgms(U0T{};z_Lee{(jhtO4;}t zaHIkAjxCT@Ub2b=>KF%a>*f1rJ#tY<6t1-ZP@uOn^KeOFPpj1m4{eNUt@jBabil@by5@t$n1u^9M~GI*c{3G+un+s z3OA;DbX0%Id9m1tn9Tn@>ohr>|7BjRVbZbHZhBYPD-{*}#gpuH0(XD1V{{`ArDqXK z49<}VDGp|6^ZJdYXxMx(4nAnjYRTP0-Yj?(gDzBHwEgI)8d)J%KE+;Bn|Uc!r4f=g z@ptm*ZYrU@z?rL3(Wdd$eyu!ThRcz$vs+6S=IHOiKVwVFK3_W2lA~7EvZ1&^oSJ$;W3n-@QQX+0VBzht@Fo!#YscvjP3I+* z#WL&t=e*MkV1DjnMGFWH-mc?e4I2*WP{A4HDUZF<*30G; zQj9w_^xNzg5s&oQ=>G2Z0i{*}bDLzfwpN`CWDu|<`g=0W6X<&O2B-Xhx|DqE>ZyjQ zKVw&6|M;&240%d_hnAvPnHbs&k4?L~(U1%kh@;h!40FR5xP;5+yulp_o^1^4__ro( z8;>;2&jV}I!A88#G zyCVOZ3HG+d5jj1Z@qONUc8LsdKfx0sEy>V?Q?a3#^&2KElm$^Kc)Q>Q*>Q#@wYXvi%=9IgfIJq4CJ*GF=7zi zS%v8s9p)n{xT7du_5nGS8L+X{PoKtp8_`NRR5!A1lsT$o(qxVzljgggUMk&<#CzC1jIs) z_|tCB6*+Iem~4fu(M`^-p2}19q!tDH;ixJ|CiHo2G0UzE`$dcVUV)_ge5<Bb;^^e$^|pb{J#r>dth=oetROO>z7bDy7iq}*n)VQ;dMh=ni1l{JO0b|QciQ%DKAEy z%pT|cmF7-h?Qwvf8@!KpPB~3?Z6+s!I}gBIt>bV(v05!$tkW0*K`1gkg-|;q*?_6R zwZUqL63DgC?Y%|yCxdVuKkFV*9p}Zt`Ndc}LQSas`no*^irm-Gz~9vS zWU76!9GH{;LpvYKku;bcgs19ieZk;_Q{<8F{(RqJ+OR{{2aUo&; zi^T8Y88m{$&S{^Uk!HJRh2FMG_`{a1hm+BRas&t$(U(n+nwfS#JTvV1J1O6H6Mc4J zS>a^T`pI59z15MLGVfJt3D24$xs|7ekp@*)`>D|LXnbxSS4D^I8@!%z0`Ic^$U*gt zOoE12uOQFA>x@>>=N0xAv&%BuD*qhii=-@Qp<6yvrjbzh<2`@ipTM0t@ARxnWke*; z?sxB(^XE4jN7(X_Rg;_Vv_yWuf5F>V*sGR~phe*ITOdC4u;aqy-_rX1j;BcBI^bNn zS=M+}@VQBeBR_-6OYb`ZdCUCB~i<6eV z?nDZIjNwj6fk0b+B^HN5pt#E66{;`Ey&U_1TASW$(QQ%3$H%9I@_m&zSV%k%zw9&s zHP!n9Ni>rNR7*Qz`Ql z45VvdTDF_`_mb}?64O3r2^D@HEBtT*timL0gmi$T)8Eex);|_JxS06ukeYF`c6#Qx zUo!hzzW3&)CsiEhqf&vY;VAHx-)){hI=$Wv`i}Z^X`-T_new2|sF|4nE8i9C&|D#y z#@Uv6shuj0m74ek@dNVdq=G*?66et`-L2f)T{G}qfR{MoEuQhV0+pk)vGepbxWS-H zulJ?iE-HtHB#iy5J`6bp27Hvv%*^&_rc>EtzM%-vrp~8!PN6yli4he~xSW`>r#}71 zZ?d7QS~O~so-gRjYWk`m`im3nIm<8Fq}+QiS*i9`XPjZi`Jj30)A@H8tu*s&)!v(} zu`!<$dCTmLSf~qSFNM(ot_g|hOmPH5HRkN>y*KVHV0tv}A)yhAw(gtd>U&-P&OMOv z1&7zey^XUc*kRbO0Met%V*8=%-%LUs=q5ApkZM`ajskmo3MrKx`2cIiV|<*9dX$p{)*$spIaf2aM}LjD>dz~kz5>Z=nHx}@E<#?U;Y zhd{A4j=v17!v(i&6$D=F{v|49v8&RpqBq_lw(i~Z%?tL{kSN;yb-as7bC&W@%GZYNrs%4#|Mu=vi-ey@>#pTTC!qIOLb({9AR zj9G}H5G%LSBcjH5;PI1FG=?h~9RM)Ix-Qk5_wm1#EQi6U zk9vHnX<0QJGOHV_Z5e!w721#+JU#*uO@M#SAJ?fJJk{i;Ai-H+W&v?V9dw8c4_K01 z%^KR52Rac845UtLiS`im698Lt94((8!|q0L!Fu}__6=#fpj%XNtga^> z1UrGR7sP8L4z0R!k+Xc&ld}IRBEIr4)7Wko^5shwoAjx8n)sj4v_#j4+hqFe5}(ox=(5#TI;v8vMKBcYI^C};jc;hCY&KCR4p_KY5y8QD)c7H zN;)S%cT-UpI<VfRDiu5iM`-kxQ?UcTF51s$y11^)WEQpb3V%q zwSW3*eOOAFvmqV4TJfW`>lc7^AIb!r2uK$`x4PRMTIQ^h0j2cW_m25uTkB-4xm7;^ z7J%EW(9DfA_Hq7R3M5uEn?a|t9j4e6~2pGoLc3*cYFrt_gq?NE%+ zNd=2}42*BdZORgN^q0J*y#nvcoed|#EeLVOaJR`DCzzm7E; z0sxuaK$TZ&tZ(I@CcDY%@}w)-QMIXME?b`KR{K= z(0U`qaCNz6D}b4w5Ev#M6|+9*G>)@*fB&ZZ(*1YfXmNAC93+Yp8gnsPzto2+V(Z22qf?W7D z2Sh;tAp@M(!9P7XI4P77=-a7eaJzH{r)NWq6A&r7`J7^gDCZ&&m7bbkhKQnf#1Bw})oa znd1a&fk#h|w!h7KXXdGlph2GbeK{9vB8J`ia&O-XF^!R)xwxZvl~{1!zm^@<2i|Dd z{MJ^ruej~WWA%OrRSwXC)D-l77_T-VgO_0VW(#Mv#-p_n&?q96{cu9->HY1L&k$X(2NU!)Qef>bx8XQ>PS~`29Ql%PT>3*oFiE1mHb#le#u@ub@aUTn?pR zHPQefDi!Voa)^i`DzY@Jv?30&WK80ZsMz2W2sOPu`;dHK&$6~LH$z(O1DS)F7wd=F zt0scqRY!BxU{m&1@7Hy!rZ-iy+;)LG>VA*6obF5|a1X&sH;88_se_>T5j+r;rIuHX zLMGj_KH=TT*4F(X{CHwAfFBQbwsll6Og{Jv^JwV`dE?!seu2n~84`*hvk)McH{vlc za9Wwjnv`E;rlxg<;2_B4&+)eSP_NmmpFBJJn(3opC>BE0_s0JaeXqLR>HbOrx7h>! zteOw$Y{;0=bgq5KUA|zb5nGw%7v#xHg9CuHFH*eayd9c+xsMA(GVz@{v?bp1#U%ex zW{K+FE*r8gX)lF%Np-as2_N^x|=A9>U~Eo`Of)%lj2VkR84dB!Zy}?yFVmgpZ-ca(KB&NIPZjNBl<)dkcUf1}Pp=x) z{~c6=vLrYa%7bOo93?K?G++8tl-1Z(_tBvaQ(L65p?XN9T&Wx+ZGDzEr>;>q7}3`opVph&QM;=aYAYk6>%D?Cc+uCr4j2rYzz8A1jv2br|tfanb?%$@wSfjfA0(qB5KJ)cB@8J z`RLLxk&~~#23@axi47+#CL=(<+X-h}EZB@W7nSfh`aqReXw~+7a52Co#YX@5B=!6y4sD?8HW>NsBdsH#W9vW>9Nc0p{y<5nZbjmO z62$|hhQGXP6hT40`@UBL!_t`YQLQ%Cj|vK|B^J}26p7z6K4*t%EIy9qe+k5_r6DO) z@*aW2Drjupejdj7|lSkdAN{365*WRz+5YwN<|nVxFzm zTf6J#@@TB(A{bw5z5nvvhE5!dTafOS8@y(#3UY9aTbJwbxEW}V#h9G{nAQWYeOGo~ z2%LOrjS2;k7N*HvujZ+5?j8sVOB9xrhjJu=*QaSsf$@wY1cLpj!1Dz)u2pb$i!?$e z@ErD5Y)V*oWILh(zATyQCiwsQ>XfB)G&p{nGBu$N$SI!Pe z_XR(pr)B2n%qNAU&!R_~(IYHDidjrT7;ywxu!E)vTlLO6bQZzW!X?j{$5MalGA26j zM@`#c&Vk$Hs`*Jzaj{F(i-*4d>zXYc@cb71L>DdznZ8Zx-#7wLi=SF84FN^LeMpj%|E;{hOyB&C*wuIFL zT7CP|*QW?9eT=TU>?GW%O-KVeMttWYCZPry!Wird2HE}(dANy+3&Q43GzTqBHFhDW zqPFEN;8E4c>AuR)^sKVILy&#?S+2$bjh2u55t{hpOjM6&=5vQt+yS>`vyma18$y8t zhJUCE2XSO`@R9>j!IqZt{rypRTVv^I76Q?1?+~OEDNLSnCFWc^Y|rt3*HouwXx=QK zpeVN)VUgJD zGxd|6Td~0I$ftKXdJnld+ zyc>{WxZlB!giusBJk&Ym&JSf#?$wvGi1itNo9RAuIJvoYyXunJ$;scUd6;I5_!?u5 zd~y^CH@CNrlZGZ9xo8`bt%*}LqML3TPJH#=2zSK?J7LC-$f;C)j7i!VF(S}%taaWV zWxNy^92&F=zV`TCOSDv`re_~Uy4j7r1kHmHsw5Grwt+DpAiW;X-v?j%1MuxdS{4>J z1fc*92Mxel-TO!Ct&qay!mRSpY`v_vgr)YDmB4hX4AWN>YJxF1>mQe9(+Zio-JN3> zSXzFnzjCVM)XL2oM}0OVCH?P1z^h<86K+xWV#NhlVrm`Tugni%Auj0sFLvpT#MWQ> z7=ymP(#uK%$4MtgJ&q5L{y9u~Ix=`MI`XTz<@CALrj$nBlSlFv-J6eA+$oedD-U(V zUwd54tC?il?8%+j*Eg1?MU41mN(6HOFiwl% z;^;mwqNC%(&}|lRj8){K%o7iSEt#mhtPw`*vv7Z|Ed@%&IO3_V-^9i2>*X)OG?578FK};5xu!uBvg| zs__li0sv_?c-U9(0OJpK1fz1mGCCybrW+hI1aZ`G6pPA!ZvsaS!;#3|M_sH;_SS-? z1PL5K-v*pB2;l*8%jez%8LJ2TD!gB7=Xr|esh1%vgtvqPgVLe-%qn(wyY^3%tvE7> zgoOHdyC?M}tUhVh2k!pM<5$$(1+CiuZTpI731zrOAP`YYYM zcfEk9;PX*wSnHWv` z`hu*slp%ihA&}zwP@hv14X-FRU%#8)4{?6ADEIe4+|Sw~-$vrY*|47VS@?QPzbdYy z-pW2u9``5Z6Gd*|2la?&ebFN9v1oj{jHOr^rWIj{mvT6HPIE*RJosArQKlM)3>4M` zBuR+^YigxT(VJXINVQNkG}AyC4S6dO6YZmmMlI~X>L=J!$K}<-^=W|n%2+^MUfZRe ztF$Z_mvR!003Pw2WyN2l8k~816~8fl#GtYL*Z*8#J}$iT=exCNo#2{)f$)j+!-R3- zlNuda>&Sj5gP&G#wCMah+r%RuV$$t`Esi9E=mgfE?Wuf|O!9W5#S6s*CycZF>S`cOY;vX z?04VCjno0VtgL18b*NaJB&xz`ZmW zR695>#7x-_-@HZ)3f+6a0U{>Y3P}WgKY@+aLvF`^r_ZG(Ncn)Q$9jQ#puY1Gnxy=B?zGw zTx5jsQ+4o+Uy=Sd#kMBphqU|1?9E{A93%;lYQPVDxj05h*Zix#+@J)|J+O>X2f*qs zYcX`=nw|xHZ%*A>AbYoAVPV81B3NeOSeM1HL-W6a>td81 zL=pivrs%KJHz9ukCt`8($NZp#*g{dDglEHb09Ky+?rdf)uGv*_Ay-%^rr)Z9>D&M$ zHp-*eujxf4h$qAdu(i+>VG{XaoC-i7*-D?>eP?FIwEtbmy^;Ohcdvor_Q!p)_4V~; zud=2$8#z@t@=`$WN`0iG_@ZgyHgm>1H zVxn`D$01tP+_JgCr%SOCUMlCWDdTQJ&M<%^DpecMC@i1U|v8f_v{aDn%6wgMr3`S0b5BTxgxsnr zl@VAwz-V%t_-&z9HY^9J_Ny~_?Iu1x9+>Er`0O`WzsF!d~Ck|H4ARgRRPlpe5vaOmY%x zqJq5NC*?)e9PG&`pEafw6A^K#sq-yS6?tB8iTtK4+0F1xVS5M)*X7jqaESY1cEW@N zep7JQK?H%|wU}RUQTfsJ6rZaRVNqF2%2O`?{D&Gt6D8Xu{Ldd%pV zjyv}Fx7M+YzZ-_&_$f9`3eQE%Z}mCqEFIOqsI>5xX2`bjJtnI`ytbeTpb@OLr&y2R z_V!PUkZdXuftl5MV(mVh`ss6T;12g1!Mn6y#weJypStWsQXqB5wyLo}U7uJ6Pa+wYn*2 z023(WG5+HQ{7YNGB2B`A^3WKgTD|^4N4xMHWXO=nGj1|74&YC7vKk;eX%fRVg9W>Zj zIKH6IRlAhz1Xi0J37Dib5YA4w3wqTt5=n$P5SXil?Eh|XJc>Z z#J$x15fhX4)!h(Qy*+=ULf+>LiJSZXmy_5opnA62a~4k*c&iJj^q>nB4JbjG^@zHo zm#~1q{xdPIJI4$rTk9+om_n_dS4ud`uoS)D#LH8sX(=}!XYZD9X!RnE*&^sD*QG6-mjQ~5!ybe~Z6 zB%g_(HvB)}+F((GG219!E>cgM{+SXYqK^UYKNAy3(Zg25lCZ)fk~SMe1>wozY)2)S z2}YnLeexriZWV95?uNH^>Q&kJ7;2*Lc!!UOh{*1^zKo-y`H)%ZaDz>W@!iN&WRVe0 zVT%k>nCzRuT=0wMcN)Xj?R4Vv>V<<@n^KO6BRW)?tuh*Tp6v~0W>P$@mh^QP^hM@s z(JwnZEuK_6s^wnkGA{(%w57*lkCNxo$=KAD2))H`iv+&NWQ*Iay%!_5CwRGHCprta zHfUmF3#SDI&v_}1#^{@|x}ZyE)IleO_x;=74H{~2tFyb4o47p52t++y^^%O>lhW&m zj@;W>cQw;pbClS?Z($O=2Xmx{cH0;E9rPElFYQ@Au+}`|pTvJA0(b69woN<3{8s4l zi(&;APcU>cmU%C2+S0_fu_gGV5PcWW>KFi0qdrt1A$7w4h?9pzclG~m94YOF{P2SN z-kLn{(_Hx!6Rz{3#BRXn{<23DQ0a@b>+l7pJPk~Q`vL6|?p(-E@(IM(u%MkV>o2`n zzJ!~CF=4!_!I^qJ67I<9%ez3~^r7R5gCV+5Xs0)J1uaSwAIyYri%Xv#m6y2i_py}c z4fDzRz58g;>JJoc%svPg8JJ`0$|b{3Mk>1}0yaXB$fYHU{Cs@lkhWgA0Rj*mu9Y5= zxl=|-S#ZW-3y@wU#+{lho96!w^Z~eO2kp{}1qo(Wf?Z5jas#;Rs{YwC`?5+c0DQxc zK=^eF1cY$lqG;r8@|Q%`)$tE&=OE+p!I)1glxtAw=4hFjO^+>{3Md71r(T8B{l8kR z-5KJG;seg#UlIS+ZczXNe11^2=6c75Tzv00mNR~`Y@8$+B3n`smd6j7xeNr?AEQ@Z zPkGW1@f?hGpw^QtZ)tKI2m0pypLBX8|57_CykJfwK2Fwrh~a{c3%fr*dl)#S6XA`H z|NUzblkP2LK9WH9wSphd#5*wx=kMqfPzv}~i}X7^Uj;i#Lk4XF+? zl|#Rgs2uuLj|?>qWlvAmU#DIqiGSBh0EO$79rZV(j=UtlOn_PgE|b~I^>1y?#p8RV z^#)|ImQ?mm5;D;xDZw^mfUl_yC}1RhQeStnn(*=DBclt)i$x4=f4Sr7WM~Q;QOchG zJ)6m1IpqtC43AzSM=kjTqlwm3-f@wYHFl&TItAR?Kk0jp z;MvkX-5$k>r&-p6*8k_5vwF|n4SBXR8gn_?A6~VF0GKaw0di8p4XBvV`r|}S0IR29 z14VFp#-td&dyl1JgXDME05~~=-0w_Z5q3(lKqahZSz;u4V3m}u%ra)*b8#?o+vIXF z+=NNM({^*Ubi?~fj3?B<5~ZeJ?yt7}<%+tJR>!CjY^5Ou1q>}fG73rQ7T|4Fr23?o zT5A|{(RK#QY4iZ{zBC?`V*n8|50Fe)({jL8SLF`5^XqZuN)r}n1w?OeYdj*X0yL(I zTY&^ap{#+Q0(Rao4rV)Z9B_jDr9H2F6OU9u0qUSvH1TTcj_r@GMH&nMnQe9*jj`g< z(!3upf*=0oY?{^F0@090sN~xMwhl?NZl)SugYEmj1M;ipnt?z)fT5QL_!hcWKhLcK zxj+nxuZ2yd4@W~}WlW!U267j|F!sJ-MJsqC`plRlwpsb4vMFarl}yyhjg2z^Wqr#n zD9mE=z%AyV&w^*|?p^8ci%zc?!=uNVpLz0KkJ(IThh`CEAO7s-8#4snOe$UV^p6Z2 zdzgJ4_+9lT0j+u=d3#1hUBFKc`ZyVkAX5g!Tr#t-o*{wNEoK+&EX?mfd)51!Y$EqK z#<#joVkWWAFq(|p<16^P$3}4Fbi#u5UkHgFd|z@C*fjJmoZ3aqdvZcWJhY}-Bd-_n zcK!BEcWyT?>Sn~)ogJ*4{FAtc+n!3^JVa++ei77n(C4t1zi<`&w$Lo$B7tzta zb%)uXGqV1)^<{=e!2L>&+IffOaKh~?dEA%p+e2norPjuk1Ep73S$Z$vZ+`&%9;YV* zk^|b(kZxxGtXC)LA(-5-aqZmi@mATS4DyQ?tw}+KndqVxQ}0c+ZY8n2z+!m-cD( zttopfpIM^f`0dLC^%MB*6=`S6F{knuB^A0N)Yzw#=rvv8UUs1#cA@^Alj{ECFpjFv zoKv+TlQ!h{4Db4GbX3cqUq~fv4GyRS7eGh|_5jQZ7lXj(hcKs5XU8CqhS*ZVaX{@= zxpdkqdi=y&|rL-JMGwgp|Fk0u+A za*4HgJC9b{G>-rs57t6S4D8C~k;9h6K0nyo>8EmJcN=oFUK3 zgV7^~R4f}T7FuFwzfxxBy-CX$);`QExj~I=7UUK;FDqS6u#%ahZ$P+Uu-(+7eD9Ox zf=(dtu&nRT0sJ{cdTM=Tr%rbd`CT**`=m9It|c|YS+x1+LByUN#EQgF(v4?yP2orjt^x&!3 z)dSy>X5L6_sllejA2-rhl$#@7eIuQ3T%00WT&WVStsCnX1Aq$hW3~BEy1W(_~wg+ z8X^T*h)`$kp8f9sr%+^18-yW}j3sb~+Q2qKVn*0h6Gs2MwK9i3(i>iIz!2aFxMFr` zFPD9+CC`j1)C`weL|%UZLa}Q#wMBFzN4E@NFa4j@pyVYqB*@AdTH+HIPccTW6`8t6 zc{}yQIdlO82myDmY<3Q1ZST2$8SM1mJj#b2yw88iAgYKYfvf|C{OR7fH+QzRm?%5 zTz!!RL)Ih!1?|JxxDr9QoIGV@W%)NRxHhTu&FrXfPiH+OPFiOI+8ck|`fR@F?%aFH z9H^-Y7ftc;?y2){J|VN;Dv@dAkIhm~z6i2M!4pTA{4dG*w$f$_# zER@W*G+`k>is!vs4*2Vrn3nUixZ&crzIoNEDHOoAOKOea;U3tR9D}8~!M&1p+9Mn5gt0aW99r$AZZsLKTwIbw zJl9le6f1kh=SCNrV_WJl2b&sJ8AO%BPZhBQ$bXJTu|;god%5iXi2c_}MEe+d<)%+N zus)6@dwXp8vW|b9Z2RY)8f%)!Qt-TILSn|fU+3}kcZn~XPw&bO4IxbjmTTYq*6urw zFq!aa?7bJiU0$sZG}d6TwrKwAhNpD@Kc6=TbYJd0ZW-ko2sQ262E+DwXpHFu6GFqW)B? zOP~k(*}sS+LfXbF@4#^;r$yZ-*kMi&AS;=hjhKs=f+ zqb)#6r}v5SW_Hl^BTlZQlwi0SpM@|C7}XuOKN|{#1VrP8Df}&|#T8t|ulp*Iyn>Z% zmgK|r;=9h3-&5k9s%chPB)ACnWHrM&c9VSHv_@tx7I=7pNE3HT#oJ-;JCZjg!+-mV zyj$+y4B)44Naf{ulB)OUE0=a@W@$hG$7xx^w{|=L^fAOIG;M9^f6_DO%Z!o_{m`a4 z6uDeJ{QSw2_c7d;bxzAeb_b)E19$iR-#V*3I(5Sa*v7{EG3Imgoz}&FU;kLY?8A$W zpyY$4>#VjXquu+XmA(u9ZV9V%?;p>pE`ERZ*A?hV5KFEClX|{mJ7Q7#h;^U zpVq?)%r<@+vaIoEJKw9n80k_q^NInseJ;Ba6aDo%FF`eWKR#Al%b_cCc+a^ST#yu$ zK9OCW>F{vjmec*Aga}{H)fMi)dWfv(=%6ncEmypPA~>|(6O?_MsDTK$+Eek`mbHQ< z6KL9{c(+z0UX-XLT!aDdqx4a~86O|xujl2q2H6phaY9j3teL?x+C(MWLcQDRrmqdwE(ch9@>AN&g{zrDGjI`1$A^!ixe zy9O;Dy}k57jxDM!sNSB~RBJ># ztK3t}Qf%(PeZvnG-(?7V>a59_7osu32y!{P!luS@Vfp(i{^qc>)sYuIW$5a8ANu2) zM93^Va^9)TfBxYhO|~n2P`JxiO+h;OlBu=d?HPnN;^b6Iy&|vWB`BX!#%l#l$~M+} z@rek3J%cgi@83%U8Ma@SZ>QgLEkIQIbFL!aF13ZAdc`AAb>ejv+ zxxz3#pj{cokp&2tR{OoZJ@7LC1S;5ae?`d|A+jFd@f;l9f0H8guR#A0|Nst z`5i7^#x1V}j1d5k1AM#v22KCbg55PW^4J%rqPF_S3byo^d1Wjs1 z-bV}@WRS_wkLi)#^)QkzcT(B(3@hE1yC^O+=W3X_Y9N_DHFYKSn~KNrZEyYj_D*)W z3EpDW`yV&9UYpFI1Ba?14+-}(qEvbx)xcX^1%r?!~>A`Ok zf8l?f%O1P*FZU^U!>He>$pp{53Kq{sQ~~m>hA-4dcxx33)~U z7T7KTow{#tcK|jI_b}3O-;lj}tR^XE^O*JK1|Wn|ZZx8!BDx#~-g zezx^zwf>cw46xBZj$y+Caw*?ko&l{up84L%&1JcwkK2g~LvBD)SA*2|WuJQifcNMZ zpVNH)tn-ijYiHKc|M3Dy^!+sB1%7E`{i(?;7*JU6JH{&jR>T;(2t-}(NkE`TU0L9D zw3k?C`6xNdp!Pekn}(qB6~q|?PKa^bVtMNLO2dM!7{GTy6wj0XF6wL56! zhi8vA7f*S>0k7@=1~k~4tp5_&Tck+F+)PjY3+v%n6kd^P2upz--4oB0H>v zoTzhj5)A;@~%#6sZ~=~M+4!-{6azvtLrx@F;AySR?dC1a&ipyNQJ0m zNF>oDWL6EL%uWpFo#D*+5*Yq=N0JifKNvAxhQd>ml*jlv7(DW8)FKl>pAD+(mw$B@ zVdn*i?!`#WqDgvuV7CSz|OC%xsBs{dm-oReMbh;0o`n)q?^Llw>bi2`2! z+qU0|!*S>Rw^DL8qgz}_zK2&YUc4Aq{Wb~61`_w)&N@02^7^GyK+_EUXYO`;W0^Xh z&|=Z%1I)fFb25F=r>nQebr?@-_kWIDRg%(4zkzuFnQ>SIH?GaLAog~5&D~xR+_rHq zN*JDVG0CA&_pCoS>;O*9Zdh23@XPWmt6I-NcQ9stu7Ap?Wm z>=YKO`FG;~638(t0Lv%BK*88KlLP10R*@Gw{2xa5HM0Eo4!slPek%sG|MHKEr6!yZ z8O!|QLzo``CnAqi=)&%{gk_);c}0`|5!l1UOG_f*>|35)6&w$iftL!VhBM~ zP9azb;J|O~7G+U@ZAcWbP?Xe{@xlR%XZ4N#`MK6!C)RW2oZQ?};0?lK^mlpFf5A%O z4jB(R)XbLCH)`Mze=E*b;uK${=->8A75mDH(SsRQ@Ni!K99o9=8?dc zoTH$7%-5Z~c2W$$Ta>IysZMZhn+A9Tvp@U#(#LcC0+?k{+e(L(2SIB%s+}_=f&+QJ>_R%9s+y? z$ms+@nQY`lwj*`*!!n+N^rQXUqYYlgd#vE{y#kP-Y29A#qhml+{D)Ms5k*C^z(5_? z6XwxjZeJ56UG)=2xXa`@S|jQ4d+B?kDP_qr`2Udg)pj=FKHie{ENlQ5JE*Iu}7NjN5Kj)_(YUFt(v zMA2T{9`3fk5|nJJ2)wqx|D*-cL>v4?uhob0PV=$~(`W&K;r(+)zNw~7?#pwzIE(_G0QbW^Jm(C|;1sPxE|G&mE8C3LZB|@j zU+HIgWCj#CM$i)UoC=R;X5&v|3A&7SMpPO#=Bq8~Q>+_J?A1`jyI)N2o&neYy1xfN z4hF#$jyW?Ow^@(WY*!AC zZuQ=(y0&$c)TloI!UDVI@1HbMleVIdTaL^S5V7j0raDx^K zsIJiHE`&N6MBnHwFphI0)4|TD$mqu92+_J`+_J@60!)GvjxJv@TX!I=m)XdN#X)uVpC)uWTKOl$IGko+ zC_Q89L)jWJhJxx7{-3y(0!hSpT(^H0Kn z-PkfePFvG~(fQ^0Q~qO=cRC=(BGFfbep!}oS+<_ZS`60(RNuhSbPI6_5dF7#ji5Omu-N0cCwCt4p_O zqVxr5(Yh^Ge@;pw#_l=~gD;LuGSirUGt_BJ(0~Y>T}BPKTNzF|T+94cg*$m&rH;qd z)ibd5=$iaY@r~$ukkXpQd{P3PUc;{H{l^mO@sL6zD&eyEEZ5DCnIwQX@0XGyt-;? zNZUFn+Vq8RwJ-`cb(tCr=xNLY>GSD#+ZX-0tajK4sG0i-=XzV}BwC|`8|==TQ*4e* z2R6iS3Z0>&)M!lgI1OCfJTBwdMO*X2@12w9gec043I-`#NrI9g61)4_ETsWnzvEFJ z1w2wN%wlD9$2b(ZgG;8}oF48rfwa#%V*eKQpC8x0_@k;MqX3C5WXomE=fAm~nBjb; zJhEk-7X^vZme-2L8Goch-+p5~8k6b+T5=1j;l?G&_Tl8zC)n7P?g^2Q_PZ0+?Ba#V zOrn8tUV8Pk&lramQ~9j(QGZ<&Pgaj)?_cfKlFrVo$w|mRns`<#^~Kpm*XhmIhR@LI zmzt&ZSUI_s{&U)#9h$Y#^kAL>HmUMB#Xv!GXI+qL{fIIeX$?Ll66%v`a%3Ve{LH3} zeYrjlI&$zzr^VG6-qNYqv6Q(gw^a2wZ;oUObW$#r8a!<5IEWwuXtFV_D*DjWf44id zveMw_ny=@-p8st04Cwi%*)%VIot2yZ`7@<%bqv^F>*$mQzz8=2_EL@1LFlQ-?ZBOu zo9%FazZn^MJol@;bH{cAr7?>;#}5#4v@G%S4AaLs{Y~MT9@J-@jMVi&rj9e~vN{`% zu9~)r`PBDv1#fe4j@3}DX|(5+b<+dc6we#938bG*HXd5+vT446VjhUf*_L5{m?H;a$sCHf*n;d(z`&tg_jmnd!U;tpBim5<{{2j!Us#yF?_TOl z>vN{GLNs^ErIfm6MwTm{EdE&>3OXT~Tth&Q$8SipasBrkD`8$nJB~EG1UWu0kC~bn$`5`@5 zR1P9s$Hqu9<@woPQv!N}!SMdmCL)>9th`Opf{)eFZf7ZSCqyebA9 z6+5~z_+>fuo&yPUnH~)>fa|}RthTROR{oQ5$HZY&EXFfoFULE_GvT+aGMgq#-%E0@ zs44U8a`4jmwvnM_WhQ23!7VwQ&qNz6<{xr)FLcwQ~)7mkQok z2j)56 z)|1z6s)Ff(GIrWUUE=c}vVV2?tmZ615wbC?@8jq~q7@tKfS{oK2tAb@u$Yh?8c0mL zK69i|XI{QL-Sh23(LEsP8dITB5A6x2AzF4O$u*8J6zeu@;6^OmCC=Sl69sg&qEDe4 zylt}CDgAbt{HfCU6I<@qfWwhigDOFRV(7814s}oG>jNUv1N1PKOnjmBL4-Yu1#mb9 ziJ@&JF75E}?8?cRa!#;?4HdlT_vriTFhnF{gKgnm^$SfvYvVs(cz2*s`tpJZ7l|cD zi;Y)AM2QAh%MIIrxxcF5T}&Ele^=IUo}=vJoZzg>hhJ)Sz~+Iowg*oMc~kWRJ4HpS zY-h!%Ek=(h722If?LUo)hc%=}znc4aM{*6>TOT@pRRbD6u%F$>QR805@Gtf$&2Ge* zgAq9jQmJdJFK90aok%`8N}a1F0@f$@UWP;ZZcK29Ek>XNRY;vBS^$1s(SDeJb2Ktv zV)XmUL>QlXUZ7aZ^pjzNj&}Y=%?SZ8hQLCCt*k+O0 zKcuWXhr|Yq9jg9eWX*@rt8c)J>xN~j3tw0xt!WwqA(&?aJ0*5wnn-_|^ zw%@_bu^1GsWFji6nZtYJioqHeD?~C3S#5BclJ_O1&8ybpclexs4`z*Hkp%bVo|S@_ z8kA5bz;L(bvRI29;E_o)G-&Eg80=f8^=xJRr4LJB^78E2LzP6m66i~^(RT`w9Kw9t z?Msh7sI(+JR=#^b|L&4N3CY+WHUQs7r!F4S`Caz4+7WoSz>L0!U9h%5z_zg~^p*h^ zgp76W?5DnT6ij^UQsyu7dBf4;j_gx{zTkNwwjL`+Z#KMF*HNrF;J`{-DoIRT7`32;^iL9NRvWF4*pDxTF zb(?BWSL(NwlrN>B$tORAZeMtG7=Uc@${jS^LU-RHR$douD=w~E*zt34-C0%0uqQ$a zt!%*fqAS&(g3ij@ogTIE*N^L-jA(4Gi)(!AV#C|`9LXE;X{(usBxW5k`SPEh`VrO1 zmy6Gj1{Yq^hQ4^X5(~g=V*TpcMZzjXjz|N(|^XRfyb8VEhUj(tRqR}q0GcPfh z?M1(PTSj?bkPIKuo=vtFq+3 zU=HP0P^G_mU{*#d#%W6F{uJ%A5I0dA8sx*#^QO1$!%5QHW4Ah;Hb9PIW7B8b1oS^d zl*RZmqt01H!ao1OQ1dqzR-)O?u>N0wgIf`t?54`sXU;WKOlk~eTtop0!wG5ODx5wj3MN1cd zSMGM}*;J562m7JlK*F|jJ!r7WCN^(!)t@8PL2 z-0&{^AjkJi@U8EGb>WE9nK&>1o4(k5X$`y2d^xU-4a16RvMlbMjjj8NFug+2bcK7x z6_{RFMbasnBK%IeGM)fr?N(M+OCuWE9R1lG{>slgn{nKed@lvmm_!kGbDu5_n|E%; z?p!vDCL;DP+DvtkKMwX^()oAS5YX`8V)1MT*B3C5e|YHmO#K*>_xuWH zhiMCHK--7)TXCJ|1(lDLzj7Iw`g6VP9IP#fbs=ApnvB>Kz8RPx9gP@^FyrBF^)VdJ z9}osPwO4Xf^3n$H@O=s7{KwzDGUk0A$YwwS@IS_wSXJk4#Dy$Y#s=)$9Cm_`eE4j*$0uF z7@(TI+PT@hfS#&xJn!dRy9wBcdY*{?NP{YO33)rHC(W}jqVLZR1%H#HnMUcF+!EEA+!?$0 zwi?jIRd8Hg+Z84%GJ7))9^Qsjux`4Y6^b`QlCu~IR6t1)MZsTN(8a^SIHtlPa`&YvbEM0&7e{^OJtgfwWI@H*|$GerJ$bQ&C z8Y#83xhQ7uK0Aoh*c$=e(oZC9FzDt`a(s0+h1VL+w-d6OZ&>nSK4MP zHDUxl5rn|OYv;`Dw6&#B9;u1E%Z4J_#<_cq`I|_zi*P#v;qGb18MA-sNYZsz;nck+ z0v~gs@HVNm5Hj>g2vD319_3Z9zM*tSIUWt(31jJ(lAiEraKb1ULU8b1nQ;mcUO<;P z4UOeZQ2vZjNK8>vb%inJjM1T)=df(3fcNzmmfG%y(x1IO#(Yys0u(1sI;SBk2k3~J z{Hw4S&%T%x`2*$Tpu8{4*iW(OIytpwfL1jW!t8GpRHtQJ`a5F{^ONnb?5JA-u+fj) z3QH3erxRz9kCGJ!{}x`gZl=~w+9LZuNF)N*5gxDz9xVSzD26dRj6u=y)%isEytTaQeFnd z6lRja3FpWwC6eiL#Hn<}k33rV$OpVF>>IbNh?K-SM6Ez*++T8MI%F>sll8EtM6qBf z9zWeCAvaf8uX3BP(&|VB)j7xn1$)`z8lT2cZPD&ih|t~Nt*6>Fg`W-zf1$|Jbi zjoxn>)B6GZcLbHvF1J7P{y^K^tEF+9{`2}t{(6Kd87VKRNtP3Blv2Y>sNX_l$2ykl zFq%-j2`0Xhm}~Ej^)B)0qB1drhDgan%|0`ZxUQdP%S8fB&Chzqa8z{suRsDabHiol zu{>VMz=fD1J{a{KyScj2cNJ=+1)utUwwJ>*>rV4WKOff2eLU9N#hXioOjh$gFW5|& z2LB+Mg}r8cqWd`-MV_v7ECheWcXSW$$h*5%IzjF+YFW5*ICNAny=BrZ=$6|?jEhz6 z9h!FF@FeP*W1+ZLp=uo(i8kzX+@iREiZ{C2(te&g=plgY#L$wKl$95D z85+nJ&wOt#$bQvXCFo%qbpsM#lY@hU(HI{SicKHXPEAcoNDusNA$spTWpkm@2~Fi1 zMk}YJUQMgz;`%`e=cZ?kjwo;cz*rD=*=bZ*V>!jd^z2!mkea_yVk`$&gNHeHRo9WH zy>6;1DNBJtT8mHUrreXIXNv9fNYlzw ziwBPv{Gbkd3tBeztZ?u1}dA`!9ErE-gh9&+=MT^my9Qw9`nnQE*oVA|5Vs? z>+w=?4o6PshgPx}G&RPVJY41+v-ZQ@U%$?sYn<9L!LbOwnMCsWL_{3=bCA>DHm}`4zP<4Mz*IN!bU|H+ zdIF{zOd%6rQ`X8$4 zLq7|%tNMI-nJF@o=&7AfN_>F+oHc=fJ+>0Yd3WC7b%lBC;$W@In%!~2cYS>m5Z%-` zIO~@yzLS4bA=4|rE2m%WN7kzJL1V}G+a-NdHrMG<;H&IsUTCOBn@NHeu|aA}jStd} zA1pI`gSpEU!`2X4x?RCVC%su|+;9G;1z=>V=)(86zyUInSX40=t0-O8r#_dHYX zSvXx@aARrJ88UOwl`-5}MJVX-B22u*-Ks{*eFs{M@4B8gY6gm1}7?w{dB7Zkyo*<#!Tk+LL$O$#3TP+JyOq4&r;Q#dn+-NO@hVR~b6CY#{@ zLrK#1K>MA3iIL@x^v9@P%}WIC?bt%znmrWRn2fO=Z(J~@i z1Yrx{^`GMZVN1?G7quHynl8oBdI^L{EP+x!P}P+Ow08xT3hMZ;Rjt- z<0-0Cg~TgWm6b&xfX>v?&Mq1Z6?-pH2}B_Xy2#Nd);(@2@I{ zfA^G43>rz_OCa2^q7bwv1$b!&x< znYyOc3ASLOCufDt=3BQyCV_RH9Uo?{3GQj+o!)sBSBngxUr?-tgY2s|;vWg<0hKV* zKv+G2%fI zFK@SAzLfMbtfs>&gdlg%8|NBZohV)jAQ15f146z$njk<$t$@`2`yrmO3%gDm*t6_3AwDsFk(3Sngx;yJKqYh~r7a^O)ZE@kNR(4(*p*A#t| zL%H#O>8XkZ0;ce%;@{jx>BiVXa3 zq3wJ{va+-MV=U&#L0I@+N^YHvlL_h)D7VpXG~bos9AnR1^ed3bw;g@p6a?75-%04I-WiWW}%SWw+l2@QWX|P z+;V+Rov5kxy^qgH&#yFLb_#bQ@R2b31WU z9pTyniGL|zx$jvksF@6pnL=?^wJnmu1&H;(e1qpwffQ4lr5O%#l{w|fDUAW$=^?Cn0<^NTns7GPYi$!mo zJNTr?Gv|G+I!GN4QZ0}F3(Xx60)4g9DHt#=;`|4HPp#=Fi3NGkn|?HcXYL zq&?qY&TSA?qV;HgbalYv?mWG_aM564ILLVF%LDg=n3PJm7RvcOds3;U)jA0U>OPNA zrIVvF!#nFR4f;?Lu!ii+B=>7bn-e?*g9ZL`&MCJ%BHScVR^2u*7^5o%{T}wvVq9I( zj+Jw4kK4KNH?@F~MulxBHqXXM7jN`>=@X|?<% z-)KGMYNF-;=;1BkSWC|pXGQ?A-!4sNaW-TeFI{CTZF7&4q z?-!KJw&P6Wc02pl5}Hhl`zQ&|%6P|a1+j2yupQavCl{$z2@@2q3mNw{!+8L>2Kcr} zN55_y#h|8St*M^L1l{dqbs!I$LthbkJX?Vk8(B3YM^O=PfA%j2t1Ux|2oXFa^=_QY z2TzO^iNC`AI2kL|Y!3Ln(rq#JRh+x4akI49|-Ff`doD&f_b+uf*h66J6SE z#&!KWah&uEi^ea4#3yt=e%(@k1#lhM!pkJ>KTw-tR%@L5KVuHzQ|>CPnO4H=cYfi{ zg++Oy7+-(<@N2Ao0dGC!&4%h^k{Ey4|sAg!rS8>wpg%H(KcMSzH#oj z+bx(O#k^m14e%5L3ga7uQ-TJUO@ffCp|LRuM30_)ouy7$f2aQlAiH90I7ic7jI?^k z)n7;xDzch0?)ds|sD7m(*Bsy`46Nj`Q{5A&`Aj0qTmc%WMi~36 zYT5e)xV~W4N@Klw;4pBRpULXSRuJ%4jIW9voxHC9@xjC||QmZdQ#B^On7<_nu5<6BF*k`~B&4H*~1 z_UasDDAe*<(tB3nE|NA*(f9K5lW36G6Dp_KS?vW-C5*zc8{b@avg_hhy;Iy2)Jf`10)JEk3C2oeWcUo3fRt`77Z2L$#9_A+ji_o+h zB-`gaKzH#cy&g8}hZ@~g)jH@jq5gM z3pkf`^*g!|DE-J$BzBL%4R3IXH~9^~lcG3js#?dxqt5yf5rx3-lgy0E2YUxw;*FNK z1d=aa^YwrI^8&TKnNeh)jobST@w+*GQ#Sh<^WQ=<-;trzx9;@R4O=J^!}(F2DvH{c z+9Lqg%Oo@VEggu4yN-Fml!YC4+|6}do05K;u0H+q{xs4MXoAJEboF&~1P(ln-`CAq z*x4z0ev~h#g90S6)*BzW`5n4rh) z)t0)+?Wo{}!Dq=n*kc>@L_mTXcHRy&8S!Fh@UU&>iox6XlPn<7!(r=u=-dk_yOO zX)}cJYlpa8vG@%uanB8PBS+mzH(IXghACR+8Qn59H72pjTmJ+a1LLI*o$?7BAPlQ- z~`6zgpJBz8p})cHe?+ zu)4?2{zk%$ALqoClyE@Ps@r=o3>qyAe=c%aM~&Jfa?vt=baH?&H`4gEGea^_!HMBi zn8Ytl`aPj4cDP7-@K|xUvW8(D!P&`Xs5-VKhvq`-lFI?RqR2RTL#+Bc0TvzJ5M_?# zEqV-0mu?c>yQn-gtoBia^{A~5&jwiM#u%3;p#tMF$~j5}&MX>XJXB*Df)dG$SNM8j z*Y<6uUqp1aVL!ef%bjqb6EpnxW_$xJ$B}>gvi7Dmu;g*c12=_C3<71PjajF5Q+fMO zH5dg{9^Z#vVUJ*xWL7X4T-x#{D>3a~zvGQPx-F9$uzX5s&M@`8#L|ak`zMTnDqEFU z*{L=*0aN~QQL%o4I&}P5lvIh|JZM>neA?`&2JZ&qjESc4hA_uR;td)a8cP+fVz6ln zeS#;7uydSkRCQ@iv(lc=6}1`}xv1Ob)ASQ=HOg%{{{#pBO1&US%z~a6hOn7eHMhaz zv4-A6sx!h~wpZ4#xPGwvVx3ptUa$0|sEQg)Ka{JQi;^lc8S+GHG6_>?y5;yl?gmk7 z`ZCgPoDpwwn&CyM>hqY~?&{@6B>SJp)Cj|ERGO|<|0l0}vU*x0U-t($+kPAB?nJWX zGClc2LiMY6*LAg4Y3*sUZ|Y04t=g-`yRWU3pJEuE#7l^K;g{=4LK8%3QlfA6+oC~u zvcXwRQ0V>UTi93bfHI1(l_#R-ls*h!Q#U7}LU+zu?bMrYh5Pdjtk3{; zE_9cTTn&MS$|8=-6j7%4cxPZV=m~3#00?#O@wFr@*V$x^(fPQlA6g##epdh zAuy`uxqB9i}G5l=O(#!UlNfM?hk>-!P!5cN6hMe&iM@Lmz z9}0E}W)7}}$SSb27v6eIon_pdjn|yHINn7qT;3kNYN7D!f=~L~9+4jmDZ(tnoq-9rs16uswT1GIo-=Tj7>dZc?FOa|%9 zDf?zI!^v7uDc5(;T+~^U1s9ZCeK&j9f>GN+E&Z^t;=D1rrN5eQ2IBfI>7(MsUm0hx zW`cJX+1_aL;Ne#uf`e>>o=IlQk2G4I4*^52BR!f(*6Le9&=jhpuRjVR4nDiug7KWy z-jFG(UgB~gV*qqDyhIY#){IBY?weKN=V{2Kyhsf*+#`TybrYgTgZ&Sw8Lj}RfW}FP zWN>5nw!K;g4T32)nut*oq~>iyxvr`j$W)6!(iY`4+xSMbbpe=@@r_ zg0-=pHE1eD*$r&{-g3*rNXv~|)$F>REy&{3@+_y<2$m{OQ7{`}a(b~`CDfSorOZlm zM1<&@e0iJ3ns;4eC{qrL3VzT%n}^H{c-PSpG|~S-KTZ!&YP)xohmNrk;26Yl?i@7# z>;pXBrMd)VS8FMUdj=CLrbMs$Q~%r}uEnUwb6;2nH>H|LO4sajuZx)+4BMYBViPGV zHEcQDcZ^HkWZxWgj35j%#@<_ge6UxD6Dz)7LPA0k z^5z*4a_Oj9uW^l09i!n+LaJ`4wIf^eUK#~GciV&6-{myymb?}`I%L8wuO2#K+YT@+ zRdn^gv5`*m;8w!=?o?bKj0HYeMIVL6=E%{;zNjot#hY#t)g8&<;9O`~GzvDmd4?&; zv3zIdDBk_NGE&(q>`y!OE|{Nu>-XQSZ3hK(Lsc5BFQM8c$o@2BLudYUVN5+BcDA>% z3$bU1o(Tmal8h5~p3^D>7Q$Mj*J*e*;DmMMrT!CKSN0B<^3SSQXJDhvcTlqxsF-Ji<=**PIQPCooZ}d52s57vZ`f5Lghq}+2fL6kIWKrQSdOZ zA-OPbZT`pf1`@d7X@~y5lmfJw;D(mhrfI;$b46^XX?vCltdYwrEw|8-G?u!C9XTyhm4!MhD38Dw2(hSN5Ee2cU3k=ugrF^6jKQLi>KVApaATY8l52z zBv7xWv$V8a{X19pPEnEL41$W>{Zg_*nMH-}-|j~U81fQ>a?jRVMfnuNr4ZTKbl7%? z?V$cInS7ozMqV4_&S?DOH^W+s0Am&|v%=Cnr4JDz`%a^O-L(XV{24=!tW#X_f`vEJ zufC(!ny(?w8P*WEzT1Q&{WosO={?sh_-{wV^fOEPZo!jDyYh;Wk+IZfG2j&U+B;v~@VjnN+1&bT>T?v2ug{ypSu8xq zV~={GCp#NKjq&iH1jRXv=jaq=aG)g_1GhjM31$GS0zCJ@#mU{CbIUN@GvroG&} zrSAAS5jFhEUjrwOxS#3SpM$3A7H54ee;74YMr0rBiPDx+w)izF4B%VMz10D!{tF3a zW{&PXN5D@ds0}}75V=!(QZ?*9mh#0;jw;pY`-rhuEBD{0UE{+R7k??{SI1mgUL9ay z6y*MiWRhYh(Q-rBsocM7v;AsgCzA}R?Yam8}j!M(Y zv|VJn5;ns}0N%ua*@>_ivK_fI6#sN$VOHLmlQ`y27z@~6u}68(ogJ^AEg@U|5q>Kg zDP(YUh};Vl8Rve{Vd!31a5Z_~xPO$0o*}%d+Z#Lx9Z{HnUG#F~4WFZ`iK&fc+y!!P zi9Eh%_rsW(=i8X>mhuHuBy^|mixc7rRM`4JwA1hJv5eX2x-Yp;R31UMpFu?5B(#MC zc|aebvdck7;OeybR3{%Vk_!SGJ=3P>gF0Q}bGdDpK-*`prV!)>)Y0M%YSUyZlYukz zhivZ@btlNh^3f`=8J=89OFwPEQZ^ckV#$$8l78E>YStrmCzgD)fheok?L6j!rjl zv67mVS;i9iBuCH*OTkwfwgsDk)xPh2n89HVGg+ghL}u~pPWa40PcTrT2UA;iesT-2f5NlD+KH0=h$`7ofBZ*~>Qg5SdqF0Lg=gTb zz5HCN@YnO^k*~O)65mJTU+qGME=%W1T=uK<6zlXbeK!Kc)`suNe8nqc>V}syvu~Fg zmrk?zAB$S{n(^pRN+i$Byp1<fl#qATsFa6N1&4c*o02ndVMJZ4Gg@)FJwj z)@4Z8e^}68;}w05+%HNhq-gSvg7u3|0U&_1p1HaVS!s##r==J1z1AO+%Xohi-N~i< z9fW!J6-B16vBV$){AB&^gsM0}x{qK5rw~X>#=Wk&v9Ph381stcMbYddaB3b{Memt%$x~l5&(QV_t#1+Y-EE2`^CPo6*11HQgju znQ(rSgtOAa`=M7@Kn9`U<`~TvzXDz8IUIQQ(~Nv`@(*shuoI4UP@x*{{x{3)oQo0J z3<481zWto?_uPHEd~v!uqtEQ%+y=8eq?;6Em0dybFaZ|wCZ`?E@sKa=$lY=|wY6~> z85H-Oh(~I&Zxci^E&dPAHm6{mBr7tKAA~%|yVJQEzi8XA<{x}XfBE*y1*dF;@8)<& zL;l|>#o&RhodHpTrY}~iICZumpRG9tTok#sBO?xW6{#MxMnfc+W9I8V;ZSGZhB_>`kF4iUEH7upAw#uf;sOgM z0QD76r-HmUo!<;J$3k3iGlr6@1{419eN8=qep!)9BYbeXxkq<<&aMr60z>^JWthUsn+P}+_x3FT z&}v&veR`)c8+^D9n1-nkv4m(8?h z?{7h?3k0E0^X?+3nI&zmduu?1g(FM@j!>Sw$0fszoz$}W`jnvlSlic13nf%;I}KWV zLnNI15+XY4z8dmJm_KXAWpB}T>({zB=sYV?m!}debuF41F9?$_oX=^fwW#qVxQmP& z$c?@*eIa0mmQ8;5^AQp?s#i6lNKE)sdS+UwHqzNGr=dpVK?p=cC&_-@yOGa&*(9&; zVjZX)cC2<^`>uM4TcZHyM9^jXfCf3idvw_=IsB{hkkTp2Nn!3ev@6MPCE6N797at^ zwu$bIqSm^!SUG#ru0i~c+_pVZWw`KH%T|WS=B=o54$&hGIxvO!K(EE|!xv3DK;4tK zxg9+GwH_^Eu$K5Or169>Z4BozVFKRwWCAfzy5~MGRF&w}pqRqELp)DoRsV9?l*LEi zFy0Cwp$GhA`}ouA2kt%TrU+x73kshS6>PZtf#!H$OYgCE_pbv# zL&!1^M>5?zAGxgOj18L0fZhoijd9tIFO@5Oq)d3Z3NfVFd7MHQ@b#Fu7gb)q#Hgqm zUv3~RJ>AlV=yRp2f<>+t0O~ZV9K@5SS@i2c)U*n0?Ppw6_f%Q;z)ipcSbDLpcTedy5*<1uN z-?gE;AMv`MP`A8mUA2|La zFCV2(XC$YGOjwxO+rQX9J3Br7F@bl}kVHR=eSU)2VX9;=|3#uh*>zBlqO7H_G`*QA zBXimqeh?DSFx($W&j0D0Ej~V8zJt(Tw0flZp!3EZ5Xs4&q?mNI&Q^NXYxMbEnN&M+ zXepB)9-12!4e`#*+WqQFK6&7xxpPxI`a)HJ5&G?hzDbpI-S-|=g98X9Yv_`q1P`52 z0G+6ZvN$&y+M=*dq_@QFYlf`ub{Xq8fz}KO#|*dZtmI$a3UaKkM_ZaZWx_;bMGxui zuC{sFG~BVH51seEIe+tzRRW8E?Mdth6O3VAa1@!CV zCiCD{-&6$(>uYeHVg?wO;va;-njz}p#b$cY5#T7zC09I2x^9PQ+J9m>B5W8YGKm8X zKax&QFMR~eE`6)!iF53AAExp|PSg z4OfsIaB{G83)>j-4jSzN;)}WtR5ik0?sMVq1Nx+y$uV7Ax`!OuF!6XBKs7oPMEV_h zXCe$hul(6ahCKTE`%BaARo=;2l*0z-^lvt35C4VOESf8p0Y!Z!`*&4_egNyH|* zvzHgy?p2{pKvcxVt5Y0@fj*APd7-hhmF!IR(nlqCw;aw-dcPLL`U>a-EkWa})|`L2 zxHEGyn@{yCJjHt0kQ^=-Z2GliP}!SJ!^>B=w5$ve_{!ep;2^SnW(D#%zyQw)Rny|@ ze~s@i{72rj85pRm|0jhNeG!uQbR$}-fVDx_@Ea)5{O!410ZuX5Px>p)n4 z*(jpDwk7$RHDh#T8|9+4CO*V&G z7XQ7LODi?%18rVWlnheC823x_naQI#_HLV{Pn@~F2Zi-j@(Q9IUXJf$;lnmeGkf;c zJ|Xn)@y^|R;RO4)-_d z6;mGVC+}4*iPCZ8j^%#VI>^^2f01KD;(ZAy3h7ajm5@UquQ_(ym_X%AXW!uh6hKr| zRLTIGblM)aLEBH+!hv3~jw}??9YaHZ(FXt!|Ni~k{`~LcG7O-h;pjw)4NZCj%~G9h zwbh0;2dq;Dbpf;qeJE%pRYvT#9M^b_(gDd;p(nOg57#Y&f$FFjM$J~vn44F<%7Y={ zXGZCBh6Euo^X_LwIlHe(clM6;j>PgmmK>C6SR&sIKM``It!{~+X&+fa|@mMnp< z!$5FB{-xXmtkJtMFc22Nt_xie($pKND*1id)m38}dn+S#-lK5yQO0g?VbZ_|=RFXx zT<^XdGvo?3>_*b=XTul`N+4u#MHfu+yDoi0KjQtbc=Iq_z7!te%n4x2LskJUx}rlH zj-xl8!;cv|f#XL6R@N+gJB28X7UswKdf9(vF@JgRb6=h{AcS(?R!eddD6FgjBBc*m zAuF9T#Lk5eqpV}CkS#V_ek+<{voMgZN1wYfxB6im4IHa@U?r~SQ4(Bvf-W8qA1dHn z#`xtv;^Lj-FG1-Kc(LV}Du1hr2SMR~d&#$i`bZNzhlS2^S6Q`Wa>ezY9k(@w27yK% zxf(Ps@JokfkF88borxa610^IRJi_CZ6-QH}8S%z&`X9M`icPixTG8Nw``M!3NwX_t zxmJwRPdtVHzWo2;>aC-yj^p zl8-6KRUj(__Vpa4qk+ixu=gAunEUBjSaQ!>bg6a}m=o=qg!q5(6_73wp(1_*0w}^0 zDwsG0syUDJ<aGtEz3pyje`AkIx2Y=3W>lZSF?(UcZ!|jJS&H zOh6yV202=D_ENuf;{ZN3tSanNrjCVPiqFO|d9q8%DT6(?Z1!iI?-(%3fOAqiJK*_q zPyi`Q(5=30aR>P5cNR8~8bT$d2oR$o6pgr`6nXAWqrI@hmGW18?e^hggyQTGTUpkk zJF#&fva+*Zu+2Vz$F1dU(5gQVCTRfA^1jcYvSfu_yw>XLXW#Yaj*`A4;N^DQIDK)) zT57M9YX2D1{nz2$>~l{p@{kic#7<;O)qbZ*F3X-@DCHAqqV)Z&6q@<+Z&{`rHk z?WFM3QlcMRK;Os5*{!wRxdCX_TD@UH*6U*EgzQu4yD-Y>`oZ$scPtq74v zxB!N}gV{@Mr&7F`Uq_x?y#>=99(rx*cD5LjZz>Q>J@MtB>KYnhUzR`F8Toqir)Sw5 zphdu&!}rBDvnT4o^&{BzVu&eRshkD~{~OUjYJM=5&*U>J4B0b$wxyDU;bF}LU2W8X z1+C<(!S@+rhV}gS?Bgz`uWR=*OvH|`ua-pghOh4jl#WR*sl0ak`Oi&ShA)rY%G_e0 zmAbO(UK~?MQ(75)2WRng1%O2(IJXr(y#4vp5c~?+N#k8>i>p4wb$3euZpLLvfk3V6 zRoZ=U^xseWl+S7H|HD$g;*q_a8u!YRoVcV64dX<(tcPEwE_O&q%}pbfXEZVoyhT1; zfp&aBbol;E$Bv5QE;q}zvpi$wokN^h-i%fa^d*uSD$z^x=z`-B<=n9zQsu9?d6)ir zz9D6mPZ>NM-d&UZUa zSifQ9rX+yQiig%@{4U)xWH3fSrglNn$%~bQKIPUAnN>GfEH)|-cK{MT${?7!DoY8(k(QkkkV`Yuvn zHUSnKWWb2GpGvP?K6mf+VgRk$^I3?EW}={G{6$oY20S;VButXo_4tj%owxAz*jliU zq6Xorj*zMvTv~E|IH~Guu_O*S3fb$9z9!QPdtztke|F9p5d<7{H2=(C32HM<2m)0oV#gxc$|>^bbmm)=}Y0 z_r}34P7XjjpR-yKN`N5d!l75sa*c+NP&Rh9|1((%N+na0Y@k*s1egFP0zjUwxm
r&zemmK=Fi%+B8^57yU&I2vJ^ImG(ZgR{ea#_fgHS?>^o1M)^oj zjPKdcjvjxng}Zpyq<5&%lSVos0i%oQ#g(OO(yA0 zi%$UvJJ1`yw;98WBZkcIr{EC{QyF}qgZ_FFdUeiH8GElz9hR)AyFPQ(`n0EhhK6ZS zK=hW?;}3#2-z!{QVZp{6M|dZ)99Q<$FYS$&7KR+1yD)~?%4B3N(vB*C{gqMxo?tDr zqFhW=830Ju4Eyd)JqK)0y|Dd>N66t1-%wsnPfuP&1>1To&uFI-xcWEO7A}d4{<~*6}=RZ;cEP0AFlE7TgUDnT*2`6+@m>Sco6!Tv%JT5*u*>F zf0b&6_e$a%5J>@A?n~9Xwg>tDhnv)!3j5UlCQsd2B#mz%OscrD(q38}J(Rh~h6rzLCi1?;|}R#PHLo6uAcl~by7v>_s! z-_8DFQ7oScqAVUbmG}PFg{b&nkqDy;4$LNwE}JCW*lTR+D*Ycs1iW+l(vEzMM7fZI%5LY`Af@gAw44dHp_e!xkKk0`#16C(xQ3J{rRa~L%8mHoSgUpqG<6AE}6x9-+ogWF0B z?1sOWx`I{o#_jNd%~QR4dfgGl1pDEG9q^hwKRpDD8R+kktyfB7bLaPNuF|jp=MVyR zZ)Pm9sP5X2MrklN2IjLk__sdMr=XQzM%W41pRfT!5ub+v@yO6CX6Li(H+tru3o;3s zffFA%#i^XmZJnH0v9PezV@FmV_DU8NQH=)FbD|%mUWihxTq8)JLt)9qkpNbpBBD1o zHo{3th253Rr7u@W9u-vaiAjl1*)<|c4n0|+FouUn%=?o8Ln(rQ zG>2k2h4A9RyhqASz_J3tu<7&noZMzugenY&LF@0=!@wtd@~vnt>M{AoaAq1MJB>Fboo1N1vv+O!Pf^~@!~Xh~o2+NA3ok3W3n*Yj0c zFv0~EYsB?l#8ypiZEtsPdzkTNZ}03V>=zW+W z^$9>cPh5bi3Ot48h^~}$-YRHNmpK0o1oovHMI~EM(vEEmlv;B z`*Q}jQ5{4(ceTxzS;W!$f)Fv<`=1b?Chq^kjjsI90Ne=yJ9!AGc1T}=K$xFF*L$z7 zf%zGXu6CQfXZ{BJ4xYSK7{MFO?o%)?I%ZP6zyFM#(B-2K#-mK65ax$rEb5ZzlU~;U z@l8YW;*g79>xNFE?88@??W>9`C3z_{ck>eqKLg8Il*9p+JDe4IitCjxv)c1n_-1Nm z{5xClSjE9U?MqHmy=GCL;mWhPy36wD-t)@8jr6?R6(oIj z5O!BfGjDK|w#YY@^_<}y?Bms7S6AMfeagV38)Z=3Q#rMg;Sr~-vV3~UVMWTo#PG_` zFYEBo6*IE6i~MU=IO$`%Qcd7flY1DMBUKlqeHX6(v8$y5@%lsHfM&0xl+mMyh;$mP!BJ_o0`c{wD_30QL50 z>u#3(C-6-*I9MlmHD&(j9P7P$jx9K^q{vxL@TtQ+%i4Im0QS9@bV-%AhV72yVDYFO z&M^4yCEQM1`|S0!LCDvM9v!Z&G`7PMfMKlPQpRI_4ZO3!D1md}efs?Q^X~KoPtV=- zMmz|Z9trDN25GD7jWfmuJC+Z{=W{xX!B^o)e%&`rjgtl4?c2T5U>RKrXD@j|2q{Z? zEo!+*^4W1I3CAhkoCUmA7E?MM>8g(Cj-Qu?&wF?V3UV!D6Y-?xJj_i&2PeKJC_`CU zZc7K5Fd4aD3W*7vu$&DKE&&0-iBW5qcBM3z`)cQ(vWedp8?ta7!7YEXFAvjI(pqKS z?i`h>FJ1Spbb1ky#U@piL#y!8$l*lQw2r>d1hns0-lmPL2^Ude z54Ag15xACLsO0GW=i7n84b01uQt{ytKpIAm-R`eZ$V^0cH-ZKqd*K{-XZtLZAFVe| zOXc5Zf(aVAL%d1zOKY1|a^GY+TcP~)Go*;gn@%^wSxwh5iTE?WnUmm{xZ_eSZhTEG zBk>@0oVITLXSuUK99CO$SQ9^NogmY6k%v=0LSwhEctQ~}ZTiWodG*kze_+JY&J1CU zXCW>?g)pRcfo?2&PeD=+>GV5P!kkiXar>7n*fmyZNedzMS$8N!yV^~ikh05 zKRT!MDN1ErT@UNM3RsY0LfU3rB~A~43%LhZE#Ds&+kTLm8=m_@`Qx93BTJS7MtY9B zQ=w;TIzEIWJVKZbfj+PAeN6e4#EooxwU}=pI;&SJ!C2TaU0%Pb4Eol5g1n8MFGNx` z8H=KbD;`+Z++RFLt}-&@_{v7l$HN2i;<6?M{r&v5w%!Tx7L@pGME&N(RVO%y{{7%D zEiGvU1O&iK;RVfjkXO*ag4gnQQCSqA4+JA$M>iAhZ^w>Q8hWafZGqhaB#q50eLQg^ zJ1|f-Md*_5Opl=}VWk1Xu-LF8U1pT+?2mDXH*Y`WE6K7>8$T_5{`7FWJc$M^zxz2) zFZtf79ozmIEY`ojl4sc+W&ADvuOnNR58H>WB4CvQCl}`cJ8?hf&Hy)Y{~)*7VE?Cz zw0@54{_*WUem~#UHw>_65|a{}j{8R6R3SK6n#+CioJaV16OvH#mFE5Uy{gwG^&sX3=YFR!ud1LTb8|#K& z-ZtCx5<($AFyB;SE&P7bRCx5Mp zlKpJhevq%!agA?%`?X+06q_SWm=2;x5HXXa+sXqqz)r1_3OrR1*DQEL?2naAdsQ2&`?P~C%cNeR)#sh;d zN7Am6)bDcL0Rn&nNLdaWO*w)22r0&ba?0J1RS1g!Bq*;Y@P6U`E&&V3Da8u$$274_ zX(Gv;OZ~jA2#aLfg6s zhAYS6&m$hMg%&1Ccu30U*k?RmPR++ZH5atBec)QfcQTeBY^@4NSyjw&*|pNy*^lSq zQqiFb-?Z+AH_7R#Cs@-q4kRaf9z*FzlO+l3lJss1`TaW0E1A(>1&Qt1&iL z;y*V;7LxYc59fisf~%;mCQ!elQ3tCfcVqjHg}}_R!nW4I;(dRrC|9W&V+c1hCDuz* zD^9HnCZ1X1`v7;f(l?tjWWnjYK?5mk?}iQgj3Z@b4}CheK4z|A&uEy=if>4XCXYo4 z`71!)-A=9@)VZ&zlwm|d>8q&tibLWYL`a&1-JX}eQ_fu5+%O&FE}D{SSz!IiN~D>R z!k?=$tW%@LE*!UcWO?N&2zL9B#zz{D=^^;ySy)@+x$L-LU2_=L&<&-2Nh^Y}!;*_D8&3nxGJm-Tqy9$|E{-_v%tbfz?mHRE2Z@`2JIt)-*m4g#AZTigC2 zpXi*mWb825PsBx16)=9hlftPL(@UM6nVhM5 zsbd&fGDSrahxn5WK@-ZPjRAxl1* zi)3J&N#>vWtvI(r3_>RXQ14!${tGk*AAlyXd2e(?KKV-bJx#ENXJl8QI=?NMP=}GC zU1JPHd`m1OWyKH&TOf*^aPa7C`_9o14w*MN^EqXcKXwvs`F3$?!GF}Bhoi+4^nA|t z`65Zw4%v0E>DXGvHct-ilHnWwLD^3%-Kk`^a@ff<5rG#0qH!|RnVFdumX@!0vpGCG zm-lzgZR}#AbDnW>aglI&aOH};={6Nl{&bMk|9!n+YIeDF6)d)T3I(S$)d%6-GFwhSpHLoZd;1d55$f;*MV6NjK zQw6D+=%x5ndfKa>9*!;s=HtZ!gUZlEQhn|;1xg4*;#F|x47tZ1<6)p>OAsUQ@;L8` zNu+yEtGm#96B+qk7RdlAE%N>zMS>6E6GwG=xZkCH;f8`TBvi}5z(5yVw1<~izgiIt z)ThgaKGY#aX;An<$8PE7=Ju}+!2_9pBTdIxZ^yEVbkY#Y9{jQEeBxd`CCNJO4z-`*Bh7X&hSXUO#vAk$|ri7edd$`LHpOMIAs90n9NQgWC zRqbXh=9XahuP?twzM8-5%@YWFUydlBJZ{5n!%do_V*R7>J-22Kg*Yr3XCo-ZYprf(?5OZp^XyI;{_3l@;HDw{58^HzuJ#Fl zxd7a18{fKgX|1KJ>X!R2!RFqX46y$UO$)or08|0UYTmtFzcotxAIxLr>NmEBUjWIugeqpG3b z)xPBYoF{I2yy}YBkAc5>qQYfM+&iiHcDnL$n`(!#vzj>Mj3F4MWyOQDU}Bkt?FAcK zLQV%SW<*fE0;Z3M@yCfEOg|A)5z{{fGsNHINuHqoYEPD4_&UEX6lr+*l4H^&zOzc>ltWtXGy!|) z<&0*C26vjt@~dmn`LKvdV3s=WIXKmXY@_YB-*@X92M9E-t5XokwSvgUU(!kzp;{T8 zi4PCr{FTXlT%5^`>Ebcjgq=XyH?^mz5C6D%dODjP!}FUYUEIw79g-5<5MzJc+baXu zi(6gnaEQqLMSl#(*ChmsF7vy72_d?0 zp~mNZ>)&D}gRF#U-C{NlM~G~JMj4$BJMTCf)aN{isv6dB(_hf=_H3NIk5con*4AD( z!7$QO?L4@C#7UDgbJM_bADWXyou;~?iGY7hH>(7HuEnG<4WU(YRJTx^7(Aer|9o&> zOpm_pL3-{eKkV56{9uyZ6pjn8xFOWMnNnjrptLGAhD1j*wkEQJwRZS)3nd-snuYQo zKjnJsNnWiIpyuvOm&-#%aX_n@;a8|>;KwJF!16+}Xz|m@`FR6~4}}Fv7NqN8H9XPD zy6H$r_P-Dcpu9c&h|tIhBv8bi<1y?%X~o(+>!sd-Hz$XSv=AHC^f}%;zkXIMhVdAj zY(3s2I}~O34^I6i?u~m(qTUg=k9+HC!|!g5$d@ji%;(PI^PX=#4jjr<)M8Rw`m->o z*xfPObT-G9bEy-%9K$R`@Xiae^A<;u-8fdR!)<-gbHvM7KALe3G^bO2z(xYrv;nx^ zG^>b%idY!}c<^JeCE$&31=gA$Ok>8D^Zmj(53|Q$jRK1Wja%tiP_<;)=ar3uY3(eH z&3A(b_7V2$ojGEWOhljJ^zx$xe}p7ZxR(7`at%3=%37YK6r68UQzp?wQqlt z$JXWg+%4FUQR4#Ap&|03{1PVeC3yy6auJs=%iFjn5Yb5xB8EQ}YVORYC<-?J+9)!H z*sb5Ki*L_@=nuv`!wb1?SW#D3z8sB4WSEPUSgt$E|udAw9KBvmoW9*`Ho`4ERNa4D%d#K5oms4U%6b-@zs*2Pd^y$xG0orytCRqSCcK!Mpl8RM~SjQ zi^|N)qf1i&zlJ1BG&D55(CJh7=~|PqlP%0lCDIhCNSJvRa&lqOuHcvO@ONlzwiz*# zgvAG}DPnAm=lMpF;|%^Eh=08%H(~Km_;8M4ms#7A}aXLWw_-%~+}d1~(GyRQL3R&Gqc!>l3XwroDoS>(Sv z5PQFo&l@%f#}I-NsVea0=c^iVPqAaHO|uI>D3PG&)<;^$T}WxK83!wvs9l(%%FWHa zxD7%^My_%0j$MlgtGE1+MWSQF{S2|rV4ZIk!!$v&k6y)S;^#=*7JnK+B1qkPQ}mLJX<) z9G=PZsN-i^Fu$bA{F>OHl*_~^yhb^fS3K`r*M%?Gh@ZsRQ%*Rw*!99IltZ=M=74Be zkuHDv1Z8?Gf4xQlBG@j}wRxzQrY~7i<3yE&dV72O`pbGulMVM^!&mBr7V9^wmvo^#m6}@#M$Dtb#5x-9}*z`(WvLu1Z;tRw=Kb2l|dqsa@$fwCBURj;L2-EA)6^ zV$e}@g2Pa7F_U%QVPQziBc>ZF;nd~%!cKzP-ii{#O{E66B27Z0(Zrrf4Wq-Hc-N|O zlpGTg6%ipWAt6ZxXh;a9OX08AIJcaZ#Jvq3%aE-MkenzHnp8@Dn^fl^5y|+ndc=Mm zn(<19?4S)HSi|n%-aLHnx{z6OxAB|^Tl5X54>s7xKGqgCrK-{#Hn#&Q_44va8Z4=4 zH`m+#y?;X%(zhw0YR!O;K2&GX?KUwp)0ado3p+2kq?E*>*NXp%;chiCQs&exyJA^J876v z;9MJ}39VY!*;hJaKi|toC2%KEc^-2X;4u|ucZ(6He54Dn!G?6sS)fNmeN;N(bHA&T zWS#Cj92);z@N`C6(PMt^7Os}yXyzFOB!MNnIZ=g&&QRsy)#5`2(_IgeIt^~L6`M50 z6K)b5#wo*^a>l8x*>B@8EnD0_60a1>o}{UNrn7?@+lt~SjGRfjYZ4{baZU$J2IhTv zhW;~lnRjb18czBMKe2()YTBj5=DjXW%KMsyP5b5Al5ruK_uc`cb9)DmLgBWxVk{V? z>7RT+to#;j*XHUA(MNFfiF?voH0t1h`{KX@C1rL|p3tRD?;sXhkHMr0I9M6gMsbbk z)zuZh$Cla@2>n%c1E`6vdaL)BReD8FwST|JjbmK3zc0-hq5nxYtWQ?N@niimh5U$} ztJ+c!+DxUG^F^B-(vEw=xv_i2_tRf(#n0->5GDI1c5{;FJ&mZQhSxJZMeQudC#P?1 zSvxi>VWMd?ig}&Kc?rQ6#;XV=Zs%efKfiUjZ{UfFsXJLe9iQ{1{MlD!hGu;E-Bx%k zzr*AIHt z&8f*fk8|f`8ETpx)r@A^TyEw@%6zAdNN}sPoxWtJ8ADGI@|EoLX-<0?qbG;s&9VH9 zej)l~Zu=Ga>5EJymBL+to2&#~IvI(J5DDo^a1*FUP5e6ums6YLv2;|Ld z*cHnC@r-+}m~mr!#^uM`29zBH-Ql|_VH=4!8BN$w0WbtVYqh%IHU@pHUs#CLk`u~9I$={0IER(sMhzfSeN{*g%Li|i5HA?1R` z^-sAk1dkYqWbfoU-_d3dyAO?`GKfTuZI-qNQnMzY>tnObcK_*u1_%l%`nV_Eud}8s zcxQgR8yn=g&(+eHU(BEUh!LqWyLU@r0V^1Gd#wVI*Q%3Zd~;b*8QMsr2Jf{uw?6D| z1e0?-Vg72B)OD`z_Ba^D)Hu{`EH*TapaSJguX-nA@$_(_&p+3+RED!WzN`Atz+Sb0 zoQ%d)WsH_V-Ue$<->}7oh!cYrvNF6oDDI5ngM<~lr-*izA*m107d(ILFesO4>>dsuT$eyMYTdMBw$qO?h>W}FVl3cH|8t_ zv19OH%t(?;mn{Dc)6)IHGt(L*WZT-hYDQ(9$obge#zR1pDrIZ4R1Z#PnY)CGy;?!Nq5 z&S~QmQNR{A{{rv#q#Bo_AqgL`>}L8Rd+_lTDlw;@A2qy5NC3ydYcm$Y_73tIGQp{8 z)fA0DDh~yAHatzdoXd-ZPp6R5(WD9r@J1nDU0=^O+Urbpf+4x34}05Q1dMge0H3C_ z@EOsL&j;{AoXAH=w09$jmEJUc?-P5-`W@(*17Og&u}$9|nS-v+ZvgLUB8pD|GQ z2hvyXo#q?LmSnXu_e68lA9N|+O~hiQl&5_Juhae`+s!cyQgVv4u6qva>(FIQ2gUR+ z5bap;>JnoJqhY?>LaY0WM~@!Oh;TpKXgZjmDlZB&p=jaY_xVFnu99na2J9^oDL921 zl^LMoyPTfgeck97Cfje8e4Q@9-puL2NR*aDA3Iq&B9U9mNU(#U<9{zQaNkdN$@MiA z?WR?`tn^ikD;^KzDo(R)<3p`A5tEwzX2G0E=^*;#cbx>B&z4sM@~)gmn1$(ZK>g0S zzc3fQ+IndrU#82PJhZ)b@(c#63(`b!Vh9;3PR^PdC_A=^!AQ_~={bO?F;5cIo}JL= z=J11X4a2+o#j~yT6jj`~xVOxAY$cJGk`FD2N^Ee)ID`2r_GCl+tpjt*dvIsk8sGn-9@U_Rk8{-t5gc>+E4feVN8g-&NY! z-`Szcby6x&;?k8X=&ZjhnD|cdIPc5p&RF8pL<_xv&_IT~;IsOMg)KOSM(?Kz*{9(n zO1wQA)iK&%Z$c)oSa;^b!H|6<>f%oV5GP3OMt&!EO$v~ypymJ~P@r8@W zN*oM6lR(c89G?v`y-0P>Ap8~KlJDM3{^@p@Pi^Vi@#VBz^<<>VLhk6FbG+%UOZy~f zHlMSVnb}Gkt(~P0zAnO4O*$wce8`dyGf+>!`8{9%e%+s;V&3Fw-c-|Nx208(=o*6% zc@{Y97xZZEdJi_~AS0PDSn&#vML5`IQqu9}^zAOAOv~y7iX6T*s-Gl9ro{+%O(AbN z!m0lJMJ-sGBU6Cj6aR#a>osf7z`(j$TJ)i7S$SE%1*Zk)bfX7r#*17PoM@?j8jp|0 zY^=&(*0uYsF)T0`04GivEaXQQ#mvAy+wH(CXPPpcH@{fmeN5416nPe8`RAEc#TOEt zCN=*qlAR}ew_yT53N^jPC(Y)rH`6L8!pgL>(3DrcZ(+8c0XEg+>m{h@rDOLHoSPEcb zm(};Kt1>`d@uE~mLI_1oYlBa;mdSm36XY-$55*ClNj=s||rQt3Y-n zxzu(c<%1zNPA?1^{Mq?vzHy2sA7LtR$7zh$bj1gE3RHL^!qU%|Q}l@wWRn8dKPPK3 z_VmOOF%ZUrX6bX`lUBpGVEMd(JrjHv`2@FoxrR@Nu@*QF^#K|;{J3W_-6f?9s+)v- zc_B}+*zS+!2o`fl)>Oqz)(*%UUjMmPM9ocJ(e!*-UZ7vHA8lkhJi$75jdYx>QeF$~ z2$qOu1P2n?*(nOr;XxC>Hjt9T?^-0!q)$SX^oCk^ciF4GR`K;BGcJ$+Z`-))X=$ly z@#m)>4tE#*^*pO z{kP7?E7Idr;iH4Ki?jcO%ATugwH?8ou*v=3-2PUqrjdfA`XIUtL9|~*ZzjAmS+3e~ zHs5(Ez#LAL?{>4K!B#L}mwl2d6zWL~Y#8V~8AGp#dy5e53UnwtSOvYY?QHj<9goGi z77({6SX2vqVn^*u%mp{c?vu>k>(Ux`7_)&$&8F$;BQ)g{55f4cRS5|G#~{8;AF`LD zNdt89bSWN=m}Y)>i0w*d`E9=5H1eA)NMt|k5nTMxs0cQk52EUos%I8W_%8~0u&KWN zSfTEYf7x+%H~N-UBU1~aJ_?~_d{Y2TGp$CFgJ=kyHxefFy`;x)vm)^;R-9a!7Fi>^ zJA)wYFg*n(3?>2!h-yL5)&6P39W!C6P}nniMSX#1GH6jr3-H5|gpJi5PEUiYg>#QD zd{*xrU!Rp z`BhpbSdp}!KxPlFDp4mQN;-7}EB`LvvoCioe%@6ES$lx{Od(v2_>EZ~1 z4&`D{meQ^2(6u$mGvdXlix#9=9)Q-#apm#|Is z<%*E~2)xe73L>!LT<>5A1QZTcFKAWOQcqyF8O8a@Dy2E@av?>!W;&XXau_6;hGyb} zlpS4o^GU1G6b(Ab8`?4w5)z4Y8bLupK+%2hzC^QVKAO(Poi&T=x-p6U+1O`cp=IXb zQG;*H?6sBptpRMksntnCw@gCkk9bkU2r)H}Qt|ZiSU$fzyuMMX>|)U)N_}|ONC)%@ zE7D$R=zMWH?MhEV=Bt-taasru`rY@gdUrM_e>b zbC9RY*0H8FH{GYoHwUKbp-EOCH z69@l$Gg>=GpHv&5Cpck0JY=}hh+sMhgPi#Zs*^si;~b4-sgqhwBV(fw*GNRXf6jaD z_zDH2e)}i){ojl3vd-=RRH*(*B;5MkBqP=-m<@V}>Bz9lrCKUwn?j>w>4tx%=`RrU82X z1mC&bPYeLN!74Q*-e^hiJo*M-5afSfMDuVpWYkm($i#wjxU30H1U!qMME{iJ|0yFZ zsyX*oE5e%PHH!felST%0T06A85%vCu4 zEdBxZkErBB5fRbuf1L=c~(0o zCMr6r#vNmu)t~%Y&sS6dxAARa4>yN0-UNGp$wUixJZK+VXvOV#6>Ab{GZ@$fc;k!c z!JQT+R#}K>+lU@wA`~$9IU4zuLm$(iUt| zOU7)aM6zJ!D}ixKyb1C<7o6#t<-?Z4-su$b(}Dq9){G&XwAgA%mXo)yA~hihe50u9 z+vg|s33~zX3fICisM6`**7?(5ZUF%~h20YN*o^DELDvE9O?xl0=&ZJxXp^#Po^m7V?ponwQhHS1W1lJO;9Jz zFtoB~g1>j~j(KpOfOOuEm&)?oQ`?xhvKBa8|CtV3;LSM;kJ;_ktt4#70+IAR7A2 zyf}qEM)DK+*L5ybZ}}2j%Ef2QrWFjG0d-nwSL)_3%Ml`!7XI7duxR7{u2NKwt_fOTg*=`7jbPBD8{PU9oDc6RjDN*3k* z0=1ayF2rFrHQA2z*Xbx@%Rz^-a)-TTs6n^UXr1?ybqyWK2n2*St0=op*77;Ca?iZR?_PXpVDt_fz5{~H)g?m@%XUu1uvJdTOc0>p=9uuy!8`5Z>ua685JCQUGHBg zAH0mw_btU6SP|D0%Uy4eQ227i8+|p^eKgZ&EwX@;vZSiCe17fQx0%gF)N1^nx}$EYd+LLH01PEj|M)TUAI;Y5`GVPe% zI!-*|B#uim4yTF4q(HF^OFmgnZPc;FZ5-B412I6E|vd z>8Mw6i}4gOay5|0UPTHLJ{Po%6qNGsGQ|dTLY&Ev(xd`-1{=*WCKaY7L$qk>u@d@r z9h#e?)F4P*Vjf0RJf!h%z)+rD_FuT2eGfRbAH^Au5@bXiZ`#hulCw@X%R6szfr_*E zhbIsm=-u)AdLO_2L*tS) zpoRcWP34r~%~@dRFRRU_c_(ul8#%zNnPP$W0WIoFz{PSDI9k(8b=HoAfNgFo13s%= zKP!SI$jkCi$<``EUXP?;+-L&~(cdOws5OB`H0YGl)aWByGRzIK!T%&<^Z<<*W3BZ(`8zX26gNBn*Jbt+4~u({>&Y{(k}O_(uIy5N9%)y=wZ zqY)FGMwgF6GFy#W2!N)KZVR0SywwhJE?u%~XX?^;X zf}{UEIJy@eI|nbyT7)cbh5zz(bK(zNXr2GL6Aq&%9`#t9;PD+1`a|I^bOmel{xa0P zQh=({f+`G<5~KfA)-h}SGg5ukxCg@RN_t*2|zOfVi zjqeeF97c@`Q&ZZF)i6@B1aIRcyP=8h?rtO|ReQY=+{PBq$5-#kA5CXp^I03$l1API z?E)QW&#OsCp+NNhH=GH>$nnRMnuL7O#i8GS2DP`L9rKBX>rH_G6k;RhI2z=U9{Qy5 zBe5rQpzE-zwYU{BAT%byYcz>BwgQo1lj_L^vYc)0?dMFQ#_2jxmfPEB4cfNd*mH3n zm{iiWhj}#bu4|))hY9Uf8C(Lf4d1UX5lYQ(U-BZP54F&b7E@mKGfe#X ziU?~yIc4&}7j9*ZBV7nVrQvMRn4znkCP~(MK%;d<79BAi9o>KwZu5{bJ24IMNpX41 zR;$vbUYR^6!3%?&o>hn|gQ%8sl3I55>?EO;b-6$bb&q+l@dud37>}q2`XCj7iwrax zrq!KH3q#Gy%GVDi{SW-x`l-jl_Z0L?rjWvEj1&!s;&%}HLB%SaH&>+{o%J6AyKZru zJQ|`y-L=#<()twnSI}XU%C1B)|36^9@kw-d3{cce@!y|I*zUsStfPQLl1C_uqEi)r z@0{QMxs$EdFhuL_t}n-tQjKUpL@$0l z0xf()29*u_L!WvID1*4PVX~$AW)27xXr6{jW<@{_2JW3un912rT_>^OYx%bp%aSd;6Ij)kq*aF@xQO5 z728GA^3S&wTpF`ot>y1Rd^c)7Eee6yI@(H4aED2V^_V3o37W^F&_g)H_qHeuh#8siDd7c+S1ZJqD3@{38SjzuJ-vNJ}Gdcp# zUVtKVb8#r$T)fmj87Utj8T5Ji)5pGfF3r9^*$ie?N9|&%k)><;%WE>EDmZ8@O=Wn{ z4w{y4=0VJ~o2g06W`4(SM5>Ov6zrheg;Ok%&45Xa6i&~DOz$?*wQYbO$o%YHS6^2L z>%?RKr9wB0uzvyadh{n*2+`+26)J|12e8eeY@Z7V+T+SY*UsYInmgA(qBdRzH+0U_`%s z&k+WTNgg^Jl_du*iaW7PY0gR}75hqYrsi;&7x^+`ls`1~Po9#KTiduKn!T6%{ETjZ z{rPunUiZhzh_=w2Qi`d(;(ErWx8gilkMxq+F~9hx+e)@945b@TwZwaAeeBPYV1H8b zE=ArNH{FFt(VCkQN3~#+%O;sT!=MKbjJ)%@1dWgU9>#zCe9(OcJ0)1O_2hvO&C#^X zn*3;pc?O5kd>)<%wNIl}e86{UQdBsAcax;iAbk2lHE-=}G@i2I)L^~Z(_q@RJnFQ; zCkfV>?})9<%zP1Cw6mqE2`bJ@#&ksP2Cu?LA3FduM_#^IG`{oi;j~&Yj_!v}wdp}_ zs1=*|Hts$u>-Y{o1iI_Kkt^xG#-NLEdbn9Lu8%tk=z=i2&<8o-+FU+x{^3dFEG=~d-{YABT4S{w{PmQ4|!3I z(*HlsGwH!{wvp}3>rWC!r(Vl=T0bojn^ZGcQh}LEYSdgTeD9VC)9~JJ$;}n=8l80} zrhD$_sJ&kHs>p_swHf1OI%+~Zhlogg)h<4EtLp-(tR{pr6Ei{*zIlkp+St}I0#-Zz z7P`h>l5>3V;|KBKtRCjXhp-yP&oB+$e9!5Igws;O`&4d?8vbfwop^DiM8yU*mR7sl za$ClE(TP|cmTMf%!NPY=0d`FN$YlAF{<5fF*yT$#Ov8%kJ!*H6HL3F15;Iw-C=1&x z!X(&OcAYlkxg@MssD%USxL=%pK59PKiBv&CCuu5GNE;%6KznSjl_9N4I#hqQ_W&Ra=h_<9s?_qHEcBOP$t4|K1EneK=ghwx1}c$3&I`p-uRx|X1&NU;b$p~AOI%FE{!*-y zfXJq4USX%le%dbtvR^saB`UchL)=!~TE~Uyl30u|O9PdZ-X0S9W0-#0j#VUr&dsyE zI{EzxQx8c7BSIy!5c}I=SFf%YI2315yZ?e{ZxFUJ3Ks2^5b#8So~3JPRjZJdM%Dbi zEvwIzf|^CUr^kv-3U=2&6)O8_6HU<$H`_Bw>ebOfqy>FX{8FqgDaoI z_T9@9Ba<&;rh6)+cgYq9kAcxnF-;}ty=6d^?eaZLDgq)Yph!u#baywKy(#H#q@@J~ z0g>*O-kTDT77+xIMpC-Fq#OPhdYZZ#2TEIf-% zrC_3^55SybYy&*O>Ka|GFw9Y3sE!bIb$NM|)LR_bwvhiNKaf-vm~C-DRKLw5Rp`XN z6y_-xelN?8``*y}#qWj6PkWQxnmH3FL?wL7PJN#ZN7(lN#mtawa$xxM()S+gP@~wj z@Tl`rSO)({`<<|Z>|bQI?Ts=*9{M0ryn43?YvX`EV4pt6Pwn;>a;W3_{ZGkZOXGDw z0gkk}Y<+JEAk+2KIQEPlfu_`=^tFih*@4e3C)%C_Noa8G*tTDv{ygWLq8wsM6*aqv zE<>B3s5x9EU)#fxLH2YGGlzHx?KLu?j6|Y2=(`Iqyzfh=Gy%?X3VDWxGso9?E2Xyo)77k`VWf6cDuGWUp;?Urh=TpC zs($<^zQXM~-Qk+O=km&G9Y2pQ$-Hc}GYso~*DhS?B)V3D6%@_k}#?b z(IEi*IlX7|ShrBRFQZJ>89=&QB^6`%ma!{uZv66LRUbRS;j&mJQ_>Y!*vLslm#3D| zsMO1oz-m&L^TYMO4v%}cDrgHBGn1i37~|~sFl_iVqj$mJ9;07JCN9dWncRW6d87_Y zNJ$~x_fC{#Q)5ZVyTS>RKGiI^6PstMD;$ip?sg~O-Q%PJ6FE_I6y#VqA#k1H%-;EI ziD3|~4f|loACT!gb8fFW()GYK-`eI5qDjmEoPwg?e)`qhl5yDwdPgDU6%AE4=VQL( zfiG;KLB;xkdad|H;I~Gw>XSjbPqU}=xHOuHM{0n=(v3rwDNa0AnfV36?Ta~6%i$p! zCntIBU4_^+>s{lnwUw>`LZdB(g%KK6Bi+nn|2pqA!gv`b^=nUR1eh#&M6EHu@*s6% zch_pHtP(9;HWw_p{v5_^OJ418`8A03;yrF*0=HDo3Q!X+{i0B`UJV1{T46l_1x7CX zc5@_)ihbzg{@HEo0&KUIW(pX*T;d4)d?>K!2sy~=Kt;3!#Sxj})w9vah8QOTTxm#C zw~Xf+f&LQ^>Y(cPKnVym2Kys?f4AIiGp+cOnm|+NM9c>5f;KjZ<)6#j&bUk-!$dm_ z9B?*#UkZ~$;DlV7acdb>XijmV584@)u!_RNS-MGd)T?tydr>(4&&=mxem6D~i`mXUJcY!M7{m>H|6F*O4;KJ=QN92{*B-7xJ z_GkkxG~^pHhSMtFYSorQ$~^o6GyMxSS8CCDh5^?>u&TRN$yY{DG-|AqrGwM>tP!^^ z^E?(_)lA{eFarA2x||h_$?rx{3Gs!z%&Z!A=o=#I2e7E-%4n*oj;_x5hGUI^`_zc_5pg}S8KU)SrhL7R!`x!y!RS?wR zh#^MH+LOs(+#~aifV$oOGCL8rB)|`yNiAS=Q4lc@4!yj>glI^NnA_plemD1EP~az% zJyPsb)~v-Vn4VPOO|zi!YPpvo=($4{V!{tA3^CE+(oh;H;;m#6npaPtf*gNAQ7FcQ z1-Wm5u6+jCv`z`Y)!dmrN{odi4u&szwdIW zyoPdGh|DsG{EU~Ujs2lWUOtLRQa8oP7GbkaC98!a|gwbzUoATon(jHjw4l$7vPt+NW`9IvRy#x*yL?~eJW!Mm^HrsU|g+0F3q@YA)m(pS~1h=OPDa%#K)tlh`-Yb-GUx)piT`Mlry=nMozA8tTpUZUv5r1amG{7Z7Y0u&{ao6kuuH= z?uO~|l%^C__6!u}etPq*?kb%2nF=41qcHh{212VB*3siBq=Rs10!#JtGY5fi8^J1# z?d`5b+sPZvh-lN~kM$Y%r(UC(FF*74@ws=Z2b9mGK7A^6#sDKPd7CV%^`)>y>NnIj zb+HPg7UugcxG8y&COWQarDL)3bezBdZSsiDvG(!3l;awY??mi3i@nI#Zm-kW)B=Zc zl?dPvUt5jst7&hzi|&!u6K3g+UI{5$rYT_wP0#{dUtc5q>~ynH+}s_Co(DgAZ#dVJ z;+dRGV(9E;Tjjc{dW*6KTnTG;s7`*@KC97ls)=fwF^v*k6BY#@CE@;E1>4aFYQV1f zqUrn~r0m`X*09>_5zpmkK5BrOcElQ*OpqafDz3a)Z#D@j5t{Ps^KD(F7j+{K_~ywB z{LKGaF}clI`Gjs=OLmX{q%0LDC-(Ul$q&RXU(CNoM|o%je6Sgr1cDaY`8&tV?=oNO zMR8GNnNWI@+CU_mOLFUZ2s_{BFr9vOYxLyvibC1M}U_25TN!EGy4Cy0cwDirq7^QcK@;p)pxlF zrs1`2KykC(?`YzoV|VK1ByEeDH{;dcF_!sj>f-^ah|mDVd195{Y7c!MI!P!Jd|CFk z!*Fn&^*h;XoV_X=@5aSIQ=ulqsRHM^!N(ZuInv7asP^0w@Z4$W_gPuwx-~e$E(WV? zT&{&}2VNt5teqZYlaPnEqcF$-9mVV$0U^Nv{+hUZcQPa%Pb^_9&N^5Usz0*a!D#?3XBg{u2UDJbZ{$MxrqyA(8P7&;sn>bt!sLZ zIxTS39`3PQ$&qq4)H!7+eoUS_Jluby3L=n>k`gRg*rWc|dbjzW4ju@;lne`J`$E`C14Ji>SXyaOBFJ8xEefg`g|m|xzn#|nG&Ibzhx~m@|y5H3Ub!n z_F)EjU41ZNJ`@)brTi1${@dyarzlrXN7iMGFPjrOgyBtRz0O{Wg44V z6^oC@gd$z%_(vJZ$-mv)GE|9xY*8qtom&!+3 z*)ws;J`-L+xasX{KQ%0CXZOnVgS=pd^HRA@Q09mLn6^p2gLWxeKt~_*-UCwE+VPC< z#1VThz@B9BFrugE%clXxqbMy48w=P*a$$xz)lZ0=P2Ovx08c-u zey^uhgUFDxe$lm0r%1vazFnls4}MG5MTx!al5pR#`Vx3PaJp@HpC!dPMfdekgtFw*erJC_gl|tZ=ag?usIE+U zpD&p%RgS8(gY?lO>MRZg3j$?&SzvfD=m!IF8nA@OW3WAesB5Gsx!|d)9fc=CUFm~r zB@5Uh@u9dF3n+(aPbn4Lx-K|=#PQG<2jwHsY?t3@%fnv#>gFgCMJ4&83t{s*jY}I# z7HnTJ6V`YxxYRRju(^zr71+GG2NoJBtMyC%q~wK1hgzudtwr>8SQ83){tk7* z*EU>rbMczIh)ebS`9ZU5P9RU(47CuwyE`0|-)Aei$A88j*{m-95rmobp1hHCSo8K6 z>C#M-l^<3NJP$tX@TJwwh&Nh%;|DI3RnyRYV!ndChRir$&L zw0?tpfO%c^u%|A~^g}2@Tr^AWplGcgn=4G=kd&ZP{YR(5VTr0>hUG=ga|Ak^hAPDp zW>WN2DL-{X`FVD#*5qfF&Z5+=V~0sZWN-3Q<%hFRDIH9!JccK%B8Gd6@~^R>;=D`< z_1;t?9T+_1LmWfi-l25#XMbptGmp`%^K)}wx7m}!ATMl~LSp1J z{m?F-L@c?`@Xd6JP~E@j!6+Q+9-4We{U9T#QXZWF_zXACa+r=nOk_YgC6Uks<66?x z%UvI<$thuBWma0%!qvKtVS3E!y9!z5-j}yibd4(ea{^QB*BFd5r$UM#XVf83U>L*a z!&uSh|2PMmTu+M-GD2m{T}OYqgA7~lU6#De=!W; zBUt#VVeqPOC*U5fhB)DDND|UxV*7K1I>k|x&Q-0Xq~58|3o{`}UJ5I;(l94VTA|jI zADEbs{JNVZghfPVV|!OsW3TS$F>e<=pF}Jzp$yd^L5q>MiZTMBI#h6!QlL3*9E6PUx^X`>{EssHZ7B%Gb_kdcDda^#pJornjc5 z;Sp6$8YUeH%?kLqWqJeKAN&UtoR3wv6S((PUJ`RhE=T`&aBB4N(jN=K1Hq^$?}?Wd z-y~vFiFHzt;EFh;cbdNTgnn-d;Oopk@$JE}!xD93tpJG_K)u7aUzR`X&Ilab%gSQ7 z`{@5CEQ4em@juDkd- z8XUQUn=0qgwwivg*drJoL!%`eMTh%DjzIL9Ni?5v3s-rdJ)Cy1*@UD=Q1Qw1Lx_xxgJk))=4tO~Zdv1X%pgo4~9MG6i=;J~ZDy znaSm>$EUjz#|A=u>>QDKUxd;tv~b~*9N`q}M~fl4pfVi%3bZ?3E;g?t%Xq;gzGH2g zJ?|#8P%#!dvg0)2aGl=;#U{{=2|JXwcG{L~+1MzyUo0R1e zg%;&#J|*%ZQ@vMyz9VB@;0*7sl99@{2_tSd;WDJhV}5sF8Zkt1HHp0Dx&_fg8s?~3 z{qKXi4u@GNHG0-bEXkY2Fbb4~vleptJ63t!e&-2<>HlEYE$ADYd61|KR6ctp-4^qN zKTQ6QJq5r=V%i|24etL9x1igP+i(NU{qJ7khd#;M}VbK&58J|Gr}5!0m~*bs{hp+wuTz10|vw=034U-APk*ofi) zFa(&D0jc;SMBfg!aZx(?GGJ&my&tEG6CF=2LX`uq9H*tMJoIg}*Wpvm+r76_XhEWo zdK+a_eOZiN=J|sQuzOMRRwGf?%}Hd^v_KmAG;yjF)fwA!ygj+uCsEDdq{K6?!8pdU z*rH~m;gtWan&KX_o2{WPFiz}V2wo`fFYP<~?(oL$aj(0jACK~KfOJ8nK*z@S=90># zA#ifBg*$xB8xy9<`0#`Jj{}9DOK}5tc77`BaaCC0!N(r`aT~1-Gv&kaiBcbP!CDkGN%M`$ z%b7(-kL%-l=B=IsG!Zt+3q;=*zp(&tBlSmk%_JOEY?7 zG)#1E%0SLY5A73&&5_$Ja4~~*v}55_aSG7dg*PgsGN@JNjI;`-n!9w&$_S_xQ_9ig zsU5oQ@<~Nd0{h_l#9K98HQ|6S=gHm+pJp}_5D;2%PDB(V;ouw8Z9(JPm#D|iB zxcK-=oJGFts{%5(~;Hf%4a$d1v^A_ZI4_01#;zYA}J zT?lxB?x;WA!Z3U9{f7fI=&QkqZm6##57T2*Z;rlBE~lCO4@L+a02o2P{MudyeZ#7Z zTHQg^ufM~)8DKwgrmzNuidg6RDj+1V|8>~0Gbh4Gl6YA`Zya99A!q= zU+bTD2VeGO~Vn zu%7HFk2K!x-)iz8Tu9F6?MOoat(qRrT z(Jk7EU2D}GX!ppt_t}U!idPg790DI0FNX+^3bm*TNWjU_$}}xKi!P*Suc_$htr!UR z2{8qP$y3ZZ?=5vctW0Ms*gN}tnUL~5xSjJ zN9E|`gdi-Q`P&=urn5R=5)nD(Zoe`@VF~nBJNF#{IZNzEQSJSYXo!n;3aWPPmAk)8 zfv*h=qZqnUyXT|kftixyjFx|;RYc;ghY{oCS+UMY%k4OJ<{2HpWKv@^3{Pn`Zu(~$ z(QDmjy)v_0QE_%CF$`&9hTCJ;l=KqqlM)iPq~S*v3n^{m4pH>5i)@e2lQVwStG(-a zCsY?p3g?|(znD&-Ld$&G3uSt+f-y56j~vEYsMauN0z)OEQX*~a0?}tAG@Mtc=?AlK zrO09k86o&cv8^|n`LQ=`$bCki+{AtRc26!8X-eP47vYCVD?&SEHHsD8$@$k-?O4hc zOFJu5<%#y(rYA3^1(Lj2`=fa}*H(EA;Hvr#kuNwR@k$}i;cE?DBb(YJ=PoGP)z4l_ zuyp5C6~BA+h%P2VgMm;T_I~DxNV3LATme?;D1YS-wJ8WTRmzS*)8%V1D5@s5W(<*XdxLhaR+d83QDyzrA3tJ_N>-Puuso6p zMQSM1sBplR%GZO(c3XsXypE9NiC0Xy(cKGo-vFsS^Z0saAKOUA=W@04_`0{HAf{;G zG#frMV`%ttKA0Be2&{E%TqX4ETv=45v0VqdDNbH`VkKb>J9D+eBT>JIkBEs#%;kD+ zG|$8MHyRG7+Ydx93|f=It9frO5-jh^*Na@m`yZZ!)}4GC$gN89vfT)}Iqm%IuiA!l zX1BO#L~yyCc7HNzNekGo!en!AFJEoHrp52{dkdniUC(wvj@(<`M9 z>c+M%vEr2CoH3Myn7Com>pL5CB-pX`O85nF;hb-ap&HK~2Fo$dV7yx9f@ouexyV?A z`BpQT^}O}KRkWwqmCuGFyCh(z&d)H(5;^)^C5(0>pwqS~^86C~!|nM>N$SqOzBmXm zs8_gitAHDJxUKn658Se^Wxu3b1x<#&mhlr_1f_3lon}d=G(RJoo3YsNmMk>)xmhvO zQ~;B3Do4l1``;+8qorGmdmfXMue^RBYW(tL-Og&Pz3^sCQD5fkU+g!-#6=xdg#tR{ zdlVFWn!a^cAH}LDPbLN*Ms4E&=1suri+Frk<$MY7Pb|*^$03+eM|VMFlEPx z??ClNuOOE>unb{{l!l8z#XyKTRrgG>-hS~R>YE&7>UoY-L~h+33BLCs;p)OMFE#a; z7#0dP4|VATU?x-h$#%uiHeqVQ98P|zhNt2A{nHbD&2=d5!CgbNRT|SW&5e{(n5j&^ zD*PU3nPm11V;C4r;Q1CTW@+m3NJe9A3*&<=rr)$dUASa>&6y^*_z$;J zN4P&V>@lfy+mL@upWX+}>SO;q z_t183xtA!qE5hOpn0ngeizdJR7^)Ca(7wP?OjN0`_qxmeW4Qhdd)b9l>OKL6HAZz1 zHFr-igd%A_$95=f0~+`oAziFfi#a^87&7)gz1Jf24r@&{1ufaE6eu7IOWFwq%wV_**G@ z)667;oSJuPf6J#v{mB4NX%W(c6Y^n9;}Inf1*?lWRR)g+#~;tkI5E1xghK73I5L>` z+Y^-X8e#aI3>gO7)(BaT9ZU2gTw~pbnkmj*`J9#m()a72@nktUW(Wri1P0l|u`DKL z-*orse1}UVy&dEai37dV9-rFcF9bp!nfY9jU>c8`*=c>4*-vFwfH{iNlRHUi5V9GZ z5v}%k&EF zswzjy6c@=9Q5j@Ck1D589}#S_DQ(ZVBVU?yp^&%Z-?|WRp(gdo;UvVH@2R*gZJ?eH z0jgTIA=ku}`|s_sD~ec}$FKiS{7S5~`*RJSi03t2MgGvE`o2AJu}LbJZzs4NNC=6Oy(~~NF5#r|-NqP##j;pl)ovlsci&{r5K?mRAk(X=RFrRfjfq zMo>QW(cG5gW=%zj`vtkoM&@+mJ+M^Afb{7>hqGmk+Kc%v5>*MSiyEXD_4H%&LhXdq zUSvMhTtb*S%R{8#S0>qy*kCJok&*CE8X7Ck3?IV3b|GuNyYa~nAGGMuJ0`)954liy zbD@Ez(_V1|%R6r-)FFJC56?y@iD4DnhALZzjE`&B3#rxXw{YivqP@>VggVA*mP(~O zl(#YACXL_o(B&75n5&2@gDFO#MYD`j-(DZK@n&50(*}%n4Csp?syt~btQxz#)*Eld zE~#cuzw=!lf#g}cljqSnYDF8S=e^CEPX7h#CCX9q(Q@pnZ{0S>;s?UHd&6@V&Gr-i zQ4MTGNk5T++pcBl|qL8J+}kmy09C zsL4(@btV@vQ>2g_0J7fy^JvYPKkE62O~u2j_e{E_3U@4qrw)6#f67PrQuXV5;4I&% zdzRL{I5c(Iy4-S&CNV_$@IAg-O8hp;a?aw>@!}P)m{@5fr zn-nQ4N8saZ(h%6ND5KJyhWqqt8gJ0lifv8x}cso|ckWfW{56>YLbEIZ;sX(8mlGu5lpXKv|QJ2qp`+Ge#&dQNLCCS*&=Z_T_hDcd5~H1z$F@ zLZ&-f;{=6IIy=^_eWnOX+AE$T)MU23x|bY&R&qonh+W`B#aC>Tr6O{eymDeFQ37*F zFetOiIr+3DG?>7E8HC|s7+R3EUU*m!f4fVl{EyA-zL$D|ixy61M;IA;YCZv;ULir` z$5H@R1uXtL#3vdh<{p`x?0yh%Dh2Qk(zkElF29LgqSos>j;zY>sM+ElG4m*_I|VE@ zQ$6X=Tru`)6LXwW-kU%E=Kt=^_4>UVlo9Je$=@Zo+&+TqQ$;N!t|sI+{)si)L;&ID zbPDN+F@r^4)BUxs$NG6Dt0le%GEDtV8|4%kOT|aISy#_9w527D5t6ZyUeN{RctiNH7>x2Y`nPA58PS&PK<7=39lI%gFQ#u9S zG`gV~{AA+f5_!GT#)0~5K&i<3O@bp7?`!_&L+M((Rio0TfDf(1Oz&e-u6p9&@DAc7 z>pS{m#D!f<2}L1g<@-%9Qo1djGccDc-y`DTTE)L=f)Nc2h%j|=4MIg)32Hp51Vzrw zl!d?yACiV^0eGY;H_wCC?(H9WbML@e)ivlRJ^g2Hn0fN0e9t9wg2pDACwzwiMEKo` z#eTXuKW{Pv(uJjiN@XWl2;z3x9*6C}Yjr&qX{C1IdGHl0T5!bcs}bMxY;uRW%c1Hb zSeEGZ6-#5*N8WsS19D3DWBugj^G$<@Nc7Q{E7MiN;)@ObBR%*eP7cL&U1l*mO69v+E$ZaMB$X-#)1 z?CCT)E$#pu`b3%M$7v7$_Bva77k6m>^iWDXJtV6Z%`6gRA*bi~W=+FZrC?)cVjczFai~bm&x` zMgNll$TyVm@bl{7%5Xt zcm$ROLF%jX1A>pQQn_Kzv~Z&EjfuV|rwBw*x3-yGd{n|A5_%U@HJmcDQgx!Q72VsX zS*lmpnh(hY8*Ct@S{j7GFR3LsW02-yLytlfSRT@n48I_I+rsXo&S0TH$K&=?rkc~E zTY!^sKnU%oL*H-W0(sV8s;tQfl(b0JryBF4U`GSm#`;hppSe?Puf%eSHiqvBtfBRe zsdvQU_z=x>&dsT5AR-bQ|8$K)WAo6m>LG~h935^E&lgc*%M*gcSvDhr;>QYAz>3$! z-ZXzRhny*1+Jp>X0?4v$Jl}i{G|F*sPkYm@GfX>I08Q|3qV?FBx#0Zx6F+f<+mn&O8r&xWmHc_>we?**&mtf7U=wpSg!5LPK?u>xB8Cbc!438Cw zf~DU@Zpq}<}C+?7vGY{p};ENba9od zdmR%|hjcY-%6VOWA8dYbi@plS;ZI+q&E8l{PmqHP%J+sJWnHng?9j7_)!96iu?1oL zN>EhImkIDzfPinT^+?>#ScJ!n3Ym%O!58|C5#*lQfCxdMnE?@nohSb^S${+7B9?6+ z66NiV6ti*6C2E`DTx+-6oHPJWQ0K%HVC`tXv{)%i&ckP1U<{OfCGA7_A@KzhCfMJi zKLhqIlePHzXU1$Unk%8?KcXWUtWbhAK!o584`F<}u+&kV{Ds3r#gX=#rjKRuoOQ?i zHKqG=M#7sg#7f5t10PtJU=q=i%K?-L{mo_$<&QoUFyhis1_mziFy8$350;|pQI9oN zZ-*yx^x}@#hFX_|=E$a`o<42xxZpT!2i~F7I;#{tyWO{w+WD<2F%5Yxx?1@&M9)=o zP%hJ~Ztg!2In8eN2WX(~<=(=b9r?{X@H4v!s9k_eW*yQHYVw^oRYT3UKhfvwxgdz+cZwOxIJ^We|gUk+j6&n-f6_Jn`=6hhCalY)KFGW2UT z(>;36DJ0Zgm$Hy`9vQlg;(Egx34~WtU;S9#R!!oo$1um33 zIq_$T^tS9{1d6``n+u3#f*;gU97m0biOeT8@}+*lB_j%YpT*fAx+2@u=;rg#GS3k7 zV9lcpV7nb+f2#QU{O5T0b;k2(lp}wM|4E(zrA|dnDXc+NGQ(arP;Fne85h~b_TLCg zLDedoxN!L2d^0iaq)f2IYg|JY_L79Qz`AtWy8AIsDa#VHr~> z9-j;x5?8R(I%Zi7(U3q3D|&Zi&U?51?DN?7C~ZqYo;})zGP6{=d3EcbSl<0G zLUgWSi3Sk~>YevV>8S&uZJ3lOORMY}jB3++C!C8sT`r&Xvj!Mx`I^cG;5+-094X{_ zE+vHUO*19=VYzdPmClaL!!hE!>@FU2EDaybLXTpnJspFAIq@3Yu#}p7|;%JJ9UIG zF@t?u2;f*0?}MK*=*d83WMDAt>90h*4W{@53q_JA<{SHS@Pvs|;@WzRF@=A;04MBa zvP7osy#B$#yw~GQ%Y*3@BnjQU3xbApvNVs5@)_Uxj~z0!7!wxiQD&$@YE;6P$vR(# z={sgh?ra$Zuyesz-m1zJ%N##^7}N=<&?i;vv>Q*a9vu959d)|-?lp1!%XLfmCqj6s z9IH6y=SD{WeJ)Ul<~9!v3mVCd`FE##F~8irYp8x!(r$S_T93UK_qNrn*3T9d^Zu^( z3GIK`5Otd8QQ&BMO#dn=MG}=Uz5+A#KeST)tS%d5_0#VL$-=43VDn?aVCG^e%#qM< zM6#q5>DgSQR4D*B0v}u$Z4eZZAB=!HsWPFgtge#Q^S(B4x%U<|8_Kw}iZ_DPBL0H_ z{w^q23jpz%uKq_^-{|SkmIsB)%uu5<9@A$%meU{vyTBU?ruXr~bBt4MJa#5z`XkXi zc)kwE6E#993Deu$!{e1sW}Y?=Yz)o>6n@aGYzpwRw+-{&T6^Ykvb{gnEao0- z_ifBcKs9`L)TI7jnkdU@vU@r5uEJYvN9^mEuW~n`dWAxcb`>M&w(raPgU7Gl$V?t% zf}XMa=ETFVpn>f{G>Td_FPMhQz8rk)k7DBK9FnUUus_RRM!Nku!8=5tH?9LzVw7et zP0V4|z8Z&_&1OUPb0}+V!|C&+7?SX@3(O+B5_e!EK(;39pM225 zu%^O~Aq=lnPuRc|N7l}^34+*=7v|*5adHZ`wf(}TRT>znK%E!F7}LcqGL=9uWiq&% zD8ya6q%@$6ll54*LYdLOoZANZN*E{654a})DxZTy|C(M(gD65gU7RwgRe0|uVehZV z0`ztH_XHT*AEGH*+0=%YeRQ!hycL`uKf0}B2<(goE?%%BiJX!rC1v3Os#Qu?iKvw% zpBqwFJ4ExC!A3NHY^K#~p%G;Nqt};^pGWgG-+YNSsY>kJ9&#yP7v5AkdXA4S6m1TU z#dGnz?e|amJ-O`(D5ddT{F6g(&<~27H2K|s5M7z&GZH|s#@6@y@)v0iOs&BPut@-e ziy1ZvzB)UO(y+>ZIk^Ey&6rQT|Ii+>TBg+laDxP!=3hmlfn#gO{`5=xUdMuC?AURU zZ8mmM+3-gNq}2G4>J`PmmMhss4CIWL-}Uq@;kJHg5|O3Jfqb%876~>X;aBxqPV{dU ztfte{t}PlTQ?_9GwXC}zj1QBe!W}5ma)4GVa^vr1HmT$0GZ+@dBW#l`Q_IQ-CmZ5= zh_N|`*iww!6@n%SG%X|Kozj>;J zw$Z6cU%_Jc-YSUnIOI|>#`{JC@I+H0GmpR3mMW1|eL19f%%_>Y!T@F7l$G-))r>%D~U<8HQjJU^%<_f;aJy; z|CiBlQ;S^zz2IMZI7S|ajQ?ve7V)U4I(7YnfwD|F5Nmn&ix1tKHoH#pYfwTuZLD%pyYEZtHKzFtX@npAxq^#nT&N^*K8A_;kldVu>wVz;xJ-_`nRp=$+w&F z&TEn|{n$Mege|0Wg0S@aRLOYJf5bMYJ8@?T-IYuuRzUrUbh|q%;2jZUfp<^t5(zFO z5k7nYj>AYO23$P8={2SeJO_-#(tUPa)tf`KX`^c_FowRKk`l7>2bt8dqceS-Dst|p zcF#r0`@=sZ-IdS4-d$~edvR60w^x3pMbdc{_z$ZFVnu_04|F7qw1~WXb*qjm^{Hw= zKvclsig(gIl9=N(dkoyyZPWnpae2rPwu z+Wx$uoxE`B2hwEJp;%8`l*qj~tl@TKh-VNX{Nygca`8qN3C*H^;79Mq;DLjWz%9+N znU(qV^bl>EahJm9DXpy@MkJ<0or%>ivSG>mW0oHI;Iw&7y9x~ycjs5}ghi!c{}Uqc z!?dx`0w}#&s&Iqodx;L_Ms!dpMo2?Jov2F~&IsjnrYsDK(jdX|(9FX!@zR(4J>@nW z2Z~2)+yLETQmqark?r5OJWm6CnQ`+aSAd_e&qVXuEfM7^d`lX%Hm~vN zDOU`+XvJ!c6!AS0>6Mtt+io5bsIil6{ty&*$7b@27JIa7^^^Igw)a{SQ&oU z-(r5{858ggA0+47SswMVF3$jOOhlc4w)*uv)&saMPnovQSZ?ZunWp;S?@cmKvvsNN1#jUmZ?Q|G0H{cuo;NQAV~&HUn=s zR*i!haW~C^Go+J$CMKTDa(bOKJQ<)k~k~Z z&#M0DFc?P!f1q*tP8nK-HB#fFb__=|Tieh`Xo@0?ifeCnW520h6Lc!9@ERJU1K zE}z6~Uwyr!G;AgpRnh1~JO{|mgN_@p@*l*E%N|mr2rV9I_)?bljOC9meFc?x1^Uus zrQVSki5|}x@lICOT&H+x2N+OZs%B{&!InRenM8Q-f?c_4C#YvjAvF6$&k9b=Dl8%w_bFKW~G z(;Kye!3jE+D~x zb6a5}PL#~5WTXAf>%9^oc}1`OdiTbqeaz;KvcE$s1&h=|-qk5!R3ZP{n`f;`K2zf;yphI*V30J#9?wc%Lv zWAEA%Pa$6i?PQ9K_TGmWcTAyOf(@3q7!2{@w1DAj584icwo6E~u=yOQYi=`oY^~+0 zGpI8VhRWtLgtz>DkDszGoS;ZP23Lpw`gP2Gc1CIx56+JSh#5X^uBCUvu@wpwkM{Fv z#A}rpkfqaXD^>nr7Z+cG?K)%yUSXX!C*#^Q-Ra|R^AN43RX{$8xFNJk^+7fsEmL%8 z_=7ukGp}l;TQkR-`CrCVp;|rmH)MqHvH#oFdrI8YxQCgICS5NsT83*rx$7H<2=EU2 zDz{s5?tPS41W<>YfuYSN7oY8mBmG~*@LN#4LtjFBpq7UBkCyVMBNgUxubL3QBVYY5 zOv4XbS})8>1O%$?cc7j$G7b5ktSW*_9yiDS+QiC-@f>b((&Hw!)N0Rj9tR_Z6i`hu z#mL9IbhfY+lY|83@W(4ztgP%~%ttHc|4?2%TNtE?)8t*=@5LOpt$t&WNo|zeQRBc3 zqb0!x+>#{5L%w7t%r)5vV-8s>IgFuUXPXWT57))-+HQqa*52Xc>-{e>&OKCTv9x}G z4NRUs&AgmzScjkS8uFV0@)1za4$0+xU)Re1^G+nOc=eHclg{O)HCiG7YExyff)VEZKF6s7$*KjdvD75y5)K_Ht>OE#g>P8O% zX7Dc}RZVv$5`={CjOq_;(a1Sj8B`Ap{KK7F}Z3SsgCEF5(7k%+;8<7?~c zwHw>^0eHb*E+ghH#oj*+=wrVLB5;9$%Um(00o?h!Z6-(AegQF9Z=7HJ_mxq0fWNIl zxnDVcHL!!&1&NO|_&lSyzT+)N-&$rb5aSKho`7?f)^S(7>)n7g`{T9*GL0I zZw2PmpF8ERffMBB1*HvZ&QFHP8i%-B)j|VgvDwQNJ^@!t*B;h5d79o^QPbSvKX7gI zV%(SYmS@|4GplpK(tu~yI2!h%SSOfTmQs@CQ9jT_m=ckQ0wWFBsNcLU6JQ`r$;imx zfXcVUwk_Oas*w@R>fpc*k{~b`duW->maeTa@QL!n3wn4Bzd?rKsvUf=ug4x)rPG+f ztfx2O<4Qm4w5!4O(tC+rTz98kK2edt$)Tt}^qJUNBiOX}h)shSYq+zAFo1-=8)}J& zDbyzbxhk<8#a-JXmH1#)o2Y2x2W4Q?HQo`q&n>22bmyJ?i_$U#0A8o4vZcC0@` zp0hmNuP;cyFeHT%-hn3ktPuaU{MWE>jPx7Lc00$f*ZMZ;gQYgbsA&EN1vgTGc}IVf zK42;Yd`~~#qFbz6Hyv*pZCY;`;CTmIOZeOnjdcSJ4lhe5-`nms|KwGm;clI=PDT(E z>XjO6>mvycGP-@EG$w%-8&ATZD!F>Bc-R=2!A$FsY~pew+|)6Y%P!;PlIZ1OMj9~g z-Zt!f8z;4@s`4jPWP}bv1O({Z?unrS?<{FZ9$yo?O_z0NRp<^&7G}Dj-}uQEciWBT zodFlhb6PjV2mGU7Kh3swCegvSUxLT!jINNhG0SXxSO3RzK)ifQli}L^K^eRDz}x}k z1zN?Oz7T@hQfY?}aTtNKbiJ~%li2sx=KDx8Lj#&N>2QTN299gwI|&jp^Ra{J7fz09 zV#gq75zqZAJX{F5g6IJ)-Ew7B@!0jOLLwC-=isAT0l_oMQ{x(xM2aOREzHO?Mpbt*W8rv|&men6&D0kk zOud(O&_YLiabnlAqHi&{aP(6C$vq6E-%++43Ln-CZ+$_4erqVCT$sri_@gLHUyA#3 ze$FDOkh-2YJsm>}xUkebrW>ES&1CZbwZ;h*J`{v8d#4VyZenJ*_&kgq-~D)M!($7B z8_7B1-ovS)C(PMsE7Bsfv~tDJ0IDgY-opYoK)S#Uw6l}ZX3A7%rVbaxs#3ep&Dy%P z#o|#f&N3Q?ryOgGRbgjE9H~iX6k$qqAM@<-)~^E13Rj&o=aqu76W2CNB2bbC;J`yt z0**StK6Qxg_}lK5H~zs78Mz&yZ<=D)0nI2N^r9xSu!!067h0aY1I=Fy;T9$?V(GUo zChAyl`r^W5KZNhCc@r|EICo%j=-ZF?G1U*yBZ1fDyL*nO!@pYi7OB z@c5p4^$9yphF)xs;pza~ZW7_Emq~*g5z7+%iaG(&J3LHdF==^{iNn&p@KcUL1|~6t z@<#Ge%c~#@0fz9P9E7x)Ly_Ja+iJz#NE~mU!Mauns+1+g1?=B=Br4O?s#(Za=Q4YgdZgSX(MBt6)6#U5fU;x#OP^_7cKX?~421u^&E~@~70U zwb3OSWG}jVPj4Jy6#V|Nb_UBh5kRFF#O>2&S##ddLxeZw z($uuP8sDjEkL8Ht)Zh8^m~z_>Qe2eMVLRI|x+_V0%B2E#{wcraup0XxBNY_nBN3b+ zX6OH~;7!`4U{bX`4(9a2gM)|E-?L+o?xjH5f7KqKbiCcutnvlSGGmM9z7&7TZC~7% zb~W~V8lB#8C3FQAVlOR27$&~?FS*p!wb}`NEB1J%bG_dgj@M`Xto8q>@7y^>o1Ac}zi}!p zE2i0~Sg>4}!L16tv5^~X%=Xf+NTlBtZV=ZF;YM!IGygP7BrKzif_@&f6)|IKpql=f z0+-QV?cHAEeR}byyqdx%20Ma}J8Cb&nf`;xWK zoMO^k))#G)d|38uPxOs!JBB~UTtv+xTU*oewB9~YJI8R<=Uu;9$(6TmCW0~h$aE*B zib{{E;5Bc=7E1@~?mKnfElTB&xyj65aq(7I^mRMLe!)^a8 zZhFTO-iLEuU%J?|e8b|tEKSl)K~de{0y`S*%%QWPl*H|nN0Fc=@MvgvSDWaN&yjo8Dch9{fV z%&Ny@Z_AAODr}^y@i4W_VmzXpJ;O_r(VdJfH-zb|r;g~CPKNVSePI@SeupSU;BETU z?a6KusdPCSpD4u}F1{roI95a~eQ&fl5UF2qAk1n9Da{05@@JlVon_xMRM&km=a=#} zV|LvG4>3AN-P}O0cJcYO$Je4ub<+(C?C15Kvx7uG+qv$pn=QSQj`gpcF5Wtz%yEcs zc~fL6c0~UO`@S;&VZWCzoIM+Rz(uP+N`)<`VHHn6tMO^7F$zVI%ab|TG#n3Lpm{2+ ztz$i4US-W&9#7sqJ()9~QMV*gI@oaN3D?>0!v)nA&*lKQ`}?S+s_@zTPiuZA0q-eu z@SQRO`YeF7s?nEnHC}#$X*!@-dcLt4w8wgHX?Cpn`oHC;FLO*f8Tggf!a_JIx#r{B z7QQmQgV*9d#0eU&thQmhsk9sSeN);~<|3^3Yw3|{n9#@G7GCk_+k^h!r14(5S%np~ zT+c0Zi6$hiScB86gO#xDCfe36Jx{|hY8}^}L4jfnT;+bv`+4wo9rY6zng2siEVI)EHa6g&^ znHnw1&TY(hA`5B%)xVHwg%aCpHKV_W`Ch)@7Q@2m@XO*$#~#CBql}@%yGKrI>dua_ z?Y$X`=}67@KA=z>Xa4O(Q;<$&p-J4v)2iX@q_JO<)As75nV#jZmC`h(;Pjqk^?QVK z9$^Sa8Hu2Y>XE!|zEa+_+J4jeo5hn3MsZUA=PW<)%#=*K;(7^5V-cUXRfUY{l!*!K zpX(yOpDv`-#eBCgJN-wsh<1=ky7PCLG3trj@Cb3A;T)!*%hB+nfkY8)&tJO!wdc^p z8@K9)ImsakG`R3c>GPYHzH0}$xZ?g2l|Q$V{-r8JJXa@ks{A%&dTre^DtMy?JOByf zOb9N9Uk;#5^S5_jgjK%~EZ1PQ$_vZx{eh!3DWjq|az!ub)$))rAD@$Y?_)xS`ED)SPNIU zt8RVd4*u3*dW~qwi&OS9`Ywkat-OnnFcz75NNA=c&S)f$_WfQ^_+_J$##YUg+k8bC zD05_SDOz{RHPATJk7s#t$amB-yCk%sJVjFgXSa?MyN^py?OS$`(_%GT%dgDh?VdcI z6i@S7_`5|}rA{>7xhidq188BwG0kSkr474*@%>r&F@&jLi%diS8riP%;c1K3}%OsDv5-$#{ zCDl|4x`~#?=WE-f>2Q=SSkLI2$b0T#%2{B*NWgc|L)gI6Vzpk z?BzGF46@JYsD0@hzj(hZ@gxfY1hW^9W$Q4heh?lvYj|-$1nBShS%)sW@kYZg^33$< zX|)(aX#euYsoa}(>u^$s;4hzWoCQsu?tpD9G3JESnCNO!~ z1KFhEN#u*YDemH*xNq>R`c z_eByX?GiA$8BPx_6cP1ih}Cv%vW20=&3JP%Tec?Vr1(QW{Wo^~8pQ(B7c)zZHCv_P z{Z;Gf&T4tf@8<)5r*qqJuB}@;d+5}96Gu0XvdjtAXCeENqHmp3*U%vCpb2&P`iczB zelBkBk5n?eUt>4p;k^nkX4Yu@u(;{f-Y+D@#FSK<);+eMw=X%5d1SD#)?f6ldcm=O zv7c{yzqZPr2cFdH-T&zEHAbuRQN*9p_>EZ&r=?r0uc5&%%WYcZFZ@kUo%1AYtE~W0 zLn#Ht4AQe@;_fOJe0gUUox@N4b6@(ExA9NjwT@{raYVK8{(MKhO)+EDc%8}@I z;YCp-S0}V)SjLAFx{T|V@avIh3f-HJ>LxOH-v9n}ybK#$_;uW6|DeDva`8?~V6_yQ zh$NWU_>=+#tLwT(%pOYS^fmXTbHksO*|{!@^thBrkGXpd8laoj9at5 z!NAagQo#k&AJZ)JuGF{R>?3idMm176TNmfy$@BQ~0neCbwogMds-#F1A|rAwZ*zj7 zXJNMTzdWMg4(`)~<3G+)pz4?WwR#R6tvEvBNiUkd2Wmb>AG_?A?G&gN`X;lhj~&bN zBCk`M%9vFkv7yz%*VB-LE8BUD_g4#6r*BF0)$wxAH)cpocUX@6mf!pXSt<5&fnKdv zOFi`!W=k{bDtrqT!!LbFyG;=yK-z5@LXiq@TeX!?l}H>!fQ(R>%-NR~-u}-FUu4w| zYgojk=_H0@u*GX4#;GK4x@Lwcy_?sYn!5f5Th~yLo2$pV4}YLcOgye>_j8%0d?gcM z&A-|)D>c^bM&x!Vhq~Wz9{kslMb;XtrnNoY$3m)KgmI}+=Ik3g^SabAJIBewf^O`L zz56}lC+56M`gCem{Y9q<$7sY1ptWBGbQ-R{(O75W`XANr?&f!YbaIUD?T7E^!W4R+ z+a)53U+5Xw1p^13H^+-wN_d@M2)VKAKT1~O-5TVDi6PiZgBWn7YC8#we)JkgMyzKQ ztOlnyaKd-a3|C&1K}`>Djn!?f{HLWK*PMx5Q_GAs^==j^AJ&Z@Z&>%-y1n~1*jRq? zwK8XvpwE3Sc@qYPBIKER~af1?E?gn{<(+D^7XLc~`!6x&PX0YFXH_ z@#LHvLv!h2_tLdPe=W|ip(c~>|GJ$=&^Gg!KH$T-lW93dd23E%7gufPvyB4%)Qi{o zi4P?Hk&BNPBRU_>f+~@H^ zkz4L0Ddwv3e5of0gLS-7w@VC;SdOq;gKW{Vyb-jLi{CMM?jVz-Af8TdVA+0YxfAqi zn2M)LZj0=h>wQJ*XXS>U3;(&Z-f9p7ZWzQek2>59fU_^i*xgK4e|=rH%1gHLJ}WAz z@cF_rttM$AJu+71lZZJ-DV24MM)LIk0-9il&F$#|4&Z0A8|-e0J&L^6Vx;Q_T*09H zKkAtHXl#1J#qgHN^oG5LzscMcND(a%Z)4`NM+QB_(qY3fbP!Q5&mUd-?TPg_z!>U~ zsXpuH_orjR&9`08;>7(fxMwBMt#d!_+N_Qz2gwA`sV{4r-Ca!g+G0Azi$DAA(*hEc z>*M2bxGP;MHv-7yd7u#++T@@B0o@C@!$f!(2B$2{ez_iT`qZi7cB3dIKDM{;+RP7Q z&Mj?i4gh-?8h1yvTC5B$y?if_;!{fcPx3XvYJ_9Dkl`Zgnnypqp|X4PX+B--^v2Rf zhl;E3C5?r{Kj)AVQ9+5k;)X)P)-z^4w&&&Zj?(yk!`oUvAP)Q=CBLKMJi`=Yygeh$_IPOZUjygJN zxQVY<2v$d^J753Wl-$LjBWoP@@&xCiVaI&mhg8|MXP<`-*aWLjnsvRuZp|^sAzRdZ zLOJ#-DWR(1@clUiRFSQy%To(`vE|_*w<fs#d$&esJ!+{|zbFZ0)n#{3h4$KqFiVz-A2?57}QI$*ZA z;b!6(%H@9CdzmXwbPn`7=bdn8Eb+Bd30hZiHtp3Sy#RUaDB}!|#*h2gGKX z61cW8WEA995Dd8~PfTe4Xq0RDxbsEzwFM$-ykAx!nZu!JlkZ{7HV**z>A$4T0U8ir zv@#qwzujIrKvA-JzNDDnz@Mh0`mlHE%TN)mpfUbLY3-J3LJxJtm=zsEj)Bx=cQ_BhF zIP3nqfz`}*y9yADcu1H(%JwweYcmZHRW&lncdl7lwio>Rpv8_+$J@ceu=9KpK__n z1|4P`Fh9B+b;rE4dD5B|H@86M4YfYf)~T?C4P3ci%<~oF{{3isQox%<;k%!oz-vk3 z{jzwhvLwwlB>T=6$#hs9J<-|_^{{H+#C-S9*2TmpOC!lKAEPRNaA#UuiznB99r=0r zqHu@#IX2aq53ZJ?Y6ryju(Ky!y-Y*|DTHu@x~3%vez>Jm16-_45tL+K3k@Gi&QyLTTQ}pSYPa$kIUJ9s<5;KtbaP*Q zyNz7w=`%`d8$SeYEqHL{_pAv;&L$J}iWX=!*Z8SE^PY2;4kFN_E@-`tRo>OrS29K9 zvc*`mx3|~R#Sm`W+1r~tI?};W1V1w9qU!tl`|068FXQ`!yVLeF{#rMM2(_})i-ef_ zhF5io^^c?pLC5sGWh1GSmTYD`j+%)a%`eo4>wWzixsEfuN`tj%Tpdc+fSVFk?qP4e zy4!`0A1xH?Or>Q>Nk*ns=P^F|L9P552kPFF)ozUi)YbG{jlsyhAIS1%_Z*IjKZW7v zE$V5ei@|>y91Od9>H}Qk+vAXpN{my7U(gnl*wcM$k{#rny<;hzt?<(3xQ+4D@eDZh z$0(p!_eCC-n=w=37hm`b*ZVwqALnVb4=|?QZ!RNLEgiz_-*bxV?Gfc$6?ig1O-`P? zC)An9(;GUM6mo1;zfN!1ibbg4@v{ZXw?6ux3_)0q{4skM5aHtKd;Nt@_XNwAFLiF|3HSTd`XEu zitbkRmkD!Eev;p?Au-Zk{*Xq?_jZ7Q%x*$sl$MhF3Wq9{Ci8)($#DPkdSY{(y8Qlx zAlFTs*t$Eh^`7J7?hZ`MeI7x2GG~4eCiIat+Jc|@<{PI@7SCPoBv4{G`KUVHKmUfO(B-`7XhOYNzTx{;XaO}!sV%-_Z!fqoY9R*qa)lbj{ zwCLF_CcDb2KPN5vq5Jc?RIF!YKeKm{TMZ5!V0!K<2&WH!$TOz(ZKms{31`d9_M2yX zv04++J+;B2w4&~o-{ZB1up!eTJE)NxMDTk4dxBb1AYrx3^9)M!thp~9MWNu|nxA&{@Nf|p)Qazica}Gfpk2IFr#l7*hs+*i>7;E%=8F!yC!AF}Pb;)t z>%02&vdF9Ci24FDUoT_kDwaes;=wpoaZ`JCzZGSoqO2V5w*Ya$k1Ve=kOqaD8PMJ{ z#geG_@OPmcRK5%kjslIQqia8oRtq1%$|{ouq~-W69H2+jYO)H=vISMrp<5j;Xl-%x z3i897?A3D^=jP^C9FC@!Lh0a{)YQ}<%g8X1&Y&AGC`SwxoO-F66EMInKD~$@z*{|c z#Yvy6sE)g?n-}Y%NuB1VOZ>Iyhgy5SloC-}Xj~Y3BJNOn0;W9cu-k%L@V@^xsIUtL zjqi^rOIcnG*oP8d(Cg|lqL0AM?Y=^@-M?wcaDCJD`OQ6Dl6x(A?{9V&bxY#qRZkqX ztCc3h4Sv~=pL#ly(J|&+1v5pr7#bA~E$!=>rHQ(De3RixmhZ+edz>G`Q>E46ugN&3 znxm3cTc_EOa3>*R6vY=zahhP}EsIL>U8#1iDvaUDi)TOj|8%FSr>4Vhp506)#!uv@ zlQqQG#Q3>HDF~=fUwG@5TMwsy+(%BHej#NbbC8oyQGus-OjZo~>pi`7T<&Um7Bi!o z?=3BUjl%NFVHt~|8m`i8S|wr<5>P0-w`xtHNl~DmgdV2Z4f9c#PUmE@7pwwW-Q0#J zcM{^trKUG@o6919(ki(J_YEBwg=JtnA(`wWM_iJXiqQ98^5@yuwX*-~W!(NZe-14q z${P2O4>#K1oiZAI?=2OQ;9apGeJ)_r_3(7{`TqJA_H<$`TF`AJ!E3PWLU3q)yn*K5 z4rI(NKXw}_610&Kn}mc!aaEP|ycQHn$rT!K%#%V@u-EcfEVzX!xXYz@k|URn{?v~l zUdD8riwP(9rErp-ymwu3Np|>4A06F1(L*=!IE8K&>28gq&t3dB*0_=sXDzp$`gNu# z7kqnPB1?D^^$%3ku1m=XN0N`Cy3_j0#wCT`QQYp3J;)pqps9jh=5h$R&sOsu! zcwZnqYkFico^m?>R9JPW)|KcpOBK+Q*jom<@DXg5aIpL`hTfr{Uu;rRZ~l{e4vSZt zT&rSHmQEYdu)Q)G{ayVwPo6=y!4&Go?XIo}zuRylOI8=S?0rM)!YpGqT9TxwhjZ;0 z4BJrMJjw?j@m+EAeE!DTsV>aQN_s0y<_{J5lgDD{0*;!(&9sYHTI7OWL8Y9LVvWHs z8p&jNdvC>}P9OU!YZ{}jo2Lc$M(bymj+1JwVOk4a_uEI@;zL_z)TY~*A@}BohVe+g z`NXT6lYTg2ft9VDxIF$a866!3#&w@&^byJKz%Qs%^IZ{TeHpr{(2s|SA@%-Y-%AtUqxfFfbfB0|X~0!{Wt6v_JeVptJo*vC zw2^N%VH=xhMSPPkzll+UVLL9FatfVQ+70i*Krjrps?F#=blh?hCBcQ1bR1$Fiy=Zk6|Aptap;hzjAMZY<-XU|6T zg=Alc3VQy&4IPct9j=*tRJ=iv(;0H>T=*@Yt((3W>?Yix0ei_ySYC4OHu>|ut76D+ zBMoF05Ja+SqtY`jH@#t9v1dd9Q%0kJe_U2OY>PZWXw53#vX`*YDt?ju;pf>s@pYGP zyna<9Eb@9m2sMf-U;LO;om{{~j(xREr`txU4gZhrQ9pPQM*TKQ$^Fl$KclUvxMv3- zzk|D1+F*VfL_NAAV`H8SbTF+amEyQRNV&B=6kp~$s>&Ja=Rs)GL@ zYE`aTzAn^qzAKeHtN)ymXRv7Z-F_{a^UZg!eXC~iaZA6nUvt42mf6On7<=`3qsb0Y z^!cCU$D6wB4XFe6747f2c^M514bQCZ#oV~q)YjOfo*+`d z>0`Yx`al=!_gU|qprCB1O-%D%da_OblCQ&U%F7xP9-qwOK94#bI8k}zk!?W2^javx zTLW}b_tBK&1qlOZ;_TZvuqw~LECe@5JVabY(B zlU@J&5gw0sPki#^-K$ptq-!Z1;0kt|XLuXbVj2V&at^pf&qU9}>3Ol}?w`g76-Fj1 zqMBBF*DK!|kZoPi*+2aJbje`d+vc3{GrsEDmRX#v{VQt7-9Ilf?sy@V>WzhZQqT;x z!mGeLKv21uIBVPI?C#2YZfjy)630!>&Ak`DUD)A7*-iDhGWvLBct((NoEuic+{`a1 z*w4ns_GMr|Z1ouC201iE%*rv@vzA~37_eKZJETb`Lv!H;`R3SA$MHt87j`{GwRKDr zB*4DwePWE^g?48u9TuhIi`1Gi+D<4!tqXO!!tkS&V`PFHC&^#sbdn!ODZ!8)P9?kg zBiD?>eLo5Y$dzXw97abjq5vkk&2iJiKU)0MyYHX^78l(YQbO?V{ewewR@M#XI%aUH z)U*DK$)SYcrTjm^F1ppvmf~h%SJE*#qE`P}^9e&?xtd*FX_lKwsD;Ajd_U19>dbj+ z&GR&x)aEw=prN~vV{Zmx-2)6I-S3bIBA?YR49hXy=h31cWK2&E`Au)gnl1|^QLRkO zK3g+Ze!djf%--eGF~R5Ke|mqT)AEbZ>Gj=Bw3^BNTJuG|1^1A(xNftyhBqRT9U~*= z&`ErDl?HU;SV%+OWnMAY1_M-vk^tq>FT26dS931@b3ESMsv+WCGDX1UfYo#r&KIc^ z1We3H&WP_FS*m*}$VuP?NaeMPe)@eu4=(JZkH^4vDOFi|=F7(~!}%Th?L%& zE7zAEm{%<#s3-hi{&9Ca;(Y#h^4w?rcSe;yw+t*u8ymN-2h9YQGe$o*UFbU_9^NN% zp?wVKX3ziI5S0P;?UXmZ!CZkW7_Wl<%R~e+8HG{KkN4fUjmePe0F@x6o zBe8FIZ=dD;nZqY$jqTJ8hepIxJM_fVDzb@A*K$(gfa!iy9oeWKo~MIDZQ@V0e4x}u zeM(W!FsZOS@see!AFtRWc0g-^MpK9NUR1S@TJ6UQbkiB}!jJYmyssu*c{wAFa7M8C zGVz|l584O!Pg~$sFw&3W2{3L6nj>DfhO>0g_L0@zlaI79IdEwGxC>cC%Kd%5Q)xjO z%@Eq6jEr3nz~m9sqU;|Sc>DD$_qtzQyCM%SV@QMf!Gi~{!c*ihgw6PCx>@9hpD#I3 zT``T!SB&Tz;u@g;-7oYjY0o}*1KbNklRL-A@_5Yx4812}3(0W)hjtSTLjltsIZB1# zZ)b9Z{KF>>y@EF(`cm+o34H!sA|nEgISP3_r`37uj0icPE1$A-E@_Bzjr=k`_}OQ& ztF8_Ey-xP2oa4lAgNC}dhf*z?D_rl#5M-deMjdX!y&qyu9dh&hliXTh+A~x|sg+8AgSy}?f^+k13eEig9OncpGdEf40=p~s2k^<-gwi+_07wd=kQT$y^S(0+dQ47Jgf z<4bp8C)}AfV5Ock?z!!6=g+mRC>7Ms3%bE1sS|$#^el%FuC1sL0D<;f-?FKutk^uOL5~GTatdnu5vb?6@{Og~S$ zk4+V>c}@ap;ZveuW7xh)Z*;*jWC_N+IUp8`jM4hm%V!s-P`amGMt(j)=@xBQyZ+?> zFZK(`ySna>&v^u2M-sDi{>tWjJ~r8DGqnUS5X*$}4c)!8hT@Hl=Xy3ojXe zU~>KmpK0ld%mXquGatSCJ_ zJ-UeZVaT^9Y!6gpVR?*nLPCP$WC#EAdB&*hw+qFqb8FlBAnHSqM@^hMb4Kmffo9#k zpZ6|D;O-IL*VE}9IX-hr`;Ni%mz~-EfFuCwnLKIr*|#4A$!rY0(QZn@C5;W7vMdTD zqTD+KGSI*J$nifM|EEp%{1u7t&d{OZ^R?RHjv%D}i$5Y*0G1DEm6t}x2o_YUUwV)# z9$AZqb)EWos4B}SfauAQ+ELGo@=5m_rfrN$^d`rSsWEO>`}lETii4>P^z_yNNFhhU z521hnCOFhBZl>kLJ4LE^`wN~pD<$lt7a*0d@$HJVIY06AG_7Xv zlNG1Mo7oA!p_*rGXU#q%BH6$iO}at~->8l`40h+~Vt=JvL2t%NPL{2Qi4!e-4a zEo!8aKAb9SX!s{TmJ3ueW66`k6q=ImyGCRzWf(YuTP^OiT738US1s+FK>6S$L7Yab zAq2uUfZyLbNpaYN!MkrQw8T|~O4}gDOR8IeEA`PdYpUS2E0u+mX30O$pnCiAg$j?3kBbiZ)9b`TIlY;be} z^Z0Q&+^X?S)?tp_C$u7tt<I~AJ9I~`0JC;RayNa=YZSFchE>L^u zTITMuz7jR2R4+cNnKNkLj0CR}KxUUyrnClGl*rJ|e4?{t+ah=*%5%I?-_0Sv2Zr-~ z0nUhwiVD^p23rr(!`B5b(3$az4Y7W_XH=kd?BTB1QOT%b9VYc_2`8U<>=&X<)emaM z-}z6J-c9f?yhn{v98me=^b#(6bky=g;k`@PV#hU1h>9kOQube_V>q zt4-cT9-`!rROIEG@79j5)bFy%S+xO@(Oz~T$8-irhVy84_5_`!=rzHc#SG#RK4e6b zIM(6bQK6!d*^{_mzS+Im&wLK1vW#yrQrOBEmarQNuev}dwcVPJi~KpOC;`lw>w;(M z#}?folo_@$+NL20MSN$^v|)_l+e7%~c<|4y0TO+ekd!Z#*Dnik zLHa}KxNF&|0K~F+bxY57Y27vi8R)4}>$`Zuo-Z7!wC9_#jO5M%hYKCZxc3msJuY-l z+XCTRmigEwus4!|EP2J@{Bsg!wXi4@rEmD8GQLMlIFgr__e=yJqPrK<)+HIV8i4n` z-sWBD`*KDpbL&%3krSDI)(c;QjfO`y3S_^pub%r@)HdO&ctQ@OKJ5krvPl;P%dFcm z1cyiK+@ftARUGnLeLXn16r1|G=XDX4S)8y~%Vw#_d2hq36$9r6&o9rd8*lng;-{7oM+Pqd~|cI zm!tv5;MwT+ve_gg-O7~!XrYS5G_JLy~NbpgRCG=ZU%n9hI=CFX`ZmNaD$g>5!Y+uojh)s=Z8X7M3 zO!V;`eyTaKCuxyck)&MUG>MJE`keKDiEv$*p7C`zMA70Y7s;_UzUQAVEL;gyIPgLw zWt-0-A9v|Qe0GWMNKA-X@`R)>uykt(IE_)TqPXV>6twW8D(Q+?_|*O(2_t!4k1kPv|a{-*6}YUG1WJmMo%U?+beb(`a7cuiHYst zs47Nqj)58Qfm@eal#Z1=(l9Af{X-3R>`CX>uA1IkAA_!8h1xEDxzahxJ~l99rp1G+ zZoM-aY%q7SBIBS}to8dR(oQTdbq0y$v5C#IPD%``{8WH23tZ(<%WvOkq$%++NDU9( zk!@=mtCLoJiE;ybH(8n-RerrkYvcVe$Ept|^T3XyCuK_#)PX(0=}aKCk1+!c9cyAf zDI`>KI0k=N&GaSI=yl*3>89A<$oL4$g8yu#IglW z6p2swU$7BJ19jU`YgfKA0m`c`VWaCY=vBb(`w!9dhPwFj<;yGBN{%#@=+FjpIErRp zc1{k$9<=1ZkLWrjUL(l-VR-0(736UrDSG?&1`~i6xW52iKuJMJR}guG%w~oBeO(oU z_D|(A_S{31rpBnJQ3psa4Y6eaag@NXTNBdxPS_ZUzMc|>pKt&2Wp7+_1IV(d529L> za$lW~^ERD;I1eni#Zl2j4via^vnuHdcpO?=|y<8qGAYQPH9Z^H^E)wm7%%GiSs_&d<{>S z#|pK)lJTO7&AlA^C#(C;p3mwFs{k3iK8sRUb^dZu3V*;G6BEZCCv_r~xI}&zZ*d?u zvGgfM%`ehm@902yy8ral0BS*~$S~b_!;t~}=P765V7@f7hfUU}4YV*Q3Nczuvefb{ z=7NHPE3`rr>#Ntc^BvF6@(P}>t*yOcU?6vnQ}UlS8;tZ!K{p57%%0IaY!}@Q z%}TZ1{zzGN-6C((mTx1517zB}f?0KEJ&H({OIZ37s2#?L_w8^h^UBJ~x+J4kZ(09zrQEaAIoUT!hZo9o!L83GN&f}U3A*FdF0$07jH@QyU*%}0lHLHCakEo z{i4(c;eJ8wF1|_e9OrvD(FfjXHamP7!;G9Zo2H3}>tQ~5kfs!dK$Ci!YR;4Ha+ca! zu2W`@4NG}&vv(3<@1byB&!x6?skJkih#O7+59|oe()L*TY`i)gZx4E77 z9*+FsZ9NEU0N|yhwDffqSk3`1S|LAvTnSUbjAi4l?FMqB9m?22#Trnno4Z!NltTj2 zt)-HpdRehI@CobOWpp19aBpbw+v28!jA zeSVPl7@KP$(Dqe~PKwOTE)*zD`EO!E2Y%1?i=tfoZnoC;Y)SQShGc0y+s)hJ1t55 zMnX=I9WeLwj7OxJV*qCnDH^Gvtc_UpX*j-+$zk$QG5=#oWHsI;Yzy8NEabJcn#`gC zA76Xq8Pyj0Nl`VwQ8xKF9>te*Js?3JfIL%4*7blqpE*PamIdni?AbGJI#$pFTd6ds z-r1v#EmJRIkHgSIfFu?M9#Xu)ETIiZ9_-4=cgXEgZR*IK*@~NUu$?J-l%>N@pPv2V zjS>j|e|t#u%fDrtK$f?HNI>a?=D!p){?&QT=v2(11<@S47iIsNQm?v>3HH_PAs_v)}kFlT{?3u;QS z-)kN_vw9E+1a>#PycCsMTRZqQnvR}14&=#`I#sHbpyLC`rTXgDvwWWmA!9ur!Zb&u zopm~0@MZC*bTzUIF|Dlsmmu$YmTNa*<@lUN%8yEjJ4ol7dv3GL-Ll2*Bf~hE-ziC9 zx1PlYfM^WFf9*UIy)KGm5`P5hyLyvFr)e*PD_ z18P07S_K7Ys(ib?`4t>Fy^@38L=fl!v+mjg)0&)32V2>7|B~|Zto}nGAAD>j*x={& zHQ`4`c;g4{6@kY>jG{fN82XN&+FwFF!4{{U9M()31){62P#Z3GY*IG{mq#Z~(yVN7 z0mOmukvEYq==Orh3EEmWMa}9rzJHLtj;}tixhzKb{6q>l+WCfl!>mr->i&Hn$cPXb zP+(>yC#PemN(;!-9f5Q8TSgpks1LI&(oz^$)3 zKvGA%xxG|S7mq4_xoJlK*rEhWLhBjQz^%$!E~Y{iI3e1+$%dnL1*Nu1fXbdu|G=)o z`(rur_s3v>Znit|lU$9o*hEHpG7_i*193+opWoft*$HKT3R#lX{qQ6jZ$iC+7WC(P z(C{wVACu}wM$EO^R50TcKmSQJ4j!1AO{0K`Z8A?6nLM!yVyel<#uqa~3 z%(h&yXd(k%5Vt+%a+PaSF`c+sfLG60gNKCY(HMw{BzULDbsY=)m?N94P$S%itw!W{ zywI*)U2{|MJ;7ykV{!{6-AQ&4P+ocI> zuY*0S)~xjG{pYSKG8SJfm0$*5?NL~;^h@$Rp3??~_OiPBk^g~b19D$FfU5crQ~!Lp zTbe;Z@MdyCQse$gH4IdEX=OfyOxWwjvzu726pTb#x}6Df~jP zt3EmQyc8eRhV5{hD#TAvk?d8uFHKQMj=;C}BaeYG;mi1LPn_J~mOLw1I%LhHkw{1{zwtbE&ciHiM8cQxOY<`Rw+ zd@w|YbLKnYsDX?rssWN)*GzNt`8IB(U0_E_gh&(wKk;_wle~*5CFEhakz(QGAr==T!q+1BeX$^mjEtWSO5oC8@uv;s^rWxEXh1 zB-U$Xh_`%c*Ph^9dFp?V*d@OQ!tZij#e7TLf0->a8Qm!5+On4)yitUn#rta@N_f+U zXa)5UK;}k`Wa{R26GB;55;2frgXXZhUjgH43`)FkTr){eynB(Sa_|@E;`Gcv=GtW6 zLv{A`8+2U4JWB*<@-!pvYJ8I`I|7da%XMlm)Ln=`92%MF3&Eowj%gdjcl=Y}er_a; zlk_(Wi983z_PW?!0%xI3;LHsV&@;U!bc9ki!$>Gvrlq6cekQaIdN4ato>ka$oD+XnB$ft3&9+klyk2)C;SsG0B{Mv~3vqF476d+j z*=!UMW$Px%TBJ)r1UKUP?HwIFH0NV8Y!!%`)0j(Orl5e};ZDkayF@Lv`6r;Ubd~Vh z7?kb_BO{|*;1d2`{&T&RGa9d={}0YIm6Vi_!KMb#Xgg@)F4NPG>6f3{<-~)^ZJ?UG zJ0Na#FH0xkFNDtP0q5)Sj>zT)p86F|berOoLGjEKKKVLpetpw&&NsvuD+SUo z2=@r1P=|jgW8|<04I3$bk>aa2$FE+=ey6q8AdryqJ=tUE8VnZ{YDaZjUXJ;N!CrvW zA8b=c?X-Jxr$#1vEx!@3D}zDNB%v}@x;CcFRnU7vyCfDQ`^jYtuHT-2-5xZj)C2sx@!HXgQi8yLLqGIr_aM;Y>c|d2h z5<<8FVjJvlyb76lyvn_?&E+TD3tSxfT|uKesOWV(K5q%4c%M%yo!36=|Nlo;yFDp( zjIwQwNri!Z#i77zJL0Vl&G$yn)L1T;<#(9XKp0nVvu>kAT$TH2aWSz}Kn(f$`SSk4 zyk}(pq4Y=BU0fJVOiYlZWNS%53jW7$=)=1{0CrBbP~XO`fRN9wx^N+o-BE|513+kA zDgB9m5or``)qp^Egxjl&z@IlBQrNVoZ zp#`TGzjlv3?NQ~ue<0-?U|`wOqLy> ztd0qUctDN7aZ5l7#3!e^do712har)c4Ke!s(sovO{H1Js&t4s zmHg{TEDUbocdn6RV>c0p`=%`fVo;S<_whgW!efC*#C0gYpB(456Q&;5eq4jPxe@{r z)b;m?>uBsVU~eW4#w&&WV%K#j7%A%=rm`Oxlj`BM+<0-j5Wu7>FYZ)&yKl4 z`#-$WcL!7jtZfA6d2=IK`u5X5Nqfy&4P?42qrX%jJI)|4H99ys|Bh~<7+IA@gw5!g z;<~yclNvegh0UTdg?j@6xVg9MRm1liIePkw8+PlDM|ILP_x}J2IZBG8u%!DQ7uxWv zlZ6d97)6jA?!qd3zQ9P&(XIvE&0SGWHD_`E|2@9XjacNraa3aP;-?Bvm({TjMg0!p zeZCVxXJrFFVd1;&ISRBXw>SuE%{e=2%Iai5yXi}x5RW0xVCZmrQJ>n|$>iuD*oLU| z@tWKF2@pJU|0oRis;Wad8Yv2=4vMWxVKDqt|4;^jBz*DO=f=&nmbv0I5Xi01y@*KY zV`zwr?QCp7p`e)%?T1^8e&tb; zPOKX0Kzv7L70Jltcn7djJz3@^(p~Wc3<+`2({pq}3okUS#gUrslWn}Q2s=ZrPGgJO zirO+PeT656Rl^?uBh_{5@SJ7cR--PKHiyls1oTx#qap!b?!fNxx{?jYK zfVbmaBHW59LERl?61ji%KcF$Yrc#NJJ*3m}!t9CHIIZ-N+?1xkf4)mzDj+ zR4(y*-otMD*#ROE@D8j!b4{V)awfw{80v+FoB|vl4YEpknfBo9 z(7kcfXj@iJO4Q6-C7@rBmW`_Lu{W{iiF#ni6-Cn`%JN{-5ZtIaj*4L$)kaC!itMyp zB^s|9)ga0VgRT_*#iJSuMdBItf7{!-K%ZM@1@FD>E=u{i>xXeeoRbOF#KHfv!fDQ7) zhhMLnhNO(zMl%db;J28Q9T!5|!TUTtIyB0i&{e9e!s!3vy*y14Pag$x1=Xq)+ho4= zU6Z!y0bQ8Bsx5+hG1m?SlLPj2)J6lSj&FaBS8AV`!)QbSV2E;+F8RLyK&S=C!BL)i zdes)tZ&}<145FVjew;4K7LqH^!PEXBkXUMz#(V0~k)P#)P9>qUSD z?0c$89L-S=D6y^KlADRZF)~nz*n3@tZ7c_zf}o~MRTKj@1LhQ11*#y_GOncOM@tbZzt&K~~TbvKqa+yBis#EYNtt;^?zLIE;A$DPo)VmqKQ0TOXD# z1{$|4sj3nX9$_owc_nZN$$@lhO3`xYtT_=-6(T5o3pKUBV9-G<8)D3H>>&_!|A?f| zBHVV#j5YS}y$b=Hq4+()Pd$#4G!(<7Vi>xZFapNd$0$xGG<_L$Sh3!$_4t$!50;pz z!WKHRi6moN0!NXC0;ItT`|AS&R$w zCNNw%+$M;bPbDBEu0(sc#C_z(6Gr#^DDxy^Rx3Wd?_0Gu&>H1yQ4|5f0RTX`^$Hxn zuO7te>|MVEsXata4f6C~j+D(PC}uINPNkgTg1Mt=)jD#+N}&*89{7tkkdd!DXfW#N zCJAgOm;<2600TdSgu)Vb0X%9!WQ`}N6!@bRUqfPj#dsb*R-31`nboNp(Xr2e+!3Pm8Uf+;K$n=ZD1G!zJW4+7@3t7id@NuaOV#3oluUl$NfyAA7 z>0=nONM2?-3bQID|B_5zyNC)h8pP6Wt zvWsTKy``?m!3}JH$qe!gIuHSg%|M$kQ$~!Q2R#Ub*l+gJ*49RCsp`GDWCnwmNWj|7lfHC^2i4gOzzz6Juqg5;F#1))8RuOpYLDT1MBu$K^v z0224>*Z1+x04V`hqLJKXVq8%m5xze38XnZziPH!)grXqcgVj|g;TRs-3KEe>?76g4T0^ETYO3ev$e0)Y)*B#y0ulPfODtAX*h(3PA}Fzniihgm-j-!Dq$7|SL|*gR^_}n*lHS}~ z;R||sRJXPF7>NP5rOaMd1C?p_NxmwA z)YUa8$kB@~?QB4C9)y-$;({Cp=6q^k!DGm90*0Gfpd5HGu{>cOGeQ_;E-X~psp+XQ zWMI31e25CZCr`HbuLOPXGz6e+s4rtMVbxe+2q{k-V`&a5v*U6IzF;)vCeeprwqQ=}CL$W#iKfQH-Yn6gpIw8??p$h(5G*<&A6*-e45_ z)le`FqTtAA?K{Bq2neJ!tCdy3TRn4e+hT4IfG(%1e08O3}1LWwEkfGlEDAGR&>)?$2J{IIM|zG{1-m3vj8$x&~*&+|59zjZsG6()_ra&h0S zqjXt{RA*=o%CfI_rBxCJt^JUBBdhz+WfjY=wsRcT+1@_81AGlm7tExhOI*WAP(Kv# zwJ8W{gJItgW1VI^HI%YF9u3vs#MB$&JZ`!#y#vO{uPIbb2=3tihr2Q9f#> zz}@kSo7;y2uxviC8P8>Cb9cAjm_LIN@MO`3XSr(qitOu1*458(g99Z5)g6}$$uR^# z ztXd63)-8|DaAkmXqD0~OV(Ayq9pcvfb$^ z$4)_Iq+LcB1cEZCN|tRmaAJDO2uNNZXwsjaMv)R1gKR`Uv~}B1`q*pCdH;=RP_{nf z84B}c(R4${LX>B=kw|R7^UT8I6iJ3zULc814>Gl7y>SGRIT6}+0how@>xLkQs?bHv zfrAPVz`~B?7XV=a^7vgpK)37wpxCFtX&gk>2Tvf5<%MZPPF?Y=^Cw_cXw~vr)3nS$ z40=mi5YqpHmrTHNbc|K2^gbJQ=;=d3Ow5!XpD{i7+H>g>(mXRY-tJWr1%VIwD~cQv z)ZExH6RuZ8Z0N&N zJw44}H?*}RJdX%|<7AxL(%9J88775gwUyiifuW~O9&?$O#`g&hcDcNLHJ89nR`xR3 zxKX1u&_RoN>+vezOAB}reUp1txLFIWm{F@&1xi~jP;xZ5Gii`?pkF6$8{veU;{?G) zUi%vc?j%9zPq|-&`xi&O%&OgEw}1cs`>f3&r08-#=OCFEgltmP)M`UDg)o7gd)0~D zh0~$x1KjY5$sTUT&%%Kjhi!z5eOtjylufg~=b6d?DeqBFkh%GX18tpZVJ5uIC6(-BauB+Rluf?3{-ZxxbRw99C@~wnrs@sHqg~5ZsimIeHlXHu0LLJSfgm4zmt|tGajwf zZ5{ujZlaMx5iE)JC^ zW(1oy#gOP!IH0a?^yJC3uL>9N-ae36K*Q)S2F5zWM6kU323pwF-tt?Uo9uOxy|fgnGxZ-=yc`?w z4_O~Q@w#AaoA}UVV!-WS_nF|&7SuQ20?kXOtf{2fuI9poD`qAxJDYnxzN8kc*mcu! zTVY{g#aW4-VhA;2LXRsAQ+kZ9{*b0cf0nd;b#YPA*yB>0rlaX+%XD_ME#QS^k(fLq zwGHl_j#Kf;i5Z`spAJ>|M(MjY|ECw#ADdKucrB@}6f|#yBV=e(yvu=@K;68|sd(2p zV`;i6ymbQ;leL{C5w$doyjSa$a=MES#BD^WJ)y^SvDcp1X0WnH#P09cO literal 0 HcmV?d00001 diff --git a/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg new file mode 100644 index 0000000000000..13ba18bbb7588 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop line defined in the lanelet map +
+
+
+ + stop line defined in the lanele... + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg b/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg new file mode 100644 index 0000000000000..6e4c152cd79f0 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop line defined in the lanelet map +
+
+
+ + stop line defined in the lanele... + + + + + + + + + + + + + +
+
+
+ stop_distance_from_object +
+
+
+ + stop_distance_from_... + + + + + + + + + + + + + +
+
+
far_object_threshold <
+
+
+ + far_object_threshold < + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/limitation.svg b/planning/behavior_velocity_crosswalk_module/docs/limitation.svg deleted file mode 100644 index 3e507c0669a2b..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/limitation.svg +++ /dev/null @@ -1,164 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - -
-
-
- stop objects -
-
-
- - stop objects - - - - - - - - - - - -
-
-
- waiting for the traffic light to turn green. -
-
-
- - waiting for the traffic l... - - - - - -
-
-
- The pedestrian has no intention to cross via this route. -
-
-
- - The pedestrian has no intent... - - - - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/no-intension.svg b/planning/behavior_velocity_crosswalk_module/docs/no-intension.svg deleted file mode 100644 index 803ce223d591a..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/no-intension.svg +++ /dev/null @@ -1,193 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - - - - - - - -
-
-
- min_object_velocity * ego_pass_later_margin - [m] -
-
-
- - min_object_velocity * ego_pass_later_margin [... - - - - - - -
-
-
- yield... -
-
-
- - yield... - - - - - -
-
-
- stop objects -
-
-
- - stop objects - - - - - - - -
-
-
- don't have intention -
- to cross -
-
-
- - don't have intentio... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg deleted file mode 100644 index c2d486a7d3541..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg +++ /dev/null @@ -1,149 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - - - - - - - - -
-
-
- stop line -
- (defined by HDMap) -
-
-
- - stop line... - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
- - stop_distance_from_object [m] - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg deleted file mode 100644 index 3842601ae246c..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg +++ /dev/null @@ -1,170 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - - - - - - - - - -
-
-
- stop_distance_from_crosswalk - [m] -
-
-
- - stop_distance_from_crosswalk [m] - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
- - stop_distance_from_object [m] - - - - - - -
-
-
- stop point -
- (planned in module) -
-
-
- - stop point... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg deleted file mode 100644 index ea7fd9be3ff41..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg +++ /dev/null @@ -1,204 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - -
-
-
- far_object_threshold - [m] -
-
-
- - far_object_threshold [m] - - - - - - - - - - -
-
-
- stop line -
- (defined by HDMap) -
-
-
- - stop line... - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
- - stop_distance_from_object [m] - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg deleted file mode 100644 index 924f642ad08bb..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg +++ /dev/null @@ -1,144 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
- - stop_distance_from_object [m] - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - -
-
-
- The module determines a stop position at least - stop_distance_from_object - away from the object's predicted path. -
-
-
- - The module determines a stop positio... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg deleted file mode 100644 index 3ac1a94fcec78..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg +++ /dev/null @@ -1,174 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%22%20style%3D%22endArrow%3Dclassic%3Bhtml%3D1%3Brounded%3D0%3BexitX%3D0.25%3BexitY%3D0%3BexitDx%3D0%3BexitDy%3D0%3BentryX%3D0.5%3BentryY%3D0.442%3BentryDx%3D0%3BentryDy%3D0%3BentryPerimeter%3D0%3B%22%20edge%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20width%3D%2250%22%20height%3D%2250%22%20relative%3D%221%22%20as%3D%22geometry%22%3E%3CmxPoint%20x%3D%22851%22%20y%3D%221780%22%20as%3D%22sourcePoint%22%2F%3E%3CmxPoint%20x%3D%22890.04%22%20y%3D%221780%22%20as%3D%22targetPoint%22%2F%3E%3C%2FmxGeometry%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20i... - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - - - -
-
-
- stuck_vehicle_attention_range - [m] -
-
-
- - stuck_vehicle_attention_range [... - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - - - -
-
-
- max_stuck_vehicle_lateral_offset - [m] -
-
-
- - max_stuck_vehicle_lateral_offset [m] - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg new file mode 100644 index 0000000000000..c517be5bb9967 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stuck_vehicle_attention_range +
+
+
+ + stuck_vehicle_atten... + + + + + + + + + + + + + + +
+
+
+ max_stuck_vehicle_lateral_offset +
+
+
+ + max_stuck_vehicle_l... + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg b/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg new file mode 100644 index 0000000000000..6eb1b25cf5642 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg @@ -0,0 +1,210 @@ + + + + + + + + + + + + +
+
+
+ (5, 1) +
+
+
+ + (5, 1) + + + + + + +
+
+
+ (1, 4) +
+
+
+ + (1, 4) + + + + + + +
+
+
+ (2, 6) +
+
+
+ + (2, 6) + + + + + + + + + + +
+
+
+ (0, 1) +
+
+
+ + (0, 1) + + + + + + +
+
+
+ (3, 0) +
+
+
+ + (3, 0) + + + + + + +
+
+
+ Time-To-Vehicle [s] +
+
+
+ + Time-To-Vehicle [s] + + + + + + + + + + + + + + + + + + + +
+
+
+ Time-To-Collision [s] +
+
+
+ + Time-To-Collision [s] + + + + + + +
+
+
+ B +
+
+
+ + B + + + + + + +
+
+
+ A +
+
+
+ + A + + + + + + +
+
+
+ C +
+
+
+ + C + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg b/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg index 8084dc2fbd8b0..c409493483845 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg @@ -1,134 +1,65 @@ + - + - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - - - - -
-
-
- virtual collision point -
-
-
- - virtual collision poi... - - - - - -
-
-
- path -
-
-
- - path - - - - - -
+
-
- object's prediction path +
+ virtual collision point
- object's prediction p... + virtual collision point - + diff --git a/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg new file mode 100644 index 0000000000000..7d4b4017c2f1a --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop_distance_from_crosswalk +
+
+
+ + stop_distance_from_... + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg b/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg new file mode 100644 index 0000000000000..a09f430e990b3 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ NOT + yield +
+
+
+ + NOT yield + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg b/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg new file mode 100644 index 0000000000000..d4dea5fb78d2e --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ yield +
+
+
+ + yield + + + + + + + + Text is not SVG - cannot display + + + From ec9144c8b7362163a9d0781f5858805dcd07b8f3 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 18 Jan 2024 17:58:25 +0900 Subject: [PATCH 073/154] feat(intersection): more precise pass judge handling considering occlusion detection and 1st/2nd attention lane (#6047) Signed-off-by: Mamoru Sobue --- .../README.md | 86 +++- .../config/intersection.param.yaml | 2 +- .../docs/pass-judge-line.drawio.svg | 473 ++++++++++++++++++ .../src/debug.cpp | 20 +- .../src/manager.cpp | 4 +- .../src/scene_intersection.cpp | 106 ++-- .../src/scene_intersection.hpp | 16 +- 7 files changed, 634 insertions(+), 73 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 58c2ce59edd48..afb2381e628a4 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -88,7 +88,7 @@ To precisely calculate stop positions, the path is interpolated at the certain i - closest_idx denotes the path point index which is closest to ego position. - first_attention_stopline denotes the first path point where ego footprint intersects with the attention_area. -- If a stopline is associated with the intersection lane on the map, that line is used as the default_stopline for collision detection. Otherwise the point which is `common.default_stopline_margin` meters behind first_attention_stopline is defined as the default_stopline instead. +- If a stopline is associated with the intersection lane on the map, that line is used as default_stopline for collision detection. Otherwise the point which is `common.default_stopline_margin` meters behind first_attention_stopline is defined as default_stopline instead. - occlusion_peeking_stopline is a bit ahead of first_attention_stopline as described later. - occlusion_wo_tl_pass_judge_line is the first position where ego footprint intersects with the centerline of the first attention_area lane. @@ -113,8 +113,8 @@ There are several behaviors depending on the scene. | Safe | Ego detected no occlusion and collision | Ego passes the intersection | | StuckStop | The exit of the intersection is blocked by traffic jam | Ego stops before the intersection or the boundary of attention area | | YieldStuck | Another vehicle stops to yield ego | Ego stops before the intersection or the boundary of attention area | -| NonOccludedCollisionStop | Ego detects no occlusion but detects collision | Ego stops at the default_stop_line | -| FirstWaitBeforeOcclusion | Ego detected occlusion when entering the intersection | Ego stops at the default_stop_line at first | +| NonOccludedCollisionStop | Ego detects no occlusion but detects collision | Ego stops at default_stopline | +| FirstWaitBeforeOcclusion | Ego detected occlusion when entering the intersection | Ego stops at default_stopline at first | | PeekingTowardOcclusion | Ego detected occlusion and but no collision within the FOV (after FirstWaitBeforeOcclusion) | Ego approaches the boundary of the attention area slowly | | OccludedCollisionStop | Ego detected both occlusion and collision (after FirstWaitBeforeOcclusion) | Ego stops immediately | | FullyPrioritized | Ego is fully prioritized by the RED/Arrow signal | Ego only cares vehicles still running inside the intersection. Occlusion is ignored | @@ -219,7 +219,7 @@ ros2 run behavior_velocity_intersection_module ttc.py --lane_id ## Occlusion detection -If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough ego first makes a brief stop at the default stop line for `occlusion.temporal_stop_time_before_peeking`, and then slowly creeps toward occlusion_peeking_stop_line. If `occlusion.creep_during_peeking.enable` is true `occlusion.creep_during_peeking.creep_velocity` is inserted up to occlusion_peeking_stop_line. Otherwise only stop line is inserted. +If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough ego first makes a brief stop at default_stopline for `occlusion.temporal_stop_time_before_peeking`, and then slowly creeps toward occlusion_peeking_stopline. If `occlusion.creep_during_peeking.enable` is true `occlusion.creep_during_peeking.creep_velocity` is inserted up to occlusion_peeking_stopline. Otherwise only stop line is inserted. During the creeping if collision is detected this module inserts a stop line in front of ego immediately, and if the FOV gets sufficiently clear the intersection_occlusion wall will disappear. If occlusion is cleared and no collision is detected ego will pass the intersection. @@ -241,7 +241,7 @@ The remaining time is visualized on the intersection_occlusion virtual wall. ### Occlusion handling at intersection without traffic light -At intersection without traffic light, if occlusion is detected, ego makes a brief stop at the default_stopline and first_attention_stopline respectively. After stopping at the first_attention_area_stopline this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and occlusion_wo_tl_pass_judge_line while occlusion is not cleared. If collision is detected, ego immediately stops. Once the occlusion is cleared or ego has passed occlusion_wo_tl_pass_judge_line this module does not detect collision and occlusion because ego footprint is already inside the intersection. +At intersection without traffic light, if occlusion is detected, ego makes a brief stop at default_stopline and first_attention_stopline respectively. After stopping at the first_attention_area_stopline this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and occlusion_wo_tl_pass_judge_line while occlusion is not cleared. If collision is detected, ego immediately stops. Once the occlusion is cleared or ego has passed occlusion_wo_tl_pass_judge_line this module does not detect collision and occlusion because ego footprint is already inside the intersection. ![occlusion_detection](./docs/occlusion-without-tl.drawio.svg) @@ -263,7 +263,7 @@ TTC parameter varies depending on the traffic light color/shape as follows. ### yield on GREEN -If the traffic light color changed to GREEN and ego approached the entry of the intersection lane within the distance `collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start` and there is any object whose distance to its stopline is less than `collision_detection.yield_on_green_traffic_light.object_dist_to_stopline`, this module commands to stop for the duration of `collision_detection.yield_on_green_traffic_light.duration` at the default_stopline. +If the traffic light color changed to GREEN and ego approached the entry of the intersection lane within the distance `collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start` and there is any object whose distance to its stopline is less than `collision_detection.yield_on_green_traffic_light.object_dist_to_stopline`, this module commands to stop for the duration of `collision_detection.yield_on_green_traffic_light.duration` at default_stopline. ### skip on AMBER @@ -281,18 +281,49 @@ When the traffic light color/shape is RED/Arrow, occlusion detection is skipped. ## Pass Judge Line -To avoid sudden braking, if deceleration and jerk more than the threshold (`common.max_accel` and `common.max_jerk`) is required to stop at first_attention_stopline, this module does not command to stop once it passed the default_stopline position. +Generally it is not tolerable for vehicles that have lower traffic priority to stop in the middle of the unprotected area in intersections, and they need to stop at the stop line beforehand if there will be any risk of collision, which introduces two requirements: -If ego passed pass_judge_line, then ego does not stop anymore. If ego passed pass_judge_line while ego is stopping for dangerous decision, then ego stops while the situation is judged as dangerous. Once the judgement turned safe, ego restarts and does not stop anymore. +1. The vehicle must start braking before the boundary of the unprotected area at least by the braking distance if it is supposed to stop +2. The vehicle must recognize upcoming vehicles and check safety beforehand with enough braking distance margin if it is supposed to go + 1. And the SAFE decision must be absolutely certain and remain to be valid for the future horizon so that the safety condition will be always satisfied while ego is driving inside the unprotected area. +3. (TODO): Since it is almost impossible to make perfectly safe decision beforehand given the limited detection range/velocity tracking performance, intersection module should plan risk-evasive acceleration velocity profile AND/OR relax lateral acceleration limit while ego is driving inside the unprotected area, if the safety decision is "betrayed" later due to the following reasons: + 1. The situation _turned out to be dangerous_ later, mainly because velocity tracking was underestimated or the object accelerated beyond TTC margin + 2. The situation _turned dangerous_ later, mainly because the object is suddenly detected out of nowhere -The position of the pass judge line depends on the occlusion detection configuration and the existence of the associated traffic light of the intersection lane. +The position which is before the boundary of unprotected area by the braking distance which is obtained by -- If `occlusion.enable` is false, the pass judge line before the `first_attention_stopline` by the braking distance $v_{ego}^{2} / 2a_{max}$. -- If `occlusion.enable` is true and: - - if there are associated traffic lights, the pass judge line is at the `occlusion_peeking_stopline` in order to continue peeking/collision detection while occlusion is detected. - - if there are no associated traffic lights and: - - if occlusion is detected, pass judge line is at the `occlusion_wo_tl_pass_judge_line` to continue peeking. - - if occlusion is not detected, pass judge line is at the same place at the case where `occlusion.enable` is false. +$$ +\dfrac{v_{\mathrm{ego}}^{2}}{2a_{\mathrm{max}}} + v_{\mathrm{ego}} * t_{\mathrm{delay}} +$$ + +is called pass_judge_line, and safety decision must be made before ego passes this position because ego does not stop anymore. + +1st_pass_judge_line is before the first upcoming lane, and at intersections with multiple upcoming lanes, 2nd_pass_judge_line is defined as the position which is before the centerline of the first attention lane by the braking distance. 1st/2nd_pass_judge_line are illustrated in the following figure. + +![pass-judge-line](./docs/pass-judge-line.drawio.svg) + +Intersection module will command to GO if + +- ego is over default_stopline(or `common.enable_pass_judge_before_default_stopline` is true) AND +- ego is over 1st_pass judge line AND +- ego judged SAFE previously AND +- (ego is over 2nd_pass_judge_line OR ego is between 1st and 2nd pass_judge_line but most probable collision is expected to happen in the 1st attention lane) + +because it is expected to stop or continue stop decision if + +1. ego is before default_stopline && `common.enable_pass_judge_before_default_stopline` is false OR + 1. reason: default_stopline is defined on the map and should be respected +2. ego is before 1st_pass_judge_line OR + 1. reason: it has enough braking distance margin +3. ego judged UNSAFE previously + 1. reason: ego is now trying to stop and should continue stop decision if collision is detected in later calculation +4. (ego is between 1st and 2nd pass_judge_line and the most probable collision is expected to happen in the 2nd attention lane) + +For the 3rd condition, it is possible that ego stops with some overshoot to the unprotected area while it is trying to stop for collision detection, because ego should keep stop decision while UNSAFE decision is made even if it passed 1st_pass_judge_line during deceleration. + +For the 4th condition, at intersections with 2nd attention lane, even if ego is over the 1st pass_judge_line, still intersection module commands to stop if the most probable collision is expected to happen in the 2nd attention lane. + +Also if `occlusion.enable` is true, the position of 1st_pass_judge line changes to occlusion_peeking_stopline if ego passed the original 1st_pass_judge_line position while ego is peeking. Otherwise ego could inadvertently judge that it passed 1st_pass_judge during peeking and then abort peeking. ## Data Structure @@ -375,17 +406,18 @@ entity TargetObject { ### common -| Parameter | Type | Description | -| --------------------------------- | ------ | ------------------------------------------------------------------------ | -| `.attention_area_length` | double | [m] range for object detection | -| `.attention_area_margin` | double | [m] margin for expanding attention area width | -| `.attention_area_angle_threshold` | double | [rad] threshold of angle difference between the detected object and lane | -| `.use_intersection_area` | bool | [-] flag to use intersection_area for collision detection | -| `.default_stopline_margin` | double | [m] margin before_stop_line | -| `.stopline_overshoot_margin` | double | [m] margin for the overshoot from stopline | -| `.max_accel` | double | [m/ss] max acceleration for stop | -| `.max_jerk` | double | [m/sss] max jerk for stop | -| `.delay_response_time` | double | [s] action delay before stop | +| Parameter | Type | Description | +| -------------------------------------------- | ------ | -------------------------------------------------------------------------------- | +| `.attention_area_length` | double | [m] range for object detection | +| `.attention_area_margin` | double | [m] margin for expanding attention area width | +| `.attention_area_angle_threshold` | double | [rad] threshold of angle difference between the detected object and lane | +| `.use_intersection_area` | bool | [-] flag to use intersection_area for collision detection | +| `.default_stopline_margin` | double | [m] margin before_stop_line | +| `.stopline_overshoot_margin` | double | [m] margin for the overshoot from stopline | +| `.max_accel` | double | [m/ss] max acceleration for stop | +| `.max_jerk` | double | [m/sss] max jerk for stop | +| `.delay_response_time` | double | [s] action delay before stop | +| `.enable_pass_judge_before_default_stopline` | bool | [-] flag not to stop before default_stopline even if ego is over pass_judge_line | ### stuck_vehicle/yield_stuck @@ -430,7 +462,7 @@ entity TargetObject { | `.possible_object_bbox` | [double] | [m] minimum bounding box size for checking if occlusion polygon is small enough | | `.ignore_parked_vehicle_speed_threshold` | double | [m/s] velocity threshold for checking parked vehicle | | `.occlusion_detection_hold_time` | double | [s] hold time of occlusion detection | -| `.temporal_stop_time_before_peeking` | double | [s] temporal stop duration at the default_stop_line before starting peeking | +| `.temporal_stop_time_before_peeking` | double | [s] temporal stop duration at default_stopline before starting peeking | | `.temporal_stop_before_attention_area` | bool | [-] flag to temporarily stop at first_attention_stopline before peeking into attention_area | | `.creep_velocity_without_traffic_light` | double | [m/s] creep velocity to occlusion_wo_tl_pass_judge_line | | `.static_occlusion_with_traffic_light_timeout` | double | [s] the timeout duration for ignoring static occlusion at intersection with traffic light | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 4e9eb50f2a462..108e021240851 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -12,6 +12,7 @@ max_accel: -2.8 max_jerk: -5.0 delay_response_time: 0.5 + enable_pass_judge_before_default_stopline: false stuck_vehicle: turn_direction: @@ -36,7 +37,6 @@ consider_wrong_direction_vehicle: false collision_detection_hold_time: 0.5 min_predicted_path_confidence: 0.05 - keep_detection_velocity_threshold: 0.833 velocity_profile: use_upstream: true minimum_upstream_velocity: 0.01 diff --git a/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg b/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg new file mode 100644 index 0000000000000..d1f488af7c2ac --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg @@ -0,0 +1,473 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 1st_pass_judge_line + +
+
+
+ + 1st_pass_judge_line + + + + + + + + + + +
+
+
+ + 2nd_pass_judge_line_magin + +
+
+
+ + 2nd_pass_judge_line_magin + + + + + + + + + + +
+
+
+ + 2nd_pass_judge_line + +
+
+
+ + 2nd_pass_judge_line + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 1st_pass_judge_line + +
+
+
+ + 1st_pass_judge_line + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 5f103e0ecd3b0..350f6d774f7cf 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -90,7 +90,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; marker_line.action = visualization_msgs::msg::Marker::ADD; marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker_line.scale = createMarkerScale(0.1, 0.0, 0.0); + marker_line.scale = createMarkerScale(0.2, 0.0, 0.0); marker_line.color = createMarkerColor(r, g, b, 0.999); const double yaw = tf2::getYaw(pose.orientation); @@ -283,11 +283,23 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); */ - if (debug_data_.pass_judge_wall_pose) { + if (debug_data_.first_pass_judge_wall_pose) { + const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; + const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; appendMarkerArray( createPoseMarkerArray( - debug_data_.pass_judge_wall_pose.value(), "pass_judge_wall_pose", module_id_, 0.7, 0.85, - 0.9), + debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, + g, 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_pass_judge_wall_pose) { + const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; + const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; + appendMarkerArray( + createPoseMarkerArray( + debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, + r, g, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index c92f16dd7b474..0f9faaaa901c1 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -61,6 +61,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); ip.common.delay_response_time = getOrDeclareParameter(node, ns + ".common.delay_response_time"); + ip.common.enable_pass_judge_before_default_stopline = + getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); ip.stuck_vehicle.turn_direction.left = getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); @@ -92,8 +94,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".collision_detection.collision_detection_hold_time"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); - ip.collision_detection.keep_detection_velocity_threshold = getOrDeclareParameter( - node, ns + ".collision_detection.keep_detection_velocity_threshold"); ip.collision_detection.velocity_profile.use_upstream = getOrDeclareParameter(node, ns + ".collision_detection.velocity_profile.use_upstream"); ip.collision_detection.velocity_profile.minimum_upstream_velocity = getOrDeclareParameter( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index dad0c194b5c9f..358dca2414bb0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1041,7 +1041,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; - const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; const auto occlusion_wo_tl_pass_judge_line_idx = intersection_stoplines.occlusion_wo_tl_pass_judge_line; @@ -1221,13 +1222,22 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( prev_occlusion_status_ = occlusion_status; } - // TODO(Mamoru Sobue): this part needs more formal handling - const size_t pass_judge_line_idx = [=]() { + const size_t pass_judge_line_idx = [&]() { if (enable_occlusion_detection_) { - // if occlusion detection is enabled, pass_judge position is beyond the boundary of first - // attention area if (has_traffic_light_) { - return occlusion_stopline_idx; + // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its + // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line + if (passed_1st_judge_line_while_peeking_) { + return occlusion_stopline_idx; + } + const bool is_over_first_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, first_pass_judge_line_idx); + if (is_occlusion_state && is_over_first_pass_judge_line) { + passed_1st_judge_line_while_peeking_ = true; + return occlusion_stopline_idx; + } else { + return first_pass_judge_line_idx; + } } else if (is_occlusion_state) { // if there is no traffic light and occlusion is detected, pass_judge position is beyond // the boundary of first attention area @@ -1235,30 +1245,53 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } else { // if there is no traffic light and occlusion is not detected, pass_judge position is // default - return default_pass_judge_line_idx; + return first_pass_judge_line_idx; } } - return default_pass_judge_line_idx; + return first_pass_judge_line_idx; }(); - debug_data_.pass_judge_wall_pose = - planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); - const bool is_over_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); - const double vel_norm = std::hypot( - planner_data_->current_velocity->twist.linear.x, - planner_data_->current_velocity->twist.linear.y); - const bool keep_detection = - (vel_norm < planner_param_.collision_detection.keep_detection_velocity_threshold); + const bool was_safe = std::holds_alternative(prev_decision_result_); - // if ego is over the pass judge line and not stopped - if (is_over_default_stopline && !is_over_pass_judge_line && keep_detection) { - RCLCPP_DEBUG(logger_, "is_over_default_stopline && !is_over_pass_judge_line && keep_detection"); - // do nothing - } else if ( - (was_safe && is_over_default_stopline && is_over_pass_judge_line && is_go_out_) || + + const bool is_over_1st_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); + if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { + safely_passed_1st_judge_line_time_ = clock_->now(); + } + debug_data_.first_pass_judge_wall_pose = + planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); + debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); + const bool is_over_2nd_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, second_pass_judge_line_idx); + if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { + safely_passed_2nd_judge_line_time_ = clock_->now(); + } + debug_data_.second_pass_judge_wall_pose = + planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, *path); + debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); + + if ( + ((is_over_default_stopline || + planner_param_.common.enable_pass_judge_before_default_stopline) && + is_over_2nd_pass_judge_line && was_safe) || is_permanent_go_) { - // is_go_out_: previous RTC approval - // activated_: current RTC approval + /* + * This body is active if ego is + * - over the default stopline AND + * - over the 1st && 2nd pass judge line AND + * - previously safe + * , + * which means ego can stop even if it is over the 1st pass judge line but + * - before default stopline OR + * - before the 2nd pass judge line OR + * - or previously unsafe + * . + * In order for ego to continue peeking or collision detection when occlusion is detected after + * ego passed the 1st pass judge line, it needs to be + * - before the default stopline OR + * - before the 2nd pass judge line OR + * - previously unsafe + */ is_permanent_go_ = true; return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; } @@ -1309,10 +1342,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( : (planner_param_.collision_detection.collision_detection_hold_time - collision_state_machine_.getDuration()); + // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd + // pass judge line respectively, ego should go + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, - std::min(occlusion_stopline_idx, path->points.size() - 1), time_to_restart, - traffic_prioritized_level); + *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, + time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -1808,16 +1845,19 @@ std::optional IntersectionModule::generateIntersectionSto second_attention_area, interpolated_path_info, local_footprint, baselink2front); if (first_footprint_inside_2nd_attention_ip_opt) { second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + second_attention_stopline_valid = true; } } const auto second_attention_stopline_ip = second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) : 0; - // (8) second pass judge lie position on interpolated path - int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds); + // (8) second pass judge line position on interpolated path. It is the same as first pass judge + // line if second_attention_lane is null + int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; const auto second_pass_judge_line_ip = - static_cast(std::max(second_pass_judge_ip_int, 0)); + second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) + : first_pass_judge_line_ip; struct IntersectionStopLinesTemp { @@ -2803,8 +2843,6 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( return std::nullopt; } }(); - const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); - const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { const auto & p1 = smoothed_reference_path.points.at(i); @@ -2818,7 +2856,7 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; const double passing_velocity = [=]() { if (use_upstream_velocity) { - if (has_upstream_stopline && i > upstream_stopline_ind) { + if (upstream_stopline_idx_opt && i > upstream_stopline_idx_opt.value()) { return minimum_upstream_velocity; } return std::max(average_velocity, minimum_ego_velocity); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 38feada725872..1a9cf74624fc0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -48,7 +48,10 @@ struct DebugData std::optional collision_stop_wall_pose{std::nullopt}; std::optional occlusion_stop_wall_pose{std::nullopt}; std::optional occlusion_first_stop_wall_pose{std::nullopt}; - std::optional pass_judge_wall_pose{std::nullopt}; + std::optional first_pass_judge_wall_pose{std::nullopt}; + bool passed_first_pass_judge{false}; + bool passed_second_pass_judge{false}; + std::optional second_pass_judge_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; std::optional ego_lane{std::nullopt}; @@ -256,8 +259,8 @@ struct IntersectionStopLines size_t first_pass_judge_line{0}; /** - * second_pass_judge_line is before second_attention_stopline by the braking distance. if its - * value is calculated negative, it is 0 + * second_pass_judge_line is before second_attention_stopline by the braking distance. if + * second_attention_lane is null, it is same as first_pass_judge_line */ size_t second_pass_judge_line{0}; @@ -344,6 +347,7 @@ class IntersectionModule : public SceneModuleInterface double max_accel; double max_jerk; double delay_response_time; + bool enable_pass_judge_before_default_stopline; } common; struct TurnDirection @@ -373,7 +377,6 @@ class IntersectionModule : public SceneModuleInterface bool consider_wrong_direction_vehicle; double collision_detection_hold_time; double min_predicted_path_confidence; - double keep_detection_velocity_threshold; struct VelocityProfile { bool use_upstream; @@ -606,7 +609,7 @@ class IntersectionModule : public SceneModuleInterface private: rclcpp::Node & node_; - const int64_t lane_id_; + const lanelet::Id lane_id_; const std::set associative_ids_; const std::string turn_direction_; const bool has_traffic_light_; @@ -621,6 +624,9 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; OcclusionType prev_occlusion_status_; + bool passed_1st_judge_line_while_peeking_{false}; + std::optional safely_passed_1st_judge_line_time_{std::nullopt}; + std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; From 128ecc76c938c8ab45ae3168e56e31fcb6c925e0 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 18 Jan 2024 18:22:55 +0900 Subject: [PATCH 074/154] feat(blind_spot): consider opposite adjacent lane for wrong vehicles (#5635) Signed-off-by: Mamoru Sobue --- .../config/blind_spot.param.yaml | 1 + .../src/debug.cpp | 6 +- .../src/manager.cpp | 2 + .../src/scene.cpp | 220 ++++++++++++------ .../src/scene.hpp | 16 +- 5 files changed, 162 insertions(+), 83 deletions(-) diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index f9ddd3ce61099..3c22d1fe65db5 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,4 +8,5 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] + opposite_adjacent_extend_width: 1.5 # [m] enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index 7da494bfd5231..5cc90afb0043d 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -145,14 +145,12 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.conflict_areas_for_blind_spot, "conflict_area_for_blind_spot", module_id_, 0.0, - 0.5, 0.5), + debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5), &debug_marker_array, now); appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.detection_areas_for_blind_spot, "detection_area_for_blind_spot", module_id_, 0.5, - 0.0, 0.0), + debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0), &debug_marker_array, now); appendMarkerArray( diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index d64f1ebc39a51..09d1c5a7c3270 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -48,6 +48,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".threshold_yaw_diff"); planner_param_.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + planner_param_.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); } void BlindSpotModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 62a5e06674d4e..efbff7e2af51d 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -105,16 +105,18 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; const auto straight_lanelets = getStraightLanelets(lanelet_map_ptr, routing_graph_ptr, lane_id_); - if (!generateStopLine(straight_lanelets, path, &stop_line_idx)) { + const auto stoplines_idx_opt = generateStopLine(straight_lanelets, path); + if (!stoplines_idx_opt) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 1000 /* ms */, "[BlindSpotModule::run] setStopLineIdx fail"); *path = input_path; // reset path return false; } - if (stop_line_idx <= 0) { + const auto [default_stopline_idx, critical_stopline_idx] = stoplines_idx_opt.value(); + + if (default_stopline_idx <= 0) { RCLCPP_DEBUG( logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning."); *path = input_path; // reset path @@ -133,6 +135,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto return false; } const size_t closest_idx = closest_idx_opt.value(); + const auto stop_line_idx = + closest_idx > default_stopline_idx ? critical_stopline_idx : default_stopline_idx; /* set judge line dist */ const double current_vel = planner_data_->current_velocity->twist.linear.x; @@ -156,9 +160,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto debug_data_.virtual_wall_pose = stop_line_pose; const auto stop_pose = path->points.at(stop_line_idx).point.pose; debug_data_.stop_point_pose = stop_pose; - auto offset_pose = motion_utils::calcLongitudinalOffsetPose( - path->points, stop_pose.position, -pass_judge_line_dist); - if (offset_pose) debug_data_.judge_point_pose = *offset_pose; /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ if (planner_param_.use_pass_judge_line) { @@ -235,69 +236,41 @@ std::optional BlindSpotModule::getFirstPointConflictingLanelets( } } -bool BlindSpotModule::generateStopLine( +std::optional> BlindSpotModule::generateStopLine( const lanelet::ConstLanelets straight_lanelets, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const + autoware_auto_planning_msgs::msg::PathWithLaneId * path) const { /* set parameters */ constexpr double interval = 0.2; const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interval); - const int base2front_idx_dist = - std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval); + // const int base2front_idx_dist = + // std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval); /* spline interpolation */ autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; if (!splineInterpolate(*path, interval, path_ip, logger_)) { - return false; + return std::nullopt; } /* generate stop point */ - int stop_idx_ip = 0; // stop point index for interpolated path. + size_t stop_idx_default_ip = 0; // stop point index for interpolated path. + size_t stop_idx_critical_ip = 0; if (straight_lanelets.size() > 0) { std::optional first_idx_conflicting_lane_opt = getFirstPointConflictingLanelets(path_ip, straight_lanelets); if (!first_idx_conflicting_lane_opt) { RCLCPP_DEBUG(logger_, "No conflicting line found."); - return false; - } - stop_idx_ip = std::max( - first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist - base2front_idx_dist, 0); - } else { - std::optional intersection_enter_point_opt = - getStartPointFromLaneLet(lane_id_); - if (!intersection_enter_point_opt) { - RCLCPP_DEBUG(logger_, "No intersection enter point found."); - return false; - } - - geometry_msgs::msg::Pose intersection_enter_pose; - intersection_enter_pose = intersection_enter_point_opt.value(); - const auto stop_idx_ip_opt = - motion_utils::findNearestIndex(path_ip.points, intersection_enter_pose, 10.0, M_PI_4); - if (stop_idx_ip_opt) { - stop_idx_ip = stop_idx_ip_opt.value(); + return std::nullopt; } - - stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0); + stop_idx_default_ip = std::max(first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist, 0); + stop_idx_critical_ip = std::max(first_idx_conflicting_lane_opt.value() - 1, 0); } /* insert stop_point to use interpolated path*/ - *stop_line_idx = insertPoint(stop_idx_ip, path_ip, path); + const size_t stopline_idx_default = insertPoint(stop_idx_default_ip, path_ip, path); + const size_t stopline_idx_critical = insertPoint(stop_idx_critical_ip, path_ip, path); - /* if another stop point exist before intersection stop_line, disable judge_line. */ - bool has_prior_stopline = false; - for (int i = 0; i < *stop_line_idx; ++i) { - if (std::fabs(path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { - has_prior_stopline = true; - break; - } - } - - RCLCPP_DEBUG( - logger_, "generateStopLine() : stop_idx = %d, stop_idx_ip = %d, has_prior_stopline = %d", - *stop_line_idx, stop_idx_ip, has_prior_stopline); - - return true; + return std::make_pair(stopline_idx_default, stopline_idx_critical); } void BlindSpotModule::cutPredictPathWithDuration( @@ -385,36 +358,61 @@ bool BlindSpotModule::checkObstacleInBlindSpot( const auto areas_opt = generateBlindSpotPolygons( lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose); - if (!!areas_opt) { - debug_data_.detection_areas_for_blind_spot = areas_opt.value().detection_areas; - debug_data_.conflict_areas_for_blind_spot = areas_opt.value().conflict_areas; + if (areas_opt) { + const auto & detection_areas = areas_opt.value().detection_areas; + const auto & conflict_areas = areas_opt.value().conflict_areas; + const auto & opposite_detection_areas = areas_opt.value().opposite_detection_areas; + const auto & opposite_conflict_areas = areas_opt.value().opposite_conflict_areas; + debug_data_.detection_areas = detection_areas; + debug_data_.conflict_areas = conflict_areas; + debug_data_.detection_areas.insert( + debug_data_.detection_areas.end(), opposite_detection_areas.begin(), + opposite_detection_areas.end()); + debug_data_.conflict_areas.insert( + debug_data_.conflict_areas.end(), opposite_conflict_areas.begin(), + opposite_conflict_areas.end()); autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); // check objects in blind spot areas - bool obstacle_detected = false; for (const auto & object : objects.objects) { if (!isTargetObjectType(object)) { continue; } - const auto & detection_areas = areas_opt.value().detection_areas; - const auto & conflict_areas = areas_opt.value().conflict_areas; - const bool exist_in_detection_area = + // right direction + const bool exist_in_right_detection_area = std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { return bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), lanelet::utils::to2D(area)); }); - const bool exist_in_conflict_area = + // opposite direction + const bool exist_in_opposite_detection_area = std::any_of( + opposite_detection_areas.begin(), opposite_detection_areas.end(), + [&object](const auto & area) { + return bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + }); + const bool exist_in_detection_area = + exist_in_right_detection_area || exist_in_opposite_detection_area; + if (!exist_in_detection_area) { + continue; + } + const bool exist_in_right_conflict_area = isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_opposite_conflict_area = isPredictedPathInArea( + object, opposite_conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_conflict_area = + exist_in_right_conflict_area || exist_in_opposite_conflict_area; if (exist_in_detection_area && exist_in_conflict_area) { - obstacle_detected = true; debug_data_.conflicting_targets.objects.push_back(object); + return true; } } - return obstacle_detected; + return false; } else { return false; } @@ -504,6 +502,37 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet( return new_lanelet; } +lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const +{ + const auto centerline = lanelet.centerline2d(); + const auto width = + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); + const double extend_width = + std::min(planner_param_.opposite_adjacent_extend_width, width); + const auto left_bound_ = + direction == TurnDirection::RIGHT + ? lanelet.rightBound().invert() + : lanelet::utils::getCenterlineWithOffset(lanelet.invert(), -width / 2 + extend_width); + const auto right_bound_ = + direction == TurnDirection::RIGHT + ? lanelet::utils::getCenterlineWithOffset(lanelet.invert(), width / 2 - extend_width) + : lanelet.leftBound().invert(); + lanelet::Points3d lefts, rights; + for (const auto & pt : left_bound_) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : right_bound_) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); + auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); + new_lanelet.setCenterline(new_centerline); + return new_lanelet; +} + std::optional BlindSpotModule::generateBlindSpotPolygons( lanelet::LaneletMapConstPtr lanelet_map_ptr, [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, @@ -512,19 +541,20 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( { std::vector lane_ids; lanelet::ConstLanelets blind_spot_lanelets; + lanelet::ConstLanelets blind_spot_opposite_lanelets; /* get lane ids until intersection */ for (const auto & point : path.points) { bool found_intersection_lane = false; for (const auto lane_id : point.lane_ids) { - // make lane_ids unique - if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { - lane_ids.push_back(lane_id); - } - if (lane_id == lane_id_) { found_intersection_lane = true; + lane_ids.push_back(lane_id); break; } + // make lane_ids unique + if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { + lane_ids.push_back(lane_id); + } } if (found_intersection_lane) break; } @@ -537,27 +567,49 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( // additional detection area on left/right side lanelet::ConstLanelets adjacent_lanelets; + lanelet::ConstLanelets opposite_adjacent_lanelets; for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); - const auto adj = std::invoke([&]() -> std::optional { - if (turn_direction_ == TurnDirection::INVALID) { + const auto adj = + turn_direction_ == TurnDirection::LEFT + ? (routing_graph_ptr->adjacentLeft(lane)) + : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) + : boost::none); + const std::optional opposite_adj = + [&]() -> std::optional { + if (!!adj) { return std::nullopt; } - const auto adj_lane = (turn_direction_ == TurnDirection::LEFT) - ? routing_graph_ptr->adjacentLeft(lane) - : routing_graph_ptr->adjacentRight(lane); - - if (adj_lane) { - return *adj_lane; + if (turn_direction_ == TurnDirection::LEFT) { + // this should exist in right-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.leftBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); } - - return std::nullopt; - }); - + if (turn_direction_ == TurnDirection::RIGHT) { + // this should exist in left-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.rightBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); + } else { + return std::nullopt; + } + }(); if (adj) { const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); adjacent_lanelets.push_back(half_lanelet); } + if (opposite_adj) { + const auto half_lanelet = + generateExtendedOppositeAdjacentLanelet(opposite_adj.value(), turn_direction_); + opposite_adjacent_lanelets.push_back(half_lanelet); + } } const auto current_arc_ego = @@ -587,6 +639,24 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( blind_spot_polygons.conflict_areas.emplace_back(std::move(conflicting_area_adj)); blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_adj)); } + // additional detection area on left/right opposite lane side + if (!opposite_adjacent_lanelets.empty()) { + const auto stop_line_arc_opposite_adj = + lanelet::utils::getLaneletLength3d(opposite_adjacent_lanelets); + const auto current_arc_opposite_adj = + stop_line_arc_opposite_adj - (stop_line_arc_ego - current_arc_ego); + const auto detection_area_start_length_opposite_adj = + stop_line_arc_opposite_adj - planner_param_.backward_length; + auto conflicting_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( + opposite_adjacent_lanelets, current_arc_opposite_adj, stop_line_arc_opposite_adj); + auto detection_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( + opposite_adjacent_lanelets, detection_area_start_length_opposite_adj, + stop_line_arc_opposite_adj); + blind_spot_polygons.opposite_conflict_areas.emplace_back( + std::move(conflicting_area_opposite_adj)); + blind_spot_polygons.opposite_detection_areas.emplace_back( + std::move(detection_area_opposite_adj)); + } return blind_spot_polygons; } else { return std::nullopt; @@ -677,7 +747,7 @@ lanelet::ConstLanelets BlindSpotModule::getStraightLanelets( const auto next_lanelets = routing_graph_ptr->following(prev_intersection_lanelets.front()); for (const auto & ll : next_lanelets) { const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (!turn_direction.compare("straight")) { + if (turn_direction.compare("straight") == 0) { straight_lanelets.push_back(ll); } } diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 3915acd3532b5..6f8450568939c 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -38,6 +39,8 @@ struct BlindSpotPolygons { std::vector conflict_areas; std::vector detection_areas; + std::vector opposite_conflict_areas; + std::vector opposite_detection_areas; }; class BlindSpotModule : public SceneModuleInterface @@ -50,8 +53,10 @@ class BlindSpotModule : public SceneModuleInterface geometry_msgs::msg::Pose virtual_wall_pose; geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; - std::vector conflict_areas_for_blind_spot; - std::vector detection_areas_for_blind_spot; + std::vector conflict_areas; + std::vector detection_areas; + std::vector opposite_conflict_areas; + std::vector opposite_detection_areas; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; }; @@ -67,6 +72,7 @@ class BlindSpotModule : public SceneModuleInterface double threshold_yaw_diff; //! threshold of yaw difference between ego and target object double adjacent_extend_width; //! the width of extended detection/conflict area on adjacent lane + double opposite_adjacent_extend_width; }; BlindSpotModule( @@ -118,6 +124,8 @@ class BlindSpotModule : public SceneModuleInterface lanelet::ConstLanelet generateExtendedAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; + lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. @@ -169,9 +177,9 @@ class BlindSpotModule : public SceneModuleInterface * @param pass_judge_line_idx generated pass judge line index * @return false when generation failed */ - bool generateStopLine( + std::optional> generateStopLine( const lanelet::ConstLanelets straight_lanelets, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const; + autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; /** * @brief Insert a point to target path From c833bea85a2ff4fa9c99ca31ce3001556f8a8a9a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 18 Jan 2024 18:51:11 +0900 Subject: [PATCH 075/154] docs(vehicle_info): add docs for versioning (#6069) Signed-off-by: kosuke55 remove non-support Signed-off-by: kosuke55 fix link Signed-off-by: kosuke55 add yaml Signed-off-by: kosuke55 please check readme --- vehicle/vehicle_info_util/Readme.md | 29 +++++++++++++++++++ .../config/vehicle_info.param.yaml | 1 + 2 files changed, 30 insertions(+) diff --git a/vehicle/vehicle_info_util/Readme.md b/vehicle/vehicle_info_util/Readme.md index c3d06a3b43260..600fd62270d81 100644 --- a/vehicle/vehicle_info_util/Readme.md +++ b/vehicle/vehicle_info_util/Readme.md @@ -7,6 +7,35 @@ This package is to get vehicle info parameters. ### Description In [here](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/), you can check the vehicle dimensions with more detail. +The current format supports only the Ackermann model. This file defines the model assumed in autoware path planning, control, etc. and does not represent the exact physical model. If a model other than the Ackermann model is used, it is assumed that a vehicle interface will be designed to change the control output for the model. + +### Versioning Policy + +We have implemented a versioning system for the `vehicle_info.param.yaml` file to ensure clarity and consistency in file format across different versions of Autoware and its external applications. Please see [discussion](https://github.com/orgs/autowarefoundation/discussions/4050) for the details. + +#### How to Operate + +- The current file format is set as an unversioned base version (`version:` field is commented out). +- For the next update involving changes (such as additions, deletions, or modifications): + - Uncomment and update the version line at the beginning of the file. + - Initiate versioning by assigning a version number, starting from `0.1.0`. Follow the semantic versioning format (MAJOR.MINOR.PATCH). + - Update this Readme.md too. +- For subsequent updates, continue incrementing the version number in accordance with the changes made. + - Discuss how to increment version depending on the amount of changes made to the file. + +```yaml +/**: + ros__parameters: + # version: 0.1.0 # Uncomment and update this line for future format changes. + wheel_radius: 0.383 + ... +``` + +#### Why Versioning? + +- Consistency Across Updates: Implementing version control will allow accurate tracking of changes over time and changes in vehicle information parameters. +- Clarity for External Applications: External applications that depend on `vehicle_info.param.yaml` need to reference the correct file version for optimal compatibility and functionality. +- Simplified Management for Customized Branches: Assigning versions directly to the `vehicle_info.param.yaml` file simplifies management compared to maintaining separate versions for multiple customized Autoware branches. This approach streamlines version tracking and reduces complexity. ### Scripts diff --git a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml index 8941b92b4e78e..72c070c17b52c 100644 --- a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml +++ b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + # version: 0.1.0 # uncomment this line in the next update of this file format. please check Readme.md wheel_radius: 0.39 wheel_width: 0.42 wheel_base: 2.74 # between front wheel center and rear wheel center From 90075cec6fa1713d90d267b83d2365c476c9aa8c Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 18 Jan 2024 12:49:52 +0000 Subject: [PATCH 076/154] chore: update CODEOWNERS (#6038) Signed-off-by: GitHub Co-authored-by: github-actions --- .github/CODEOWNERS | 80 ++++++++++++++++++++++++---------------------- 1 file changed, 41 insertions(+), 39 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 12ad752b3ef29..89bcbce9e5656 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,7 @@ ### Copied from .github/CODEOWNERS-manual ### ### Automatically generated from package.xml ### -common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp +common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_auto_geometry/** satoshi.ota@tier4.jp common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp @@ -9,9 +9,9 @@ common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wak common/autoware_point_types/** taichi.higashide@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp -common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp -common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp +common/component_interface_specs/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp +common/component_interface_tools/** isamu.takagi@tier4.jp +common/component_interface_utils/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/geography_utils/** koji.minoda@tier4.jp @@ -24,7 +24,7 @@ common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.sa common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp -common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp +common/path_distance_calculator/** isamu.takagi@tier4.jp common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @@ -32,16 +32,16 @@ common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp -common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp +common/tier4_api_utils/** isamu.takagi@tier4.jp common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp -common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp +common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp common/tier4_debug_tools/** ryohsuke.mitsudome@tier4.jp -common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp takamasa.horibe@tier4.jp +common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp @@ -54,7 +54,7 @@ common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp -common/traffic_light_utils/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp +common/traffic_light_utils/** mingyu.li@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -76,9 +76,9 @@ evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4 evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp -launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp +launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp +launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp launch/tier4_map_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp @@ -86,29 +86,29 @@ launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp launch/tier4_system_launch/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp -localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp -localization/geo_pose_projector/** koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/ar_tag_based_localizer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/landmark_manager/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/localization_util/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/pose_initializer/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/pose_instability_detector/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/stop_filter/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/tree_structured_parzen_estimator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/twist2accel/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_monitor/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_particle_filter/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_pose_initializer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -map/map_height_fitter/** isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_loader/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp -map/map_projection_loader/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_tf_generator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/ar_tag_based_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/tree_structured_parzen_estimator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp +map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @@ -156,6 +156,7 @@ perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_goal_planner_module/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp @@ -166,6 +167,7 @@ planning/behavior_path_start_planner_module/** kosuke.takeuchi@tier4.jp kyoichi. planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -223,11 +225,11 @@ simulator/fault_injection/** keisuke.shima@tier4.jp simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp -system/component_state_monitor/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -system/default_ad_api/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/component_state_monitor/** isamu.takagi@tier4.jp +system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/diagnostic_graph_aggregator/** isamu.takagi@tier4.jp system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp From 8dbde9db0b9f8409d66ec3563f677709a04c9a4a Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 18 Jan 2024 22:22:05 +0900 Subject: [PATCH 077/154] refactor(start_planner): visualize shifting section (#6103) * Visualize the footprint from start_pose to end_pose along the path Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 9ad7f72d6af4e..0e0a6b123be12 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -1415,6 +1415,39 @@ void StartPlannerModule::setDebugData() add(start_pose_text_marker_array); } + // visualize the footprint from pull_out_start pose to pull_out_end pose along the path + { + MarkerArray pull_out_path_footprint_marker_array{}; + const auto pink = createMarkerColor(1.0, 0.0, 1.0, 0.99); + Marker pull_out_path_footprint_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "shift_path_footprint", 0, Marker::LINE_STRIP, + createMarkerScale(0.2, 0.2, 0.2), pink); + pull_out_path_footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + PathWithLaneId path_shift_start_to_end{}; + const auto shift_path = status_.pull_out_path.partial_paths.front(); + { + const size_t pull_out_start_idx = motion_utils::findNearestIndex( + shift_path.points, status_.pull_out_path.start_pose.position); + const size_t pull_out_end_idx = + motion_utils::findNearestIndex(shift_path.points, status_.pull_out_path.end_pose.position); + + path_shift_start_to_end.points.insert( + path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + pull_out_end_idx + 1); + } + + for (size_t i = 0; i < path_shift_start_to_end.points.size(); ++i) { + pull_out_path_footprint_marker.id = i; + pull_out_path_footprint_marker.points.clear(); + addFootprintMarker( + pull_out_path_footprint_marker, path_shift_start_to_end.points.at(i).point.pose, + vehicle_info_); + pull_out_path_footprint_marker_array.markers.push_back(pull_out_path_footprint_marker); + } + + add(pull_out_path_footprint_marker_array); + } + // safety check if (parameters_->safety_check_params.enable_safety_check) { if (start_planner_data_.ego_predicted_path.size() > 0) { From 9b03b23090b6ee354f4c120f447493e5ace578a5 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 18 Jan 2024 22:23:23 +0900 Subject: [PATCH 078/154] refactor(start_planner): separate start planner parameters and debug data structure (#6101) * Add data_structs.hpp and update include paths * Refactor start planner debug data structure * Update debug_data variable name in StartPlannerModule Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../parking_departure/common_module_data.hpp | 5 -- ...lanner_parameters.hpp => data_structs.hpp} | 26 ++++++++-- .../debug.hpp | 39 +++++++++++++++ .../pull_out_planner_base.hpp | 2 +- .../start_planner_module.hpp | 4 +- .../src/debug.cpp | 30 ++++++++++++ .../src/start_planner_module.cpp | 47 +++++++++---------- 7 files changed, 118 insertions(+), 35 deletions(-) rename planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/{start_planner_parameters.hpp => data_structs.hpp} (80%) create mode 100644 planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp create mode 100644 planning/behavior_path_start_planner_module/src/debug.cpp diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 3dc230f0e92ed..74f6b843803df 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -38,11 +38,6 @@ struct StartGoalPlannerData std::vector ego_predicted_path; // collision check debug map CollisionCheckDebugMap collision_check; - - Pose refined_start_pose; - std::vector start_pose_candidates; - size_t selected_start_pose_candidate_index; - double margin_for_start_pose_candidate; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp similarity index 80% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 66dbddebf99bb..8f8d2baf9c85e 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -29,10 +29,30 @@ namespace behavior_path_planner { +using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; + using freespace_planning_algorithms::AstarParam; using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStarParam; +struct StartPlannerDebugData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; + // collision check debug map + CollisionCheckDebugMap collision_check; + + Pose refined_start_pose; + std::vector start_pose_candidates; + size_t selected_start_pose_candidate_index; + double margin_for_start_pose_candidate; +}; + struct StartPlannerParameters { double th_arrived_distance{0.0}; @@ -105,4 +125,4 @@ struct StartPlannerParameters } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp new file mode 100644 index 0000000000000..6aa5584807d90 --- /dev/null +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp @@ -0,0 +1,39 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ + +#include "behavior_path_start_planner_module/data_structs.hpp" + +#include +#include + +namespace behavior_path_planner +{ +using behavior_path_planner::StartPlannerDebugData; + +void updateSafetyCheckDebugData( + StartPlannerDebugData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; +} + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index e345e3dc91337..0144fdd45ae50 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -16,8 +16,8 @@ #define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ #include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_start_planner_module/data_structs.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 7b463f4fadf80..47cfa408162ca 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -21,11 +21,11 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/data_structs.hpp" #include "behavior_path_start_planner_module/freespace_pull_out.hpp" #include "behavior_path_start_planner_module/geometric_pull_out.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/shift_pull_out.hpp" -#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include @@ -191,7 +191,7 @@ class StartPlannerModule : public SceneModuleInterface std::vector> start_planners_; PullOutStatus status_; - mutable StartGoalPlannerData start_planner_data_; + mutable StartPlannerDebugData debug_data_; std::deque odometry_buffer_; diff --git a/planning/behavior_path_start_planner_module/src/debug.cpp b/planning/behavior_path_start_planner_module/src/debug.cpp new file mode 100644 index 0000000000000..0f95470d86d40 --- /dev/null +++ b/planning/behavior_path_start_planner_module/src/debug.cpp @@ -0,0 +1,30 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "behavior_path_start_planner_module/debug.hpp" + +namespace behavior_path_planner +{ + +void updateSafetyCheckDebugData( + StartPlannerDebugData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; +} + +} // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 0e0a6b123be12..1b54b655fc424 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_start_planner_module/debug.hpp" #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" @@ -126,7 +127,7 @@ void StartPlannerModule::initVariables() resetPathReference(); debug_marker_.markers.clear(); initializeSafetyCheckParameters(); - initializeCollisionCheckDebugMap(start_planner_data_.collision_check); + initializeCollisionCheckDebugMap(debug_data_.collision_check); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -589,8 +590,8 @@ void StartPlannerModule::planWithPriority( if (findPullOutPath( start_pose_candidates[index], planner, refined_start_pose, goal_pose, collision_check_margin)) { - start_planner_data_.selected_start_pose_candidate_index = index; - start_planner_data_.margin_for_start_pose_candidate = collision_check_margin; + debug_data_.selected_start_pose_candidate_index = index; + debug_data_.margin_for_start_pose_candidate = collision_check_margin; return; } } @@ -848,8 +849,8 @@ void StartPlannerModule::updatePullOutStatus() start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); } - start_planner_data_.refined_start_pose = *refined_start_pose; - start_planner_data_.start_pose_candidates = start_pose_candidates; + debug_data_.refined_start_pose = *refined_start_pose; + debug_data_.start_pose_candidates = start_pose_candidates; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -1196,14 +1197,13 @@ bool StartPlannerModule::isSafePath() const const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; - utils::parking_departure::updateSafetyCheckTargetObjectsData( - start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + behavior_path_planner::updateSafetyCheckDebugData( + debug_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane, - start_planner_data_.collision_check, planner_data_->parameters, - safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, - hysteresis_factor); + debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params, + objects_filtering_params_->use_all_predicted_path, hysteresis_factor); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const @@ -1354,7 +1354,7 @@ void StartPlannerModule::setDebugData() add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createFootprintMarkerArray( - start_planner_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3)); + debug_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0)); @@ -1399,14 +1399,13 @@ void StartPlannerModule::setDebugData() visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), purple); footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); text_marker.lifetime = rclcpp::Duration::from_seconds(1.5); - for (size_t i = 0; i < start_planner_data_.start_pose_candidates.size(); ++i) { + for (size_t i = 0; i < debug_data_.start_pose_candidates.size(); ++i) { footprint_marker.id = i; text_marker.id = i; footprint_marker.points.clear(); text_marker.text = "idx[" + std::to_string(i) + "]"; - text_marker.pose = start_planner_data_.start_pose_candidates.at(i); - addFootprintMarker( - footprint_marker, start_planner_data_.start_pose_candidates.at(i), vehicle_info_); + text_marker.pose = debug_data_.start_pose_candidates.at(i); + addFootprintMarker(footprint_marker, debug_data_.start_pose_candidates.at(i), vehicle_info_); start_pose_footprint_marker_array.markers.push_back(footprint_marker); start_pose_text_marker_array.markers.push_back(text_marker); } @@ -1450,29 +1449,29 @@ void StartPlannerModule::setDebugData() // safety check if (parameters_->safety_check_params.enable_safety_check) { - if (start_planner_data_.ego_predicted_path.size() > 0) { + if (debug_data_.ego_predicted_path.size() > 0) { const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( - start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); add(createPredictedPathMarkerArray( ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9)); } - if (start_planner_data_.filtered_objects.objects.size() > 0) { + if (debug_data_.filtered_objects.objects.size() > 0) { add(createObjectsMarkerArray( - start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); } - add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); - add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); - add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); + add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info")); + add(showPredictedPath(debug_data_.collision_check, "ego_predicted_path")); + add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation")); // set objects of interest - for (const auto & [uuid, data] : start_planner_data_.collision_check) { + for (const auto & [uuid, data] : debug_data_.collision_check) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - initializeCollisionCheckDebugMap(start_planner_data_.collision_check); + initializeCollisionCheckDebugMap(debug_data_.collision_check); } // Visualize planner type text From 5d98bad08c36f972065987fa8204d41bd5f28db6 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 18 Jan 2024 23:25:16 +0900 Subject: [PATCH 079/154] fix(start_planner): expand lane departure check for shift path (#6055) Refactor lane departure check in ShiftPullOut::plan() Signed-off-by: kyoichi-sugahara --- .../src/shift_pull_out.cpp | 24 ++++++------------- 1 file changed, 7 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 51673410199b8..36b8b1dc347a1 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -69,25 +69,15 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos pull_out_path.partial_paths.front(); // shift path is not separate but only one. // check lane_departure with path between pull_out_start to pull_out_end - PathWithLaneId path_start_to_end{}; + PathWithLaneId path_shift_start_to_end{}; { const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); + const size_t pull_out_end_idx = + findNearestIndex(shift_path.points, pull_out_path.end_pose.position); - // calculate collision check end idx - const size_t collision_check_end_idx = std::invoke([&]() { - const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - shift_path.points, pull_out_path.end_pose.position, - parameters_.collision_check_distance_from_end); - - if (collision_check_end_pose) { - return findNearestIndex(shift_path.points, collision_check_end_pose->position); - } else { - return shift_path.points.size() - 1; - } - }); - path_start_to_end.points.insert( - path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, - shift_path.points.begin() + collision_check_end_idx + 1); + path_shift_start_to_end.points.insert( + path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + pull_out_end_idx + 1); } // extract shoulder lanes from pull out lanes @@ -131,7 +121,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_shift_start_to_end)) { continue; } From d1059e5101db9af80bd60bdd9fead5bcc77f7e26 Mon Sep 17 00:00:00 2001 From: beyzanurkaya <32412808+beyzanurkaya@users.noreply.github.com> Date: Fri, 19 Jan 2024 01:55:06 +0300 Subject: [PATCH 080/154] fix(behavior_velocity_crosswalk_module): check if p_safety_slow is empty (#6104) * check if p_safety_slow is empty Signed-off-by: beyza * style(pre-commit): autofix --------- Signed-off-by: beyza Co-authored-by: beyza Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/scene_crosswalk.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index acb88a214b0c0..48ae5ff03b8bd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -813,7 +813,9 @@ void CrosswalkModule::applySafetySlowDownSpeed( const auto & p_safety_slow = calcLongitudinalOffsetPoint(ego_path.points, ego_pos, safety_slow_point_range); - insertDecelPointWithDebugInfo(p_safety_slow.value(), safety_slow_down_speed, output); + if (p_safety_slow.has_value()) { + insertDecelPointWithDebugInfo(p_safety_slow.value(), safety_slow_down_speed, output); + } if (safety_slow_point_range < 0.0) { passed_safety_slow_point_ = true; From 08b3a3df189edec3a90e101e064709b0c74ca824 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 19 Jan 2024 10:00:02 +0900 Subject: [PATCH 081/154] feat: always separate lidar preprocessing from pointcloud_container (#6091) * feat!: replace use_pointcloud_container Signed-off-by: kminoda * feat: remove from planning Signed-off-by: kminoda * fix: fix to remove all use_pointcloud_container Signed-off-by: kminoda * revert: revert change in planning.launch Signed-off-by: kminoda * revert: revert rename of use_pointcloud_container Signed-off-by: kminoda * fix: fix tier4_perception_launch to enable use_pointcloud_contaienr Signed-off-by: kminoda * fix: fix unnecessary change Signed-off-by: kminoda * fix: fix unnecessary change Signed-off-by: kminoda * refactor: remove trailing whitespace Signed-off-by: kminoda * revert other changes in perception Signed-off-by: kminoda * revert change in readme Signed-off-by: kminoda * feat: move glog to pointcloud_container.launch.py * revert: revert glog porting Signed-off-by: kminoda * style(pre-commit): autofix * fix: fix pre-commit Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../launch/localization.launch.xml | 7 ++++--- .../pose_twist_estimator.launch.xml | 8 ++++++-- .../launch/util/util.launch.py | 18 ++++-------------- ...era_lidar_fusion_based_detection.launch.xml | 4 ++-- ...dar_radar_fusion_based_detection.launch.xml | 4 ++-- .../detection/detection.launch.xml | 10 +++++----- .../detection/lidar_based_detection.launch.xml | 4 ++-- .../lidar_radar_based_detection.launch.xml | 4 ++-- .../detection/pointcloud_map_filter.launch.py | 7 ++++--- .../ground_segmentation.launch.py | 7 ++++--- ...probabilistic_occupancy_grid_map.launch.xml | 8 ++++---- .../launch/perception.launch.xml | 6 +++--- ...aserscan_based_occupancy_grid_map.launch.py | 7 ++++--- ...intcloud_based_occupancy_grid_map.launch.py | 7 ++++--- .../synchronized_grid_map_fusion.md | 4 ++-- 15 files changed, 52 insertions(+), 53 deletions(-) diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 3fba4bd773e68..f4d6679291849 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -15,14 +15,15 @@ - - + - + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index 7733b4e3117a1..02c6da20e17da 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -23,7 +23,9 @@ - + + + @@ -142,7 +144,9 @@ - + + + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py index 1a34429f438ed..22a45fe7b8530 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ b/launch/tier4_localization_launch/launch/util/util.launch.py @@ -15,8 +15,6 @@ import launch from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction -from launch.conditions import LaunchConfigurationNotEquals -from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -71,16 +69,9 @@ def load_composable_node_param(param_path): random_downsample_component, ] - target_container = ( - "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container" - if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("pointcloud_container_name") - ) - load_composable_nodes = LoadComposableNodes( - condition=LaunchConfigurationNotEquals(target_container, ""), composable_node_descriptions=composable_nodes, - target_container=target_container, + target_container=LaunchConfiguration("lidar_container_name"), ) return [load_composable_nodes] @@ -115,11 +106,10 @@ def add_launch_arg(name: str, default_value=None, description=None): "path to the parameter file of random_downsample_filter", ) add_launch_arg("use_intra_process", "true", "use ROS 2 component container communication") - add_launch_arg("use_pointcloud_container", "True", "use pointcloud container") add_launch_arg( - "pointcloud_container_name", - "/pointcloud_container", - "container name", + "lidar_container_name", + "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container", + "container name of main lidar used for localization", ) add_launch_arg( diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index d269144067e0e..4cb648852a03c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -6,7 +6,7 @@ - + @@ -62,7 +62,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 4838187ef8fbe..ba86bc1e334ff 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -6,7 +6,7 @@ - + @@ -91,7 +91,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 68d5ea944e6cb..885fa80ed7004 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -8,7 +8,7 @@ - + @@ -59,7 +59,7 @@ - + @@ -96,7 +96,7 @@ - + @@ -111,7 +111,7 @@ - + @@ -126,7 +126,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 2c8b7d6581c53..04c4264b70e5f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -6,7 +6,7 @@ - + @@ -25,7 +25,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index ed37f6270c913..adedc2312677f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -5,7 +5,7 @@ - + @@ -23,7 +23,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index 93d395ca3e466..1dcb9cfc90daf 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -141,7 +141,7 @@ def launch_setup(context, *args, **kwargs): components = [] components.extend(pipeline.create_pipeline()) individual_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -151,7 +151,7 @@ def launch_setup(context, *args, **kwargs): ) pointcloud_container_loader = LoadComposableNodes( composable_node_descriptions=components, - target_container=LaunchConfiguration("container_name"), + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) return [individual_container, pointcloud_container_loader] @@ -168,7 +168,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "pointcloud_map_filter_container") add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index fecdd29bcb7e9..b54172acd4afc 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -511,7 +511,7 @@ def launch_setup(context, *args, **kwargs): ) ) individual_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -521,7 +521,7 @@ def launch_setup(context, *args, **kwargs): ) pointcloud_container_loader = LoadComposableNodes( composable_node_descriptions=components, - target_container=LaunchConfiguration("container_name"), + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) return [individual_container, pointcloud_container_loader] @@ -537,7 +537,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "perception_pipeline_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "ground_segmentation_container") add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud") set_container_executable = SetLaunchConfiguration( diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index 281a52a85af71..244e0940acb70 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -7,7 +7,7 @@ - + @@ -24,7 +24,7 @@ - + @@ -40,7 +40,7 @@ - + @@ -56,7 +56,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 4e9f0daafa736..528038c5158b2 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -127,7 +127,7 @@ - + @@ -142,7 +142,7 @@ - + @@ -193,7 +193,7 @@ - + diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index b99335b0e09ef..62cfa4bcb5228 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -115,7 +115,7 @@ def launch_setup(context, *args, **kwargs): ] occupancy_grid_map_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -126,7 +126,7 @@ def launch_setup(context, *args, **kwargs): load_composable_nodes = LoadComposableNodes( composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container_name"), + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) @@ -174,7 +174,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_obstacle_pointcloud", "false"), add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), add_launch_arg("use_pointcloud_container", "false"), - add_launch_arg("container_name", "occupancy_grid_map_container"), + add_launch_arg("pointcloud_container_name", "pointcloud_container"), + add_launch_arg("individual_container_name", "occupancy_grid_map_container"), set_container_executable, set_container_mt_executable, ] diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 49bf228905dcc..29435c4ea8e24 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -64,7 +64,7 @@ def launch_setup(context, *args, **kwargs): ] occupancy_grid_map_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -75,7 +75,7 @@ def launch_setup(context, *args, **kwargs): load_composable_nodes = LoadComposableNodes( composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container_name"), + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) @@ -103,7 +103,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_multithread", "false"), add_launch_arg("use_intra_process", "true"), add_launch_arg("use_pointcloud_container", "false"), - add_launch_arg("container_name", "occupancy_grid_map_container"), + add_launch_arg("pointcloud_container_name", "pointcloud_container"), + add_launch_arg("individual_container_name", "occupancy_grid_map_container"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), add_launch_arg("output", "occupancy_grid"), diff --git a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md index e1046fa24719d..6f4d4e22aa26e 100644 --- a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md +++ b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md @@ -105,7 +105,7 @@ You need to generate OGMs in each sensor frame before achieving grid map fusion. - + @@ -146,7 +146,7 @@ You can include this launch file like the following. - + From c2de78d9a09fa26a7946363942bac094d7501a94 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 19 Jan 2024 13:16:52 +0900 Subject: [PATCH 082/154] feat(map_based_prediction): use obstacle acceleration for map prediction (#6072) * add acc filtering and decaying acc to model Signed-off-by: Daniel Sanchez * fixed compilation problem, acc is used to predict search_dist Signed-off-by: Daniel Sanchez * Use an equivalent velocity to calculate paths Signed-off-by: Daniel Sanchez * change decaying factor to T/4 Signed-off-by: Daniel Sanchez * coment out cerr for evaluation Signed-off-by: Daniel Sanchez * simplify code Signed-off-by: Daniel Sanchez * comments Signed-off-by: Daniel Sanchez * add missing constant to decaying acc model Signed-off-by: Daniel Sanchez * Use an equivalent velocity to calculate paths Signed-off-by: Daniel Sanchez * add missing constant to decaying acc model Signed-off-by: Daniel Sanchez * Implement lanelet speed limit for acc consideration Signed-off-by: Daniel Sanchez * add option to activate on and off acceleration feature Signed-off-by: Daniel Sanchez * add params Signed-off-by: Daniel Sanchez * add params Signed-off-by: Daniel Sanchez * delete unused class Signed-off-by: Daniel Sanchez * update docs Signed-off-by: Daniel Sanchez * delete comments Signed-off-by: Daniel Sanchez * fix comments Signed-off-by: Daniel Sanchez * update comments, refactor code Signed-off-by: Daniel Sanchez * remove unused line Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Sanchez --- perception/map_based_prediction/README.md | 48 ++++++++-- .../config/map_based_prediction.param.yaml | 3 + .../map_based_prediction_node.hpp | 9 +- .../map_based_prediction/path_generator.hpp | 20 ++++- .../src/map_based_prediction_node.cpp | 86 +++++++++++++++--- .../src/path_generator.cpp | 87 +++++++++++++++++-- 6 files changed, 224 insertions(+), 29 deletions(-) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index ec2f57963332f..2419cdb010799 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -124,7 +124,7 @@ See paper [2] for more details. `lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.) -#### Pruning predicted paths with lateral acceleration constraint +#### Pruning predicted paths with lateral acceleration constraint (for vehicle obstacles) It is possible to apply a maximum lateral acceleration constraint to generated vehicle paths. This check verifies if it is possible for the vehicle to perform the predicted path without surpassing a lateral acceleration threshold `max_lateral_accel` when taking a curve. If it is not possible, it checks if the vehicle can slow down on time to take the curve with a deceleration of `min_acceleration_before_curve` and comply with the constraint. If that is also not possible, the path is eliminated. @@ -136,11 +136,47 @@ Currently we provide three parameters to tune the lateral acceleration constrain You can change these parameters in rosparam in the table below. -| param name | default value | -| ----------------------------------------- | -------------- | -| `check_lateral_acceleration_constraints_` | `false` [bool] | -| `max_lateral_accel` | `2.0` [m/s^2] | -| `min_acceleration_before_curve` | `-2.0` [m/s^2] | +| param name | default value | +| ---------------------------------------- | -------------- | +| `check_lateral_acceleration_constraints` | `false` [bool] | +| `max_lateral_accel` | `2.0` [m/s^2] | +| `min_acceleration_before_curve` | `-2.0` [m/s^2] | + +## Using Vehicle Acceleration for Path Prediction (for Vehicle Obstacles) + +By default, the `map_based_prediction` module uses the current obstacle's velocity to compute its predicted path length. However, it is possible to use the obstacle's current acceleration to calculate its predicted path's length. + +### Decaying Acceleration Model + +Since this module tries to predict the vehicle's path several seconds into the future, it is not practical to consider the current vehicle's acceleration as constant (it is not assumed the vehicle will be accelerating for `prediction_time_horizon` seconds after detection). Instead, a decaying acceleration model is used. With the decaying acceleration model, a vehicle's acceleration is modeled as: + +$\ a(t) = a\_{t0} \cdot e^{-\lambda \cdot t} $ + +where $\ a\_{t0} $ is the vehicle acceleration at the time of detection $\ t0 $, and $\ \lambda $ is the decay constant $\ \lambda = \ln(2) / hl $ and $\ hl $ is the exponential's half life. + +Furthermore, the integration of $\ a(t) $ over time gives us equations for velocity, $\ v(t) $ and distance $\ x(t) $ as: + +$\ v(t) = v*{t0} + a*{t0} \* (1/\lambda) \cdot (1 - e^{-\lambda \cdot t}) $ + +and + +$\ x(t) = x*{t0} + (v*{t0} + a*{t0} \* (1/\lambda)) \cdot t + a*{t0}(1/λ^2)(e^{-\lambda \cdot t} - 1) $ + +With this model, the influence of the vehicle's detected instantaneous acceleration on the predicted path's length is diminished but still considered. This feature also considers that the obstacle might not accelerate past its road's speed limit (multiplied by a tunable factor). + +Currently, we provide three parameters to tune the use of obstacle acceleration for path prediction: + +- `use_vehicle_acceleration`: to enable the feature. +- `acceleration_exponential_half_life`: The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds. +- `speed_limit_multiplier`: Set the vehicle type obstacle's maximum predicted speed as the legal speed limit in that lanelet times this value. This value should be at least equal or greater than 1.0. + +You can change these parameters in `rosparam` in the table below. + +| Param Name | Default Value | +| ------------------------------------ | -------------- | +| `use_vehicle_acceleration` | `false` [bool] | +| `acceleration_exponential_half_life` | `2.5` [s] | +| `speed_limit_multiplier` | `1.5` [] | ### Path prediction for crosswalk users diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index fe4d2a51ec6f3..fdd64174de9be 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -16,6 +16,9 @@ check_lateral_acceleration_constraints: false # whether to check if the predicted path complies with lateral acceleration constraints max_lateral_accel: 2.0 # [m/ss] max acceptable lateral acceleration for predicted vehicle paths min_acceleration_before_curve: -2.0 # [m/ss] min acceleration a vehicle might take to decelerate before a curve + use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not + speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value + acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds # parameter for shoulder lane prediction prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 02db91b989116..f61dc75ffb85b 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -89,6 +90,7 @@ struct LaneletData struct PredictedRefPath { float probability; + double speed_limit; PosePath path; Maneuver maneuver; }; @@ -175,6 +177,10 @@ class MapBasedPredictionNode : public rclcpp::Node double max_lateral_accel_; double min_acceleration_before_curve_; + bool use_vehicle_acceleration_; + double speed_limit_multiplier_; + double acceleration_exponential_half_life_; + // Stop watch StopWatch stop_watch_; @@ -231,7 +237,8 @@ class MapBasedPredictionNode : public rclcpp::Node void addReferencePaths( const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths, const float path_probability, const ManeuverProbability & maneuver_probability, - const Maneuver & maneuver, std::vector & reference_paths); + const Maneuver & maneuver, std::vector & reference_paths, + const double speed_limit = 0.0); std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); void updateFuturePossibleLanelets( diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 4da4f62be2ede..6dfc4a8db9e20 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -91,7 +91,7 @@ class PathGenerator PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object); PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths); + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0); PredictedPath generatePathForCrosswalkUser( const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; @@ -99,17 +99,30 @@ class PathGenerator PredictedPath generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const; + void setUseVehicleAcceleration(const bool use_vehicle_acceleration) + { + use_vehicle_acceleration_ = use_vehicle_acceleration; + } + + void setAccelerationHalfLife(const double acceleration_exponential_half_life) + { + acceleration_exponential_half_life_ = acceleration_exponential_half_life; + } + private: // Parameters double time_horizon_; double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; + bool use_vehicle_acceleration_; + double acceleration_exponential_half_life_; // Member functions PredictedPath generateStraightPath(const TrackedObject & object) const; - PredictedPath generatePolynomialPath(const TrackedObject & object, const PosePath & ref_path); + PredictedPath generatePolynomialPath( + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); FrenetPath generateFrenetPath( const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length); @@ -125,7 +138,8 @@ class PathGenerator const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path); - FrenetPoint getFrenetPoint(const TrackedObject & object, const PosePath & ref_path); + FrenetPoint getFrenetPoint( + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 6a6f24081655e..c949eae21aeff 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include @@ -385,11 +384,7 @@ bool withinRoadLanelet( const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, const bool use_yaw_information = false) { - using Point = boost::geometry::model::d2::point_xy; - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - const Point p_object{obj_pos.x, obj_pos.y}; - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); // nearest lanelet constexpr double search_radius = 10.0; // [m] @@ -788,10 +783,18 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ prediction_time_horizon_rate_for_validate_lane_length_ = declare_parameter("prediction_time_horizon_rate_for_validate_shoulder_lane_length"); + use_vehicle_acceleration_ = declare_parameter("use_vehicle_acceleration"); + speed_limit_multiplier_ = declare_parameter("speed_limit_multiplier"); + acceleration_exponential_half_life_ = + declare_parameter("acceleration_exponential_half_life"); + path_generator_ = std::make_shared( prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); + path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); + path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); + sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1)); @@ -817,6 +820,13 @@ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); updateParam( parameters, "check_lateral_acceleration_constraints", check_lateral_acceleration_constraints_); + updateParam(parameters, "use_vehicle_acceleration", use_vehicle_acceleration_); + updateParam(parameters, "speed_limit_multiplier", speed_limit_multiplier_); + updateParam( + parameters, "acceleration_exponential_half_life", acceleration_exponential_half_life_); + + path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); + path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -1010,7 +1020,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( - yaw_fixed_transformed_object, ref_path.path); + yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit); if (predicted_path.path.empty()) continue; if (!check_lateral_acceleration_constraints_) { @@ -1555,14 +1565,63 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); + // Using a decaying acceleration model. Consult the README for more information about the model. + const double obj_acc = (use_vehicle_acceleration_) + ? std::hypot( + object.kinematics.acceleration_with_covariance.accel.linear.x, + object.kinematics.acceleration_with_covariance.accel.linear.y) + : 0.0; + const double t_h = prediction_time_horizon_; + const double λ = std::log(2) / acceleration_exponential_half_life_; + + auto get_search_distance_with_decaying_acc = [&]() -> double { + const double acceleration_distance = + obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1); + double search_dist = acceleration_distance + obj_vel * t_h; + return search_dist; + }; + + auto get_search_distance_with_partial_acc = [&](const double final_speed) -> double { + constexpr double epsilon = 1E-5; + if (std::abs(obj_acc) < epsilon) { + // Assume constant speed + return obj_vel * t_h; + } + // Time to reach final speed + const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc); + // It is assumed the vehicle accelerates until final_speed is reached and + // then continues at constant speed for the rest of the time horizon + const double search_dist = + // Distance covered while accelerating + obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + + obj_vel * t_f + + // Distance covered at constant speed + final_speed * (t_h - t_f); + return search_dist; + }; + std::vector all_ref_paths; + for (const auto & current_lanelet_data : current_lanelets_data) { - // parameter for lanelet::routing::PossiblePathsParams - const double search_dist = prediction_time_horizon_ * obj_vel + - lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet); + const lanelet::traffic_rules::SpeedLimitInformation limit = + traffic_rules_ptr_->speedLimit(current_lanelet_data.lanelet); + const double legal_speed_limit = static_cast(limit.speedLimit.value()); + + double final_speed_after_acceleration = + obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h)); + + const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_; + const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit; + const bool object_has_surpassed_limit_already = obj_vel > final_speed_limit; + + double search_dist = (final_speed_surpasses_limit && !object_has_surpassed_limit_already) + ? get_search_distance_with_partial_acc(final_speed_limit) + : get_search_distance_with_decaying_acc(); + search_dist += lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet); + lanelet::routing::PossiblePathsParams possible_params{search_dist, {}, 0, false, true}; const double validate_time_horizon = - prediction_time_horizon_ * prediction_time_horizon_rate_for_validate_lane_length_; + t_h * prediction_time_horizon_rate_for_validate_lane_length_; // lambda function to get possible paths for isolated lanelet // isolated is often caused by lanelet with no connection e.g. shoulder-lane @@ -1644,7 +1703,8 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( // Step4. add candidate reference paths to the all_ref_paths const float path_prob = current_lanelet_data.probability; const auto addReferencePathsLocal = [&](const auto & paths, const auto & maneuver) { - addReferencePaths(object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths); + addReferencePaths( + object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths, final_speed_limit); }; addReferencePathsLocal(left_paths, Maneuver::LEFT_LANE_CHANGE); addReferencePathsLocal(right_paths, Maneuver::RIGHT_LANE_CHANGE); @@ -1966,7 +2026,8 @@ void MapBasedPredictionNode::updateFuturePossibleLanelets( void MapBasedPredictionNode::addReferencePaths( const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths, const float path_probability, const ManeuverProbability & maneuver_probability, - const Maneuver & maneuver, std::vector & reference_paths) + const Maneuver & maneuver, std::vector & reference_paths, + const double speed_limit) { if (!candidate_paths.empty()) { updateFuturePossibleLanelets(object, candidate_paths); @@ -1976,6 +2037,7 @@ void MapBasedPredictionNode::addReferencePaths( predicted_path.probability = maneuver_probability.at(maneuver) * path_probability; predicted_path.path = converted_path; predicted_path.maneuver = maneuver; + predicted_path.speed_limit = speed_limit; reference_paths.push_back(predicted_path); } } diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index d3f610583577c..413a0e233186b 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -149,13 +149,13 @@ PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths) + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) { if (ref_paths.size() < 2) { return generateStraightPath(object); } - return generatePolynomialPath(object, ref_paths); + return generatePolynomialPath(object, ref_paths, speed_limit); } PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) const @@ -178,11 +178,11 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path) + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path); + const auto current_point = getFrenetPoint(object, ref_path, speed_limit); // Step1. Set Target Frenet Point // Note that we do not set position s, @@ -384,7 +384,8 @@ PredictedPath PathGenerator::convertToPredictedPath( return predicted_path; } -FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const PosePath & ref_path) +FrenetPoint PathGenerator::getFrenetPoint( + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) { FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; @@ -400,10 +401,82 @@ FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const Po static_cast(tf2::getYaw(ref_path.at(nearest_segment_idx).orientation)); const float delta_yaw = obj_yaw - lane_yaw; + const float ax = + (use_vehicle_acceleration_) + ? static_cast(object.kinematics.acceleration_with_covariance.accel.linear.x) + : 0.0; + const float ay = + (use_vehicle_acceleration_) + ? static_cast(object.kinematics.acceleration_with_covariance.accel.linear.y) + : 0.0; + + // Using a decaying acceleration model. Consult the README for more information about the model. + const double t_h = time_horizon_; + const float λ = std::log(2) / acceleration_exponential_half_life_; + + auto have_same_sign = [](double a, double b) -> bool { + return (a >= 0.0 && b >= 0.0) || (a < 0.0 && b < 0.0); + }; + + auto get_acceleration_adjusted_velocity = [&](const double v, const double a) { + constexpr double epsilon = 1E-5; + if (std::abs(a) < epsilon) { + // Assume constant speed + return v; + } + // Get velocity after time horizon + const auto terminal_velocity = v + a * (1.0 / λ) * (1 - std::exp(-λ * t_h)); + + // If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at + // most stop, not reverse its direction) + if (!have_same_sign(terminal_velocity, v)) { + // we assume a forwards moving vehicle will not decelerate to 0 and then move backwards + // if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed -> + // time t_stop + + // 0 = Vo + acc(1/λ)(1-e^(-λt_stop)) + // e^(-λt_stop) = 1 - (-Vo* λ)/acc + // t_stop = (-1/λ)*ln(1 - (-Vo* λ)/acc) + // t_stop = (-1/λ)*ln(1 + (Vo* λ)/acc) + auto t_stop = (-1.0 / λ) * std::log(1 + (v * λ / a)); + + // Calculate the distance traveled until stopping + auto distance_to_reach_zero_speed = + v * t_stop + a * t_stop * (1.0 / λ) + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1); + // Output an equivalent constant speed + return distance_to_reach_zero_speed / t_h; + } + + // if the vehicle speed limit is not surpassed we return an equivalent speed = x(T) / T + // alternatively, if the vehicle is still accelerating and has surpassed the speed limit. + // assume it will continue accelerating (reckless driving) + const bool object_has_surpassed_limit_already = v > speed_limit; + if (terminal_velocity < speed_limit || object_has_surpassed_limit_already) + return v + a * (1.0 / λ) + (a / (t_h * std::pow(λ, 2))) * (std::exp(-λ * t_h) - 1); + + // It is assumed the vehicle accelerates until final_speed is reached and + // then continues at constant speed for the rest of the time horizon + // So, we calculate the time it takes to reach the speed limit and compute how far the vehicle + // would go if it accelerated until reaching the speed limit, and then continued at a constant + // speed. + const double t_f = (-1.0 / λ) * std::log(1 - ((speed_limit - v) * λ) / a); + const double distance_covered = + // Distance covered while accelerating + a * (1.0 / λ) * t_f + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + v * t_f + + // Distance covered at constant speed for the rest of the horizon time + speed_limit * (t_h - t_f); + return distance_covered / t_h; + }; + + const float acceleration_adjusted_velocity_x = get_acceleration_adjusted_velocity(vx, ax); + const float acceleration_adjusted_velocity_y = get_acceleration_adjusted_velocity(vy, ay); + frenet_point.s = motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; frenet_point.d = motion_utils::calcLateralOffset(ref_path, obj_point); - frenet_point.s_vel = vx * std::cos(delta_yaw) - vy * std::sin(delta_yaw); - frenet_point.d_vel = vx * std::sin(delta_yaw) + vy * std::cos(delta_yaw); + frenet_point.s_vel = acceleration_adjusted_velocity_x * std::cos(delta_yaw) - + acceleration_adjusted_velocity_y * std::sin(delta_yaw); + frenet_point.d_vel = acceleration_adjusted_velocity_x * std::sin(delta_yaw) + + acceleration_adjusted_velocity_y * std::cos(delta_yaw); frenet_point.s_acc = 0.0; frenet_point.d_acc = 0.0; From 43797811d32500492c711ee537f68bb800588126 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 19 Jan 2024 20:41:28 +0900 Subject: [PATCH 083/154] feat(behavior_path_planner): add enable_all_modules_auto_mode argument to launch files for behavior path planner modules (#6093) * Add enable_all_modules_auto_mode argument to launch files Signed-off-by: kyoichi-sugahara * set default value for enable_all_modules_auto_mode Signed-off-by: kyoichi-sugahara * fix enable_rtc configuration in scene_module_manager_interface.hpp Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- launch/tier4_planning_launch/launch/planning.launch.xml | 6 +++++- .../launch/scenario_planning/lane_driving.launch.xml | 3 +++ .../behavior_planning/behavior_planning.launch.xml | 2 ++ .../scenario_planning/scenario_planning.launch.xml | 5 ++++- .../interface/scene_module_manager_interface.hpp | 9 ++++++++- 5 files changed, 22 insertions(+), 3 deletions(-) diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index bf81125286256..e02ba883d5115 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -7,6 +7,8 @@ + + @@ -21,7 +23,9 @@ - + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 73e466afc0bda..bae084f80b51e 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -1,4 +1,6 @@ + + @@ -8,6 +10,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 1c7643359c496..d7da81c969da2 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -4,6 +4,7 @@ + @@ -195,6 +196,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 66c90ef2ff833..e673d28804480 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -1,4 +1,5 @@ + @@ -50,7 +51,9 @@ - + + + diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 43ecd17fa1844..e8a28ee684ce6 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -268,7 +268,14 @@ class SceneModuleManagerInterface // init manager configuration { std::string ns = name_ + "."; - config_.enable_rtc = getOrDeclareParameter(*node, ns + "enable_rtc"); + try { + config_.enable_rtc = getOrDeclareParameter(*node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(*node, ns + "enable_rtc"); + } catch (const std::exception & e) { + config_.enable_rtc = getOrDeclareParameter(*node, ns + "enable_rtc"); + } + config_.enable_simultaneous_execution_as_approved_module = getOrDeclareParameter(*node, ns + "enable_simultaneous_execution_as_approved_module"); config_.enable_simultaneous_execution_as_candidate_module = getOrDeclareParameter( From 94357d27c4003384d76aacbe24dd8718b2e69d6a Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 20 Jan 2024 15:07:26 +0900 Subject: [PATCH 084/154] feat(behavior_velocity_planner): add enable_all_modules_auto_mode argument to launch files for behavior velocity planner modules (#6094) * set default value for enable_all_modules_auto_mode Signed-off-by: kyoichi-sugahara * fix enable_rtc configuration in scene_module_manager_interface.hpp Signed-off-by: kyoichi-sugahara * Refactor scene module managers to use getEnableRTC function Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../behavior_planning.launch.xml | 1 + .../src/manager.cpp | 5 +---- .../src/manager.cpp | 3 +-- .../src/manager.cpp | 3 +-- .../src/manager.cpp | 5 ++--- .../src/manager.cpp | 3 +-- .../scene_module_interface.hpp | 17 +++++++++++++++++ .../src/manager.cpp | 3 +-- 8 files changed, 25 insertions(+), 15 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index d7da81c969da2..a297697234ef2 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -237,6 +237,7 @@ + diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 09d1c5a7c3270..d114ab0c65da9 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -16,7 +16,6 @@ #include #include -#include #include @@ -28,12 +27,10 @@ namespace behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.use_pass_judge_line = diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index b8190cb6124e7..dc3ce32be8505 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -31,8 +31,7 @@ using tier4_autoware_utils::getOrDeclareParameter; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".common.enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".common.enable_rtc")) { const std::string ns(getModuleName()); diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index e7ec6b37f7f20..834ffc03e5dde 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -35,8 +35,7 @@ using tier4_autoware_utils::getOrDeclareParameter; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 0f9faaaa901c1..d2311d1c0c992 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -35,11 +35,10 @@ using tier4_autoware_utils::getOrDeclareParameter; IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc.intersection")), + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection")), occlusion_rtc_interface_( &node, "intersection_occlusion", - getOrDeclareParameter( - node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) { const std::string ns(getModuleName()); auto & ip = intersection_param_; diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index d5e9eeddb6377..c4192750af1d5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -35,8 +35,7 @@ using tier4_autoware_utils::getOrDeclareParameter; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); auto & pp = planner_param_; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index fcde16d8a871c..d8b042ec880e4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -52,6 +53,7 @@ using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; +using tier4_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -251,6 +253,21 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishObjectsOfInterestMarker(); void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + { + bool enable_rtc = true; + + try { + enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(node, param_name); + } catch (const std::exception & e) { + enable_rtc = getOrDeclareParameter(node, param_name); + } + + return enable_rtc; + } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index fa1a516c107b0..89817f3342dbd 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -31,8 +31,7 @@ using tier4_autoware_utils::getOrDeclareParameter; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); From bbf678de4b9b61b06dc8b2f6e94512a1f1a03f28 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 20 Jan 2024 15:09:01 +0900 Subject: [PATCH 085/154] refactor(start_planner): calculate drivable lanes efficiently (#6105) * Refactor ShiftPullOut class and update expanded drivable lanes Signed-off-by: kyoichi-sugahara * Refactor drivable lane functions Signed-off-by: kyoichi-sugahara * Fix lane departure check in ShiftPullOut::plan() Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../shift_pull_out.hpp | 7 ++++ .../start_planner_module.hpp | 2 + .../src/shift_pull_out.cpp | 23 +---------- .../src/start_planner_module.cpp | 38 +++++++++++++++++++ 4 files changed, 49 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 2d50ae189dc13..fef9a4aa8ebfc 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -50,6 +50,11 @@ class ShiftPullOut : public PullOutPlannerBase ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, const double longitudinal_acc, const double lateral_acc); + void setDrivableLanes(const lanelet::ConstLanelets & drivable_lanes) + { + drivable_lanes_ = drivable_lanes; + } + std::shared_ptr lane_departure_checker_; private: @@ -58,6 +63,8 @@ class ShiftPullOut : public PullOutPlannerBase double calcPullOutLongitudinalDistance( const double lon_acc, const double shift_time, const double shift_length, const double max_curvature, const double min_distance) const; + + lanelet::ConstLanelets drivable_lanes_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 47cfa408162ca..5bbde1c2fc523 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -242,6 +242,8 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; + void updateDrivableLanes(); + lanelet::ConstLanelets createDrivableLanes() const; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 36b8b1dc347a1..3b4d08b65923c 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -49,11 +49,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_.max_back_distance; - const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - if (pull_out_lanes.empty()) { - return std::nullopt; - } - const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); @@ -80,20 +75,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_end_idx + 1); } - // extract shoulder lanes from pull out lanes - lanelet::ConstLanelets shoulder_lanes; - std::copy_if( - pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), - [&route_handler](const auto & pull_out_lane) { - return route_handler->isShoulderLanelet(pull_out_lane); - }); - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip)); - // crop backward path // removes points which are out of lanes up to the start pose. // this ensures that the backward_path stays within the drivable area when starting from a @@ -107,7 +88,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); const bool is_out_of_lane = - LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); + LaneDepartureChecker::isOutOfLane(drivable_lanes_, transformed_vehicle_footprint); if (i <= start_segment_idx) { if (!is_out_of_lane) { cropped_path.points.push_back(shift_path.points.at(i)); @@ -121,7 +102,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_shift_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_shift_start_to_end)) { continue; } diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 1b54b655fc424..7857462e87320 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -128,6 +128,7 @@ void StartPlannerModule::initVariables() debug_marker_.markers.clear(); initializeSafetyCheckParameters(); initializeCollisionCheckDebugMap(debug_data_.collision_check); + updateDrivableLanes(); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -560,6 +561,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; + updateDrivableLanes(); } void StartPlannerModule::incrementPathIndex() @@ -1325,6 +1327,42 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } +void StartPlannerModule::updateDrivableLanes() +{ + const auto drivable_lanes = createDrivableLanes(); + for (auto & planner : start_planners_) { + auto shift_pull_out = std::dynamic_pointer_cast(planner); + + if (shift_pull_out) { + shift_pull_out->setDrivableLanes(drivable_lanes); + } + } +} + +lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const +{ + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + if (pull_out_lanes.empty()) { + return lanelet::ConstLanelets{}; + } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [this](const auto & pull_out_lane) { + return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); + }); + const auto drivable_lanes = utils::transformToLanelets( + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes)); + return drivable_lanes; +} + void StartPlannerModule::setDebugData() { using marker_utils::addFootprintMarker; From 4194028f5561a3bf948c5ced9b6338cddb898f7f Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Sat, 20 Jan 2024 18:22:21 +0900 Subject: [PATCH 086/154] chore(pointcloud_container): move glog_component to autoware_launch (#6114) --- .../ground_segmentation/ground_segmentation.launch.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index b54172acd4afc..66a0bb4d0fb63 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -474,13 +474,7 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, def launch_setup(context, *args, **kwargs): pipeline = GroundSegmentationPipeline(context) - glog_component = ComposableNode( - package="glog_component", - plugin="GlogComponent", - name="glog_component", - ) - - components = [glog_component] + components = [] components.extend( pipeline.create_single_frame_obstacle_segmentation_components( input_topic=LaunchConfiguration("input/pointcloud"), From 9471311225f902f16369eeec672130eb4348be37 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 21 Jan 2024 14:01:14 +0900 Subject: [PATCH 087/154] fix(start/goal_planner): do not call freespace planner callback when not necessary (#6125) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 4 ++++ .../src/start_planner_module.cpp | 10 +++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index a13344ee87e7c..4dc0d1cb05cc0 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -275,6 +275,10 @@ void GoalPlannerModule::onTimer() void GoalPlannerModule::onFreespaceParkingTimer() { + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } + if (!planner_data_) { return; } diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 7857462e87320..6c9000caac956 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -86,6 +86,10 @@ StartPlannerModule::StartPlannerModule( void StartPlannerModule::onFreespacePlannerTimer() { + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } + if (!planner_data_) { return; } @@ -96,7 +100,11 @@ void StartPlannerModule::onFreespacePlannerTimer() const bool is_new_costmap = (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; - if (isStuck() && is_new_costmap) { + if (!is_new_costmap) { + return; + } + + if (isStuck()) { planFreespacePath(); } } From f391cb574a1c30ed58c2d54d03fd4e90935a6052 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 21 Jan 2024 14:02:56 +0900 Subject: [PATCH 088/154] feat(goal_planner): expand pull over lanes for detection area of path generation collision check (#6124) Signed-off-by: kosuke55 --- planning/behavior_path_goal_planner_module/README.md | 1 + .../config/goal_planner.param.yaml | 1 + .../goal_planner_parameters.hpp | 1 + .../src/goal_planner_module.cpp | 9 ++++++++- .../behavior_path_goal_planner_module/src/manager.cpp | 1 + 5 files changed, 12 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_goal_planner_module/README.md b/planning/behavior_path_goal_planner_module/README.md index f777e49e5fd78..6bd2f8b9c79e4 100644 --- a/planning/behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -155,6 +155,7 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio | use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | | object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | | object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | +| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | ## **Goal Search** diff --git a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml index 0ab617a1a7ab9..6f51b906c4c8f 100644 --- a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -38,6 +38,7 @@ object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 th_moving_object_velocity: 1.0 + detection_bound_offset: 15.0 # pull over pull_over: diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 21eb22046bff6..2310c0c4d422c 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -74,6 +74,7 @@ struct GoalPlannerParameters double object_recognition_collision_check_margin{0.0}; double object_recognition_collision_check_max_extra_stopping_margin{0.0}; double th_moving_object_velocity{0.0}; + double detection_bound_offset{0.0}; // pull over general params double pull_over_minimum_request_length{0.0}; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 4dc0d1cb05cc0..b2c647467ca87 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1491,9 +1491,16 @@ bool GoalPlannerModule::checkObjectsCollision( const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); + + const auto expanded_pull_over_lanes = + left_side_parking_ ? lanelet::utils::getExpandedLanelets( + pull_over_lanes, parameters_->detection_bound_offset, 0.0) + : lanelet::utils::getExpandedLanelets( + pull_over_lanes, 0.0, parameters_->detection_bound_offset); + const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes, + *(planner_data_->dynamic_object), expanded_pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_over_lane_objects, parameters_->th_moving_object_velocity); diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 8608d6cce2c81..c6fe97874f012 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -101,6 +101,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter( ns + "object_recognition_collision_check_max_extra_stopping_margin"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + p.detection_bound_offset = node->declare_parameter(ns + "detection_bound_offset"); } // pull over general params From 1a23d01de07034c80962e7ab167b046a6ae3eadd Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 21 Jan 2024 14:03:50 +0900 Subject: [PATCH 089/154] feat(goal_planner): display stop pose infomation (#6119) change message update Signed-off-by: kosuke55 --- .../goal_planner_module.hpp | 24 ++++++ .../src/goal_planner_module.cpp | 81 +++++++++++++------ 2 files changed, 82 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 9fb9c17f1ca70..a6ebaf00aed78 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -254,6 +254,29 @@ struct PreviousPullOverData bool has_decided_path{false}; }; +// store stop_pose_ pointer with reason string +struct PoseWithString +{ + std::optional * pose; + std::string string; + + explicit PoseWithString(std::optional * shared_pose) : pose(shared_pose), string("") {} + + void set(const Pose & new_pose, const std::string & new_string) + { + *pose = new_pose; + string = new_string; + } + + void set(const std::string & new_string) { string = new_string; } + + void clear() + { + pose->reset(); + string = ""; + } +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -385,6 +408,7 @@ class GoalPlannerModule : public SceneModuleInterface // debug mutable GoalPlannerDebugData debug_data_; + mutable PoseWithString debug_stop_pose_with_info_; // collision check void initializeOccupancyGridMap(); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index b2c647467ca87..fa6a44b97a623 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -61,7 +61,8 @@ GoalPlannerModule::GoalPlannerModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, - thread_safe_data_{mutex_, clock_} + thread_safe_data_{mutex_, clock_}, + debug_stop_pose_with_info_{&stop_pose_} { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); @@ -845,10 +846,13 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const } else { // not_safe -> not_safe: use previous stop path output.path = *prev_data_.stop_path; - // stop_pose_ is removed in manager every loop, so need to set every loop. - stop_pose_ = utils::getFirstStopPoseFromPath(output.path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); + // stop_pose_ is removed in manager every loop, so need to set every loop. + const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path); + if (stop_pose_opt) { + debug_stop_pose_with_info_.set(stop_pose_opt.value(), std::string("keep previous stop pose")); + } } } @@ -863,6 +867,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output parameters_->maximum_jerk_for_stop); if (stop_path) { output.path = *stop_path; + debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); @@ -874,7 +879,11 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path output.path = *prev_data_.stop_path_after_approval; // stop_pose_ is removed in manager every loop, so need to set every loop. - stop_pose_ = utils::getFirstStopPoseFromPath(output.path); + const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path); + if (stop_pose_opt) { + debug_stop_pose_with_info_.set( + stop_pose_opt.value(), std::string("keep feasible stop after approval")); + } RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } } @@ -1018,6 +1027,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() return output; } + setDebugData(); + return output; } @@ -1242,37 +1253,47 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const // (In the case of the curve lane, the position is not aligned due to the // difference between the outer and inner sides) // 4. feasible stop - const auto stop_pose = std::invoke([&]() -> std::optional { - if (thread_safe_data_.foundPullOverPath()) { - return thread_safe_data_.get_pull_over_path()->start_pose; - } - if (thread_safe_data_.get_closest_start_pose()) { - return thread_safe_data_.get_closest_start_pose().value(); - } - if (!decel_pose) { - return std::nullopt; - } - return decel_pose.value(); - }); - if (!stop_pose) { - return generateFeasibleStopPath(); + const auto stop_pose_with_info = + std::invoke([&]() -> std::optional> { + if (thread_safe_data_.foundPullOverPath()) { + return std::make_pair( + thread_safe_data_.get_pull_over_path()->start_pose, "stop at selected start pose"); + } + if (thread_safe_data_.get_closest_start_pose()) { + return std::make_pair( + thread_safe_data_.get_closest_start_pose().value(), "stop at closest start pose"); + } + if (!decel_pose) { + return std::nullopt; + } + return std::make_pair(decel_pose.value(), "stop at search start pose"); + }); + if (!stop_pose_with_info) { + const auto feasible_stop_path = generateFeasibleStopPath(); + // override stop pose info debug string + debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose")); + return feasible_stop_path; } + const Pose stop_pose = stop_pose_with_info->first; // if stop pose is closer than min_stop_distance, stop as soon as possible - const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, *stop_pose); + const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) { - return generateFeasibleStopPath(); + const auto feasible_stop_path = generateFeasibleStopPath(); + debug_stop_pose_with_info_.set( + std::string("feasible stop: stop pose is closer than min_stop_distance")); + return feasible_stop_path; } // slow down for turn signal, insert stop point to stop_pose auto stop_path = extended_prev_path; - decelerateForTurnSignal(*stop_pose, stop_path); - stop_pose_ = *stop_pose; // for debug wall marker + decelerateForTurnSignal(stop_pose, stop_path); + debug_stop_pose_with_info_.set(stop_pose, stop_pose_with_info->second); // slow down before the search area. if (decel_pose) { @@ -1310,7 +1331,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { - stop_pose_ = stop_path.points.at(*stop_idx).point.pose; + debug_stop_pose_with_info_.set(stop_path.points.at(*stop_idx).point.pose, "feasible stop"); } return stop_path; @@ -2091,6 +2112,20 @@ void GoalPlannerModule::setDebugData() planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } + + // Visualize stop pose info + if (debug_stop_pose_with_info_.pose->has_value()) { + visualization_msgs::msg::MarkerArray stop_pose_marker_array{}; + const auto color = createMarkerColor(1.0, 1.0, 1.0, 0.99); + auto marker = createDefaultMarker( + header.frame_id, header.stamp, "stop_pose_info", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 0.5), color); + marker.pose = debug_stop_pose_with_info_.pose->value(); + marker.text = debug_stop_pose_with_info_.string; + stop_pose_marker_array.markers.push_back(marker); + add(stop_pose_marker_array); + add(createPoseMarkerArray(marker.pose, "stop_pose", 1.0, 1.0, 1.0, 0.9)); + } } void GoalPlannerModule::printParkingPositionError() const From 547a0251975ad3077d414adbf8463082dc55357e Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 21 Jan 2024 22:37:39 +0900 Subject: [PATCH 090/154] feat(planning_validator): add invalid size error (#6126) * feat(planning_validator): add invalid size error Signed-off-by: Takamasa Horibe * Update planning/planning_validator/src/planning_validator.cpp Co-authored-by: Kosuke Takeuchi * Update planning/planning_validator/src/planning_validator.cpp Co-authored-by: Kosuke Takeuchi * Update planning/planning_validator/include/planning_validator/planning_validator.hpp Co-authored-by: Kosuke Takeuchi * add const for some functions Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe Co-authored-by: Kosuke Takeuchi --- .../planning_validator/planning_validator.hpp | 3 +- .../msg/PlanningValidatorStatus.msg | 1 + .../src/planning_validator.cpp | 42 ++++++++++++++----- 3 files changed, 35 insertions(+), 11 deletions(-) diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 037c7fb0f4e95..a12eb7dedcf88 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -64,6 +64,7 @@ class PlanningValidator : public rclcpp::Node void onTrajectory(const Trajectory::ConstSharedPtr msg); + bool checkValidSize(const Trajectory & trajectory) const; bool checkValidFiniteValue(const Trajectory & trajectory); bool checkValidInterval(const Trajectory & trajectory); bool checkValidRelativeAngle(const Trajectory & trajectory); @@ -116,7 +117,7 @@ class PlanningValidator : public rclcpp::Node vehicle_info_util::VehicleInfo vehicle_info_; - bool isAllValid(const PlanningValidatorStatus & status); + bool isAllValid(const PlanningValidatorStatus & status) const; Trajectory::ConstSharedPtr current_trajectory_; Trajectory::ConstSharedPtr previous_published_trajectory_; diff --git a/planning/planning_validator/msg/PlanningValidatorStatus.msg b/planning/planning_validator/msg/PlanningValidatorStatus.msg index cfc991b35ffc7..b218e7996888a 100644 --- a/planning/planning_validator/msg/PlanningValidatorStatus.msg +++ b/planning/planning_validator/msg/PlanningValidatorStatus.msg @@ -1,6 +1,7 @@ builtin_interfaces/Time stamp # states +bool is_valid_size bool is_valid_finite_value bool is_valid_interval bool is_valid_relative_angle diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 58af2c08ccb22..6185163811614 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -113,6 +113,9 @@ void PlanningValidator::setupDiag() d->setHardwareID("planning_validator"); std::string ns = "trajectory_validation_"; + d->add(ns + "size", [&](auto & stat) { + setStatus(stat, validation_status_.is_valid_size, "invalid trajectory size is found"); + }); d->add(ns + "finite", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_finite_value, "infinite value is found"); }); @@ -251,14 +254,26 @@ void PlanningValidator::publishDebugInfo() void PlanningValidator::validate(const Trajectory & trajectory) { - if (trajectory.points.size() < 2) { - RCLCPP_ERROR(get_logger(), "trajectory size is less than 2. Cannot validate."); - return; - } - auto & s = validation_status_; + const auto terminateValidation = [&](const auto & ss) { + RCLCPP_ERROR_STREAM(get_logger(), ss); + s.invalid_count += 1; + }; + + s.is_valid_size = checkValidSize(trajectory); + if (!s.is_valid_size) { + return terminateValidation( + "trajectory has invalid point size (" + std::to_string(trajectory.points.size()) + + "). Stop validation process, raise an error."); + } + s.is_valid_finite_value = checkValidFiniteValue(trajectory); + if (!s.is_valid_finite_value) { + return terminateValidation( + "trajectory has invalid value (NaN, Inf, etc). Stop validation process, raise an error."); + } + s.is_valid_interval = checkValidInterval(trajectory); s.is_valid_lateral_acc = checkValidLateralAcceleration(trajectory); s.is_valid_longitudinal_max_acc = checkValidMaxLongitudinalAcceleration(trajectory); @@ -279,6 +294,11 @@ void PlanningValidator::validate(const Trajectory & trajectory) s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1; } +bool PlanningValidator::checkValidSize(const Trajectory & trajectory) const +{ + return trajectory.points.size() >= 2; +} + bool PlanningValidator::checkValidFiniteValue(const Trajectory & trajectory) { for (const auto & p : trajectory.points) { @@ -427,12 +447,13 @@ bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajector return true; } -bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) +bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const { - return s.is_valid_finite_value && s.is_valid_interval && s.is_valid_relative_angle && - s.is_valid_curvature && s.is_valid_lateral_acc && s.is_valid_longitudinal_max_acc && - s.is_valid_longitudinal_min_acc && s.is_valid_steering && s.is_valid_steering_rate && - s.is_valid_velocity_deviation && s.is_valid_distance_deviation; + return s.is_valid_size && s.is_valid_finite_value && s.is_valid_interval && + s.is_valid_relative_angle && s.is_valid_curvature && s.is_valid_lateral_acc && + s.is_valid_longitudinal_max_acc && s.is_valid_longitudinal_min_acc && + s.is_valid_steering && s.is_valid_steering_rate && s.is_valid_velocity_deviation && + s.is_valid_distance_deviation; } void PlanningValidator::displayStatus() @@ -447,6 +468,7 @@ void PlanningValidator::displayStatus() const auto & s = validation_status_; + warn(s.is_valid_size, "planning trajectory size is invalid, too small."); warn(s.is_valid_curvature, "planning trajectory curvature is too large!!"); warn(s.is_valid_distance_deviation, "planning trajectory is too far from ego!!"); warn(s.is_valid_finite_value, "planning trajectory has invalid value!!"); From 19009f78b7dbed1d4e41387cfe6be72ddee567c6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 22 Jan 2024 02:07:20 +0900 Subject: [PATCH 091/154] feat(dynamic_avoidance): add a guard of LPF for reference path changed by avoidance/LC (#6112) Signed-off-by: Takayuki Murooka --- .../scene.hpp | 5 +- .../src/scene.cpp | 48 ++++++++++--------- 2 files changed, 29 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index d2f9cd95066d9..a5380b628387d 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -382,8 +382,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface const TimeWhileCollision & time_while_collision) const; std::optional calcMinMaxLateralOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, - const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, - const double obj_normal_vel, const std::optional & prev_object) const; + const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, + const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index ca58bbf0adcaa..9388a0a354d50 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -676,8 +676,8 @@ void DynamicAvoidanceModule::updateTargetObjects() ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, time_while_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( - ref_path_points_for_obj_poly, obj_points, object.vel, is_collision_left, object.lat_vel, - prev_object); + ref_path_points_for_obj_poly, obj_points, object.pose.position, object.vel, is_collision_left, + object.lat_vel, prev_object); if (!lat_offset_to_avoid) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -808,9 +808,9 @@ TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const double signed_time_to_start_collision = [&]() { - const double lon_offset_ego_front_to_obj_back = lon_offset_ego_to_obj_idx + - lat_lon_offset.min_lon_offset - - planner_data_->parameters.front_overhang; + const double lon_offset_ego_front_to_obj_back = + lon_offset_ego_to_obj_idx + lat_lon_offset.min_lon_offset - + (planner_data_->parameters.wheel_base + planner_data_->parameters.front_overhang); const double lon_offset_obj_front_to_ego_back = -lon_offset_ego_to_obj_idx - lat_lon_offset.max_lon_offset - planner_data_->parameters.rear_overhang; @@ -1070,7 +1070,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object; } // The ego and object are the opposite directional. - const double obj_acc = -1.0; + const double obj_acc = -0.5; const double decel_time = 1.0; const double obj_moving_dist = (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 / @@ -1120,9 +1120,10 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); - const double dist_threshold_paths = planner_data_->parameters.vehicle_width / 2.0 + - parameters_->lat_offset_from_obstacle + - calcObstacleMaxLength(obj_shape); + constexpr double dist_threshold_additional_margin = 0.5; + const double dist_threshold_paths = + planner_data_->parameters.vehicle_width / 2.0 + parameters_->lat_offset_from_obstacle + + calcObstacleWidth(obj_shape) / 2.0 + dist_threshold_additional_margin; // crop the ego's path by object position std::vector cropped_ego_path_points; @@ -1204,28 +1205,31 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, - const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, - const double obj_normal_vel, const std::optional & prev_object) const + const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, + const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const { - const bool enable_lowpass_filter = true; - /* const bool enable_lowpass_filter = [&]() { - if (!prev_ref_path_points_for_obj_poly_ || prev_ref_path_points_for_obj_poly_->size() < 2) { + if ( + !prev_object || prev_object->ref_path_points_for_obj_poly.size() < 2 || + ref_path_points_for_obj_poly.size() < 2) { return true; } - const size_t prev_front_seg_idx = motion_utils::findNearestSegmentIndex( - *prev_ref_path_points_for_obj_poly_, - ref_path_points_for_obj_poly.front().point.pose.position); constexpr double - min_lane_change_path_lat_offset = 1.0; if ( motion_utils::calcLateralOffset( - *prev_ref_path_points_for_obj_poly_, - ref_path_points_for_obj_poly.front().point.pose.position, prev_front_seg_idx) < - min_lane_change_path_lat_offset) { return true; + const size_t obj_point_idx = + motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos); + const double paths_lat_diff = std::abs(motion_utils::calcLateralOffset( + prev_object->ref_path_points_for_obj_poly, + ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); + + std::cerr << paths_lat_diff << std::endl; + constexpr double min_paths_lat_diff = 0.3; + if (paths_lat_diff < min_paths_lat_diff) { + return true; } // NOTE: When the input reference path laterally changes, the low-pass filter is disabled not to // shift the obstacle polygon suddenly. return false; }(); - */ // calculate min/max lateral offset from object to path const auto obj_lat_abs_offset = [&]() { From 57933b86298ba88eb8ff01ebdf91b3fc58284210 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Mon, 22 Jan 2024 13:08:49 +0900 Subject: [PATCH 092/154] chore(planning): change params to vehicle tested values (#6118) upadate params Signed-off-by: Yuki Takagi --- .../config/crosswalk.param.yaml | 6 +++--- .../config/walkway.param.yaml | 2 +- .../config/obstacle_cruise_planner.param.yaml | 8 ++++---- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 273db320f1027..aa5dfed1e4bc5 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -50,15 +50,15 @@ min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] - stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) + stop_object_velocity_threshold: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding disable_stop_for_yield_cancel: true # for the crosswalks with traffic signal disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order - timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] - timeout_ego_stop_for_yield: 3.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. + timeout_map_for_no_intention_to_walk: [1.0, 0.0] # [s] + timeout_ego_stop_for_yield: 1.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. # param for target object filtering object_filtering: diff --git a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml b/planning/behavior_velocity_walkway_module/config/walkway.param.yaml index f21e3d12db56f..07f493edcde12 100644 --- a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml +++ b/planning/behavior_velocity_walkway_module/config/walkway.param.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: walkway: - stop_duration: 1.0 # [s] stop time at stop position + stop_duration: 0.1 # [s] stop time at stop position stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index d8b9c942a986a..aad4bd7e17757 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -85,15 +85,15 @@ obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop stop: - max_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 0.2 # lateral margin between obstacle and trajectory band with ego's width crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] cruise: max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: - obstacle_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] - ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] + obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego slow_down: @@ -188,7 +188,7 @@ static: min_lat_margin: 0.2 max_lat_margin: 1.0 - min_ego_velocity: 2.0 + min_ego_velocity: 4.0 max_ego_velocity: 8.0 pedestrian: moving: From 454e19d8fc6eadf095f74407ca193cc444ba6f5d Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Mon, 22 Jan 2024 13:13:34 +0900 Subject: [PATCH 093/154] docs(crosswalk): revise proposal (#6109) * revise the doc Signed-off-by: Yuki Takagi Co-authored-by: Takayuki Murooka --- .../README.md | 74 +++++++++---------- 1 file changed, 37 insertions(+), 37 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 37c3fb58049a6..ce231659ccf78 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -2,7 +2,7 @@ ## Role -This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage of crosswalk users such as pedestrians and bicycles based on the objects' behavior and surround traffic. +This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage for crosswalk users, such as pedestrians and bicycles, based on the objects' behavior and surround traffic.
![crosswalk_module](docs/crosswalk_module.svg){width=1100} @@ -14,7 +14,7 @@ This module judges whether the ego should stop in front of the crosswalk in orde #### Target Object -The target object's type is filtered by the following parameters in the `object_filtering.target_object` namespace. +The crosswalk module handles objects of the types defined by the following parameters in the `object_filtering.target_object` namespace. | Parameter | Unit | Type | Description | | ------------ | ---- | ---- | ---------------------------------------------- | @@ -23,13 +23,13 @@ The target object's type is filtered by the following parameters in the `object_ | `bicycle` | [-] | bool | whether to look and stop by BICYCLE objects | | `motorcycle` | [-] | bool | whether to look and stop by MOTORCYCLE objects | -In order to detect crosswalk users crossing outside the crosswalk as well, the crosswalk module creates an attention area around the crosswalk shown as the yellow polygon in the figure. If the object's predicted path collides with the attention area, the object will be targeted for yield. +In order to handle the crosswalk users crossing the neighborhood but outside the crosswalk, the crosswalk module creates an attention area around the crosswalk, shown as the yellow polygon in the figure. If the object's predicted path collides with the attention area, the object will be targeted for yield.
![crosswalk_attention_range](docs/crosswalk_attention_range.svg){width=600}
-The parameter is in the `object_filtering.target_object` namespace. +The neighborhood is defined by the following parameter in the `object_filtering.target_object` namespace. | Parameter | Unit | Type | Description | | --------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------- | @@ -51,13 +51,13 @@ When the stop line does **NOT** exist in the lanelet map, the stop position is c
-On the other hand, if pedestrian (bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `far_object_threshold` meters away from the stop line, the actual stop position is determined to be `stop_distance_from_object` and pedestrian position, not at the stop line. +As an exceptional case, if a pedestrian (or bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `far_object_threshold` meters away from the stop line, the actual stop position is determined by `stop_distance_from_object` and pedestrian position, not at the stop line.
![far_object_threshold](docs/far_object_threshold.drawio.svg){width=700}
-In the `stop_position` namespace, +In the `stop_position` namespace, the following parameters are defined. | Parameter | | Type | Description | | ------------------------------ | --- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | @@ -69,12 +69,12 @@ In the `stop_position` namespace, #### Yield decision The module makes a decision to yield only when the pedestrian traffic light is **GREEN** or **UNKNOWN**. -Calculating the collision point, the decision is based on the following variables. +The decision is based on the following variables, along with the calculation of the collision point. -- TTC: Time-To-Collision which is the time for the **ego** to reach the virtual collision point. -- TTV: Time-To-Vehicle which is the time for the **object** to reach the virtual collision point. +- Time-To-Collision (TTC): The time for the **ego** to reach the virtual collision point. +- Time-To-Vehicle (TTV): The time for the **object** to reach the virtual collision point. -Depending on the relative relationship between TTC and TTV, the ego's behavior at crosswalks can be classified into three categories based on [1] +We classify ego behavior at crosswalks into three categories according to the relative relationship between TTC and TTV [1]. - A. **TTC >> TTV**: The object has enough time to cross before the ego. - No stop planning. @@ -92,10 +92,12 @@ Depending on the relative relationship between TTC and TTV, the ego's behavior a
-The boundary of A and B is interpolated from `ego_pass_later_margin_x` and `ego_pass_later_margin_y`. In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `ego_pass_later_margin_y` is `{1, 4, 6}` for example. -The same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}` for example. +The boundary of A and B is interpolated from `ego_pass_later_margin_x` and `ego_pass_later_margin_y`. +In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `ego_pass_later_margin_y` is `{1, 4, 6}`. +In the same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. +In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}`. -In the `pass_judge` namespace, +In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | | ---------------------------------- | ----- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | @@ -108,17 +110,17 @@ In the `pass_judge` namespace, ### Smooth Yield Decision -When the object is stopped around the crosswalk but has no intention to walk, the ego will yield the object forever. -To prevent this dead lock behavior, the ego will cancel the yield depending on the situation. +If the object is stopped near the crosswalk but has no intention of walking, a situation can arise in which the ego continues to yield the right-of-way to the object. +To prevent such a deadlock situation, the ego will cancel yielding depending on the situation. -#### When there is no traffic light +#### Cases without traffic lights -For the object stopped around the crosswalk but has no intention to walk (\*1), when the ego keeps stopping to yield for a certain time (\*2), the ego cancels the yield and start driving. +For the object stopped around the crosswalk but has no intention to walk (\*1), after the ego has keep stopping to yield for a specific time (\*2), the ego cancels the yield and starts driving. \*1: The time is calculated by the interpolation of distance between the object and crosswalk with `distance_map_for_no_intention_to_walk` and `timeout_map_for_no_intention_to_walk`. -In the `pass_judge` namespace, +In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | | --------------------------------------- | ----- | ------ | --------------------------------------------------------------------------------- | @@ -126,19 +128,19 @@ In the `pass_judge` namespace, | `timeout_map_for_no_intention_to_walk` | [[s]] | double | timeout map to calculate the timeout for no intention to walk with interpolation | \*2: -In the `pass_judge` namespace, +In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | | ---------------------------- | --- | ------ | ----------------------------------------------------------------------------------------------------------------------- | | `timeout_ego_stop_for_yield` | [s] | double | If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. | -#### When there is traffic light +#### Cases with traffic lights -For the object stopped around the crosswalk but has no intention to walk (\*1), the ego will cancel the yield without stopping. +The ego will cancel the yield without stopping when the object stops around the crosswalk but has no intention to walk (\*1). This comes from the assumption that the object has no intention to walk since it is stopped even though the pedestrian traffic light is green. \*1: -The crosswalk user's intention to walk is calculated in the same way as `When there is no traffic light`. +The crosswalk user's intention to walk is calculated in the same way as `Cases without traffic lights`.
@@ -152,12 +154,12 @@ The crosswalk user's intention to walk is calculated in the same way as `When th #### New Object Handling Due to the perception's limited performance where the tree or poll is recognized as a pedestrian or the tracking failure in the crowd or occlusion, even if the surrounding environment does not change, the new pedestrian (= the new ID's pedestrian) may suddenly appear unexpectedly. -When this happens when the ego is going to pass the crosswalk, the ego will stop suddenly. +If this happens while the ego is going to pass the crosswalk, the ego will stop suddenly. -To fix this issue, the option `disable_yield_for_new_stopped_object` is prepared. -If set to true, the new stopped object will be ignored during the yield decision around the crosswalk with a traffic light. +To deal with this issue, the option `disable_yield_for_new_stopped_object` is prepared. +If true is set, the yield decisions around the crosswalk with a traffic light will ignore the new stopped object. -In the `pass_judge` namespace, +In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | | -------------------------------------- | --- | ---- | ------------------------------------------------------------------------------------------------ | @@ -165,11 +167,9 @@ In the `pass_judge` namespace, ### Safety Slow Down Behavior -In current autoware implementation if there are no target objects around a crosswalk, ego vehicle -will not slow down for the crosswalk. However, if ego vehicle to slow down to a certain speed in -such cases is wanted then it is possible by adding some tags to the related crosswalk definition as -it is instructed -in [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) +In the current autoware implementation, if no target object is detected around a crosswalk, the ego vehicle will not slow down for the crosswalk. +However, it may be desirable to slow down in situations, for example, where there are blind spots. +Such a situation can be handled by setting some tags to the related crosswalk as instructed in the [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) document. | Parameter | | Type | Description | @@ -182,7 +182,7 @@ document. ### Stuck Vehicle Detection The feature will make the ego not to stop on the crosswalk. -When there are low-speed or stopped vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle, the crosswalk module will plan to stop before the crosswalk even if there are no pedestrians or bicycles. +When there is a low-speed or stopped vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle, the crosswalk module plans to stop before the crosswalk even if there are no pedestrians or bicycles. `min_acc`, `min_jerk`, and `max_jerk` are met. If the ego cannot stop before the crosswalk with these parameters, the stop position will move forward. @@ -190,7 +190,7 @@ When there are low-speed or stopped vehicle ahead of the crosswalk, and there is ![stuck_vehicle_attention_range](docs/stuck_vehicle_detection.svg){width=600} -In the `stuck_vehicle` namespace, +In the `stuck_vehicle` namespace, the following parameters are defined. | Parameter | Unit | Type | Description | | ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- | @@ -203,7 +203,7 @@ In the `stuck_vehicle` namespace, ### Others -In the `common` namespace, +In the `common` namespace, the following parameters are defined. | Parameter | Unit | Type | Description | | ----------------------------- | ---- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | @@ -231,10 +231,10 @@ In the `common` namespace, - Ego footprints' polygon to calculate the collision check. - Pink polygons - Object footprints' polygon to calculate the collision check. -- The color of crosswalk - - Considering the traffic light's color, red means the target crosswalk and white means the ignored crosswalk. +- The color of crosswalks + - Considering the traffic light's color, red means the target crosswalk, and white means the ignored crosswalk. - Texts - - It shows the module id, TTC, TTV, and the module state. + - It shows the module ID, TTC, TTV, and the module state. ### Visualization of Time-To-Collision From 704f50909f8b0fce0545e9e84ab999880f0b9f2c Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 22 Jan 2024 13:58:47 +0900 Subject: [PATCH 094/154] docs(start_planner): fix image filename in README.md (#6121) fix image filename in README.md Signed-off-by: kyoichi-sugahara --- planning/behavior_path_start_planner_module/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index aeefd18357a5c..b26864eb450d5 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -16,7 +16,7 @@ Use cases include: - pull out from the shoulder lane to the road lane centerline.
- ![case2](images/start_from_start_from_road_shoulder.drawio.svg){width=1000} + ![case2](images/start_from_road_shoulder.drawio.svg){width=1000}
pull out from the shoulder lane
From 378bf3a14c14e8b74de2228bb8e2fe726640c555 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 22 Jan 2024 13:59:15 +0900 Subject: [PATCH 095/154] docs(side_shift): update side shift document (#6123) * update side shift document Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../behavior_path_side_shift_module/README.md | 25 ++++- .../images/side_shift_status.drawio.svg | 101 ++++++++++++++++++ 2 files changed, 125 insertions(+), 1 deletion(-) create mode 100644 planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg diff --git a/planning/behavior_path_side_shift_module/README.md b/planning/behavior_path_side_shift_module/README.md index b76e549eb602f..6a460d8423cbd 100644 --- a/planning/behavior_path_side_shift_module/README.md +++ b/planning/behavior_path_side_shift_module/README.md @@ -2,6 +2,29 @@ (For remote control) Shift the path to left or right according to an external instruction. +## Overview of the Side Shift Module Process + +1. Receive the required lateral offset input. +2. Update the `requested_lateral_offset_` under the following conditions: + a. Verify if the last update time has elapsed. + b. Ensure the required lateral offset value is different from the previous one. +3. Insert the shift points into the path if the side shift module's status is not in the SHIFTING status. + +Please be aware that `requested_lateral_offset_` is continuously updated with the latest values and is not queued. + +## Statuses of the Side Shift + +The side shift has three distinct statuses. Note that during the SHIFTING status, the path cannot be updated: + +1. BEFORE_SHIFT: Preparing for shift. +2. SHIFTING: Currently in the process of shifting. +3. AFTER_SHIFT: Shift completed. + +
+ ![case1](images/side_shift_status.drawio.svg){width=1000} +
side shift status
+
+ ## Flowchart ```plantuml @@ -27,7 +50,7 @@ stop @enduml ``` -```plantuml --> +```plantuml @startuml skinparam monochrome true skinparam defaultTextAlignment center diff --git a/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg b/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg new file mode 100644 index 0000000000000..447a00236d2b6 --- /dev/null +++ b/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg @@ -0,0 +1,101 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + SideShiftStatus + + ::SHIFTING +
+ + +
+
+ + +
+
+ SideShiftStatus + ::BEFORE_SHIFT + +
+ +
+
+ + +
+
SIDE SHIFT START POINT
+
+ + +
+
SIDE SHIFT END POINT
+
+ + +
+
+ SideShiftStatus + ::AFTER_SHIFT + +
+ +
+
+ + From f5cef6c5ad7c5d9fc88ef507d1594cb8eb43c3ff Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 22 Jan 2024 14:12:04 +0900 Subject: [PATCH 096/154] refactor(run_out): reorganize the parameter (#6064) * chore(run_out): Reorganize the parameter Signed-off-by: Tomohito Ando * style(pre-commit): autofix --------- Signed-off-by: Tomohito Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/run_out.param.yaml | 62 +++++++++++-------- .../src/manager.cpp | 16 +++-- .../src/scene.cpp | 2 +- .../src/utils.hpp | 16 ++--- 4 files changed, 51 insertions(+), 45 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index 5534228c1b86f..196f7c6296cb4 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -3,8 +3,8 @@ run_out: detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points use_partition_lanelet: true # [-] whether to use partition lanelet map data - suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet: - specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates + suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet + specify_decel_jerk: true # [-] whether to specify jerk when ego decelerates stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles @@ -12,15 +12,7 @@ detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision - detection_area: - margin_behind: 0.5 # [m] ahead margin for detection area length - margin_ahead: 1.0 # [m] behind margin for detection area length - - # This area uses points not filtered by vector map to ensure safety - mandatory_area: - decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area - - # parameter to create abstracted dynamic obstacles + # Parameter to create abstracted dynamic obstacles dynamic_obstacle: use_mandatory_area: false # [-] whether to use mandatory detection area assume_fixed_velocity: @@ -34,26 +26,42 @@ time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method - # approach if ego has stopped in the front of the obstacle for a certain amount of time - approaching: - enable: false - margin: 0.0 # [m] distance on how close ego approaches the obstacle - limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping - - # parameters for the change of state. used only when approaching is enabled - state: - stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping - stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state - disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value - keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition - - # parameter to avoid sudden stopping + # Parameter to prevent sudden stopping. + # If the deceleration jerk and acceleration exceed this value upon inserting a stop point, + # the deceleration will be moderated to stay under this value. slow_down_limit: - enable: true + enable: false max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. - # prevent abrupt stops caused by false positives in perception + # Parameter to prevent abrupt stops caused by false positives in perception ignore_momentary_detection: enable: true time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration + + # Typically used when the "detection_method" is set to ObjectWithoutPath or Points + # Approach if the ego has stopped in front of the obstacle for a certain period + # This avoids the issue of the ego continuously stopping in front of the obstacle + approaching: + enable: false + margin: 0.0 # [m] distance on how close ego approaches the obstacle + limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping + # Parameters for state change when "approaching" is enabled + state: + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value + keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition + + # Only used when "detection_method" is set to Points + # Filters points by the detection area polygon to reduce computational cost + # The detection area is calculated based on the current velocity and acceleration and deceleration jerk constraints + detection_area: + margin_behind: 0.5 # [m] ahead margin for detection area length + margin_ahead: 1.0 # [m] behind margin for detection area length + + # Only used when "detection_method" is set to Points + # Points in this area are detected even if it is in the no obstacle segmentation area + # The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints + mandatory_area: + decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 3ba9bf8bf52e6..f2ef366cbe121 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -105,16 +105,14 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.enable = getOrDeclareParameter(node, ns_a + ".enable"); p.margin = getOrDeclareParameter(node, ns_a + ".margin"); p.limit_vel_kmph = getOrDeclareParameter(node, ns_a + ".limit_vel_kmph"); - } - { - auto & p = planner_param_.state_param; - const std::string ns_s = ns + ".state"; - p.stop_thresh = getOrDeclareParameter(node, ns_s + ".stop_thresh"); - p.stop_time_thresh = getOrDeclareParameter(node, ns_s + ".stop_time_thresh"); - p.disable_approach_dist = getOrDeclareParameter(node, ns_s + ".disable_approach_dist"); - p.keep_approach_duration = - getOrDeclareParameter(node, ns_s + ".keep_approach_duration"); + const std::string ns_as = ns_a + ".state"; + p.state.stop_thresh = getOrDeclareParameter(node, ns_as + ".stop_thresh"); + p.state.stop_time_thresh = getOrDeclareParameter(node, ns_as + ".stop_time_thresh"); + p.state.disable_approach_dist = + getOrDeclareParameter(node, ns_as + ".disable_approach_dist"); + p.state.keep_approach_duration = + getOrDeclareParameter(node, ns_as + ".keep_approach_duration"); } { diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index b14152863d8d0..50b69fc6139c9 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -46,7 +46,7 @@ RunOutModule::RunOutModule( planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), debug_ptr_(debug_ptr), - state_machine_(std::make_unique(planner_param.state_param)) + state_machine_(std::make_unique(planner_param.approaching.state)) { velocity_factor_.init(PlanningBehavior::UNKNOWN); diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 5524c0f76049d..c4976a119dd00 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -85,13 +85,6 @@ struct MandatoryArea float decel_jerk; }; -struct ApproachingParam -{ - bool enable; - float margin; - float limit_vel_kmph; -}; - struct StateParam { float stop_thresh; @@ -100,6 +93,14 @@ struct StateParam float keep_approach_duration; }; +struct ApproachingParam +{ + bool enable; + float margin; + float limit_vel_kmph; + StateParam state; +}; + struct SlowDownLimit { bool enable; @@ -143,7 +144,6 @@ struct PlannerParam DetectionArea detection_area; MandatoryArea mandatory_area; ApproachingParam approaching; - StateParam state_param; DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother; From 56dcff54b3f361b0aa5b4e17d6e7c756c49182c9 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 22 Jan 2024 16:05:38 +0900 Subject: [PATCH 097/154] chore(run_out): add maintainer (#6130) Signed-off-by: Tomohito Ando --- planning/behavior_velocity_run_out_module/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index 809579b383461..606c63ea3448c 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -10,6 +10,7 @@ Tomoya Kimura Shumpei Wakabayashi Takayuki Murooka + Kosuke Takeuchi Apache License 2.0 From 72b619d8348b5b77f338f567855fc71a1803d3b5 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 22 Jan 2024 18:23:19 +0900 Subject: [PATCH 098/154] feat(gnss_poser): use header.frame_id as gnss_frame (#6116) * remove gnss_frame param & use header.frame_id to get anntenna's frame_id Signed-off-by: Kento Yabuuchi * update README Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix --------- Signed-off-by: Kento Yabuuchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/gnss_poser/README.md | 5 +++++ sensing/gnss_poser/config/gnss_poser.param.yaml | 1 - sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp | 7 +++---- sensing/gnss_poser/schema/gnss_poser.schema.json | 5 ----- sensing/gnss_poser/src/gnss_poser_core.cpp | 4 ++-- 5 files changed, 10 insertions(+), 12 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 1ec7dc1f516ad..6799a1f79ae89 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -4,6 +4,11 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates vehicle pose with covariance. +This node subscribes to NavSatFix to publish the pose of **base_link**. The data in NavSatFix represents the antenna's position. Therefore, it performs a coordinate transformation using the tf from `base_link` to the antenna's position. The frame_id of the antenna's position refers to NavSatFix's `header.frame_id`. +(**Note that `header.frame_id` in NavSatFix indicates the antenna's frame_id, not the Earth or reference ellipsoid.** [See also NavSatFix definition.](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatFix.html)) + +If the transformation from `base_link` to the antenna cannot be obtained, it outputs the pose of the antenna position without performing coordinate transformation. + ## Inner-workings / Algorithms ## Inputs / Outputs diff --git a/sensing/gnss_poser/config/gnss_poser.param.yaml b/sensing/gnss_poser/config/gnss_poser.param.yaml index 30be98bba8cf1..1c00b43c2066a 100644 --- a/sensing/gnss_poser/config/gnss_poser.param.yaml +++ b/sensing/gnss_poser/config/gnss_poser.param.yaml @@ -2,7 +2,6 @@ ros__parameters: base_frame: base_link gnss_base_frame: gnss_base_link - gnss_frame: gnss map_frame: map buff_epoch: 1 use_gnss_ins_orientation: true diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 2118d33bc4b30..97ca1d8bffe77 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -89,10 +89,9 @@ class GNSSPoser : public rclcpp::Node rclcpp::Publisher::SharedPtr fixed_pub_; MapProjectorInfo::Message projector_info_; - std::string base_frame_; - std::string gnss_frame_; - std::string gnss_base_frame_; - std::string map_frame_; + const std::string base_frame_; + const std::string gnss_base_frame_; + const std::string map_frame_; bool received_map_projector_info_ = false; bool use_gnss_ins_orientation_; diff --git a/sensing/gnss_poser/schema/gnss_poser.schema.json b/sensing/gnss_poser/schema/gnss_poser.schema.json index 1d63bb2f4850e..6d9bc716e121a 100644 --- a/sensing/gnss_poser/schema/gnss_poser.schema.json +++ b/sensing/gnss_poser/schema/gnss_poser.schema.json @@ -11,11 +11,6 @@ "default": "base_link", "description": "frame id for base_frame" }, - "gnss_frame": { - "type": "string", - "default": "gnss", - "description": "frame id for gnss_frame" - }, "gnss_base_frame": { "type": "string", "default": "gnss_base_link", diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 3a18dca815f12..ec58226273441 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -31,7 +31,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) tf2_listener_(tf2_buffer_), tf2_broadcaster_(*this), base_frame_(declare_parameter("base_frame")), - gnss_frame_(declare_parameter("gnss_frame")), gnss_base_frame_(declare_parameter("gnss_base_frame")), map_frame_(declare_parameter("map_frame")), use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), @@ -142,8 +141,9 @@ void GNSSPoser::callbackNavSatFix( // get TF from gnss_antenna to base_link auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); + const std::string gnss_frame = nav_sat_fix_msg_ptr->header.frame_id; getStaticTransform( - base_frame_, gnss_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); + base_frame_, gnss_frame, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); From d04bb57d621dfd8395013968b3d6ec95cc59e4ec Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 22 Jan 2024 18:27:36 +0900 Subject: [PATCH 099/154] fix(external_velocity_limit_selector): revert "refactor(external_velocity_limit_selector): rework parameter (#5238)" (#6132) Revert "refactor(external_velocity_limit_selector): rework parameter (#5238)" This reverts commit 19dab56af8fffde3b5778cf2c8ab6d20a4237f7d. --- .../README.md | 12 ++- .../config/default.param.yaml | 4 + .../config/default_common.param.yaml | 15 ++++ ...xternal_velocity_limit_selector.param.yaml | 18 ---- ...xternal_velocity_limit_selector.launch.xml | 6 +- ...ternal_velocity_limit_selector.schema.json | 85 ------------------- 6 files changed, 34 insertions(+), 106 deletions(-) create mode 100644 planning/external_velocity_limit_selector/config/default.param.yaml create mode 100644 planning/external_velocity_limit_selector/config/default_common.param.yaml delete mode 100644 planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml delete mode 100644 planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json diff --git a/planning/external_velocity_limit_selector/README.md b/planning/external_velocity_limit_selector/README.md index 2dc76dd3aec6e..92579bfd0abce 100644 --- a/planning/external_velocity_limit_selector/README.md +++ b/planning/external_velocity_limit_selector/README.md @@ -51,7 +51,17 @@ Example: ## Parameters -{{ json_to_markdown("planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json") }} +| Parameter | Type | Description | +| ----------------- | ------ | ------------------------------------------ | +| `max_velocity` | double | default max velocity [m/s] | +| `normal.min_acc` | double | minimum acceleration [m/ss] | +| `normal.max_acc` | double | maximum acceleration [m/ss] | +| `normal.min_jerk` | double | minimum jerk [m/sss] | +| `normal.max_jerk` | double | maximum jerk [m/sss] | +| `limit.min_acc` | double | minimum acceleration to be observed [m/ss] | +| `limit.max_acc` | double | maximum acceleration to be observed [m/ss] | +| `limit.min_jerk` | double | minimum jerk to be observed [m/sss] | +| `limit.max_jerk` | double | maximum jerk to be observed [m/sss] | ## Assumptions / Known limits diff --git a/planning/external_velocity_limit_selector/config/default.param.yaml b/planning/external_velocity_limit_selector/config/default.param.yaml new file mode 100644 index 0000000000000..8023776e191ba --- /dev/null +++ b/planning/external_velocity_limit_selector/config/default.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + # motion state constraints + max_velocity: 20.0 # max velocity limit [m/s] diff --git a/planning/external_velocity_limit_selector/config/default_common.param.yaml b/planning/external_velocity_limit_selector/config/default_common.param.yaml new file mode 100644 index 0000000000000..a23570a5fc791 --- /dev/null +++ b/planning/external_velocity_limit_selector/config/default_common.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # constraints param for normal driving + normal: + min_acc: -0.5 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -0.5 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml b/planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml deleted file mode 100644 index 472b9370607b5..0000000000000 --- a/planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml +++ /dev/null @@ -1,18 +0,0 @@ -/**: - ros__parameters: - # constraints param for normal driving - normal: - min_acc: -0.5 # min deceleration [m/ss] - max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -0.5 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] - - # constraints to be observed - limit: - min_acc: -2.5 # min deceleration limit [m/ss] - max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -1.5 # min jerk limit [m/sss] - max_jerk: 1.5 # max jerk limit [m/sss] - - # motion state constraints - max_velocity: 20.0 # max velocity limit [m/s] diff --git a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml index c499c0741f09f..5ef089f3d3ee7 100644 --- a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml +++ b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml @@ -1,6 +1,7 @@ - + + @@ -10,7 +11,8 @@ - + + diff --git a/planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json b/planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json deleted file mode 100644 index 36fdaf31cbc6f..0000000000000 --- a/planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json +++ /dev/null @@ -1,85 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for external velocity limit selector node", - "type": "object", - "definitions": { - "external_velocity_limit_selector": { - "type": "object", - "properties": { - "max_velocity": { - "type": "number", - "description": "max velocity limit [m/s]", - "default": 20.0 - }, - "normal": { - "type": "object", - "properties": { - "min_acc": { - "type": "number", - "description": "min deceleration [m/ss]", - "default": -0.5 - }, - "max_acc": { - "type": "number", - "description": "max acceleration [m/ss]", - "default": 1.0 - }, - "min_jerk": { - "type": "number", - "description": "min jerk [m/sss]", - "default": -0.5 - }, - "max_jerk": { - "type": "number", - "description": "max jerk [m/sss]", - "default": 1.0 - } - }, - "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] - }, - "limit": { - "type": "object", - "properties": { - "min_acc": { - "type": "number", - "description": "min deceleration to be observed [m/ss]", - "default": -2.5 - }, - "max_acc": { - "type": "number", - "description": "max acceleration to be observed [m/ss]", - "default": 1.0 - }, - "min_jerk": { - "type": "number", - "description": "min jerk to be observed [m/sss]", - "default": -1.5 - }, - "max_jerk": { - "type": "number", - "description": "max jerk to be observed [m/sss]", - "default": 1.5 - } - }, - "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] - } - }, - "required": ["max_velocity", "normal", "limit"], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/external_velocity_limit_selector" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false -} From b5203a22d122cd648491319af05765bcc7982915 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 22 Jan 2024 20:22:13 +0900 Subject: [PATCH 100/154] feat(simple_planning_simulator): add option to use initialpose for z position (#4256) * feat(simple_planning_simulator): add option to use initialpose for z position Signed-off-by: Takamasa Horibe * Revert "feat(simple_planning_simulator): add option to use initialpose for z position" This reverts commit a3e2779cd38841ba49e063c42fc3a2366c176ad6. * update initial z logic Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --- .../simple_planning_simulator_core.hpp | 3 ++- .../simple_planning_simulator_core.cpp | 11 +++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index d096130e65f05..4c3a3aed24d90 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -280,9 +280,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node * @brief get z-position from trajectory * @param [in] x current x-position * @param [in] y current y-position + * @param [in] prev_odometry odometry calculated in the previous step * @return get z-position from trajectory */ - double get_z_pose_from_trajectory(const double x, const double y); + double get_z_pose_from_trajectory(const double x, const double y, const Odometry & prev_odometry); /** * @brief get transform from two frame_ids diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 8de5be9d71503..37ec5b33014a7 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -371,9 +371,10 @@ void SimplePlanningSimulator::on_timer() } // set current state + const auto prev_odometry = current_odometry_; current_odometry_ = to_odometry(vehicle_model_ptr_, ego_pitch_angle); current_odometry_.pose.pose.position.z = get_z_pose_from_trajectory( - current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y); + current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y, prev_odometry); current_velocity_ = to_velocity_report(vehicle_model_ptr_); current_steer_ = to_steering_report(vehicle_model_ptr_); @@ -429,6 +430,7 @@ void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::Co set_initial_state_with_transform(initial_pose, initial_twist); initial_pose_ = msg; + current_odometry_.pose = msg->pose; } void SimplePlanningSimulator::on_initialtwist(const TwistStamped::ConstSharedPtr msg) @@ -591,11 +593,12 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & is_initialized_ = true; } -double SimplePlanningSimulator::get_z_pose_from_trajectory(const double x, const double y) +double SimplePlanningSimulator::get_z_pose_from_trajectory( + const double x, const double y, const Odometry & prev_odometry) { // calculate closest point on trajectory if (!current_trajectory_ptr_) { - return 0.0; + return prev_odometry.pose.pose.position.z; } const double max_sqrt_dist = std::numeric_limits::max(); @@ -616,7 +619,7 @@ double SimplePlanningSimulator::get_z_pose_from_trajectory(const double x, const return current_trajectory_ptr_->points.at(index).pose.position.z; } - return 0.0; + return prev_odometry.pose.pose.position.z; } TransformStamped SimplePlanningSimulator::get_transform_msg( From c2daf19043ada0caba673a510b4d78bf6c1cdaba Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 22 Jan 2024 11:56:55 +0000 Subject: [PATCH 101/154] chore: update CODEOWNERS (#6135) Signed-off-by: GitHub Co-authored-by: github-actions --- .github/CODEOWNERS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 89bcbce9e5656..6b4daaa4b1773 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -175,7 +175,7 @@ planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp From 0a5427d64a932cd0a3f1f74cfd9c758d69839206 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 22 Jan 2024 22:03:31 +0900 Subject: [PATCH 102/154] fix(dynamic_avoidance): remove cerr (#6137) Signed-off-by: satoshi-ota --- planning/behavior_path_dynamic_avoidance_module/src/scene.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index 9388a0a354d50..b43102ecbd16d 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -1221,7 +1221,6 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi prev_object->ref_path_points_for_obj_poly, ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); - std::cerr << paths_lat_diff << std::endl; constexpr double min_paths_lat_diff = 0.3; if (paths_lat_diff < min_paths_lat_diff) { return true; From 3019aab3b5b466880e57d4e5f7c3e90b27699927 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 23 Jan 2024 03:02:28 +0900 Subject: [PATCH 103/154] refactor(intersection): divide source files and modifyPathVelocity (#6134) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 9 +- .../src/decision_result.hpp | 203 ++ ...il_type.hpp => interpolated_path_info.hpp} | 20 +- .../src/intersection_lanelets.cpp | 82 + .../src/intersection_lanelets.hpp | 197 ++ .../src/intersection_stoplines.hpp | 78 + .../src/manager.cpp | 5 +- .../src/manager.hpp | 1 - .../src/result.hpp | 44 + .../src/scene_intersection.cpp | 2995 +++-------------- .../src/scene_intersection.hpp | 884 +++-- .../src/scene_intersection_collision.cpp | 582 ++++ .../src/scene_intersection_occlusion.cpp | 407 +++ .../src/scene_intersection_prepare_data.cpp | 869 +++++ .../src/scene_intersection_stuck.cpp | 380 +++ .../src/scene_merge_from_private_road.cpp | 5 +- .../src/util.cpp | 58 +- .../src/util.hpp | 37 +- 18 files changed, 3792 insertions(+), 3064 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/src/decision_result.hpp rename planning/behavior_velocity_intersection_module/src/{util_type.hpp => interpolated_path_info.hpp} (71%) create mode 100644 planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp create mode 100644 planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp create mode 100644 planning/behavior_velocity_intersection_module/src/result.hpp create mode 100644 planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 9e7eb196cd0c1..4c8fe5c6fa0f5 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -8,11 +8,16 @@ pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) find_package(OpenCV REQUIRED) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp src/manager.cpp + src/util.cpp src/scene_intersection.cpp + src/intersection_lanelets.cpp + src/scene_intersection_prepare_data.cpp + src/scene_intersection_stuck.cpp + src/scene_intersection_occlusion.cpp + src/scene_intersection_collision.cpp src/scene_merge_from_private_road.cpp - src/util.cpp + src/debug.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp new file mode 100644 index 0000000000000..48b0ecf1349a5 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -0,0 +1,203 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 DECISION_RESULT_HPP_ +#define DECISION_RESULT_HPP_ + +#include +#include +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief Internal error or ego already passed pass_judge_line + */ +struct Indecisive +{ + std::string error; +}; + +/** + * @struct + * @brief detected stuck vehicle + */ +struct StuckStop +{ + size_t closest_idx{0}; + size_t stuck_stopline_idx{0}; + std::optional occlusion_stopline_idx{std::nullopt}; +}; + +/** + * @struct + * @brief yielded by vehicle on the attention area + */ +struct YieldStuckStop +{ + size_t closest_idx{0}; + size_t stuck_stopline_idx{0}; +}; + +/** + * @struct + * @brief only collision is detected + */ +struct NonOccludedCollisionStop +{ + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +/** + * @struct + * @brief occlusion is detected so ego needs to stop at the default stop line position + */ +struct FirstWaitBeforeOcclusion +{ + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t first_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +/** + * @struct + * @brief ego is approaching the boundary of attention area in the presence of traffic light + */ +struct PeekingTowardOcclusion +{ + //! if intersection_occlusion is disapproved externally through RTC, it indicates + //! "is_forcefully_occluded" + bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t first_attention_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck and shows up + //! intersection_occlusion(x.y) + std::optional static_occlusion_timeout{std::nullopt}; +}; + +/** + * @struct + * @brief both collision and occlusion are detected in the presence of traffic light + */ +struct OccludedCollisionStop +{ + bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t first_attention_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck + std::optional static_occlusion_timeout{std::nullopt}; +}; + +/** + * @struct + * @brief at least occlusion is detected in the absence of traffic light + */ +struct OccludedAbsenceTrafficLight +{ + bool is_actually_occlusion_cleared{false}; + bool collision_detected{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t first_attention_area_stopline_idx{0}; + size_t peeking_limit_line_idx{0}; +}; + +/** + * @struct + * @brief both collision and occlusion are not detected + */ +struct Safe +{ + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +/** + * @struct + * @brief traffic light is red or arrow signal + */ +struct FullyPrioritized +{ + bool collision_detected{false}; + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +using DecisionResult = std::variant< + Indecisive, //! internal process error, or over the pass judge line + StuckStop, //! detected stuck vehicle + YieldStuckStop, //! detected yield stuck vehicle + NonOccludedCollisionStop, //! detected collision while FOV is clear + FirstWaitBeforeOcclusion, //! stop for a while before peeking to occlusion + PeekingTowardOcclusion, //! peeking into occlusion while collision is not detected + OccludedCollisionStop, //! occlusion and collision are both detected + OccludedAbsenceTrafficLight, //! occlusion is detected in the absence of traffic light + Safe, //! judge as safe + FullyPrioritized //! only detect vehicles violating traffic rules + >; + +inline std::string formatDecisionResult(const DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + const auto indecisive = std::get(decision_result); + return "Indecisive because " + indecisive.error; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + return "YieldStuckStop"; + } + if (std::holds_alternative(decision_result)) { + return "NonOccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "OccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "OccludedAbsenceTrafficLight"; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + return "FullyPrioritized"; + } + return ""; +} + +} // namespace behavior_velocity_planner::intersection + +#endif // DECISION_RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp similarity index 71% rename from planning/behavior_velocity_intersection_module/src/util_type.hpp rename to planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index 763d829dacf9b..c47f016fbdbda 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -12,26 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTIL_TYPE_HPP_ -#define UTIL_TYPE_HPP_ +#ifndef INTERPOLATED_PATH_INFO_HPP_ +#define INTERPOLATED_PATH_INFO_HPP_ -#include - -#include #include -#include -#include -#include -#include -#include +#include #include #include #include -#include -namespace behavior_velocity_planner::util +namespace behavior_velocity_planner::intersection { /** @@ -52,6 +44,6 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; -} // namespace behavior_velocity_planner::util +} // namespace behavior_velocity_planner::intersection -#endif // UTIL_TYPE_HPP_ +#endif // INTERPOLATED_PATH_INFO_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp new file mode 100644 index 0000000000000..555ea424dfef0 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -0,0 +1,82 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 "intersection_lanelets.hpp" + +#include "util.hpp" + +#include +#include + +#include + +namespace behavior_velocity_planner::intersection +{ + +void IntersectionLanelets::update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr) +{ + is_prioritized_ = is_prioritized; + // find the first conflicting/detection area polygon intersecting the path + if (!first_conflicting_area_) { + auto first = util::getFirstPointInsidePolygonsByFootprint( + conflicting_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); + } + } + if (!first_attention_area_) { + const auto first = util::getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); + } + } + if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { + const auto first_attention_lane = first_attention_lane_.value(); + // remove first_attention_area_ and non-straight lanelets from attention_non_preceding + lanelet::ConstLanelets attention_non_preceding_ex_first; + lanelet::ConstLanelets sibling_first_attention_lanelets; + for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { + for (const auto & following : routing_graph_ptr->following(previous)) { + sibling_first_attention_lanelets.push_back(following); + } + } + for (const auto & ll : attention_non_preceding_) { + // the sibling lanelets of first_attention_lanelet are ruled out + if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { + continue; + } + if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { + attention_non_preceding_ex_first.push_back(ll); + } + } + if (attention_non_preceding_ex_first.empty()) { + second_attention_lane_empty_ = true; + } + const auto attention_non_preceding_ex_first_area = + util::getPolygon3dFromLanelets(attention_non_preceding_ex_first); + const auto second = util::getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); + if (second) { + second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); + second_attention_area_ = second_attention_lane_.value().polygon3d(); + } + } +} +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp new file mode 100644 index 0000000000000..11deae6bdc001 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -0,0 +1,197 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 INTERSECTION_LANELETS_HPP_ +#define INTERSECTION_LANELETS_HPP_ + +#include "interpolated_path_info.hpp" + +#include + +#include +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief see the document for more details of IntersectionLanelets + */ +struct IntersectionLanelets +{ +public: + /** + * update conflicting lanelets and traffic priority information + */ + void update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr); + + const lanelet::ConstLanelets & attention() const + { + return is_prioritized_ ? attention_non_preceding_ : attention_; + } + const std::vector> & attention_stoplines() const + { + return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; + } + const lanelet::ConstLanelets & conflicting() const { return conflicting_; } + const lanelet::ConstLanelets & adjacent() const { return adjacent_; } + const lanelet::ConstLanelets & occlusion_attention() const + { + return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; + } + const lanelet::ConstLanelets & attention_non_preceding() const + { + return attention_non_preceding_; + } + const std::vector & attention_area() const + { + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; + } + const std::vector & conflicting_area() const + { + return conflicting_area_; + } + const std::vector & adjacent_area() const { return adjacent_area_; } + const std::vector & occlusion_attention_area() const + { + return occlusion_attention_area_; + } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } + const std::optional & first_conflicting_area() const + { + return first_conflicting_area_; + } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } + const std::optional & first_attention_area() const + { + return first_attention_area_; + } + const std::optional & second_attention_lane() const + { + return second_attention_lane_; + } + const std::optional & second_attention_area() const + { + return second_attention_area_; + } + + /** + * the set of attention lanelets which is topologically merged + */ + lanelet::ConstLanelets attention_; + std::vector attention_area_; + + /** + * the stop lines for each attention_lanelets associated with traffic lights. At intersection + * without traffic lights, each value is null + */ + std::vector> attention_stoplines_; + + /** + * the conflicting part of attention lanelets + */ + lanelet::ConstLanelets attention_non_preceding_; + std::vector attention_non_preceding_area_; + + /** + * the stop lines for each attention_non_preceding_ + */ + std::vector> attention_non_preceding_stoplines_; + + /** + * the conflicting lanelets of the objective intersection lanelet + */ + lanelet::ConstLanelets conflicting_; + std::vector conflicting_area_; + + /** + * + */ + lanelet::ConstLanelets adjacent_; + std::vector adjacent_area_; + + /** + * the set of attention lanelets for occlusion detection which is topologically merged + */ + lanelet::ConstLanelets occlusion_attention_; + std::vector occlusion_attention_area_; + + /** + * the vector of sum of each occlusion_attention lanelet + */ + std::vector occlusion_attention_size_; + + /** + * the first conflicting lanelet which ego path points intersect for the first time + */ + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + + /** + * the first attention lanelet which ego path points intersect for the first time + */ + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; + + /** + * the second attention lanelet which ego path points intersect next to the + * first_attention_lanelet + */ + bool second_attention_lane_empty_{false}; + std::optional second_attention_lane_{std::nullopt}; + std::optional second_attention_area_{std::nullopt}; + + /** + * flag if the intersection is prioritized by the traffic light + */ + bool is_prioritized_{false}; +}; + +/** + * @struct + * @brief see the document for more details of PathLanelets + */ +struct PathLanelets +{ + lanelet::ConstLanelets prev; + // lanelet::ConstLanelet entry2ego; this is included in `all` if exists + lanelet::ConstLanelet + ego_or_entry2exit; // this is `assigned lane` part of the path(not from + // ego) if ego is before the intersection, otherwise from ego to exit + std::optional next = + std::nullopt; // this is nullopt is the goal is inside intersection + lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the + // path +}; +} // namespace behavior_velocity_planner::intersection + +#endif // INTERSECTION_LANELETS_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp new file mode 100644 index 0000000000000..64d20b81e3fad --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -0,0 +1,78 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 INTERSECTION_STOPLINES_HPP_ +#define INTERSECTION_STOPLINES_HPP_ + +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct IntersectionStopLines +{ + size_t closest_idx{0}; + + /** + * stuck_stopline is null if ego path does not intersect with first_conflicting_area + */ + std::optional stuck_stopline{std::nullopt}; + + /** + * default_stopline is null if it is calculated negative from first_attention_stopline + */ + std::optional default_stopline{std::nullopt}; + + /** + * first_attention_stopline is null if ego footprint along the path does not intersect with + * attention area. if path[0] satisfies the condition, it is 0 + */ + std::optional first_attention_stopline{std::nullopt}; + + /** + * second_attention_stopline is null if ego footprint along the path does not intersect with + * second_attention_lane. if path[0] satisfies the condition, it is 0 + */ + std::optional second_attention_stopline{std::nullopt}; + + /** + * occlusion_peeking_stopline is null if path[0] is already inside the attention area + */ + std::optional occlusion_peeking_stopline{std::nullopt}; + + /** + * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value + * is calculated negative, it is 0 + */ + size_t first_pass_judge_line{0}; + + /** + * second_pass_judge_line is before second_attention_stopline by the braking distance. if + * second_attention_lane is null, it is same as first_pass_judge_line + */ + size_t second_pass_judge_line{0}; + + /** + * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with + * the centerline of the first_attention_lane + */ + size_t occlusion_wo_tl_pass_judge_line{0}; +}; +} // namespace behavior_velocity_planner::intersection + +#endif // INTERSECTION_STOPLINES_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index d2311d1c0c992..615991bc5e027 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -180,8 +180,6 @@ void IntersectionModuleManager::launchNewModules( const auto lanelets = planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); // run occlusion detection only in the first intersection - // TODO(Mamoru Sobue): remove `enable_occlusion_detection` variable - const bool enable_occlusion_detection = intersection_param_.occlusion.enable; for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -211,8 +209,7 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, enable_occlusion_detection, node_, - logger_.get_child("intersection_module"), clock_); + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index ff9302db0b6af..88af4412e34eb 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -44,7 +44,6 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC IntersectionModule::PlannerParam intersection_param_; // additional for INTERSECTION_OCCLUSION RTCInterface occlusion_rtc_interface_; - std::unordered_map occlusion_map_uuid_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/behavior_velocity_intersection_module/src/result.hpp new file mode 100644 index 0000000000000..cc6ad880b8153 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/result.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 RESULT_HPP_ +#define RESULT_HPP_ + +#include + +namespace behavior_velocity_planner::intersection +{ + +template +class Result +{ +public: + static Result make_ok(const Ok & ok) { return Result(ok); } + static Result make_err(const Error & err) { return Result(err); } + +public: + explicit Result(const Ok & ok) : data_(ok) {} + explicit Result(const Error & err) : data_(err) {} + explicit operator bool() const noexcept { return std::holds_alternative(data_); } + bool operator!() const noexcept { return !static_cast(*this); } + const Ok & ok() const { return std::get(data_); } + const Error & err() const { return std::get(data_); } + +private: + std::variant data_; +}; + +} // namespace behavior_velocity_planner::intersection + +#endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 358dca2414bb0..90242dc3edd7e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -16,137 +16,42 @@ #include "util.hpp" -#include -#include +#include // for toGeomPoly #include -#include -#include -#include #include #include -#include -#include +#include // for toPolygon2d #include #include -#include -#include -#include +#include -#include -#include #include #include -#include +#include +#include #include #include -#include -#include #include -#include #include -namespace tier4_autoware_utils -{ - -template <> -inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) -{ - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - point.z = p.z(); - return point; -} - -} // namespace tier4_autoware_utils - namespace behavior_velocity_planner { namespace bg = boost::geometry; -namespace -{ -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) -{ - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); - - Polygon2d one_step_poly; - for (const auto & point : prev_poly.outer()) { - one_step_poly.outer().push_back(point); - } - for (const auto & point : next_poly.outer()) { - one_step_poly.outer().push_back(point); - } - - bg::correct(one_step_poly); - - Polygon2d convex_one_step_poly; - bg::convex_hull(one_step_poly, convex_one_step_poly); - - return convex_one_step_poly; -} -} // namespace - -static bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - if ( - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -static bool isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - if ( - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) { - return true; - } - return false; -} - IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) + const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), associative_ids_(associative_ids), turn_direction_(turn_direction), has_traffic_light_(has_traffic_light), - enable_occlusion_detection_(enable_occlusion_detection), - occlusion_attention_divisions_(std::nullopt), occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -185,6 +90,32 @@ IntersectionModule::IntersectionModule( "~/debug/intersection/object_ttc", 1); } +bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_ = DebugData(); + *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); + + // set default RTC + initializeRTCStatus(); + + // calculate the + const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + prev_decision_result_ = decision_result; + + const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + + intersection::formatDecisionResult(decision_result); + std_msgs::msg::String decision_result_msg; + decision_result_msg.data = decision_type; + decision_state_pub_->publish(decision_result_msg); + + prepareRTCStatus(decision_result, *path); + + reactRTCApproval(decision_result, path, stop_reason); + + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return true; +} + void IntersectionModule::initializeRTCStatus() { setSafe(true); @@ -196,6 +127,255 @@ void IntersectionModule::initializeRTCStatus() // activated_ and occlusion_activated_ must be set from manager's RTC callback } +intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( + PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); + const bool is_prioritized = + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + + const auto prepare_data = prepareIntersectionData(is_prioritized, path); + if (!prepare_data) { + return prepare_data.err(); + } + const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); + const auto & intersection_lanelets = intersection_lanelets_.value(); + + const auto closest_idx = intersection_stoplines.closest_idx; + + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; + + // stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation + const auto is_stuck_status = isStuckStatus(*path, intersection_stoplines, path_lanelets); + if (is_stuck_status) { + return is_stuck_status.value(); + } + + // if attention area is empty, collision/occlusion detection is impossible + if (!intersection_lanelets.first_attention_area()) { + return intersection::Indecisive{"attention area is empty"}; + } + const auto first_attention_area = intersection_lanelets.first_attention_area().value(); + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line + const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + if (!default_stopline_idx_opt) { + return intersection::Indecisive{"default stop line is null"}; + } + const auto default_stopline_idx = default_stopline_idx_opt.value(); + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, eog has already passed the intersection + const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; + const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; + if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { + return intersection::Indecisive{"occlusion stop line is null"}; + } + const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); + const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); + + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); + debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + + // get intersection area + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + // filter objects + auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); + const auto is_yield_stuck_status = + isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines, target_objects); + if (is_yield_stuck_status) { + return is_yield_stuck_status.value(); + } + + const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = + getOcclusionStatus( + traffic_prioritized_level, interpolated_path_info, intersection_lanelets, target_objects); + + // TODO(Mamoru Sobue): this should be called later for safety diagnosis + const auto is_over_pass_judge_lines_status = + isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); + if (is_over_pass_judge_lines_status) { + return is_over_pass_judge_lines_status.ok(); + } + [[maybe_unused]] const auto [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line] = + is_over_pass_judge_lines_status.err(); + + const auto & current_pose = planner_data_->current_odometry->pose; + const bool is_over_default_stopline = + util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); + const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; + + const auto is_green_pseudo_collision_status = isGreenPseudoCollisionStatus( + *path, collision_stopline_idx, intersection_stoplines, target_objects); + if (is_green_pseudo_collision_status) { + return is_green_pseudo_collision_status.value(); + } + + // if ego is waiting for collision detection, the entry time into the intersection is a bit + // delayed for the chattering hold + const bool is_go_out = (activated_ && occlusion_activated_); + const double time_to_restart = + (is_go_out || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.collision_detection_hold_time - + collision_state_machine_.getDuration()); + + // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd + // pass judge line respectively, ego should go + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = calcIntersectionPassingTime( + *path, closest_idx, last_intersection_stopline_candidate_idx, time_to_restart, + &ego_ttc_time_array); + + const bool has_collision = checkCollision( + *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, + time_to_restart, traffic_prioritized_level); + collision_state_machine_.setStateWithMarginTime( + has_collision ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("collision state_machine"), *clock_); + const bool has_collision_with_margin = + collision_state_machine_.getState() == StateMachine::State::STOP; + + if (is_prioritized) { + return intersection::FullyPrioritized{ + has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + + // Safe + if (!is_occlusion_state && !has_collision_with_margin) { + return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + // Only collision + if (!is_occlusion_state && has_collision_with_margin) { + return intersection::NonOccludedCollisionStop{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + // Occluded + // occlusion_status is assured to be not NOT_OCCLUDED + const auto occlusion_wo_tl_pass_judge_line_idx = + intersection_stoplines.occlusion_wo_tl_pass_judge_line; + const bool stopped_at_default_line = stoppedForDuration( + default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, + before_creep_state_machine_); + if (stopped_at_default_line) { + // if specified the parameter occlusion.temporal_stop_before_attention_area OR + // has_no_traffic_light_, ego will temporarily stop before entering attention area + const bool temporal_stop_before_attention_required = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? !stoppedForDuration( + first_attention_stopline_idx, + planner_param_.occlusion.temporal_stop_time_before_peeking, + temporal_stop_before_attention_state_machine_) + : false; + if (!has_traffic_light_) { + if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { + return intersection::Indecisive{ + "already passed maximum peeking line in the absence of traffic light"}; + } + return intersection::OccludedAbsenceTrafficLight{ + is_occlusion_cleared_with_margin, + has_collision_with_margin, + temporal_stop_before_attention_required, + closest_idx, + first_attention_stopline_idx, + occlusion_wo_tl_pass_judge_line_idx}; + } + // following remaining block is "has_traffic_light_" + // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; + const bool is_stuck_by_static_occlusion = + stoppedAtPosition( + occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) && + is_static_occlusion; + if (has_collision_with_margin) { + // if collision is detected, timeout is reset + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } else if (is_stuck_by_static_occlusion) { + static_occlusion_timeout_state_machine_.setStateWithMarginTime( + StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); + } + const bool release_static_occlusion_stuck = + (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); + if (!has_collision_with_margin && release_static_occlusion_stuck) { + return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED + const double max_timeout = + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + + planner_param_.occlusion.occlusion_detection_hold_time; + const std::optional static_occlusion_timeout = + is_stuck_by_static_occlusion + ? std::make_optional( + max_timeout - static_occlusion_timeout_state_machine_.getDuration() - + occlusion_stop_state_machine_.getDuration()) + : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); + if (has_collision_with_margin) { + return intersection::OccludedCollisionStop{ + is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stopline_idx, + first_attention_stopline_idx, + occlusion_stopline_idx, + static_occlusion_timeout}; + } else { + return intersection::PeekingTowardOcclusion{ + is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stopline_idx, + first_attention_stopline_idx, + occlusion_stopline_idx, + static_occlusion_timeout}; + } + } else { + const auto occlusion_stopline = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? first_attention_stopline_idx + : occlusion_stopline_idx; + return intersection::FirstWaitBeforeOcclusion{ + is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline}; + } +} + // template-specification based visitor pattern // https://en.cppreference.com/w/cpp/utility/variant/visit template @@ -218,7 +398,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const IntersectionModule::Indecisive & result, + [[maybe_unused]] const intersection::Indecisive & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -228,7 +408,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::StuckStop & result, + const intersection::StuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -248,7 +428,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::YieldStuckStop & result, + const intersection::YieldStuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -263,7 +443,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::NonOccludedCollisionStop & result, + const intersection::NonOccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -282,7 +462,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::FirstWaitBeforeOcclusion & result, + const intersection::FirstWaitBeforeOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -301,7 +481,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::PeekingTowardOcclusion & result, + const intersection::PeekingTowardOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -320,7 +500,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::OccludedAbsenceTrafficLight & result, + const intersection::OccludedAbsenceTrafficLight & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -337,7 +517,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::OccludedCollisionStop & result, + const intersection::OccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -356,9 +536,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::Safe & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const intersection::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); const auto closest_idx = result.closest_idx; @@ -375,7 +555,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::FullyPrioritized & result, + const intersection::FullyPrioritized & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -393,7 +573,7 @@ void prepareRTCByDecisionResult( } void IntersectionModule::prepareRTCStatus( - const DecisionResult & decision_result, + const intersection::DecisionResult & decision_result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; @@ -408,7 +588,7 @@ void IntersectionModule::prepareRTCStatus( setSafe(default_safety); setDistance(default_distance); occlusion_first_stop_required_ = - std::holds_alternative(decision_result); + std::holds_alternative(decision_result); } template @@ -416,7 +596,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); return; @@ -426,13 +606,13 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const IntersectionModule::Indecisive & decision_result, + [[maybe_unused]] const intersection::Indecisive & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, - [[maybe_unused]] DebugData * debug_data) + [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; } @@ -440,10 +620,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::StuckStop & decision_result, + const intersection::StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -488,10 +669,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::YieldStuckStop & decision_result, + const intersection::YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -520,10 +702,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::NonOccludedCollisionStop & decision_result, + const intersection::NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -563,10 +746,10 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::FirstWaitBeforeOcclusion & decision_result, + const intersection::FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -614,10 +797,10 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::PeekingTowardOcclusion & decision_result, + const intersection::PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -671,10 +854,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::OccludedCollisionStop & decision_result, + const intersection::OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -718,10 +902,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::OccludedAbsenceTrafficLight & decision_result, + const intersection::OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -771,10 +956,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::Safe & decision_result, + const intersection::Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -813,10 +999,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::FullyPrioritized & decision_result, + const intersection::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -853,96 +1040,26 @@ void reactRTCApprovalByDecisionResult( return; } -void reactRTCApproval( - const bool rtc_default_approval, const bool rtc_occlusion_approval, - const IntersectionModule::DecisionResult & decision_result, - const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) +void IntersectionModule::reactRTCApproval( + const intersection::DecisionResult & decision_result, + autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( - rtc_default_approval, rtc_occlusion_approval, decision, planner_param, baselink2front, path, - stop_reason, velocity_factor, debug_data); + activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, + stop_reason, &velocity_factor_, &debug_data_); }}, decision_result); return; } -static std::string formatDecisionResult(const IntersectionModule::DecisionResult & decision_result) -{ - if (std::holds_alternative(decision_result)) { - const auto indecisive = std::get(decision_result); - return "Indecisive because " + indecisive.error; - } - if (std::holds_alternative(decision_result)) { - return "Safe"; - } - if (std::holds_alternative(decision_result)) { - return "StuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "YieldStuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "NonOccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "FirstWaitBeforeOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "PeekingTowardOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedAbsenceTrafficLight"; - } - if (std::holds_alternative(decision_result)) { - return "FullyPrioritized"; - } - return ""; -} - -bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) -{ - debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); - - // set default RTC - initializeRTCStatus(); - - // calculate the - const auto decision_result = modifyPathVelocityDetail(path, stop_reason); - prev_decision_result_ = decision_result; - - const std::string decision_type = - "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); - std_msgs::msg::String decision_result_msg; - decision_result_msg.data = decision_type; - decision_state_pub_->publish(decision_result_msg); - - prepareRTCStatus(decision_result, *path); - - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - reactRTCApproval( - activated_, occlusion_activated_, decision_result, planner_param_, baselink2front, path, - stop_reason, &velocity_factor_, &debug_data_); - - if (!activated_ || !occlusion_activated_) { - is_go_out_ = false; - } else { - is_go_out_ = true; - } - RCLCPP_DEBUG(logger_, "===== plan end ====="); - return true; -} - -bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) +bool IntersectionModule::isGreenSolidOn() const { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { @@ -970,260 +1087,134 @@ bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) return false; } -IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel() const { - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); - const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - const auto & current_pose = planner_data_->current_odometry->pose; - - // spline interpolation - const auto interpolated_path_info_opt = util::generateInterpolatedPath( - lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); - if (!interpolated_path_info_opt) { - return IntersectionModule::Indecisive{"splineInterpolate failed"}; - } - const auto & interpolated_path_info = interpolated_path_info_opt.value(); - if (!interpolated_path_info.lane_id_interval) { - return IntersectionModule::Indecisive{ - "Path has no interval on intersection lane " + std::to_string(lane_id_)}; - } + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - // cache intersection lane information because it is invariant - const auto lanelets_on_path = - planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); - if (!intersection_lanelets_) { - intersection_lanelets_ = - getObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path); - } - auto & intersection_lanelets = intersection_lanelets_.value(); + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); - // at the very first time of regisTration of this module, the path may not be conflicting with the - // attention area, so update() is called to update the internal data as well as traffic light info - const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet); - const bool is_prioritized = - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update( - is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); - - // this is abnormal - const auto & conflicting_lanelets = intersection_lanelets.conflicting(); - const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); - const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); - if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { - return IntersectionModule::Indecisive{"conflicting area is empty"}; + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; } - const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); - const auto & first_conflicting_area = first_conflicting_area_opt.value(); - const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); - - // generate all stop line candidates - // see the doc for struct IntersectionStopLines - /// even if the attention area is null, stuck vehicle stop line needs to be generated from - /// conflicting lanes - const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() - ? intersection_lanelets.first_attention_lane().value() - : first_conflicting_lane; - - const auto intersection_stoplines_opt = generateIntersectionStopLines( - assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, - interpolated_path_info, path); - if (!intersection_stoplines_opt) { - return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; + if (!tl_id) { + // this lane has no traffic light + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto & intersection_stoplines = intersection_stoplines_opt.value(); - const auto closest_idx = intersection_stoplines.closest_idx; - const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; - const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; - const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; - const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; - const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; - const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; - const auto occlusion_wo_tl_pass_judge_line_idx = - intersection_stoplines.occlusion_wo_tl_pass_judge_line; - - // see the doc for struct PathLanelets - const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); - const auto & conflicting_area = intersection_lanelets.conflicting_area(); - const auto path_lanelets_opt = generatePathLanelets( - lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, - first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); - if (!path_lanelets_opt.has_value()) { - return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto & path_lanelets = path_lanelets_opt.value(); - - // utility functions - auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path->points, closest_idx, index); - }; - auto stoppedForDuration = - [&](const size_t pos, const double duration, StateMachine & state_machine) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < 0.0); - const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { - state_machine.setState(StateMachine::State::GO); - } - return state_machine.getState() == StateMachine::State::GO; - }; - auto stoppedAtPosition = [&](const size_t pos, const double duration) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; + const auto & tl_info = tl_info_opt.value(); + bool has_amber_signal{false}; + for (auto && tl_light : tl_info.signal.elements) { + if (tl_light.color == TrafficSignalElement::AMBER) { + has_amber_signal = true; } - return false; - }; - - // stuck vehicle detection is viable even if attention area is empty - // so this needs to be checked before attention area validation - const bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets, &debug_data_); - const bool is_first_conflicting_lane_private = - (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); - if (stuck_detected) { - if ( - is_first_conflicting_lane_private && - planner_param_.stuck_vehicle.disable_against_private_lane) { - // do nothing - } else { - std::optional stopline_idx = std::nullopt; - if (stuck_stopline_idx_opt) { - const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < - -planner_param_.common.stopline_overshoot_margin; - if (!is_over_stuck_stopline) { - stopline_idx = stuck_stopline_idx_opt.value(); - } - } - if (!stopline_idx) { - if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) { - stopline_idx = default_stopline_idx_opt.value(); - } else if ( - first_attention_stopline_idx_opt && - fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) { - stopline_idx = closest_idx; - } - } - if (stopline_idx) { - return IntersectionModule::StuckStop{ - closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; - } + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; } } - - // if attention area is empty, collision/occlusion detection is impossible - if (!first_attention_area_opt) { - return IntersectionModule::Indecisive{"attention area is empty"}; - } - const auto first_attention_area = first_attention_area_opt.value(); - - // if attention area is not null but default stop line is not available, ego/backward-path has - // already passed the stop line - if (!default_stopline_idx_opt) { - return IntersectionModule::Indecisive{"default stop line is null"}; - } - // occlusion stop line is generated from the intersection of ego footprint along the path with the - // attention area, so if this is null, eog has already passed the intersection - if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return IntersectionModule::Indecisive{"occlusion stop line is null"}; + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; } - const auto default_stopline_idx = default_stopline_idx_opt.value(); - const bool is_over_default_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); - const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; - const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); - const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); + return TrafficPrioritizedLevel::NOT_PRIORITIZED; +} +TargetObjects IntersectionModule::generateTargetObjects( + const intersection::IntersectionLanelets & intersection_lanelets, + const std::optional & intersection_area) const +{ + const auto & objects_ptr = planner_data_->predicted_objects; + // extract target objects + TargetObjects target_objects; + target_objects.header = objects_ptr->header; + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - debug_data_.attention_area = intersection_lanelets.attention_area(); - debug_data_.occlusion_attention_area = occlusion_attention_area; - debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); - debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); - debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); - - // check occlusion on detection lane - if (!occlusion_attention_divisions_) { - occlusion_attention_divisions_ = generateDetectionLaneDivisions( - occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution); - } - const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); - - // get intersection area - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - // filter objects - auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); + for (const auto & object : objects_ptr->objects) { + // ignore non-vehicle type objects, such as pedestrian. + if (!isTargetCollisionVehicleType(object)) { + continue; + } - const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding(), - &debug_data_); - if (yield_stuck_detected) { - std::optional stopline_idx = std::nullopt; - const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; - const bool is_before_first_attention_stopline = - fromEgoDist(first_attention_stopline_idx) >= 0.0; - if (stuck_stopline_idx_opt) { - const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < - -planner_param_.common.stopline_overshoot_margin; - if (!is_over_stuck_stopline) { - stopline_idx = stuck_stopline_idx_opt.value(); - } + // check direction of objects + const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); + if (belong_adjacent_lanelet_id) { + continue; } - if (!stopline_idx) { - if (is_before_default_stopline) { - stopline_idx = default_stopline_idx; - } else if (is_before_first_attention_stopline) { - stopline_idx = closest_idx; + + const auto is_parked_vehicle = + std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + auto & container = is_parked_vehicle ? target_objects.parked_attention_objects + : target_objects.attention_objects; + if (intersection_area) { + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); + const auto intersection_area_2d = intersection_area.value(); + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); + if (belong_attention_lanelet_id) { + const auto id = belong_attention_lanelet_id.value(); + TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stopline = attention_lanelet_stoplines.at(id); + container.push_back(target_object); + } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { + TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = std::nullopt; + target_object.stopline = std::nullopt; + target_objects.intersection_area_objects.push_back(target_object); } - } - if (stopline_idx) { - return IntersectionModule::YieldStuckStop{closest_idx, stopline_idx.value()}; + } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( + object_direction, attention_lanelets, is_parked_vehicle); + belong_attention_lanelet_id.has_value()) { + // intersection_area is not available, use detection_area_with_margin as before + const auto id = belong_attention_lanelet_id.value(); + TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stopline = attention_lanelet_stoplines.at(id); + container.push_back(target_object); } } - - const bool is_amber_or_red = - (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); - auto occlusion_status = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red) - ? getOcclusionStatus( - occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, - occlusion_attention_divisions, target_objects) - : OcclusionType::NOT_OCCLUDED; - occlusion_stop_state_machine_.setStateWithMarginTime( - occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, - logger_.get_child("occlusion_stop"), *clock_); - const bool is_occlusion_cleared_with_margin = - (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // distinguish if ego detected occlusion or RTC detects occlusion - const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); - if (ext_occlusion_requested) { - occlusion_status = OcclusionType::RTC_OCCLUDED; + for (const auto & object : target_objects.attention_objects) { + target_objects.all_attention_objects.push_back(object); } - const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); - if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { - occlusion_status = prev_occlusion_status_; - } else { - prev_occlusion_status_ = occlusion_status; + for (const auto & object : target_objects.parked_attention_objects) { + target_objects.all_attention_objects.push_back(object); } + for (auto & object : target_objects.all_attention_objects) { + object.calc_dist_to_stopline(); + } + return target_objects; +} +intersection::Result< + intersection::Indecisive, + std::pair> +IntersectionModule::isOverPassJudgeLinesStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const intersection::IntersectionStopLines & intersection_stoplines) +{ + const auto & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx = intersection_stoplines.closest_idx; + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); + const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto occlusion_wo_tl_pass_judge_line_idx = + intersection_stoplines.occlusion_wo_tl_pass_judge_line; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); const size_t pass_judge_line_idx = [&]() { - if (enable_occlusion_detection_) { + if (planner_param_.occlusion.enable) { if (has_traffic_light_) { // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line @@ -1231,7 +1222,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return occlusion_stopline_idx; } const bool is_over_first_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, first_pass_judge_line_idx); + util::isOverTargetIndex(path, closest_idx, current_pose, first_pass_judge_line_idx); if (is_occlusion_state && is_over_first_pass_judge_line) { passed_1st_judge_line_while_peeking_ = true; return occlusion_stopline_idx; @@ -1251,25 +1242,29 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return first_pass_judge_line_idx; }(); - const bool was_safe = std::holds_alternative(prev_decision_result_); + const bool was_safe = std::holds_alternative(prev_decision_result_); const bool is_over_1st_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); + util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { safely_passed_1st_judge_line_time_ = clock_->now(); } + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.first_pass_judge_wall_pose = - planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); + planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, path); debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); + const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; const bool is_over_2nd_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, second_pass_judge_line_idx); + util::isOverTargetIndex(path, closest_idx, current_pose, second_pass_judge_line_idx); if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { safely_passed_2nd_judge_line_time_ = clock_->now(); } debug_data_.second_pass_judge_wall_pose = - planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, *path); + planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, path); debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); + const bool is_over_default_stopline = + util::isOverTargetIndex(path, closest_idx, current_pose, default_stopline_idx); if ( ((is_over_default_stopline || planner_param_.common.enable_pass_judge_before_default_stopline) && @@ -1293,2093 +1288,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( * - previously unsafe */ is_permanent_go_ = true; - return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; - } - - // If there are any vehicles on the attention area when ego entered the intersection on green - // light, do pseudo collision detection because the vehicles are very slow and no collisions may - // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = isGreenSolidOn(assigned_lanelet); - if (is_green_solid_on) { - if (!initial_green_light_observed_time_) { - const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); - const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( - path->points, closest_idx, - tier4_autoware_utils::createPoint( - assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), - assigned_lane_begin_point.z())) < - planner_param_.collision_detection.yield_on_green_traffic_light - .distance_to_assigned_lanelet_start; - if (approached_assigned_lane) { - initial_green_light_observed_time_ = clock_->now(); - } - } - if (initial_green_light_observed_time_) { - const auto now = clock_->now(); - const bool exist_close_vehicles = std::any_of( - target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), - [&](const auto & object) { - return object.dist_to_stopline.has_value() && - object.dist_to_stopline.value() < - planner_param_.collision_detection.yield_on_green_traffic_light - .object_dist_to_stopline; - }); - if ( - exist_close_vehicles && - rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < - planner_param_.collision_detection.yield_on_green_traffic_light.duration) { - return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - } - } - - // calculate dynamic collision around attention area - const double time_to_restart = - (is_go_out_ || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.collision_detection_hold_time - - collision_state_machine_.getDuration()); - - // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd - // pass judge line respectively, ego should go - const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; - const auto last_intersection_stopline_candidate_idx = - second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, - time_to_restart, traffic_prioritized_level); - collision_state_machine_.setStateWithMarginTime( - has_collision ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("collision state_machine"), *clock_); - const bool has_collision_with_margin = - collision_state_machine_.getState() == StateMachine::State::STOP; - - if (is_prioritized) { - return FullyPrioritized{ - has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - - // Safe - if (!is_occlusion_state && !has_collision_with_margin) { - return IntersectionModule::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - // Only collision - if (!is_occlusion_state && has_collision_with_margin) { - return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - // Occluded - // occlusion_status is assured to be not NOT_OCCLUDED - const bool stopped_at_default_line = stoppedForDuration( - default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, - before_creep_state_machine_); - if (stopped_at_default_line) { - // if specified the parameter occlusion.temporal_stop_before_attention_area OR - // has_no_traffic_light_, ego will temporarily stop before entering attention area - const bool temporal_stop_before_attention_required = - (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) - ? !stoppedForDuration( - first_attention_stopline_idx, - planner_param_.occlusion.temporal_stop_time_before_peeking, - temporal_stop_before_attention_state_machine_) - : false; - if (!has_traffic_light_) { - if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return IntersectionModule::Indecisive{ - "already passed maximum peeking line in the absence of traffic light"}; - } - return IntersectionModule::OccludedAbsenceTrafficLight{ - is_occlusion_cleared_with_margin, - has_collision_with_margin, - temporal_stop_before_attention_required, - closest_idx, - first_attention_stopline_idx, - occlusion_wo_tl_pass_judge_line_idx}; - } - // following remaining block is "has_traffic_light_" - // if ego is stuck by static occlusion in the presence of traffic light, start timeout count - const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; - const bool is_stuck_by_static_occlusion = - stoppedAtPosition( - occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) && - is_static_occlusion; - if (has_collision_with_margin) { - // if collision is detected, timeout is reset - static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); - } else if (is_stuck_by_static_occlusion) { - static_occlusion_timeout_state_machine_.setStateWithMarginTime( - StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); - } - const bool release_static_occlusion_stuck = - (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); - if (!has_collision_with_margin && release_static_occlusion_stuck) { - return IntersectionModule::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED - const double max_timeout = - planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + - planner_param_.occlusion.occlusion_detection_hold_time; - const std::optional static_occlusion_timeout = - is_stuck_by_static_occlusion - ? std::make_optional( - max_timeout - static_occlusion_timeout_state_machine_.getDuration() - - occlusion_stop_state_machine_.getDuration()) - : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); - if (has_collision_with_margin) { - return IntersectionModule::OccludedCollisionStop{ - is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stopline_idx, - first_attention_stopline_idx, - occlusion_stopline_idx, - static_occlusion_timeout}; - } else { - return IntersectionModule::PeekingTowardOcclusion{ - is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stopline_idx, - first_attention_stopline_idx, - occlusion_stopline_idx, - static_occlusion_timeout}; - } - } else { - const auto occlusion_stopline = - (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) - ? first_attention_stopline_idx - : occlusion_stopline_idx; - return IntersectionModule::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline}; - } -} - -TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(lanelet::ConstLanelet lane) -{ - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - - std::optional tl_id = std::nullopt; - for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { - tl_id = tl_reg_elem->id(); - break; - } - if (!tl_id) { - // this lane has no traffic light - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto tl_info_opt = planner_data_->getTrafficSignal( - tl_id.value(), true /* traffic light module keeps last observation*/); - if (!tl_info_opt) { - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto & tl_info = tl_info_opt.value(); - bool has_amber_signal{false}; - for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color == TrafficSignalElement::AMBER) { - has_amber_signal = true; - } - if (tl_light.color == TrafficSignalElement::RED) { - // NOTE: Return here since the red signal has the highest priority. - return TrafficPrioritizedLevel::FULLY_PRIORITIZED; - } - } - if (has_amber_signal) { - return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; - } - return TrafficPrioritizedLevel::NOT_PRIORITIZED; -} - -static std::vector getPolygon3dFromLanelets( - const lanelet::ConstLanelets & ll_vec) -{ - std::vector polys; - for (auto && ll : ll_vec) { - polys.push_back(ll.polygon3d()); - } - return polys; -} - -IntersectionLanelets IntersectionModule::getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path) -{ - const double detection_area_length = planner_param_.common.attention_area_length; - const double occlusion_detection_area_length = - planner_param_.occlusion.occlusion_attention_area_length; - const bool consider_wrong_direction_vehicle = - planner_param_.collision_detection.consider_wrong_direction_vehicle; - - // retrieve a stopline associated with a traffic light - bool has_traffic_light = false; - if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); - tl_reg_elems.size() != 0) { - const auto tl_reg_elem = tl_reg_elems.front(); - const auto stopline_opt = tl_reg_elem->stopLine(); - if (!!stopline_opt) has_traffic_light = true; - } - - // for low priority lane - // If ego_lane has right of way (i.e. is high priority), - // ignore yieldLanelets (i.e. low priority lanes) - lanelet::ConstLanelets yield_lanelets{}; - const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); - for (const auto & right_of_way : right_of_ways) { - if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { - for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { - yield_lanelets.push_back(yield_lanelet); - for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { - yield_lanelets.push_back(previous_lanelet); - } - } - } - } - - // get all following lanes of previous lane - lanelet::ConstLanelets ego_lanelets = lanelets_on_path; - for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { - ego_lanelets.push_back(previous_lanelet); - for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { - if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { - continue; - } - ego_lanelets.push_back(following_lanelet); - } - } - - // get conflicting lanes on assigned lanelet - const auto & conflicting_lanelets = - lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); - std::vector adjacent_followings; - - for (const auto & conflicting_lanelet : conflicting_lanelets) { - for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - } - - // final objective lanelets - lanelet::ConstLanelets detection_lanelets; - lanelet::ConstLanelets conflicting_ex_ego_lanelets; - // conflicting lanes is necessary to get stopline for stuck vehicle - for (auto && conflicting_lanelet : conflicting_lanelets) { - if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) - conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); - } - - // exclude yield lanelets and ego lanelets from detection_lanelets - if (turn_direction_ == std::string("straight") && has_traffic_light) { - // if assigned lanelet is "straight" with traffic light, detection area is not necessary - } else { - if (consider_wrong_direction_vehicle) { - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - for (const auto & adjacent_following : adjacent_followings) { - detection_lanelets.push_back(adjacent_following); - } - } else { - // otherwise we need to know the priority from RightOfWay - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if ( - lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || - lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - } - } - - // get possible lanelet path that reaches conflicting_lane longer than given length - lanelet::ConstLanelets detection_and_preceding_lanelets; - { - const double length = detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(l); - } - } - } - } - - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; - { - const double length = occlusion_detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); - } - } - } - } - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; - for (const auto & ll : occlusion_detection_and_preceding_lanelets) { - const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (turn_direction == "left" || turn_direction == "right") { - continue; - } - occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); - } - - auto [attention_lanelets, original_attention_lanelet_sequences] = - util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - - IntersectionLanelets result; - result.attention_ = std::move(attention_lanelets); - for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { - // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that - // back() exists. - std::optional stopline{std::nullopt}; - for (auto it = original_attention_lanelet_seq.rbegin(); - it != original_attention_lanelet_seq.rend(); ++it) { - const auto traffic_lights = it->regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - break; - } - if (stopline) break; - } - result.attention_stoplines_.push_back(stopline); - } - result.attention_non_preceding_ = std::move(detection_lanelets); - for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - std::optional stopline = std::nullopt; - const auto & ll = result.attention_non_preceding_.at(i); - const auto traffic_lights = ll.regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - } - result.attention_non_preceding_stoplines_.push_back(stopline); - } - result.conflicting_ = std::move(conflicting_ex_ego_lanelets); - result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); - // NOTE: occlusion_attention is not inverted here - // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and - // then trim part of them based on curvature threshold - result.occlusion_attention_ = - std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); - - // NOTE: to properly update(), each element in conflicting_/conflicting_area_, - // attention_non_preceding_/attention_non_preceding_area_ need to be matched - result.attention_area_ = getPolygon3dFromLanelets(result.attention_); - result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); - result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); - result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); - result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); - return result; -} - -std::optional IntersectionModule::generateIntersectionStopLines( - lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::ConstLanelet & first_attention_lane, - const std::optional & second_attention_area_opt, - const util::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; - const double stopline_margin = planner_param_.common.default_stopline_margin; - const double max_accel = planner_param_.common.max_accel; - const double max_jerk = planner_param_.common.max_jerk; - const double delay_response_time = planner_param_.common.delay_response_time; - const double peeking_offset = planner_param_.occlusion.peeking_offset; - - const auto first_attention_area = first_attention_lane.polygon3d(); - const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); - - // find the index of the first point whose vehicle footprint on it intersects with attention_area - const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - const std::optional first_footprint_inside_1st_attention_ip_opt = - getFirstPointInsidePolygonByFootprint( - first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_1st_attention_ip_opt) { - return std::nullopt; - } - const auto first_footprint_inside_1st_attention_ip = - first_footprint_inside_1st_attention_ip_opt.value(); - - std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; - for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { - // NOTE: maybe consideration of braking dist is necessary - first_footprint_attention_centerline_ip_opt = i; - break; - } - } - if (!first_footprint_attention_centerline_ip_opt) { - return std::nullopt; - } - const size_t first_footprint_attention_centerline_ip = - first_footprint_attention_centerline_ip_opt.value(); - - // (1) default stop line position on interpolated path - bool default_stopline_valid = true; - int stop_idx_ip_int = -1; - if (const auto map_stop_idx_ip = - getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); - map_stop_idx_ip) { - stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } - if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; - } - if (stop_idx_ip_int < 0) { - default_stopline_valid = false; - } - const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; - - // (2) ego front stop line position on interpolated path - const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; - const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( - path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); - - // (3) occlusion peeking stop line position on interpolated path - int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); - bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the attention area, invalid - { - const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects( - path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { - occlusion_peeking_line_valid = false; - } - } - if (occlusion_peeking_line_valid) { - occlusion_peeking_line_ip_int = - first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); - } - const auto occlusion_peeking_line_ip = static_cast( - std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - - // (4) first attention stopline position on interpolated path - const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; - const bool first_attention_stopline_valid = true; - - // (5) 1st pass judge line position on interpolated path - const double velocity = planner_data_->current_velocity->twist.linear.x; - const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; - const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_accel, max_jerk, delay_response_time); - int first_pass_judge_ip_int = - static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); - const auto first_pass_judge_line_ip = static_cast( - std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( - 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); - - // (6) stuck vehicle stopline position on interpolated path - int stuck_stopline_ip_int = 0; - bool stuck_stopline_valid = true; - if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching attention area and already passed - // first_conflicting_area, this could be null. - const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); - if (!stuck_stopline_idx_ip_opt) { - stuck_stopline_valid = false; - stuck_stopline_ip_int = 0; - } else { - stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; - } - } else { - stuck_stopline_ip_int = - std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); - } - if (stuck_stopline_ip_int < 0) { - stuck_stopline_valid = false; - } - const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); - - // (7) second attention stopline position on interpolated path - int second_attention_stopline_ip_int = -1; - bool second_attention_stopline_valid = false; - if (second_attention_area_opt) { - const auto & second_attention_area = second_attention_area_opt.value(); - std::optional first_footprint_inside_2nd_attention_ip_opt = - getFirstPointInsidePolygonByFootprint( - second_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (first_footprint_inside_2nd_attention_ip_opt) { - second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); - second_attention_stopline_valid = true; - } - } - const auto second_attention_stopline_ip = - second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) - : 0; - - // (8) second pass judge line position on interpolated path. It is the same as first pass judge - // line if second_attention_lane is null - int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; - const auto second_pass_judge_line_ip = - second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) - : first_pass_judge_line_ip; - - struct IntersectionStopLinesTemp - { - size_t closest_idx{0}; - size_t stuck_stopline{0}; - size_t default_stopline{0}; - size_t first_attention_stopline{0}; - size_t second_attention_stopline{0}; - size_t occlusion_peeking_stopline{0}; - size_t first_pass_judge_line{0}; - size_t second_pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; - }; - - IntersectionStopLinesTemp intersection_stoplines_temp; - std::list> stoplines = { - {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, - {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, - {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, - {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, - {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, - {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, - {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, - {&occlusion_wo_tl_pass_judge_line_ip, - &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; - stoplines.sort( - [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); - for (const auto & [stop_idx_ip, stop_idx] : stoplines) { - const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; - const auto insert_idx = util::insertPointIndex( - insert_point, original_path, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); - if (!insert_idx) { - return std::nullopt; - } - *stop_idx = insert_idx.value(); - } - if ( - intersection_stoplines_temp.occlusion_peeking_stopline < - intersection_stoplines_temp.default_stopline) { - intersection_stoplines_temp.occlusion_peeking_stopline = - intersection_stoplines_temp.default_stopline; - } - - IntersectionStopLines intersection_stoplines; - intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; - if (stuck_stopline_valid) { - intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; - } - if (default_stopline_valid) { - intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; - } - if (first_attention_stopline_valid) { - intersection_stoplines.first_attention_stopline = - intersection_stoplines_temp.first_attention_stopline; - } - if (second_attention_stopline_valid) { - intersection_stoplines.second_attention_stopline = - intersection_stoplines_temp.second_attention_stopline; - } - if (occlusion_peeking_line_valid) { - intersection_stoplines.occlusion_peeking_stopline = - intersection_stoplines_temp.occlusion_peeking_stopline; - } - intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; - intersection_stoplines.second_pass_judge_line = - intersection_stoplines_temp.second_pass_judge_line; - intersection_stoplines.occlusion_wo_tl_pass_judge_line = - intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; - return intersection_stoplines; -} - -/** - * @brief Get stop point from map if exists - * @param stop_pose stop point defined on map - * @return true when the stop point is defined on map. - */ -std::optional IntersectionModule::getStopLineIndexFromMap( - const util::InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) -{ - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - - const auto road_markings = - assigned_lanelet.regulatoryElementsAs(); - lanelet::ConstLineStrings3d stopline; - for (const auto & road_marking : road_markings) { - const std::string type = - road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); - if (type == lanelet::AttributeValueString::StopLine) { - stopline.push_back(road_marking->roadMarking()); - break; // only one stopline exists. - } - } - if (stopline.empty()) { - return std::nullopt; - } - - const auto p_start = stopline.front().front(); - const auto p_end = stopline.front().back(); - const LineString2d extended_stopline = - planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); - - for (size_t i = lane_interval.first; i < lane_interval.second; i++) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - std::vector collision_points; - bg::intersection(extended_stopline, path_segment, collision_points); - - if (collision_points.empty()) { - continue; - } - - return i; - } - - geometry_msgs::msg::Pose stop_point_from_map; - stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); - stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); - stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - - return motion_utils::findFirstNearestIndexWithSoftConstraints( - path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); -} - -static lanelet::ConstLanelets getPrevLanelets( - const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) -{ - lanelet::ConstLanelets previous_lanelets; - for (const auto & ll : lanelets_on_path) { - if (associative_ids.find(ll.id()) != associative_ids.end()) { - return previous_lanelets; - } - previous_lanelets.push_back(ll); - } - return previous_lanelets; -} - -// end inclusive -static lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval) -{ - lanelet::Points3d lefts; - lanelet::Points3d rights; - size_t prev_idx = start_idx; - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto & p = path.points.at(i).point.pose; - const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { - continue; - } - prev_idx = i; - const double yaw = tf2::getYaw(p.orientation); - const double x = p.position.x; - const double y = p.position.y; - // NOTE: maybe this is opposite - const double left_x = x + width / 2 * std::sin(yaw); - const double left_y = y - width / 2 * std::cos(yaw); - const double right_x = x - width / 2 * std::sin(yaw); - const double right_y = y + width / 2 * std::cos(yaw); - lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); - rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); - } - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); - - return lanelet::Lanelet(lanelet::InvalId, left, right); -} - -static std::optional> -getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, - const std::vector & polygons, const bool search_forward = true) -{ - if (search_forward) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - } - } else { - for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - if (i == 0) { - break; - } - } - } - return std::nullopt; -} - -std::optional IntersectionModule::generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx) -{ - const double width = planner_data_->vehicle_info_.vehicle_width_m; - static constexpr double path_lanelet_interval = 1.5; - - const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; - if (!assigned_lane_interval_opt) { - return std::nullopt; - } - const auto assigned_lane_interval = assigned_lane_interval_opt.value(); - const auto & path = interpolated_path_info.path; - - PathLanelets path_lanelets; - // prev - path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids_); - path_lanelets.all = path_lanelets.prev; - - // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; - if (closest_idx > assigned_lane_start) { - path_lanelets.all.push_back( - generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); - } - - // ego_or_entry2exit - const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); - path_lanelets.ego_or_entry2exit = - generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); - - // next - if (assigned_lane_end < path.points.size() - 1) { - const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); - const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); - if (next_lane_interval_opt) { - const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.next.value()); - } - } - - const auto first_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) - : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); - const auto last_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) - : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); - if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { - const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); - const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; - lanelet::ConstLanelet conflicting_interval = generatePathLanelet( - path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, - path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); - if (last_inside_conflicting_idx < assigned_lane_end) { - lanelet::ConstLanelet remaining_interval = generatePathLanelet( - path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); - } - } - return path_lanelets; -} - -bool IntersectionModule::checkStuckVehicleInIntersection( - const PathLanelets & path_lanelets, DebugData * debug_data) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getLaneletLength3d; - using lanelet::utils::getPolygonFromArcLength; - using lanelet::utils::to2D; - - const bool stuck_detection_direction = [&]() { - return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || - (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || - (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); - }(); - if (!stuck_detection_direction) { - return false; - } - - const auto & objects_ptr = planner_data_->predicted_objects; - - // considering lane change in the intersection, these lanelets are generated from the path - const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; - Polygon2d stuck_vehicle_detect_area{}; - if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { - return false; - } - - double target_polygon_length = - getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); - lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; - if (path_lanelets.next) { - targets.push_back(path_lanelets.next.value()); - const double next_arc_length = - std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); - target_polygon_length += next_arc_length; - } - const auto target_polygon = - to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); - - if (target_polygon.empty()) { - return false; - } - - for (const auto & p : target_polygon) { - stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); - } - - stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); - bg::correct(stuck_vehicle_detect_area); - - debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - - for (const auto & object : objects_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { - continue; // not target vehicle type - } - const auto obj_v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { - continue; // not stop vehicle - } - - // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); - if (is_in_stuck_area && debug_data) { - debug_data->stuck_targets.objects.push_back(object); - return true; - } - } - return false; -} - -static lanelet::LineString3d getLineStringFromArcLength( - const lanelet::ConstLineString3d & linestring, const double s1, const double s2) -{ - lanelet::Points3d points; - double accumulated_length = 0; - size_t start_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s1) { - start_index = i; - break; - } - accumulated_length += length; - } - if (start_index < linestring.size() - 1) { - const auto & p1 = linestring[start_index]; - const auto & p2 = linestring[start_index + 1]; - const double residue = s1 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto start_basic_point = p1.basicPoint() + residue * direction_vector; - const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); - points.push_back(start_point); - } - - accumulated_length = 0; - size_t end_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s2) { - end_index = i; - break; - } - accumulated_length += length; - } - - for (size_t i = start_index + 1; i < end_index; i++) { - const auto p = lanelet::Point3d(linestring[i]); - points.push_back(p); - } - if (end_index < linestring.size() - 1) { - const auto & p1 = linestring[end_index]; - const auto & p2 = linestring[end_index + 1]; - const double residue = s2 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto end_basic_point = p1.basicPoint() + residue * direction_vector; - const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); - points.push_back(end_point); - } - return lanelet::LineString3d{lanelet::InvalId, points}; -} - -static lanelet::ConstLanelet createLaneletFromArcLength( - const lanelet::ConstLanelet & lanelet, const double s1, const double s2) -{ - const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); - // make sure that s1, and s2 are between [0, lane_length] - const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); - const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); - - const auto ratio_s1 = s1_saturated / total_length; - const auto ratio_s2 = s2_saturated / total_length; - - const auto s1_left = - static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s2_left = - static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s1_right = - static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); - const auto s2_right = - static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); - - const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); - const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); - - return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); -} - -bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data) -{ - const bool yield_stuck_detection_direction = [&]() { - return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || - (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || - (turn_direction_ == "straight" && planner_param_.yield_stuck.turn_direction.straight); - }(); - if (!yield_stuck_detection_direction) { - return false; - } - - const double width = planner_data_->vehicle_info_.vehicle_width_m; - const double stuck_vehicle_vel_thr = - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; - const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; - - LineString2d sparse_intersection_path; - const auto [start, end] = interpolated_path_info.lane_id_interval.value(); - for (unsigned i = start; i < end; ++i) { - const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; - const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); - if (turn_direction_ == "right") { - const double right_x = point.x - width / 2 * std::sin(yaw); - const double right_y = point.y + width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(right_x, right_y); - } else if (turn_direction_ == "left") { - const double left_x = point.x + width / 2 * std::sin(yaw); - const double left_y = point.y - width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(left_x, left_y); - } else { - // straight - sparse_intersection_path.emplace_back(point.x, point.y); - } - } - lanelet::ConstLanelets yield_stuck_detect_lanelets; - for (const auto & attention_lanelet : attention_lanelets) { - const auto centerline = attention_lanelet.centerline2d().basicLineString(); - std::vector intersects; - bg::intersection(sparse_intersection_path, centerline, intersects); - if (intersects.empty()) { - continue; - } - const auto intersect = intersects.front(); - const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( - centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); - const double yield_stuck_start = - std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); - const double yield_stuck_end = intersect_arc_coords.length; - yield_stuck_detect_lanelets.push_back( - createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); - } - debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { - const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); - - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; - } - for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { - const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); - if (is_in_lanelet) { - debug_data->yield_stuck_targets.objects.push_back(object.object); - return true; - } - } - } - return false; -} - -TargetObjects IntersectionModule::generateTargetObjects( - const IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const -{ - const auto & objects_ptr = planner_data_->predicted_objects; - // extract target objects - TargetObjects target_objects; - target_objects.header = objects_ptr->header; - const auto & attention_lanelets = intersection_lanelets.attention(); - const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - for (const auto & object : objects_ptr->objects) { - // ignore non-vehicle type objects, such as pedestrian. - if (!isTargetCollisionVehicleType(object)) { - continue; - } - - // check direction of objects - const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = - checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); - if (belong_adjacent_lanelet_id) { - continue; - } - - const auto is_parked_vehicle = - std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; - auto & container = is_parked_vehicle ? target_objects.parked_attention_objects - : target_objects.attention_objects; - if (intersection_area) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); - const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = - checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); - if (belong_attention_lanelet_id) { - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = std::nullopt; - target_object.stopline = std::nullopt; - target_objects.intersection_area_objects.push_back(target_object); - } - } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( - object_direction, attention_lanelets, is_parked_vehicle); - belong_attention_lanelet_id.has_value()) { - // intersection_area is not available, use detection_area_with_margin as before - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } - } - for (const auto & object : target_objects.attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (const auto & object : target_objects.parked_attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (auto & object : target_objects.all_attention_objects) { - object.calc_dist_to_stopline(); - } - return target_objects; -} - -bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const TrafficPrioritizedLevel & traffic_prioritized_level) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getPolygonFromArcLength; - - // check collision between target_objects predicted path and ego lane - // cut the predicted path at passing_time - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = calcIntersectionPassingTime( - path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); - - if ( - std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != - planner_param_.debug.ttc.end()) { - ego_ttc_time_array.stamp = path.header.stamp; - ego_ttc_pub_->publish(ego_ttc_time_array); - } - - const double passing_time = time_distance_array.back().first; - cutPredictPathWithDuration(target_objects, passing_time); - - const auto & concat_lanelets = path_lanelets.all; - const auto closest_arc_coords = getArcCoordinates( - concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const auto & ego_lane = path_lanelets.ego_or_entry2exit; - debug_data_.ego_lane = ego_lane.polygon3d(); - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - - // change TTC margin based on ego traffic light color - const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { - if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { - return std::make_pair( - planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, - planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); - } - if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { - return std::make_pair( - planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, - planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); - } - return std::make_pair( - planner_param_.collision_detection.not_prioritized.collision_start_margin_time, - planner_param_.collision_detection.not_prioritized.collision_end_margin_time); - }(); - const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { - if (!target_object.dist_to_stopline) { - return false; - } - const double dist_to_stopline = target_object.dist_to_stopline.value(); - if (dist_to_stopline < 0) { - return false; - } - const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - v * v / - (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light - .object_expected_deceleration)); - return dist_to_stopline > braking_distance; - }; - const auto isTolerableOvershoot = [&](const TargetObject & target_object) { - if ( - !target_object.attention_lanelet || !target_object.dist_to_stopline || - !target_object.stopline) { - return false; - } - const double dist_to_stopline = target_object.dist_to_stopline.value(); - const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - v * v / - (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light - .object_expected_deceleration)); - if (dist_to_stopline > braking_distance) { - return false; - } - const auto stopline_front = target_object.stopline.value().front(); - const auto stopline_back = target_object.stopline.value().back(); - tier4_autoware_utils::LineString2d object_line; - object_line.emplace_back( - (stopline_front.x() + stopline_back.x()) / 2.0, - (stopline_front.y() + stopline_back.y()) / 2.0); - const auto stopline_mid = object_line.front(); - const auto endpoint = target_object.attention_lanelet.value().centerline().back(); - object_line.emplace_back(endpoint.x(), endpoint.y()); - std::vector intersections; - bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); - if (intersections.empty()) { - return false; - } - const auto collision_point = intersections.front(); - // distance from object expected stop position to collision point - const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; - const double stopline_to_collision = - std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); - const double object2collision = stopline_to_collision - stopline_to_object; - const double margin = - planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; - return (object2collision > margin) || (object2collision < 0); - }; - // check collision between predicted_path and ego_area - tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; - object_ttc_time_array.layout.dim.resize(3); - object_ttc_time_array.layout.dim.at(0).label = "objects"; - object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id - object_ttc_time_array.layout.dim.at(1).label = - "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " - "start_time, start_dist, " - "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " - "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; - object_ttc_time_array.layout.dim.at(1).size = 57; - for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { - object_ttc_time_array.data.push_back(lane_id_); - } - bool collision_detected = false; - for (const auto & target_object : target_objects->all_attention_objects) { - const auto & object = target_object.object; - // If the vehicle is expected to stop before their stopline, ignore - const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); - if ( - traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && - expected_to_stop_before_stopline) { - debug_data_.amber_ignore_targets.objects.push_back(object); - continue; - } - const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); - if ( - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && - !expected_to_stop_before_stopline && is_tolerable_overshoot) { - debug_data_.red_overshoot_ignore_targets.objects.push_back(object); - continue; - } - for (const auto & predicted_path : object.kinematics.predicted_paths) { - if ( - predicted_path.confidence < - planner_param_.collision_detection.min_predicted_path_confidence) { - // ignore the predicted path with too low confidence - continue; - } - - // collision point - const auto first_itr = std::adjacent_find( - predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly, &object](const auto & a, const auto & b) { - return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); - }); - if (first_itr == predicted_path.path.cend()) continue; - const auto last_itr = std::adjacent_find( - predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly, &object](const auto & a, const auto & b) { - return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); - }); - if (last_itr == predicted_path.path.crend()) continue; - - // possible collision time interval - const double ref_object_enter_time = - static_cast(first_itr - predicted_path.path.begin()) * - rclcpp::Duration(predicted_path.time_step).seconds(); - auto start_time_distance_itr = time_distance_array.begin(); - if (ref_object_enter_time - collision_start_margin_time > 0) { - // start of possible ego position in the intersection - start_time_distance_itr = std::lower_bound( - time_distance_array.begin(), time_distance_array.end(), - ref_object_enter_time - collision_start_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (start_time_distance_itr == time_distance_array.end()) { - // ego is already at the exit of intersection when npc is at collision point even if npc - // accelerates so ego's position interval is empty - continue; - } - } - const double ref_object_exit_time = - static_cast(last_itr.base() - predicted_path.path.begin()) * - rclcpp::Duration(predicted_path.time_step).seconds(); - auto end_time_distance_itr = std::lower_bound( - time_distance_array.begin(), time_distance_array.end(), - ref_object_exit_time + collision_end_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (end_time_distance_itr == time_distance_array.end()) { - // ego is already passing the intersection, when npc is is at collision point - // so ego's position interval is up to the end of intersection lane - end_time_distance_itr = time_distance_array.end() - 1; - } - const double start_arc_length = std::max( - 0.0, closest_arc_coords.length + (*start_time_distance_itr).second - - planner_data_->vehicle_info_.rear_overhang_m); - const double end_arc_length = std::min( - closest_arc_coords.length + (*end_time_distance_itr).second + - planner_data_->vehicle_info_.max_longitudinal_offset_m, - lanelet::utils::getLaneletLength2d(concat_lanelets)); - - const auto trimmed_ego_polygon = - getPolygonFromArcLength(concat_lanelets, start_arc_length, end_arc_length); - - if (trimmed_ego_polygon.empty()) { - continue; - } - - Polygon2d polygon{}; - for (const auto & p : trimmed_ego_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); - } - bg::correct(polygon); - debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); - - for (auto itr = first_itr; itr != last_itr.base(); ++itr) { - const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); - if (bg::intersects(polygon, footprint_polygon)) { - collision_detected = true; - break; - } - } - object_ttc_time_array.layout.dim.at(0).size++; - const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto & shape = object.shape; - object_ttc_time_array.data.insert( - object_ttc_time_array.data.end(), - {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), - shape.dimensions.x, shape.dimensions.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x, - 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, - start_time_distance_itr->first, start_time_distance_itr->second, - end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, - first_itr->position.y, last_itr->position.x, last_itr->position.y}); - for (unsigned i = 0; i < 20; i++) { - const auto & pos = - predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; - object_ttc_time_array.data.push_back(pos.x); - object_ttc_time_array.data.push_back(pos.y); - } - if (collision_detected) { - debug_data_.conflicting_targets.objects.push_back(object); - break; - } - } - } - - if ( - std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != - planner_param_.debug.ttc.end()) { - object_ttc_time_array.stamp = path.header.stamp; - object_ttc_pub_->publish(object_ttc_time_array); - } - - return collision_detected; -} - -std::optional IntersectionModule::checkAngleForTargetLanelets( - - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const bool is_parked_vehicle) const -{ - const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; - const bool consider_wrong_direction_vehicle = - planner_param_.common.attention_area_angle_threshold; - const double dist_margin = planner_param_.common.attention_area_margin; - - for (unsigned i = 0; i < target_lanelets.size(); ++i) { - const auto & ll = target_lanelets.at(i); - if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { - continue; - } - const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); - const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); - if (consider_wrong_direction_vehicle) { - if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - } else { - if (std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is - // positive - if ( - is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return std::make_optional(i); - } - } - } - return std::nullopt; -} - -void IntersectionModule::cutPredictPathWithDuration( - TargetObjects * target_objects, const double time_thr) -{ - const rclcpp::Time current_time = clock_->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(target_objects->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } - } - } - } -} - -TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) -{ - const double intersection_velocity = - planner_param_.collision_detection.velocity_profile.default_velocity; - const double minimum_ego_velocity = - planner_param_.collision_detection.velocity_profile.minimum_default_velocity; - const bool use_upstream_velocity = - planner_param_.collision_detection.velocity_profile.use_upstream; - const double minimum_upstream_velocity = - planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; - const double current_velocity = planner_data_->current_velocity->twist.linear.x; - - int assigned_lane_found = false; - - // crop intersection part of the path, and set the reference velocity to intersection_velocity - // for ego's ttc - PathWithLaneId reference_path; - std::optional upstream_stopline{std::nullopt}; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - auto reference_point = path.points.at(i); - // assume backward velocity is current ego velocity - if (i < closest_idx) { - reference_point.point.longitudinal_velocity_mps = current_velocity; - } - if ( - i > last_intersection_stopline_candidate_idx && - std::fabs(reference_point.point.longitudinal_velocity_mps) < - std::numeric_limits::epsilon() && - !upstream_stopline) { - upstream_stopline = i; - } - if (!use_upstream_velocity) { - reference_point.point.longitudinal_velocity_mps = intersection_velocity; - } - reference_path.points.push_back(reference_point); - bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); - if (assigned_lane_found && !has_objective_lane_id) { - break; - } - assigned_lane_found = has_objective_lane_id; - } - if (!assigned_lane_found) { - return {{0.0, 0.0}}; // has already passed the intersection. - } - - std::vector> original_path_xy; - for (size_t i = 0; i < reference_path.points.size(); ++i) { - const auto & p = reference_path.points.at(i).point.pose.position; - original_path_xy.emplace_back(p.x, p.y); - } - - // apply smoother to reference velocity - PathWithLaneId smoothed_reference_path = reference_path; - if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { - smoothed_reference_path = reference_path; - } - - // calculate when ego is going to reach each (interpolated) points on the path - TimeDistanceArray time_distance_array{}; - double dist_sum = 0.0; - double passing_time = time_delay; - time_distance_array.emplace_back(passing_time, dist_sum); - - // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so - // `last_intersection_stopline_candidate_idx` makes no sense - const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, path.points.at(closest_idx).point.pose, - planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); - - const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { - if (upstream_stopline) { - const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; - return motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, upstream_stopline_point, - planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); - } else { - return std::nullopt; - } - }(); - - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { - const auto & p1 = smoothed_reference_path.points.at(i); - const auto & p2 = smoothed_reference_path.points.at(i + 1); - - const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); - dist_sum += dist; - - // use average velocity between p1 and p2 - const double average_velocity = - (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - const double passing_velocity = [=]() { - if (use_upstream_velocity) { - if (upstream_stopline_idx_opt && i > upstream_stopline_idx_opt.value()) { - return minimum_upstream_velocity; - } - return std::max(average_velocity, minimum_ego_velocity); - } else { - return std::max(average_velocity, minimum_ego_velocity); - } - }(); - passing_time += (dist / passing_velocity); - - time_distance_array.emplace_back(passing_time, dist_sum); - } - debug_ttc_array->layout.dim.resize(3); - debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; - debug_ttc_array->layout.dim.at(0).size = 5; - debug_ttc_array->layout.dim.at(1).label = "values"; - debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); - debug_ttc_array->data.reserve( - time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); - for (unsigned i = 0; i < time_distance_array.size(); ++i) { - debug_ttc_array->data.push_back(lane_id_); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(t); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(d); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.x); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.y); - } - return time_distance_array; -} - -static double getHighestCurvature(const lanelet::ConstLineString3d & centerline) -{ - std::vector points; - for (auto point = centerline.begin(); point != centerline.end(); point++) { - points.push_back(*point); - } - - SplineInterpolationPoints2d interpolation(points); - const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); - std::vector curvatures_positive; - for (const auto & curvature : curvatures) { - curvatures_positive.push_back(std::fabs(curvature)); - } - return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); -} - -std::vector IntersectionModule::generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets_all, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) -{ - const double curvature_threshold = - planner_param_.occlusion.attention_lane_crop_curvature_threshold; - const double curvature_calculation_ds = - planner_param_.occlusion.attention_lane_curvature_calculation_ds; - - using lanelet::utils::getCenterlineWithOffset; - - // (0) remove left/right lanelet - lanelet::ConstLanelets detection_lanelets; - for (const auto & detection_lanelet : detection_lanelets_all) { - // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet - const auto fine_centerline = - lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); - const double highest_curvature = getHighestCurvature(fine_centerline); - if (highest_curvature > curvature_threshold) { - continue; - } - detection_lanelets.push_back(detection_lanelet); - } - - // (1) tsort detection_lanelets - const auto [merged_detection_lanelets, originals] = - util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); - - // (2) merge each branch to one lanelet - // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::vector> merged_lanelet_with_area; - for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { - const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); - const auto & original = originals.at(i); - double area = 0; - for (const auto & partition : original) { - area += bg::area(partition.polygon2d().basicPolygon()); - } - merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); - } - - // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { - const double length = bg::length(merged_lanelet.centerline()); - const double width = area / length; - for (int i = 0; i < static_cast(width / resolution); ++i) { - const double offset = resolution * i - width / 2; - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); - } - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); - } - return detection_divisions; -} - -IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects) -{ - const auto & occ_grid = *planner_data_->occupancy_grid; - const auto & current_pose = planner_data_->current_odometry->pose; - const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; - - const auto & path_ip = interpolated_path_info.path; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - - const auto first_attention_area_idx = - util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); - if (!first_attention_area_idx) { - return OcclusionType::NOT_OCCLUDED; - } - - const auto first_inside_attention_idx_ip_opt = - util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); - const std::pair lane_attention_interval_ip = - first_inside_attention_idx_ip_opt - ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) - : lane_interval_ip; - const auto [lane_start_idx, lane_end_idx] = lane_attention_interval_ip; - - const int width = occ_grid.info.width; - const int height = occ_grid.info.height; - const double resolution = occ_grid.info.resolution; - const auto & origin = occ_grid.info.origin.position; - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - - Polygon2d grid_poly; - grid_poly.outer().emplace_back(origin.x, origin.y); - grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); - grid_poly.outer().emplace_back( - origin.x + (width - 1) * resolution, origin.y + (height - 1) * resolution); - grid_poly.outer().emplace_back(origin.x, origin.y + (height - 1) * resolution); - grid_poly.outer().emplace_back(origin.x, origin.y); - bg::correct(grid_poly); - - auto findCommonCvPolygons = - [&](const auto & area2d, std::vector> & cv_polygons) -> void { - tier4_autoware_utils::Polygon2d area2d_poly; - for (const auto & p : area2d) { - area2d_poly.outer().emplace_back(p.x(), p.y()); - } - area2d_poly.outer().push_back(area2d_poly.outer().front()); - bg::correct(area2d_poly); - std::vector common_areas; - bg::intersection(area2d_poly, grid_poly, common_areas); - if (common_areas.empty()) { - return; - } - for (size_t i = 0; i < common_areas.size(); ++i) { - common_areas[i].outer().push_back(common_areas[i].outer().front()); - bg::correct(common_areas[i]); - } - for (const auto & common_area : common_areas) { - std::vector cv_polygon; - for (const auto & p : common_area.outer()) { - const int idx_x = static_cast((p.x() - origin.x) / resolution); - const int idx_y = static_cast((p.y() - origin.y) / resolution); - cv_polygon.emplace_back(idx_x, height - 1 - idx_y); - } - cv_polygons.push_back(cv_polygon); - } - }; - - // (1) prepare detection area mask - // attention: 255 - // non-attention: 0 - // NOTE: interesting area is set to 255 for later masking - cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); - std::vector> attention_area_cv_polygons; - for (const auto & attention_area : attention_areas) { - const auto area2d = lanelet::utils::to2D(attention_area); - findCommonCvPolygons(area2d, attention_area_cv_polygons); - } - for (const auto & poly : attention_area_cv_polygons) { - cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA); - } - // (1.1) - // reset adjacent_lanelets area to 0 on attention_mask - std::vector> adjacent_lane_cv_polygons; - for (const auto & adjacent_lanelet : adjacent_lanelets) { - const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); - findCommonCvPolygons(area2d, adjacent_lane_cv_polygons); - } - for (const auto & poly : adjacent_lane_cv_polygons) { - cv::fillPoly(attention_mask, poly, cv::Scalar(0), cv::LINE_AA); - } - - // (2) prepare unknown mask - // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] - // unknown: 255 - // not-unknown: 0 - cv::Mat unknown_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); - cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); - for (int x = 0; x < width; x++) { - for (int y = 0; y < height; y++) { - const int idx = y * width + x; - const unsigned char intensity = occ_grid.data.at(idx); - if ( - planner_param_.occlusion.free_space_max <= intensity && - intensity < planner_param_.occlusion.occupied_min) { - unknown_mask_raw.at(height - 1 - y, x) = 255; - } - } - } - // (2.1) apply morphologyEx - const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); - cv::morphologyEx( - unknown_mask_raw, unknown_mask, cv::MORPH_OPEN, - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); - - // (3) occlusion mask - static constexpr unsigned char OCCLUDED = 255; - static constexpr unsigned char BLOCKED = 127; - cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); - cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); - // re-use attention_mask - attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); - // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded - const auto & blocking_attention_objects = target_objects.parked_attention_objects; - for (const auto & blocking_attention_object : blocking_attention_objects) { - debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); - } - std::vector> blocking_polygons; - for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); - findCommonCvPolygons(obj_poly.outer(), blocking_polygons); - } - for (const auto & blocking_polygon : blocking_polygons) { - cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); - } - for (const auto & division : lane_divisions) { - bool blocking_vehicle_found = false; - for (const auto & point_it : division) { - const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); - if (!valid) continue; - if (blocking_vehicle_found) { - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - continue; - } - if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { - blocking_vehicle_found = true; - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - } - } - } - - // (4) extract occlusion polygons - const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; - const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; - const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; - const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y; - std::vector> contours; - cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); - std::vector> valid_contours; - for (const auto & contour : contours) { - if (contour.size() <= 2) { - continue; - } - std::vector approx_contour; - cv::approxPolyDP( - contour, approx_contour, - std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); - if (approx_contour.size() <= 2) continue; - // check area - const double poly_area = cv::contourArea(approx_contour); - if (poly_area < possible_object_area) continue; - // check bounding box size - const auto bbox = cv::minAreaRect(approx_contour); - if (const auto size = bbox.size; std::min(size.height, size.width) < - std::min(possible_object_bbox_x, possible_object_bbox_y) || - std::max(size.height, size.width) < - std::max(possible_object_bbox_x, possible_object_bbox_y)) { - continue; - } - valid_contours.push_back(approx_contour); - geometry_msgs::msg::Polygon polygon_msg; - geometry_msgs::msg::Point32 point_msg; - for (const auto & p : approx_contour) { - const double glob_x = (p.x + 0.5) * resolution + origin.x; - const double glob_y = (height - 0.5 - p.y) * resolution + origin.y; - point_msg.x = glob_x; - point_msg.y = glob_y; - point_msg.z = origin.z; - polygon_msg.points.push_back(point_msg); - } - debug_data_.occlusion_polygons.push_back(polygon_msg); - } - // (4.1) re-draw occluded cells using valid_contours - occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); - for (const auto & valid_contour : valid_contours) { - // NOTE: drawContour does not work well - cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); - } - - // (5) find distance - // (5.1) discretize path_ip with resolution for computational cost - LineString2d path_linestring; - path_linestring.emplace_back( - path_ip.points.at(lane_start_idx).point.pose.position.x, - path_ip.points.at(lane_start_idx).point.pose.position.y); - { - auto prev_path_linestring_point = path_ip.points.at(lane_start_idx).point.pose.position; - for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { - const auto path_linestring_point = path_ip.points.at(i).point.pose.position; - if ( - tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < - 1.0 /* rough tick for computational cost */) { - continue; - } - path_linestring.emplace_back(path_linestring_point.x, path_linestring_point.y); - prev_path_linestring_point = path_linestring_point; - } - } - - auto findNearestPointToProjection = []( - const lanelet::ConstLineString3d & division, - const Point2d & projection, const double dist_thresh) { - double min_dist = std::numeric_limits::infinity(); - auto nearest = division.end(); - for (auto it = division.begin(); it != division.end(); it++) { - const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); - if (dist < min_dist) { - min_dist = dist; - nearest = it; - } - if (dist < dist_thresh) { - break; - } - } - return nearest; - }; - struct NearestOcclusionPoint - { - int64 division_index{0}; - int64 point_index{0}; - double dist{0.0}; - geometry_msgs::msg::Point point; - geometry_msgs::msg::Point projection; - } nearest_occlusion_point; - double min_dist = std::numeric_limits::infinity(); - for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { - const auto & division = lane_divisions.at(division_index); - LineString2d division_linestring; - auto division_point_it = division.begin(); - division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); - for (auto point_it = division.begin(); point_it != division.end(); point_it++) { - if ( - std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < - 3.0 /* rough tick for computational cost */) { - continue; - } - division_linestring.emplace_back(point_it->x(), point_it->y()); - division_point_it = point_it; - } - - // find the intersection point of lane_line and path - std::vector intersection_points; - boost::geometry::intersection(division_linestring, path_linestring, intersection_points); - if (intersection_points.empty()) { - continue; - } - const auto & projection_point = intersection_points.at(0); - const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); - if (projection_it == division.end()) { - continue; - } - double acc_dist = 0.0; - auto acc_dist_it = projection_it; - for (auto point_it = projection_it; point_it != division.end(); point_it++) { - const double dist = - std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); - acc_dist += dist; - acc_dist_it = point_it; - const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); - if (!valid) continue; - const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); - if (pixel == BLOCKED) { - break; - } - if (pixel == OCCLUDED) { - if (acc_dist < min_dist) { - min_dist = acc_dist; - nearest_occlusion_point = { - division_index, std::distance(division.begin(), point_it), acc_dist, - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; - } - } - } - } - - if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { - return OcclusionType::NOT_OCCLUDED; - } - - debug_data_.nearest_occlusion_projection = - std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - LineString2d ego_occlusion_line; - ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); - ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); - for (const auto & attention_object : target_objects.all_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - for (const auto & attention_object : target_objects.intersection_area_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - return OcclusionType::STATICALLY_OCCLUDED; -} - -static std::optional> -getFirstPointInsidePolygonsByFootprint( - const std::vector & polygons, - const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - - for (size_t i = start; i <= lane_end; ++i) { - const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); - for (size_t j = 0; j < polygons.size(); ++j) { - const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); - const bool is_in_polygon = bg::intersects(area_2d, path_footprint); - if (is_in_polygon) { - return std::make_optional>(i, j); - } - } - } - return std::nullopt; -} - -void IntersectionLanelets::update( - const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, - lanelet::routing::RoutingGraphPtr routing_graph_ptr) -{ - is_prioritized_ = is_prioritized; - // find the first conflicting/detection area polygon intersecting the path - if (!first_conflicting_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - conflicting_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_conflicting_lane_ = conflicting_.at(first.value().second); - first_conflicting_area_ = conflicting_area_.at(first.value().second); - } - } - if (!first_attention_area_) { - const auto first = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_attention_lane_ = attention_non_preceding_.at(first.value().second); - first_attention_area_ = attention_non_preceding_area_.at(first.value().second); - } - } - if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { - const auto first_attention_lane = first_attention_lane_.value(); - // remove first_attention_area_ and non-straight lanelets from attention_non_preceding - lanelet::ConstLanelets attention_non_preceding_ex_first; - lanelet::ConstLanelets sibling_first_attention_lanelets; - for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { - for (const auto & following : routing_graph_ptr->following(previous)) { - sibling_first_attention_lanelets.push_back(following); - } - } - for (const auto & ll : attention_non_preceding_) { - // the sibling lanelets of first_attention_lanelet are ruled out - if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { - continue; - } - if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { - attention_non_preceding_ex_first.push_back(ll); - } - } - if (attention_non_preceding_ex_first.empty()) { - second_attention_lane_empty_ = true; - } - const auto attention_non_preceding_ex_first_area = - getPolygon3dFromLanelets(attention_non_preceding_ex_first); - const auto second = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); - if (second) { - second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); - second_attention_area_ = second_attention_lane_.value().polygon3d(); - } + return intersection::Result>::make_ok( + intersection::Indecisive{"over the pass judge line. no plan needed"}); } + return intersection::Result>::make_err( + std::make_pair(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line)); } void TargetObject::calc_dist_to_stopline() diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 1a9cf74624fc0..8a34aabe8b918 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -15,7 +15,11 @@ #ifndef SCENE_INTERSECTION_HPP_ #define SCENE_INTERSECTION_HPP_ -#include "util_type.hpp" +#include "decision_result.hpp" +#include "interpolated_path_info.hpp" +#include "intersection_lanelets.hpp" +#include "intersection_stoplines.hpp" +#include "result.hpp" #include #include @@ -26,271 +30,21 @@ #include #include -#include -#include +#include +#include +#include -#include -#include #include +#include #include #include +#include #include -#include #include namespace behavior_velocity_planner { -using TimeDistanceArray = std::vector>; - -struct DebugData -{ - std::optional collision_stop_wall_pose{std::nullopt}; - std::optional occlusion_stop_wall_pose{std::nullopt}; - std::optional occlusion_first_stop_wall_pose{std::nullopt}; - std::optional first_pass_judge_wall_pose{std::nullopt}; - bool passed_first_pass_judge{false}; - bool passed_second_pass_judge{false}; - std::optional second_pass_judge_wall_pose{std::nullopt}; - std::optional> attention_area{std::nullopt}; - std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; - std::optional> adjacent_area{std::nullopt}; - std::optional first_attention_area{std::nullopt}; - std::optional second_attention_area{std::nullopt}; - std::optional stuck_vehicle_detect_area{std::nullopt}; - std::optional> yield_stuck_detect_area{std::nullopt}; - std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; - std::vector occlusion_polygons; - std::optional> - nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; - std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; -}; - -/** - * @struct - * @brief see the document for more details of IntersectionLanelets - */ -struct IntersectionLanelets -{ -public: - /** - * update conflicting lanelets and traffic priority information - */ - void update( - const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, - lanelet::routing::RoutingGraphPtr routing_graph_ptr); - - const lanelet::ConstLanelets & attention() const - { - return is_prioritized_ ? attention_non_preceding_ : attention_; - } - const std::vector> & attention_stoplines() const - { - return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; - } - const lanelet::ConstLanelets & conflicting() const { return conflicting_; } - const lanelet::ConstLanelets & adjacent() const { return adjacent_; } - const lanelet::ConstLanelets & occlusion_attention() const - { - return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; - } - const lanelet::ConstLanelets & attention_non_preceding() const - { - return attention_non_preceding_; - } - const std::vector & attention_area() const - { - return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; - } - const std::vector & conflicting_area() const - { - return conflicting_area_; - } - const std::vector & adjacent_area() const { return adjacent_area_; } - const std::vector & occlusion_attention_area() const - { - return occlusion_attention_area_; - } - const std::optional & first_conflicting_lane() const - { - return first_conflicting_lane_; - } - const std::optional & first_conflicting_area() const - { - return first_conflicting_area_; - } - const std::optional & first_attention_lane() const - { - return first_attention_lane_; - } - const std::optional & first_attention_area() const - { - return first_attention_area_; - } - const std::optional & second_attention_lane() const - { - return second_attention_lane_; - } - const std::optional & second_attention_area() const - { - return second_attention_area_; - } - - /** - * the set of attention lanelets which is topologically merged - */ - lanelet::ConstLanelets attention_; - std::vector attention_area_; - - /** - * the stop lines for each attention_ lanelets - */ - std::vector> attention_stoplines_; - - /** - * the conflicting part of attention lanelets - */ - lanelet::ConstLanelets attention_non_preceding_; - std::vector attention_non_preceding_area_; - - /** - * the stop lines for each attention_non_preceding_ - */ - std::vector> attention_non_preceding_stoplines_; - - /** - * the conflicting lanelets of the objective intersection lanelet - */ - lanelet::ConstLanelets conflicting_; - std::vector conflicting_area_; - - /** - * - */ - lanelet::ConstLanelets adjacent_; - std::vector adjacent_area_; - - /** - * the set of attention lanelets for occlusion detection which is topologically merged - */ - lanelet::ConstLanelets occlusion_attention_; - std::vector occlusion_attention_area_; - - /** - * the vector of sum of each occlusion_attention lanelet - */ - std::vector occlusion_attention_size_; - - /** - * the first conflicting lanelet which ego path points intersect for the first time - */ - std::optional first_conflicting_lane_{std::nullopt}; - std::optional first_conflicting_area_{std::nullopt}; - - /** - * the first attention lanelet which ego path points intersect for the first time - */ - std::optional first_attention_lane_{std::nullopt}; - std::optional first_attention_area_{std::nullopt}; - - /** - * the second attention lanelet which ego path points intersect next to the - * first_attention_lanelet - */ - bool second_attention_lane_empty_{false}; - std::optional second_attention_lane_{std::nullopt}; - std::optional second_attention_area_{std::nullopt}; - - /** - * flag if the intersection is prioritized by the traffic light - */ - bool is_prioritized_{false}; -}; - -/** - * @struct - * @brief see the document for more details of IntersectionStopLines - */ -struct IntersectionStopLines -{ - size_t closest_idx{0}; - - /** - * stuck_stopline is null if ego path does not intersect with first_conflicting_area - */ - std::optional stuck_stopline{std::nullopt}; - - /** - * default_stopline is null if it is calculated negative from first_attention_stopline - */ - std::optional default_stopline{std::nullopt}; - - /** - * first_attention_stopline is null if ego footprint along the path does not intersect with - * attention area. if path[0] satisfies the condition, it is 0 - */ - std::optional first_attention_stopline{std::nullopt}; - - /** - * second_attention_stopline is null if ego footprint along the path does not intersect with - * second_attention_lane. if path[0] satisfies the condition, it is 0 - */ - std::optional second_attention_stopline{std::nullopt}; - - /** - * occlusion_peeking_stopline is null if path[0] is already inside the attention area - */ - std::optional occlusion_peeking_stopline{std::nullopt}; - - /** - * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value - * is calculated negative, it is 0 - */ - size_t first_pass_judge_line{0}; - - /** - * second_pass_judge_line is before second_attention_stopline by the braking distance. if - * second_attention_lane is null, it is same as first_pass_judge_line - */ - size_t second_pass_judge_line{0}; - - /** - * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with - * the centerline of the first_attention_lane - */ - size_t occlusion_wo_tl_pass_judge_line{0}; -}; - -/** - * @struct - * @brief see the document for more details of PathLanelets - */ -struct PathLanelets -{ - lanelet::ConstLanelets prev; - // lanelet::ConstLanelet entry2ego; this is included in `all` if exists - lanelet::ConstLanelet - ego_or_entry2exit; // this is `assigned lane` part of the path(not from - // ego) if ego is before the intersection, otherwise from ego to exit - std::optional next = - std::nullopt; // this is nullopt is the goal is inside intersection - lanelet::ConstLanelets all; - lanelet::ConstLanelets - conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with - // conflicting lanelets plus the next lane part of the - // path -}; - /** * @struct * @brief see the document for more details of IntersectionStopLines @@ -317,19 +71,6 @@ struct TargetObjects std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy }; -/** - * @struct - * @brief categorize traffic light priority - */ -enum class TrafficPrioritizedLevel { - //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. - FULLY_PRIORITIZED = 0, - //! The target lane's traffic signal is amber - PARTIALLY_PRIORITIZED, - //! The target lane's traffic signal is green - NOT_PRIORITIZED -}; - class IntersectionModule : public SceneModuleInterface { public: @@ -457,144 +198,77 @@ class IntersectionModule : public SceneModuleInterface RTC_OCCLUDED, }; - /** - * @struct - * @brief Internal error or ego already passed pass_judge_line - */ - struct Indecisive - { - std::string error; - }; - /** - * @struct - * @brief detected stuck vehicle - */ - struct StuckStop - { - size_t closest_idx{0}; - size_t stuck_stopline_idx{0}; - std::optional occlusion_stopline_idx{std::nullopt}; - }; - /** - * @struct - * @brief yielded by vehicle on the attention area - */ - struct YieldStuckStop - { - size_t closest_idx{0}; - size_t stuck_stopline_idx{0}; - }; - /** - * @struct - * @brief only collision is detected - */ - struct NonOccludedCollisionStop - { - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - }; - /** - * @struct - * @brief occlusion is detected so ego needs to stop at the default stop line position - */ - struct FirstWaitBeforeOcclusion - { - bool is_actually_occlusion_cleared{false}; - size_t closest_idx{0}; - size_t first_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - }; - /** - * @struct - * @brief ego is approaching the boundary of attention area in the presence of traffic light - */ - struct PeekingTowardOcclusion + struct DebugData { - //! if intersection_occlusion is disapproved externally through RTC, it indicates - //! "is_forcefully_occluded" - bool is_actually_occlusion_cleared{false}; - bool temporal_stop_before_attention_required{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t first_attention_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it - //! contains the remaining time to release the static occlusion stuck and shows up - //! intersection_occlusion(x.y) - std::optional static_occlusion_timeout{std::nullopt}; - }; - /** - * @struct - * @brief both collision and occlusion are detected in the presence of traffic light - */ - struct OccludedCollisionStop - { - bool is_actually_occlusion_cleared{false}; - bool temporal_stop_before_attention_required{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t first_attention_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it - //! contains the remaining time to release the static occlusion stuck - std::optional static_occlusion_timeout{std::nullopt}; - }; - /** - * @struct - * @brief at least occlusion is detected in the absence of traffic light - */ - struct OccludedAbsenceTrafficLight - { - bool is_actually_occlusion_cleared{false}; - bool collision_detected{false}; - bool temporal_stop_before_attention_required{false}; - size_t closest_idx{0}; - size_t first_attention_area_stopline_idx{0}; - size_t peeking_limit_line_idx{0}; - }; - /** - * @struct - * @brief both collision and occlusion are not detected - */ - struct Safe - { - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; + std::optional collision_stop_wall_pose{std::nullopt}; + std::optional occlusion_stop_wall_pose{std::nullopt}; + std::optional occlusion_first_stop_wall_pose{std::nullopt}; + std::optional first_pass_judge_wall_pose{std::nullopt}; + bool passed_first_pass_judge{false}; + bool passed_second_pass_judge{false}; + std::optional second_pass_judge_wall_pose{std::nullopt}; + std::optional> attention_area{std::nullopt}; + std::optional> occlusion_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; + std::optional> adjacent_area{std::nullopt}; + std::optional first_attention_area{std::nullopt}; + std::optional second_attention_area{std::nullopt}; + std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; + std::vector candidate_collision_object_polygons; + autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + std::vector occlusion_polygons; + std::optional> + nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; + + using TimeDistanceArray = std::vector>; + /** * @struct - * @brief traffic light is red or arrow signal - */ - struct FullyPrioritized - { - bool collision_detected{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; + * @brief categorize traffic light priority + */ + enum class TrafficPrioritizedLevel { + //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + //! The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + //! The target lane's traffic signal is green + NOT_PRIORITIZED }; - using DecisionResult = std::variant< - Indecisive, //! internal process error, or over the pass judge line - StuckStop, //! detected stuck vehicle - YieldStuckStop, //! detected yield stuck vehicle - NonOccludedCollisionStop, //! detected collision while FOV is clear - FirstWaitBeforeOcclusion, //! stop for a while before peeking to occlusion - PeekingTowardOcclusion, //! peeking into occlusion while collision is not detected - OccludedCollisionStop, //! occlusion and collision are both detected - OccludedAbsenceTrafficLight, //! occlusion is detected in the absence of traffic light - Safe, //! judge as safe - FullyPrioritized //! only detect vehicles violating traffic rules - >; + /** @} */ IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); - + const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup primary-function [fn] primary functions + * the entrypoint of this module is modifyPathVelocity() function that calculates safety decision + * of latest context and send it to RTC and then react to RTC approval. The reaction to RTC + * approval may not be based on the latest decision of this module depending on the auto-mode + * configuration. For module side it is not visible if the module is operating in auto-mode or + * manual-module. At first, initializeRTCStatus() is called to reset the safety value of + * INTERSECTION and INTERSECTION_OCCLUSION. Then modifyPathVelocityDetail() is called to analyze + * the context. Then prepareRTCStatus() is called to set the safety value of INTERSECTION and + * INTERSECTION_OCCLUSION. + * @{ + */ bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + /** @}*/ visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; motion_utils::VirtualWalls createVirtualWalls() override; @@ -609,137 +283,421 @@ class IntersectionModule : public SceneModuleInterface private: rclcpp::Node & node_; + + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup const-variables [var] const variables + * following variables are unique to this intersection lanelet or to this module + * @{ + */ + //! lanelet of this intersection const lanelet::Id lane_id_; + + //! associative(sibling) lanelets ids const std::set associative_ids_; + + //! turn_direction of this lane const std::string turn_direction_; + + //! flag if this intersection is traffic controlled const bool has_traffic_light_; - // Parameter + //! RTC uuid for INTERSECTION_OCCLUSION + const UUID occlusion_uuid_; + /** @}*/ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup semi-const-variables [var] semi-const variables + * following variables are immutable once initialized + * @{ + */ PlannerParam planner_param_; - std::optional intersection_lanelets_{std::nullopt}; + //! cache IntersectionLanelets struct + std::optional intersection_lanelets_{std::nullopt}; - // for pass judge decision - bool is_go_out_{false}; + //! cache discretized occlusion detection lanelets + std::optional> occlusion_attention_divisions_{ + std::nullopt}; + /** @}*/ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup pass-judge-variable [var] pass judge variables + * following variables are state variables that depends on how the vehicle passed the intersection + * @{ + */ + //! if true, this module never commands to STOP anymore bool is_permanent_go_{false}; - DecisionResult prev_decision_result_{Indecisive{""}}; - OcclusionType prev_occlusion_status_; + + //! for checking if ego is over the pass judge lines because previously the situation was SAFE + intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}}; + + //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line + //! is treated as the same position as occlusion_peeking_stopline bool passed_1st_judge_line_while_peeking_{false}; + + //! save the time when ego passed the 1st/2nd_pass_judge_line with safe decision. If collision is + //! expected after these variables are non-null, then it is the fault of past perception failure + //! at these time. std::optional safely_passed_1st_judge_line_time_{std::nullopt}; std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; + /** @}*/ - // for occlusion detection - const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_{ - std::nullopt}; //! for caching discretized occlusion detection lanelets - StateMachine collision_state_machine_; //! for stable collision checking - StateMachine before_creep_state_machine_; //! for two phase stop - StateMachine occlusion_stop_state_machine_; //! for stable occlusion detection +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup collision-variables [var] collision detection + * @{ + */ + //! debouncing for stable SAFE decision + StateMachine collision_state_machine_; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup occlusion-variables [var] occlusion detection variables + * @{ + */ + OcclusionType prev_occlusion_status_; + + //! debouncing for the first brief stop at the default stopline + StateMachine before_creep_state_machine_; + + //! debouncing for stable CLEARED decision + StateMachine occlusion_stop_state_machine_; + + //! debouncing for the brief stop at the boundary of attention area(if required by the flag) StateMachine temporal_stop_before_attention_state_machine_; + + //! time counter for the stuck detection due to occlusion caused static objects StateMachine static_occlusion_timeout_state_machine_; + /** @} */ std::optional initial_green_light_observed_time_{std::nullopt}; - // for RTC - const UUID occlusion_uuid_; +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup RTC-variables [var] RTC variables + * + * intersection module has additional rtc_interface_ for INTERSECTION_OCCLUSION in addition to the + * default rtc_interface of SceneModuleManagerInterfaceWithRTC. activated_ is the derived member + * of this module which is updated by the RTC config/service, so it should be read-only in this + * module. occlusion_safety_ and occlusion_stop_distance_ are the corresponding RTC value for + * INTERSECTION_OCCLUSION. + * @{ + */ bool occlusion_safety_{true}; double occlusion_stop_distance_{0.0}; bool occlusion_activated_{true}; bool occlusion_first_stop_required_{false}; + /** @}*/ +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @ingroup primary-functions + * @{ + */ /** - * @fn - * @brief set all RTC variable to safe and -inf + * @brief set all RTC variable to true(safe) and -INF */ void initializeRTCStatus(); + /** - * @fn * @brief analyze traffic_light/occupancy/objects context and return DecisionResult */ - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + intersection::DecisionResult modifyPathVelocityDetail( + PathWithLaneId * path, StopReason * stop_reason); + /** - * @fn * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const DecisionResult &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const intersection::DecisionResult &, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path); /** - * @fn - * @brief find TrafficPrioritizedLevel + * @brief act based on current RTC approval */ - TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane); + void reactRTCApproval( + const intersection::DecisionResult & decision_result, + autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + /** @}*/ +private: /** - * @fn - * @brief generate IntersectionLanelets + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup prepare-data [fn] basic data construction + * @{ */ - IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path); + /** + * @struct + */ + struct BasicData + { + intersection::InterpolatedPathInfo interpolated_path_info; + intersection::IntersectionStopLines intersection_stoplines; + intersection::PathLanelets path_lanelets; + }; + + /** + * @brief prepare basic data structure + * @return return IntersectionStopLines if all data is valid, otherwise Indecisive + * @note if successful, it is ensure that intersection_lanelets_, + * intersection_lanelets.first_conflicting_lane are not null + * + * To simplify modifyPathVelocityDetail(), this function is used at first + */ + intersection::Result prepareIntersectionData( + const bool is_prioritized, PathWithLaneId * path); + + /** + * @brief find the associated stopline road marking of assigned lanelet + */ + std::optional getStopLineIndexFromMap( + const intersection::InterpolatedPathInfo & interpolated_path_info, + lanelet::ConstLanelet assigned_lanelet); /** - * @fn * @brief generate IntersectionStopLines */ - std::optional generateIntersectionStopLines( + std::optional generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, - const util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); /** - * @fn - * @brief find the associated stopline road marking of assigned lanelet + * @brief generate IntersectionLanelets */ - std::optional getStopLineIndexFromMap( - const util::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet); + intersection::IntersectionLanelets generateObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet); /** - * @fn * @brief generate PathLanelets */ - std::optional generatePathLanelets( + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, const std::vector & attention_areas, const size_t closest_idx); /** - * @fn - * @brief check stuck + * @brief generate discretized detection lane linestring. */ - bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets, DebugData * debug_data); + std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + /** @} */ +private: /** - * @fn - * @brief check yield stuck + * @defgroup utility [fn] utility member function + * @{ */ - bool checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data); + void stoppedAtPositionForDuration( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t position, + const double duration, StateMachine * state_machine); + /** @} */ +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup get-traffic-light [fn] traffic light + * @{ + */ + /** + * @brief check if associated traffic light is green + */ + bool isGreenSolidOn() const; + + /** + * @brief find TrafficPrioritizedLevel + */ + TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; + /** @} */ + +private: /** - * @fn * @brief categorize target objects */ TargetObjects generateTargetObjects( - const IntersectionLanelets & intersection_lanelets, + const intersection::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup yield [fn] check stuck + * @{ + */ + /** + * @brief check stuck status + * @attention this function has access to value() of intersection_lanelets_, + * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() + */ + std::optional isStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::IntersectionStopLines & intersection_stoplines, + const intersection::PathLanelets & path_lanelets) const; + + bool isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief check stuck + */ + bool checkStuckVehicleInIntersection(const intersection::PathLanelets & path_lanelets) const; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup yield [fn] check yield stuck + * @{ + */ + /** + * @brief check yield stuck status + * @attention this function has access to value() of intersection_lanelets_, + * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline + */ + std::optional isYieldStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const intersection::IntersectionStopLines & intersection_stoplines, + const TargetObjects & target_objects) const; + + /** + * @brief check yield stuck + */ + bool checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets) const; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup occlusion [fn] check occlusion + * @{ + */ + /** + * @brief check occlusion status + * @attention this function has access to value() of occlusion_attention_divisions_, + * intersection_lanelets.first_attention_area() + */ + std::tuple< + OcclusionType, bool /* module detection with margin */, + bool /* reconciled occlusion disapproval */> + getOcclusionStatus( + const TrafficPrioritizedLevel & traffic_prioritized_level, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const intersection::IntersectionLanelets & intersection_lanelets, + const TargetObjects & target_objects); + + /** + * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) + */ + OcclusionType detectOcclusion( + const std::vector & attention_areas, + const lanelet::ConstLanelets & adjacent_lanelets, + const lanelet::CompoundPolygon3d & first_attention_area, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const std::vector & lane_divisions, + const TargetObjects & target_objects); + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup pass-judge-decision [fn] pass judge decision + * @{ + */ + /** + * @brief check if ego is already over the pass judge line + * @return if ego is over both 1st/2nd pass judge lines, return Indecisive, else return + * (is_over_1st_pass_judge, is_over_2nd_pass_judge) + * @attention this function has access to value() of intersection_stoplines.default_stopline, + * intersection_stoplines.occlusion_stopline + */ + intersection::Result< + intersection::Indecisive, + std::pair> + isOverPassJudgeLinesStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const intersection::IntersectionStopLines & intersection_stoplines); + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup collision-detection [fn] check collision + * @{ + */ + bool isTargetCollisionVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief check if there are any objects around the stoplines on the attention areas when ego + * entered the intersection on green light + * @return return NonOccludedCollisionStop if there are vehicle within the margin for some + * duration from ego's entry to yield + * @attention this function has access to value() of + * intersection_stoplines.occlusion_peeking_stopline + */ + std::optional isGreenPseudoCollisionStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines, + const TargetObjects & target_objects); + /** - * @fn * @brief check collision */ bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const PathLanelets & path_lanelets, const size_t closest_idx, + const intersection::PathLanelets & path_lanelets, const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, const double time_delay, const TrafficPrioritizedLevel & traffic_prioritized_level); @@ -750,7 +708,6 @@ class IntersectionModule : public SceneModuleInterface void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); /** - * @fn * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of * (time of arrival, traveled distance) from current ego position */ @@ -758,28 +715,7 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, const double time_delay, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); - - std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); - - /** - * @fn - * @brief check occlusion status - */ - OcclusionType getOcclusionStatus( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects); - - /* - * @fn - * @brief check if associated traffic light is green - */ - bool isGreenSolidOn(lanelet::ConstLanelet lane); + /** @} */ /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -789,7 +725,7 @@ class IntersectionModule : public SceneModuleInterface const double assumed_front_car_decel); */ - DebugData debug_data_; + mutable DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; rclcpp::Publisher::SharedPtr object_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp new file mode 100644 index 0000000000000..43bb68cf56f67 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -0,0 +1,582 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 "scene_intersection.hpp" +#include "util.hpp" + +#include // for toGeomPoly +#include // for smoothPath +#include +#include +#include // for toPolygon2d +#include + +#include +#include + +#include + +namespace +{ +tier4_autoware_utils::Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + namespace bg = boost::geometry; + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + tier4_autoware_utils::Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + tier4_autoware_utils::Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} +} // namespace + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +bool IntersectionModule::isTargetCollisionVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) { + return true; + } + return false; +} + +std::optional +IntersectionModule::isGreenPseudoCollisionStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines, + const TargetObjects & target_objects) +{ + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + // If there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because the vehicles are very slow and no collisions may + // be detected. check if ego vehicle entered assigned lanelet + const bool is_green_solid_on = isGreenSolidOn(); + if (!is_green_solid_on) { + return std::nullopt; + } + const auto closest_idx = intersection_stoplines.closest_idx; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); + if (!initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path.points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + if (initial_green_light_observed_time_) { + const auto now = clock_->now(); + const bool still_wait = + (rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < + planner_param_.collision_detection.yield_on_green_traffic_light.duration); + if (!still_wait) { + return std::nullopt; + } + const bool exist_close_vehicles = std::any_of( + target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), + [&](const auto & object) { + return object.dist_to_stopline.has_value() && + object.dist_to_stopline.value() < + planner_param_.collision_detection.yield_on_green_traffic_light + .object_dist_to_stopline; + }); + if (exist_close_vehicles) { + return intersection::NonOccludedCollisionStop{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + } + return std::nullopt; +} + +bool IntersectionModule::checkCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const intersection::PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getPolygonFromArcLength; + + // check collision between target_objects predicted path and ego lane + // cut the predicted path at passing_time + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = calcIntersectionPassingTime( + path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); + + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + ego_ttc_time_array.stamp = path.header.stamp; + ego_ttc_pub_->publish(ego_ttc_time_array); + } + + const double passing_time = time_distance_array.back().first; + cutPredictPathWithDuration(target_objects, passing_time); + + const auto & concat_lanelets = path_lanelets.all; + const auto closest_arc_coords = getArcCoordinates( + concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + // change TTC margin based on ego traffic light color + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); + const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { + if (!target_object.dist_to_stopline) { + return false; + } + const double dist_to_stopline = target_object.dist_to_stopline.value(); + if (dist_to_stopline < 0) { + return false; + } + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + return dist_to_stopline > braking_distance; + }; + const auto isTolerableOvershoot = [&](const TargetObject & target_object) { + if ( + !target_object.attention_lanelet || !target_object.dist_to_stopline || + !target_object.stopline) { + return false; + } + const double dist_to_stopline = target_object.dist_to_stopline.value(); + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + if (dist_to_stopline > braking_distance) { + return false; + } + const auto stopline_front = target_object.stopline.value().front(); + const auto stopline_back = target_object.stopline.value().back(); + tier4_autoware_utils::LineString2d object_line; + object_line.emplace_back( + (stopline_front.x() + stopline_back.x()) / 2.0, + (stopline_front.y() + stopline_back.y()) / 2.0); + const auto stopline_mid = object_line.front(); + const auto endpoint = target_object.attention_lanelet.value().centerline().back(); + object_line.emplace_back(endpoint.x(), endpoint.y()); + std::vector intersections; + bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); + if (intersections.empty()) { + return false; + } + const auto collision_point = intersections.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; + const double stopline_to_collision = + std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); + const double object2collision = stopline_to_collision - stopline_to_object; + const double margin = + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; + return (object2collision > margin) || (object2collision < 0); + }; + // check collision between predicted_path and ego_area + tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + object_ttc_time_array.layout.dim.resize(3); + object_ttc_time_array.layout.dim.at(0).label = "objects"; + object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id + object_ttc_time_array.layout.dim.at(1).label = + "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " + "start_time, start_dist, " + "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " + "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; + object_ttc_time_array.layout.dim.at(1).size = 57; + for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { + object_ttc_time_array.data.push_back(lane_id_); + } + bool collision_detected = false; + for (const auto & target_object : target_objects->all_attention_objects) { + const auto & object = target_object.object; + // If the vehicle is expected to stop before their stopline, ignore + const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + expected_to_stop_before_stopline) { + debug_data_.amber_ignore_targets.objects.push_back(object); + continue; + } + const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && + !expected_to_stop_before_stopline && is_tolerable_overshoot) { + debug_data_.red_overshoot_ignore_targets.objects.push_back(object); + continue; + } + for (const auto & predicted_path : object.kinematics.predicted_paths) { + if ( + predicted_path.confidence < + planner_param_.collision_detection.min_predicted_path_confidence) { + // ignore the predicted path with too low confidence + continue; + } + + // collision point + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, ::createOneStepPolygon(a, b, object.shape)); + }); + if (first_itr == predicted_path.path.cend()) continue; + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, ::createOneStepPolygon(a, b, object.shape)); + }); + if (last_itr == predicted_path.path.crend()) continue; + + // possible collision time interval + const double ref_object_enter_time = + static_cast(first_itr - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto start_time_distance_itr = time_distance_array.begin(); + if (ref_object_enter_time - collision_start_margin_time > 0) { + // start of possible ego position in the intersection + start_time_distance_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + ref_object_enter_time - collision_start_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (start_time_distance_itr == time_distance_array.end()) { + // ego is already at the exit of intersection when npc is at collision point even if npc + // accelerates so ego's position interval is empty + continue; + } + } + const double ref_object_exit_time = + static_cast(last_itr.base() - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto end_time_distance_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + ref_object_exit_time + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (end_time_distance_itr == time_distance_array.end()) { + // ego is already passing the intersection, when npc is is at collision point + // so ego's position interval is up to the end of intersection lane + end_time_distance_itr = time_distance_array.end() - 1; + } + const double start_arc_length = std::max( + 0.0, closest_arc_coords.length + (*start_time_distance_itr).second - + planner_data_->vehicle_info_.rear_overhang_m); + const double end_arc_length = std::min( + closest_arc_coords.length + (*end_time_distance_itr).second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + + const auto trimmed_ego_polygon = + getPolygonFromArcLength(concat_lanelets, start_arc_length, end_arc_length); + + if (trimmed_ego_polygon.empty()) { + continue; + } + + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + bg::correct(polygon); + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + + for (auto itr = first_itr; itr != last_itr.base(); ++itr) { + const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); + if (bg::intersects(polygon, footprint_polygon)) { + collision_detected = true; + break; + } + } + object_ttc_time_array.layout.dim.at(0).size++; + const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & shape = object.shape; + object_ttc_time_array.data.insert( + object_ttc_time_array.data.end(), + {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), + shape.dimensions.x, shape.dimensions.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x, + 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, + start_time_distance_itr->first, start_time_distance_itr->second, + end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, + first_itr->position.y, last_itr->position.x, last_itr->position.y}); + for (unsigned i = 0; i < 20; i++) { + const auto & pos = + predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; + object_ttc_time_array.data.push_back(pos.x); + object_ttc_time_array.data.push_back(pos.y); + } + if (collision_detected) { + debug_data_.conflicting_targets.objects.push_back(object); + break; + } + } + } + + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + object_ttc_time_array.stamp = path.header.stamp; + object_ttc_pub_->publish(object_ttc_time_array); + } + + return collision_detected; +} + +std::optional IntersectionModule::checkAngleForTargetLanelets( + + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const +{ + const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; + const bool consider_wrong_direction_vehicle = + planner_param_.common.attention_area_angle_threshold; + const double dist_margin = planner_param_.common.attention_area_margin; + + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { + continue; + } + const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); + const double pose_angle = tf2::getYaw(pose.orientation); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + if (consider_wrong_direction_vehicle) { + if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + } else { + if (std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); + } + } + } + return std::nullopt; +} + +void IntersectionModule::cutPredictPathWithDuration( + TargetObjects * target_objects, const double time_thr) +{ + const rclcpp::Time current_time = clock_->now(); + for (auto & target_object : target_objects->all_attention_objects) { // each objects + for (auto & predicted_path : + target_object.object.kinematics.predicted_paths) { // each predicted paths + const auto origin_path = predicted_path; + predicted_path.path.clear(); + + for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points + const auto & predicted_pose = origin_path.path.at(k); + const auto predicted_time = + rclcpp::Time(target_objects->header.stamp) + + rclcpp::Duration(origin_path.time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + predicted_path.path.push_back(predicted_pose); + } + } + } + } +} + +IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) +{ + const double intersection_velocity = + planner_param_.collision_detection.velocity_profile.default_velocity; + const double minimum_ego_velocity = + planner_param_.collision_detection.velocity_profile.minimum_default_velocity; + const bool use_upstream_velocity = + planner_param_.collision_detection.velocity_profile.use_upstream; + const double minimum_upstream_velocity = + planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; + const double current_velocity = planner_data_->current_velocity->twist.linear.x; + + int assigned_lane_found = false; + + // crop intersection part of the path, and set the reference velocity to intersection_velocity + // for ego's ttc + PathWithLaneId reference_path; + std::optional upstream_stopline{std::nullopt}; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + auto reference_point = path.points.at(i); + // assume backward velocity is current ego velocity + if (i < closest_idx) { + reference_point.point.longitudinal_velocity_mps = current_velocity; + } + if ( + i > last_intersection_stopline_candidate_idx && + std::fabs(reference_point.point.longitudinal_velocity_mps) < + std::numeric_limits::epsilon() && + !upstream_stopline) { + upstream_stopline = i; + } + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } + reference_path.points.push_back(reference_point); + bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); + if (assigned_lane_found && !has_objective_lane_id) { + break; + } + assigned_lane_found = has_objective_lane_id; + } + if (!assigned_lane_found) { + return {{0.0, 0.0}}; // has already passed the intersection. + } + + std::vector> original_path_xy; + for (size_t i = 0; i < reference_path.points.size(); ++i) { + const auto & p = reference_path.points.at(i).point.pose.position; + original_path_xy.emplace_back(p.x, p.y); + } + + // apply smoother to reference velocity + PathWithLaneId smoothed_reference_path = reference_path; + if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { + smoothed_reference_path = reference_path; + } + + // calculate when ego is going to reach each (interpolated) points on the path + TimeDistanceArray time_distance_array{}; + double dist_sum = 0.0; + double passing_time = time_delay; + time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stopline_candidate_idx` makes no sense + const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + + const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { + if (upstream_stopline) { + const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, upstream_stopline_point, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + } else { + return std::nullopt; + } + }(); + + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { + const auto & p1 = smoothed_reference_path.points.at(i); + const auto & p2 = smoothed_reference_path.points.at(i + 1); + + const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); + dist_sum += dist; + + // use average velocity between p1 and p2 + const double average_velocity = + (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; + const double passing_velocity = [=]() { + if (use_upstream_velocity) { + if (upstream_stopline_idx_opt && i > upstream_stopline_idx_opt.value()) { + return minimum_upstream_velocity; + } + return std::max(average_velocity, minimum_ego_velocity); + } else { + return std::max(average_velocity, minimum_ego_velocity); + } + }(); + passing_time += (dist / passing_velocity); + + time_distance_array.emplace_back(passing_time, dist_sum); + } + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 5; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + debug_ttc_array->data.reserve( + time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id_); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(d); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.x); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.y); + } + return time_distance_array; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp new file mode 100644 index 0000000000000..353826070eff7 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -0,0 +1,407 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 "scene_intersection.hpp" +#include "util.hpp" + +#include +#include // for toPolygon2d + +#include +#include + +#include + +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +std::tuple< + IntersectionModule::OcclusionType, bool /* module detection with margin */, + bool /* reconciled occlusion disapproval */> +IntersectionModule::getOcclusionStatus( + const TrafficPrioritizedLevel & traffic_prioritized_level, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const intersection::IntersectionLanelets & intersection_lanelets, + const TargetObjects & target_objects) +{ + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); + const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + const auto first_attention_area = intersection_lanelets.first_attention_area().value(); + + const bool is_amber_or_red = + (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || + (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); + // check occlusion on detection lane + const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); + auto occlusion_status = + (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) + ? detectOcclusion( + occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, + occlusion_attention_divisions, target_objects) + : OcclusionType::NOT_OCCLUDED; + occlusion_stop_state_machine_.setStateWithMarginTime( + occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); + const bool is_occlusion_cleared_with_margin = + (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // module's detection + // distinguish if ego detected occlusion or RTC detects occlusion + const bool ext_occlusion_requested = + (is_occlusion_cleared_with_margin && !occlusion_activated_); // RTC's detection + if (ext_occlusion_requested) { + occlusion_status = OcclusionType::RTC_OCCLUDED; + } + const bool is_occlusion_state = + (!is_occlusion_cleared_with_margin || ext_occlusion_requested); // including approval + if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + occlusion_status = prev_occlusion_status_; + } else { + prev_occlusion_status_ = occlusion_status; + } + return {occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state}; +} + +IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( + const std::vector & attention_areas, + const lanelet::ConstLanelets & adjacent_lanelets, + const lanelet::CompoundPolygon3d & first_attention_area, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const std::vector & lane_divisions, + const TargetObjects & target_objects) +{ + const auto & occ_grid = *planner_data_->occupancy_grid; + const auto & current_pose = planner_data_->current_odometry->pose; + const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; + + const auto & path_ip = interpolated_path_info.path; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + + const auto first_attention_area_idx = + util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); + if (!first_attention_area_idx) { + return OcclusionType::NOT_OCCLUDED; + } + + const auto first_inside_attention_idx_ip_opt = + util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); + const std::pair lane_attention_interval_ip = + first_inside_attention_idx_ip_opt + ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) + : lane_interval_ip; + const auto [lane_start_idx, lane_end_idx] = lane_attention_interval_ip; + + const int width = occ_grid.info.width; + const int height = occ_grid.info.height; + const double resolution = occ_grid.info.resolution; + const auto & origin = occ_grid.info.origin.position; + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; + + Polygon2d grid_poly; + grid_poly.outer().emplace_back(origin.x, origin.y); + grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); + grid_poly.outer().emplace_back( + origin.x + (width - 1) * resolution, origin.y + (height - 1) * resolution); + grid_poly.outer().emplace_back(origin.x, origin.y + (height - 1) * resolution); + grid_poly.outer().emplace_back(origin.x, origin.y); + bg::correct(grid_poly); + + auto findCommonCvPolygons = + [&](const auto & area2d, std::vector> & cv_polygons) -> void { + tier4_autoware_utils::Polygon2d area2d_poly; + for (const auto & p : area2d) { + area2d_poly.outer().emplace_back(p.x(), p.y()); + } + area2d_poly.outer().push_back(area2d_poly.outer().front()); + bg::correct(area2d_poly); + std::vector common_areas; + bg::intersection(area2d_poly, grid_poly, common_areas); + if (common_areas.empty()) { + return; + } + for (size_t i = 0; i < common_areas.size(); ++i) { + common_areas[i].outer().push_back(common_areas[i].outer().front()); + bg::correct(common_areas[i]); + } + for (const auto & common_area : common_areas) { + std::vector cv_polygon; + for (const auto & p : common_area.outer()) { + const int idx_x = static_cast((p.x() - origin.x) / resolution); + const int idx_y = static_cast((p.y() - origin.y) / resolution); + cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + } + cv_polygons.push_back(cv_polygon); + } + }; + + // (1) prepare detection area mask + // attention: 255 + // non-attention: 0 + // NOTE: interesting area is set to 255 for later masking + cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); + std::vector> attention_area_cv_polygons; + for (const auto & attention_area : attention_areas) { + const auto area2d = lanelet::utils::to2D(attention_area); + findCommonCvPolygons(area2d, attention_area_cv_polygons); + } + for (const auto & poly : attention_area_cv_polygons) { + cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA); + } + // (1.1) + // reset adjacent_lanelets area to 0 on attention_mask + std::vector> adjacent_lane_cv_polygons; + for (const auto & adjacent_lanelet : adjacent_lanelets) { + const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); + findCommonCvPolygons(area2d, adjacent_lane_cv_polygons); + } + for (const auto & poly : adjacent_lane_cv_polygons) { + cv::fillPoly(attention_mask, poly, cv::Scalar(0), cv::LINE_AA); + } + + // (2) prepare unknown mask + // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] + // unknown: 255 + // not-unknown: 0 + cv::Mat unknown_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); + cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); + for (int x = 0; x < width; x++) { + for (int y = 0; y < height; y++) { + const int idx = y * width + x; + const unsigned char intensity = occ_grid.data.at(idx); + if ( + planner_param_.occlusion.free_space_max <= intensity && + intensity < planner_param_.occlusion.occupied_min) { + unknown_mask_raw.at(height - 1 - y, x) = 255; + } + } + } + // (2.1) apply morphologyEx + const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); + cv::morphologyEx( + unknown_mask_raw, unknown_mask, cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); + + // (3) occlusion mask + static constexpr unsigned char OCCLUDED = 255; + static constexpr unsigned char BLOCKED = 127; + cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); + cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // re-use attention_mask + attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + const auto & blocking_attention_objects = target_objects.parked_attention_objects; + for (const auto & blocking_attention_object : blocking_attention_objects) { + debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + } + std::vector> blocking_polygons; + for (const auto & blocking_attention_object : blocking_attention_objects) { + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); + findCommonCvPolygons(obj_poly.outer(), blocking_polygons); + } + for (const auto & blocking_polygon : blocking_polygons) { + cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); + } + for (const auto & division : lane_divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + } + } + } + + // (4) extract occlusion polygons + const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; + const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; + const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; + const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y; + std::vector> contours; + cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + std::vector> valid_contours; + for (const auto & contour : contours) { + if (contour.size() <= 2) { + continue; + } + std::vector approx_contour; + cv::approxPolyDP( + contour, approx_contour, + std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); + if (approx_contour.size() <= 2) continue; + // check area + const double poly_area = cv::contourArea(approx_contour); + if (poly_area < possible_object_area) continue; + // check bounding box size + const auto bbox = cv::minAreaRect(approx_contour); + if (const auto size = bbox.size; std::min(size.height, size.width) < + std::min(possible_object_bbox_x, possible_object_bbox_y) || + std::max(size.height, size.width) < + std::max(possible_object_bbox_x, possible_object_bbox_y)) { + continue; + } + valid_contours.push_back(approx_contour); + geometry_msgs::msg::Polygon polygon_msg; + geometry_msgs::msg::Point32 point_msg; + for (const auto & p : approx_contour) { + const double glob_x = (p.x + 0.5) * resolution + origin.x; + const double glob_y = (height - 0.5 - p.y) * resolution + origin.y; + point_msg.x = glob_x; + point_msg.y = glob_y; + point_msg.z = origin.z; + polygon_msg.points.push_back(point_msg); + } + debug_data_.occlusion_polygons.push_back(polygon_msg); + } + // (4.1) re-draw occluded cells using valid_contours + occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + for (const auto & valid_contour : valid_contours) { + // NOTE: drawContour does not work well + cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); + } + + // (5) find distance + // (5.1) discretize path_ip with resolution for computational cost + LineString2d path_linestring; + path_linestring.emplace_back( + path_ip.points.at(lane_start_idx).point.pose.position.x, + path_ip.points.at(lane_start_idx).point.pose.position.y); + { + auto prev_path_linestring_point = path_ip.points.at(lane_start_idx).point.pose.position; + for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { + const auto path_linestring_point = path_ip.points.at(i).point.pose.position; + if ( + tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < + 1.0 /* rough tick for computational cost */) { + continue; + } + path_linestring.emplace_back(path_linestring_point.x, path_linestring_point.y); + prev_path_linestring_point = path_linestring_point; + } + } + + auto findNearestPointToProjection = []( + const lanelet::ConstLineString3d & division, + const Point2d & projection, const double dist_thresh) { + double min_dist = std::numeric_limits::infinity(); + auto nearest = division.end(); + for (auto it = division.begin(); it != division.end(); it++) { + const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); + if (dist < min_dist) { + min_dist = dist; + nearest = it; + } + if (dist < dist_thresh) { + break; + } + } + return nearest; + }; + struct NearestOcclusionPoint + { + int64 division_index{0}; + int64 point_index{0}; + double dist{0.0}; + geometry_msgs::msg::Point point; + geometry_msgs::msg::Point projection; + } nearest_occlusion_point; + double min_dist = std::numeric_limits::infinity(); + for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { + const auto & division = lane_divisions.at(division_index); + LineString2d division_linestring; + auto division_point_it = division.begin(); + division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); + for (auto point_it = division.begin(); point_it != division.end(); point_it++) { + if ( + std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < + 3.0 /* rough tick for computational cost */) { + continue; + } + division_linestring.emplace_back(point_it->x(), point_it->y()); + division_point_it = point_it; + } + + // find the intersection point of lane_line and path + std::vector intersection_points; + boost::geometry::intersection(division_linestring, path_linestring, intersection_points); + if (intersection_points.empty()) { + continue; + } + const auto & projection_point = intersection_points.at(0); + const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); + if (projection_it == division.end()) { + continue; + } + double acc_dist = 0.0; + auto acc_dist_it = projection_it; + for (auto point_it = projection_it; point_it != division.end(); point_it++) { + const double dist = + std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); + acc_dist += dist; + acc_dist_it = point_it; + const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); + if (!valid) continue; + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; + } + if (pixel == OCCLUDED) { + if (acc_dist < min_dist) { + min_dist = acc_dist; + nearest_occlusion_point = { + division_index, std::distance(division.begin(), point_it), acc_dist, + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; + } + } + } + } + + if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { + return OcclusionType::NOT_OCCLUDED; + } + + debug_data_.nearest_occlusion_projection = + std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); + LineString2d ego_occlusion_line; + ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + for (const auto & attention_object : target_objects.all_attention_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + for (const auto & attention_object : target_objects.intersection_area_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + return OcclusionType::STATICALLY_OCCLUDED; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp new file mode 100644 index 0000000000000..746e615d8c6b0 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -0,0 +1,869 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 "scene_intersection.hpp" +#include "util.hpp" + +#include // for to_bg2d +#include // for planning_utils:: +#include +#include // for lanelet::autoware::RoadMarking +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +template <> +inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + return point; +} + +} // namespace tier4_autoware_utils + +namespace +{ +namespace bg = boost::geometry; + +lanelet::ConstLanelets getPrevLanelets( + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) +{ + lanelet::ConstLanelets previous_lanelets; + for (const auto & ll : lanelets_on_path) { + if (associative_ids.find(ll.id()) != associative_ids.end()) { + return previous_lanelets; + } + previous_lanelets.push_back(ll); + } + return previous_lanelets; +} + +// end inclusive +lanelet::ConstLanelet generatePathLanelet( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const size_t end_idx, const double width, const double interval) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + size_t prev_idx = start_idx; + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto & p = path.points.at(i).point.pose; + const auto & p_prev = path.points.at(prev_idx).point.pose; + if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; + } + prev_idx = i; + const double yaw = tf2::getYaw(p.orientation); + const double x = p.position.x; + const double y = p.position.y; + // NOTE: maybe this is opposite + const double left_x = x + width / 2 * std::sin(yaw); + const double left_y = y - width / 2 * std::cos(yaw); + const double right_x = x - width / 2 * std::sin(yaw); + const double right_y = y + width / 2 * std::cos(yaw); + lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); + rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + +std::optional> getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, + const std::vector & polygons, const bool search_forward = true) +{ + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + } + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + if (i == 0) { + break; + } + } + } + return std::nullopt; +} + +double getHighestCurvature(const lanelet::ConstLineString3d & centerline) +{ + std::vector points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector curvatures_positive; + for (const auto & curvature : curvatures) { + curvatures_positive.push_back(std::fabs(curvature)); + } + return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); +} + +} // namespace + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +intersection::Result +IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) +{ + using intersection::Result; + + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const auto & current_pose = planner_data_->current_odometry->pose; + + // spline interpolation + const auto interpolated_path_info_opt = util::generateInterpolatedPath( + lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); + if (!interpolated_path_info_opt) { + return Result::make_err( + intersection::Indecisive{"splineInterpolate failed"}); + } + + const auto & interpolated_path_info = interpolated_path_info_opt.value(); + if (!interpolated_path_info.lane_id_interval) { + return Result::make_err( + intersection::Indecisive{ + "Path has no interval on intersection lane " + std::to_string(lane_id_)}); + } + + // cache intersection lane information because it is invariant + if (!intersection_lanelets_) { + intersection_lanelets_ = + generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); + } + auto & intersection_lanelets = intersection_lanelets_.value(); + + // at the very first time of regisTration of this module, the path may not be conflicting with the + // attention area, so update() is called to update the internal data as well as traffic light info + intersection_lanelets.update( + is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); + + const auto & conflicting_lanelets = intersection_lanelets.conflicting(); + const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); + const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); + if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { + // this is abnormal + return Result::make_err( + intersection::Indecisive{"conflicting area is empty"}); + } + const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); + const auto & first_conflicting_area = first_conflicting_area_opt.value(); + const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); + + // generate all stop line candidates + // see the doc for struct IntersectionStopLines + /// even if the attention area is null, stuck vehicle stop line needs to be generated from + /// conflicting lanes + const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() + ? intersection_lanelets.first_attention_lane().value() + : first_conflicting_lane; + + const auto intersection_stoplines_opt = generateIntersectionStopLines( + assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, + interpolated_path_info, path); + if (!intersection_stoplines_opt) { + return Result::make_err( + intersection::Indecisive{"failed to generate intersection_stoplines"}); + } + const auto & intersection_stoplines = intersection_stoplines_opt.value(); + const auto closest_idx = intersection_stoplines.closest_idx; + + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); + const auto & conflicting_area = intersection_lanelets.conflicting_area(); + const auto lanelets_on_path = + planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); + // see the doc for struct PathLanelets + const auto path_lanelets_opt = generatePathLanelets( + lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, + first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); + if (!path_lanelets_opt.has_value()) { + return Result::make_err( + intersection::Indecisive{"failed to generate PathLanelets"}); + } + const auto & path_lanelets = path_lanelets_opt.value(); + + if (!occlusion_attention_divisions_) { + occlusion_attention_divisions_ = generateDetectionLaneDivisions( + intersection_lanelets.occlusion_attention(), routing_graph_ptr, + planner_data_->occupancy_grid->info.resolution); + } + + return Result::make_ok( + BasicData{interpolated_path_info, intersection_stoplines, path_lanelets}); +} + +std::optional IntersectionModule::getStopLineIndexFromMap( + const intersection::InterpolatedPathInfo & interpolated_path_info, + lanelet::ConstLanelet assigned_lanelet) +{ + const auto & path = interpolated_path_info.path; + const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); + + const auto road_markings = + assigned_lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stopline; + for (const auto & road_marking : road_markings) { + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + if (type == lanelet::AttributeValueString::StopLine) { + stopline.push_back(road_marking->roadMarking()); + break; // only one stopline exists. + } + } + if (stopline.empty()) { + return std::nullopt; + } + + const auto p_start = stopline.front().front(); + const auto p_end = stopline.front().back(); + const LineString2d extended_stopline = + planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); + + for (size_t i = lane_interval.first; i < lane_interval.second; i++) { + const auto & p_front = path.points.at(i).point.pose.position; + const auto & p_back = path.points.at(i + 1).point.pose.position; + + const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; + std::vector collision_points; + bg::intersection(extended_stopline, path_segment, collision_points); + + if (collision_points.empty()) { + continue; + } + + return i; + } + + geometry_msgs::msg::Pose stop_point_from_map; + stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); + stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); + stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); + + return motion_utils::findFirstNearestIndexWithSoftConstraints( + path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); +} + +std::optional +IntersectionModule::generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, + const intersection::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) +{ + const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; + const double stopline_margin = planner_param_.common.default_stopline_margin; + const double max_accel = planner_param_.common.max_accel; + const double max_jerk = planner_param_.common.max_jerk; + const double delay_response_time = planner_param_.common.delay_response_time; + const double peeking_offset = planner_param_.occlusion.peeking_offset; + + const auto first_attention_area = first_attention_lane.polygon3d(); + const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); + const auto & path_ip = interpolated_path_info.path; + const double ds = interpolated_path_info.ds; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + + const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); + const int base2front_idx_dist = std::ceil(baselink2front / ds); + + // find the index of the first point whose vehicle footprint on it intersects with attention_area + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const std::optional first_footprint_inside_1st_attention_ip_opt = + util::getFirstPointInsidePolygonByFootprint( + first_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (!first_footprint_inside_1st_attention_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_1st_attention_ip = + first_footprint_inside_1st_attention_ip_opt.value(); + + std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; + for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { + // NOTE: maybe consideration of braking dist is necessary + first_footprint_attention_centerline_ip_opt = i; + break; + } + } + if (!first_footprint_attention_centerline_ip_opt) { + return std::nullopt; + } + const size_t first_footprint_attention_centerline_ip = + first_footprint_attention_centerline_ip_opt.value(); + + // (1) default stop line position on interpolated path + bool default_stopline_valid = true; + int stop_idx_ip_int = -1; + if (const auto map_stop_idx_ip = + getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); + map_stop_idx_ip) { + stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; + } + if (stop_idx_ip_int < 0) { + stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; + } + if (stop_idx_ip_int < 0) { + default_stopline_valid = false; + } + const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; + + // (2) ego front stop line position on interpolated path + const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + + // (3) occlusion peeking stop line position on interpolated path + int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); + bool occlusion_peeking_line_valid = true; + // NOTE: if footprints[0] is already inside the attention area, invalid + { + const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + occlusion_peeking_line_ip_int = + first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); + } + const auto occlusion_peeking_line_ip = static_cast( + std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + + // (4) first attention stopline position on interpolated path + const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; + const bool first_attention_stopline_valid = true; + + // (5) 1st pass judge line position on interpolated path + const double velocity = planner_data_->current_velocity->twist.linear.x; + const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; + const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_accel, max_jerk, delay_response_time); + int first_pass_judge_ip_int = + static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); + const auto first_pass_judge_line_ip = static_cast( + std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( + 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); + + // (6) stuck vehicle stopline position on interpolated path + int stuck_stopline_ip_int = 0; + bool stuck_stopline_valid = true; + if (use_stuck_stopline) { + // NOTE: when ego vehicle is approaching attention area and already passed + // first_conflicting_area, this could be null. + const auto stuck_stopline_idx_ip_opt = util::getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); + if (!stuck_stopline_idx_ip_opt) { + stuck_stopline_valid = false; + stuck_stopline_ip_int = 0; + } else { + stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; + } + } else { + stuck_stopline_ip_int = + std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); + } + if (stuck_stopline_ip_int < 0) { + stuck_stopline_valid = false; + } + const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + + // (7) second attention stopline position on interpolated path + int second_attention_stopline_ip_int = -1; + bool second_attention_stopline_valid = false; + if (second_attention_area_opt) { + const auto & second_attention_area = second_attention_area_opt.value(); + std::optional first_footprint_inside_2nd_attention_ip_opt = + util::getFirstPointInsidePolygonByFootprint( + second_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (first_footprint_inside_2nd_attention_ip_opt) { + second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + second_attention_stopline_valid = true; + } + } + const auto second_attention_stopline_ip = + second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) + : 0; + + // (8) second pass judge line position on interpolated path. It is the same as first pass judge + // line if second_attention_lane is null + int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; + const auto second_pass_judge_line_ip = + second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) + : first_pass_judge_line_ip; + + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stopline{0}; + size_t default_stopline{0}; + size_t first_attention_stopline{0}; + size_t second_attention_stopline{0}; + size_t occlusion_peeking_stopline{0}; + size_t first_pass_judge_line{0}; + size_t second_pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stoplines_temp; + std::list> stoplines = { + {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, + {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, + {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, + {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, + {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, + {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, + {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, + {&occlusion_wo_tl_pass_judge_line_ip, + &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; + stoplines.sort( + [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); + for (const auto & [stop_idx_ip, stop_idx] : stoplines) { + const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; + const auto insert_idx = util::insertPointIndex( + insert_point, original_path, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + if (!insert_idx) { + return std::nullopt; + } + *stop_idx = insert_idx.value(); + } + if ( + intersection_stoplines_temp.occlusion_peeking_stopline < + intersection_stoplines_temp.default_stopline) { + intersection_stoplines_temp.occlusion_peeking_stopline = + intersection_stoplines_temp.default_stopline; + } + + intersection::IntersectionStopLines intersection_stoplines; + intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; + if (stuck_stopline_valid) { + intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; + } + if (default_stopline_valid) { + intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; + } + if (first_attention_stopline_valid) { + intersection_stoplines.first_attention_stopline = + intersection_stoplines_temp.first_attention_stopline; + } + if (second_attention_stopline_valid) { + intersection_stoplines.second_attention_stopline = + intersection_stoplines_temp.second_attention_stopline; + } + if (occlusion_peeking_line_valid) { + intersection_stoplines.occlusion_peeking_stopline = + intersection_stoplines_temp.occlusion_peeking_stopline; + } + intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; + intersection_stoplines.occlusion_wo_tl_pass_judge_line = + intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; + return intersection_stoplines; +} + +intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet) +{ + const double detection_area_length = planner_param_.common.attention_area_length; + const double occlusion_detection_area_length = + planner_param_.occlusion.occlusion_attention_area_length; + const bool consider_wrong_direction_vehicle = + planner_param_.collision_detection.consider_wrong_direction_vehicle; + + // retrieve a stopline associated with a traffic light + bool has_traffic_light = false; + if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + + // for low priority lane + // If ego_lane has right of way (i.e. is high priority), + // ignore yieldLanelets (i.e. low priority lanes) + lanelet::ConstLanelets yield_lanelets{}; + const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); + for (const auto & right_of_way : right_of_ways) { + if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { + for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { + yield_lanelets.push_back(yield_lanelet); + for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { + yield_lanelets.push_back(previous_lanelet); + } + } + } + } + + // get all following lanes of previous lane + lanelet::ConstLanelets ego_lanelets{}; + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + ego_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { + continue; + } + ego_lanelets.push_back(following_lanelet); + } + } + + // get conflicting lanes on assigned lanelet + const auto & conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + std::vector adjacent_followings; + + for (const auto & conflicting_lanelet : conflicting_lanelets) { + for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + } + + // final objective lanelets + lanelet::ConstLanelets detection_lanelets; + lanelet::ConstLanelets conflicting_ex_ego_lanelets; + // conflicting lanes is necessary to get stopline for stuck vehicle + for (auto && conflicting_lanelet : conflicting_lanelets) { + if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) + conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); + } + + // exclude yield lanelets and ego lanelets from detection_lanelets + if (turn_direction_ == std::string("straight") && has_traffic_light) { + // if assigned lanelet is "straight" with traffic light, detection area is not necessary + } else { + if (consider_wrong_direction_vehicle) { + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + for (const auto & adjacent_following : adjacent_followings) { + detection_lanelets.push_back(adjacent_following); + } + } else { + // otherwise we need to know the priority from RightOfWay + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if ( + lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || + lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + } + } + + // get possible lanelet path that reaches conflicting_lane longer than given length + lanelet::ConstLanelets detection_and_preceding_lanelets; + { + const double length = detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(l); + } + } + } + } + + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; + { + const double length = occlusion_detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + } + } + } + } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); + + intersection::IntersectionLanelets result; + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stopline{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + break; + } + if (stopline) break; + } + result.attention_stoplines_.push_back(stopline); + } + result.attention_non_preceding_ = std::move(detection_lanelets); + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + std::optional stopline = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + } + result.attention_non_preceding_stoplines_.push_back(stopline); + } + result.conflicting_ = std::move(conflicting_ex_ego_lanelets); + result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched + result.attention_area_ = util::getPolygon3dFromLanelets(result.attention_); + result.attention_non_preceding_area_ = + util::getPolygon3dFromLanelets(result.attention_non_preceding_); + result.conflicting_area_ = util::getPolygon3dFromLanelets(result.conflicting_); + result.adjacent_area_ = util::getPolygon3dFromLanelets(result.adjacent_); + result.occlusion_attention_area_ = util::getPolygon3dFromLanelets(result.occlusion_attention_); + return result; +} + +std::optional IntersectionModule::generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx) +{ + const double width = planner_data_->vehicle_info_.vehicle_width_m; + static constexpr double path_lanelet_interval = 1.5; + + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; + + intersection::PathLanelets path_lanelets; + // prev + path_lanelets.prev = ::getPrevLanelets(lanelets_on_path, associative_ids_); + path_lanelets.all = path_lanelets.prev; + + // entry2ego if exist + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + ::generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); + + // next + if (assigned_lane_end < path.points.size() - 1) { + const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); + const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + path_lanelets.next = + generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + const auto first_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? ::getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : ::getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, + path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } + return path_lanelets; +} + +std::vector IntersectionModule::generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets_all, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) +{ + const double curvature_threshold = + planner_param_.occlusion.attention_lane_crop_curvature_threshold; + const double curvature_calculation_ds = + planner_param_.occlusion.attention_lane_curvature_calculation_ds; + + using lanelet::utils::getCenterlineWithOffset; + + // (0) remove left/right lanelet + lanelet::ConstLanelets detection_lanelets; + for (const auto & detection_lanelet : detection_lanelets_all) { + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet + const auto fine_centerline = + lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); + const double highest_curvature = ::getHighestCurvature(fine_centerline); + if (highest_curvature > curvature_threshold) { + continue; + } + detection_lanelets.push_back(detection_lanelet); + } + + // (1) tsort detection_lanelets + const auto [merged_detection_lanelets, originals] = + util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); + + // (2) merge each branch to one lanelet + // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); + double area = 0; + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); + } + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); + } + + // (3) discretize each merged lanelet + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); + const double width = area / length; + for (int i = 0; i < static_cast(width / resolution); ++i) { + const double offset = resolution * i - width / 2; + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); + } + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); + } + return detection_divisions; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp new file mode 100644 index 0000000000000..7f23bed3c36cd --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -0,0 +1,380 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 "scene_intersection.hpp" +#include "util.hpp" + +#include // for toGeomPoly +#include +#include + +#include +#include +#include + +#include +#include + +namespace +{ +lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) +{ + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; + } + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; + } + accumulated_length += length; + } + + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} + +lanelet::ConstLanelet createLaneletFromArcLength( + const lanelet::ConstLanelet & lanelet, const double s1, const double s2) +{ + const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s2_left = + static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s1_right = + static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); + const auto s2_right = + static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); + + const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); + + return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); +} + +} // namespace + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +std::optional IntersectionModule::isStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::IntersectionStopLines & intersection_stoplines, + const intersection::PathLanelets & path_lanelets) const +{ + const auto closest_idx = intersection_stoplines.closest_idx; + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path.points, closest_idx, index); + }; + + const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK + const bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets); + const auto first_conflicting_lane = + intersection_lanelets.first_conflicting_lane().value(); // this is OK + const bool is_first_conflicting_lane_private = + (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); + const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; + const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; + if (stuck_detected) { + if ( + is_first_conflicting_lane_private && + planner_param_.stuck_vehicle.disable_against_private_lane) { + // do nothing + } else { + std::optional stopline_idx = std::nullopt; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = default_stopline_idx_opt.value(); + } else if ( + first_attention_stopline_idx_opt && + fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return intersection::StuckStop{ + closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; + } + } + } + return std::nullopt; +} + +bool IntersectionModule::isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + +bool IntersectionModule::checkStuckVehicleInIntersection( + const intersection::PathLanelets & path_lanelets) const +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getLaneletLength3d; + using lanelet::utils::getPolygonFromArcLength; + using lanelet::utils::to2D; + + const bool stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); + }(); + if (!stuck_detection_direction) { + return false; + } + + const auto & objects_ptr = planner_data_->predicted_objects; + + // considering lane change in the intersection, these lanelets are generated from the path + const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; + Polygon2d stuck_vehicle_detect_area{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return false; + } + + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } + const auto target_polygon = + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); + + if (target_polygon.empty()) { + return false; + } + + for (const auto & p : target_polygon) { + stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); + } + + stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); + bg::correct(stuck_vehicle_detect_area); + + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); + + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { + continue; // not stop vehicle + } + + // check if the footprint is in the stuck detect area + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); + if (is_in_stuck_area) { + debug_data_.stuck_targets.objects.push_back(object); + return true; + } + } + return false; +} + +std::optional IntersectionModule::isYieldStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const intersection::IntersectionStopLines & intersection_stoplines, + const TargetObjects & target_objects) const +{ + const auto closest_idx = intersection_stoplines.closest_idx; + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path.points, closest_idx, index); + }; + const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); // this is OK + const auto first_attention_stopline_idx = + intersection_stoplines.first_attention_stopline.value(); // this is OK + const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + + const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( + target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding()); + if (yield_stuck_detected) { + std::optional stopline_idx = std::nullopt; + const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; + const bool is_before_first_attention_stopline = + fromEgoDist(first_attention_stopline_idx) >= 0.0; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (is_before_default_stopline) { + stopline_idx = default_stopline_idx; + } else if (is_before_first_attention_stopline) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return intersection::YieldStuckStop{closest_idx, stopline_idx.value()}; + } + } + return std::nullopt; +} + +bool IntersectionModule::checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets) const +{ + const bool yield_stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.yield_stuck.turn_direction.straight); + }(); + if (!yield_stuck_detection_direction) { + return false; + } + + const double width = planner_data_->vehicle_info_.vehicle_width_m; + const double stuck_vehicle_vel_thr = + planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; + const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; + + LineString2d sparse_intersection_path; + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + for (unsigned i = start; i < end; ++i) { + const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; + const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); + if (turn_direction_ == "right") { + const double right_x = point.x - width / 2 * std::sin(yaw); + const double right_y = point.y + width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(right_x, right_y); + } else if (turn_direction_ == "left") { + const double left_x = point.x + width / 2 * std::sin(yaw); + const double left_y = point.y - width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(left_x, left_y); + } else { + // straight + sparse_intersection_path.emplace_back(point.x, point.y); + } + } + lanelet::ConstLanelets yield_stuck_detect_lanelets; + for (const auto & attention_lanelet : attention_lanelets) { + const auto centerline = attention_lanelet.centerline2d().basicLineString(); + std::vector intersects; + bg::intersection(sparse_intersection_path, centerline, intersects); + if (intersects.empty()) { + continue; + } + const auto intersect = intersects.front(); + const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( + centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); + const double yield_stuck_start = + std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); + const double yield_stuck_end = intersect_arc_coords.length; + yield_stuck_detect_lanelets.push_back( + ::createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); + } + debug_data_.yield_stuck_detect_area = util::getPolygon3dFromLanelets(yield_stuck_detect_lanelets); + for (const auto & object : target_objects.all_attention_objects) { + const auto obj_v_norm = std::hypot( + object.object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; + } + for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + if (is_in_lanelet) { + debug_data_.yield_stuck_targets.objects.push_back(object.object); + return true; + } + } + } + return false; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index b929959e43f12..8b0511c741b4e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -51,7 +52,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, - const util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -120,7 +121,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR } const auto first_conflicting_lanelet = first_conflicting_lanelet_.value(); - const auto first_conflicting_idx_opt = getFirstPointInsidePolygonByFootprint( + const auto first_conflicting_idx_opt = util::getFirstPointInsidePolygonByFootprint( first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, baselink2front); if (!first_conflicting_idx_opt) { return false; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 6afc2d61d77f2..dfdfb01fb2234 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -14,14 +14,13 @@ #include "util.hpp" -#include "util_type.hpp" +#include "interpolated_path_info.hpp" #include #include #include #include #include -#include #include #include @@ -29,23 +28,22 @@ #include #include #include +#include #include #include #include +#include #include #include #include #include #include -namespace behavior_velocity_planner +namespace behavior_velocity_planner::util { namespace bg = boost::geometry; -namespace util -{ - static std::optional getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) @@ -135,7 +133,8 @@ std::optional> findLaneIdsInterval( } std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, + const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -155,6 +154,34 @@ std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } +std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, @@ -333,12 +360,12 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { - InterpolatedPathInfo interpolated_path_info; + intersection::InterpolatedPathInfo interpolated_path_info; if (!splineInterpolate(input_path, ds, interpolated_path_info.path, logger)) { return std::nullopt; } @@ -367,5 +394,14 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -} // namespace util -} // namespace behavior_velocity_planner +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec) +{ + std::vector polys; + for (auto && ll : ll_vec) { + polys.push_back(ll.polygon3d()); + } + return polys; +} + +} // namespace behavior_velocity_planner::util diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 86a34d1e95114..f103191b2dc6f 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -15,25 +15,22 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include "util_type.hpp" +#include "interpolated_path_info.hpp" -#include +#include +#include -#include -#include +#include + +#include +#include -#include -#include #include #include -#include -#include #include #include -namespace behavior_velocity_planner -{ -namespace util +namespace behavior_velocity_planner::util { /** @@ -111,7 +108,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); * @fn * @brief interpolate PathWithLaneId */ -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); @@ -139,10 +136,20 @@ mergeLaneletsByTopologicalSort( * polygon */ std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, + const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); -} // namespace util -} // namespace behavior_velocity_planner +std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec); + +} // namespace behavior_velocity_planner::util #endif // UTIL_HPP_ From 9cda3b3ab2a34f2179d931d7e2d99eb604d4143f Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 23 Jan 2024 11:51:29 +0900 Subject: [PATCH 104/154] fix(raw_vehicle_cmd_converter): fix parameter files to parse path to csv files (#6136) Signed-off-by: Fumiya Watanabe --- .../launch/vehicle.launch.xml | 2 ++ .../config/raw_vehicle_cmd_converter.param.yaml | 3 --- .../launch/raw_vehicle_converter.launch.xml | 7 +++++++ .../schema/raw_vehicle_cmd_converter.schema.json | 15 --------------- 4 files changed, 9 insertions(+), 18 deletions(-) diff --git a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml index 0493e1ae0bbcf..0e1d22bfd1827 100644 --- a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml +++ b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml @@ -6,6 +6,7 @@ + @@ -21,6 +22,7 @@ + diff --git a/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml b/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml index d63a115304a87..1001696d96679 100644 --- a/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml +++ b/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml @@ -1,8 +1,5 @@ /**: ros__parameters: - csv_path_accel_map: "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/accel_map.csv" - csv_path_brake_map: "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/brake_map.csv" - csv_path_steer_map: "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/steer_map.csv" convert_accel_cmd: true convert_brake_cmd: true convert_steer_cmd: true diff --git a/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml index 0a9962a7c2a30..a74cdcc56132d 100644 --- a/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml +++ b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml @@ -1,5 +1,9 @@ + + + + @@ -9,6 +13,9 @@ + + + diff --git a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json index 4b562f401e09b..aa8936d331a87 100644 --- a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json +++ b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json @@ -6,21 +6,6 @@ "raw_vehicle_cmd_converter": { "type": "object", "properties": { - "csv_path_accel_map": { - "type": "string", - "description": "path for acceleration map csv file", - "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/accel_map.csv" - }, - "csv_path_brake_map": { - "type": "string", - "description": "path for brake map csv file", - "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/brake_map.csv" - }, - "csv_path_steer_map": { - "type": "string", - "description": "path for steer map csv file", - "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/steer_map.csv" - }, "convert_accel_cmd": { "type": "boolean", "description": "use accel or not", From cb8de680a31db8aaa13bd0a850028ff4499e93ae Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 23 Jan 2024 12:31:09 +0900 Subject: [PATCH 105/154] feat(goal_planner): use expanded pull over lanes for filtering in goal searcher (#6142) Signed-off-by: kosuke55 --- .../util.hpp | 10 +++++ .../src/goal_planner_module.cpp | 20 +++------- .../src/goal_searcher.cpp | 13 +++---- .../src/util.cpp | 37 +++++++++++++++++++ 4 files changed, 57 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 294a5125a0d13..be56d9dc30196 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance); +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset); +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh); double calcLateralDeviationBetweenPaths( const PathWithLaneId & reference_path, const PathWithLaneId & target_path); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index fa6a44b97a623..2d3133768154e 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1509,22 +1509,12 @@ bool GoalPlannerModule::checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, const bool update_debug_data) const { - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); + const auto pull_over_lane_stop_objects = + goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, parameters_->detection_bound_offset, + *(planner_data_->dynamic_object), parameters_->th_moving_object_velocity); - const auto expanded_pull_over_lanes = - left_side_parking_ ? lanelet::utils::getExpandedLanelets( - pull_over_lanes, parameters_->detection_bound_offset, 0.0) - : lanelet::utils::getExpandedLanelets( - pull_over_lanes, 0.0, parameters_->detection_bound_offset); - - const auto [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), expanded_pull_over_lanes, - utils::path_safety_checker::isPolygonOverlapLanelet); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_->th_moving_object_velocity); std::vector obj_polygons; for (const auto & object : pull_over_lane_stop_objects.objects) { obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index 31f99e8ea5aa1..fd795177eb0b7 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -266,14 +266,11 @@ void GoalSearcher::countObjectsToAvoid( void GoalSearcher::update(GoalCandidates & goal_candidates) const { - const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, - parameters_.forward_goal_search_length); - const auto [pull_over_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + const auto pull_over_lane_stop_objects = + goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length, parameters_.detection_bound_offset, + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index dfdaa2a3a18c1..8ad90d26f58f2 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -14,6 +14,7 @@ #include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -41,6 +42,7 @@ using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; + lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance) @@ -74,6 +76,41 @@ lanelet::ConstLanelets getPullOverLanes( outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset) +{ + const auto pull_over_lanes = + getPullOverLanes(route_handler, left_side, backward_distance, forward_distance); + + return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0) + : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, bound_offset); +} + +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects) +{ + const auto lanes = generateExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset); + + const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( + objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + + return objects_in_lanes; +} + +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh) +{ + const auto objects_in_lanes = extractObjectsInExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset, objects); + + return utils::path_safety_checker::filterObjectsByVelocity(objects_in_lanes, velocity_thresh); +} + PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside) From 1aee603e258e391825340c0f847ddd314943c758 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 23 Jan 2024 12:31:29 +0900 Subject: [PATCH 106/154] fix(goal_planner): fix sudden stop and simpify process (#6120) fix(goal_planner): fix sudden stop and simpify process Signed-off-by: kosuke55 --- .../goal_planner_module.hpp | 21 +---- .../src/goal_planner_module.cpp | 91 +++---------------- 2 files changed, 18 insertions(+), 94 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index a6ebaf00aed78..56ca29543fcec 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -240,15 +240,11 @@ struct PreviousPullOverData void reset() { - stop_path = nullptr; - stop_path_after_approval = nullptr; found_path = false; safety_status = SafetyStatus{}; has_decided_path = false; } - std::shared_ptr stop_path{nullptr}; - std::shared_ptr stop_path_after_approval{nullptr}; bool found_path{false}; SafetyStatus safety_status{}; bool has_decided_path{false}; @@ -387,7 +383,7 @@ class GoalPlannerModule : public SceneModuleInterface ThreadSafeData thread_safe_data_; std::unique_ptr last_approval_data_{nullptr}; - PreviousPullOverData prev_data_{nullptr}; + PreviousPullOverData prev_data_{}; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. @@ -428,7 +424,7 @@ class GoalPlannerModule : public SceneModuleInterface void decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath() const; - PathWithLaneId generateFeasibleStopPath() const; + PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const; void keepStoppedWithCurrentPath(PathWithLaneId & path) const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -477,19 +473,8 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output) const; - void setStopPath(BehaviorModuleOutput & output) const; - void updatePreviousData(const BehaviorModuleOutput & output); + void updatePreviousData(); - /** - * @brief Sets a stop path in the current path based on safety conditions and previous paths. - * - * This function sets a stop path in the current path. Depending on whether the previous safety - * judgement against dynamic objects were safe or if a previous stop path existed, it either - * generates a new stop path or uses the previous stop path. - * - * @param output BehaviorModuleOutput - */ - void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const; void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 2d3133768154e..34278bfa74939 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -806,7 +806,9 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const if (!thread_safe_data_.foundPullOverPath()) { // situation : not safe against static objects use stop_path - setStopPath(output); + output.path = generateStopPath(); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); setDrivableAreaInfo(output); return; } @@ -816,7 +818,11 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints - setStopPathFromCurrentPath(output); + output.path = + generateFeasibleStopPath(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path"); + debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); } else { // situation : (safe against static and dynamic objects) or (safe against static objects and // before approval) don't stop @@ -836,58 +842,6 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const -{ - if (prev_data_.found_path || !prev_data_.stop_path) { - // safe -> not_safe or no prev_stop_path: generate new stop_path - output.path = generateStopPath(); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); - } else { - // not_safe -> not_safe: use previous stop path - output.path = *prev_data_.stop_path; - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); - // stop_pose_ is removed in manager every loop, so need to set every loop. - const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path); - if (stop_pose_opt) { - debug_stop_pose_with_info_.set(stop_pose_opt.value(), std::string("keep previous stop pose")); - } - } -} - -void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const -{ - // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (prev_data_.safety_status.is_safe || !prev_data_.stop_path_after_approval) { - auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); - const auto stop_path = - behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( - current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, - parameters_->maximum_jerk_for_stop); - if (stop_path) { - output.path = *stop_path; - debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); - } else { - output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "Collision detected, no feasible stop path found, cannot stop."); - } - } else { - // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - output.path = *prev_data_.stop_path_after_approval; - // stop_pose_ is removed in manager every loop, so need to set every loop. - const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path); - if (stop_pose_opt) { - debug_stop_pose_with_info_.set( - stop_pose_opt.value(), std::string("keep feasible stop after approval")); - } - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); - } -} - void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { @@ -1096,7 +1050,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() path_candidate_ = std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - updatePreviousData(output); + updatePreviousData(); return output; } @@ -1123,12 +1077,8 @@ void GoalPlannerModule::postProcess() setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); } -void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) +void GoalPlannerModule::updatePreviousData() { - if (prev_data_.found_path || !prev_data_.stop_path) { - prev_data_.stop_path = std::make_shared(output.path); - } - // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. prev_data_.found_path = thread_safe_data_.foundPullOverPath(); @@ -1152,17 +1102,6 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) prev_data_.safety_status.safe_start_time = std::nullopt; } prev_data_.safety_status.is_safe = is_safe; - - // If safety check is enabled, save current path as stop_path_after_approval - // This path is set only once after approval. - if (!isActivated() || (!is_safe && prev_data_.stop_path_after_approval)) { - return; - } - auto stop_path = std::make_shared(output.path); - for (auto & point : stop_path->points) { - point.point.longitudinal_velocity_mps = 0.0; - } - prev_data_.stop_path_after_approval = stop_path; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1269,7 +1208,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return std::make_pair(decel_pose.value(), "stop at search start pose"); }); if (!stop_pose_with_info) { - const auto feasible_stop_path = generateFeasibleStopPath(); + const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path); // override stop pose info debug string debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose")); return feasible_stop_path; @@ -1284,7 +1223,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) { - const auto feasible_stop_path = generateFeasibleStopPath(); + const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path); debug_stop_pose_with_info_.set( std::string("feasible stop: stop pose is closer than min_stop_distance")); return feasible_stop_path; @@ -1316,17 +1255,17 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return stop_path; } -PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const +PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId & path) const { // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return getPreviousModuleOutput().path; + return path; } // set stop point - auto stop_path = getPreviousModuleOutput().path; + auto stop_path = path; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); From 05f8ea3719d00c7df1b6b7812d863f593cb7a8e3 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 23 Jan 2024 14:21:38 +0900 Subject: [PATCH 107/154] feat(goal_planner): add deciding status to check collision for for a certain period of time (#6128) * feat(goal_planner): add deciding status to check collision for for a certain period of time Signed-off-by: kosuke55 * add hysterisys and debug print Signed-off-by: kosuke55 * check goal longitudinal margin when deciding Signed-off-by: kosuke55 * clean up Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../goal_planner_module.hpp | 13 ++- .../goal_searcher.hpp | 5 + .../goal_searcher_base.hpp | 2 + .../src/goal_planner_module.cpp | 105 +++++++++++++++--- .../src/goal_searcher.cpp | 37 ++++++ 5 files changed, 144 insertions(+), 18 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 56ca29543fcec..51638e5485fe2 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -230,6 +230,13 @@ struct LastApprovalData Pose pose{}; }; +enum class DecidingPathStatus { + NOT_DECIDED, + DECIDING, + DECIDED, +}; +using DecidingPathStatusWithStamp = std::pair; + struct PreviousPullOverData { struct SafetyStatus @@ -242,12 +249,12 @@ struct PreviousPullOverData { found_path = false; safety_status = SafetyStatus{}; - has_decided_path = false; + deciding_path_status = DecidingPathStatusWithStamp{}; } bool found_path{false}; SafetyStatus safety_status{}; - bool has_decided_path{false}; + DecidingPathStatusWithStamp deciding_path_status{}; }; // store stop_pose_ pointer with reason string @@ -439,6 +446,8 @@ class GoalPlannerModule : public SceneModuleInterface bool needPathUpdate(const double path_update_duration) const; bool isStuck(); bool hasDecidedPath() const; + bool hasNotDecidedPath() const; + DecidingPathStatusWithStamp checkDecidingPathStatus() const; void decideVelocity(); bool foundPullOverPath() const; void updateStatus(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp index 94cf7bc7ff5dd..e487d9d2ae0b2 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp @@ -35,8 +35,13 @@ class GoalSearcher : public GoalSearcherBase GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; + + // todo(kosuke55):　Functions for this specific use should not be in the interface, + // so it is better to consider interface design when we implement other goal searchers. GoalCandidate getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates) const override; + bool isSafeGoalWithMarginScaleFactor( + const GoalCandidate & goal_candidate, const double margin_scale_factor) const override; private: void countObjectsToAvoid( diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp index 93472ab6c0703..2ddacc0aac46d 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -60,6 +60,8 @@ class GoalSearcherBase virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } virtual GoalCandidate getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates) const = 0; + virtual bool isSafeGoalWithMarginScaleFactor( + const GoalCandidate & goal_candidate, const double margin_scale_factor) const = 0; protected: GoalPlannerParameters parameters_{}; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 34278bfa74939..faa9e28e28b3b 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -903,36 +903,108 @@ void GoalPlannerModule::updateSteeringFactor( bool GoalPlannerModule::hasDecidedPath() const { + return checkDecidingPathStatus().first == DecidingPathStatus::DECIDED; +} + +bool GoalPlannerModule::hasNotDecidedPath() const +{ + return checkDecidingPathStatus().first == DecidingPathStatus::NOT_DECIDED; +} + +DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const +{ + const auto & prev_status = prev_data_.deciding_path_status; + // Once this function returns true, it will continue to return true thereafter - if (prev_data_.has_decided_path) { - return true; + if (prev_status.first == DecidingPathStatus::DECIDED) { + return prev_status; } // if path is not safe, not decided if (!thread_safe_data_.foundPullOverPath()) { - return false; + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } - // If it is dangerous before approval, do not determine the path. + // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved if ( parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) { - return false; + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " + "approval"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // if object recognition for path collision check is enabled, transition to DECIDED after + // DECIDING for a certain period of time if there is no collision. + const auto pull_over_path = thread_safe_data_.get_pull_over_path(); + const auto current_path = pull_over_path->getCurrentPath(); + if (prev_status.first == DecidingPathStatus::DECIDING) { + const double hysteresis_factor = 0.9; + + // check goal pose collision + goal_searcher_->setPlannerData(planner_data_); + const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose(); + if ( + modified_goal_opt && !goal_searcher_->isSafeGoalWithMarginScaleFactor( + modified_goal_opt.value(), hysteresis_factor)) { + RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // check current parking path collision + const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5); + const double margin = + parameters_->object_recognition_collision_check_margin * hysteresis_factor; + if (checkObjectsCollision(parking_path, margin)) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // if enough time has passed since deciding status starts, transition to DECIDED + constexpr double check_collision_duration = 1.0; + const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds(); + if (elapsed_time_from_deciding > check_collision_duration) { + RCLCPP_DEBUG( + getLogger(), "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); + return {DecidingPathStatus::DECIDED, clock_->now()}; + } + + // if enough time has NOT passed since deciding status starts, keep DECIDING + RCLCPP_DEBUG( + getLogger(), "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", + elapsed_time_from_deciding); + return prev_status; } // if ego is sufficiently close to the start of the nearest candidate path, the path is decided const auto & current_pose = planner_data_->self_odometry->pose.pose; - const size_t ego_segment_idx = motion_utils::findNearestSegmentIndex( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position); + const size_t ego_segment_idx = + motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); - const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, - thread_safe_data_.get_pull_over_path()->start_pose.position); + const size_t start_pose_segment_idx = + motion_utils::findNearestSegmentIndex(current_path.points, pull_over_path->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position, - ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, - start_pose_segment_idx); - return dist_to_parking_start_pose < parameters_->decide_path_distance; + current_path.points, current_pose.position, ego_segment_idx, + pull_over_path->start_pose.position, start_pose_segment_idx); + if (dist_to_parking_start_pose > parameters_->decide_path_distance) { + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // if object recognition for path collision check is enabled, transition to DECIDING to check + // collision for a certain period of time. Otherwise, transition to DECIDED directly. + if (parameters_->use_object_recognition) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " + "period of time"); + return {DecidingPathStatus::DECIDING, clock_->now()}; + } + RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: NOT_DECIDED->DECIDED"); + return {DecidingPathStatus::DECIDED, clock_->now()}; } void GoalPlannerModule::decideVelocity() @@ -993,10 +1065,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() return getPreviousModuleOutput(); } - if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { + if (hasNotDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ + RCLCPP_DEBUG(getLogger(), "Update pull over path candidates"); thread_safe_data_.clearPullOverPath(); @@ -1083,7 +1156,7 @@ void GoalPlannerModule::updatePreviousData() // this is used to determine whether to generate a new stop path or keep the current stop path. prev_data_.found_path = thread_safe_data_.foundPullOverPath(); - prev_data_.has_decided_path = hasDecidedPath(); + prev_data_.deciding_path_status = checkDecidingPathStatus(); // This is related to safety check, so if it is disabled, return here. if (!parameters_->safety_check_params.enable_safety_check) { diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index fd795177eb0b7..e2d12dd82cbe4 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -312,6 +312,43 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } } +// Note: this function is not just return goal_candidate.is_safe but check collision with +// current planner_data_ and margin scale factor. +// And is_safe is not updated in this function. +bool GoalSearcher::isSafeGoalWithMarginScaleFactor( + const GoalCandidate & goal_candidate, const double margin_scale_factor) const +{ + if (!parameters_.use_object_recognition) { + return true; + } + + const Pose goal_pose = goal_candidate.goal_pose; + + const auto pull_over_lane_stop_objects = + goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length, parameters_.detection_bound_offset, + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + + const double margin = parameters_.object_recognition_collision_check_margin * margin_scale_factor; + + if (utils::checkCollisionBetweenFootprintAndObjects( + vehicle_footprint_, goal_pose, pull_over_lane_stop_objects, margin)) { + return false; + } + + // check longitudinal margin with pull over lane objects + constexpr bool filter_inside = true; + const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( + goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, margin, + filter_inside); + if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { + return false; + } + + return true; +} + bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & objects) const { if (parameters_.use_occupancy_grid_for_goal_search) { From e4cae6fa3880649acb83552eca11e585f7ed21c7 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 23 Jan 2024 14:23:44 +0900 Subject: [PATCH 108/154] feat(system_error_monitor): enable invalid trajectory size diag (#6127) Signed-off-by: Takamasa Horibe --- .../config/diagnostic_aggregator/planning.param.yaml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml index 7cdd949ae3cfe..aad8a4ffffac4 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml @@ -22,6 +22,12 @@ type: diagnostic_aggregator/AnalyzerGroup path: trajectory_validation analyzers: + trajectory_size: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_validation_size + contains: [": trajectory_validation_size"] + timeout: 1.0 + trajectory_finite_value: type: diagnostic_aggregator/GenericAnalyzer path: trajectory_validation_finite From 9fb03ac1878d810e2b554476706cb091ee2c558f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 24 Jan 2024 18:01:57 +0900 Subject: [PATCH 109/154] refactor(lane_change): update lc status in updateData (#6088) * refactor(lane_change): update lc status in updateData Signed-off-by: Muhammad Zulfaqar Azmi * update current lane and target lane during requested Signed-off-by: Muhammad Zulfaqar Azmi * set is_valid_path as false by default Signed-off-by: Muhammad Zulfaqar Azmi * reset flags after finished lane change Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene.hpp | 2 +- .../utils/base_class.hpp | 2 +- .../utils/path.hpp | 2 +- .../src/interface.cpp | 24 ++++++++----------- .../src/scene.cpp | 17 +++++++++---- 5 files changed, 25 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 9c3c371627a78..f991ca0d849f0 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -89,7 +89,7 @@ class NormalLaneChange : public LaneChangeBase bool isAbortState() const override; - bool isLaneChangeRequired() const override; + bool isLaneChangeRequired() override; bool isStoppedAtRedTrafficLight() const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 451899fbf27e6..400d5505dc49f 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -80,7 +80,7 @@ class LaneChangeBase virtual bool hasFinishedAbort() const = 0; - virtual bool isLaneChangeRequired() const = 0; + virtual bool isLaneChangeRequired() = 0; virtual bool isAbortState() const = 0; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index 9e42b49635b82..3af4f487a0fa0 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -45,7 +45,7 @@ struct LaneChangeStatus std::vector lane_follow_lane_ids{}; std::vector lane_change_lane_ids{}; bool is_safe{false}; - bool is_valid_path{true}; + bool is_valid_path{false}; double start_distance{0.0}; }; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 448f83ecaf15e..c8e1229fe5af5 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -50,9 +50,6 @@ LaneChangeInterface::LaneChangeInterface( void LaneChangeInterface::processOnEntry() { waitApproval(); - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); } void LaneChangeInterface::processOnExit() @@ -80,6 +77,14 @@ void LaneChangeInterface::updateData() { module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); + module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); + + if (isWaitingApproval()) { + module_type_->updateLaneChangeStatus(); + } + setObjectDebugVisualization(); + module_type_->updateSpecialData(); module_type_->resetStopPose(); } @@ -98,8 +103,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() return {}; } - module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); - module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); auto output = module_type_->generateOutput(); path_reference_ = std::make_shared(output.reference_path); *prev_approved_path_ = getPreviousModuleOutput().path; @@ -128,22 +131,14 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); - setObjectDebugVisualization(); + out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); for (const auto & [uuid, data] : module_type_->getDebugData()) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - // change turn signal when the vehicle reaches at the end of the path for waiting lane change - out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); - path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { @@ -211,6 +206,7 @@ bool LaneChangeInterface::canTransitSuccessState() } if (module_type_->hasFinishedLaneChange()) { + module_type_->resetParameters(); log_debug_throttled("Lane change process has completed."); return true; } diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c7ddd8fe63636..f5100f16129c2 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -115,17 +115,17 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) return {true, found_safe_path}; } -bool NormalLaneChange::isLaneChangeRequired() const +bool NormalLaneChange::isLaneChangeRequired() { - const auto current_lanes = getCurrentLanes(); + status_.current_lanes = getCurrentLanes(); - if (current_lanes.empty()) { + if (status_.current_lanes.empty()) { return false; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); - return !target_lanes.empty(); + return !status_.target_lanes.empty(); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -420,8 +420,15 @@ void NormalLaneChange::resetParameters() is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; + status_ = {}; object_debug_.clear(); + object_debug_after_approval_.clear(); + debug_filtered_objects_.current_lane.clear(); + debug_filtered_objects_.target_lane.clear(); + debug_filtered_objects_.other_lane.clear(); + debug_valid_path_.clear(); + RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() From 51a5f22c1cf15cb688b3591e76f11cc23d772e42 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 24 Jan 2024 18:54:10 +0900 Subject: [PATCH 110/154] docs(map_projection_loader): add limitations (#6140) * docs(map_projection_loader): add limitations Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- map/map_projection_loader/README.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 94b142da3dcf9..1887a1cd8934f 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -28,6 +28,15 @@ sample-map-rosbag projector_type: local ``` +#### Limitation + +The functionality that requires latitude and longitude will become unavailable. + +The currently identified unavailable functionalities are: + +- GNSS localization +- Sending the self-position in latitude and longitude using ADAPI + ### Using MGRS If you want to use MGRS, please specify the MGRS grid as well. @@ -39,6 +48,10 @@ vertical_datum: WGS84 mgrs_grid: 54SUE ``` +#### Limitation + +It cannot be used with maps that span across two or more MGRS grids. Please use it only when it falls within the scope of a single MGRS grid. + ### Using LocalCartesianUTM If you want to use local cartesian UTM, please specify the map origin as well. From 3b9bd421e486405fc26a5a606e79069446ac4ca4 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Wed, 24 Jan 2024 19:08:13 +0900 Subject: [PATCH 111/154] fix(gnss_poser): remove gnss_frame from schema.json (#6155) remove gnss_frame from gnss_poser.schema.json Signed-off-by: Kento Yabuuchi --- sensing/gnss_poser/schema/gnss_poser.schema.json | 1 - 1 file changed, 1 deletion(-) diff --git a/sensing/gnss_poser/schema/gnss_poser.schema.json b/sensing/gnss_poser/schema/gnss_poser.schema.json index 6d9bc716e121a..d6e104b7432f2 100644 --- a/sensing/gnss_poser/schema/gnss_poser.schema.json +++ b/sensing/gnss_poser/schema/gnss_poser.schema.json @@ -42,7 +42,6 @@ }, "required": [ "base_frame", - "gnss_frame", "gnss_base_frame", "map_frame", "use_gnss_ins_orientation", From b5896a3c9ee751aede8eb04b903d469c14e8408c Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Wed, 24 Jan 2024 21:13:46 +0900 Subject: [PATCH 112/154] revert(perf_ring_outlier_filter): temporal revert for further investigation (#6148) Revert "perf(ring_outlier_filter): a cache friendly impl (continuation of VRichardJP's work) (#4185)" This reverts commit e45c1ce8df336aef408650532b0477dd174ae27d. --- .../docs/ring-outlier-filter.md | 13 +- .../ring_outlier_filter_nodelet.hpp | 16 + sensing/pointcloud_preprocessor/package.xml | 1 - .../ring_outlier_filter_nodelet.cpp | 342 +++++------------- 4 files changed, 123 insertions(+), 249 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index eedc8e6bcb573..42ca694235a00 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,12 +22,13 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------- | ------- | ------------- | ----------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | +| Name | Type | Default Value | Description | +| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | +| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index f655a9245ca6d..ab7d4e0012304 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -46,6 +46,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter double object_length_threshold_; int num_points_threshold_; uint16_t max_rings_num_; + size_t max_points_num_per_ring_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -53,6 +54,21 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + bool isCluster( + const PointCloud2ConstPtr & input, std::pair data_idx_both_ends, int walk_size) + { + if (walk_size > num_points_threshold_) return true; + + auto first_point = reinterpret_cast(&input->data[data_idx_both_ends.first]); + auto last_point = reinterpret_cast(&input->data[data_idx_both_ends.second]); + + const auto x = first_point->x - last_point->x; + const auto y = first_point->y - last_point->y; + const auto z = first_point->z - last_point->z; + + return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; + } + public: PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index e3090f34d6854..a65d14c0a1194 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -31,7 +31,6 @@ pcl_msgs pcl_ros point_cloud_msg_wrapper - range-v3 rclcpp rclcpp_components sensor_msgs diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index e277b221a090d..d2570b9c4d786 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -14,18 +14,10 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" -#include "pointcloud_preprocessor/utility/utilities.hpp" - -#include -#include -#include - #include #include -#include #include - namespace pointcloud_preprocessor { RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) @@ -48,6 +40,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions static_cast(declare_parameter("object_length_threshold", 0.1)); num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); + max_points_num_per_ring_ = + static_cast(declare_parameter("max_points_num_per_ring", 4000)); } using std::placeholders::_1; @@ -67,252 +61,122 @@ void RingOutlierFilterComponent::faster_filter( } stop_watch_ptr_->toc("processing_time", true); - // The ring_outlier_filter specifies the expected input point cloud format, - // however, we want to verify the input is correct and make failures explicit. - auto getFieldOffsetSafely = [=]( - const std::string & field_name, - const pcl::PCLPointField::PointFieldTypes expected_type) -> size_t { - const auto field_index = pcl::getFieldIndex(*input, field_name); - if (field_index == -1) { - RCLCPP_ERROR(get_logger(), "Field %s not found in input point cloud", field_name.c_str()); - return -1UL; - } - - auto field = (*input).fields.at(field_index); - if (field.datatype != expected_type) { - RCLCPP_ERROR( - get_logger(), "Field %s has unexpected type %d (expected %d)", field_name.c_str(), - field.datatype, expected_type); - return -1UL; - } - - return static_cast(field.offset); - }; + output.point_step = sizeof(PointXYZI); + output.data.resize(output.point_step * input->width); + size_t output_size = 0; - // as per the specification of this node, these fields must be present in the input - const auto ring_offset = getFieldOffsetSafely("ring", pcl::PCLPointField::UINT16); - const auto azimuth_offset = getFieldOffsetSafely("azimuth", pcl::PCLPointField::FLOAT32); - const auto distance_offset = getFieldOffsetSafely("distance", pcl::PCLPointField::FLOAT32); - const auto intensity_offset = getFieldOffsetSafely("intensity", pcl::PCLPointField::FLOAT32); + const auto ring_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; + const auto azimuth_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; + const auto distance_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; + const auto intensity_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; + + std::vector> ring2indices; + ring2indices.reserve(max_rings_num_); + + for (uint16_t i = 0; i < max_rings_num_; i++) { + ring2indices.push_back(std::vector()); + ring2indices.back().reserve(max_points_num_per_ring_); + } - if ( - ring_offset == -1UL || azimuth_offset == -1UL || distance_offset == -1UL || - intensity_offset == -1UL) { - RCLCPP_ERROR(get_logger(), "One or more required fields are missing in input point cloud"); - return; + for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) { + const uint16_t ring = *reinterpret_cast(&input->data[data_idx + ring_offset]); + ring2indices[ring].push_back(data_idx); } - // The initial implementation of ring outlier filter looked like this: - // 1. Iterate over the input cloud and group point indices by ring - // 2. For each ring: - // 2.1. iterate over the ring points, and group points belonging to the same "walk" - // 2.2. when a walk is closed, copy indexed points to the output cloud if the walk is long - // enough. - // - // Because LIDAR data is naturally "azimuth-major order" and not "ring-major order", such - // implementation is not cache friendly at all, and has negative impact on all the other nodes. - // - // To tackle this issue, the algorithm has been rewritten so that points would be accessed in - // order. To do so, all rings are being processing simultaneously instead of separately. The - // overall logic is still the same. - - // ad-hoc struct to store finished walks information (for isCluster()) - struct WalkInfo - { - size_t id; - int num_points; - float first_point_distance; - float first_point_azimuth; - float last_point_distance; - float last_point_azimuth; - }; - - // ad-hoc struct to keep track of each ring current walk - struct RingWalkInfo - { - WalkInfo first_walk; - WalkInfo current_walk; - }; - - // helper functions - - // check if walk is a valid cluster - const float object_length_threshold2 = object_length_threshold_ * object_length_threshold_; - auto isCluster = [=](const WalkInfo & walk_info) { - // A cluster is a walk which has many points or is long enough - if (walk_info.num_points > num_points_threshold_) return true; - - // Using the law of cosines, the distance between 2 points can be written as: - // |p2-p1|^2 = d1^2 + d2^2 - 2*d1*d2*cos(a) - // where d1 and d2 are the distance attribute of p1 and p2, and 'a' the azimuth diff between - // the 2 points. - const float dist2 = - walk_info.first_point_distance * walk_info.first_point_distance + - walk_info.last_point_distance * walk_info.last_point_distance - - 2 * walk_info.first_point_distance * walk_info.last_point_distance * - std::cos((walk_info.last_point_azimuth - walk_info.first_point_azimuth) * (M_PI / 18000.0)); - return dist2 > object_length_threshold2; - }; - - // check if 2 points belong to the same walk - auto isSameWalk = - [=](float curr_distance, float curr_azimuth, float prev_distance, float prev_azimuth) { - float azimuth_diff = curr_azimuth - prev_azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; - return std::max(curr_distance, prev_distance) < - std::min(curr_distance, prev_distance) * distance_ratio_ && - azimuth_diff < 100.f; - }; - - // tmp vectors to keep track of walk/ring state while processing points in order (cache efficient) - std::vector rings; // info for each LiDAR ring - std::vector points_walk_id; // for each input point, the walk index associated with it - std::vector - walks_cluster_status; // for each generated walk, stores whether it is a cluster - - size_t latest_walk_id = -1UL; // ID given to the latest walk created - - // initialize each ring with two empty walks (first and current walk) - rings.resize(max_rings_num_, RingWalkInfo{{-1UL, 0, 0, 0, 0, 0}, {-1UL, 0, 0, 0, 0, 0}}); - // points are initially associated to no walk (-1UL) - points_walk_id.resize(input->width * input->height, -1UL); - walks_cluster_status.reserve( - max_rings_num_ * 2); // In the worst case, this could grow to the number of input points - - int invalid_ring_count = 0; - - // Build walks and classify points - for (const auto & [raw_p, point_walk_id] : - ranges::views::zip(input->data | ranges::views::chunk(input->point_step), points_walk_id)) { - uint16_t ring_idx{}; - float curr_azimuth{}; - float curr_distance{}; - std::memcpy(&ring_idx, &raw_p.data()[ring_offset], sizeof(ring_idx)); - std::memcpy(&curr_azimuth, &raw_p.data()[azimuth_offset], sizeof(curr_azimuth)); - std::memcpy(&curr_distance, &raw_p.data()[distance_offset], sizeof(curr_distance)); - - if (ring_idx >= max_rings_num_) { - // Either the data is corrupted or max_rings_num_ is not set correctly - // Note: point_walk_id == -1 so the point will be filtered out - ++invalid_ring_count; - continue; - } + // walk range: [walk_first_idx, walk_last_idx] + int walk_first_idx = 0; + int walk_last_idx = -1; - auto & ring = rings[ring_idx]; - if (ring.current_walk.id == -1UL) { - // first walk ever for this ring. It is both the first and current walk of the ring. - ring.first_walk = - WalkInfo{++latest_walk_id, 1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}; - ring.current_walk = ring.first_walk; - point_walk_id = latest_walk_id; - continue; - } + for (const auto & indices : ring2indices) { + if (indices.size() < 2) continue; - auto & walk = ring.current_walk; - if (isSameWalk( - curr_distance, curr_azimuth, walk.last_point_distance, walk.last_point_azimuth)) { - // current point is part of previous walk - walk.num_points += 1; - walk.last_point_distance = curr_distance; - walk.last_point_azimuth = curr_azimuth; - point_walk_id = walk.id; - } else { - // previous walk is finished, start a new one - - // check and store whether the previous walks is a cluster - if (walk.id >= walks_cluster_status.size()) walks_cluster_status.resize(walk.id + 1, false); - walks_cluster_status.at(walk.id) = isCluster(walk); - - ring.current_walk = - WalkInfo{++latest_walk_id, 1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}; - point_walk_id = latest_walk_id; - } - } + walk_first_idx = 0; + walk_last_idx = -1; - // So far, we have processed ring points as if rings were not circular. Of course, the last and - // first points of a ring could totally be part of the same walk. When such thing happens, we need - // to merge the two walks - for (auto & ring : rings) { - if (ring.current_walk.id == -1UL) { - continue; - } + for (size_t idx = 0U; idx < indices.size() - 1; ++idx) { + const size_t & current_data_idx = indices[idx]; + const size_t & next_data_idx = indices[idx + 1]; + walk_last_idx = idx; - const auto & walk = ring.current_walk; - if (walk.id >= walks_cluster_status.size()) walks_cluster_status.resize(walk.id + 1, false); - walks_cluster_status.at(walk.id) = isCluster(walk); + // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) - if (ring.first_walk.id == ring.current_walk.id) { - continue; - } + const float & current_azimuth = + *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); + const float & next_azimuth = + *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); + float azimuth_diff = next_azimuth - current_azimuth; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; - auto & first_walk = ring.first_walk; - auto & last_walk = ring.current_walk; - - // check if the two walks are connected - if (isSameWalk( - first_walk.first_point_distance, first_walk.first_point_azimuth, - last_walk.last_point_distance, last_walk.last_point_azimuth)) { - // merge - auto combined_num_points = first_walk.num_points + last_walk.num_points; - first_walk.first_point_distance = last_walk.first_point_distance; - first_walk.first_point_azimuth = last_walk.first_point_azimuth; - first_walk.num_points = combined_num_points; - last_walk.last_point_distance = first_walk.last_point_distance; - last_walk.last_point_azimuth = first_walk.last_point_azimuth; - last_walk.num_points = combined_num_points; - - walks_cluster_status.at(first_walk.id) = isCluster(first_walk); - walks_cluster_status.at(last_walk.id) = isCluster(last_walk); - } - } + const float & current_distance = + *reinterpret_cast(&input->data[current_data_idx + distance_offset]); + const float & next_distance = + *reinterpret_cast(&input->data[next_data_idx + distance_offset]); - // finally copy points - output.point_step = sizeof(PointXYZI); - output.data.resize(output.point_step * input->width * input->height); - size_t output_size = 0; - if (transform_info.need_transform) { - for (const auto & [raw_p, point_walk_id] : ranges::views::zip( - input->data | ranges::views::chunk(input->point_step), points_walk_id)) { - // Filter out points on invalid rings and points not in a cluster - if (point_walk_id == -1UL || !walks_cluster_status.at(point_walk_id)) { - continue; + if ( + std::max(current_distance, next_distance) < + std::min(current_distance, next_distance) * distance_ratio_ && + azimuth_diff < 100.f) { + continue; // Determined to be included in the same walk } - // assume binary representation of input point is compatible with PointXYZ* - PointXYZI out_point; - std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); - - Eigen::Vector4f p(out_point.x, out_point.y, out_point.z, 1); - p = transform_info.eigen_transform * p; - out_point.x = p[0]; - out_point.y = p[1]; - out_point.z = p[2]; - // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI - std::memcpy( - &out_point.intensity, &raw_p.data()[intensity_offset], sizeof(out_point.intensity)); - - std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); - output_size += sizeof(PointXYZI); - } - } else { - for (const auto & [raw_p, point_walk_id] : ranges::views::zip( - input->data | ranges::views::chunk(input->point_step), points_walk_id)) { - // Filter out points on invalid rings and points not in a cluster - if (point_walk_id == -1UL || !walks_cluster_status.at(point_walk_id)) { - continue; + if (isCluster( + input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), + walk_last_idx - walk_first_idx + 1)) { + for (int i = walk_first_idx; i <= walk_last_idx; i++) { + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + + if (transform_info.need_transform) { + Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); + p = transform_info.eigen_transform * p; + output_ptr->x = p[0]; + output_ptr->y = p[1]; + output_ptr->z = p[2]; + } else { + *output_ptr = *input_ptr; + } + const float & intensity = + *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + output_ptr->intensity = intensity; + + output_size += output.point_step; + } } - PointXYZI out_point; - std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); - // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI - std::memcpy( - &out_point.intensity, &raw_p.data()[intensity_offset], sizeof(out_point.intensity)); - - std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); + walk_first_idx = idx + 1; + } - output_size += sizeof(PointXYZI); + if (walk_first_idx > walk_last_idx) continue; + + if (isCluster( + input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), + walk_last_idx - walk_first_idx + 1)) { + for (int i = walk_first_idx; i <= walk_last_idx; i++) { + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + + if (transform_info.need_transform) { + Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); + p = transform_info.eigen_transform * p; + output_ptr->x = p[0]; + output_ptr->y = p[1]; + output_ptr->z = p[2]; + } else { + *output_ptr = *input_ptr; + } + const float & intensity = + *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + output_ptr->intensity = intensity; + + output_size += output.point_step; + } } } + output.data.resize(output_size); // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform @@ -332,12 +196,6 @@ void RingOutlierFilterComponent::faster_filter( sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); - if (invalid_ring_count > 0) { - RCLCPP_WARN( - get_logger(), "%d points had ring index over max_rings_num (%d) and have been ignored.", - invalid_ring_count, max_rings_num_); - } - // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); From 007d1565acc991cfb7d2fbf1c529922682ea65d9 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 24 Jan 2024 22:33:47 +0900 Subject: [PATCH 113/154] chore(shape_estimation): remove unnecessary param (#6157) --- perception/shape_estimation/launch/shape_estimation.launch.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index 65d1944417cc0..f13b5d1e5f273 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -4,9 +4,6 @@ - - - From 19f4484365bd8dc81bf54472efa96cce693e0d80 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 25 Jan 2024 06:56:26 +0900 Subject: [PATCH 114/154] fix(planning_evaluator): fix crashes with empty trajectories (#6129) Signed-off-by: Maxime CLEMENT --- .../planning_evaluator/src/metrics/trajectory_metrics.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp index d0d8d61c4c231..53265872bffa8 100644 --- a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -125,7 +125,7 @@ Stat calcTrajectoryLength(const Trajectory & traj) Stat calcTrajectoryDuration(const Trajectory & traj) { double duration = 0.0; - for (size_t i = 0; i < traj.points.size() - 1; ++i) { + for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double length = calcDistance2d(traj.points.at(i), traj.points.at(i + 1)); const double velocity = traj.points.at(i).longitudinal_velocity_mps; if (velocity != 0) { @@ -158,7 +158,7 @@ Stat calcTrajectoryAcceleration(const Trajectory & traj) Stat calcTrajectoryJerk(const Trajectory & traj) { Stat stat; - for (size_t i = 0; i < traj.points.size() - 1; ++i) { + for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double vel = traj.points.at(i).longitudinal_velocity_mps; if (vel != 0) { const double duration = From 1aba0ac0ade8ee292b4628bcd50949f167e66447 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 25 Jan 2024 08:32:31 +0900 Subject: [PATCH 115/154] feat(tier4_automatic_goal_rviz_plugin): make it possible to register checkpoints (#6153) Signed-off-by: satoshi-ota --- .../CMakeLists.txt | 1 + .../AutowareAutomaticCheckpointTool.png | Bin 0 -> 18815 bytes .../package.xml | 3 + .../plugins/plugin_description.xml | 5 + .../src/automatic_checkpoint_append_tool.cpp | 122 +++++++++++++++++ .../src/automatic_checkpoint_append_tool.hpp | 91 +++++++++++++ .../src/automatic_goal_panel.cpp | 125 ++++++++++++------ .../src/automatic_goal_panel.hpp | 4 + .../src/automatic_goal_sender.cpp | 8 +- .../src/automatic_goal_sender.hpp | 16 ++- 10 files changed, 328 insertions(+), 47 deletions(-) create mode 100644 common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png create mode 100644 common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp create mode 100644 common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp diff --git a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt index c6d4e30061626..cdc57743a6cb1 100644 --- a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt +++ b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt @@ -11,6 +11,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED + src/automatic_checkpoint_append_tool.cpp src/automatic_goal_sender.cpp src/automatic_goal_append_tool.cpp src/automatic_goal_panel.cpp diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png new file mode 100644 index 0000000000000000000000000000000000000000..6a67573717ae18b6008e00077defb27256415663 GIT binary patch literal 18815 zcmc#)ig}7!VL~0BI3P>4u>jX{5WQyQGE==@gXiZcsv`yJ3)ShVHnpzyIKV zx%UIl!@PCQ*=Oy&*IwtGaAid)9L!gkAP@*gMjEOL0wKly`$K;YTnXsp8Ulf2BxRtY zY95&f%V-`irs2ZJD-E_>W|QJ#$f(lDGUzn9FhfdJ6ls+f7g13tl1h)8=nGVpD&-eU zEXJbkUMvxiN-bt%sdy?0Ws#;2U}Tww$D9k?tJrv3b90N4w6q7mY>Vu>j)cq`Gq>&g ztxVotd7x=_%0QrfCTU6#h!Tqc9R&LG1r-?ts`)>?Y`h5pRS_lWag8OXawfXJHk9@r zwSCxzbPRm-g9n1hq$!Q)8a9)#!i(M_cz&h|6vnpsyKVD7twGSL0-4KL6Q=BD1ZeP- zzfzYzWVM-q+?d)>KwJl;5;rOeaVP-p8?mb$R~c z!r4STVAB1bJ?W`Q1N^wk6a>;QVI%JxOu}Vf{^G|)Co3MGH{(9RMV<;nHU4Za)%KSC z`;I=`QN*K~)?CjlT%q3CJ0d$G%J_7^;$JI4Ms3f{6oyJDbjK_N!4ju2 z&$?3P$rm$WTureE-aq`r%H%qf&xx}1+^E65XE7*U*@ASHv#;8ex#{Xeuo?I&+x*og zCp;~E10-qE582|U!y4K~`SF#B31Gjpj3?LDSx6hf<#=NRr5x>o;P$^l9osYrEfL{WYjQydU9;g?K5#3k=W*FphTCuCHewl5&`Uu9F)(f>MMOxB>pE z88huBN|GrNq|eH~M(ELpl-tdZkMhs%;OV+^2?6{7^3=m>h4(Sn1AF4SLTeDL;O9g! zBWaM6c%X0DechlP8B_BUQ^1_B5ImG0q$&AGba@Q1!Sx#uv;b)FNlKJ%FF!gu4goAi z+jugMiX+lt3%Db%$x}1fa)yh%A&xxGW-Lle9BaRZcr(Jl8YJvmXVoM8b*d?nDn zr>xqjT|4gjOmgjyb!-I_L=1WSO(};jF5)F9(KK9k$6juv0YKyrQRHz_DK~8jI2)+w zm-Yt$QfvYLhOutzxp@d+SUG4qO=vJ%C2^D=7atD;N#3;^c;DQzmR&&!oPfkcBJT}x-4FY#kT02m@DxoqS-{xT|$ z3PAj4Y06h5S$bR{FjZhmf2ImPCOB3Cm{OLqda0jhqvfLSxmZ+?A2<^h+z64sYjMk*OaM^512bTNz1yJX#3oL;p>F~$69vB)J^K&J0kyCuHxo2qc9?N7 zpx^)Vh!`XhEMW3-8hQ#=jbwx3L_Yk3cspR5f1h~;US}c=&En=n$AFqJRa-Trw&}AF z#AgA`>wx2@JWAg4l9l5}1GY|0Z9CNsXNdgw(z**Vj@S4BOp0I}AA5Z|Zl~>Z3kVbj zqQ+>~?`j3P7S~|HAAK#cX8I@4)zn@Yx%DiDRJ@~c4bTC4ySPXJL&So)v8RA@6#{J%M_y$DCRzdd0*FPR zEH--gPG5+|{w5Yegr~;hMSkzb)vYj91mGV5xcRrf*}fIu`8(Gk(oJHQTMWSTlpe3W z7U|f@97Ozui1Pqn>xtK!34&Gp1$<#-lg>4RGs7K!6%!+v&|>f%Vhw^400u&sGY~q5 zJ^R2hK)+x%+?N_ujGMppd5=D@{V_pS_0HC2*7x*eApoKv>S`$MhIW>f zMXBEn^5P16FfqDGOxdOTX$;a&7%;G-28$P)$1&8Il!2^?K;(-9A|F432ZYVYM^|}I z3>OV1q`?mb9wc7}dJzwJ)_0FSe8o7Cm(><8hzS0{vBcWu7VZ9!xVd$!*%0qZqPEJb=%hJ$Z_L zz~Miu9bQOq9)39Df;+w>C4k8SnA^IR``!NH4|SUf61t|;@Y}Cs7L7w2kT8`ufki#> z(|*WcvwwZbRQK{xx%^U^9it@ui(i`PZ(a!{U!?$lf%er!QwQi?Jq@BTnXsQ0^KiZy z)@H)?s!l`=|Lg4jvo;>IFs7CP;9l8V4UpAo>sjIq#`%v=kuRv~Dh zsiW)pz26t!@MhZgf>?_yj35jsQu16~;-8_l{?rj9%$d(V-9L6Mv^t{|GGZ5-C`W&WqNh29e6d|hsvmvzE$*g zQmyMXe|TD_9?2}u%kdXf_x#(4xd=B4sAmzB{R*=+ZDMv*{0=ZZw`U z1zHnRLzGA%_kVvk=&UnLH`*nK6OLz)IZ+1RaP z$_a6)pzGO}t0_(S2EeN@?u0pWGf^QOT;Fy&fIf+061Vv&k6#eQ9E?CJbN7 zR~KK=eS#kf?q9C)BZqF)@T7l`iF(-E2|R4|?91b}4Ez)Kk2FyYsGq$iY-#*_u@~1} zergZr={EWSme9lc1Lvw@G~X6iq*U zJ>6TJJ1l)Wu8Wk+%I_jO@X>ttg#2Gtvr6H&5?wENhU3wfOD|1HK$EE0JWPIo?o%@w7QX>1z=lRIcDYmm^RdaH!J$U0qdKjR{v?Nj@k8=mLg z!QaffX+{G^hFLx>yzv{!{4I7+*?C}Mw;W;Akw&+^oGNB zKbo1D;RxOCe8*HY+Wq2Z{a&$bq>kL0E*5uqV%bZU97(Rgg}axR>R3#V{3&SGo$cMU zo>il+6}(CD_7>Sn+RG>LD*ZH>q}A^P_|jLN;2-ZSZ)e^-CR7g&UCtF)C)#ml2sjLp zna0G_?3W<_14PCTp7u%y`1jJLdPCb;WW==}ZA1fH$8?tCsR>bJF?vJ$I|J{? zlTYZ~d#P!IC7nc5MI zRsbBkP4l%D7PaBB{&6DKt4yR^NwBqPM*lr^I~U=AH?+LJ*&C$ZAXr^~8P$6rvc|OQ z@5*T^JLt$TP5T0t(=wiG^sXg zEd~AVuZVrio-s!T&LrqYTve>D*P81_=>7;qF3oJb%9-_$ zl7tF5{GK`$Y)Po@y_Df6U|SUSg7!xbv_*4yVe!Q#Ebc;ZcwoSK)rHUB2ho1zr!2(h z%dVk7oadz}SQ849EKz@)p|3b8py_+bZq+KNZ|#0KdDLC_ZhWXy#9XXyyOh3&D_KBd$d5r~dZ&@?gHCJhG5mniAQ@R>y{O zFvmlJ?JS~a-%K~9k;T?h@u&=-btl<6W(Mr?#p_#Nvx_0v`ilrR<$^+%&Dej}o4vNPn* z;owRb6rB0y*XFvkd6OhCQqGB6I?r)a!X@j6i&CJJ1K0a?>M~fR$7l@HLqkBtR{DNi z#>_e~=e6CHkiV3$L3^TB!NYiWD6Xr+?LjRJ218fc9?44c9VrfHEASeRf4u5=;4Pz2 zbmAarlXGC+<}+MNXuEB2%{j_j84W=YGt*J6aB$pdMP%f^yA?e@n6EWm{~fLLbbmH# zB#6dX=5Pv<(zsg%*jkAKL5r0knZM%tivqc1CatP(x82cXPdwWwcmjs-W@q|ImYY99 zYwlE~Y-hze~lk`x3mjPB!27mzk`xg^y;8^_L_ahVv6~)|t~mfqeU``w(TZ zxrTjOPssfK`oySR$6>Am1>m)tK+)!_P=0!6FfqlwTf!xubMJn*u$lh(uwJ8c<*Bgn zDIx;XIk9`S^Zs-&ZN{2BW%h21L^x#Hy{t&3#$u%2EIrNNs9uMGjS4;G4WkVIc79S?9<_)}5sGh^o2GGu01pfp6?z7wr_`sf6M}tS@hT zNCXk5MDOIEc)b#>BL5Byp4r;f?uV$qGqs%c8FpR1U>>e-v`u`iQ65*US#Q)pH|_z0GX?kiG%%(LOU!%Ckq@v`f@JK|pfX2{xiK)~<5soB5N zZ^n6hgiX)Z0q$xxBO+$zshAz%JT;G?R^ui!yaQF=CYolEefA_D418x5EBtuv6S_T# zt>8C5ZK^P2{Tk3jP{74C9i5Z*XB90Em|L^1Lp$Ty?ZVFx%zQ38_!1Hlo_wx*Jxm>5 z;cSTnM62^vnW0! zv(<%*u%dQ#ZEdDm`^UPysNb89-plAwdG@K0)y_jBD-qUQUN%)_5(TsN78%{K)f*=5FXVTI-5jC-M0r)_e2{Uu>Io;LFZ)WTSayCh zH$y%7yq?2_WZ>t-lJJ(ErM5;8)CMsVjU&indTZFQ!V%ji9)yN-+O%Xp_zMN?x2c&~ z$h26bQIZIOY$0|Hl-A{A>HW>D%Vl99Us^*}%6q+6(o_lL&Dq|Mo_&23{zG+CyVGoa zZdPuB-3?-rE;hgTQB^{1j%hnpE81vUZxe{q7+r62)9R zQjuMBfm(#Ms?@4Z$;Ik^iw9oWqRW`6isW|}$3~+7uuRp_uSRo5+uoV%o;%fYOqe&( z|2-;yrIE5RJ-g8wo6hVoIj1wlF#nl)eXp0d4XYB-b^eG|Cv?<)1s7iS_r5%WWTvIf z2b#SW2vI-|e@Dud15=%5ScGn5xI-4JpMXeprsukuG_&3pJ9vw|;{D{iSX$ZsyN-QS zIkazCCe z5tjTIqBD|W;A`f4 z(o2%2vBi}U($ZixbedSg2KMZ`WSTOQSc*AmMXpEHQN>d_Ts3UpJGs9*!n#G=T_Pyr zRXRI_^rJ$49%InZRr>IOdrQ?H&qWVYSv0t<)_MjA=@l6!>}m{uAv-nCbu4CkNL1I0 zszx2HgbeMhEPPFWT{;-iq%i3PmyXoFdjD&`g5k23{NZw;w9<>q|Iu^oZgl-bE6dSw z^mlKsUR{#$i%`XX%3&%n7Tqbvr-=iv3Vn!kYG;%8(aknQHmT914|hK(l9+ATj)ON8U+A7k}qtQoh z(o1n!2r2de-qqvX8LPa^T2)TVwotK{{5PUlaF}h}t$?CyJIDXX_j2AsLij`a@Rsi6 zT9`WmSJ@(5~J3(YDA@J|o3An#c=nA#EWWa`laHX*T$SjobapA&u?(UWP+Q{C%@7Q}QA^ zJNv{`cm#_$-9Pc`0QTcIA;7WaQ;?NPKQ)wA2wg2XAF*`a%1+DL{Ei|8qxs#0>_+qE zIygdQ6ZJGXrnY0=Ug)~OeO>jx(RvJib8E2<`5+hRj<=UQ`XSK&;bJyBjKUK-vv>=H zGCKa(@!Sj)ME<{YC-0A1b(Bqx63NF8LaQ+pwr7++?Y;83W-@6>tv_S?kk#VM|MVs2LY-DySvyPfa--K5g8|I^)y zxR)i5=c%cxpXe{y&Af@{;>*~{b02i&{aB_$54p;>&#@87qjme%iw$;g@1y3hdBV!k zZH`JGW7Tng^Ak~%}#;Q-2Kv@J$5 zPoiIQ%EmkU-JkTeJJ;0SF{mVKykranq(ebDsHb&e)<5FL`@L~x6=ryQdf$-7vCQ0< zVKw1(9v>zMv~Hr4MzS?k%k=W&z-eNFmOLHeG{ZZM>d@B7MqQoA_xwa^NGPaDE9_=G zqs6AdhWYsONzd@+OaTr-8XOBq(wvBuPJ?Raqs7jrj-Q(vB?55luOh~nXxNd`{VH~I zM;8yWZJr7r(}dDk^xNhxwsalXR!sA1N(T6xx8%3 z9i?;He4?%(U&RqXs^zm7y_}z_XZ`rKJWdqi+!lDPbqjFCc03mCyq$|H4*?12d18Xu zsc9-IL6-9k#5yU9z|37?M|bFc*A_tKQa1XE*M2$dVt-b%RtQm5BcL!+`EtBIz{=SB z>EVhR7s6-iy&6K2n(c0w;sbP}fDO(N@z^>W*1LXDq9TP$lSj^&IGFi{KpGIP-++mV zR2tI$TwO=$)6FcJ+Rs%5-rd7wzS5;BWefCwh}pkOiJM7&g&1R6o^Ft!tBsfjK$~LV zchl!;(BZ>dy(Cy;~wVJ*0yFzq6!EcXR5$rY?4gI54mUi2JbS2$QJGSQ5Xyxt;M>AF4Sv@28`bN@} zlqwK4bFtGi`_*QHwN9+3#N)ub$|i36r9T2zdP63$?>Q-i9luzG2$gB(w!3~+Q&2P3 ztg~7hwvPLAu**HR*1BKO`siW+cbQb!0_qL(aGzg2%;p*MKf-C??dsxN<>Sk(?%vrz zzc_PV+EeGDFeZ;i84*ZtW=H3=#2j|W^|ND<(N6Ys^vYL`H`p!um`KLX43QJ|#XvZ& z5VNDI>gv4*nZ&heTY8$rMpSG1;mfQ=IFry0%W*2X;r!dHgLxC(0(fUSoX7 zwNjEXD0t!s_`m5$=5BFW$;8yBAcYLBacVr_p>vrk2KwkD((ynWWJf^m6M0>Rs^j&K zcuG{Bb;k~FsXL?edF#}7S zHZDLmq@^Y_Y4fI*ecn|MwalQipJKC!`&+AEi=Oxumg^tYvS0s6v}&dED>#4Eap zAJNq9yf;-G>9>swxt+fKLmkVyKVy>_Z%8^@94F5{sE|(%yu=RF} zU|f+?>1X;0!Jp9#;p0r9dszlE)NBFz%Xb>g4B!|F;iIe%Rte_9u|W9MNnz3b-6obM zRP{c^sD1ZD1@8DThqDat@NK>7r_aG&(BpD|3!JTXho#kHn#b%IMn={-%208He(I{j z8s5K{sLB<8*d9?YNvdlAkE1Xq4M&^5V`}zED!eajyno=M;-hVhny2RT;+R}Z9r4hs zHg0|z%N0-eaMaHz%=Q`>bdt9ENtBg7zrZD9*0FRwt#|ss^{%z@XNh|0y-*Iz&V$F+ z5b1tlj^6+i1CSkW24b4a6t9GRb-61JetIWG3TakcE8lU{iJ1*WfY zhT3M?F_AEMwx8^2o>J9?B73d2M-Ao0yblIV=w|pGk7+ zChG0Dy~rHOK1Z}(>0iZDOF^?7HV2aYQZklSUv5SgTzl!7E#o4p(rOC24ScRx|^VD=~3JdRynp4w?)W(5A0 z)_C3;5w{#ut;AeK9af+D3$6?#Btje>?{BQL*L-vD2I8Ik7Vg!@QnLe zppa1pgsr!=`wq1_mTHZ}C3xl@K9Vf_gQQRAD_36GdaZ87H=x1+REu&}7i5gjPv`MP z?=NN3^VvFySA4Is?wUDmnv~`eOyF%d=CNUipe$ZOJ}FZgXz3+QW*Yt>xpMko-JL05 zks6_=q<8Zb!SRc%GSdhvd#^TbuH?h6Nef0sA3Wjvs`Q+zGw6AI#HO>wHLBBX;iyJI zFZ5A$b~Ji_KEX=v=l0q7@oKkklDfM5JEnZw<(BE1>)c*>-8TRsJgfKBDNYu#C!HGU zbLvbP6<#?=;4iU0T&Pb*LBnMo{A)%V&E)8oVNDoVfNMGIGqDC~Tscy8s*HW5h(C^+ zG%$nRO&>=ccGarn_c)}SF|TH0#$7Cx8+pN%20+veAhSU?RF1MOcSo&5t!{_L=^j!S zwpKQodunYpRvLi#(OCA4LtJoKXSCHK;j8k9qyr5?e~KQ|t>1+SXe) z>)&;E*Mlsb=gfL7YJkKP1nqI#dXEOw6i8@(OKrTvK3jR+J9~Xn-iY+EwA~bLWlsRR z23NE2O*dU)=1+e<1VW`GQj_6h3MbAnyDHL5QY*f|gl9W++|d2#xN2PS<3Ii9G|AA= zOQyQ5g5I}YLVzr($N9aHm+H&mo*)(pv+|j@cP5Nnz1nAZ%$JsKX2hLXNhUIS$$QnG z1AL<~%s^invTXLbb=gI`FD6^i`_j_7{ay3FG_nui=Feq_SET$RM11j`*GRQ*M5-@2 zd2E<{+liz^%K)8H_K-`#W19`%J018J8$M5t{XUL%67!Kv?T5Y6%HoSPfj9xo@SRNA zO&i%cnz)EbdPRxH)73v%VP9f)rk1ApgoAeZVYB+))9p8Bqd4&bBR~*`oNY>09O~N2 z!Rp>7KnHX=U8>;-697~ZJ_sN<+Irs|Hl_`;G1z694OixT`*V|Cz_YBGLc(tM@Kohm z>JD&MYd3Xf+B|B;lk$xwEPD9_~%=V(vx(C@a6Y-H$#=bfVOn57$&>rkwrVs4#bi zIXh7e(IM6w&y9tw+~I(;y6HV5=yM_(oXxC~r9=|;e>##2d}?n{dgmZtm$kdn2_X2$ zZn1&1c*eQwvMIIRLX^A-fxxJ(8WBPe?||LSuy z{t{iR`A!>-41uqe*4_8|FWHQLq|ffyeq*aJzDYNo!z||t!@KwR;k$ICGOlv)`wp!Q zFe7=&V)-C1g>f{5f5E>X@5^pc*;|x_)H3t0Wj(Hdf}t6f7dj;Lm3y|b61$Fgis3HI zZZTE=iadp@o=Y|={({-`cL3Mp!SitW{cQ0t++xA?d#mq<7jHsy-_FH#^te&BG6e9=TM4K?yE4$gLtJB{QzeetB zryUa6*x1vKi=Ur?^*FjI-@8jse%C$IlbLGM)=y!06b)*c>gqrMkCt)IoHxxwf;h>K zI$ZxUI0Qd&(@HM3+nD1SE<}@A-dyZ1FM9v|iBvCi@E-ah@k7-!4x7-5u`g+?MqO*l zm)P-Zzu$iR6iAlY+w9>D6kT>tx3it8Zf2XNw7OwQn=-dqz_;&l(OUI~)JrvPrtJfn zwVGJjkAIYJ@iHeSyf)eOw~G0ZJld2UGPIIZu3h{4455%Qbz6lZW74FexmYeYM;8aI z$K-h3Y^GFFCuw>-YX)_9?Ig^9A4wx1xQ2#eYVG!-LK}|bv$R8@eqlJgC_fD*}q9KhIE1f_i#O7_ zT)%x`)T#K2OJP6!v6Vb?t~cc+MQq6?;as!glC6mA<(e z<%lFRX`h0@1C67z0NHWh|ZjuVZ|V8t@FP@kIUYdtPp)2I9s3B_BXeBp}-ZP z5|l~AJ-|-~e7adHXKm6rZHQ~nBqb!qRwUhuE_?tsh^Kl=Hlz{jJv5 z%a`iA?S8PdtMf0~>M^6t2;7z3^z?rn(s|R}N+Jo!JO1-*rxQyqqw0F6X>RM!ACLOT zz2%2LmeJ%LT4#%}#EGCHI_{2UWy@EDyhONJs|(RvT?Z?St5 zJe~uJKn9oH8ehO#Qs>h$z{xbsOPOK-DR@vG_GmD%r#DqpIINVFeAg~=yeiV@S@ycUgHP|m(|5vgek*ZEH4RdwK{d$6jL>+4X z`4ChMuWjS9nJF{!ij95ygn<6)v21l;z0gZ@UjZab`9s9KK++(ZU}hS#jlPp@YHsdX+N+j<%}bAm_k9)T8@LAgyH4JzSyw>)>XO5eYZ-f(KZq4CZ8KAE}&p33a#nAw#ZL!zhkYZa0_fy&Jh12UfTAj4W?< z)O2(X-CC!M1+7n}8-n0j-m&cbyUhc#29lDYkyrH{#~wfcNz*@%?SyYHR3jK#qYkg` zfOxt;Xp>!NetI3?-j2$*c6E0peFJ1k0qtRyN~0J}eb0QIcI)6yPMaCaqc?e?=5%L0 z!wWTLgHwtNdLFYf->C5f&&TSGJKKO9egTz|q7H=dL_|bSuP9CLa~>76 znCtPqJkWLkR->ldp7iOcx({hE83#s3x$hnxA+ElF{8rgnRi*qnM5H$vfR4=(~l-hR-WQi)59iT6|TZNDzuRd5R$FCqNF)uguYRz#H!g{_d46@8|;6d)C31g+?wwl9UWUN!_7@iU(y8@`IP%@=plBH%=02yJ&ynGhwto!JlUOX z4ZlnR=UHTX(m73bJ`MY1G35^fepmv;GCh*P+1Gq7e{QeWW1q~jSlc~K9=z&1x!R5Sw+>Pg|4d7B*QMP1eov^xZ{boH28L>~l{Zhb#6){LZ zm@6K(L>wtG?EU!}!s|J9nZo{zoX5(&q`-6B*F~kgn1RWbz(S1+KjtWyba%eqCgn)S zFN@z*^(@_A3M$`75z6`%5E_kQVPm~8vUW(0-)Xpgi@MOw2BBVq0JDHsQp!G~mGIp` zt+g8jO^5Tn$1Dr}bH1cx#Vy}6vXegp0~5m=T_@{(WO?uY<)bNy2_cW;Z}e~9$_)RA z&R+=3N)8DzIPSQK_PD=ZR~&XP^$^GwPJke6@_WV`6v&1R5CO#>3rm*Q23Pe8MP7zS zN4FFUff1Z$-rRg+ zUOSJkl&izPHIn6>?4GZlL7(#}f01PSSIocavf%MkVJQEWW_pySvMFl!k06}^LD&vwY0rFM7Sdo{5 z!^6JOK>wyp&Wqx=>r``x(>dQU>14zTUKY_wnmZX{if`ugWtEtf5;FbfaR!RPMaJd=iiHkN)+{o zw=7_sEj@pbvWiNnS(}3oHAP5B$P(ALz1hl~T>aEY-XZhaZD8r8xs7nX)xEONDCjv; zOUTNmo&)Iqji8`liKzLOKAgKFJ{IZWuJ!WaoUryyAsQa}xqK??8(ZCEx7KBV%c+g} zKb}#TJ3DI#oj_gNy)Lv4->#&&w<8@3lV0qv{JbmRz6E=*>$kXAjlB;|IzAm_X;uCB zv75{DlZUCPX_L_PUzBOWE8nZ-zMA|iD#R`FU!}FRHDgRnO#Q#U-C^A-YHAzgg4T%^ zfT5UUrP9zgG!1;~y?UlKy%*}N2#Hw+^T|{6+|stQd7Vod9abu&XJ-I|3zTY{NPshIIhMP4(e)?%0Uv@SDj|pppr_Jb?SU%^oes09s*d=hLG% ze`5g7Pg|}#S=k6d+Fsg@%%xAxPEJlx7nk#=$7MiN?f*(I2$fs2BE8qna6zlDpw#AvWn%g%t!02^>Yj~m`Jc$>iH%XJ+e zZKMxoZxEm;V0$ox2?Us2H~)s3I>H<)HIzL9C2*B2%2% zuoz$qGG)~AC_jk0_89l)-w}Kjrep=sU*p1KA%bo<^)Jj!7x@I^8msO> z4>V)boi5cC6{?m%cYKi1zpQXO7Sj9MjcR~0ZwA;qj*h|~Zix)e+JeBwbFm_`UgUBj zgZ3CD_k(a;jH;dGQ*p3Bd%O`FzipXyFXu?vx-t zRRt(cFcEnGVV%fzDOH|TTM%e0EhS#yS{w_?&t<&+7NqhCIwh;+O2Q)sTsc~YfC3bU zq)x}416#3Ue7ioXI|D~PEaz~kUWr15fPZx;b=F9Tzwr0+<&|tGXq@ z-!p+0U!6Y(5C2L=UkFUuSvc=t6~zQS+H_VW)Q7oK9zW$`$z@cCY7+mxgHfQ2uQhZ-Bg6F)mmEz|j=0p4I}11k?as_b+mA%#j1I8iXEwQXk2s&yQGt+}5bJPGb zYs;@L`=D=*mVrBcjz(@?+%AG&0^6ZbXHPVwwQtYzgjukZEJ2bVC7?Lfli&RQ%4ql> zZ3Bjstq?yQBdHTP-^B*^9^)d5i0V_-08a5k6bqU|cZ>@XF)H;$15M;U%QGN@6}Z0a%$-dB3&%`n1N7*~lrtIJ9})K!Z&`SKbd?mA;INup)SGh_}|FuM4}K3U^9Fb8+f3I)=dOLQaPRBqE3YvwXIT1%t;9*xKkQh>Kn}+ z;Q<4<$i;f+^O`#E1n{wLZ`^)rpKs3y{DkQ8RNlH1vu<1qB`m4Ar|D>VgG6BuM+f>~Dm_|t=53>L+p^5?Ap9utF zppJ$KnZyZs@R1KmMUXW&i^`seS3FLP#h)k*_YGfPxP+Lf^-x z_sPQj+kVNDcrI(6;l2Q!R|cD$^7tp7bf7;!t@&f~h=LG73c#aHVNd&EsO|JDJ9OOx z4GDygnsW{dr3{vF5xpd#k62wqCZIlHJHXZfU5XlU_yV<>Rk0;H^GfkD(N zC8;^cd!U6lfJ^KX0@2Cm4 zd=8d)ev_63t*c7*w-JRN1{3v+Dme+gIiEr$x928QNy2pGAlduak_Nks{AB%r4d$fH zGhrW1qRbq8d%BIF&*L}E-fw%oD&cP&m%}Rqb%t5mz_v?@_2O7|JOgdKNWi}S@&06z zP{QC}7)~36DkZQSelp`cI3;!}dXmDmA1C4Sk2IJkXca2?Rq1CeU5pr=Z$Eb|MA79r zy57rE7PN*8KqGRYA39>(eT2c|(NGsfr{%mU54|>P>rf=dU$a zVl0M^67U>rcEgp*DN>+P_xfhO3&BA|ja^IR9nZ)3c|`h+cLeVW#xuq+3uv}nG%A6$ z#JM=Sf#pn?8f_ORN$Xqfs`b>*IN!zY+eSWe1i~INHf-G~8xDL(&1R?cv=sOgWv}k5 zvteF@Dw-i34yuxg_Ahk2WZ_t?;YIDiHl87$aT0m1go-04L)r0u>_1x^n=umxKkx@T zXKB)YVqzl*IYMPkmI~jo$xVn-{?=xiJ8`YE6AE&DmVLZJomVQhqu;I!nEhjZSW}A& z9aUJDnK_|~5?EL>2L>l;*|=qJIbzC{IPFmO%Mz2JRMpR3A?xzA_EP8?g@56F6FM$q zslHsKdNUki-|0=HpNsVPE(Hds%j;8TT#?q*MZ~(-<~C%b_A2|X}`fHdwvLeljj~n5@a2L|D4uGt&g#XQX&30uL+(&sRQi{`=)W|ELz*%Z{nK;cEHSxzI zsrA`<4Rz6Z8InRbWPpDMuP7)CHtM(&gT2jMVmcA6EvW4pd-}Qk^8M1f ztkSjGqJ~jOXiyi^+0T{nVA#S8`(S%RprmWLq)G}RWd2UQ3q_lH)oTEP+*uVvg!LsN zp4D^TqM1J4uq;@HeYBs!V3ZI;JIi2~d7$Bqe6W{zYP=5{*J{>)N}3r95vhNZ5{>PP zuI(%1IF+xoU4$(JU!{WLTR0_#*HMh#ml-ls=i#wjLt?A2l@z+gU?w4~g&W3HA^s`4 z4F}UB7IHlO-A+nWfeRdy@gjRdiHelQl{X=Mi%^&FKd0*<@l5nI+3Vc#h{rC$Kmoe- zvpZ3bMn*|-qZj5396y5q;CzC9kn7s`b9To2s502|i^nLOu-LIIk#*-sAS;X19xv|T z{0*ls}liCZmq|f?Z;8zxgGkM?Er0=gxe!}5B)Cfts%zx+KRG~>%xK$^Qwgd zxLTF&cv9bZ<-o^?!iTp33(@-}y0vl_)2I~G)HF}92jl3v=#<)O=E7Rmdy2oglf|Hk z9aQ(|WOUn)IJ?Ev)&Fx6bF6)kaLn)DLr)thofgM!&@~!rT^O7bt@e5Or7|3fA$%9Z zdcgKVh^j?@4Sr`l^A=M|1^Pj?Yjm{L@U2TTmvgF?=HwY>`I&p}76Dqk08?8TUT-A< znh@%m4Kr8Z2eAG|xkt@vbVWTWWo?7J*w`48u>8$6i$5I*42V{tPKX%OlI`)B z()!3ib)k>8#b55^e=w>k^6=B1ZbJ{JnSv5WmCH4hd6f6{<7;|@XRLCWiDm99yGhA{}Wd_r0D9El$(X5~ZD@6Yeq%JJ_WnN-%QoT_^& zls{`|J%9EnfAEk~Gyx};)G-ocS*iS3?;B@s#A40HNR)|D9E{SnkxmA^FH@JMsvova zX8&ayT=lAu;Am_z@Z2<%$ z6{BjEfS{w}t2VXwOJOAH%tlL96JNh1#=Wf*>*+C=wXQ*vNIs(oaU1*ElJg~AWBoz3 zYsEM6%XU&tu#eu|7g)PJi(C|&tXR}Xn&hnaxd#gx>+j>0Lv??-YpZnq{-}DCX85D* z_dAPe6Sn6vuwUIAo->ylZ}KI6nQVFL;mT@J6n=}jh-Pjuf%pEv++Pdkdj*Zwp7DBP zrhTtVw6{gZP)&6$66l-|HJ4w8%;eg~^EPs4nrqIew2C|SrN%S*vk{KJr_g;`Y{qKK za@>)WYnnH1$Y7g~H208STe0$q*DizuUr(^Zw@=B+o)Nq~aS$e4yGfT@>daH^DKIsz z)cwow6@MRvpi+T4eltCU8Nd3QQI$>-+ zFgW?Ai;?m(M6`3~3t0MUNoj|7dB1(@$eb{5jiAiUXxRcsX%N7syb8a-?5DbXQY&dt z%QtBuucYrm*1Dmz5?dn{A0>WU5x4YjETQ|M(8cAmSNvkb`2Ku7q`f}>zYyO<$8g@x zUzs*Gk#M%Gr7AVQ9ITO)uwuolOW^S4&Y0P4a>%SIVE2Gjj|O|=mQu{^v#Iu?g_u(5R=+V-wf&8^chyn4kgY80C{bYAldhLKCoo`h9f)w_=Um8!5STwbwF{_U3Tizp{AyHC84h8bs|GATle zy3F*$Rqlvr6MURu^<;ctoJ-C-Z#8oO_$tvow?$;EY1DOi-(^S-2TIzlGbRuwb&;-U z7yZw0dus6-cg1D}!{`%wA1C;jA;~pKJ=T-XVnJJKHRncn?@V?ZaEO-)De!(_{cb?PQZM`vF>be}<$+yB>x!n%iMG>Ka&r0;UkQJwE7Mk z$`Z}Z4DInTXYHvlPzHtdM7q)!O@~4(CDCwL6=&P!6H8`ZIu8IG){ebnFOOsgJi5#J zo*23$q%3OfclD3VJx10L$%S6mR#0O{Z-fm;yH{W~(%~1?#@9nDw|X-FV4WFNNE?u= zNsUpZB9>X39R6ko>B{E@palselu!fi@;2{*99+kw!vd7==a0k1U~mtD?VxZfC+5?F zS*Nm{8|Z%mvGF+lu?k#)zW_q@WO_h`!~-H-wk-C(55!&CS9;0~tCO{qo!|0Ld`A4R z!?{Ezel$sA7!Q>QFASC6a)j3Q9elimCP?JOd_Bc7+iW%D38BRl_}=XI4MruG!$~Y! z)#3G0Ing0FLqmnq)N|04xKy0c__kp$%3zYQ=jbnv1!pZF??**9r5!AA6QQz8HaBac z-)DvlmLg^;&QlPoDNzVPj2csI+Sp_GZ>Y~5tTw?OOsB`~G>Igi6#!z}vZxRn z_mZ3%^Wj5E=1-Gbo9g3YL(lz--KY}z4P9;Rev`lWf1j9yQ{VAp+Sf5=7Hy=={c`2yJadWY-BAQd=Lk{ z_R5BjK&)=KT9B)gLV{1>rb2!28ThgAIW#%qO5B*yF`N0*A1srvHd0v*yWIk#^?)Ts zM5jSs*i*xFz2D7|(i500ZVx1J5^<9kp}e$hGHAC2LutRtBH%ytYf07!h%N|p$O_p@ zM76*Ze2pGv`4>e77PC@W+j}V~VvZ$pJA7_u0J6SUt6#Bso7y)~k6x^LN|Y_7g&3t~ zc23kDyJ8C;xg3Hmb~#>n;^|83;U1180$1Uh7VPe5U0Yj3=xI`|HIDic);1 z%Zx)#tB~cm_w^{1mrymqhd`i6ceMi`P*4dL1i~7qi~&@l{y)Y0VJg~EDlzW{YoEjk QxT*u><#`b=z=h}h2b9~@i2wiq literal 0 HcmV?d00001 diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml index deda078ace22e..fb5331379acb6 100644 --- a/common/tier4_automatic_goal_rviz_plugin/package.xml +++ b/common/tier4_automatic_goal_rviz_plugin/package.xml @@ -6,6 +6,8 @@ The autoware automatic goal rviz plugin package Shumpei Wakabayashi Dawid Moszyński + Kyoichi Sugahara + Satoshi Ota Apache License 2.0 Dawid Moszyński @@ -22,6 +24,7 @@ rviz_default_plugins tf2 tf2_geometry_msgs + tier4_autoware_utils visualization_msgs yaml-cpp diff --git a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml index e7d5224550868..e9d77491941ed 100644 --- a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml @@ -9,4 +9,9 @@ base_class_type="rviz_common::Tool"> AutowareAutomaticGoalTool + + AutowareAutomaticCheckpointTool + diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp new file mode 100644 index 0000000000000..4efa6fedbaabd --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "automatic_checkpoint_append_tool.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include + +namespace rviz_plugins +{ +AutowareAutomaticCheckpointTool::AutowareAutomaticCheckpointTool() +{ + shortcut_key_ = 'c'; + + pose_topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "~/automatic_goal/checkpoint", "The topic on which to publish automatic_goal.", + getPropertyContainer(), SLOT(updateTopic()), this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void AutowareAutomaticCheckpointTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D AppendCheckpoint"); + updateTopic(); +} + +void AutowareAutomaticCheckpointTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = raw_node->create_publisher( + pose_topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void AutowareAutomaticCheckpointTool::onPoseSet(double x, double y, double theta) +{ + // pose + std::string fixed_frame = context_->getFixedFrame().toStdString(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = clock_->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = position_z_->getFloat(); + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + pose.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("AutowareAutomaticCheckpointTool"), + "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y, position_z_->getFloat(), theta, + fixed_frame.c_str()); + pose_pub_->publish(pose); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticCheckpointTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp new file mode 100644 index 0000000000000..4ea3fa4bd009a --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp @@ -0,0 +1,91 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ +#define AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class AutowareAutomaticCheckpointTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + AutowareAutomaticCheckpointTool(); + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: // NOLINT for Qt + rclcpp::Clock::SharedPtr clock_{nullptr}; + rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; + + rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; + rviz_common::properties::FloatProperty * position_z_{nullptr}; +}; + +} // namespace rviz_plugins + +#endif // AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp index bee390bfe29ed..6b8d7765a989a 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp @@ -16,6 +16,8 @@ #include "automatic_goal_panel.hpp" +#include + namespace rviz_plugins { AutowareAutomaticGoalPanel::AutowareAutomaticGoalPanel(QWidget * parent) @@ -139,6 +141,9 @@ void AutowareAutomaticGoalPanel::onInitialize() sub_append_goal_ = raw_node_->create_subscription( "~/automatic_goal/goal", 5, std::bind(&AutowareAutomaticGoalPanel::onAppendGoal, this, std::placeholders::_1)); + sub_append_checkpoint_ = raw_node_->create_subscription( + "~/automatic_goal/checkpoint", 5, + std::bind(&AutowareAutomaticGoalPanel::onAppendCheckpoint, this, std::placeholders::_1)); initCommunication(raw_node_.get()); } @@ -244,12 +249,28 @@ void AutowareAutomaticGoalPanel::onClickSaveListToFile() void AutowareAutomaticGoalPanel::onAppendGoal(const PoseStamped::ConstSharedPtr pose) { if (state_ == State::EDITING) { - goals_list_.push_back(pose); + goals_list_.emplace_back(pose); updateGoalsList(); updateGUI(); } } +void AutowareAutomaticGoalPanel::onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose) +{ + if (goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select a goal in GoalsList before set checkpoint"); + return; + } + + current_goal_ = goals_list_widget_ptr_->currentRow(); + if (current_goal_ >= goals_list_.size()) { + return; + } + + goals_list_.at(current_goal_).checkpoint_pose_ptrs.push_back(pose); + publishMarkers(); +} + // Override void AutowareAutomaticGoalPanel::onCallResult() { @@ -418,47 +439,71 @@ void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const void AutowareAutomaticGoalPanel::publishMarkers() { + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; + MarkerArray text_array; MarkerArray arrow_array; // Clear existing - visualization_msgs::msg::Marker marker; - marker.header.stamp = rclcpp::Time(); - marker.ns = "names"; - marker.id = 0; - marker.action = visualization_msgs::msg::Marker::DELETEALL; - text_array.markers.push_back(marker); - marker.ns = "poses"; - arrow_array.markers.push_back(marker); - pub_marker_->publish(text_array); - pub_marker_->publish(arrow_array); + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::DELETEALL; + text_array.markers.push_back(marker); + pub_marker_->publish(text_array); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + arrow_array.markers.push_back(marker); + pub_marker_->publish(arrow_array); + } + text_array.markers.clear(); arrow_array.markers.clear(); - // Publish current - for (unsigned i = 0; i < goals_list_.size(); i++) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Time(); - marker.ns = "names"; - marker.id = static_cast(i); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + + const auto push_arrow_marker = [&](const auto & pose, const auto & color, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", id, Marker::ARROW, + createMarkerScale(1.6, 0.5, 0.5), color); marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = goals_list_[i]->pose; + marker.pose = pose; marker.lifetime = rclcpp::Duration(0, 0); - marker.scale.x = 1.6; - marker.scale.y = 1.6; - marker.scale.z = 1.6; - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.color.a = 1.0; marker.frame_locked = false; - marker.text = "G" + std::to_string(i); - text_array.markers.push_back(marker); - marker.ns = "poses"; - marker.scale.y = 0.2; - marker.scale.z = 0.2; - marker.type = visualization_msgs::msg::Marker::ARROW; arrow_array.markers.push_back(marker); + }; + + const auto push_text_marker = [&](const auto & pose, const auto & text, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", id, Marker::TEXT_VIEW_FACING, + createMarkerScale(1.6, 1.6, 1.6), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose; + marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = false; + marker.text = text; + text_array.markers.push_back(marker); + }; + + // Publish current + size_t id = 0; + for (size_t i = 0; i < goals_list_.size(); ++i) { + { + const auto pose = goals_list_.at(i).goal_pose_ptr->pose; + push_arrow_marker(pose, createMarkerColor(0.0, 1.0, 0.0, 0.999), id++); + push_text_marker(pose, "Goal:" + std::to_string(i), id++); + } + + for (size_t j = 0; j < goals_list_.at(i).checkpoint_pose_ptrs.size(); ++j) { + const auto pose = goals_list_.at(i).checkpoint_pose_ptrs.at(j)->pose; + push_arrow_marker(pose, createMarkerColor(1.0, 1.0, 0.0, 0.999), id++); + push_text_marker( + pose, "Checkpoint:" + std::to_string(i) + "[Goal:" + std::to_string(j) + "]", id++); + } } pub_marker_->publish(text_array); pub_marker_->publish(arrow_array); @@ -469,13 +514,13 @@ void AutowareAutomaticGoalPanel::saveGoalsList(const std::string & file_path) { YAML::Node node; for (unsigned i = 0; i < goals_list_.size(); ++i) { - node[i]["position_x"] = goals_list_[i]->pose.position.x; - node[i]["position_y"] = goals_list_[i]->pose.position.y; - node[i]["position_z"] = goals_list_[i]->pose.position.z; - node[i]["orientation_x"] = goals_list_[i]->pose.orientation.x; - node[i]["orientation_y"] = goals_list_[i]->pose.orientation.y; - node[i]["orientation_z"] = goals_list_[i]->pose.orientation.z; - node[i]["orientation_w"] = goals_list_[i]->pose.orientation.w; + node[i]["position_x"] = goals_list_[i].goal_pose_ptr->pose.position.x; + node[i]["position_y"] = goals_list_[i].goal_pose_ptr->pose.position.y; + node[i]["position_z"] = goals_list_[i].goal_pose_ptr->pose.position.z; + node[i]["orientation_x"] = goals_list_[i].goal_pose_ptr->pose.orientation.x; + node[i]["orientation_y"] = goals_list_[i].goal_pose_ptr->pose.orientation.y; + node[i]["orientation_z"] = goals_list_[i].goal_pose_ptr->pose.orientation.z; + node[i]["orientation_w"] = goals_list_[i].goal_pose_ptr->pose.orientation.w; } std::ofstream file_out(file_path); file_out << node; diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp index 0ec9ca530f074..72a5bd4fb80fe 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp @@ -61,7 +61,9 @@ class AutowareAutomaticGoalPanel : public rviz_common::Panel, public automatic_goal::AutowareAutomaticGoalSender { using State = automatic_goal::AutomaticGoalState; + using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; + using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; @@ -96,6 +98,7 @@ public Q_SLOTS: // NOLINT for Qt // Inputs void onAppendGoal(const PoseStamped::ConstSharedPtr pose); + void onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose); // Visual updates void updateGUI(); @@ -116,6 +119,7 @@ public Q_SLOTS: // NOLINT for Qt // Pub/Sub rclcpp::Publisher::SharedPtr pub_marker_{nullptr}; rclcpp::Subscription::SharedPtr sub_append_goal_{nullptr}; + rclcpp::Subscription::SharedPtr sub_append_checkpoint_{nullptr}; // Containers rclcpp::Node::SharedPtr raw_node_{nullptr}; diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp index cc8d46e2f60c6..3ca368a3bd1a4 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp @@ -108,9 +108,9 @@ void AutowareAutomaticGoalSender::updateGoalsList() std::stringstream ss; ss << std::fixed << std::setprecision(2); tf2::Quaternion tf2_quat; - tf2::convert(goal->pose.orientation, tf2_quat); - ss << "G" << i << " (" << goal->pose.position.x << ", "; - ss << goal->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; + tf2::convert(goal.goal_pose_ptr->pose.orientation, tf2_quat); + ss << "G" << i << " (" << goal.goal_pose_ptr->pose.position.x << ", "; + ss << goal.goal_pose_ptr->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; goals_achieved_.insert({i++, std::make_pair(ss.str(), 0)}); } onGoalListUpdated(); @@ -178,7 +178,7 @@ void AutowareAutomaticGoalSender::loadGoalsList(const std::string & file_path) pose->pose.orientation.y = goal["orientation_y"].as(); pose->pose.orientation.z = goal["orientation_z"].as(); pose->pose.orientation.w = goal["orientation_w"].as(); - goals_list_.push_back(pose); + goals_list_.emplace_back(pose); } resetAchievedGoals(); updateGoalsList(); diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp index aacdada352811..88b7b5e7dac20 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp @@ -80,8 +80,11 @@ class AutowareAutomaticGoalSender : public rclcpp::Node } auto req = std::make_shared(); - req->header = goals_list_.at(goal_index)->header; - req->goal = goals_list_.at(goal_index)->pose; + req->header = goals_list_.at(goal_index).goal_pose_ptr->header; + req->goal = goals_list_.at(goal_index).goal_pose_ptr->pose; + for (const auto & checkpoint : goals_list_.at(goal_index).checkpoint_pose_ptrs) { + req->waypoints.push_back(checkpoint->pose); + } client->async_send_request( req, [this](typename rclcpp::Client::SharedFuture result) { if (result.get()->status.code != 0) state_ = State::ERROR; @@ -120,6 +123,13 @@ class AutowareAutomaticGoalSender : public rclcpp::Node } } + struct Route + { + explicit Route(const PoseStamped::ConstSharedPtr & goal) : goal_pose_ptr{goal} {} + PoseStamped::ConstSharedPtr goal_pose_ptr{}; + std::vector checkpoint_pose_ptrs{}; + }; + // Update void updateGoalsList(); virtual void updateAutoExecutionTimerTick(); @@ -155,7 +165,7 @@ class AutowareAutomaticGoalSender : public rclcpp::Node // Containers unsigned current_goal_{0}; State state_{State::INITIALIZING}; - std::vector goals_list_{}; + std::vector goals_list_{}; std::map> goals_achieved_{}; std::string goals_achieved_file_path_{}; From 2719ad4d830f93787979905714beb18b7d2ea720 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 25 Jan 2024 08:52:12 +0900 Subject: [PATCH 116/154] refactor(tier4_perception_launch): refactor object_recognition/detection launcher (#6152) * refactor: align mode parameters Signed-off-by: Taekjin LEE * refactor: cluster detector and merger Signed-off-by: Taekjin LEE * refactor: separate object merger launches Signed-off-by: Taekjin LEE * refactor: radar detector module Signed-off-by: Taekjin LEE * refactor: lidar detector modules Signed-off-by: Taekjin LEE * chore: fix mis spell, align typo, clean-up Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- ...ar_radar_fusion_based_detection.launch.xml | 437 ------------------ .../detection/detection.launch.xml | 145 +++++- .../camera_lidar_detector.launch.xml} | 236 ++-------- .../detector/lidar_dnn_detector.launch.xml | 56 +++ .../detector/lidar_rule_detector.launch.xml | 55 +++ .../radar_detector.launch.xml} | 0 .../lidar_radar_based_detection.launch.xml | 51 -- .../merger/camera_lidar_merger.launch.xml | 124 +++++ .../camera_lidar_radar_merger.launch.xml | 154 ++++++ .../lidar_merger.launch.xml} | 114 +---- 10 files changed, 563 insertions(+), 809 deletions(-) delete mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml rename launch/tier4_perception_launch/launch/object_recognition/detection/{camera_lidar_fusion_based_detection.launch.xml => detector/camera_lidar_detector.launch.xml} (57%) create mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml create mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml rename launch/tier4_perception_launch/launch/object_recognition/detection/{radar_based_detection.launch.xml => detector/radar_detector.launch.xml} (100%) delete mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml create mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml create mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml rename launch/tier4_perception_launch/launch/object_recognition/detection/{lidar_based_detection.launch.xml => merger/lidar_merger.launch.xml} (51%) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml deleted file mode 100644 index ba86bc1e334ff..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ /dev/null @@ -1,437 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 885fa80ed7004..ab9ed65999048 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -3,7 +3,7 @@ - + @@ -34,9 +34,10 @@ - + - + + @@ -57,23 +58,61 @@ - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + @@ -94,47 +133,107 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + + + + + + + + + + + + + + - + - - - + + + + + + + + + + + + + - + - + + + + + - + - - + + + + + + - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml similarity index 57% rename from launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 4cb648852a03c..1c9201a9af8b3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -1,17 +1,13 @@ - - - - + - - - - + + + @@ -54,6 +50,44 @@ --> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -192,190 +226,4 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml new file mode 100644 index 0000000000000..1a97659b357d8 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml new file mode 100644 index 0000000000000..ce34640bd3179 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml similarity index 100% rename from launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml deleted file mode 100644 index adedc2312677f..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml new file mode 100644 index 0000000000000..2f62e83ae0ef5 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml new file mode 100644 index 0000000000000..b4d19c9052a63 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -0,0 +1,154 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml similarity index 51% rename from launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml index 04c4264b70e5f..6ccff44933966 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -1,113 +1,19 @@ - - + - - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - + + + @@ -133,9 +39,9 @@ - - - + + + From 20ff907be39c926c094721eecb682be9a77145fe Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 25 Jan 2024 13:11:50 +0900 Subject: [PATCH 117/154] chore(ar_tag_based_localizer): add json schema (#6164) * Added json schema to AR tag-based localizer Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix --------- Signed-off-by: Shintaro SAKODA Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ar_tag_based_localizer/README.md | 6 +- .../schema/ar_tag_based_localizer.schema.json | 87 +++++++++++++++++++ 2 files changed, 92 insertions(+), 1 deletion(-) create mode 100644 localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md index f0c54d6393f7e..1a776bd810fff 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -2,7 +2,7 @@ **ArTagBasedLocalizer** is a vision-based localization node. - +ar_tag_image This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. @@ -30,6 +30,10 @@ The positions and orientations of the AR-Tags are assumed to be written in the L | `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | +## Parameters + +{{ json_to_markdown("localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }} + ## How to launch When launching Autoware, set `artag` for `pose_source`. diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json b/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json new file mode 100644 index 0000000000000..bde5221aa1bbc --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json @@ -0,0 +1,87 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ar_tag_based_localizer", + "type": "object", + "definitions": { + "ar_tag_based_localizer": { + "type": "object", + "properties": { + "marker_size": { + "type": "number", + "description": "marker_size", + "default": 0.6 + }, + "target_tag_ids": { + "type": "array", + "description": "target_tag_ids", + "default": "['0','1','2','3','4','5','6']" + }, + "base_covariance": { + "type": "array", + "description": "base_covariance", + "default": [ + 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.02 + ] + }, + "distance_threshold": { + "type": "number", + "description": "distance_threshold(m)", + "default": "13.0" + }, + "consider_orientation": { + "type": "boolean", + "description": "consider_orientation", + "default": "false" + }, + "detection_mode": { + "type": "string", + "description": "detection_mode select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST]", + "default": "DM_NORMAL" + }, + "min_marker_size": { + "type": "number", + "description": "min_marker_size", + "default": 0.02 + }, + "ekf_time_tolerance": { + "type": "number", + "description": "ekf_time_tolerance(sec)", + "default": 5.0 + }, + "ekf_position_tolerance": { + "type": "number", + "description": "ekf_position_tolerance(m)", + "default": 10.0 + } + }, + "required": [ + "marker_size", + "target_tag_ids", + "base_covariance", + "distance_threshold", + "consider_orientation", + "detection_mode", + "min_marker_size", + "ekf_time_tolerance", + "ekf_position_tolerance" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ar_tag_based_localizer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} From 1a9dbf4532d8315bd443ad2732cb4fbd34c23673 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 25 Jan 2024 14:39:55 +0900 Subject: [PATCH 118/154] fix(ndt_scan_matcher): fix type of critical_upper_bound_exe_time_ms (#6161) * fix type of critical_upper_bound_exe_time_ms Signed-off-by: Yamato Ando * fix order Signed-off-by: Yamato Ando --------- Signed-off-by: Yamato Ando --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 6 +++--- .../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp | 2 +- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 5 ++--- 3 files changed, 6 insertions(+), 7 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 9bc62d3f919c6..a5a4941bfec62 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -54,6 +54,9 @@ # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100.0 + # Number of threads used for parallel computing num_threads: 4 @@ -98,6 +101,3 @@ # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 - - # The execution time which means probably NDT cannot matches scans properly. [ms] - critical_upper_bound_exe_time_ms: 100 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 71895c59ec37d..e2503c20aef6e 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 @@ -225,7 +225,7 @@ class NDTScanMatcher : public rclcpp::Node double z_margin_for_ground_removal_; // The execution time which means probably NDT cannot matches scans properly - int64_t critical_upper_bound_exe_time_ms_; + double critical_upper_bound_exe_time_ms_; }; #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 942df03410165..0382d805b7276 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -118,7 +118,7 @@ NDTScanMatcher::NDTScanMatcher() lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); critical_upper_bound_exe_time_ms_ = - this->declare_parameter("critical_upper_bound_exe_time_ms"); + this->declare_parameter("critical_upper_bound_exe_time_ms"); initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); @@ -316,8 +316,7 @@ void NDTScanMatcher::publish_diagnostic() } if ( state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= - static_cast(critical_upper_bound_exe_time_ms_)) { + std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; From 6bf7532a0299e9d0c0be18529d9fe42f1660c6bd Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 25 Jan 2024 14:40:24 +0900 Subject: [PATCH 119/154] refactor(localization_error_monitor): rework parameters (#6156) * refactor(localization_error_monitor): rework parameters Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../localization_error_monitor/README.md | 8 +-- .../localization_error_monitor.schema.json | 56 +++++++++++++++++++ 2 files changed, 57 insertions(+), 7 deletions(-) create mode 100644 localization/localization_error_monitor/schema/localization_error_monitor.schema.json diff --git a/localization/localization_error_monitor/README.md b/localization/localization_error_monitor/README.md index 2ddf41e4eac70..7912ad843ef19 100644 --- a/localization/localization_error_monitor/README.md +++ b/localization/localization_error_monitor/README.md @@ -29,10 +29,4 @@ The package monitors the following two values: ## Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | -------------------------------------------------------------------------------------------- | -| `scale` | double | scale factor for monitored values (default: 3.0) | -| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | -| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | -| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | -| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.25) | +{{ json_to_markdown("localization/localization_error_monitor/schema/localization_error_monitor.schema.json") }} diff --git a/localization/localization_error_monitor/schema/localization_error_monitor.schema.json b/localization/localization_error_monitor/schema/localization_error_monitor.schema.json new file mode 100644 index 0000000000000..0cdd5fb946756 --- /dev/null +++ b/localization/localization_error_monitor/schema/localization_error_monitor.schema.json @@ -0,0 +1,56 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Localization Error Monitor node", + "type": "object", + "definitions": { + "localization_error_monitor": { + "type": "object", + "properties": { + "scale": { + "type": "number", + "default": 3.0, + "description": "scale factor for monitored values" + }, + "error_ellipse_size": { + "type": "number", + "default": 1.5, + "description": "error threshold for long radius of confidence ellipse [m]" + }, + "warn_ellipse_size": { + "type": "number", + "default": 1.2, + "description": "warning threshold for long radius of confidence ellipse [m]" + }, + "error_ellipse_size_lateral_direction": { + "type": "number", + "default": 0.3, + "description": "error threshold for size of confidence ellipse along lateral direction [m]" + }, + "warn_ellipse_size_lateral_direction": { + "type": "number", + "default": 0.25, + "description": "warning threshold for size of confidence ellipse along lateral direction [m]" + } + }, + "required": [ + "scale", + "error_ellipse_size", + "warn_ellipse_size", + "error_ellipse_size_lateral_direction", + "warn_ellipse_size_lateral_direction" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/localization_error_monitor" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} From 98f2a71eaa0c7f06c766ae8b91621fe1e8173c41 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Thu, 25 Jan 2024 15:11:47 +0900 Subject: [PATCH 120/154] chore(simple_object_merger): update README (#6146) * chore(simple_object_merger): update README Signed-off-by: scepter914 * style(pre-commit): autofix * update README Signed-off-by: scepter914 * style(pre-commit): autofix * update README Signed-off-by: scepter914 --------- Signed-off-by: scepter914 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/simple_object_merger/README.md | 86 ++++++++++++++--------- 1 file changed, 52 insertions(+), 34 deletions(-) diff --git a/perception/simple_object_merger/README.md b/perception/simple_object_merger/README.md index 4609638cc008c..9bcd03e8abfa1 100644 --- a/perception/simple_object_merger/README.md +++ b/perception/simple_object_merger/README.md @@ -1,60 +1,78 @@ # simple_object_merger -This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) without data association algorithm. +This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) without low calculation cost. ## Algorithm ### Background -This package can merge multiple DetectedObjects without matching processing. -[Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost. -In addition, for now, [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) can handle only 2 DetectedObjects topics and cannot handle more than 2 topics in one node. -To merge 6 DetectedObjects topics, 6 [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) nodes need to stand. +[Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) is mainly used for merge process with DetectedObjects. There are 2 characteristics in `Object_merger`. First, `object_merger` solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost. Second, `object_merger` can handle only 2 DetectedObjects topics and cannot handle more than 2 topics in one node. To merge 6 DetectedObjects topics, 6 `object_merger` nodes need to stand for now. -So this package aim to merge DetectedObjects simply. -This package do not use data association algorithm to reduce the computational cost, and it can handle more than 2 topics in one node to prevent launching a large number of nodes. +So `simple_object_merger` aim to merge multiple DetectedObjects with low calculation cost. +The package do not use data association algorithm to reduce the computational cost, and it can handle more than 2 topics in one node to prevent launching a large number of nodes. + +### Use case + +Use case is as below. + +- Multiple radar detection + +`Simple_object_merger` can be used for multiple radar detection. By combining them into one topic from multiple radar topics with `simple_object_merger`, the pipeline for faraway detection with radar is simpler. ### Limitation - Sensor data drops and delay -Merged objects will not be published until all topic data is received when initializing. -In addition, to care sensor data drops and delayed, this package has a parameter to judge timeout. -When the latest time of the data of a topic is older than the timeout parameter, it is not merged for output objects. -For now specification of this package, if all topic data is received at first and after that the data drops, and the merged objects are published without objects which is judged as timeout. -The timeout parameter should be determined by sensor cycle time. +Merged objects will not be published until all topic data is received when initializing. In addition, to care sensor data drops and delayed, this package has a parameter to judge timeout. When the latest time of the data of a topic is older than the timeout parameter, it is not merged for output objects. For now specification of this package, if all topic data is received at first and after that the data drops, and the merged objects are published without objects which is judged as timeout.The timeout parameter should be determined by sensor cycle time. - Post-processing -Because this package does not have matching processing, so it can be used only when post-processing is used. -For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing. +Because this package does not have matching processing, there are overlapping objects depending on the input objects. So output objects can be used only when post-processing is used. For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing. -### Use case +## Interface -Use case is as below. +### Input -- Multiple radar detection +Input topics is defined by the parameter of `input_topics` (List[string]). The type of input topics is `std::vector`. + +### Output + +- `~/output/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Merged objects combined from input topics. + +### Parameters + +- `update_rate_hz` (double) [hz] + - Default parameter: 20.0 + +This parameter is update rate for the `onTimer` function. +This parameter should be same as the frame rate of input topics. -This package can be used for multiple radar detection. -Since [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) will be included later process in radar faraway detection, this package can be used. +- `new_frame_id` (string) + - Default parameter: "base_link" -## Input +This parameter is the header frame_id of the output topic. +If output topics use for perception module, it should be set for "base_link" -| Name | Type | Description | -| ---- | ------------------------------------------------------------------ | ------------------------------------------------------ | -| | std::vector | 3D detected objects. Topic names are set by parameters | +- `timeout_threshold` (double) [s] + - Default parameter: 0.1 -## Output +This parameter is the threshold for timeout judgement. +If the time difference between the first topic of `input_topics` and an input topic is exceeded to this parameter, then the objects of topic is not merged to output objects. -| Name | Type | Description | -| ------------------ | ----------------------------------------------------- | -------------- | -| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Merged objects | +```cpp + for (size_t i = 0; i < input_topic_size; i++) { + double time_diff = rclcpp::Time(objects_data_.at(i)->header.stamp).seconds() - + rclcpp::Time(objects_data_.at(0)->header.stamp).seconds(); + if (std::abs(time_diff) < node_param_.timeout_threshold) { + // merge objects + } + } +``` -## Parameters +- `input_topics` (List[string]) + - Default parameter: "[]" -| Name | Type | Description | Default value | -| :------------------ | :----------- | :----------------------------------- | :------------ | -| `update_rate_hz` | double | Update rate. [hz] | 20.0 | -| `new_frame_id` | string | The header frame_id of output topic. | "base_link" | -| `timeout_threshold` | double | Threshold for timeout judgement [s]. | 1.0 | -| `input_topics` | List[string] | Input topics name. | "[]" | +This parameter is the name of input topics. +For example, when this packages use for radar objects, `"[/sensing/radar/front_center/detected_objects, /sensing/radar/front_left/detected_objects, /sensing/radar/rear_left/detected_objects, /sensing/radar/rear_center/detected_objects, /sensing/radar/rear_right/detected_objects, /sensing/radar/front_right/detected_objects]"` can be set. +For now, the time difference is calculated by the header time between the first topic of `input_topics` and the input topics, so the most important objects to detect should be set in the first of `input_topics` list. From a49ede7003b6e524494e788a16e7b667e701c7f1 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Thu, 25 Jan 2024 15:28:41 +0900 Subject: [PATCH 121/154] feat(crosswalk)!: improve stuck prevention on crosswalk (#6150) * improve algorithm * sync param Signed-off-by: Yuki Takagi --- .../config/crosswalk.param.yaml | 2 +- .../src/manager.cpp | 4 +- .../src/scene_crosswalk.cpp | 54 +++++++++---------- .../src/scene_crosswalk.hpp | 2 +- 4 files changed, 31 insertions(+), 31 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index aa5dfed1e4bc5..29079b99b6718 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -30,7 +30,7 @@ enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. - stuck_vehicle_attention_range: 10.0 # [m] Ego does not enter the crosswalk area if a stuck vehicle exists within this distance from the end of the crosswalk on the ego's path. + required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle min_acc: -1.0 # min acceleration [m/ss] min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index dc3ce32be8505..094369b0a7a3c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -68,8 +68,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); - cp.stuck_vehicle_attention_range = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.required_clearance = + getOrDeclareParameter(node, ns + ".stuck_vehicle.required_clearance"); cp.min_acc_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.min_acc"); cp.max_jerk_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_jerk"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 48ae5ff03b8bd..cee6975df3155 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -906,48 +906,48 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( continue; } - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pose.position); if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { continue; } - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto ego_vel = planner_data_->current_velocity->twist.linear.x; - const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - - const double near_attention_range = - calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()); - const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range; - - const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos); - - if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) { - // Plan STOP considering min_acc, max_jerk and min_jerk. - const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle, - p.min_jerk_for_stuck_vehicle); - if (!min_feasible_dist_ego2stop) { - continue; + // check if STOP is required + const double crosswalk_front_to_obj_rear = + calcSignedArcLength(ego_path.points, path_intersects.front(), obj_pose.position) - + object.shape.dimensions.x / 2.0; + const double crosswalk_back_to_obj_rear = + calcSignedArcLength(ego_path.points, path_intersects.back(), obj_pose.position) - + object.shape.dimensions.x / 2.0; + const double required_space_length = + planner_data_->vehicle_info_.vehicle_length_m + planner_param_.required_clearance; + + if (crosswalk_front_to_obj_rear > 0.0 && crosswalk_back_to_obj_rear < required_space_length) { + // If there exists at least one vehicle ahead, plan STOP considering min_acc, max_jerk and + // min_jerk. Note that nearest search is not required because the stop pose independents to + // the vehicles. + const auto braking_distance = calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, p.min_acc_for_stuck_vehicle, + p.max_jerk_for_stuck_vehicle, p.min_jerk_for_stuck_vehicle); + if (!braking_distance) { + return {}; } + const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); - const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop); + const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop); const double dist_to_ego = calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); - const auto feasible_stop_pose = calcLongitudinalOffsetPose(ego_path.points, 0, dist_to_ego + feasible_dist_ego2stop); if (!feasible_stop_pose) { - continue; + return {}; } - setObjectsOfInterestData( - object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); - - // early return may not appropriate because the nearest in range object should be handled - return createStopFactor(*feasible_stop_pose, {obj_pos}); + setObjectsOfInterestData(obj_pose, object.shape, ColorName::RED); + return createStopFactor(*feasible_stop_pose, {obj_pose.position}); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 247907ad2dc80..ed1e342df9f7a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -124,7 +124,7 @@ class CrosswalkModule : public SceneModuleInterface bool enable_stuck_check_in_intersection{false}; double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; - double stuck_vehicle_attention_range; + double required_clearance; double min_acc_for_stuck_vehicle; double max_jerk_for_stuck_vehicle; double min_jerk_for_stuck_vehicle; From f0b85554b1e9090c982b441a0f28e0a2ffe63566 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Thu, 25 Jan 2024 19:53:29 +0900 Subject: [PATCH 122/154] chore(yabloc): rework parameters (#6170) * introduce json schema for ground_server Signed-off-by: Kento Yabuuchi * introduce json schema for ll2_decomposer Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix * fix json in yabloc_common Signed-off-by: Kento Yabuuchi * introduce json schema for graph_segment Signed-off-by: Kento Yabuuchi * introduce json schema for segment_filter Signed-off-by: Kento Yabuuchi * fix yabloc_common schema.json Signed-off-by: Kento Yabuuchi * introduce json schema for undistort Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix * Revert "introduce json schema for ground_server" This reverts commit 33d3e609d4e01919d11a86d3c955f53e529ae121. * Revert "introduce json schema for graph_segment" This reverts commit 00ae417f030324f2dcc7dfb4b867a969ae31aea7. Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix * introduce json schema for yabloc_monitor Signed-off-by: Kento Yabuuchi * introduce json schema for yabloc_particle_filter Signed-off-by: Kento Yabuuchi * introduce json schema for yabloc_pose_initializer Signed-off-by: Kento Yabuuchi * apply pre-commit Signed-off-by: Kento Yabuuchi * fix revert conflict Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix --------- Signed-off-by: Kento Yabuuchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/yabloc/yabloc_common/README.md | 6 +- .../schema/ground_server.schema.json | 43 +++++++++ .../schema/ll2_decomposer.schema.json | 51 ++++++++++ .../yabloc/yabloc_image_processing/README.md | 15 +-- .../schema/graph_segment.schema.json | 71 ++++++++++++++ .../schema/segment_filter.schema.json | 65 +++++++++++++ .../schema/undistort.schema.json | 43 +++++++++ localization/yabloc/yabloc_monitor/README.md | 4 + .../schema/yabloc_monitor.schema.json | 33 +++++++ .../yabloc/yabloc_particle_filter/README.md | 35 +------ .../camera_particle_corrector.schema.json | 77 +++++++++++++++ .../gnss_particle_corrector.schema.json | 94 +++++++++++++++++++ .../schema/predictor.schema.json | 71 ++++++++++++++ .../yabloc/yabloc_pose_initializer/README.md | 4 +- .../camera_pose_initializer.schema.json | 33 +++++++ 15 files changed, 592 insertions(+), 53 deletions(-) create mode 100644 localization/yabloc/yabloc_common/schema/ground_server.schema.json create mode 100644 localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json create mode 100644 localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json create mode 100644 localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json create mode 100644 localization/yabloc/yabloc_image_processing/schema/undistort.schema.json create mode 100644 localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json create mode 100644 localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json create mode 100644 localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json create mode 100644 localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json create mode 100644 localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md index 1160f1b314b99..9ed871c6ac401 100644 --- a/localization/yabloc/yabloc_common/README.md +++ b/localization/yabloc/yabloc_common/README.md @@ -63,8 +63,4 @@ This node extracts the elements related to the road surface markings and yabloc ### Parameters -| Name | Type | Description | -| --------------------- | ---------------- | ---------------------------------------------------------------------- | -| `road_marking_labels` | vector\ | This label is used to extract the road surface markings from lanelet2. | -| `sign_board_labels` | vector\ | This label is used to extract the traffic sign boards from lanelet2. | -| `bounding_box_labels` | vector\ | This label is used to extract the bounding boxes from lanelet2. | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json") }} diff --git a/localization/yabloc/yabloc_common/schema/ground_server.schema.json b/localization/yabloc/yabloc_common/schema/ground_server.schema.json new file mode 100644 index 0000000000000..a9b0674cedd1c --- /dev/null +++ b/localization/yabloc/yabloc_common/schema/ground_server.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ground_server", + "type": "object", + "definitions": { + "ground_server": { + "type": "object", + "properties": { + "force_zero_tilt": { + "type": "boolean", + "description": "if true, the tilt is always determined to be horizontal", + "default": false + }, + "K": { + "type": "number", + "description": "the number of neighbors for ground search on a map", + "default": 50 + }, + "R": { + "type": "number", + "description": "radius for ground search on a map [m]", + "default": 10 + } + }, + "required": ["force_zero_tilt", "K", "R"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ground_server" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json b/localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json new file mode 100644 index 0000000000000..fa5a7b37464f3 --- /dev/null +++ b/localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json @@ -0,0 +1,51 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ll2_decomposer", + "type": "object", + "definitions": { + "ll2_decomposer": { + "type": "object", + "properties": { + "road_marking_labels": { + "type": "array", + "description": "line string types that indicating road surface markings in lanelet2", + "default": [ + "cross_walk", + "zebra_marking", + "line_thin", + "line_thick", + "pedestrian_marking", + "stop_line", + "road_border" + ] + }, + "sign_board_labels": { + "type": "array", + "description": "line string types that indicating traffic sign boards in lanelet2", + "default": ["sign-board"] + }, + "bounding_box_labels": { + "type": "array", + "description": "line string types that indicating not mapped areas in lanelet2", + "default": ["none"] + } + }, + "required": ["road_marking_labels", "sign_board_labels", "bounding_box_labels"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ll2_decomposer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index 85e1d408a1b4b..921a9277a727c 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -89,14 +89,7 @@ This is a node that integrates the results of graph_segment and lsd to extract r ### Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | ------------------------------------------------------------------- | -| `min_segment_length` | double | min length threshold (if it is negative, it is unlimited) | -| `max_segment_distance` | double | max distance threshold (if it is negative, it is unlimited) | -| `max_lateral_distance` | double | max lateral distance threshold (if it is negative, it is unlimited) | -| `publish_image_with_segment_for_debug` | bool | toggle whether to publish the filtered line segment for debug | -| `max_range` | double | range of debug projection visualization | -| `image_size` | int | image size of debug projection visualization | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/segment_filter.schema.json") }} ## undistort @@ -127,11 +120,7 @@ This is to avoid redundant decompression within Autoware. ### Parameters -| Name | Type | Description | -| ------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `use_sensor_qos` | bool | where to use sensor qos or not | -| `width` | int | resized image width size | -| `override_frame_id` | string | value for overriding the camera's frame_id. if blank, frame_id of static_tf is not overwritten | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/undistort.schema.json") }} #### about tf_static overriding diff --git a/localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json b/localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json new file mode 100644 index 0000000000000..c0bf96dd0de7a --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json @@ -0,0 +1,71 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for graph_segment", + "type": "object", + "definitions": { + "graph_segment": { + "type": "object", + "properties": { + "target_height_ratio": { + "type": "number", + "description": "height on the image to retrieve the candidate road surface", + "default": 0.85 + }, + "target_candidate_box_width": { + "type": "number", + "description": "size of the square area to search for candidate road surfaces", + "default": 15 + }, + "pickup_additional_graph_segment": { + "type": "boolean", + "description": "if this is true, additional regions of similar color are retrieved", + "default": true + }, + "similarity_score_threshold": { + "type": "number", + "description": "threshold for picking up additional areas", + "default": 0.8 + }, + "sigma": { + "type": "number", + "description": "parameter for cv::ximgproc::segmentation::GraphSegmentation", + "default": 0.5 + }, + "k": { + "type": "number", + "description": "parameter for cv::ximgproc::segmentation::GraphSegmentation", + "default": 300.0 + }, + "min_size": { + "type": "number", + "description": "parameter for cv::ximgproc::segmentation::GraphSegmentation", + "default": 100.0 + } + }, + "required": [ + "target_height_ratio", + "target_candidate_box_width", + "pickup_additional_graph_segment", + "similarity_score_threshold", + "sigma", + "k", + "min_size" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/graph_segment" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json b/localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json new file mode 100644 index 0000000000000..bfe74ea80569c --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json @@ -0,0 +1,65 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for segment_filter", + "type": "object", + "definitions": { + "segment_filter": { + "type": "object", + "properties": { + "min_segment_length": { + "type": "number", + "description": "min length threshold (if it is negative, it is unlimited)", + "default": 1.5 + }, + "max_segment_distance": { + "type": "number", + "description": "max distance threshold (if it is negative, it is unlimited)", + "default": 30.0 + }, + "max_lateral_distance": { + "type": "number", + "description": "max lateral distance threshold (if it is negative, it is unlimited)", + "default": 10.0 + }, + "publish_image_with_segment_for_debug": { + "type": "boolean", + "description": "toggle whether to publish the filtered line segment for debug", + "default": true + }, + "max_range": { + "type": "number", + "description": "range of debug projection visualization", + "default": 20.0 + }, + "image_size": { + "type": "number", + "description": "image size of debug projection visualization", + "default": 800 + } + }, + "required": [ + "min_segment_length", + "max_segment_distance", + "max_lateral_distance", + "publish_image_with_segment_for_debug", + "max_range", + "image_size" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/segment_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_image_processing/schema/undistort.schema.json b/localization/yabloc/yabloc_image_processing/schema/undistort.schema.json new file mode 100644 index 0000000000000..d166d85024903 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/schema/undistort.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for undistort", + "type": "object", + "definitions": { + "undistort": { + "type": "object", + "properties": { + "use_sensor_qos": { + "type": "boolean", + "description": "whether to use sensor qos or not", + "default": true + }, + "width": { + "type": "number", + "description": "resized image width size", + "default": 800 + }, + "override_frame_id": { + "type": "string", + "description": "value for overriding the camera's frame_id. if blank, frame_id of static_tf is not overwritten", + "default": "" + } + }, + "required": ["use_sensor_qos", "width", "override_frame_id"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/undistort" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_monitor/README.md b/localization/yabloc/yabloc_monitor/README.md index ed4cdc36b6ba0..849429290b427 100644 --- a/localization/yabloc/yabloc_monitor/README.md +++ b/localization/yabloc/yabloc_monitor/README.md @@ -25,3 +25,7 @@ To be added, | Name | Type | Description | | -------------- | --------------------------------- | ------------------- | | `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | + +### Parameters + +{{ json_to_markdown("localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json") }} diff --git a/localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json b/localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json new file mode 100644 index 0000000000000..c83a9d3ac45f3 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for yabloc_monitor", + "type": "object", + "definitions": { + "yabloc_monitor": { + "type": "object", + "properties": { + "availability/timestamp_tolerance": { + "type": "number", + "description": "tolerable time difference between current time and latest estimated pose", + "default": 1.0 + } + }, + "required": ["availability/timestamp_tolerance"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/yabloc_monitor" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/README.md b/localization/yabloc/yabloc_particle_filter/README.md index d42ce4647bc95..f46a363102c8b 100644 --- a/localization/yabloc/yabloc_particle_filter/README.md +++ b/localization/yabloc/yabloc_particle_filter/README.md @@ -36,15 +36,7 @@ This package contains some executable nodes related to particle filter. ### Parameters -| Name | Type | Description | -| ----------------------------- | ---------------- | ----------------------------------------------------------------- | -| `visualize` | bool | whether particles are also published in visualization_msgs or not | -| `static_linear_covariance` | double | to override the covariance of `/twist_with_covariance` | -| `static_angular_covariance` | double | to override the covariance of `/twist_with_covariance` | -| `resampling_interval_seconds` | double | the interval of particle resampling | -| `num_of_particles` | int | the number of particles | -| `prediction_rate` | double | frequency of forecast updates, in Hz | -| `cov_xx_yy` | vector\ | the covariance of initial pose | +{{ json_to_markdown("localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json") }} ### Services @@ -80,19 +72,7 @@ This package contains some executable nodes related to particle filter. ### Parameters -| Name | Type | Description | -| -------------------------------- | ------ | ---------------------------------------------------------------------------------------- | -| `acceptable_max_delay` | double | how long to hold the predicted particles | -| `visualize` | double | whether publish particles as marker_array or not | -| `mahalanobis_distance_threshold` | double | if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips. | -| `for_fixed/max_weight` | bool | parameter for gnss weight distribution | -| `for_fixed/flat_radius` | bool | parameter for gnss weight distribution | -| `for_fixed/max_radius` | bool | parameter for gnss weight distribution | -| `for_fixed/min_weight` | bool | parameter for gnss weight distribution | -| `for_not_fixed/flat_radius` | bool | parameter for gnss weight distribution | -| `for_not_fixed/max_radius` | bool | parameter for gnss weight distribution | -| `for_not_fixed/min_weight` | bool | parameter for gnss weight distribution | -| `for_not_fixed/max_weight` | bool | parameter for gnss weight distribution | +{{ json_to_markdown("localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json") }} ## camera_particle_corrector @@ -127,16 +107,7 @@ This package contains some executable nodes related to particle filter. ### Parameters -| Name | Type | Description | -| ---------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------- | -| `acceptable_max_delay` | double | how long to hold the predicted particles | -| `visualize` | double | whether publish particles as marker_array or not | -| `image_size` | int | image size of debug/cost_map_image | -| `max_range` | double | width of hierarchical cost map | -| `gamma` | double | gamma value of the intensity gradient of the cost map | -| `min_prob` | double | minimum particle weight the corrector node gives | -| `far_weight_gain` | double | `exp(-far_weight_gain_ * squared_distance_from_camera)` is weight gain. if this is large, the nearby road markings will be more important | -| `enabled_at_first` | bool | if it is false, this node is not activated at first. you can activate by service call | +{{ json_to_markdown("localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json") }} ### Services diff --git a/localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json b/localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json new file mode 100644 index 0000000000000..a8b4873347f52 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json @@ -0,0 +1,77 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for camera_particle_corrector", + "type": "object", + "definitions": { + "camera_particle_corrector": { + "type": "object", + "properties": { + "acceptable_max_delay": { + "type": "number", + "description": "how long to hold the predicted particles", + "default": 1.0 + }, + "visualize": { + "type": "boolean", + "description": "whether publish particles as marker_array or not", + "default": false + }, + "image_size": { + "type": "number", + "description": "image size of debug/cost_map_image", + "default": 800 + }, + "max_range": { + "type": "number", + "description": "width of hierarchical cost map", + "default": 40.0 + }, + "gamma": { + "type": "number", + "description": "gamma value of the intensity gradient of the cost map", + "default": 5.0 + }, + "min_prob": { + "type": "number", + "description": "minimum particle weight the corrector node gives", + "default": 0.1 + }, + "far_weight_gain": { + "type": "number", + "description": "`exp(-far_weight_gain_ * squared_distance_from_camera)` is weight gain. if this is large, the nearby road markings will be more important", + "default": 0.001 + }, + "enabled_at_first": { + "type": "boolean", + "description": "if it is false, this node is not activated at first. you can activate by service call", + "default": true + } + }, + "required": [ + "acceptable_max_delay", + "visualize", + "image_size", + "max_range", + "gamma", + "min_prob", + "far_weight_gain", + "enabled_at_first" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/camera_particle_corrector" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json b/localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json new file mode 100644 index 0000000000000..a3990ad333981 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json @@ -0,0 +1,94 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for gnss_particle_corrector", + "type": "object", + "definitions": { + "gnss_particle_corrector": { + "type": "object", + "properties": { + "acceptable_max_delay": { + "type": "number", + "description": "how long to hold the predicted particles", + "default": 1.0 + }, + "visualize": { + "type": "boolean", + "description": "whether publish particles as marker_array or not", + "default": false + }, + "mahalanobis_distance_threshold": { + "type": "number", + "description": "if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips.", + "default": 30.0 + }, + "for_fixed/max_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 5.0 + }, + "for_fixed/flat_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 0.5 + }, + "for_fixed/max_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 10.0 + }, + "for_fixed/min_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 0.5 + }, + "for_not_fixed/max_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 1.0 + }, + "for_not_fixed/flat_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 5.0 + }, + "for_not_fixed/max_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 20.0 + }, + "for_not_fixed/min_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 0.5 + } + }, + "required": [ + "acceptable_max_delay", + "visualize", + "for_fixed/max_weight", + "for_fixed/flat_radius", + "for_fixed/max_radius", + "for_fixed/min_weight", + "for_not_fixed/max_weight", + "for_not_fixed/flat_radius", + "for_not_fixed/max_radius", + "for_not_fixed/min_weight" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/gnss_particle_corrector" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json b/localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json new file mode 100644 index 0000000000000..487dffdb00094 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json @@ -0,0 +1,71 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for predictor", + "type": "object", + "definitions": { + "predictor": { + "type": "object", + "properties": { + "visualize": { + "type": "boolean", + "description": "whether particles are also published in visualization_msgs or not", + "default": true + }, + "static_linear_covariance": { + "type": "number", + "description": "overriding covariance of `/twist_with_covariance`", + "default": 0.04 + }, + "static_angular_covariance": { + "type": "number", + "description": "overriding covariance of `/twist_with_covariance`", + "default": 0.006 + }, + "resampling_interval_seconds": { + "type": "number", + "description": "the interval of particle resampling", + "default": 1.0 + }, + "num_of_particles": { + "type": "number", + "description": "the number of particles", + "default": 500 + }, + "prediction_rate": { + "type": "number", + "description": "frequency of forecast updates, in Hz", + "default": 50.0 + }, + "cov_xx_yy": { + "type": "array", + "description": "the covariance of initial pose", + "default": [2.0, 0.25] + } + }, + "required": [ + "visualize", + "static_linear_covariance", + "static_angular_covariance", + "resampling_interval_seconds", + "num_of_particles", + "prediction_rate", + "cov_xx_yy" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/predictor" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 907b79c1459ec..33b9788cca3d3 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -59,9 +59,7 @@ Converted model URL ### Parameters -| Name | Type | Description | -| ------------------ | ---- | ----------------------------------------- | -| `angle_resolution` | int | how many divisions of 1 sigma angle range | +{{ json_to_markdown("localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json") }} ### Services diff --git a/localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json b/localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json new file mode 100644 index 0000000000000..26a53e0fe408f --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for camera_pose_initializer", + "type": "object", + "definitions": { + "camera_pose_initializer": { + "type": "object", + "properties": { + "angle_resolution": { + "type": "number", + "description": "how many divisions of 1 sigma angle range", + "default": 30 + } + }, + "required": ["angle_resolution"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/camera_pose_initializer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} From 058ccd3eb5417284fbb5e1cdd1d1fa2e6c0c6b38 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 26 Jan 2024 12:48:53 +0900 Subject: [PATCH 123/154] docs(crosswalk): update docs (#6177) * update docs Signed-off-by: Yuki Takagi --- .../README.md | 2 +- .../docs/stuck_vehicle_detection.svg | 111 +++++++++--------- 2 files changed, 57 insertions(+), 56 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index ce231659ccf78..c1ddc28e03426 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -196,7 +196,7 @@ In the `stuck_vehicle` namespace, the following parameters are defined. | ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- | | `stuck_vehicle_velocity` | [m/s] | double | maximum velocity threshold whether the target vehicle is stopped or not | | `max_stuck_vehicle_lateral_offset` | [m] | double | maximum lateral offset of the target vehicle position | -| `stuck_vehicle_attention_range` | [m] | double | detection area length ahead of the crosswalk | +| `required_clearance` | [m] | double | clearance to be secured between the ego and the ahead vehicle | | `min_acc` | [m/ss] | double | minimum acceleration to stop | | `min_jerk` | [m/sss] | double | minimum jerk to stop | | `max_jerk` | [m/sss] | double | maximum jerk to stop | diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg index c517be5bb9967..773edf7989de9 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg @@ -1,92 +1,93 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -
+
-
- stuck_vehicle_attention_range +
+ vehicle_length+required_clearance
- stuck_vehicle_atten... + vehicle_length+required_clearance - - - - - - - - - + + + + + + + + + + -
+
-
+
max_stuck_vehicle_lateral_offset
- max_stuck_vehicle_l... + max_stuck_vehicle_lateral_offset - - + + - + Text is not SVG - cannot display From ccd852b381cbd5f37420dd1c06cf7ee9ae929ae2 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 26 Jan 2024 12:54:55 +0900 Subject: [PATCH 124/154] fix(dynamic_avoidance_planner): inherit modified goal (#6179) Signed-off-by: kosuke55 --- planning/behavior_path_dynamic_avoidance_module/src/scene.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index b43102ecbd16d..73cabd00592a8 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -397,6 +397,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + output.modified_goal = getPreviousModuleOutput().modified_goal; return output; } From 57cf88d2b85db000ad19ff195eb4106283367edf Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 26 Jan 2024 16:03:48 +0900 Subject: [PATCH 125/154] fix(start_planner): update drivable area info and enable idle to running state transition (#6172) Update drivable area info and enable idle to running state transition Signed-off-by: kyoichi-sugahara --- .../utils/drivable_area_expansion/static_drivable_area.cpp | 4 ++++ .../start_planner_module.hpp | 2 +- .../src/start_planner_module.cpp | 6 +----- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 03fae6864fe50..4e25e9257ddfd 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1884,6 +1884,10 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_intersection_areas || drivable_area_info2.enable_expanding_intersection_areas; + // drivable margin + combined_drivable_area_info.drivable_margin = + std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin); + return combined_drivable_area_info; } diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 5bbde1c2fc523..cffce8218c554 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -139,7 +139,7 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override; + bool canTransitIdleToRunningState() override { return true; } /** * @brief init member variables. diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 6c9000caac956..8937c8a837694 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -347,11 +347,6 @@ bool StartPlannerModule::canTransitSuccessState() return hasFinishedPullOut(); } -bool StartPlannerModule::canTransitIdleToRunningState() -{ - return isActivated() && !isWaitingApproval(); -} - BehaviorModuleOutput StartPlannerModule::plan() { if (isWaitingApproval()) { @@ -1317,6 +1312,7 @@ bool StartPlannerModule::planFreespacePath() void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (status_.planner_type == PlannerType::FREESPACE) { + std::cerr << "Freespace planner updated drivable area." << std::endl; const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; From dc0184fd540dbd030f6d93d0fa7ff6b91469110b Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 26 Jan 2024 21:47:48 +0900 Subject: [PATCH 126/154] chore(cluster_merger): rework parameters (#6165) chore(cluster_merger): organize parameter Signed-off-by: kminoda --- perception/cluster_merger/CMakeLists.txt | 1 + perception/cluster_merger/config/cluster_merger.param.yaml | 3 +++ perception/cluster_merger/launch/cluster_merger.launch.xml | 4 ++-- 3 files changed, 6 insertions(+), 2 deletions(-) create mode 100644 perception/cluster_merger/config/cluster_merger.param.yaml diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt index 49506f4b439fb..5ad0de572b44f 100644 --- a/perception/cluster_merger/CMakeLists.txt +++ b/perception/cluster_merger/CMakeLists.txt @@ -24,4 +24,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/cluster_merger/config/cluster_merger.param.yaml b/perception/cluster_merger/config/cluster_merger.param.yaml new file mode 100644 index 0000000000000..adca6c203282f --- /dev/null +++ b/perception/cluster_merger/config/cluster_merger.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + output_frame_id: "base_link" diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml index 1bbd0ebd91e12..f0f90efe051a0 100644 --- a/perception/cluster_merger/launch/cluster_merger.launch.xml +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -3,12 +3,12 @@ - + - + From 34a7c2dfcb1e11b67b22787303dc287af7dc675d Mon Sep 17 00:00:00 2001 From: Motz <83898149+Motsu-san@users.noreply.github.com> Date: Sat, 27 Jan 2024 01:19:15 +0900 Subject: [PATCH 127/154] refactor(localization_error_monitor): rename localization_accuracy (#5178) refactor: Rename localization_accuracy to localization_error_ellipse Signed-off-by: Motsu-san --- localization/localization_error_monitor/src/diagnostics.cpp | 4 ++-- simulator/fault_injection/config/fault_injection.param.yaml | 2 +- .../config/diagnostic_aggregator/localization.param.yaml | 4 ++-- .../config/system_error_monitor.param.yaml | 2 +- .../system_error_monitor.planning_simulation.param.yaml | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp index 0c5a4a7800639..375fccfa06787 100644 --- a/localization/localization_error_monitor/src/diagnostics.cpp +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -23,7 +23,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy"; + key_value.key = "localization_error_ellipse"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); @@ -47,7 +47,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy_lateral_direction"; + key_value.key = "localization_error_ellipse_lateral_direction"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); diff --git a/simulator/fault_injection/config/fault_injection.param.yaml b/simulator/fault_injection/config/fault_injection.param.yaml index 1a57b852f7361..ac02442d70b4d 100644 --- a/simulator/fault_injection/config/fault_injection.param.yaml +++ b/simulator/fault_injection/config/fault_injection.param.yaml @@ -4,7 +4,7 @@ vehicle_is_out_of_lane: "lane_departure" trajectory_deviation_is_high: "trajectory_deviation" localization_matching_score_is_low: "ndt_scan_matcher" - localization_accuracy_is_low: "localization_accuracy" + localization_accuracy_is_low: "localization_error_ellipse" map_version_is_different: "map_version" trajectory_is_invalid: "trajectory_point_validation" cpu_temperature_is_high: "CPU Temperature" diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml index 55af6ab2d2c55..9ed82304036cc 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml @@ -24,9 +24,9 @@ contains: ["ndt_scan_matcher"] timeout: 1.0 - localization_accuracy: + localization_error_ellipse: type: diagnostic_aggregator/GenericAnalyzer - path: localization_accuracy + path: localization_error_ellipse contains: ["localization: localization_error_monitor"] timeout: 1.0 diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 1778f6594f0c3..2b58f5b42c0be 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -24,7 +24,7 @@ /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/localization/performance_monitoring/localization_accuracy: default + /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/map/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 9784259490ec2..f1031444394d0 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -24,7 +24,7 @@ /autoware/localization/node_alive_monitoring: default # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - # /autoware/localization/performance_monitoring/localization_accuracy: default + # /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/map/node_alive_monitoring: default From 0a057e12d4dfdae0e8a5afcd88805581047db931 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sat, 27 Jan 2024 01:59:51 +0900 Subject: [PATCH 128/154] fix(behavior_path_planner): sort keep last modules considering priority (#6174) Signed-off-by: kosuke55 --- .../src/planner_manager.cpp | 67 +++++++++++++------ 1 file changed, 46 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index cbcec2e3095d3..4caf5683f3982 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -630,32 +630,38 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisKeepLast(); + const auto get_sorted_keep_last_modules = [this](const auto & modules) { + std::vector keep_last_modules; + + std::copy_if( + modules.begin(), modules.end(), std::back_inserter(keep_last_modules), + [this](const auto & m) { return getManager(m)->isKeepLast(); }); + + // sort by priority (low -> high) + std::sort( + keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) { + return getManager(a)->getPriority() < getManager(b)->getPriority(); + }); + + return keep_last_modules; }; - move_to_end(approved_module_ptrs_, keep_last_module_cond); + + for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) { + move_to_end(approved_module_ptrs_, module); + } } // lock approved modules besides last one @@ -768,6 +774,25 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() == ModuleStatus::SUCCESS; }; From 29faba589fa9429501f536a54711a7ef34ba68d4 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Sat, 27 Jan 2024 03:35:54 +0900 Subject: [PATCH 129/154] refactor(lane_change): add const to lane change functions (#6175) Signed-off-by: Fumiya Watanabe --- .../include/behavior_path_lane_change_module/scene.hpp | 6 +++--- .../behavior_path_lane_change_module/utils/base_class.hpp | 6 +++--- planning/behavior_path_lane_change_module/src/scene.cpp | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index f991ca0d849f0..8d2e3b451fe98 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -55,7 +55,7 @@ class NormalLaneChange : public LaneChangeBase BehaviorModuleOutput generateOutput() override; - void extendOutputDrivableArea(BehaviorModuleOutput & output) override; + void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; @@ -65,7 +65,7 @@ class NormalLaneChange : public LaneChangeBase void resetParameters() override; - TurnSignalInfo updateOutputTurnSignal() override; + TurnSignalInfo updateOutputTurnSignal() const override; bool calcAbortPath() override; @@ -141,7 +141,7 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; - TurnSignalInfo calcTurnSignalInfo() override; + TurnSignalInfo calcTurnSignalInfo() const override; bool isValidPath(const PathWithLaneId & path) const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 400d5505dc49f..e78d706334bae 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -66,7 +66,7 @@ class LaneChangeBase virtual BehaviorModuleOutput generateOutput() = 0; - virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0; + virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) const = 0; virtual PathWithLaneId getReferencePath() const = 0; @@ -74,7 +74,7 @@ class LaneChangeBase virtual void resetParameters() = 0; - virtual TurnSignalInfo updateOutputTurnSignal() = 0; + virtual TurnSignalInfo updateOutputTurnSignal() const = 0; virtual bool hasFinishedLaneChange() const = 0; @@ -225,7 +225,7 @@ class LaneChangeBase LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const = 0; - virtual TurnSignalInfo calcTurnSignalInfo() = 0; + virtual TurnSignalInfo calcTurnSignalInfo() const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index f5100f16129c2..b115ab7c163c0 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -183,7 +183,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() return output; } -void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) +void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const { const auto & dp = planner_data_->drivable_area_expansion_parameters; @@ -431,7 +431,7 @@ void NormalLaneChange::resetParameters() RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } -TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() +TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { TurnSignalInfo turn_signal_info = calcTurnSignalInfo(); const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal( @@ -1447,7 +1447,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return safety_status; } -TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() +TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const { const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) { double accumulated_length = 0.0; From 16ba2d76b5b44c7b3e3e954b1f72f0ff266aac47 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Sat, 27 Jan 2024 03:54:59 +0900 Subject: [PATCH 130/154] chore(yabloc): replace parameters by json_to_markdown in readme (#6183) * replace parameters by json_to_markdown Signed-off-by: Kento Yabuuchi * fix some schma path Signed-off-by: Kento Yabuuchi * fix again Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi --- localization/yabloc/yabloc_common/README.md | 6 +----- .../yabloc/yabloc_image_processing/README.md | 14 +++----------- 2 files changed, 4 insertions(+), 16 deletions(-) diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md index 9ed871c6ac401..6368305bdbfad 100644 --- a/localization/yabloc/yabloc_common/README.md +++ b/localization/yabloc/yabloc_common/README.md @@ -32,11 +32,7 @@ It estimates the height and tilt of the ground from lanelet2. ### Parameters -| Name | Type | Description | -| ----------------- | ---- | -------------------------------------------------------- | -| `force_zero_tilt` | bool | if true, the tilt is always determined to be horizontal. | -| `K` | int | parameter for nearest k search | -| `R` | int | parameter for radius search | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ground_server.schema.json") }} ## ll2_decomposer diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index 921a9277a727c..9816a02c48121 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -53,15 +53,7 @@ This node extract road surface region by [graph-based-segmentation](https://docs ### Parameters -| Name | Type | Description | -| --------------------------------- | ------ | ------------------------------------------------------------------ | -| `target_height_ratio` | double | height on the image to retrieve the candidate road surface | -| `target_candidate_box_width` | int | size of the square area to search for candidate road surfaces | -| `pickup_additional_graph_segment` | bool | if this is true, additional regions of similar color are retrieved | -| `similarity_score_threshold` | double | threshold for picking up additional areas | -| `sigma` | double | parameters for cv::ximgproc::segmentation | -| `k` | double | parameters for cv::ximgproc::segmentation | -| `min_size` | double | parameters for cv::ximgproc::segmentation | +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json") }} ## segment_filter @@ -89,7 +81,7 @@ This is a node that integrates the results of graph_segment and lsd to extract r ### Parameters -{{ json_to_markdown("localization/yabloc/yabloc_common/schema/segment_filter.schema.json") }} +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json") }} ## undistort @@ -120,7 +112,7 @@ This is to avoid redundant decompression within Autoware. ### Parameters -{{ json_to_markdown("localization/yabloc/yabloc_common/schema/undistort.schema.json") }} +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/undistort.schema.json") }} #### about tf_static overriding From 70226203197b30a0445725657694487da43abaf0 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Sat, 27 Jan 2024 05:56:26 +0900 Subject: [PATCH 131/154] chore(motion_velocity_smoother): add maintainer (#6184) Signed-off-by: Fumiya Watanabe --- planning/motion_velocity_smoother/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index 4bb0d8a2883dd..9792aa2bdd60b 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -8,6 +8,7 @@ Fumiya Watanabe Takamasa Horibe Makoto Kurihara + Satoshi Ota Apache License 2.0 Takamasa Horibe From 01fdac086484e90b960644a3cdf05ffd974ec8c4 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sat, 27 Jan 2024 11:38:35 +0900 Subject: [PATCH 132/154] feat(goal_planner): consider moving objects when deciding path (#6178) feat(goal_planner): consider move objects when deciding path Signed-off-by: kosuke55 --- .../goal_planner_module.hpp | 2 +- .../src/goal_planner_module.cpp | 53 +++++++++++++------ 2 files changed, 38 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 51638e5485fe2..2c839f582be12 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -419,7 +419,7 @@ class GoalPlannerModule : public SceneModuleInterface bool checkOccupancyGridCollision(const PathWithLaneId & path) const; bool checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, - const bool update_debug_data = false) const; + const bool extract_static_objects, const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index faa9e28e28b3b..00810432a6f82 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -624,7 +624,9 @@ bool GoalPlannerModule::canReturnToLaneParking() if ( parameters_->use_object_recognition && - checkObjectsCollision(path, parameters_->object_recognition_collision_check_margin)) { + checkObjectsCollision( + path, parameters_->object_recognition_collision_check_margin, + /*extract_static_objects=*/false)) { return false; } @@ -711,7 +713,9 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); for (const auto & scale_factor : scale_factors) { - if (!checkObjectsCollision(resampled_path, margin * scale_factor)) { + if (!checkObjectsCollision( + resampled_path, margin * scale_factor, + /*extract_static_objects=*/true)) { return margin * scale_factor; } } @@ -771,8 +775,10 @@ std::optional> GoalPlannerModule::selectP const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); if ( - parameters_->use_object_recognition && - checkObjectsCollision(resampled_path, collision_check_margin, true /*update_debug_data*/)) { + parameters_->use_object_recognition && checkObjectsCollision( + resampled_path, collision_check_margin, + /*extract_static_objects=*/true, + /*update_debug_data=*/true)) { continue; } @@ -914,6 +920,7 @@ bool GoalPlannerModule::hasNotDecidedPath() const DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const { const auto & prev_status = prev_data_.deciding_path_status; + const bool enable_safety_check = parameters_->safety_check_params.enable_safety_check; // Once this function returns true, it will continue to return true thereafter if (prev_status.first == DecidingPathStatus::DECIDED) { @@ -927,8 +934,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved - if ( - parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) { + if (enable_safety_check && !isSafePath().first && !isActivated()) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " @@ -957,13 +963,20 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5); const double margin = parameters_->object_recognition_collision_check_margin * hysteresis_factor; - if (checkObjectsCollision(parking_path, margin)) { + if (checkObjectsCollision(parking_path, margin, /*extract_static_objects=*/false)) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } + if (enable_safety_check && !isSafePath().first) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + // if enough time has passed since deciding status starts, transition to DECIDED constexpr double check_collision_duration = 1.0; const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds(); @@ -1406,7 +1419,7 @@ bool GoalPlannerModule::isStuck() parameters_->use_object_recognition && checkObjectsCollision( thread_safe_data_.get_pull_over_path()->getCurrentPath(), - parameters_->object_recognition_collision_check_margin)) { + /*extract_static_objects=*/false, parameters_->object_recognition_collision_check_margin)) { return true; } @@ -1519,16 +1532,24 @@ bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) bool GoalPlannerModule::checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, - const bool update_debug_data) const -{ - const auto pull_over_lane_stop_objects = - goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, parameters_->detection_bound_offset, - *(planner_data_->dynamic_object), parameters_->th_moving_object_velocity); + const bool extract_static_objects, const bool update_debug_data) const +{ + const auto target_objects = std::invoke([&]() { + const auto & p = parameters_; + const auto & rh = *(planner_data_->route_handler); + const auto objects = *(planner_data_->dynamic_object); + if (extract_static_objects) { + return goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, + p->detection_bound_offset, objects, p->th_moving_object_velocity); + } + return goal_planner_utils::extractObjectsInExpandedPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, + p->detection_bound_offset, objects); + }); std::vector obj_polygons; - for (const auto & object : pull_over_lane_stop_objects.objects) { + for (const auto & object : target_objects.objects) { obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); } From 02ba67fee4785dfcbda382c3315764464ba9b90e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sat, 27 Jan 2024 14:12:43 +0900 Subject: [PATCH 133/154] fix(avoidance): turn signal doesn't turn on (#6188) Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/utils.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 3a9b5db283304..953f3f5cb2f2f 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -2278,11 +2278,11 @@ TurnSignalInfo calcTurnSignalInfo( return {}; } - const auto left_lanelets = rh->getAllLeftSharedLinestringLanelets(lanelet, true, true); - const auto right_lanelets = rh->getAllRightSharedLinestringLanelets(lanelet, true, true); + const auto left_lane = rh->getLeftLanelet(lanelet, true, true); + const auto right_lane = rh->getRightLanelet(lanelet, true, true); if (!existShiftSideLane( - start_shift_length, end_shift_length, left_lanelets.empty(), right_lanelets.empty())) { + start_shift_length, end_shift_length, !left_lane.has_value(), !right_lane.has_value())) { return {}; } From 1c4c3e3e5b41341c513b536da19efc297dba4dac Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sun, 28 Jan 2024 14:00:19 +0900 Subject: [PATCH 134/154] feat(metrics_visualize_panel): add new plugin to show planning metrics (#6154) * feat(metrix_visualize_panel): add new plugin to show planning metrix Signed-off-by: satoshi-ota * fix(tier4_metrics_rviz_plugin): fix small issues Signed-off-by: satoshi-ota * chore: add maintainer Signed-off-by: satoshi-ota * docs(tier4_metrics_rviz_plugin): fix docs Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../tier4_metrics_rviz_plugin/CMakeLists.txt | 31 +++ evaluator/tier4_metrics_rviz_plugin/README.md | 16 ++ .../icons/classes/MetricsVisualizePanel.png | Bin 0 -> 18815 bytes .../tier4_metrics_rviz_plugin/package.xml | 30 +++ .../plugins/plugin_description.xml | 5 + .../src/metrics_visualize_panel.cpp | 86 ++++++++ .../src/metrics_visualize_panel.hpp | 205 ++++++++++++++++++ 7 files changed, 373 insertions(+) create mode 100644 evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt create mode 100644 evaluator/tier4_metrics_rviz_plugin/README.md create mode 100644 evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png create mode 100644 evaluator/tier4_metrics_rviz_plugin/package.xml create mode 100644 evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml create mode 100644 evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp create mode 100644 evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp diff --git a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..4c1e8cf58c262 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_metrics_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets Charts) +set(QT_WIDGETS_LIB Qt5::Widgets) +set(QT_CHARTS_LIB Qt5::Charts) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/metrics_visualize_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_WIDGETS_LIB} + ${QT_CHARTS_LIB} +) + +target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic) +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/evaluator/tier4_metrics_rviz_plugin/README.md b/evaluator/tier4_metrics_rviz_plugin/README.md new file mode 100644 index 0000000000000..be94141254030 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/README.md @@ -0,0 +1,16 @@ +# tier4_metrics_rviz_plugin + +## Purpose + +This plugin panel to visualize `planning_evaluator` output. + +## Inputs / Outputs + +| Name | Type | Description | +| ---------------------------------------- | --------------------------------------- | ------------------------------------- | +| `/diagnostic/planning_evaluator/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Subscribe `planning_evaluator` output | + +## HowToUse + +1. Start rviz and select panels/Add new panel. +2. Select MetricsVisualizePanel and press OK. diff --git a/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png b/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png new file mode 100644 index 0000000000000000000000000000000000000000..6a67573717ae18b6008e00077defb27256415663 GIT binary patch literal 18815 zcmc#)ig}7!VL~0BI3P>4u>jX{5WQyQGE==@gXiZcsv`yJ3)ShVHnpzyIKV zx%UIl!@PCQ*=Oy&*IwtGaAid)9L!gkAP@*gMjEOL0wKly`$K;YTnXsp8Ulf2BxRtY zY95&f%V-`irs2ZJD-E_>W|QJ#$f(lDGUzn9FhfdJ6ls+f7g13tl1h)8=nGVpD&-eU zEXJbkUMvxiN-bt%sdy?0Ws#;2U}Tww$D9k?tJrv3b90N4w6q7mY>Vu>j)cq`Gq>&g ztxVotd7x=_%0QrfCTU6#h!Tqc9R&LG1r-?ts`)>?Y`h5pRS_lWag8OXawfXJHk9@r zwSCxzbPRm-g9n1hq$!Q)8a9)#!i(M_cz&h|6vnpsyKVD7twGSL0-4KL6Q=BD1ZeP- zzfzYzWVM-q+?d)>KwJl;5;rOeaVP-p8?mb$R~c z!r4STVAB1bJ?W`Q1N^wk6a>;QVI%JxOu}Vf{^G|)Co3MGH{(9RMV<;nHU4Za)%KSC z`;I=`QN*K~)?CjlT%q3CJ0d$G%J_7^;$JI4Ms3f{6oyJDbjK_N!4ju2 z&$?3P$rm$WTureE-aq`r%H%qf&xx}1+^E65XE7*U*@ASHv#;8ex#{Xeuo?I&+x*og zCp;~E10-qE582|U!y4K~`SF#B31Gjpj3?LDSx6hf<#=NRr5x>o;P$^l9osYrEfL{WYjQydU9;g?K5#3k=W*FphTCuCHewl5&`Uu9F)(f>MMOxB>pE z88huBN|GrNq|eH~M(ELpl-tdZkMhs%;OV+^2?6{7^3=m>h4(Sn1AF4SLTeDL;O9g! zBWaM6c%X0DechlP8B_BUQ^1_B5ImG0q$&AGba@Q1!Sx#uv;b)FNlKJ%FF!gu4goAi z+jugMiX+lt3%Db%$x}1fa)yh%A&xxGW-Lle9BaRZcr(Jl8YJvmXVoM8b*d?nDn zr>xqjT|4gjOmgjyb!-I_L=1WSO(};jF5)F9(KK9k$6juv0YKyrQRHz_DK~8jI2)+w zm-Yt$QfvYLhOutzxp@d+SUG4qO=vJ%C2^D=7atD;N#3;^c;DQzmR&&!oPfkcBJT}x-4FY#kT02m@DxoqS-{xT|$ z3PAj4Y06h5S$bR{FjZhmf2ImPCOB3Cm{OLqda0jhqvfLSxmZ+?A2<^h+z64sYjMk*OaM^512bTNz1yJX#3oL;p>F~$69vB)J^K&J0kyCuHxo2qc9?N7 zpx^)Vh!`XhEMW3-8hQ#=jbwx3L_Yk3cspR5f1h~;US}c=&En=n$AFqJRa-Trw&}AF z#AgA`>wx2@JWAg4l9l5}1GY|0Z9CNsXNdgw(z**Vj@S4BOp0I}AA5Z|Zl~>Z3kVbj zqQ+>~?`j3P7S~|HAAK#cX8I@4)zn@Yx%DiDRJ@~c4bTC4ySPXJL&So)v8RA@6#{J%M_y$DCRzdd0*FPR zEH--gPG5+|{w5Yegr~;hMSkzb)vYj91mGV5xcRrf*}fIu`8(Gk(oJHQTMWSTlpe3W z7U|f@97Ozui1Pqn>xtK!34&Gp1$<#-lg>4RGs7K!6%!+v&|>f%Vhw^400u&sGY~q5 zJ^R2hK)+x%+?N_ujGMppd5=D@{V_pS_0HC2*7x*eApoKv>S`$MhIW>f zMXBEn^5P16FfqDGOxdOTX$;a&7%;G-28$P)$1&8Il!2^?K;(-9A|F432ZYVYM^|}I z3>OV1q`?mb9wc7}dJzwJ)_0FSe8o7Cm(><8hzS0{vBcWu7VZ9!xVd$!*%0qZqPEJb=%hJ$Z_L zz~Miu9bQOq9)39Df;+w>C4k8SnA^IR``!NH4|SUf61t|;@Y}Cs7L7w2kT8`ufki#> z(|*WcvwwZbRQK{xx%^U^9it@ui(i`PZ(a!{U!?$lf%er!QwQi?Jq@BTnXsQ0^KiZy z)@H)?s!l`=|Lg4jvo;>IFs7CP;9l8V4UpAo>sjIq#`%v=kuRv~Dh zsiW)pz26t!@MhZgf>?_yj35jsQu16~;-8_l{?rj9%$d(V-9L6Mv^t{|GGZ5-C`W&WqNh29e6d|hsvmvzE$*g zQmyMXe|TD_9?2}u%kdXf_x#(4xd=B4sAmzB{R*=+ZDMv*{0=ZZw`U z1zHnRLzGA%_kVvk=&UnLH`*nK6OLz)IZ+1RaP z$_a6)pzGO}t0_(S2EeN@?u0pWGf^QOT;Fy&fIf+061Vv&k6#eQ9E?CJbN7 zR~KK=eS#kf?q9C)BZqF)@T7l`iF(-E2|R4|?91b}4Ez)Kk2FyYsGq$iY-#*_u@~1} zergZr={EWSme9lc1Lvw@G~X6iq*U zJ>6TJJ1l)Wu8Wk+%I_jO@X>ttg#2Gtvr6H&5?wENhU3wfOD|1HK$EE0JWPIo?o%@w7QX>1z=lRIcDYmm^RdaH!J$U0qdKjR{v?Nj@k8=mLg z!QaffX+{G^hFLx>yzv{!{4I7+*?C}Mw;W;Akw&+^oGNB zKbo1D;RxOCe8*HY+Wq2Z{a&$bq>kL0E*5uqV%bZU97(Rgg}axR>R3#V{3&SGo$cMU zo>il+6}(CD_7>Sn+RG>LD*ZH>q}A^P_|jLN;2-ZSZ)e^-CR7g&UCtF)C)#ml2sjLp zna0G_?3W<_14PCTp7u%y`1jJLdPCb;WW==}ZA1fH$8?tCsR>bJF?vJ$I|J{? zlTYZ~d#P!IC7nc5MI zRsbBkP4l%D7PaBB{&6DKt4yR^NwBqPM*lr^I~U=AH?+LJ*&C$ZAXr^~8P$6rvc|OQ z@5*T^JLt$TP5T0t(=wiG^sXg zEd~AVuZVrio-s!T&LrqYTve>D*P81_=>7;qF3oJb%9-_$ zl7tF5{GK`$Y)Po@y_Df6U|SUSg7!xbv_*4yVe!Q#Ebc;ZcwoSK)rHUB2ho1zr!2(h z%dVk7oadz}SQ849EKz@)p|3b8py_+bZq+KNZ|#0KdDLC_ZhWXy#9XXyyOh3&D_KBd$d5r~dZ&@?gHCJhG5mniAQ@R>y{O zFvmlJ?JS~a-%K~9k;T?h@u&=-btl<6W(Mr?#p_#Nvx_0v`ilrR<$^+%&Dej}o4vNPn* z;owRb6rB0y*XFvkd6OhCQqGB6I?r)a!X@j6i&CJJ1K0a?>M~fR$7l@HLqkBtR{DNi z#>_e~=e6CHkiV3$L3^TB!NYiWD6Xr+?LjRJ218fc9?44c9VrfHEASeRf4u5=;4Pz2 zbmAarlXGC+<}+MNXuEB2%{j_j84W=YGt*J6aB$pdMP%f^yA?e@n6EWm{~fLLbbmH# zB#6dX=5Pv<(zsg%*jkAKL5r0knZM%tivqc1CatP(x82cXPdwWwcmjs-W@q|ImYY99 zYwlE~Y-hze~lk`x3mjPB!27mzk`xg^y;8^_L_ahVv6~)|t~mfqeU``w(TZ zxrTjOPssfK`oySR$6>Am1>m)tK+)!_P=0!6FfqlwTf!xubMJn*u$lh(uwJ8c<*Bgn zDIx;XIk9`S^Zs-&ZN{2BW%h21L^x#Hy{t&3#$u%2EIrNNs9uMGjS4;G4WkVIc79S?9<_)}5sGh^o2GGu01pfp6?z7wr_`sf6M}tS@hT zNCXk5MDOIEc)b#>BL5Byp4r;f?uV$qGqs%c8FpR1U>>e-v`u`iQ65*US#Q)pH|_z0GX?kiG%%(LOU!%Ckq@v`f@JK|pfX2{xiK)~<5soB5N zZ^n6hgiX)Z0q$xxBO+$zshAz%JT;G?R^ui!yaQF=CYolEefA_D418x5EBtuv6S_T# zt>8C5ZK^P2{Tk3jP{74C9i5Z*XB90Em|L^1Lp$Ty?ZVFx%zQ38_!1Hlo_wx*Jxm>5 z;cSTnM62^vnW0! zv(<%*u%dQ#ZEdDm`^UPysNb89-plAwdG@K0)y_jBD-qUQUN%)_5(TsN78%{K)f*=5FXVTI-5jC-M0r)_e2{Uu>Io;LFZ)WTSayCh zH$y%7yq?2_WZ>t-lJJ(ErM5;8)CMsVjU&indTZFQ!V%ji9)yN-+O%Xp_zMN?x2c&~ z$h26bQIZIOY$0|Hl-A{A>HW>D%Vl99Us^*}%6q+6(o_lL&Dq|Mo_&23{zG+CyVGoa zZdPuB-3?-rE;hgTQB^{1j%hnpE81vUZxe{q7+r62)9R zQjuMBfm(#Ms?@4Z$;Ik^iw9oWqRW`6isW|}$3~+7uuRp_uSRo5+uoV%o;%fYOqe&( z|2-;yrIE5RJ-g8wo6hVoIj1wlF#nl)eXp0d4XYB-b^eG|Cv?<)1s7iS_r5%WWTvIf z2b#SW2vI-|e@Dud15=%5ScGn5xI-4JpMXeprsukuG_&3pJ9vw|;{D{iSX$ZsyN-QS zIkazCCe z5tjTIqBD|W;A`f4 z(o2%2vBi}U($ZixbedSg2KMZ`WSTOQSc*AmMXpEHQN>d_Ts3UpJGs9*!n#G=T_Pyr zRXRI_^rJ$49%InZRr>IOdrQ?H&qWVYSv0t<)_MjA=@l6!>}m{uAv-nCbu4CkNL1I0 zszx2HgbeMhEPPFWT{;-iq%i3PmyXoFdjD&`g5k23{NZw;w9<>q|Iu^oZgl-bE6dSw z^mlKsUR{#$i%`XX%3&%n7Tqbvr-=iv3Vn!kYG;%8(aknQHmT914|hK(l9+ATj)ON8U+A7k}qtQoh z(o1n!2r2de-qqvX8LPa^T2)TVwotK{{5PUlaF}h}t$?CyJIDXX_j2AsLij`a@Rsi6 zT9`WmSJ@(5~J3(YDA@J|o3An#c=nA#EWWa`laHX*T$SjobapA&u?(UWP+Q{C%@7Q}QA^ zJNv{`cm#_$-9Pc`0QTcIA;7WaQ;?NPKQ)wA2wg2XAF*`a%1+DL{Ei|8qxs#0>_+qE zIygdQ6ZJGXrnY0=Ug)~OeO>jx(RvJib8E2<`5+hRj<=UQ`XSK&;bJyBjKUK-vv>=H zGCKa(@!Sj)ME<{YC-0A1b(Bqx63NF8LaQ+pwr7++?Y;83W-@6>tv_S?kk#VM|MVs2LY-DySvyPfa--K5g8|I^)y zxR)i5=c%cxpXe{y&Af@{;>*~{b02i&{aB_$54p;>&#@87qjme%iw$;g@1y3hdBV!k zZH`JGW7Tng^Ak~%}#;Q-2Kv@J$5 zPoiIQ%EmkU-JkTeJJ;0SF{mVKykranq(ebDsHb&e)<5FL`@L~x6=ryQdf$-7vCQ0< zVKw1(9v>zMv~Hr4MzS?k%k=W&z-eNFmOLHeG{ZZM>d@B7MqQoA_xwa^NGPaDE9_=G zqs6AdhWYsONzd@+OaTr-8XOBq(wvBuPJ?Raqs7jrj-Q(vB?55luOh~nXxNd`{VH~I zM;8yWZJr7r(}dDk^xNhxwsalXR!sA1N(T6xx8%3 z9i?;He4?%(U&RqXs^zm7y_}z_XZ`rKJWdqi+!lDPbqjFCc03mCyq$|H4*?12d18Xu zsc9-IL6-9k#5yU9z|37?M|bFc*A_tKQa1XE*M2$dVt-b%RtQm5BcL!+`EtBIz{=SB z>EVhR7s6-iy&6K2n(c0w;sbP}fDO(N@z^>W*1LXDq9TP$lSj^&IGFi{KpGIP-++mV zR2tI$TwO=$)6FcJ+Rs%5-rd7wzS5;BWefCwh}pkOiJM7&g&1R6o^Ft!tBsfjK$~LV zchl!;(BZ>dy(Cy;~wVJ*0yFzq6!EcXR5$rY?4gI54mUi2JbS2$QJGSQ5Xyxt;M>AF4Sv@28`bN@} zlqwK4bFtGi`_*QHwN9+3#N)ub$|i36r9T2zdP63$?>Q-i9luzG2$gB(w!3~+Q&2P3 ztg~7hwvPLAu**HR*1BKO`siW+cbQb!0_qL(aGzg2%;p*MKf-C??dsxN<>Sk(?%vrz zzc_PV+EeGDFeZ;i84*ZtW=H3=#2j|W^|ND<(N6Ys^vYL`H`p!um`KLX43QJ|#XvZ& z5VNDI>gv4*nZ&heTY8$rMpSG1;mfQ=IFry0%W*2X;r!dHgLxC(0(fUSoX7 zwNjEXD0t!s_`m5$=5BFW$;8yBAcYLBacVr_p>vrk2KwkD((ynWWJf^m6M0>Rs^j&K zcuG{Bb;k~FsXL?edF#}7S zHZDLmq@^Y_Y4fI*ecn|MwalQipJKC!`&+AEi=Oxumg^tYvS0s6v}&dED>#4Eap zAJNq9yf;-G>9>swxt+fKLmkVyKVy>_Z%8^@94F5{sE|(%yu=RF} zU|f+?>1X;0!Jp9#;p0r9dszlE)NBFz%Xb>g4B!|F;iIe%Rte_9u|W9MNnz3b-6obM zRP{c^sD1ZD1@8DThqDat@NK>7r_aG&(BpD|3!JTXho#kHn#b%IMn={-%208He(I{j z8s5K{sLB<8*d9?YNvdlAkE1Xq4M&^5V`}zED!eajyno=M;-hVhny2RT;+R}Z9r4hs zHg0|z%N0-eaMaHz%=Q`>bdt9ENtBg7zrZD9*0FRwt#|ss^{%z@XNh|0y-*Iz&V$F+ z5b1tlj^6+i1CSkW24b4a6t9GRb-61JetIWG3TakcE8lU{iJ1*WfY zhT3M?F_AEMwx8^2o>J9?B73d2M-Ao0yblIV=w|pGk7+ zChG0Dy~rHOK1Z}(>0iZDOF^?7HV2aYQZklSUv5SgTzl!7E#o4p(rOC24ScRx|^VD=~3JdRynp4w?)W(5A0 z)_C3;5w{#ut;AeK9af+D3$6?#Btje>?{BQL*L-vD2I8Ik7Vg!@QnLe zppa1pgsr!=`wq1_mTHZ}C3xl@K9Vf_gQQRAD_36GdaZ87H=x1+REu&}7i5gjPv`MP z?=NN3^VvFySA4Is?wUDmnv~`eOyF%d=CNUipe$ZOJ}FZgXz3+QW*Yt>xpMko-JL05 zks6_=q<8Zb!SRc%GSdhvd#^TbuH?h6Nef0sA3Wjvs`Q+zGw6AI#HO>wHLBBX;iyJI zFZ5A$b~Ji_KEX=v=l0q7@oKkklDfM5JEnZw<(BE1>)c*>-8TRsJgfKBDNYu#C!HGU zbLvbP6<#?=;4iU0T&Pb*LBnMo{A)%V&E)8oVNDoVfNMGIGqDC~Tscy8s*HW5h(C^+ zG%$nRO&>=ccGarn_c)}SF|TH0#$7Cx8+pN%20+veAhSU?RF1MOcSo&5t!{_L=^j!S zwpKQodunYpRvLi#(OCA4LtJoKXSCHK;j8k9qyr5?e~KQ|t>1+SXe) z>)&;E*Mlsb=gfL7YJkKP1nqI#dXEOw6i8@(OKrTvK3jR+J9~Xn-iY+EwA~bLWlsRR z23NE2O*dU)=1+e<1VW`GQj_6h3MbAnyDHL5QY*f|gl9W++|d2#xN2PS<3Ii9G|AA= zOQyQ5g5I}YLVzr($N9aHm+H&mo*)(pv+|j@cP5Nnz1nAZ%$JsKX2hLXNhUIS$$QnG z1AL<~%s^invTXLbb=gI`FD6^i`_j_7{ay3FG_nui=Feq_SET$RM11j`*GRQ*M5-@2 zd2E<{+liz^%K)8H_K-`#W19`%J018J8$M5t{XUL%67!Kv?T5Y6%HoSPfj9xo@SRNA zO&i%cnz)EbdPRxH)73v%VP9f)rk1ApgoAeZVYB+))9p8Bqd4&bBR~*`oNY>09O~N2 z!Rp>7KnHX=U8>;-697~ZJ_sN<+Irs|Hl_`;G1z694OixT`*V|Cz_YBGLc(tM@Kohm z>JD&MYd3Xf+B|B;lk$xwEPD9_~%=V(vx(C@a6Y-H$#=bfVOn57$&>rkwrVs4#bi zIXh7e(IM6w&y9tw+~I(;y6HV5=yM_(oXxC~r9=|;e>##2d}?n{dgmZtm$kdn2_X2$ zZn1&1c*eQwvMIIRLX^A-fxxJ(8WBPe?||LSuy z{t{iR`A!>-41uqe*4_8|FWHQLq|ffyeq*aJzDYNo!z||t!@KwR;k$ICGOlv)`wp!Q zFe7=&V)-C1g>f{5f5E>X@5^pc*;|x_)H3t0Wj(Hdf}t6f7dj;Lm3y|b61$Fgis3HI zZZTE=iadp@o=Y|={({-`cL3Mp!SitW{cQ0t++xA?d#mq<7jHsy-_FH#^te&BG6e9=TM4K?yE4$gLtJB{QzeetB zryUa6*x1vKi=Ur?^*FjI-@8jse%C$IlbLGM)=y!06b)*c>gqrMkCt)IoHxxwf;h>K zI$ZxUI0Qd&(@HM3+nD1SE<}@A-dyZ1FM9v|iBvCi@E-ah@k7-!4x7-5u`g+?MqO*l zm)P-Zzu$iR6iAlY+w9>D6kT>tx3it8Zf2XNw7OwQn=-dqz_;&l(OUI~)JrvPrtJfn zwVGJjkAIYJ@iHeSyf)eOw~G0ZJld2UGPIIZu3h{4455%Qbz6lZW74FexmYeYM;8aI z$K-h3Y^GFFCuw>-YX)_9?Ig^9A4wx1xQ2#eYVG!-LK}|bv$R8@eqlJgC_fD*}q9KhIE1f_i#O7_ zT)%x`)T#K2OJP6!v6Vb?t~cc+MQq6?;as!glC6mA<(e z<%lFRX`h0@1C67z0NHWh|ZjuVZ|V8t@FP@kIUYdtPp)2I9s3B_BXeBp}-ZP z5|l~AJ-|-~e7adHXKm6rZHQ~nBqb!qRwUhuE_?tsh^Kl=Hlz{jJv5 z%a`iA?S8PdtMf0~>M^6t2;7z3^z?rn(s|R}N+Jo!JO1-*rxQyqqw0F6X>RM!ACLOT zz2%2LmeJ%LT4#%}#EGCHI_{2UWy@EDyhONJs|(RvT?Z?St5 zJe~uJKn9oH8ehO#Qs>h$z{xbsOPOK-DR@vG_GmD%r#DqpIINVFeAg~=yeiV@S@ycUgHP|m(|5vgek*ZEH4RdwK{d$6jL>+4X z`4ChMuWjS9nJF{!ij95ygn<6)v21l;z0gZ@UjZab`9s9KK++(ZU}hS#jlPp@YHsdX+N+j<%}bAm_k9)T8@LAgyH4JzSyw>)>XO5eYZ-f(KZq4CZ8KAE}&p33a#nAw#ZL!zhkYZa0_fy&Jh12UfTAj4W?< z)O2(X-CC!M1+7n}8-n0j-m&cbyUhc#29lDYkyrH{#~wfcNz*@%?SyYHR3jK#qYkg` zfOxt;Xp>!NetI3?-j2$*c6E0peFJ1k0qtRyN~0J}eb0QIcI)6yPMaCaqc?e?=5%L0 z!wWTLgHwtNdLFYf->C5f&&TSGJKKO9egTz|q7H=dL_|bSuP9CLa~>76 znCtPqJkWLkR->ldp7iOcx({hE83#s3x$hnxA+ElF{8rgnRi*qnM5H$vfR4=(~l-hR-WQi)59iT6|TZNDzuRd5R$FCqNF)uguYRz#H!g{_d46@8|;6d)C31g+?wwl9UWUN!_7@iU(y8@`IP%@=plBH%=02yJ&ynGhwto!JlUOX z4ZlnR=UHTX(m73bJ`MY1G35^fepmv;GCh*P+1Gq7e{QeWW1q~jSlc~K9=z&1x!R5Sw+>Pg|4d7B*QMP1eov^xZ{boH28L>~l{Zhb#6){LZ zm@6K(L>wtG?EU!}!s|J9nZo{zoX5(&q`-6B*F~kgn1RWbz(S1+KjtWyba%eqCgn)S zFN@z*^(@_A3M$`75z6`%5E_kQVPm~8vUW(0-)Xpgi@MOw2BBVq0JDHsQp!G~mGIp` zt+g8jO^5Tn$1Dr}bH1cx#Vy}6vXegp0~5m=T_@{(WO?uY<)bNy2_cW;Z}e~9$_)RA z&R+=3N)8DzIPSQK_PD=ZR~&XP^$^GwPJke6@_WV`6v&1R5CO#>3rm*Q23Pe8MP7zS zN4FFUff1Z$-rRg+ zUOSJkl&izPHIn6>?4GZlL7(#}f01PSSIocavf%MkVJQEWW_pySvMFl!k06}^LD&vwY0rFM7Sdo{5 z!^6JOK>wyp&Wqx=>r``x(>dQU>14zTUKY_wnmZX{if`ugWtEtf5;FbfaR!RPMaJd=iiHkN)+{o zw=7_sEj@pbvWiNnS(}3oHAP5B$P(ALz1hl~T>aEY-XZhaZD8r8xs7nX)xEONDCjv; zOUTNmo&)Iqji8`liKzLOKAgKFJ{IZWuJ!WaoUryyAsQa}xqK??8(ZCEx7KBV%c+g} zKb}#TJ3DI#oj_gNy)Lv4->#&&w<8@3lV0qv{JbmRz6E=*>$kXAjlB;|IzAm_X;uCB zv75{DlZUCPX_L_PUzBOWE8nZ-zMA|iD#R`FU!}FRHDgRnO#Q#U-C^A-YHAzgg4T%^ zfT5UUrP9zgG!1;~y?UlKy%*}N2#Hw+^T|{6+|stQd7Vod9abu&XJ-I|3zTY{NPshIIhMP4(e)?%0Uv@SDj|pppr_Jb?SU%^oes09s*d=hLG% ze`5g7Pg|}#S=k6d+Fsg@%%xAxPEJlx7nk#=$7MiN?f*(I2$fs2BE8qna6zlDpw#AvWn%g%t!02^>Yj~m`Jc$>iH%XJ+e zZKMxoZxEm;V0$ox2?Us2H~)s3I>H<)HIzL9C2*B2%2% zuoz$qGG)~AC_jk0_89l)-w}Kjrep=sU*p1KA%bo<^)Jj!7x@I^8msO> z4>V)boi5cC6{?m%cYKi1zpQXO7Sj9MjcR~0ZwA;qj*h|~Zix)e+JeBwbFm_`UgUBj zgZ3CD_k(a;jH;dGQ*p3Bd%O`FzipXyFXu?vx-t zRRt(cFcEnGVV%fzDOH|TTM%e0EhS#yS{w_?&t<&+7NqhCIwh;+O2Q)sTsc~YfC3bU zq)x}416#3Ue7ioXI|D~PEaz~kUWr15fPZx;b=F9Tzwr0+<&|tGXq@ z-!p+0U!6Y(5C2L=UkFUuSvc=t6~zQS+H_VW)Q7oK9zW$`$z@cCY7+mxgHfQ2uQhZ-Bg6F)mmEz|j=0p4I}11k?as_b+mA%#j1I8iXEwQXk2s&yQGt+}5bJPGb zYs;@L`=D=*mVrBcjz(@?+%AG&0^6ZbXHPVwwQtYzgjukZEJ2bVC7?Lfli&RQ%4ql> zZ3Bjstq?yQBdHTP-^B*^9^)d5i0V_-08a5k6bqU|cZ>@XF)H;$15M;U%QGN@6}Z0a%$-dB3&%`n1N7*~lrtIJ9})K!Z&`SKbd?mA;INup)SGh_}|FuM4}K3U^9Fb8+f3I)=dOLQaPRBqE3YvwXIT1%t;9*xKkQh>Kn}+ z;Q<4<$i;f+^O`#E1n{wLZ`^)rpKs3y{DkQ8RNlH1vu<1qB`m4Ar|D>VgG6BuM+f>~Dm_|t=53>L+p^5?Ap9utF zppJ$KnZyZs@R1KmMUXW&i^`seS3FLP#h)k*_YGfPxP+Lf^-x z_sPQj+kVNDcrI(6;l2Q!R|cD$^7tp7bf7;!t@&f~h=LG73c#aHVNd&EsO|JDJ9OOx z4GDygnsW{dr3{vF5xpd#k62wqCZIlHJHXZfU5XlU_yV<>Rk0;H^GfkD(N zC8;^cd!U6lfJ^KX0@2Cm4 zd=8d)ev_63t*c7*w-JRN1{3v+Dme+gIiEr$x928QNy2pGAlduak_Nks{AB%r4d$fH zGhrW1qRbq8d%BIF&*L}E-fw%oD&cP&m%}Rqb%t5mz_v?@_2O7|JOgdKNWi}S@&06z zP{QC}7)~36DkZQSelp`cI3;!}dXmDmA1C4Sk2IJkXca2?Rq1CeU5pr=Z$Eb|MA79r zy57rE7PN*8KqGRYA39>(eT2c|(NGsfr{%mU54|>P>rf=dU$a zVl0M^67U>rcEgp*DN>+P_xfhO3&BA|ja^IR9nZ)3c|`h+cLeVW#xuq+3uv}nG%A6$ z#JM=Sf#pn?8f_ORN$Xqfs`b>*IN!zY+eSWe1i~INHf-G~8xDL(&1R?cv=sOgWv}k5 zvteF@Dw-i34yuxg_Ahk2WZ_t?;YIDiHl87$aT0m1go-04L)r0u>_1x^n=umxKkx@T zXKB)YVqzl*IYMPkmI~jo$xVn-{?=xiJ8`YE6AE&DmVLZJomVQhqu;I!nEhjZSW}A& z9aUJDnK_|~5?EL>2L>l;*|=qJIbzC{IPFmO%Mz2JRMpR3A?xzA_EP8?g@56F6FM$q zslHsKdNUki-|0=HpNsVPE(Hds%j;8TT#?q*MZ~(-<~C%b_A2|X}`fHdwvLeljj~n5@a2L|D4uGt&g#XQX&30uL+(&sRQi{`=)W|ELz*%Z{nK;cEHSxzI zsrA`<4Rz6Z8InRbWPpDMuP7)CHtM(&gT2jMVmcA6EvW4pd-}Qk^8M1f ztkSjGqJ~jOXiyi^+0T{nVA#S8`(S%RprmWLq)G}RWd2UQ3q_lH)oTEP+*uVvg!LsN zp4D^TqM1J4uq;@HeYBs!V3ZI;JIi2~d7$Bqe6W{zYP=5{*J{>)N}3r95vhNZ5{>PP zuI(%1IF+xoU4$(JU!{WLTR0_#*HMh#ml-ls=i#wjLt?A2l@z+gU?w4~g&W3HA^s`4 z4F}UB7IHlO-A+nWfeRdy@gjRdiHelQl{X=Mi%^&FKd0*<@l5nI+3Vc#h{rC$Kmoe- zvpZ3bMn*|-qZj5396y5q;CzC9kn7s`b9To2s502|i^nLOu-LIIk#*-sAS;X19xv|T z{0*ls}liCZmq|f?Z;8zxgGkM?Er0=gxe!}5B)Cfts%zx+KRG~>%xK$^Qwgd zxLTF&cv9bZ<-o^?!iTp33(@-}y0vl_)2I~G)HF}92jl3v=#<)O=E7Rmdy2oglf|Hk z9aQ(|WOUn)IJ?Ev)&Fx6bF6)kaLn)DLr)thofgM!&@~!rT^O7bt@e5Or7|3fA$%9Z zdcgKVh^j?@4Sr`l^A=M|1^Pj?Yjm{L@U2TTmvgF?=HwY>`I&p}76Dqk08?8TUT-A< znh@%m4Kr8Z2eAG|xkt@vbVWTWWo?7J*w`48u>8$6i$5I*42V{tPKX%OlI`)B z()!3ib)k>8#b55^e=w>k^6=B1ZbJ{JnSv5WmCH4hd6f6{<7;|@XRLCWiDm99yGhA{}Wd_r0D9El$(X5~ZD@6Yeq%JJ_WnN-%QoT_^& zls{`|J%9EnfAEk~Gyx};)G-ocS*iS3?;B@s#A40HNR)|D9E{SnkxmA^FH@JMsvova zX8&ayT=lAu;Am_z@Z2<%$ z6{BjEfS{w}t2VXwOJOAH%tlL96JNh1#=Wf*>*+C=wXQ*vNIs(oaU1*ElJg~AWBoz3 zYsEM6%XU&tu#eu|7g)PJi(C|&tXR}Xn&hnaxd#gx>+j>0Lv??-YpZnq{-}DCX85D* z_dAPe6Sn6vuwUIAo->ylZ}KI6nQVFL;mT@J6n=}jh-Pjuf%pEv++Pdkdj*Zwp7DBP zrhTtVw6{gZP)&6$66l-|HJ4w8%;eg~^EPs4nrqIew2C|SrN%S*vk{KJr_g;`Y{qKK za@>)WYnnH1$Y7g~H208STe0$q*DizuUr(^Zw@=B+o)Nq~aS$e4yGfT@>daH^DKIsz z)cwow6@MRvpi+T4eltCU8Nd3QQI$>-+ zFgW?Ai;?m(M6`3~3t0MUNoj|7dB1(@$eb{5jiAiUXxRcsX%N7syb8a-?5DbXQY&dt z%QtBuucYrm*1Dmz5?dn{A0>WU5x4YjETQ|M(8cAmSNvkb`2Ku7q`f}>zYyO<$8g@x zUzs*Gk#M%Gr7AVQ9ITO)uwuolOW^S4&Y0P4a>%SIVE2Gjj|O|=mQu{^v#Iu?g_u(5R=+V-wf&8^chyn4kgY80C{bYAldhLKCoo`h9f)w_=Um8!5STwbwF{_U3Tizp{AyHC84h8bs|GATle zy3F*$Rqlvr6MURu^<;ctoJ-C-Z#8oO_$tvow?$;EY1DOi-(^S-2TIzlGbRuwb&;-U z7yZw0dus6-cg1D}!{`%wA1C;jA;~pKJ=T-XVnJJKHRncn?@V?ZaEO-)De!(_{cb?PQZM`vF>be}<$+yB>x!n%iMG>Ka&r0;UkQJwE7Mk z$`Z}Z4DInTXYHvlPzHtdM7q)!O@~4(CDCwL6=&P!6H8`ZIu8IG){ebnFOOsgJi5#J zo*23$q%3OfclD3VJx10L$%S6mR#0O{Z-fm;yH{W~(%~1?#@9nDw|X-FV4WFNNE?u= zNsUpZB9>X39R6ko>B{E@palselu!fi@;2{*99+kw!vd7==a0k1U~mtD?VxZfC+5?F zS*Nm{8|Z%mvGF+lu?k#)zW_q@WO_h`!~-H-wk-C(55!&CS9;0~tCO{qo!|0Ld`A4R z!?{Ezel$sA7!Q>QFASC6a)j3Q9elimCP?JOd_Bc7+iW%D38BRl_}=XI4MruG!$~Y! z)#3G0Ing0FLqmnq)N|04xKy0c__kp$%3zYQ=jbnv1!pZF??**9r5!AA6QQz8HaBac z-)DvlmLg^;&QlPoDNzVPj2csI+Sp_GZ>Y~5tTw?OOsB`~G>Igi6#!z}vZxRn z_mZ3%^Wj5E=1-Gbo9g3YL(lz--KY}z4P9;Rev`lWf1j9yQ{VAp+Sf5=7Hy=={c`2yJadWY-BAQd=Lk{ z_R5BjK&)=KT9B)gLV{1>rb2!28ThgAIW#%qO5B*yF`N0*A1srvHd0v*yWIk#^?)Ts zM5jSs*i*xFz2D7|(i500ZVx1J5^<9kp}e$hGHAC2LutRtBH%ytYf07!h%N|p$O_p@ zM76*Ze2pGv`4>e77PC@W+j}V~VvZ$pJA7_u0J6SUt6#Bso7y)~k6x^LN|Y_7g&3t~ zc23kDyJ8C;xg3Hmb~#>n;^|83;U1180$1Uh7VPe5U0Yj3=xI`|HIDic);1 z%Zx)#tB~cm_w^{1mrymqhd`i6ceMi`P*4dL1i~7qi~&@l{y)Y0VJg~EDlzW{YoEjk QxT*u><#`b=z=h}h2b9~@i2wiq literal 0 HcmV?d00001 diff --git a/evaluator/tier4_metrics_rviz_plugin/package.xml b/evaluator/tier4_metrics_rviz_plugin/package.xml new file mode 100644 index 0000000000000..7bd930f4f3c4c --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + tier4_metrics_rviz_plugin + 0.0.0 + The tier4_metrics_rviz_plugin package + Satoshi OTA + Kyoichi Sugahara + Maxime CLEMENT + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml b/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..5aca5bd7faa54 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,5 @@ + + + MetricsVisualizePanel + + diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp new file mode 100644 index 0000000000000..79da02b9b65c6 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp @@ -0,0 +1,86 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// 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 "metrics_visualize_panel.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent) +: rviz_common::Panel(parent), grid_(new QGridLayout()) +{ + setLayout(grid_); +} + +void MetricsVisualizePanel::onInitialize() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_ = raw_node_->create_subscription( + "/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1}, + std::bind(&MetricsVisualizePanel::onMetrics, this, _1)); + + const auto period = std::chrono::milliseconds(static_cast(1e3 / 10)); + timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); +} + +void MetricsVisualizePanel::onTimer() +{ + std::lock_guard message_lock(mutex_); + + for (auto & [name, metric] : metrics_) { + metric.updateGraph(); + metric.updateTable(); + } +} + +void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg) +{ + std::lock_guard message_lock(mutex_); + + const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + + constexpr size_t GRAPH_COL_SIZE = 5; + for (size_t i = 0; i < msg->status.size(); ++i) { + const auto & status = msg->status.at(i); + + if (metrics_.count(status.name) == 0) { + auto metric = Metric(status); + metrics_.emplace(status.name, metric); + grid_->addWidget(metric.getTable(), i / GRAPH_COL_SIZE * 2, i % GRAPH_COL_SIZE); + grid_->setRowStretch(i / GRAPH_COL_SIZE * 2, false); + grid_->addWidget(metric.getChartView(), i / GRAPH_COL_SIZE * 2 + 1, i % GRAPH_COL_SIZE); + grid_->setRowStretch(i / GRAPH_COL_SIZE * 2 + 1, true); + grid_->setColumnStretch(i % GRAPH_COL_SIZE, true); + } + + metrics_.at(status.name).updateData(time, status); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MetricsVisualizePanel, rviz_common::Panel) diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp new file mode 100644 index 0000000000000..6708ecd7071e9 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp @@ -0,0 +1,205 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// 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 METRICS_VISUALIZE_PANEL_HPP_ +#define METRICS_VISUALIZE_PANEL_HPP_ + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#include +#include + +#include + +#include +#include +#include + +namespace rviz_plugins +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using QtCharts::QChart; +using QtCharts::QChartView; +using QtCharts::QLineSeries; + +struct Metric +{ +public: + explicit Metric(const DiagnosticStatus & status) : chart(new QChartView), table(new QTableWidget) + { + init(status); + } + + void init(const DiagnosticStatus & status) + { + QStringList header{}; + + { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + label->setText("metric_name"); + labels.emplace("metric_name", label); + + header.push_back(QString::fromStdString(status.name)); + } + + for (const auto & [key, value] : status.values) { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + labels.emplace(key, label); + + auto plot = new QLineSeries; + plot->setName(QString::fromStdString(key)); + plots.emplace(key, plot); + chart->chart()->addSeries(plot); + chart->chart()->createDefaultAxes(); + + header.push_back(QString::fromStdString(key)); + } + + { + chart->chart()->setTitle(QString::fromStdString(status.name)); + chart->chart()->legend()->setVisible(true); + chart->chart()->legend()->detachFromChart(); + chart->setRenderHint(QPainter::Antialiasing); + } + + { + table->setColumnCount(status.values.size() + 1); + table->setHorizontalHeaderLabels(header); + table->verticalHeader()->hide(); + table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + table->setRowCount(1); + table->setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); + } + } + + void updateData(const double time, const DiagnosticStatus & status) + { + for (const auto & [key, value] : status.values) { + const double data = std::stod(value); + labels.at(key)->setText(QString::fromStdString(toString(data))); + plots.at(key)->append(time, data); + updateMinMax(data); + } + + { + const auto area = chart->chart()->plotArea(); + const auto rect = chart->chart()->legend()->rect(); + chart->chart()->legend()->setGeometry( + QRectF(area.x(), area.y(), area.width(), rect.height())); + chart->chart()->axes(Qt::Horizontal).front()->setRange(time - 100.0, time); + } + + { + table->setCellWidget(0, 0, labels.at("metric_name")); + } + + for (size_t i = 0; i < status.values.size(); ++i) { + table->setCellWidget(0, i + 1, labels.at(status.values.at(i).key)); + } + } + + void updateMinMax(double data) + { + if (data < y_range_min) { + y_range_min = data > 0.0 ? 0.9 * data : 1.1 * data; + chart->chart()->axes(Qt::Vertical).front()->setMin(y_range_min); + } + + if (data > y_range_max) { + y_range_max = data > 0.0 ? 1.1 * data : 0.9 * data; + chart->chart()->axes(Qt::Vertical).front()->setMax(y_range_max); + } + } + + void updateTable() { table->update(); } + + void updateGraph() { chart->update(); } + + QChartView * getChartView() const { return chart; } + + QTableWidget * getTable() const { return table; } + +private: + static std::optional getValue(const DiagnosticStatus & status, std::string && key) + { + const auto itr = std::find_if( + status.values.begin(), status.values.end(), + [&](const auto & value) { return value.key == key; }); + + if (itr == status.values.end()) { + return std::nullopt; + } + + return itr->value; + } + + static std::string toString(const double & value) + { + std::stringstream ss; + ss << std::scientific << std::setprecision(2) << value; + return ss.str(); + } + + QChartView * chart; + QTableWidget * table; + + std::unordered_map labels; + std::unordered_map plots; + + double y_range_min{std::numeric_limits::max()}; + double y_range_max{std::numeric_limits::lowest()}; +}; + +class MetricsVisualizePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit MetricsVisualizePanel(QWidget * parent = nullptr); + void onInitialize() override; + +private Q_SLOTS: + +private: + rclcpp::Node::SharedPtr raw_node_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr sub_; + + void onTimer(); + void onMetrics(const DiagnosticArray::ConstSharedPtr msg); + + QGridLayout * grid_; + + std::mutex mutex_; + std::unordered_map metrics_; +}; +} // namespace rviz_plugins + +#endif // METRICS_VISUALIZE_PANEL_HPP_ From 6835bc47da0f596079514454e220375f2b9030d5 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Mon, 29 Jan 2024 09:02:15 +0900 Subject: [PATCH 135/154] refactor: update TRT build log (#6117) * feat: add support of throttle logging Signed-off-by: ktro2828 * refactor: replace custom `Logger` to `tensorrt_common::Logger` Signed-off-by: ktro2828 * refactor: update to display standard output on screen Signed-off-by: ktro2828 * refactor: update log message while building engine from onnx Signed-off-by: ktro2828 * refactor: update logger in `lidar_centerpoint` Signed-off-by: ktro2828 * refactor: update log message Signed-off-by: ktro2828 * refactor: set default verbose=true for bulding engine from onnx Signed-off-by: ktro2828 * refactor: remove including unused `rclcpp.hpp` Signed-off-by: ktro2828 * refactor: update launcher to output logs on screen Signed-off-by: ktro2828 --------- Signed-off-by: ktro2828 Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --- .../include/tensorrt_common/logger.hpp | 63 ++++++++++++++++--- .../tensorrt_common/src/tensorrt_common.cpp | 4 ++ .../lib/network/network_trt.cpp | 7 +-- .../lib/network/tensorrt_wrapper.cpp | 40 ++++++------ .../tensorrt_yolo/lib/include/trt_yolo.hpp | 20 +----- perception/tensorrt_yolo/lib/src/trt_yolo.cpp | 35 ++++++----- perception/tensorrt_yolo/package.xml | 1 + .../traffic_light_classifier.launch.xml | 2 +- .../traffic_light_fine_detector.launch.xml | 2 +- ...traffic_light_ssd_fine_detector.launch.xml | 2 +- .../lib/include/trt_ssd.hpp | 21 ++----- .../lib/src/trt_ssd.cpp | 35 ++++++----- .../package.xml | 1 + 13 files changed, 133 insertions(+), 100 deletions(-) diff --git a/common/tensorrt_common/include/tensorrt_common/logger.hpp b/common/tensorrt_common/include/tensorrt_common/logger.hpp index 98fe18794998d..73190d13b9de3 100644 --- a/common/tensorrt_common/include/tensorrt_common/logger.hpp +++ b/common/tensorrt_common/include/tensorrt_common/logger.hpp @@ -19,6 +19,7 @@ #include "NvInferRuntimeCommon.h" +#include #include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include namespace tensorrt_common { @@ -200,7 +202,15 @@ class Logger : public nvinfer1::ILogger // NOLINT public: // Logger(Severity severity = Severity::kWARNING) // Logger(Severity severity = Severity::kVERBOSE) - explicit Logger(Severity severity = Severity::kINFO) : mReportableSeverity(severity) {} + explicit Logger(Severity severity = Severity::kINFO) + : mReportableSeverity(severity), mVerbose(true), mThrottleStopFlag(false) + { + } + + explicit Logger(const bool verbose, Severity severity = Severity::kINFO) + : mReportableSeverity(severity), mVerbose(verbose), mThrottleStopFlag(false) + { + } //! //! \enum TestResult @@ -234,7 +244,44 @@ class Logger : public nvinfer1::ILogger // NOLINT //! void log(Severity severity, const char * msg) noexcept override { - LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + if (mVerbose) { + LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + } + } + + /** + * @brief Logging with throttle. + * + * @example + * Logger logger(); + * auto log_thread = logger.log_throttle(nvinfer1::ILogger::Severity::kINFO, "SOME MSG", 1); + * // some operation + * logger.stop_throttle(log_thread); + * + * @param severity + * @param msg + * @param duration + * @return std::thread + * + */ + std::thread log_throttle(Severity severity, const char * msg, const int duration) noexcept + { + mThrottleStopFlag.store(false); + auto log_func = [this](Severity s, const char * m, const int d) { + while (!mThrottleStopFlag.load()) { + this->log(s, m); + std::this_thread::sleep_for(std::chrono::seconds(d)); + } + }; + + std::thread log_thread(log_func, severity, msg, duration); + return log_thread; + } + + void stop_throttle(std::thread & log_thread) noexcept + { + mThrottleStopFlag.store(true); + log_thread.join(); } //! @@ -430,6 +477,8 @@ class Logger : public nvinfer1::ILogger // NOLINT } Severity mReportableSeverity; + bool mVerbose; + std::atomic mThrottleStopFlag; }; namespace @@ -444,7 +493,7 @@ namespace //! inline LogStreamConsumer LOG_VERBOSE(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE) << "[TRT] "; } //! @@ -456,7 +505,7 @@ inline LogStreamConsumer LOG_VERBOSE(const Logger & logger) //! inline LogStreamConsumer LOG_INFO(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO) << "[TRT] "; } //! @@ -468,7 +517,7 @@ inline LogStreamConsumer LOG_INFO(const Logger & logger) //! inline LogStreamConsumer LOG_WARN(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING) << "[TRT] "; } //! @@ -480,7 +529,7 @@ inline LogStreamConsumer LOG_WARN(const Logger & logger) //! inline LogStreamConsumer LOG_ERROR(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR) << "[TRT] "; } //! @@ -494,7 +543,7 @@ inline LogStreamConsumer LOG_ERROR(const Logger & logger) //! inline LogStreamConsumer LOG_FATAL(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR) << "[TRT] "; } } // anonymous namespace diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index f1e4835d2ba2d..2b218cd3e49f2 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -186,7 +186,11 @@ void TrtCommon::setup() } else { std::cout << "Building... " << cache_engine_path << std::endl; logger_.log(nvinfer1::ILogger::Severity::kINFO, "Start build engine"); + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); buildEngineFromOnnx(model_file_path_, cache_engine_path); + logger_.stop_throttle(log_thread); logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine"); } engine_path = cache_engine_path; diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/lidar_centerpoint/lib/network/network_trt.cpp index 2d841d22c2eb1..ad1a7ae90630c 100644 --- a/perception/lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/lidar_centerpoint/lib/network/network_trt.cpp @@ -14,8 +14,6 @@ #include "lidar_centerpoint/network/network_trt.hpp" -#include - namespace centerpoint { bool VoxelEncoderTRT::setProfile( @@ -65,9 +63,8 @@ bool HeadTRT::setProfile( if ( out_name == std::string("heatmap") && network.getOutput(ci)->getDimensions().d[1] != static_cast(out_channel_sizes_[ci])) { - RCLCPP_ERROR( - rclcpp::get_logger("lidar_centerpoint"), - "Expected and actual number of classes do not match"); + tensorrt_common::LOG_ERROR(logger_) + << "Expected and actual number of classes do not match" << std::endl; return false; } auto out_dims = nvinfer1::Dims4( diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 2255df99dbcf4..91fcce9a80f78 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -14,8 +14,6 @@ #include "lidar_centerpoint/network/tensorrt_wrapper.hpp" -#include - #include #include @@ -42,7 +40,7 @@ bool TensorRTWrapper::init( runtime_ = tensorrt_common::TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create runtime"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; return false; } @@ -51,7 +49,11 @@ bool TensorRTWrapper::init( if (engine_file.is_open()) { success = loadEngine(engine_path); } else { + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); success = parseONNX(onnx_path, engine_path, precision); + logger_.stop_throttle(log_thread); } success &= createContext(); @@ -61,15 +63,15 @@ bool TensorRTWrapper::init( bool TensorRTWrapper::createContext() { if (!engine_) { - RCLCPP_ERROR( - rclcpp::get_logger("lidar_centerpoint"), "Failed to create context: Engine was not created"); + tensorrt_common::LOG_ERROR(logger_) + << "Failed to create context: Engine was not created" << std::endl; return false; } context_ = tensorrt_common::TrtUniquePtr(engine_->createExecutionContext()); if (!context_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create context"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; return false; } @@ -83,14 +85,14 @@ bool TensorRTWrapper::parseONNX( auto builder = tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); if (!builder) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create builder"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; return false; } auto config = tensorrt_common::TrtUniquePtr(builder->createBuilderConfig()); if (!config) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create config"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; return false; } #if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 @@ -100,12 +102,11 @@ bool TensorRTWrapper::parseONNX( #endif if (precision == "fp16") { if (builder->platformHasFastFp16()) { - RCLCPP_INFO(rclcpp::get_logger("lidar_centerpoint"), "Using TensorRT FP16 Inference"); + tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; config->setFlag(nvinfer1::BuilderFlag::kFP16); } else { - RCLCPP_INFO( - rclcpp::get_logger("lidar_centerpoint"), - "TensorRT FP16 Inference isn't supported in this environment"); + tensorrt_common::LOG_INFO(logger_) + << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; } } @@ -114,7 +115,7 @@ bool TensorRTWrapper::parseONNX( auto network = tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); if (!network) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create network"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; return false; } @@ -123,23 +124,20 @@ bool TensorRTWrapper::parseONNX( parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); if (!setProfile(*builder, *network, *config)) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to set profile"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; return false; } - RCLCPP_INFO_STREAM( - rclcpp::get_logger("lidar_centerpoint"), - "Applying optimizations and building TRT CUDA engine (" << onnx_path << ") ..."); plan_ = tensorrt_common::TrtUniquePtr( builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create serialized network"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create serialized network" << std::endl; return false; } engine_ = tensorrt_common::TrtUniquePtr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create engine"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; return false; } @@ -148,7 +146,7 @@ bool TensorRTWrapper::parseONNX( bool TensorRTWrapper::saveEngine(const std::string & engine_path) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("lidar_centerpoint"), "Writing to " << engine_path); + tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; std::ofstream file(engine_path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); return true; @@ -162,7 +160,7 @@ bool TensorRTWrapper::loadEngine(const std::string & engine_path) std::string engine_str = engine_buffer.str(); engine_ = tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( reinterpret_cast(engine_str.data()), engine_str.size())); - RCLCPP_INFO_STREAM(rclcpp::get_logger("lidar_centerpoint"), "Loaded engine from " << engine_path); + tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; return true; } diff --git a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp b/perception/tensorrt_yolo/lib/include/trt_yolo.hpp index af8065ffed379..a64c94c008526 100644 --- a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp +++ b/perception/tensorrt_yolo/lib/include/trt_yolo.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -67,22 +68,6 @@ struct Deleter template using unique_ptr = std::unique_ptr; -class Logger : public nvinfer1::ILogger -{ -public: - explicit Logger(bool verbose) : verbose_(verbose) {} - - void log(Severity severity, const char * msg) noexcept override - { - if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) { - std::cout << msg << std::endl; - } - } - -private: - bool verbose_{false}; -}; - struct Config { int num_anchors; @@ -105,7 +90,7 @@ class Net Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, const Config & yolo_config, const std::vector & calibration_images, - const std::string & calibration_table, bool verbose = false, + const std::string & calibration_table, bool verbose = true, size_t workspace_size = (1ULL << 30)); ~Net(); @@ -138,6 +123,7 @@ class Net cuda::unique_ptr out_scores_d_ = nullptr; cuda::unique_ptr out_boxes_d_ = nullptr; cuda::unique_ptr out_classes_d_ = nullptr; + tensorrt_common::Logger logger_; void load(const std::string & path); bool prepare(); diff --git a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp index 7f1d2dbbf4577..c8793aa4c8512 100644 --- a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp +++ b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp @@ -113,10 +113,9 @@ std::vector Net::preprocess( return result; } -Net::Net(const std::string & path, bool verbose) +Net::Net(const std::string & path, bool verbose) : logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); load(path); if (!prepare()) { std::cout << "Fail to prepare engine" << std::endl; @@ -136,9 +135,9 @@ Net::Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, const Config & yolo_config, const std::vector & calibration_images, const std::string & calibration_table, bool verbose, size_t workspace_size) +: logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { std::cout << "Fail to create runtime" << std::endl; return; @@ -147,7 +146,7 @@ Net::Net( bool int8 = precision.compare("INT8") == 0; // Create builder - auto builder = unique_ptr(nvinfer1::createInferBuilder(logger)); + auto builder = unique_ptr(nvinfer1::createInferBuilder(logger_)); if (!builder) { std::cout << "Fail to create builder" << std::endl; return; @@ -168,20 +167,23 @@ Net::Net( #endif // Parse ONNX FCN - std::cout << "Building " << precision << " core model..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Building " << precision << " core model..." << std::endl; const auto flag = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); auto network = unique_ptr(builder->createNetworkV2(flag)); if (!network) { - std::cout << "Fail to create network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create network" << std::endl; return; } - auto parser = unique_ptr(nvonnxparser::createParser(*network, logger)); + auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); if (!parser) { - std::cout << "Fail to create parser" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create parser" << std::endl; return; } + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); parser->parseFromFile( onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); std::vector scores, boxes, classes; @@ -250,28 +252,31 @@ Net::Net( calib = std::make_unique(stream, calibration_table); config->setInt8Calibrator(calib.get()); } else { - std::cout << "Fail to find enough images for INT8 calibration. Build FP mode..." << std::endl; + tensorrt_common::LOG_WARN(logger_) + << "Fail to find enough images for INT8 calibration. Build FP mode..." << std::endl; } } // Build engine - std::cout << "Applying optimizations and building TRT CUDA engine..." << std::endl; plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - std::cout << "Fail to create serialized network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create serialized network" << std::endl; + logger_.stop_throttle(log_thread); return; } engine_ = unique_ptr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!prepare()) { - std::cout << "Fail to create engine" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create engine" << std::endl; + logger_.stop_throttle(log_thread); return; } + logger_.stop_throttle(log_thread); } void Net::save(const std::string & path) const { - std::cout << "Writing to " << path << "..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Writing to " << path << "..." << std::endl; std::ofstream file(path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); } diff --git a/perception/tensorrt_yolo/package.xml b/perception/tensorrt_yolo/package.xml index 8a6756449b70f..12d07647093b5 100755 --- a/perception/tensorrt_yolo/package.xml +++ b/perception/tensorrt_yolo/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_components sensor_msgs + tensorrt_common tier4_perception_msgs ament_lint_auto diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml index 343531f4a00f1..e96f21b8ff01b 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index 6e32d3410c260..e7a68ea9b9e94 100644 --- a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml +++ b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml @@ -12,7 +12,7 @@ - + diff --git a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml index 714c4d288b603..ad2851ced39c6 100644 --- a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml +++ b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp index 52d411c253fe5..00c21cdcaf29e 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp +++ b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp @@ -15,6 +15,8 @@ #ifndef TRT_SSD_HPP_ #define TRT_SSD_HPP_ +#include + #include <./cuda_runtime.h> #include @@ -39,22 +41,6 @@ struct Deleter template using unique_ptr = std::unique_ptr; -class Logger : public nvinfer1::ILogger -{ -public: - explicit Logger(bool verbose) : verbose_(verbose) {} - - void log(Severity severity, const char * msg) noexcept override - { - if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) { - std::cout << msg << std::endl; - } - } - -private: - bool verbose_{false}; -}; - struct Shape { int channel, width, height; @@ -78,7 +64,7 @@ class Net // Create engine from serialized onnx model Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, - bool verbose = false, size_t workspace_size = (1ULL << 30)); + bool verbose = true, size_t workspace_size = (1ULL << 30)); ~Net(); @@ -127,6 +113,7 @@ class Net unique_ptr engine_ = nullptr; unique_ptr context_ = nullptr; cudaStream_t stream_ = nullptr; + tensorrt_common::Logger logger_; void load(const std::string & path); void prepare(); diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp index 22aa0f6bf3880..8aceb32d2ec58 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp +++ b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp @@ -52,10 +52,9 @@ void Net::prepare() cudaStreamCreate(&stream_); } -Net::Net(const std::string & path, bool verbose) +Net::Net(const std::string & path, bool verbose) : logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); load(path); prepare(); } @@ -70,9 +69,9 @@ Net::~Net() Net::Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, bool verbose, size_t workspace_size) +: logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { std::cout << "Fail to create runtime" << std::endl; return; @@ -81,7 +80,7 @@ Net::Net( bool int8 = precision.compare("INT8") == 0; // Create builder - auto builder = unique_ptr(nvinfer1::createInferBuilder(logger)); + auto builder = unique_ptr(nvinfer1::createInferBuilder(logger_)); if (!builder) { std::cout << "Fail to create builder" << std::endl; return; @@ -102,19 +101,22 @@ Net::Net( #endif // Parse ONNX FCN - std::cout << "Building " << precision << " core model..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Building " << precision << " core model..." << std::endl; const auto flag = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); auto network = unique_ptr(builder->createNetworkV2(flag)); if (!network) { - std::cout << "Fail to create network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create network" << std::endl; return; } - auto parser = unique_ptr(nvonnxparser::createParser(*network, logger)); + auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); if (!parser) { - std::cout << "Fail to create parser" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create parser" << std::endl; return; } + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); parser->parseFromFile( onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); @@ -141,28 +143,31 @@ Net::Net( config->addOptimizationProfile(profile); // Build engine - std::cout << "Applying optimizations and building TRT CUDA engine..." << std::endl; plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - std::cout << "Fail to create serialized network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create serialized network" << std::endl; + logger_.stop_throttle(log_thread); return; } engine_ = unique_ptr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { - std::cout << "Fail to create engine" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create engine" << std::endl; + logger_.stop_throttle(log_thread); return; } context_ = unique_ptr(engine_->createExecutionContext()); if (!context_) { - std::cout << "Fail to create context" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create context" << std::endl; + logger_.stop_throttle(log_thread); return; } + logger_.stop_throttle(log_thread); } void Net::save(const std::string & path) { - std::cout << "Writing to " << path << "..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Writing to " << path << "..." << std::endl; std::ofstream file(path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); } diff --git a/perception/traffic_light_ssd_fine_detector/package.xml b/perception/traffic_light_ssd_fine_detector/package.xml index 994f84bacdae4..cd44857e8fe86 100644 --- a/perception/traffic_light_ssd_fine_detector/package.xml +++ b/perception/traffic_light_ssd_fine_detector/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_components sensor_msgs + tensorrt_common tier4_debug_msgs tier4_perception_msgs From 521858470452ddeb0a61995fec58320fb88f5359 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 29 Jan 2024 09:11:30 +0900 Subject: [PATCH 136/154] fix(avoidance): fix bug in turn signal decision (#6193) Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/src/utils.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 953f3f5cb2f2f..5fe47715bff08 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -2278,11 +2278,15 @@ TurnSignalInfo calcTurnSignalInfo( return {}; } - const auto left_lane = rh->getLeftLanelet(lanelet, true, true); - const auto right_lane = rh->getRightLanelet(lanelet, true, true); - - if (!existShiftSideLane( - start_shift_length, end_shift_length, !left_lane.has_value(), !right_lane.has_value())) { + const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); + const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); + const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); + const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); + const auto has_right_lane = + right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); + + if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) { return {}; } From db637bb8f3b33b60e1d2d9648e0d563fbd149dcf Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 29 Jan 2024 09:28:25 +0900 Subject: [PATCH 137/154] chore(detected_object_validation): rework parameters (#6166) chore(detected_object_validation): use config Signed-off-by: kminoda --- .../config/obstacle_pointcloud_based_validator.param.yaml | 1 + .../config/occupancy_grid_based_validator.param.yaml | 3 +++ .../launch/obstacle_pointcloud_based_validator.launch.xml | 1 - .../launch/occupancy_grid_based_validator.launch.xml | 3 ++- 4 files changed, 6 insertions(+), 2 deletions(-) create mode 100644 perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index f7a27a52bfa8a..cddee782e8af0 100644 --- a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -13,3 +13,4 @@ [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] using_2d_validator: false + enable_debugger: false diff --git a/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml new file mode 100644 index 0000000000000..fc5c634735e23 --- /dev/null +++ b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + enable_debug: false diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 799b605658345..a1d941e66db4b 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -9,7 +9,6 @@ - diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml index 04bcbd87172b3..3acb1f2d1907a 100644 --- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -7,6 +7,7 @@ + @@ -22,6 +23,6 @@ - + From 6d6a7b06d8de8785a49937e45b778034d3501c8c Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 29 Jan 2024 09:28:49 +0900 Subject: [PATCH 138/154] feat(avoidance): make it possible to use freespace areas in avoidance module (#6001) * fix(static_drivable_area_expansion): check right/left bound id Signed-off-by: satoshi-ota * feat(static_drivable_area): use freespace area Signed-off-by: satoshi-ota * feat(avoidance): use freespace Signed-off-by: satoshi-ota * fix(AbLC): fix flag Signed-off-by: satoshi-ota * fix(planner_manager): fix flag Signed-off-by: satoshi-ota * refactor(static_drivable_area_expansion): remove unused arg Signed-off-by: satoshi-ota * refactor(static_drivable_area_expansion): use lambda Signed-off-by: satoshi-ota * fix(static_drivable_area_expansion): fix invalid access Signed-off-by: satoshi-ota * refactor(static_drivable_area_expansion): improve readability Signed-off-by: satoshi-ota * fix(avoidance): add param Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 6 +- .../config/avoidance.param.yaml | 1 + .../data_structs.hpp | 3 + .../parameter_helper.hpp | 1 + .../src/scene.cpp | 12 +- .../src/planner_manager.cpp | 5 +- .../data_manager.hpp | 1 + .../static_drivable_area.hpp | 28 +- .../static_drivable_area.cpp | 573 ++++++++++++------ 9 files changed, 417 insertions(+), 213 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 6107314bc2824..32c92b4f4b88b 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -173,11 +173,9 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( const auto shorten_lanes = utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, true)); + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true)); data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, false)); + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false)); // get related objects from dynamic_objects, and then separates them as target objects and non // target objects diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index b300de2236fcf..d9ae9ed623cb3 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -19,6 +19,7 @@ use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true + use_freespace_areas: true # for debug publish_debug_marker: false diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 9b5ae3cb4db9e..5e162a17a5064 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -115,6 +115,9 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; + // use freespace area for avoidance + bool use_freespace_areas{false}; + // consider avoidance return dead line bool enable_dead_line_for_goal{false}; bool enable_dead_line_for_traffic_light{false}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 9f2fdf7ab96d9..03902e4328eb2 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -57,6 +57,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); p.use_hatched_road_markings = getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); + p.use_freespace_areas = getOrDeclareParameter(*node, ns + "use_freespace_areas"); } // target object diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index bc1d8c6d75424..7cea678fb8b03 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -235,11 +235,13 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat auto tmp_path = getPreviousModuleOutput().path; const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, true)); + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, true)); data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, false)); + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, false)); // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { @@ -938,6 +940,8 @@ BehaviorModuleOutput AvoidanceModule::plan() // expand intersection areas current_drivable_area_info.enable_expanding_intersection_areas = parameters_->use_intersection_areas; + // expand freespace areas + current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas; output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 4caf5683f3982..c60bc53a1129e 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -226,8 +226,7 @@ void PlannerManager::generateCombinedDrivableArea( } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, - is_driving_forward); + output.path, di.drivable_lanes, false, false, false, data, is_driving_forward); } else { const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); @@ -239,7 +238,7 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, - di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data, + di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data, is_driving_forward); } diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 31dc117fbce35..28943b5724fe8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -106,6 +106,7 @@ struct DrivableAreaInfo std::vector obstacles{}; // obstacles to extract from the drivable area bool enable_expanding_hatched_road_markings{false}; bool enable_expanding_intersection_areas{false}; + bool enable_expanding_freespace_areas{false}; // temporary only for pull over's freespace planning double drivable_margin{0.0}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 9053da45708a0..9afa2608e1db6 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -19,6 +19,7 @@ #include #include +#include #include namespace behavior_path_planner::utils { @@ -40,8 +41,8 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward = true); + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward = true); void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, @@ -62,6 +63,11 @@ std::vector expandLanelets( void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles); +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left); + std::vector getBoundWithHatchedRoadMarkings( const std::vector & original_bound, const std::shared_ptr & route_handler); @@ -72,14 +78,22 @@ std::vector getBoundWithIntersectionAreas( const std::vector & drivable_lanes, const bool is_left); std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool enable_expanding_freespace_areas, const bool is_left, + const bool is_driving_forward = true); + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left); + const bool is_left, const bool is_driving_forward = true); -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left); +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left); DrivableAreaInfo combineDrivableAreaInfo( const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 4e25e9257ddfd..18627e8396239 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -154,6 +154,7 @@ std::vector extractBoundFromPolygon( const bool clockwise) { std::vector ret{}; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { ret.push_back(polygon[i]); @@ -765,54 +766,27 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward) + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward) { - // extract data - const auto transformed_lanes = utils::transformToLanelets(lanes); - const auto current_pose = planner_data->self_odometry->pose.pose; - const auto route_handler = planner_data->route_handler; - constexpr double overlap_threshold = 0.01; - if (path.points.empty()) { return; } - auto addPoints = - [](const lanelet::ConstLineString3d & points, std::vector & bound) { - for (const auto & bound_p : points) { - const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); - if (bound.empty()) { - bound.push_back(cp); - } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { - bound.push_back(cp); - } - } - }; - - const auto has_overlap = - [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { - for (const auto & transformed_lane : transformed_lanes) { - if (checkHasSameLane(ignore_lanelets, transformed_lane)) { - continue; - } - if (boost::geometry::intersects( - lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { - return true; - } - } - return false; - }; + path.left_bound.clear(); + path.right_bound.clear(); // Insert Position - auto left_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, true); - auto right_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, false); - - if (left_bound.empty() || right_bound.empty()) { + path.left_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, true, + is_driving_forward); + path.right_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, false, + is_driving_forward); + + if (path.left_bound.empty() || path.right_bound.empty()) { auto clock{rclcpp::Clock{RCL_ROS_TIME}}; RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("behavior_path_planner").get_child("utils"), clock, 1000, @@ -820,135 +794,10 @@ void generateDrivableArea( return; } - // Insert points after goal - lanelet::ConstLanelet goal_lanelet; - if ( - route_handler->getGoalLanelet(&goal_lanelet) && - checkHasSameLane(transformed_lanes, goal_lanelet)) { - const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); - const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); - const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); - const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); - lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; - if (goal_left_lanelet) { - goal_lanelets.push_back(*goal_left_lanelet); - } - if (goal_right_lanelet) { - goal_lanelets.push_back(*goal_right_lanelet); - } - - for (const auto & lane : lanes_after_goal) { - // If lane is already in the transformed lanes, ignore it - if (checkHasSameLane(transformed_lanes, lane)) { - continue; - } - // Check if overlapped - const bool is_overlapped = - (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) - : has_overlap(lane)); - if (is_overlapped) { - continue; - } - - addPoints(lane.leftBound3d(), left_bound); - addPoints(lane.rightBound3d(), right_bound); - } - } - - if (!is_driving_forward) { - std::reverse(left_bound.begin(), left_bound.end()); - std::reverse(right_bound.begin(), right_bound.end()); - } - - path.left_bound.clear(); - path.right_bound.clear(); - - const auto [left_start_idx, right_start_idx] = [&]() { - const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); - const auto cropped_path_points = motion_utils::cropPoints( - path.points, current_pose.position, current_seg_idx, - planner_data->parameters.forward_path_length, - planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); - - constexpr double front_length = 0.5; - const auto front_pose = - cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; - const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose, M_PI_2); - const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose, M_PI_2); - const auto left_start_point = - calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); - const auto right_start_point = calcLongitudinalOffsetStartPoint( - right_bound, front_pose, front_right_start_idx, -front_length); - const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); - const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); - - // Insert a start point - path.left_bound.push_back(left_start_point); - path.right_bound.push_back(right_start_point); - - return std::make_pair(left_start_idx, right_start_idx); - }(); - - // Get Closest segment for the goal point - const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; - const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose, M_PI_2); - const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose, M_PI_2); - const auto left_goal_point = - calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); - const auto right_goal_point = - calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); - const size_t left_goal_idx = std::max( - goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); - const size_t right_goal_idx = std::max( - goal_right_start_idx, - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); - - // Insert middle points - for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { - const auto & next_point = left_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); - if (dist > overlap_threshold) { - path.left_bound.push_back(next_point); - } - } - for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { - const auto & next_point = right_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); - if (dist > overlap_threshold) { - path.right_bound.push_back(next_point); - } - } - - // Insert a goal point - if ( - tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_point) > - overlap_threshold) { - path.left_bound.push_back(left_goal_point); - } - if ( - tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_point) > - overlap_threshold) { - path.right_bound.push_back(right_goal_point); - } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { drivable_area_expansion::expand_drivable_area(path, planner_data); } - - // make bound longitudinally monotonic - // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic - if ( - is_driving_forward && - (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas)) { - makeBoundLongitudinallyMonotonic(path, planner_data, true); // for left bound - makeBoundLongitudinallyMonotonic(path, planner_data, false); // for right bound - } } void generateDrivableArea( @@ -1400,15 +1249,333 @@ std::vector getBoundWithIntersectionAreas( return expanded_bound; } +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left) +{ + using lanelet::utils::to2D; + using lanelet::utils::conversion::toGeomMsgPt; + using lanelet::utils::conversion::toLaneletPoint; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto & route_handler = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + + auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr()); + if (polygons.empty()) { + return std::make_pair(original_bound, false); + } + + std::sort(polygons.begin(), polygons.end(), [&ego_pose](const auto & a, const auto & b) { + const double a_distance = boost::geometry::distance( + to2D(a).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + const double b_distance = boost::geometry::distance( + to2D(b).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + return a_distance < b_distance; + }); + + const auto & polygon = polygons.front(); + + const auto original_bound_itr = + std::find_if(original_bound.begin(), original_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto original_bound_rev_itr = + std::find_if(original_bound.rbegin(), original_bound.rend(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto other_side_bound_itr = + std::find_if(other_side_bound.begin(), other_side_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + if ( + original_bound_itr == original_bound.end() || other_side_bound_itr == other_side_bound.end()) { + return std::make_pair(original_bound, false); + } + + const auto footprint = planner_data->parameters.vehicle_info.createFootprint(); + const auto vehicle_polygon = transformVector(footprint, pose2transform(ego_pose)); + const auto is_driving_freespace = + !boost::geometry::disjoint(vehicle_polygon, to2D(polygon).basicPolygon()); + + const auto is_clockwise_polygon = boost::geometry::is_valid(to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + + const auto extract_bound_from_polygon = [&polygon, &is_clockwise_iteration]( + const auto & start_id, const auto & end_id) { + const auto start_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == start_id; }); + + const auto end_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == end_id; }); + + // extract line strings between start_idx and end_idx. + const size_t start_idx = std::distance(polygon.begin(), start_point_itr); + const size_t end_idx = std::distance(polygon.begin(), end_point_itr); + + return extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); + }; + + const auto get_bound_edge = [&ego_pose, &is_driving_freespace, &is_left]( + const auto & bound, const auto trim_behind_bound) { + if (!is_driving_freespace) { + return bound; + } + + const auto p_offset = tier4_autoware_utils::calcOffsetPose( + ego_pose, (trim_behind_bound ? -100.0 : 100.0), (is_left ? 0.1 : -0.1), 0.0); + + std::vector ret; + for (size_t i = 1; i < bound.size(); ++i) { + const auto intersect = tier4_autoware_utils::intersect( + ego_pose.position, p_offset.position, toGeomMsgPt(bound.at(i - 1)), + toGeomMsgPt(bound.at(i))); + + ret.push_back(bound.at(i - 1)); + + if (intersect.has_value()) { + ret.emplace_back( + lanelet::InvalId, intersect.value().x, intersect.value().y, intersect.value().z); + break; + } + } + + return ret; + }; + + std::vector expanded_bound; + + enum class RouteCase { + ROUTE_IS_PASS_THROUGH_FREESPACE = 0, + GOAL_POS_IS_IN_FREESPACE, + INIT_POS_IS_IN_FREESPACE, + OTHER, + }; + + const auto route_case = [&]() { + if (original_bound_itr->id() != original_bound_rev_itr->id()) { + return RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.front().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::INIT_POS_IS_IN_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.back().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::GOAL_POS_IS_IN_FREESPACE; + } + return RouteCase::OTHER; + }(); + + switch (route_case) { + case RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), original_bound_rev_itr->id()); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), polygon_bound.begin(), polygon_bound.end()); + expanded_bound.insert( + expanded_bound.end(), std::next(original_bound_rev_itr).base(), original_bound.end()); + break; + } + case RouteCase::INIT_POS_IS_IN_FREESPACE: { + auto polygon_bound = + extract_bound_from_polygon(other_side_bound_itr->id(), original_bound_itr->id()); + std::reverse(polygon_bound.begin(), polygon_bound.end()); + auto bound_edge = get_bound_edge(polygon_bound, true); + std::reverse(bound_edge.begin(), bound_edge.end()); + + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + expanded_bound.insert(expanded_bound.end(), original_bound_itr, original_bound.end()); + break; + } + case RouteCase::GOAL_POS_IS_IN_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), other_side_bound_itr->id()); + const auto bound_edge = get_bound_edge(polygon_bound, false); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + break; + } + case RouteCase::OTHER: { + expanded_bound = original_bound; + break; + } + default: + throw std::domain_error("invalid case."); + } + + const auto goal_is_in_freespace = boost::geometry::within( + to2D(toLaneletPoint(route_handler->getGoalPose().position).basicPoint()), + to2D(polygon).basicPolygon()); + + return std::make_pair(expanded_bound, is_driving_freespace || goal_is_in_freespace); +} + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool is_left, const bool is_driving_forward) +{ + const auto lanelets = utils::transformToLanelets(drivable_lanes); + const auto & current_pose = planner_data->self_odometry->pose.pose; + const auto & route_handler = planner_data->route_handler; + const auto & vehicle_length = planner_data->parameters.vehicle_length; + constexpr double overlap_threshold = 0.01; + + const auto addPoints = + [](const lanelet::ConstLineString3d & points, std::vector & bound) { + for (const auto & bound_p : points) { + const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); + if (bound.empty()) { + bound.push_back(cp); + } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + bound.push_back(cp); + } + } + }; + + const auto has_overlap = + [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { + for (const auto & transformed_lane : lanelets) { + if (checkHasSameLane(ignore_lanelets, transformed_lane)) { + continue; + } + if (boost::geometry::intersects( + lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; + + std::vector processed_bound; + std::vector tmp_bound = original_bound; + + // Insert points after goal + lanelet::ConstLanelet goal_lanelet; + if (route_handler->getGoalLanelet(&goal_lanelet) && checkHasSameLane(lanelets, goal_lanelet)) { + const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); + const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); + const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); + const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); + lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; + if (goal_left_lanelet.has_value()) { + goal_lanelets.push_back(goal_left_lanelet.value()); + } + if (goal_right_lanelet.has_value()) { + goal_lanelets.push_back(goal_right_lanelet.value()); + } + + for (const auto & lane : lanes_after_goal) { + // If lane is already in the transformed lanes, ignore it + if (checkHasSameLane(lanelets, lane)) { + continue; + } + // Check if overlapped + const bool is_overlapped = + (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) + : has_overlap(lane)); + if (is_overlapped) { + continue; + } + + if (is_left) { + addPoints(lane.leftBound3d(), tmp_bound); + } else { + addPoints(lane.rightBound3d(), tmp_bound); + } + } + } + + if (!is_driving_forward) { + std::reverse(tmp_bound.begin(), tmp_bound.end()); + } + + const auto start_idx = [&]() { + const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); + const auto cropped_path_points = motion_utils::cropPoints( + path.points, current_pose.position, current_seg_idx, + planner_data->parameters.forward_path_length, + planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); + + constexpr double front_length = 0.5; + const auto front_pose = + cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; + const size_t front_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, front_pose, M_PI_2); + const auto start_point = + calcLongitudinalOffsetStartPoint(tmp_bound, front_pose, front_start_idx, -front_length); + + // Insert a start point + processed_bound.push_back(start_point); + + return findNearestSegmentIndexFromLateralDistance(tmp_bound, start_point); + }(); + + // Get Closest segment for the goal point + const auto [goal_idx, goal_point] = [&]() { + const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; + const size_t goal_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_pose, M_PI_2); + const auto goal_point = + calcLongitudinalOffsetGoalPoint(tmp_bound, goal_pose, goal_start_idx, vehicle_length); + const size_t goal_idx = + std::max(goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_point)); + + return std::make_pair(goal_idx, goal_point); + }(); + + // Insert middle points + for (size_t i = start_idx + 1; i <= goal_idx; ++i) { + const auto & next_point = tmp_bound.at(i); + const double dist = tier4_autoware_utils::calcDistance2d(processed_bound.back(), next_point); + if (dist > overlap_threshold) { + processed_bound.push_back(next_point); + } + } + + // Insert a goal point + if ( + tier4_autoware_utils::calcDistance2d(processed_bound.back(), goal_point) > overlap_threshold) { + processed_bound.push_back(goal_point); + } + + const bool skip_monotonic_process = + !is_driving_forward || + !(enable_expanding_hatched_road_markings || enable_expanding_intersection_areas); + + return skip_monotonic_process + ? processed_bound + : makeBoundLongitudinallyMonotonic(processed_bound, path, planner_data, is_left); +} + // calculate bounds from drivable lanes and hatched road markings std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left) + const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward) { + using motion_utils::removeOverlapPoints; + + const auto & route_handler = planner_data->route_handler; + // a function to convert drivable lanes to points without duplicated points - const auto convert_to_points = [&](const std::vector & drivable_lanes) { + const auto convert_to_points = [](const auto & drivable_lanes, const auto is_left) { constexpr double overlap_threshold = 0.01; std::vector points; @@ -1435,20 +1602,35 @@ std::vector calcBound( }; // Step1. create drivable bound from drivable lanes. - std::vector bound_points = convert_to_points(drivable_lanes); + auto [bound_points, skip_post_process] = [&]() { + if (!enable_expanding_freespace_areas) { + return std::make_pair(convert_to_points(drivable_lanes, is_left), false); + } + return getBoundWithFreeSpaceAreas( + convert_to_points(drivable_lanes, is_left), convert_to_points(drivable_lanes, !is_left), + planner_data, is_left); + }(); + + const auto post_process = [&](const auto & bound, const auto skip) { + return skip + ? bound + : postProcess( + bound, path, planner_data, drivable_lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, is_left, is_driving_forward); + }; // Step2. if there is no drivable area defined by polygon, return original drivable bound. if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } - // Step3. if there are hatched road markings, expand drivable bound with the polygon. + // Step3.if there are hatched road markings, expand drivable bound with the polygon. if (enable_expanding_hatched_road_markings) { bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler); } if (!enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } // Step4. if there are intersection areas, expand drivable bound with the polygon. @@ -1457,12 +1639,12 @@ std::vector calcBound( getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); } - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left) +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left) { using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcAzimuthAngle; @@ -1508,7 +1690,7 @@ void makeBoundLongitudinallyMonotonic( const auto get_bound_with_pose = [&](const auto & bound_with_pose, const auto & path_points) { auto ret = bound_with_pose; - const double offset = is_bound_left ? 100.0 : -100.0; + const double offset = is_left ? 100.0 : -100.0; size_t start_bound_idx = 0; @@ -1588,14 +1770,14 @@ void makeBoundLongitudinallyMonotonic( // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is // opposite. - const double lat_offset = is_bound_left ? 100.0 : -100.0; + const double lat_offset = is_left ? 100.0 : -100.0; const auto p_bound_1 = getPose(bound_with_pose.at(bound_idx)); const auto p_bound_2 = getPose(bound_with_pose.at(bound_idx + 1)); const auto p_bound_offset = calcOffsetPose(p_bound_1, 0.0, lat_offset, 0.0); - if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_bound_left, is_reverse)) { + if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_left, is_reverse)) { bound_idx++; continue; } @@ -1681,18 +1863,16 @@ void makeBoundLongitudinallyMonotonic( return ret; }; - const auto original_bound = is_bound_left ? path.left_bound : path.right_bound; - if (path.points.empty()) { - return; + return original_bound; } - if (original_bound.empty()) { - return; + if (original_bound.size() < 2) { + return original_bound; } if (path.points.front().lane_ids.empty()) { - return; + return original_bound; } // step.1 create bound with pose vector. @@ -1734,14 +1914,14 @@ void makeBoundLongitudinallyMonotonic( p.forward_path_length, p); if (centerline_path.points.size() < 2) { - return; + return original_bound; } const auto ego_idx = planner_data->findEgoIndex(centerline_path.points); const auto end_idx = findNearestSegmentIndex(centerline_path.points, original_bound.back()); if (ego_idx >= end_idx) { - return; + return original_bound; } clipped_points.insert( @@ -1750,7 +1930,7 @@ void makeBoundLongitudinallyMonotonic( } if (clipped_points.empty()) { - return; + return original_bound; } // step.3 update bound pose by reference path pose. @@ -1771,11 +1951,7 @@ void makeBoundLongitudinallyMonotonic( auto full_monotonic_bound = remove_orientation(full_monotonic_bound_with_pose); // step.7 remove sharp bound points. - if (is_bound_left) { - path.left_bound = remove_sharp_points(full_monotonic_bound); - } else { - path.right_bound = remove_sharp_points(full_monotonic_bound); - } + return remove_sharp_points(full_monotonic_bound); } std::vector combineDrivableLanes( @@ -1785,7 +1961,9 @@ std::vector combineDrivableLanes( const auto has_same_lane = [](const lanelet::ConstLanelet & target_lane, const lanelet::ConstLanelets & lanes) { return std::find_if(lanes.begin(), lanes.end(), [&](const auto & ll) { - return ll.id() == target_lane.id(); + return ll.id() == target_lane.id() && + ll.rightBound3d().id() == target_lane.rightBound3d().id() && + ll.leftBound3d().id() == target_lane.leftBound3d().id(); }) != lanes.end(); }; @@ -1884,6 +2062,11 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_intersection_areas || drivable_area_info2.enable_expanding_intersection_areas; + // enable expanding freespace areas + combined_drivable_area_info.enable_expanding_freespace_areas = + drivable_area_info1.enable_expanding_freespace_areas || + drivable_area_info2.enable_expanding_freespace_areas; + // drivable margin combined_drivable_area_info.drivable_margin = std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin); From 5b918742a3994d9e97c2dfb3264c62461bdc70b5 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 29 Jan 2024 09:30:04 +0900 Subject: [PATCH 139/154] chore(object_range_splitter): rework parameters (#6159) * chore(object_range_splitter): add param file Signed-off-by: kminoda * fix arg name Signed-off-by: kminoda * feat: use param file from autoware.launch Signed-off-by: kminoda --------- Signed-off-by: kminoda --- .../launch/object_recognition/detection/detection.launch.xml | 4 +++- .../detection/detector/radar_detector.launch.xml | 4 ++-- launch/tier4_perception_launch/launch/perception.launch.xml | 2 ++ perception/object_range_splitter/CMakeLists.txt | 1 + .../config/object_range_splitter.param.yaml | 3 +++ .../launch/object_range_splitter.launch.xml | 4 ++-- 6 files changed, 13 insertions(+), 5 deletions(-) create mode 100644 perception/object_range_splitter/config/object_range_splitter.param.yaml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ab9ed65999048..78c6c585d90f7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -78,7 +78,7 @@ - + @@ -177,6 +177,7 @@ + @@ -236,6 +237,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml index 26e7af07646cd..0ef0201d973e1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml @@ -5,7 +5,7 @@ - + @@ -29,7 +29,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 528038c5158b2..3bd3aff10a4e4 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -16,6 +16,8 @@ + + diff --git a/perception/object_range_splitter/CMakeLists.txt b/perception/object_range_splitter/CMakeLists.txt index 7b27a344a5e0d..92c6c0668a085 100644 --- a/perception/object_range_splitter/CMakeLists.txt +++ b/perception/object_range_splitter/CMakeLists.txt @@ -16,4 +16,5 @@ rclcpp_components_register_node(object_range_splitter ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/object_range_splitter/config/object_range_splitter.param.yaml b/perception/object_range_splitter/config/object_range_splitter.param.yaml new file mode 100644 index 0000000000000..45bde990029f3 --- /dev/null +++ b/perception/object_range_splitter/config/object_range_splitter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + split_range: 30.0 diff --git a/perception/object_range_splitter/launch/object_range_splitter.launch.xml b/perception/object_range_splitter/launch/object_range_splitter.launch.xml index 6588c3e71ef73..3f2f3c6ba24c6 100644 --- a/perception/object_range_splitter/launch/object_range_splitter.launch.xml +++ b/perception/object_range_splitter/launch/object_range_splitter.launch.xml @@ -3,12 +3,12 @@ - + - + From f514bcaa06d2aef6e7d02b49262a3728e90b5b31 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 29 Jan 2024 10:04:53 +0900 Subject: [PATCH 140/154] feat(intersection): publish and visualize the reason for dangerous situation to blame past detection fault retrospectively (#6143) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 2 + .../config/intersection.param.yaml | 2 + .../package.xml | 3 +- .../src/debug.cpp | 122 ++- .../src/decision_result.cpp | 65 ++ .../src/decision_result.hpp | 67 +- .../src/interpolated_path_info.hpp | 1 - .../src/intersection_lanelets.hpp | 2 - .../src/intersection_stoplines.hpp | 5 +- .../src/manager.cpp | 5 + .../src/object_manager.cpp | 309 ++++++++ .../src/object_manager.hpp | 294 +++++++ .../src/result.hpp | 17 +- .../src/scene_intersection.cpp | 559 ++++++------- .../src/scene_intersection.hpp | 233 +++--- .../src/scene_intersection_collision.cpp | 746 ++++++++++++++++-- .../src/scene_intersection_occlusion.cpp | 49 +- .../src/scene_intersection_prepare_data.cpp | 95 ++- .../src/scene_intersection_stuck.cpp | 26 +- .../src/util.hpp | 14 +- 20 files changed, 1928 insertions(+), 688 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/src/decision_result.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/object_manager.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/object_manager.hpp diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 4c8fe5c6fa0f5..07847a08c1209 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -12,6 +12,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp src/scene_intersection.cpp src/intersection_lanelets.cpp + src/object_manager.cpp + src/decision_result.cpp src/scene_intersection_prepare_data.cpp src/scene_intersection_stuck.cpp src/scene_intersection_occlusion.cpp diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 108e021240851..c7f6f62d575a0 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -59,6 +59,8 @@ object_expected_deceleration: 2.0 ignore_on_red_traffic_light: object_margin_to_path: 2.0 + avoid_collision_by_acceleration: + object_time_margin_to_collision_point: 4.0 occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0c9b3407d0f38..0bed7d32f412f 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -22,10 +22,12 @@ autoware_auto_planning_msgs autoware_perception_msgs behavior_velocity_planner_common + fmt geometry_msgs interpolation lanelet2_extension libopencv-dev + magic_enum motion_utils nav_msgs pluginlib @@ -33,7 +35,6 @@ route_handler rtc_interface tf2_geometry_msgs - tier4_api_msgs tier4_autoware_utils tier4_planning_msgs vehicle_info_util diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 350f6d774f7cf..978855b36c5f6 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -31,8 +31,6 @@ #include #include -namespace behavior_velocity_planner -{ namespace { using tier4_autoware_utils::appendMarkerArray; @@ -40,14 +38,14 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; -static visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( +visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, const int64_t lane_id, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = planning_utils::bitShift(lane_id); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -158,8 +156,59 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray( return marker_point; } +constexpr std::tuple white() +{ + constexpr uint64_t code = 0xfdfdfd; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple green() +{ + constexpr uint64_t code = 0x5fa641; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple yellow() +{ + constexpr uint64_t code = 0xebce2b; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple red() +{ + constexpr uint64_t code = 0xba1c30; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple light_blue() +{ + constexpr uint64_t code = 0x96cde6; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} } // namespace +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; + visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; @@ -168,14 +217,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.attention_area.value(), "attention_area", lane_id_, 0.0, 1.0, 0.0), &debug_marker_array); } if (debug_data_.occlusion_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, 0.568, 0.596), &debug_marker_array); @@ -183,14 +232,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.adjacent_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.adjacent_area.value(), "adjacent_area", lane_id_, 0.913, 0.639, 0.149), &debug_marker_array); } if (debug_data_.first_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -198,7 +247,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.second_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -214,7 +263,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.yield_stuck_detect_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, 0.34509, 0.6588235), &debug_marker_array); @@ -222,7 +271,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.ego_lane) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.ego_lane.value()}, "ego_lane", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); } @@ -235,59 +284,58 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } - size_t i{0}; - for (const auto & p : debug_data_.candidate_collision_object_polygons) { - appendMarkerArray( - debug::createPolygonMarkerArray( - p, "candidate_collision_object_polygons", lane_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, - 0.5), - &debug_marker_array, now); - } - + static constexpr auto white = ::white(); + static constexpr auto green = ::green(); + static constexpr auto yellow = ::yellow(); + static constexpr auto red = ::red(); + static constexpr auto light_blue = ::light_blue(); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), + debug_data_.safe_under_traffic_control_targets, "safe_under_traffic_control_targets", + module_id_, now, std::get<0>(light_blue), std::get<1>(light_blue), std::get<2>(light_blue)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + debug_data_.unsafe_targets, "unsafe_targets", module_id_, now, std::get<0>(green), + std::get<1>(green), std::get<2>(green)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, - 0.0, 1.0, 0.0), + debug_data_.misjudge_targets, "misjudge_targets", module_id_, now, std::get<0>(yellow), + std::get<1>(yellow), std::get<2>(yellow)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), + debug_data_.too_late_detect_targets, "too_late_detect_targets", module_id_, now, + std::get<0>(red), std::get<1>(red), std::get<2>(red)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2), + debug_data_.parked_targets, "parked_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, - 0.99, 0.6), + debug_data_.stuck_targets, "stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - /* appendMarkerArray( - createPoseMarkerArray( - debug_data_.predicted_obj_pose, "predicted_obj_pose", module_id_, 0.7, 0.85, 0.9), + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "yield_stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - */ if (debug_data_.first_pass_judge_wall_pose) { const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -297,7 +345,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -314,7 +362,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.nearest_occlusion_projection) { const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); appendMarkerArray( - createLineMarkerArray( + ::createLineMarkerArray( point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), &debug_marker_array, now); } @@ -369,11 +417,11 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); - int32_t uid = planning_utils::bitShift(module_id_); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( - createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), + ::createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp new file mode 100644 index 0000000000000..93a7c2a29d2f7 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -0,0 +1,65 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 "decision_result.hpp" + +namespace behavior_velocity_planner::intersection +{ +std::string formatDecisionResult(const DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "InternalError because " + state.error; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + + state.evasive_report; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "YieldStuckStop:\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "OccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "FullyPrioritized\nsafety_report:" + state.safety_report; + } + return ""; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp index 48b0ecf1349a5..da71168c2c4ca 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -23,16 +23,23 @@ namespace behavior_velocity_planner::intersection { /** - * @struct - * @brief Internal error or ego already passed pass_judge_line + * @brief internal error */ -struct Indecisive +struct InternalError { std::string error; }; /** - * @struct + * @brief + */ +struct OverPassJudge +{ + std::string safety_report; + std::string evasive_report; +}; + +/** * @brief detected stuck vehicle */ struct StuckStop @@ -43,17 +50,16 @@ struct StuckStop }; /** - * @struct * @brief yielded by vehicle on the attention area */ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief only collision is detected */ struct NonOccludedCollisionStop @@ -61,10 +67,10 @@ struct NonOccludedCollisionStop size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief occlusion is detected so ego needs to stop at the default stop line position */ struct FirstWaitBeforeOcclusion @@ -76,7 +82,6 @@ struct FirstWaitBeforeOcclusion }; /** - * @struct * @brief ego is approaching the boundary of attention area in the presence of traffic light */ struct PeekingTowardOcclusion @@ -96,7 +101,6 @@ struct PeekingTowardOcclusion }; /** - * @struct * @brief both collision and occlusion are detected in the presence of traffic light */ struct OccludedCollisionStop @@ -110,10 +114,10 @@ struct OccludedCollisionStop //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; + std::string safety_report; }; /** - * @struct * @brief at least occlusion is detected in the absence of traffic light */ struct OccludedAbsenceTrafficLight @@ -124,10 +128,10 @@ struct OccludedAbsenceTrafficLight size_t closest_idx{0}; size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; + std::string safety_report; }; /** - * @struct * @brief both collision and occlusion are not detected */ struct Safe @@ -138,7 +142,6 @@ struct Safe }; /** - * @struct * @brief traffic light is red or arrow signal */ struct FullyPrioritized @@ -147,10 +150,12 @@ struct FullyPrioritized size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; using DecisionResult = std::variant< - Indecisive, //! internal process error, or over the pass judge line + InternalError, //! internal process error, or over the pass judge line + OverPassJudge, //! over the pass judge lines StuckStop, //! detected stuck vehicle YieldStuckStop, //! detected yield stuck vehicle NonOccludedCollisionStop, //! detected collision while FOV is clear @@ -162,41 +167,7 @@ using DecisionResult = std::variant< FullyPrioritized //! only detect vehicles violating traffic rules >; -inline std::string formatDecisionResult(const DecisionResult & decision_result) -{ - if (std::holds_alternative(decision_result)) { - const auto indecisive = std::get(decision_result); - return "Indecisive because " + indecisive.error; - } - if (std::holds_alternative(decision_result)) { - return "StuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "YieldStuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "NonOccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "FirstWaitBeforeOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "PeekingTowardOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedAbsenceTrafficLight"; - } - if (std::holds_alternative(decision_result)) { - return "Safe"; - } - if (std::holds_alternative(decision_result)) { - return "FullyPrioritized"; - } - return ""; -} +std::string formatDecisionResult(const DecisionResult & decision_result); } // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index c47f016fbdbda..9002c88354d68 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -27,7 +27,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief wrapper class of interpolated path with lane id */ struct InterpolatedPathInfo diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 11deae6bdc001..9624d375de122 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -31,7 +31,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionLanelets */ struct IntersectionLanelets @@ -174,7 +173,6 @@ struct IntersectionLanelets }; /** - * @struct * @brief see the document for more details of PathLanelets */ struct PathLanelets diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp index 64d20b81e3fad..99d79d4468b38 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -21,7 +21,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionStopLines */ struct IntersectionStopLines @@ -63,9 +62,9 @@ struct IntersectionStopLines /** * second_pass_judge_line is before second_attention_stopline by the braking distance. if - * second_attention_lane is null, it is same as first_pass_judge_line + * second_attention_lane is null, it is null */ - size_t second_pass_judge_line{0}; + std::optional second_pass_judge_line{std::nullopt}; /** * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 615991bc5e027..8ab67c810a30e 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -133,6 +133,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = getOrDeclareParameter( node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); + ip.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point = getOrDeclareParameter( + node, + ns + + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp new file mode 100644 index 0000000000000..ea5d89fe8052d --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -0,0 +1,309 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 "object_manager.hpp" + +#include +#include +#include // for toPolygon2d + +#include +#include +#include + +#include +#include + +#include + +namespace +{ +std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; + } + return ss.str(); +} + +tier4_autoware_utils::Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + namespace bg = boost::geometry; + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + tier4_autoware_utils::Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + tier4_autoware_utils::Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} + +} // namespace + +namespace behavior_velocity_planner::intersection +{ +namespace bg = boost::geometry; + +ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_str(::to_string(uuid)) +{ +} + +void ObjectInfo::initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + std::optional attention_lanelet_opt_, + std::optional stopline_opt_) +{ + predicted_object_ = object; + attention_lanelet_opt = attention_lanelet_opt_; + stopline_opt = stopline_opt_; + unsafe_interval_ = std::nullopt; + calc_dist_to_stopline(); +} + +void ObjectInfo::update_safety( + const std::optional & unsafe_interval, + const std::optional & safe_interval, const bool safe_under_traffic_control) +{ + unsafe_interval_ = unsafe_interval; + safe_interval_ = safe_interval; + safe_under_traffic_control_ = safe_under_traffic_control; +} + +std::optional ObjectInfo::estimated_past_position( + const double past_duration) const +{ + if (!attention_lanelet_opt) { + return std::nullopt; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto current_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto distance = current_arc_coords.distance; + const auto past_length = + current_arc_coords.length - + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x * past_duration; + const auto past_point = lanelet::geometry::fromArcCoordinates( + attention_lanelet.centerline2d(), lanelet::ArcCoordinates{past_length, distance}); + geometry_msgs::msg::Point past_position; + past_position.x = past_point.x(); + past_position.y = past_point.y(); + return std::make_optional(past_position); +} + +void ObjectInfo::calc_dist_to_stopline() +{ + if (!stopline_opt || !attention_lanelet_opt) { + return; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto stopline = stopline_opt.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline.front().x() + stopline.back().x()) / 2.0; + stopline_center.position.y = (stopline.front().y() + stopline.back().y()) / 2.0; + stopline_center.position.z = (stopline.front().z() + stopline.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet}, stopline_center); + dist_to_stopline_opt = (stopline_arc_coords.length - object_arc_coords.length); +} + +bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + return dist_to_stopline > braking_distance; +} + +bool ObjectInfo::can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const +{ + if (!dist_to_stopline_opt || !stopline_opt || !attention_lanelet_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + if (dist_to_stopline > braking_distance) { + return false; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto stopline = stopline_opt.value(); + const auto stopline_p1 = stopline.front(); + const auto stopline_p2 = stopline.back(); + const tier4_autoware_utils::Point2d stopline_mid{ + stopline_p1.x() + stopline_p2.x() / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; + const auto attention_lane_end = attention_lanelet.centerline().back(); + const tier4_autoware_utils::LineString2d attention_lane_later_part( + {tier4_autoware_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, + tier4_autoware_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); + std::vector ego_collision_points; + bg::intersection( + attention_lane_later_part, ego_lane.centerline2d().basicLineString(), ego_collision_points); + if (ego_collision_points.empty()) { + return false; + } + const auto expected_collision_point = ego_collision_points.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; + const double stopline_to_ego_path = std::hypot( + expected_collision_point.x() - stopline_mid.x(), + expected_collision_point.y() - stopline_mid.y()); + const double object_to_ego_path = stopline_to_ego_path - stopline_to_object; + // NOTE: if object_to_ego_path < 0, object passed ego path + return object_to_ego_path > tolerable_overshoot; +} + +bool ObjectInfo::before_stopline_by(const double margin) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + return dist_to_stopline < margin; +} + +std::shared_ptr ObjectInfoManager::registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle) +{ + if (objects_info_.count(uuid) == 0) { + auto object = std::make_shared(uuid); + objects_info_[uuid] = object; + } + auto object = objects_info_[uuid]; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } + return object; +} + +void ObjectInfoManager::registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle, + std::shared_ptr object) +{ + objects_info_[uuid] = object; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } +} + +void ObjectInfoManager::clearObjects() +{ + objects_info_.clear(); + attention_area_objects_.clear(); + intersection_area_objects_.clear(); + parked_objects_.clear(); +}; + +std::vector> ObjectInfoManager::allObjects() +{ + std::vector> all_objects = attention_area_objects_; + all_objects.insert( + all_objects.end(), intersection_area_objects_.begin(), intersection_area_objects_.end()); + all_objects.insert(all_objects.end(), parked_objects_.begin(), parked_objects_.end()); + return all_objects; +} + +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt) +{ + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (first_itr == predicted_path.path.cend()) { + // even the predicted path end does not collide with the beginning of ego_lane_poly + return std::nullopt; + } + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (last_itr == predicted_path.path.crend()) { + // even the predicted path start does not collide with the end of ego_lane_poly + return std::nullopt; + } + + const size_t enter_idx = static_cast(first_itr - predicted_path.path.begin()); + const double object_enter_time = + static_cast(enter_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const size_t exit_idx = std::distance(predicted_path.path.begin(), last_itr.base()) - 1; + const double object_exit_time = + static_cast(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const auto lane_position = [&]() { + if (first_attention_lane_opt) { + if (lanelet::geometry::inside( + first_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::FIRST; + } + } + if (second_attention_lane_opt) { + if (lanelet::geometry::inside( + second_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::SECOND; + } + } + return intersection::CollisionInterval::LanePosition::ELSE; + }(); + + std::vector path; + for (const auto & pose : predicted_path.path) { + path.push_back(pose); + } + return intersection::CollisionInterval{ + lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp new file mode 100644 index 0000000000000..e77849570cda8 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -0,0 +1,294 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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 OBJECT_MANAGER_HPP_ +#define OBJECT_MANAGER_HPP_ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace std +{ +template <> +struct hash +{ + size_t operator()(const unique_identifier_msgs::msg::UUID & uid) const + { + const auto & ids = uid.uuid; + boost::uuids::uuid u = {ids[0], ids[1], ids[2], ids[3], ids[4], ids[5], ids[6], ids[7], + ids[8], ids[9], ids[10], ids[11], ids[12], ids[13], ids[14], ids[15]}; + return boost::hash()(u); + } +}; +} // namespace std + +namespace behavior_velocity_planner::intersection +{ + +/** + * @brief store collision information + */ +struct CollisionInterval +{ + enum LanePosition { + FIRST, + SECOND, + ELSE, + }; + LanePosition lane_position{LanePosition::ELSE}; + + //! original predicted path + std::vector path; + + //! possible collision interval position index on path + std::pair interval_position; + + //! possible collision interval time(without TTC margin) + std::pair interval_time; +}; + +struct CollisionKnowledge +{ + //! the time when the expected collision is judged + rclcpp::Time stamp; + + enum SafeType { + UNSAFE, + SAFE, + SAFE_UNDER_TRAFFIC_CONTROL, + }; + SafeType safe_type{SafeType::UNSAFE}; + + //! if !safe, this has value, and it safe, this maybe null if the predicted path does not + //! intersect with ego path + std::optional interval{std::nullopt}; + + double observed_velocity; +}; + +/** + * @brief store collision information of object on the attention area + */ +class ObjectInfo +{ +public: + explicit ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid); + + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object() const + { + return predicted_object_; + }; + + std::optional is_unsafe() const + { + if (safe_under_traffic_control_) { + return std::nullopt; + } + if (!unsafe_interval_) { + return std::nullopt; + } + return unsafe_interval_; + } + + bool is_safe_under_traffic_control() const { return safe_under_traffic_control_; } + + /** + * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline + */ + void initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + std::optional attention_lanelet_opt, + std::optional stopline_opt); + + /** + * @brief update unsafe_knowledge + */ + void update_safety( + const std::optional & unsafe_interval_opt, + const std::optional & safe_interval_opt, + const bool safe_under_traffic_control); + + /** + * @brief find the estimated position of the object in the past + */ + std::optional estimated_past_position( + const double past_duration) const; + + /** + * @brief check if object can stop before stopline under the deceleration. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_stopline(const double brake_deceleration) const; + + /** + * @brief check if object can stop before stopline within the overshoot margin. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const; + + /** + * @brief check if the object is before the stopline within the specified margin + */ + bool before_stopline_by(const double margin) const; + + void setDecisionAt1stPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_1st_pass_judge_line_passage_ = knowledge; + } + + void setDecisionAt2ndPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_2nd_pass_judge_line_passage_ = knowledge; + } + + const std::optional & unsafe_interval() const { return unsafe_interval_; } + + double observed_velocity() const + { + return predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + } + + const std::optional & decision_at_1st_pass_judge_line_passage() const + { + return decision_at_1st_pass_judge_line_passage_; + } + + const std::optional & decision_at_2nd_pass_judge_line_passage() const + { + return decision_at_2nd_pass_judge_line_passage_; + } + + const std::string uuid_str; + +private: + autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; + + //! null if the object in intersection_area but not in attention_area + std::optional attention_lanelet_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional stopline_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional dist_to_stopline_opt{std::nullopt}; + + //! store the information if judged as UNSAFE + std::optional unsafe_interval_{std::nullopt}; + + //! store the information if judged as SAFE + std::optional safe_interval_{std::nullopt}; + + //! true if the object is judged as negligible given traffic light color + bool safe_under_traffic_control_{false}; + + std::optional decision_at_1st_pass_judge_line_passage_{std::nullopt}; + std::optional decision_at_2nd_pass_judge_line_passage_{std::nullopt}; + + /** + * @brief calculate/update the distance to corresponding stopline + */ + void calc_dist_to_stopline(); +}; + +/** + * @brief store predicted objects for intersection + */ +class ObjectInfoManager +{ +public: + std::shared_ptr registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked); + + void registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked, + std::shared_ptr object); + + void clearObjects(); + + const std::vector> & attentionObjects() const + { + return attention_area_objects_; + } + + const std::vector> & parkedObjects() const { return parked_objects_; } + + std::vector> allObjects(); + + const std::unordered_map> & + getObjectsMap() + { + return objects_info_; + } + + void setPassed1stPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_1st_judge_line_first_time_ = time; + } + void setPassed2ndPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_2nd_judge_line_first_time_ = time; + } + +private: + std::unordered_map> objects_info_; + + //! belong to attention area + std::vector> attention_area_objects_; + + //! does not belong to attention area but to intersection area + std::vector> intersection_area_objects_; + + //! parked objects on attention_area/intersection_area + std::vector> parked_objects_; + + std::optional passed_1st_judge_line_first_time_{std::nullopt}; + std::optional passed_2nd_judge_line_first_time_{std::nullopt}; +}; + +/** + * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically + */ +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt); + +} // namespace behavior_velocity_planner::intersection + +#endif // OBJECT_MANAGER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/behavior_velocity_intersection_module/src/result.hpp index cc6ad880b8153..5d82183cee2fb 100644 --- a/planning/behavior_velocity_intersection_module/src/result.hpp +++ b/planning/behavior_velocity_intersection_module/src/result.hpp @@ -15,6 +15,7 @@ #ifndef RESULT_HPP_ #define RESULT_HPP_ +#include #include namespace behavior_velocity_planner::intersection @@ -23,10 +24,6 @@ namespace behavior_velocity_planner::intersection template class Result { -public: - static Result make_ok(const Ok & ok) { return Result(ok); } - static Result make_err(const Error & err) { return Result(err); } - public: explicit Result(const Ok & ok) : data_(ok) {} explicit Result(const Error & err) : data_(err) {} @@ -39,6 +36,18 @@ class Result std::variant data_; }; +template +Result make_ok(Args &&... args) +{ + return Result(Ok{std::forward(args)...}); +} + +template +Result make_err(Args &&... args) +{ + return Result(Error{std::forward(args)...}); +} + } // namespace behavior_velocity_planner::intersection #endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 90242dc3edd7e..75e2da034780a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -40,6 +40,10 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, @@ -95,10 +99,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_ = DebugData(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); - // set default RTC initializeRTCStatus(); - // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); prev_decision_result_ = decision_result; @@ -112,7 +114,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * reactRTCApproval(decision_result, path, stop_reason); - RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; } @@ -141,133 +142,145 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); const auto & intersection_lanelets = intersection_lanelets_.value(); - const auto closest_idx = intersection_stoplines.closest_idx; - - // utility functions - auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path->points, closest_idx, index); - }; - auto stoppedForDuration = - [&](const size_t pos, const double duration, StateMachine & state_machine) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < 0.0); - const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { - state_machine.setState(StateMachine::State::GO); - } - return state_machine.getState() == StateMachine::State::GO; - }; - auto stoppedAtPosition = [&](const size_t pos, const double duration) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; - } - return false; - }; - + // ========================================================================================== + // stuck detection + // // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation + // ========================================================================================== const auto is_stuck_status = isStuckStatus(*path, intersection_stoplines, path_lanelets); if (is_stuck_status) { return is_stuck_status.value(); } + // ========================================================================================== + // basic data validation + // // if attention area is empty, collision/occlusion detection is impossible + // + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line, so ego is already in the middle of the intersection, or the + // end of the ego path has just entered the entry of this intersection + // + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, ego has already passed the intersection, or the end of the + // ego path has just entered the entry of this intersection + // ========================================================================================== if (!intersection_lanelets.first_attention_area()) { - return intersection::Indecisive{"attention area is empty"}; + return intersection::InternalError{"attention area is empty"}; } const auto first_attention_area = intersection_lanelets.first_attention_area().value(); - // if attention area is not null but default stop line is not available, ego/backward-path has - // already passed the stop line const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; if (!default_stopline_idx_opt) { - return intersection::Indecisive{"default stop line is null"}; + return intersection::InternalError{"default stop line is null"}; } const auto default_stopline_idx = default_stopline_idx_opt.value(); - // occlusion stop line is generated from the intersection of ego footprint along the path with the - // attention area, so if this is null, eog has already passed the intersection const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return intersection::Indecisive{"occlusion stop line is null"}; + return intersection::InternalError{"occlusion stop line is null"}; } const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); - debug_data_.attention_area = intersection_lanelets.attention_area(); - debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); - debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); - debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + // ========================================================================================== + // classify the objects to attention_area/intersection_area and update their position, velocity, + // belonging attention lanelet, distance to corresponding stopline + // ========================================================================================== + updateObjectInfoManagerArea(); + + // ========================================================================================== + // occlusion_status is type of occlusion, + // is_occlusion_cleared_with_margin indicates if occlusion is physically detected + // is_occlusion_state indicates if occlusion is detected. OR occlusion is not detected but RTC for + // intersection_occlusion is disapproved, which means ego is virtually occluded + // + // so is_occlusion_cleared_with_margin should be sent to RTC as module decision + // and is_occlusion_status should be only used to decide ego action + // !is_occlusion_state == !physically_occluded && !externally_occluded, so even if occlusion is + // not detected but not approved, SAFE is not sent. + // ========================================================================================== + const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = + getOcclusionStatus(traffic_prioritized_level, interpolated_path_info); - // get intersection area - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - // filter objects - auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const auto is_yield_stuck_status = - isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines, target_objects); - if (is_yield_stuck_status) { - return is_yield_stuck_status.value(); + const auto + [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line] = + isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); + + // ========================================================================================== + // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the + // exit of intersection + // ========================================================================================== + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = + calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array); + + // ========================================================================================== + // run collision checking for each objects considering traffic light level. Also if ego just + // passed each pass judge line for the first time, save current collision status for late + // diagnosis + // ========================================================================================== + updateObjectInfoManagerCollision( + path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line); + + const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] = + detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); + const std::string safety_diag = + generateDetectionBlameDiagnosis(too_late_detect_objects, misjudge_objects); + if (is_permanent_go_) { + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{safety_diag, evasive_diag}; + } + return intersection::OverPassJudge{ + "no collision is detected", "ego can safely pass the intersection at this rate"}; } - const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = - getOcclusionStatus( - traffic_prioritized_level, interpolated_path_info, intersection_lanelets, target_objects); - - // TODO(Mamoru Sobue): this should be called later for safety diagnosis - const auto is_over_pass_judge_lines_status = - isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); - if (is_over_pass_judge_lines_status) { - return is_over_pass_judge_lines_status.ok(); + const bool collision_on_1st_attention_lane = + has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + // ========================================================================================== + // this state is very dangerous because ego is very close/over the boundary of 1st attention lane + // and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this + // case, possible another collision may be expected on the 2nd attention lane too. + // ========================================================================================== + std::string safety_report = safety_diag; + if ( + is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line && + is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { + safety_report += + "\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st " + "attention lane, which is dangerous."; } - [[maybe_unused]] const auto [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line] = - is_over_pass_judge_lines_status.err(); - const auto & current_pose = planner_data_->current_odometry->pose; - const bool is_over_default_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); + const auto closest_idx = intersection_stoplines.closest_idx; + const bool is_over_default_stopline = util::isOverTargetIndex( + *path, closest_idx, planner_data_->current_odometry->pose, default_stopline_idx); const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; - const auto is_green_pseudo_collision_status = isGreenPseudoCollisionStatus( - *path, collision_stopline_idx, intersection_stoplines, target_objects); + // ========================================================================================== + // pseudo collision detection on green light + // ========================================================================================== + const auto is_green_pseudo_collision_status = + isGreenPseudoCollisionStatus(closest_idx, collision_stopline_idx, intersection_stoplines); if (is_green_pseudo_collision_status) { return is_green_pseudo_collision_status.value(); } - // if ego is waiting for collision detection, the entry time into the intersection is a bit - // delayed for the chattering hold - const bool is_go_out = (activated_ && occlusion_activated_); - const double time_to_restart = - (is_go_out || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.collision_detection_hold_time - - collision_state_machine_.getDuration()); - - // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd - // pass judge line respectively, ego should go - const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; - const auto last_intersection_stopline_candidate_idx = - second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = calcIntersectionPassingTime( - *path, closest_idx, last_intersection_stopline_candidate_idx, time_to_restart, - &ego_ttc_time_array); + // ========================================================================================== + // yield stuck detection + // ========================================================================================== + const auto is_yield_stuck_status = + isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines); + if (is_yield_stuck_status) { + auto yield_stuck = is_yield_stuck_status.value(); + yield_stuck.safety_report = safety_report; + return yield_stuck; + } - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, - time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -276,7 +289,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (is_prioritized) { return intersection::FullyPrioritized{ - has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx, + safety_report}; } // Safe @@ -286,18 +300,51 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // Only collision if (!is_occlusion_state && has_collision_with_margin) { return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; } // Occluded - // occlusion_status is assured to be not NOT_OCCLUDED + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; + const auto occlusion_wo_tl_pass_judge_line_idx = intersection_stoplines.occlusion_wo_tl_pass_judge_line; const bool stopped_at_default_line = stoppedForDuration( default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, before_creep_state_machine_); if (stopped_at_default_line) { + // ========================================================================================== // if specified the parameter occlusion.temporal_stop_before_attention_area OR // has_no_traffic_light_, ego will temporarily stop before entering attention area + // ========================================================================================== const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( @@ -307,7 +354,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( : false; if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return intersection::Indecisive{ + return intersection::InternalError{ "already passed maximum peeking line in the absence of traffic light"}; } return intersection::OccludedAbsenceTrafficLight{ @@ -316,10 +363,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( temporal_stop_before_attention_required, closest_idx, first_attention_stopline_idx, - occlusion_wo_tl_pass_judge_line_idx}; + occlusion_wo_tl_pass_judge_line_idx, + safety_report}; } + + // ========================================================================================== // following remaining block is "has_traffic_light_" + // // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + // ========================================================================================== const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; const bool is_stuck_by_static_occlusion = stoppedAtPosition( @@ -355,7 +407,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_stopline_idx, first_attention_stopline_idx, occlusion_stopline_idx, - static_occlusion_timeout}; + static_occlusion_timeout, + safety_report}; } else { return intersection::PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, @@ -398,7 +451,17 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::Indecisive & result, + [[maybe_unused]] const intersection::InternalError & result, + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) +{ + return; +} + +template <> +void prepareRTCByDecisionResult( + [[maybe_unused]] const intersection::OverPassJudge & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -606,7 +669,22 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::Indecisive & decision_result, + [[maybe_unused]] const intersection::InternalError & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + [[maybe_unused]] const double baselink2front, + [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason, + [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] IntersectionModule::DebugData * debug_data) +{ + return; +} + +template <> +void reactRTCApprovalByDecisionResult( + [[maybe_unused]] const bool rtc_default_approved, + [[maybe_unused]] const bool rtc_occlusion_approved, + [[maybe_unused]] const intersection::OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, @@ -640,7 +718,7 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, @@ -689,7 +767,7 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, @@ -807,7 +885,6 @@ void reactRTCApprovalByDecisionResult( "PeekingTowardOcclusion, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { const size_t occlusion_peeking_stopline = decision_result.temporal_stop_before_attention_required @@ -1125,84 +1202,7 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori return TrafficPrioritizedLevel::NOT_PRIORITIZED; } -TargetObjects IntersectionModule::generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const -{ - const auto & objects_ptr = planner_data_->predicted_objects; - // extract target objects - TargetObjects target_objects; - target_objects.header = objects_ptr->header; - const auto & attention_lanelets = intersection_lanelets.attention(); - const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - for (const auto & object : objects_ptr->objects) { - // ignore non-vehicle type objects, such as pedestrian. - if (!isTargetCollisionVehicleType(object)) { - continue; - } - - // check direction of objects - const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = - checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); - if (belong_adjacent_lanelet_id) { - continue; - } - - const auto is_parked_vehicle = - std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; - auto & container = is_parked_vehicle ? target_objects.parked_attention_objects - : target_objects.attention_objects; - if (intersection_area) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); - const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = - checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); - if (belong_attention_lanelet_id) { - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = std::nullopt; - target_object.stopline = std::nullopt; - target_objects.intersection_area_objects.push_back(target_object); - } - } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( - object_direction, attention_lanelets, is_parked_vehicle); - belong_attention_lanelet_id.has_value()) { - // intersection_area is not available, use detection_area_with_margin as before - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } - } - for (const auto & object : target_objects.attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (const auto & object : target_objects.parked_attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (auto & object : target_objects.all_attention_objects) { - object.calc_dist_to_stopline(); - } - return target_objects; -} - -intersection::Result< - intersection::Indecisive, - std::pair> -IntersectionModule::isOverPassJudgeLinesStatus( +IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines) { @@ -1216,8 +1216,11 @@ IntersectionModule::isOverPassJudgeLinesStatus( const size_t pass_judge_line_idx = [&]() { if (planner_param_.occlusion.enable) { if (has_traffic_light_) { + // ========================================================================================== // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its - // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line + // position is changed to occlusion_stopline_idx. even if occlusion is cleared by peeking, + // its position should be occlusion_stopline_idx as before + // ========================================================================================== if (passed_1st_judge_line_while_peeking_) { return occlusion_stopline_idx; } @@ -1226,16 +1229,22 @@ IntersectionModule::isOverPassJudgeLinesStatus( if (is_occlusion_state && is_over_first_pass_judge_line) { passed_1st_judge_line_while_peeking_ = true; return occlusion_stopline_idx; - } else { - return first_pass_judge_line_idx; } + // ========================================================================================== + // Otherwise it is first_pass_judge_line + // ========================================================================================== + return first_pass_judge_line_idx; } else if (is_occlusion_state) { + // ========================================================================================== // if there is no traffic light and occlusion is detected, pass_judge position is beyond // the boundary of first attention area + // ========================================================================================== return occlusion_wo_tl_pass_judge_line_idx; } else { + // ========================================================================================== // if there is no traffic light and occlusion is not detected, pass_judge position is - // default + // default position + // ========================================================================================== return first_pass_judge_line_idx; } } @@ -1246,147 +1255,67 @@ IntersectionModule::isOverPassJudgeLinesStatus( const bool is_over_1st_pass_judge_line = util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); + bool safely_passed_1st_judge_line_first_time = false; if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { - safely_passed_1st_judge_line_time_ = clock_->now(); + safely_passed_1st_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_1st_judge_line_first_time = true; } const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.first_pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, path); debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); - const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; - const bool is_over_2nd_pass_judge_line = - util::isOverTargetIndex(path, closest_idx, current_pose, second_pass_judge_line_idx); - if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { - safely_passed_2nd_judge_line_time_ = clock_->now(); + const auto second_pass_judge_line_idx_opt = intersection_stoplines.second_pass_judge_line; + const std::optional is_over_2nd_pass_judge_line = + second_pass_judge_line_idx_opt + ? std::make_optional(util::isOverTargetIndex( + path, closest_idx, current_pose, second_pass_judge_line_idx_opt.value())) + : std::nullopt; + bool safely_passed_2nd_judge_line_first_time = false; + if ( + is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value() && was_safe && + !safely_passed_2nd_judge_line_time_) { + safely_passed_2nd_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_2nd_judge_line_first_time = true; + } + if (second_pass_judge_line_idx_opt) { + debug_data_.second_pass_judge_wall_pose = + planning_utils::getAheadPose(second_pass_judge_line_idx_opt.value(), baselink2front, path); } - debug_data_.second_pass_judge_wall_pose = - planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, path); debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); const bool is_over_default_stopline = util::isOverTargetIndex(path, closest_idx, current_pose, default_stopline_idx); + + const bool over_default_stopline_for_pass_judge = + is_over_default_stopline || planner_param_.common.enable_pass_judge_before_default_stopline; + const bool over_pass_judge_line_overall = + is_over_2nd_pass_judge_line ? is_over_2nd_pass_judge_line.value() : is_over_1st_pass_judge_line; if ( - ((is_over_default_stopline || - planner_param_.common.enable_pass_judge_before_default_stopline) && - is_over_2nd_pass_judge_line && was_safe) || + (over_default_stopline_for_pass_judge && over_pass_judge_line_overall && was_safe) || is_permanent_go_) { - /* - * This body is active if ego is - * - over the default stopline AND - * - over the 1st && 2nd pass judge line AND - * - previously safe - * , - * which means ego can stop even if it is over the 1st pass judge line but - * - before default stopline OR - * - before the 2nd pass judge line OR - * - or previously unsafe - * . - * In order for ego to continue peeking or collision detection when occlusion is detected after - * ego passed the 1st pass judge line, it needs to be - * - before the default stopline OR - * - before the 2nd pass judge line OR - * - previously unsafe - */ + // ========================================================================================== + // this body is active if ego is + // - over the default stopline AND + // - over the 1st && 2nd pass judge line AND + // - previously safe + // , + // which means ego can stop even if it is over the 1st pass judge line but + // - before default stopline OR + // - before the 2nd pass judge line OR + // - or previously unsafe + // . + // + // in order for ego to continue peeking or collision detection when occlusion is detected + // after ego passed the 1st pass judge line, it needs to be + // - before the default stopline OR + // - before the 2nd pass judge line OR + // - previously unsafe + // ========================================================================================== is_permanent_go_ = true; - return intersection::Result>::make_ok( - intersection::Indecisive{"over the pass judge line. no plan needed"}); - } - return intersection::Result>::make_err( - std::make_pair(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line)); -} - -void TargetObject::calc_dist_to_stopline() -{ - if (!attention_lanelet || !stopline) { - return; } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); + return { + is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, + safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time}; } -/* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel) - { - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - // consider vehicle in ego-lane && in front of ego - const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double object_decel = - planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive - const double stopping_distance = lon_vel * lon_vel / (2 * object_decel); - - std::vector center_points; - for (auto && p : ego_lane_with_next_lane[0].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - for (auto && p : ego_lane_with_next_lane[1].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - const double lat_offset = - std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); - // get the nearest centerpoint to object - std::vector dist_obj_center_points; - for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, - p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), - std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); - // find two center_points whose distances from `closest_centerpoint` cross stopping_distance - double acc_dist_prev = 0.0, acc_dist = 0.0; - auto p1 = center_points[obj_closest_centerpoint_idx]; - auto p2 = center_points[obj_closest_centerpoint_idx]; - for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) { - p1 = center_points[i]; - p2 = center_points[i + 1]; - acc_dist_prev = acc_dist; - const auto arc_position_p1 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); - const auto arc_position_p2 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); - const double delta = arc_position_p2.length - arc_position_p1.length; - acc_dist += delta; - if (acc_dist > stopping_distance) { - break; - } - } - // if stopping_distance >= center_points, stopping_point is center_points[end] - const double ratio = (acc_dist <= stopping_distance) - ? 0.0 - : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); - // linear interpolation - geometry_msgs::msg::Point stopping_point; - stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio); - stopping_point.y = (p1.y * ratio + p2.y) / (1 + ratio); - stopping_point.z = (p1.z * ratio + p2.z) / (1 + ratio); - const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, stopping_point); - stopping_point.x += lat_offset * std::cos(lane_yaw + M_PI / 2.0); - stopping_point.y += lat_offset * std::sin(lane_yaw + M_PI / 2.0); - - // calculate footprint of predicted stopping pose - autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; - predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; - predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); - const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); - debug_data_.predicted_obj_pose.position = stopping_point; - debug_data_.predicted_obj_pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - - if (is_in_stuck_area) { - return true; - } - return false; - } -*/ - } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8a34aabe8b918..9527ce8e5ebea 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -19,6 +19,7 @@ #include "interpolated_path_info.hpp" #include "intersection_lanelets.hpp" #include "intersection_stoplines.hpp" +#include "object_manager.hpp" #include "result.hpp" #include @@ -45,32 +46,6 @@ namespace behavior_velocity_planner { -/** - * @struct - * @brief see the document for more details of IntersectionStopLines - */ -struct TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -/** - * @struct - * @brief categorize TargetObjects - */ -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - class IntersectionModule : public SceneModuleInterface { public: @@ -154,6 +129,10 @@ class IntersectionModule : public SceneModuleInterface { double object_margin_to_path; } ignore_on_red_traffic_light; + struct AvoidCollisionByAcceleration + { + double object_time_margin_to_collision_point; + } avoid_collision_by_acceleration; } collision_detection; struct Occlusion @@ -204,36 +183,37 @@ class IntersectionModule : public SceneModuleInterface std::optional occlusion_stop_wall_pose{std::nullopt}; std::optional occlusion_first_stop_wall_pose{std::nullopt}; std::optional first_pass_judge_wall_pose{std::nullopt}; + std::optional second_pass_judge_wall_pose{std::nullopt}; bool passed_first_pass_judge{false}; bool passed_second_pass_judge{false}; - std::optional second_pass_judge_wall_pose{std::nullopt}; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional first_attention_area{std::nullopt}; std::optional second_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; + autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets; + autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets; + autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects parked_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; using TimeDistanceArray = std::vector>; /** - * @struct * @brief categorize traffic light priority */ enum class TrafficPrioritizedLevel { @@ -246,6 +226,41 @@ class IntersectionModule : public SceneModuleInterface }; /** @} */ + /** + * @brief + */ + struct PassJudgeStatus + { + //! true if ego is over the 1st pass judge line + const bool is_over_1st_pass_judge; + + //! true if second_attention_lane exists and ego is over the 2nd pass judge line + const std::optional is_over_2nd_pass_judge; + + //! true only when ego passed 1st pass judge line safely for the first time + const bool safely_passed_1st_judge_line; + + //! true only when ego passed 2nd pass judge line safely for the first time + const bool safely_passed_2nd_judge_line; + }; + + /** + * @brief + */ + struct CollisionStatus + { + enum BlameType { + BLAME_AT_FIRST_PASS_JUDGE, + BLAME_AT_SECOND_PASS_JUDGE, + }; + const bool collision_detected; + const intersection::CollisionInterval::LanePosition collision_position; + const std::vector>> + too_late_detect_objects; + const std::vector>> + misjudge_objects; + }; + IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, @@ -325,6 +340,9 @@ class IntersectionModule : public SceneModuleInterface //! cache discretized occlusion detection lanelets std::optional> occlusion_attention_divisions_{ std::nullopt}; + + //! save the time when ego observed green traffic light before entering the intersection + std::optional initial_green_light_observed_time_{std::nullopt}; /** @}*/ private: @@ -340,17 +358,19 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; //! for checking if ego is over the pass judge lines because previously the situation was SAFE - intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}}; + intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}}; //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line //! is treated as the same position as occlusion_peeking_stopline bool passed_1st_judge_line_while_peeking_{false}; - //! save the time when ego passed the 1st/2nd_pass_judge_line with safe decision. If collision is - //! expected after these variables are non-null, then it is the fault of past perception failure - //! at these time. - std::optional safely_passed_1st_judge_line_time_{std::nullopt}; - std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; + //! save the time and ego position when ego passed the 1st/2nd_pass_judge_line with safe + //! decision. If collision is expected after these variables are non-null, then it is the fault of + //! past perception failure at these time. + std::optional> + safely_passed_1st_judge_line_time_{std::nullopt}; + std::optional> + safely_passed_2nd_judge_line_time_{std::nullopt}; /** @}*/ private: @@ -363,6 +383,9 @@ class IntersectionModule : public SceneModuleInterface */ //! debouncing for stable SAFE decision StateMachine collision_state_machine_; + + //! container for storing safety status of objects on the attention area + intersection::ObjectInfoManager object_info_manager_; /** @} */ private: @@ -388,8 +411,6 @@ class IntersectionModule : public SceneModuleInterface StateMachine static_occlusion_timeout_state_machine_; /** @} */ - std::optional initial_green_light_observed_time_{std::nullopt}; - private: /** *********************************************************** @@ -464,13 +485,13 @@ class IntersectionModule : public SceneModuleInterface /** * @brief prepare basic data structure - * @return return IntersectionStopLines if all data is valid, otherwise Indecisive + * @return return IntersectionStopLines if all data is valid, otherwise InternalError * @note if successful, it is ensure that intersection_lanelets_, * intersection_lanelets.first_conflicting_lane are not null * * To simplify modifyPathVelocityDetail(), this function is used at first */ - intersection::Result prepareIntersectionData( + intersection::Result prepareIntersectionData( const bool is_prioritized, PathWithLaneId * path); /** @@ -518,16 +539,6 @@ class IntersectionModule : public SceneModuleInterface const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); /** @} */ -private: - /** - * @defgroup utility [fn] utility member function - * @{ - */ - void stoppedAtPositionForDuration( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t position, - const double duration, StateMachine * state_machine); - /** @} */ - private: /** *********************************************************** @@ -547,14 +558,6 @@ class IntersectionModule : public SceneModuleInterface TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; /** @} */ -private: - /** - * @brief categorize target objects - */ - TargetObjects generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const; - private: /** *********************************************************** @@ -598,14 +601,12 @@ class IntersectionModule : public SceneModuleInterface std::optional isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const; + const intersection::IntersectionStopLines & intersection_stoplines) const; /** * @brief check yield stuck */ bool checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const; /** @} */ @@ -621,27 +622,21 @@ class IntersectionModule : public SceneModuleInterface /** * @brief check occlusion status * @attention this function has access to value() of occlusion_attention_divisions_, - * intersection_lanelets.first_attention_area() + * intersection_lanelets_ intersection_lanelets.first_attention_area() */ std::tuple< OcclusionType, bool /* module detection with margin */, bool /* reconciled occlusion disapproval */> getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects); + const intersection::InterpolatedPathInfo & interpolated_path_info); /** * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) + * @attention this function has access to value() of intersection_lanelets_, + * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects); + OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info); /** @} */ private: @@ -654,15 +649,12 @@ class IntersectionModule : public SceneModuleInterface */ /** * @brief check if ego is already over the pass judge line - * @return if ego is over both 1st/2nd pass judge lines, return Indecisive, else return + * @return if ego is over both 1st/2nd pass judge lines, return InternalError, else return * (is_over_1st_pass_judge, is_over_2nd_pass_judge) * @attention this function has access to value() of intersection_stoplines.default_stopline, * intersection_stoplines.occlusion_stopline */ - intersection::Result< - intersection::Indecisive, - std::pair> - isOverPassJudgeLinesStatus( + PassJudgeStatus isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines); /** @} */ @@ -678,6 +670,25 @@ class IntersectionModule : public SceneModuleInterface bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + /** + * @brief find the objects on attention_area/intersection_area and update positional information + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerArea(); + + /** + * @brief find the collision Interval/CollisionKnowledge of registered objects + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, + const TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time); + + void cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * predicted_path) const; + /** * @brief check if there are any objects around the stoplines on the attention areas when ego * entered the intersection on green light @@ -687,44 +698,58 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.occlusion_peeking_stopline */ std::optional isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects); + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines); + + /** + * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and + * blame past perception fault + */ + std::string generateDetectionBlameDiagnosis( + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; /** - * @brief check collision + * @brief generate the message explaining how much ego should accelerate to avoid future dangerous + * situation */ - bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const intersection::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const TrafficPrioritizedLevel & traffic_prioritized_level); + std::string generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const TimeDistanceArray & ego_time_distance_array, + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; + + /** + * @brief return if collision is detected and the collision position + */ + CollisionStatus detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line); std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, const bool is_parked_vehicle) const; - void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); - /** * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of * (time of arrival, traveled distance) from current ego position + * @attention this function has access to value() of + * intersection_stoplines.occlusion_peeking_stopline, + * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); /** @} */ - /* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel); - */ - mutable DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 43bb68cf56f67..4f7e8b45d107f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -18,42 +18,18 @@ #include // for toGeomPoly #include // for smoothPath #include +#include #include #include // for toPolygon2d #include -#include #include +#include +#include +#include #include -namespace -{ -tier4_autoware_utils::Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) -{ - namespace bg = boost::geometry; - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); - - tier4_autoware_utils::Polygon2d one_step_poly; - for (const auto & point : prev_poly.outer()) { - one_step_poly.outer().push_back(point); - } - for (const auto & point : next_poly.outer()) { - one_step_poly.outer().push_back(point); - } - - bg::correct(one_step_poly); - - tier4_autoware_utils::Polygon2d convex_one_step_poly; - bg::convex_hull(one_step_poly, convex_one_step_poly); - - return convex_one_step_poly; -} -} // namespace - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -79,38 +55,303 @@ bool IntersectionModule::isTargetCollisionVehicleType( return false; } -std::optional -IntersectionModule::isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) +void IntersectionModule::updateObjectInfoManagerArea() { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - // If there are any vehicles on the attention area when ego entered the intersection on green - // light, do pseudo collision detection because the vehicles are very slow and no collisions may - // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = isGreenSolidOn(); - if (!is_green_solid_on) { - return std::nullopt; + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + + // ========================================================================================== + // entries that are not observed in this iteration need to be cleared + // + // NOTE: old_map is not referenced because internal data of object_info_manager is cleared + // ========================================================================================== + const auto old_map = object_info_manager_.getObjectsMap(); + object_info_manager_.clearObjects(); + + for (const auto & predicted_object : planner_data_->predicted_objects->objects) { + if (!isTargetCollisionVehicleType(predicted_object)) { + continue; + } + + // ========================================================================================== + // NOTE: is_parked_vehicle is used because sometimes slow vehicle direction is + // incorrect/reversed/flipped due to tracking. if is_parked_vehicle is true, object direction + // is not checked + // ========================================================================================== + const auto object_direction = + util::getObjectPoseWithVelocityDirection(predicted_object.kinematics); + const auto is_parked_vehicle = + std::fabs(predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); + if (belong_adjacent_lanelet_id) { + continue; + } + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); + const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + const bool in_intersection_area = [&]() { + if (!intersection_area) { + return false; + } + return bg::within( + tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + }(); + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + if (!belong_attention_lanelet_id && !in_intersection_area) { + continue; + } else if (belong_attention_lanelet_id) { + const auto idx = belong_attention_lanelet_id.value(); + attention_lanelet = attention_lanelets.at(idx); + stopline = attention_lanelet_stoplines.at(idx); + } + + const auto object_it = old_map.find(predicted_object.object_id); + if (object_it != old_map.end()) { + auto object_info = object_it->second; + object_info_manager_.registerExistingObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle, object_info); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } else { + auto object_info = object_info_manager_.registerObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } } - const auto closest_idx = intersection_stoplines.closest_idx; - const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); - if (!initial_green_light_observed_time_) { - const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); - const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( - path.points, closest_idx, - tier4_autoware_utils::createPoint( - assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), - assigned_lane_begin_point.z())) < - planner_param_.collision_detection.yield_on_green_traffic_light - .distance_to_assigned_lanelet_start; - if (approached_assigned_lane) { - initial_green_light_observed_time_ = clock_->now(); +} + +void IntersectionModule::updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, + const IntersectionModule::TimeDistanceArray & time_distance_array, + const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time) +{ + const auto & intersection_lanelets = intersection_lanelets_.value(); + + if (passed_1st_judge_line_first_time) { + object_info_manager_.setPassed1stPassJudgeLineFirstTime(clock_->now()); + } + if (passed_2nd_judge_line_first_time) { + object_info_manager_.setPassed2ndPassJudgeLineFirstTime(clock_->now()); + } + + const double passing_time = time_distance_array.back().first; + const auto & concat_lanelets = path_lanelets.all; + const auto closest_arc_coords = + lanelet::utils::getArcCoordinates(concat_lanelets, planner_data_->current_odometry->pose); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + // ========================================================================================== + // dynamically change TTC margin according to traffic light color to gradually relax from green to + // red + // ========================================================================================== + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); + + for (auto & object_info : object_info_manager_.attentionObjects()) { + const auto & predicted_object = object_info->predicted_object(); + bool safe_under_traffic_control = false; + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + object_info->can_stop_before_stopline( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)) { + safe_under_traffic_control = true; + } + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && + object_info->can_stop_before_ego_lane( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration, + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path, + ego_lane)) { + safe_under_traffic_control = true; + } + + // ========================================================================================== + // check the PredictedPath in the ascending order of its confidence to save the safe/unsafe + // CollisionKnowledge for most probable path + // ========================================================================================== + std::list sorted_predicted_paths; + for (unsigned i = 0; i < predicted_object.kinematics.predicted_paths.size(); ++i) { + sorted_predicted_paths.push_back(&predicted_object.kinematics.predicted_paths.at(i)); + } + sorted_predicted_paths.sort( + [](const auto path1, const auto path2) { return path1->confidence > path2->confidence; }); + + // ========================================================================================== + // if all of the predicted path is lower confidence/geometrically does not intersect with ego + // path, both will be null, which is interpreted as SAFE. if any of the path is "normal", either + // of them has value, not both + // ========================================================================================== + std::optional unsafe_interval{std::nullopt}; + std::optional safe_interval{std::nullopt}; + + for (const auto & predicted_path_ptr : sorted_predicted_paths) { + auto predicted_path = *predicted_path_ptr; + if ( + predicted_path.confidence < + planner_param_.collision_detection.min_predicted_path_confidence) { + continue; + } + cutPredictPathWithinDuration( + planner_data_->predicted_objects->header.stamp, passing_time, &predicted_path); + const auto object_passage_interval_opt = intersection::findPassageInterval( + predicted_path, predicted_object.shape, ego_poly, + intersection_lanelets.first_attention_lane(), + intersection_lanelets.second_attention_lane()); + if (!object_passage_interval_opt) { + // there is no chance of geometric collision for the entire prediction horizon + continue; + } + const auto & object_passage_interval = object_passage_interval_opt.value(); + const auto [object_enter_time, object_exit_time] = object_passage_interval.interval_time; + const auto ego_start_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_enter_time - collision_start_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_start_itr == time_distance_array.end()) { + // ========================================================================================== + // this is the case where at time "object_enter_time - collision_start_margin_time", ego is + // arriving at the exit of the intersection, which means even if we assume that the object + // accelerates and the first collision happens faster by the TTC margin, ego will be already + // arriving at the exist of the intersection. + // ========================================================================================== + continue; + } + auto ego_end_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_exit_time + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_end_itr == time_distance_array.end()) { + ego_end_itr = time_distance_array.end() - 1; + } + const double ego_start_arc_length = std::max( + 0.0, closest_arc_coords.length + ego_start_itr->second - + planner_data_->vehicle_info_.rear_overhang_m); + const double ego_end_arc_length = std::min( + closest_arc_coords.length + ego_end_itr->second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + const auto trimmed_ego_polygon = lanelet::utils::getPolygonFromArcLength( + concat_lanelets, ego_start_arc_length, ego_end_arc_length); + if (trimmed_ego_polygon.empty()) { + continue; + } + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + bg::correct(polygon); + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + + const auto & object_path = object_passage_interval.path; + const auto [begin, end] = object_passage_interval.interval_position; + bool collision_detected = false; + for (auto i = begin; i <= end; ++i) { + if (bg::intersects( + polygon, + tier4_autoware_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { + collision_detected = true; + break; + } + } + if (collision_detected) { + // if judged as UNSAFE, return + safe_interval = std::nullopt; + unsafe_interval = object_passage_interval; + break; + } + if (!safe_interval) { + // ========================================================================================== + // save the safe_decision_knowledge for the most probable path. this value is nullified if + // judged UNSAFE during the iteration + // ========================================================================================== + safe_interval = object_passage_interval; + } + } + object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); + if (passed_1st_judge_line_first_time) { + object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + if (passed_2nd_judge_line_first_time) { + object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + } +} + +void IntersectionModule::cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * path) const +{ + const rclcpp::Time current_time = clock_->now(); + const auto original_path = path->path; + path->path.clear(); + + for (size_t k = 0; k < original_path.size(); ++k) { // each path points + const auto & predicted_pose = original_path.at(k); + const auto predicted_time = + rclcpp::Time(object_stamp) + rclcpp::Duration(path->time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + path->path.push_back(predicted_pose); } } +} + +std::optional +IntersectionModule::isGreenPseudoCollisionStatus( + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines) +{ + // ========================================================================================== + // if there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because collision is likely to happen. + // ========================================================================================== if (initial_green_light_observed_time_) { const auto now = clock_->now(); const bool still_wait = @@ -119,22 +360,341 @@ IntersectionModule::isGreenPseudoCollisionStatus( if (!still_wait) { return std::nullopt; } + const auto & attention_objects = object_info_manager_.attentionObjects(); const bool exist_close_vehicles = std::any_of( - target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), - [&](const auto & object) { - return object.dist_to_stopline.has_value() && - object.dist_to_stopline.value() < - planner_param_.collision_detection.yield_on_green_traffic_light - .object_dist_to_stopline; + attention_objects.begin(), attention_objects.end(), [&](const auto & object_info) { + return object_info->before_stopline_by( + planner_param_.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline); }); if (exist_close_vehicles) { + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")}; } } return std::nullopt; } +std::string IntersectionModule::generateDetectionBlameDiagnosis( + const std::vector>> & + too_late_detect_objects, + const std::vector>> & + misjudge_objects) const +{ + std::string diag; + if (!safely_passed_1st_judge_line_time_) { + return diag; + } + const auto [passed_1st_judge_line_time, passed_1st_judge_line_pose] = + safely_passed_1st_judge_line_time_.value(); + const auto passed_1st_judge_line_time_double = + static_cast(passed_1st_judge_line_time.nanoseconds()) / 1e+9; + + const auto now = clock_->now(); + const auto now_double = static_cast(now.nanoseconds()) / 1e+9; + + // CAVEAT: format library causes runtime fault if the # of placeholders is more than the # of + // vargs + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval()) { + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_1st_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 1st pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the first attention lanelet of type " + "{5}.\n", + object_info->uuid_str, // 0 + passed_1st_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 1st pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + ); + } + } + if ( + safely_passed_2nd_judge_line_time_ && + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval()) { + const auto [passed_2nd_judge_line_time, passed_2nd_judge_line_pose] = + safely_passed_2nd_judge_line_time_.value(); + const auto passed_2nd_judge_line_time_double = + static_cast(passed_2nd_judge_line_time.nanoseconds()) / 1e+9; + + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_2nd_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 2nd pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the lanelet of type {5}.\n", + object_info->uuid_str, // 0 + passed_2nd_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 2nd pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + ); + } + } + } + for (const auto & [blame_type, object_info] : misjudge_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_1st_pass_judge_line_passage()) { + const auto & decision_at_1st_pass_judge_line = + object_info->decision_at_1st_pass_judge_line_passage().value(); + const auto decision_at_1st_pass_judge_line_time = + static_cast(decision_at_1st_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 1st pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the first attention lanelet of type {7} with the estimated " + "current velocity {8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_1st_pass_judge_line.safe_type), // 1 + decision_at_1st_pass_judge_line_time, // 2 + decision_at_1st_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + if ( + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_2nd_pass_judge_line_passage()) { + const auto & decision_at_2nd_pass_judge_line = + object_info->decision_at_2nd_pass_judge_line_passage().value(); + const auto decision_at_2nd_pass_judge_line_time = + static_cast(decision_at_2nd_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 2nd pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the lanelet of type {7} with the estimated current velocity " + "{8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_2nd_pass_judge_line.safe_type), // 1 + decision_at_2nd_pass_judge_line_time, // 2 + decision_at_2nd_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + } + return diag; +} + +std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const IntersectionModule::TimeDistanceArray & ego_time_distance_array, + const std::vector>> & + too_late_detect_objects, + [[maybe_unused]] const std::vector>> & + misjudge_objects) const +{ + static constexpr double min_vel = 1e-2; + std::string diag; + const double ego_vel = planner_data_->current_velocity->twist.linear.x; + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if (!object_info->unsafe_interval()) { + continue; + } + // object side + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const auto [begin, end] = unsafe_interval.interval_position; + const auto &p1 = unsafe_interval.path.at(begin).position, + p2 = unsafe_interval.path.at(end).position; + const auto collision_pos = + tier4_autoware_utils::createPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); + const auto object_dist_to_margin_point = + tier4_autoware_utils::calcDistance2d( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose.position, + collision_pos) - + planner_param_.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point * + object_info->observed_velocity(); + if (object_dist_to_margin_point <= 0.0) { + continue; + } + const auto object_eta_to_margin_point = + object_dist_to_margin_point / std::max(min_vel, object_info->observed_velocity()); + // ego side + const double ego_dist_to_collision_pos = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_pos); + const auto ego_eta_to_collision_pos_it = std::lower_bound( + ego_time_distance_array.begin(), ego_time_distance_array.end(), ego_dist_to_collision_pos, + [](const auto & a, const double b) { return a.second < b; }); + if (ego_eta_to_collision_pos_it == ego_time_distance_array.end()) { + continue; + } + const double ego_eta_to_collision_pos = ego_eta_to_collision_pos_it->first; + if (ego_eta_to_collision_pos < object_eta_to_margin_point) { + // this case, ego will pass the collision point before this object get close to the collision + // point within margin just by keeping current plan, so no need to accelerate + continue; + } + diag += fmt::format( + "the object is expected to approach the collision point by the TTC margin in {0} seconds, " + "while ego will arrive there in {1} seconds, so ego needs to accelerate from current " + "velocity {2}[m/s] to {3}[m/s]\n", + object_eta_to_margin_point, // 0 + ego_eta_to_collision_pos, // 1 + ego_vel, // 2 + ego_dist_to_collision_pos / object_eta_to_margin_point // 3 + ); + } + return diag; +} + +IntersectionModule::CollisionStatus IntersectionModule::detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line) +{ + // ========================================================================================== + // if collision is detected for multiple objects, we prioritize collision on the first + // attention lanelet + // ========================================================================================== + bool collision_at_first_lane = false; + bool collision_at_non_first_lane = false; + + // ========================================================================================== + // find the objects which is judges as UNSAFE after ego passed pass judge lines. + // + // misjudge_objects are those that were once judged as safe when ego passed the pass judge line + // + // too_late_detect objects are those that (1) were not detected when ego passed the pass judge + // line (2) were judged as dangerous at the same time when ego passed the pass judge, which are + // expected to have been detected in the prior iteration because ego could have judged as UNSAFE + // in the prior iteration + // ========================================================================================== + std::vector>> + misjudge_objects; + std::vector>> + too_late_detect_objects; + for (const auto & object_info : object_info_manager_.attentionObjects()) { + if (object_info->is_safe_under_traffic_control()) { + debug_data_.safe_under_traffic_control_targets.objects.push_back( + object_info->predicted_object()); + continue; + } + if (!object_info->is_unsafe()) { + continue; + } + const auto & unsafe_info = object_info->is_unsafe().value(); + // ========================================================================================== + // if ego is over the pass judge lines, then the visualization as "too_late_objects" or + // "misjudge_objects" is more important than that for "unsafe" + // + // NOTE: consider a vehicle which was not detected at 1st_pass_judge_passage, and now collision + // detected on the 1st lane, which is "too_late" for 1st lane passage, but once it decelerated + // or yielded, so it turned safe, and ego passed the 2nd pass judge line, but at the same it + // accelerated again, which is "misjudge" for 2nd lane passage. In this case this vehicle is + // visualized as "misjudge" + // ========================================================================================== + auto * debug_container = &debug_data_.unsafe_targets.objects; + if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + collision_at_first_lane = true; + } else { + collision_at_non_first_lane = true; + } + if ( + is_over_1st_pass_judge_line && + unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + const auto & decision_at_1st_pass_judge_opt = + object_info->decision_at_1st_pass_judge_line_passage(); + if (!decision_at_1st_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value(); + if ( + decision_at_1st_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + if (is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value()) { + const auto & decision_at_2nd_pass_judge_opt = + object_info->decision_at_2nd_pass_judge_line_passage(); + if (!decision_at_2nd_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); + if ( + decision_at_2nd_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + debug_container->emplace_back(object_info->predicted_object()); + } + if (collision_at_first_lane) { + return { + true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; + } else if (collision_at_non_first_lane) { + return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + } + return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; +} + +/* bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, const intersection::PathLanelets & path_lanelets, const size_t closest_idx, @@ -383,6 +943,7 @@ bool IntersectionModule::checkCollision( return collision_detected; } +*/ std::optional IntersectionModule::checkAngleForTargetLanelets( @@ -422,32 +983,9 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( return std::nullopt; } -void IntersectionModule::cutPredictPathWithDuration( - TargetObjects * target_objects, const double time_thr) -{ - const rclcpp::Time current_time = clock_->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(target_objects->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } - } - } - } -} - IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { const double intersection_velocity = @@ -460,8 +998,42 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; const double current_velocity = planner_data_->current_velocity->twist.linear.x; - int assigned_lane_found = false; + // ========================================================================================== + // if ego is waiting for collision detection, the entry time into the intersection + // is a bit delayed for the chattering hold, so we need to "shift" the TimeDistanceArray by + // this delay + // ========================================================================================== + const bool is_go_out = (activated_ && occlusion_activated_); + const double time_delay = (is_go_out || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.collision_detection_hold_time - + collision_state_machine_.getDuration()); + // ========================================================================================== + // to account for the stopline generated by upstream behavior_velocity modules (like walkway, + // crosswalk), if use_upstream flag is true, the raw velocity of path points after + // last_intersection_stopline_candidate_idx is used, which maybe almost-zero. at those almost-zero + // velocity path points, ego future profile is almost "fixed" there. + // + // last_intersection_stopline_candidate_idx must be carefully decided especially when ego + // velocity is almost zero, because if last_intersection_stopline_candidate_idx is at the + // closest_idx for example, ego is almost "fixed" at current position for the entire + // spatiotemporal profile, which is judged as SAFE because that profile does not collide + // with the predicted paths of objects. + // + // if second_attention_lane exists, second_attention_stopline_idx is used. if not, + // max(occlusion_stopline_idx, first_attention_stopline_idx) is used because + // occlusion_stopline_idx varies depending on the peeking offset parameter + // ========================================================================================== + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); + const auto closest_idx = intersection_stoplines.closest_idx; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() + : std::max(occlusion_stopline_idx, first_attention_stopline_idx); + + int assigned_lane_found = false; // crop intersection part of the path, and set the reference velocity to intersection_velocity // for ego's ttc PathWithLaneId reference_path; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 353826070eff7..b7c2d4c12878c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -34,25 +34,18 @@ std::tuple< bool /* reconciled occlusion disapproval */> IntersectionModule::getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - const auto first_attention_area = intersection_lanelets.first_attention_area().value(); const bool is_amber_or_red = (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); // check occlusion on detection lane - const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); auto occlusion_status = (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) - ? detectOcclusion( - occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, - occlusion_attention_divisions, target_objects) + ? detectOcclusion(interpolated_path_info) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -76,13 +69,14 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & attention_areas = intersection_lanelets.occlusion_attention_area(); + const auto first_attention_area = intersection_lanelets.first_attention_area().value(); + const auto & lane_divisions = occlusion_attention_divisions_.value(); + const auto & occ_grid = *planner_data_->occupancy_grid; const auto & current_pose = planner_data_->current_odometry->pose; const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; @@ -208,13 +202,15 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // re-use attention_mask attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded - const auto & blocking_attention_objects = target_objects.parked_attention_objects; - for (const auto & blocking_attention_object : blocking_attention_objects) { - debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + const auto & blocking_attention_objects = object_info_manager_.parkedObjects(); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + debug_data_.parked_targets.objects.push_back( + blocking_attention_object_info->predicted_object()); } std::vector> blocking_polygons; - for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + const Polygon2d obj_poly = + tier4_autoware_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { @@ -390,14 +386,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( LineString2d ego_occlusion_line; ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); - for (const auto & attention_object : target_objects.all_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - for (const auto & attention_object : target_objects.intersection_area_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + for (const auto & attention_object_info : object_info_manager_.allObjects()) { + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_line)) { return OcclusionType::DYNAMICALLY_OCCLUDED; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 746e615d8c6b0..c44008db9b08b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -161,11 +161,13 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -intersection::Result +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + +Result IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) { - using intersection::Result; - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); @@ -177,26 +179,32 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - return Result::make_err( - intersection::Indecisive{"splineInterpolate failed"}); + return make_err( + "splineInterpolate failed"); } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - return Result::make_err( - intersection::Indecisive{ - "Path has no interval on intersection lane " + std::to_string(lane_id_)}); + return make_err( + "Path has no interval on intersection lane " + std::to_string(lane_id_)); } - // cache intersection lane information because it is invariant if (!intersection_lanelets_) { intersection_lanelets_ = generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); } auto & intersection_lanelets = intersection_lanelets_.value(); - - // at the very first time of regisTration of this module, the path may not be conflicting with the - // attention area, so update() is called to update the internal data as well as traffic light info + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); + debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + + // ========================================================================================== + // at the very first time of registration of this module, the path may not be conflicting with + // the attention area, so update() is called to update the internal data as well as traffic + // light info + // ========================================================================================== intersection_lanelets.update( is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); @@ -205,17 +213,17 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { // this is abnormal - return Result::make_err( - intersection::Indecisive{"conflicting area is empty"}); + return make_err( + "conflicting area is empty"); } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); - // generate all stop line candidates - // see the doc for struct IntersectionStopLines - /// even if the attention area is null, stuck vehicle stop line needs to be generated from - /// conflicting lanes + // ========================================================================================== + // even if the attention area is null, stuck vehicle stop line needs to be generated from + // conflicting lanes, so dummy_first_attention_lane is used + // ========================================================================================== const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() ? intersection_lanelets.first_attention_lane().value() : first_conflicting_lane; @@ -224,8 +232,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, interpolated_path_info, path); if (!intersection_stoplines_opt) { - return Result::make_err( - intersection::Indecisive{"failed to generate intersection_stoplines"}); + return make_err( + "failed to generate intersection_stoplines"); } const auto & intersection_stoplines = intersection_stoplines_opt.value(); const auto closest_idx = intersection_stoplines.closest_idx; @@ -239,8 +247,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { - return Result::make_err( - intersection::Indecisive{"failed to generate PathLanelets"}); + return make_err( + "failed to generate PathLanelets"); } const auto & path_lanelets = path_lanelets_opt.value(); @@ -250,8 +258,24 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL planner_data_->occupancy_grid->info.resolution); } - return Result::make_ok( - BasicData{interpolated_path_info, intersection_stoplines, path_lanelets}); + const bool is_green_solid_on = isGreenSolidOn(); + if (is_green_solid_on && !initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + + return make_ok( + interpolated_path_info, intersection_stoplines, path_lanelets); } std::optional IntersectionModule::getStopLineIndexFromMap( @@ -421,8 +445,10 @@ IntersectionModule::generateIntersectionStopLines( int stuck_stopline_ip_int = 0; bool stuck_stopline_valid = true; if (use_stuck_stopline) { + // ========================================================================================== // NOTE: when ego vehicle is approaching attention area and already passed // first_conflicting_area, this could be null. + // ========================================================================================== const auto stuck_stopline_idx_ip_opt = util::getFirstPointInsidePolygonByFootprint( first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); if (!stuck_stopline_idx_ip_opt) { @@ -457,12 +483,13 @@ IntersectionModule::generateIntersectionStopLines( second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) : 0; - // (8) second pass judge line position on interpolated path. It is the same as first pass judge - // line if second_attention_lane is null - int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; - const auto second_pass_judge_line_ip = - second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) - : first_pass_judge_line_ip; + // (8) second pass judge line position on interpolated path. It is null if second_attention_lane + // is null + size_t second_pass_judge_line_ip = occlusion_wo_tl_pass_judge_line_ip; + bool second_pass_judge_line_valid = false; + if (second_attention_area_opt) { + second_pass_judge_line_valid = true; + } struct IntersectionStopLinesTemp { @@ -528,9 +555,11 @@ IntersectionModule::generateIntersectionStopLines( intersection_stoplines.occlusion_peeking_stopline = intersection_stoplines_temp.occlusion_peeking_stopline; } + if (second_pass_judge_line_valid) { + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; + } intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; - intersection_stoplines.second_pass_judge_line = - intersection_stoplines_temp.second_pass_judge_line; intersection_stoplines.occlusion_wo_tl_pass_judge_line = intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stoplines; @@ -556,7 +585,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets } // for low priority lane - // If ego_lane has right of way (i.e. is high priority), + // if ego_lane has right of way (i.e. is high priority), // ignore yieldLanelets (i.e. low priority lanes) lanelet::ConstLanelets yield_lanelets{}; const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 7f23bed3c36cd..389c73d651f1e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -262,21 +262,19 @@ bool IntersectionModule::checkStuckVehicleInIntersection( std::optional IntersectionModule::isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const + const intersection::IntersectionStopLines & intersection_stoplines) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { return motion_utils::calcSignedArcLength(path.points, closest_idx, index); }; - const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK - const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); // this is OK - const auto first_attention_stopline_idx = - intersection_stoplines.first_attention_stopline.value(); // this is OK + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding()); + interpolated_path_info, intersection_lanelets.attention_non_preceding()); if (yield_stuck_detected) { std::optional stopline_idx = std::nullopt; const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; @@ -297,14 +295,13 @@ std::optional IntersectionModule::isYieldStuckStat } } if (stopline_idx) { - return intersection::YieldStuckStop{closest_idx, stopline_idx.value()}; + return intersection::YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; } } return std::nullopt; } bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const { @@ -358,19 +355,20 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( ::createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); } debug_data_.yield_stuck_detect_area = util::getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { + for (const auto & object_info : object_info_manager_.attentionObjects()) { + const auto & object = object_info->predicted_object(); const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); if (obj_v_norm > stuck_vehicle_vel_thr) { continue; } for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); if (is_in_lanelet) { - debug_data_.yield_stuck_targets.objects.push_back(object.object); + debug_data_.yield_stuck_targets.objects.push_back(object); return true; } } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index f103191b2dc6f..e37bb3ee88cc1 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -34,7 +34,6 @@ namespace behavior_velocity_planner::util { /** - * @fn * @brief insert a new pose to the path and return its index * @return if insertion was successful return the inserted point index */ @@ -44,7 +43,6 @@ std::optional insertPointIndex( const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** - * @fn * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( @@ -52,7 +50,6 @@ bool hasLaneIds( const std::set & ids); /** - * @fn * @brief find the first contiguous interval of the path points that contains the specified lane ids * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is * found, returns the pair (start-1, end) @@ -61,7 +58,6 @@ std::optional> findLaneIdsInterval( const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @fn * @brief return the index of the first point which is inside the given polygon * @param[in] lane_interval the interval of the path points on the intersection * @param[in] search_forward flag for search direction @@ -72,7 +68,6 @@ std::optional getFirstPointInsidePolygon( const bool search_forward = true); /** - * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -84,7 +79,6 @@ bool isOverTargetIndex( const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); /** - * @fn * @brief check if ego is before the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -99,13 +93,11 @@ std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); /** - * @fn * @brief check if the given lane has related traffic light */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); /** - * @fn * @brief interpolate PathWithLaneId */ std::optional generateInterpolatedPath( @@ -117,7 +109,6 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); /** - * @fn * @brief this function sorts the set of lanelets topologically using topological sort and merges * the lanelets from each root to each end. each branch is merged and returned with the original * lanelets @@ -131,7 +122,6 @@ mergeLaneletsByTopologicalSort( const lanelet::routing::RoutingGraphPtr routing_graph_ptr); /** - * @fn * @brief find the index of the first point where vehicle footprint intersects with the given * polygon */ @@ -140,6 +130,10 @@ std::optional getFirstPointInsidePolygonByFootprint( const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); +/** + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygons + */ std::optional> getFirstPointInsidePolygonsByFootprint( From 6d8f026dcfa3e811ebeecd8fa72de9512ac47762 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 29 Jan 2024 12:52:17 +0900 Subject: [PATCH 141/154] refactor(lane_change): combine all debug data into single struct (#6173) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * refactor(lane_change): combine all debug data into single struct Signed-off-by: Zulfaqar Azmi * Rename valid path → valid paths Signed-off-by: Zulfaqar Azmi * move the marker creation to marker_utils Signed-off-by: Zulfaqar Azmi * Remove alias Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../interface.hpp | 2 +- .../utils/base_class.hpp | 22 ++------- .../utils/debug_structs.hpp | 49 +++++++++++++++++++ .../utils/markers.hpp | 4 ++ .../src/interface.cpp | 44 ++++------------- .../src/scene.cpp | 40 +++++++-------- .../src/utils/markers.cpp | 44 +++++++++++++++++ 7 files changed, 129 insertions(+), 76 deletions(-) create mode 100644 planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 4e741a2b3db2c..6a9a8c40c01d1 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -128,7 +128,7 @@ class LaneChangeInterface : public SceneModuleInterface bool canTransitIdleToRunningState() override; - void setObjectDebugVisualization() const; + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index e78d706334bae..15fe2f8d2b8d9 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -15,6 +15,7 @@ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ #include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" @@ -36,7 +37,6 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -137,19 +137,7 @@ class LaneChangeBase const LaneChangeStatus & getLaneChangeStatus() const { return status_; } - const LaneChangePaths & getDebugValidPath() const { return debug_valid_path_; } - - const CollisionCheckDebugMap & getDebugData() const { return object_debug_; } - - const CollisionCheckDebugMap & getAfterApprovalDebugData() const - { - return object_debug_after_approval_; - } - - const LaneChangeTargetObjects & getDebugFilteredObjects() const - { - return debug_filtered_objects_; - } + const data::lane_change::Debug & getDebugData() const { return lane_change_debug_; } const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } @@ -257,12 +245,8 @@ class LaneChangeBase Direction direction_{Direction::NONE}; LaneChangeModuleType type_{LaneChangeModuleType::NORMAL}; - mutable LaneChangePaths debug_valid_path_{}; - mutable CollisionCheckDebugMap object_debug_{}; - mutable CollisionCheckDebugMap object_debug_after_approval_{}; - mutable LaneChangeTargetObjects debug_filtered_objects_{}; - mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; + mutable data::lane_change::Debug lane_change_debug_; rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp new file mode 100644 index 0000000000000..8ac1ba3909a50 --- /dev/null +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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 BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ + +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" + +#include + +#include + +namespace behavior_path_planner::data::lane_change +{ +using utils::path_safety_checker::CollisionCheckDebugMap; +struct Debug +{ + std::string module_type; + LaneChangePaths valid_paths; + CollisionCheckDebugMap collision_check_objects; + CollisionCheckDebugMap collision_check_objects_after_approval; + LaneChangeTargetObjects filtered_objects; + double collision_check_object_debug_lifetime{0.0}; + + void reset() + { + valid_paths.clear(); + collision_check_objects.clear(); + collision_check_objects_after_approval.clear(); + filtered_objects.current_lane.clear(); + filtered_objects.target_lane.clear(); + filtered_objects.other_lane.clear(); + collision_check_object_debug_lifetime = 0.0; + } +}; +} // namespace behavior_path_planner::data::lane_change + +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index d6deecd4f46fa..427a26e9b4295 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -15,9 +15,11 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include #include #include @@ -26,6 +28,7 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; +using behavior_path_planner::data::lane_change::Debug; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( @@ -37,5 +40,6 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & current_lane_objects, const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); +MarkerArray createDebugMarkerArray(const Debug & debug_data); } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index c8e1229fe5af5..41d6b7aa6fc19 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -83,7 +83,7 @@ void LaneChangeInterface::updateData() if (isWaitingApproval()) { module_type_->updateLaneChangeStatus(); } - setObjectDebugVisualization(); + updateDebugMarker(); module_type_->updateSpecialData(); module_type_->resetStopPose(); @@ -109,7 +109,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() stop_pose_ = module_type_->getStopPose(); - for (const auto & [uuid, data] : module_type_->getAfterApprovalDebugData()) { + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } @@ -133,7 +134,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); - for (const auto & [uuid, data] : module_type_->getDebugData()) { + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } @@ -291,7 +293,7 @@ bool LaneChangeInterface::canTransitFailureState() bool LaneChangeInterface::canTransitIdleToRunningState() { - setObjectDebugVisualization(); + updateDebugMarker(); auto log_debug_throttled = [&](std::string_view message) -> void { RCLCPP_DEBUG(getLogger(), "%s", message.data()); @@ -312,43 +314,15 @@ bool LaneChangeInterface::canTransitIdleToRunningState() return true; } -void LaneChangeInterface::setObjectDebugVisualization() const +void LaneChangeInterface::updateDebugMarker() const { - debug_marker_.markers.clear(); if (!parameters_->publish_debug_marker) { return; } - using marker_utils::showPolygon; - using marker_utils::showPredictedPath; - using marker_utils::showSafetyCheckInfo; - using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showFilteredObjects; - - const auto debug_data = module_type_->getDebugData(); - const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); - const auto debug_valid_path = module_type_->getDebugValidPath(); - const auto debug_filtered_objects = module_type_->getDebugFilteredObjects(); debug_marker_.markers.clear(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; - - add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); - add(showFilteredObjects( - debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, - debug_filtered_objects.other_lane, "object_filtered")); - if (!debug_data.empty()) { - add(showSafetyCheckInfo(debug_data, "object_debug_info")); - add(showPredictedPath(debug_data, "ego_predicted_path")); - add(showPolygon(debug_data, "ego_and_target_polygon_relation")); - } - - if (!debug_after_approval.empty()) { - add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); - add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); - } + using marker_utils::lane_change_markers::createDebugMarkerArray; + debug_marker_ = createDebugMarkerArray(module_type_->getDebugData()); } MarkerArray LaneChangeInterface::getModuleVirtualWall() diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index b115ab7c163c0..b56e9e544ea8d 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -97,7 +97,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) lane_change_parameters_->rss_params_for_stuck, is_stuck); } - debug_valid_path_ = valid_paths; + lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { safe_path.info.current_lanes = current_lanes; @@ -421,13 +421,8 @@ void NormalLaneChange::resetParameters() current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; status_ = {}; + lane_change_debug_.reset(); - object_debug_.clear(); - object_debug_after_approval_.clear(); - debug_filtered_objects_.current_lane.clear(); - debug_filtered_objects_.target_lane.clear(); - debug_filtered_objects_.other_lane.clear(); - debug_valid_path_.clear(); RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } @@ -1112,7 +1107,7 @@ bool NormalLaneChange::getLaneChangePaths( const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const { - object_debug_.clear(); + lane_change_debug_.collision_check_objects.clear(); if (current_lanes.empty() || target_lanes.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; @@ -1158,7 +1153,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto target_objects = getTargetObjects(current_lanes, target_lanes); - debug_filtered_objects_ = target_objects; + lane_change_debug_.filtered_objects = target_objects; const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1385,9 +1380,10 @@ bool NormalLaneChange::getLaneChangePaths( std::vector filtered_objects = filterObjectsInTargetLane(target_objects, target_lanes); if ( - !is_stuck && utils::lane_change::passParkedObject( - route_handler, *candidate_path, filtered_objects, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_, object_debug_)) { + !is_stuck && + utils::lane_change::passParkedObject( + route_handler, *candidate_path, filtered_objects, lane_change_buffer, is_goal_in_route, + *lane_change_parameters_, lane_change_debug_.collision_check_objects)) { debug_print( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); @@ -1400,7 +1396,8 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, rss_params, is_stuck, object_debug_); + *candidate_path, target_objects, rss_params, is_stuck, + lane_change_debug_.collision_check_objects); if (is_safe) { debug_print("ACCEPT!!!: it is valid and safe!"); @@ -1423,7 +1420,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto & target_lanes = status_.target_lanes; const auto target_objects = getTargetObjects(current_lanes, target_lanes); - debug_filtered_objects_ = target_objects; + lane_change_debug_.filtered_objects = target_objects; CollisionCheckDebugMap debug_data; const bool is_stuck = isVehicleStuck(current_lanes); @@ -1431,16 +1428,17 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { // only for debug purpose - object_debug_.clear(); - object_debug_lifetime_ += (stop_watch_.toc(getModuleTypeStr()) / 1000); - if (object_debug_lifetime_ > 2.0) { + lane_change_debug_.collision_check_objects.clear(); + lane_change_debug_.collision_check_object_debug_lifetime += + (stop_watch_.toc(getModuleTypeStr()) / 1000); + if (lane_change_debug_.collision_check_object_debug_lifetime > 2.0) { stop_watch_.toc(getModuleTypeStr(), true); - object_debug_lifetime_ = 0.0; - object_debug_after_approval_.clear(); + lane_change_debug_.collision_check_object_debug_lifetime = 0.0; + lane_change_debug_.collision_check_objects_after_approval.clear(); } if (!safety_status.is_safe) { - object_debug_after_approval_ = debug_data; + lane_change_debug_.collision_check_objects_after_approval = debug_data; } } @@ -1802,7 +1800,7 @@ bool NormalLaneChange::isVehicleStuck( using lanelet::utils::getArcCoordinates; const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; - for (const auto & object : debug_filtered_objects_.current_lane) { + for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point // Note: it needs chattering prevention. diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 719acba405a68..523b770734bc5 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -120,4 +121,47 @@ MarkerArray showFilteredObjects( marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end()); return marker_array; } + +MarkerArray createDebugMarkerArray(const Debug & debug_data) +{ + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; + using marker_utils::lane_change_markers::showAllValidLaneChangePath; + using marker_utils::lane_change_markers::showFilteredObjects; + + const auto & debug_collision_check_object = debug_data.collision_check_objects; + const auto & debug_collision_check_object_after_approval = + debug_data.collision_check_objects_after_approval; + const auto & debug_valid_paths = debug_data.valid_paths; + const auto & debug_filtered_objects = debug_data.filtered_objects; + + MarkerArray debug_marker; + const auto add = [&debug_marker](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker); + }; + + add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); + add(showFilteredObjects( + debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, + debug_filtered_objects.other_lane, "object_filtered")); + + if (!debug_collision_check_object.empty()) { + add(showSafetyCheckInfo(debug_collision_check_object, "collision_check_object_info")); + add(showPredictedPath(debug_collision_check_object, "ego_predicted_path")); + add(showPolygon(debug_collision_check_object, "ego_and_target_polygon_relation")); + } + + if (!debug_collision_check_object_after_approval.empty()) { + add(showSafetyCheckInfo( + debug_collision_check_object_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath( + debug_collision_check_object_after_approval, "ego_predicted_path_after_approval")); + add(showPolygon( + debug_collision_check_object_after_approval, + "ego_and_target_polygon_relation_after_approval")); + } + + return debug_marker; +} } // namespace marker_utils::lane_change_markers From daf082474c3e286949f132ed7b0b5ad02b65d224 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 29 Jan 2024 14:01:37 +0900 Subject: [PATCH 142/154] fix(tier4_perception_launch): fix a bug in #6159 (#6203) Signed-off-by: kminoda --- launch/tier4_perception_launch/launch/perception.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 3bd3aff10a4e4..e8c19382864ba 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -16,8 +16,8 @@ - - + + From f9ed5b0ff141c0aaac62b523129282a925437430 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 29 Jan 2024 15:06:46 +0900 Subject: [PATCH 143/154] fix(tracking_object_merger): enable to association unknown class in tracker merger (#6182) * fix: enable to association unknown class in tracker merger Signed-off-by: yoshiri * chore: disable warning messages caused by unknown association Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../config/data_association_matrix.param.yaml | 6 +++--- perception/tracking_object_merger/src/utils/utils.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/tracking_object_merger/config/data_association_matrix.param.yaml index 702809b3cede1..c232dbde40b51 100644 --- a/perception/tracking_object_merger/config/data_association_matrix.param.yaml +++ b/perception/tracking_object_merger/config/data_association_matrix.param.yaml @@ -3,7 +3,7 @@ lidar-lidar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS @@ -59,7 +59,7 @@ lidar-radar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS @@ -115,7 +115,7 @@ radar-radar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 81f63538f9e22..85ab105e61038 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -484,9 +484,9 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su { if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { // warning - std::cerr << "[object_tracking_merger]: motion direction is different in " - "updateWholeTrackedObject function." - << std::endl; + // std::cerr << "[object_tracking_merger]: motion direction is different in " + // "updateWholeTrackedObject function." + // << std::endl; } main_obj = sub_obj; } From f6e4550c9de48136ba809d5de5e96d0473a11a4b Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 29 Jan 2024 16:25:09 +0900 Subject: [PATCH 144/154] refactor(ndt_scan_matcher): rename de-grounded (#6186) * refactor(ndt_scan_matcher): rename de-grounded Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/README.md | 18 ++++++++---------- .../config/ndt_scan_matcher.param.yaml | 5 ++--- .../ndt_scan_matcher/ndt_scan_matcher_core.hpp | 3 +-- .../src/ndt_scan_matcher_core.cpp | 9 ++++----- 4 files changed, 15 insertions(+), 20 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 95a7cebdc01c8..7956a663f92d5 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -31,13 +31,13 @@ One optional function is regularization. Please see the regularization chapter i | `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | | `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | -| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] de-grounded pointcloud aligned by scan matching | +| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | | `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | | `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | | `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | | `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | -| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | +| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | | `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | | `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | | `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | @@ -228,21 +228,19 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample | single file | at once (standard) | | multiple files | dynamically | -## Scan matching score based on de-grounded LiDAR scan +## Scan matching score based on no ground LiDAR scan ### Abstract -This is a function that uses de-grounded LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. +This is a function that uses no ground LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. [related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044). ### Parameters - - -| Name | Type | Description | -| ------------------------------------- | ------ | ------------------------------------------------------------------------------------- | -| `estimate_scores_for_degrounded_scan` | bool | Flag for using scan matching score based on de-grounded LiDAR scan (FALSE by default) | -| `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | +| Name | Type | Description | +| ------------------------------------- | ------ | ----------------------------------------------------------------------------------- | +| `estimate_scores_by_no_ground_points` | bool | Flag for using scan matching score based on no ground LiDAR scan (FALSE by default) | +| `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | ## 2D real-time covariance estimation 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 a5a4941bfec62..d4c49b7e8eec5 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -95,9 +95,8 @@ # Radius of input LiDAR range (used for diagnostics of dynamic map loading) lidar_radius: 100.0 - # cspell: ignore degrounded - # A flag for using scan matching score based on de-grounded LiDAR scan - estimate_scores_for_degrounded_scan: false + # A flag for using scan matching score based on no ground LiDAR scan + estimate_scores_by_no_ground_points: false # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 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 e2503c20aef6e..2883aa761444d 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 @@ -220,8 +220,7 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; - // cspell: ignore degrounded - bool estimate_scores_for_degrounded_scan_; + bool estimate_scores_by_no_ground_points_; double z_margin_for_ground_removal_; // The execution time which means probably NDT cannot matches scans properly 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 0382d805b7276..0278a62341981 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -64,7 +64,6 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } -// cspell: ignore degrounded NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), @@ -160,8 +159,8 @@ NDTScanMatcher::NDTScanMatcher() this->declare_parameter("initial_estimate_particles_num"); n_startup_trials_ = this->declare_parameter("n_startup_trials"); - estimate_scores_for_degrounded_scan_ = - this->declare_parameter("estimate_scores_for_degrounded_scan"); + estimate_scores_by_no_ground_points_ = + this->declare_parameter("estimate_scores_by_no_ground_points"); z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); @@ -526,8 +525,8 @@ void NDTScanMatcher::callback_sensor_points( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_in_map_ptr); - // whether use de-grounded points calculate score - if (estimate_scores_for_degrounded_scan_) { + // whether use no ground points to calculate score + if (estimate_scores_by_no_ground_points_) { // remove ground pcl::shared_ptr> no_ground_points_in_map_ptr( new pcl::PointCloud); From d57381b6da871c435f41c00c247aa6e226c37bca Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Mon, 29 Jan 2024 16:40:29 +0900 Subject: [PATCH 145/154] fix(simple_object_merger): change the default param of timeout_threshold (#6133) * fix(simple_object_merger): change the default param of timeout_threshold Signed-off-by: scepter914 * fix conflict Signed-off-by: scepter914 --------- Signed-off-by: scepter914 --- .../simple_object_merger/launch/simple_object_merger.launch.xml | 2 +- .../src/simple_object_merger_node/simple_object_merger_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/simple_object_merger/launch/simple_object_merger.launch.xml b/perception/simple_object_merger/launch/simple_object_merger.launch.xml index 14fca01fa6438..ccd038c23b257 100644 --- a/perception/simple_object_merger/launch/simple_object_merger.launch.xml +++ b/perception/simple_object_merger/launch/simple_object_merger.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 693fccb7e937c..683c6921eef5b 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -82,7 +82,7 @@ SimpleObjectMergerNode::SimpleObjectMergerNode(const rclcpp::NodeOptions & node_ // Node Parameter node_param_.update_rate_hz = declare_parameter("update_rate_hz", 20.0); node_param_.new_frame_id = declare_parameter("new_frame_id", "base_link"); - node_param_.timeout_threshold = declare_parameter("timeout_threshold", 1.0); + node_param_.timeout_threshold = declare_parameter("timeout_threshold", 0.1); declare_parameter("input_topics", std::vector()); node_param_.topic_names = get_parameter("input_topics").as_string_array(); From 43100454f9b49c24773f26d8870038f7fdd73129 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 29 Jan 2024 16:44:55 +0900 Subject: [PATCH 146/154] feat(intersection, blind_spot): add objects of interest marker (#6201) Signed-off-by: Mamoru Sobue --- planning/behavior_velocity_blind_spot_module/src/scene.cpp | 4 +++- planning/behavior_velocity_blind_spot_module/src/scene.hpp | 2 +- .../src/scene_intersection_collision.cpp | 5 ++++- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index efbff7e2af51d..54314e50308ff 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -348,7 +348,7 @@ bool BlindSpotModule::checkObstacleInBlindSpot( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const + const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) { /* get detection area */ if (turn_direction_ == TurnDirection::INVALID) { @@ -409,6 +409,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot( exist_in_right_conflict_area || exist_in_opposite_conflict_area; if (exist_in_detection_area && exist_in_conflict_area) { debug_data_.conflicting_targets.objects.push_back(object); + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); return true; } } diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 6f8450568939c..2ca2fe7950989 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -113,7 +113,7 @@ class BlindSpotModule : public SceneModuleInterface lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const; + const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose); /** * @brief Create half lanelet diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 4f7e8b45d107f..4496df77134e2 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -621,6 +621,9 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( continue; } const auto & unsafe_info = object_info->is_unsafe().value(); + setObjectsOfInterestData( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose, + object_info->predicted_object().shape, ColorName::RED); // ========================================================================================== // if ego is over the pass judge lines, then the visualization as "too_late_objects" or // "misjudge_objects" is more important than that for "unsafe" @@ -910,7 +913,7 @@ bool IntersectionModule::checkCollision( } } object_ttc_time_array.layout.dim.at(0).size++; - const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & pos = object..position; const auto & shape = object.shape; object_ttc_time_array.data.insert( object_ttc_time_array.data.end(), From 685b5b68098cd0f19c1c9de04646949fb805b208 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 29 Jan 2024 17:17:26 +0900 Subject: [PATCH 147/154] fix(tracking_object_merger): fix bug and rework parameters (#6168) * chore(tracking_object_merger): use config Signed-off-by: kminoda * fix(tracking_object_merger): fix bug and use param file Signed-off-by: kminoda * Update perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml --------- Signed-off-by: kminoda Co-authored-by: Yoshi Ri --- .../config/decorative_tracker_merger.param.yaml | 2 +- .../launch/decorative_tracker_merger.launch.xml | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml index 4a108be657a27..92a7fca229818 100644 --- a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml +++ b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml @@ -23,4 +23,4 @@ # logging settings enable_logging: false - log_file_path: "/tmp/decorative_tracker_merger.log" + logging_file_path: "association_log.json" diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml index 5affbe474e8ae..52b0841f97e97 100644 --- a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml +++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -5,8 +5,6 @@ - - @@ -15,7 +13,5 @@ - - From 2f761e1b0382036b76ba6eb584b28b1a66cf070c Mon Sep 17 00:00:00 2001 From: TaikiYamada4 <129915538+TaikiYamada4@users.noreply.github.com> Date: Mon, 29 Jan 2024 18:56:58 +0900 Subject: [PATCH 148/154] feat(imu_corrector): changed publication algorithm and content of /diagnostics in gyro_bias_estimator (#6139) * Let diagnostics be updated even if expections occur in timer_callback Signed-off-by: TaikiYamada4 * Fixed the summary message to be "Not updated" when the gyro bias is not updated. Fixed typo. Signed-off-by: TaikiYamada4 * Removed force_update() since updater_ originaly updates diagnostics automatically. Set the update peirod of diagnostics to be same to the timer_callback. Changed words of the diagnostics message a bit. Signed-off-by: TaikiYamada4 * style(pre-commit): autofix * Let the period of diagnostics publication independent to timer. Let update_diagnostics to be simple. Signed-off-by: TaikiYamada4 * style(pre-commit): autofix * Fixed typo Signed-off-by: TaikiYamada4 * style(pre-commit): autofix * Added setPeriod after force_update to reset the timer of updater_. Then, there will be no duplicates of diagnostics. Signed-off-by: TaikiYamada4 * style(pre-commit): autofix * Set diagnostics values to have the same precision Added gyro_bias_threshold_ to the diagnostics Signed-off-by: TaikiYamada4 * Updated README.md to match the paramters in gyro_bias_estimator Signed-off-by: TaikiYamada4 * style(pre-commit): autofix --------- Signed-off-by: TaikiYamada4 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/imu_corrector/README.md | 6 +- .../config/gyro_bias_estimator.param.yaml | 1 + .../imu_corrector/src/gyro_bias_estimator.cpp | 99 +++++++++++++------ .../imu_corrector/src/gyro_bias_estimator.hpp | 16 +++ 4 files changed, 89 insertions(+), 33 deletions(-) diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index 7af82c1129aff..2df12c94b7d3b 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -70,11 +70,11 @@ In the future, with careful implementation for pose errors, the IMU bias estimat ### Parameters +Note that this node also uses `angular_velocity_offset_x`, `angular_velocity_offset_y`, `angular_velocity_offset_z` parameters from `imu_corrector.param.yaml`. + | Name | Type | Description | | ------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | -| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | -| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | -| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | | `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | | `timer_callback_interval_sec` | double | seconds about the timer callback function [sec] | +| `diagnostics_updater_interval_sec` | double | period of the diagnostics updater [sec] | | `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml index e10329c920137..eac423f94fa78 100644 --- a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -2,4 +2,5 @@ ros__parameters: gyro_bias_threshold: 0.0015 # [rad/s] timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 50e4a702ec949..e99667ed1c4a6 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -30,6 +30,7 @@ GyroBiasEstimator::GyroBiasEstimator() angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), timer_callback_interval_sec_(declare_parameter("timer_callback_interval_sec")), + diagnostics_updater_interval_sec_(declare_parameter("diagnostics_updater_interval_sec")), straight_motion_ang_vel_upper_limit_( declare_parameter("straight_motion_ang_vel_upper_limit")), updater_(this), @@ -37,6 +38,8 @@ GyroBiasEstimator::GyroBiasEstimator() { updater_.setHardwareID(get_name()); updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); + // diagnostic_updater is designed to be updated at the same rate as the timer + updater_.setPeriod(diagnostics_updater_interval_sec_); gyro_bias_estimation_module_ = std::make_unique(); @@ -57,6 +60,18 @@ GyroBiasEstimator::GyroBiasEstimator() this->get_node_timers_interface()->add_timer(timer_, nullptr); transform_listener_ = std::make_shared(this); + + // initialize diagnostics_info_ + { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_info_.summary_message = "Not initialized"; + diagnostics_info_.gyro_bias_x_for_imu_corrector = std::nan(""); + diagnostics_info_.gyro_bias_y_for_imu_corrector = std::nan(""); + diagnostics_info_.gyro_bias_z_for_imu_corrector = std::nan(""); + diagnostics_info_.estimated_gyro_bias_x = std::nan(""); + diagnostics_info_.estimated_gyro_bias_y = std::nan(""); + diagnostics_info_.estimated_gyro_bias_z = std::nan(""); + } } void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) @@ -99,6 +114,7 @@ void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_pt void GyroBiasEstimator::timer_callback() { if (pose_buf_.empty()) { + diagnostics_info_.summary_message = "Skipped update (pose_buf is empty)"; return; } @@ -112,6 +128,7 @@ void GyroBiasEstimator::timer_callback() const rclcpp::Time t0_rclcpp_time = rclcpp::Time(pose_buf.front().header.stamp); const rclcpp::Time t1_rclcpp_time = rclcpp::Time(pose_buf.back().header.stamp); if (t1_rclcpp_time <= t0_rclcpp_time) { + diagnostics_info_.summary_message = "Skipped update (pose_buf is not in chronological order)"; return; } @@ -127,6 +144,7 @@ void GyroBiasEstimator::timer_callback() // Check gyro data size // Data size must be greater than or equal to 2 since the time difference will be taken later if (gyro_filtered.size() <= 1) { + diagnostics_info_.summary_message = "Skipped update (gyro_filtered size is less than 2)"; return; } @@ -140,6 +158,8 @@ void GyroBiasEstimator::timer_callback() const double yaw_vel = yaw_diff / time_diff; const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); if (!is_straight) { + diagnostics_info_.summary_message = + "Skipped update (yaw angular velocity is greater than straight_motion_ang_vel_upper_limit)"; return; } @@ -151,12 +171,45 @@ void GyroBiasEstimator::timer_callback() if (!tf_base2imu_ptr) { RCLCPP_ERROR( this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); + + diagnostics_info_.summary_message = "Skipped update (tf between base and imu is not available)"; return; } + gyro_bias_ = transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); + validate_gyro_bias(); updater_.force_update(); + updater_.setPeriod(diagnostics_updater_interval_sec_); // to reset timer inside the updater +} + +void GyroBiasEstimator::validate_gyro_bias() +{ + // Calculate diagnostics key-values + diagnostics_info_.gyro_bias_x_for_imu_corrector = gyro_bias_.value().x; + diagnostics_info_.gyro_bias_y_for_imu_corrector = gyro_bias_.value().y; + diagnostics_info_.gyro_bias_z_for_imu_corrector = gyro_bias_.value().z; + diagnostics_info_.estimated_gyro_bias_x = gyro_bias_.value().x - angular_velocity_offset_x_; + diagnostics_info_.estimated_gyro_bias_y = gyro_bias_.value().y - angular_velocity_offset_y_; + diagnostics_info_.estimated_gyro_bias_z = gyro_bias_.value().z - angular_velocity_offset_z_; + + // Validation + const bool is_bias_small_enough = + std::abs(diagnostics_info_.estimated_gyro_bias_x) < gyro_bias_threshold_ && + std::abs(diagnostics_info_.estimated_gyro_bias_y) < gyro_bias_threshold_ && + std::abs(diagnostics_info_.estimated_gyro_bias_z) < gyro_bias_threshold_; + + // Update diagnostics + if (is_bias_small_enough) { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_info_.summary_message = "Successfully updated"; + } else { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diagnostics_info_.summary_message = + "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in imu_corrector. " + "You may also use the output of gyro_bias_estimator."; + } } geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( @@ -172,36 +225,22 @@ geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) { - if (gyro_bias_ == std::nullopt) { - stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); - } else { - stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x); - stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y); - stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z); - - stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_); - stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_); - stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_); - - // Validation - const bool is_bias_small_enough = - std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ && - std::abs(gyro_bias_.value().y - angular_velocity_offset_y_) < gyro_bias_threshold_ && - std::abs(gyro_bias_.value().z - angular_velocity_offset_z_) < gyro_bias_threshold_; - - // Update diagnostics - if (is_bias_small_enough) { - stat.add("gyro_bias", "OK"); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); - } else { - stat.add( - "gyro_bias", - "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in " - "imu_corrector. You may also use the output of gyro_bias_estimator."); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "WARN"); - } - } + auto f = [](const double & value) { + std::stringstream ss; + ss << std::fixed << std::setprecision(8) << value; + return ss.str(); + }; + + stat.summary(diagnostics_info_.level, diagnostics_info_.summary_message); + stat.add("gyro_bias_x_for_imu_corrector", f(diagnostics_info_.gyro_bias_x_for_imu_corrector)); + stat.add("gyro_bias_y_for_imu_corrector", f(diagnostics_info_.gyro_bias_y_for_imu_corrector)); + stat.add("gyro_bias_z_for_imu_corrector", f(diagnostics_info_.gyro_bias_z_for_imu_corrector)); + + stat.add("estimated_gyro_bias_x", f(diagnostics_info_.estimated_gyro_bias_x)); + stat.add("estimated_gyro_bias_y", f(diagnostics_info_.estimated_gyro_bias_y)); + stat.add("estimated_gyro_bias_z", f(diagnostics_info_.estimated_gyro_bias_z)); + + stat.add("gyro_bias_threshold", f(gyro_bias_threshold_)); } } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 821cba0b213ff..3592ff1f7d3b4 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -49,6 +49,7 @@ class GyroBiasEstimator : public rclcpp::Node void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr); void timer_callback(); + void validate_gyro_bias(); static geometry_msgs::msg::Vector3 transform_vector3( const geometry_msgs::msg::Vector3 & vec, @@ -68,6 +69,7 @@ class GyroBiasEstimator : public rclcpp::Node const double angular_velocity_offset_y_; const double angular_velocity_offset_z_; const double timer_callback_interval_sec_; + const double diagnostics_updater_interval_sec_; const double straight_motion_ang_vel_upper_limit_; diagnostic_updater::Updater updater_; @@ -80,6 +82,20 @@ class GyroBiasEstimator : public rclcpp::Node std::vector gyro_all_; std::vector pose_buf_; + + struct DiagnosticsInfo + { + unsigned char level; + std::string summary_message; + double gyro_bias_x_for_imu_corrector; + double gyro_bias_y_for_imu_corrector; + double gyro_bias_z_for_imu_corrector; + double estimated_gyro_bias_x; + double estimated_gyro_bias_y; + double estimated_gyro_bias_z; + }; + + DiagnosticsInfo diagnostics_info_; }; } // namespace imu_corrector From e67ab70f27eaae9f3a0b54b73610737929003c67 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Mon, 29 Jan 2024 11:41:19 +0100 Subject: [PATCH 149/154] build(yabloc_pose_initializer): fix dependencies (#6190) Signed-off-by: Esteve Fernandez --- localization/yabloc/yabloc_pose_initializer/CMakeLists.txt | 4 ++++ localization/yabloc/yabloc_pose_initializer/package.xml | 2 ++ 2 files changed, 6 insertions(+) diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index 164f280becae8..91b272b413c4c 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -14,6 +14,9 @@ find_package(PCL REQUIRED COMPONENTS common kdtree) # Sophus find_package(Sophus REQUIRED) +# OpenCV +find_package(OpenCV REQUIRED) + # =================================================== # Executable # Camera @@ -28,6 +31,7 @@ ament_auto_add_executable(${TARGET} target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) +ament_target_dependencies(${TARGET} OpenCV) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index e9921db50093e..afd0d4aa86bcf 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -17,8 +17,10 @@ rosidl_default_generators autoware_auto_mapping_msgs + cv_bridge geometry_msgs lanelet2_extension + libopencv-dev rclcpp sensor_msgs tier4_localization_msgs From 59e869666a985e1bd63f1389a0176c2909c7aa6e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 29 Jan 2024 22:15:57 +0900 Subject: [PATCH 150/154] fix(intersection): judge pass judge at intersection without tl with occlusion detection (#6207) Signed-off-by: Mamoru Sobue --- .../src/decision_result.cpp | 14 ++++---- .../src/scene_intersection.cpp | 33 ++++++++++++++++--- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp index 93a7c2a29d2f7..aefd59a72f9b4 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -19,11 +19,11 @@ namespace behavior_velocity_planner::intersection std::string formatDecisionResult(const DecisionResult & decision_result) { if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "InternalError because " + state.error; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + state.evasive_report; } @@ -31,11 +31,11 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "StuckStop"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "YieldStuckStop:\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { @@ -45,18 +45,18 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "PeekingTowardOcclusion"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "OccludedCollisionStop\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { return "Safe"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "FullyPrioritized\nsafety_report:" + state.safety_report; } return ""; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 75e2da034780a..3f6136673a44a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -354,8 +354,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( : false; if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return intersection::InternalError{ - "already passed maximum peeking line in the absence of traffic light"}; + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light.\n" + + safety_report, + evasive_diag}; + } + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light safely", + "no evasive action required"}; } return intersection::OccludedAbsenceTrafficLight{ is_occlusion_cleared_with_margin, @@ -364,7 +374,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, first_attention_stopline_idx, occlusion_wo_tl_pass_judge_line_idx, - safety_report}; + safety_diag}; } // ========================================================================================== @@ -1251,7 +1261,22 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat return first_pass_judge_line_idx; }(); - const bool was_safe = std::holds_alternative(prev_decision_result_); + // ========================================================================================== + // at intersection without traffic light, this module ignores occlusion even if occlusion is + // detected for real, so if collision is not detected in that context, that should be interpreted + // as "was_safe" + // ========================================================================================== + const bool was_safe = [&]() { + if (std::holds_alternative(prev_decision_result_)) { + return true; + } + if (std::holds_alternative(prev_decision_result_)) { + const auto & state = + std::get(prev_decision_result_); + return !state.collision_detected; + } + return false; + }(); const bool is_over_1st_pass_judge_line = util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); From 4f7fded30f48d3017dcf8da291fdbd429bdb130a Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Mon, 29 Jan 2024 22:25:57 +0900 Subject: [PATCH 151/154] chore(simple_object_merger): fix README (#6202) * chore(simple_object_merger): fix README Signed-off-by: scepter914 * style(pre-commit): autofix --------- Signed-off-by: scepter914 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/simple_object_merger/README.md | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/perception/simple_object_merger/README.md b/perception/simple_object_merger/README.md index 9bcd03e8abfa1..4c037963361a1 100644 --- a/perception/simple_object_merger/README.md +++ b/perception/simple_object_merger/README.md @@ -1,23 +1,21 @@ # simple_object_merger -This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) without low calculation cost. +This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) with low calculation cost. -## Algorithm +## Design ### Background [Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) is mainly used for merge process with DetectedObjects. There are 2 characteristics in `Object_merger`. First, `object_merger` solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost. Second, `object_merger` can handle only 2 DetectedObjects topics and cannot handle more than 2 topics in one node. To merge 6 DetectedObjects topics, 6 `object_merger` nodes need to stand for now. -So `simple_object_merger` aim to merge multiple DetectedObjects with low calculation cost. +Therefore, `simple_object_merger` aim to merge multiple DetectedObjects with low calculation cost. The package do not use data association algorithm to reduce the computational cost, and it can handle more than 2 topics in one node to prevent launching a large number of nodes. ### Use case -Use case is as below. - - Multiple radar detection -`Simple_object_merger` can be used for multiple radar detection. By combining them into one topic from multiple radar topics with `simple_object_merger`, the pipeline for faraway detection with radar is simpler. +`Simple_object_merger` can be used for multiple radar detection. By combining them into one topic from multiple radar topics, the pipeline for faraway detection with radar can be simpler. ### Limitation From ba21eecc260051a0d7c7e8d0108f833d75d58192 Mon Sep 17 00:00:00 2001 From: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Date: Mon, 29 Jan 2024 17:49:49 +0300 Subject: [PATCH 152/154] feat: new rviz ui - speed/gear/turnsignal/steering (#5957) Signed-off-by: Khalil Selyan --- .../awf_2d_overlay_vehicle/CMakeLists.txt | 140 +++++ .../awf_2d_overlay_vehicle/LICENSE | 12 + .../awf_2d_overlay_vehicle/README.md | 54 ++ .../assets/font/Quicksand/LICENSE | 93 +++ .../font/Quicksand/static/Quicksand-Bold.ttf | Bin 0 -> 78596 bytes .../font/Quicksand/static/Quicksand-Light.ttf | Bin 0 -> 78660 bytes .../Quicksand/static/Quicksand-Medium.ttf | Bin 0 -> 78948 bytes .../Quicksand/static/Quicksand-Regular.ttf | Bin 0 -> 78936 bytes .../Quicksand/static/Quicksand-SemiBold.ttf | Bin 0 -> 78820 bytes .../assets/images/arrow.png | Bin 0 -> 260 bytes .../assets/images/select_add.png | Bin 0 -> 18359 bytes .../assets/images/select_topic_name.png | Bin 0 -> 422591 bytes .../assets/images/select_vehicle_plugin.png | Bin 0 -> 72553 bytes .../assets/images/traffic.png | Bin 0 -> 622 bytes .../assets/images/wheel.png | Bin 0 -> 1632 bytes .../awf_2d_overlay_vehicle-extras.cmake | 31 + .../include/gear_display.hpp | 49 ++ .../include/overlay_text_display.hpp | 157 +++++ .../include/overlay_utils.hpp | 141 +++++ .../include/signal_display.hpp | 124 ++++ .../include/speed_display.hpp | 49 ++ .../include/speed_limit_display.hpp | 49 ++ .../include/steering_wheel_display.hpp | 54 ++ .../include/traffic_display.hpp | 62 ++ .../include/turn_signals_display.hpp | 63 ++ .../awf_2d_overlay_vehicle/package.xml | 31 + .../plugins_description.xml | 5 + .../src/gear_display.cpp | 98 +++ .../src/overlay_text_display.cpp | 556 ++++++++++++++++++ .../src/overlay_utils.cpp | 267 +++++++++ .../src/signal_display.cpp | 501 ++++++++++++++++ .../src/speed_display.cpp | 109 ++++ .../src/speed_limit_display.cpp | 105 ++++ .../src/steering_wheel_display.cpp | 123 ++++ .../src/traffic_display.cpp | 113 ++++ .../src/turn_signals_display.cpp | 122 ++++ .../rviz_2d_overlay_msgs/CHANGELOG.rst | 24 + .../rviz_2d_overlay_msgs/CMakeLists.txt | 19 + .../rviz_2d_overlay_msgs/msg/OverlayText.msg | 31 + .../rviz_2d_overlay_msgs/package.xml | 24 + 40 files changed, 3206 insertions(+) create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Regular.ttf create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-SemiBold.ttf create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/arrow.png create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/awf_2d_overlay_vehicle-extras.cmake create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp create mode 100644 common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp create mode 100644 common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst create mode 100644 common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt create mode 100644 common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg create mode 100644 common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt new file mode 100644 index 0000000000000..da67a6f63aeae --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt @@ -0,0 +1,140 @@ +cmake_minimum_required(VERSION 3.8) +project(awf_2d_overlay_vehicle) + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(autoware_auto_vehicle_msgs REQUIRED) +find_package(tier4_planning_msgs REQUIRED) +find_package(autoware_perception_msgs REQUIRED) +ament_auto_find_build_dependencies() + +find_package(rviz_2d_overlay_msgs REQUIRED) + +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(std_msgs REQUIRED) + +set( + headers_to_moc + include/overlay_text_display.hpp + include/signal_display.hpp + +) + +set( + headers_to_not_moc + include/steering_wheel_display.hpp + include/gear_display.hpp + include/speed_display.hpp + include/turn_signals_display.hpp + include/traffic_display.hpp + include/speed_limit_display.hpp +) + + + +foreach(header "${headers_to_moc}") + qt5_wrap_cpp(display_moc_files "${header}") +endforeach() + +set( + display_source_files + src/overlay_text_display.cpp + src/overlay_utils.cpp + src/signal_display.cpp + src/steering_wheel_display.cpp + src/gear_display.cpp + src/speed_display.cpp + src/turn_signals_display.cpp + src/traffic_display.cpp + src/speed_limit_display.cpp + +) + +add_library( + ${PROJECT_NAME} SHARED + ${display_moc_files} + ${headers_to_not_moc} + ${display_source_files} +) + +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) +target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic) + +target_include_directories( + ${PROJECT_NAME} PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +target_link_libraries( + ${PROJECT_NAME} PUBLIC + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay +) + + +target_compile_definitions(${PROJECT_NAME} PRIVATE "${PROJECT_NAME}_BUILDING_LIBRARY") + +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +ament_target_dependencies( + ${PROJECT_NAME} + PUBLIC + rviz_common + rviz_rendering + rviz_2d_overlay_msgs + autoware_auto_vehicle_msgs + tier4_planning_msgs + autoware_perception_msgs +) + +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rviz_common + rviz_ogre_vendor +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + DESTINATION lib/${PROJECT_NAME} +) + +# Copy the assets directory to the installation directory +install( + DIRECTORY assets/ + DESTINATION share/${PROJECT_NAME}/assets +) + +add_definitions(-DQT_NO_KEYWORDS) + +ament_package( + CONFIG_EXTRAS "awf_2d_overlay_vehicle-extras.cmake" +) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE new file mode 100644 index 0000000000000..a2fe2d3d1c7ed --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE @@ -0,0 +1,12 @@ +Copyright (c) 2022, Team Spatzenhirn +Copyright (c) 2014, JSK Lab + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md new file mode 100644 index 0000000000000..6728f26878e10 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md @@ -0,0 +1,54 @@ +# awf_2d_overlay_vehicle + +Plugin for displaying 2D overlays over the RViz2 3D scene. + +Based on the [jsk_visualization](https://github.com/jsk-ros-pkg/jsk_visualization) +package, under the 3-Clause BSD license. + +## Purpose + +This plugin provides a visual and easy-to-understand display of vehicle speed, turn signal, steering status and gears. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------------------- | ------------------------------------------------------- | ---------------------------------- | +| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | +| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/hazard_status` | `autoware_auto_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | +| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic is status of gear | + +## Parameter + +### Core Parameters + +#### SignalDisplay + +| Name | Type | Default Value | Description | +| ------------------------ | ------ | -------------------- | --------------------------------- | +| `property_width_` | int | 128 | Width of the plotter window [px] | +| `property_height_` | int | 128 | Height of the plotter window [px] | +| `property_left_` | int | 128 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_signal_color_` | QColor | QColor(25, 255, 240) | Turn Signal color | + +## Assumptions / Known limits + +TBD. + +## Usage + +1. Start rviz and select Add under the Displays panel. + + ![select_add](./assets/images/select_add.png) + +2. Select any one of the tier4_vehicle_rviz_plugin and press OK. + + ![select_vehicle_plugin](./assets/images/select_vehicle_plugin.png) + +3. Enter the name of the topic where you want to view the status. + + ![select_topic_name](./assets/images/select_topic_name.png) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE new file mode 100644 index 0000000000000..cc04d393573f0 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE @@ -0,0 +1,93 @@ +Copyright 2011 The Quicksand Project Authors (https://github.com/andrew-paglinawan/QuicksandFamily), with Reserved Font Name “Quicksand”. + +This Font Software is licensed under the SIL Open Font License, Version 1.1. +This license is copied below, and is also available with a FAQ at: +http://scripts.sil.org/OFL + + +----------------------------------------------------------- +SIL OPEN FONT LICENSE Version 1.1 - 26 February 2007 +----------------------------------------------------------- + +PREAMBLE +The goals of the Open Font License (OFL) are to stimulate worldwide +development of collaborative font projects, to support the font creation +efforts of academic and linguistic communities, and to provide a free and +open framework in which fonts may be shared and improved in partnership +with others. + +The OFL allows the licensed fonts to be used, studied, modified and +redistributed freely as long as they are not sold by themselves. The +fonts, including any derivative works, can be bundled, embedded, +redistributed and/or sold with any software provided that any reserved +names are not used by derivative works. The fonts and derivatives, +however, cannot be released under any other type of license. The +requirement for fonts to remain under this license does not apply +to any document created using the fonts or their derivatives. + +DEFINITIONS +"Font Software" refers to the set of files released by the Copyright +Holder(s) under this license and clearly marked as such. This may +include source files, build scripts and documentation. + +"Reserved Font Name" refers to any names specified as such after the +copyright statement(s). + +"Original Version" refers to the collection of Font Software components as +distributed by the Copyright Holder(s). + +"Modified Version" refers to any derivative made by adding to, deleting, +or substituting -- in part or in whole -- any of the components of the +Original Version, by changing formats or by porting the Font Software to a +new environment. + +"Author" refers to any designer, engineer, programmer, technical +writer or other person who contributed to the Font Software. + +PERMISSION & CONDITIONS +Permission is hereby granted, free of charge, to any person obtaining +a copy of the Font Software, to use, study, copy, merge, embed, modify, +redistribute, and sell modified and unmodified copies of the Font +Software, subject to the following conditions: + +1) Neither the Font Software nor any of its individual components, +in Original or Modified Versions, may be sold by itself. + +2) Original or Modified Versions of the Font Software may be bundled, +redistributed and/or sold with any software, provided that each copy +contains the above copyright notice and this license. These can be +included either as stand-alone text files, human-readable headers or +in the appropriate machine-readable metadata fields within text or +binary files as long as those fields can be easily viewed by the user. + +3) No Modified Version of the Font Software may use the Reserved Font +Name(s) unless explicit written permission is granted by the corresponding +Copyright Holder. This restriction only applies to the primary font name as +presented to the users. + +4) The name(s) of the Copyright Holder(s) or the Author(s) of the Font +Software shall not be used to promote, endorse or advertise any +Modified Version, except to acknowledge the contribution(s) of the +Copyright Holder(s) and the Author(s) or with their explicit written +permission. + +5) The Font Software, modified or unmodified, in part or in whole, +must be distributed entirely under this license, and must not be +distributed under any other license. The requirement for fonts to +remain under this license does not apply to any document created +using the Font Software. + +TERMINATION +This license becomes null and void if any of the above conditions are +not met. + +DISCLAIMER +THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT +OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL THE +COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF THE USE OR INABILITY TO USE THE FONT SOFTWARE OR FROM +OTHER DEALINGS IN THE FONT SOFTWARE. diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf new file mode 100644 index 0000000000000000000000000000000000000000..07d5127c04b17a9a62121d12aeb00b701e920500 GIT binary patch literal 78596 zcmc${31C!3)<0TRx6_?{Upq;svqJ(Q>Fn%TSp_6)f+7I|34{bgSVd$I6_-KB!DSQ` z71wb<9Y+yyM+Z@HLq)~~5D^h~L`6xz-#K^b4xr=rfA9OhbgJ&Tr%s(Zb?Vf9Z(SjU z5Rvd~LJX;`tFL~!>TMxZ7O+Q#j2Yc9?bq<*LJaFKMCD^c8pcM&bk?!dBU^-6bxeqd!ke2KXC}?F6(js=gqJk~5jZB`LEIyKk<~n}bIDbkF9QFJ zF+zmi*4jR!G4#28o4~452>aN1jZ5Zh11Uo?1)jUDabD9+x4d$d5E{}9m^r_FLFbNn zAx{cX@Qo18Ve>nh<~Q4~z7+JKpm%JM-b){{{Ta7Hdc7h7SE2O~?ZP3h5rK5)_h%<6 zgdg_W!Mnj*+La$rcnXcsa+ykx8_S_7LM@)%+{w(8PA>!0Hs(x7O*d4n|0!RG|E3BSQiZ4}$PuUF0H>)m z`1h#$;6I=qfd3D*8U8=jNJ}0s9gqZ6DjHu0}c>Qb&~yADT2i&16Cp&rOR=p2AXvSY(t5yHsAnXUZh>P#4H2v5E-J@ zfCGhFWEpS}AS3TGnDceESSVV=3^7+M5RIZu%oM#vgJ=@-fUQRDX$9s?G-rWkCTJFd z@=W1l5k4ECt>D^mMv9S$JzF%xW=>7dpLcgYzBm)X-;7T@I>mhCps(nI-vZ>P1EKSQ zT>v{@0Js&`c3@{Ce6*++BgHtR-GVST?%I*_PCyNSW&v8vQa1skQ?%kb11YwFp7QNR z&PZ2H;OGW@Ga!mR41C?-Kc8v+Ig{i*4!uA%fn$?wl)76KK|*qFKgqhMYf9577C}DB zQ=^y;pVD58dy>Nq9HmC|M8b{q%U0*AB#;gYX&9zZ;rZ$Yn$JNA}j^ zwDOp3l-@#=r>Va_tW8scz0auYK4(b39`UI5wnFv-ln&WNPeB&vvk|IQWXT<{B;B5U zbzl_7r@UpHfqMqpk5aT2i_yyeRoo^1DP9n7i;u+@;ujf$);?eMlS5>KJYUY0i{&M9 zt^8CCRwLCUwC0bf6WRc6m^NN()Gp92(yq{+)Lzlv)edN1X+PTnY*DsUTW{M8Tf6N- z+vT$IXs=91lC5a_n%t?Kt50((zNE78nue49pHZFR(fA zhQK=mHw7LJJQfrelpfS8s3NE)Xmrq&pxHr-gH{DS8FV7p9^5y$K6q^K)ZjV6i-K1L zuMOT6{88|c;Nv0XA@f3(hFlu*mylaS{t@z2$ZH`VhI|?Fb7(+lTxfP^QRslsVWHzg zn?e_cUK)CR=-r_Yhdveha_IY^pNG|l-57RP*ygZj!d?&iIJ{^0)bPKC{~R$YVsgZ+ zh>nOA5m!ds5OHV3&yfLywqQj$|(K*qc=z-D0qbEeqie40bS@aFj_e4Ju{dDxs=)EyTF#}?T z#hep!S} z$6XtDOWXr-PsHtrdoS)#+;?$Z@nP|)@x9{v#gB-e5Z@R-FMetKrSaFs-;!WUh)i%N zg3es)CH+4Qm;r|pL%cVx2eCn z9Ikv|j2B(cko02vw?Qdzv+``?@J;XiMJ=NXfUg-Xl z`_Jy(?t|{H-6zrqrC*SKQTm$nb?F<@H>W?7zAOEs^smx?&ah>~WMpRa$>^6cB%>i? zM#h4SOEUhNac9P+j3+X-XS|(pAhU1g>db31Z^?Wh^NGwInY*)6vu0&&&f1rCH0#G~ zEjv6rDLX5>PxhtRf62Zh`@!rdvtP=7GyB8r&$EBX3CLNLvpHvL&PzFO=6sNIIOkZ7 zH#a!9ICoI)@Z9ma({ktLF3Me%yEgac+tH%dgBInm;jrdj7op<@s0T-<*Gc{+9d~@^|I$&Hpt2oBUsT*?V2t>-t_B zdu{HW)H|zppWc0Y*Y!TT_qn~BdoS;OQ}6qFKim6_-X9b=3+fA&7i=on-6x?>Tc6wd zd|lYTaAx7tMZrb!MO8(!ita8t=1KRodhYYQS?n%8x45aet$1@?vC5O4gLzRC0gG z){>ni|0y|I^6RO;()iM<()Q9#rB9b0DN87uQ}$HZdu893Pbq)6{H^jI%1>4VRD@N; zRiswrR1B=Bub5h~sA5IMWff~H?y1;R@mR&v6-O(bm93S}RUYme**CGTyKj%ag?%gf z4(>a&@3_9x`nLAHvG4PJclF)dPxQ;}H@e@fejWW*^t-Iz+J5i$pWc7>fP?{e42&AM zdf>i6p@U)v^%^vB(5gZE2d55h9sKd&&#H!0-C6CdzP0-Pn%J6UHJ8@>rRI~`3v2&V zmsU5w?(zEU`X%)ThBOa(V`$#c!9yE|UOn{gp?il#4;wJ-Ps1J_c3^ns@R`H!7@HSBHpwBgH!9~w@M3mBI?ZsfQ{*|bnCLW|atwJfcNR-hGWrP{NuAXkJd!IkPtcV(kh z=;JDP4RAHMrnp*M7q~7&`*4lxudW+ix4C|Hhqxo%ac-yE<<4;DxC`CI?!N9?_k8zK z_Z99t+#B8Zy6<=Y!~KZ+U+%5$r_y86Y%p@NL2j1YWX%yxZJKRSDvc?EnHt$wQI6#s;kwt#I@43)^#1{WP{s*oJ6}5-6?Lj zk&{Z!NvC@ya&njZ9^~Zj?uU_+C*0foaxw)u2|!Mwkdq`UCo_=~%&=7rW>}vhKf>Eb zs;>P6>3?1Q^s6_c>tnCPILPnM`?2>U@4MdDy}P_Sy*s=wc8&83{7vwaj00&DcwmM} za-i(My$9wTXgFZozYD(y4#0{IgdM<2fe`z@WqcF#E0PyUL_DOf&PFrYiqQ%#Et&6tF^V-wc2{^9&MA3 z@%Od+vB5m)})4sB6>`wN%YePoY-K!|1C? zO;%IXcJ-WkL7lIzM;%MWd@&ofs$LAiIJp61b$zV32qU|D#r@&|@rZav{2ODted1Gb zNPMG~t6AzI^@{3LC&dpkN=C~hnU1-AUs)yVI#`#mlcriP(l3xDIpV z%f+2yt$Idm5U-;hc}2V?J`?xJN2Eg>7hN)32FOquA>(AB%#b;(=8l_TU?(kU0o z%jHtJT;3=zQO{znV!M1wJ}aM>AE4#=NW^3Atcf2*f(#L9GDc*|1d$~Zg&QklJ!G=T zlc}Og7KEhkG1mjlF5*-wm=gTx3qP@FAm#b{YAX2^-6QH~cAM0g~v|LSU3hjN47NCAbTN$i5lvjPC4q|oa3w2a|t`4aE>PvM*eX0(N z3>h!-rAzdZX`(0A8f#>!sFo$7SZ0f}B)Mof{T#C(Z4lAJB($?0MtT8QQH z0&x*mu~y23Vwr3gcgdT?M)@~!8`iXLmDh>Qa-(=y-Xpfk&EiSu{b6_7tvVBiIss^UGs`CtO$im zB_e1A2;HJh{Dj`{1lBZv!J6i;ST+1v43K@qAXy~pu+mm4b49uADJo>1sFeAlO!g4x z%dz4-IY#_NUMBu3*N8RpB5|d>SX?DnimTM@V*Ycn`TQ#T&YOI>5E>P`i zKKkJ`>I$`5U8XKqm#PiwPBk4Xcn_(+s|VF)^)IzaJ+2!)NiP_(9b=o-bP>d zBHA)!Qv}P8u=c2G>qm~0n_3$?+n|LNBB*L|om)<>sT${&Wi?~rM~|qQ>=s|0HF~65 z>>M?+)-CQCJNhiQScS!Gx0pI^>F!jCjg76H z(leuRL6ht?W5&Gs(#?M4%+{9KGI(ZN`#foz)zLUZoB*Ox9K$|AtN4oj!|Wem{{!~l zW&aKKce1~o{ioX&&g&3c+B;^piH8=nw9OI^ELc!jBsMNsIDLV*ZNb9%3&c&!nmXFi zPmo>YuurSu64pR|F8igN3c)(ArclvKJ7(F)LrtU*binu8@`6# zDh2&6g*n;Jf=@dESjR={Nsj8cc~5kiz$m8e;jr6>2HX~a=}LgGy@X$X?IUfc^?L}T zk4|ltHVG^Af$AIl4yfIzQ;)!(qt3<8C4WUvT!0q%CG_oM!JSeF1plET9ple0#WS*KQ0;mOqR7=!FA`qjRKZ|g6Jw{y7=z~5LG3b-N7K!RxbzG#Y zpRl8ni(KilO0-70Zft5Ja^QU*= zXs?IjrNdX^63|cx?aXu!k>UrY(^DSkJ!%EBSd0l zl}d+vfu2Ol^XZ;_!7`X}sfJU!k7LyJAjVz(ISb-AWZ5S5C{Xu8+SrdmI*}q+eE|FV zLj4E+mufHkPt^PH5BgE;V~UTN;v=T`kSU-C*xTpeXmWR<#w>?L&Bw@XCPvJY&<>2m z9AGf|Nou_?szXS-xPZrWZaGVKsz|PxsE1T1Wj*}4&@JjJ`$-rRDzS)Q@SO?2h2a*} zN-$%xnI>C&XXRaC*EartRO56DPcf`A3!E?yVtx+TB0*2XQQ7Nls^nckokF*lGLeY2 z>2v-^sVZT|+@>CECLvdn>oDo`YvJHJ9LaTl^r@mJ|sN+OOd!S60fiPVI#8p{b zzd;WJTcBB*K}q)P##+QfVxz#QT3iOJ=)k_jRP1aF$L>gfk_s&Y?fX)_gWbdOL(tka z!Ag`Gs#>u7Ived?E2Qs<^_Kx^2%sLyqxvh@v&vOPs-LO@l!G;mzN!{bw(6rQRSlpl zRiG+VHK0ta-ju55nKqZ+dBN6H{>Cgm&-= z%n<%x_FZzcoUCHxCbn_y;JWWS>A_rM)ll3dX zmUHQ_R%hUricGB-a>1#Op+kp$CHQpb{Y?AL5C1*jACdO|ZTAV%BzH=>+P{ID0-4Bt z4|p@Kzvsq-=8w7~&`}`hJA~_Z-E&Cuf79I~T-s(4d8$L1_g3RX)E{zdM5a2`bs}&7 zo335wuiYkkkV9HkiuCzgj$|1Qx1DLb$!41b+^GLuhcz#kT7fiv%MFM8!@XZZM!J4Z zS8-I7!}Zmjcb7Qo{SxjQxXa+)(H+x(&ig*-B(ASvm4xnJfg}1SP^XXjJ5>prG8|g` z5$w*UtJe$IiPadw4MiVExDVitYB!52aJ=LjF=qfjM`Z(-1zZJSqPqlf z2jkj`_3{~5JAVxDg(&~?fj>|5#u~7P;s2lL@Qh!qHWKYzKe!RXqvhZ_9oGwR9SYg7 zf~0aVuA7BHJ7#orPYyc|`rpr3_m06@c$BD@IikP10QVtSeTKs-MlxKg_y{Z0=V3i& zuE=OjTMaUbm(tRUV zn@#*Rz+VIW3gA}&PqIFOIR6Ij-+<|!+#%e53w*4IVYdmh_ubeFG4a6&4@UTBB1WTj z{zcfu60`+_Ac~wE|mFtTvv)7*!>#8@o8+RA^rmB zq6W`bqHmYfek}0*tUTTmJTAM&`xV-n5VaI`NB#82-lJ-Y_Y2)&{?Zfl2&ev)`cmlJ zqsk#yHSlwxryp>StL4G~hlwQgrvup$T`h1aa1X%EfxA%TXs}Dnaw627pnVH#I{R=9 z6S?Xsgik}9sbY*)h&pmB(wc>*D@8Zw!M(rh;df7Kj3sbIaP&+x(a}BeAlRgJ118#D zaKFF}hckJa@v;n@8P|hd3XeE|d~4Wc2oNRMV=2O}0NW9l|h`G(Fzy; zdDnAV`i-BjQEFM($5K+0Htg@&r2{)D|CT{A7*D5#$}se%(`5v9#UgqCG6s8Zv3TP4 zS{Wy@k%yDmbD1gQWddq#E}mch9D9MuGDW6JmrRpxXuL_L%M6(*cE~K5ja|cB*#rA6 zJ7u2C7q7@(*hMUmeXyfgBt2Ne$`e1yVp$?fWf^v7@?{0@mi9vp?~fg>0oVh4O%B3N z%V2RrR*BbTwXBh~vJUGFy|Gd;6gzCgL?1aEyOJZZFF6Xc)G^ql9E)-*#GYA^94E)i z334K4hv&%2atd}}&%>@Eo*c){&G~W~_URh&)b$KGQ#N6jZnm5)n`H}jaOTLlvQ^H* z&SATpFE5ZCasi&pUMLq~muN9|v6f)>a~akKip7_(f>L=Q_Chbhvlkc3mDn*VldI5D zA4F^Xi@a1`hP~1?@(Ot+_KeEq)$-4H=IyIFmbYM!_*UK}-hgL0?v!`Qjo9_8M0~{VKyTBjHkL1U4pWKfT$uQXS&+-#_P=1Q1I6lKj?XdiuR@1Sr_MYgZ zHFd0xeI*v_>+0A?^jlfSjw7$Fi&3*4M=+VvGt9XZu#!Rfq~zVJaMb#S*NQ zj}=Q*6xKm5#F}s{R%+r^f=U$2Rg!Y5WU)-8;OznzucD-PU)2zIphdh0Yb*Jxmv~q- zh_A5nGahR(1z4jg#M+HV6^jX2Unmuysxt8_)@UlQcGFk&6IY0dSmPO>24Y`+u&Pqk zXp<+26{;5P;7Y9h{0U=*i?Kp9Oby3={YZ6|8l^_7G1w&>i+#dz*#Dm(&JmNbTR2Ib zgXig|h;s$nYOxi26z8d_cs^ko)|$@4n#N~p2A*iZ3cIy}kM$-s56=nEF5?BNLoLA5 z2>O~nJ@2p#&lfDmGYbCe`>U}7N&AqrBYCBGRy-$e6}O37{PzA5(aJb!ZwAj8T&@0$ zrzozmb_Ld9#pDJ&g>Vy|QMg&%f@c$M#ZwHoV^8xA>}uYnHmbYvWb3`^K0M9v0Pk=9 z1G}4>vDZoaoBzam`!TGc9aoQH*Yh##dTyb&7u1tj_t=JKGoHpC5%u{l@pr8H+=W%} zJMld2Ra1gmjV#WXwv zF&KNPuc=+?b?m6Vi8l>i6pxC3iO0m_{4B@Qc$OmoBd@ox&$?T^BksY@Qjl1Kb-$~` zmEvk~iF#L@FAj@U>OJwi+Jm<%_F~0(HP)V=!b<0d>Lc-^`dID5%FzMdS^rcW!uru+ zaVb`pZ@|v_7g+WDT6}{y8Qgf^CLL|}^>}09uXvKAQEbF0`>421tjGHGS6FTQo48xt zgq6-f>_a_)eJXt|^BAvY{;02K;+c?N)k!=7;>8ZX#O}U^{rv#!?K`wUEeJdO-S%q4 zTJeFX5r4+BDtl<(M(o#O#ec+y;(f6f{l~uF+Qq>$1Q}W;o+8M`vje$!O01`rr{!zC z@QhV~)<-MUitJU>JDL_XITp6H6xCJL>DTHq<63B3J;t?!u7yQ4wfcP(-y>d)ac{({ zsxa=WnBG%p@ToGd%&*W>=y6odYn;*1-sY%kpWWWpG&i`aqor+j*N%QYD(<2Ga88zQx|z~+tJ?0p+$9thJ1yb zzrvz=J^iAZQhOa|-(Ke**`TgAv{7T^s>YDM#?VHMt_@FNNpM{^af<4y4Fy!23Mg~b z85z~(GguatIO_C_>vC7u*oUwrAwztb)pO|a1P?hS6;GjU==8=8J*uZFWN7y|MRlGM zHEg(jm{r0kxq9a2DKrH1n0aJDi|T3^kCLf!3^N2CrVD5v*4ffJvnhlOps=K>u(+@& zWO(-k3yX?5Ur46hK9UR1KGILI#jIFRRog})E4tiP6hoq7Ly{7+jH>LTbXKGMSe59C zEh;J1xs-5O6xEd&0#vevLXc`Byh5vMn{6NMr;6GlJ*d_w)>=b=TBCSt4VJY!OOI|u zg&vPlrp2Z?80Jz_7cjcHy~D7;S|b~ECO3ns)=+p|Vc6*Ag)sGwh4We)7qT*-_BzLC ze>*8PVwZASg{WyEqZhO`E@<|v86^hi65Y5xo*MgDU14J_g%uW+8o4Mkq%JjbQDml2 zXK(OhT4rRvtSqGAltT6t8Hy@1Sd|$p$_#v^E=W;Dy=@%K(>~5m43oB8&yc6cP*a(a ziLyHTc>gjgrZOt@pq_BOsWY!-0poqTt}_y@Hw3CPb#E}Os|y={x~_}L9OM0UU0&rF zubXquZH3aiye)Ik{XQ`y3yA{H#Yg1Okrin3}5zxXIL4r z&$JAnsIJHmw9pW!P;ZcmYRc?QoKJg`Kea(sZDhN~5WmI{v&P_7qi5SwSQ^}9rE6%& z$Z55i({e|Xf3X)CEQ?AVP2HMey#?`-;mN%AJkGnPx#=;E?s`J1*>5SP*$s!IgS=13oS^^tz}aldDzzq5 zz;=H{88yXfwGAO^4GX9>Sk@XWb^AhVZ4`U4X)%UH)zk;H`&y7%BP(?#H-oCyu)DgV zu=dki5ZG?JzbTa(u}e8EwAR%j?WeWY2Imsp?meDbJDvr@aL3+ZDGaT(k&7Zj>QW;Y zX6-4hw=eLkdSyoD%gRF*oKo7JB12JS2CFhd$g)aXC$-LX14VBeb+Ltl= z;AN*&`^sW_V*x#Y7t+WC2FyBQIF!4us(Qbh>T>^^BLACW|C>_lhMIN0iEQ-czzRj@ z;3LINt$&DrT2+<)A=TCm^#Xho-e_vf3IjhqEToYYrPlafIX@Jl8NWZq25u{&gkohLU*D70qXnFMvXtU0r6##dB zi=F*BeA8+JVone}F0na}PF2te+gc2UWP2Eh^PmM+t`@j*Lx(FH9j;utaOFycD-RuTW#hz^2S2!0 z>N3-Vp)4~4Br^eq%mnH(>k!GzbR;v==`u5@%glrZ4|-);OU zF{xk1v)RdKMl%Nz213*Z&uW>y5UF9PuFI#lji_OTA4bqX0C zFP;rrLl!1ul#k55plL>X+f2@BQBiSF(}GS6P&%7TOqrum&xo#z>e8?o?XB%?^HBJm zjU7vEGuzu{+v*l}v>V|@6{xDUwYGFL8c3s{t9guo32K>re$xWPH-e4oQ_Z~zMoC8b z>jPbn(GB8S8rU>%e& z{_FGvoAd;m^aOLg#+B;;uH`}8qRwcaH$8|u4}wTGgjE~-Y6|dBxq;K`TCp)5g=_}C zDu|m;zt|;V&F$@T8>hD~Lh)EU^?^sRKC~z-)+d0tR)?Zfo8HvgzSu|X;fh>XtSbpu zy+Sl}Z^A9@_=lD7O>{Rs`G?p0-4qr1-{_J=`RmLt*rPMnGwAUb$D`-g z2=Uj0r_PT-v7Q*0tw*={LXSCzG1^^^J}m-XSHDLeKSRwGv}Z)m-vwhwXPn^oOhmZ; zo(a}r@rU;quyGF>CA7Is9c?0KUSr2x;hHyh-dsV?`(s%WlmUR~$xhmrB7XMV3fpA$ z0oLXFV?ET4r+4W@7otePkDkrMvy(Z?a+XnyoKAL#&8P2})btGTqj)Pz;vJ<>k&Rs| zJ6NBv4#IzcX;x_$0$#=N1==Nm z-_}+^vdLO6!0%vHSBj6Zk}JjKnhpMk3a9#DzhFQ7JF(MCG}pp^N*#g!4Ev8V{VMh^ zz|(TzsdmBNuU>_})u1GvUgp1t{dcfFP29f5KAp(k0`~~qgK+o2Z7|%;xUPd+3%3St z72I;TMR4=s=D^K_n+i7xZY0^91Gg>W}w8PBu}spV0)7ETtRN|-huuyPJ|_BA+M#@NfS^N$*YeJ^^S27Anz zTj3ss+XQzX+?|G7kLwL^W}a39UIDiRt^=+WZWi1$xXEzi;6}j>g(G~mkeOw0g>ZRr zS#UVpDico~XNDpi$!DB^I|la^++nx_a3ARLGu{RK2HZ~EgKx%mI6RYX^32#GWX2-~ zze426;eXl|>pg+b*hg5C-4_o8-06n) z+yxfY!-6s_2sn2N)5Tl3NDB(KAlvCs8tQ|Q210c%X{aAME)BKBfKXQq2sOrl_L&e# zYQ)`Z;?j0o&@KzwVM4yVr9E%r(zaR9V?v}ogaQ$14_MGfK({gOCP3FQbhQP+nhYF; zuCzjR$QQcIqU!|I2G>lq^q1D0X5rGNo1u(5_mohgn_%I_5Tx_y4y6speQnx6_!Sle z8#i#hEQoM9Oy|S7tx&=_IW*S7g8`axue6}mfL6dQadp6Ng`4Ff z+%(2bhOdVbZXDxA!5`|XhTk8q%vA_K&%$L{kn0RkqJ@jLpim34TM(roD2>z;CNA}u z1$|{fhfT<3NtJrQ#HD_K`0rBAQ{S+lohFpF5jwq%LJ3OU&U8P=ZqLAv%UOn2;WqaW|jBg<`B@y0!4vFmHlZF|-`eB8KK$&>Rb*(3uw9RE8#X z3mwb2k-*hkbb}e{YtfaY7Nqt_%}7m2jklmk3kp61Wb?Dm+r65M~agJeyGVU9~B_HWVchJNo>(ED_ z*aP=gvJZN-8}215^jQnqYC(@$&?biNOTIICeew-}ueG2nEok)_pcNKwi3Jgt4(8Ho z;bvLTwB$QoD>!tr1rcr>R?0Lpv#MrX}rsjGNHs@E$FxfePcmKEa;#G zeT0L4_rSfC*d2P6aW8cXeU@=s;Xj(V3I2U>cQUlzf^M*&Yc1$X3tDYKD=cV<1rcYR zA3+_=xz)nWvY=@uM7ak{vP?E{eo~FIaH9YXW!h>B>Tf}17E}l*kLd`?V#sBVK@t-a zqZ31cwT_lSifx(BTo_b{}fTj|Q_9iS(Sd=i|g63GzObeQ7 zL6a;9xP-AB*N5}P7-`YfThL$&>T5wICPcCr{0dB5LJt#4bS11xt>@G-Oo(s^DU6FZ zaX4|(qzkqnpL{lxUwoGZ{bWJknb4W^5`WYTjXz{Tmj2@RTDaW_tKxS-*Nc+&LBo4V z&+$9po+ro|No_3SwgLATLl0Td0~SP~8#(m0Zn&EmhnB{OOLSK=?lM3t8CqsRofgz) zLCqFKF{X2jbGzXtFm6mY+;9u0L$&b(<10YniT^FAmlc|0L2ibe-Qvbtq2UY##;dqr zp!F7Xg9TlS+1*ysY21~ljVMn*t1W1S1)-fZLOU#| z6|}P`MtmTkX#~Z6V?iHT&|4NX*@QU8IF9j>iHm#If;L&udJ7r_Xeh@(D{4UfEvU?b zh^~<7@-X&pi_5~j%YqUu2S?-|AP})tV9mFoj{UQqS;b;b=&zW4$>K5j|!r~iyry{OCQ}>(a zb&~XO%|4u!?tI@!-#2c@y8mjNPbQ_JvwLKoIuCvs-p7;DhByDne};SwtJpF`?PZMa zuLK+n?g*9N!;hpL0$Ht5Xe8|u0E3+aiBsH!6o+YV02ta8_>SRl+8dAwv^M~}?jJ?y zFT6K!lJ*8WN0? z@Vaz8<6&P2Jw~B&63eraWRsUOhewEmT*;wHIt|`2BMy|u9L`;iwwy7P#~hLwKIJiy z^V`L6j*%{L4rEMk=AUEul%q+UBVJ#ZIh?O0$RBZ|-0kPRf&HAvhqW<;;a!3Kyic&7 z_X%D^=>gAi7vViMDIQjr!vB_Ieao8pmSg=y=>ku?3*TyVYR$JSNv3uyQp{w%(Mg+x zr_`5m>hE)jEMsm@QZB?Y=Jr0v4dGbJIM(~DD_AhNIms&2OoG)+##d<90AHa!2!9Ui zvx4;*$*IrLE~ZennF`itFAjB)K4FKX+g_~Csa!gFjE`h|g?16e$~lDQ4x zTu7oeUf+PF6T16oan^~re8yQc(NCB zOXbx0)G?XLq1TZN*pnm6zlt&Ua@?!9^cPVs#3J6wTg0W=%vL&zbRsTh8Sc~8GY!>H zt`T^Xh|OMIWg?j7D}B77fCtBg4~ zm|Gu)8`%aN%%KlcRxn(_@Ou=hozJP<&G@?+U(EJ4OM3;Ji@BW{VBo2p$!41mVfl+W zcX13Caqfy4?#rnc8$Pv5{aHVC+@cO(4Gc0kP=1>@zaEx>&-s$YoMJ8Kx0ZH@Wi9O@ z%f+;FENhvv7T=+;tR7E#s1EWJwOGmnjwIQ%O6!vXVLUr=4BE z)Fwo;B~&t>O1!;E=hu-fRdTF+rq5^H7H}*Z)92er{(O!#)rP%GIhAFmH*kUHxRW@= zNu0+?%x4nIJcap8;<#6FtVvAIEwfz4a3u4IBpC0AQJarZ82KD}kl}+2FSiW@ez|Q5 z{LQTM<*d)UnR5qIb};2|+eoIt&Z=C_mav>pNXA+|K;pVwzayc01EtO>&B>`B{ytNmpV8*PY3vC2=*Sf>6{Q@rzHyir?^^d1s^_3Qzmg-50^zc!-6fO$wn69VV)kgkSw+h z!M5SyR6J}I9vx%DzP-${-41`9Ef2nna-kA5)IRYe*B1}V<6(K4SkL#fZD7_yX>sZU zx$FkA1_pAum#_{mprhI6sdRzfi6dLhYxcXd(X=rQS;l{jc85i-%&yAttK$0D5jw?wP}v{L*@T!?eY zwuy`QOJ0xDm%L;&-d~@F_uKI$FT8cW5SGzQU-H5m;P{dk-r&ZUyySCu3pq~yf_IM7 z58_ZZwm za1X$3gu4yyCOFuZg#F6P;8wz+4P$&Ka@mGcD}p>I-hps-2w zv=TjCRsqfj$r7sW6~Ca$JNwTl1)2vK9?kxv>|enC+w5<{`QmsQNaxA^R`O*C zr+5itu3-Od_Ag*xaBn@Cd~uTf>)HR1{ZHAyll^zte~SGr>_5u>7#i(VA0{bt+_kT|61b<`BjkksIO*Wyzr$KfVoJG&NguWEA0%u83-Sqpz z$x7Cr_YLpo-fz9%c&RM9{dwsO48eHJIdP387^g4^oNdZ~-e5^W#~tg#CzlVhX#6zhpEC;Y9@|e=zc?lzre|I2QhGo0anNZDZpbr- z7+UNuk$0c>J!s_}GcE5Slw|G4WRE~O~<_-nfy=#zW2VY z^D%K0k0{}+7LD`xBX}J5wJym0NowbOX%YvHYuT$GPi}(|pRLOKDbrf~bvin$%KSqO z|Js7P*+6%DFlqd+XQt@Z#-jd0mTs2i&*6Ur(~SH~e>6AxKl&r9B|YK~V-D-DKSn|x z{#hsnG?W01JJ6q?Opc=FuwKd6ZPGuVMq9#mqK9^8cKY3!>d@&_I_6Z4r^O98Jtk`N z5mQ1)Z}d%PpySdqDa~-R6j7c&tx=g}00F zzuEsD()DG=#nmS<+mt%@r%Tp0@*lUhrS3KjWg7WaN=m#&Z7AMDbtsfaGDanr$eWS9>qH% z-{3cZznL&l;(JPB627REBqn37m5lGtq~px6bLqq|oIO>A^T8JKSkAS0&BbShF_Ptc^8mW6j#2B^Pu)1iv8GR0wM) z2zt367}859>m`i!62N*1V7&yeUIJJz0j!q*)=L2N@-fP6AAS+g&Zj83L(q|fb>v_j zMX`<~bQC25F$;~x9E&uiSVwd!eI-sJ>nMjhr^)}{=yD=+u%6Fik-nyqJCeGWTAMw2frY;&;Yj30JhKow$K2!&;Yj30JhKoSZJLH!1-nM zuyiVea4v%gw$@0t)+nPq$XcU~Qh5M28^dK2%ViVCWn<^Ev2)qjxoqrQHg+x>JC}`} z%O;-7CV|T)k;^8K%O;S^CdnupDitS}N($R=2$v0gzoQbT2vXU&xNL&iiqp7+++0HG zTtXRKLLuEsh)N}sOC^g-C7Vknhf76qspN90^x#tI$))1pQpsbx4&w4iMp-PUK2%iV zt8ClxjkYz~3T=rtN-M!hQcE-&&boRP=W5-DlfF9DRBby>GV6g(WaX6Q698UD};MBifGDF6)3&x3m_;wfl@U<-RC0Z+LL+%y# zp*)@uAM=@l^wqRRKL73!oME?H?uEQ`b{n7DW}Mnar=uCCp{eO=2A_jAk54}9#F_5? z=bF(OWjIX?r-jj3VLNEeS;eCdv!VR3n&CZ8t14=CU zS=c6E6|kTAI8kh@nJ^F3JeJGgcz~&}HwdZmmKmiD$s9;~E2NFb>F7z6LluvJ6z>6? zcf3cPgPcg5&?!)>C|5(^E`hrrXMx7!EKbT}GShAa?E&U<6sOW63lyKYA=GJbH1tAu zps*92dJ67oxM$&Zz`cwRXWHflHznvTvTjkdPZqX zMuLnlD~&42l(E3(${c%Kk4*VUW@gu@%yNfKZcw&>u3-W4R)?*t5$lkD%gj7^WoD+j z(0=lMoEfSn1f0Ayz^*0&hOGxfqJ`>ttdF{csM75A%#57UlJc0cvII|jLQH&IEMamr ztvo$Jj@;u&jY@WEKL@7AM5j592V}-Yrv|>azFdaKsI)FyOq?q|C|FI1#bZG+Cm#z7 zNp{D_$RD~^FIYhDn{ry(CH5RxyATd7T+lqt#9wLQqj8Sq6DHo)-@xZ;j5p%x;q%l& z&5W0;ImMgIw|o4-r^OEwYyJ2yIxRdNr>AyU>7O1y7N9m|EsiRn2=Nx-t5(lr9D%j=Kr& z!i$DvRSZrJDElOpY}yrX`%$-PJX;YO*}Gp%aA;6ak^|rFF6~;D1@nHrq`YT&RzhH2 zsEV|cr6(nYg{TJI($@zD1;+=3Fv6^a2g60#gntH<-N|r1K&7IJ^zi1=TUX`G&Dt1IANNzN8$|drd z&T7w0FOSRBOCU27povBKl_sRWcrZGy>&fV(m`sJQmfjSS8Yv5$nVHV6UxI_)BVbQ( zunb_#i_XlXuD$r6olLiPxe`;7V&l}Xh>&z=Qd-a}F?5Oe(3zQ#`E ztmg%%%~BgX0>(5OGCMJLDMGpE){$B6ajCf2aFt07VN^+3aRP~p0}xP3nh(s8tEWvZ zm|Sg38#AIPzkW>i&m`TEN5VoMO)bbzek>^X>4LH>?72;z)MrR;@2LE<`d5!Dh{|-T zYG>x6$bSXpWaZ~}eHHk2a8Xh39<)Q*1AX3Poa@~Svc$*3ZgX>B7U2-arI37iS$smg z-2uTe3KZEql&MOcK`~)H;}T0#%f|Ol%N#haD7h>tu4i~`NRqR>B6aAWn);SDt{&BE zoVq!zA|cuq91@x_XhO+=$^BDL6TR?53p3W4DUnrmVt+=%ALTN=& ze^|w3gucowqZm&a%BKK@Rvwir7Z06(R&M>JjbBrdd^%`Mc3iL8KAVH>YFk<3#X}o! zpQ@L_$;Yea4(c<$DkrEb9-kuhQW^n;+DcmvEJ9!u_zg~7zI*Pb^o4euNVy=!y<8qVQ zX2^;ef!b-||E7+Zc>tdCpbl#g4pxSo;1uO;nE8tavkv}d&4l@{k)yfV9S#aSLcnJM zfii(H55rg6sKQ0P5J;E6ok4oNBNI3Dsbbj%`mkog#V4kWz*E1i8tx<|2Y=YcQTG^I!V?LU~W zfBaZmxD{^355qk0a--y9ZMUBm9*-~O-eQIKI4wLDCn4N{@B%6e?r-({CWwiC`l0?- z$A{qDVxK;#Zs_<>@tmO_8J zjG*sOdyM|zDm>3qK{~RU{DgRONKU;24{k8JDJyrBCh*9;te84kec&UnAGvyFW#!D( zBl}Fsj)-t41x5uXRivTS8#K4dH8de8s-1+`=*)--EtnTvF=N$;5vyiY#Ks0DhbQLP zY$2gl^Qx=bszQTZe~-v;Ix{kzyJMY!De2G$wbNH|9Uj7UCR%J}8`AMqr*wRjc$RJG zRD7g((P;1c;!N_FEcz4~>PJtuuJZ|%F@AWmMI9f9RR^;-r>AeiFwUYG6O<6eQy1#P z$)tRDGCeN+s>JM^q?C*-`Cdv!W-4yFrpTwe@W@D}^W@W{6l%l#;jRxCU^GYbvXe$C zJ?U!>4JdPr5e%En?v~E!sZNPcO^sLW2`MQFCy$<40(m4gJ`Uk=DR22F?+=qGWRc!F ztdLM|G>S_g4QG*?^>Q`0YC1j=HI91L5$Iw3>7Dq#yJ5jtBPGp=#g@j%h<31sQWCQV z_!NTi_Xw1{VETOIzgvtL%oJH4m^#LjS<*a}$KBAreV56vrgC~(Xl_hIYG`b35(cw` zAKWVYQ8KcY^}e_uKRPNYBRVuX0z+82k&s>I4rm^1}YDykc{%duIRxzDJfaOk-Z1RgoZ@MM$kmaoB|EXQ0H+WZKO$0PEvGqL^E9ZHZ2Ahv{;Cm-!(rC%`>a2RFM13~p>+rnr#I)$H z^8)19gswSgJ5o~8u@+N!Uf1@R_>@fbTyAV!bXWT|@~niDgZvv1F7>_&X`*aqvy3U? zd0h3;81)`8LNrn8cC7H|vkpo+SH8w}|D-cB3)L`wAx2s07Uxf+D3_>>wg?^vwh(8m zieaaY@Mx)mSu2D`9VxM}>Kn+x)asD=DOyZZ5bop)b9tOIYo{ME>?fgJ$QscC3Y!`}&eA!_xx4_xv2= zO3cYfNy*H8H7GeeEIH^IryH_ltEVDDyRKBSGdUyq*^ z?sCYBqVO=TJMrWb&di`d`F&(pR-)UTsD?N*LqfWoq4JGHw_XazZQHn17g6cJl0LRd zdmUR6<1uupuS5lA=Xm3=1NRUawf(G80v^mPfEn< z0ISL6>Q#{WqD)UnOK~_{J>u0YKdt!b?Bs#+Aw8mOx?KIW``MaFUb1HBiLKeJOU>Qn zc4Kty%je~$UbI+oCFKV-xB~k!+pycJuV$4|eo7N$g3rpV0_LpoBG!*7S?Nh}DdG06 ziz0P9jz|iYYof`}OU~Yrl9-q-Wq4ZBFqoX|8o_pkAx&s_S9+k_PPZrjIe5re6s6*p z@^y?fW2nwm(b|VK-gGHPNwugPzkk(*L7)4olniJ~yXpHM7oL;7Ys%=z#BoxO#jR#Ik1zE6B#1lw@Go23_9n8lCrkT(we zXq3VF;W;YB%hf`S{80G6IUM=r@wLvE^J~V_$QpHg820$B^mxpnxgCZ`s(j*GG}OuuB6Lhy3pXx5T(ZRZd`BaBN!W$-Ry^HRX2&O-qf7 zj!^dm2c_V8axxZvFZw+RS@)2T+msEkL26SneP!lnI!p<90{TCHA?X zNcmNC)}V2H2DRYJ@;n|ahD>oHgV(xgu!;54avF0QLr#4W6|Ft3a?93|DHRn{O11>k zNG8*HFeWQ8F)LFmqW_S6CAXnPfH|aWikJyYN@y5vU(1#OBp!4FwYf}kerC|;p>y1 zugokhEKX0(4v$I>{aw>d$xKM->5fiFO$ZCiiO&r?D}HEtoHH&uBrHxY`#mG3S@lWv z7nDx{?GgE{B(aVhJl#D#sfWokgc+CtW)K(=7?3C^U|iFpt_ekdy8c{O*~L{+46HfJnsHZ# z(HSHz21J?H|L>`~@4nYP4eR>z^XJ3de)rtEbx)l-byA(GS{QaKuJ+DIC?7UCx_u6l z$!IN1_jzIgoAT3ehFzE%N>@gb^E^RTH zLV=o|MqC0+Sm@BJ6LAVrIg=T?4RdylA2 z&y9M|Ma{RyRA`={{GHF}j`Md8;}k@SxruLCuo6mpi`zy5?$*6Ti_x`~kK%N@W_gNm zi1Kz=S^|FQiu%K?aBk^Sd5X8Ayo#5wg#Uo-o*(J9@*YE&Ky;ZFOX7XT=e}x6hh)k_ zcj|@s@`?E{-j6g#h9I%r=)zxS&sva~D;73~%*uQ^U6I=-GO4kAc6cte5UC7xXJ-eJ zQZ5$FN~NVM=4a1OO+?F~L~lG?4aY|M5>pphefg-A@GB*=CDiVU`4X9s)7d`R-8)|i z>PKCE8)I_ETvz=zl2dHY%$2*l{mxKNz-uy@UGbQl3R`|y0BV`(g%8Qa>~ zb#~cT_4@Vl@J(Bx1!@EPZ?u-TUbPs?<*vz3bYi>Hy`xim2gAX%%kAm&^%NS$Ib=1W z)_kp=StnB@1fhpscpZ9jwzZ%p8;oXe$^D79?Xz@+*qA+yU()(yIGmt=;3>5xLqYlz z#&KCPA`gssfx|gZ;U0bM(Oo6rm^a+crGkK4bbqJ92V3Fr9&b|N!>w?5kN={=FI@o- zt9EQ0^JcB}C+Wm$?rr^9x*9%yf@?wY6G>4tV=iDEE>X#w9t|hwuRrUo8|Jf7C0Cv= zmKQtX(&wOx@7#9t!otnlm{3f2_N*@!*Z0sJwRi(EYOdz;@vFSWPRtfH;pE{6xEE_u z$-@!lX#@+nr{OR1dYW)IPR6Aq{Vr?%%X}ujcwh&?`u5gthk1 zYvluC-d6OJK~doacwn#O{Y!Li%xMv4jdSTGBCY8cWG*#Gp$e-N`6-_k_Wk8nzm@Y| zG%0v5Er*kVuJ%+_UOAQ3KG~HtA7E=TBr$7Y&z6W>DXf~$;@tHOt(89b$<5X`xx$Y% zvKpM*%%T@KocFR9HZSAj{0{7n6B6UfZ=zHd=vaM2@A^!>0SQ9M_^c)6amUTi>ZOt| z@A-M=>mKR;RLJ2xeZSwwd|qkR=rG0-h0~{z3`m(`Zf;F~p65kymjgAMC?$<&%kWQd z&5mp}1ndSBJC*fYYcIa5oF_Q7hmyItARu+FHsJy#?IC5bcE5RYm`t4|-ZaQP|N_qUD9!K&}V* zf5Kd0B&IRvLiu`%$tabSl;i8Op9s{?jrrN9>_vafUte$(9CFh2&FO#jOP|a7eV+QW z=R6jZ!l(bu9f&5w(t5W)oH;$3@*`2tci?wXl93*0g?0EgWHe5{{rXl8)A&H86b~6y#sw z6#O+>40(ZkY~nNlCkhI<8yOEI_XM0MNN}UxwyRm1Y!Fe}uKTp8W%(u41`Qt8-H+Lc zMlD2dQQkpON)1j@PrzlI?n}}|e3hhu_-bHNvvi{tquxJDbVENOgPG5Ai2h=&LF&F5 zQz*I*%_?Q(va|jQJKIdesjm;`o$O{y{rhx>P;JELtNRTJ1p9Nc&&QrLsydd}&nfn_ zO0nN_+y$H{Cg2|3%`I@Ejey&A4+=Om0`Sz}VSOVj$Af0f5TSDxNx!E=~xd(a;CDevuk6evazdkV+GU~82uTTqNRwkozOjkwNVZD zcT~7hdh}fC6<%8Z1{Io@9z9~@?>wcuia*_t)-~QKNY6m8fsaFQv z@6c-gVDsrgKgIlXAK-Z$;=9*3pJL+GBs~UA@cPEP(r?tKCh3&QaiVQa_(@gL5`4Y> zuvXhiOb4Ll>>@tLQe381G~Q~o!h|@akkbN}n^wL32y76_@DxwCG@oAk2(^QaLAy1c z9#fwdQ0LWHcStQ+;`yZlHMsP+_RXi7JuQ7ggFe1f@NWTUxTlb7iP^71tJssvNt*p!o10aAtj&kE z#N(Djh2T+QB z!2vM!lQ$yVDYA-dB1Q=!ahsWEv7#2-rkbfJ12qISnLD>>s`#>OPaxBkcKgD8a}sY#|<-!S55csn`>v;;OWn0Gl*Gu ztD`wfzjJh(lyYUqs@IzKFDK{=ia@?Fmyj=P2d$aVO<@dfgtTc$ zLLPxETR7kgA?6GIicO@>Fg#iHDrzw6#gp1g?;nJXc zSm)4=&YO_r9yucU*Tr@MvhPW&i+2?mha9Y=9mmZlJD0{x_u-Yid+)M6V7^=y|%YOJcdXm(8}X z+g`3Mb(tsS%1l*>haxWb@VVWOQMbHPB(8$14D(B=MHbw^Eur@dxR>o38n_;(0t@J6~ z#f|aOQkura2TMQxrVA?A}#I(i9yrw?GQJXhZUXkRgbmsVym9aL{3SJIG!OOArQ$kBx^B!x^ z?#Y*fEAm}w9AgJpjY^MAt{QjnaGF|?j0FW#S~Sr@LCh641xzXC*?E0q+(}p*b7Q>z zCRv}=On_{)VFH{7z4-D?du~=0HU?g_lk4=_V!`1yS`t3e=_LE%owARkhk}@ABR^Hu ziQFA&c&mN1ijACQ)$wB6$+n>NGkEax7rh zdtIabyIKs(%Y+Lhbd%t3!ky&NFqc!0s%xx@$UYh45=p>M(d-H5_?7UJl&#W$>u0oj zo~Auf8gQpzn%m&uRUo>Ot+EQ#Vi;*R#C~8=^!l zcM0k-Lci#r%aZIElv3jtO)Tqpm+4ad$z*>@+(|q@`$xT}kPg84e?oi}xZwL3-G5rX zk0pTP)be)1Mui)tKa!kSew0eKHE+~(^kx8G3T2AC)XYs7c1n)foee`{VA=-lzBuSeH z=qa9+O1}Q{EzpzqQ`y^4ua(PXDr=IST)vk2hxXsXuG7R9qK=2})}Z>YsL;FwD3^wL z-9G`xHCjs0<^8(*wf5&FYQIr>#!2=4lv9BuxA@8<-vSiW(IG>YD_PQ-4ywSYK@P-1D&}11ae@1ihXJM ze7u$gt0oP4{7y{u$t%ofJq10)$cFGZ1fOqUDt?4I%pZ&STazxF>1ZOEiN-0r0u$io z68jJ8$ohrRgx4KbTJkLP1v%=A1^5M+ej1qebNMP-`PiEX|Hqd{b>HBv+=F%*rQ=}~ zf^4T8FlEq!gPRHko5ZnSGHkQftTl$i!Qs z_7fw-TlgmPaDEZclL!WhELjgmr~_HZDV;VW4QYz8$w;UoQ?`caW1&#v({8tm0hwl5 zB^|6k*!C-5G3@>HzJK+#Bc{d2Az9IqlJ^D!^~YZG3*YJA`2T(TzxzYs6!q_h`u8P# z54fGpdvuTPt;>5gJZSv<4e$`7tbV}@;Q@|BXZfM!OR>vn z^V`p1qF%z@1Ul?)d;)uQBpZ3WT5!b#GYJHt(1SW0W1(Q;$3~km6!bWMY_z|xL3JeW zI^(VjBz?~544N*`OCngx84DjEjiTQJ-HgAm1(X6t2NZK75frtr)-0>oz#KAXlYZlyo>0y>DT&`5#<%c@3#=GjkJqqh!oN0)Y|`OX8m4D2o_P51<=^VXUSW_N%YAx@_`rD zU$=4Nb?e3Tn)Y?2(zX6_pk9KP|j+TC~u_A|}=+l~iyukhA#`EjcDEbb~LtfyP7(IuF zJko3-*eB+OAkhwb{w@A|KWI!Nk?JH% z70(;*Q4C&%KcZ#|a4K_qOq72@2eVHtzh1&@ms%)S?;rPx8-9u_$ z?w6YHHJ?ADyRW6*1|H4lkKlZF?YkO2&F4?(-qw81Z79OG@f_o6zVY^g^o)k}ue2B> zaUMkl1f`#9;V`ls1@!TKS{*zpKu|;l_HjOfwb_UYEbyp6>r)io zW80d&^-~p^mjLD5cuMz$2G{F9y6Wk}xy{rX4z zrFnuu%o8+cOexj?YX%9L+Jw34M*MB&3F^;4_wpst=MXkgTgi2whCk-mp} zJ!@h_5r*Ct8ofx29$w8~7o%4Yqkku%@LWp_ga=C|Fsr<2MTnkTSL{6;qgKC0wH_#1 zltwfjvtIalgYarhwoIib6mL)aeF?uem+)cTm{6YQ&IEHIds<1woStld_6d;#1`Urd z5Z}I6<=a|;{lr?;3_&iLph|O$9rS(@xJ8f z-p-M$w>c|rtKJh!GE*olhrMQtoEfh4%;jCFh^yqW>+Nt?m5>tgn9Xu_sM5QTYo~Sz zT#1q&QYra9PDu{ezgvYDpmPAnXg{vINB14=J)$(d2TJqxkK(OQs?a<^`8zl3PV#pS z6J4}-et%gLgJw-9hd2~{)+|f*?8V%k75I`pEATz8S|maAT5I`dR=^wO$ribpx0AmI z2@{7Ua}7?xnW3y$kr%FdHjp%?0_Hq~Tt-k#?MQV#2_2^f321jAa5MaNu64u?0pRja zgwv_<-L={My{X=)zPz=6;M}pnS-0q0Ytoa*xl_ZrknD1V3i4n_s6FmZAEiNdZrJ5Q z7AX7lz^?IRbyHVnpx`h#2X~E69UM%_qNA-2tlRDw&05SMpFd)9#PY#x#+|6f>NChE zzdnUEn#oXztWC_kvX@A&fv;SEf5_F1*w_UgG7mR^IeUZ}!pMYZg{L_G1O;sAax0b% z_-uB+!>NpA0=?CEMsg&BN`n5~fI!TQ6RZ|iqFGa9u$z4;oIyXUUqm1M3+IhLS-z`6 z@6(T}P|S4cj_WoO9CMbyPb>X0_H`{^e9l0lmgc0bhyq2phfBf%^;ynyCv-m&Qi^|xY*B*42H)B&P2)g7PaR$h zdXkTs0bT=1E>ZSLK9m-_^5%B+pu1>10ypGFaf|Hd9c#D-v-*3LxeBQ89MZFC?n$H_nDu1rw(|q2rQpNjZ ztF(T4E|cx`AF(&9;(_C)x@q@5>+Q zzJRvIv5Lko1+k|W?Eu2_K*a*8Zqz-g2U_(?4vp32kM z6Yz)CY;RGXxLd#?dfe3;Cy6qUKszr&wGiV!=j}2~FOJwA0Owyn?jT zdmOotzuqR8kQ7W|d7$M} zyY8l)Y{{x7j)ihYERPfyi8?4W@FT1e2s)Zvb0RefI!+7Y3pj2`0LAQtkbmR|TA?MsV0@{ms%uA5r?ax;iNa3ktYu@Mj1P`+=esH2ZGT&GM{2 z%s#_ng%7Q0nIZ73vCM|RCayI3C5h9Ii~Leb@X?EjEiMkg4`2t>G>nMHSc+ZGNND<_nxQ0#3X`aOhwy zN|dJzR}RffkG6{p*NX=J#HToZxoDa{*ig`V6^y(_SwtQ9JY$Q;aCl3ue~WUl#6 zvjwg1Qr1fAyN}?kw?=NO$XZdK?i5kEuojgQSu5((Z6a%>%42d#&O>Cc&{M?Zd>XD? zW(w%Tw9gP)(8x|9C`HvkK3`#(sYW6N4xdQrlui)>3 zRObABU?qQR8Lb$p4QbfeSd`fPf8zIsns3qa4c7meNOSGz?o7IBQ}NoisyYxNv$=Xbzd6;2i|sDFXQg-D6sxkkk|JJO;kS4*$zN5++5U+afrj zU;$c2Xc#RHhZwgIAy756)TBU;NV5#7cvTI7%B1W`+mo1NPmUd)o<2M#uGdU-cTdp2 z>zu(h@7CA9bMtHV&0bgw$)U8{HJIt&)>YVf_3%EkY?ZO^soBi7j-Aurf6mw#ZKg5Z zJu=eWH8S!In`g1@{B6SrM$!W@egq$^@rJ_yu8}kyJ1(Fw? zPixShY0y*iYe7$L)S&uat?bhu(V(=Bg4av_^;>WvS>t=}(@H;H;82(jhxiQP5RDD& zjDIg58_3l$Bs6$Iz%gGkuRp@a&~g3cqTKRmoAiR^5aFhBG|yrf6z>_%((13C@t%pD zNA+`TNA;Upp3f6rkdVjOXGDF{t)f2Z-HrOBBWILH>#01|hk<#G_8i~g^BOJmr90nNp^eZ zQfFm-d187j;tB=bzxB$BCshg!mo``1OFiAG!Vvk<410BAT`6((i*hvEQd-|pT_}|n zsvYY~=VcP{bUL2MNV)2Kd;5H~W1(DL=tz~zsYH7_Yy>~rO6M%YI~VZ>zU!(m4M=Nd z(v657_9PcKuL(rju)HW5V4tL`yDb_3`vQG?q?}mD2d46yN_J*{(bx2f60-%_g&L}$MJUoq#l2dx;@ zp3wMoV&;N@soL$f%XlCo6!JY<+@9ze^Ckmsb4vCkJ)wM{|G;$Z_K`PQ2n)zS_PF$K zw9@_93eiN7J7LFRAB3hsM=qYJz=c*rtiiYjhaR;A9G=cZqLxkc#^hMUtAuQ}k4OH% zO}(B(!fE|P#8z;O?CPApW+}H2mR+gW8!ZlhVyKw!37aiR*(p2jQ-uBJ2wCq>x0%Nd z3@*N8*z2;#910vKMkj6|9?W;lJOIt*QB12=NeQQu(o9~3{VcxN#?I%&+7KwX*r++| z#%2-dj~}%?#REO*knCl*+8k=9Fx@0{xdGIGguonzXxNH%|0-4Tyu)GN5)-GeD_%3M z_TRbf5r+%yN9LiwYc8J|N+mk-zEsjIB@EaHKgAKM+aeBQu`e`pWNh|2t0LPnZcp52 zb!Wy)`GL4U5G%TW8;B4wEZ@vqy$&da4M)c?QhR`*5$o&tek$tTo_g*vQqMT`0u>S3 zJA>(C1G~mjuKeubjdq)oiv`m&L+#jftbL?d*ff!!I6Rf?jmLYF{A#UkKEE%$|H#3m zfIgU%QzmO+aK4>+3bE|Oj;_fgt7$2kEDsonrF92`N86hz;L?j zWT+CwlKonHsdk`C-6MUPU7&adCeuUb4dMTenGRRfnj0<*?i(1`H&`A{zbaA+g-VfP ze}8?)NC^qCk&aktgk+HDov&jxAIvYY(=@uIF^({2*J5j(k<}Sl)jJ)#Cc!1xcxU>+ zz{HVB^h&fh&HG0BdUf-G{`C2mUUXJKA4(`mlXL2v8gmz8Lx<-kj!ayZs-<(axagpU z{R7zujT_*)>Ht<*Q?3yr#Ox4uW8!ijy{1_)U?y5!p!q{z>R7Cl7Q4FFw^!D8#8XMg z{gkzD_dsoTU*GQ9!0x_1qt)st{Ue!zZ#>P8Hl$=fq@*ALMhz)449^FAVN<)wW0vL6 zexFs2hQj{Gg3&f($`x|f%u%KO-~KRaLZ${zg2A2uLf*pn(xhXf&Za1WT%y`BBKP~l zR?CJ+!SAsvW|P}2E2Ys$$52?#24;*e^(u-t?8DyW>%x5@Ka!x#Z6UXkmve)0rCRb2 zTNRH7Ow4qnpw0;P?rKN_p@)U}CMdI7K=SXekdObI=yJ-6w+)7Fz$51pK~K=X#h>m)Y00WKg^wIqa9Mo9&W5*&8bj z#|S%68D|XNKpwIx3v0bS0l%tYxquS1W>#Ogbi3he?d?y=i8kX0ZDBJfI5T_RrWA`& z+3Yoa!D#}~Pk`vCxoi1ZHqS1n7z&qyXSSQVlYc=aI1UeK(FyMfbbA!9--NATkXM+A zMLi*}HR_Z+davUHK1K2Uea4Y>_oQOIsg6w+yTcptcnn5^H4^m3E!LFd%~q%E_68%j z2g2b{f4OfAMhWri?{GrWb=U<9Ef>7Xr7k+WkT(WQLiDfVTdW_=hOpgHM=?|_jt_cg z%l$LO>4HBFGbt9ccW*BDo;4f~DgKDr)HyOgG(J%+PvsVq!9XGr2quXd)#YW@&n`tQ zOUShWESOgHp=iActoUYk#Gt>%{w|*^``#1AktdfKgIM!zvYS|25esqsDQu2odd7iD z^pFz2V@o)RIgS-s1C6#IaAI09{!9hUbkT;P(mEsy(}i%}V+fV9W4YXDwll!8rNUI+ z6ApX)!Jwa7ASaXdWIyb{klAG}CX;lWLo$;yJ6*joz<(QXdi;KmGw|V9z#qbC2(dNj zjNVt|fqtYLh*r}O#wc@2t@7d3MBw%@jlk6@BD<0HEC#LZVRdVa29O#4))2c& z_eqpN>~VPrkgnxU6Xd z;>#7h^)G0uLWiPuBYf$)g~GQ}|@14=+%2Auzhw}`!OK$#+YEpYxbAcXU4c|F$x=RW`fI1A z2}6{~NcW?L3^c+be7^;(?G`0Irq=O0soqitzOh8li%dMnh}RFUs^QhVh9W!Fs6oG; zm&h&uk!|GPxLYmpoQlQo^hsW#fHSqWP&=*=-}oNN(P{yv`#e@!fJVpE$vu)!qz|u- zB@!_QU05RKp})A$JJIE1*n#5;^7@WJa;oaA;eZHvgi3z8R!XsX?L1;gkU8M|i+`3>;p?Vx1Z%-iCK5=BGXHzX@ zVv>}jxs#_`nhz{v%kAt0<{CJbhtS^Us%aR6u*iTKNC5K{HZ~d{ES5k)<^KkC*ahiu zai)+O&P02B?hcvI+&?&bbWJpoBn~W7{jFGDGX26>GVC(*r0x^!*jgj-Vy*R0wzdoj z-ns|bD0@PEXNCR!vHVoNFp2*Qg~e_sO(+9#G@E=BYy z+TR6sC_*-LR26w1uzOVrT2fZg!_IrzWf>7V=c$B z1^af8|Me`}%PvEH4c95?0p3nyh9Ca%F&e4QVsK*zPNd_IrWECY!KILd^!{_~2gsam zzOTki`1>qR6+EZq^YHTe*%vG?y?sS_{yknlzK7VZxTp7Ve^kBa_&4#v{L34#77zIF zp5t?rBtOp&L?Qw9JYDaK`2G0frxkTQEP?jl$=@&X`xM&qR{lNp9^a$;qwq>7hhUNI zg#3q3wvM)h1kVnr!5J}Qf&qxnv8>C!@$kR^;xMjYI54y^m)STHh<(!Bd;49xcHMP* zuQ}^hJk`r@oSC`#^0F)BO`~oX>Yf*M3$9bM?)bpKjsQHH%VeBFg~JT`UFa!?^ZfIKw> z>uyZ_Pzom6E0MQokHKVEza=3l#jwE_iTI46f+Bf}^R;Ny=PDK*(r{1Du;eHf)4^{r zSO3P&3-etH3;X?HN$JX6m>KKJ+S+1%xW>*@HF@#yl-uB*8ooGDNx2;{55)ppu+VQu z`$rpnM~m1B2nhr*dN{g*5}XtqF_l*{9AT#ffrnjOeXjO`LmKJs9ue4h8i==tnuxpG z=W7Pj*HaE>0QhtyE*_b5W9x(Ai<9NFi|}F2ftDt)U}s)5fzi3fOUu*jR$!`njFe^2 zKm|vPsXuQfUVD%(f8S<*7yZ3I63~a~2YjW|U3U6Qed9-M)bFTmN;vdf+f$`{<6qut zrkqio^w~(@`CqH`N#7IofrsPE*Mmo&#HhapmM!I|U611ix_ldZJq&Fa+~3~s*YZDp-5Vm+>D0ZY4H$WC-Y za(s>b93I2#bU%S4G)k{~p4uD-{5XfdUiS=#zurcD?1Q9y8rc;HJHcu!pi7~wIir}c z=MZn&XM(O{7R$BHpeN%v|B$gQ*Aw`l)9JE*z#maO-dDNqFnP_P=*qfaDbnbK#^)os zhP|1&rdhwLE+lpYKIm{!Wl@<|hSQ#Kx(qC@1bY*sy@hRr+cwU~ygI@U2#Vi5ZJjZM zk!J1jr2cXK+rocKuj@*Lzn^OV0sm8?mZCo0zw!E3)P@=oQ5kO~lKRG5#3Q;}*w>`8 zO2;KmM`$Y0@fKLE1SdKY{Pi}1yRZ-A<5E9(?{&}93b8ou|IF`SZ`+I3_?G|7PJxHg zl=G}=fF2n(uWC*RQWx?_aWkH2c5GKlVV}ik^!eK6oJ(6>VP`lH_JdTJym{8SBSO&* zCeaB^ZGmK1HdMpx)(uONzAfqr#2CxA*I&6b#}3D2IU1E^inSD%&(*&hb3og42Qdo- z0~T{FYHkwbE$E9KsD%v#`>nY>0tA{k*b&MF@)v`kdFlc&i&M&BD+Snud36US=}oEO zxDjEW+H@+nuAH0N)l;4}L;_Mcl2O`TfTJkig#_v&RtOdS-6~(4FOvv7Vpk@v6aMNrkWfWk&BDHoriW0j1ElQ z;S49D&!^)Z8_(^UJ2E6hoDM2goO&34(6g$lC4x2N05~0;2;o4R z`3fjg=3hFMnXhf0>x|D1b#AC-j8Y&JjvFk!JFaDZyQgj8*uhF>IMaXr%(}~c%p486 zy>0SPzS>zC@+CHHo(VV{igo3vyqG!N`$7io_D02yR7-Ktgqy9xvl1?&8}FLNObDQ=0ws`tfzPxvK!?{((d-|OrOa+i_<{6)=decE?Iv~eP9(UAZbod?EE!*bZG3n^;FB*n}bH#xz zIPuibS+OWcmoUW;|M2;ueuu%g>#_)QguNccm^7NgiZzA!EalBV3GVY@-g-0f9~p8q z9LdI_xF=a&jor=SQcasT0W~ldHA?UfRyy6}*2Tu(k&CxfDqAid8M$an z6@NDlTsSp)XtY-K8|~Jl(-CuLN3!|Rlq=?RCaq43H`t4F_HJElYz9x&EYka<7g0SI zjZ%FJz58d7?-8_=-3|l3njTCS24m>gHix&}Vu!Xd==|`{euVbr`EDdgqH2xN2+NN5 ztKUQ(C3r8N;hX)dNe8sWzoNC7kGC^+qPo}SvRigUOPJboGNw10jTWEDj7W3A>>PIb ze9j)Dp*()k=;%e`I1XU!!tss`9rXXkYIP&K#O1i(>?kEYNwdMC*zE2$qrc6BqshZj zr`xjVaNU8UlAXqB<5Sw}sA{;nkypWMK`;8*cIi0w)OBF*g9*{brXi+>Wy|-g_JO-` zN-qy0G;$I8dqFr1fI^|y!+%hs=%`0gJP){o9_64nyf66eK*;*axtm;BcR;3;i;3r-p?DLpAc&lXs;NQD8l*=2sy6M`TDU~wmQi;8yx>&9* zRw|3t@?v$VH<9Y;$|QQp_E0rBe3dtF-)wUER^6xIgOV@AEvH60?57&*<|oPB((_za z;I$6rKgY|H1-9Z12qiKw$k2s_ie0YhY%J~-^1tjss)%{$`us!9-z0ykx?i*RNk0P? z!mgtV)anYt`%pc`k5CYa{w;Df>m)TR@tse^-s#_fN+q?b=7Ri={Z{%vn1w}r1i2sx z2%$?kI}f0*Yna&V<06&3c6xxg7D-#d2PmrN&J`DE$fKUzz0WAMNujF6qy!xyyCJo4 z`m7zrvo{&C$i=n?O4vf(AKrXHJnvBgo`A)E_HMILVn_!K;8<#Fo88FT+P4-5_m7-6 zUp;$oJ8PTF#qDON!Q<=p$SHP*(pmL~W4@&SN((r*s{1OtT>1s42_oP;Dz};{L?=+4 z;)+eKzOzB20iw}EM52eZU(#Jf`T9Y+E;oqvy2cOc3(AMvBW>op&k@x$&O?ZZ&frE` ztSZv@*Fh#9HZjUWM3A`z8Wz~(jvLplyK%?T%h$Itr>%ajL1M{G9TWTWW}C^?)^W*h z3zO2Ds~wvXdW&`G)!Vk;u{3wxSyq?%>yloIC;Eo%R-4H@J;JuZF4D7PB9YJ=-ft$U zwE>rpKwnZ+c|x}d+Dd3k4a?Q5%Nd3?KiaIxQ!9=>V=MT}@R5nblVgWQM-Pon9-g=+ z-kXSaMDag1^M8IPKMHNd*qtaRq1#2FpcKQv85JjOvd={*@{3ZRn^(BnbzlTjoeexPWl$Xrzrh#d-($KV8-~5*z|2;$uvwc>wSGLoyh3?ZQ z2Bqu&k|V*1r)Y-j3F&*le-ak%n)ri`_%@Y%-;=%ydtv!s(es>G?`eB51- zJMbP6H6d?wV4#gFAQO^qkg`fU_rM0^mY)$8IMJd8yU%27+CppF1vkrqbv=9=>>0eRLHi#uohzIgp#oS3?vcg14P`M_)YcJ}t}?Ca-OCGK{|73{?7 zPD-A@9M!ZgAUhMXW&WA@K&p3_sH(qjSMTM4 ztgK`MfvkeW5NOEa@~t>W1#3954wNDl@6pjZA%1c%`%g%oB)rCP9d_zF*21w$V<@Ux z3)_csHNA3U&K07DjE|LovMb|q? zo_~ricU3m`^lq+12MUoBw6eQ?F>!de#naz@zMS#+GqRlVdopsp-Uw()W{N3A`~j$1 zJz_0?koLkCCe4c(JG6N&&U){0y_C1?!Ij`UE zYjE=D;km7s^jRHNqc_xVM-~rk8#<>KD^xyCayF;TJyWmEBFtRjE<+ zbN-S;Uy0{z%a2KC!wRI{nWeMrU#x#j7@Vzgl6t5^Pu2^5k!8jgttc3?@@VG zkhVg9L)eNC@&=xr*TvHmf6OOIl63t)?Ms_7o(`u)u>?c;c3+P_IJY?IwMRo%kKf~s zMnnFdUhlb%fWsg1*DhLUUxzeAQT{D+p*P`?NwHdjPZ(pIxYKc$${-HGCcb(67)1A{)dD~oEZCa*Wq6_!V5rlW-^ zq$$ywvp+}^7^4xKf;A~5wrY<0$X|nD$9ovw8<7L2!HRqMMWEFR5JCPt(m`61nPQ{0 zio=+6$K!qbuKWobV_WY3E-#3LbXa8i5*UVxnzrU<=pO~QrIw_y^kLr zE9tXt>_3I$V?V6j^V4KWPl}UV$QnmH0G3UQJxz8B+Cm6xR$%L%RP5aigS&nTr_S(Q z@iQ@;Iy2W!r_QixJ`&&0@xppuc4Yfa3kx@Guk($G*%5I94c!3`+Hnh+40P5T5}wH~ zBteDkImP(E*=v}i)lBpi<`l6MH@Dj9OA{q_Z!eCC&4f4h*4Jg(pI_i~SyrZTVYSi- z!Et?WTGK?*3@DOYj{(EK1BP@)9HMwCj;h?kIn3Om7Y=KKh`z?8Ho2}vLP{#p_17eu7YXCVLj8LYE^*Ea zvrL7b0kK%F|2Y1e7i`M)O2`v*Iz2cW-tWK66AF3#eji4i)%t@MYZ_z_1Ene3+Kf1A zndXL$30czH3l{jF5Z9}`Bm9&LIn|5I_(Eu2MXlo#ighAA+wdZ|)t}caW^N%b!`1q% z@Cih$2C*V0dR9$l~R zuWss$cSV^$g~$@l%0rN*{?X=h<}c55(II&8a5xelNLO&gqHu|e#kp#Hyx-$-l{yqU zHP0M|H_JZUcn)dSl>P&{{TffH*#AwX{aJrw3D3s=wvt`a&hX(L(AuL5?1)(oqzS;4 z;47+)8ALDjTzEk$tvuUj8}-pZUfNjhrC*!I>ZTFtfxq;;A0e;&ev(&yKYQ!{^7J3{ zEwTpq2|1+MF_-ybdfmqd&zUFaKoy8aeSq^DpGL3$r1^V_PS$zIn~gYtLnTolun1( zo5HD77=u}%0imSONoe&A>;cKiBMze7Eqhwk_z6nr2=zpz!-2GiB|UKCz4c#uPt<>j zy#cVgBm_#ei=CxK_y+qh z>a5>Kb+UDsIHJ$hKWJhH?AFu*>NMlzGSq3d6arSC_A%nTaorqb#2!cp#Q(U2c!4HB z;I;T^Z^900?4YUsT8AU(cgEdpkHc(m*8VukWL11^cBE&E1lYyuz7|gi}%FQxVE(LcTgA#y4mriT-WO z!y~4tI;Z*?^!iIfd57uU_EIESw7tjV$cOD=)BA!SG=UzIdS4w|`@$wEnu33|Yb zbe48a&+IH!ig&{!D5X**zuD)$yI48s3*OycF72F|-c_m=?sogk;wkmF3;A=%=O?}b zB~|;1A`c={Q|&VzctEe+7OQ3QX|LNI47Z)-KW`>p%f{j|&WccMh9%#DyKJ#&tZME{ zuwP$&fgOV{Z;iAE>fx&|z?YEiB)uB(5BT!xIyyumN$o=sih&4vVDf>c9(Yfw;f#je zZ|cAS&Epc=I`z^W-HEP#C3HzBkqEur8@C#!*Z1w}@7vv%8m}0vaqrvd!Bt7Q)}8F$ zafS4ijz5TyLS%s9X!-;K%kW$B2bCU?>M)6=B3-< zvP;Om)JY$az65&}i>MCCm^{VF5rsU3jTITY{2;UjlQeCTKGJc+kBOKV>0E{6DO~6;;y~{ z{&FO9OMyoVr3IP@x-p!)CI7WtOnUCLDVEdQGm%g#6A7lD|6MMocRc^wSPoRDH5tE> z{(;)^GW0L9Q6R;%pkN9SUnCPXmfT*EE;8l2ZU3RLVwz3TH|kHDzu7dTwDfawO?cTI{bkdUNjOd;lSuvlBVE$vM>%b0g_I!;RL&u&jNV6e5F z{wJ>sy+dNK77_D$f)OGv(4uu{dL0ykYamp$+BASwrEj^S2GOnbN*!qHpi9+t+m+ z+SyMhb9HSjmp68)Cvz=fJO!~nUd=|*BiI5Kr=%60>_e(uSh~d4jg2}AbdK2TV+ls= z=`QDEm4vGiIF=0Qv5F-Tw;1AgC1N7V@ss>SE|BkuaR;G(jNF8n$y&{uT#oSDXdOr! zWjyG;(wF(1a~av!h?hbh)A%L?xXfvtd4{L_g3p9y*Ib|TrLM)&>XoDHTWc&Qt>3s} zB@dfib9rg~oHfB)SFQWenW#;Mn^v@G8=YPE|In_#eVf_q)*!@b$NvK%_WVWUf%aMc zBQ)>b*wG+KGvwS^=Jq(I6NN8bkfCyrzXScFu%58*yB5p0ag>#p{#rvGgW)Zb^cGwh zGly8p>2TRuDjcr=m)+%X)_>`6+-NZThhG1n!SEo&7ozc1X_DPY`wvNv>cq}Nui`ro zRi!z=uSCA*qdKvxP?_&4RFyUZ{y`2G`w30*{e-ap3j3J;dV+^_673{peSrvmmHs9T{s>Noq&g9pN)g(Rgf(R&E(K?^s_Vnuw=j@kbNUcq|i(OS_c_wyKe%-nbvXqH-XH zU!+e_r|#o8T}1Usn{^yj&jyipH&6aRd8AR}EQX)cx&iPpI!9H(uo)l|{t1Z&2ferPM^tAQLw%&UUR!0Ze>v4Z@0OyagVLW zecLLy|3+{juai>rNI(mVQB;d*xF6;wI_y?I4!LyL5KK+koFRmMt@e%yX*f`dhvGgD zqLpY*EEq52U>M@HJ($_m#++MG=$ z2QiCegB6fTcuEQW8|DP>#og=<=@U5d8sF*fL+4>3{K1}{XL>Ngs+CF=Ws*|A?l*eA z(rBBe*3jkX2esF^)+b(q!JQxIb7v!^DwyBu^gA5^i^=T5U%$nKT+>p<_nNqSC<63C0yqZJ=r4PqCq9KqjSc`NN+HK14B z5+y45>W$dtpZEisTez)ah7ZlcH?eBshxzdiyUp(pgq(su9N~a3V0Ba{q{mnBN20|0 z@z2r%`;>l$k3C@%5chJ!o2>tav-amTbMNnYq1^jL_GYG+4s+?(Pl;{Z+QHafEOdlJ z(LjDHd-K6~(woV-k<7;Xd3nF?GHR*MrsdMG3+JYTRx04u;a8hjqr(THRKPz z%$f+=H_zAXZ?grQF4y6|hni)&>~c4AW2OT3^)W88#i}5pVZ-1R%s4Lnyu)C3V}7Ca z#HcvmT5ZB+w&1{Dvl8{RoDn7VN#pnovT>Yd1&OvxbUB&{RsI)Cw4-YaG};MaJG&8Q zwt=^(rTyOiCr{1qCxyH<_dxA1{WEz;H5-W99DAoFP0Hdmq5wHYC< z*}101Du9RiN)uJf;{G_lSG6eaAK~|^PQ?8Qey{2`-2X~_pX&(RlVw3#dp-LBJTX;k zgKgDlY0IiMX?US&veW zs(5fIjeB+Fx{$88$DF9zZb(7gV-}Zl!vCH#!KO5_Xtmc025s`^yVU*t8tkbZ9YN=q z)v}7Yl|8jHB1p&lkzJ{$wS*XpFKR#aCt~qwvuXqj+C{ca>4O<0T`r#0x=F1@td zYIj@AcIyIUnRCHvH(T6xYd0SYC)jTN&D?&{c!VrIiqdHMO;PTl%AGh^yQOk3cK_P0 z9~iJ%+!mAFChE4?O%}Hw+oC|;NMq1t->0+d$l7h1I^sk4$PWj%`N+`WimWRj>>S@V zX{@^abfzDmC=z`{u+$iT^yfkQtV_RW**4*mR^_RW);eKY*m+Bb^F;(Ias z#)fpO)%MNukEN&C<@$Fvy#Ssl*fg7Ky#H4WuwyF+*nKN+rKeYoH|wf<;)x2z&W-xJ z{!05sf7jaf&0k}QTnbBs&N$@qa;4-UOXMvrf``WZ@`bQO9um^;e`SgM!&;UIC^5S{ ztos=-z_>>KzAziefPyj6-v@Rh1BY8wKg0H9AqCl09<$A9H5uUv=Huz=#d0|&V_8X? z%i*xdzM#TSTUM^O*37&^g6!VytfY2_-6lN7b%<=H;nr&QZx?edJT Jl2>2&{{USQh2;PM literal 0 HcmV?d00001 diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf new file mode 100644 index 0000000000000000000000000000000000000000..800531084fa6cecc3b7ee732bc66582d0f889aed GIT binary patch literal 78660 zcmc${34ByV);C;Lx3{E|kbO_n*#m))bapn$%Dzb0L<9l}VF?L@u&T%?uH%9ZGA@XW zGA_7`-74hKmB7jSgjJmHhOOTl6hKx%8*Qk=WeN=+j!4!KSG)s(zLFa z*Ses6=UnG2Li7p|B60A%w#IogZMR+zdfW^9Ch5KY8Ov?B71HY!U2qjzj%XEjaffiw zUBJ&qR0u!nwSjk?x1nRcHT66ip=C3b9ygjplZ9H`G_#$VDV<(Q*@HO~Qj<4_*Nojb zrAyx*gr*%O{3ku9AdfF^&i=RcMfP)p~c8fwuiJ|&cP8J@S z#}tOQUI_01;lA958!>W3jo2na&nP_~mIq}ow~)V~aQUO!f{R;dkWvYh5dR|Cm%s`^ zyixinQBbIpS_0@a^@;ill6#Dhh6Jc?P z9S@f%+Tgc~U%|glY=QrpOb}8g$x`^`uqi30Nzll4NGRpq@{jPJlkda-vkDecIaLJY zh*2?syQ;46A5o9N->5die@guU{&VUj_%Ex?@JUh)lKu=$J0T6_Nr)slgz{_&f)3&} zmjO%RLa>fkB21k!U`-UMtp;ofps|WXwZXvKgiGCEzz&h78Voon0CtN0D$Br!1i+y( zOJ^wY=y`K1P$`wSWjO7a#1w!-4C zHQ)gGrCp?nsRrHw>lk3bK_WvW8*nfnAubgQ#Vj#h%n=Jjy=V~)qNk`tZE6N|v9LPP zDCQ!@zoHoqng-A;1m(qej7E49LYt9do5|^X&VgLUBgQPmnF-4qWW*HlS(J7j7 zoepVQKu@`JBOj!-MsRe4ekLG_Js5o5;6IOP138oYehxiAHI8GGY?QiN6hK09?f}U; zr|XlZUMzxql&5+z4L+s4822QH8#qe00%bsOJ9Oe^4gSwH>@sEbfuSdu*h%K3c#6oPX;{w2Z>^${bW?Gd6z z^tq_bk=Awl>1=%$rtA(#xeD^n6HD2K^w@6ULCOW99J(aCA&p;*$0)>U6|)gO9h!GT z2Mf7W$nwb2axN%|CZpUIq9lEq>&1HX>8|HRwcP6>>DMA2)zW6jo`-ysMRXHnYkun> zJ&+Bz!G?58_FMl5j!$_@y9oC*v>V0Xzf@c=?iRlnFNn9q$Kng|cX3XJ$`sjC_LGC< zSXnRU%BAvpxk?^YHR@7zg?dtL(lo7B8?8;&W^2o|8??K$?b=7$=i1-2A1%rfW=XVU zSxPJ|mSvV3EO%KRvOI0sYT0c$VENY4VGXv%TGOm~*51}y>uBp0)@JLK*1gt4wg}r) zTeEG6ZKZ9MZLRGw+h_I+d%nG|eUQE0{*e7C`zHIF_TBbR?1$~&**hFTj%bJ5k?SaN zv^bVJ9(6qDc+GJp$R3mvR1|bc(9oc9LDPa-f|do{5_EUapMpb!6M}~aUl!aLJU{sA z;G2W*4*p&67r`fk&pCsfL!2v|w>s}}Zg4*1+~(ZlJm@?TB0@q!;zM#m%0j9`MukiY znHjPqJPspbs$3xDBUKYAB^u^FOLO%#S6nZMGe^`6i<6XLRY3MSq z%T--&>ax1a`Yum)*%aO-JSjXkyd=CP{L=7A;WNYA!&ii_ick@QBU&PsMcfc^SH!xA zjS(+Kyb#h#Pei^F`F7-ok)KEYBWhq& zUDTAQ)~Iz+Uq^ix<&Abm$3>5ezB&5t==IT0M86dMX7u~fe~ms8eKICBre{ofOij$C zG55th67$EHZ87i19Edp{b0*dv8x@-t>xr$19Thtzc6RK-*k8ro7JE}lb@iXJw<5$G5ieDT5Sp2Dk%MuzB<|ka8aC5@ygbfMbB>a#l5U>PX09cX!3U{&Xm}c)RZ16r74vu!&5F#nUT_-a!txDDR-wlmhxQ6 zIhVr~K^Z&=5BE> zbzkqk)BS+^u={(rH#InQWa_U{Z%e%=bwlbisjsEJpL!tm+thPujv@}mz|Fj`# zPNok^zbk!R`s3-F(%(+sn|>&xN5-OzZ5bys z&Su&(BQsMnb21Av`)B?pb6w_BnVT}-&io+rv&?TYf5?(q5n0z|ZOM8&>%*)AS>I%R zpDnVT*|FIb*~7CZWKYj-$zGbhGJ93_+U!TOpUZwT`@`%nv%kwxIbk^oIT<;HIXC3o zmGeN(lR5jlxw>`lR?@Aq+lX$rcKf8;SKUr@JJa3XJ*s=x?%lf=cOTGwLHBFA-_m_e z_lI(2ZfI_NZgy@-ZcXl(-08V3xl40z%KdHbBe^f+zL~o__mkYOa!=%*>0#>;(PMRw z2YWo-=dJ$v>n>si%vM9+ym8+*?0d412{_55SccY1!(^Q*k{ypeg==e?A7pjT?I zCA}W+btZpM{=EEM1+fKb1;Yy#7Ci53gtWYQIPO z$M(Om|KUr*FNwb-|B}lux$%-C1JVby4mdR6SjE7K2P@+$e^dEjRZ!Kusw=B*tlD4Q zR=u|-v1Vq?Gqnk|&9!?6P8#^ypu|D>gDxF()u6iv?Ha5G_Z&Qb@b3nHFeGY7-H=s7 zP7ci+S~GOv&^Lx94;ww~iD91(4;x-PeEINqMud)7JkmC@-^jru$BvvjvU%i^kt;{8 z8hQW7w?=+8vSU=&QJ0One$;cLKD*R*sprzBOP5~y?4^Gn9Xq;m^yQ-$j9xo>!|2VU zkJlyCRo0EEYp!dryQ*$w-K}-2>(0J7b2QQsC>29Pvg2%tiW^BAQ6fYTe(;+m&-fkUGf3>JNbL{T8@^d6==oUHdm0Vi!08R;!1U8 zx^i8;&@%LM)wwQr&2r6mUFo{sbqCsr-@4Yh{^@q2O^9(Px?S!xca}TfUFh!Zu6EBu z`*5@SckT`DN8OLRpK?Fze!;!j{c37dYHVs+YF27q>f|()W=rdm7MT{4mYC*B%S{`U zHZ`p=bJrP#XI6onxG{?8E!G=3Sug(}x632)m^>waR^3&;nUh!4R<%Rz(}ZS2P9l(# zbS+EkY2?J=3UNicl3i|BhO4_P&sE~;jaF`=YqG1^wZyf;b-U{>&dGYW9XW|~$Gel= zZX+kgx)N{oX7e%{Z$2fTZ|?|XN9cX@Yu-|83>5crGWmuZK(QsANK zK9WNvhaNpN`%v8>%NM)x-+0K5(9lDeMG)ePlkDIA#qD1#{o?U279Ko#@B<+Zz5(|r z+#e1;d~n6VX+nG!X6G3Mu>u~*3w`x)mkkKvR1oEyPdevPrFsSU0bEC)gIBF z)-i#;_L#O&dq&%=ZPB)B+q5?@w%MV*r+urPu-Gi&mRO71LSrX^E2OdP!?@;<+f@UerT3~e>JGI;EmhOitEd%o)dbb3 zCaTNTcJ;b?Q%zBKqmHErm&in|sucqQIq`#xkdZP$rsBE3x2%x0a)O*Br^s1yj$9(w%60NV`6!2NN-c|!th3Zfr ztKX`6El6FWqSUX|5H(m0RZ~S2T8m)JMnqs%B1xo*{-{qy82e8aW5on9QDkF=;x4gL z+$bIrx2xC0dhtHmk$1(P#Mk06`7EB0Pm2y2Cap3=c9AhMA5ZXE7`a|52g#vwxJ;Cb zH@*nCHn5Lu5ZONcIuK@O~r)ncTq6w~E+Q7^}eadNPj zCdY|Ma-bL^$A}s7axqh0fmxzCa;j*N(?zqa7p<~EERZwB64@-Slz2jsEpo27RxTE6 z<(=YsxlG(A?+}m3`^6J-y?9DKB%YMN6VJ)V#3uQacu77fUXUBb8}dc*x_m)wlh2FK zXH+5WnqYkMr)Zf)H^_4m* z(qyd2l`hdkb`{+)4_PIPMWrkfg)&nNmjlFTStCZtDsj0SA?8UuBgrN)S56ZP(L!7$ z=ZmW`!?i*#6w73*cv#*eHpqL$I?Q)HAny`?kQ>AwUye^vruT&F>%H`Cv;|m{=(6EM4}~$7BM0gGn^Nr z>GB`Yg#8CJq5lC*$bUfN{10e?{{xz!|A5AE5t=YA9nI8XXAN2qN?O#Z{F?4e6<=T9 z+%7%S>lZZ29@D4KohRMwhc`6OYLdYXEv<87YatUi7Kbw8n1#E_9LI{_@zMj8OhP$v&YbBCaP2kfyg+$#YKZ5;W_A}vY z=&h2`?^0MI`x)?Qm4N1QgV<3W_uUg+Iw%y==GbkvLDqHFGW>;Q2mXDv1KKY0{|rVS z?b-}&0%q?W>ID9W)JLdO&%&RrCgJarXVDYqp#|Q7zI`;fQwk37A0$#S{`{LfF8?mS zmH&_@P2 zj0grGhd%xidBp5L=JR8aJ6dZYH<7Pa=-!2XJW_-xtPRL1Dja^T!n%VjS7BIfDF-fu zueAZOlR376zHO=tXve5frWpe~)*|5h@-{(qQ%!27nguO1tA%QjTCA3j*IaouPtL6$TwSbUniMm=iFsiu?YY}&2#1)A?=qnM0KItDKUY%5@MXLG< zt1a2cl`gA9YozPOLMsz;iJCyU!5p1GH(^L65~CqFU@dUhLFY@LVX~79=$W3M_QKIR z5XDP{uf%nrp%7ZN=^P@(4@{@0+~0fL;FV?YibQCh!K*7cN#$1Ea8KN*6nxxV99E?I zF|AaC0=U||U*b-x#sFMCYKbpj5;Y8~K^&88BN{W7{yQ7mXi7ImxePf-R)y4+Xp|FK zUR`>!!3;=5Ic|`oJ!nY!BoW}F>ujvSUB~#epV3L8Smd9ys&gi(!r-K;2k%&fNX)EK z>5wnblSny*?%5YCgD)=Ca7yzS!?RQ~ z!HmgdnoM!Z%)7#hZtVZ4#_1NGY*=LmIN>>n`PpHM1Z{<*ve(;G$t#9Bg>Ek;B3=%Z zm;H}YRl!VDUhA>?N#8sJG zPoRf^Eznb%K}q)P#$3cRVgpteSBV>76>V6Tn2eQ;Ay}8|OH!d_pfzEtcd&a{z7wro zBdkQJL24FOW}DFNHADJtn1AV~1_H`a9@SUDo>jIgP<>Plpe)R3^j6h?GF2~CuBrfK zs617sDgmWq_NG)-07_FmREZh@C{^XEVs#00(G{&tcU7eNL;7P%&zoMFTg2tKZ$vxz zGM*6rKlWX6q@1Xt5woJl4m1F-OzjdzwJA)YL34^-Dtq|I{Xz{hnG+` zZ{Tkcr_qaAxfgY@^<}d4W#P$siO5D9@+6)!UqnmN4VvjEx?^-b4XfNU&=VDNPgKG^ zQ7QLCW!w|>=ALK(_e9lf7d32cL)hBt*xD{bo1LR>7HR(*PJ37ckyE#b2DL_be$nj_ z4OYxfs6FU40UOSQEl-LD%TvIY{uf+L0M8d8<1abLr42S^V;RxbHK@x`zPO50-XCza z=+65wu2#d1!xfIhd*Ipu_m%h*ds|L z$OWf9h7RNOE5R2!@3-0)0r+naPayvPvQvMCj^Qpy_XzN#A>TmA6$bbo!2il+iFEa! zy63^`BzTTNoPXus1>Jwstri*DZjpYzQ^T=0C0#`SNA5b&SDo+1qh;jx4BXCVZ*#A#-m}y8*SD>$00k;+IcDUcdJq&jT|3OwZN9Cjt{uY?YtNBKOD`gAj9x^ISFX8=C~_-BCs1Mt@X zza0CNp2pfn6xKL;gQhp0EM5ohQPis~OxF|Tk;kr8^boILM%}<;RyqN$C+080wD)1_ z55SI^p`#E{pt*6)Kp$U)JePqF#oa1~Vy$T?W~4osT`s08;ug`kj^oq#P@*kagY@AK(vqLjkXzeZ7C3VDgA75$Az9)^1Z z_mB;`WrydAT8ypI5ig&d{0aCoa4&%F8e9*HKCsyYZ9eSuOO*Q~kcDKp8gXFf82hDj zKNl}XDa5ms0hfvL73^|wB}ex(V?u5)9A>MOW1zZMPEgans zF|Gvr;`!)Hd|_A#7$qJD{a=t5w-_tO<2nrPJ>U;x<)9pCy#jrY11<;dI=HLglECvH zrhLQ~!!>aU+)y}Qz6tyjhzlsXEll%vHV*kc{m7sp_y-8eZO zPluPuiSlx+z+Qn>!^zOj9?Z^6#X4O*c4SYN4YCpSyGb_5nQ|6ZaAso$rx`o+T4bx7 zC+EvHxd3~%7s^FgC0dMCtR-0eT!y)ULh*N$Ua`CqYoS+T@5ME81y+noudS{YA9XoH}O5 zz7>o0d3CHK2F$Eu#gXUM#R!^R$7=9-^XuYEv>sM*sehIod+0+{s0u@0v_yqt_HL<) zz&yy6DoRCTrY2U!sd#afN>GU^Ni0*zDn+??7A3XwtcLiV>L#wn+)6H5z&~Pa`z>aE z#$qle4|6p6n7i>{WHb)*3&r9qRU*z}j;0KAH@#IKakChYIi7y1Ki1_3s0vl7sxUrX zuBtKHvjTHJzru6cHJBk9j1~K#YM2_XMyQc$6jljGt2*pc7^}vK%fv*i7EVx?sfprp zyt{B5?d4{yQCxw&0aMge%r#wsIgPK?bnIxr47)jlkNGCdZM5(z<9yYo7N~Y(PM`KW zEW`eStFT8QaDIO!Rv>8|l2#;d5!=v*J%B#<{(!Z=c=T3@0&6qaV{ogw4ZA4rFjoau zW5#3+b|Kt@Jqq`!`>{9S0qkOU5Nn#h!>Z=PYJ>Vc-eGuDJ%-&38+m>6DXeb(0c)MK zzWE&H+rP&g+G+JXRy|+Ds^=zpvq8Orxw9?Uo3RyZL|u3%#uJzwco?(b4~aLiZy^-( ziIL(G>}1-89UE_8&&GBUj@8j;#dBEEd0XuimDoqI3;moEvvHHfRP2EmfVI>=som;* ztf>B3Y{$Ob=dnBYMa(MEUXHEkU#*x+{s8N&AL04`5s{7+r<*YE_iMaSaI3gZ?G;nR zQE{#Mi+Drr!p(K>VmaR|he3bck2hzfwmqe{@t_uf7p$u(JL) z%zFMqoDiEZt9XY)niMw zmHFj+@;&y7x%JcAT3hTDtxc^hjdOx4+Ge#h)lXm8-e|9=(XT98ewnjs`mDC;3+K*g zZd?*v)zI2rKYe;*OFO4fP*r5Bo?cIkn7Y7&+qTwv4lSt3H{{Fb{N)$a>gg9$729e! z`?i|E$Od(#p^YjdS5=1eRfaaIbZvO@i-K!9iBnKhX(*u5r+^ZBjge7ZK7(aJk-bLG zxGr~Pm2DtP;vDGDte!)UCwSm_sd(}&gQnHD=}|ou&Ox2y6x4W%)Zihu!Db1gTLU{$0mwxFn3=TgLFQBYH4 z2vE)z3PCE3@G`BYrO7rjKo!*mdQi1dtks49)kg7F8!W4JmLA=R@;x4-ObdPHV3w*-N)mp~DJZ)nF#PHFU>KXDB7-}jpGEq`v8yi?gg;YlQ z9@G=AH#NR%iFK@B*EL4MwT3`7KHVEkYidHrUa0GW68qRdU6)qa$LgkT9ZQXnZ7dtS zo^WZIZ9J>TIo_|1pz#f}8rvEd%vxX@-_%yWsL^?uKTwZXW)ydAk?k@j(b2U=X%rf& zt1YulWcI-m&#U9*9$USs5R^aHUo{xyak;IDC2}_Tb4RT63p|CvP3NWNDX`4+H_4UGnVsXHNiMd{GRrf+ zKyQ4Ye?v-7ZP2Vd+TnwpUAp|f7OaXXKv}6~&9=`nvM|eNoM)M~n(rwr)f?#&y^)^X zSp+n{r6F^g+8P^Mn(JE{W=*#>b0ONA1N2^~XTG4Y%F>J+>XH<)reIr!tcAX|wb0h0 z*Nv8XdcnD^b>7TITT1}DB3<_ddh3i<+Sh9996hC64Zzvgl$C3ZsDQ13iZW`7*=id? zR2vphZLq91SnBqL*4ilcLZ8JL7FAViZS}Vx)kaooeB2DGYQyeo3PM{iY(ZeVt%0Ug zY{V|+w9r~tI$JMjtqsmay4`y`)i&$}!*IveW-1J=wULVgL+WB97rxq4Tx(kpQ1wcT z%$Jlp7o1nxo&rNrB?hY!L&%bHOFOmB?E%U9!bC9sdYH4G6%-iYmlhn18>7?E$&8+ZZ-Tk|V6cn7C*gZ%Jr^az zwk93b42jYmq^>VAc|yZUG*wT6C~+7EWBy z30r0v49WH|5a&S)u3Rl}<%SMdHac9na^cFA3RfOF;L65{D-V8fE!SnHjiD?v10*v6 zhRg)&GV2h@%yc9()9ErZsLRZREHgp6%*N;~zmltTetxCie&_3>5Kn%oA#hJ>R3`l0cAuN({qpC*hBIFya@SwtK)E{W_^YuqeJaMvU zxK>8ZYiyg<+Ca~W7_2qq(av1ww1v&hjqMBva}&*A*tGiTbNsil=f$Lc8GEynE{P+gZ#ZyQm=@|`o6qJM1B8;JJEpt)$?e%R-Ee)+LO_rL4 zZLLPQQ3WchEzPsq>J6k((3Lz!zyoTDZC>L7#5aPC>Ql+R2}Vgq`RfB+kI@a{TI^_? zJFk5yVsgX+k8Ycue0@lsUs=qfr^5Uae#E8n)ZzR9e0cy|XuxRu(EjW61pDX-_R$l} z^%_^M1Gtt3af>>=b?&qv?mP%0*$`G~@T_5#2&85`GvZYaMgRnLXRQ5hqVp7 z{>WFTkFxR$3%TOwdwhe|DlRG?NW~xEIHFXh4S{Zob3B0KayeSF1 zso$y@xivxp_28)qU{I(h z#%1f#Z9d=QdxkOEU61})1iY?(k3N2en#*X-i1yzFV?}3-;P*^KnEsv#=3()}dknOy zg*}r(o733VBC_Vzx6Ki*xpU^u!P+6MdFk&#(3%uc>HKWj%Pei`dzFK=i!jXL(z!dd zCz*Ci>wBfKcQWh6tQRRp)-&u7n@{HPQPUpc_pk~lWeMhXGqKL(lJPPUt5-IN{4?g0 zzr{T5A2ecl5AK=g}?6+xIz|7au;g8iO!GDDPnd&g$7c?9E=hdU| zH!|nf)s28($NP8Sj9F#Ss8#SEP!GX>3}+Wf%mP0HzfIi$f4;g4{#Zr3-M6b&`0r!y z1kM=JQpv}Bu@o<12fY-JV}@6X-)ViwXZ&OE@rFg_AvnA-$!;&M*dd&`3vN5yR=7=Y z&%!+k_Xym2xclH%!`%*d6Wq0MSHUfUn+G==t^p2fisCaZY_e~JCrA(-{=ydS#=t15Z0&@{H4-+S){-9~Rc z{Oi06;E(Z=9>#hX!{5gK3toDw;t_8d{8px!=q+T-3ixBaS0D|Mfi>8S9&lN3ZUcr* zXJDs7`qTP-`eT3}f?Er>2D$j<^P#MQa)JHCSg&jc<#Oc2j&&*hpN~~lywQ=kDg8y- z%MPl|L@zhPYEFW{hMl55mJN6mhO5s?4fgND2ooHWB3qiXmz1`sOJ!8L@-yxoU zrWCI$ryqu_2Mo6l*AL*{gWCbO4Gt-%KM#jIo|iw7z6!Wo;8wydhg$;I2Gq3bWWarK#d>;rceo5V7hF7CBwPrb4GyBF{R{`0)4qi}3I{7q`&7s@yrn@r z)Aqpag4+(a6?w7;p3ueTa>envTuE52$Zg8pb-#%-^VanvAFk_e6WWRG1aCm;qU#nPq-%q5F9P=r zLmN$Kg9%aSIx|#okN+>N*boC=(iDLe(Zjxc*F6X5u_1M7SOtnq}hLCX{GG(I({Q?1xAeolBTW=P)6^ zRLX~Qoi(A;CUnAtj+xM5AzTOG_PIWQ{~qPvwZnwAnb2kvBD&|9&L8@;8TuHYhv3$_ z*1%r{cZ-W~D;c*Oz8*@rC5&r>-|U(Je=6KW*BJOCOxz$7sx+a#CRAcV{?Po3;3$pm zoJNKj>M|jJ8u2DB5>SZC1|M0~(96{;5-cVFrh3Ha+^@12~mt_juF-g=U|*a2GJ=K z$57H)93^=g?nD5DwkZI2*bF^jLi_O=yh? z`9q1zD&}&F8M@MhE|$i>jV8#xCsTD5QSRIP=*pZe4zgl;vV8%$`0 z2`w`rl11mzZsPoDw3xV=CIqW9q@QF$<4kCj2@Sz>!ywLaHE{hIDl;LE3H30cEE95@ zP@)M%0}6w4By@%p;o{E*hQ_1JfjfcYgpb7^hJV0>_LO?J%KjCbZdvo;M*s zXM&#QG#)cUA2Ok}K8SJ;m}FVw!?9GW72!qaPB^#`%%d`%a2e;(xo{uP;RHo86oPcO zGh{O%+{XP(xVZ049ML_?bb#W%<ch)!Rj-kT?NuG(oYaQ0B)NI&MNoObDT|2RU?qATIuC#(flsi`~t*orv)U zLt9MfMH70)gf^NG#n`|x)^)<&!??RT;chi?I&?$qir8hKXpj9RsKpGOX+qN&n$#)o zI5Tt?VCX#)+F?T5 zOlY$Sq3w)eJBWEqZ>e$rkO`rkq_!QnH72ymgb*X<77kr$;+C7x5)*3kLGd#XqnTs) zp{XWrq7P!aF-$iCxIr#G(rj2o#`@6oXdpbO(@cYLQKeJLWI+~_&Gxi zJ&ov}O$cqLf&116#gL>NH~OfFGdV{e^5LS+Oe+?h!PrxSuq&TE- zb_Qs0&If2hfcb*>0JEI^1Ms70%>bc28H3dV8LD1q_*u}O({PdnLI*PD41DEnVi>Cn z;PWo~YvBLL=_>YvC|z-kDQ%P&Y2$Qlylz3Qx0DI$1@@6&z#oJDB&}!2PF%y7=jLcA3rk~YvLuOWH}Ec&Cy{fMNF{<;B-##NjN|rN+^K~2Ew~*o+j`fXh103t`Yy;nL ztZ%doqG5aZM#CxsV5E*S2T3R2FrOw)eG}zHY$BOO1M}I$u{JUNCS4oscd!f{4Ck^f zigLyla_F1sm__} zm%(Qla-F~{VOMfnq@UZ?L@SQ6jO#@i>#dAhB-xkkzl^n4#^r+6lPEcrM>xMJtn(Pg z{Gd_$mZDXV&)iZtM?bKIq%h|cqoz~+%2xZCK85M2^kca!QrNCiIrUho3*bg2n8Gro zu#KH$ONeE8Qdp8aDm9tM9P+5VAx( zUCRDCSi)JZ#gR-CNh{L`B}?e1jUZT$#Ws-5a5lq#=2%}d{ntdVBANbcrumv_hA=#Y z;X5tVKFpw%a=gt&7BvGW8IVts%+OF4@JY5A8l;k=nEwoxYKBJb!wi;c2B$cK*4pKr zlp@aUp!{NV1vp5(Ofd8BPV2t1J9FqxE53lK+%vg+yEC8eax&pr&h8v5is_@6K8jNe zWBMrVEGVNmR)6hV!2LPDQ5-jl;|}B$2eNDfna@C$d?0fh$Z^{^)mVw&07K?>ErBuduv z3fA)q*7FM1^K91h3NDKkEbR*IDAON-|5pubta1hAMP+OEGoEZ?1?Oc2=ky6qcLiw@ z@9j}ptY8^#0=dga}ux944PUdil%wcY8n9m%JJBMRE$dnH<=0V1|n1hS) zF6Q9kw)Z}cYvt0!7#K2~CVJ7zv3f9N55|vW%q}k3A2?Pj<99LrQj!xRD{3K^5ne1d z@UT*`lr$!$lg7kSl0j@BO8_4B2_Kq)KZ$+VCqiML;uv$gnN#_ax$S4nQs%#u<(bJj zn#oelq@4$H#H4`7*3X}={rOWucTN+p+O zCCf0EHQd2gF@RIA+LGeBfSHUvnZMJD&}?v^IyaK?_kVk=6pX> z-a(X#wT-ur5m)J2t>HY@XqSUq4d<(d^VN&)H ztHv^bI9O)G?`4T2m?fWQNhFwaI*;U0gE*%(EL9DpQhahEM*M=l0Tz#a9*b~7;1aPM z&IO?N#6s*yW;pB7_0){1Cy`g zedQQ=25%>Km1h;c1SZeXm%wmv9livH6GrhRFq|+-@5=^>QX!NDD#6%}-A}k;+$@j7 z9f3Otw;%2!xZQ9&;ogAT0*7ZMyg4rLF1a8}lZPY`Ey{^GRG z4{ymT)F$SSlNJ=-P?dNi7Q}c@mwY^x!5_muo&SJ+TVxs7T}A#iG;$R?)vw{d!I;(T zzrg+j*gr#itfs-=!XvPOYBk^++2>Y6Ji?S~a1J{54KaV-XMpcx$P#=u9_$l=lXgrG3t>1&ckuk3`{5s=TGvx~?14zQ~29z0gdog7jWA0^tERPqr zv;P)9Wxd8UuQB`*`!C?+4eaw`{A0ib?x^~izhLLa`^E}8kW|=Iy(5{$}9nR$< zmlx2H3E$6)Zyox6-aX!LyeGXUkh}AK-ZR(%c#`qKfKH2Og1zs;!QSZSea(CL!gw6= zz4vD(>EwA&`ft65yr=zf{g9sm5OSdZyl1?h`Cz2`q4&5yHp%JxvCMi-V1F#{Y46uK zYfkv6@Xdt{-eW#`{}m}5_R{k%|8U1#{BrprlO{l8fjOh_&anez4T$68!}LZTSbiFH z860#PgB$Y9A%+$^OXNN1{R_16u`ey}5tQp@@7J(}fS(twgZFQ6n8hJV(jy0>mg*Yv z9?{7uM+p4^n)u@U+!++7`AZ%qcTlqAM(Cl7$MU6PM*F`8{HY_S=X`8nu|Ii#_I__j z5nvmH(dm${llULhX@LC^|C^A1I?38^DbNgA1ZjxNmE(eJ`w;TURtmpUF1_d#yl1`Vy#Mr~ZwUM$-^aY$5vrFt)uY|s zLxB`J430;cC#8cl;PxTJv?MRJl=O439@pY(a0IRk=va?{Efkx0z@g3XQE@CUG5F7Z z7i#Q6Odu!kT9oEZUi5(8n@DQpd%O2FP`GgC&&7uv0$P#`^d}km4%T!Ur_32Y)PV22 z??7@h9tCi2@Xa2-9Gu4=!Q-^QbwTdWQ9E~$xTd}O(g81SgAt#t3g=%^Isq*v-=pXE z{ZPaIVZxnkptC*rXacVnr|8tiqW(gbPL>tO;r|I7{(sAb?9u3jF!nHieKA7v@ZUr+ zprHh441)dyWpW%fhxJOnZj%?uCzrR+Vmq@t|L$US=zJRA{rMa(h->vx`md)lt4#Mlb zSYSEnFzOqRdwg+m^-FAwRrIin#QldamXQ+0HBu!E^kC$S^XqG$FH#Pif1jS1nLjl@ z1S)@vtS5P)uW-^7%{G$kn18@2I^o!}9*KV_dYBlzyODx_xX8pm0^dmLg^&XLqi`Zt zAt;JPZ&3BaKM5zF4Z!a8N>PP2wnhvTZk+eqfQ{cx_-Ek!^I70=9sXH33+GXs_D`qr zXzZ_e-=mPf#f0;i zFgso}hM!nbbl9VhV+PULzN-?@1X zr*8coCv4&LAbdyS3$YC+UVVip5&E9x)^A$?& z2y|p;9obn&5v(H#9YqKSo=zk2EJm7AtRp(tzFb7%Y_{Gwagj9TU>(JougWc-B_}^z|X0^*+LrX`=iXx=VuY_T!Aq zze0mH)?f-w3p<8K#R+Nx&YdMaLONI!S?Dk8kt`JN2jL&Y7HVY+wX%g;*+Q*sp;op~ zIxn&cCqUKUZ^a2{wXk$5gD@_GE^MvgY^@PSd62b68l|!kHXFrd6U}84!)0URvaxa5 z*tl$LTsAf?8ylC6jmsvM%O;M?CZ5a2!DZv%vPm$?hDs%oOC_1@*U4o=UjQi=DO@%# zE}LMs;;vjmZZ4rzE}=9oA!nx&qEbocQpw;_$>dVW;!;suD%o5rIb15;xK!+1D&5(x zgSb4BP!d;BA1cbVJ^1g?R%^FtE47K*0DK=LTnom@SRdg0t*3E5*fQ0i?ZL@s18|aB z4!*ha6V7`(DEH!>t{rk4POjU4)9+TvWtbhAh|~Ng%Klhs@ZjXYG#Sk<7-t4f#8)j& z=?=a`Yej9yqvA1?$7|wqK9vx)8u_K~tUZfU@II3JAupZ&#wWiS=f2VTYQ}kLY8uW` zqm$I;^4V$aI2AtdWHdVE4Cj&IyfHd$Y^TCJ64s%pef-?J2XpkNyZXZJutqbNB;M?FofI3NB?gN(txO0FLz5kYtZ=}Rxq>6Ol z!%n=ff#R5a*84Lkjv$2&HN(3B=in!L4+8r=u=swB_qb{VRss7p>QuDO39%33jl9os z#&RO+AE@|l2dEAq-XX}52-zhljw3GQL#iY*e8_!BSZLKvC3_F43BX(iDv7&&s9P?; z1L3ZNdmO39;v7%PX%f?J2kma+gZZfhNP}7oI^u>`mJE)>fwXfR9KS}c33?UbTj93B z?Sy*=x~80-R`r}i%KLG(23mHIM(+`Xo>lGM7eR3bsVx<$>S~1C zAe`z(v*dKI}p9d*AK^KK}ewUB=;m+p4x03#86@nH*k zp%|Q5C+dm{Q#~;e(dnsa#ZhT-GPa~Rq9|QP1Dh?gY%w|M^4awCjuGjlc8grEEY^;} zR{4P4(ov6@%6rq(&)t%quCBD5dmQJFs&UqH*IR9BJYZO98d92pGEYSRCn_|XEj=wO zJF8TdloWbmky1=FW29D^8Yc&yQ^B_2VC+wjp<0mL5sa%039>s?$C-^`vN~AZd~T)9 z9&ESR)EbA?ZaX(yvjzp*ZE9u5pqw0ff0fhIM%%Jr^+MRO?$#l4O#ICzK2og0n8~2G zxD0%@#&{#%_Z043tY&Dwc-dMa+HT_e@8XAwt%BoI{w=dF;2(>x;@xScKj4D!XtaFq z`NFmNrrZ%|4f9R>G7}#T{6rJ~kjannlpnc3ETM4Dk1w7-Kb_<6z94=m)!^eI1s&G3mvxLu1Aclyggn`Gjh0xL&8dDT`E&-yp{_{*8}?Jwtb zCOz~l-ZJTjnfQ2dPe8i02HvK{TwOp`>m7*GGq++?ff@Lkw5;Ny{;D+2F5@n0!1zR2 zOmr;XgJ+|LMcX8-S@lmX?ULXM4-V=$)8!(Dt(-wgJ=uXJ9u$+>(Vr~*Wl#6G zELXHuv#XVk^NrnxN};TU_>OOg%@Je`jSH1`!R#%D)nATwA_*hW-l%D5SrD4#p(Q)pcl^s)l0n=)+F{!O6r+t_!$|A37MHm7s){9n)zTtZen6?g6_LB6B0RM z$@y8hczrzy{l}a2^=-4f+RgGJ`=MF+Ag@=OkCv-Qh{dV)h&V~jM{yB1D5)N9PEt!F zvM&OHtT-<9FE3ai z%pce*F2tc$J3?}zI^K+s!-n=A z*s;~tvC}bdL{Vi|=+c8eq+RX6Q<$iTjfE9wXTe0mAd*YrbcfQC*tl4m9pa_sDXIZz zvDIvcvRb>?yH770&^jhRe@yFuvT5Dz;a01%#}6LeZP4_u2)(zh(H?Qjw-3YZp4V|>Ps@9GnRsNh8}`jDtwQ}#8b$p!K ztX+!`Yni&iTNXfVS3lyEThh!ga6wPt=WR#NMr#C+$@}Y0csI_;Wjuz(%hVP_Ry-}J z_f7njCO#W@&VxFtl|wr0T0X`|T zo_Se;$;FquEW(m*QO2+SVkk*~eF7-afjBRG3udnTeY&qc|0~Nx6L^0A{;uA)J31aE zyZj}K4GoFRN=l1KvRZ=ec6*eYI`_=PC~JgntrxRnDdRHZT&YgG-oKl9qMlIK50ztJ zy+W(C!00|Mnu%!J5~?1CjQ403*_Z3m8#bfTkQkPZgATenI-Ylg5O-=77ou+{BVC!z ze_X6@z)n2{4|_1~DJiuV$8k4YQiwhx1_KUsCh}gi@WsVw=}TwiSS?{zi%rX%P;kW^ zmtK0u6`uM`OV}GE$4(?}?J+mFxPIk`5i9G9lajO`YjCh8V-o9DU2(;oqq`QJvvrQ*MnsrJ;RK_N245$p_+ zU;ChrEHs=QacJCv&b0)EgaoO($oIn?+wjn4;ac>Sky1C&6KSFBF>)}9KC@F=7pA;~ zRIV-~|J-3;+80P6ztQy_HqU_W z!wTgWTyMJepHgJx5nHLLovTA0Y+9HH8o}4G1oqq0^P-N8+)lI>V2`t1>ez@+$tra$ z^#nRL{8Q+N_AW@FQTwc8H%&JPVNWH@EeW@*53Yv>U>5oa9Y+#e7p zB!l9N!|td1xy*D#O&j*an*di-`&k#xs)Me$vB{MH#hYad40|s8_lxH8hkThe!&nLg8HpYA_^)IX+$GQ;2-5u z^4aohw);ELiy*nk9KCIz2MaVJEs?;MJnBde=|xZP z8027Tbp&5c(z%q890uN|eq5qs@s$u0YuA#o-eU6hr>W|p-BB#d;!gC50<(;a?~U(Q z(D;>7&6byujF=HUq`Dd5zYd6J)7;qNsjvnmR2p;>&f{0I-_lZUOQP+t+$i%w(_dcI zP|{AD)oBmiscduYa+;|<^sjP~(-CT$pmnjrm$G9!_qx8}fZh_~9Hgyy7NA}vPNvHe z>U7a^Q3o4QDD@dMS?I$Ql`OVmeB3o!c6B%%!J&?z5NF4C%5Jp=sgA#gQ(*-o*Vc}) z@(HJsPRF@s5=b?5%(CDoA3TT0ToS+@s%vR6rH?XbC+%p7wD-@Di=MmrS;!|3?A9*^ zHMi1^imfIGK@3Zf){!6PbOZ&5*enh_YdbJG(J{XT@p|^}u z9-#v8TC$7_;6KSnuZ751gMM)*`b5GD>HXGQPI!FnNQ}2pZ&TV|8QqMo1*5;=X%%QG zOtS=sIPI}{Zc`n0zna1m)3bJP-p!TySw9frlG;xTf;tUSTp61 ztQmS@YyRGl``4Z1c4HpQuSaSfSudEEdz|uv8eE1i#%#grtUfk|jicWzjk{XN-rxq1WT{+5kow{n&s8v(hhc$6pY)v$)M6;v#?w5(D-do2EQ5L*OBA44#SqZ1VGmhZy zAptgF2{y};@kUu5^p&M0`hsw(dz*aWTE_+9WE0=$BNvVK&S%kFxqn>8BXHm2;(1f$ z>6lR0CxG$Xq^;-n`T2%l^-CMrf-joUZO3}6Z=U=;n=RRHd8209C9aYMm-DD_%7as@ zmrpOJw%^k6Ii?qaoVJeF?ZN6Qn`ivIe&cSdgIpN>;T!W?u=0qtx(W}^DA;Hk)fnR7 zQB9v*MT^gqt24{T_UbjZ{D38t9+@2{@+vYiD)K(lENV;sxbm`zMfQ%pde7nr>3F!? zpq_bES=Np>Y0o^Rm54VuL#a<$>`wsWT8%q6>Q>UT(Vx+<7gF3xGLs8EU?dZD!bm=v z)7ZjVaOtV!=;;f#&&#zG)T|85m<&SAqH(m|h0#0-PmWxoc)}m)qLyj@##}WBky_F( zxzD8))x9E9VuBnY&fiV?jqFidF(f~?EIcf#&}X;4YbX1y2y%2uNeD|yPjX_|9%3!C z{IkLflBtSS5&LkwuZn6pF*bqoa2yCvOK(5_{>?R@C za+%oN#00|S(f_@w?w%Q026q4aByLT=uC9Ld>eYL%UcGu{vn%e3qN?V$l2>g`3A-f6 z66=>Ag$tMF6D@P?$>psMYr|}JYg?&`r521q*%}(@Z|SPGJF~yAI@xw}{ZwoJYBl=+ z#sKy2DSXU*NgH#B?K9w{uLazPzof5e285Ow@JiFGT;7U$NFR#wL7d@6a(gHI75U}M zv1Y*G{JSK#Mfp>)2*zo$ZpUSUt96D%{`DHc;<#S0{#3t+Oo+4Q^F+E2R;Cs5g=9jeA`+ z$%&5SN}>|SmQ(c}K~-1>L{$NQ39Tl099~zzA+em0H=i(`6V>UtBKt1L)%K1CjS-Z; z^Ig*&{GEM78SSYm+q5+c@h!5DL|xts$gYYktSl&9iMSx@4^f)z7J;!-ld+M70|n*F z^WoexB3i=X8dky@eqH(y&IVk~b)?Z@#PeKiP>=B?v5ZO`1^=)tgo|G+lkr}pr87QI zv<`fvuDd0`uqtM@naf6MY%wM@q(zy9GBk0N*^&i2X`{>Ky9Rat|nB_dxl@G@Z^1~W@Wj-AKh~H@N0|oGMlbE?i zu%4D*f5`L^e4l1quj72r{24W=39$}Leiw*X&TBNIp0_zB&YzszKjE<3oIN{w`%dX~ z&^lOl6<`zlC)wRxEv@R?(c8PDkM6jXqA&1q{0MKco7D*E$l)~05%3~-v2*2V{0g`a zo)aN8sU8}O0`3J|cykF(Qd7W#H2Od}-ZDPA1l&RU?Ku1Zwb&?Mj;PdJd71@?@-9SA zaw*R7q*;J~2TX^>iV@(Cacf?{%dk_DW&!lw$Bg#E3MV*vI@j(4dGF<(=i{-|`}z1h z#=W8R-Q4?yc#)-7_`KNk`Mh@0EI{Dxf%c$TfS?y?69F$Z-Je&UW&r|Tf)iZ%8W^X` zUySc?INC{c#;$qPH_16`fJ-m#am7GB5p!vd3QbxSM6tlc_m+pe5u4whJY!8SZu0l)fTrZ{g!ejEyoC&EH&e##OEEn_>k-PP>0R=$N?RaivoFc8N)TT53X`0w z2jL1%W>KXF?N+mL>?{Q?KBQ!a`^w=xb{&(>$MOVpIn2E;!$Sw(IILIHuEhk&ZTb)K zl|az1`h`yCKjd3r@H`_qEC^?N!1{pfP%U=cdoUr9eye_4!i2*ieaH4&$u51@X2 zR8)9&AIsL`k0RZq$Z(t}$8KcoQ9>-+#H8~SmFUMY;(Y&vHqP%5`kFB1<5s}E@a1tW z%HgCf1>Awi@1^Q-v28qhrF!8QPDlPe{MBXjEv?mYf2Sp_NAQ8R5Y$+Cfq&GG03)fw z>a@HZIILttP76D1aakRfY$*&nIP708{gD#1YD1%5OAWHfa_5N zeV`5SG~neFT>yKa9Wu_0eb2Szfu|N|)5DGopE&Vu7gh-wE;5hC$+-yGD`FPs4Ry^l zjh^1ufBHybraR1G{>=cMcDq+KUzojP3-cuThIJs}$N!z#rA%iuCL`3B9zuN`uCvXFt%v-}#>D zPX6?4v`&8~E**vU&W`w=6TEv-C*FM;vi%OD=ILC`7e(;qi)Wu_JUcb_?2=l%`8;;F ze^Y-djJm9>gw4t;Mp_sO|8DuTUEhAGMP|r8ekq>1|m2b|Clm zUV8g!(;KGUM%`7pryqC+PhT`0GmRNfE48O_j?pV<<@2ZR(Vt4>9i^U7q?h)_4d@pQ z=#$&97>HgYEM*O<3Q8NL?>691Yx534i(ebHym;|DM(H0L(3j7lz0jiUM-Au;JK~&k zC$R%UMl9wu`CWK0U(A@y(cpu}O&MpE79moEAIH+b^G` z!Q;|%rhn38!Aqu5gFZUHq4#zm7X;NNPlG-crX4i05dm4E!fC)sq>`0_@r=b3nQUR2 z6Nl9fZH}c`#2Jix(z}{T!{^P_QVk49J(LWt zsEUjyu};AL%Wmmjm8c%+inMN@tnEvAtTxqduG!u@eLlu~Pvfe=IA%K8$nt%|bsc3* z^MlSnQU3->c2%|3UC9nKC4H5RL7M{xfLzfQI`(RKIMuy8YAfoPidJOSboK8Z2*v_< zu3GB*hW5{{xnQuOdu^}PRoT;2mnu#4mxU?@n+2Ufk#8$8<`I&GorLv9$utic7{Q3q zWguq+S`~?BxRW3^SBMYD!MwH;Ia%-MY5kCU;r{l7*X1Dp5xB$UZtNVW>0X~sukY?! z-`2KXl1KN?OkX%r9PJ3ZFXcl20j>ycQKMb4;+PPBeWwl$@9ZYMg5G0)Lhm(lsZ1E5 z$71oE=oRiQ-b1+#VyWg?_;v7kkGn0}v#q=14^i&1f}qGa1{1T2^su=F2M@mM+^?#N7x$WUo+jJ zl_$7fPh8?~l5L0-BaJOu3E8H7=dkH2T_WPW>kar*rpHiP*H(v&o_ZH;h3VW0wN*{` zOw-8e)N2jXJ+;tQ)KOepwQZ0NVX+3JyH z)w(CLP5hV(v8e--9Vd;)o5!O{=FH8*P6>ND9L%$F&$*{Z=Pz3p8`#~KIiYR=nBZikT6dy7(Ie8wrRP9-SoP?sKVx1mda4WZ{g46w8|9JF%J=2L-(n-| zWDX~NkSl)_yY|Ry(2iMp4z5UVo~_k_Sd&9$@l}d#UC+OM1oo&t8s5NYKxCevuke&` zSrmU4!NuCA_l@L+nUImB8L0D}h5}(&2N3m4F?=g5Ic6e}}AuA8<=2 z|6SM!`n#tK8{sdyjeweABj``}3mf5e-A2Gu*a-U5i@1fbO|ua2Ragl887zd;H46by zSO^;Q$&JE7cpXq0(}HqOQS6j~Imr$I=a?ImhUK8apVs_BBs1QSU^!^Dzqn7c8~}yo zpg~_gO|u*Th2@|@U)XHmC-2apF$qwuC&|~%>zB`1^pw8-fQjVtBx;mRGdd^IvR+OA zlmCu$Wk%ylBX$#u5kgom5#@Skps~_kROU68q{|B#cDKx~b&&Qxg(T?^=c?6NKPyj)%-eQBNVLxo9 zj7);ge9X=3>o%klh5Is7LVFj;fmvWtz>s>QK&!Lcl8CS#%N2`u~m22Ypfu5t}*>oaQglm~DsT6(OY8uuo zC+@TrlOwK&xkL{H`SuO$c5%z3oXLth&N_%Suj~!OEU-BS&t6u)y3=O0`vSI_rizo# zvOAN#!`0(w4Go<&*|VjiV@prZ=FZN|k{oL9tBUUA*1_D0y?rI2BC9=MlkMvMtvgtv zFX|IIDJFDK_pZ^=U0u42F6A-^>q+3~XR&%Mq!6`-w6K65F@4&Q>7`s|2>4;sO`7(l z@=qA$Uorg>nKEc(5S&J^DF2k{X>9}(oJJYJ73qZoaq!PPdhK!OMTFC0UTI7$eY*0? z^&E{0p9*uGHTTWNskbWo(uuha`bGcB>UyWxo9!DqvVSC^u)4%VW8*|Z+(}}k@N=oU zk+v$L0^*G5yTAs$2P(X^@C+mG-~7TUd2mH~V<$>eb`JJ9`EGb=_#J*qeuw#mv-8T* zY~~@Zy~a@ERh$BoyroYSEr^zyE&;z#RION^(84Y+(ObaRG}#{rd(^%h@BV=Ihp6Sy zYlO)mK!3w~Pe7l&2Ytu0hx|UT?2(^RS(=aiiT9i+d-(rALoECj(4TT?ETAvl3u(82 zl{eELxHJ~fCqD}4O~Q7luXZYNiCo+-$RmmY@vtklx~0pAn>8u<7^|M%%~5a z5_=&iI}P^k5T}WEU-$xMf(u6x`T7azda(kp$G%d89A_+qge{)rk^f8#VTz$&4)1GK zou5{M9mQ4E#fY+Sn-vF-vaovn#aILO^KgpFV#O&IM6%7!BA>saI4`h5HWj(8?jn9c zD+$xT<@MY1UqCC7W2VP+3vcB{JXfTrYQTORdL^>dVMS&=t&R)qa)E9oS0)E5;cHU^ z%{3%RdYT8YaY3?bL2Tv3R>T!l`+rDxfjXl0LlV=V&q?#vQ=89xc!Gco0*X)l|zI@RpINNiMbS=FP}e^C(oCH~${E6hS=vj7v^9oR-GL zVMEiY{icJw{foxz!GbX>8b-LI{9n18IGK7*c1Kr1QxFxMn2)cy*fxP6U5}Xmluc z*Znfw|KOJU&B{KV>TD}k-YjbnzyMt=tzlC3KI%@<4?@<;J>V6tnFB!;qXeV;U9^if z)hg6BC0yIocWv>xEUw|zvA&g2V(zN`R;V5NRne1boNd>Us;=!`#@_57w)w;DHSOy& z**_6ZtfpyMBC)I~%l9i3Y9+yhXSGE}Izpf*^a)`N&Oe<~@+dnG*16D;&q8y&KOHHs z?i`$r5@J<@ZB1)3I*Mhn0XTco>pHRs00wSJO-KHU-|-=F3RjVY4Y>AME}PEa*c>u_ zi}&l}=tk_MG{>at?*)&uGq^roT>6N%PjP;s3$>8lPPOP!Om{CkaiPU;Zf8JVBX-$H9X{8F-~9bytQW9l|D$xRZ zmDa^W4?7SNEdqech&+NY$J6e^(bkYxp;0xz(1Q#tq@4(Rj0g#ScG64INkln3zdEng znAL9O&;mNcu0Dyxrf&-)4Y~y!JPQgx6mtxkVD~YUfbDuZ_%9iH{h+sZ-;VkAE39N zHZ7R;>bS^p?J4=jFGn63iv2a4RvOP^dC#xFt{wF9*%bLJ+Wn;V6f@%kw1y<0G)ot) ze)3dW6BDg)Xi(T{{}oN4wz^~WU+2Y!d>G!EnY`g^(ON0F^5*LP$a7QX+q z7W*Y=Li72;=P#Nr(`Fsi_lv&|I-fyyqj>v11M6o{YpylF(%_2p@&!3=yupCJu*c{X z@~?9Y;1Ae?y{>5A{d_f0^v?c8Pl=j8!Wc>+2B3`rJnIpvxq=;2_BN5AfNm z0)5DhQbR*3?6v$M_0MAEK`wI}LoU6ps}ykn#Wv(rpaq9_@INQY%1NkbS0xbi4#KV`yZDmCFlATP!z3e??z)IC+!Hy{SxGmXXh5ht|5}}%Q zM@jH0BlzIh>&P+mt`dT9`U>cKvmqhOM$eMnN_>I--edG2?K;!?3Q(=DD2t2et9^@} zK507D^fTk>+@hy6&R!Ky<$sf8$awmm%EAmgSp3-ID1P*;qoP2**oa7G^X4#vU zK#H^pd5C2LC*3#i`=6~OzngC!2EFwd&rrbDEq2b|ilyX8p>JN;S35XP`7<0{sftAv z9#)8-rwV6-ucCdr)g7{16`waW&{j>x6gQvlDvz%kPawwAX^RamkGv&fzsRoH!KKqb zaxOhIL0mez2zsP~pbfB%fDQOhl-+12%Luj~pyWdp(3i~Qqr_~xpsbi@FXLs&%PGpf zEJFq^{0`Pfe!Xvg5yc>xUT6P=w!fkjllu0t#VN8`sQacrQg1Wu!Ki zQJHG-Ddnp(;r@86FB}<6RW{C~OX20RGF5R~%E#m3?m8(x+z{?glr_(`wQpPH^jIuR zjV1$4q1uL0t1TEwh2sMeXH~i4LprId%0MDqQ(x||mV{Dujs4+9v{hhA5*~B0AnCO!7K_*B3i+GnIyzU^+p%OBX($@#Zt9D8B>QUekoJ?W6ejZTBCDsG9yYodGNPzjm_*(Z7$|(Zju~UTE)`7h+e`(1|0R4l_Wet$E6oFDAu-M!N>$(*hlyo zrC*ySz9@p7p3H;(tyn@&YaKki=1<%4AB%AD z{HK{!t7hmo;md2~^65d2*&*aF%fV^Air}!ncaf*Ku(To_`G;ItvRXyi!xzv95oJkk z3Fu3^Np1-!t^Ek-lk4e6T<;A_^e@5NXtVEWk zDE$|9kXxt%PV$4`WPt*X+H?yQv+un3a?f=ORlKibk$bLNsN%VfP42mFp)UP>-9p7v z_5`3;iz7Ow>%Dvwy) zi2M98V|EE)p4oAg{m^B#BA-^a&ZR2s4_L#;h!J&5520>&^e7$<#NzR$aT(#3qHb;V z#~$j?BL} zOZ?NweSwJ6YzNzh+w4EGpYt8!Xe02r2^9FX7`aS_hj)S64)|vb_+jh}ebRv6XuzLD z7Nh44`0WP#Y11Ehe$IO2JN*t$PVpexw2tVwRE#N-@hRLJj8NEkrnBF!u+z;5Cb4F3 z!8S3~Y5rR2oeJA#x5?7iO8&*lvbJp8qFRyJ<1xgTI*(=1GRxC7f3Ql3yOmY>xyT42RBUnNUfCVrkvDhm+w+${Yt8T4oUDw09xVZfBe{y|WkR^~bP5}afp!4>K7ejx)1O43e1pIj@X9YIOwi#nb%J;ZH<`Gtt&mdO4P z@T2Tjob*Ctp30N_5%8x?2lKv3a!1tu;yFSJQfZP90{X%RAt4B=pg!cxN+K45V~YG2E#@g2KEme`0T%ac*CwAI06Df@j-j8v}2g4{yv zAuIquxm(SuoE@?NfAJOi_A?sIZsPbC!l6x0CrSz^QAd=1in47H92OLCHQ+}PhbdNL zUzeO{gM-$k4&>)oB<2vix!?%qW{{#19|4$i3lq}_*?=eUj9WT7GQVbIaNW8@`%FXE zx@Oq;F57%>a$``$trhnzi?yvrNyU zia5}DXe0gUf!xz=X*_)%r$kXabYUCNk7wpu@B^a-hj$S#i0@Fe2(=3yxh2d- z4=-Avzmsc0{=2c6`q`VDySE?5-CUCg!O!4YFmEBV$7n(6UASFW@i`6u*1(FXfsxteZwlX1 zNu;L8);bo?%r<7$r>eV?PG|jKG`U(I2VLW_)aq1_EN`o=#8Fg_?dLq;gh#uI;gA`A zTys3Bk(#pF=0N>Kb9yr73k7{Qrdz#vqoZ_aAp2Or0Ylx6pq28v!rW_uVn+?yJh$JV zu1uOs@DiZJOQu_S#^1$U`TUfEOiwuJ5IYgcr(J>A`6=j^<9%A7oLCW7AraytZiFaF z1cs={n`HT+^EBz9vU@sD>h3xMQ{hm^@ZR(PbOkK2mXs+IAv-(um zY!e~`TTa`hLaErT9;=m2W+;hgCbKq`TH8G_H#adpH+PxU5wx8$(=*=`ioy5bwYkf} zBdNNf2Ii_Avs+zu)j`n$qD8RO>4Z8O?X6^~3n*l+20gNjWUGKeOKH%;Ixa`#OpGBs72dIKhKyqZE=LV`T)?qH zGAZ3o<*-76nKzYN=s}ul{r$z|;-Va_tjK%Bd-7FA{gr$zlD@0Ir?Qi&eo?Px!LBh z^b)4*S-SRFs8N>eQ48>9Kje1;GQbP{&|=Br7uwJVJ?Q0afGv`X;S}ER#X+$wHhWrI z+iA0_PHk&Db=Aae_&0B6*z50eNn{SvS%e@Re z{VH~X3!`$3!6^87&WhUJm5HmBKU#qL?zJ3QYo za8i0`?6)?a$HC@=<)w_Ck(gXpnXJT$w`x|^a91R?uD9*N<$raO1qUzQzf0eu9qk7S zL=%LV{`HK$#|Xw8%~w>4(Gy{ER3y#?+iJ5_2@J$D8#{V7B-J7hL+%Lke<|7T@{Zc% zvdZTFEDqZ{b`6i5HI$gEk!9y5SuHJ1t;-ByX;E_8%~s11o(tAzHml|%imaqgnI1o_ zrzR{Tx3-&ThPZn6N9=~e9x}eSILPu7cZuxLVg|}iLo7-5DVcqYQ|lyf5gWBQfE9Jg zxz_BZaR1O|D655iUBUJMN~eV{3?onf6F5H|Ztw$o({elf;o8*b5~F_789AI$<64{m zM(2^lT&)>f?NMA9V1g`qMJCcBWL1oMFtfutEZd`|CCjg zkP)uE<5)k`w?0!|*)e@`23g3lEv$L{@^*W%e{eRDn(nB~Y#(l$ZH`WN_pfbE%-b8r z*Ee^ZfBva+LCN7%Y;y7R_A@(+VqG=y;nj)mU4!+>)t!hO?O5N|yt<8edLawx_bnPB z+S*S6>rj4|FTrqNUT5|*oG&hLC{p%W$$_;ToCWYfWa37eGpk}gwijC>Wg69p(|-^V zL@mUj!*Ro3$>k&M+Se^~Fs2gK_geE5>3)%Pn>q|~lI@hi`I(b@d$(q4yFyY$OLg0Z z4q#r@lKorbcsxGVI6pEvf6Mgdd;x`@@>yCR8xqG`xoQ&R@|x4`YXA8!#68?d3d6DwAhz9T(2& zU(`GOd*@u}oRv+3n_IQMndzdwnUfyoopbIvJ7$BD$BNFWIQ`1WqFAPS=HVXV{CxADYRptrWE!<= zQ&VGOQDa|DIn-Qy0H=gO3ob1SqgA>ZJR(&;gm}#64hpZIh1j3SJC8BjMgs(!u zW?LE25))(vG6!-GJ&)sbv8UFqCM=ZYE_+OPO)#r7y$vj(9ZB|L1_q(4&&f1FWe6om zC~ZnMfpKqH<8LglD6Uf(vQIinlJTmzzbxz?xA4^_h$)+La(PR+x2!a%fcG2?;Xtjw zsxji9v(gJBcbajMNfsyNLMnhlar(Yt$mvK3r1J9T1_K@`Cn&E8wD@eO9%3Qnt8R*v z+sYkwmn0*-Dvg92Xd-OgRvNYk6P2MhyVnKl%&A(;j=J*la%9?bVRb`31I}~oc17sY zR*P)+#A>UPa8WT+>B5`PEm7JfO5?5raZ7FJKivrqQV~6%pYBtgmfh^je*X_ueESOI zpn#afm;P+M!X|kU-DXzI_sU4)l6{+e>s&vW7Fd`(DJN(-5uZAPrwoRXg2P4P{Ck{% zZVaZ1kk0{gc~96IiiNBEWgvqKYZ5lcerotW)nDWahpHL_vg|F2R#w#c zirn@~DZ4S|z`HCSUsXzVHE`LcoVOc{J?OkLk9gHN3Be0)r z9#yR$an?IqD_Rm|iK<|2S$R#U%3j@4Ry#dhkB`}9_hfxEQr{mfYbc9V2ZMEyKp+fi z)PoO>B4Z&e0irW;VP1#R*Wt_C1=6y;_D@jj{>82`c0h92?N)?7y^57`=o|}D9%bK# zfP;j?=rg^_+Mv-IapHs4md?&hNx?W&5ZQuk34-?(L5D5fB)xkg%vZ{I^iTsRbBWn=QSm*+zd?AgUohJhl)*xxbC%(1DrXeR8o+?I1LDJ*4~m$3vv<=;LIV_ccQgD)Qy9vsRBPl5e^Xjn_#CDtTE$kNm+AdC zrE8=g;Qa{ByW;Opi|1Es&tI1|YwtJb)n89>nf(3Naf(^N`yKRtJ>oU>=jZbGc~295 z8S(ss#`7)wdEDe;w;A}?BcCL?lzgA2GfiuWHl!sxq@z= z&mD9X;f~AEE4I3dia-N2zG3P16nu2~l8vKY1gQ_3)*_B| z16mo!_1pNXN*mZE_^Y)YR;z=kdH@mb+zKecn)af0&$^MN#N|>g%xkp!0WbR%VVF&N zA1G*bdZ8N>eNEJg)f6q>+25JbdYJl-dOWXI>TXU_*5l14srBqv-UoQ!^STLJ#BZ<% zu=}Zze$C^3W78E>Ddu*a!a^o+pL?6VqJ%2t78J}qfZzhL?xTSMdTz#;;jWbocXe#;na@Ix%d z{;0h}MulO=eyO&rR{#GY{Rcs4th?APtQ7fchWQwP1r}3PzS$Q)+vB3+g26y1eo}>a z7{HSN#p9?-F`f|3@KC`|<>N3v6&(6Dr~0r5z_v-2H`y$R<51@$GakV0s6Ds(%oasF zS1cfi_MF|~vTc`Tsmo%vTD|53GqZ%nXT>2yT}+a<+mP3NJJx;ZF-{KRkIh~ymT$Wx znI6NlO8HOD!l-EKVbyQC{^jw08w17OqNch73iPpM$R0o^_|-sR_Z;=!X@k-IYN;g@KzZq zjkKfm*bz$R(kn5$&5NvWKa!mvajNo#m;oTMj`f`@VdL^0Fa507bSb+5Q|Il7rp4|I z(@Wr`KSS-E!s8z_Jn?)N*B{zfalf12o8WyMVV|S-ADV@GawnTz_yYU989O)@>P@SF zXCGGXrr0ccML(f}6bi=QEXB|KnJz3&>_Pv4fmR+EoCR4(BjOOw4YkwzG$L|6xSF5O zu!*jR?9kEpKkZmRdFd_WPguIVbQ3R+#6V7*@<3}LTK)LmIlM*M`?%jL?gjoid=|Dz zb2u*^^>gdN;HWvOl08S)JCNQ8|M5tlK4fB^Vjtq~r+NDVXip#ip12PI-)a86KEfaC zEnB3A#Q6j~xghLijW@J3f*?P=j~q#M`}7&znKM?{tp7)@zvk}ETkg81 zMMjndrT)YJG(Z2j568@y45IGpg#k7w{YanXwTs}am^K~7Zc$W>`ANy^kuWmwnvFwXIW7aaT;fnNm{KumYo@`Z7}T+~U})7lUH zp;-xBE08G=B@CG#q*e2kCG9N=32>#g-Cx~47WaYcN5@K&O;Ss9aD22#X0{)*qRtiR z+4xivUNU=fDn8pj+EC%JxGjihC~oX;ogUiZV9ry9r&EKCeoK*A1%@eL_-Xi6;5QXL zN6iOAA?q>eJp>(Us;UiyAgqiDDLH^heIX7J>mD{0*t8_1WHLB5RwPRZBsR0va|L+& z6U#G9{;@;gGmx4d+G%Ib9YfQt1BoI_kwsy?jNSm%K@EtuH#Ag2Ffg25Fv7M&j%jm5 ziZ0NG3}wO9e(od+aToXFc7^ZT{I1g~W9u6^CDnJn@1o!bs;wTib&J}DG4L>jH~NGp z`Q<~3MUShFvI@TQuUdWb5b6_o`TG{m0e`-Z5q}|cD$TU^f~Wq5`-}K}*}@+1!P~gM znBS*x|7qzB++V`)SK)p;)-M$413br7J?^{t{Xg*g2<*x0O({IzXeNsmwi?4;6xd1| zArAn(kf8O5cn(Pq0eU63oUYXN{1gFh=kTjI{3?PI6<}}Qgi!^13KohOPBha;H_VMj*2q!W#R)Otq(zfzuW0RYUw!w)_Zp?)5;lx)_@4MKIA6(j6l&(0D zg;-`vv+LQvW1eu0>0f!fuX&!@-3<6G9Dc3on;d@a0<|v*oAOEa@8m6my+{KZs+zM3 zTv^c67;HbYSwE^MAB7TesvpAP&1O&9b}#PKd+iXO>es#Zz-jKZFRBf8BIPiJ#wQa9 zwFY4rV9<=&)kU?jf4AKWN04<9UGqHY zx@N#{;c%iQ!LMB)ILe!T4tQQU>|>;RJclLeUburDk#0mBId({CIv~y2NXzmk3?m3X zEP%uC@C&u2ZT}w+grOOVZ0&vOD#yx6w;#?ei8&pxc{6?1S?5Z`Z#!-8&}Ey$0|#PH zlKpaQ9w*_#9)K^1b+=?&asL=u#Ad|Cf(#Ny&Q|#|kZtp(omdTm9Xp6X9`qQxiu<+T z&*NMVe1NHLpp!nShPg``Ce3AV?x+Rg8MhQIC>xL-tZpopZSKI-+C(#D-`>zZGGp@v zB|Fr(l(vNYv61$wnX9*+{*5bUVyiCQm|4*uvbeD@k=VOt*L52k=WqPs-t%ssjr7lD z?~TolHq`Vsv@CD07|H}1DppuKXB(5#O)a}Nbgt@b{)EHn@?;;Xt?pQVR%Y%46N49h zGlnz1 zX+?B4x1vY6Ft}_-!!(k(Wte}^w3@QTs1XAOeywN@9>>G)6g%hhX%QpDoipI%AvVXLHbiBGfoY=f+E)1Wn zeRNMxH5{P(d1-(ca*7S3-;AkHxGAK zPt8PIH%#{hWt@4;*R#?KKa#Nbl-2;ndKOfKA%{du(7o{Il0qiIM=}8=*kA2-|Hph4 zZIRkmvNILc=L$9W8n>O_mifTxUqDoV%Pw8%a2&g>roE=zi=%3gMl4zLe1+7sebt)H z*xz~V96L-P33*8O6+6T#72le zMi9J{W;(m|AIdRrR$r_Q(AdQ4c)$DwaRp zwWVsXwXrH$=Lt>>l{UvP6D^)-Y{-P&wZ%wB;x_xLikoBMWTn?!I(oaarlqVY9q?4O zRMf+<&-a2#FQGU5Sh2<`|8@!$=i`qoE4LKqBuHXhih-9g)=H?se6FmmHF0SordqIz zZkW1sduQkNOQ)tTr9)LOof_J^E|prhcS!p?Kd_;tWy8Q|yCPf6UbQ4uk)Eupn`o;@ z1r)CtXRVeEv~T&)%!-3s+S|7rTru;ZE$y-7{CVSJ=dDR5*PJ&te%^fY!uIvFmQ`m) z)-7gsd+W!WnkMRfE;9}g@+n?F=2vORprg_c5#LF9N3a_p-z%dTb2wC(D;usXVExf- zUvur^cRXmcn3p;(+0hIq*&_0(+;l#H2*4BA;hF$rk#a>-kS!J=Yx^gda%2U{CRi&@gzEQDX?)R6tTwAO! zASa2)j)iNiWlg4QzNKZpE3>AB`j4%V9>@B;3z-fEMf-BbmkcrPqtllB;OAfj!h@Ph z_7s1B{v&}AZ_AzVH$g_#^K`k`W{)|Z!0}917T;t>+HP@X@MYp~rVqHGtB`>}x(z9B zpc_<2_F)7oq-mox+iJGF{4#c|&`Q?#sVe6G9ZqX9Kbd`o-wolkCXB)|b~md*-|z?w z^b4tI(*HEH3eJ4KJ=zS6=2SbsD+7>o7Q$NK0W*&CYf z#_Z%eZbwbSXTA^lOlkI`wHy?JOxZ^Zuk`O5gqHjjx&kjMe?BiymRG?WTB39po4&zTVw}-TpL?80s0E1r8S156pM8ZVK*!>VgY72t{z@0= zOGM>tC0$vY1C*zKM8yuQWL_C)fR zT6aEG;Oiz&+==Kye#G~n5+~KW(CZe1aI~}TuhNv1It#wvv$;3-q~KT66{WKULl(x`;I+@w6eXs1_tRm*g;~fLkKe*5~dx6mVfA%aZuW^ z-;u+E>f>{vg&)eFksC2aZ$^vB)45GLUv9^_DkI=x-iUt5M>Acts!H;e_;cFEumCm3 z9_ejeX$9C_`dIwI`#tRY8XBipmPof1@k71NbROH|mo9%l2Y}P~gFVfDCw&!|55bCE z0(0=K(RbWvphO(VJtc(&u#cA^$9! zL1>uT>u_MjP*u8}&TiFW&E8AEewTAV`AoxDef?NNBV8Lq?KL&+Ay}<7?d(654!1j_ zDi-)ptrkV?aJ$--?A=Pc+tmT%0?T@s)v4VscbDR7oJfd_6u4H0yK5ulIgiwKhoJ={ z3)|SMSZCBu6_GxRo%i*SFZBxx?3>U-A#m;hW!yWE&!2H)UkJ$@Bk;1Ik(waZ1WKXu z`e~A~wD=YmPuGu-BlSykIj42L4Te#;qq>sS5jP)u zhJrRXTf=?PSZt)BVK^2WZm1uL^RqHwB|D^Ea_*4-6+B&rS<}^+HIZ(eFnq2SlG{oI z6D1NO+MPcqDunu5JQiULlB+Mth21P&vtQ5s?jO654hOk#EOT1DVCLwi^s38OuDpCz zdSet`eps%7k?GfsiS9>@U)jq#ahx=cmF}zy>AXAl@sqcHVy?ClZ{b~E$WO8n_JMXb zIog65{#(!f0?_orMw|kTNPTcHt-F1ebs<8`V)AR}v0CAmuRwGh#b0rar6lyOVeffH zE9-xh+W2VtY5H@NjlJuUt?VZ9Lz6!`d+Pf+y_M|ng(K4GS`UrESFy-1uJw`5QW%e1 z^J;Ptx|bw;Bu3^7=Sw(12P6tVq@!Lm>jvE^#H-Px!Z$q0d+#c`W)I|k|F&_{+S;0k zOQn6TcDXDQ3=IwrhN`*~2pOl)Qd_JmGhX4U4tk0`9ovU-Op(Rnv0=Ma#fHv~m5qP~ zqR&ujVhYvSqyNZ0S6l*!>_Uu@?DM&NF6MGclEY=eMmc+tEA6$!`l6BXsrlArXY@8S zs16z2W{xCUH}<91E*}j?`a$vmT`MLB*+1EBmm$$3Qfk4O-XcFO4I#+(3N5KZJZ#pe_~QQLFSac-W|OT2D1u5 zt%w(csr?-07{KC;dlT}}wqb7w2ttU=&+8lda$LLS*e(W5^s*X-g&Zwfxk&>-Z?Sldk`rDLm-T;>aTJ2gVS8%Ix8>9y-B}B%CG~2A9L@LA@@5?+jLISc^fB&|NoKme;3)#X7z^rU1U0!LGs;*UEp(^rN$J=pkngA zqnG3Fbs`TR^=B4K2aC27NA4{?cC%kP_I^+4$&*kx@cMn9*8^!q5esyp4cS^)m4x+} zXI(9_wYULx{E0UZ`9V7JrjujNb`bvA&(Kx;&J>=6gX9?eC>+_w$39;_$eC5_Q&`(l z{CH#-Cs2pW23m-s~LU`#o zhbpHNn{~0~}aiLS|u6=9ov zmYV(xx($A2s`^g53+?iZmjCUXsfAdKselG(Qg4r&?=;ZWXlG#MiUMZg)wE+g+^x{b$~U zPw?w&o{|y|9%b*>%6w6$i)h*J**DlXm!hzx;2VNA8>q_coxEcHn_a8dcCY?}w_+#z zhE&aKZCXs_!g~1~qH(zOLWA}MAIknG?7&|ucEx7FANFzmYsP0Urbtcp2^-DqlLyFPoS-ky*1_MB?AGa3h=60SdUsy^H8@~8Ed-67EU95bIv z#I@s;L4m<0U@(ln)G;ukUdTeuYcrR+dXsr<4*NKLPC|>b=MV<$lb{1o%bv?_!k)8D zaL!_blIq-SX8PA9#IERQfdn)nu3-;j{a-|OEgG?fQb4mXv{A8=b15IqU!1T%+kEeSXP)g8UW{(>QEynmEy>W^LXChUKfLm_Z1OMp2h8 zVH98&RSt~~mU{xKge5ndFIrVKGKPD^w9v*8e_h#<&#M*wxRIUbE%Ew;aDrGpu_|AQ zFIa3vVve$S;2#$~NDw^Q3jR#9Qg%7}AXcm28-LbToNR3_%0sWDHI*#V(Q8C7#8TfE z-!Xk!TEYIAEMANQtT@8kL)wgb0^>mHwz<675{lzvFF4$ePrGc7LeN_gC-++{Br;Pk z`vSXDdV|K1(C1osUJ%|&yl<%S6;_D67xs6C`<@#D3Uw(|~zQtzMrLtFqp!O<% z>DV+HjgD^W=-5P?xHfel$*7(k5ot%PDti(;k*sRAMpf!3Pwwj4Hd$Xkxvi_~bqi2Kigf_fY0MNEBg-sk`AXLbuaW4#FlhNhypQEL-bHK2krKNG*%C zE{{D@(pekoio`SJRc$f*ugk~6k&co_V#`}&%TkF6xXQ|ZZI888m374Z;Vtv;ixGUPf4np7JkJZLnIlv3Y6p> zogV^0J*K%1$Og-J$~@?61U>XxS2a^HM&sJ*vB zz75L->Q`~z=Mv?;^{cC(9%p+;x1}R}Nu%0Gb!x@H#8QW*)Jw9fm!DeGc zZgw`!d9qi#8O5_^Usl-}1n`wQSXVJiIZFA77sc5}RIiG&yh#k;dl8VXLHaWB)OIr3 z;ke~pC?c7RLo`XLf~VMWm&bB#h!J{7e2G9Mn3xl4q2h7Mt&Mr`gEYP90 z0W+K^mow=co7JsYRO#5wE+2YHoQw1yLLQ8BE~fF1Xhh>wBz`6mE!-C}Sm7Z^&Ze;~ zeO9Z>$&Wy)8^YO0xigURPdw_`y}H|lG(&LZdzS6kvokt)+KR@4Bac7@ijVshcCl!& zMTUJ#vHA9Acxpe(( z&%o^Jp8l2D^Ogih7VX|KsnjNnto%03n|5%0b3)wTqg^L_?|C9T zugUV)%;vjg*~cK^UCuW=o;MtBmm~WZtF6y$e#0z3DdYc?JvS|_XLT|%3E>nNkwa*d z=MbuwW>~qr29cf5zTN}h{L_eV|2)dqOWlCaa(En`{1>06oI7biSIcJ-)ES4z=XsOK z^c}$ar4jZ~xf1Zhm_>?AdzAH>;9YXefKvuNs%IJC*UMpoBS?|5_S|Ng0=x(CjdByg zAqJ7J$9gd&$13d9>5}dgGp1n(Su%8lokJ2UcgF(S@f`PPf29q@UaPlw(STADH) zkZwSo-2Tbcu~vU|DtCSdFW+eT7wK+_|HrsQrw}Uw3Eo-Cr*K%qq>QPx!zm9~6sgf} znKk2FVZ<43v0@JeeQ~W+i#W$SwHkO|^HxBNOl>lUQZ za4D*Jg@wmUq@_j{>lf7>((M^5t+Q%oC+rlGW8o3v4T^Bitz!Mf<`yv%{8XNj1W^u3}{nT%E?g z%GZ%)=RISHZo($viso|dT31(I7L7`=vP5}#qD)*v(Xw(57nshl)zUqr*TEwkxx8r; zR&4l!6rWd(VEM+DEcHg}LM}1u@JO;HSX1P#jQ~Q^CTXU~7p~OiK7LPWl~bFO@ILHh z%cYZ%eI8%xXh+v!3;RTI@hio}QfD!yAtushAKmc=lY!MdNI$7$J%$g zVxn_F6=ZNSojLPL6-0F zvhwykQAwPQ-B#rbzZCMi@C_nF?)^6wiR*)l1U0{$kIVsrJvpJ7dGG~LHB+GnC% zcMywWK9tF{smfXlj&IpiIjUZ(!W^@leI*u82>VH#Ywst!`T*~3ZTgnKOe^HifOJh5 z!o?pit1Zc8H!Z<&P~{=N%#doKtiGf=3?V7b=%rL=m6(9jH@eW`dq|qC&vEwuK$@Mm zv@m0)PMlIkSqva|L`$i+#7s3OcfPiy-1+PhVu#~;H5*#En?~*W{89Vo;^O^9em@i( z+Jh6G-(nTi;|EBoh(KnDc2mPaIX!OsMd>5CdU2*@EcF(mZ?w{uOnl8ls zA^kaNHrzj>-E-}L`@^s>V5Lhpu@6975YGyHv}w^!+WN)dPqR5BEbZH|Jqdd%fKexQ zE!>8)YZU2v5w#_B0`8CSFC_8&20Wj{Ibjc)Oy9?J7jLB{F6B|`Sq%>^qjCS7cF$!i z?qT`x@>~Mq{soR_&B8v@r;g*xksM!Q1v{TFzX!gA^+#g_Q|3&DtG$>u#&iAqF_ED! z4eec=K-xVQ0vwUF))nN(kd^ys1`ByvT8}Jxk|bj-%T3XsY1s)IZnGGNM7P|?d-riD zJC_`Y;0wf-9^f_de#q6QTTS_IqTgg#|2N8wIjc#$p(*$)uspKejkMqn^CmOGPad*J zEmp71W06Nm$#bh|5Hb(fG8Z-CL-@!y(_={O4p*Sr-BLSRA;W2{ zO3pFa*$H>9)m9rB#{hJ+A~6ctH-}8G%Xj=Q?VCd&WZhI%mcg;hjho7f68L*Ti+1T{WS}ZB zY@0CIHp>G691|6gUM?yw7IG5S&CmY_)=l^SvvuQ@USYlRCX6UC2SAq>jHw{!f&$P@ zg~M#ot@PJLqYgV5jvIw(={J}?&b+&I^JQV(*d$^7YSxWBQ<*F$>!zxvri!fJ@?<6G z`@gY54!{a|gmeJO%j0FwRXL%>`S0&*g#hQeg_SsG>SugEuQ3;bItCBo2;EvGqG)|_ zMR84$*Ns3(2Qn+zaWsO%Sr#a+II|chw_0)3y(D=(2vYE=?&1n)5lw4gDHuALw2H7; z;C_6aYn3$i{M3OH-_-_KdhT)ciEV(|C2W8n>ox$e(DwZSi|aX7+MZ76I_v>m{2bIt z!UD#;o0RYv*dY(q|*pZ`jTw7bTKQht}a(>3V!g}5~2yt^jNCO$}8~^k3~om x_$CEg?lPkDfv*TJo~(9A7mNX9!r;J-{vtV;sulG{{p{Y8f*Xn literal 0 HcmV?d00001 diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf new file mode 100644 index 0000000000000000000000000000000000000000..f4634cd7c3f16e8b1ffd01f2ea4a824b3b25991d GIT binary patch literal 78948 zcmc${2YggT_dh%{ce9&bHp!-MHiaaVgk)0*H5CX|I;cQMLMOoxs)z^{^ilLtA8e?o z*c)~qMMNKLsMr-15wI(EL`BHH-*ax+4WK`N@B4rL@9t;jo_l8I%$YN1&di;eJ6A{{ zL^M2`5QA&$>Z@O>dP@kE4eZy0M~@o&PE77BA%>nNMCFx($BwU77i@S(2-hJY!d8wN z+uOV5;faYt$Q_73eeAfZamz2Cdk*fWc+-J{T(6Mal+yC4n zL?-Bj-!Q+esWJbEd-u0qQbZNecg6~S~D@Uv4a zgn#L?gZ5b8h|V!V-H(eSv|Ns*r;X##RH2s6n%%*~lpbGd67s~4kea-3b>zmvX;J-t z5t;_M3GsgZv|V)FvwiA?z7f8#pzgL+xU~yKSo~}%4HSYl!*-+W?Gf?{3YWjCt+;rE2ELRy%}C?qs{~dEqK(=| zjUtBnQA(NSz3m zEZX6Bh;!ht6$C*TvO8aO=yNr!v$rpfF3!h8~%8j;HI7F%i_v*dqWp<8Co{IFUP|*C7_7 zB$c8!{)>{*-MS$ny+6L?_gpU&SVz?LwKIS0IgS$3JpaalYKr;a?`8%UYT<)_dKu_kb8;*E0mh$3)E z&J(~{*L+QG8pRUuM`db6&Cln&m*SrI@Bl~oRzXe#cR(f{miYf(LZ)1GsV;?7mV!TB zTiTK9Qk1h15+yEM0g=>N5u*tuS^y46LV7%p7=4nMC)Q-X2(0PjB~%RBh4_~VKh#%@ zguF+JI??wen$eZs|D>OuF1gpB6br?2)+AkLJ;ICJi$ottl(dKH{KPU)S=*6zF2b82 ze-9+FnCpi0kMu3?g!-6e)ZSv$rzyYQEKO5_`6rci?~~+Tk95>h=Y#hG)DG!IcR?EG z*AbEgX>vO>N!Mq88yLyysce}ip`HobQ3~42#JS>1u~|GUc8IsdK5soE@UskTKQTVq>qyTNvs?FrjX+XuF-Oppz5HJL6d`K2Q3YHBj_W0 zn0<ae57RQs07aVUoK5`s%{2Hu+BZ8BH zvx9pF&kAl2-V}UK@Z-UULPSVLNPb8~NNvcdkW)iug)9nL8?rv+`A~bPGqgT*Oz70m zxuHu!*MzPQy*Bjy(62&&36o(}VN1i-hFuzVUD)kmkA=Mu_FmW*VZVlJ;Zfn~;l0EA zh7S%O8$K<(E&S~8OT%vrzd!uR@E5}03jZYh=ZMh}H$~hV@$ZP2BHoYqDzY?kUgV}I zd(>%Bv!gnqR!3bNb#>I{s4dZs=-BA2=z{3}(Lj?Gzn5{A2#{BLKa>h8*oZX$J&MM~!=VWJ#bFuS0=f9k{Iv;U9<9yZmzVoZt zs@M^+lVj(^UKP7P_S@LsYn#O>woj7P?lt*14{9-RQb2AvPgB zp?5-M!j6R36O$7sC$=P>k$6_(C5hK0-kSJ*(uAZLNv%mMlFm>1XVOhcpC|p4bSya} zIWaja*_&LIJUV$=@`B`L$!n7@O};Mq_T-0>w&vB#&~plvtG!0H|s!luk0n++p~Yn zKHAOE&DkxjTVA)~ZUeeq*=#_ZZM)XpfaW*7dle z$BjMi?it$C)ia}KVb6g*NA^6u=lq^ad#>sE&z_rl-rsXu&sTfy?)gd2?|UBU71S%b zS4yu2(;D+}K($}B1?;W_i781g$tvkpQd%;gWN^volF21AO6HYxl$=#^ ze#z#Nhf8*pyj}8X$)QqPY1|3_g3`&Q7nHtG`d*n*S?7gxd%OlDcm%m(oup+7= zp(4E^x1x7NdBvcLNfpy8mQ`F367q zdH-hy6b-m_pc+^(ux#MSfy)PO9;6K#G-!R5tO}``Q?;YIU-i?~FV~dSTwilX&BL{h z+8gU+U0vOVx{vCI)!#5Uc<}nczYG~YWaf|yhCDdr<)LC|#n7gqw-4Rj5Yo`l@Q;QU zhjkxz`mnQx-8byW@NvU08vew zAN9(p-J{j$u_UW-NjD2nFdt*Ny``b7%E_ht*xRh~O<9d!O9yfH{igBCA zZ5{W~IN$iP@h#)882`lh&nF~J=sBTz!i^JlPWWWP9}}}Dj-EJa;`E6ZP24zf^Tc~5 zK0NW`Nx_pmllsuG9#0wq<5MBxXq3uVo8vI9FJhc)7ehn@#%+DXD!B?h%H{HUd6V2C z-Yn31!+oavJolyUE8JJPZ*c$a3G+mI;yuY8w-Gb z#OX=&qOe+qt* zc_5tv4>Xxk4wN0Z{lMG1E(AVzJ?$WksPiR}UZQ6G2S#5{*y!MLro%VyxZi}|LY#tkpodmAn#s2$xS?{V1>Qc2#EmuwI zDYS|OYLaSEQ!obGp`KCyQPb4F(8kj6+}I7Rs$LAn_<1bG>iSr537+q77k7%g#Dn5# z@q+jmqr1<<=i&#oQq5Fnsh3rUIwpRRG14iMWCou7D`l0emy_gaa+;hY=V9EpQQjbL zl(*yge=Az)JMshhq5Mp(P&cSE)k|u;nyPMBkE=V?6Y55FwfaL{qnwx(*{NF9wOX*+ zs9sQmRF&#f@2IO(qZXnDs#x_8)u4u|VQRXFg|!I9j6@7(CQ?L(7=ZRvg7N=UF89U@ta=Uz1?t$gmD_k-ZR_a%gAj3quj1^fjL1fEB z;la#Vo=g!vWSXdw#iB@NiGG;lsg-4z>&X@kvcDK2`-P~+$(Pu_sE;XL-G#sxV%?9ChrlC$h*X|@=@`O zd_-)Q4~viFo8o=BOS~kvi9PZS@g8RFK9sMcO}r<*lpl$&#EZWa&7+r$&{e(|K-A|94^ zibv($Vk>6Do|8|A=jD^)Kk_lLSH2}az}(oU@_n&izAHYFyT#Y?V{u4+C7jYO;$$#p z-+UqhGeeP>VTz)eAasj1@f&)>Bbf9212aHJF{}8y=r4PVfwD-{VJ5Cr=8AIJT~x>( zqL1t;%4D9HCdY`=hsjw)lr!EiRVlhzsQ^@h^FixC-;SSIbMp zHFCY!ApaqD$^VGg<#Xaq`GR;`z9im}JH@;5W$~7LQ5=-}#W!-F_*Nbe-^)+cDQc{m zpvI_)>I~JU7NQ?sr!G|Is`J$a>O8ec-K=I{hHs0yTiv7XSC6Rs)MM%a^`LqbJ@xDA zP4sh5s<+VBJqKHcVv10?7ju)Uwto0HdEflTj#fxvl?bVtQsTkq}{2Os1{zjbezY!lC=OvAVZQi| z{V&-+!2TZg-(~-G_II+sgZ*u-ix;$u$J^SQTg8?|b6RJLyA~}fEE1a+EuOJR+^}fz z!bRfh6)o*;=qE@oa@eQYatUo9KbL*z1$2gHLI{_{zFxivhNIZ0^%6-dC-7;lLZa=G zAH#kq``zGc=&e%G?^0MY``PemrGVyhL)cLpH}5H47Gfxlo9D3GhXmaaRDr*+J&%7s zZLhY|`ftJLqeGjiO~MR*u=)Z21L|$GsR!ZDRj1+amPgSO7r+8PkG_2js8bHXpg%-p zVEp-wJSe}F-^uUg5AsKhMt_!vpc1oWTN-Cu^Wm7@Qt{f^@ zg^c0kekBSAjsa0e%vX-71jsjG!+fMUSaJ) z_EC{oap?nGIA7}nQm1li1$o<56ylCk5gcb6@K}$4Z&EwN_oG{;4ikSEyUD$`GzaXptCeM`N_@)MB+bEnahJ30k6-1gWNIsal%m z=Cu*ztQyq}R8EVUiFJ*6Y97{67N`Xxh%6wuTBgnt!5GzCg7t`hVZ`M`AM}}sMW6J& zNK`+n!y-fdh834wluGAS!W!wavC-;;T&5;bX)s6UFHIzJabh$C2doWlEo8n75+*&# zhMej7>0LNl3!-!x@ReALI21xFH(f)d_=V%?IS=q1G-%}*w44YnFleQNl2jhm9rr|y zYQd!D=CBggpW{k3B!H^j_X+N#Y6-v>r zUKMgzqD4+&es%6i2eZKumAF}w^dKQ=aw0%Sm)UrOx{fiWpWTHc7s^jk)hQEKk#JHq zg0>4G5;Ln*JLC)WBvMYJd-etMV5X%OPWe8DQP({fcLkO#gwv2_o3TeCb}!^@L-|~= zDxqo*^y?e-A^dOE2k<{p@5BEzAjUq9@iE8P%P~IU7?1<>?JH0;sk_l)Rzjl|V&vA0 z5%VP2f#G-t7=(V3tQSUg2x${%@R-gcXUYy0%`Fq{klLiIhd&RpMO$S*31dPfmJkfO z&G6?iJV(tZm@(ZrPB-zBRd$6{UDyAr#pxQJYG`FPDB(GX={cZ_1Z{((y4P*0PzOOzU-=3sSp7VO@9aNiyCFa6bEKzYim`YGtM%2h?Guc`x-gE@^#RST$_ z>aF^y8bH~qKvk$}Kv|f*DOXj1GF2~CrUn7ZP(4+t8VFgW!`k#vC29b;|61u~(`$39 zI2HFTu!H}`6T<(;zDtghQ&gD! ze+BFC80zL({B7bedeI>6Mcu4@-B|l_@MOJAUN1qL1PhT7c3!Q!&$KHUNOmbAMlm`1y^OzEXDOP@c3u$bumdR z7xfBP)8UD2k^bpZZ5z(-Nu|J&{Zb23p5JvkBv|zjDtb-hb0wFEX^JMe^}ZwW1vT zFmC)i?tD>*_SMzR6a}yg|L0wcL9%PeIe_~!N4!+St=H29@@A_CuK$16 zVV)^XEk+)H<_3U&^fqv~`o5#9_y}{1A7P%9o$r0|k?#k%pW*I@`&4%v2l0I0B3>k} zpJ1+#?%#x?_%F$5jINLRoyrnpOecQENDNLo>FPTQxCdr#2sc#z!nlvbA>SeGO3^?L zwpw>Bz7H+f#L>0Ew^y4K5RY(FSN}KN6!cAhlAm@6ypXQwt^jkF4FN6#a0uKWxc>5= zzPIHEm}C8$PAw8?a-N9!pBzRQ4gXb_ialh%oanG_lKOY^#Y|h?@eVrE`@iB+L~$3o zdBUy!#A%vH)Aorp+kRXR!%aiGBDWZ_pN?6~(?y&*9X53`=DwGq@9&MiEE;w;OGKz% zq6#|z%kj)!iK|z60aLuwFc;v%T*L^#gE5=gBs`c8FOmhqjoHnfau{UMAMXFBcnULx z>K&9}ugKSe!1o8D0)2;93kB|=D8X8QP&pXa&BUM`Pjqxo4l57(?-lG_`Up>`yD`50 z0wcCM+`ogB3pn{R%J(hy2E770lTpUOm?gLldXg-n;bO3#u$1ta?TChp5i@8t(KiA! zy5;2Xh*z#n@Lw5sd)F|m7UaVv$Xif9w?MX+VW#^s$aW_1GlAa({Efh$4g4z9#eR{4 zI_QR3;#9aa@im^)KSKS!ff?%85f+QOB6q3C!3s-`g~!v1ROq)nB1YQ@o*#f7wTqq@ zujFbj+&e^;mWy?hM$o0SAL0pgKh{QW7ln{RZ_Gd!;<=*0_8j_qN>AfM1zU2S$i+NU z1@-OnL*OqHAs8Qp@wn`IJfqAN>1wp_a6gUpGc^(Gl;OY!G+qlw9 z4AG$QeB(M0SDKromGrCN8sI3LoSDwd%fx%-RmkUg#Q7HW9gEe2btv0RxG#a<16`bq znEg^JfRDjls9xuccdS92FFtp|9l&nwtf@fV?Q{R_$8^ZYukqPJ-c*ZCFKPfB12^u zR{J7kBv#m>uqqbK>zA=udyB*F+{+ZUf% z+ph{W?+RSFskjOAZo8WR-YLR?8Y$E9)@dkdK*)Ay{D>iq_D8RmtI4 zmmGv!dQSf5)Y-^cjl46J1Dksryu@?*JA?#KGqQ0VjT@)P-~ z{0zG|zQD@vm+~u`O~<_2d!mEp)G<5uomi^Rt79E8U}haFjy$(6M$+s$R)de5Ux$?% zt%AfU{#kbHqYqaRDiVFbGW4fo#Bvp*oZ?K(3CCfk#-$QeqFAYtRI*ADD^#jVQ*NF` zIc`=%+yqN^7UouZqBnj3>;K9ih&i7AY5>;d2dOGmjTxOuVwI{zf3_Mn?i|bnosAiyp;)mWriQB#Y9w}mj>anC z7_1YH!}|XOFbj&rKjya7lu!EtQX4tJ6 ze9Skg1*(--8P8DdY7urL=yUqC-(dyz7p%k{g~0j!bFl(R>yWe}d6C#Io)OoJ8^m=1 zYk!Hbp2@T}gFOZpt4pwp;!;7ZJxY``vrtFcGnT6G=vCR~qQ3^!s;^Cqln-l8_E zTk#&l?dlHfX1I&jH}A#j=KWafr1i~*FyDR%b7+Uv!&vov6sw+()7uT|Nz9#X#oml< z=*Oebue!zEnDw~@R_|uZ@v(dJQY8Z(vvMbK+s~hBaC={RUfN; zm^nJYE9;-B&oO`Wr8p0>%Nww={tcdZz861;$I)wDDl#ye|1azhz5+W*8ZpCIh0)RF zVk73SzZ2JrYs9VMYRq&7qyPCg)~WQl%tJhz`Kvyki9I1l)iLY<@nMCZR`)fm?+0OR z-=PI-Az0z>vQ{J3qsOWdmte2TZd$hy`?WZXDLxYKix0$JvG31Taj=IVQ_I3Gf^OJ5 zkc(Yn-L)QCPpudBSQTi!wL-1PUNxh=Wl4);aqFC-x~e+;T3u#b3yrJSxR%hhu&Ab1 zzpvtZq^mLRjdWEN#(fpX_ZAv-s?00XEA$q69aRe&o7&r29aU|!+FD!Yg;urCX`R*B zw78?iQB|j3nYF@-u$rbh?M;gp%$(n{EVQP%t)sE2sin1pb114QvDY>=5+ROV|9si|Y0Ki)u>kbzFRVU0`A(cC{gm8lzM-2KP0FG-`BdcneEH>$>@TcdLa#S;@Nv0#3vEMYG`8zW zy;WgDx~3_r^OmTg4fdf{4Ws7jnVPrIV9;xpkr^$jt6@B9rphtYV0fs`pnYh^ocYZy zVWa?sB~^vRg+*ZvT{A2!D&}$_n{xYbt~~qj0KpcsU=gd@HXKFK`L>c692Fa!l$do? zWgn@N8W}*UL>FvPNvTeygzKWHuEb!V4{InGsW!qZw7S+=_E7<%s4dciYK>~GH5jNh zs<+l4S*w%u>Pl4T^%`|rY^sByE;V&Qqh`0Y8yZ+^6r;|hX2hyB1YTDdF>3Z=sCxV2 z1@jvhvoIj`I>)F$Jt;L(mvUZ(XlY@i7R_&5G&`VWlo*stbmjJXYwTlmfsL^QR#;SO zl%mMsy3{B|k(ooCeQW^HGNbTiWnp8Ft7LDHA*eEgRGC4d%)s~287Zo$w~d2(+Q$X3 zVa6@j3*;>_#8hS!qO8t7KCq68sg4T0XeZom>db3d(0ISB>x_)+4TkDW*&9Ub>LSLU zDC?p!$M`^5msdH)>#80!o{W%vJS)7Oae0M(B8w+%qF)>#6PxF>w6`ppv&cSiR(s=; zmaxhGKs{ZBQQh?=_Q@PYN7oy*QEZ5=zQR6*$%jrku8sHc+8Zr_p#Hi2!oZa4gsKfC zu4X|2&+P$vZ{%BLw2vy&VjJnI`snGaxP3tJ3_4Zj)ewAPv7^xtQlnv^8~ql#u_Zud z3j2gL`HLUgWEI5TY$-rdU6H|Pp}|n0Zjg#<%Iqy%PJ2sWY$H~+QS2In{ThSK8iQJm zUTkk+X=sa;uOT6$q}66g%N;F&)m~(fEGl)hbTP%c1@V^lvCm?T!e;qPN2Cjjyv3oj zj?2wkWSi|b$<<-AyQYCjF163G>a(y&H$ITR!KJr8WKIF?@WIY5oqyAU)o={NtX6a8 zI_4Nfm}3~{IhL*#dW*|-BVDE&>A785fcY&CpEs+$rKNR#V{7xACi{G@MEm>zxfkn& zFDkCF%|{7!PKsGl&@F@4V$-%3+gtUv(b`BaIk&ehoZVt?4Io#d%f3jr&al#^)z&F` z%efhVvT4ftXf0@fZGnO^T8d@04JK+04X8Cp)*2*r{erbNs=e6M7(=6K>Vw++7NpiF zN}Wl~h*fLoU0qQ`+ldwgy4x10N~K2XQqBw3x;m`w1Z!#en8FBpb9_I68P zu+~N?iVUtxjZ&Dcr?lR_D4^+;8HFz^4_kCxZF`FhL6sS#$_ysU`q(M7YEi2TV+^jgN-`#S{}66FXK9ck$QuXI#cEb(YpGG#V5*` znrEPl%aO{k(u>(jFSazlsII(^eW|XAOZ{R9S!!A7rMi_~?hn*8yTYjE`cnJyKr3z3 zLa`yT`abp*Og?nQam~I@vAwZ?HsFOdas&fr88IBr-B(q8z)f{|;7w8BO>y8&sdYnU zoo}KW{UxwM(K+~|;-)q*Brvb4K7k?C)(!Ond=uGdO3VraJv}U}kp&bO;O7<;j2olV z(4!eW2j7Hp_rYM4zbE1Qutu&*hV8R-)GWUUVSj0W&-my7Tg(WOFBxRMiSk=zD-f1f zFMu|4+RPxhX>;uC&*hu>HX!DP(2mIh@^pwcYX;-7q=12rrELs^cFczLWH4lA+v0Wu znzO`+w`k5XJ>DWbSheb4%N(4#phs+-V-O_W!$6z|Ex2;Cz?BUhuB>#pa^u348x^iR zbikFB6IUMm;Mzy$nKp(p&kPXH1Q}eGH*Ch@`SV*k7!G9< z&0yq=#-@4xTi03&{IhZP~lK=`9LXDyxs z2`p&yM~byj$3?^T;#r7m@WRm;<&S1x)Y8<}+RP;_Dk=_XS=506N=J){DRVUH1<_?u zT^iBUHovWP0V=sL^?|O}=mv2u4Q^Smuwyw=a>637uAAOMeMnwdUCN`U;=(e1#HISw z;lcoXp8&YnfMNS!|MmC;oAC)Y;}gv78dq)uxR!^oMQv(ZFe8LJ4}yp{gjE~#Y6`Ha z+`#E=t=M=R1#bqvDum5vKOEqy*I?eu(gt3C$^vhy_$JblPGDFG-#EL-DKNY~;HIc3@J8n(CQxPp z!CsxPUO;akJ6^rCMo6F>ymbKtiuKI6ZoRtB7kbTS7{l&*^~WOMb@6-k@iW9s3vRss zE)**|;{?BFA|mzoOfV0NAJ$eij-!M&ucaM(a27PS&lBzi^A^m*PE6XxslNw7Yf_Te zrLd$R?0Gg@yE>#Yv33!OIb1q>hxR1XPHBCwH1VLzM{*QuZ2KZo^NDelGU ztP~Hcx8UE#p(`*S4*KdD_)Y2-_^+rN;qS%_H05H4|D^gF{@bV&!q7hXC)FqLpGIDI zw<4zuk6gaHqjdfEx{m_2%qa zIIOQ{qv>a3U7E{jmN6OkdO1aQFkrOJtfO#;;eLRF=4I`LL+i+T6Ydqb?Ql=PJ&f=> z;BJQ72)6<5GPsN2&V^eAw+s&YnKd77Cfsy5y#10j4vuJ!gu^=uS^eP3P$oy=Do;(Z61eD4+7occ{LM_NaiJ7%mxX(gASS_7{80L{2-%wcDEuuJbe9EfwjjdY!11oO zaF<&U;V$OT^DW$J3tC}89TtQ(WaK;Bf`}KL$_xv4ngva;pwSl8U_rGOgw|(JuCO34 zpk8n}=^psWRQ~ih3yQR$U<;ynisHGCnmG4i3;F^0ui-v*?}fh`?oBu0USZtx@byr_ zZD-sQ@E>;HCq(8(KzF!rhEEXV+#4+DG7GxMg3h&|RTi|&g4!*J@|aI~xMy0p=@vA_ zg2q|UNQQ>MueNagP)f)O_hlATXhA)I&*o5q+zch+e28$|+2NdwvEw`(1Lw9v2}(Oc z@fbQ}LV8-peRmucO7SJ((hdlbwufjl^e#iM<8CKIJ1l6M1yShZR=fupx~EI%ZH(Il z+_hG`D;ZjE#`Cz+*5Q<&m1#>np(d2J(1biT6H5Qhgwp1k5aXIDElvwEsH9D@pfMIS z+=A*YXpjX}nh^7%(ad_z3z}^~lwt;_IIRnA0^>$^!8KSo9jZm>063g2 zWkFsGB3v&kG{=HG7L;s3aSTPK2B)f&qd27Uum$~KL0?ZnmI}CY6*87Ic{fU1UP3ktydInlc}5CPULLXo>}mv!Ia{1lvY^DDGk5Qu;9jyJkRz7DRdIq1hHhDcHiL zBw9G91%+FX-GV?q`3U8ae8_^nvmjV9Jv4=49N>6+P^0^jVb79buMB9X1?{jP;BZzM z#e+>V;yq|V_gK(v7PQHNuC*ZG3@TSzxb+sa&Vtrh&`Jver$*W(7H**h&9$Ir3z~`) zlbGi*7H+r&)mzXY#H&m$fnQ)jd4MumLaBgU3`JW|s0C4|%?f2GsnZNi`ptxrrzZWB zJPF%Z4|au;pc^OP_L;b(4+w($iE>CfnDjPyUY)cH@QW7otOaegAmEZ7<#<~x++7v~ zT+(I^y}`m=Z9$h?(8U&Xz6GteAn;;PSz+NiEU49jW?Rq<3p&k$CRosD3u?e9r3aU| z7I_R{sKSE07SzjvaxBPWLCF>rN0f0FnbZYJ3TB)N3{BchxWuF2;Q7SEI9T`xxUU)d z)PnX}&~6KQ(}G^Hpyw@Uy9I%=L64v(nDWCG?mi2;!-S~xfQgrzO&oKzk#QR=++~0+ z;<)Ep&?*aBW^4{#_- zq^C`I7ja)_+|Gm@sRI(WB|M(+patDyLAP1ZCKF2DVL}POC0xsCue5OMEohwut+Ak$ z7PQ2I7FrN^G3d>;aLp!^ya&)e$}RbI3j!{A8{>$c9!m6dsNRGUrUKGSImv>?SkQ0_ zq7-_(K^BBlEe%|W1r=COo&{xEP%6sdf`lKZn$az0LNx9}xjv=>H;H9$!+GtU1i5~* zpr1@AfkF>*=;vK<`xy5@7u?&7+XdW<3_WW>TP^5O3)*5ql;SQO$aBU8vMl{wV=Zm^n(R`Z9$(} z&|VALjRPp&B$>v;I>y5;!ryK|Pgu~y7IdEl-2n)e4-y&;Xd^=vCKSKHgj~rc6n~is zaf*vLMWlrbHlg^#7WAnFoeO9crHEf_{Jhq+j(*aG19|wP=1r4zv*i9p} zAD}XhS7_m2360Qf3vydfq6IlEh;TX;*hwQTtfc`F-^@!Ktfc`RG9l)a)5d*g;^Mxv zAlOYK#U2ZK7Y9|o9=8+z4hy%owF++Qy{0+VmnAMa$6i!!>-6frd@{78I{cG9pg#QQ6{{MoV?-Y6id^)WW zFy8x;GJ;kGWR7+h!$&Z?i`oQC=P*g}yV4l{IQvK8+wpd##QO;^G3GYFo$3p~F_=XL zH7%U+m@5Zm%;HKXcDWP03%&zy@PTJ78NP;H>{2HBa2kVD>OS~3TEjq0f^*cv-~==0 zfT3rM2Pfh?ijOxVZf2iWY~mHo=tGMLgZXKxl6(Vz_fQw2&T7Tmt`!PivAM&T*xpIeQP~D|Yc3!!9oMjapA& zUg6b+UA*3~i`N?-6rB{4(~eRkha1&8_}_A>Z&_9cIn~dUJMgXKYYWI{PV8J3J4^Cg zqJvZBDEBis_m>n(2bd)P1eHRxF}0UiuMTjkGdR^tEGt+)@SjZDrRd!{_>A|`ikI|i zi{KB_&LNoAqU12Haj!fEI81SA`9yq zQkE&q?*orhevs-Q`CJR-Ou@@F=v8+EKMe0kB9x?u)kBhbmC{oY|FC`VvzhZEPL)m4 zkl9SFh_xk)>qRhyiWMx|6)e?dEYB5Or*~?X zP^hdRpW=&;RR_Ubx@gW3HW+lAOy@kV(`&dER!g){yjeo2R`aS}8?O#7WK0|Bs%Ycd zoXuJ~nAiUnF%RpsQ5*_g71OzvJD74C*K!-_s#wRlwCU71_oqm1ph@kqUE2h%(s?~> zxf)FvuHXLLM*6eF`qRpytl?JBpC#6xTLG^x;q5$<%vSPc2FpB?F~4hzfvMD{GoD&p z29-i4b18o3wwA&4Gq|lexvh2M8qd&d9G`4dI@dymIsiDAc}S!h%<&m3pn2rreL9MP5n&fv6ePV1&MY{a}6{s^X!!4%v~p@RLboN6D( z+($806vy1hG52wtQ4C*U%Lkoi4Xqw;e-RJOwn)Yh56!gt4xcQ~Z1x+8D_P6*XE9e- zG38lI?Q{+u#G%a`+RQvOYwMW~@!ZU1Z`QCvEU%!n>TIsZW~QK9{$}RHORLDRKV&zG zY5f>5*@{%w6ED;8;@wfQLZl~NPL8 zbh01EtYkW)81o3%>8G6P5stZ*c*gtwWKq``80fHAOVSf7xiw8AiD5?qf&c>dvr> zHD`{EG^dg&SF+|5v$nWcTPiuPO4gG~){{!spJLl8q`F4)!S79FQ9&B@6qU?R61Sa7 z=Bko4XAVpLZPpxmLmn}ihYGIW3YJC%>qa?C@*FOE1;;Gsm>tZ?DyF}T>91n?_cG=6 z9CH=N9L(@wg4G5NeU&hH!3khf7_{<*nuN)@pws&R&gr zv70oEjU zY@{(aeSa(w`%9MKWWr@)6`4BnHDmw(``=ydEu!B@!SG5QJ_4$Q+>$Z)bLzCwnRP3b+| z5K%4!PW=#g(^_C8ER+qR>V*3Z?kBi|aClCV``|u+dm9d;U-=^3vv6DC9)-iGAMdgY zxf$*TxU1nVN4YP?xpwEnt%h3x*MZkvTXCHYHv{f8VUrUO7q(G0z(F=RH9%lQCdf{c zC6qaE9=K$jm@56a6*fplEfj^9&0B>NwiQ_-T{+&1@ z9lMs8{&Va@LcsjSJS=B;5&KWE-{gayAp-gPnTO~2>EeC)B`}*9^BTi%G5$J^`5fv7 zJI)xsPf5VHam+?~su4FczKO@Fudx3%jeEpX9Or3$kzHto*|e?Dv#&bM%*905O^B4z!2ulv68{pkC_ci2K107@-< zzcU`%hie?czL(*kdGzx=?fdk^bR2TXcZ8#K@qF~WZTx%(e24vM{g4^XSCl~i`Tp>I zWWvbzE#E%=?XH&iO*a*%cbqy7A*Ybks~d-OCu z^bfwHzGG;+=p_PwDEHUC9SAk}^gWK&a3C;-4uj&COq23KOMu&r5KEJMj~Ho!!Of4p zT_V_|2z(Ucu^a*0C^gZ5gUv8wajGu5W6=AP%ZU;@5fhlyw-L3u4t&8v5!Wd94nUZC z#GStsZ*j;*-{r&^;{WJd2k2*L(_!CUlO9^Y&%T$yxs{GWxHOnY^Yg)F{1r3~`>hK~ ze~j$hNzz*SYUTr4Y=e=WwaWJy=M!Kt`MxtC{^!2$%{$~2paX%h8Kdj<y3~i{9|!8S21GXgkZ$#kADizUmJvd^wpvUHntAu zT6yqRK{L+znT3D0n1g>eJTI-qj{Dp3&*gK0^2CGq_YhCx-&4GRC*ofCQqs$!0B07y zirikqzYynGy@x#C$KNYH6nl^}eH98PL*d&%<>Daz{qSV*1Gu2?KMaugHWW@hPQnRl zQ}9%lf^X?$$Z~ww2;YjrIa*aX>ufQfYmHOmC^BkO0 zb|y|Kn}=`i;CoW|>JGjrg%SJhIEQXCp5-3G*MPQ4oMVJ9TI?6warV|{ctWACOTUCO zr6%LO7$wSBvNo2ijU{Vi$=X=5Hb}`0nGeQ4ge4Wm(g}fF?gWP963%joV7UabT!L6G zK`fUbmP-)JC5Yt`1i5^SI@^bT6r}SRYVLE$$iXslu#93@Mp%Q9F(Mext4=(fk)#yM zh)&q=BVuv(cO}kHBuNFcjN(~F!7QU-mQgUvD3oOs3K=Z}g^iGu3zE74@QsjHD9bC6 z<&^|^y@jXCxABacEZ>3bQXsnza026pkf5CqU6LHZ9W;tG^v~r; z8j3fS@DE`P4Pp%qVhs&q4Gm%q4Pp%qVhs&~hSrH7oW)iTO{Y4D&DJ?W9PcDbKTgvZtPq)cCH&c*NuzoCV}fFk?SUy>n51% zCdsH9s+DA}l~mT>Fs>W=7D*qReo1xX=DG=GEl%ee@^B4ha1CX04TW{7A*z)uu9a-A zm2O-sIb17>YbBR!C68;RJJ*VXYo!P4bqLo-3Tk2{^`WAV_AdUfXxHMKbQfvU@vXX4 ze5)iH=W^}FDPIrcbh6cIuJ*1Pt?F@#TY<{Pms@_r33Yqqn>g+6d7PPdm)s;b$kq79 z!*rY?I2|Vs4v-bnixUl#*+t`=yyJMrXP$;8WZ>a6WzD8Etgl8cs&T$!E{v z+_M*HZCAO_^O1iLCI8s>rA+o6mT&vMSIdD{z#Ntzz;bJAxQLBL|D!ER z`I8aypzjc{oxmPeEx;;Zzd*~PF%Bpp^-sv{W8Xo{%7juZzWW5R4j>(^=q8ho7?_m- zXAb205_kp&;QIjHw{qd@2dTaTY7$B@86}svJBSwMhCdi?E!>^HgNT>UB~9VDPb2Pb zrgK;&fg7}Cotp1Qm26NX3M8Gwpm+$SCg>^5ZEb_w4)-G5OVBVX=^v_*OGsrutTsT( z!NC5mrs2fX>BzkaXUetsenx1g>cC0V8+@JMeYwa`XW{(c^Kmxo1+Wbcl=%;;Bb@6O zs+y3l#dip4{y-YcLMa<~BxxPyT<9&bP?p>EICr%P=km?ONrcPAT+nO=&1%pbsn&}D z>N?tSfD&U~2H(+3>yl~+Qne!0Dx|`f$Z+1^1-M@?W+2rP*gN8eN}zM}BRKj5-0TE@ zQ^ChHyhk1(^tE@DpBumBtn=5nd@p)PD!K|##ky+&qo7|*qL7hW`BRM7#|fClaH=E z{Mco|!Or-|2-UB1=E#v4&2w(rJbMmwUI>R4i7(g1S@?@Ayi=SlPB-zkd;_10)j{Bm zbnkI^yK2?Ubh+5Yv7hO7P2c;3^bvSVR!@)8+ZLV>?!veG)>!!uIw3p`U(WfD8Llli z!W~+SxI!da__Hj0wAdgnvhZ6idW@&?VD`6#!>K%GI)8b(rvLtg^bs;oY_RAbI3e66 zPZfu(@Izg~mm+^E&r&PgU!DZ?GV~U-UT-Z%I)@g9(sTzreziy#4ij zpM{5<#nV>&B^Evry^BBJ!3N&0#h-x@l1T9NLtn8SBMZ#S*Fj%P2B`7`2fon7>YH2A zMSbzXwD>p|-pVg4EB2B;+a+{a<#ZpIoY&i>(hs>vtK*`f)ymLn6fh#YI62xG?1*+m z#H94>JS_?Oy`#9uQ;^}ZyFyiiBvp@z4hdF^byZ*Mu!lz4Lh@4*lfpwBF_DpJvGVMZ zBWXmhOK&oIoD_^kE78s}bHFt7!wltm%SuafvNDNLqY!`E25VD`($b1jQw!bh!c7LwCm{#CJaaZHinPdF-v2xR}Ur)fVb-#l*yi{1KIa zOL*r`&cvt~S7OY0k>HxSW80IWmj>s4)0dSd!PZU(*U1>u6rpZ(9dYD(-6}pVQo-it zz}l7-CuD(l9P)sg(z4293@}HYH)&elhzga}Ffcb~;E3GL-(wOIVt#)wJZyKbft5W! za0Kt|J1lRY+Is5PUJboeYFw`&rF};9j!TSD^I{T4Ie(AnQ{gS|eAm(WM@YYcMWuKQ zmLea0%Ph46Phq0U<$?z1=0H6n!H`>lh4M02g3ImzbD0H-v>%GCM#lt&honYk4DZo* z#*n-oLmT^aAD$VR8X9hoP8w8~HDvXSK4p#P3@d3;>qD}mB9tR8p?ky30oAjI=D6Y= zw(#hz;HacX@6_|gj$AXnEEN)CE?8pd8`w(WS<}LkmC*4q7;%%;%lEYs*A_kmXWmjP zX1qlw7yF>-jc&4ff%M4g>GYz->ufFc{73@5Lo#%(`nTA>i96Md;9U! zI&^$Eo^=@y?Ln?D)AE!4o~5SC<%I)2Cs4L>fFICzNk5dAsn;UIr+9&cLRztT$>z~g)NyU?rmC`{;l zh?@JKscS-PVvm%R=s1Vn=?ITW?umXqwU@``=%(8OR}8ujQsStD5GXN}IaaeJX2c~X zn;pAZBF(IA8bQs3H1swVjIzLzEiw$!NsAC?KgUTdPUF_|oIOZJF!ZAX{T;O!S4^}k z_?NH*=3;*g?WiGpxEj0VDo9S2;ZJaxgLLX6c(8-fPFcC5G=WSaEFukiFz!J&BCi^; zwyC_lY3+#OX}O^x2~qYid-{;PQD+Sqa@MH4@oq=-H6*x4ovyHu`~{)qGu93tv35pT zLSm3JG$zreqGKCYjUIJ&16u!Ek?3O+5@WsyciPjkAQ9${+wovX!hx~+6Woql>yGlB8x z5t|?rVu_0?e1xDgUw$1Ig(%Tcv2O;l5D0@E(o5gzm`S1559Uw{>6oE0TkSHIwvLZR z?XcB>wFr!#ENRt)^;q1^r4mamsgWYx%~FDZy8C59eKKZWj&l5-kU*Cdv3P!DJ;Vd0 zi&T@IUU@8z8n8cV(rfA8l!(uv?PTnb5aIrcgF=8H>JYKuLHQ26iEt zdD#Q=NG?(vp zveuYI5;9gJYpusle3oc@0&I>(Z9~V#Z%Gmot5*F4?dHK4Wg_V+o}L`J{wjf;V6$pH zFEw=^n3&r;!Jd`mj7HMT$_2S zM(i&i&82J)pyI&D>fZ+SzW7pw$z!gZOgv%^^Z>ptqUWNCnVl3L$YLeM0|8&qSKj}bkH6X z65_Of6;DIs1oc#CaA%v8m&Hoyavu8^ThjEP&TAZ@p$<7K`PeIrnHS9?D_Cgck2>fe z>1fGVcQE-{3pwyJkjW49*3Smab%Jz22x@8!o;O3BL7n5`;$x!Y5~4$5gXC=y@oH;) z^sy^q6C8F`7JlsA==k_()#h|L9LL^_R=LsfdMzBbJiSc*_+Gf;pZ6IDtm6#i}=##C~>4eKLBYu0A_{oHa zRr|)*m+g9s*#MT$FiLRKuo-y{$F(HX;6hdR{@_}$t^zI{H zjEjzONmq}w5pI#eC&f@70=dV@lL2(*wApsph0IeG%T!=Q)v)cHe>CGk!u^v z${N>>9J6Z3kX2(&IeW;EvmqPKi!}+ee$-nA%nQ86eYcKx!fw#4upXaAlsZ0eMEM$9 zcO4%%kMNrIq!FK<%rL*e`Rw3X*L<`ve>yz*@osSR{{iz5uW2;vN_73{Bd|yJDvO_A z&2aSvtoQ?{`)~18Ez%9<*Jd}vt^DI)H!n29xy9&wC*W&6q#JrUX@sHU!^E=z_#+lQ z#-ltm@_ofh=MN7z%Ht15d3l7b)8+D7>1f2E<0CMePQ8Q92aP#&yh|M7Cqy0J%fctg zbHsC2d>V;S{9KLkUFwzOZKm^wpPUbW_{sV4hj-=c{1ftz6E|UhoLQdpEWV?#Zfrim zyvAcFy_|`#T>g5XQIwA7Ck&GgKlAG4)SopY8amcd&e`tFTI7f;re_Yiqsg*jY6y+RK;Px&xtkmf{4Cqc8sZ^c}O zkx5XSV`pz|t~lJL*zPBifl@l&AfF7WdldB<0DI<9@cgmXL# z7rJoz!fKLlByN4|#|K}%u6gnBg497jD-U?QOyl34^|~kg-cJv0D$e zo4x*EnZBxjRr#M~hewr4fwuJr`xmcWAZ?0tHrhcIA1pGfzlQa$N!D~fb)|NfET(*^=B3b_)rDt9%FHRXn~{>sDT z=;ZIpW6!8cGG(n*DRZ-s#7Y*9^=dCZMly_KSKynCy{S}h<2Tb!^zZKL+ueUuSf6j> z7T{cVq`7%0y_vs%P|)Y19Kp(Jg0EyAD~rfdh%oYyaJNVq5u-AE$9OT;nf%`@FzI)i z)jT|z99g^DB$kRr zeEtY~~`>t9(ji7PyssO0w{e2MKa{ataC0ci0^wjR8?5%-U;1b#OI> z2Tmz94m3qG{Tb};4)`^%&mE0=j8r(BX^s9@AnbXrzBfJ9`;`! zIzD9Ugwr^!k90x-PvWG*m3BzN3%F(b%XdRkuf9KllPOp3C+k<-AH(T$91g8(_5F>g zi%hz64%&?Y`WWS0H_xMSy*EYt3dK;P}D1YW&$9??jMT8mqssiu&YdI!GAINeR zeK4>Ci~L79Sg@<_PuhB|xSzClaesjJ81efrU2}h8HJp1!#8^1o?iaUqFk`n#KXq*3 zGX{6TaULa_Gad>nTR3z+2N5a@4i*yOF+PtJGJ|U)TAjiNn`Vcj44z(h&>fq}Gz~Pc z++bsRB-c2&p*3suj1+0Rn3%Vrf^aIdk7mX4_BJXV)QXTA(7BeS87`Yfd*9}C3lD%@`s#6D@DN|bS zKK7u}KE1QEZ|`iOWp;1h)Oj6&R6vup@=d#EkL)j%PCer5+I`J@#IiQ`?3ipSES^0+ zb@qI&t;^4R{;WD&9ysGw>({^Pi~-`;@PtTH;5;5j)ZsXtq(Lw2Cz(ucbc=YZJIlJw z*pAp=i4U_M65}Tq2l-2-izBeFB0-ZhNV_FxUiu5p4edp3i7#<#;Bd|t@Efxo#BM&K z(KKQ_cr&O+z`ZyvlBg#Qt7sJ*Hsq^p`1EQxJQ82C;g_y~Cy*5=+BvcY-pH*l_+60G zbQpf16`Pubg@`M1>v6?@Da{9?b60Ozx@Oi28S$PSJ-s_iVd)DH%9kx2nVUPZ#BSz- z`7$ix?ww`&MQ`SB;A{EoVlA_{;E?oYZtDoR2@XlNj<}!Juz&}lISOlS%H)_wVTu&f7sfw?MxEFNo^D1(Cx#tS~ ze2Du)*ItieU*k%@NZ(s~Jxr3IhF`J;=(-~7u-wQN5cndABH%Hc%erzu*#ZI{alFML zyfnZ&=f&bZ9FFz~&mSU>9&~-8tTE2NgwK_0A}1v-+f`xnt2MbR|JX7fZ|SlYd?6pF zy-*ai*SE6|&WmVWxg}n#PXv*-5~RphD=GLrxrBR^v6E6HQEU62MN7{V;<1F^C-PQS z#`({+afXl+z5=h}lj2@)UV(G{S&W~<`8>xRzrgrurC$dxTAXW{!br&Ga!6kjjNA%k zL;ccr1eq#-bw>jJu&eT-B-N>PfoH6v0qF~opvQOY9jeOIfOMYI>k37Z#~zSI&>TbN zhC{wX`V!BU;06ftfD}qv`{lppZ2cqsfJs)*P1OABf zCsMs0h&u1lKH>_ifr#^ifxDgX;zq-Y=h)i=c)!ZtE>{kVo?PH@hb`o}{hYlmKyKd4 zTv9%u5uSLT_&WcUz0Tjk=~cjK-3qwDdic5(a9W=N?uWgy(hjXd0auaXNLeeneGVOu~934ET@ijmB&rs8(wLg82x3QG03HW zvf})-V{|=^^6ofa4yH*AO&G zjG+ck*xpESKjB;4uR*6JU}!??Ep9EXAPd))Sw2tS)FJq`1gD4+j>V9g}8%BqhO&hEjcVEz=i_k6*B^ZL!>th~C?? z)Yi7t)4kN*wj}uw(VL@j9+0Aa9rbfZHg39RCfPMq@4gTQWq;qEf%2Z-p5223d-~8* z(W4)sr#zzww&l06kEsg(fem*{kDnc_Lgm9YG$%cNSqfgdN0B!{`5DFKvoX5rGkNJ* z$2YMvVePXASDw8hjb|TmyxZ}CTFY0jTs@p5{~vT6lF7W6@}(6*_1fRjI3TO%YgZTTt9i6}Ep}+Rr6>1YQ@tuZW?ywnzuiXb zMvQ`duQWq$>7mnz!UV2O8>$Fg*Y19x27d&8T5&&7-qSVs(~dV{hgh}s|5t1MnX{`H zN=omqL7zG`3~jc!T*is^CbBTslLv;9#8!xtb4iMWo}89#c?{KCL`GM6=5nj3_m{#8 z*K`7jn?cJT%yiWmUezT;bKAsfQm#Btsih4Ocf(C5A$u%|5`ss*7RYF~+fFX6jsC?N zR-S+^Qp1+~VH=*8o*?a+WHI(2E!Ljwt#D1?XL=5z}E3ePhw?*6Mdwe-#bcbYWjpddboaeR_G^;71hcrc161|Pb9?i`3mDd;4NXu|z zq;s)vXm_t!r+LF#P;Ka(zG~yHYbP_4dk2hYyd#xtHnTmEf!I)s;Dm$|&==DQgIHzI z3xTUeaKZs_jw&Z4QwS)6#kSgU25bcLW3shs^n_5hK-};AspiR2lP4MlZ4g`3m2B^L zTo6;whW7Rik~wqrrcFoYEQH=QB;Hx3^e%2ue8&^-Y>qDW>>e06rLXUlfq~s%$Bb4Q zdkpi}%6T(4&M;N^6&e?wE1(JF82DsJu1wrXa`2!?ug^l!Ly zeDZS1jOJo$%xAb2Paod8yD#AJg`%G3EGfl@kLF?q9dJ*-u27k_R^D20?ojfW#0#2A;(;oPn-QH2h4OOm)_Wq#;e;7V}h+CWgtjtt3 zZhMk-jzH@9&^f?esT9X z^hMW}@~-)77Rv|bTDmqCV*~r=dsJscGh&i4ciM%Q^z44s?55rwecju7ySMiB?dWAc zc6xO!ET_$+?A7$J95HNe@+0ESuw-ebej_#7GdTdB{b&vTyzNOMiE!#F_+MBrTj1@G z;#ar{pZ9R*>{W0mE@t3a#NylpFtm2pSesCj7 zrO3Yw>tchOub!GYJXYMft^giWxx!>O8qUrsVn_9z+Xu3JIaLklAtQO3Z#CL@y2FXHaUDC*B!xoFkhv@Sk+!c(*=L|aEM ztIEMJX$l;gla5{>G=5(&10@%BP75*V ziwE4gr>>)`K3a$TS3_6pYxt0$L)ch)awFAbqP1_uWq#~&c7WRHnxP?b0xK&_l z6;P&_YprN!Z!jy@8_r@)3&@tLTEJMz-M0X*c^iXPJtAgiblV{# z8a4h(AJW5~aL|bO!|Cv`pAom03R4?@9WjhZR1b!QtQnBnFwfnNNn3;B4qQ?LxO%{) zak(FiouW?2=FnvBU4*7|f{u_S+bBfJ!OZ3JsrdmI#~yEe)SK^)?bz#gHTvlmZJ z9^TZurK4j@Z{Ox(akFIB7ki?CU8GP{7Pnr|Z&(3e9C035^Y2+W)s^gOjtJqjD1;F{ zfi+IQ)N|rRL!3uF?zkGemPo%QI7x2-f6Vsw2srT$0Y3^0N~|5+FVo5t@ZUN1*(;6O zp_MG)55sFHRye_Ftr6TUJ#|qSR7A$|A0Z#H3zAb&ZMJM(ruLgFoIzzwX4C7I(iGv0 z_O`c77w8+!Uf7lCn~>DO14EVH(u^@PH`?4hniIby5k34#G*N1eQ*R{kUi1d@ht>U; zt^ls@{1DK&4UB|m9=44~$k?+!MXn=m1b8YeN zr@s%2k7u;HIL{W)XYL}p8(SV`uarImZGfPn^*q~yOmphl!-$7bzkdXTdpWg_XJ~8FBmz>UHwSd@Uc=P8|$}HAMTOYgA`gKqaoA@vQpY^>r-K*U#Lz+ zq+iH!D;|qmF>=GUr9w&_`vqT{3t{AK)YM2&{{2YUKV^fb!3tLW*I@25F> zYI)f489vHA7?)dmm{=T#ehd*6#AtR2HbcEzNq&obtYf-47zyiEqe(QC>76gY+R#m> zJKmCHPt(F6>`?1};*=#?f3)FQ8(QJiBA`cqgdO&dt$6XDu|idcMV{SiR9bL4Ml2OM zv(4~5=JRD9c{^eXLx&9;mQlG^X5m0xQ2oisj{`|CQoayLg_b;VHK$0ET2MXE8XLOI_L#Pl{?T6?8q965O5C6nxXTwYM1FIF|cHG9teO&&+ zZPiSLRe1^2Z{SIn--do4pc#|3&Jl1Kg6@QONTpoGwRsTGPphUj*ZLu?6g$`#t-a%) zGCc{e8C8v7P<88W#fL)sb?L&YS7+)6n{nCg_E>sAcRF2~Kibemv;5-n0A`sm$$KO{ z|1DZ;t;>GL=T|}ZD6g}7zmx0Obgv?vj%eH#z!$KV?^<>Dvlj8UtyqKasNGGg@7MgP zhZ*=P-u#Q@S1^6mQ-A&hWG2;7V=iFSbTqDH^K!n94J%NRLL!Q5FUK&pbAH3=bLm0V z_YIf#^;N91{+K`F+b2nuFA#L%Z{-hMLUY0r5=CAFm*sL9xt&-Yu!F1VpvV$EUa}nj zoA)R}l4f#xho&omp$#qTHbCTnM{|)wk~-4UURYNXK2c|ygwM>lne5999vH4X!$pvu zA8&3R&sTnbqCgV#w;`73YfsRuEH0l-y~bRf%Q5*ZI~y9hkdGC~{2S#X@AdlWja)uj zrDE%B>xz7A>R%_B1N(<6M|pQFTRuL{fAQro$HgP{1~XOR{Na4wn@1f#AUg(CGaM4? z<~noIEik>@?EHG(cYZf%4&v^|95-=^zqmYrR!Bo2I5NsF7bEqrf-2G;$7fLmuKG@r znWB}4iK7e5thxVL$9;sE<^aQ+HHI+yNhe*CnI6Uj3I-W=0V_>w5SA#k zHGR#}BBsmcnGnVTF(=~ggT>*dpiA^}SbBl;YwA5#goIoql1I`Zdlau&H40d^-0Bf{ zCe2!4=*j)0SrgutF*5k<`Hpv<4}2os*637>G>S~TC6NM62|zYp3X54xaW@$}}Er>~}`A3+v+)z@F* zHaGQ!eB?JSKZom2v#evO+E;m#{j|9L6xP9$2VYse%5@%k8rJb1iY5tcAXdf42Os)b z#BW7ErfdBG6z_Qo-rIX?SLbW*dDtEWUH>w_PNSrCC~(F{S$$6Wb`AarH2&J#q-SgJ zryXx~$hG@P7ZmOM9=bE_wikW=kqysF9FG32L}=*!)#p~O^QcShdD0059;?qk;&=mJ zJEDEE>DOL=+HrHuwl8rFYV~#Cc@OD};^_x#z5X@!2(OIhzihZ$dgek}$Kvi=YtX08 zsLhJ}1{(^Gz!})_it+u>@rmj)^7Y4EC2Zae4WJbs0TGKBfsIp@KFCZWKRW4`v?q%P zv4yXl*cMhA)r-Ox4G9BoDlMa+zM zx4`PN?TMQr4a1#vkx(S;uj?6af?`VaNK*bru1$T2bNWZmfi7i3Nox|&$C7k!8rBbb zQ-kiQkZ$DVEs!Tw-22<#kza0N`Rq0KKC2P;MxHC66vqJn~ zZ$tOC?&RoD)GKNJI&Wk=7b`Zf%uu#*xGUMWqq}_mR<+)*NP%>oY1#T@I^ywK*;1}! zU6a~S=Su}Vk~dzbrR(c55uYcLDHV!S*;c}g=qpK?J8d5DM($hZaQQSFo(ES39P9mI zWO03__8j4vo^wl&P1IWBUR;9a2+E&%J+cbuT^A8vYR^2!HCNFC(mg~=N5@ING-1Y8 z?j?Qp2-j!D-K5Wo{yuESx0^6)tM7jnaGEy(uij6(2;g>~Xg$|M7h9126SY2(7fV*S zo$V^51#-hlYG6(Pp+?nVYNL5Ov#qOkpmZ)LC zr6FODiG$5buZOjqhlOabKa7d`&m9hYRMmBfBw*Nz{ZGtivm64K4MGum5ZM|q_Z`;C~Tf8^Bh7w@x+d;GxgucGG3i^CXK>cg(-LscgGJhZK(&q0|q=%Lt1iexlXRo>Y z+4G4ef>^a@tZMns%d~12mtSDdNiIb1S7+wbyjY}I5MtPO&FCeE&b)+J*6jG?j3v2% zgl7&NK75FAr)lMx>Dk$7`cC~7t9pa$XRmPR;;n#JM@hOU;C!?qn+We;_1wz!s;(;9t@dN(dR12y z*Q@|eT4vA`s+CtD9)uDIJM-$|bfT>{ zw&=40tLTdyj%|^)R66W~Ta;&|wtaP^tFgCG=UJ4b6UmOqvAnxJP@k)hrxe#pwu=}q zvR(KX6lAr$#Qz@!yIiX{Jw_e-BNEGE6zWvN!+mElYAwwUt+ax@eO zJO3_ng|AKvI*-Kv4iYqKI=#-y3|@%Rv?Frm>v%hoGw<su@YjQ-iy!A&j1BS*#ylliEGSYTzt z_QU;#>aEjFO;O^7rNOy)Low04YmgPWcKhh`Ow)LdC99m1dPY12{xLy$Hs>TbaYKT; zr7AZhDDgc3eQ3MjoCGE5FWPw6-j7O_h42ds__OR^mVd=L0^Lu1NWdSlD@6$`5&seJ zr;#%wI0)TOyn^6}zS+EjptPO@lvWNSUnBdxvv2;w_ROxv6|Bc zJKI9c7Ag@Is;y(#u40QvfvRdoE??H9%HZtm;K1CRRo>V#ytk`svCG{x*VVkqLPlH6 z=?S9DUwL;Q(>>mx>Q(zQ4LK#Q_stHMik*FZoyF2j$NKj2hJ4d-rs?Yb-iAgU^_X-| zG#|@snD}!r=-0GZV|y}K-@(_h=sofKCv1xPBx=vyJwB7; zRQPeeW4#j+EiP4`A-(dSfgwCQy7KIKE6+}~;MqqUXFKkwwS2?M)yXDYecG|t@z&Z^ z%7ma^;=Qxv6?qc-Flk4c7z5enbQSY>nx_W+X$^Yx0QqA0z4BD8rK7v}Izpw>gZwIu zp!&?p2v$G4ZROc*^z0*!-@rRmZFy|v>ORVCdD>Co)k?&Bx30SS3?IQLw20~merI2W zhpvw*C5mxxg_&PJR; z%%QV}pC>M@VOS=uRnR7&nAu0ML;H8tXShE}%*vBPGHSP`mP@E%PTuZ1oNh1=Idh_~ z3*17)h-uSFUMLd8J;+2M@xEeqg&_L~1ocW`KNvhXHhOSq=-}ws!NE-%M@E*GMn*Qi zSBtsS4L7}c{f*n!?dvoRBjMMk^TYc}Z3iw&FM5JL9rE7eVH=R1)4O|Uh-z+(4$sXE z56{ki)Tebx2R99!(w8oV42yObC(HTzo<z9X}H@@V3d2t^_UF09y&mm(& zyjQ-1$0_N()%F#3fZ7-B;!wA0zwln_7qs$+8D*kU6lb$&Pr9Jmo|Hb}e)N;>x5tTd zpljniz}u4?CQc(9MMcgAs%W%LPz%up-#kHop*WHVLZ<$5qnm&8-8;%vej;FtQMh z5b`xcda4x2cDLu!T{-r5#ie5VeCxzSZ!Tb(ffo=#541JK%lV;!p6-F6){b@H6k+x{ zwt#)6MAa`#(QKhG*U>gxD9pBX%oWykwG=u#3oTtzbH`ju%UnlszO8M(*fu!WRu~w7 zmVjIU_+(|W)J0A}HL;461DD`52&+z;0RNDL+1GmUV19JUDoG+qT041BbVj7IL|T zQfVQdU-0d}{j76t+rR&|bI!Vb|CZ_NcJ8`vYU;XOJFkO^z>wYGz-jScpuc=P&|?d5EdWUHR9`5N~7_IAX*E~V59t-#)Lv z#q{b)^j5`0+toBrCv()2SC{wVb-rn(U>ev}LCbi?$*$zc+NQ^SY#gnid<5nr>#lqw zK&ocU(EaSaUd5giykT+Yc~8H!wr*Xk4#+QjK`IzFJP&`aO;NN z;!F;8!4iHZt6`M=jCdc8_`=%oS?d-q!Zb;aGUjX-WxTzPGCOc6Zs&A&HeZx?C_|Rm_WG*w_jtv)0Q@*yvecf&6 z9X@MIRD$0u;?f6~_H=pbx>C7`Z5^X$kFU?qw71N*wa&E_rdx@oVfzvGus-aV&>GUA zN-&={;y5NCQ28RKi*rrOS^2#yB1cdUO@h-2h_W(P37v9U9Un+F?QiOy92{(-@L<;Knz z(u3(tIWyDWKXdKarrbn6H<8=KPS^ClseJ$5e*Ej0?$Qi@+i1R@glvDyXv>?L`WqVh zoBF1vDzW)e^I$qX*j$1-K(q54X6GB&h1pA1C`|*Vq`H<6$6KXwosi~f9XC7uXDkF_ zM&C^9?lvnDV%>G2LeH_|u6x*ovE>N#_g$Too#8IYi*h0 zzHysBLh44iygD8G+1k+G6sTzo**To|W7dx6n>DTR)9n*Y*@=$gM5bw?y(yn-Zp!C< zU7NdmHg|Px?&;p#H9pbPy>4B1&jctTjng;2$Tn0dArDR|Pyqgh)fB;M_@HJorRWN{ zbi>@H`tZw?L)w#Sz3htmO<%XdSjeb6uZ4(DAzXzscTOSMNSqfuvBoE>Ew9i4I25q~ z2$*eJ$fInsTCAv%Qk<$s4>p$4&0SV3V@|t{sHUl!s5#`>n&?hoCt^_X1pUE$5>=BD z?YZ!*7fuFp#yU#CQ9avK;|i-4Ie{Pead@Yy)-T>~p>EzPPWi#QPL#=1-5!rS5lpnC z<7!+F1ZCGb+U3-bw`2UwcQ7bNlyq03dB~^weP#gV=A404ofY#+Ea-Qu&Qm>R0GV8x z?%OHLZmXlAsV_-AY{beNW?Ss)naCwp%DupVEpG`~He>vi8j4*de?FD^xgK>o->H~x zBV@{^_I%(LgC*1 zt!gp<-=lIB_8teXR3njiSGKhuDhg5UbIf47P%8^ozo1&qfidZMNKQqJ_c%@I-T87d z)ic&umoyTE33r!1l_ZP-8Z5zk3!JuYJ@^DXov3INmX3IcBESgS7 ztTeFE296YF3-E0T4oA{ah)G^nStO~wA`q6P%l+?Ck&t(rX-U#$&Y(+IU0&ylmZ7Oe zOsB72101e^abz%#6!vNLS4RRjin3-MDtQs`U#<38t5m={GJ~0D+K{5ThEij?ub~iO zsg}$@Hr$YkMC((LcsgrY*|Z)H~Dwz^c#;|X-u1OA{D3ddsMkoEqQ z6-^{i69rhN-X+-18kRxg7CMJi1TkhJDo~jwl~;A+jsf_(wYL%64HxtE04Ij)x6K_ zVziTHmpQ&D-46)*%&r8ahh50(q^|>lKC=q}DdDMl=}Um1&-|%IoZU8zo#?#J>@e=> zW@odk^m#zgXLbo7^ezKFT;696X=4|$F!pHkKC{blPk|j`8U8NxnYT=k1pI-#&m59s z_pmWJ1xO?A`5K-|vu`@SD8~SyK2!C;9`+ShC)Wc)ef|m{C7e!HkI0wkGv!M(visOD zhXBv)tAKRlB(W?kLeb~@0ZFmDQ8hUU2=L6`)yBTa!g2r*;F-5vKz}m)UDRi4nIH-N z6!6R*zzVcbo7aI#2eA5V9Ybh7P%eg5%gt^J1XW9^><>t$;SEUF1Wm)|KlWOKY_ASH z`!VT$)SXG=T&64GkD6Z~8!}_tW*y7Nq|dIs&b>@cxR;*q!S$a@U&8ZQobq)sJ%1TJ z-^4z^uMglHs889?TcZ6ET`#aV+Rr0$v-bG|;`*of^$tA$3x2&Bdgvw8{)OT?qBnSd zAFh9iU(Y+dI29h(XT^2AAGo2j4d03S2f)o$WGcenKs=cw;lYSyMX(lwH{x)- zUsBK$SYxne*Yrg`?)N#hzIkfj*t_Eq^bmcG#4G2ldR=!1Q^#(#>g%n_fx7Rk8Tat=6592OcDMCd zp(u?Mh6u&IX^x30)104o(stQsUFASDm5Q=k>%PPB%3G%%1CQ+c@Xxf8Rk{r=)HY>7 zxq=ZS)CbP#l}vwiPqTI0aPGE(?XxXNW^SIa{PlWJR?`Q|qi2pZ3@oAgvWT2hLd6zBrhHfv}Y_jvrbzeDtz_xa4=FdeO^67;kN_ zIr;~s5`6^*poc-p?ssFeI!Al{IOMPqE7&&I(mauGo{aiZy5UsQrw@!C8g3X^pl(t! zG|SOK;l%hLirO_zh;I74et%leQ#aez4`yV>2&u=bU6tBDwJ92daNBfrJ;q+^Mba|O>Mq_ zjw?0>KC!}D&OrDT)tNF%Ngu;Adgk8_>q0k9{Y_NbXB&=lBMdp zn|;a^e~spo_d$2mP1XP6?Ow0WYrB_N3+V0LPPcSA-=9`+JOo;L!tIt0^7w+?Ph3C5 zrHK7j{633*BlbP(ID`E&J^$^^__@U9mOsP(#fcpr%W=nMjI_*jwgo#x;C~@r4lI*c z!3sP;vFk<9{Q6zJvcxVm#;Y_le(WyzgTEz8>_yV|kjLB7K>x zF`OMBoD{Z$rdo24(8mUR-mMo63?1I&^%P%MZ<2zJ!qKv6g4=bhjhH=!hylV zn?3i&0zrSnio}1}A=xEw*pgz=Vv|b?g*10_M})@ELM2S1Oz`x>c35En|BMrCiV( z^99k%Mri(T0S2IBh}k0)YKq?$BpVQrwwQP1F4Sr4TC> zSyvx3`}?Uw0d#+9^KsobTj$HN>l;z8Pj?2J`wM%A7B zg)}vUQS5CR=*WfXeB{l`>)55>XSTgaIS19w@H6B1D!P8H?7hjU!OF&*|C`u7#U|DJ1De@`b%01p}Voa3|7S4h8twj!7= zXuevF2dV%tlgVXPZ;UfsD308UeR0x!YM{0o&JSOMy&Deg;(d-!+U=fHkA$Lu>HFmB zc2_psNr&qPnghWgWMfoUX`3Ai#x-_h9AQ{dpJWfhZg`{PD;R-WdgC8y+&RGSdh}2F z0|DPBba?&M_Zc5>2izgq59*VMDyf0@>AX2Q_dCZv#?Ir~Eqb?&lQVCQdWbHH@8iyNgQ>bEJ@JVAOa9$G z>4_n2>V4@*@F($x2gM&6E825>gtxb*HMG!Zs(1=5RG;FQyqbMdO4yj3<(Qo1n7o?Z z%i)Acg5T^Sc-V2B<3A;Y7l85g{J!fPzvaJgcCE)~g3I4SotV3jm5-fNwk+t0B0JJn zEJ#6uODUHD?|=qhRX{{gH@jpto0`}3WDD-~flV89_$_rfEs?d+(d}7Ox3~?Q(X>n- zJqDBpcV83x`1}?o%VCw=eZ}_5DT^D}hr_xNl0t@#HBMUYe)$843vF|piO3N2Sxizj zM$cVbaP4s_M0e29s-{l5aEj^(^mFwNi(%!21|k?uYR0|L4|8^nCh5+`p18}SnG@@C zEmMW&$z8qWId?QFnK%H$+2N`WWkxy@bFbQc=BHk@(6s)l?d6T54XDHDYizrC`=K}O zY2Eg=`!Bxq{hPAuPN_W5INH-7`6GSV+*q5{TShI4(Ln!9W83=9&O^KVXZy=HtD$JL z@@OX2v*oPb4VO)hTz1EKgNOH&jHK>%t1%>R!3jFE|S|72s= zL_^#3#%y;#@B%-z^lsp#!Ley2zE?&$DOj@{4>ndb8Hylet}S*!l48Mt;6dw{M!FyA z=>p2kxr@fLbLH~-;oPR-;oT!S=&>kND|>pjUdf^!zjN}k3(DESOxK>#xidq|6$;{5 z*YH@+z)*8J(Y$^8`Z{cj@lBpRSll`p9xyib6c&rE^Se6&S~4_n!LB~PODgR@G+vmW zZAMX@UQ~|q1|nWpeP*CNH8ok^vSnd3;xYU_q#lxY@hegcYoWuguZx|j;PJFp(RGXu zeZ~yKyGx>(7$5Njigvw#qQ#eJ>49urhewfoN*D#1{K-sYV*7br<*N_ehrm+MlrG1a zVxKCM>w^JL4Ckggok}kKcq41uzqsktmY{mFfNj_~)M}NucS}e70 zn+l4HK9C>_4%5zjy>z}2haxIPmY+x`dT$dUfboHP1s`VRgJgvX~ zv>P|9$5E{V9a}D(n7EMYfQY}R_3a%Q*jG*|DZfv4`~0xbTgI~iuiNSK*LyUWoK8m; z#~nWk%`eBRm244uK53LhVM5D8##?PL00GMYL9vU-R1GP9;!UUf3>T8RVLYc*bHWf$LU~&k=X9X?9lYA z$p`sN>r_5J)!IIl%TKj6cXl>6b#}7%pbSdObW6)rOJSyExZH%nHD}9Q2NCidw#%Ei zZZ*5S-|2;EPU+d zwi_d9E8R|(eq{?aRw3z1nK5X!6Y-2MNgqXI7XA`gIPjKXQWfDaA<0?l>$A~HSXJC` zs}aW)lz>}h&hfY%5eZ|f^d`4Qa~lncOVhOm-I<&moSpC9JLYW7Sy@#}1~e%Uow_tr zO4X-g^?v`(%{fJOd1V|TuEiC_^f6c8xqZd0z1zmR&N@(#oZH(wf>@+}wJ{J*voq@^ z*EQA`5-rg?J)qq#L{typEMMUpISwmCyS3$-B~RnZuUl(4+DJHhj8ODg?VEH5N7DqM zjK!LuzlP_4);m$!t)JR9G3 zWP19@wvAWMIhj9D+3S`XcXW)M(X4pA0ax+d)4=@GJ3BgdG)Nx*#@BAydeg?~Yc?x} z`wJ3E+GYAieSWXvnHpy6o$q9FwxJ;-$?r!7i_-xgtn*EfndE66cPv3-35i+jT|T%8v1g+Y50uMk<*8UP9GUPV|X)oXKu8qX*7qj4KGgeW7F20 z6Gr(OmUKI9H)8Yr^FJmJ4I`~$@~okuvnJc8^ZDs^{>jwZv1+SN5UU-!cI=x)Ll(nsp2-0&@@w7bb#Bo|wEv*t+8kmvVw!Tti%~vSOgEbsohR<(a;15d$_-S=(hT5WO1&f9y{U+* zSov1n08-7K9lSQmL<9RwW`u?@oUBaMS)zC))wYa#QMJd@w|97WZwX#c9FT|FmRN!L z<*TI6!Cx0db_cTlxV^CEoFm9U*lKG|KKdJHVZ9M{vJct*!h>$}ZF}9y~ItR<72DJ&`EBX($WF)DVhRPAC9Os8(@PXBTN3U7eeX zvvr-x`eI#Ou|C;ZM>ZqsUmJ3p+djKQ`XJ&xVQ?ObQGN@YrxBcajH>xvwTfb`RVS!| zbfL2gF}-HkHk-Cx)@xb#{e`(U93zt*Xm%y2nheo)C_NNPw#DLYb;(xCYQ2C28|$af zn~9x&mOfrr+|q;cZw;kvk~ukN&(Y@`ix1?dQ$6(!bU;%>vN!b|C{ZCeI87-XhF=Ko zPIwDshZlC%WzaK7${@IiF%eI}JkuJ)q^|lET45qJ&5+ARk%v`dHa%R-Pta%O%PZeU zr?1&p>e+i_u5(XQ&>iq5wd7Ru{N?MWFQ08*m(=RLs$xV3$9_YV%_Tbj`j*S| zzh>J;XuHWom6qqO+qUcasc1xT28^snkMp|welQzk3}Wsu7I+2z^6ylQp=0?noHgAJ z=p3Mm^g*UUAEb&g;@npcR(Kuq>v|zgxI}ZeRVRo2w`ak!?cb*{e*cXhBscX(*_AJM zG%R~Rxv92?lhSzmgQ!S0SsSp zL3-leTn$8!M>41k28~l|SePxVa8!pvl4tDH^ZwB^O*ZTec=V9pSNWR9o5$}EYw-pY z!;DmZ?(zDNd3rgK#Ibjzf>}E3iAhO4KU;3RFX0b)f21hUfH&y=vD^KQxIgT@Us0?; z*#8wbJo9v(Hl5>=#+<`901by`&K?BmEBH$DR@G#X#=LD~DNEdK?wZZ0q=iuZx^;do3mr*Z-S(yCuwa*H2Q97wRgX zifp10Y!9(bjWYxrZ>Zd5J^PYFc`K6Cf{LPrbZ7@xYsk*>`E+FLxm=%n`LW>~C~;Px z2ia?F6f@c;_(^YX=)do{E&6P_xr_^kiRWLcmlx4M?lckaV|9djw*BS$_8Th9$~9+D zENDP-Vg|Tva5|S*@Y7U#D(pG?mrW$uDwV99rU!Pg)-1qj$qn_PNV+Y01!4P*a7az{ z+{{Z)V9=1XQdYD+y0hg-^)wC`dd#168y^NBYx%cv1|TE=LBJ9 zp@Q+VbV#-%zWK7+7gxznJZlGnfbS&le>L@=P19%Po|W$-bOz6_HJoA>bN|xq(xTo- z;Jcv*+~&=YzF{d0G5m(wH}=VspUmSA`9hVS%dE-ggWtM`F~?`w4aiKY@;BbMOB1j_ zJEaI}=Mj#o@2k@OidckYzn!}@-b&Gj zJC*;$E=8-ywRs9(wU_fx`)M7k5Ic?>H?zC2yNTzx@tImx1FOtWTf$Tz!HdN8Zruv7 zR;0j01C^hu%avdFElpQhCJ4RyMSTX1NJ)vZmysvD{tTp^Y83Nfn_~<4~7O-tXq5(TiNr{C~vj> zcr34Nm;XWt4&p&9-IsV#uN7{ zsmxKXkyu~lW~-U|8W7gATwLaos&_gn#RS;ta^<~VpQU;XFWco;eSyjy9%FA@^J~gi z`@ZYe{YH#=TwisE18PXQ->J|WQjTrNryHj?90JP`jn~vjXqI{L7M$V~^j7YIi#Wix zVlyTVcy5-m(h_E~+JhQZfDVrzb8X1yaVmKWiE_UuoPjnDodQ;`BD~XF5$Azek#UE`kGG%nj@k_<9Wp&X5z&H7 zHUoMjQEaH2o*R#7I+KH{7LB*nuely8rup@h#4q=f z#NGUYZX^;`O9J1Ub@&iEX=i4Sqr(1`y$Nyem+XIuXAF|&volf}>c++l9N&iSMT^-; ze;L1Z{`yXII%?`cEf|l+TVnVg)af#WFwcCrv04{CbFLrP-CcZ!BxZ(ctONR?;(UZNth4yREwL>D`@ z$T}ns9#>3}Q{G$iaa1FHJcyaA`Z;FwsK$PzgK=t=qW;U$sdI(=?Ee0~Q|AzA!Qagt z2toC1DRyr5S&03x2ehbWXq6$obpfHe)8fTI ztm~|AD8`~KS@quTDPmUkhxEb8+8NiZ37U+%;8ynkEBZfMN|bmazU{= znS#%lFic4sI}pVwlTqyV$+fw+7&~WTg+wHxhSa*|uxqov`)nl?3e~yGdG;Tt?uFA2 zX}PXMd!o{O>Rx&mvB5XM3j&MY-em_b2A{QRQWP*knQ_&bvYi@L&0i_vOy@C)ZJoY! zduKz>KwP_#;v%=HQIBhS>+reLQ|FH4*0sBQ5%nYFVS8;jKG5CJx&1QffsU<*r~1x3 zWvL!4(}-cqb-=y-quYdszkJa}18y9K6XIurrh=W5dgZZ}Qj&T2&y8f-j(WmfO=c!8f8K=F>p~>zd1OL-jmM$ekl70l zDmZhMJJAwf6OD%1{F2v%mbb!d?=DVev+Ih*$xPE^d#)+hoNLaruXRi|0oX~vj(k%t z2Vmv;Kup0-9|IeHu+xX(SSp;i+>1lA@o#dbuQWT?TbikyeNynyar=TC3XRD*wPsA) z9B2Hm#`S+cW_Ha<5OFNOX3Nw63=xxm6ME<*z!#;c^h(4$8YyZH9t8U7>FNls(~YXL350+C4Nk2pnEtvv4c zpXzk}#wq_l8ULIV?d*~^uq{-rNO~L@DO8i?OkSs`O zZ>zx{K?NzQ5Hu!@u@A`u1P?sR+y69bjbWFYG|aA-w-UTbR0VQ6ZUhgR#9pAC(mh-= zg?>eO2AoR?jb+s@)@;QRmKBSA&!U4tVlioVG@@GJNE9FV2K=G`9Or~Rs5A0*=`(1P zYeFk6o`GW^@nl@u9FAxa92|lVaEwN&2fGg+puGiHpO_pXNufWW8@J=Q-5q{Zl|#nL zqF0l86z4hxo1a4gWrxp4b~ba&z*hSibT82Y=3J;4RMiVQ$6iHukGQeHB8*-A8by4)xZnw;Hq)02X_0jwnwtLlpH{f(l z8g-Ty^o-%~VIM5VUDEBd9dCcE0I>SO=SGi`l_aS zqGg~f`{6o$+>2_1;f2%sSLR@$;e25!$F@j! zi#92E&ZnPik76c~!vE#$aNh0GvDHEG6zYfZ|qHx z7SkhjF}L(0=l?XPFUjfNpX^Gdx{}H6`ugsqm1>Aa8yci8Y&Awy6%kf^=}kqfWYUVD zUFa3S?zNy5NP4u&JIKu_VTPcql(Hb~tS!i-a+TU5`D~-gt&k8*6RVIibNLKP&B9EP zmg?e-ttJ#0q+Qink!;koOepEdq@sD8W9K5r9P0nJOsNkkZrCWlBb)twHY?4g8yjh# z_c**Qzn7lj6%1>NH6|Uze>^?8-N5Vyy*ZZ6L%~z?)7<_9=$melMKhT1S zCL>s3ieHJR4URSRcCcttq*UJZS3f2Ae z1Zl%7T=QTIj2EXdeQ*`iB`;yzP}g}mFlN4;j+daVj2Fa>g>D zSfW-zl@1tgq`g+OkG_5ROT^W7i7Ce*!RjgV^Kas6m&_`@0JikG7bD)lg5q^6I4NMB zx4f5aTYiRUi$;1<{&W9J@}E0WAg$xr5B2sH?9AnRXzlJ=y>_3`6_mUHwN0I(gix6RkY-x=ddWi2vu_Y~jKg@sIk`%um;lFLUh~JOe z_j4(S-@mqhb2)+EM;+9kN$D`VlSd?pvr&Jl-ndZIGFgOtVbWpvO~T<|C_?&A8}>we z3}@)LrCa#PF}8HT@5lHX>^&3CIq4S1H*hQ(j9@%v^LyO&xZMxVsqy;>{@doT`2D1P zo%2Ebev0?AW%(kV*_G!+O`pqEu{AiWTYYP5>+ftmAzAOD-YXGMn+ zPuR=i18%?TjJT!yOl7y*tw}#|$~_vkGI-tdSPepl>V>}WD|r|Bzr@^El_;SAZB#Y4 zYq!VrY0$(Dx&qRVH9YB>cY7m7AnfjeIxImJ{fJ#8ALLnQTw<+lqZjL7Eo5uR)8|4? zGGd2g-pIQ`Ui?zEIydyCi1Z`VuWs^cVU$|LRTSDG^+`89D*eKkTcvZbqxcWBzi``X zesBUq`1~%v;endto=2jUs)fAsZWq*s9PVya+G(T4}xE)-*%+s)hQYm$3-;d9GzV{AA?kLIy7AD8GAL5)7X7b;UR;!8idZsUG#{(EgHP1oSAsp@El_-Ga zTq4GqbzKLHL-c7{U>w&8t%`41cNoX24q%+(LO*BA{QYe{&>JVD@Oeq%tS6uYcQx;!9hc^A5Kx^=|&F2 zjZvKO5_^5WimESYm`t6F>LwqT6*Ic@D`AobhN$|LBu|k!IW>LL6>%^g8A7;1V{>`b{BJKCoZ*A3ki<@OA%k2{CYp5WVj2KWa>kx?Q|A(;x#)6zJv=_c*93xv>+o^}5G z)&=b^&2v60L=OZf51ilDG=G-u>dQfoJPP|$(s%j8mTPb;q|YZJa1~mOJ)-Bsr?S6JeTDOB4yfTGR>kBd|ZIUxYQYiLeC3 zPJ~MqZSdR0W$@RC9q^x*NkYmLSpmN%>`Kb15;U?M5=!}J`3U^SSNsT@Kor;36c zaVidQx=M$?Row%ByV?%_0re34$JCSXpH|PnCrLF(dJ39$LK-R&A(Et*@@xr(4idFo z1C}BhmZamA2-j=|tcgms&wwo=Q{8XCR*|f(H((p^^9?uz@K6H|4T7DbuZlGAt{^x} zW{PhNe7GnVPZ@AhKpvQlOBm@&DIDSsokj-9DK*e+H1HOb*g6BY!n#)&uuZtd6a%&g zWR@YqBl;QmP?0TC4cGxlh!J9;m@TG>xnhB66fI)9=q_qdbD9C2D{Pc#67vw_-_e{8 zn(3ff2+DKu7=o~wViv5$&*e<6!5jkA3@_ zp%6J~Ls~t=Fwr1}h|x%OHo`o(YejC`0gVDQ1JGjRzYQsP42ou4r{TT@^pqbD@!W#y!d50glqGhPDZAhfX}K-~YXa{Cd^3x)^CJhJ3nZkaiX$pN-HcN!bF3wAKQO zX~@w$NI)9W={#ciIVjI%4@jychxy;Be z$qw_+DyNx7sV+o0`}Nv`wd~hy_j8s(k8`BofOu3}n<0B4bWHZrMUW*1Y=`nr7TyL6 z)9pM^H->V2%3J0+xM!mMC??I_q%jh1NONrPjBshinnHNwzt*MYh$p z4Yr$YTW$ZcXW9$xz3dJ4$@VSw`|MBH|7L&H{*L{K{kZ+N5PL{;h&!Z9NNGrO$l{Q@ zLmmlvF65U`YiMq$H?&Xaz|hg5Q$m|VmxQhl{Zr^G4woa*F~l+6G2JoWvD~rVaf4%v z<0Hqnj$fU2=K$vl=T**)&O4kBI(ItvIS)F&adx;Iu6S3jtK3!N8tR(hn&DdHTIbs6 z+Uk13^@3}k>wVXku2W&-!tM!sJZx9k{;)$~--Y)MZwucR5f(8uq9tNk#JY&%c zcQL=khQ!9kj*PuL_PW?jvD;!Fi+w)!mDqP;kHmf*mloG0t}L!P?)2cgGjT_lX}8e_{NL_>1E&jlVMf&+%L0?@NeH@FWx^R3+?A*q@l3 zctK)Q;>C%VCSH?xQ{vr;-zJSsnwr#-v@B^|(sfCC~G-YJUMJacte305JwIOw6>ZH^;sf$uCOMNc& z<7^<-LjT2fkeT933|Y4vHt(|nfY?&TbUncp2!-Mbwk#ctoyQ_$$B~K-K-$$cmHNbc9UKj&HUBJz^+>hnhA zP0X8_*Oqr_-uk>7^0wsNm-l4ei+S(l9nJeG-J><^B>Ots7r1a zZKG*e?t_Qjv?)r7tpSx+@!n-AP+th7awp@_tcLS<+r|Y00{hKbG8Ba(l`4lE+GRl>DP)U&%Wq zhf2OEIZ^U!siicmG`=*wG{3aCv{z|u>5$SfrISi$md-C-TDrP)OXEw#D!r9em7^*z ztXxp}hsr-y-cosI<+GK4uY9HQ&7QJnkDjZ09_V?xS5B|)y~=v^=~dtB{9a>vUDRuK zuZ6uX>-9jd_j-NW>v&aS)p=Djt5#P1q3SPHw^ePcI^O%z-rx4|_IajnLEp`Ne?2en zydLNEKdzS5sE=RL$<%9<>{5Z?AozPSpLS?w9&L^&9FB zG}JfT*kAR(qW?Do1`e1!VAX*82J9a2%fRA+69;Y?_|HLVQ0<_#gZ?%+WAK>43kPo< z{QZ#ghpZX$!TDk5pLhP^^B+3@)X=^|?-^DxZ2qw2!`2VGVc3>o_YZq&*zRGk5Bp_! z)Nt?cQNuS3e`fgSBcez288Lgr<`EB%_+n)C$YCQd8F|CV`$xV#^23qekIES}a@6us z8%Av%_28%{M(rH6XVkt?2S$B6>Zeh@(T>s4qmxHxjxHG8Z}i2ZZyEjA=(k3n9MgTw z#4%Tmd1TCoVvxI4yeANTgSj`8v1 z>G2K^8gMk4=Mg(!E%uYRz9d%RxoLn1!+5TzSRq%SN4Z|!DsPt$%eUlF`IS7OQdD)RrR4}(Og=D7Okad*;>9J2;jVD^ zagTCe=$`Gq*nNrna`zwIe{%oDz1jVX$LWdm#CeiEZcnBs*Hh#v@$~Z4dFFeTdam@` z?zzje&2z8k0nekJzj~hWJeLuZ5ucHnk(*JNF)34J+A^Mi-SRW}r92@|sRC7G=HywmQ|(djYC^LiCsD{rmX@n^ zH*yk!mMz+y>h`#^-39JKce%TlyT(1iJ;~kdUgBQm-r&BTbF#%_M^2(WiJnxC$H+-f z&Plsx6>@T?XDf1YpXU+e52-p1 z6Qut-s`RTbv*RP5#5gGE=ljTa(09Q1x^JIvukR(_KRQMS1^zDhapsY93Oq8+PjaOE z$hISMj*L2DIlK@5?MLhg4LgF_3Ly?3XMe-t4TqN=zW4CLL&p!jA;h5<;I_d%bm-1Q zs}4;S;=^z|&l-q5pg$rFIW>=#35%)I8eotO+InpRaigDhwYC9cmQC7L?Li$A>}&UE z+qH+aXS5yKPHh)PHoLVw+N;_(+IJS4CDIaa@mOf=Byfc^mUk@&Ek`V$1#{40%ezc< z!16w*-nV?vnZmLkSi>3gd-battu9g<)O3uq4ycXlk7|ips-~&uP%GxC@v2Epz!+?| z`kVT@nymheXZ18ZJLaHPHHiKgGmpX;ULPwi!n6D~aj)1e9u?1v7sW>y-F+%P6W^)j zYKFR0y`tLHY4M|slF>3rX5cx$m#mfza=e@^7FRL+XlG>)8QunH7)NSfU^{cu`MPmkJuWC^@YawcrdQtUL)v80it^T4K zwNQ1Qicx=1gVaDZSWOWzXe}I=eTc#=M2g4|eNmrEG3K8n#)$D^g2=;+#Pt}TUV-Q8 z4eEKZMZAu7A>(9`%#^vZyBvX$>tK1lOqPq}6>_Ou zF8?Ces9l)1*e#!vyW|V<1GGE`MZ9#Nt@=qMNT*1bF(OMQh-{fCJeVoVmnot^rip4< zB8p{}=#BZDI$4f+ooq2k_7MYQl^7z=6N6=6F+$dfVX{U{ljB6A93#fcfnus0D<;bR zVzeABX2=W0EO`-Th33jBqD4*<&9YIn%IRW(oF$gXW^swc6N+q+^TcYoSZtEliOc0O zakKoR*eY)k_sK2d0eOeGU*0YrllO?Hs17O%>`iFf5|;%WJy*e#zDf0S#*-STE}t-M@(BR@tR z&CqgCPcads<>9$Pp}o)5tm-$kl@86WeClKM31*7EQpeO6>WDh5zE)qVPu1rlQ^t#~ z(k;5lbkPNKkF~N))W}j%B6Gy~vY!|!>&0+cD=w5n#e9ipBso*elT*b)v=GbX#o|)g zCRWLXVwr3ecgh>ZUGgTe8S`7W%In2L@-Fd++$x@t4~b{x!{TvyuXsYmTwN zu}3~D{w|*s2j!dMeawS>BHt5-qSHzo`KRzZ8i?8G%@wGf6zLlS-5o(kgt46AE>SEQZ=A$28 zudY;U)jD;Bx?F8hcc`hD&3jngr|wq|slTcR)syNG^{9FRJ@tO|2Ku>Y)tl(+{(-g( z*%S_W5c80#u3^Y%`CxNndkeI%LWEXNsQ1VTwbi3Nvb=UA{OG~e6FlOZ^M?)bh`mFH z)Op0#k;BgSh}D?f_J~QNM-KL&rxiRuOq?;V37y%Xzeuzm(P&9xMVyGojOMv$BK`xK z@c)1&>_4D!{RcG8|A5BvAJByU2Q(q)pb6*F(M%n7)}RHUq(z-7py|$3iH(iT?b17~ zaY2*pHf`Fx`O?FFCoBf?F3+J_or&`;lw}^)q%x;+>wl7#vR4ndVuyE=Ev3bG5`3uC2%bMC+(NBmw2tMLw-fNYq{Oqu6(_p95b*Z~vzsOVaSHwRpJETw1GKf-|vM8&vDZ2_$p_tDH8sh@!2P1-h z$f2LVL>@8QkNNvJJykeXS9${H;%h@t z>{O1epl_Rs0PScM#x$dW$JzsYf8NH6E^4NlrDj75&1#`qq!z0cYNcAGE>o-38g;F@ zPF=75r0&Lwf=dh2!ZFs4#ArKOi_v1WI4xdF&=R#IXf;Jk)zUOKuY(|E)u^VTaGKN% ztYOSmbFpSJPt6lnY5^hD5_PEv!KmgM5w8A>5mz+&pie~%`lN3~qB^cliVXELR#)X!98)KQt)$gb6BbB!?aQj z2;yqFXSrt-OqDf9*d3EW@ z2D2d%<#@Uz?LkA*FNpvjU1wtq?mEV={p?N>#UuZuRh=_Q6%Hp=BY4LnL}F%@N{4)b zo06U>+# zrpXZ}%)BeC;>Q24YMgH2sfJZ%gA<;En4cZCNYG9=Dto<6mApczQ|R_mE)r#bdBOiG zRV8eAwrK#H@yM0rI!rqKR@k`?M{*q|d!Uwk74%sEJE6J}TyLmNCtLXeRQ@o3z0nY6 zVZO8m`z_iJY8%3tFU^Kf4hDfHmP?3IL2f{ELVd4(R40j!)>ifyL~GXsD^Y5I znvIp%nP~T#A$=Fjzw}Z40p%;N>aAeUDo+)wDpe0C7jqiDR2`rk)kF1EwScl!p{i6h zfU+=qQ=zH>WvXtfT=fH#p}MLvbslt)j@G6?m8!mw{!69jO)t$I;zHavp&fi0PYC}X z`z|?5PEaxOLHQKY%YYmi6$U_@)Kb|w6KugjEn&~47FuI;9O7V0)LzHn(RL(t71@}ai z+!OWUo~R%9M0IQz^=xf}*xE+1wOxQVJ6~NPQvWM%J^1<^($M}QLddBJJhyQfw!rD1$t{JX9a@>hy5I9z?- z(G}zAYPj=t=le(;Kz%;o`vLCnaL07VG@$dH1YInyU&{{&=X)ED==aNT&>jysm4G(G zaF}5o!|nuKeWw8Tkfn?phKWW#BVdT(v>Ta^{LlgGn=vwRhNE;qRN4SZ| z_y0{dN_79d^tA6G3)zb9iZK&4%y133=EEhz)xg!t%Y5(1w=u8!A3E?%!<=H&|KwCp zr2pS^QD}$0JKM=mP^Wb}BU}5o4zq&Y&vYsoy8CZ9r|8*I z4G?Z|P`EKq?!_~FvFwF;<2u0cz-3{!wh(je-Jp;EpXe~&OjI`^4-df|MqBr|I8W`x z^&Q+lCrTw|d{i#Rbu%z%#}gghlf%k`{`)J|DPF)6>a(IkyePV%O)tghwVWL4xEJk3 zcky@3-j2l#bs1(1HXuC*#z;;)ncjmLUBYAL7tSe05=-BB% zge}4h@gk9o8RXc)(GO{HsDtRzXEelFJiP=C2~-fnQ$p+*K#0# z3TA{88?Ny8^x518DMWKol-Lw?637CP`%8-{K zNR#5eh8d{+SRc6oWquHPxB~V)RTNvEMt@K7X?&;32ma#teSvLOm~`aP+US)R$sS$O~I^f>#mxDlcZ^y{ZJxZn$XJ zs1tKEBaq$*^s7$Lg&A~LfyRcqG9KgDX4vCzcxJg8FJ$>^7ju9POtAki!Y$sR0UH{6qq#81x`>7Ko$%RM$5K+07Od~tq#Y|MFUnBqkWQ@jh2gnss*J#@ zSfr%&%NVS^#fk^9suYJ5b78YS%p1ka1gz%eK`UR#Wb9^6m1)u~(4{40fVqAIRxvHL-CY49IKQg zQD#Nx+lsNzZj2l&$KmPl0y#lmh!xn2uxf~1?dt`R%1r^)HE3H5uXoGE9? z*;v7ugBhG=IS(s`t#ZD+ShmRp*rUA=ZNqg~M|=p+nM>s|%ng)?uTgqs@)E3tUW&aJ zE9ENm$K`UhT!Xc%wc=NKxm<^}()IF6c@@@-D&*Dj8o2>G8?KetVLhx8D@T7qt$rKU zGYhMK2e6`gqr6GpEN{UY@vXc{yajtX?vQuNyRhol6YbSjtQ+5hb+anWL41Xk%4g+$ zc;bBk&*l$dMXe83(mul)?qi}3^GuJ+zv4OgN%@p~8mnt_<+E6CeopR`&!cr}#tPyb ztl9n@tGEBa>ggWrirj)0s$Mi;^;cjW^(FZ-*0lO#41=F~Ae_KjGq&#Pk{F=%ET^^WJ(#Za1E#|Y?*`E~KJ7_O{hL|~R3d*)p# zOogNET7tRqkz%QeLceqg=7eK0QxmTe&|@!GNh(>Th-E5Or71VhqMR|SA#PV)#HE;9 z>8iSkN3j0?4W2{CU@oQ*b2LSmyP?)_Ean%=#HVOuf5RM2CFX8=p?$qljKdsHAJrG@ z^8Hk`szEz9UaU}cnC)4Gxu4506SNXDL<6y6KUfV>=c}RE0XiJ3gd?#|I2!B!W5oqx z0#*yhV|T&?aUtGSIHoQX&tQ$>BJ2&AtfpYD=_2$1N3nxpI?b?~Gx(TqQuDA+fL0kV zR&8nlb|dI>`n2C+8TJ<}#~y{?`Tez6fuwavT9Ld;>_WS8tJo}VK@Z?x`%A>iPBN{{ zV2{Do>Kg2#_@lWha06ycHewgTjo71bv$_R)6K=&WhTE{Fc{^4$?^JiGyYbG#Hgykn zGi>Me%?Gf$`4HARX?^oC%(wr5Ikc1NajbeifmP3^=uHOoEauL3U~k4wtPw@vJb&!S6u-e+A~{Y?#M84?CH5VaLV`*t4-)L}GRHQSlg7bY4_1VP4>6?3Z{2;{X@t z7p7njL_e&hzNYr6*Ri7dPwdM5hj?84RXl+)l=J=7B$%yw#+WBuddgxHRZ;&$hdlqYbjleifili`)a;NyjtVl zh*w={+*dQbx5(gA?Y}a=B5#q`UOlgIT3c(2y}ET~YfIByM|IommYI#y7PdFptLybE zi&j+WterNyZQ8x&He zia38o#SMD;#kFO&dd|MBJ~*;LU1MmY*2q<@A$_f(japqB-l9@ReJ62>>uU@J)c6%p zZm%~os>^4vEH1Uz>lxSOuBo;4XGxs>1DVxx==D1KpOK2U$TDDRW1Ak;TkRasIZkoC zw^R)rWE*IfFiNh0xp|8W0loe_vY^HFwTwr}RNDs{0uR&$v<+;Z-8{X?Nd{0}l>xQa z+lK|)NtqG5jMFMYO>+)g(A>CSR#43-H8_{*#_jdi+D7UM8)+)6sJP6?MX@1unURZP ze+u=sQ9(@0jm(#qJ4c;S$lhW@QRN1!a)U*=f$ymcQe4?!84dHajSdpSPg|j9$Xjfv zsocm!dA)5+a2b_Q85Ma^Pq^OH`>*BJF#%oI8wocU0@eF~#= zaW|COE?^QJ-C&ePiJ`iNO4|fx@0f5#9q;M2HJS=R`E&h615=?hsxgeXh7|=o*9X|W zk#4n7KdSvLwh^zoryjqW>jwAghHd~r#wr5QQY zB`IM|!L|%pOZ;tXiLFJi8!e6WQgd7D{8>%5mLPVey6%hh))}p|ztz?`dMmgZfV00T z>!~%N0=5P#%BU%3t8EBTXIMa;!LrU^soNJ?Yopjp{1#(aRBeN`HPC|88Cj|Kb2F&w z47;l@4r@KT1%d6h2Afiu5xb1jLTg>)Y(1;BHaM5+cJKAp*{~N3!yQ|jsW7zGMlOmC zsmqL9_-jvDgKa@j)hjnLUtZx{a7Jl+iw#AU8?4F=A)c>k-w?L&Y#mee4AyZ4 zVi~RULT;rOnwDQ&U(wUHShvK*0X2j!He2b%dMmv&5U5*rrBTccWwxckt+Y`JC5Flx zdfJvTd&jafs(sHATVo+@z;iY-fdR9Q7E9_DOh1qBBLqy-1##^^M3GNb3< z8wYnE3`PWc625mfa#1pDo2jE_22_almj?KZj|^&y8A1A`2AOXn0M}DS%S@0ivoU%rs^KbKR8*t4 z-$nW;#9LHh$XsE_Tw%yuVaQxz$XwAg6x!uMJVr_-|{25Zv`X=ko;>cZycrgnxM+(a`NKDBY$ z+`w)888N9}#@_6dbEBC92?HVO95ZImT!_>#RM+Lx+eXx|BIm57=pS2jIhtCC#DM3S zaQOVjHuTR;GjtIn__mF0h{O+{iaw($5X`rM;7Gck+th9nL>Y(xN%YKxv!Q``tpTDK z6Lkg|winL?tsx7OF)Bc2ThKJEwPiZzw79q=v}r**1}N=KeoVQ&QO}63i<+{qX|2tz zE%Q+L?Tu|qEz?_DW?JeOwzV4JMir>8voz0cYc!BXLD%pY0S~Cmrw$hd;d=(b zB?gSP5ADBBPq3e!U_U*tOsv>5Q7puF)mxLZu3Q6|1*ry?t1mdBH(rPd-d@%)LcnxMzsIVffb$6 zg5NU{;re?fn1{s=`?K^Nl}c!Ho7!4L?!3mfxxzhf?!38}H>O>j`g;(xCWW1qzz{!M zex_xKI;HZlb`g#_Tsmim_9W9zX??FW_D<$a%9}(n^2W16Y(9y{PfdG>U&R|(QkG+G zHwWuXZkdR+Di>C>1a^u2fSLNwu>$k~R%7>zy_o6UiCNc2#r>Gm-GbTG8!#KXUaZD` z&NFxUO7Wz&0kZ7W*1*3^eFFbAtr-3*+H&|eYWv|or_t`{ZQ4NiTeT?oPifWg7hxxv z6brRl_!G4m@E>9SZtPa2SfqjH)zk2AXU;F-G#x4SVy+sT8S|XF7VzCzC6(eq#^0uP z0bZoGz+b5Dg+EbU4gV!|1^lp{w>z; zMDA5^YvESFErDx;Ylg#meC`yu32>v~uwt5v^~Bs7xZZGBb}Wo`i+RwHwe#!!1WSpX+=je_4~x?#G|@ z_`5Lcf98(tQmmWiV^x4`=Mmpk@c-(g_a@f(E{5NTw`&Q%4E{vlLijJT|D5jv!1wv+ zEr@oeoZ(9b{D^M}{E5CvNJ(VB1-B3GCAb&ho`8E8Zadr{I0~zS>kEgKW8Bj|<=;CM z$_ne}o$^?=k^Fh~c-qkp+x{2P%YVTxRPr0yH3;`0ma*d< zyNw^GaIB)h8nEk!Xx{{^lDB5_o_K`u+c+vo{)Bk?Nm9HboxKjSu7XYDMJeJoi?yG1wda(uW-Q0sDNfJlLU@jN@CLExfC=q4p}i(V zxZO+#+csjr!VL&^Za}bh0}60~^#|c}E?Z0->Vy$`g9!!VZZL5qsUCN|i9`J{;x0F# zMJ5!;+k6u@$AqQ>nk4Y^j5nc?fQB%x0Z>1NdYMqE2~lXF8LC71W@x4fr2>kFizHh5 zc_KX~&g1ZhGR|^FDCp8V{J8X=3DS9VhSE=f?pXR~@DG{L`zG|32@!4|(*;6b>J<6{ zhwdrLot6Iy3Nt4wH_3AF=ift!^+75+rh zMEY108g4=X9MKJ8x;itouL)HG^1^jX&xP-SOHL!E}TVVuGSn%~?f;eQAB zrTY{32Tk0&CiI301#kiARWo#t37NQbN@EwN@r)VzxCuRILid=^9e_5uH^RTx#9akw zE#=(3!i1KXP#d6T4kc&?LsM`j#Aw_Ng`2>bq3!{GoZAd#sD|hm>TNEvY8NtQlM}HN;^eR+7H2@Y2PsJbKs8<#B?7p?tq^zZGYO{ zwB2bt)1FFu)P(Lgp{*tqz-{S-yP4zOVB$8I(0UUJ#9eLTmY)M6S#*Aj%+UEJbS_E% zeT+F~-03DX$%Mw6&`1*+VnTt`8i4D^x$I>^r6ygW3FVtmrU|7&_wlUpNI(uaOKOJ@ zsXsGrFQ5|yr5=O(jG;p&^u7sE=vy4RuM-aK6YgK=gxkTmCxCmHq3tGgmkDh)p&Lzz zVqDKLuI_|e$GBCUaLY`b4z(k+1#VVq0GiqfH_;3oYeK_KXpjljG1ND;GS!>f4REdr zc}ytTgaWt#6l;bOmvH71V&aq^O8G6dGUX(NrhI2Yg!__lpP0CVCiJceyMxO&hj1;tK@CTToH73;Cgvw2*2($$p18tQ7p%F5mL=z%9Zn;uifNUv< zl6=a9elQ`lP(~%iZ#JPDOlX4%tw)U2Ec0>`x5$L%o6sE4O;4T#f4m8egicXsa1SVX2ty4f)X#({ zw3ivGL#1YDp$V-|&QD&AyUgUyQ0h5w@g^>kAh>)^Av4(l*pk$N17UwQp%W%_%!GhT z`i$ugnYi~&2)LxTICP(hLtABVe!+xxn9vg@^spZySqv`Q{kWvNOlY$S-DpDBo6yxJ zw9bT9nb0yJGQXr8C$+((hWmvJ8W z$uveviiHbjD8z)6A4>eqgie~!cP4~9=y4N?lg@=Wn^59G;+*)dAD8%s3BBrvNDmkd z5ZxX>j-_Jx5_kD=iO+xv@__Df6ME2u?lGY|K(UGG2-?WdwScZ-Xl>$(#3i_EOKdhF z!p&eDK~qdfrz6}1!ojUz9&L$)8+``dP}~pTaDr+W>J6xzpyUQWlvo6)fJ3uQ9MQSW z7>Nu;cM5fxp*Du#>6{ZzA?Irem+(WvH>oWNpC=qi_`rk?_@UHxKa{ZFg!Y;ca5(po z;_ftYPnpo8CUn0EZ8f1SCUmn2K^BAG4JK}bA4*gjA}@r19kS zaI0DO@t}!h$YDaDF>nZt@1W55pF81BFz#3<+-HnC1lso*ddr0Nnb1on^nwXdj2#@~ ziB7nO8MnO??k*FjL!0AojK3ZfSI7Szw9X7&WkSmsYVQ=c#SERr(A4;e?$PmM0S`B! zK_*mZLVZoB(uBMw1TOL2m`kpS^Z22}YvU&-UPYk^>-`Yn5>_*AxgVFX$b{zmq4;D! zm-tu{3OAt;6H! z;kanw4Cp}@IxG96UW%-$HiT1LRXp4S`$JG7Po|Bw3$$|3C%DeqMO2W69A2l zLz@;iz=Uc{2rZWpS`Mg)=?Y9-wh6gSDA9zXO$e=zDHHm^gud}Z zaU?0njs4t@i#=jOADGYq6WWhfbW`kJ99p^C#O*Ypr%dQkP~1=2h}~*JTTJL?6S~2K zHki<;u((lzZj>E-=v04F zPmAcLuaCVqwZsWJ^M6|e*!z; z3I8zrH2?i67y~1}M=Ym$6@G$-^#j0|YsGtl9I9}31~7;#otg!{U44yMAFIDIp6E4o z1b#HF9DoM%=AcK4@Ue0rBe7$i;9c;Q77gE~6_Kw_zz_2^F}#C)Jr=PT1#*?pTt{ElwyH46@D70 zn?~y{GEM8t7?L4P{mlMz@RM1dA4wjW#(7C&N#fN3U@}>rG|pEVr&vI;K@Bv2ze1j- zSd^mJ&8rT(iG$dPxoIh0;Pr;xyaKVCS0L^Ym^;T>73r|IJVli1O88%KtglFi;tP)T z9ihk z3u#qaSgYMwt3A0)yRrq^)FT{gF#Gl7<1J4q^YDg+lx6G}k`96YihSl!$o5>KUS#}J z@T+Le4DZ82t9XN!O5{8V7;^gHXK;?XvpgB3ADO{?y0fjOlV*Uy`jG6-<=dCbWe~^8 zV7NQOFLSJy8S^q>!~)j#0>X=nS;Gssl(%S?Q7GO@gHQD04Mp{O0b5`<=Fp9KIyiL~ zV_eL0CCMq)k%i+_MJmT-Y!7o8KbPu`n9JomlP$KER|@B`42$v3ABCa@iD_K!^Vxpp za=Fjt7G^P*{#=%7E~hw8hgln`%x9i<4YZNUD`(5pP=c9ep4tPritBF`(^n~KqwsDH zwbHtp8NT{G>I4vYSS&Vmfn6=bD(xIr@pMC0#Eau7inO2XpZ@ z4(PQ=jzw)+GM7j?-pi3Pn`KC*ngX6ANjgi?jqU9hwu)qyDxKx=vL$$#gO^MHeKiC2 zqnCRjr|xB*Hs)E#JbN)W^rzro$b9xwd#DX#{84&UHTL*Tebk!THVS{N}UG=W|ZGvsC$d9yJ^I7iih=yQ$CMXK{|Y8uZjQ zmXgeP%RGcaS%!R;p_VmJOY7FMR+|TZzIFk@OjE0N08Z4Fvp*JooVFEq^(&WMlCPcd z6gN>@1HYc*CUPu#e^tsJ4DVt3k2#f(iBd%}<;P6(G1ClTcnHJSTc|!Zak@>k29I~e zNRlQC@odsak|tWYhfj5RCR0u$+2kNjYbNLG22N!rbDqf@8kuGQ(=;(n6HC>^d2ixe zTu-soWn7v~6c-0aQSZ>i913aG8Lbed+k@7d0aLr-(Jh4e6yl9j;5pqwj+MmpNt{+P z$4cU~TpTOO5)b+$j@w{CPbwQ&a$eCzFG+bH%;^s1Tn}b$gPFq+<~Eq)F5y^%Io-h= zYc0c}x=$FfO56i(7*mqZG{+b|#_&p(WThn+q4#qstYqEZ$eg*wkaL-Gs`e|<;M58! zS8|E0WZ717iCm(s0)9Dbdn#*tC70bw*1%;Pccu0=Xb$6?9dMu=sS=H3TgiD@$+^3q z(_KmWL_a~T`bw5|tu8J5%`9Ov>*qoay->%qmYO-md90;oE~#ebe-m?VX8LBP-^}!z z8MB!&Y0NW?@owgj#%=rU9M{36%%@kvLP)pm9IKQmOPJ3{#yrn?ImWRtaz`$nXZjT+ zxj0Pu5}Yr*r%HTQkUrtl-jror;~F)zmSP1-BJLv#K`81SeAHR^s8ixT;*U31iQDOF8?Z=C4jG9F(!rM_T@6_#&8Cg{#>@4zRa^PTTT^QONNG=%D$XRU#6_G zP`k8A`x1U7Oa7~N7vKV}JAGM(zAVFB*6CzYIfUrw=<%dwQ>ma+|0vfkLPaHb4N zUdfb8INf#3?F#0@pYWXI?iJ? z=P{4-SjTDAX&-?8eeD4JE!rXYC$#O%fpT4Ese@n58pyNc6NYnfu_c3PQ10?#N(FBi zquga3OH~IM6rb%#<2m{&St9nJEW#OvOT-F5t8gCpB{*Ykhq#o#Q1&E!p$t2!@P#tG ztB)^~;SKtQcu##6eW46*v*Qb8cpDvGD3gD~8`W|0SG+%+E`OtMlgZQcZ8F$DzD=h1 z+hjNcmEPzL#fct5IpC<`cfkD&cLEOWjr{~V2U#Wr;#{H7qX86~$ zKanXPVSkIn6B^Efpmd*O|33D=Wf|HT<{FDufM_QAeu0ng;1NBIgK^&W`|$5(d0t|8 zFXL}CXfV1$DAC``_}iFr4#T%Hej<-YUt<3?e(KxFG|w~qH2W_}^bI&u;uQSt>^}^A z@XoJO{AE24-iuara%uP!zuE z^?m4vk?xzmV}aNtr~k(?|H5~$LYDWW?)e#qslDdLs`k|Iq*koyOpXJadSl#m*A>4*A}JR^Ilf<@*ff z`i$==Y$539LmT1y3ho1v5r2ax?8eZL53LFPC`SnW5t=xBX6_7%li0@^^aCYJZiF5= zcPxK8K@t66CrSOt-@gV@M@~=s>0rx0`%dBXH2(QZ-3(;Sp;9HE*$3&v~2#L?{hzd(4z?doze|Z5%ha{=}dL@o$o8k6;e4ydxTMce%7rN zeozfS2>J%!Z@$yMUwr5xf`7>Om%iNyHRSaD1GV8uFoh0-+kbG;A!pzkh{ z8u{Msdma>S+y!#+CWmbDT|<(A{FpzmQ#Cvh5{@k0&x-uE&jH{($N=LR!q0XaC2 zKY_=|KQ95?|#02>1BY}7=f6--WVr&`EQ~a&`<&-1Nswa z>KJMcx1r?gHW{2xqg~Ll0gFXT=8qe=2IC+@kR1kSf+61zR1eO|onPNZ z3S?uT`U}UTlGA_ajr>2ZX@(V|-v@MN{!B_gm2vgs&%Ppuej2oJ_~q*~z7FR9d;RZc zy1x8zaScdpj8*io;4;=>-?#o)MoJXdNR=?u&+0kj{QBGHbCd&9`Srxi0;vTcP#HY* z^v>xkfw>m^N%l}&o!C8Z$H_p^B8*Qj3dh+&X~5;+AB8j2dLX12{}`OyRRW4KoGTY6 z`rw};&J+Das;CjQXk+Vf>Xip?8%)P3Kr`{r7PIls!86ht?8M)Oe;%J2lrJ8|zW`?k zJ&!T!i{d4m#r6vRg*cJzHKg`B{zc*){ENkV_!?vSVvcw=PBFU#XPC{!7kTitDSVR$-ya!J?=j(zQxWmEvV~gNLal6}R<=+pTd0*S)Cvo&2W12PVO$2` zTm}(rt&wc4QAT-?wMHAIvK=Q~#c~C6(>h$z?-dD(Q){PpNF&Ts982;&d({50_8|mry2` zkh4<>QK@8csbq7h)ZwP%e)Yl*DqB1T8hKjU1wL-GLLJ=-I9 z;e@@ra7y2`avA1GCg9Y;39>IvFZAM{k24X;MdB2_3HbWP&u}Lgl4z}{4cUe{*L$J= zkNEUN`aWDEpY-=APV0M1z7KinlsZ14&N!cr&Ve(|eN$6$<{O>(HjmGEYscyJ!6&%U zX>2$*4dA4^NN#y(^-v?MDIVs=reWsS8M^wNZm+yl@Q%SzvD#iCt z<;EyV;qG0AB-$9Okn&d4Ns9kC;vd9a2zb1V_<8u2OT6!&oWef^?8NsvD87`B`c8r3 zYw-4|85qS{fjtE555S%V_PA;SRsnkybqe38Ax?<>73Nhw@*TlEjDx87?rl&VK|Ith zK*v#vkQ#Bv<1HXcTfsj7iI3n7tazLUo9a8F#zX!K&=+C-5BN~w2HYQR4cxuHW1uVI zJf<-13!r_I`TUAEry>w*Je~!?&G(B+HaHRo(#vsh{2I9?=sC=2?S$I}_Yxef1yW9b zQH`8K%KLG(5&8`Q_D3}tC#p_Ciqk{{^8YnLPpfv{%b=h$QkUXv)Jt*x>N-$efmXqe zJpV$aqZ)BG-84wuY9B z3~Ba4zDdaGWSq)7#rG<5e?(37eTC515&F3LGfoQMi11s`4^x`>ei5|zGQy7H%WZE# zJH+=Q;)49^{FBsF=wupl+k~>5fi#hhy2XdtH{__^6_)JBUuoi_aVlb=A8*Mu@Ojw7 z0lX3KQx0FK=4k$Sd0H~wl_kEN=;_XYn z+zX9xJKmq&EV51fauXjZZWC=L{vnee<0(IKxoG8Z%8x%@AU~bsA3Q65nCvQUHTfSn zD?A=&{hlzx-|G}U2I*6NrkLTgjBvY_0DaTj(R#Txns_J9derfdS3PgyS^p;eWD`&N z4wUm&6AwLO_sH4&6LGp@Al(Lo-loMZ&S9Ia!6~25V@!cr`1;J;veLe)A_3B#(|qwE zwYb=LysghB4STjp*s}8GRK;fGMcdJdr@j?s>>b{<_J%Ykn5|eXe_U5 zZvxuk6pTZAp`K;tLTHwUDwr+LTV7V0o0Um|o;wp%EA={9mr|IPR+y5~BQ>>0N>WB< zVnSwS%Kua^Hzs#aN$H-f`vn;ZNuG?vgp3jYQEmMd+D|lV>$7HQEj3G#tcPagLwp?? zt)`U-)!M_+31Rn zc6IzU#4ghqIvi@3gBaQz79G>E?c;cPNA&3j!y>~Rq3SZbEh;Q5($Vp?E6PRJ_rjxb zjdtA_Mz+M#aodxk=SJnB&C_t6s=!IKJJ15=V5};tSx}oj&+ArkvEd4BZZ2Be@)Bzn zWG^YFlF}IDLd%txkDi>}P-@BPUzwRzHKa?&_avW`U%8xLmiF&g`jyr8O~0`PSWCNb zM8S|AT5McleR1jF!uV*HTIGtaithL{Le^AQoY%3})^Rwry1%z~GIUvlUa$pcz+x?@ zIzApYoRn4aps_VroVClvM+J&Qa?_l3bz6nv~jgL`zLw>+piqM0`lqnPiVljVhhI zcEr#%Q_4Ni8B4)BL+`-tR3EO#I-c4+9Up~}G_`w0zKcm}6Ca8`hIG$(lTV(;__O#? zyQlMu#JVxHmwGxAO}vZu=_oF<+fZgv)H_ylNulQ=Yon5i@lq)jsw^rddC8#G0a@o= zHr+?%^5d9U#VI{6Jnvt2o7z!1ZAJZ*+a~J;aeC#@%ZK-xJ)%eG>1_^@8FpdK(H3I2 zKo0Vrhc6=28YtR$Z7%0E3U&h-ARC3-)Txh#=&{so8fk{G)FWKS)RTObLDY79gXT)s z%T|6tq?4@2 zQ0OrmV~lfD%b?1tt9H3;<=S_T!zsHlbRg6!M=^9Ae6@pWU{uG^FsgZujzc!R29l*4 z@~BX@gC4A%aPcGC6Lbwc_2W7|8f&=J|Lb_trjCyie=)RqaS%Q12~Qz`9Lo ziBpKlo`~N?+Pwxo4?B;5XFI%H%wqj61YdtV>VL5=gYe+^q1xwX#Sg=J>o&i^fXi3E6QeY0eP6dpGk$ zJ)y2QDo204LIXps*#e_cI%g(gZI^~eLB{5=sHm`xhb&f^!_a$nV5!nTj3`^j$x!Oh zLOS-M78!lRCD^H0NxCwd|Act|V4Zpj9`s<;Q(j>&OWnJ=$RnwPX(OK6c%ZA?wC=zc9lZ@jzH~blCG@Q6ctj^BhD-5t5RuVfOGS zO~u3yT|aj0`k~43ai5`+4UdcpJ022lb!R~*)Mj78b-6!uL1Xdfxi0H?s>?b)O1y3K z8AMNYPRB>$q#M2MABgka_n7!pna8%R^C6qp>0QzjgeRNS@p0lIw847+-iERW!|02i zrJ#f;e$1i1f=VTDCo}9BD_k)$#)VRmABSUFE}X_0C76Ke$W%L`!%nXzy{HxB2jGqu zF{rcPt4#u5?$lH2f^WNsQ7C)#M@G@-P+I&rb?%fec11?I)Fkpx!|-BU5s?rg4D(N6 zrVttOizp`ooUX_>gCz)tNgA?E-)x+izXSRh%H@zQ9y8mLHrBU}kHi`D+%}@m38qh$ zF2v(LLfp!^5{pf}ktyA(>nYq7lO9kCRDhIE%3nAJdYMt3;>6I?CFukYm+@qt^dw7n zq#4_G8SRRQiiXY?o;NMU660`$*<$*4?@^y09UdN8F-1&Uv|$JVh?@6bqw zo=<;aE$P_EccCME%MZI~G-{u9Y{D*R8q`9Z=GIxqI$txe5(6@WHdh0dTf;K(5@L)- zoNXFeu^z{jNo6(~`=7RPndw-(@Cx=PKzpl3t+tL0J4S6bY6H%5+G=3!8qJI0S&CN3 z@hC%WwoaY+BJubHShq&CL&wHFl7l>2eKpvXvxCMe2~$ z7*B4jjEg}Tg}PFU-7!&-VWAFJ0=1v~Y!u^Am+Q5|;j%iqds5QE!ue^4q`;n7>;$Fu zQkNq3F(5<@?ByV&NaMEu-`Iz=oHsq$8Vj?q#ti6Q*pL?!79LeO)$wmE0ld{vwuI@> z7d>e&;vSFiy|{dJd?aR?+Nibg{f2K$?7$3L4EFD!ea*w>F=|*f*l4UBUw|o>MvG2H zYqP15VWAzL^pI|QLcA*~w4+yuyvxy1g9ZdDlF=5YZ0gam(cz4YR6C=cuF#Hgb@Cd= z>4xA|1f@C2H;~eWW@fUSsp3ssx%NpPv>us(JDPk`c3^M-*1p zINs`@cs9+`Ozlh|JlcUA$>UeD---%uOJeP?+!*sgGf-Xw!^5Hu+3n%3=tC-Gg4@QxGBj%Mn2PD>2o)7}8XZ2m{aV#=naz&Z zoosSV*y#ru=N~_#jfMm$g?7@9mW;Va!%8s2ze9f-@=FBE4G2OFuC$|St4U!IF)mxU zrK2V)(iIjF?F1~Jc0{Qi=yz5{L|Lusg3!|s!C;+gO+*2jvA6<-gt-PYIGCCSv3r3Ga(<-E;aLpMW72%4{Pcqcu zjB>dm4K+o_x^r{(M7km(lr^!yt^OXQlwg(3$SBT?ce1rsX_uHG6$$4oU(Oj#D#?T47(NzzW2CL4x0Ov9 zgEvj?VYAK}@=^pI)nsyy^kM0$L7je8x3lob5bNnDLsTESJ$>Eq9)k-|(`d}K4?KI#lSNn=5TrzOrB@>GM*M9ZOh7Y@>zW$P7!vIv1nOk@r zA1U59=M?s74;Xyt84Y+&=Q%Nt(_a`A56^(GWs;+F{EyFyA10f{9j5%>`oq;xwB*mC z^uNY;xyU;7e9$u>$rVUH7GLjKVTRKfLzg?js2h4b>eqF=Q|Qn5I{rJ8ALAi!9Ohj2 znfwCbE+an_{yK*vzdX*?`Eq{Ecr@zJ@nPcYpmLybhmMcO96rtA>T+~5@kw&6_=kz7 zIe((h(-_~WTuI;lc!BV9%Ml1aw>*LH&T?IPR{F8{;+e&iuicb80{b4yOngA!iNI5v zqvw-GQhNSLkM#7b>~PhsRygePFCW2x9<(a)&qG_P$u z;9|;8@7qwb|Az`F@4?)*Cod*1xoe6$CN#`qi?q`aO-A@Dr2pEfV)6*i9cN2ZNx68; z)vTco9=NGge?|3QgDR|G$-jW)2QV9#gB5;#Bx$T5(U9qX7Wg7ZgiZCaxk4O~A*Wxo zhO3!=0sYrbf+k=|DO5ckVhwX*jcK^uAs6~3^IwA{WZgqTZd10v2B}TS46)Bp zB+ZO+ScR2V+MK=FtkdFk!r2DnY_KtK*!T>_a3&r(hXYKq$s7iA2S>8k_N}wQKd`a! zj{e_Q-4nFC2JZK}zdv3{{kprl>eZ{4>y>=MeWHoSh@S#&p9DuwkgvR~*KrNJXf7V- zDv!gVZaaWsH;LqIGmd@x)T_titIt`mxpm<)vcj6_M&Wmg)TK&aoY*;B+k0s92r>V? zcki2d{i%8SQz0KUn7>+hSk{H@YS)G{ht^-a7X2c>PCNP)b~STPI4PX)t5)g}wsRF6 zpF3C2%x}$Pw&ov%e6iqZeJmZZkDCI^CJ<^LfK9MZzuqUO~Wlui=l?dQkw$&;w>b z9uu=xRa13TRm1G|A_*6NSCX1!KEoi!(v zbi%`2UfXa#&W9s;O%@PFhBafu>s*uQ;8+HB_E2QlzE9pax-W_T^9ko1>NXr`f}s4Juc3M{-93*m)k^iU#^H=$F9nLu>yxUKc?r!(kpxr$je&LGeYbxPf*SAN);ZCo( ztpi>3ZICt_xQr}KSjnH%C2+zg(Mj_J@hRHOC1{7q_#{&346cnxasGbXGCmMtO2G7( zo~l*NiP2V}CD#}mNi^oy_IN`BqpcIC4Auxfuh%Cu&Yizu@U*5n-LK~sll48dHT~(D z%yuo9s1v-Z5%Nf}w)%m1TYrl$*gnzII^0n$Hu$4P^Vo)TVXx)$S?rGN!Hqq25r0_I zJc1JIZfxp|Syp4`M0d}I3}}*ch3|qUAt%Vgo8(@^Xj20iBQ0Z2e=u=DtXwOwBbDs( ziHmmSgpet0_8hy<-LvSs0Lk+^%eXSI$Rqu)=|b zOXlV-SzzzsF8u>TyK=c*Lv%$w+Ov!=@OAuMK4J@zP40i;aI$i2cmUaTJQBv?w0>>4 z-}QBlA-f$~i#FT`OxfOCYM;2K4UbZUtn^*tjy7C(G@d=+rWkPvUc=Oi=gA7NpEqI6 zl>7O?5}a}=$qJzU{6~ zUHQI=C_=G6zv14{mG>ivh?VI$A=kT0?H>}!3ZVP4h*P7v6=37HjIS!3A6>@d zZ0Y&1>ke)O*w0hWg#8^3M|*_lJIRLxEuScBjB_vBx`t31w+mc|)$R+K%?B0edD=*}`;xaMq+pTqe)*I{odBVA9p79Q#*c zdk8Hf)+T-!-nq_V%T6{>DJ@8}zYvKAF9?N2{8ojl6zENwdh82~eMZ$$Z%8ey zcMD6Z2CC+L;;Z~2XO-W{snv$ls20{&);$ zGv5nubv5_QkY+23$spod#oRq$7Hk@E7kEd5a8U}4hF|{greA4=9-qp7Er)aj7F%SP ziI)7|vHwzqd%e1*7QWld+DzlvWvXf+D0RJ}nf_yAeiZ{8%oKiU0^jgRtl7)|Q!LhcA4SIBs z>9-2M7FgT@2KiSw25BE8_ysb$X}#NU!XUvV(RTrHJsV2+v!VLwM8h_8mjfl8$o5hP zmtRLCD8Y*wi2Xca+y18My1wF@A1uKm&@73!*w4>8a74q`kUzkN(h9Nr5?Uh30)s@@ ziuGS#q*7Mtrbez*l%n2{tHOXcAQc7$#-rGUCd)6e`=r9@Buo&AG&JSd2NWNo(;;0` zgspCJSmLX^eU7g`IQV**_uYn*{IKDE@I8{SHk@!{!wuI_PH|bt1N5^5uOT>efnI1P zK0$}X24&K`J1*vd9JiVHw8KvUjf9jIk!RwR-Gl6Ta;RenjoJCs^r6wQvsQJi$v1}m z--g=Ty>Xy_6MnMgfLRzLg1A_z8_G1!UbeXQ;^~I|RW05(n1MofZvV*eo_u!C=m?MX z***FidJ69<{A`c9zK)%i%T7;!>cA!8i9@j>RNUu46T%ahwDWg<=z15wdkCW|zLON5 z2Nze6+f(`Oj`F(~cjDbgkdyMBQp*R*cVE|tH=nuU^`(2SE#Eu025&y)Iu+-36npw* z=dLU~?r1ai$nGt{kGkG~opR3eV!YJa(To2?W5BMOz5FhXq4-vD3^M$Xm2dZy-#(At ze#Euxx~SB4s(knI7jX9(R6?6A-R*PkCeh~Wk>M^0Pwl7GXZQCZ=dL6?d!W7q{d5WX z@IC|r@htUHcc8M3>C)5xR)Rl*6U%80+igEwfvH z5XsV9C`VhnDhc_yzI1J>E+mC+JqgWYJq*ZxNln$%)_Y`Rm#S6`kH^?pAz!mi`%k*w zTE>=mg9A?rumK%z@N{_|g|C$G^VFV~fS(K^g%SUEGcr?vyXx{y>>?$$iDnWke;HLdE)tw|c8?y2UgzIEB*efdyR zK=o;c+A=V8(Yh^{Og4^gA2R$k{jIT7IGKxf){Jz3x@?SmUrI2q&0Dbh#RE*O0~btV z%@nyH_r6d}B$?wLg0eD~9227zV<&X7sp&Jv#?G3GkN3AxhL7p@AuMTW$^NnWEV04t zz=ocl4T3&>@!G}9W+Sbcdh}kyJ?9Xf;ZtL^~k@#YQA+I@aeBuH@54VK1 zpy87xEx&hoWM9rw^|06QCF^G|U7WvU-d>0JI=C=)OW9obJ4G%GICz@_f86mQ^e&%A zy)D82?0UOHxdbn^lN30dxEtyYIx8FZ!QGtpj=HWZ@@ z3c=A=l~)b9`P8aIn6*{y3xiFNSJ@3kd9_J+6rtPgTwYmqqy4ejOO_Tdo#FC|9W(;R z7IJ$>NA~7&dq;pd@<;YUYcnvS7I=SWZ7+tC!W)Ug*@P)01t1e`aMMg7Ih=BiON>e+ zCslS_*C_-OIBxh&n3I&Q{Mb*HVqo07dbY9cg?|bUfa3l>eWModvXIi z^Y}lvXOKND%DUkfTfI$gh5o2D9S;0y${1kK=`mCPEkN_(@H{yDqb2x@PVOM_tfdP0 z%WRl!=FgKvC_n!^^1u`hzqSG{2`?RTS|l~2%z{ZOg!RmCquQnbE zgnFuvBiXY(?0SnxwrhQc%TJ)zI}Cp36<^*iAZwgQusqP(3Ca%GcedV&sP8BB=|}rd*gp zEIblXsZ~IkDnPQAmYv^VZ#N1TnV!BiNk<829_+AGgDU}oj^1v*Yyu*C&KqRiq6mPu7r)drTa09YhP5+wxl#uyn z-S5{yv|cZ~zQK6)v2PK-7se*0pMnkM^Xn!Q1d=v+p$RkH>zZ^lDDJwo%_J^!!MbVh z29Gph&x<7%1@Q@5LQ8saFa~4K@=cw$I=++>6{cvBh}PU)v-4D4ZO>1}W)4qIp1qjc zMlrA)1>yv~u{T#;wVM*~3hUM%9t_lZRlg+bs%WUEZ{8=iWLtx_e9GJMD7$xTZ0~@* zMyT(^jfOavdIHtdY<@&=;zu_8ao6o7{@%p7hYdgKdb=Zu>G_vR&;P-7yQ8TSoYu1a z{1NQ*wAV7hX}u9#5}v-G9(1%0bN3`dbKeI?j2!>`60hr4SD3#R>D^aeAC75?(_ zuh|IT>z4OYe;l|Z{ADkm#@fjK_XBVNig zgql{P7 z%ci-rp^x8Bz5WxRkMWtap+~<3S+V?WK>v&LWgGhJ2f+8RN7&_hlJjL7`tUsf+E8)P zX*VGN%DEwP-H0BZWSj(DK7=aJH2>66LZH#361N{Rp4Y~>Ng%#$f8~*%&|HM@+*btR z+BWo=`@yxbSJ~xyk#lVu`rIdoYfml@v-5=aLLVr%p2V(4Bn1$&~B;!rgW>zLW=BN`ti!;v@5>Z1RyejjL!ppMC>uzLt(iJ#mXreM5++ zT*zLYav_Gp(NHiPAqQ+*eNYauBSeISVPDuEtf|bh5Y=En_XmT1JrG1se+tTyId`>t z`qcYqmY-f8cHP5AxdS7Sgd=diQMTK#>kd@tjv)8KR_&>G<&?MZ9qFki6Hdx#vWhy{ z)w8O_r@IZy&S!H|j|e-|&JoHk1m39iM}5U_{)xO~4)o{)*j-P#?(7+?Ptz5pJlP9? zkrKirMl6-*$_AU0u%?K?UO=`B{;@edpcg(RvQ9N*s*k3RsI(J~FGEtI1s2pzt#D+H z3EHt`{rUDkYT$rG4O^Dm=!@vbE4LyVMi*ztNK_u!nE(`I$I*PrTsrjK=PI z8z1{|EB0-)V(DK+J_7X|FEP#~_7Yx2D>VyQLC{{|Y4jUeiYr$>EM^iGG>KgIhs2Vh z!}0{6(l_24OGgzefK39(fww%8EHsB};}u_y*Y-7OmL)1mmCv-?ZjYh*t2;5zFE8g^ zPjO80{s=FAkMP^Q+~)d71$2jgx6}7L*R$za8T;fv?%DwOJXZ6&E1rHnO1k`&jj*#ZC8RMLs1Ia@mq(!e7Y6* zhFf*(rmlR`t-PU#bGCr=U3t48_>i~c-X;iz2Wcv8(y}FrIE6}$;7*VzNcZ5ICjW+H zaZIk2k|s=&W?*c)&yvj1wQb`|aiZ#)e0TerZu+Y#+togwE()Jou_-$vh?5iUK*K;R zd&;0AgR0u6+uEkv3y)I|S4t$C0C%@GjP#J+xo-Io^%_)vCddEt>}=@hwtV~(G>v~u zKJs2qOs$Jk$7+UqI+l8hU5pB?Lvw;Ya7wQ5U%WT*wy8G!U-&J*;>%%kOAO+XdIOsM z6PG=QcyAo5oa&SbNFa76gj+uc3TKDviK6VMN&47NKkmAbYYyv{^Js;%cY-6A{38A| z)^`~yqu$Jymx%)HR!DBxtsHSZ07~MRS@HbyuKNfx%fAN=5@z6^p0n2w^i79$2oKN~ zPB!V5C>dVMh$O3N8aY;4Rv@f8k!rfnr+Y1tX90_Z9vd+y;_hwjxn`fo9@Vh$80Xm3 zd#ngyiev7Gu+PE8wH2d)b-S~SInu3d3_W!!>DCV3?5qIJ9mqBCDk@RsP$f3yg+|!Z zA3Dk0(7_>902bG|W5fd|=Y@JAh*^kC0cmlL)ydUE?qJUt#p)}B%`Bya=AU2U;7*10S+$V7<&}+OSrM& zzjWZRn$K%1K}ky4Pm`aCIFkMJcS~^c7ZLSvxY*&qVHv;v71Cf3hdZ6$J%sTU-y+NS zXP_)@#~E-l^_ zjyiW?AD_`$f_|w4efV^_2o;kK6nfa{GQRV)>onKrN_P>9=Qv413;G#y_U-#$=J#oo zv<_{Y@lh7v6TVe~KY~5XWn4a9fyJ6>%XyX?5Tgw1;)|I_da zB>5T|fkZ0WHI64;B+dmlfh5T#@0Fbg33>pg8@~>R#(acc2%sj!W;U!>pPM5>#LT_EAjL>zur|>*PyDZy?Kkg zmXBe!>iL@QE@%0Y`9)ED_IJ3zR^2bN&nqT=6&4is{a57)wLV=}*fQ*FT`4e))xHXg zuxEw(M63R`B9B0A#v(<2vou>zy{0sW;w`250MwbS?-$<@-+0{Jhh3+;epNir*4+SRX`bGHYUA$8U-g`&9ObziOZS|Eqlx&!wIfK6R2#*?yCg zMV0;kv#9dl^wMJ+?XlN!oo4)aH~kwCR!*<<(%Tx=`Bvp6?FhXoL3go_i@W@9CP#BqU4C#Sjy`n07WS zFq2W3p@h}irGdI!OUq!ad7`(WYh9*#C?8QZB`jA>CF=W{h1StTY&c!pyD^hJxWS0Z zvY^I0g2{%)L_|@e@k~o6ni6TsBswRWQ-l}0rzB%OC#WeDu2ssI;@{pY+%cYK8Cjj() zo#kWE-nF^m#YCvRk@c+Y9zHPA(C<^-m_gl$1={C2d)Ks?mZrtpgQMAGzRAaYJCF-+ z%$NoW7Fg_owoNms_Q^yj)2X|yiQQv!uN^Z@)Tq&zDr;SvvJ)H9su<9a=A|~Z`&$PZ z`$l7hE`0c_l+OaUst;rIgCzTK{+Y>lUUGp`^w;LNYn6}6frGh{w%_#xxow|7XW)47 zRvC(^sAeppwj`?k?3mxs(YWEe1sqAMt5ZFeZUx0)Pnx}C1b|oPi|egix9(pZ2HgLyXPEYSWDA0 zYklk$AmP7HJ>!g1Sq*2=|Gj$6n$`3-^_REGc@%P{xU~EnUtczyyu}2E7QQc5dYV?1 z4SoEQavS%SphsUvE5vSrc$fX`vpmjXLqA=DKD-6X1J53!u{luksD6{L4*O}+5bfSQ z;=0PQs(=gbIkek;<`Q@g>5IZn;q21We`K$BNhLV(5PF*QP{8?EZ9NrVuXwL~zo@6$ z?G}4czF*W+?fVa((0)-*weOR)SNZ*-o{GB|3wh=2F+PTz8)r0t6F;}d_}o5{9`@63 zD?y*$LK1;P#WNi!^wcdBy?Pivs4sHLnc|d#Vr9i@YS1#l#fxUMpZh<{SEYDVg-?`S z(mol9ry~7komD|y$ZSZa!fn+pQNwaeD5gfGR`Oj9-Ocd8h{B0PN5nX^t=gz*X{c+G zJ(8_|7RMkU*TuoZlgP>|Q&$mtz}lt~9z^oY6Lt~=wW$}oMrqH-GJJ`etH$2RO?jYp z7)TZ+8S!;HeQ}lIbAQtOtP<7&BKwXKRQ>MzaJrX(?9;+Ng?%{0OhZDI#r^Vfi3ZKw}0=pCGrvderz?Jw~ zKzxtrM`b$kEx|V`A#Z`(Xp*KsVs;D&aW`` z(GcOs?#(BFiC+K{WaC<>US5QMssulZ-JnmD;QvyBKa2?HA4>3#mEe!KevSA7bX(Lv z{T9x_@gfJch46W@%u{Tyvc1A^=p}R6Nw3NdU}KS`7w(dRh8C1R()4bXZ8p84`y-8a z>Vn%WWEBk=qwEK=j?~kaEO`5V)$>C`kWpSzbN{aACzOLq9+4*zJLxa+JR;kbvJOEu zYy*L`Tfqu#Uic%^Kk?d`j`^rzcsya{WOY?zPi^nkA=by)=6}pCwy#RCRFQ8|&xoh|+Qz!;vku=RIPpS) zOTy6$Y+guE;(9i;$n^+Hvfpmw2<h=WdnY z#DDD8i`=CE^>H?HtbZ=kxu2L)cB18d#mI>#8@d8_XtSC^O~Jq?-1s z*z9QGFX6C>t${UdHQ|PIghorggrGo~Xiqx0d0)v}5fpyoz#)g%!ozPrEs|%2o=yl) zY#QL)??b#w8Tnnx?~qRUX-Dh0eTyHAf zy`+41Qv!FNf=%p5EQ-2A4*g=1jCAx1M4h@%x`u4yZYx7HR*cwx~}d(#pt$mr2$@G98M*-bW`57`$ew(D52djWZJnIjym@MJ>%V%d-1@>*Z<>Go+R1I5 zNGb_y=6nj}tkS2R8Jbiq%?L3?VGD!U@w{~qj=s#c;mO6t$%)0qcc?~-=fK*bZQZey zg;HUv5p5Xh#O7XRw5`*WfCb}+c!)iwA4v&1wT{+%Z(Rxc`4aT;`BzF%$P(w-qceXh zLH}HW{$ZYUExX-!IZ(2(-;Og;ig5VboHm|LR0E2v9O0|NL-3Ui(#k*`{WG|f0$hAe z37)j!h?vX?h=pUmjtIZw{Jnm8*gg5G&%w)0&rzgBe2@K}_;025_wm>yeYe=Y%=S|I z)UV}X;a3&y&QZT$4IE(?+wBQRv*rB&JF&-sQv!?QKtK8O=qEkz+<&R`J$#4X$H|2_ zbFg3d0c2?hk9HEZaLH0k6|#Lx1IR;%)>t8@{wbBOi;pIICcIwM?V&Po-o{)wmCdxb zWK-;ht~Fhqv#H7PuC(g&YJb%&uhtW*9%$_u%MJ{Ur+X>p9A!7+RLA?FtJVH(ie|HI zGo2mN$>elL=S*9Ft}8t-knYL}t?9Y;_PKP|>JI2dJ>%m&>G5&s2T9E75d56@ZD#@$ z)2VE=09VDlThuMLFWzo=Jv~%p2-`(rsrP!kNJ93!>>m3A=_- z*~t)gYCEKm;Zx6X3yk?uP06m;IE>a_M314dlxsckVpUGrXQ&&;=^Nk^a+3WQSEX(; zgUZodZvAxQXt#;l(?$Rl8zLI(6=J`}zjdWj@_4N1~nU`iAD>qT6rE zy8K%od3eGaRKeexqTIiKZuZQfx;n{_j4*m@q06XRM!U=Sj$>5Ya%hR~k1CcI!9=ig zQ2x2_lE|*&*xCm4uqlrhyU1ut)+mMdqxKdRbApNX0U3oV__Q!&3>DZDIMqGRV;T0) zR_yzC@RG=3w_v#_hn#fs27r<$+c(7|*@rSDHKUEg+j}}U2^it_I^%-pZ_YNMMnm-%UB)En! zXMYtQaPhh-POVQd$GHV4#1dthI%7NeP9Xipp>2b;Rhjwyqq4^gntE#EY_Apx3@&t} z7jun+r%raQZjaCO4J@^{t#2QaQ(t3Za%1Q4Y2!nw zHNEMj-kx=oTZj1>hSm2ds0k~CXG|Su2~gFS&mh~JX?o!koG#8m;Jok*P9>oooWiLV zQLPW9(2iZF>aZen@Nztc`EEkSHikY~_FQuQn3JiPMVS~|oj94Ttp_=mOwfcuAhXws z&H3iR10$pR2jhc{Vr_Q=Lf5$+0}VaPuEbbt%UJ8;@aW=A(_2!r?d`Lv&1|1)_RX|s zxA*sN&vwrAn3|a$Ph=^fGMgMrzrAgwxp|~*cwyn#OKXSQCR$o1+J>oG%FyzkF+1PI z&dWZsKlv_vvBGl5Rp?tMq<4mgx8>`qd*=^~pvF83M+}~|WQP4i^BohrGnkuh%*|{j zyO?TQ5WdSN=j?M{vpynVpNQ98wfD+ZW~{${{EQK2YC6|;cdza4Sm+|jU*46H=KUQbjg2E6ox}7q+1{SSKP|n~-MyGjFLrk? zrF&Nm^i5Cq4XgqsG~ld{_d}LDlrR8ZSz?wBuY9v%Nxg2xqnYNCrs+sV^6F2?je_V? zEw#(TnAa%0tb2(|A!~!_Y&FS7k|eOez;cT=nZu?^ED1c)G0Bz;uOw{@w^vmMVv2x5 zLRL+0OLIrKs@XR!U8Q-w__VA`J8F7r{87X!6+^Mw8?Z^JF4Y=b#CB(m7!KJ@%tQ`A zktb9ta>Y*AovN&<8}SNd%DkB{B^138ok=o;np}yxfm*|{&xJW?GLd! zr7m6DoYlPOi0+j^!ZK?1dSUxWPxNZry5Z6_0zV0%+jF9bHjNdkXm4Q!Zn zC|Y&Eb*>My=SbUP))biH4>hlKn0>aY>L>7Bx!>ZpB*W+Rc=czsHybQc<+*+7r-qH0w5F{)X0AU9j4ZLluN7 zK{u|_A)YR`0uie0+PX%Q4c&F| zY(sK!qV?0w;i_0;G(xqa8YIC4j`8x9XPrP)FhdK;VIN-s-}cYG(t|>OdNnyIg;9T?t46kU_NEzTCq^*I_`Q zxAM2*%Uw+4&w$=IB)yzwvg-;w18**eG_XrqO85aFL@L)%pV=FcKYTAB=rg;X`pn+Q zba-ocpV=D$>BXsRCNdRxpBecf>e!X60e%DCXAa4rWxw!6K+tEW^@-!o0@0)+ZZkTlwn`CHUyoQ&nFf`;)|)HZ-vAYMnuh;YsW z)uLbbd)UWR>_^FM-O(`ohjUlIx|hK0J|O@u+_a}yEWrf zL6VR?IECpAEAJ!JjZrapPjEkj`=1fMf%oG`-MyOL$BT6T9sK?{Y!IA4!`pAR+sDu_ z8l0VOzyE@8=gRkCSJ3^pI^Tam_z-`;6Lc>lotwwLV>V9f;U>0Z^9WwlVtZ5bHw=? zcchwANGNi5tSh2;gQ#9iNyT6wOlXSK#+8$%5_4+|J-HT^AI}8bbX;G=`o=rR zf3Ww3(a$jc_lFC6(OK#E5?bWV23%C6@=m_4*Me!zeEl7oqRki zKDzCsg98-+%j~bN1ITLKj!`D@^DFo*3)^Xy(LeGBTmzkzc1*i2Zcd8a?IR<|bKJOh z!|#k@tX^m~5lZow0)BtsJ7M;fKr|XC%!a>HF>Gpgd6nxLwrsbHcuFwBWp5C5U+p<2 zrc86bJBSv&el)BJ5o%MS*4ap;Fc+w*3a~FnzRhvTo2MR=c6Fcd5M&U@eG^(JZN-Ap z1``OV1RX)6umrg|>+EIDMUNZK_X9@P>Qr(n)xNq)2^)r6jUC7h?ajyX3mu~7p~y08 zD2GU-gpr}|YTE*W&hzq;PccGDWX_&5aZ@rT1$gZS}R z42*sgL1PtmxE^AijLrk4cSw_T@GVInjSt27|1B*;58;oNq2^|k=Np2EW8h(LWv!@f zGs!(h(83Zj1rm{(lgsue?TL#SwW=R@UFtYB3y)(LuWFy(7w&|)vZ7h`>gAc7A$B=oCr zrx^^I>_xg$Fk^@hcMf$7f$L0FJkuk)v5#AQ<#t)oEs`mNs0DO0dxu*>HT4{>J6uOWOOK-h z{Q(|XaQcb+2e}M!zKZM9__d2IxHhtn(fi+92M6VLwr2UG?Bj0i=~#}r)}x<8INx-M zZNsd%h?8RmxWI)>Aq)POJa?`(fMS>Q4?S?6M-R>hC&c@%C)j-~PVdu-D9_*y%kQ_? z5-p-9uoZ=B4G?DWyhtbKocwv=HvT*n4FX4@tQ@C(eD54SBIkWvpKD*+{V(@tc^}4h zJGIYeu0&B=O)t)Sdyf2$@5NCX@TclNKAtrg|MmR+9)4Yo@oePZv#(?5?{5CR9Qr@7 zJi$&8zD3p;&Izy`6pnMISYD7!Yc?q>o6j8_K7WHO8D z-rVE%!xCwK!yW6^-T8*3?9*_jq^o9ml1&QVE?Rp1cA^%RJfRh(0eD`Q3{A*uIt`j| zrflJ#k?w^DN3k2cDhcBBxjWwc(Oo+~@}?d*{uy$X^6XmpBjE?1Y33czQC=TO6`FI% z1eBr>z(3G=8VI%{al=HWf{7HvV21>^xOQVbi}a>EbLaS`j^QLW z$tgN|*@j-;E_?^k7iabe1?XTd3dRYw1-P)qRUeQ6J1J76R_+GkB1~$YG;!p@$Y?mz zDRlRS&>aEJ0nx+OZLAAt+9l2B)2Q!()IyHAzvn|~74&DQbL03aIzxXprU%>oa#+zZ zgr(js7=jO5|Jx^%TUv6dwkk&0T)#ZU&H^uUY(>g4aFz{K$aVfaL|A&1e*9Qfenh{& z?$gC--3N?#M$C-+o+F*)<*5tCLz!$OSm{MyZ|ttBGz2docu$&K=pq8m9IMO zi7jXkc0Fi#VEKH|=%29aFM(FO47$RZ+rpo5eJQ^VFP{Zk_zSKtoQF^sMIa1RbJEUaeGYFSKUeY#rIsdIqjZQgS2n-(BnhSOhmy-BU!@Zhns7?SOxT!*4;hAi-}D zs2@q_Vvn&0$ZH18k(Ms-*|iONA>?H!Wj)N<~)uCrvOc z?SsA#qr8p1Be9}2=!>-95^|r@4ICFx53p=zEY)evuUsl#DSE=}0pnAOp{d2@K7?)l zXik8=_E|`8CQi=0IqG2_R0(Q2H!Ie}OvODeMSS8fEB+S#rN(FcGFpytRrJnoL=opce3!^Ek)l1?(;%;NWtS<8qSY@&e?@5}a^J@LL3eM{!=l zgF*;2c=L19{JD!eyRiFh$B0uns@6->J&wBlLlhZqajnuSj{bVc2&6<)QWtz#d)^TD_5NNxJ3Z=HMoyVu3X zw-vq?pB-vto|;^uW4fnmFjtkXpAoVPt?7lX?t|O2YqFWQ>b^jr@L+R8-m~ZazXnZc$ zv34T0d3193L_!rz=>LM+v+;E-3a{npdFPDA^UYnGhgRs zI`=MZ+?Did$1byAevoy2A>v=(0iNO7n1nYqXB&RNAfeFagS=3MO?;esA{ii%D_XLUM>jF!rv4GZ(d8ZS6o*-t|KJ*8tk$wN@J+GlKn=5 zF0v1YOtX9zC4kvm=C9hH%k95vezpC3_1NJp-Q8ObkBuMR+}*wT@P?67`ZK#mCo&qO zS4atVR&`Ccv`lwZb%Ye?U2e5{SQyxI^_r!t_6`i}y=rOA)q4iI(;Ls7nmT7wSJ$R< zrl!u_n0{^F&U|*~K$E9R#nGyUm7Gj`zc7i1g}oA!PfD) z!UyVdG(4J9XE5X#ijxPxarxr!WVAG9E}%{*P4(}Jp~~}#j>bB{SI0cEBr*|s3PE3> zTQjnu+N#j7rS^=TJ~DFp=-9#0(Su_dl+oId>7#mQ?2xYf$fGyKs~aRmY>IfXrc@7( z{$+J_{)oCo(XS2q5yspqKRtBX#Kghj;e!(srww&OOJ7hYcvhGJNj@Mf=O;6Ke<__B7%7FWVz2Pd=$3&SmO_ACP^_ ztY3LNlKbJpll%%{AX$Yc_U-d^_p zw#h`>WHLFK!~qDsL#?etgUJ?t5}+f`VYj@6+i9zp*SM}ljcu}vIL9oe#$HRV8&rPt zy_0M`@l9lTp$ta10uP?q&ZE!cxKXapTCpGlSMfTtHlSbR? z`X)OS{D*u1+!y7jyRi0Ymda}M$9y>a5C8mZFWGG`(+}Z8JhYxA6OL8mBqNJ*V$doo z_OTBLA4EhJz7km5uoN(&^9g&NTQ5vVtA*+k`gim z_t5JQJnvqc?>lRMo8aEloejeTQM4*G6k{72=9gMyX~^hrD4^Y5*O#Gr**Jk8oIG@| zh86NLmT~l<5{`BfdY&bl_Sw>3sDS`H6}m`AVF|(%LX_4!N@%0mz$ple$Z(>Xbc8o* zoIW0b@fSzkrcflzrBYEzg_Z=zTOw&IO~7K3jc>ek)v8N3E?qS1R?NaqnKka{8a>z| zshTFG&%}WTtZ8RwdPgHu^`&dpZ@6Y@`jT}Xzw{-jNbz{zxQcb8ObxR+>7SUuS{h?< zq{D+!+;ozJ8{T^EV;-maHz&!(BavTZzFCIF%?~>(V{gS_Xw1f)<6m;WA-`_~wXX8} zP}(p*lAP&G&9t@6q&jDkujKjRXe&+wL*jxrAl%858s<1ge>YCdXnf+d;o;LJdKWu8 z7kj(uCo@Zjr&XR4Rytbk*!?JrZy^-Q>zlMKe@naz71bo+Ht2+O=GGycpV5o6Sf(%| z_BtDYMM<*`q6otr3>KNSt?Y5d(2+c(OBMxKgngJ6q<|yls1Wn#|IuT@FQ#RCd6RHq z6+egl)bOzz>x4J{BS(kR`i6GHeuw;;uxkPu?n(Q9+}>tz5{ICQGl^A6W5q&cqju-* zfkYNim-752l&t-@APKc3DN4_$P1y&*06i4RuRXU3>7jVzg@19 zSF=RfYgNtf9YuzvS2x1&6ejDj0#i6~;4@+j-o7||VkhzVK?d?zl-)7Pb7BABEX$#?ik$9K3v^0Ev5(TO=bpevHN;CLB7uWQYIU6sU57P1^P^_7nq zsu=x;rb)tiO&75*vPU=?;gp=ImiZokhx zq219j-+}+r^QqK)XMJ~VZFfEY$sW-L-GP8OgfnX_Y$T-fn*s2PgKFU&T3!tJ-Gdt4 z(r`x`b^HDJN+7+E?po;VTl16k=UI4f76Y;)IVAR%SpJ&5!) zfb+oPfk?zxT`^dWV^qKI;!4g~aZ)M?!D7NDX}=|Qw6X_>HH#XIin-hA5i2Z{vAo_V9pYz$0uNL!6_7lu1>Y{TM zEHjakW>~>#OU$N+e6?(j{Je!Pm;WA{ro%>dUOJoEpD;xQ8$X-Yb}U`Kdj9goj-@6& zBCBpAGB*7z_#jMU{CE?kh2by5U%0k^@3kA&Ub8P7t1r?rP6<5a#)Y~nN!0Xe*;mI; z(x5u)IBp7ahq1sX@bbf7!;JPUpN3PV(}2#|Gs@!7GdwQMIp0;m3a>?89c6CWET*HV zb+EsC<|~{3cN*!x@BJnX^+#;?-#h}A-2tWrPHOoJulYw#gXMJhgm4MG;a zx(c^)@pcZ0x*w*tZECJdn6l}yu%jiKjWkS7jn`FY5}H*d8>VSCXS3a%W*x$~zJXI$ z4V>Db8DT@x-PKDyy|W2Gqb_)qh%vqUw7YQ(0-Mzh z1@2m<@D)`_SYGJ!ECv6v?)4S^Lsc{!>3bs)#IY-?Ly2rN&V~>oMq<@K{dYpDDZi|$ zh^d>>%ZhSyK(&-33a-7{BZ^GY(mqckh8c(4gnmN|#Tf)?EBKQpt!%0kSq!-at|R6_ zg(3?tn7-MI=k2p-f>|Uz?#_FWhz$A1{z#9!@@K5l_bgDgorS^yzE0mWv@4t4HAGj` z4;4Rt3|xlkB)?)52!%Us(!zOcO)@K~q$`vvqA9L)wNbm1VfJ9{B(0L!nnG_7h5t`} z>MYyD*FInTg22u^siIU;qlkM^56Rz7=f=UG=k(E83kU*pb%D+~D_N zk*P)}0xFP)9TYLzM3hN}NAT}=-4T8c?OeKs$YISNUag;Jk(GB3J;PFjezyH@_U=DX ztqZ0OxBB{!t`8Xidmq-pK`yhv0E<1fEjs7QA(VKLQp#uMfgP?i4A@7M+S&jehj%#< z!_D#G#M0O;gmA`VllSFt{9r8K(i|Uaqcik0Me!HzD&J>D%c?w`j)$K5#8w=Qw@?)G z&7F%qskw})iK*FMI!@06hog`Ews@a&wW18jy7rZ*>i-v>zwp1z=h>S7MgA^O9Z>OK zk^R6ep_8vJ;z=R$-D8&dK5P3APO0EaD^L9Cqbb@`EnHBwmHs-T;n-*Dg=hcKQ-Exa z(8nJXS-UL5 zXPrR~=3VR}#Dt3cjrZ-0NGui!$6`$9zrv9ip4FGHKzz9ATzusWMP8isQA4i<(CB`IQ$Yf!|$S>|850>*cKIFJ>c73 zY|8NeGGMU}q41qm*ly9j{0!@152B8uSsH`W<_3L92h<2qMh$V{b}8!{;!&kq-2jvB!KcBF{Y?17J_H*f_(y%xe_wDP~oe$5_*jyVH;+%~6>`C=;(AX?kk#4! zSA2k7Q@d(ryvho>ade6|oNB0BH9J-Xmo`EInlIc|%Wr#w{B|hS$Zt0Y{B9RJ6sQUW zLps6>p;UdaDu{~7me;CDRec;ckcAKS7ig=)jW)oIj(*PTba1Zgse0Mp?LC_gWd^$a zPUk}Hv2cH1kB>SBbIjL?4AY3uA4IUj7fRM5H#daTNw3ik)0y_`+=hJE_mfn^IzY@4 zP9c(KM2$d{+Zxioxah`)* z-)i_h;+=BP@&;rg6x3Mo{FC=Kc+GAO7#@_92&1oN;Yrj`(1W_hysYpC4Z=#qo|zYf zVN_%BVlL5TvJNYA$)j9br1*I;&d0--iK3Td5ap`b!#MQ-%(_v3d|=m1A~ADHcHk8J zo<0RdOnZ8_qWWScvt178hQ=P$u%fj>Ck{$py*HQNKiA$iw|_9VZ*^Nfzk6t8PY%1k zhxX)1x};s*;yC*}G;8d5!wM(F5kI7SO&IhgKN1oyDAbm-y`h8_ju(W!i=F%tY7Y0T zY3rL$d^OVF*w7b?_f=PSwHWs|Ot;4R!(UCz_qDC*>6}k>P1k={Z|SP3>W|0zyHney zR&7mnw%v!LWygDa#~VU*#siJ515MHU+B#EPS50k8B^n+u>O%Hins4{=7qBn$XGB-P zB%kWaR0`^x<4_=NY)vp0LMC}YPiK_%*2dXTQ^*gaSC4g8d)8T-7Ce?|1wDf)_KOYM zJeCi2znEClRafX(zYX6)MwM_gydbdYo!xbCVmQWxy%2$sN@ttn)3ERL^~qg0xOq%q zn`SQ9)D`O+tWj@)?PGjc4alCEZ4>9MSv+q%InyC&e(l2)<-66Y9YmSB&F2gEr#GCl zYT%6B>l?wNjDUqHm9~Cz%iQ7d@xycZ^Itb8L&{s^uQM9g?K&+tdCo?d?bu(9AO9?T z)8(VeLI;t3ODA8!!N3q*f}S8@S)jI1vn<@^_Q)PDPLYuPDvPVa3j1lY_g|K=q*f-bYyigy{&F zrX2Foamb`|c}ge>H;BLr$4pigWc*u&4;afTQ;tb&6MZ_09ZO9iR&T|k5TV{c;UP7w z79LY6?UHCJ39QWspCp>vPwIOPXydDt2b=hbptLMNT0~Q*66BZOR(}?JDQ4hwRaN}% zH(U3*y*Q|nvG02V$n4zf&U<{C4_;wiD||%Q2!X1(74g`+EUwkj`AY%eL7N}Ua4NR* zXQH4OP%oAtKYPhS(0?U?v8JiR6N$;K-2+S6KvYdlZp(rUd^2aQ>5j%HcJ$+nZun+1 zn|gXTiK{Q2&+eIRORr7|`GXtsy4$B=lbAle|Ln8V!>6xH=Lh!<4(u94QE>bLEUaHX z13PL-=B@Bx@r|yqV9Sz@a8oR5l*DF6>9B<|Iw(D!OE(UtYi$z}xh-1HbbIY!N-}+S z_$*;Bd4UQKq40Qh$3Pv6@k^Ya5-`@PRejCTj+pWT6x#P758^MvuMvZ!D0Byp2oPs3 z@+QuGDtFt#WPa6qwvJccxAXZl_LYvM)1!^C(aw(6_V!e3JNs7WSYwQU4YARVR06<6 zs_=#tJABwSpfs~NliA$cvpLhhkQ-W9%ni;L&ORx)`?&o;sCkSrxm#9@X%n4s_x~`i z|NSwub54SYV^@)ThpyyI8md(5}QM<5)^@+QX4f@1O-GD!WodQ&?vt1Yi{2+%n z1OAK)`=0>M2&f(+o=b3FJuJqjz#%^g_>eHi?i9NLKdMkX=TX=9c-L?#WcbFR448k*F8Al0H>o0H9_q4HnE12e*+>pJ1RXp`$f$rhP>iR)T``d6Ye?b zZZAFqb1W`=k87gnDVIQ`b5S-oX=L{7CX=7rChLlDMzhJz_NCQTO!2hQ6@Pccm{W8dL%rGBsF<>% zxjn1(h%o3|tEjSSP7AX#u;o@gYq`|y73$cq@F}}Z3Y_!l=bEE4la}y>=D}7`FlooV z)KOp80XHKGCy8Q9UZ`uz$8`8P;>lzjr&k%VJj6ZCX$MZ|c#d;_n$uV1b9dBs*VR!_ zmVQ<>H&;cQn}v>=bZu?A#{QXVidM&(s-kEY)klOSb{c2}G9In+4px{@*k#~^tmKF^ z?^8u7(;R4RX++G7j1a#jxT7sq-qw~T%M5v#S6GUKo8kfN6%6PWxD?LGiunB@&EWGm z&UOl~BftEn=7D}pA2!IZt*iS(U7avqQ(H?J$!mpP*Pp-@TJVe$SOLja#>=?eCwh`3 zV0OE=HQE^W22f;xp|G)r5`=~y8ER1#iNTT}+-C-=;$DBPPcje(GGtiSlnU(EWtl0m zA9!sLvMh_eB7CiYXB@v`L2k!`SbXIwey3!4*2-Aas4p@tYeH6W&%j7;DB}QsEXP z=VO1Ao6e;%5SMV9bS@3rU0~mI{YE&Q^S}YxXznl$X`#Ct&Q>ix!M-^VPg#*FBRLFF z1+KY#Do$khBE3PH@Jg-;h8w=Cf@uE{Ckb-Ezn$Z%a<7w<@sz z<+I-ma~XOz&Yd~}>kkX$VxCb^thGk$%vN}9HZ&%@w*|x*X|HJrI%QN*#y^VsYfWtp z9?^q>Dnb>NLMzovrD|#oA>N-ni!XnPxY{;*T7abg30FIhn3@O7Y`sqcQ&W(43}J$n z`*9-L7|yK#ZP7?i%74E6s{H4DO}Ur>_S2GV1N-sm#C`~S*RqYvU!b+St#a)?QCoXO zRZR`H4dNNsi!8vpY0fVu0mB(IG3WIZKYBm?c(D{3!VEdm8Z-1T--+T#T3jFD*N!B` z^&|Y+k&C!~)OnsuIb1*HTyr^r>!Z*@7}|d$dpj&LM{=V-j%9Q@>{*K$*q!1P-WV_u z-nP6v)2j#jA>NDAawOqaUYE_04!C}te<6kY|BU-{!mX%Q>-vUk!q%BMzsFNgIQ`(9 z8rM(qYlp+)`YGo==YzO@n)kB>l=3d;OB_MTyp(Ym7DU@Hi; zOrN*54hNeOpMximLfkoOBx&0bl~$|VJ8 zQBysh9g?M@GOv2TqYFRMW$ZPWam#)a{;OWp#S>gJ!spllv_TnXTw*zjSV@U0sb&sj zvscceKpGq;?itztcAq>*<~=39Lk-D8%AyB)4$6=16n-`1v01`Kq|hlFI8TtJNwnS}{Dd`& z8@LWtq7lLb#bQhHEgF5#cE$22DjJuNn56q;-nj1DDHLX9NrevO*{CXtN^MK3v5IE5 zNBAXM5Ko~~?ttqGc4L1CANg9}YRj!b!;#I7*jSAOF{ZiA)xs|adXQzQ*EJwx%<}di zv4QlQqppJZ(f><5=V(p3rmnNLwzCd@w3D6_t9JApQDmo!m(t29>N&Pb#?>>f=h)&m zA}yGxd)C`noq_As4_7bz^`aPUid6OWz!wWG2Up=(yuLmHe=D56F@Jqs(AIJKh3DAW zV)IHJXG4Aca(%tTfu|mTU_d``B4To?>8ctmrj=TRm zP2@soB0nMCLmbl{BzeHeej`of&7q(k4DlbF{+2I;Ch~JU4?E_6q={T#Zm9eh`rNrZ z?|S$01ANDD$u5Ex!KWDtUtHqCG81`sBpwR-RLNuNl$tH8hE-P;jU5Wbk(jP(9)bDb zL1F*jgt!DnOM%LrI21NqQa3vfvC8@OIE{GZ7PJPi(SI{@MhzQDA-70p_E~gTe1*|fx)#3covwlk_50*!^I0MH94e2WFB%z4f;&ph+YGtZPWGiQ#F zLWpR179sjp*VI?k*Pa08IQFqDBGrz(8B;0qJJg0tM|LYn)5hC)a5W%6->gUe`J{sZi zxCT#~eaY0{2Yr+&#LDZ1cy!G4#`-BqEtcL0$Gs?-4n**npq;qyf%~lKb6OTJ7>J^P1+j z?3okxtPouxv$Oxa=Eix`t=C@x`f$+Owo2a>k6LcPt&l#Sh{9E99Ym9`i5o>Q-39!t zM1}D0eOB-u;j3uv9n|?O8lmMfl^!>qLsNuWG;MkdGgCUfw5T!6nUI>i=D{sdyDp0A z@vG3Z?+O1&my6z_>t{PBUW0TF2X(fr#;sK-!s2I9so)TVa=Vr}4b-o4qVURmokaK7 z3*qY{+~fVY!9xbuh@B$hl+yEIxmR{|3wbkz%iq*?T--u~luAr6;>7Y*0xJaZh9*&> zpin<+NublzCkhszzEejKBD_P`|0)g++5}slukEX>JjQumr?Tgma2! z_$}fx_$$SB_|MBEA!V{Ggc5G;PFfwu~m zFl3fCks*2*_+VH^iUEhT7lgJO_9F80Shx{94331ZMFV_FdlBwQ4mWU=ZUyvDa0_(eW{v;vHRRW;uGK|IV-e)jEu|T$ zEv__zMLeoIvmtvvN{Vcxqaf>R zvlr3^S#>k4O1JH{`Y@Q|Q{FPp!#x9SNHJQEMd%G~7WawA#f##7@rC$NoRVSa74l?v z*;kH`7s*+2kz6TP%WqU4HAsz7e^>uhr?g($0Bw}kpfzjDwQIHQ+8f$O+E>~U?WDzO ziM6;a`Iae`d6uP?t1Wj}Hd>yr?6&N+d~G=vB!a?&5`(gWJV8Bz`UQ;$Y6!YG==GqF ztQPAC>qXXC)?XZRxfywsPBW+s(Fhw#~LDZ7cJN)n_Xj@~{8RAnA{#5v`@VCPEg&&R>7_l~DQ^XSyyCe2Sd>xq|IVJMW zNMBT4)TF4{QH!HiMXiZi8?`CQ7abOz9Gw$g99-QV%lmXw5u3}do*Fe`w*Nv`MTwk~jrsbx2(|V;1NE?~fm^Lr%zO+_%sJq5J%ss(9-96v^ z7x#7U+ui%zhuy!YThjZdUz&bZ`c3KU(l@6+lm2S@$LZgvpUBWMA~RAmx@44R)MO0H zxG-Z*#?$gRvBoI5smN^Vo`lH4nD|C)PG?t{5c=Dw8sLGCxXzjm;6i0+WwA*VxehpRi> z(&63?TRME+F{`7eV~>vgI@Wc(spA(Nf9!a?lk61HDXCL-r-DvBI`!?eq|=q1{@UrD zP7me<=f&rx<>lw~%o~(9K5u5;g1i-ZH|DL&dpK`<-s^cE=6#v>Q{IWrTIa~lNuBTM zys7ikop*KV+@-WjWtR)OjOj9^OH-FiyWH62A6=g9@_v`Ex*W;xl0P>8#{3uZzw6qm z>t$UZ^Jt!-p2eQ`3S0%93Pu)OTCmL<>MiwN?%m=2uCS!Ax$x4$RfRVc-d1>T;ikfW z7H%tivG9$;4+{4ceph(3@b@BB6jl^plv17?u3d*{b4KEv4)?9XN*-d44m2D{7R<^6`wX*lhMR|VtW#u20w|2|vmfx+UTd!`l z-3E6X)oo(8ncWt4yS&?$Zl84frrXcm6T0{6KDGO@?$>m`wfjBY|K9y@k0m`0_3Ya7 znO+@x-P7x2@0{NGy=!}4++%T-E1fg{o+%c&@Uza%<)8s{E>3tJYUNRMlF2 zTlH@>-D|F|*;`v#duy$)?-hN2>esK|#D0J2x4GYo{Z91v^dH~<-u`b75CbX(Tt49W zf$o7L2F@S2ap2E`1`b*==#vXVFX(Z>ybB(@;Mm}@!5fAY44FA(;gHLRtR8aLko$)` zHe|<;*M=M$8a}kk&>=&w9s1PJuZKkrD<9T4?5<%C4f|%ed-%ZNi-xZmzG?Wo!#^E< zbVSC8AtNpsaovdfM*L&M<0GCO@#2WrN4!7c(1>FrPS@G$qUsXr(&}>Sde+UYyQA*W zx_9gTJ+jltu_LbKM{d3T)9{-m&+UFP4XW3H~FaC zE5DOJ%3oBn>Y@r&sT!-A@eHwDJ+IzSpKBH^T#M3TwPY!9Up1zNGT(-q>1awWJ@ zUFoiDSDve@tJKxgHNrLCHN$nW>r&ShXd!NP-R@fF`rRGoj&|GKPPfaQ;m&b;+=cFL z?rQfu_a*LY+<$X#bU)yJ(EYIcG53@1ZSLpNZNb$IQvIYKMAReXI%1 zik!qCCz)D~*2Tz4uq)gZ>q>FCU0JS9u6$RCtDCFRHO@8BHQTk=wZgU9bra`gz1xPI z#JUsRDQ>rslXA{Ui+crfa-aKtbgq*}6CrM^brXVMH5?57t z62mhk9$vdj)%qnt`meRSe)VOve&LfC2L=3mU-lqd&~ElZ;$V#*1CYeKLo$Z zIFLqx2PXST4wM{t;K0lSBMw-;d<*~02W$w9IDi=iA-?>X{ncNt{_>J9AN+E`{-5{1 zC&d0;a1X#evj4vQEA}@C@p+_;XA{KBpg$lDIW@PI0gI{DYGIJI+SM98)$>ogUR$lL z(bj7BYg_!dwyU;D+pImRZNr#mhqhDOrR~;U#^~m-b`&FalA2Zd5mQO+TspT_2UcXx20oHH^<~9AQ>ePj5wVHzG>JQaz>PEF#U7{wd z=TIx=VD#0f#;NgYw|YUns4h~sqK>8F`7s-{s#f&HIC=!e?fO`8Ax3r&hzG@H@tAmC zyn?aae({a?RvcAJ)l{`iy{=l+8S$%(k+CvKrehAEo2-zva*UjSQQr)V`xeW!a-FwzyPN_RpEM`YuQ*+f_TCiHH zUcs2LLba+7)a|NX3sJpQoVrd8Q2o_FHA%#wwFt$`L=0vql0~}ch5A&4=YWY~q!=T{ ziCoN9+=S=ntHcJeT0Jk;i?`8^ye{4p--%7~F+43F7p*c<2FY+4CGFB9Gh~kJB8SO- za-h6GI^{xnmAphQmAA{4YA5D2cFX7FPPt2dhL&faaNt>56TgWB879(XoXC_3B1%rD@w5C^{9Ar6ev#jaTjVw3 zUHP)e5&6=M+I_K{il>3U$QBVI&WO>Nr@2yIDqfS%i~g7)yjR{Ll4OKPkhkpl3oR@cAr<3l+;Uu1MV(ugR1Jq8JFsG$ zTO7V%$RM|PZSbIKx43`!kPF=6a!h)=#l*Vd1KsFp1u0w}@8tUVjk5FP$#dpOH~Z03X3v-=L#NDbnj*4Je#OH5Ka{v(zlCr_51vL=d%rkZQ47CW0}lxdH1Ew_?NDfThN=S%@Tz}|iUM?ZHLYRpnt)I5ybreMT8 z2JOHgJOlJWKS`|@Ms)~j5*PEB&Ml|P78T7k6ZMelq^yNM3%W&JWj_gHLM0Xw48Bv~ z&tQ0lnoTfcvY94Z{9@)^VRhH>zp8P%g{K%+nFUUG4q|>b*djqY;Hd2NHdXTKp-!RO zONmIt-1ON0RjNu@@oZBIHe--0$#s}?dPLZ`4o7nxCVQZkdj<5_33fttBe333n@+ZJ z3{?Iwf4$KVW?{axK6?c12el1Y%S78ivmum&0icQJ5~5U)8_@is{;hsh$BB;CN|`PL zVY&v0t1`JBMGpg8pr%&y< zVE3^6Ftm1!uo9*EsTo+MorZRAHl*)}`InxmFQ5*}t9mHdv&vNks=KNIl!G~qZmJqk zw(6?NRTZEtm9NTFC7?{q-ju2eKpCpDDp7p^rK>zuta?KiX=rUasUp=2(jQcM-t^Mk zF2>`&5$)hJctZI9*>}kya-52jTjW-xmkvoXAz?No&qaQtpY$xF_nyJy9R-iK^KyYS`Kau(gd~Ya5F;yMwx1ME_UZ6(UAm0p8@0 zhPFZkkyES1Ftt?V{h@n93=0|onm1r?fDLEDmMvnK%I?tl#O z9mVwn)m0Rj?l$lv*8%w*tzQW?U8)Xi z$nXDc_XW}ly#pueK)k95WSXE(S$y7TQ7-}ny0{S0>(+~>Mu8qi^^ z`weCozVUsG86CQR3y$btkTJePGPupDd@i?#jBy#>JJ?%JTAzRU19_A@)16+dK;QJl- zJ>g0KzlV8O*PnH$11`)bM*mL^qm$bIs>>B_b?RJ)*;M`wSN_0hz*~}qE69N>VC_=Su7Fz$-PdEjZX(*Ok=SoHNp!(%qg@q=GCcq1sWOZr zOGJgr!dN8*>VEr%SU1s^9IbT&%iu-U*HE|#&#Nb4aR)?|06oc zYuBPho)!+*L3lCf_Gn#k?S*?MW{@#=q;fE>n~Fg@p6KYF99ACm-vLpE8C*A>11h8y zc^{AaqoOxCg}ENtis#@Dgh!4?zQ>AG?31Kt?F4e-4IK}?C6E)%w3_Ivz|3tiIi*Cg zM&m!@%D5-mhsjwY2QCit=#g*dQjp=;qbV$Oae zX0CjANC!FloLGHJ3@5ftt7CS>}B363^W6Eb?7YooQ=ZFj~2KoG5 zq-cp4?<@c>ihEF0iJwJZ%#P=)5X{csit9p=hn22j9G}LA8rqVjA`?8iQ{Rr6avG~j zRfwk%8kgPX`weZ0T}^?0sGt7acU+D2{i-{RGxJer5KjFo^`%(9@v2UkiRp>B{b6t4 z<9-a>0K-LLJQprfAV+t&KFR|+FTxbuy>R_8;%*X|S_AC23vBv6k)-w@pRXWJIQs0r zBM+kx=K?WQ%M_WGRgirY&!o_73axsR^THJyt_;^MaG0@XJXuZ;xB@sg9MKX?@d?f| zu3d4BgDZr46>b0=g_Cm|*AB*&XgN&Yi*)t^K7sYDaI90UM?GtX9*zV5G3p@d9m+JF z%0l)59y-QKh8N@1F0vE$u(hRu89&Yo$=?ai0~aVG$%Xk(-!}$6P(I2Z<(c^DabQQV zyEL*V@j0&K$Udq-LGu{+`POnC^qT-5brL($v5uvrCM{Usvq~FQQeKfEG8DVh!exYv z#0py!R>h)u{W1<~Z}DOaR+a3KFbC^bKFk|AWCB+6a%GbE0c(NDGDW6Jv`dn9?lsDE znISX99+@Sxv1*trJIIb$?dpWpme*xxtRm*iu2@klkY3Rd+BhK#WsxkFC0L!wlV!YG z+8r%f53F$Y#2Vn6vNu**`iPUVLcA?2WtFU!HL?~u?koF2^Zi9vIRLAYgRm|+7*DA~ zu}V2y96(zz0_9aFN6JxhG@cH}%5ic$R$wp0sv&lmW98-|ISK1@_1K*~Sx%9SSf!gL zr^)Fk*Z1X2%;3zHbL3pvBF)ERl%avHWT7^A-SI8@|R(iF(MqZ0Gqf&Xjya9XuZj@`}U$GumhLxk6 z(ZYNHE1HhgzYnpZdWXDI-X-tG8u2~6O1vI>IX1}q^*#3`L zseD%c15dmU8wr%D1p4*AJ_H z@5pztCfY3D=hf>Ekths(7KgDY2zpx7Yx!fneko)DA=$ZSYzWpx0l3&Yju#4k6 ztn7X-f1ufP%&UDQT4+ukvtx(FB7I&R>xcm}>sWE*xpj>1FuRT?!L#Pq#aCje3PNjU z&az`aeYlEHk(l*fjJfjR;t~~ud5}vnCmfHN8iz_yiDIcrQcjgDmZ%iGh2Y{@l(S|v z#NW`XEW_MN9(w$LV*URxW`0IuE+!vyG#<>|c+s+s!u&$9_y+y;Y0S}-VeY0Io`>VzDlwxoMl4s=7}Ko4xcxHB1pNgwME$X1KM;FAFHnQk5H%F5gu}5; zSf@s!9ULphVYP4!b|;JzP5p?RQv${RK<0M&s$?svEb+7sWZzg1l z5OFo;{jL+&itEKn^`W>(d@n9nABkP+W4wj&DQ28kVea`k%yfRP_KDxr7ivFdjt=n3 z`ZwxZ%pZL(uE6Z_ZCF|V5wo60Fpk-Z9_L1pjyC&NaSL_`KY-oD8!_5Dgq60nn7=-Z z*~UA?-^Cr6=?um?)H7J8(&sXd@oeUA`g|t#gq&7qumi-06@H1;eGTjTL0H?jX~9|u zR`}bk)ri&NGxQHPV6V!@v~DB5)Z)dz&~tnuJ{9}K{y$p9!5)GPEfc#4vaxp{7rVqd zYMrz^tuyvm54?ZhdlsWcQ& z=~qCBt;WcxE}y}&pvYFEXIz)NvdY?*B?;@>mRUWAUT_&IbTSo)H;X@&pIeTv4yNyP*qw6AuGDvW)wrBLPL@we;HL+2kWc` z2e2yA6ny#x5qZ2`qf86^=3tmhRZY;4=}pas1y&o`sPS_%sHzQx*LWg^OkV&~Z(cBGcKrfY z2Gm|-8xm+I#YXI6PRoOu7B*!5?E3lB18PQ*!MR8`Zm+k>I$T%Sa8qHPf?^{V1%}kc zMlK5cDb!d;1TZZzGG9^>HsY*8_7)h5Dlu4<7%WN*e7P=2L0PS(4(4gC3lPIkTdHTs zTVSZE#K=TRjdf&T85L3)dAz76TyJXp*OH)-ZMv>860S7_s`2aIU|LfXG4fnp7nImW z2I{)B!ZuPj^`Mc|2w6w6!RrZ^mRU!$dcsDxsUu|clo^f9jq_*Bw~n6HT)(g}Y;0Sg z9jSSHcYwMJV2Dd6b+g?v` zXrq~~p&=uumHwQT+8P6ky})2uP;6^#*A(k5h_|@hI*la?o7R>)V(lsL7KTncD>ZL{ zWqMnaTp2dKeH=8&#nu^Sd3p-;#s~U0r1aK?%*dx5KG@l%%kOW&s+a7=}C6W>aBk zt&Ln17*ZD-x$xJX;#%wcfT~wwWWJ;{Z2noL?JY1ARbsFzF@!8Bx3o~}+!BziKde;G zfVaRA1!aq}C9m#;vdC zN}39}Dfa#po3FSOR@(+0e-dL}Sn))B+u+4ftE#041~5!N9)O8$ke6< z%?31Mp+Pr)#$ugrJ|3**>R{sxoW7tF&YfW}B-_J4oChtqa<#yf8#-Ls=y2uAg)3Jo zTzTk#D;p=SJov%2T$h2T* zC0A*Wr&4dfJ^Cob>nSy4E;VE>HDoR|WG*#iE-ep%b~&Bt;jC31o*y<9k{NI~i)7rW zs^Pi_9)lGgR9KDr1C7U{KVssElSRX|GHzaD^Ngk`^sI=%+LV0SnH$!yVD{|B7KTH) ziDodep?>nLwp+(pF{xk1-t6S_qnQH<10kwIr_PwR0I6Z9uFI#lji_Osu<4hef1In! z(Kwe#40w(SN6xEnM*rM6RTnXeZ=2bMNc_kK^cjt9!F<~m98LGL8e2?)7z5Ep5<6|d z3}|3ZQyWp7i8_l6+l!}x){uqC7}Z8*o!>aQY3>xxX+c3@NaOq#3{YAc{g@J4y`B+W z7nQ{klbdEY&7Fh7Z>ev-#4@F6?leoyg61Y8+^7N-)t1>an(GauQP7nnVC5Xup3%t5*dOiA(+*4W1qo+bo2|wae zdFrqy0AC&e7aB0yKD7TjJ;8o@g8lRabG^ou>j18$A>5))Zkp2&!kq^}Bpbpi4SrSm z*i>%d^tx7PJdQ#(178us&1XRDqKN5DO|$A7niisXOrH9{qfj4Ocnb9gKwK-s(Wx~w z&Td-NM(pK^>?zchgsa{o7J3cgy{v8E^+&!!eU#-XEaZys@%jg?Ra|(uujGarnP-zX zZc6zk(o}cLwRz@Si1jLX)m+q}o?e}*yIU9bLF1iY?(uReZ;nrWem z_uqwLMW;^idnO`Mf6oN-u=rtZRbv_@v{{YKb4AXa`sP`}HD}hGS=fn5yEygtAZSfW z^12k36ofT*on^5)i9K08Fdu5g?p-?Jg(y<+r@fijJDKxo4tDVhk+YW_V)H3Ierno7 z{082@lClKU*;r?C$wV28)hjEKIEne>!NXz<*MG z4F4JI^OE9W_LpNm8k`wFQ#}LtZAH7JKUaIBB= z-2%57?rOL}aJ6uK;IKlQT?B^}%WSMUW@o^qz+uHU8}ET+hr+>*vRYBj=R14JU)Gd^ zhw!I8{^6MQKYK@Z5!OUIU{!!@XOC|_<Z~8_7 ze$q!fx|cKMvexeb@A1)I^_jklkdnw63Wt^0tZKudQek4}sV5|kJ0D#og13v#_2`LSVjO8AL8k!O!10z7HU@JX}IHXN8t{_eGNyr zeQ+Pcy$6Rpn)%xX70*6v92lJrVvG6B=$pgG`8UwM^H? z#KFRixJ4!eYd3JPc>~HYA;RfgQcN7`gFzQ=t6 zmbbKT{kXLKCiJNgX?vkyk@l7e?E$olaoYhs#n7WBwAq9xbfX!nL+i}YJ51;%K-a@v zNwoA!yE4thrLFLXGH%IPp+whW;^q>>ESO6hlr|k94QUhLk20a5CN#i=2v^N?y-Zx0 z2@%fAp`A@!jtRL<$Z10H=YdETolB${8f-$!54ld8&~XzwYC;E1=xY<&Cxq)`xc6Ld zz<-(Y@7ifX+f3*Q6C%1TOt;CzZ7`v=fNq0ZnM*`j8)&YK9V&dOy)IwB8Tt zaT#~lSzIW_Eljr>{?*KzpvxIr3TPoi^Gs-_2~p@2lWrnIW7>rdXWSs*YE8O640SW< zic<4aJEUf$rldMdDB6TVO~~ShQd<2`DsU+$C~nFxeq72S6Z+PK_M6bBCbail2LXt@mNH4{QhX5iW&im`)ZY&An4Gogn} z=zbGg59ltAaf=C|y)tlDn-I})8|GXJXrUABmvg2GO);T~CPcU~OgG%b4Kkrx6Y66^ z-At&+gb1f|$v1JRVTJ@5CX`}A4ih3t^|;X{F4Tl9ekiHcgiZ*N^b5(HbjZYgYeM@? z=u=SaO?nId9uwMy5=5N=4bn~8PEgWQe#qI!gbB& zY7gCU9^6eP?s|gYHgfvwlCA{2B54Wy789ClLeou%a1Bg1!NiR+A;Jyi&;cf{+Jt(U zP?-sNO{lX8kt{lw924g@A*Tt&n^2?)1)GraLy4zN2>MJsN;ytE2>l;m=xY<&XF@0+ zBlJBJdc%ZXHldy1v<>cw#P-k@#%*dBx`Ah;K z&|?H8?V=Gf;~p}h`%!LpF>bvdM|25{V+d^^GrLO=zzPy=6jsOlX%0Z8xE(Oz2S) z+H67_A=Nr)cp~XuZ!sNrV1)BAm&XZ|$u9&sZi2g>p({;jg$YsU5)MVXX3!CCF5{-R z!?C?MCIE*P4gOFQ8el?b&5Y1qCPXpHIEJ?!t~29u+Tq+LPKTU06EqSO!Hz$Ilt0vd z+Jw-KIfCs+1LN8cnxSaX9KlIf+V>@`utQr(EhaSAgr=L&1QRMVA+HG$m!Zrh$Hck) zP@=H!OE^iP2^K#@I7cht94GuZ$1f&y$Pd}y^K(gZnoztSvcF+MFZ-dy(|!nNsv3E- zZ^JPZPmoUSTl`~c`z90GU_xt6=r$8V%V)oq;!^KtU&UOO`=KP1s*X!SsTvSU)PN|) zVvdXQHR3k=p(ON7I*$4tBXl;Pshq|n6B=hibtXh~gPE=$BwJyx#C;DFDls9n&qin` zKtq`>%fz`%DA9ysO(@)itR_S_oeL7zIme$gp<^a=*o40KLw1ssWTSI-Z#t;3l-;%yj1^L(j(e9vX5& zH~2}s333#^qLY1OCp8v+S3V8p3{JC=;*5Hjd@TZaO})dIH`u=sek*YhXCzh;WTg5O zev!6|V9W$d8KlUMR8O#v8B=Ldr$PTM&He+w4e$xV;PjM(L50uB4W~6!__+^T|lduZA?WMerc;fvU%m&MNPBES- z`)juXlSykQGEnZ> z`q`$wBbd3p%PTs^Io1M>^)BbjNu{g?5=ISVOtE$?_!nzy;E$u#DOpVGMmUq>d4{PJ zBDCuXqhYQbp^vjq)>5p#gx?wOFG!g|`iE_NOc;_!4(C$n#P$%uc2%q)cSvg{d>7tc zz&e|Hk1%8v@G%cBi#cvFTU2+v`#|`Av40Qzaq0zvNvl}tqq3;Ro2^8n;G@-opR4DK zrOGw9Q7Pw;W@Ijv4(LhF;Y>N6<$QxF-(U`J5MHcc4X+@)SixFd!6o|`ui>r0Nt3`6 zz4!@l=19DI^EG&OW)7X1a{{NBz}&9q^8E*C6(>a!|2sME6>QbbB(0dwm}d1T;ASq_ z>AV6rfJ^fdmgEj?0f#;Xe*%~ICCs^*X`0pDz~8~;j{bxwIjudUE1XKt7`FKg=C)X) z(#+shtHo*r?hJT z_t)kzp15UFzGM>T>lE|JWd517?kFQDM>31rC{X^!KD8~GTsoO-GaXrmOsXlMp>oV* zN%~W+RWMrxE%5-)@|06K%5vsV&ZWOkVLeri<fZ8H$9P`}3 z_(!-s?8eDmH9{9~DO58}wb~9$ z7Og$Qp9J5jJqZi>oiQDJO^m;U{afLWSMH8{WWqZ_t(??#&bnZ!%N6fgTqA5xm(2m{TvNRbI3|WG!X1z6+Sz ziUhW_V!Yc449Ci5nru!hmnpM3EgMs2TLywMo6{X-=>m8ZON&))>vVdSEt26#g7J>p-@u37+=b7f-!c3h!^u8OyxN;^tUSz&~uk8EtYI zeVmSGubQa#M2u#Dwdu3n_LEL9Vy)x=ur!J$2LJnO27Q@@yX z)x@RK#QfJY=O(6aV*0z8{%*$H%@`;1bTU4bIXJmJe}v=4vsCd!iTw~{;Z;mi!5juM zW;5sLUrbNuQ%bp+>8~Q0#lI;>*#7~%xJrXQ;lqZ-GOk$@nC2=d3 zY7Jo2Q~0n|v6WI0eTXMcJwYpt_u5IeW1PxWdMYf{bk5OqmTfxiQNWwNWUIHcgtzN7 zjKRnf`00$fj3ktou|EIGIlYWIT*msmjLYjrroW8ST`8#Nbuz}u97?#PvKbCzTbjnU zRKlD~*p@o8{e&@P38z)UmQ!NM05|$>7v!|q;pb8=RHXJC;1ZT3m}^rB%TvPgOk>UO zV9Uv6NlG~N?p(6nSr6UWDvDW?t2o!)nX;HE=dlb63|&zVvVi&D&zxDSasgBJN8tJo& zC8=T=s<_=KvrsFP$Z!?ssKt`cl&}ifLXxOz)<6|YSOs|$pRPzFX8LAWB6fi+#3_P{ z#d1I^a3c4mIOS}+SjOKEdz!u)`Hyy9w@kIEMVWHJZN4F~(BaX!u>5Hc7}2_a7F{Kawbq5U&Q z;SQod#r_z)!Adyn{Q;d6uW;Sl!0;0GXR`k~*@SqP{WV~TyM{5avOkk4_ptxC>;ODH-(mL_`%jV&iK&%Y&KPJB@JwGTd^8gX zb#dx%(r6fO@c#<`F2;Pw@Mny_mnd=i!7=zxGWA7;w2{A_bS<7cw}Hv6CPli2g@ zKga%0Ouw7`XW4%N_`qFPC;5waZoHGMus=yfPJ-+zG>i7kgufiN9H(fwkb{7q?~}G0 z-#fk^d_Vh+`i{5ZbjXJ%JKt%>hXOh-;tBS>4hNf~pYM6!*XPFLkYm1+Ow!K7c~a)j zcffbNEp8iR5)gnK=s({noIz#&eDC`XwZ$em{XdqumG5AGEbnpOcfJn*8&vo%!$#jh zKYiO3DSYkw&3}(O)<>IMZIDS5ps~Q5QF!~<0kQ_f@$+GNzjUPY@7g%%GzK^1nL`XM zwwK7a-}e!;@_|1s-?u2&ZQzBnYy0`oI{1Es!Wvhqi=%lp}=x3Qc@@ zcJ2&{<8(ex+Ygj1xe&;XCO&rr-NZ-3(3|ov!HCaR#b*fx@Ym_k z2bn+A@FV^kq!eHSfv}&V{q_75?b=w>U&s=`wLORb)m_jZP)Gj({(nSM-8Fh4j6KX> z4~&p-g4H=^Cut}Fbe+_nKvRcMbGY2d*KIN|pN5XPy!{qyrf^>1e0Au&gg~Cn@tnBl z(G_a*LBE8M-sqdoO+lyeQ~JaGrHJxGT{pBw?bQK4Oj7yV3Ro;!GJo8*Yak9X{N5IF ze#m!>>cKg=^XuD4fou#^H!zR3azk(A|8Y$-tPuS^wIqLFlYYGa>gRCo6*=_tKnsUo z2c5>(O87tQf4|lB<&TSNo5aRgMGp%sV;%M#@y9Y!qPRw?gh2@#Iph5L+voF?1Lr@` z<^wG1+;sKS{=io_jY{HAvWMUrCZe&UJr@56^e}e3-;s)cG~PSN#wkAZ4cb_om{x#u zg$nVH$Ej}JK-ClfWSsEV2YcHqMHSlE8qrs{vD1GFwt!E=KTFKOKbud1&JhpbpDXCZ zo(_1Y<1w5d_B{T181?VL8EdcOpO2CFn@H_#{5_b}{|IS*g1=Y%3-5#!3i@6X&TJAt zptc{vzXzTrjzS9hGD9zkuQrJ>_-<2@7>8%DWPIf&9Ve4bpmWJ^!c_%MAzQ#FPUAdx zIRTD;0sSP@ZF(VI7e_Ped!6`!@;+n#0H$&^#Hzw^EgiC z`a4eH!U;q8*2I@$Cr-us25)K5_n%+IX-{ME6r)55Yu3V=wXkL_tXT_d)&ecLp!2@? zhp?u?SUVxm%Y(p>Ucy-~5v-RW)=LoUC5ZJB#Ci#0y#%pdf}oc#P-gq_kAil-LCJj! z9obk%Hr7!L>qtUJF(MdGr?GhMB26jQ5uJTsjx!RwK~wRpsbJQToplt(A}pvAMsz% zpp`Y4iW9{S;!$yo8iVrePv4sY)g$A*O2C;<(v4sY) zg$BVwYj9RnE&dT)29aC_QEaWzY^^ayd62cn8l|!sHXFxf6VGL1=d!VK*;u)3tXwu$ zE*mSCjg`yB%4Or=vPs~wN#wE#=CTRqvPm+^hDycBrINz-8^&crUkNG4S&~#XE-sr; zw&FA{Avc#$I+suemrz){5~5PcCB_Tq?O-Djm2~I&!JlxKui^U59Xa zB%>sjQXeYHwRiA;S-S;axLu`<)B4~WBGFnXPS1J|XLN1B8DdM+6zv_Hj@AdKt#!b6 zSx(?gxc%}&y!Z98+=iLwt$8b{KUim5Hr4!=#bU5Q|I68yPICD)k;Jh_D zZEX&puhya#;~P$Gr>D_LXgISBXOhv0WP8-Butx{_G4c-}=U@1aNvH3)+>4VqFF}u} zfH@^U#fU*uNxt1G+4rGxq5n|0`xIJ)mwkPGZRn=1tftiZyZP0Hpu&32U;5iAWo=(PTr;Wao2=%EJ-(gVr z5ORq~SIa~^R(`|ORcIA#$a5=|4$j&ORg)ogqwg4VY{B?mDhp&}ZRt4ZC6x7ckLZCE zQm}e83Or|m=M?a)1kb^0wdjRauyKez7SEg#-?qc^PC%@Fh&30nmLnFv4u&%TufqLm z(STSB(bADLlOTyM(a(_RE9Brc$Ttx=y$E`ltKqr$)k0{HjNE7L(yM35KGgkyz)o8%5(rA?8#aLZ-qW=>W zn$?<_k&|0g8dp-1;B_R#IqdO_(X`U^1Uc|aYhskcA+=Upa&(l_ddiX-8fSCg zIYxCm^L2D=Qk*SBO^rdL8GYuy&`?KmT$D;`T`*}9EP~V07FlypvO?IjNX$Kkn)qu? zd@M@lIzQf0Y~XV>#vAeWaQHlRvF4ALi?>2)9?hoHneofci60^E!kdYPJeK+Aggfx< z!X;*SpL4?F#Z$E`({SQ5Mz{WQn`y+e zX7+RAjBe5rAMuUsP?iu86@u@&M>$-c)>&z=?pF(ovI;XDHg}lnqLHCH;=|B} z>4v^0*cKXV3G0N-h~c3&dt8*;E@ARCj@NB|3|ggRj6u7hj%DOPWR`}-$@P{L7v*GT zkeCVoL;b5t^`@nHQ+3~-oEm3ONlE>Wb#hgzC&lGSP4&1^JgGgC;t~>*9C1lk{i&L2 ziGt=6&6?`B;ET1(&5|O^p}F`V-)U_m+N&ZV78TlJ5~R_LNaQn})t;GNYR}b6ATtx7 zAB*xUPDtNzIySC#U6dm_RfWmFhbBhI-uBc~d+S%BAx8)}8X6)~7_-%pn$Y^{akm^4 zdgerYqCF-?jgJUPvD=*?U&YWR;p8&*w)DlaVQ66|Lt-b! zFa;T;*RHVjchQ)-~lX~=W_ggWgytw}I3q0f1En)d_QI?RX z*zB5#<-O~x(xan8f}-qsVUh9So(Wfux?tr+#fi`uOTijLkHD=Hb{v^_YVCAy|n%>ee3U?sF%Z; zjkWV@3di^D7;@(4P?8x|5!6{*YRQ3B2#f`f;@dx@gBWcT=T(CskO8t$xK*8ew?vPn zHgG>cSn3BZW$IzRvH)ru_Pkuln)w4RzIlPv=*`|`>S4Z%+u_}`UrJBavRZ97WX00~ z&gS*wZ&aW9^8h^OL4A+@maL2{KN@<>!nnd;3)?E>S#=XtV3i!lHSaytLBWvCCi57$ zAL>=xsn*5p4^45P4t^J`S3a_GLo?BA16^4&k$CG&8t z-%yV_3^o>R(vvM+!Tj3CBU{qrMT-?|OT=%nw%fqx;!6s^vkk5hqnYnK@b$-|9v35E zga^itw#akhN5~{`lV4uT>T|*!;sJT98J>7fcsx#{rZMsWDho4T)Z;e#%Mf_#arOL! ziMauIsv9~!9N%c*GKcM;OieuJ!=JuJJ+V$7ffI|{^iMspj)%P56YKh?o><2xidg~a zQcp~H^u!anR7iSDvVqMZEa?_yyv3ik#dTIFNw9^i5WP5AVV{6L+~4N@C#yvBcXoe% zC;IZjZca<Wcn6&6Am6%hy|i6nkt+%$Km?7#D2V z>99M3tO@QoyFDyS@6U~zg;Js3PuC8WqQ6FkATMag<{QmXKo-nSFDBl4WnwBM9PUU> zb+mqFv!bg?b({!x`DKVp3H>5G#U7Uu+UNxv-LUu%Wa=b0dn$jn$UrSY0OTWy)$-&Vp zNr;E-snMZb=7g3`zI@Q2%O{sQ?6!n3dzwXyjH+4Kzu%&on8@TEktq%b-h18`?Xaa} zpoCdEw%)#6YhrQ!H`QVtPqkRb$B0*qo&vey+NR^9F;hdMiacL$%B4x4f^Q>`zI8ri z?K*w9ObEb}HR^af=7I3NR&G6L4^srjTJ+QeCB*Op4fPOIA}9f)zmVy+^sD02WSTu8 z)g}Lxn39@kccrxs#tHqc=(STFXEu{ss2K}{TR)tHF&)O8I2S`trz3q!VuY@BqtLV4 zrF3?>_Vk9>q@-9iGcGYP?##jXG`Bq=1tKIs1l+VEB9)F_*th?Oti7Bpx__Vt2lT$;} zqazYSC$i?}lz5^d>?u)U&gAs+4EZ=ATQBH2wWpCsnZdow z2;{-4MRr5mMyqu)m#vbpcpCI=BdeqpX&J}8v`!uKCfYEX<(ILnC!IRl_b%zlcQ^2f zCKs!g@Dx&{H5r-8?Xu1zY^NWaVJSoz5H?2xmfOK@;uUCo2yoA9WW{=%aD3DTSnP3n zp36+6wbq2SX%5UX;JJy`bFhn*T5Fv;aXazAnv(3Q zQSH#N_D939N#>}p6tm&mY1}fJtd*Xba@z{U|CnSxB~9v5mFg)?jwnb=OmKxp<(4Nz zMnuO(MA=h4{zoB?Q_bW;x-&E`D7*uP0g+Ms>_bv~04ch0Td7MCzZnQo0GpxRL8&ao zf3T5XyEnK(+|iK}E?Ge6+OIXK8W5?nK zYArBV)r@`VA(%r>r}i~hE9LnP4Hg`4!+!b_X|(24p{-U09sGVlYj{?4yfdnGm{rb= zZM_i9M`A(>KG+f`D+aed86B6HqISAtVxwB8EtCy$XDR|45lAx(Z5>iY=}coOQ?SZH znxUQ-!&As28Yz9N`2<3V=gPf2;Cj@7R$v8@xs;I{2HvWEYt*s51F$wN1$%K#-fd~Bdg&EI znajL(^iF}DZjA4tzERI(Y)Yx-%CRIPW<3um?CRt2iv!|WH8+fh=46Gcfo`IC%u3c< znyo+0+k&jtgurJ>!=i^f?8!1RECgC={lyZx&MLD_{n;JKGQ$<=3||zK7LgpjNU1%c zsg5>v`b*<$%!VRm^dbpyumx(@{QU<#r{`L8U@(46QOOnIPFri9)S{E((^3)=Q{A80 z9N}RJw!MkT5GGAM7arESOv%d<(1<_CE!-(v>pw#xu=zhk4(M>^2g0e*c9*V)c?#*H z6k13>no}0ws$0o*&;&mL`Q^!#0b)SR1ZjgB)WrCtR7YrnwY4$98E;Q=*~5~o@~=_Y z#F`v`=6*+Nuua(`&YX!$PL5OK6H-D#&K!@Fr{a_K^p9K68=#yPQu?r^&&~3_j%|tY zn7PU@$CKA_+p6PZ@Es6+++Kz+oRBp!9#25qjB@TBh=)D>8o;00N}W%*3^M5FwWD_u z9Fkh~^-@Zl zGr?+2>6oal2vAC(%AWD`&WW??)~wyd)=bipHA72m%_j`0FK#Eb8>4L2F6{X_ZX5Mn z#^XyRT)KE(ycfCYWQ>VRN~z~8O^}6cW@eTz=ZxpGjwHHL?5?aRX>FYro@%#ah#6rI zmTRM2F!lT)dlKX0Q)Fy*^1u{%x;35648xkRursZ}@(?%Rn`(Lw&7zt{Bd)hG-i*Ua zv8bSV5OdV&QnsRY!_5-)ws-u}0sWVbFDMwlwEuvm<{M=hAwx4c!;e-&ksHO>G&|zC9)cw zpJwu7Jmj_G+cvM7;TAJI+{h1wzsBLnFORZyzMNlwJRWoC_y~NXjd}=O4jOgnc!xO5 z&xks{vx!f_cl&mm^fbFq^tl@2+m$Qn+aIqj{QPpXg`Z!Zw($0H(X)8_^yBeOpf)?X z&6GO|Ys0ao+%$^P^O=Z|vZ-$#N9lOdqsfP#di8V|Z{(And-Zan=ic_=l#eYKcjV#8 zm+Dvw_EPj|d-_c<9&9nqr}u4`ss0}-puDF`m8)x+GbO8wBO%fjZVAR@AO>jC@gFND zjo(VL6N8I`Q_}H-8x#~4%)>U7>#wN(Ye0n+Ecw@v{2R=`{eP^z2Y_Tnoj>07Ug!9_ zU%%HmrK38)0EtKvRaDjRRu)y1;?$EiQBkv`_Mh5U-=_zs7YUD3p>`R_^$`Pc(P&Qspc z*Rq%8R?sq8?qV*3CX~14YN0h(9B$0z&s*igFDa+B_g}4Y^6!YbZoW@6f%q%kmneS# z9KA@M@nyY^>)$Wq7|+3dE}XuwoaBp0Z8`qnf$P>3He9e)N^&jcK8N!~`r}2Xb;g`jDa-xd zq-?^ES|^>;(07G172KXXQ_t;x#lXNT`oC%?MJA~oi&kQ>O7v?^xIBhV9~(PkMEOlq zs5Cy29n53~GQP&|VMvN^bz*<0hvdntmp=f4k-1q&A|*$00RxNABs4uva?5q6_{^8v zjt%S@sPF23LYSXd)2AAH(gW#CfAS#zK7uN=s-|QGt11s)$pTgtk)#k`ok}LC#EYRHdBd)! zLzUQ2>2a5^CHnV_wyhoXs;>5WF4Z2jIolMs&0=vDrUxSVkSqAKp1`$^xzS?RRJ8Fk z&_BnNuQ!-?eBIcDK1e;_vRc6XhyV(GaDsY<^&;SL+?K{QB3_?lzJP~lJ@NLBtg0Wy z4wdEl#Ir^HFe0x4e&MS6X+&O0vnoNaNr5`XIKM?$g`8wPP2U8FuvC?epj?PXIhV!2 zHeG9>q)bbMwJjX!b-Ci2;w^3)Y+KXTzHy*u^I&rF{Mm`~v*Cp5bt(g6@y^L?X|A)r zV>CT;fwQe@54qK(E0>RUB=WVC%aQJ$=^WeHRmy1LK+5iq1tOJ1wj<_pCu?(kqp#?p zUT|#rs6rCasN!~1lE@s+qjCf<2{_8#$Xfbzvz(7Qm0M;1xiseDtxk*;L5l?C@BF*z z2fXw=8X2S12G9EI`A7;JNUIV!D5eXzR#k%iy0U)U&{{-&(%eOTH=;wLFGp6@kFJDs zuL#vA&pYZHcnMwjkGRe7C&aA~p;1XfB)UiLugHCNoyd4XrKqM{2{J)%EqHl#F13l{Q|qZt`HJ zSI-4YBbit=8XxLTj-KvRi*YHcs5zTG)*dW`v+Z%8(mvT$TT_Y1JwDBil%Bq|wZ`Sd zr`Y-97o6H1QbKKh#cHu@`9!o3b-Ut~HNE{i`$3naE8GXV?4n+xOGK8LP$miHNK2X1 zAY3!(J=Y3EY?8fh;ib{v1mUY^`k={{w;P@LM)J#?_cS5W+NVCb6d^H=Q~ z=-+pxv%3A&YeUK8!H#uro=}Z|n(|RjPy*7BpHS5=3a|RsD5?PhFJ@XFi zS<+3W10)N`@Q}&iZiR7Ke>ib&>d)NM{!J1>WVDh&EA57Oh{iNM3aT<%GMA)3Vy(>p|cSVSw}upeJ*z$aG1VLxI=9kmagenovy zm57O@5JOuDPg6XF>}ljIT?9KHcufiWoMiUj7fsjoZ|>~ej0>( zUdbb19DXH6LcsmVt`nS->eG4@a0Tm5_;Xjc6EeMxkA(OxSrMYX3-^x-e2!3$Tkxo< zX{%B@6e$w*J+Rc5@gyriz;)9PMWhILJ|b6J@CbX_^bMoGkF@$rS~}pEX)iK=MSDk9 zytiD={m`xVn|LgjdrV(x;qwUhg%Uo?*(@)kQE^M<3r4X ztN<}yBvAxBY@%H#O*nZ|1zbZrt@$u}4R{Gw7~tq9jWeA+L-8%5tZ~l2gugZWvc$!W zdB~WPM7KW6b^O?FXVA%MFBB8B7gzOYxVJq~jfA{LR7<#KhbwXW7IspCBw{V^+87Cx zN{A0NLtCw0ex=n*)pXa2-jm)edLN{?YqP$Y^k#yWkU<1EpJfgCwG7$jCg~bv5=O+j z3fd^K1zcQiN?tXm4oI7|q~*9h=JiHw$0aGNq? z%^ZI~N~2K(prP#D3a@R)ik58%H^@A9hJS*~b~x?uiGdJuCR7{#kk1Qd5e%Vsjqc+p7uhAg0Auj*-ei}U!hNqCD^B&lHpbybPOSn?t5(1jR^Y2vz-e^~xQlk@Vq68B zs9(T6te0o2l#!)(3ink}4-|Tk4JZ^AMr<097fOJ_U`WMa%=LqzFZ&yt z!+!QPSJod^8l$eXTaCJZRR5|X-HFcl8pk#-g_(BzR*yd%)ucUMMNb?bi~0S2mRfB5 zcL*aMz+4XE+lX!wk3>qYR$9e#|cZui+6 zcZ@W?$HiiMf|XpVLyw0VPsz+nSttA}d<1_ZMi4pM#4iM#Mo_>NaBkvz0!|}HaI5Uv zP2(q^q=ksuZo}UgSh|4v(1J%Pf3H=a##_|)Aai5+tHkvLTnBe1?jqVJZXmv@Z6IDC zptMQ^G&oO`3ZJC3AJP=D4nA&(C{QdAS_;Lv`|2#j{~hR+%4*KlIP75ObL(zIPr2Dm z_69<-2@TF#T~UrdZAEHXJf(sR{Z^8*_$qIokJ#^w5qqBFF5o0V1iYD*DBv_U0`4|+ z?{>&c;Msyl2~M^vbdwUYg|pb_(Ugv4NA-ue<-}JqlxXNAHV~f7^KW3y8S!ZLoE}F; z2=s%VnbPd}lasGpQ&~HZNF_d3TWD)rs8#3N+vg=1M2#&(5({#mqdhi%<>tksbMcOD zT|P}qHb%R)_4aM;>e|}ZyR8d778v~om_A26hFy6t_F*;Q-!tG=>9NyYO(>U`RJ$lW zc0qu@^N{HpUV0F{Yra#Gp202=85un*-#yo85$|3U!n+T_iuuP@%Qr5U9uAY=4-$iN zJ~)cgX^EBMM6efvcVe#~?wc7BQ0`sA&xZymezMd}6H zVLFAEQZJfsHG5%$FLL?ql6>a!+ZWT@4>9)&OxG-zUV+^y@LzVC);2Mf_Zy`pw0SvV zi`0`-O|NT~OTRbDtM0iFGs7;J^tM>O>@P}|SxTsIG z_d*N)B;nht|4^&-r%!LRpdV^Mf4hrz7gm;PQZMM&XIf9b7oNh`B2UgIDF%9SYBuC? zD=NCeC)d7$e!nODZv!tOayT(uO4|UOavn1hPRJ>@btN%3?(gd@^kj8g<%*LKJ{Cp4 zL$kO`5ly#Rkx&{?(ps~hvZFOBj~lw4ptvs@@RIa6xD}Viremg!&G)4HTjTS@Ho7II zbLo%rxfq|A=?q%mSjV)G#R|5Orh-+Ca0*6>{(=Eu>nD3eSXAV^sT+lUNOfaZzANMQ zdDXBU+t;`4==8#clifRK+LOWKKk2MTqb0R-y5j8gjc%8szU*-2D$D7mqDRXF1G;MW z+haYMsvd0y3-Ny>w#$7?B$zwUbUt)yQdzEhDxDu%9rX; zXZ3oCs7{Q?NdxExy1cN$K%dG3{y zljqG8XZkYc7`eBIEh=)ldm_{|-_|zYRhw&XpOd0!YkP0sB8)|DIkG1)du7g>g$lsU%BDe%krmRxDc88gsAMaA9|IFI z&;9jDEH`^(P1~ANItI3MJNl$p&$=Eptmz*2_~~<1ea(^C@^o8iO}TxhRGO))3A0%Z zTr_x|_+x#$ec|Z(!2@gCySMhHhfkd-dn~a)ASL_PoObRd!@I7TJJ`9YR$JfMvA(Oe zsgr$Cb_N2PIj)6dmp_0FWqyN~JVSW{$bh7o|BVDqoWlpfu^(>1pEvwS#GB4v0sk}W zVB@?#NrdJ4&mbBjbNHnz;8y8R2Mu{(?vfrhi?5d0BhoAR*B_PsAO017lo(b6^76=A z#Q7C?5dyinOzAH$E;h35iZ$~WfyK5=nCswxG?ua$h0(t7-4dNdH&T;3X^hdy-nLwe*LW^pF8Z{>!ON{UIhnf8e#P z(r-7mFqGeIK#K$wv)R%CW8`Px?=(y4`=2xsJ=O_d(}pHJlB%)HhDmE)MVl+cr4Xto z^90`lk31F_Lyhjt>_>g;lz$hJINt~RMw3)#KVT0&QR4z7bFyS^3b%;mbXVSCN$ zSVhfh6x5vCyhg}MXUb;}I`ncUm_BzBc+>jws6vl0#_5T9I)RB+xUu$N=Pb)cQW-+n z^+9_VcwB<>G()q1Ld9IviHj$@cFnXgDG_9^4>zt?&F$lvu8Hlnu0!iqEN55Ntukkr|ozl;W{Mli}H z&Y;wXneH}C8Cn!sLrs4UITOL)(OY`rd9GGA)|H+nN-^bO`;~|lW-@Fgg6su`!Vb+5Il+CB*g{%QNwpufj72~ za}WVPhFxQV?*J~BTlIfu${MSU>eFf#^&g~jz4)pnIITB=W4)b2XL~_5vL$d!WQ6gV zYt5G-3|r4)bZz4@+dR7=MKh+>%JAbib*q3a(>*GMMh=cPp6BicM{YQq9nOg-NkkBz zcIwq)2-wi+JoH^)gBipq{CVjctQ}a+E$v(Zw@NRZLbacu+VaiK+Oo3rRnu2^?Sm_7 zlZEqnF3+fkALC;HrvUo;pdAfR+~4#8iOzL6Au9R-QVyvjJle@_C;ZVm)&C=xDWa8! zqp*8%KB5UH$I|=>=wlzm>_KCtx-@$N`skMk@81LZET208J@#eH+|qji{WIsy0{YbT z;CyuI46}3IET9kEMlEnC?>Rw>5}=$PMos?=3{TQef-W62eX`a2BJaIbdd3Gyzw{7V z`2gqL;wuk*2VX(1naOlFS$+ogPu_>BblMRMaK0_-KK()B+rvwv?C&Z6iq{pbm)I`T zchLrN3b7A?@WvC)ktS*r@Ba3Eunw1=LO$#_;nm`i`Bp}3Da##FqbS? zp&nC+GG?2RX)=6*Rz^$}KCU`^kfl zq)AIkS_%3ak6!$^a{TxHAI|;IkN7{)!luSk^gZCVo6qPj(|eXKY~i8&-{8SMA|~hM z@W5(CzfJ$f`+dS{{Wp_xVr*KLNjV?iLINw8By3oM>60U0t_yOGcOF6v@Sj(Sp z)yFE~Fw-w66zo;Z+hzP~eCQ0*O4~b4v#aYs|}GK9bL{ z(?RpRHQiCv^y8{;PO1Gqzu+ygHq+!moW zyJ}@SLOd&2Imz5pFKb&`dBJSQ%gsM^^L9!N7M>Ll4ZNu-=x2go8Fn@@k(w(BxfbLZ zl*Z>STfhFY`FZ-CJJ_+lTHVmmv7uUB-@$BKj?Tf=wRm)X{^(+Tu~u8G_iwFMw_;?- zk{FjB!x&s<(AqB`iJI{LGT^YB&-S#SWY3D)L~Fu>K&||;1wUrG#?U$em$w^mSjR`9 zZNQVh$F!5Tc5ua8WF3DSv_+oum2V$fe*1EI`yrg$)b#3)%d z#8p0HMfufB52O4E(`h)trP;$j80D~$*HK(ajK?=y&hvNj`g&qY3Yx1J(5ao$Ys5LMQ@r|!!K|)tx3_Bj~Z>kPTAL*75Tje6qf%!#Qo8~ zUzl!gz9YZ>gi;#KPh%d6@Cyv{RWyoKbi%434U_g|Aq@a#qwrw~dm0k)#7(b1=~!Q@ zuJ7zzU#+e0I9#1gb`M1(_4vBW=jSh9hozUF>L~R`eO^a1C)l=Ptu@XZ7;GOdMeX6L zw!9V(%m>vJPB%rt(x{oDZhDnH7A)H8Ks^`;-}3P<=382N-OOlQ7V61io*Kfw8S!Wnm6W{c=j~3j*~V*&D zMvl`;>xL$ZlRPP1Tn{l2)Wb>w03=t7}8Qi*rvgK;eu@#vM| z{O?3Q7<3zUBiE;H0kv4AhcAOR#eE+*vjLPQppV9hc99){y2Psq3jGMDB11!`vpxYQ zkE*Es)Nf$5o5q$7uBZ+D+(rC!fY&9Dr>Oh1nQY1*Kr3Hv89K=ONncK4cjo8Od)V@? zF2WSiDM41a?w0do3239B1V%!*Esg9-j7`ZQ1-&V6e6~ASPNw^k>8ZX<|0z9*$x+=Y z2i#egI$BiQ$C!2 z(rKwS;UhAsl9;uX^`8Np=1stx^+_KAy!jqhG}^+a2sn`m2?ALbAt#YBZ(>qt}2d zbp>>glp0Q2!~I?CNBq1JkXAZ3YxEFXUqn6C88_{93i2;}S=K zwY?Jhe3qz10bMLZi7s|=y5L`wH?-P*0Cxerk+*$tMSZO1R}xhyShtt0_~M@=TDNmJ z{Sem-AdhK29)$wM6Fubk)B9xP;QQh{D@Y_e<Vz{P*k%6Sr?UyxB|AHvva` zO?{QVzx>{Ec~f5%?KbgPE^q3qqP&UEa(Po<73HL_5?;;nroKw0Ch36|){mI5bF$TY z;_CEPGma%{f2ai&aV!p%Uu8g>k*Wtw+j*pF4jL)Z&IpfGi9A&vs3MA?rRC>-(B-FA zWQu%MV(nSkt9uqwJ)`jh*xI#%zCdTG4y~Grt9I;73yO1B#(Oh^ooV|XTtV{^vLkZr z-<9^HN(n8-GhgT^wPt@Q^Tn9G#|`c&dfXg6jM_YD`J%^PYW0}nR5YXCrn`JhY3wzz zNvbJo?CnN&s0aulNtFbfNNL>ai&<}0Z}FyrF|&N9C+SPrza*JslBy_;cRJX`85Rjz zJ=Vtdq$D9(@^-Vw;Y%8={~WEu$A>5em@uWjme49vGDYjciHtqvDh(gcPL)2cCM4-S z)})f~UKl#toA9Nq?={ClO2qQo(Al0$AZR@&_ByAeCPEgcxv?FGp5X}B4yo}IsUzSI;3S{_ zZo$9ZffKa*BVRCxd+k_v2!fi-dg2Y=z%z_(!M~|<$a6Z2f3I}brY`{O5@1Dsi zO5c2?zPU_1a(JjXm`+FmQ03O0=l6Qt9BG?PPtOd@hBF=E?j3`a@`3VarpMAlMdodC zPQvUr7_Fa+QOD?VEstYN+>qc_smTqgHt{_HZSp;W(oBdpnvrm_E)Ge77W^6Z4V>*k zG*9)34~hEA{E*r-Qr#nJead3Q9Z#!^1F>Sm|ANf0MAhu_t> zmG(=Euh6JHVvO1?eAEP-Bn!czu}u?Y3n-0`sQn<$mEv&dQ^2(ae+GU7x}B#3`cfLY zt;;mSvjMsG!_Qn?VQYs<0&F8n5?T_LsIG-L4;0LfXsnKuHgo^@L|@O*naN2o701 zmJiBt(M=uT-p;y z>65UC-_t5Rx?DQwLFp6Vg_Jcb`tVDm6!W;t+=7zTE_!h6G}eU5r?y%;ww+}B!)yRr z6SYJ=Xuh-DgO%?dUVeA0&<~zif#dvg>8>ZK2grz}Oi7NTd~msR=Wloq`c2n0OQq+G zQh4d|-WC+6x*9!rU_$hZa3q^S%ve)z5u@;pow?j!K!kGYetcaA9 zM3=E1+Hn*nov@nG?d9%@Gf~#6n|kUSN}Y3UBj*-6f-BZXVR7~e#ba0edfaJG6_n+T zEVTZ`40>9l^Z1e0_{z=oQ(MpPKr-QyG&^-#Wafp6Lk(5@DBJjdW+D9WmzWJLt5xf?nbX{oSwr(1c2#GN5GLz8&|CG+TaetM+e8 zq@N*6NA8p!g6C|AxEW5;|3}Wd3I0F}UJ`IbQ08TvxQ6+9P@WL=mPXBYy==Xbs7H|( zvqijTPPf{BjK?bJyUq4(Y#+7Hu^p9*E83kWyr7dmNcZ#cNREieJt$q*Y!BxWHTwZP zd41qX^+i9WO|9?YJG@*nd2!$1tn^=yrxhOkZ2EdNh##&}gNjH1_18g7E6)dhOJyjY zov73Xd|_1$Mf_^m7wfeo%f(!>oMUgSZK`!FlqaSJGag;@{!vkbzG5jd)Hc)GS?%jD zRK~z30_=LW0Xt5Ks{bNIvkmPFot>1L+}XL%zSy46wYTT;?NXtWc5ZZ57b=y7YO$xM znD6R>HU@24>c%~Iu+&9f06&=(-GORA-N|K}xwtsFTs+(m2qFJ891gIr(QnKW4gm5) z3qfATA2^`#8eZph77P7FegWiZBeS3S0F7ih-&6R?PfVa?UE41i8M$Qp_KQbHFWz2T zQz)#d)n@Yf8Rwqc&OUtG?%lT?KKr&kXO3R7ZTlsoqnB*ocF8E&)Ea&=*u4`n`U4%7 z>jcd>HPp?=LrKpbw?CMQzT2TWLh%%0&*8w7Glk<&&$ODwVSUurAbuLaw{xZbgdITO z2twZg13p3HfHGw(vF~wZ>Yw$f_g9tL^gyWA?sEnmYAWE>C+4!#2l}Um|IxK4M%ie3 zQ2BlJ71{cXmJ509v4A(~ja9;ZdnX63o%pDeus}3}JuH2W_Q1b-6>B0to}us5n|qSP ztN{nkw3=+K#C=C++XJpZZ>BKWRvgW!(XdxVdi@vl#`9d)8;wR>_Af^4MfcEYz0;R& zD6WtB-LcnN?249~?C2ayAegGU1Fmlb$r+Ojx`K{-VrKjBfw8sc4r@VY$fah1tqT*K z!1#kF9KcfWhvXA>PYL4_)4;CQ;(_}t>^weLn*(VN8^_UGI7R}VsN>egZ$0#(L{Rmy zw>WXq7oF@#<1w}LC*1P+Ihq~BHAL4|oe5x!OOa2xLB<)JnrCDYU~N8K+Lmx(JGr6- zJHve&+KW?dnVvR(G-{J_GW6a8N9kEt+UKa$`{6XV>-Z;pelUHk=mvWhvz#sMQX{x{UalL2h&}@OZKFaJ&EyJ z_xJ?^YmpI%yubx^dcfB|ogds&ukRV`oT>VBS823_14sII)ypHrH>JAb@vc;DbhI%r z)s^W>Ci^m7Q^bYD?A(L>n7w3y(llU7nkxx$JXi@>R=fi*N%f4JFgx9QXWS8Ab+SCQ zzfa6fa=1iuGc4WHv3_4~>CE%bTGS;yqDHNO^@m2xT1R5!>^0+OPwdWPY9{i9$zpM= zKvKuxj*z+FVUU^!ut&EHe8~g7$j%X*AG6k!I(%`jlro?1oNX)4Rco__(rjlYpUdKZ z&YtakecOA)?}qWN+W2^_YaEnNz!&eueZvMN^n*{f7$vz?ut5J+(XAD$*A`HKf~-kP_w7{VtzYHEXAHk+o;p+bZI_KvY6db?6Pn&rh06Od51Nqc!RoMb#ApwmUt~y zs>gv}9&2PLJ7k=l3D<_L-ix_2`>%RL;3nXNZFbQ^Y4Ng@A|@9jaB z&4vRupY0B}6~q3xAJaysaz4-ckz)r>3g?6`?Kw>;|AH~(xM*ZWum4f&_N%^t)opbo zgVB649*6`SVXqXF6wkYrU{JXs?oBJ5nM8N8ea_+Y`a)g>-Y|DE5=^_?arX@lZ_uj* z_1E}uJwZ=fbr`A%(d%=p9XU{O%6Jm=$~iBziBb{bQ3!w&G^7udN7M1HsbV1$%(qR* zeg273-&kQZk24wda5CZQm@8EmM>1Lvp$FDKFy1>fQ7MjOr&3`plh#9Nj7G=Oah7MN z!J{R3+W@vx3-QR0RVK-7wZ73ClI2&q-s4wQ|1Ek-l8;z4IiO&(Ivk9uUkSuDtIPUF zHzj{94=3H0`P})?Bme`e*M;@ zu0^oBFo|)_;SQtfigCv5hjc8xrcyxi26X0?a2jC?v7?2omTMoE`xF{rU?IlciZKoc z(+Rh(`a>3}e_*0-aJ*6)$#%|`YFkD#-%M-abULi1xwc(ri>8lTZBiQA@)RHi^YXf5jl%250S;PzN}(J%HqKgVs*mnZR*o zrvcKAU(LTtIQ#U^FIMH$llF5WC?W$=UV~Eqs~qU zIDzx~0qMs5>HX64fDq2_10>1b#oA>jAcQl0tAo7O2 z0I6~a;d~p`ApFXb8Rtj%j5W9-=VZ#8bg*~(R5;`tXZxg3!0nX|2X)ouJAPA;(jS}qR1JsAzqJ__)9$6F&CtqDfl@e zH8g+H-mLoO#V79?<`JU|>c0v(t~=4o5`MqLXK5$99REtV!O(==4RlKvjVaELE^-eK zMB54dzAS+o@RHF;O4g+;_1qc7|NS`4-W1K}qm5Umf4r*S6H7bLt}5Ey)`MHPxcpUl z)LR(SV$N5!OHDedaW;`@To%jcW9<6OkN8;f)(K-;%l9ExTBaP2ZJ?Ug#w^G*m_Wiu z#ksxB+S5zlLRUg3bm!vt!2Pe;+NY&R7l9aj4k>pF|2U>z9 z$2tomz|rIM_{#oL$LOKy+Iqr~0ZXJ-J-T8j88UL8!QG?hpKJx5Nz}&){25NwoW5xV z=AkLq*<;2#tMu-j^A4tFSL#4^W$y!gWgN9*RyeOL##Ed0_b5ys31jb z3&x*)B{6Lw$ZY({3D9QMQKHjjK~Q#4LS9D{TJfj#cDKUpxCU;{8MfIS?oG11$!)jU z!p=F#i~_&Mqev*1XI&_Fx;M)5Mh}&{W@VI1e)jr+cN^GSkKN+->XsZcv%DqjcAITI zk|b~Q2B=t)x>4-WEqN5@tRc7CV(*b;Du!n@4N|-w>xS-WXs6p0pzYI*b6{Z^Q>Hin7A0vVYVg^4W4&2}R9kOOZM^H6NzmPZ2~8`$G{Fgd6YXC6CZ=HZ1M&Z*u`ZM%2U``^QEj-@F!z4RG8!#-T{o3;SYekQY1*;$yC=6x21vVVxy z=pFpTH*8qaakW-3jxQSZfx#=m2l2k?S@vDFo!%#zv^;}n^8~zJYT((Jrp4%gqQ2b2 zPI;O7(oMWR&bg=h{ClE(eD4C@Lwt|k$MX^KEbw2(XKBCmnx#)t`_Q;o;IlUn3I*5; z^!xshpZ@VvCP+WD;8WS#`1=+9oJ4=F=HC;~8GP>={=H7%UtF4p4tFQnW3zNzB)%|U zIA@y24w7ljsvhU&bL;i-rDu+;!{qJ+>sQpN>o3I6r^= zrR^TwN4B1dwr54#9Cwkfg(Xkj=)qo6G;Fn5$Ed+=<+g2TFyMCKGlS=Da(*G{)jXoX zSGBv*ptT#H+Ibs3Mf2o^zD)P$!3RE@<%ri2pC}RIMB^k8N}RP4^3)OR#j*9n0Mu zv!mc=hQ&y^2FB8%^K*?qFDF9#sXrX{f5hs%o&LQu6p-5l`Pq*L-9(1eS_G}lh}Ncr z1H~}$d+BC9W14UyAzJt;z5NlJ>vsBAx+4^L@jjzH>9?Xi&~b6;a?t7Tu<|d1X1WC0 zx*W7c&zJLO@6ut=!yoW`1%D3V`OVVr@qCm&Z&^AAy?YqXSDM=Jegw}m{P`;WoPn11 zP1ETp-^ED##WTAW&qdtwOFHW=(wYF**nRBv(w6|g{*V6+`1Q!gq}-AOyPsVn-39oY zz(;KO{R-zNDRw`0g5E{C6*LtR4n&aL$4478L6KaEP37tdr_Q&8qJSRT@o? zVKQazic-9_-;6iFTQ8G%V_uHIwY+z{jQU3x0B9*vWU0}tRT(j5}WK|#eVc6 z02}Iy>^@ixZ|3_?t@>@?Tj{s#KJuGEbEJh0kJcfFJb-mDPE8#kaz0zVyv-<)cg*N8;Fj$9Ad8_hvo7WZ!K}XEu=J2byRtwy= zaZ={Z5q>~W{5@oDONRW`HO^Q}{tf@zHtmc@W$EqhQSC>G{15p*Tzf%#rcdzpR<(u} z(jmWXjWZr=y~W4zD)vPwXN==|jMtlASkK4tD)v1Nr*S0s4R(SDkO}s%R0j3E`Gsw~ z-gVgLOwTvix1l$IrH9$0@Kk4M50xPYdUPTnp>WVplj@s=lVyeSVd2!4+oHGWe!FUm zC9DhH&6~Xuw;o`6z^2&SI_z`aZGO7S32r-F)fC575~`&u#co==D9PrqFA!p^z0x?c zaUHuZ5>UhNa|Up;JJ!rz`DR3ew&9L*)0xm@F-c8NF?VpmvDegvfM`=gCoMQhX9z&Z z5FaMuC!xb%!IXQY0_s9HEE;DtN$*P!#4Qf5e`G#inrSag?da{AwS;jUcPOEn%a)j$ z9;}3Cu0G}TkG^IhyY`aB`lj)uH)M5|Do3`T`IcSfZEwHp$VKnloL{rA@%=eGp~skNDoD#nE}ns3Fpr0B+^W>l|^ zV=~|ezy0%Sf zGs;{oH&w38@2Glx+3@fMr}aB6^3cIE2HR$)3tfvn-39n?ePO318Sg8mCPyNL%?qO% zGN)Z>TJ@!+yO0%f2Sox!R0{$^@F1X${sCvqFnqh{3@uwBNGQnlXLrnZ(48B}>m4q) z!)XuWZe>>_8D78nU~TZy)4rnw;oO$ajrfm$zGFD%cG?p@w^=q@vzfx=n(3XvG8mFVj`R?vg76l^e z0$o7YEg_wI6Ufq_Ny92QeELjZU$Aq_X#1%PP>C6ft2WK;Ee=)ug_PR4sSD>?q{n*P z1LgLq^_ihl5Uz>YY>~-)yfYT>sHuB*Hy+*}DQKzG1!`L~#q_oFmf_S`e`tKTq-AwC zjxUN>y}o3?>Wc@%*{EAxoK(8H3+7sAuDy3{XOPJqMSC!il&sN2AQK9ub+0+N^@(+{Y*N&dE*x9*w&gj@-{JZ6xO@pV84V^wZTvMzrd))2I zs-^KfY{5WQamVd$n=jIDs~@;_-GDKSUo#?U0YtW>BcVXR?+*m9btmX!xyJpzAlvKo{pwdiE1ebF_$9T+%s;g{ zoR$X~&+@0N!dGR2-2gr9Zg>Ys<9X>=w|wtkG%_$K52Kpz?9AsnJJ}7D`SyPi|^MBz4Vqf8wA5W2e@r&Gm=V=EpW{ z>NsuAjPv_5ZhubkOG2D#QDN={EWJm?8V2y6L5kW zPB|pzz{iO&g8FM!;dmuU zxniarPUiPPYEr(>glXeR^Og)j#IYP;&l(;+YkHb8`lj2)ve~gV zoDGp3WA4_aYAf##YhAf^{GX#5HK0XlowhCACEtuwl&sP_p&1e_zg9XRSCEN>;KHm} zY5lM#Y2Gmj8jL}8S@yKlLqqnEG;gVg?1Qu5ic=F%Fy{}U2=khB;9Is0Ufd1 zJNQYJ8wa*^g+lIt&*dwpI_Apdwbfjk*MnifVV3-4lwoXp>>QwO)U%E4`M5a1lFqkG zIgny)>pEp<=+thH$E7Cdd`qqeTrGW4&cWZ8gIDY%K0io7*lMe8J^CAGVRImAVef$c z<4af@ue53JqUX08&*$4THv6|u%sElN(<%2De#Y;5ykR&~oN|W&_@T!m>wnWUS+Vcu zG|ZA@@SHks)ZGEj11l096tYSO24D)&l1B=TakdXML9YK;^tjikF$+d$E z1A%F0H0+-CD-pKAgVeqmXC&-i<5vTLDHnE~Px^h4#;06Se_$4m&Y8elyXoxb?jHUd z$pwSCNTi^u1xdmS$TFJ>1YFSo7_cYcp9a7=tu#&xOv0h>obGmwIR$_Gx0l%vz3my_Do;F#*b;CO($Av&>7BaT0R`k z1p>MAdG2E`{r&^lnxCPccXV#&wM{{D3kTFBX zlMRbg=r5uhG)GmL^cVaEeu@|2IHjCLx=jX8M6j4k!EIa9DICh{c$j34JnoGzbICSB zzn>~^-JA<{cpR!-3#L+9uNIw|pVa)hgj3NyZ~$xJzCQCtPY9Ras`WGHI~U_xB!$FI zt5(mK2IFc-4d=fZ36eq*j2w*y1K}u?kne=UK8M8`3uq~q)6QTscr8}h?1h2?4)3;% zX|`NnrnqiyCOdt`P&njC;NEt-RgdI{bBXEM=|pJ~31c!m#gq+v2S%$(l4b`w-IV-f zwUGz`?RsUdR^sAVL!!{tu>>l%Km>WZR!BhKbSBq~cYCZcZzRmjL%T1yTh}A3+vc_g zb+4!KeVenW>nzGjHaA?)vBpnrlvpb5C$c#HiM%(}llOWUY`KMz?42oZ*!?lHCE;~x zR{VQQ5?-WR={f4T)odn7DefN0V8-DWz%B!tH=_%rso+n-xv3xH*)Y4wFYr#GbOVVe zb(}@K35P-?=zf}w)f(=nfg%Ky*c}`(H~u=hc;&*vm5Yt*IQ*!%tA?I{$I>71^Atu> zquc$OmJ^Qf6r_cF+A=mYXsGF{6e=Pru5fYvej>-dQkkVMtgAG}lkA0;xrvqy(#p5l zXz62L!|P9~B(+p1%=OZ9!0=jNNcYLXpJ(7`tOeZLOmvN0^OeVPtH!=}x)rU|Mo39* zv~C}Hxm)enCy4=ZPUQN__uH8{K9}$jtH9!b-{9n<#z_ypoYiBA?IM2@zB@u}DnCa! zugf&<3~#4Cyh3L~#dQRmEi}Fwd+KF7^Tr@D3|%e-29nQru@cn$KA&IHaAV9}f3b6B zlLcc<_XOIJw^pHO(Kf+PdVAHsj<_NA49Yo=D`UM&FNfttWXFA+lca|a{N}%e+j9{e zYzbEy13|h6Mv{Rgw+jw%c?B;`6H{T$8BYe8YU(-W3=0&h~kY$F8U1MUI zYVl(GLNzvnJBwjBrqzI%M&?bHJ{K);P| zapLQz-wC>f=hsRy98TJPYPYH}cSQMm=!>@SYaxFThJYZxfuu1bam+q_^1FF_A$O?p zbJ*c-H~iK;rsqv}BR|g7!kFWBWwN6qnXFWv=fBA$79%r-mySxRoH=>M#2ogw+sBuA_4$cyo|ws^Fyt??ONr&Rjq)jhjI3bT7pHo_K+@Uphq92+#;xY`&e z`p6Z{*6xgB7dP@{;-Kvw(^@GkZRD+j6jqP&>UQ}PLU0feV)4GrhZwvAgb;4E zKH+0WU-H>Y6vgglpNrVwdv-V0LMAX|6zyPtkk%t}+*n`rLa4w!4Wy@7IQJ)I?@T{- z@31}A#`|23xX+=v*e18zm`a0Qc2+gEK+{B+x$RtMUTn>Mi@ zOViZ;nP54hX|6^N@D*z*iEprvIW%|UQ)tr7*5ke_f70b_+-70>JH?YKWXuew0CZ%`RY5#fmN%a@L@TtAqXoyTH=8*5%gx z?y!d)!l4D8#!W~@<8#MJn!{di$a1~e_IBEqLp$_=!K7(D z&gkBTxol$4vOqwEEBnm3Z?nr6lCc94#INECdmHa@dt4rN*d2DMfn?(!tn4a}OIzQh znYZEVmXOyQHsk9tU&;lv%$i4`jiBHHJEVYgA*UwthOVMZ>UUGi2ajA;wwts!_6GX> zS}2tYgDc$68O4?S_f^<95W^|{(b=`*0k224Xg)O->q_x* zk5`n(dUL!yY30Rrwp&l?enr(&(L_%&lnALlRZl15UGaPH5KvUDkc*b1p=2l<)KlU9 z5#n{G-?8trchM;cXA10alhDiH&kkur<9+a<;d6mlq`RaI4flH89%czDYP9Ql zs-8SQ(u5r5+aM*zp5;FfTQ+<4aMAR5Xo{XyCwDd6CImloNE`Px*EhkjZyVS{d?Dz+t%V%)xGP=?eqHvyVtc<7P>mu zRXWyr;(U@q1-lq`fwdLuI%pKT~pVJ|!eyP$>-y9~T6_n@IkoV`EqMrb>OG_@Pi& z6waSmIi%*(zK`bTI-=eBhl&gJj*Z>5jh+2-xsQ3%`Ji5jMJu)Pj_Eb0mOI-1B@zmE z7mM9tWQzS;p}ajD{+ISj`P4PjJIa;(zxi~xC?&q?TDl4+_B}yVg^}c9Vr43YF=yQQ zfLXgJ(v?id6vdayS{DPS%|t=xk$|Rzl0M7k!0u}u;ZQhl8_2Q;4)1ed|4PyxD~B8D z!~5_hQtkf^J`mXR#?CtUF!-#|PGb5p#-lkzlpkQYG(;H^rK-5jd0b*!r!U&xmFljC z)mPDemz#Z2hk4DW!TqD72kO|3YIa7Ho9QZttE1t;?o`()7fIi!E?&5%cmIxcu{62k zu*uqM-g@)m$lig0y(0r>pHsKs=I;yd_n;c)PX zo;)voPbw_?Uwk^8lGBNh76UIyy!dOt(l7oxoWi)5G0hK1x0y0LCzmo)z{OgOKLfun z9Kp?vxaTCYO#QX_VU=YQYy*9%aj)aYZud`QmZzs|O?y^-POoLd*lp!`%9HXu=?WoA zitki0T37R|9Mb{#{N)w9t4Y4m*+^oMLjA=E9CB;6_oQ~Y96>#P(Cu-m)@$7VVGVlJ zsDz`%L(GSJGNrO5u0%aGtK@Mv{vb(VFZSMwM6 z_9pGv*%K5yd)PwnseJ>d_Vu0G-?yW8!&Fz#g&L2!d>&8ZVYmBy zv-y5m{)yTA6Ywwi8fG`imk~VV#Oiqh9;$Z(-Y;!sUzJY<{22Hpwf_h|Er{S} z$%k9;hj4;5#RvwaQT8EuhTv8@A@oU{gR~CtPHB+6O+HBQf;jz0GF=A_GA!+6XM&!1 z#=;cXtg$+Urn2IRb;cv{C@ui`c{~z}CZjQFryhaJ06)lsi}>IaisCPls6Enl6WwNP z*rdxX9!^B#ae8&T7E$Pi5G5K!eO($beOvlIc@eNaF*!t%LTdmTAF+5WxWPB6cy#*~ zw<---E&vcXc%Zw!<(>6kGX2L zkWWe2?E~)MO)kY%v#N?eWwQ^u@ENbu9hiq705=8O>PSpQ;^KnK?bmQ|Fdl(r%Y#Q>Blt!3JB0)oq`NxV$rVm(AgEIoHx?cT1btkn}Us zXSRtfUs_?fiJ~RmpeKAVqxQQ3iZ8UF&$v~eTd~^v)r_<$GU4&MJnASLaXLLttJg8X zt=p>9#m+{i8gG*V=9~by)@aP7SNe8-tm3f4Ho(ao`B*fMedYmw!0xDwN?kY*L5ui( zlt$z8hl8maBmit4sh{CO1Zc{CCHv_8iJnxdCn0{dcv20<<5GX3D~>at5?wG$5@QkM zFh5@`y;zS;?Ilr%UHYRjiQ!u-&}lHQqD+uL#&X=c%+ zO{sXMI~e9hNGhh5qKG(oJON#-r7i4o>1GoY|83d+KIAH6bNrr4<@c3}w1w6R=A$O{ znVyvX#Lp_^Q$VweNyn~+6X^+RcX41+$w!N2pWEedF&rctu-hGq=JjX}JF*$=(l0zp zwB+|kyiUd8L@u=>=ydDYKIXDn7|sQuaaE;_>|)#pz}E`6#))edpFI)8wya*q|Bj9I zdywSlGnVy6vw6f3atA{mENhp?f5Ib;MK5^XD{)RO(Fm*tVZ4G{V4wwwBPjU&!f4sy z09oixXFeLsJDj?z1Z>XonDm=SEfpjh(REHE=_+W1nZ6~>vkQ=sA~Lp!?8N9nU!fQz zqHKH^zr{21qb?u6s77Su7dxd7rAOO+Dl+ozL7xjqo8d++BGAN4>)5ThvFdEj1^a1} zxzV(BUAd~`a-_m~cI){0{)-$2I;}8`DQE?jorf8W-vl*lc*be z8ul)U%qk-yNLdorQN50g!t)$)&AB)>?0Sb2X0+#=f56}|{s(#ismV6#L&dJ3W6M$zFDz2J@L*sPW8bds<=y7Pj}$9g z?J(}Q0CiDMPs)M*^JO{EM2m;b(q z|Kqc6dXY_IuRmt})tvh`x7nOYIxUX%N$4-OVndiCLts z;?IU`#Ph@a*^qO1e#Cg6OA0(6GZ8++(r)%8SY(Fu2H6^ZIG&ftl7x`7n_`?0{`^L( z0`@}uKiryQm2TuZks%-O{3!oI2Ia8v%=6NXw2R~`Fo4kygX5#tV+J0aSL69{{%r7A zJU?NSb54lo-*P-VL78{Igfb5~V!CT_lqgemucS=Ve}gii71GM0onWg7beTTi4I-=! zMr#(ACtgj2ydIlfv--4nC*6BS2l?5tzZB#NCy=Hg-BnY)i6ECW$Jt)_9E=*4!e7** zim@27?S;?N~Xg^h;Up_2Z5=mwhc*vCyMBq@SA}m0wA|FUU2qvW%5W z`p#xWwOMyKbQiQS-$9F8`nlhOch}lop`a&X?={202o3aUc7=Q$&o$#R%TUIMtD=ow ztb^5%En!#H7W4%ZcH5x)|Lg4hU84x1IDWh9?e6W){{HFx+Qg7VJ-sC6Cm0b)h+r%t zVz9E%MvI{7QYl!8DH70Du(L=Z*b6o(MEnCRVk5yOq!Db_@0;EGl}jwr95>wU?Cj2) zd7t-q?@jzJ22gpnHON~y{K~LzG90170ZI=H@RQ;CB^x-Ub9&b>XBnP5nE7;7Id4rD!!dlbyHO|^)@7iEte3n3BUU+dLI|}geAOD^5wN< zgr0)@HEj(SMLVmeUt(raUC?94bYgu`LFZ0+RT`QvsVEYumzE8{D*BdHs8!t9^ya-ajP{#MYvLMckC9~sH zy#jt$ALVj`(fYu(DK`V1gTdOsL{dkk+;FhsjZ~7lNti>L{ZM!zUG1fu2aU#GP|hJV z{3!l2EY1~v9YZ-;!W_wJo0>W0OmoVy83W~b7LHMGQI6s>4aWd>Q;wz`LpkX_>ZU)G zUUw`49y$m`manK;E(1M_?0N4Z`-E~ikCIxx=3L|-ic?O_i*)6dtSg429J2bvHzUeHxIR8&zuw^am#3kTXP}Wi+I?ux%pQ}9agY^9G_pk6XK&h>pX@`f`=her4}%=mnIy|A19%5$!G*F{Vp>tKOr{Ef9fo-=F|Ikz$gm8u7-h~a zvMeH22={HKcZA33{(ni}WLxB8x!Grm5J#;GPt(tRB=Eb@*a8TQ&^Yp(LX2aJk1Nj= zg~ofu+daQyoC#`o>`0gR?nI2*4u!)cp~=&gAgw!==_0bfK3O#!W=L9SR|4jU8_c&V zqcIE|msxaboa04qSFc$9JXSw5MO!vG!7)Kx;%h;<;!6hebX35qPu46;+%OTn9fgu< ynRaG1aen~M%_w~3l9ZVwxiIwRX8a+tp!dHZtQ9`+Six3i#?k)^?XmdKI;Vst E0R8`9>Hq)$ literal 0 HcmV?d00001 diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png new file mode 100644 index 0000000000000000000000000000000000000000..c5130b6092384e5477ca0c52e856eee862f69431 GIT binary patch literal 18359 zcmX|p2Rzm7`~R`YmXJM8k?e#dG7eFQP&V0nM|O6E;z1EY2q8OroFWNHcIK(j$;#fp z>m1+z|9SO1Pvf}VpZmV9_w|0iuj|v}+gfVWr&v!R5D04Z8!Ea81fekkK|nxG3csT* zV-W=ZBlc2K*CU620?Dmo;MZrpQTM#>xY>I9T6)?b>|EVkY=ph6JZ)@Tz3kn*SBP5W z5eROCx{9Kn-^;&~hEJ{cP5HN03_Yj&l8jwUCxgyiXIQu&|6Yll;E{C)O$-Y!mCER) zSz$j3Oti;yU$rl%YqS5-(LI`Pm*VUg^&{*;obniWf-qzdKq5_FUo2Z*S}uNjF0Jb4 zmHL-*C0v!4-1L2Ye3$CKZ=W6*nAER75Y#sKEq&axLav^K_J?q`ZW_1x%wqEg9Ry-enU2s$@r*A?$+z$u9rLQ!a*TZ13mWe{B) z%Te6q7U}1ch`;)97#mNBX%asT!wYF-Y9u+%M8LOyNA61SC{o5t{{7VASRxA8O zRoO4AZszks?QRQgkHj;ixZs&HoBl-C341E(h}&x>wd+kDm0_R$Vnj31)0c`W(mn0@ z^~ymOR+y6svt)TEW~JDF4d7I&tnA28~kUw<=_ar?hhw+2;8 zhLn;%m=J4=4msAmDjqZ~-VNkT)fkZ=I?qLf<+czUKF&-6FL61E!kC^ae}R!uCD_VOl=d9v&_BgwtdM#n{X>t}Q?RZ&%J_ zB+|x^C9%tHN(6riLWZe2MFQFxdZa8=r0y@}&nI{1-po65nGy~>cfkIYnHz6%cSo)8 zt@FwASzZa4rhiwu@ZbAFX5aNeZ-Lk_fmjQ}-(GxlDrpdQw+62#At7KI%9I&V@5|zD zou@V}S4us%;cqIVb)3?FB}$l=*GN55iJ-=a13gQZ$kkf6;A!6rSBhS+lz3s`R%M*tdnIi$1Dkg>#fuB+yIuv^KnE2V1M8=Yw=L;DP}#7Bv;|GJNh-;~P}xuWFPONT?t zun3m-3vnshKI#zevKH@J@IKj%h z(G*E>yIlGeRv)SES$3qR(pGI}TyGKup^2i18i;gYEpM8VbfNXUmqzD`*mf4vMe?=MIhF1sfJc5)u!4gz@2?wceZJ| zZgyYZu|~FdXz`OHr1+45Y zhK7eTMWNgC)fAJF?S66{vzC;pHuvr3Ii;kf2V7y*)3w3efB_6Bdglq%LN9b`B0pvG;*r)76uR73tc1IR37z@TB$RnU(Wp_g})nz#avf z?l2gKE4c!%2JCq=dPN$9jNU2bkZTS;>$aYc*N8^9*ChD&N!fWfa>V4yTyxm0nUsaE zWOz|Jl4L_sTns>TFLn2amUshOK_j*PjU+(*85xM$mCoh<%3F{m`ug}|3+f$LA`c4% zAm%2A3=f<(_jd5UsawPdc+474jTkg78T1K~E}yOg5XPTVt5#vSjMgg$Ru!EOYinzP zg)F7~fWKOH@LO;zqqUJ*h4HiQekDFK33zbvRW2VvC4G2f;50fl)t=0s$Yjbe`N~8j zdTEJ6Pw*6{LNM{5B-8NERLY-^V;1_}xv({x(H8ypWPh7Wq=CJGCtvq0K!2`)Wk?8Z zs*Ui5Ga>sJ1nX7I;rT)eJ^ilbWg>`!m%1jW>rFUQG)TsrX%uJjMzEF=YkhL&vo3R= zphiqysH&xl8c>$XP^=nLk%i(!ef1ke(QB}IrmiE-gE zh(W3gq-eY`z{X%$3LM%z_HK%&`!L{6and1EiwyBfKyX6oEx95cq%TxHfRdSg`K|42 zA=6Lk_=0k^d~lIFmbW(Y9{p}0L4z5p;$EnWv5btpaKr1gIl_m$yu651n;iI1ibjR- z2ZoXn?E8>odAK_oT3V05!0Lr0v&XpHA|JeLQig4-r214DF*!;06sd&}PIjz`(m=KQ z{cRWXXQBELxz#T#Di5EH$x5$@L=Z!?N@i$6fQh7=iy3O=7?kAc3AX!iyf35dXXB%~ zYvsa$dy)YPOBO(NM0b;LgahmITj%3MminF{DWF>Y@+5(iD#O10`S=6wh+?wnQS?;k zQ0|!9`e)RWpw2^)U--8C1mMoI{)q-kQ8qKo=EcH;#u&`6Y^gvd{Q8uD_%XQ%)Bv|S z!-Ue0B!`ttBsE6V9xrERUXhIWFqsbLdpweZjokE><+zvn=zc}a(o!ZW&5Iw(K3rec z{OpWHNPaj|+cT&bxpyU~h%8wYM@rzJcp&!N2F{go=rHPt+|5d`{kY(rFe}ri@mDJF zOai1LeX|mK7t}JUJW9h<_v0g?7~W~6@;ITM+l*(nmzR`q<^0;leo%xZTs4#FRLa^5 zB^4EK8Xd-V4-=NCNAO^!r7qSIFHmVU|2*s20e7HGV`S{>67Lp-3_tW>eFl;9GFKo1 zvjN4z+cdgh(V5bN1gctbF%%06F%|%bCtZ0pEWNV$i(DWqaN+TXG7p7Ko#9?;cbgAE zk>06AZ_abVOQk?B@MQGPO*QYwiP$;VNSt+ccIM3%omp6*gSA>hqzlZhR075DCh=g;o310c zRmiw99TM8y%+EgSFPPDrtCybjKrKl{&VPe5Fr-q-g`+PVR~c3AGDh+29UcwV(p}Ii zxqO46>k@=@@sO-{bF9EAi0z13_fT%~1H{twKZTMn4P-zeHh!sPT-JgC{0=OwMq82v& z-?{Af2}Oj^;|NMF+tB~6rN*+cy*`j|*PKxL0Hp8Y*;^_)DpDB$I7&r&oqlp*!1ZXz;zV2=a7>kc zGE9sLYCo~!%eqK+Q{VhO_wLFkxin+3O*&Y;5+Y5EDyNtgGJ=~tPGs>MaZGLu z07Q+EGULb5i2%ZhT9)d*$M`^@{OVW(g&)cfvEse)TDo7JX8|dJ_q4iUW?n@i)rTF` zera0a?%XI8`FH^9128118O@DY+LEY=q`NJWR?*xX4u7&(ZE1LZPR7jJ@?ta&HSHBU z#8g_{`U@B5WFA?vP@1F9sY~FKbXATvVK5r=_V;qPO&gB{tr z(<`R!!?X~mH(w8pclpUh6&K&ur%pmnE*$py2M;;=yw;VtHE8{{!4_9Pd5t8ZqoYSh zM?(u7=um5&I2sTG@&`^Yyq&JV0VKlIr=Uq2d=TBV9e2zL7l{3RC!niqgzde=3##SG z#zZ~A$hNobnxHlZXMz?=FvLY2-s1-WIw06e*sNKr<^7 zv@F%6c(U;4?V!OFyK=l<1#(w0L>xT?sNmP^8yPWx9G6r`DBC)(c`k){It4@g}G?btA zdn{D=w4eEQ$q=xpuPfWY+Rj$H(yQT+(%{3?)M;lI7iE2YpzvrhvxcaPc#yPDr$NZb zIp<8TOdHg1FrIw)Q@bK1oDzgtsX@T(FWn+Z@8&2nT25Z_K)^AP@Zm6bYh3(%1n~Xf z5ut&)QYEZXaZjar>bksnTQa{kFip@$Va4b0(QxLH6D7gSVt2{38|6vR3uvqlE}J~q z<~5}~`NYqCd7&N^P(+d(Nk}La5Gp&th^|FTNmg-tVSA zuJVyz4!)`6_&kgS@&a`NQ$eQAKyU+|vZa61uqOh4XCxqhb&Dd5N@)zWX7{H<*nkv4 z1urgct`sJo(Oln}_pgEnq@_)4^L|VgwW;nFLG2-;|=-(sB4FP*+ciaF&V#lB};U z&O#*yKiFx^&b|zsj<#e0*umG!jr}bzK1=U)d~r;Dilb+rfs4Gj#e`X7=>qV%H-9FI z0AloAO${|l2Z1;Y&W_h|JFBgt2`O%^qN7xCFQV+Oj!^21j8vA~L+_kdKHne~6zSlB z-xQzH`LC@0zW-VL6P~+MGW4!7fU8mn)1BM}=-u?s#VD1+F!igmlupS|S&oLRs1_e+ zQQ>F6baQVDw0Zm}yh@YlRBt-sSrYE+uw-`rkHS9gK{?OJ91Nl1#WE;Z2C^ z5-(^vD!)X&6q8M`+SsR=lW89wmU66NB_QrPf*Qeh*bG0^Ev|I^?M&_$38R|n&7(^c_&a{Ja64d4>l2*=Fo>dg4KtjUnY z%-q5Pd5eN#vzc1nz&&qur^rDZJV*!f_>U z*`@p2dWFo>Fd%1@fUu; zx|M8Qc#^ESCMK=|g+$>ozJYY`RZ5prc@P~)BZwThq>IeeQ)wVY>}$5hnC4!#3_z;}z;WgCleBDPvJz|=t4_lf$2BSnDR0^v=DQ})?10ii8 zcAdS8vdncCf7qs9-|pIhySJhZmLZskVHQCXd|$?u5Z6l$)bc6_W_BEAVwsG8B;gCX zEWZvTfH}B-IS#T=1T0y=&VaKpP|+do@}NV}*VfkFF4gkAKtoETLQgaj=iZCcELR}- z1LbK=%X7@i!3_bjy4puv`eO0F+&DB^8L$=nO+XSI>(_qtSIEOqBiF0-!nm)qM9r*_ z4(u=QHLxqu%UQj?hQ@iq@E!j$Kl+vH z#BC&~NM|AOVkXmf%Fz3Q%1w~ylC1Hlv3 zHId;&V(yuWN6)x+pW%0rgj7sIvNQ1ecbdwX-Yn9KDp>q9zwT~IHt9;H_>S|is!RSt7rij2Nwm+3}-Yu}kxazk~cUeoccXoCTY8UO>OR|2lmH@(PGQA$e z1$jdV3+S4}_8Hg6@y-1=AU-z; z7rj}Fi$Sg{rrLla-q@Ft>$m#azyokpE6k$GoP@mZ<;4cVJP8Sm+Zc^g#SN!HXfUD_ z!7pS)fj?j10FQi?hn(`gU>sOS5;6~Pnn<4&y9n#4yBy zKXpxd46=QE`w}jtb8C=27wprcY`y%;TDlUs+CBEb$wL63)9Eng4cJ>)S)EpAP`LJR z(;x3y)+7TLEEa@NqKYPB=i;)IoU5Ld5P!l+Oz^c@P)e#6!CvO_zs+CF*QEf?CY2Fx zxnYq$=z#XgDlkHUF{B=LI_Ui{uoGhiI@t2_TAw$3kl14GCx|w@+#6s@{0KS`mg${E zwRG5Fl{CD2`wgJ@e6pfQT6A$S6s0Ij3mJCEaiIn|f0F5KOXztZ!ESoFqGwWAONS#z z)s^rSvY9v?12oNEGRo3hoSlZ*l-2Y)-46I~Kd#eXcANcvjAPF36#>cB8 z@0Fjihk9f8sq%?H>{{Q|>1dl5R7Os(2OTgwJ1mt#mQGG&`MM7W&Y4o+IIjIUYEor5 zP98wNG^G(CoS%3?)4^vZKwg|+3On(y!`iy+KSZu#|9BgdQ-BqZi>kLb@%$h+U($)v zGX2`?!Lxv;dV)|1MSvT+<4lj3)RAi82!iXO#g1T3*1D8_*zk}y4=uZkH;U}*BQ0^3 zeG*b>q~u1Db&)g*;KeYtqN0L8kq%$)?JqY?Gv&gn@p*aSsI(1s9Y&ze%U~_Ew|yKK zpdZ1>)PX+>-s2F@AMXXDMrLttAAM;8G#}DQfsW=1 z;DmfhP3uDCZ2?Bm3PAS0x^h7&_nTq?^uh7^7z1zsz!qZfQk2SoLR|5+mJa+myk*no zV+ynvESuej9C+hHZje(XAK*qa>t^ynGaQ46s8`cn*lhvz0G9Xb->AT1m3bt*#>S1{Qv$wN_}r zhhtoI3I|o$yFj8pwK+>hR_fGz=q(GR2hJ3n6JW&RoQdWSuPsBL15#AXiE>~Cx)7dl zR`so7wIzbG>}zqqRi+ zR~NhiGB8dN2IeQg{F`D~w+R0!+{tlg^g?$MIs>D%)Ps^D?9~VyYVjJ~!Jq3ZgA%Y0 zyg>646V(6M&s>CkI`15P1aAZ@ADsgO1c1ok2}7d| zdOuWozD1F%q8<2M`bMqnueaqF3#xdq0ojDqm0|CYjQ8@1Y$eAFih<0zeR(KyVpJ8N zKfw0Ml70kz7rFjfo0F5vG*?AsX znK-2f{Njy4NjZ>8Lwpc=+kr{(-O&fSYHmT;47|6EYw($rHB}3Lu)%wpG0VjI`XKRU z_D_Qdhul?9f@UNn#=X?X&}IdL)~VV2)y%nQ^(1PRww-2C( zhrFZahjbzRV}K;#-ae0eLaDa}QvfreRYIFnHKljzZjjvcA^M@iI@16ip4Uj7! z)_*o-0gU&8C<`1cB_8fRqzS^I9$bELamP0Q!ZwG>z^{|;4}IwwD5c9{Z*z2YzAn`c zEMV72*MXG!iDyc}9rNveZ zED%homHik@Picj}0ZSCLQaspm?gpcp+8excut_(20Bi zO_V?^%oD`eR#AXeP0qU)#O4vWbw6+mXI15Ic!c>k z)9CA)`(44i^#dCAQW;-2SwW=h8&mbdrmSl)j%*Ms-Ori8+yiIYKOPa7%-u; z)8-$7qCtS~n1qb7FR9iTk(C9FTK@AP=lGJ9wZwJ~7HqLjiy2RxY@rushTALA^xe6V z5N_ZUGH@=_m?g?@rI`y_0pA@jLt38Pg~;~uBuL5#%MFphZ}T>5%Z&=p>mp|)d2wsJ zgo{qQ2*CXa(AHaaVS5SM1?K_+65&8jif@Irp=!mw4iM9L6X_9F_GUfZ5pRbWEO>H( z9*~@R`pVD>2WJ=z9Co`RqpK#7^AAr>fMv{?33wKWzLkA=q7FYfKxt5AMtFrcP}*ZH z0fz}qj_`H_a3)Ds5WQ@8%SWY7BMAv)0it7~7W`Od({J&3U7c%O1C3pSCn8%2TtCCz%LoXyZf(=PZpAf)@_bS*(+TgIuvx+rBw35~dx{v#VNVL`JH>vU`7%LMvixi*ve&BU%(s(~Z<20KsbCj z^@Q&*!|)3*0ZSHMjAKiYg=lVXD1Ss*|)8#msnP6 z`Kb7r8{@T(y~)v2myVB1w%u0qwvmjLZDgzxoKokSg0)q}1G?v63?gSv1_!@Lb$~W- zWB@Z6QROWXrq8*_Zxj~xYWSFT*-JwF&NNUNiB>*+=kp_R?KnuGtu8BK=d;3(BT;l+ zHfcrj_TfMPL-3zyn?Ucoon)-vN=JYe+#Ou3u7LR+oZ&>wqMA5pD9xSL18jy8f?;Sp z+HwPClHdUx>zHbfmepg*0~J1`9rIi-Yz@>ta$7-ZD~}Ry#)gjg+8lhREnF?)GKy_i z;N_K#HYkY%jlIx7qn-p5Qxw!9j8DOE)?<*YM@O8X8(Admn?J|%KNbz_>0)l-_@b6< zpjena)8ER0`}dU@{c3iwY?`zrE$Qb00n;DKCv?4^Cb{hCEGb{Cy z?OgW>3lwZu{n+*3T%d@xY3J2us=v>utdO&0%~YL_BKYPHj3b(FcbH!j#FTTUtgU4d z9EHtPzVo12?? z4|TH-;$X|1VxfMX)@|22EKvaw?0vCaTaNMerx=8U_OVHMRq&TtWV>;6+jI+M;5K4` zy6^7($adV0Yh$Q+5MR4!OK@)_kRW^WxdQqUpO2iq7;crIDi(qVa_xWa1$mm&0ueL)fafVfE*PvjpA6u*+9W>0b4|ZU;5H*QOfggn-Anyggm#jo zHHLBq0Nt|E`p&nLE4VSijs3s=12cn!)uMf=qeEJwHex8&85jJ{9@v!=xw$Z|g@lwm zu#JloZ59UEcrdPZb-f%KG0?_c;%84{!KZiVf3ucpiatk9sF4r-N;Lppq0{Kur;6D0ZgB2s zfcSyZNmd^9q#FNc#^^iiH-dVQXyWq(QXGAvh~3Oup-qG zIl;37W42YynL1h-EKHChMS8B42Umrul)(~%2^Pat^Bvm`jK@_iPFfwthjL7Ro4Tyd zXcDDS8>YhiDtp%>Y(osD@$`EzWa>>6!E1*xS@87w*+d)O)|?{@oqq@rn=X}E9<RL+jM>Vx*BW{?fqZ(~ zeD4CZKL6{n`LE|!$zF*XCSg*tnjwL zPas64^u#cPM8U?E0fZkWLhpZVu;fL&FWdj-#eTRF6125_v@2?}W|WEE`6#qHefVc= z-s0e{InBTWEi(Ns53>d|dR?wexgG`jKYuo@3-cgemG3B6=-FM4M13aIY*URQcu+;& zTe>fNpdMu{fBFIADFco*bSUh@z_ji_PNyx0--d22XL1Kaj<5<1d^3ge+*uDZ8{e1_ z9wlpc@7y&HK2qP0blcr2R5(0($YQ?vJoCU?T{^MLs(#f^D9tD2B}S$brY!G+=>$v( z#@wdO`@N3OoI>HwAh{co3OBaWy%%pJl6NOAs&6k}wzBW&!}0XNecPoF3ei7Y$9arL z2NOIE%Zue90ix*&2FzF1q`9Uux-bt| z`$=CKBLftwNY7E&{7u@f-w%`=r?;ssf2j!5O;FX)Ab)n_OJ71&a zH@>~w4CgvPWbb~la}{zX0Ez^Uw0t&kw#39X-5M-{b-4&j+^iXSN0YghNE!@ejIf60 zf@+;qtucZ@59mO_D96zMW2 z$)b%WtSl$k*E*6M53`ou_t;i_vaNEy=+SlS{Ardb0{l=F2p;?>i_f_vWPdmlH+WPq zHahZvdP*W-dFXJA`CV052N|n$qXD*8diul4;na#{Kmc3oZbs{4?ZCOyqRj^=wRqRc zt^Lg^qRzT4-azm9IngQRqM=G57_fqo1iqU|$lf~Xv>uz+gNt{kzn*ivl|&5u8R80@ zaPXUUJvgaIVWtIUc%j)bqPH|;^2_d27y{hu0*5{})l*@i&Qg?`gRcZWUpDZUHu>eZ z(~{WSd=1&{UHDEQ7C;L>WK3QEV=TkWK4|v{?8;u{&(&kw?&s8Q7xEe9-?MrebWyW` z8yM?D7|NC7M0*ryzXjK=f4%SEY`O6)$%`4(V6~{%3#0X2IX(Tz;$m_%UDkZgq-)-g zBlxwH#l=dDA9oFwlbgM4vme;<2~{0uzn!&i&9qZ46NrU}EetbALKfC0^b^j!jT+0g zYn5&^V??cEgktBKYGo4U`53C`o8q%_12#kQ5JY9-r8&elBR%9Vd!|3;hM)b=ziVF+d% zta|pES}n>)!d)id1dWYXvc3NPT&I41G$ps+cqu?6%Q=%fcJ9E5pKE(8C_C`SoQx_< z)Vh`5Kk@9zF2j+Gpq01Nc_r@?Xm6F+gRDsa&r(y)Z&`SSSMv4K9#lPCz zS!2!qW`k<-X03Rt{#2X4Khtk_%z$)caj9;)6GkmoJK5Y|JaRT;`T#d`5oir3jWIvQ zhQurEgezPJ*%(enyIQ5NU$x#wa!bvxWg}m7)+>9T9lCI=M@%dm6MI)#sd=>q2i_ztnye;W*ucNGNKBO2VN3Ose)E21NplhMqza6yDkYH3w+T{T@6J3|M zp8m(Jt*wIx*I>9NeVzU*jO+rdW{ks);|I6mrhj^Sdmm=Pv@W!Cgkt7wlimOacc02%$)aYwu*ugF%Y+ zV29yk(-AN};IZMp4y0g$0}yB48G7D~D0frlM9ynUc>|#?6VS6?WGjf3Nx*$uZgTLL z(tE+QB!s359WhwnS;q;#)*pf~A`t7D-{$iSQ{f8EP(otjyZQ@YSi%L^=MTHVU=%pF zxOkTZCq!9B4I547Ll818dswsbw@W(pU+Ze5D2yv(E@kghfj~Z*6Fnj{-ya`YUDOU? zMXaiYY^d%0OKW|i9kMsLQ=h$m1v)S+t%nn>*N(rR?GkT+FQ3sq-q2p7Y+a??`D>>T zw>=d=+q&P_8s}K!kKr@!I;=tuqof&43i_&4`)#v5DI*EN^o1Xu^*;_O{2!>S{v5&C zKw{VP2Cjpmntuq#Kt@J3Iyo8SYV9O7C%%8|E~8R->6*gNzCIf8pE|@|lJSD=hLwf! z!6YQGnbGO#E~s74dTn(?v;(HTsA6ri7bcdZ;6wydPSw`o#^T{4`AD7x_18 z)|Ii}ZN=Cn4&KQDb>LQ~6twtV>)<2r8XGAs#xs_>llfr}C}@EJs$iFvmmyDKoE`WD zFq2R*77xP9av%aYir^iVjq70{vBvt3e|P0IRL7~JC^`?M>)274^!Nm4G@xL z*j)TH^Xx1-8gdC@E7hjAKhKw~Y~Al=;BsHsu+S%jUqfI1aW%zHQec%1>EJLal^Q4~ zApspNS)f@q(;iL+KMx2900>-v_7PHm_Dj}$FIQJGUlNSztv5Zqq|;OB4pDQzLPnvl zs|#j)7w<2_NMrhWI1k(i2`KWF?O7QbV#S&78)t4wn=fJD9T>0|7am-_=<)3jba}0- zu8eMN_=a$>eJjhoEAq6}{LJch>7aSYcXhGV0zUH*n=Oi^IP(Ku`GZ+n44Ugm42LM2 z~tw4xCO6gAVOX1;#PuI0)WD{ z{=3*37O=f+X=g`felW)@{^FLaoO969?T_rjAd%tL`|{ahA% zq%fKDwAU5~I3lbreB@!wW_DXE$$1j_x(@Orm-j!&8E=TFe+!(vRX&1-A?0$1B;?|E z!d-Y10B}dEFsE4X+v3I`hYAW_Tll%Y%VumJZ0fe0fSGCDo(hK!`KAvfPw?4^+WGnZ zm+?#>UAPWr$X4atj@ID?GGGw9F-y+hFJ|=iNyE0Rs7Q$fVkO+BWGO4Fq;~;92J9oC z;6TNj-UbtH{_3r8ZV2ym^9M|%wmCsi5ZC3XqS3~apWIEm@+Mu;l|qQ;iCY(9Jsn>M zJ|Nu1-K)XUgMcF*MB9M`#1D+iB)#74uC&Yh!Y_@INq{|S1tACv2ETl~?{W0#p{Vm-4=zAyv0rb_3>+NvL9Oi#wm~}K95#b zr2EakckK$RG-u-M{*fFp!Ag0C?CvP^iQ#+|XsSYG*@WS>5>}c*%0T{P3SvbWAG1Et zr_dSzUD~x4__@c&l<^b^An2z`Ia)eMu1Myrtxj)O8 zf26H90sU7z!-Q_9A*{>F0r=s(c>keE~kA$_Me9cu) zg9)Jx&?!`;;k)1GoqGkDcgz#pE%q8gK87wjeeHJmjmqdVVT2AbZp3csfyIPW*vv<} z$efAo&yz83f!Eu(0)OrL7$48yJ$-PX#_+wo_Lh|^P=WrVzx^`}B2)fYVZ`91{YvvT zC+9W)2pm46vE{8ah(abEq7~7FtY(?MDSIuERk?bLjq|FtQKtz)R)?yHI{HI?&QWTb zp9$*=I2>!e)_>KjV&m^hl`7io@*kgJo9yOE!}tDMvV*~*hw}wrZKngIoUa8;@QaF) z6M~M1=@@InmS>oNA&ker1}MZ(!z+c*9xr#R`-MU?QH&^11-qogi1m1ael51_z^QDL zcZbE<{OSeR-vCx7BbjxoKy3?$7p@s&gnnkSXVyE6c*un=_UnqIwXqwwFvVFY@2o{PXjdUAZxTd4Q)?$KRH$!+JaOjBEU{ALmLr z6MaTjCT`>x2phrFy;(Wgw0Yo+M&TgLnG2*94t@j%ooeH$;zV&xI)3M`s9h=f!M9fa z8QRvZgOuaKezhIuR?khH3}{=eY_I*!aLXNYp5r>2<$70FM`qDr22*V_>)FS0^8cPC zLX!r3gkI)biK=`}>wQ<(`S0fILU$ciHvw(Q zj&d)2K)$FAgE?*}3<{2wH0aa0m8}idVBH@Yc{u= z%zN2wb>0(V0`&b&nOimXH}JGFY!fi+_*NBtlAUirivUt#e`V~=PiM!Qz|C*WzTJe$ zxAmeA@Kz z<1k({SzA@qG4|h{VcnJeaeK)QsPaGo@^xRBht$Tcp}PLQrM<#`tUy_hgY?UR!(?0y zto`fI4Am(g$q^6?oK`FM?Ud{kPOsS**o67`>m@a`BR+m#(!Sp`R`_!8EirCKdiqHg z+vMIp(MgXJE()7+DAy)Od4FG zF~)t|?f1_-npezg2)D@g3>DO1?rb{@BfmukQXs0OjN+Bbx$!qAG(xc5GQi@l-sEJeM)L;kt3nNZ?FrLb z{@0vOjXuZMrWu%4PntbY{Nre-tM4+FFW7kQHn&W~(+{@J{14buWwtgfvMm2T3Ax2E zk-QoxqScvY-hb)R%B7WkxoPPXcx!W{|3y+=bX1gMU%7tbYmc$EworL*^+(Gc9br>> zHeX1RZO&FzRr#-?F-lMTE%-=C8BaTIS9pAnsIk*LsAjV^G?=!)G-XG;p=>{CXU;p`B^Dozc zS8A@LJPR5f(7W3?3>FglvVALZa@M_Lg_fIz3L3puuC7eG2g5J>JxabBoUfSswi=Em z1m62+h4T%eTXSnm=uLh7C-L!?{aM#!Jb&JZKP7kVOJCNtxQRM9%!kLP>6B*XKE}Ow zpCycnj`sLFDsEEmi3)kcp%BatGt)ony<|d;^3|?u#FmzD)9AW@Z)_7l(PM4McHEPC?Z9OfBq?CK`?2+kGDNZJLOPh&)mW z=;n>{$^K@v-BW2&%SrSYc;6t9)8dBt!?4wDOTSKe>b($gthrhi{5wU%WVr z7*geDc)Xx&wDgra4FmDkMaKl#~>P+(^mD=&49~BEDXvyO?syM1+~}(e*o5 z&gOA3q6W+b9o}aUNB0t9R40nfRh7TN_L7s68{S5AVFqf@!y4rWmUbYHV5TH@fMsi$ zqrKD^^G2^Bn;#g*>^7O0LTl*Qn40Odf)xkw#%ktA8Wi6wQBG`GU_{L*E8Jg zZlDueh2LGYKIAqqFt{zmR8d_WaRXWTr#I8v`bOr<&%cV;vNei|WJ^s%uo5JQ4NitX zsL_}Y19NvCnIG0X=}-QRe&cG077YW21ynLBPl(2vO-&;vKnrI>n2Gbz*{H)Wa+BIu0S%W;$1v&NI=`XIT z6NB&SJ+7(_+&uNtfo)T&oVg9T&z2bdlj~1nN*~&ua(v!DQnH(C!_&G`buYZk9OGlv zRQt)*yZpJSTibdEm6aCq%rlk{}Igu$CxZiVJ%#nCkd5WUYY6t=U|h z`aL9(!XcmgKf2ZNo(^5nG^G0KZeW4UXT57RR$VkfXE$S6#LI3r=w>pAAFh(F7KRwS ztNtPLRrc`rpX()q-%E_7(Jb$uTT+eM3v@fPbqdN>b=DdZ{peeh+KW%|s-cW`)}qc^ z3p64Tv7IO~uT5Q&GV0&qAR*7l+{31cRq2#qd%tjPPPjs4>TfXEFFkG@YX0IZ^k2C2 zwQDfQnM)qc*H4_9?d=UO1V{KCIJwIZOPoEkdq7BLlo3-4KsokzTrE)P2d&P3yC&Wg2FI*uZP9 zwwU8D;Xc}Ibh3qyk59(ukDm6HaKv?Py1-Oz!w>&F?edw}6aKs<8VYQ^-~aF9^O}K5 z)(TGewi{SNw~((gGrE+7g$jr*)aAgHr?rdWWv@>KdMnTTS^AB>13@@Z?sS9LlGUDI zNug<+BKYcKIzrAwwGrpaeP!QLXQq%Qfp-djBPW+KZXloDK>7!{viV8mn|NL^87Wx& zsEk_S`o8+-(H7^9}6SL<={6WS`2n4AGluc48F?8VGF8w|R8a{FKRFCF^VUAJl<-#j`W6P86GLy*01f zb|l?}AJ-BJ-Ip}#)hs#RA#JWbSUi}2>(;IBx3rv8&CR*M{sw0uP@bz*{{f=dGw5K) zqo-JSOKsjVWR&vakp)Y^W!|(Nhl>|4-Y6Vwh}|@@lL~pifbO~@nsd8S{;Z3=1-<#U zE%()%e(VspEzZ%M(l-4hx^3k@5Uuu+r)T0R$GN7;b&)Nist+F})0q8#F3 zn2~igFw9&x+>G}4M#Jhh#aZ9Ml3#>xE2VXnu3zAjyEn0ZX1%;kxO|zqF{4v8~qLpu7ati+-CnkI;=W^l6qbQq1DyEjh~j zjbvufc&M$!kheM0y=Ul8cXD)1SzNLN6}-e*<+Gsi&IW0Ukk5FtilUcmzLZw6!G`*(^3dST2x4AN>Z|DMA1FCl@k?pA`bsl1>x@h`{O}H7+xO5 zT3O)s88udUL)vXBqQE0T4!@&Y{B*2pM!03rVjP&`pFOPq8p(+4;VI-@Lnf z&h|Oo({uZ3b#=*8bwd^9B~X#^kzrt9P^BbAm0@7u{GcZsB0RJN-m9Vz`UB@AETxJF zJ-iT2LZJWgoW(SqRRCtrZibGgFy?juTT^-`V@Fd{J0}Z(^Xc1m0ca)q|5OrjG&OX# z1lWC6wX`*bk%Qi_uzwacHU7-P#KQWSiJ6Cmori_%v!dK*S|--eeoa0Yn9nd$qC%?f zsYfer?jO}~IyTl4$Z~GeVUR>UPvw%Gb8v({s)~Mm`T!>;^aexhLxrDTRsq##y<}&^ z)_NG}*i|W5BD_&l(+3uH$KU%!r+i@=r}+ zK07*cKNcC-Bw!iyWm_%83Pw#~XCR}-(l%(0o}b01Z?c7%H4?S_|7dYQM{Zsp3XMO8}#RMisN%Kf^+RL0SnS3^>d#FpToBpFy!*$azmuCxVH z;~b!!o%Y}LZaREtcoWBAEAv%yfXSBOOPmAoK9-2(z-N{y3=``ti4;Q~TBI)}Pa~3; zB7Y}GWy5j^eG15|5sl_paeg5Cu%B0pr$1*zffwf!kvWVhxR3P>F9YE%yRY;QoOM*L zGyAXo+C~yF0)M-3*VSsS1~)c`lE2|pAwlrf>mWZte_txanRb&_6Y#Uwg{N3Dx@n%S67COgM;LwlamumbMxKexx;66{K(jml)pWG5MIuv8q|v&aGo(?(QEg( zG;_AGDe+cTo|IbWT>nUolwK2TwQ(=9g_NrRr`5DPwA_uhr;;Q2E9lFYFXGTX`^2-A z08F@wYM){U;?Pq>|E1A+&Vt4Lq?djFt>M z^^c;h7cQX$&u0^_hBv1k9v<=Z`YrByYHBi+WMmeR;-q0&nMJdnJV<{z2vMh?D6FUm z_dR?}Y%KfD%}wvq(~}n}ynph~pNdHH2xR7_>6W3NW&FP#PPXQenl5K-`1tsIdGqGY z2o@$LVa{#z|1d3-2BNF1NwQ>=luJNQ&mdkduH_c*M^~-y1Uvn>u&i!YbH5yuz`5z) zn%;v9F0QY=2iMlx?^l}LTs&&@|D|9TZ1zNfgR}E{4Yg}CVPQWUYJaIEXRNXsQAIPU z&qRuoot?kkBqb$V0|MZJ`83i0U5Me^*nyn$tE;Pa@5eg-0TUj$o}9M$^VzE$Q2LAd z`Ti-s*~z(=qT)|xYU%-Qg}Hx;D{MmZnxeeLy8-ceBQc_Zw5*(XMxbHmyfYa?_jq+JoY$rjCxP&K*qsBW$M z+n4)S3pRXRJv|3ExBA2@Nrm#ct(~2q;^Mo71W&kJ1>}3G zu+5AVg1_xQMB_w?`v(Ujc(w{$qMIv17wXJIR9RVBVAbakMUG}&qg^U!@Baw)rPlm& zZ82rk?OFa=p;Y}a1yT_B4?CRq*gH53s85ncrlk=<7ltW;y)|yZ_VR-==?dO2ZmA1r zqjRapyv4Q!ZSP=zA1~qfPv8gcY=>Nn86GwZMWXVQSNksv=34t+x!3vPU}ERfdE5*X z-g{08k)y^+J8b|>jYWTTDgL^X2T494B`KIb10yiH(J{ViR4@AAX1Lh3lBvF4p^37z z4EakB>7UNdH|D#kYZkiYbH;4=%t^AC!I2J7Iz8^~?OEqUV8Gq>BT6g0o4#83QsL#1 zsEb7*+UA(^k{C%C#WNhld`)Gf5jmIdGh|L`0NIDJ-7#QP#50-$|DIX`OwKd%E$FAB3dg+7yTjsu= z%d-;OiLtRNWh1ndNO2PUXr;3k=N*&4q~2ei`3P9FD`Ijj6sypXJNW<*^j=Zb-(7}9yuY#zUN^97J1_Rqx^vTnWRnD(qHxg3yS< z^vlf$V#r}?>{gQN*L+eK+n+GSl&T1Vb=W)ftT!hY;|XQu zZ__z`$QuC6PJLT#bgpShZmFWJADn1Q8QC8qjAsW+k5<})|CkD%8Qz99+Xe$*38 z>TH1!%-8M01mB#lpRNh~ewFdRH#*^QJy{zYy}rEPtkrhUUAuJgnf5W|tA1VWYb_u@m6gTCRE*=hC_5;%C6;dqOfXFCas|p8{UFT9p2g7 z4ru_RKDbzrchfX$t#K?G)e!Q8W?+{ZT3aOn5;;SxT)FAyB9=66KduNq|MD1`!)P>S zL_j{McuM(K{&T%?1f2tY*R9SU`rW3 zy$vl;VG-1vLaG$Jz*K~GLyhwJX~u&vf~V_6s}->hRFPH(;PKUy$O;MbJae$`Jmkx* zt?(*z+tEtaU;xWBTG8)KmLQO}d-)gPqe33#H@kOJCTd;esysf*nr{EWIWAN#WSPOg zUB4`4rqXt^aqvDG`-W*%5G_i{Wqd|rk+grf6gF=K!J08QXSkkvY&wT^_ znB(8V*Ocsr%{oWT&_;iEUzg*Tk3x3a>}1cOAzFJ3`+cok{%NK`(Re;umSoZQ;5P%6 z6tZ3K;rO#Z$$gnYQd}&C-Uqdm>EF7{Bw@Hlp>nQd4bs3>!bPz8V1r?Ub=O4s`w@7b zQJ~z4ikpwQ)%&E*n$9uhfw@1|#phg~%0E7c|X`MH#YFG|I)!3wci=WdujF} zz=BQ)zdNc_f~kP#X(*vvp}4cw=+jqce8NGsyc(iC8MVxkR(Qw`S{-q-HNUzyE^2d4 zrkUvH(Y&SQu&E8Rh^;(ooS{$Oa6}<}wNx6z=;oUtp>yj>E!cX3PhnqNl&ie)*KWL+ z&e@X_Zo1s>$@0s{HcreHlplOmNEY=Qb)eieJGAK9Sg~SLz`8Ek>&trQ;jC}~z7kHc zPZ90^raQBAd-!{%j`W%oZ*avzMsHXuC64kjZF+{XvJ9Wjme%#{w)h0Bt6VITd9DKJL|+=!W?E#u#Vam+*rSzNffJMh;ryf+ zJI|BHjf!6b**Zol&_=hKVFX_srhs(xfn{*60|<9nIx5;VHeJ;uwqNA8`>Q|i;U(?KVUH#?0u0_GC1d*N#NMId>Q zJ`0W;1H+hejMnqgM4hb;&wGPG)b`;)sg1!p`cx+0HdXRL?nRZ~;^bi#-*$D9XvB^Y z%QP_z(z=e*ei2GmF{?vh+uLUY#8Iu3n{Vkx?|0N2%`$aOBYuL*%?cfc`dosP64(m8W3R+vwEk!9(=Y0BF+TaeMxVLUAP(|c@IJ6dD79&YNO z3aE6@l{lEQ{no0D>TWqpeNs!TzJ$i4aZ)$at7{SK=mM2d4`j1yGD`=7Vi2=!o*T-- zJ>f&8_5kcipe?>?jfC-2Fe<@V?0FV!chuPaVjaD0#&PE!JLj8ETYB3d;orZQDAIePiXt=PDXH4WuYJ`wFbBIHj8wc{c6!I5#y4oGYP-+Q&ZVjQResslso6&gpM$C|bttlM2Qf&b z`0h~<@}l+fmhFzsJ3|2|vHk({gK-UiXz{R+K?h2YWd{cSwvFOx%OJ6^v;Y)013~>QmQZx(AH!IC7byHF$Td`)1@gyFY zec!`(ix|U0K1a!3@_L?7da|jvQ3d=#&~DGGDN=%qJjX455%8^jZA2X?Y=^h3wRl6$ zn|)ab|6cslc;<}d$Qb8!dGZ~4vCq<Ejw3Sas5isHm58jHCTq17TFz zuj355-x`}ND$D1^+AMoT`6vV`XM3AZD+YG6`mB4F3r%}Ap}Wy?cdQ)SEMo-A#Qs`X z;%HPWQ^$>oCDFtV1tvWi0pwe+kQtqABq6SE3ztinRe-lfiyoItw46iwgNvS8=3-?w zoUAOfNi{bJYPE%tL-Vdb0(oq58RYqY-FsE*F*;SON$GhD9~QA6pmNZyb@E>0D?KFL zIJc`4;KVF6JJ$AsCTly1@K}rJg^~$M@NUrriJL(ROoeWZs;7>Px-kqT#uqw=%IRJ= z*aX$MQ6A!G3MU;X#nxxX%jgBW*pl%Fzvmer8LyQYzHyHkT-IH9LMZOo4tuJD<@mOW zMH(xE8m$4^^a9q0q{IMt|2I&hxB66zAOw#NeLZ#NO)_gZX|p3Qnv5|lDiP1e4kMO7 zeOz=9rxS=z4W0oLD}CtaFBgFcKl11c4`BL?i#5G@tj&r0Kxi28Ia${Q3jJojdzJa_ zOB|}zQ<;4vqzAI@#V13xL zX`5hq#B*|3s%Ys+Sq{6VkeGnIPb)IaQPQ^c`h1`zqUFeOr z215V)=FNzUQ4ax@-c_ytdGkhxMQSwlvy3L*HDPVS0DbE{*z+Z=hRsb8Fi9?57S)Ob z`j(|ae0aD7>rT1Mz5wO*sCMhrJ9eXW%2>QdDc0 zuE&A)5vp^>{*zaq+>!_%j~`FVkac`GUjwoA?%6FnmUcbGd*Ce|Uk$~J4h#7BT-_|0 z_K*~r)<1p-n}FTQSmI9d}aTiPBm+;l-}$-M;C6^o!hL%OT(JZkPwX9_4-)IkK_{3<#Dc}~%`ZUjhkTxmlbDW4&SWMQmW7@9 znTBUC-Y4s#7v?yt83dM{UfZjC_9?^$$17Y;+4!^aYmVyOn)M@F4e5ql>jYVIZtvL; z?SaUh3_7E$6^@9-bVH6E0jzyn3Oj-XbFM#4sB`=C3mHwM;L5^RTjvuz`5&kf^NoV( zv#2c=><;WR0J05tH|lbi^mGsIH+|W~T5e_N-rdfLqc&eIwq43f)+8pUuBp0+>dM2& z#MK&7v`X$3%jzbE4k78`YU3Dmq=C~o1nM${TJ%pc)Qjx>Ha zd%%&IzchGHA>p=s9NX{!_y~?AQubLG5L>4ST_zP>z zWcg_&wYju&q@MtxTW*wSrFTa}L#64GRzvGz0yrGOCss3n0RSM_t;zT4TN`sHMOE1> zwBxE_fk`J^L1^1PxU$8IMdzukW;D9z>LiJm)uz~ zWt2pS*H*Mn^Dj^VsSoMEwQ6Q{=1`@fQ^2XkTZHbPSGwI1xU_!ci!zZK%CV7pUr(RV zJkw;+?S*3WAEVG@c4nB|Vf}WE`)5HGz5>(h96ff>@x-PY8=MAxW@R@&Q$$5YoW!{z zAT>;?wOaS|p^jm07ZO*5Bg65nf#chYKVn_HzWctgnz4M(V=>bRQ~96mpDZ>v5?aCHBUHuS8g;1If0U@}6Q;rqWWVXcck}T` ztsC2_Vd!!VtXxJh9{#$E+_a41c`qE&A6Z;XD;D-~;K^T|jDo^(H!7YWOp?$nO~Bhz za6BXK@dNJ26Z@OKEAO~r)5si&pC9gpbYgbsWqxbK&eu8;%^^=AX&OehR`jJA5a%CX za+S*HR}>HNwztdhJAtE$q->JT_^|3BSL(g*GMN&9h7sI?P?^jJW(;sY>gnzMJ+)s^ zU2XYoJQ#|r?{eK=1oZ#1w37hryw!A4L^CXaY$BW$Mx>C38!{&K*5e*dCg~k(1 z8YyLkg@x-FdtFBn|4K1gpwkMhXBJJ~oP%M>_GG7+qCkK9HPk(=G zYU;xC2^pHTG9xN5?Gfy6+QK%Bm}rAH+HC*Q^7Suj4jUU6C+g_PqFss&1Oktak0&gY zmES|Lb0%C#dHK;KMyCg-9<2k|CW6S1TXL@^b!9X(E6~+Y)X$o*6R09*N9(`Ny52__MzUw zU)Va}58eM$1--bDu(g(Rsu_4|H!6bh?*Q-mc0|W3jr%WH(Ebt!@t0hBdOC-`Wf zF+E|)uaf`9a*)12ZI*vV451Y5r1YD%E=jq=e+%M@sLV7TU<|ZPL(ysZI}%ed45pCV z!j-;Umo?~5o#?mFjrA~cn8mvSM&8hKt`4|k4WRb9XN-W6k{i-a9AgK;%B-RQ%M*5 z9@LdaFek-uO(Q6GB%A37-U|=bRC{fF#dM1=;MWxCn)MW;*T zju&r9YH8rMLVkH0TVX05lHTYL+EXMs7585%Uml3UAt<&OvFbm2@^XHZ$rv-TurpLG z*78qwA(+fMOp(3cB28yL-T^lTGr{H=Pwi_Jvx&X+I!9p;Td~X5@)`YkVR{W;-0%Eb zDw5?R`bGDgV1TkqGppwUh%8@6ysI7MK2yS&CKv%jGmFM{ExGqMZ^8p1`Ni0>VNJPG z%r`-hu*^*6$~Hf)qq&Q1Os@M3)F_?`q?nOWV(Y+J`@uxRe%#$)nlClnpZGTz@{8Sr zOZ;cpfJIN@4O5b$P1B7u(=IjU$Xk#ZaUJ9kttqu~K zlDLq5&e|S~cPjdolOu|Wbt)JW6Irl7__g>vtM43ez>)ay==Jhs3dn-*YS)!@I94d#G zfi@}Q5ecq%S&c4GsZF3T2M|7%KCvi}yiP4MD%w6KEhJ{U*x+O%j(gh?2XUX`3Yf|B zhz**58<}1h4Um2PmOJ@RE&vg{!hPV@@!D^jw&>Tt!XjjNTkq?i-)~6h@I|(2nElvX z@8T**r-Xdp_j7-YmpmGoqsF@HYXyHYW{GRX3M zA7Qoq{@Z4#m=}jD&CqT7hE)sN?N=mR%(s#?h->Pdh$Scnj{~K zdo^P{i|j2Ni>V?k*MedQoW&t#+X864LRz=Fi28p&>_!@N3Gtgu_gorPhrxqeRaG^K zGpDJ<$980N_`P+xO5Ht6BaMAIpS0!BZwQpj-u_58PHpXk-TDHDGZ|Xx-uEU4a5VRa z4Qy_R{N|b|*~Gm3RAJQQKJTtf4a{%QTd5wtAy831d5t)mS4C&umh)P)*nSaI$mk1k zT{{yiC(-06WcT52aaY*Flrhaj2*Nm;I_F zHnFwvQK`aW93Nf1sc8~sm>W)FLVubZZzjB^f>|F-pq~|*19cMkG;eF*r4K0>rzc2q?)9y#l zTQ$53FCt+gtj32^v6jUa$?P9|glqWY?s0jtnO!7;awb0Cfaw?G7~VVBGz=W0LK6G> zfXRdYIlPrixlMSNtg!a<29*8~G*J4kRYTT5#i*^`&H2&;Svn0#Sh#1JH$wSuF7=*j zNgp!RDq=vh@=un6+uMi?k6_OYdEEuJ$Y?Nb!L21D4YpV5-x&wsd8DXS2KIq(M1n4y zFN}gfjc?!L^Cd@_p926&8(VO^W59HHb8Fd3izYjX@F1h zkCsOKav@fBi|82#OynD^gw*lrY1lfzmo#<#XwmWetHdsTT>W2Ke}0*C?*cO9ffxL@ zO28ju4oW}i1iB=!!{3iiIE2s4>8qqtTuX$=zlGb(c%=V-t076esz$R3LNWXQpKlP3%?C*k* zEBTJ|u&}WF)r)&gSRfE1Vjx8*be?s_1!Pay5VZR;@V!-{JxyLF9ud^_mj8m9 z<-Fl14~1xFEJo`8RgpwL*D58shZT(Q;j`lJ=?!^29%eWI5>_3idpHAJ*6` zv!8q#PcJkoi_-Dq&08v^EK&U+a!0Lr8Z|2Q65P|)TjH($XoT-~XB$cK=y)svug|BT zqqX+-@30_Ia&q#C94V|QVt$$}8>C-DFEi;7nEt)I8oE8j9#YwZf$XuIf`}3E)9}i6 z+kE+jZ%0>kub=7CWufGAciy&$$h@i#7#lp^I6gi;zr1Ybq<7TN#-#H8Dj6dbI5cMf z8ew%o_dVf2#i^6=fI`kSum^@=IE?AlfwXR1z;(KZ3kZ_WdshWDwb1bJx9g*6ywD6G z@oeh#f&i)~2FtSZ%9YzAX7NgyB$e@v5DC?d5=!wb1>*H09X07ke(r3lVSTqaYi6X9 zlxhDD2AmJ_xsnP02^Bf~dS>b_tND^28DQ9XQ`rdETGk9Ezeq>|SC9_##S`dEQLTW;$ zj+V`*^=0t8T?H~q45=G~nHsJUXM4=*cO!oFh%RAud+d+@7LiTkiSOw6Mx#+CYj4lA z=6y>f5k*M7Hy%J$V`piEXT<627)7L+Q1k1$j|7D$2R4_C;-~XzlJrs4i)z9B8{YSg zwE+_OawB2FdJ3}UclR@BUUjT<$mhRsu)u$CP;OXCfb`PAo(_}`JXnvMovSA=f`O}c zQ6+aZ1cGSRmmTOd8@|MB#adOLyGD)0Z@iZS`nFtSU)ei5e5uFIH_;`iy@2f=%KgNK z2c~AO^kW2|l`lHxuX&=FB`0Ja4mKaTydA?qeaMKMhH{tp&UDA4@pH z2WPljbWd&7DNSr|7UCN`5nT+DhT^?O;c6|Xf}nX8EDoEZeNjX*DIB);_V$@>r>izA zP4T5V^^?tR0I2t7IaBn>xIe1(_MXFbF(*ZZ#d?)WUUF`&A=Quw(=94941kiCKJ*x` zVt2CM>|0u>JdA^`0Fs_P>Oi1n#Buj4T{o%ZR%cjlmZgInG9NwdkmDyHcT9B$|ExmY ze>_CGV0gCr>}X+%FVCgCPRSqdWU~4WLh(J`6Y>kFo0I5^Sk000%&4^c$Sc@88w=9)?8uyVvapi@TdtdLh z(n&;DEiMN&hG)q~%<>90f#Xl8^0_(_kAdPJhvG@pNYXfF)bI5bU?kVKV$UL9zRByur**vfRTRu8j1=lV?~&AI}RcFfapZ2f{6#fCU{ z0xO5OVoi3c@8u7^n;{zzU1n?c_FZ`auz_~`MZ55gswArHK%~zwDjzf0i-wzj%{Ccq z-&87X2N6bMm@wfE%bN$v=aWK(>twFXB~-#p{1!og!DiHd>Tph=hL7;LT{{s`^?ze5 z@MN4&Nr=yG)l;C9&6Fg|cf0Ilv)q6I0)f7glAe0!X_V`;S&aYieT%?i)PuM)oa6-f zehR!)*=qE>=3us_JU>2Rh$!9vdC}w;+L$r2E$Ya`X_u@ha#`VI^@JwiS@*WR>2_;t zO)y{mM;F9jr*Zu-jDiwX^$VNVo&ES~kc}(6Jgr7|)cF3aS_`tmb?F<$MdA|$Cz^fC={uWDh^g{{s-TnQ+ z|A|%uR{0{tNg+dEkUM-4@?zH2^Ce)lIC)XliqQVD&O5bC_xGyF%zDC=zCqBJEo`RR zEKVfz+X~WI6v`?UfxsyZ1Noil{py7WyU;VEC&2j6ILc^YUJb;;0@=#P_yO1IkQuVK z0|h~%*nQp#4Js4CH%O2^fx6YA?gB9^>Xa5=rPejmKUWkbc)FPE%n7*Oyk_YS$$1C# zFH6;NQ2Mf5A0md#dcV-XVt6oZCK9b{UJswRzU0hE`=*=nm zA%lG-a!BA=zQHV3xoUwW%juN!qcA(1UtE9N_c^>Os| zoP{0%niGWHW+FC(7}k>t86PEZZ#JzSQC9&4Db2iJ(_mf>{nEG!hxYV9Xm_p=s?8C_ z3A_?rsfntW!C8H^uOn>yLs`y2A#j35DiO8)aD7;!+ZfZSmwd6Cw?#%y;F0S>4ux^$ zbEV^Pmuo2q&{DGva}~9x5zTu-Fox53!G_&I{A{N!7FgnH?2eb)W^EqLBzo^<<~j0s z8bR#9MFH@_3mCzL$+TQTZJ^?)O9uf(g$QUmAudp2wB9#xJe+d(#x|Y117J;Y9x<2g z>hn|PdSms~A8(0|fJ!_jHh%h*fDXjz79FV&m6Jmz9!V?$Rrv*)zhPn46f&Z~U>rRj zx6uiE9PR^*8tb_-AlN@Qqb>)U6(cGt%4hFZc`#F4?X)XzHkLk;BNclG0UP}1Jdqnq zW*@p(>BV$q$~Bic(4bY=XZcjPWJu){m465uSCicNi8LgHs-G6LDombKO)HS6Ftn&z zq8WT6;n<-vxX#~)oDV1G-HhVp>+KjWNBf5CcPMeUeJJWadV6xJ>DsYe{@^mAQ zgRjaU5qL1-0hp0f?Wh3Kp~{GTP|9On%NT8^i<__z&7Yd62N(#v+!@=gwLOoUL+!~# zCPP^7#~ZQyDSp1ZSVw>Bdffu`6<_W(hF$doG%GH?Sj*rcMcOA0&MXyWSZ|XxG7Ah~a+BM7TU79> z`;d2E?-DKRffwB^(e7n-cwJpXO7+HqO=?c?5EdtcGD&6-pv8Z1%IeOMf#!#2WG;a7 zw{qyW7XB8*!sWnQL2obvd-wS*QkGZBM2XB{uVGWqLFGyT7RC>Q2`lGUFM-`Y+)k^A4>#? z85kPCBy7;B^&U zLco&LvfQPc0fyTu(ebwL2}K$SxIY9aTXtaD6%N?A7qiv&5lcRsGWZ6z!dePk(}gYA z+Q{f6z3Llq$DxCs6B840KjY2B_2A5{W&cvKtx zbms6+jwc`Q&xpBxJe!Hyi}OtT(5v8ms4_W*b(&dqb+<1oy`aSd&sQTW!T0oZdi?0K z$}n$e3X;>7CIR8->bC529l}dluV#vtMsu6Mo!FV#gj?}_I`R)ThkAecjzyYd+Zg;=hOlt!K`i`HbwmE`UELTU90%TN>O41 zvbJT;T>d_kqd!f(5uBed*h|Z{2|{7WJ5iyTkbzgEo=3>|8CFA3`v8@m+=c$?x8AhK zhD3C-F2kL@MChmAq-PI$9;;v1K~e4JFViBprL*KqW9<;r+NW{TdJ&)+LXP%}wd=YY;N>lkdvAO>t;9eN*{ddLSMJqX%Q- zC#%&mV(}mKI4=t9MjfY=%0W&Z>xugc0}tN!=M_mTo_^5%7UN|r)e$7Gc)Mfy5&73P zOLcU5O>uH5oVymQ+|qC}r8;v*HQX2&7{kfz;c2{X%23x3SkBAMjq&kgKt=`$8XDT& z{XIY1@y3RsPhMZ~6Ba}kjAtF9%NhOaQ`Vi$QHHK$aX;EfQq3t>e>91aI1XdnmLStz zcProZC`pM9vu~3*C9C7C6x5)|zaj1@Kh3aNY)TLen(ov(6iegGuQa*j;CPxx9qFnZ zo;QZO<{;>A9hkP3yK~;)nSJ6;YMbGofN^CPuXx#Wh$6X<_1UO>dpD=NQQsdVXZfBXh zbd0I!@1h&aW93tBzc~T)t7B~*{GqRTf*Hq>u-6=~HcN6HnW%)k4nNx^*t>csFNj?R z-w8Y(RRb0DtT?lGa`E2dKVU5}})ID*;V1MEHrm%0A(tv^0UFx5qxWpi+PKZsX*+Bw&>V%RJ2^AFr z#T}L#or3@Tkz~+o3P?#I05ckxE!Elf^z_&tOj8Ssh$IPk^MIbt5m0a#hP}REAHM1- z8f#tKF$y$Kh83ouJACg}cwcmAo2#I~qSNk`QA;jpI@gFoXz)lsCYA%>{Kz0~x2?J# z<;-yUedm!!c+qEYlK<7-YE7Z0i6wA@HOf$!{9*bH!%X6z6Hs)ui?j=x*ER1szt7i; zb=N$0zTW(7(5@eBAF08l`MB#~lwNmd+LWc26&paFHwr0`K%&mRs7tf%^%5P3jha=I zmJ5N9lu}rONQ|6#&2v|EEC}Mlo$y#Uuvfq@L%G$!J0}ZozLFP7V>1E*8%x^DwfhIR z@s>@bVk4M6w>DPqm5l1+A*4sGX7(g)R2ri~>hVvfvbQ|-^9=%n(i*OT%{cYU#-j|* zSw_-euRizOM+4 zf1deGB&Z+ug<=OEMgPjfK7&(#^N*0m108o)sDHSJ7Au&s9=x+<`Z(YIB)5#y-XHge zjP7*oOfjXXzVdn0{GFOYL_cT6y{BB08*2Yil13J!$>@B!OFalT>h&6hb`;?Q#z{x- z8vM9<9sl#riP38jv#5K&2Nn`x^fg;1s%YHeOa7L*3u?-bjiHI)3i7XG7nsEms0k`7=$!r@z&%s7U1xr+%@y;6@eO>z>Tfws| zzYpGsqouf%$9BdR*`@I~pG=3M3lfUy>JkbG3AH?q@Y@m(We8X{s;!&QR?Ap zdrPy37Biop7JW)By+(eU60kv_oYA&I>uB|OGHnpZ-tZ8-eh85$Xs5-$x(gEfu(uIq z0AZ+srgt}9Iuy9oS6Gyy=&gA~K5V>!3y9ATP*^mZ&Z97>x#wtlH2^*LCNdi*vX~;r zQYx5jbOm0Wu4QC^UXm7TtyP*`ZGB$u%^z-0$Nb)*B=UK%{HM}%ffAio)%Huov}V?6 z!v@}J`U_DRll81mOc}#Q|4i=QQa)On-3M=4Y;qrd5S>%*ZK4n#5pkEw$m)iFnK<_7 zLRh5^F18}6}olm#ny$`iWP$Sv=^?T9{bA# zzvzteKxxZh&;Uz-yT=F3OEDgeD5ziVj9_^RwF9 zD^%qe8h`1gn78)mOphZedCcivYDxDiMLw7Kh^+zPQ}-I>&x+5SZ0ca}tug~Dt+Xp6 zs!TTO;m}b$mZkeMLVCG&kS8N(Ai)~B$M*Z`TKX0K3(JfMY~fYcw?%GSV%CE7T zbLWo+WmkB-0eK4Vuym`Cb4ylj#r?BoFpS1T9Vpbp%`0Qzc=H(nfqMiK>s!&~feT!L zaakrmDAUCVCJq-68Pa)xrvc)pJ0d?6U~(64GCeZk%g-)8qJ(9NMeo3+3iMfK|H=-> zu`lX;buV|3f(_#|9haH8H{ZgZruJ5HcA?Y&kJbDaL#uNnE)wh75nv-Y4ovNz8i zb$E#kmviGUPk0>3l|ECByY z9U~)Xnxkt39%~SLv4+#;JjSZeqb&?H%>PUJnlvm^&rp%Rey)K%)<2f4ENB(|tM6>3DO*>D#;7QJHVydPf(SGUzdCSN^ zTH+J?tp^ubtI*(AUumwsqYVJ7<-HICCC}z}HpSX-YRKY#9BM3PbTt7(AJ=A#nkoY7 zXxZq`ILWGg8gLnT<<|4yU6kxfLYTB-yM=7gR=4pROj%@h#>rJF)8NQzd zevbF|$9&l$D$Awq)td>G=QJ%Zz6XB&@_SMN+f~C0^XCHBa)MBJHwCxpl(_~&zrk&}_HLphSDC9H>r{4a*(wCr|Erh(TdNY%D)1y&NDro>iBW|FJ6iq8fcs3R;0 z5?N+LV(N&d>d4Iokz&f`$JKXYy{$eT6Cq$^H~SA+#`iX5gaEJ=($?|oYW=O)QUs3% ze}bAWmi%z2%4CQR{hS}?Tk9wiisvxTsdmrT?@Xb)B9**<;MH{6dNlx_VK{3JNsEwX~~A z$w)J)nT~2ry`Kf#jqwRSyS|*=oEULXG$cgQ!Xvu*9Xy$&OEiVrY__|qdI&k$~JBNC!N#tBBVy{md_vfStMj^#<|Vc-hk zFkQ;S5bIN`gVpWipz_m>vy3hO(AHx5P6H1)OfxLSm2d$@U}yW|-G%GbwA!{M8yj1S z^NQ>7gx9}z*h-4yt65!nRaMnB`xX5g|Bv*#w^VIdIzt^VP4OkOFCo48-9qLSUN3d` zaFb0K?gMkqW1)O}+k3XpyDKmE2t?%c!Fl&E&A|_1bv+pv><3y8;yBCnVq=B8F!Di6 zGuNNL#0Q@H#ocaf{9?N;cz^G#eR?N7@D*EMjQGJ{Nmp%_y%aI(GRS6f?udl=RD8&+ zF~-~a_I;+VH-fQ1KV!^&!&hrwi?OF=^FsRkiI`BjyFHeojXLVFD(~An&9<_ERj$vv zMYU9mOO-by_>bG!kNTGI^!Avod2bPK3cOi7F|#&*nJ$Zo6d~!3!oKA(RBtf*^m4xj zndY%!3xE_Hb&CXg`t zR^I{QEYI!(?xpZ!1y9bnLC4qu|N5YGUQW*KlYqq>W59bHF)y#@ao2lGj?w1kW~bf- z3--IFvetNRU8tya7#S*o^@AbXrKbldD|PNv6GTzb07c!y4&(%x=!V#q2fj#5i0bmT zoX~Nx2{BPdY-yOn>o{1Nq2;5=!-fA5dDc9 zNsou)x~Z9&=#-S9t{}9+q9PI!62Dho0giR^7akE2xbY`NNogfS&5-BYQ#50uc84FH zxf8AIGEq_L;kX;WFeauLMDGnQCU^OF&YNBa_0@Fx9Y@2zA7|@_H!|Z?gry6<1SV8o z1}-*C?t7oP1g~tI^xoJ$M_xK}^4hp+1nKzHBDU-R~m&YuYj~ zY@tqA8$5KKfPKIA+#BzBryi7XYTvlIBEH?^b_2umH2TDLD%188&$}s1+PXQW9p^>P z$bgb9(9d;KBJyTnb2K@?`+Ts^_{`N4aV4pb!gDoC*FaYSjvbca0e!hgHa%=3SKhmG zZtpL#>fuRua~-n}Rh@B-HzkulvN_H#zTq|zTFIwhbgP42`_G$Hxm!PiS|#Bz!)#^h zujD;Ek0IjeZkaBuwfhpc?XSEc7viu!uTHT}D6=rvl+Lo%B{8n&&AB0*ujJ#7-H z8@aEmdwg2xb23N~9b=f{kNCTT%R~edTUfHjWdy|0Zku4zxS+~d z+o9X2E;iMZ(O*nP%oN6iX}@3GcP)kMrWv1vjx+WaHi>>5E2)Vo_VHiG-U?I+ zi?=al9((Hw@FXB-<2a9r*^ub+UL(X`d&Z9h+n1-T$RM=$ zxMz7AiA-pR(`EmTDEhT{*x{9Av94@IBxm7-wl3TjKT0~Y$kMgy%VB*%YtTDK<2|1$ z+FH6HsU+6MJtvQevLLY?^gWUcV7W({Pgih1cyy%kCb#fPUfc$=+`nemDN+6C>>9m66dz=4;~_ zvTT=w?uWagO?)H7ce!4xC>`PkQCU$5Wlh_TF{P#0M!}84r!N6lr)LR#g);bYVh)FI zpC4w+3ZzmyfA|*;rCxvay7^nQ#m|aiT%GI}C&nC*kMa$kB-GPlRaN#d_RW6JsA2eB zKJVk$Jn0g%kAI$B5^CUtKna22i%yTTyV9O_PI$rk2Z)i#gKvm1kU-bR1LVOa+g`lp z+l>%qRn_RYxb8GQcbf*S$Mb}?tyAxpraVFHNWslEc|HN|o4O^>)v8d>`~1*ff3hbQ z33khw3=$rr)SexQ+ESw>TveXN1K%)gua>QsJ5oXtPhdu)ONykv#QI03;4~QTxGw=_ zuP}Z~>P3pJ%O5X2ZH;&KsVlV{+ABs$*qx2YlTbAj{4ws%Ubgv}%g;sa(_re~OpWH<{t!6MVu{gV=IwITEzqYkcTXZmy2kxvk z_k1-!8j`fk%#v%{r4?GHiD2OHU+T>&XE^W^S zl1ev9O8TpZ*#4a>DWWXuAlPoeWBN$@1%-j{61RJ|wXcJ!J((!JBEi;lix=Pf5kk@0 zLE)+$vRQ*aCVwQXyULp6FzKFU2wm3A=v{oA;}j0WBTV&0t$C4d!_k=)IW<4Jn$W{x zT)pyrp!eu5f#v2=)q7?4V*{tsx`nZpRv8e9Iw6)4niQQdI176uyI8%>ilSuNZkWAR z?io4c1_|?AIX(>o8o~VhEV@zm*%AHhDpjC1_~HkPwm90KF-QKP5zYqbgm039ojuqZ=`tM`y!3R+7{&1cz2 zeC3M>)tp29F&p*M1rB6%^ns|bMM_KD2tM9lx;{z+c4Tsufi(=|a*y z69w3G+jXt6gy0+woLe3evCgW@D|Vm46SB)T(lwR71ZH%kjhL$BiA#H%&wp@^4Y zcW>4(s$Jc|7~Rdl5Nm)2)(lu;GKq{r*B_DqA`aMl zI)dBFYP#}%WWI1{KYSARkh_|D7Lzw$|dtC$U+Kd{PIyBheVoHGI1Y`-i1N9>sQj1J z`FIw5W?;Bub3%tOZzHOd(7SMa-q;%>&kvuLx^{=pyST9Az`F?H#qm@A%l1a@^%(v0 z&4lbw8e=qyv38IB1Nqq{jx*DM#rDK%TW45iNQ3Qw4eq>B{e8pBmwjmBJe%p!tlPEg z-rHI-g~lq^+tUbUhvk-`0b6FX8Ouk@zyu$)$AClH)3ePKi;>pnQH!IWpA>*xjg*lQ zc?br>4w!cD7z;dL0qvfxgQBwk{M>T0LPA3mQe0d-Vu)+*>)F~JRE;CvmiqYPl z0tr_fC@c>`X>A$781&db?dJd=+m4x+PWT~{qR)N%Y=e1F+p@>gkOt|v^{2PNbKyk~ z-`ad#xMOaU>t^&@^%B)$RT|0c6JtjNWNhhBV=RvMcrvOd4O$mN&ZJs(zZg~%#m$Ui zlx|&4Ja6CwN*evBNP29iAEK7-Pwcf~;ihp`)@t`PF6H~Tngt%}@7E78@lhYGe~i}n z<5V&C^>Ny$zh9U&pPOHLUaYWaa(>s@t2J=SzrS!d(bgIYg4g@EppNELAsM=8;9m~4unIW?gc_JP+p=jG)o zs?KX+DBX)UDT>8g{M$-M^HY(g8tw_m#H1j~i)|AMxX%uB!R zjb-BH;V>J(0kW89;C}cbUwh==Bc4*C$7OLB^E&yq7bd8BjAWaoM~XB)bEJsT5u-US z=Zd2rNL9HkqaNCLegSW0z2HU~W|?efvmZB>A%$8!xEy=dBzbxeqq@Y@&aMsRm;=MM zACq$(BLru`C$$&Z}kc@#%XCV=klW> zv-Nr#{5~l(iPJl`kC1w~!z>baOf>vNe`#uJYPb&khIiQ2j^pFe+fGpj6Bvvy_r|Mh zYnfgC=>eX0MEZ`OP{A)!)loo{cNo4dQ-CC2z$ zF4jh+r^{<7-@gj&%YsEku8|HFK^w>+d;|%RQ*duR_)D8k zxDO5vT<*3Lg2Kbed3jTMdc-upe${Anpt9c`>Hc1ETk<%$un^p?N65+=J2RttH#spO zY+*rLQc|MM%~tb=gK6S)LQUOspQc4SY7&*Hr!kz#k)I%b4?U_@656qzO_Cfu5+|L06TdRz>1$fR&aG3odGJ83$tG#8_o_&0ByUg36S=JmKq01O5n=sb0` zdOnn^(>7l(n71s83NeuMBnsM`qgEs5)bO_~9?v||b9$Mi!ukjzCx$Szsz1}0620CM zIK8utZr<}g5&&-hd%PG$!@}ynS@o88bYx>_xkPopIWYv)zp}D2GT1?|wPgY%iq{`d zQ2`1Lsix}*#y)Isc%N@Kn=eLqfX9GU1u`#d=g9Su zcE&6W)YQaeWYA?dqV%jDZa|aqbyTvypQaXV1L_zAI3h=_EhOkjwH zhK7JVme%eEdt-+H-4ceW578d9+fR|s3+I980_o}d0bN^3X>wn09Kd=Fy5%N9vr)dE zM-!Q{>xzhh>HgU_$}nR3{d+g} zFVB40WysNV$`SyC%7mf%f4^QRVSnb&H#`&^oIya>zyb&c5m9eV!6bIstTec}V9U2f z`_q3O%YP4%&)BPy+CMauQ(hjKl|{_8(3Eij9u+bUjrq-|GlLD{9QPU5P}duH2Kr6{;#Wj z{S~gUFS_sf#YKcM-+xY#Qy+`NO3PYJ=zq_{CP44#_}%~S?c@-9J*OG}^UiYO1A~L6 zW@bPNE=#HR>Uc5X-#?rC>tpv6GxGl)YLCR9@INo+mUMOHIy*Z{@qWJ3=6(ioGczko z_}720!xv31;b#WVX>$w+pN{);C?sP4rj+CzYfS$0*KTondE`dJNk2z-4=2v5jD|PD zI9B%7HR1nzZ}BD#i2zgBve|t*u9x{8d?u)8-C^rIk>KIm4VY1C-7$~wLi-J(>sFn0 zts<0L#Z8w++H~(YhFMP5&(}M?NWEV3J#dG$E4QcG8g}-|{dwcU+as2LcU$rsm;0fO zH#)){or~K{dVhbQX;}W}tiZuD9@?{j(5q1uR!H~8joQ75NMB;Yt4^0EG7HxAgj1yyJJ`9X6Pel8(D zVTwMTHaf0de8b|7y=`$XyuUYRZ`2C)TJzBoOnrc`XZ;8tIetA~NJ z=6^0pXjb8!P+;q0^fMI|YyEQK&PX?_1ErClPx*Luq3aIT_xCWZx{V)KPrYUbYtaAK z1XK1Mq~R*nX)EjXnSYlj4kr%t3;0OvqkK$3c8Kl!h$|8}G5aQRP-Aj*_sAK&eC<^J zBK|^tiJv-U#<6WTIPm%+MnGVY>?z|p(C~3z{^k4j2=!7tX0-UTzx!#bY*J=Gjkjw+ zo0n48^MGa&JjHlgXZ2v&It7|ixrypu@$TSVm$hM)SXB-o15}L|?~dVFd5V(z)g`tU z2GPeyOZ+sHkhg!UV%nA-X;im(+mktXIR8zjyKbMp7`nSc=J87!=s0jhX!DEGPQLm_ zKD^gtbF&Wb)zvLEHCQZtyI!{>49vEstW|0=94|3DDTJ z_{&UN(;bxkBj9R}>fHe~nr!RURy!~75B-~y<;K|>ZY0Ad z$QpT?)MYCQDypFABDIHHa|?^9VdLwgdD^eF#eQk;!hup^%I6UD{gcy^J5)H|%HWQyMGl;t|x7QV_viz$jMA`>5m)EyjM#~ zOD|Ox2dy4ay9u#thslE`O@Y4eznXFyoNTL0}^=H8)#HXrjR zU4;j}1S7n5e8U{8hy%B_+^LmJ6q$}G6P)oAX1PjMlc5;8JVGN%sQ@JL9#R&`1$ zDxbZ)_%C*bV}8cH$6;4cQRxP$nTmm-ciqNjJ5EjU>(`LZbrZGH3BoBfpCc!pu&}VX zW2bm}?cmA%*wIV}4eNYpgzwq zA--%S38fg0Q_`w-{gsiG9pAkpe&B?Xq2&y1?@`*H#&@r%?Cti8Tz*VJ0p&Xcgo?#u z&Qv+DdL{`DA}cG)%FfOVxU%3Zz&s9UbwHaK>iIU{Uy2eMruw18aQ_ki{+&1koi2J% z34wLMO@cU5r%iehgHDq)y%)${n*&S{#?Tdrc2=!8H7zrw#)<<&M-R!><~D6p^#-;g zbK{lKtOI$KTP-gS_0A~e9aKKaGP*AWbYQJIZ6e)qcNRTbypR5mpu7oBWjB?FX7Pej zpF|(7EzZe)n;m9DVHlL)e?mK3-$IA+BuC~B<=_G%ILAVY0Olr>;s<1lsg>SOUHCIM3EMn47oXy1=taGFR2J+li3&Y^2l z`4rtae4WfdJEV8uhknn<)6ME=v+D&WhgC{xX0E8^FQ+_OI*6;(cyYvPcydkP*aKP8 z`uZf`8FYtW4E9Rvpn>|I7*y3@Pvl=++td3W;2+g2P&NMK@P?YXzhdL#VSaS3>y2dy zh2n60$e--*3Bv*5NDqXGoSYnF6coS_dkaEB<=*$Y#i!6+z=zjiZy5A%g~i3M1=g!ZFDOT@j~9EvN%0it&(wSxszIhK znLvOEzxp}jg4E+Ky}YWJI*rdO1>ss>Q(X;WlOfR;Dps>ht3YMgrsYx|^c<#~k0v>1 zE!M02Z4SWj>>BlWi>nt3KXzS?1WoQ+)SYgGFvLe7?dafB%9d~D<$JNnO@%HeOy5!R3*#6Fx;5OfzQNP@Z59qjuSqeNe4dbw{ zg@lt6GswF@riw{yh{$Id%1FRi%JbbW9y?jJ-WfgL&$dC7yImc8=*HtC$!Bq#Kx2g1 z&LxABgvnF<8|nX{Xj;n5><=xBWUsW68R@vtd6!QuBdkxTSqnE;svL0N*SzD@lo}n4 zI>P^80U90})#6B954mLa_rOf5O($J$mYtvS7>1MB5PpCqpEhn<-PUGp0V?P>xfK5X zta6v3+qRoMckrf)ttu)icG#wEo$LRKBF?p__yTggA{7ug5A3O3_bH(<^Cu-$RN_|e zqMxPq!4{kh@M~bUS@rda9Ua2KAtAO+%OK?I)o236`0s#B2OSnML7(wlIV3mb zQB`x?{w)5%bsuOuxR@Mg%W0Kkw_4g$gZ+2&XQHSsmRvwUw-rCumGWo1;Bn+Z#I909e}G_*s58N1oKnK8 zYk`>W%T2D(!hHdwJ1U)--8`iOPTU=Jn4O`3NnD9xv6Q7k zu&P<6C{2rU+s4Ls5CvXC{_Dws z3ZJEXRLa8LBs?>FL2(3{`?JG*Utb8&9MuEZXq&moP9A9rsTWex9}UjU7ykloBAJ{V zI`)K}A7c^yDePrh^Eb2p$AyJDpT6hphJQWcF588!m&+}&hB%HMwjNhUbB(aNm8oxo zw|8>`?MGKqkFIm@tCqNcs*$WQlN?Uk(CBi;U=iVh zWWZm)Sr#|ZNfh7~e{_M)iRpoIjr zip5Iv?_B8wMPLeG3}?9h`Mi~qfdN+U@%9XqyJ#pVL^nTGweWKZbHy_fxetz|zA%ij zl1a*Z$zuB{MH4a7)%zv?ORWmkhJ>N@T>0CkWqJgQr~9i;U`!r&cC_drTD61Q9aeQ! z-@Ah|@>TvoziDBjwp4S&btJ`yaxa6+Ys<>cUW9*pu*u!HlW~mQ*WW+>QDN$Pn2wI( zBOQE+Jwp)h#FRwIli9jTBRcmv#Ka$^j57vT#loL?psI~bOq^SHoj5?+$49kzg7S?C zqi@;|#bokbdN?_psK^!zJK&5eiuI-YN_oMq^tB>+_~j{-fqjn3=8$Puf@aGmVs)qx)4YZ zs|T8H72()8I9ObEq+qc_iT$Gm!67xwnYBH&uUrF+4y=QiuX)4=nW>oPYL z@)n2ac5e4;i{RkkEqyW~83rUr&nc5IvMcs&=aM%)pi%%uK|HNSN4@Rl4|XPD&FNvQ zy1^o0s$Ss??!8Qz{G;^xdQOlvo**2hEoX2>eM*DEH#Q0mQ_|ApEG;enx_mbqS2X8+ z%gV-P8lQ=-m%|%Zv@7ZA=x_|%s3vho-EtjElTmn>SDJ5E!(`f|(^kOe6YDCWg}P=fZ8z2QR0C_#=*8=)aTebkA!Ih06Ot0x!y@#udj|ZJBibmKYT?qbq`) zi>_T2R`+&fR54}YcUU65%vT*;GB~IdWv#hd{LdQ&VUOFH33t`yDUGUVUnKWKe|zF_ z4C9Hh8hY5wUNvhcbSrf7*Tyw6er5UX(toApy^eg3xiCPdc~3j~=t(kTuCu%Tdp*|U zio1;do&Xxw%#rHcP^9zmD)1XeJwYQ)6sQlkI)#%TDu;~v! zEMddPEPfHT^G9Q;Eq8s=FaEy$IvnDBKL-{oz_ghQ#xHZyCmoDP07ngUVN5n_f^X)_ z_uqG&pS9ap@;y4`@!RhVeFGcb+JC&A6UY)c)1LBK5#ywboU4}{35BGxvfZQ&JDIYr zK}!b|5k3WW3Vr_kllGi4OuMPYlIvKo=jm*Z#gPv*ypmv!35J@2$!ZE!K>E~QnIq?py@~ZsxO1MS=Z*F6(sGyru;!pNyiE zU)!62wZaZj_ohTJWMlHr<7FapHTX;(K>F+fMB2v{J5JmWuRF_Es1U71z78VF!D~?XbjZJJT@o%HL-ys(lHoQ{{Z;i<_Bx$xOz3NxpRn z(?{tnY!mkg%J(P02kh+axBpg4y%qayOsLNh`IaD*5=8ZbJ8QOlv3Y4T-J;@>)Fq!`J$d?kk`}L1Jqv!lMJ6XHi36wh>=+#is&|WL7R~Nr$j9d+POs#S%Po!eW+fte&FE-K$1mNT>Rm;vD(|g2I?B>B+6TBLjAE(PdLeP+= zdJ)RV$dD2f3#0o8!U_4FFxl_OxB^v|37j(^5c97CfuTB?_#>R89vmsF1F7&HmeaCp zXPI%AbQXe%{L3CK-n2F~$oEl4M;5P93q&MGM@Lo;jyjg&i}P~{S=rozf?ptse>la# z!%EgRO^9!22#W$YhMAdJy~Zp~YZYv|WbgrjjeNU5ij13Iq?f=L6*np>DmE^zU+K;C z(=?u68%1~I$S=i%eV^W{@Rnov$y5)&PoCA`U&7n;?&Vz|;N6lYrQpo0tahTr@uU3K z-kvXhF+_w)9`1oe;vL)P$mbJALJ)et%Gc>A{}>q1rz|$y>XWX^70kJg_!_x=>52U_ zCXq_0{Cauhl%1Svj|w3efuP9s$#MuNqbe&azq)=cQKhD#*=`%KOeKo^W>_`PN)Vl$ zm9;g;>vKMZN@u~IZ1hlbKoqB6HUBF#bPGhH$Q!Cfq}%QM)_^6eI$Vyv?-grK1YhgB zzE|{FDyNs7WH{0ns}`kmW?dojgK@nKc}%mv^Vzt|XQe!uZ{WjH>b2;>jr&n6IONk@ zG^I2GF&<~CxO^pXNHv-uC@ji&cb!v@ED?Hafx{z}9v$8B{PX}UXam^0V6t9eD>?*L zqjpPAT(8-32)B}s+R4|kyNj7=yV%*n^QQ5+61Frb3i$juHQ@ZrTe>(>d$?Z0oJDA--9N{?qG{H^7)WPS3X*nZDj^X;{m(2s5=D5dUqrn9CmH0v0gC5kAHW+L z8y7~$##u^ZH^T0TROsCHTp!X-1VO1l&_+UMUfLD_uJ)pNT zaO?(xqn%yjwVbAF%6SvBNnIsw%eha+!Jh=V0THeenFu{KvY2}SBidtRsH|8_N-!{5}yj;bB>Agr>Duuu3ea>rY+a1e9!=K4rZqJ6SN_}m{!w$O(O#GDEmz_;tMawb*t|p zGGY)*I5u03_b0F>CMN-odNP)%EWfYV=6XtrAKAJ7ciXN%3!)m!vc2nJj9Nre9G|Xz zYkZu)<<>d@l2ETAGdO>{XJ#bZl<N2J|UcLU$@a?{{7E| z7htCYgXJ`v)L~#99u$I0R?bb==@Rl#j#~J8Zg0#>n#trDwyXcFyrW!-Qp;LlWxw(= z4~w(320dcEN&1O)PihGd`{+5ETOdAy*gp#d=1*wWpI zYHe4U3@RhdUGVZ5uaIttYFAD&Qqr-dB^eJ7Ui6TkLqjrB($cR31=XTB$HSR*rzTSi zi!KA^{yPxN@3^B%e}{%5HX!niCY5HfK6iF@`knuQ_rI?P+jLI8!Ky=9;Rkuwkq7V0 z%0wN#Ca$1wr!~$kqQj%Q2!Sz0Oau3dj*bR&>I*ax>iZAG1Jbp8n zvm#WluF^Kg`x+Zfr)R5Jd*Z+ZSQ}-}!9BCEnI0N~PtGhNV`D-@i~U6-&3#St#A8o( z#rui=sMa4%^A(}S%oQJNbUZ95EZm3^Ky2X-0`9j^JM1#G4d#UAb!JiQR2C zs{y2OT$@}Ls>qTF93iaJJf6X3bJzNUaZM|J1%Az5n3!6cD)N7&in?$_xm|PU)b!+!wyn+^B^1dMGHe7;<$!?!srj$6hYPU_8R2Ks+T8Xp zT;n>K1LxJH{VQt*zYv_533+{e1*ss8=XAsEWBmqq1l-+RVeT~N?w^EKZnON6kkSy5 zGWb8BtDV=CNxL|jqou8shOfnD1O5P5%W^)L)L54Q0Gb=bHjYv*oI`Vq)6``wvYM{0 zyoAKB9sZU%CXgN~Z?nSXm9(^uZpd|lgR)sM$wuEeVxgKbV6d5y)Kyk0qJITuwHSv~ zooq+m{b5|>P__+R01FG63EGc04O%|{bp})nl+@IqV^&gBBtBK^fl=I{aHV+-gz@n^ zv*hsoA`)gXQ*u|)a|h4khvHW+ZaQk35*0td$69Vz%95i2g9W!3uq671hp$*PNl_%` z`PrzbsE)zofcUv^D0X~?N~sgUTta$XpbskH`e5ptQ6HG;4;GEqQ>t6W+TCKE^XM6_ZHYc!D!_#F;Vcr`@BZj?e^q z3Fo(Eq13>Se&(RL1mnH|%W2foT=oX7QaRYdnS1*2-_3G4a5?ny=T28HZnKqW#AE%v zl2lMspEI-#RT4#H43KJ3R@+u>QT#r5wS~+hm90N^tKf1*xrNrHbxS*69L>UNxaM3y zyHgJ~9Pdxdc$gx+6rD!BtDtZDB`&~VC@G0R;m1nGdG#wO2>zC;=)#9f>sgnObNQ8+ zLg0D%#ad0oCN^AaNB$DBX@V!eC?`Lk1!!i0o}_%xno> z?0~ROb2jiJoW>xU*l#iI_-Mfu-U+4wZkl{;s>u?amL${3JZRATe}_9<#yE0nqUC@6 zPY{J(`)z;5rseS2!)^<9Abl}M|3LvH<&^aE#%O+k*I45g9umvi`|CrLIPg*R8f6?R zNpnsJYwyy1t?gUPxga@?Ihgv=oE@%EtIeP@U+6!&i#FK}<(1+f9H|IJUCJ5C)%9!M z0D{2(EA1Q}Bo7^ot5!{OlK~F~UquqiO)2Sa)Rorx4!zFeeosh5s(^A;ens4dubIAu$f|s#eqchoZadUx5dH)5XQ8m0@dJ zy_;8O7e7}SH=PG<>VgS~+xJI-RNHn0vY#80<0AoGY>$`m9#urzi3~$fXsE&QLXDSi z5Nb;zlN3s~QB?)#8Rs8*%xVVG2o5;;L6uqgX3ME*`!PE^o2R(Qt>3l8^WcII%So{q zEtgWiZqaenr&_y3mr4sX9N%g*0mGzg^Q^C@v~xWpH}@xah#v^}X>nQrc5``%Q`jn1 zhR+A7yg!`tac1f%O9rf*nWoMP>If^kTGb^3ij-?35T=I!l@4fE-vAHZ`ZNlvcnAGH z3B$sc>M7zMipYv5lc-|Vd90>SW&C320Z|i@I>HmHBfl6A$Y!T?WDXjo_CYU|nVi7v z$+&mrXZHZ6G+3-Akcx%gh8zIA$a)w{yHXu&@3 zrw}x_eSQIRaO6p=;6V&;_`AvZH2#nT1snTd7T*w&jINP~P2BpbN(s6I)5Vu1CPoTQ zF<9^c39sFPxyDmiMw!4=Pgxav#OH~d0;Zg_Zyw`p9~ap#_k)-0X@ z@7IF%skEGhxM%aQ-Rf2V8^@!v)nD6?O^bCToNOzn@d*x!FP!v^q{rpBxG*AjJD{oY z;>#-x9#H2KI|iWP35d`k0-h5S7;`PVNo)ie6c_SEg2Jmpv*esrJ6EXQ>&Xp7UxRNF zR66y`R-n%Sl>nGY0e>kqjzS-JoGKXg+v59^@=SzP3!;7FT|qbc)g^Oar%(5HA}+4q z2M1=LZ!y&8;O9@9m{5G3qywtPx=r5-KX|B>YBFwaZVEjX8HFr9WACu(GOwj3{`gHW z*3Dbkt(}&aGXlyQXrC?SD}RDKz~wa#1ma#XwcK3jguzc^O43OEO;W$viHL;I7pIsS zsv&SidN+c$By%nUeDo*Bt5ru1DI*=#d_@npi!g4v6k73eZ(ThlL5m@BWEi3Eg6w^3^3I$qbk5ypmU$6Y+0FV&(x;J0Q|6X zDOW`uP0OR=IeOXd*vMw@*yBhDF=;4Gp>m2n$+rbnKDF-eh+yC|=#U-iUX?x^rq2Po z2tM9!>Knck%ATGhol(rbi4Bd-G>0F?MmC9LXh>FVEf%iL{G+k!6H*-$uhNTUQ2r5e zI_~8Li)JDpLvpW}I-IHLH)C5!BX>BM3=elI@4m8bH47uRy!TBg|7y#G+d<7mH2MpS-HhYqBWoR`!`}y&o@A!|ENq(QnXm7a#kc`B> z{VCYmF8iQhU_t6>fS4e9Td<@ufR>(cVfj1RNC__J%(LDo&5!2cZti?oE^S_6G|h~I z%?j=mO)i`zC^`Q|-yp|J#S9^&;VOpOtH3IA#hjF$DA*JY)@F8&lK4ZsP5%ep#D#d}Yky=MxG;m3`gMITNl1IzdJkkC zgSR4Xi>GHUkE&6MFadCOc3`F5wp3+R)%LzZW@hH=>JAv=0v3!? zzT7sDgdt~UR%98gY@`FM7%;4811Jwi@6&TE+2|u$#HYK{p!zDT@G^v@PRs zps*0tZ8lDoEA@TA@??9}^oj!oDiwcPHFn0@P`3Ed1yCo8Oiv$;CX;^Tzq`9b#9}5b z;3Y*V_*08-FOiH9Id72F&=B{hlU|HdNm4q>?Sm^cH3j8gSI@MpXl!w%!)swyW$EEGw9f8nR5(Jo%n7$su3Jvi)t z$qY{w*R%=rM>f7zRM8n;e3@$}xWV%r{KhzOWR^8!(i5*oG_*VvU-HnfNdP-P8}wU? zXnkFV%boEij8VUK*gUo&58(jInsJGzcNgM0mrQH+^Q>6f4}u}Y2X(&fO5%hQEE5=%74*o*8iDHC@w zhB*Z?uJQH?z z`8!{gK4&4kOUoW7hUK>pl)2N3b{s zv?K(CP5|_L`9jpIwov{J68OY49A^COke@ zaziv-e!0A_WVunJ(%rr)b+m^6I7_Lppr;VhGPd$?tSo$<^eN@{<*%@`#Kb-z@UvA1 zKXz^Ot=$*e>+KB0**$^P=1AZQc#_dQVjQLSDJKSA=tU&kyUqOE#I}>iMeHZC-?9LhfIf z5NdFDJm1Vh0wcTJ_i@nEIh6dRuJiZm=#SCKu36|SOBx`(dAonLd3$n#((Jy>i2lU- z8+aGa%Ybg2x zFQ-b2De_!m8v5a9-Z}Ggb$8ebNtNM0Le)nr*7POjSPpCK9&0=z%oeK|bhZs9F(yk1 zO7ipwOP+9reZS5obIMC$2`OHs4eoilc2gly6S!n2wX=!dRr~r<4HrmwC|@*TMy0>8 zP&gUYRRv*XCt0QWpk3tR(qNx~lx2uqAS|KG3*{pz`EFRFx$VLU)Sf`#9q3U7aqx&K zTXc{_ba0XoQ&UrOp+=31PXd8TuLirtD?#;5%t4KQ?}t}=1=zo?=hY|B&;erxwz{P* z8J!X^ysVA3r&wQiM!u+q2i@6oM zb`4uDJev4IKE9NoY6Yoiu||6tJt+L7ec%#rIbwQpX-z%->XO=M;9dgayf7HqZ$!&& zNJE4Q#!(dt0yE8Y^J(^U0~lY9_bO-YF{hwm9700T@6flNAnN8n;)!8D$`s zJrq~J7~(Cb)@(4wKep+!lZRx^!}e(zH#@%F7L^`)hFcH(gRk5}6`n24z0^f)6+^Me zgVm%YFcBdMx;mrXryno8eoVI!D@8ZZF8^xZHT){+H)40je{D^(la#L(^RUM~;_Ai{ zab#ky>sbX$08?3ohN%bT-2d%do35C;xHIhSZn?{ZKhGGJB(N#xVZ5NCil3_t%e2it zP!R35pU)u_d-34A&eD3OVNd;5#Fi5)nrgnCA224XnRZQPHpuk*rtI$@l z+>Fj|&Zw$}#Q?G-C(p{9aZ{;i*{#AK3{E8FTa+mJWsVEK8;4kXLj9^Qkae7fO#j{4w0yY{K8qE3B zwqJv3`MmX-|6H+Fq`{9F&3NlS8;5-OjGt{+#{R8|vbLM{FTq1%#iNg|K3B!){y$WG z1yq&Y7VQxbDQT3F?gr`Z4hazvX`}@a=@9AePHCj1MF|N>rI8Ym5|9$|JdZl`2Ff;qGlX=p4IjL#00b|@D|?LVYsDHPjBy_v4YN5 z+DcmryFEFweWj=Vhu1oJHdZ-*E*P{GBf-3^@ATvR^T%87=eChm68xlj-Arj^B_kO9 zynDw6E#n~mk)9j-F;*{PvyY{!q4CA8fzRu}rv1ovQkTpL6UF;<9g{tv$0BdTOebCS zMHSFQ5fNCYA0UlfcKcLTSveVXvrvFheSkY{Q-07}UM(nocIVJ*rqx?WGIv-D&7jq* zfdi&0Ol2G~Iwm?0u$tEEleeCs4VVn}#2?+dpRJyrI#Dd0BX3v855hYdkXlQ4AAxYx zig(JcVXWQX|Fgecjw{#w`}eJed4vTu3wfUah6r(vqwyW;C$(j1QBA_Hy* za%BDz?0qtq>qpjtvGTIcZP2VyuW4)vHHwx&A=DJb#~AO`&QWOEnPS{?6chW~)K!%w zRg+x-8T#{Ox}rCA4JV`SqJIBsSw)fEciMF}GF;tzyPh*^rD)4JQu9iZ-0qLT*&Z$x zx6K!Oo~5tbgU+16U*=SknX_irp4F~TiHL9Ebo_c%Xa3}#>Me%Y-l;o-B|AL1cfI$> zaB%+Ijlo1^q9^_PqVOYGDqc%N^p^b%_4uzMV(B9Egy|Dg>$~xmQNI#uMuV2DRv$55 zlwitk>p$D@4#65iz`;2-$zi|dhA3uD?ubpb?jNPN$TxGZaDyRx4<%v5knPH&g;#$} zcS+~@bFNGwrUrV_d(H)a1zdA=6#K)X1T=i^Eq zd{=(X5hqhF5Uf6op>tJD%%8N*NmMAu{wpQ4!tN)|OJ-BMuDl7vUC? z1S&31{FTO^RPOc*BvJ?(Dy0`MYPKFSynR*Gu=HpBi=IxCl9RX_fANp!La^IG87W1p zI_R8S+|1gVSxD2Bu$IU`1^mpJC-==ys#)?hHQja%=)@k$YTfLk5xoURC}WW`1;oD& zdbV#-`{ZRh<+U{YOCGcHMBO-ycJ%@6by->2zedzsxP5O$k^&OlcYd%sCB_Y{g1aV- zmbadeM26LM-*{;Vi4(AfH# z-*i`PN}e#V5HTjT#bUiTwaLo$qwBnSA$UGzytXR+|fUT8RXRF~ARH$SG5&aO9k8U3}it;9>i``Kmmw8Hl$3v-l2o1}D_Q#@x%(ngeQU|b z=Z7($E%`c3d42&8_F*MK?Y-yzv#|UyC@G)?HBl2iTq&;KQdUa~uToM$l9{B&Ksg9s zrV8s$fV!#5M2wCCXHgPSU`L#ein1dIh)W@`+tw$6tPlK=5Ht!Sw4?X{6ZU}PzYW_% zcXuH)nb(7>vK)81;Q!k&3c48CCC5WHfZ-BIIJPfxOF#UNSQ z$JF;-wz1y4ZftB!+Fm)M#z0%YT=@Pyg$|=`PIcFcQP)GHP}aTJTXrZ*Je5vrF4SQx z@_wb^Dv?g_NOvR)`WOYp@Gx>9TXv9mVMX6y{g(BL3 zMhO$MUr!sAkz58bOiWKN30=aQ1W$LG2&n&$AoVXxZh+&|>sKBDt?acD{ z^$Q}!#k*1!pWQ@}O^a@k!;{HZ){E5bAA^)oUxbJ&77$qUM5%bzv|+=% zoT6%wk(+%@ot9MrOa$_xyKV|J0#Eoeh3WVLgNI~1qFo1L4})iFm2Kv3e| zva`2eJoF+6m;ZHqZhH{V5-|-bN>Ig?kupWlAp`kM27M2+c*O(dYN-a-nk$;?=I7$u z_d7kuHrrvaw{Go9ULI$!G*35B9D#C17dV}o;z@$=@B_xz0oHD`sC>0!HYz2332BGx4`{T!taq3J!YiCXF;jT<$%*e|sH|4@ph8+ao#Q~}gMn{=XRXycb zh?ak0X&3oGUq1u}w3xxflnKigNH$D*Q@(vDy;rVS5TIDl5k^37IbJtGXP?vS#sOfokc0kENlcky?;5C9Ko|jTGD4>DkAHfCBBX!t3-McUv(R0_&}X^c==!=?&2V zH>vcqRDXD#j9fKVA5;|r&y5{a8U+hI5Fp$2t*s!S6`}FzJ(AW=t8|I}8*x!NKV6I=c-w$_Ktjb3` z*t+cL%o)7;ftoH%B45L=?>z!IE}k}0wz41mxBK#5M<0rU&m9jG#) ztxS1mYbt#+{`Bb+0d6P(Rjm^F1-Qkt1a83!lR z%M46=D;~a*xefxntd{9Nf|8>`U5y+}QmdKcfiU8L4LIh+S&*ehtPz>+`;wjl=TP zeyjOw(%7*EP3rf4l~5(v?O7IYkpJ=h8VF1W(w6&d`Rf5vtQi-K5@1%M*b4y z%@6!3buiUS#^1hshXwf|mw+-;JSTeKjpd%`E)X3f4#F8NJ56+TirJg;qPI8{0Qk zXpieB?Y;m4X39^lt)qj|zJOOUp1!nQAlUeByn+8w zdKooCGH<-{X9q#+C^njHm0@oCe?N1RDy|BdaT#4+{b+8ncA5Mxo&R^mG2nBH{@K}6NWPn!TN!T30}5l>N79(HjKpnTNJOl z=pxJwZCz3a6$+dXr+iB;iR2=6kIT&7O;n^+QrX^jEIT=Z6BEeZE&Kegz(09QHdJ4N<&?LA|bS>^BEJ}peZ-G zM#_a~P(4wDZ&Z_k@Nu%`?)LW0ILQr$wFy(6&rr@2{!9167ba1lp%LoM-n{m9VPvuL zc1Wjl@%Hj^8O_d$XD7hu%NDsX4aS!F&&oQZll0OPu4l%WJ_rmW+OpgdTR6pi(_tEc zAv+cgA8BNC6k2FB=_S0py*H{a!M8sRW;wp;paK2xqO0hJpK^w-nHg>N=f;MHx6nU@ z04gDe?m#&h2YzafE;pW^kWL;RB8a;XdB%M5Ms%{6nVO#)8G|pgI900MB>NfCNJZ)# z-`hUdAqc9_@=^L;^%NVypB+J&yRfTdZOt6rme6?=O){Jmr)bhon<~hhZ3=cF>#m~E zb^#T{2r*mgU~=s}5h2w}(_k@pa;fgGUX5aDquFBR%@zucOO)zqL|tg4B>FNXs(|E- z9L__*=0K5kzbA^7A@lf}%b<^>iJ_%@ahPq{NMVX`iZa8P4jblJ&9@{wr5*1CHdYe% znfAvr0u+Vk5UH6V$bs)Q7K441sB! zP8NfK9DSW#gFBjumOyRGn{GBvISJY*MFWJ=)otwU3zkwS3mxufb~2JgC0(UxzM1(q zJ!Vxg($V-u{~5jS*WuxkWsNbBWV9UhcrmoH2YE->^?F%}5!`TuAku>G&HB!cB9up2 zn#|EC@m`qyhoO{-9j346pDTHLdq;RER*98u2+CB>8+Giwgr>b|0G)iFS?ZFh+eY0_ zj-kd#r^6~tT#t#tRoG*RS1vr&z42%CnaG-?J0f(PS{bdY#-;WG zh4Xmu6z**2U!QNZobP;ME&eFIz`YLv(6I6jqV%uaSTh#Z*`ENnSFheSHQgnT4k5oe z_5C{v;-}}@J=k~TU*wMF=I4PsmW~d=6wT)1)CsKAS8PnhVq;hhI+$PH8}Um#;MI?d zEB3EJf4@cg{=GJiUwiKJLB+a04cEsn!ZrNqdLQWMB=8C5vvjVy$P|ozHm{K`8DE@t zU(g^DomM7q=;xkuvQ5I4(Zq=}#*@x9Rg|Zo+nL#R6K$l5Rno4f?Oi)QM|R6$ym_t0 z)r^L)Y2YOkVT>k?S7X)XyH2T^qOK)L1kD%352jD*qS_BLKGaS%8zu!BCdFpFF08-z zDS9F?$bmjPY_LdkfR;G|MM<}Ng6?argbAt!$%;?i!J|A~!a(#yDf|+DXIf?1mc!UI zDK8u@-;P_1>CE5%;pP~9#EO3Ei=rj|tK*~lpZDxcwu9$_DU!#JJYdXP$Mh49l%zjT(3Ujlr>O6gtv#T@&+|`wXM+F-iR`6jxo#@>xy*0qp#MHs zA5wt8KD-74k_#vMrrYor<+f*aBY(tE-4)YUO|*j>d}O5i-}_qeyX39ZhUZo=w%%>-R20bjhaFkuaf&&r=2h(*>uL-z<6biNr5*BZyxqT~{a%bRP57PgAm5&fYx_BZtL=!@LByrv^0#vcBGjZ8r4=0- z%9Tvc%HQ*GJm$U-xoUAn!MX1r`Rv)1`-*?HObrwFtMeA|l~wEyo{q$nxYp}?8V_$# zaG;mMIqopU2V6r=O)YXuKK5V|aFs>p2Q|gOy?P0YvjMJE*kFe z{JH)?p7wsRnZ{d{kBKxG;P8-_f7`RhR(A!mBj)oxuU3dB#g{2WUkiO{iHM5E{%m=R zP%7yZ+1`QYK9y)TE)X+e@^r8h>z@|! z(=iqj@f9tjP3(RF7!$ldgbIW-`O?)Ru@V&5-;YY$q2N63@#A7%iBoo(nf{*^z(OxF zB916@1&xW3FkF6MUwf!Rg5pKk59^s7p(nKAS}j_)6Z$O`m)AQ!KM?X5VwBUv80bB* zwa()>B5U;j?tQSl;@9Ffo?d#^f#uV0)LS#$Xr}bz%{#XHSYIFgYQ9bVt4vMt87ng8 zr>wkvPC2Ij`I~=PF92)j#<}9*o`v~t;;(y>W0wXtte20PW@#`B1D_8qpV(~bI}}yp z?OjY-yrmGkyC72VOP(518$H_Ga{_`$=||^<(U~w_&%6!jP=pboy7IwH_c@yTrnQrpaTgD zTq=NQMEmVA#S6%^SQgG-;P>s!5`{-ho{i97#{Sy$C;$5@*rDXfxI zOwF6DqbD(Sa`PPM-WP79xG-+r@Vrs`@uO&^Z_%bV4-HAHFr}s_JYE!6-Qn&eAA;!U zbDM}qI!2L?9tkXs3qAVfbgM6Q2<=EfaOw3>@E=w~nhz+c8;2h*Cgm*IeJ1}hU$pPD z@#cTP5792MxT?Mie!k)UJ?FaXILbF3J7ZDER3IET!Z*g6I~}dVAZV#J{N#J|@KRz` zB7J>K7a2x)HOhJ?OO{|O7I@TbdN(_eULB4zwe82A>js7Sjbc;T+ON6oi6~l=1IDw`fIqGInr{)sGR2UVyTrrD* zj`Ubj=5@~DwLnLck=D?-4gO?M3PCoUo%hPkt+-xBr_%tB`j?FWYvVxL1a=shtCWIO zhG+nTBOs3_mwtfk6v@xON5Mj5Bq;t=E3hkEis`rP6j{p(goe3ai=os#;2{&${BjN2 zZe*-@TU8O;hMWb~dXd|7@WUoaYFtL9)x#mcEg zbKb|~qQEK;iJ>}^lxRI=9UsY;U;*ZH??zRFjHH?h%aS>6-@e`Jdu`V@PvLT0me4z0 zvlBVq+mBDs-Je4%Rz5&v=ls1JP4c|(hA$bb3Z>7khJ&k7U^%lxPv&l8!EU)VTJ#es zRL$JrjjpjdmHO5oir@1A?t}|p5|aJ9Os;*n+XOh?lpc87FWx-aPPqrn^DQ?AFUqw9 zyZnsB(vOQTT57#+|MXZQWVrUitNQ448&}1>dl83#aOs9C!>h1c$Fevj-hL~&S1%{F zXTwr=vIA>29=bC|H!Ka~iAOfRr0(SYoxH~)foqLHtKlyEkY+;DC;}-})zwN+At5_lV{cQulDqKkYjkPje?v2}j{mFFpi6%{K@(9#^w5ov2`Zlq zGFz<-Hmjz&aXKHQjH%`FXzpaH{R&BJE@iv_>?p5pi%_ESL<4VqacE`@0hY4h(_G>@31AGEA=<2e)tT*5Q`TB%bNkIWUdx14;vpV;d)6H5aeiU+{zoq~{_3iEL zgRFRdnT=E0-l>_HaL8<7bJ62td|ulJXC~qRX#@hUaa+A%s5JG?>84DITqOtzipDqA z1ys6wLE(n*EeivE6n{1^MPGwOjRD|e<3r~)@>TfpN}S)=A6eDU3|OZ#{ZvifpZK0A zw`1fx9-qK52H11Y~NX{{_imS*pab@DIgF6QY`Y??jBpN~%owjN1@ zomY{^#+rCzdlMndHKspq^C%9dg@>PxDK4E{6j}fETXGRUY}Z=)uC``6R3x;x=${)& zFcv>}-z@7T7R2IVP@&Z!z*%87>X8D|Ot zY+bSap4j)6Cg`Y(@4B_DO9mB;OWfvvEa{2s|JVD%Z(R6dJ$1_B$B3^XfM1m1^5!G{ z>0b{Slnwo!5G#PLk^)S9yOHiz$z*&pxa&kV%KX^Qu5?cyKgNXaLqMxxWyiyCD)rSA z@HV<23>4tbu(-j}-|X$eyAqK#B6kb%1bIdAKS$8t2{HT08?mZs$7P+4@_>YZYe4gUqG0Z*9+m=h;_M#70>FnvM!5)CF)m zR8P?phL7;}4-ba|*9N`Q0LI~C1;ZDgae2BtpA0^a?HI%C5KwS1JC6f4ZKgi^KEU!V z@3n1ooG9P$kHth$Pt`^zCDgpJUFJ z$KC2Gi8r$fk2*wya`h;O$`q^U12j{3{IrRY`dm>F7?;QZ2Jjvj1YhxlU_7$ zl5g>I0P1v;6E$EmUf$`K zPDDQB{lk#-+aU#^qs2;ls8*FCBln=2t5zi7ce4gGIs}dKnA}hZN-GIx$__Ze8Xz zBLfe?`bU%fnzN|992?0RgW?+GF*WYH#s>$b*CWn7WY%EEd;=*;1l!S@03&>hsMF}* zKfj#lGvLf;OAT2a8(v|fCD674-Uv!9uhyKDCgu%a2LU0si%%avBH6q{q2_l+aN|LC z49JZf4Zbvx+Zy+vPt|sc@!Z(fMI-2TXjeR4&c>k^UH|0z6y~n4UO`Yb z#X*;womM{n(nif|9`p{uN%CrTcGl)q%~gtGDpB0@QNIi00$uaFXLbb7RGDTM3fqTG z|J@l#dCTVfLf8(+mKNituJZTh?O{Y&RXO+ zQ^ww$SEs}N#d?wEBHQ)j?;U3N&;n&(fg!zw1%7-(v5G-rM2L2n0f!~Ia>%u9Vyqp* zMVniG1=P*>0{57ErWo==1uhy5E8#p%NT~v#Z&a1^a@(aDX&(M(RUUwh5xes!VmO3f zpS_$tO;RlAU)( z4-Atd3c}^zcrjV!x5rDtt(E6rCmZvlA)(AiBJ#j9U8VbGQbAwLqi!|9r3q8UIaSLn zSIsZigQ0YCJ|PcZ+deXiiLk_3eRP3;_r9c*X8~6)9yhBDPRVlZpk`nNE*g!~1c|}b z;`O5O+Fb*;^VJWgLq0~wM%X?6-hx?0zT2x}&wHu;UW&Nd@AMSieHt?7cg-X@KdkXu zxP+!(Zr#Q(c5UY)`aV}$WcjWisA=8JSEa6BtDP;^C2z^s9V7lZki2J|&!WU#E2N!j zlMW=4!AGw2515w0!og7}$q@v-Jrp2I;`5anG0Cq>cffojUBF!X*hGbx)^}@rTY-U4 zTcA8yks#jxmFPW)Kek_3^0);YAffcQJe;yp$djiF01xmE2SJO;cMn0FD-B{{fD<61 zX&V@Xf$0Jo$_=1e#GycfBBR!UpBQ22MTH=tUO^+(*P|Kif6Odi1q0P?D9jbHo zs4;GW4K5~7#fRl%e45Xxjp;Czc5>nd$z@ghl^1wH;&g@Lg@SK}u{c12#e?y8d9k>m zG6(Ew{&^@M)BvACEu*ogOHQ<5-94-X{0o#SfUp9m3!Z06EW~g6E%V#VBH>T|Ifuy#-}&v9>jh_h;r@z55r(1Ro zVe>?wA#Nu8|D5?{@mc?(GdkIDNNd1aoz0a}g#2i;Z-J|*}d)%uzqUv?8|{bH6nRWbA) zwIObrccVZy{Y^VTNF#i#FxCXM9?~lmrg!mI&j0*rY~<3n%%n1L|BxLRwk#C-Q|Q4NSFdq1Lv+YepjKU4`qS>r_? zNl66v1E^BL+g*~w;5`CRD@he(4Y5$uF^KT) z03o8`w|ev~0v$0XPcv3Li?*=5S6HthPgBdOf=Ayb0E9Iu_?sjDp^^rY$S=;{1g0Jk zgcUV5-T|Z=RA2gCF3m=uE)9kROPI0;X$=p3CCDKuwsZCkhTz-FuYgGKx+p#U@qX~k zVo5D7M&LZ%)wgURa`sh$o?@0mJGfVZRp$~|!uQ^D|8b+$82GQyVC=g~>DD>m&>66G z%E&an)m#qT8V$z!#zqHN(!<3hbLm@HX3k;Bp>jrL`w0pFaOgor*yRirPuEQ0c@FDy zTU6VNEO59jFo?#>?^4e!~4WIqZe$2LALzl9L6oA$haL55p(u7U7hJ-%UY`I;iIR} zc&Jb=To%7AuRb)L>PrpVu>LFPyhi5dv!9`5Ymj+vNOmqd_q3^$>(*$~6{7h<^_#+N z!bOq2*GC`w%gO(o!dFW@gYynH$h&{-pMqgbY5O;DI(iT7VGuhYP(!$$b`TUlTQ4|) zbsC?|1-zo|e14aWsw8VT+K4U6@Q+qwnW+oOhZWTTB0i0_)gV&3Gh|?gt?I5yZVbc@ zSKEm>@-vV}D+(4x7lslHzXd4(kUUjYTw61Bw~VWWG%6DIRp|!9(i=2OM%M)x&QVgu@SfFPMqKNf)OIWr>)W3YEV*Ko;^yPY>&6{Kr_M@7%PL6 z2^&5hFff3t0W-CEXbw^zsOf~p&P1R85C#Wa=7L|vu0v3_?UjfNRU`W*|< zYzLuY0|E=sy>5$Gi4gEhSDUkg5Mo%50nX7OaAj>nBMVTIe_j+#+ASWRYd?K@8)O?c z{Hn(QEJ5AJ*no^&w5*;%L+)4floZRqM1^HtqdULJXa#3N3xD@-g;#=UNMqj z^tVBfbF<_9hLVGpWJ#&jAnpxsmj%>;C8j~?OH~izk+N?OuG=s50{%x#BUZjMFY75| zdauMQIm1R9Bo7MhGP?jf+8&Uuk(3+MSe{u)QdHB>9i8~J5sw1>U94V=M=gniU=3-V z(I-hfYd{RxN?T@|@+eDH1Nje|eMsv1qTW&R;;$SqwMjKJlRdDJKsy2+by}unWXzL z@!%if0NS3@*}p;V8t|ID`17be;T~SAaV+$gQpJk@!*Ewi`x%lz- zW?bdFUd{ATfF2i=!AQ3y*3CG*iYq#yQz7zb&(?C%z6ZB zZ{3~<#yOk2E>uOwkLlv`=mw-E0$gN@eaq8R25CtI!`q3}{0jm!U9o2RlU$hAD!+6F z(+-&iVaxOeqaFGG#i25|XrGoi*e3W-uy`ByPQ16%^lHojB86o?q4?j=sH{CBLD0Q~ z9B&TQs^XI;PxMC->GCKk4r7C1e=T`P@uL-OlkjKS;7}oKL}LuQ6ZGE4???g8^1KP) zIo+DqCoR+4NA->XJ+vLS?iOwLuI(>!`=4UJmb`BUYYif%Y|UGsS)EI03c(K?!k&KMN zhaO3>k5sV}{kZ1cfh|z|W9c6?A7Byt^OTA7>+q$zCE=uI%F8&;v3leG1m#+qpJz&6 zsU^@;+kVNpnK%+npC7O79c>3|HvLZQlFYe&2BQ zfLV?&205EpBBZ(7PUs*Z(DBhiysA!Dr@QeOPx&rsA;4#%W_Y1ptgL1_yZDTK-A}4& z(_B&1bn0HW)$RjTTJS3;Cnu?ms6&wp#ddWdnYFkXQeqY^F7y#*Suo+~u;Dj{(|}}Z zSgFBWTV6W|iV1B-hk;QT8#yvNo6)$yUfrVOhI3{yz;T?OrBXwDBo5&KDKCcP5{c^~ z5i^huAoAWk@Q>;)`aPsyE?_lhz!9=Kn7Vx2e%+c+>I$FYS8DqOBT@vXrdB2_caZ~% zowEM>-r~RyLs_8Q+~_Ds0Z$bA?E?T_h`J|sM|wd763U6fjA$S8f|_ZTN()cPzEj?s zVGft=-&ke%S(SJc;cw{T{OS~U>~OwYmy$DE~(&5JKIOX{;a{)AB#oo3wdMYT2=RsZs6v_VuMFhZ3iYa9||;r-knJY)PEN zow*?6e`Y?nt_oB`VW4sSqeYSS-m2=|$#p5Gt~8&1mvry+u0qYk|p;saPiT8(#J?OtEuwYIjh$|Aiez*-6k{UFx& zMQ7W5#o(xsWTVL_t0P#_Le{{`Qb0QJN^qRhsl9K*8C5xT5lHG<`;X*~c7;Pet zYsIKudYmME2w|Xx$D%nIb1L$oy4(Ceru(-#C__Tn!gsO_E;9kQJ)>(`*;aX)W)k4< z{fY>zpH)qATV6>GkqhEj4@uqqU7f4k|3Ij-=+V+C=Uk1i_k#VVKWF}+2<1?Ca2RH~ z4@b;HG|@ZzW6Q`<4m&W<@dBFTA0421{p)Am13~u4dbB$F=!~YzDvn38&zmqFKFDL> z_Wk+uCvkj86gMvxKtXymG>!>}{XumlB}#(dR7%E+O5;f+zD^8gQjZ!>DhWLIo8SIB--Ubp;j^I1K*FZxC8R>_eY$6j+#3 z17#8nL3a5u_QERulVZQ%*a4HRw?ppAY-wq!G{+q|5TNuf%P0Lg-D}6ddpYES{J+NERg3(nCzNWfM<*J*%|fLe@Y3n>extkp zWw7A?cPlAC0lNdL24-f{^(|@b5Fa~wcqV2pLB0?!wUN#eRY%Hi! z$wG6TrWz)_t+B<y{d0Znp?%S#Gno zw!T;JfIwg!tw@M=1Gr_RY!*b1z*)Qpcpols$o-)20ci3LK#C7|p`HPAYA6wB-34pI zDY;c-%wf90^I2=OB0DqHweY-y<}m^Lx|xyb=`cue3^>q1Bn}-TFhUHNQr-4Kbr6f| z$h@nqC458k3m&N+Pa^mo!F|7XvK+e%HF$g_(=#4YXBc(L_7fG9p#UgA7s)F01Au79 z`O`dTETENPg4*VL#be|~gi;ACohm%IOEHBmn~zs@1`2=Abz7+7HoUV+H{GoXNYgo9 zOJ}j3&Iu1li+g#d<$vv@YhH?}%F*kR`vSdZP`GVP2i(i8>hOK&lQAeVGitik zeo6AFv9gn!D>G;b$DZ2on3T%XN%;9=7cr*sL8%*iufqx@!{r>$UxaNIP!6edJg6jg zLso-BV!KJ%2qiiKBtWMndCD*(yo`gaTe)=4I){oq`eY=nnd2^1Ueemi=+sTC6A=*h z%QJ;oUKido7uRU$o2V$IbYJXs7xRZO=I7l z6|g!$UBkLsBSD1+S3pk`Ss-Xk05_hEp>$bx28m(+QKW#UK4DOwu*$G|BV$)lydD&s zP?Q1M4-y#2gOQyqct)GksTiJBET&~ z^cFE4QQGs<5DdO9m?}(Qk-hNZQ#i21CYV@dYP%!|FPzG9r7X%X`jJ+%%tJ~#AgjdH zmzrbS>gSP%Z{>&6|#uyRb=Oa}Esk|;nQ)|3<$!%9ODaEeOn>^UB6)!fD za~B6@$xeL-8y zEwhhET+?10JlXn^3uZ~5(>|+xSgPkGHRii>-~TVaAggHgOW~{zX|ABWj-$;3&ZZx4 zymy*!0c))GmUgYBP4cUj8(cW|24cnl2o9MRVp|*m<60sHqXIleq#ZK06u^D{jv31 z(AY(=ogeSs8evuq%THTkCc+9XrPcu3Ur;}4Jf!m_QXl|Po9(%~FlBA0hQE&8N>8$H z^@>C{mZ~N7sEuBu*Bxwv9Sm?VA`ez7&~|lm>)dwJO>``}ss+3NT*c9l;~LFtT=52> zlV>rk{@*~FlPH4nV#PfN~rSuVB;VaqIDlPk?GFc zre&Ho@(igA7vW2~-&al?3kD(fJ4T3?5iWYKGeRZh2Y&{x@BED2uAYw=dnNVd%T)Vo ziHo0rdNazS>70*cX`Cr>`DG6Ur|AKMI`{n3?e?p~YG4X%_#+@Ywtx}~IlC2lFN5LA zFDX>&=Z!OYS(hyh=N&LU$g}?}%KALH@bk}z4RzJ&)3-5S@D``Sc!K~~bZBUZ_-zl! zAvO=8aYrXN2MShr%FnoP4j%R-c&PyjgLHNiUdwukI3OFIbr1xhEL7B)#dW^mX~vo# z4kAf=dl0G0^OFZd8vns*qONk02KKJ63v4~@ z)^af15jO9Pot#WC9EB;7{R^P&0$~@#AGu@pR;j{sIP4zlZ$sY$&hU6R54kyzYAs}u z4<-^w90}A?Ae90fTFo|8&j|qC;w4(|dkKB?B3i5slC_qEL42pTR8*jiK#BN$XPln& z;i*rAwwdbhKQoGyHBDI+$H;}{4r^GyG=FK0VO+c;dD ztzO~eXqj|36cpCn#_Tr7%#+4G^rG1yw!wbwL5Yd*G-(IeGLf{g^+>cOg}tU!UnGE| z+J?a>RU+yxRfU4bWu}M8ina`-lzHu}`0^M!nLaoSmw_X>CRVWz)yFnmk@)6c>gzpf zQb`|=*?*8>BMT*1o)7?9;TgnGiWmmF(j-0>Sz&Td2a+bVcUdL-}KiGPAGN4t2vz zE^9jtc|tmz(rj;-VARAoss&t=qFS&l?SJ(+CGzDeH?f<2AtZ=j{OvMm^J#X-n1
iQinPRE~JVt+BVN1ZHRQ5aKcGwaRQ7LO9 z^$Oa17)O!ktSMujyWA=u8TDb^Tzz=US4E=rg9k8Oq-Xos4Th=)#oTRngEPLl$|gb7 z7r$2B)dXIEiJ8q#+8M$ zGp||Pa6%ozSISbWev_$Gx5UDkazka`62CNd%V!ZPE@Yd5DR}U20vx9qOi7mlQ3Hw* zm~aSW9s&X6`+A$oK|HX4FJHbKsDalV(-T2JmxQ|^^?{cJc*U^`6-Fr2mrTEi-*>Nt z%7qja0q_h=9O@0?u%1hj6WtKZ{DuUiGQaU3VhP**Q60siy|rC_dRzy7yw3)W&~DaO5H>-5cS z?}z!1jB1Nki|B^nf<+uWy>`jRw+$7D8cOP1+CN<{%&h!fV(GxijG@w-rcuJd&aO76 zztehPXnl@K*;-vh*K&}Cx#cx>WktWDHvCp6f|5Y3u2sKqSkByZIG+i5 z-FqqJ;5;Kw>q?V$N#^a9cT%56jwzO8@&COz(~$a7)N^<;V7sKL4g>@bS<6LTg?20y0nZ;^=78?L<6_FH(}- z3kx1M{4cX2WwUex3qO!$wy{VbR@|qZ3`j8XeQoW(N2!11C?V)`BB?L&O6<&g92gz|)D8zGWX`Gm9vm7N zYJ39*%H=*A;81E@reUTzSWC5TxCHC}3`NH5ivf+K>F^RGc{p~io$PBCa8_B<#7y8#Ul5_RZ>Ht~_~-xcY-ltu5Bi3)%!;iFb#QVXy=Fy+9uKH`VR z@6}b-vl``IrGEa5iV|0exII@O`XFTcsRaRB3Sxyndj&!LmT#al2j(;ij4&m72X^EM zHxDWrB~WPruuTO{!pI@fkP_I=-3B}rNI~v990sqUIS24N=yiph6^akAWWxa`WI18V zVq32nMKvo?^o@opSD!I&)eS#$dhNdZ8swvVz=xlSJHR+!*YE|5km+Kp}R zpCV~xm|(Fas4V6`Q*QMEpIG1on+<$V6ceU?8ok5nz zldI-ZU+rE>4D5RcTVYJO(vOT{XQZR5n=BqeFw><7$#1Sy()HtD&Bs>c58q4=d=+NAn{b%Cw z>-_HW8*J|0*p%mA!gkwV`?vf z7;Qxh4?Y>0OpMWlsk|S%ij07MWfUOJNCa1y%oq;sp}z+1b{KgTI=&$fMwXB2TlwdQXQ`it6l8CZjpFWi0sNbDWtnxN4ECJr2lkm)bH zX`tswu$3uhH1k(uXmw{35}ZpKwT=umnG%J8hSa4q1fB@s5BzX>9(PFkq`|ftdWqqW zBa@Q~)%&n@kt`R`-;m3|2p{AK29lVGsKWj$4Dx8ERSy{E<)GrVQNwocJ^`XI7jLB* ztk`KI^7BthnObT#SN^5vvc4*pD(hM~i6Idb<-Y5>zP-&2+=KXkof4~3# z-1oV!^S!?3y3TR<`1F3iUeD)azSxJyj`6wUFB2ZRV3I7=zuhvWWF#G>r(eB@If2K0Rsl+~Gk%kT)*f%RiK5B;-}oIADFQ!k8w>4nYQQ3b z_%mH$@eoibAMoF{9c1|tIK50Gw6b0E206D;58-m-Vcgj%V>~tTT-5_`D zw>CFAon%3spkf7+z1{%|?v@RWrri_b?nYu(;OLKREcBeL^x%CqupspkmJr{ie#bd* zK;#qivE5UPm_#ggCTAQKOt~cf359PS1b6>yZ+yC>xsmDhM17W~{j~za z&<-Hq{~zIo&<@qV#j(C)bftEStFe8SThgDa|CkRh888W1Uln{e;uW%>c=)zaDq0quUmdMz)X?>h$!vwmdm^$(A% zr%w-zy=0)^JvWUrrTkpmZph%R;KA#1O7YuwuE$Z%(#p!V;tS%xoj)E!-2~`^o3IX< za*K&V%hU|91o0t8Pk>IB-szZ=`_aVmvBUYa)*StNWJN#2HF!GYlengE&Lg=7-Rs|> zfx!4$kGKfp41xqr`(RY7*?Ut2XdKW$x- zkE)$)|IhaA#ld^_?G7$w6?QV*+ys_Om=qA9b?Dr%)X{vI z_YUc<@zUmkGLsYu=_0z@xENXZW4jZybr8G_0lcFikIuFo-JkJ=K=|Plbvun-Q!EZs zTQ-bIbG~V_$+=ABsnrO zJlv}JxeK9^hT(P$6lzPB@4U}0oe<&a2-6WzH-8BVs|=yx0E)67Kc>a8NyzPp<|o1% zK%I!68gYnRQ|89ejJJxwDG?xCpt(2E|8{Pc&hK+DSjK9HfryF8A9AtV=WlrF5IJx1 zZMN~zu*?sn31_b|ZH^~P{W;eQ?WC>u^eTnBvd=cLO6vbaJaJ+TugyO^o7a48v|9zv z=Jh|}Dk&^s2rt2T1VNZu&n=mMCLH=z7xI$YT-+S)kvt5@KmPTm<|mSVE#|UMEjmS4 z7{3lLubtb*D)UA#LtiMh#SBe+$|QfOeRCs<*TmmFKj0cnotK-`^q4>?kGc=9bg?`l1YVp&r*m*dJ4vy>26)H6Dgq<;*b@niha z(%u5sD-)d$o-H9;i)7*01h1%#&#QFpHBCMSoMk}70f7@%dcyaEdma_JI>H%(Tx*_7(+%e z*8tsSmA0ec!C&nK}J2cGl1^!&&N08kJ4-8HVZKNWiL=+TxFm1xLNzFZZ87GqXZlCY7e_C8}lXV zRB;O%XF!rbm)UWYzXhYu&3NV2h~u58C*{Cv!V-nJlHmCc6>e+=IG`tf{R+h%ADZ|v z=C~LML!2NZKYQ2d$<^;<4B@YHLT+-c$NXnr_loKnddS_}!#<%|uT4j|Sq^^nWg3M{l<@i{qJ+1HcaXmBE;MF3yFDAoT0 zDD2dn5hNgxr$~gTsy=r&Gv-kE5$9$_-5&@n3P^+k(2nlUfRO-kS5D^N|1(*T**z1{ zS-%u{?Q^D{tzK`!!a%asiP@_{73l;1GCrdU2(r*J%mN#YzX`NA=mfn4FqL#-{vTRl zLhwf5?t_P%3a<&=FBnPQ%=i!nBx1H8Axh;RI;T6Fy&huJkk`SwlvWJUG?A>}DIHO- zmBmY~QvAQk76y;?3iK6jxm@2!uPkHIg%L_|wvrs9RF$hk0>a|Vjg@YQn&;v%^zU7&4I%h13swouoS^}djHLNwU{)}69>?*IuRZo{q| zx3NjrQ5Tu_+P84S5GHuGhz?j9Yjr$FTJU}k6Yxg5e{6bmz5 zHrhCD<@Gwr!pZ~vDi}zMKE6#|isRMx&53t(lP{0P8HA~yd^tM9`zazNPV>;V7$ix#kq|C=RwMEN!KJ$BzA&8U&6XP2+l@hT& zxshsYvD=~C@EuD%;gm~px1`PRVQxU;<7DUcQ6X!WO;HJ#O_lzV;%5*S^)Imh&A78o zUtF_%Y>co2+p82bWJQ&UY7uP+GWHZH$hWgXQly60;W z^hfSPwJ9SOQ7;*_7{HEW@x}xE%4Y`IFIaP%95VxM5~SUt-@u;X6CeBfDF1_Yjo9SW z-hFfxY`^D1aR8pkoM zlqF==`n)-cJ2y=+M8aJEz*K2zFO+HK89@Tu3`%>+Nocu_MBq=x4grQ%*kt7i=DGQ;ZJ2iaaF9th(5uqvSX=znjqMQ~st%v$ zTj#H3U=M}xlJM(6_T1DE$@HLiDlsW3^uBxT-CLQCL++m06@&7gD!H2`E)$V9sA6Ur zAM;Lm{WFczgJELu#!x@Wv)*fQN2NF;7XmFcf34atFf}^Of6ku1pW0WuHAodbjz_|1 zNK5Q*i<%i9rDuL!L6S*rz-()1ILA}l`t2~??Nwdon?O!RBW`2|{xIyZg!(RqLqTSO z@3*`k*L6LLZ0KilU5FjqEsE(Fu4y7#^76w$ku7AN%l&5 zXz6;_KS@%XFAi@*2dnPAyBrNAcgBFzA>GR&jhr!2Ku|$pFB(Fe_GaV>{~s-Yi_2Ri zhbLrY#Gw;~kV*8F+3FY@Bj*Iu!k@XN$f&5$p2-H>Ma%*BBKo{wj~av!6KnFvlZR0W zx>VFOCDbWDxr7dpYb!s(f!RGtRK-JHIiBHutPI!F=TQlG4&<+%!laN0}%e$=6!Ts;VkfuhsXS4ce8prnCly zzC9Fa&(@DzUDn%`-R~xW)~08nCkrWxkF@+~c_V%!{YzT?mCs@Ej#Kn<&ZO2#U9N|h zhcmt0->T3Wzwyf4pu6825LGv{pUn1gyhra?#mHCgFa7}y9PL$_QyJ$f12k23234}U zDf;)7^1RG0 zZ>g7KX67d|?-SX|X?0JYbeGm3yUEOq(J+2>>5Drl$=$nmiTr?9U!7cB{zm#FX57v- z*oI@4ho4_#$&yU%VFsxxrWs(ag(BuwR?#SbLnc)gRA`fvlM%78S^)Mj*uAZ^lUx9#|8Vu^B|2k;ID@Bg|`87SQ ziRHKpS<1kLZJ+`pn0X%``R%*dG?nTx{u3g5VsV128E8NV*7wc_8gl~|O1lDq|6nc( zI1-*~Z_1~rsE8nus7kMge1{drw^y6>Reey9phy>Zq*xYMyywOhC6MzO!E;PP_IBqv zR+>ZVsSF(L1#7Qu3Ngo|hXwUptuOhvZQc|w4cr^o6+e>05M z+9PZeg9XMzUq@K<|8$OBWxdA{eP^@YA=c-(CF3sU4!33x4zxmX?T+L?OX3V zi(MUgT8ebvA4TWT@7{g0EW&nW<9KcGn8(Y>hD|}bAGOAo9*x=Y9zidjX}U9gmhj~~ zNB+C%OZ03bOV9Zqn-$-hPLuoCy%~!Be%AOV)Qv_TS!FY4pY6}`SMzzOBnlP+GI^+; zuR&=`$OrJyKYhxCAn@0zC(ek9Z@&>9Za-LgX=OCo7YPk3$%I3SeyEnFJV6s^3 zPh>A(^yE_0Oi7&Yvn>rV43EMK3a=JPR#6d=u%7=s_4aLZh6@PZL~IZ^$}k>E!v5W9 zJ078$u?Wqxm``I<)9N#-W8OQj!;)ob$wt`C0Z8&38C>3~h@|w!Rkt( zQNgT(?XnK$j){P}*_Tr!!P?BEUw@x2=$X9D*Hjyymw4aIYrconn19MjXJ}iRSG=+~ z#pTbEjqXw}cZu_rHtCB;mAU+I&pNF@cjCy39Gg?Ih{3-v@`Uv7l~1b8oA;Sp@h5w@ zd#e{uFE9PBYpSnGbes@7l=gJjuI=A8w>_-0iV`kmjbojhU7~gseXP-)8@>PVuW`37 zdk z?u-5!-?%jEP9hfU@87>BVBl9)e8oPauAUP;$ja(R@nf688Vgfw;;qcv`e*sZQCfN( zBnHCwv802+#6&_EyBg=VzYN^{*WlNp-!+JG(Q%ZzNC9nXB|M?$dR9|x%u7;Y?&Wnk zbT4(g*mYk~&Qzt{Be8Oy6!PuQe!0K{PZL$yMu6A#JBCk|BL%VDV5?>%AiOIY*T zGa?HNbq{ubM_YXXr8;kE4)XFbg;a<4FPEp?{xTW&`A}-2_UK)10J)-yJB}(HVJvj- zJo?*nhuBa6Xc8v!f`+QS4=g!Gxu!YVPlZ`~ z(?@PUbYHA#!QqnKepV-0s#CsGi+(#f4>Ln!y2YI12>05h4y9$0C^bS3y#8-q8U>M= z3Vbw&6?)FeaPfF3I6eQAN-`F8B4prUQGNrpX+!f(mQpyn!|on{U-upDat53`vA^SB%b9S-1K-%axhbgrx9I=dO{iyf1}R0zFu; zs&C)PN`kayyssrE%hiwHDZ;&qGd*^R_#a@shb~ZZNWxVHYG%R$f8fP7<%COvhL%GpvS{Z@%DZV?wYoJea9?mf7f2|a5j@gPaaC< z!lsW#|8VoHKOMAM|4kvSR(*LuSB&M-@;=&eSN^_=u7V`<^iZUYPcFizy^~#jC$dbM z=SGAXEf)Q1N-7ELZP2Sjsjm*o-C9KSUtUK?ha_v-osX05`|nuKCtXGoE<3aG z+{SDg=_e0qnKsOgG!tfJ3@aQefgD1~k+(uhA3bVB&(iv;fu7!5;Dan3+enD46Lk${ zt%in%&Z{&IyFV;lnt9&4IwO3uqx*$K$?`7Ip^GwiVO~e5>7O2B6<%?1!XN|)4RzH2 z1L!wmG%AW<#&84<mo()(zT~Z#2h6EEw0w?#ezzi8=9k;nz(uB<-&YQXzbfC1>l- z_!#8|6hG42Be7xenA=%y|I$EMVQzX6T6f}(oNoQ_VOwYV zH^L!?j1fDi6`y57srKe#h5gU>L;-Y5dwUbAAkQ_ow(pv}osgbBGjAkRx5MgLW=tn2 zoawm-QNQ10Gg1i*8yiM>iWFXzKlIguq0qhS=x^^mhZRF2=DA*v52ZdRnI7lv_5SdGWQ0IC)u~u$o$(DycAe z+G+B)!!)eXZcdjfIzIlWxj7ROH&nlf97`gKI`Hj#AWlE)#f#5UG9r9@WT>)#1AhZi zd;8Sa{8ez0_EzD(ZI4rKlD8b)6w%_@7S(Rsp}#YsH8e3T2=hCfnndg^!Crv*%ek(+ zv0!eg6+Kqo%#}lTGZ`Tto225ci}W9UTsOGBjcK0glEcX1loy-Tj}{e6zF5bP&M_tIW%%QS z5%yl9q_)v>Q*RNwIB(u@pI3O?EjX>toT07w5{ssDJ}D_Wi1JRcu$rpdiTD!m^mkX^1Qk7rf3D2TVijY{!{qx_-Qc-o%lpWZvQi{zRoh`p<$cQ z@t2fHq=9@R2(o}^-znmxS6TlPHgxj*`#WlTf}u%_Ej&d=<)C-|Ey1n;!Q2>-vU(A7 zro!8(=8P2DJZ+)pod0l80|6@a7&(+Y|aS?zrvz%jYLw-C8fHza>T|QK57l;F<4q)X;FAVDX>2T~mL? z!sq7#FY%LYI6f9S$LzTsCjPddx5IK<^#?O+!=wU7v+2Wg;!aw7|2}f^w7bphSDLw@ z>{Q?R_NtSWQhVlS(}&$9Km71$7c+WJpSyJ5if^T?b|1;LY|WIIyZw+?yNAQnRHUXS za!hfc`sx9$tD_93q;+z-Y>3$Qe`B{ND;D)8kGSl48^U*)(t-4-W~xoJqmQDfJ?F$V z*HJouWuGrNhY8~zLHbai((FU~PUGLPwvAOzPtBOuHB zzW23;;4er9myzi%u)N3C;?+uhpnIV#gnY*^n1Cjv3!l?(c!f-Zzb*V959sPf(6A_k z$b57-N3?v0pIM6Ban5HV89fRua}&uVZ-UKpr9ZkE}mnm z=E5(QS9BIzj%<{uq9QxKv!hs35x3@?D!p_EC`+AVTFn{3K1c1=LS>N(D^yPx{Hfg z?aD9>;i-GVz=5hQLZ9c{>J)03Gojq|EjrQ2uShjDwwa>|YdEXCF z7K%^zGu!E zijR*O-It#+h$YP9c{+7!KRs5(h+2dMUJa+o?>64v-r${?h$m-s%RqJa4xwbx?_@bo zpC%vQrD%@11&e#88@4Gx13v4yf9bya2WV=xw6)b^-q2tROCmuz+x&smbji9LBqG>5uIP)4K5OZ(Tu5VY5jZd(4cFT*{rLe_XocnvW!-OL?6X8h6p z;k^txk3BQemRV@}xwyHFE1)}3VGucZ@PsLaoyza-fD*cdG)m1{0amyWa*X=fsn4My0HF7Z={`7HYzYb<`U-pK=8IQ zq=X(=Tr{)sE=o~#N-{a)XzdQt;&L1 ztfG>)+oZ&RUDp3T`J%z+)Tz71ksdi+ln9S1-p4S+{uXF>*~PDi>%1J#pC1Ngw$!g} z){|o9pQ1?x{vCSdwtRJ<<^j|vhED6g|KS0)33CG{{$yO>ME(c1AD`JlDpW0OJ0P<| zz%pX+tYDW3J%xjVgDSHYCPwTDQ0(EobfMtb>AcGG@5Jg6oqjkO<-(AJUk|5sbgKA? zZfU)?Tl#BeR_O+j8LV%Yud5D+-h1bA9h4P*r`Iz+{FqiQcC@z6Dcq!V#>*SHv5m1m z?B5Qzp<2$v>2YipdstZhQ5orNEH~MyB#2&d->a7y_e^H?tT&k?*(J(#(eI!LXbb3) z@86#~k)9v4wMjCn&yh*hGQ8kWC%9XS0T>ScVYf(4ryaWn8Oxc{J(+>4q8DGy$Xf?( zEPgb&m1Y%Z3?#Lcq9V?b-j}Y2PqWkcug9YONqLK`?FqNBh}K>^k9h4_+d=;-=iFG2aECQBiU2F^z}9h8bdvXfq{YXHTt7s zE|(FM{@?*`?#M^g-CFp%u@TG_+3YUqP^EEqUNFfLvvK{Uk7WqNi1dkDT*9%pk`9EQ z?@0sUGBliO(ZN@ARzOCZ7W6EtgI_~523^wmr$4a5EL2e*}f=w+t`Ym4>7r zkU0Uh1i=&&Vx78L48_<0HMVX^o6|p{4=DF>Xzq z6>Vc=BnU|WR)vR}+iM;<7#O=iLby;HYQDmwT6ak&i0;J-twWN*&YrJCNk{{(a&7%M zrY@u6Pu(qhCg&e4c|CaHcYDCaLYPJ|!(=s?ItKa+lbG*RqiZ?mtv%OHCRSq0@n4=z z-6vjp6D2^vx>Wd`1*KOEG&?E8RbNl;wY9Z{0&bA`-pG#sn+5Xc?47cpVo}Z^t0FQ9 zC=sUSvU97d`la}wh@m)b5WYH7K6qJw&nr)%_uccYc|qkf5vR%r520?+`6Cs(n*DRj zje&Q2-z2%^KH06Z>qnGpi`6uH)_L)Vhg^{(UOpOBLblx&)272sQ>5{J4 zh^WV;c?cNRng5EX{42mB0WjUDpr@3LJ9=t&Dg5S>0miz_F39rUE zXhT$3{^t%9ap>wyb_uRY7whb4CZE_>8RJDN6Y^CY$~ zzSz0(J8)SM;feoZ#U&(mo;ZD(rFLV$kR zMn-WZL#8})+J=)}c{dLEccAqaC*gKzIS?B6EAts52%$Xc^h!#pQDt!Qq=?>b#=gjV zaE~5Y5wo1ouxH`M^p3xart{wf7~*pGeaW<7=5xcPtn?TywB_zq{~RmMIXY@!0|F{j zBT2hp@odD|s)kONYLtv#n0|fozoR)^V^1kc5u8N-^?;a~PAo$JMd1oL2}Vf5kAyn$ zyk9Fed0n3*UgQPoFW)drJxW!iqkWxCej}Vn0exxM9@gY4Zys`!QpTZNcOZE|$ZUWk z*f>>uSiEuRk;S5}--gQO3V*(Frcp7EF0XnAq^p+}CQJ!N0xSSsCJdFt*UWnGV%phX zG`8?YR{nh_zVSl8XEZmjEGIiVt7o!M#|!~9NCYD#-=0-m9DRI5=Kco|hL|t#N&=n^ zg~R4Q5`j2n@g)HKE%RT#*f0ICh*7xr7SG~kzOs%`s3I`9LvW2Opi9$zM6e66K@(}? zf*w<~M3$A5)e~&-91k=Dbk-~wbv|9I{n*K_?vu5gFUzPS)=9F-m;5Z&Eb^*Cvb!ko z5H71zrzZJ#iv(QEUC@5lR}{f9mZn2GLPmdd-?}KFmY@v}0|!s?m4>0JG3MUKvTy35 zhT6LX{BCI0VZO@LGw@`fa?^hwimv#ZfVFzQ?pwPgkmrLyyN2d=4Kzb2kDJat8~De!pW z$UvP;iwKfLuD(@?}GITW4(83rO=#THAv7VE2k*t z(jE|bZa$LJRNLDxT{fxlMp@sJWmJ9awu8)ue(>PI46-JwRIhpGN$-&lMW&QAG|5|3 z+!0r&Y8?GiINB8q|6i&vOCC=ada2_#~c;9<;81ksqOO7(`jsG$O_gS(Wjr{-cC4~A0K>>y7 z`N43P|A-G-&xxm`@a~{vXz24+;A4GqtdoS8LO^bdh>DVSrjyCzT|-DA4hP85GMB^~ zkp%-}6d%n%^R#bD#%}Em}5V)zo=Ad=UrpVHfx=NZ}EkrYgBAS_c)L&?a)Q@k92`$`{ zHHTB*-;>$ruPM|)tn2ePd!n$vw5OUXeu z`ke{7vcMj?$+o{Kh7?6UrFv7c8b=XlEQk~iHge$2e~6Q0U^WzOqN9H(x7rwl+!@WS~M>*(F-Hh z2$4ue*f~H-P}~ z(CT?XeA1SMh)86llH8TwyLy&0cvw!6<++u=U7KY5@hL4lRwgnc;tvBygsNsB<(r@P z{Yi_$(ye^jTLVnkYd+lv7cv|Zq}wnV&~C^}X}ObiQJgRdRDKWDWj!cMd+!f(%=)Ko zU%v>k*!ZfmT5^j2DG)zuZ(}WMmlXcs&#e<>b3Ff=3Yq*j0}kf@q}9^1mgRe1PA?O9 z<9^23uz(rIg<+uNcZ{ab{-vK?%iVTDVwG~)QD6U_-6 zZ?^tA*%hOj-CKp}!7BUCM6%F~+kIaW-f!Pz-q*F0B>j-9_}iZvIe-2POO^jB7WAF# zZ50Xh6OvpHx^QV>;gq1qp*kK}R^PQfg_{>eGpg+l+2y|aB3_nPNvoSI`PRv7Ll%uf z5(ck-4DTx(<`WP1(gxW>_Ah$PN6)=x`{n5X0LR!r4A=4wJI)P>Wi)qS;gsF_cfqgZ9f>aom@v)AtJq_9B`P( zZ(YY#*9^maB=IDmdPE)f6Q(l|{gVBj3uYj#36leEx7>mPLJ@Wdald%p@F8Ht!Ibc* zObgPYyk6C&qYZ3TQ#_%Jx}UtNtcf^n>?*KBjP~Br3_kVfIOYh57BVjt2YkaDe%6n> z>I#%jOVJH{2^UhCg2(t0{O$o8kfMLHnxcKTDCEd}sB6Bbz0-0mVrfn{DiC9Ge0{?D za5Bxs&kLt%mj+Usieyy^6kn#)|Dec^r&i-E^jj`nS?+T7ol^;FPQQINBDyqy;()IpCfdg$89e$IIO|E4xi zuNL(T!>~8Os4D^e$OXZ4GeY#@;A*5Bx;DQHKvBSvV9Jo$MySaXFx3EdB~b+)jIl4_ z_U)tm_c5`{?GyggXJ=&lw&veyo+bB5`-+tGD#ug{>`ifV+6J+5%OfUJ@^2`!nKFI?T{!v|z2^STNKQ=f_ zpN%fXt87w!UtkTq`O+k9zbzTo3?iTn*Ar}jYN-{itr7+)y9+!;mG!)|vsHqb)XD$# zI>>;)zZG@Nl^b}?<~OL+?j1kMrTp9Mk2IaeWLQ>vT3EYxh+F8E9+Uj5Ir)!L=y+K(s?M-46I*$# zHX(ii#sK;=ubDta@_#0dM`~ImZfa~CNr>Q=$$0TX9{L-6YJw;Moc(P`m5lPLWZ#7a z-y;kE6av=|;U&eQ2YwLcUxvl^&~QYHl?{hYL{yZS;NJyJO4FCdO1kM5_Z;)0u6Z6? z>kCY2c%^D@Ze@3xR-x;ZkvA1{Vh?+wy3508hLnc%jN>T!J>PC3UP?-%y1L^j_c=Fw-^$+$xoz7;Yo z=XUHCzt>X39U9p_%BpZ_!XVG$#=TYGYk{X0y3U=DEwR@oc@OO z6!y?8kJ){zF<<|Xr@+y^7e&Xzkapqeh7J}U`Twv#Os8CnS7b~z-yHFcpu`bN{_5*v zu}20*Ew(`?7^8CS75XPc1bKB`Yz(taM!O3sOHRp7DS;6uENyOI~g{wv@jPoCSK=g)KdVUc%<1)`xQjygX)#-q0Cx;w~s?l%pn zurg!Fs9dspU+G55pTS|Lf!Rd}JhLXl#ZBp(6vkGmWLG5UcQ% zl)<=Nue(1O>Mx3mLwJQDQrfhee#Mn40Jlo=!f48M&5yf61>%i6(0=#j-LhHgM!P+5#&L4r9g2zBSz`H|8fVu=_UJI1Fw*9J8J%^<9b;#259O~ZTFQx-Quu1|)gm;@dnc-V)cpJo1FhJP z_&?x8mLCjVesVhPq>!P*uOY-tDBgW?t_}0It^9pr)GsU)0o=mI((pE)K@TFw;$rAt zLU3lHC6-rNnF~KSLY{#EpEOuol{9NvZ=WZFa<}*U#I)G$D*4smD@Xl=9HGW zFKFp+a4|~-GMjhT2Mjow<&RWd_Tjj=G`#XaujJpF;UVMG-s0gs6_G7(wq{)37)$Nu z>Atcf=C*Q}lndU?*nQA| zi3+DZk@&koH;aKa?BR%i{XreW0m`g2<=qmVAvj!B>0jzsM-NAu&2`)7ZYl|6UuSVA znnl5tcx|etrs^ytV#X5Hat+$HZQDX6lqVR>Z~zj6S#K}UQ+#EnbGq~t+`Lf{A|boK zX~vixIH5-oO^@ijzULSAV5CY&P7b?shZV%HfRIr4M^SjIkYPqbf{3@SoJ(b8*$prs zl_6{%gE8Jq{2Kwj_+x+#&Rz(^En9! z2-qyoeqDj<{McGR8nw=7G}OcD>SSQ=_Ih&bO&)t7@iAq0jKjs;W9d2;yL1AWY1+Vu{Dj7)swIQ?;Qtz6LMaY#er`z+Q!TBn55`3$ zCG!Ik3K^HKkTJknlT;{7U!*ETNw%nF#>kVKH*hLNQ~vL79ENk>{D1#@mUurIpxE@+ zS9`eP!|yrwe*9>nF^L}liRy_Hdn9)2>o{w;TxUTFHRKz_uL4X@2&y$r7ewbDJ1{;< zX+rk)Bad4tDVpZ(kP1ch^_|4e0FslGl$4gG0@*;hv4Q?eB8%d^Li8nlx?=4&JWDvb zJ3^9l#Fnp=UD?LosxjN&pEirfWR>fGTVKQZXZ$>UlB+yD3u!O4kHG9)`y6;e41%KSa3 zaI5Abu-e>Q0pQmwxhVj|20jDEL_#_KO3u)P(vwvo_b+jpSMbJ%WviE=&<&P*7M9AX+pF5dt-L zd;u{74`XA=pJN@&X0Y;nd&5jic6e7KwRX3x`U}l<=tT(+J1juQ>~1}aO)BK8P%;sS zW0R61W0%v`G&y^iGb}pv=$NH1+>Ir-d ztZUYSmhZNrj+|-zFK0jCI#8ljMR5}Xoq#?4>&wfIHWr&4xU&KH;A<=?@h#J`FZ=s7 z*P-HGtqwb%a)Qn-pG-^#|Jh)YI;z^*+JHT5?A{0t5{p~7*1&OZJsm&<09b_oL4`&c zs@iis;tFkNdf@Gh45QK$t$|mobjhuH@@Wswle4QatNB8~AzB%oS)8O3yUFC`RR|!EG zrd*8fEW_%L4)35a>U9{``rsdu;(x-Au88Fv*|A+TEL>MXjD384gNF!X_E`})f>edQWmDbEj5HWn`3yYTo8MCSh2B=Z*7^2$Y0f8Ww3gXlKXiK}P_9ea9? zH%hp`u}}h@MpVh@*qCWz6j|ujtLUf`;qA(LGi|%)&?!^xsrR22n&0$b3VCk@Y zk`cl6UqBv0Mpyo=VhKTXes?)sD!g)XpQKiIkkE1-&h?!!8`VoGjx@RS>Z|XM8+-mB zfYihm-srWwQ@TAxeMx@=>@L9T0}^6ls4hr{>#*kt&GCO?^d%&%To>f;qmc%mZC7$@ zE~}|PY&G@e>h1tfioa<|VKF!EWbeq%&7}yZ#0F+uK95 zz3lo^?%yxg^O!6Wnb&9X@?~ZWpKlBM-)#JaODVfKOGFm%iNJ+SrNpW|dae21-*|f! zCAXZt>0dS806Y}jZ^BRKCJh-1C7<;+I)6StUPtZPWjq|^DCNn`&tIif@PwR2&8}IW zPxR!x^RO|99~*pbh#rw}g;(0e8lmL`#dBDaZy`aNCNvmiqJW@amzO@jS&PqcR=)9F z*PpH4eWs#-P2l21NrDlC#z*f(tl8P=tI&tTSxk-T8UPEMp|^~>Q<+nNN&E-2;$S5~ zDganV$PXt5cQD@jNNx7!T3K#$o6_$$6jHcO@Lo4=csk#9`Dqlpm2hv$+@-#jz2lr8 zTNj?6_Q3zG4WT_+ZcvWx%E*ZJg=>E(=7@JMsN~L#I$+Bo0J{C?yyHU8oYqRqn=@8r zzckJGonEV~78Dh=7LeT$iGuZHJ~AybHueUt37r2Rsp9Y_TRu+XsUUIOb-BMT@-#z>ef^c5Ez&{f{6D_wyE2}M6_|Jes z&(y4^7UL!1+JvJNOcD`5Hm+=9nVwe2taq%`>4EUHD$!Fd3sb#xa63h1_L!Jy`-NeQBBG~ZKG<3H+ob12%40*x zhlMX*+{)KUL;za6#cP0^-t6K$RH2aT6TWRC&=hR4ce%vAiuU}Na+))bj!y}zCO$mZ zziR3l9 zFm4dZOXS;kzJ@}GsKZ!C`aBUcLkxQ`p;`O%pId=s-D7>_5(ERtI6rRs3{WDGC?I5b z@A~r#y#(IyA~rITv~%X_0*Xj%aUmV@nP#p1_mjO&OZn9LlIFF919`W;(5*xXG&m^I zo2k8@TFkDjWF@E=2?>CMRq^i3(T#RgeRXL(4QDj=OJ$h+)N6H8q7N~s=vU4i)-Rh- z{@b4zs2rBp$e?y3z&*)LsR-PeB6ImotzUJ5;cQG7oEtSDWWLz#=$4?!vGw|WUu-^V>HF7<2`1yQQ39DoJ^ zJ|b8Pp%dDJ&ZYa)fkkTNW~XK{w2x7QrX;j~_@Jes5!O+$5)zZFo3eB2Iny+&LP~ZN zdciv)%gwv0ssRN9#Lubx4}^^z;;6p?ei?sBf>u?_+xbW&-cSfWzb6zPunM zk&EJzaiGM64`()fJI<%BdP+`mBx&V9MsV#~b*8Fz(*Y!hq0#fdEJ}#3NJ*fgA=1^Q zkzaoVUpJ9Hf-#9`La1Ko)(7Sg5gGXgY)e$m(RjPJ*QtnIQOG!BdO0mo!58L+KS+0f z0G%b~cPtaoUg1CR1vU__?k%9ZcYuNctMx!Cur1^^U|>YxQSBXB*ys5ugfDe;m6?L{ z(xppyJ^?u^JBNi3e`I`oIFI-3NZ~HyQQxJs?O|${>!A5`ROoXIlgmCHI(?h%k$yA# zFV-=!H`~HCuHnu&HZ~i=l*y@}Ciqd$R${u+-|W)+N$(}gmu!1!q~8i{V88N;_BzB@ zNYm^S5V6DYr=5u%!KzHSziVu)*kY2GqTiiN?R8%0W`5K663alJt0r^(J(bQ+wvi9# zFzjUse^xSoX;Fhar+U$VF0Ni-!~2ta^HhaVL#HNZ_Q-a=D@&Pw8o90pCD1O9-?{%t zBX{i5v@6}rkw^Zwek?I;>|g(3S0?TMD6V5}A$AXsqG9~RgfFZ1>8Kqlk5vySHrb!^ z^`9-ZF^W<%=-zMea{hFmg?9TDxoh!_@$K3f(xfj719d)Rgera$QzI{az_~?nVoJ7_ z!Qy2xs

A(*5UFM@5DKo)sZdQB z^M93EWaD##RFhAft6W(_;H^VT0x$NQLxJm+XB!Wog{oL8ub=?zk3EP-<&2}F>xlSm zUOQU+iSOXSJqU9rLUd-}cZXmAZc6QDd8q$f2P+wRt-W?BuY4!ZoO<8d`rqS*@H7mb z#BVh_SVaVe(larI5$7?O9h7o3y}Ueyh`IG`(A#7VrCoS#N!X*H%4x(j0RzZ(0CG5Q z{FK1@5itROKqcB@xrSn_0i9(em{5z%3zk;??K+>`KARkUl9x-NB%+>g`8`N1L=lZl zbmb7n4hswB#bCR5$HwZ6*M5f+=fxQGG?NDcTt|&~{MhzFXhzQFpkI+5!^0QQ{3Iw@ zWn!D+-P`vL5G%Um9Alnp?{FvGP@)+rDvpt7$6!gSNKbktBW+!&TSn zT3e+vuO!B%8A4zFGFj*oh^d=BmLl+>!M=2hee2%>=lbH6Z4n9s!H<>M>l!DYdVl`@ zm0hy2-fH{Eo4otwGsSzPk6A}taNK0Did&PPd1w_LE|ngA%ut%QrtdJj_}tk1!3Q?e z9P4f0hAUKL)(^!Ae|XLPZIk2dqm$SD4?O(y^HE*X{Log3>}3SU%~mtd*T%dp-P+u% zY~``*?TNui{onsMj)rd6y!>{tb!27!9;@}zsQ=3J(;Jl?Pw0LR1+Z|P4N`acb@+3H z{XO25nPx?FC=6^o-8g5^+qK>4(C)Y{x~)Be4qI0)%xS7QP#$Xcxc|tZHO8nX=vbKU#qO?c4j@9{>_5w|PTWC-3@+BaivE`Hdu1a|jq< z;5sRBE5X1ckAYHel0KK)x&LBbd13?-YxSVy4C>QKRBetJj(qZ6(PuiLM*GQk;aZJQ z9vj4QWByQ{v?c);5zeb|GwbC`Hr0y4rVM>cTbe@*m+j^6|A4*3(vrHgSKlx%WY(e_ zLPZye%`ZcnySW1v7+~@#yELL*H?{gZXiJ~}IwfEaB2f*o!W){;0f!T%BFY;$nIJa* zuXyD^$B{5>lGp<~$B&_e5aR=&8IU%b7|aZJ{2HoLAudt`daZn@u^i2=X{j>{!LpO6 z?A2YD*NjJvplkstV3<__c>)I!J;-k&`bPDN;KA1?-Yf3ac`oCuqO>?^Ml%(Zm(Q6c zY4~Brr%l_VwWbX#>ec1NhP80YDT(PJfz_LN1)_E}A=baxI|Deor&N+hMgdZdr?G?% z1X;(&hBJD6qT&ADI&>yd-_bg5>i7DAGeJu(tg<9*yGN+gX`d<|_8HE~_gr85(Uxc* zp<&^`ybwy2BeX^P`^06E3C+dkE4D7P`*RzovSMSjtEK6BkG!JY;go+u*gD$1sZdQf zBJv}jc1y#@tK8q4M>a>2NBUk0|9N#H@NJ85uicuX)c0C;M-*e0HoDQ(M9_V!qTzo_ zn#DQ$yyM5gZ5j!M{|`-P9oALWetjgA0a0lLk(3T;l=0z3#O>D_HVs-o9#QqgWYtFZhHHicM)w&;zqr3fvu->Bm%HTjoUq+t*D4$Pj)yX}Zwt?+O>U;5Ixm}$l;+Cs~^ zBxXkYXQN$e3`=w|;GW7d;j^r!`6;d64NFw~=Tj3^CaYLyOG39H8`vpNbLQ!Yp7 z|0044epZ8!v2pV=;j1u9S?N=!<2OXl9groXt}`VK({2hPsxPU*RDQ?nCEvhPDxMRe ztsJN|ha}$uNG!3|a+FMpACHvMhGyUxm3i4E`l`qwrc8(++qPb&R(LK*u0xU_Ue+T* zout|kuey`G!r-?MaO;Vxo|0ggZ{<+Qb zfU8r(+;|scw{Q4z(IFWVp6u-GCR33vhy(~6ec;Lh$BkU{dr8jvO6{qPe@f{>i8tw+ z7|zg0aOb^3EoT*)02o1kENMK4rm7+?L;X)iy{{`PA(=^b?ZeAtNInt)Iu6OjI~c?S zc3${~+Zoe*k>D|G@h4mIm&6x*mdtfFmef_Xah+K;KR~vaUv6OBMb?r2rboy9V;Zh7 zZP=Dhc%OfP^I2xht9r}FtG6_F5>spLeqDzb!`? z^-TX5*MwzkZL+?>wQ1%vC~B+0w_VuKe!ORZ^PV>+w!b~o{L=%OE;dJ9=XUduA+nZm z?zMr`d`HN&E%X_9JvgvhRCs6Lm~ z3t#QC#2=pBW}n(rskcq3d&@C0j(V?|iCeR`+cEu2v)XBm)|YGdQ?`llzt!*;2FNS@ zK9Q0#w!7s$8fd@^cdsCs)(pwKTQMXL+*}xZ?sqd5BW5`tr|tcfrr-p0L3T4#hLMf- zlh_sKHTB03&`H4A0*>UDe0-oz<+{qtnl?Cebe|uTT#hiYPpEU^EQq$^4SfmVw3>rU zle!7{N}o&_Fqz8u0Syd?>nH_piYf4JEfUtZri45Hh0}&1_^9Bc2KXZAImj9mrQQaE z>RBKW;;al2^w4K=-wYwr=2#TH#82Pdb&e@5`J=439aj5xfHLH%5IcrUt@-k}fnqR#H_X3(kshfqXd#{eJg!?#<9 z3nUVR0|cf@9+cWJoPR_@5)5ty@UbpdOxd&m^ePaVIiYk|@OPcdAsFTe$r%{BcjIZi zdqh6kKT7+iOo(cDQ|1l*W=!k`;bB7&i~yYu0S8@KORcsSDcd% z$eITgk<34DZkp{S8ie}NKg0fyo@)Q{xNzOlTJl{JY^ncuW^QL>v_VUi$H&-^3 z1H8D6AkEJrQ8|?PnfK==O}d9pmx+KyAR>e{;Bmy+esy*)1U~~iZ?uK;Tn2B{Uqo8Y z`+F(zF8>*^j;$BkX&8FaT?bX)hwaQ{R$}&pUVEogY9$9eQFfO>pU+%xnp=Ovn4( zUjp{l(xEuj7Nhd_#R(bu+UFmUreK~fag?bJYdXJL_Up)qu5 z3KL2Q)50VihPLqX5jmA=?Q_4Ck+FZvFTj6*3BVl!>u{Zzu8)phs*+7u^G_B`M_7=EXzEX zf|4D1eO;LLQ`ir*xcA3%d&cboXC1L5B`$qN(}_8Fvs4uBkG?z%m2`8V|3L2G9)!;Rzud+lCR-^q8)UYJ@bgQCk(JsqyU9z+NTh!^OE`Ph>4KUf&};-;cmZ zNylCL$M1+_WJawz9H~)nQcGO9P|T6D#42Um6g0}ZNGT#BGt}Dr{AA4eCe+OkyYcrg zg`u(_^j0kT)@|E5pG4J@d7;#pPjdB7%1drVlx~)$!Hob<0MVfWo);L@qrl^ZeY7!h zv0kPGL)216POb&IJY3y(g!9a0r^ZL{vsD+FsDDV|wSB@S^dT269)rOZ?8xV5M_(%- z%LLy#F-J#E5Y8S!a=)`v8n$H6+aZqx`H(=CtsWS*0t*LpWH62nb-&pnqN1c!HZTZ> zfHl%gh6;=$KiPcN<@T_KgzXWp5yAEC1mEG=_a8^MjZtMX!&z>_et#lW%`Lo*(6sgL zs61*FdEzu;kGGZ|)}Nc%UllT?H29db@^O;NpuJ-|*0i$7J$!7D509AssHQ!d{OdFWd@@1BEBu#S9e>~`-z>u1&A+j(MprPK%7J>RUEE=QO5@2R+WDr6 zecj6phqh;@mZh$eU?Sy`7VCO!8pl!IMmTeDIE7o6dCY z;S?fb3Bmf6%(<2Hz8ddJo~z*eE3D5KX3ujq=C`+6v06if5|JxPjbg5)UXRN4{V1ka zO>Wm-!b-0_RNtRf5L(My-8{FOSW1e;v*A_w`axfKCg(S`pW0W$(GpZZmGu%DxQjq` zT{Br~zn_WiI{ZdUt zWMQ1$5Hqs2l6AhlQG>(uAL>Ex&)kHEdozuyj} zvjooF`jtNZG9FZY;MnxyO$rC)M@}mB)Pp%%16r@yjjZ*_3AOjFG1714X|(6MYM$Bt z+SuA8N-c_dQb_5tqsD)Gh&63DR-IIP%zd-F_|v4_*}MY3)Fvv7k#M>x=5)68Jz?Kk zP-2Gj(d1yR93jVT6IYggt*?R3$fsCpHOkp*##dKuqpwU?eFY^0F;sai_J{O-Mnzt) zMB7}1@Z6l=B{^!GKlZx)@`hMAs`Kl#xXdLBf7eU(NouAS@vTyy#%_o?IBeR^)#>wh zN{&>oPn3@*-q&fe`Px_KiGQ4=zgd)cYVfov%goeR<+JHoan-w>!>a?Ov3>IqGTy1$ zgq)kBXKb;oi?Q04X;I}LSJ+p!EtvA1{V=cD$6xZ?hWf|mo=^0|Mk(*3OflA^YDr{A za!``i=lCcAoN`oqsgrHPym0(?t&)(E{p4GIPaC{H5?!@ktHURQc&5+N?4$P~liR%M z?|)d^(~&M+kCBdU76rfq;#C$`m#!1vgVz9ZtUUc-V={s}Q<8oA zV=x8$dN;+>e!f02nMbtlnFGxS0t?Xn}q# zXr1K~1;^O3vJt5#u#0E-KSV^DT%RJ)V-B_`N!0a5{Ruk>HPA|X`=IV=gKY`9pes;WxM$iQ?vW_j5J zw34@Nsxh(>;N1krsxib1_%GHo-LLjLs%CZs8(BpQwQW-g(btk5U6sn1j~S%CZm#z2 zjK8_ki)Q5T;#~U*c!L*NFmXqd-;8Xa*`5=^(uP&k%5LaK9!nFk{P)yl<%V#=YdlaZ z)%R_$`i_T};HeUqaMawL$>D0RE`R48*JvKwTHnU1$j7had=;}Laaa3QqP*_e)>$rv z3C#^;7mqFs5d|ncZjI~dT}R4bUEVp5Rpq?Ze|qiypvNKK9?E2{)Ovom-o^XEsDCoknqb zA$2xGSb2Ex8Z+K$p)?8qACUw)ETavRC0vfG<=3q7R2}bcYzg-lZB3&{a6JoxE9*l) zP_jY2A_eM@-20iQ4IcZu{IzS36gBRDKImWm<>lT+#dG~iTPdb{1v7TgZfu+1<*<#v zi@d*TF_p>fL{(nsyJ1CTd8VvP+lVpJ(+Ad*zM~pVDI_#|-Mpenj?CKg67ra} zyM%5|INdBK0|Exp3=gcCTo><{(*8gq3=`wiuGu5&oik7}Q^(QvBRu#+tF-4|+AOc2 zp}dX;BR~=`rS~Sihyt-|dW{Q8y}_p2769*xoLumLq#LXafZ*X^V^e&m_am_&pW*Db zCJ(1Mp#Rfu@Q;=%+CCNo5yv)~_!!$W+S1qoH@P@&OmPW`z>ZGqBj3Zd)m2p;9fq6Q zh6a5ml!s>p-nUf%FcozbKmJ%2XIJR=A{E*fdsBtxq@-~vKZDNDH824G7KD}!A_ib? zVJZtaqxHcN!sgH~n3~M;uHiD+DV0;(h}U@9Wi7e2tqm>2W^-%H1XOeoRJ>uD2U@%$ zFu{S@0tc!fm37PV1h5vt91)TAgziL@Oi6oiXhcRt47R+-sAoT7r(68;{jqInWZ&r%cexgJguE$-7A%n)`|B=H(C_rbw^FyLZN| z0oYIYo&V5wOF#1WT>=FA#KeSAR`@5!2a=f)@NEEv6OdzYL5fI8=|{kRzk%!ISuOj1BMZXYXM}zEH$7TngoZ>ZyQF7G_`r zMa+j0+q#FV!710`J)b!g@32^XV=w!c)6x{>I@`u!WGq3>Y<4O|h0TKw1&xGv!2D;0 zVuLm}O;C{{JnfOQRe|&ITLO=UAS_+(($!%vyUpa=^ol)pmVQlfax}l$Az;+* zt$Wps*IF>9V6NKxt)O~Lzo;m&u;m`><#`BB=RnP{jSy2v^wD zbk+X+7uDG$ocrO{X<=c(WYECRgxzWMvs05cG8pH-Wvb;l!KBL(b3;Rv znqQ$-0N_%8iw#&$bPWtxYYCdr%V&pjALk6yU*Ml7vSLzxosTgoodEp~aJ3PLRrBhV zG|+Oue};#0M12DZ!0>+pBpbYEe`~y7+29zer*;O z#r8q1d7x5ugnz@iYbO8V(8_f74B1U;W+3u(m(pO+8xPMj!<@)vK<9zy5#x|j^m~Wt zXLSu;Bwq2km@}Fp3#i6FUp3zh2{|lFBbe)*@MeNWgwxo|!*#xDa9Tm_MvkmOG+Ao) z5HeX0%=wx8Il1W6gJetCm%KBueJJoBpx%;IAB>9PiJ6+x&_v1L5umb?t%q&VGcX*y z$RXbYXDoOUm`<3>ux|($0s%RL@LqPA7(plpIGW0unluUOn1UQ&Oh&}CODC8T6fnQ^ z+`L=x3Pt|UG@T(I?l3-0E;EwB$&;pi{j@=RvMprG?|um z5q}0a++ZwyG~Ew?qv{mqj#>gjdcZhbfQAv->BEO#5U+%Kgs#Q5;Pds&aQ7dJXfKsY zx^fHg(V(|sB0N+RAxbQ1MOfVdPT?Jq`^eI}o1uJ&EU>Sh; zwJHp7p}#pdR~k=b`7hfOv1MU&Yp9DcEuAonq6>I)lfuzF2j3a+JiiAA5iSfn!MtGf z6Ij#Knu1fMX5j;x&@T929&Ig!Z88nGg(hnV< z?ivK(-Iqee5o8n#TpN1-WS}$Ft{*>)?Pzemk$dPJZhy5GLxd;>hLD8o3bx~1|2QsJ zf61P+p5u!rxty|tCTM=f;7yY-4tdr#C0a&plSRWzB; z*d{*JJ>P>hP>iy3b318C#U{3AARkK|3E>`b_T~1j zlZTSE^*O3IDIPAaz_s4~eu7N5pr9bcE&~c)^NK@6e7wx-*U3brT;TorDVpRs?GyfW z#EkCMac+YHry#psR#4J>EOme-8U#Qc7y766|{8#$Ox0B_;LZD)Bb~2W(a>4%8Q;z67qTi?PP< znVFe$3Qgg6H;!Kmz39_~)pwZEe*$+M+=2lxX*E-$aWoqtc>>`$VC~RR%3Q|g7ZCL8 z`MiE_@s|6=pz^=R%eDth+giDlW>89M&IyO5K+yn*X%!U6hun?Ol*cr?LUtImsoxMQ z3e_)OL&!HkJQfg71KCp>$XT2oylatytXlL+&V}ILL&GG1E-Mcbg)zBt~<|4Cd^Ak!N~`uH>tK2K9^QO~WFOJ{tz zlFlF2BNp)q^bsr{zco1?h34;<`Na*32({hl=h5?*UmC6@<#pIFYj}zvpaqsL{pqWF z&2yNNijOc>Ur-8YUf5PUMcDjW*%JzH^a>Xma^$_UaRGLpU_oA=af&!6Ehw)X-hdkt zATa{khk$ATl0%TibM8ciI?W*$^u_S`a>}+_*UmyeFO2Bv!yzs>ya@WllAoa)#c<-t zoJ}=45ISDc0RCneg1g4-%=j^`Y8wT`4Vnv2dB7W8RJ}?J(FXUFQ)hmw*}8bRwk3dK zmwMw9DD16DcHM#d&!Y;iluupPDd3*R@GYGX7&@y^phHmKESj8OiF$y7Il?mTDm|y` z(7xMzO&34d;0m$>0p-sf7{o?1&R{b(lYpJ%IiXRqaQgoe{Aa(b4iK}|7{)> zb$0A);;@oG_(efg^5sW)+2-ko-BIn)&7TR)9Q4WLoR7zn6z?X zpJ=&`6Lr7CvOF>-rVT1E=z(0L{nGhzWNh1R>yx z8oUM(5Tn$g#+Ju?XF)FsyNYSrrxz*z#kdgc%BfIja7X3jVOv>sbN@s_XsOv-BJjHRrt z-39N?cuT-*NW(@&wLr_v+u__WO`-$BE79^OG2Wc{K(r<_syA58`}Z)ONuPOfOW%^& zFvOEzp>LxtteN15g^j&JRbpsU*AYp-B{CF!G=H4(>!Fi8PPhR_kdy(*IrtM}X4Ks5J2h35? zD>BJPVr_3jqa0D{Um>tr)HDw%}VMTfY(uyxY9#(9!ds z!h-mHnal3JKKxYNR-ASHO*mJ<&kPC}VdjkZ>2?yQkJQ?2!Ih_tGZo~uR8B67BTaG& zM)m5wS#+cu{AARhlH&!sg__`9fKUCS;dW5q;2;m&kwA|g2{e&PBoL=r`{iDVy|EU} z)|BC?z8N#v@kBSlV6OjuGv)#H5D316fNC>ja+L(nsll_1^|D?0NV7Gvb_dkJrM#S! zf7tk7{A>fIxE1=pRQNrxhr_sW7^CB;X^l{EynQQfA_NzXCbWlwZ9f(&W4C6Hn`lDp~lu;m8DtUqB7t#Z&iXUrqPLi|RsTXOw`k z_DY2(J;M{ZoUpwf+9RNn1lTMvjYoBd)H?)nMIgrr3KRbiNL|p84zDS@KS8g*<{f;G-mVTV#zlP6O#;KSO=Qq~V^2pj3Ev)C; zvazIQ!hJ-&3`?lrhgJCbxlQf++IXrvCmr)wSKSb$uAKGf-Kwa4O%%8MoXoo2u~w?I zkke6NJ=JVUYej>BiD_YLODNZMDRm#6g#FD@gHam>q|*S0wP@??hfZjbO?KyY3<(m| zO>q6SuNCWwSACc^dx4YevKn6pfC8y)C!G7xXbN~|1RxAV%f%qZB19xeAj2W+5bRdi zJ*i`(qj&yyZy1u5c(iakRE_zi3q(Os;?Lq}E=lg*gG^#Ack^NW>rI&Cv$a|8%{M_| zEfz?KX(FFt7lhbXl41_s1&rSjVV(>)tw^T|w-1Ppz#Ri5zRV8S?uAqx{2$Gl2K01v z^J_x4)#2D2j#4P~fc4A}QqN_04|WfJ=-QG`><=9m+L*{Lh-JLb!7#xQWsihzu=1l8Bg&~@zZ|N{O$RAM@yv&G> zwR$ZWft-5w1h;8@(6opYTdqPyL065#z^Z6IIY_Yv$;l_s|9iH|BV6ylQxb2TNNqi} z+G@V-JXQM=S6%)*CFxAsXjGrrN@M!ek90vQd~8&)D}Be3GqgSCu#K#k&o2a=~TLZq|rb^UM|GDsO&^{z!^pDP5Yvk^Oz3w}NuZ2!> z!(>+XH`Sy=N9V1rjjtcxb8%3^bm@ey=Z# zM*rIJp`3|i{?OxiH^sYJ4?-z8y2WA&zxmBYA4=38g;K_=aislaJ1=1JK{h)7_=QjLcNdqlHr0mpEq$S$GZiKlM~A7#lhQo7zCTwK(|m04 zZ1%*{+oX72jj_5s6;A@M0s|JB-$Xna*Y21lQzCer_`7(3wD;LJb5+Wwjjj3L=`H-_ z1qnD0(=hB`k2I}Caa*CEDoLA|WPEcDjh+0wGJUj9(ft|PJl1(vlZAYHmE{Wf5_cb@Y#%-GYO z9)f1EJImeW{X6I8IL=q!o~SlVwcA$IlDEFPi*u8hY$r!4$MeEL^sA6+YOKN4A|aDb zS#HO7c{+{lRBq~!{Os&!R+$FXSgGVT=w3Y<1}|X^Y5z1rBUVUtkiLYPcB8f+d{|VW z^=_wkr@!t_l-`3A74QM!(+~1%WLzAgM+<~An2o|bkEvn|EGox%sDzO11-BOr)jL`LN`NySy$Pbw#oZ7*cf zu+IJ^Xc%|@`cQY~(c9(4z|U2eTOVX&$5i}-&Z2qSi6>n)6CFM{X848i zWxsVCy}~x%&@;f7C{p~Z^IDIG zKP4jJZiV9=2GS*VN6uKDUzu_Wvsu_obVmnW|2(eh^(A)JDWyhzMgpmiO&UMN?w+*v>R3|J)KUK`LnpL!L zy>w097LwFG*}7S5N-hPDMz)bmJ*hxnq%|Mkw*4wq=bxmBC_VI4c4`la!2{W=hBDTI^n?CQ zM$*hS1N*_3z2E3m8w_+J3bU<-V{MqThKDK<(J8;P;<)v1<~%DZ^8>WX-_w~tk4z}- zf85{qohC)QCN+Z_|GPL=OrexcXJL3{cKoxsnEg2d9`O#lHkbI`)W7Z7a+a+Fi$NIz zS8sGrr>By)okfc+JAdBCgYzS}8etNJ@J5bWV!0j8-zFYHWepnsXJ2%GkTJnGdA`Ex z)x|UCzNChQQN!a2Cto9K*dvYLSc_0>3x0N4^IucF%6it34N5jt@m?=6ApZwz6C&FJ zAXm86(IiKykK`5?6Z-uG=jeys*=;omg_;M=uFcq)rXruPQ*bX%3AwJYer1m}l$NS} z5L^%+f)gzwv?nHp0)@`_ZN85=OiSRykEoMvGKAD7X62NEt82|V*0WwIsA$6L!jt_P ziOakk_VAe^I<}m*FV`Ii+QO2CnI;IjgKJ#HV?DLI4RRj!w$b_10?FULwF42UAEcQW5Vyt~`O&}Zm3F4u{Vf&hFHW=j z!S>siyf}{d`_M+`AD5q_ycYj``+)zrWBQ8j#EN+}W@tyfaHd?};qJqgX$l>|n{-ko zH%5{N==XT@*xKz^H}3vYFCTG1Dkij*Vw@v$Ea6da_e+|v5odajc{H*+-J&*?G&3kr zVLF*1b>Qzq{beT~tW4W{i8|xz3u(CHA4=leJNa!$V4(d=?8)8+N$-n1`9P~hGPwr_ zZ>Ln4@_j^wmu=6^3QTBzBsjYoR7#h=^Fk@lX?w326vRBLKf<=36t(!Kh2Y-yq;nB- zh$(dE@RyeA){U#6oNHI>@iGNf@;FHiyG$k*Fj%jLndV#3(jRi#^oNNU==tBy@>4R0 zlKiT3Kk2ebnc8JUeRn-6nJ`>sQxaS`%VHccgUP8joxawaA6Z3S?nA1fAO9jRfK5EZ z>0`;vr1($V+k{SWrJMKK;j*AZiF58cAut66sfR-5yqh@x^Ar^qmWIrH)T{9FFz zk-WRgW;ykbnsN2P)`}LJI^KOJ2W1Wx9`CaROHEGaJNS~1yxUR# zXhTXw?O`>bUgLReQ_KkFpX{6=~+bfNK^ah~S3bg1-Fr8$isiiEJHiB1lolt;FN%Vp$3`Z#7@@>l z?b0JXy-li%j#b~E=zPQP@cNG02h!CRGRuiSL|5(%$=<2SF-L(a~d3sLs z0v{sY@{>#Vm*=LN6fCI%z8{N%mr65;1*b=UH=N9Csl>*$_L=2yJX28m^;fP`SD{tK zOY+p3Nl1g3IP9#4RDr=)Hai1?BWvZ#^qjzp2aEntgxHVq4o(O9j%uGMdy5yW)fc7DJvT$2xNWffo zw;@6d0bU)gT5f+DrcB9YcCsra%iG>%S{e^7JCI@^@(kQ~$ebgANk1z+-5VyLDxi8H zNDV~yVJ8Cj2Ev+X_#85mr0TqT;aM2spz47wS`gW!N_uQyuI{CBE!Yny%AV(sPG+jt z3W4rXuI zEc7J~@X=~$07++*k&QzAxa4MDJ;Y%@bvLi-E(S(Nus29E*fC^Ex_tgG>ynbQ#CEL@ znPUnxBhrS`E`RPsbWe4CDR*|SxFkL z(qx=}<;UN~e6X7+p+5KvADzIapSC=SFYn;z+Liv>d|fxM71T2`i(OKq5!->uAaZal zhHvM5U|pNwVzKY3d4XP}pVXJ~>HPuc*#5d#;yE3yAEVO1{H~v=rk6&qK&^={BNAtl z%PjiSPRo^Y4L5C#HOZ)?_9c$RwnMZGzF|oT@ie+`McjfHCVV7!Y2Qz5Y)pHcldo=1 z_uLld=JaEJq3B8~v|hj;T&~gm)3hXjLqS!_(o`J$rPQZC$Ci_nzq43Bf2A%WPtWPX zjv&-_b(d^swJ!l_Gn3olaNr9^<@TcirpjMa8_%A3(RtyXYI*-14er1scACdA@$O(R zIe6#CItfYBMafYihTpe6RiJTt$+c0H>nLbx#ag`#{WN1k$P+9YxChC=i>%N8!Pgnl zdTrG=zk%Pj(*WRHxWJSsS9>sQ9;Q zerz%W&nllfcnUVPVs4!InG^ocTXk9;mflKTy$^HydMOQB2tPvj1uWUn%ncSn@MFL` z&4#8xhg|p!Zb6al1eq#=Vu!;zweaNhH1xI6lOOHyZ+k)5V94 zz3qhFaVF>oW5Mz<0gvjnAdQ9NY97lcAoZpmyh}^7V|r{j>R2yzQW~}Q99_S(Ige`_8B4@Op@bH0*=)7qmUPVC+Vknq zZ>mSdK9$HiD@cei72r*Zu0|%!qv2u3NYMRxV!pH>{F-Eznkw_?nn!ab{6OkL$|fqA zohh)nh^B#e{#)I@w_`=b-9}MWymPI+M-K$&HquDfEzRGi%84neqWb@vDv^B}Q|K{e zH#VW0r$+Kvh}SN+QfE>#+ss*>_MS)oTv$EFc+w{pLOxjrm%U^%XUE1b@0FBHb20Ri zpVp`o8)Z|_GmyH_NANySKh(+lJX2m6{lqdyd+3c+&b=5#v5>h}e3N>d`I()H=$bq; zVj6F;SlxA6S+y#ko0|};W!$0UW#mc8{V{11wMjOmv-wi_S;uo4*@numW6wg)k7A0r zX&EcG*1q3DALfSm?IHl4x;)%4%KH=uv7 zgMl={1SGhv&HyEI1xTW?Dc=s!B1sxQsry2BKTiJpQ+i6#mvLp`-@kuWSQ`@ICIDi1U!@NsWDq} z^?25K^QC3!dQK`VpFd8V@7UdAe##}2S3^c+hopr|wrjbRhm85qN%Jti5GCiOPm{gA z=lG~^VJ23-T3gw)XXA6`lbcmKL0l4VNl#C@5)5*IhIKNxD>>t&>ijWJ!*9t-G6oB7&mYm&HyC~nM& zlYADcZT2?$Oz$fAaw|v|6%qq$`^iKy(-THd6zeYNzT_v@>HcdaHB{XHtQKdpo>ny& zszst8Qjk#efrt51{@*UoyYE-D^Tvaw0)v9*|D;HE|C?!jn{7X1_wZd}4Y{IJ+1d4Q z&rO2nOH9?Ob4S|X?XO1bMlCk?oHVcs?7!PhsAFm*{(CW=PRX$mP?8=NYE~rC=bi9C zMcshHQKZwxN+`!R7SFyr@rxmmL~z7VwHZ` zX#<@twIPj0h!tN>7xSJ@{rZye;|w&ZL=xMyJVz)|NdU zL3aiVnsDWv698^Q+cyHl1y((16$Ec8z+J!)E(ePM1g)|%P9XClih=>%{J#H%Kd|qQ zT(MSem`}Z`U-(+Gew2rL0c>o6#xDbjJmIOgS8KwR=nSF%AcGGaJU={d0JwE+SI#Ly z)-5Ob12Hhcg5th>fL2@tdVvk{ytD3y3P2Nb4;$S)*0XQJM|59SUP{Xsl`$B~huju3 z))*+s%bS4FfnsfTYV5N;qy#LTcD-m>(BCxea5RaZtLHa=|35E)$%;vS>$9s~6-tQ%oZQ}$lCbD@vZ@!t9RTqI$WHV}V`mLV-&zn^(s=M|B z=2!_VJQc)zmvY2S!w{duM}{&y4k*T&g$ts zrs4pni#Uo4FND4tJBgLcwao8&<|MUp(qGr+0=WotM}c<>XO9;i1AyJ6UxXkKBTY(UWT7?ne?V$o&0!OTH+cGlcPoVNyru zd^z9sZw%RD+iSLIL5iryZ)=_F8$v${O-Ni$#OrYhCiY~EI#yJw{TlA3&Hh8lM~nYo zpnY$A-0vAlEQZQplo;Qe%{&Uxx@6lbZ@Wmf9H;)`uYh8T?095 z?}G{SSa_U{`_!-E6f|{>R=SR?Yy>!BV@F<(Ggxo2Iw`MN&Q~iH=F&b~tO!(Z z`Z|&``ob?mslt}AYyD9&UIEjK>ClPB{Lg)=k*2b*eb zj6DUdii&Hq)f=t3^=0KBRqd-HTv>&oJ!xlcCXPanizD2{hVa=`&#O1vzb~%!cgD=@ z-@W1X;**nz&5sg0e$ZffG>^sh&07+r7+4a3$lT`{Y9@Ryf_iY;ie3@H>%2z}mZ>AR z2#%L0Qp8=qpotPZpHPm4sm(Ck_@wSnte&=W^%5#_AUkZHIm3^?36l#*CV1laOY)AK zT}AY>Dq|!Z&57!Ypki>}tNyh=bOLM}yh-0NzwXyV(CrhPwU}h6pBx|G1%oLp$3!#R ziC<{zMKs*zf5?H{=sjaY%gwFH83tH`2ebrVB%X)(C88jor#hel(_O(BY)f0u)&okqQyW4mx$YIw1KNs(GRYY4b;UQd` z-|yC+9`teyJomb}dg=ilvBfYRMP*Ln(x&{-np(-^x84xkikYF_a3IPV#f zQ5oL$-6-(Pw2D0x7@?}q%FO%>rtKuYohquV^P)fcL5gES&*+ya?NEKs(XD-6=NN7h z=0<8)HHZL$8G$edbr2xbU@L=Y0-PeAAY{qX8@L^YJQYZQcBsJYHTVyJJSD;=gWpN+ zTwWrq(RY=)TPUTFOUFnd`wym)^L;#X_iN|uT)_PA0^UwQ8q)&1YZ;V9Q)q~}0=pAP zhR|y0{0SOX4{PVH!`9c$Q=kynlkb#kOs@rzsoy&`eCNVc|7jV1HuXh#f3@&hmX|!e;!Vbo>ZL8AMJmze(R4KlVb5=kn`K<&vRPZ zIMjJs!5o{YcjL;F!kKgL7+&8mRg_z;>m`!>-fs7Io#JZ4Q9#3ObD}UnIOmDam>)T^ z$f<2vw!shZWbZ^#(gbL!Mo_V`4&)fT$;4j|4p%N@j2?8LyYi4(c9+dsd(=--5Gv$j z4ja?vk}<+>-0d0yS8bO1Bh?SvlYSFc52iS1A9=f5W!)02K1*)YqmJ6!3qSggJH_xF zF5SViuYtdwuY#ruA!=w31Cp?sdwo&SgWxvG@3Wxz1}s_I+L}T2Gi;o=q@&E5 zlT%X?6q1%%Pr^Fscr-2O&kScphP>|8*!tyy{17Dt>1bU@uOdpCkPw?zvSJ6Y<>CRs z595AtcY;66Nvo48Q19RZ@P8;rI3KU%jGFT!HcQ9D>BSIn;>?lJ_SXQe$7L}B8R zH+}!o5dGI3EoEL6=Wx2a3Cq+h@;fG@miI_S9-PzftEH+{b~#anpC#&{{4f`Yl1zyI zn)XZ(C#kQ{ihi<(iX8Vm zN0$1_5#1PmYyXFXRF*}SE-4qQx{RTkGY`B9UWw+T7vU+&Eny(n7R|G>GoJS9Pd;x1mv^^uP9&ws)RJ|lnr zBn!hmIsT>7k5_DR#bbzQlsy&owL+QR%JVm$DT%&cT)ve?^J|C`#+2qcOYi=Ux&8T= z*58nlnQQcz)&3y@!A8KMw|=PSR+_tkJAUjN9ns$QkdBc7dWY_;qB454goUMlkY_Wk& zPf4rypBLt-DO7z{v3O8K(~*BHmZb^-Pifp!hHNka79_2P>lD&}M(aNeIoQ@`| zPo|Bc`q!~4JUJjtoM3;*g2MaG3!>4Hvm5#UPHzNKo{ETSv^0>fg@5=JCjD;aa z3iGL%sIXC9ScaY%XSNvlc#0a}TzAXx|3izaVmw>tf@ro_JoG^?1DrU*g97L{V0$An zCZPBpxz<-v#MpevN~9D)Qq3QQ3`F4evUr*>3B!w0BYKBoKav;otO&mAsX4+c++_kU ztPgy9VFm^NuK-W-oBrJZRtf%-abw|1>|f-lT`t3&*gO0_<+zl_zK35nkEx_eLhGmP zp{z1TY2j59EiEY5h#Fj&4uh}*xyNA4GD%TlJ=Ls!jIo z&?08It(0YTWjG*SNnoos4*hpip9`D8$Us2_zy zrtmQ`27^xN4^W2JC(}q^vWBd`;2Y?Vf==^Dcz9q=YIZuU{MsC{2fr?AV~w ze8(p`h2IbsJw(t_u%mj$1{R!q3SUZ!(87=wwsesC)k)(+SXmPl*1Cs?d9!_Rc-V%O?}&x!8}JPnzG+jsSp@|l@N#@z`+szO z1yGg!x2}Q$g0x77w1jjiCEXyPNDD}(ba#kIixLvjNJ%$H2+}PLlG5FI*8a~ubMBov z*O~7dN8HGM_xt|VTF-g{U*f(mU{Dhrhx8O9VF3SL$tD^|x6Ia-gWY~xJH_KZ z>IQl!6KUn;>K|2O5w{>{MSzR{i`-?p8Qd%-wX_mn#|S?FZaZxJYim6>XwVmlIN!eS z_hNY!#u3Bv}@`NW2u;YjO~(#0)>d1q_~sBQaEr zl@%55fC@!%1rR5kzz=o>^}Wxs7+E^O2mb7ESUD6JxiCF40JX^zXc2I4+!2(Lb8z4S z5!F5ZF+j_KnBXqxbGmB)b@}$32WQkNGtTdRrtzB#f33ppFJ@GJepFBI2MWmSDL;P& z_iY6Ao#YtDlA9|L`y*@H5`B&(75-qyO`V?9RM(^}A)mDg3_-GUliMf4=Z{cp#j3D^ z>Q;Gd8uyi13OAD0+;4i;BnE$78~5*?=3A~U9xB_{j}+DbsJA^C;>jgOm~n~*~0UC`1o#mP-mUD)CU zWF??XjiZJ=jS5SF5!|d|l47)ViRtM*vEc6I^xRyu*iKr~=5knGK)hL-w6%5N>zXx0 zOf_!AWqwwtwPVmfB*~mRG%9V;>??M|bx6eW7gZ;e8E>*#Gwp5*o4bgS%tB-$cy7KO z$$cSOwaV{)XvR_7_LdX;&cW|FRyi}g*94JoRyOewb{fN^S)-tgGMcuFAw?&F9`MExPpa4iNn7>3NN(05Wn_)*jAK+jDiC9nJV~o$-R5W z0n#PzQLGS%-iGO<8H@rZ7I_S?;Iu8>8HPBF-%|WXcLkzP%-Yg2_U@v$?ctF}7_WP! zt%pKI$U*r4{OWi6M?2C{-20IQHX?fe4pl0)k%iA~MEYXwxW-=|a}s7@F2wj~5-g6V zax1;M-cnj&%WqG^n0d-h!PMv(-NZIaqEf%wD>Pi;L$~8ydQUsG%QuKGr>NGj@%k<1 z#j+PK^4~1;Ig-!bx?-nCAKgPQJoK&>1L|w|ki@<8+g&$TQ%}4IUoz`0B-|7%MxomI z^k44Lm#%mTiUt*5&(DWY(nIbCbk;_k32osazYG!Km+*Xs09Out$Kr)NcwuSZZdd~R z({ZEN1-4tA)`pjAZJDHkkA@6@!0#2*t@1~0K=rJT@6 z_eE2GFo0bk3Yd{%=#%=IY z6Gs?$fPN`08o7)FHAYDHaADmAX3`*=fn^xLLx#Lb6XoXku%v~`3C}-dGB7YPp@YLY z-1abN0JTt%Voq5QXpVOYb$p=?k;{WUHh7y$iHff^E*!b%6&5aoq%gCvu(0FnU7s{? zc!W-S%b}oS(Eu8=hVSkF=9yx!)=;jqe02__p-$8rxPyw*Vj*|{7*lUNiE%{FY?KE zJ?66cKp_PRET*S%o6|bM*-~8W8%@d7D| zdC(UKxV>Yk`=Z%&N}A+O$6Ir{9V(!6J+3iANprW{;lRVw?hWG?-FHu|y+{^I7UOlq zU*2E&s$==deo(nLs-MP4WuWja;)KuhvnAVmvH{lh<&_l# zGyy(K5VeS}Rtp#f9QBCg1x|!}7(#srG;|Q6|KfnCggrq}=GV>ifpr+HHH8f&;kky4 z^#_j8JuwD}|YdS{@f|~{o>f`Mh=0r(C(AVkd^&z}|DdH+& zH9)9i=NG9Qbwh?IAt@j?_I=rK?hhsu&Hi`6rCQmu&!-O>CLO2ImqMP-78VxcRkr%r zFEIQs9FIn0zw^D|>}VxieeHgz5YIjze^dyIA`7mxu8y!=#U8h9=6zB@-;fV3KHZqM ziEgK{r~(XlDr@YKYnrkP@qhD{!Z%FqFKP{RTy{iU4b?eEzLYrWa=i<+5bYSR6EWVJ z=`iK@k`_O9x-tCa#_H- zGJV#CvXxLl&N2HQf`5xn{bcaJ@-!h z=J@Gf{;l%i)6>Psn#G7`upTkIclO}}`Yi-Z3iF~OJe0tU0xbCDp#YRsVFF9|aA5

Dd&b?LOYG)b zgyVW1_WTII{jU7TP_~F#n|PnNnkn0dm|${rUW~G+&T1P#6(hQ*L^r9p1b_y8;sW zXP`6NgKTaO;9jH|>PQu#ZksA22xQYLMBvdapX_#arm7-G^EDiP5onwqiand%IrtexcWA*%xcmm!r{hla57oNd_q=vOG6^c#!rlgq)95-)1 zg|LYP^qexL8>$is8w+nJ#mJ!CC9V(mEHM)(G}Z0=dT1@kt{S#hn?+V5K4azehPJk6 zZch<)z$xA~wIQPOEa3fUhvAd@PtwB0yP@m{>h-)x_{&~)~!ub&6Ea1=M`bN7gdZQDQc zxyFcJ^51`7oGpKR=zVv<=%c~cZteaq2JgO!Gn0iQt@3tm`rf&Z&p!5NrJd@Ey9~byk)xTR#emtqYshwYT&dD+&w$oPV!qtp3h# z_fP}34PEki#)I3>aqswlTslEbS(N?VGWfmsq}D$A>%mvkipmciIwoZOl)Lp5E-u->+}wU-45$lK{B+g4)sZ}Wh}f0L7#yf^ zFn#K>-2*IQ2;OR+nMwZp_YF)XlBLpxufXwDa&WMvMQp@^r>LmN#HWHVBIzPaFHe18 zXNS$*Wqf_`5zmtfE$if64s)|Fe?zH9@5ijy$vhIGA5G^7InVBSm8VW^U(hJ=c3}U$hEW4%ddct;TVrrKQQp$w4~<6a4(* zB2Bg|p^)lOd}rKBf3p2=d?0wvA`Okg<7qes22; z^e^yRMD75>BrnYPB@;ov;DG|UA;h|r0)QN#e$dWdK{#H@8-9;ct^ae4liM>#Kbwbt zWYQp4?Pj>l76>skSzg%T|Y9#iUlXmAyN&qGt2#KioUucigHAtT$()NID@Wb=fif z*QHr5a&%8ERjbcU?d{7WgV|)kSaRXWGOMvi-ZT5GH;E*n-)HkPViv=sa4+nUW`~6r zeUdcx`g{#PD~7Ioixa45*IXvXvXXD{nkFomDvhfy*8e6wk1aUqLl&l-$T5V$%T8k= zm}=SLww8oxb$!OXA~>7+v*~wUcf`54k2ywur7oRC3!{1;IT2?r@vpk7cM_*X`Omty zbv0do-G4N+%p|*mWqRtb7T8e#(fXbU2ZLZ3HvMdWj_NP{D(8QD_`U5$x3Y_EQ1E9ReVEDMsxg<`@V*7$B1(O@WmXdtYCFm1*LF*6ji8=fA_726vBPbvzQg>Y&l62M*f;Yi9Ak>)!!Y*XU?NIxVwjn zU3%5w;`Uu10~uRrt8uh#IfT>O^Y_tW_PH76d3UDOgv@t)ra7zW=DupW9`c!osHDeP z=b|pxe3g_1nKzfCm!;Mi`48`BnOrHfHBX_l%J4gT-d;~%H`MD|_Jt-4VEZ_3PbX;Td&NQJ2lNIF z3(EwwPK*!G5s55f->86c93=K!MJ38_yhtjexwWVWx74mU|4icw zCy|PKfp>oGbtELx{VH8~Ba7UjC6YBJ*U6Zr8VWIR&&d@Nx#yub0KfnQSHU?s3_vsi zT^u-^L?k7VLYKsC`R5}P_n5@Q55>jBu9R6(c#K015Q$>aVSu^PMI089KWc%5j0{l7 z&*5RU9u|fp!712%1Dgb)uhZ0&H8;=84WNtChTyt+2#zlnUvgX>@B^?bX)A|)7IcF9 z;PB|MKJvI+t57j8N+~A;a4V3`?SX(}AB?oM8|WdT6^tN=$c8tL$V9w#Xj^)kNA%vv z`E;E&+;T9Sd*iO#^|mzWIOaV6+pGM}zCPaHo~9w>&g1W%y|WDto{{@aiqGN5c%_YA zAj5O!+fzpFh1!8Lk9LTW{zijw$7)h}Vq}lV^R6S>au(%v>slvQ!uYk1Bhk|Y>1>g{ zPvcaAiny%CQ)h1mAAM4`TFklG2ze$-U}cvYY2b~7s;(P?M>=!seRdUfP~l?b54x+H z_xBwnR)!OB7XwRm&EN4xRJ-5_yt-mk3=eNe#8@I&-6Iw8ji;EezMX9Qwn%RA?C(`8 zx0f?Ev+0TGZJe28l!;~q&0T^H`g-xxiblQ--mlFZn7%}YsFmsIt^BhWR+N$6f3U?d zXGxU2ntbCV173eUCBVLz3KHJ-Tzx!U|ACz63NkzK-~N72#1TlZ-(GfRMsQX8?AwI{ zMv@u{`)(ZLbkSu?NNP&j#+1+QAWd&H^31a}*-6gC(RT@gBKf?@Wz$s{8T45BUMuI7 z^tPrRD~awazTaE*f4s@3!N!(Vs`zg%0FX-q=2K7-x!NP%#)aztFw^HUp*T-8Z6WhZ zaXdT3UhpV3P#k(DaqP&+K~MhWU_a09tm+;ikU8 z*8tSA!K3^2IX*nZLhjtKQ!_bS9YnYl^Wq9;gi8EC55HUMot)Q>3kF5L^+<%@L zs30kX%3+}7;Mn}BcXNP;;DC<@a|j9w?w*|FXcX&_3D~#&)p`Eh&&(CWC;YBM zP(L4yvn3|d8FdshACkjW*+UDH^=kGiq=|>PxXr+PSsgFQSQUW2!)z$?9(cwYN|itW z6^zeqyLm9%aKiQ#R3VR9S+l_AO_XI)Cp8jsf^vL5zh_yi<*4%3s$+$RzRr?tJKBOa=|w;|=Q+I5c_oRDaQg;RjJuBN7D^zu^h>C;4D z=@j+1*dRg%@(iCewZmos8W#37-h($y3EZ~L&GZT?u4vSah9 zy4v_V@U^m#K%GGEZ|S{A(y^!zm~WL!@q|qbW4VQsM*MhR6qk-luxL817{omB7&4W? zp5Az6{P4q1Dy`q>HFhPHO4jWpdZc3ke{a$0%_^>yQ6+pwXC=tClKWzo;#SD#7IuzH z!j*iM?@cqO^C01_x+%rHSqWc*QSC^K=q*nDb4)a8?{&VI-nQ4j>obTahZE{EX|D^o z=m=;jG#kX?jz>c>I?E+l9Qx7R{j=hRTCP;bnf@P#>Unm0 zOe+~D6VAL}1e?hd5BY+Sg2enkJcTF5NCSH{MPdjy{>f?R)A-mmOJB zqF-g@Er~sY*R_4#(0@L8$8W~-S3%NFbyPnvRj9!wK6*tPJL6(y zW{TX(xj~-KiXi{TDSR=Mtc7VHb(LF9#K3A5@D{k=PcJUQ+EPJNeEabi2uy%XE>vua zw;=pa&sKtmTN3HFR#Vp1C-;PSn$XNKFl>&Bj&6>okyc@BvlVdS&VVct6{ffaEg&Z} zLjgTB1!hJJ3=ADM&l088)RGfL*nA>XB)v&|eEdaY8v^eZ?m!w*tW4?Tf|IcVlI+-z z2bIA*a#No=-#VojJPMdj4%Vh)_j)$mJ!gK(5I&0a>>B;jD3mHeCj$AU`b>HtMubv? zikgw|R!L9ziVy?GMmK9F#aaCOkCCp2y;~#q9_ANiqey=x+mLnV7npw^kw}uZ&c5?8 zp=In$$o1-ib#tyM=tjZXwE3z#*`~}n>YXPmo0DMw$#5P1$N}c@gR$YuK@h3^;C(p3 zlZ@67x^cAX=pLn6{43%aa_#g{!R+Es-%#;A0-Hp6mu<@8m7beF_J1%M%*0DX!}a7K@&u5oRkdb6PI%B7^Zd=7|y?wv9Pb zMc-E{t2nG|G~SesV4Ya z7yYM%5I$9f%Tw&p@eZt(PKKhdqd zGlD0(BGHVfFCa*f_~v|@GvOo5$+PYotjN^uL-GAkQ(>V#Ui9G06J6>jPa;6zku*9N z-4~XcN(tnX40vZwhxJ7|H7%1BmY^ZYR||PCtH!fHetFwf!gLJfvz@zx#KLe zGb7T;wVKNP>e=Qg^KSp?XG9)?=#9U$6Pdo%@y8(0 z{ZmZhD~SW=Ei;Bje>AI2i^j)gQNx)=(upHv2LEOkYIBS^MP0&0t6#nP(zjpGQM>m`LXA-@=xKZ1+)z)I@3DuMhMkt>C`Oy-f`F5+WE$|-yYZ) z7!o`rkdCPr!?Ux#@}cpRvz@HKGNaw~&sXZLPeHoRS0ZY6XnORM+UA3B%|3fc=`>Ot zN?PeQ#Odbi2tNu z;U=5SU-ze_E-oyH#7R53t+aVmMvXU*(5JM#B2!1K2JuuCL319~bb5s34my zx7Uh>Kp-yWc8lqnzBzHY6wNx8q=dw+ilwk9SvfG&0r&!NcsSY-sK>VVjOi=(qPy6X zadN-1cAs#&X)!T&-L{+KR^G- z;O`O?b7J{f*3UB3YHwo-DTIbLvO0Rri-|(yiK}dj^u=BijaU$7WoIueElI$?J3l|4 zjvCd}BmzGG=xmX7zkU5m#Ai2xIIc z6G*5tIa(imwGrJ{$GY~n5I1AxcURpbvQ4Qs`Gg8~cDq|!?-7$!|9omb?#K@jBNX@D z)s_yo){DC+-)Cyhwdu2}5-l0$dT)d(W4`NMtqx`1qo|dCYo*^KEsGU+nNJwLOr@B{ zd_d93HLS&c8h{%A$4)r%WH)nnKot+&QEF0P<u{&qcXcOuP{o6yz+rYvR#~q%6`H@h0>d?YxbwPAfMR;U?{84H#$b=K6!?pPvi#QLzRF=9=y>xHl z>+&_@nd!KA|6}M6IdXyszP^WlWJrDY>b=o=fd)2Koo-oy7jG+zSH^z5ed>tTzAR#S z_P1M4a7sH!+lk~~MT`&H=;O<}9Z_LN&+z1XY=h17lO@8)XTO&*@to^V3GbgDr$eL0 zqEn6I3z3PX_RDe{5AIuV`JbJir;l1_mYHBSH#fstZTB}#I*BMZ;=rZ-7XvsLrfdo+7__?|A7 z&u6zkW;Ml7FkeCI_;_I=7x3$=balha)Q{e{H-}g zj&yY)!!G~q|&L<svA*IevvXftB$?deTcf*fLhbHQLnGWz9sw#r89kO2MC z^C0wQNs=C$@^{)`Dlh9lJL4nAFuxUDFcFzH?p2?-2%pK?b)=#%O4P)nvo^|eP zY@3efGMq6z^XD70edBC8to}vENo05BncmgiW+JtQ?xO_>S;;8JdLl~ZwkHf?G zNK}`I+R8+B-t6%cR`w@%{kSde`Ztbk;$1CS4n;q^tp$cEVST$#nbY6#h=12rv1}c6 zWgg9~ABotbO1K`DJ`#vjZK&Z`W8|+%w$&$*pDV&Wq!0-63Qv8z_9YUB3N<7x=X9ZA zkl)rVIyyj#W3jb@tlPnK=^{UViaC`o@;()3I;j_huDqKH15$&r@BN!j%H1<=pNM*) zl;^D!hO?#O6}6g}A-mW8lpI~}Fl-gbdVV&aaCj6m&t$ZkFWeeE>(IQ}i@m=?{=CW? zKOvQ(YlHbAN4q>u&31R&i))lwh3t#hMiqt6?qOcAzU}yYL-XB?jxC&0!g2G?tiYyh zf4p5{{N$6~qA+yz;fDvUJyb*2LZRNeH19up2kCxLdCKk5By8*MwIz?#pl9p3TGR?# zY=AjdM)K64#H6V+{jx)!A2ajI1$QKFn`xq49wlf=<-a`n)!(no!ptM1a3#k|hF=;E zT-W)nEk)o_Kuen;8%qy-K13v0Kmf9U0OFCJo|yqtmn?8rGt8#?}So#2LU9rtE`a4NWVoYD2+AdLT=lurEbO9{OJxv^WNM z@!oX##(tPa2gfNy-VGO{X~c9XiuOk{?UvtnTDsL4Nk@**@vcci)LyEq+pY8v1ID>` zaL_tBN&py5+AFj1;bBn9Jy2Cs1L%_-ZM07J$Ef)4p$b)*ifD!8SiD%Vbm+g2sZg-sh-0QqF&=L1s?%o9alM<7s33jZdv}4J?}y%O#4`%96{_4viw!A( zUSwUhJ+{PJln^QR(B&-t(2a$BHkgpSiHggHsjJTYp!#4BNbf`724J}qWzCkL&<-X# z7BBA~I~#(y6k(1Z7!nc|*z-G@20kb{0Bev@v2Im7dM!qz38Sij<7(9FkP-ufFb;fD z(r)NQ+Ss`@G&F$Y0;?|olA;F&RDc_E1}rC_$#dJzeudYky~>x2i~*aHe9gkZ0wrjN z_yq)BfSs`W!-QHBh&g~v3(ZTNmoI-oBR^5=luwk7@_ykDpkj0)B6Aju(F25egVS)F zQh01~Aht*WyGbCtNP(2>^u8ks3X1C5+7E$Z0G@TSD5%CCh?NHEe2+x^C~&wng;-4u zlo&|^?{00ff*A;NkivzBgYy$$Ysn#{MCekQ-&-&=^L&NN!Dm_a}wfkMcA#C)p1KN9K_2k=-Sl2tSSCA%pV zh9wRVCWA|}*}-_hH(^VKr^9*xUTudJONJM_My)~90-MB}G5 z^9cyV?nmoEQW=kc(wm#hc)T?w4I}34?5trm#JrMNPnPG!zS5Ipg3mWJcoEbv<`9@k zUK|Qq0^##j8pLqwUIbr!cXM^(0RPRMck=nc7lGg+uvhkJ6<0M^zQarbX#!8naD(uL zcB>Z)1O$GHio$`dL10-K7ci(1X9_d}Zf@3HAK|6v$6N9yCfPFOGmQ6W4-Z~Ny;hAM zdW}~<@Nq0DD@s_c}AK(9EbzW(b(nQ4*&fe%+^a*YRV;jpWoEwAKd zvI5fRm(1^M_wKk9SA9bC*p>zi{@eU!X9rbIo~0}Vir2>9bw~zlr|cfuB=(C}F3VK5 zMl{_WW$-ra4Q>CZH4@?>S-ZAPG0YQf`R{cj>B0eD?C+Oku5^L9CtMr^1ymaTW-OIn1_q7*B?HSzLr#orm5Fe*)GX;qWtf8hK6omv& z7=*ZeN>%zua` z+XT>XD=RAlNC`TDhv64#X;d6W@guJH(b2`esn}g#y1_!R+HT(exmkSFN_|1WeN$6Y zi0y>t`OVg3MY`$0Ry&;35Xf5pd_26wCF~thd?5wH=Qj~OC8IKxl0>hwR3XJ48*ZJ> zuC8>qgB8$y0Cp=8_K+0}Sn0xd+M- zVxAC)(Spbm zc0SPVATG=5stnY@=vx3#qe^Lm6ME_x7BjgAsv%+PuuZWXv}8L#r;8;}Wm62bEZ zos&k9&ODsIh$D5P5b-k5!=?IM3&7Ew9x_mp2cuy#!)7BJULE2SSF`iV;_VF zMIa}C=itx^5hX4zE|-^=2n9488HR3)*K=r@UYte!u-#6VCQvag%3RKJVD++PUUvW^ z?wsuGU@!w5N_&nBH|vupPfQwI-f|K{y@!kSL#fLp*XvJ1eLc&cN9Ee;>TT`eq;6M7 zV**fB8yg!Dou&$7wf*vk=7xvuMwgd!=!|i4&{zQUS+#k_wY|8tda~sU*dO@u0Z62M z|6UzwyI~0VjXEMj19EFXmBNNSet(svVe^}T^SA+GZ@h;s1xihlaH$@YKhQt`;t894 z;KYOg6a&Wvll%3(w_kwIE&da3P|T6_a~Ypd5U#-C7q%S4PKAw)4KDCN;-ME5)ERl~ zKUpB*pED1w101CQhavzXu*`+i1Wua|DfF-^zXhK>(wL7mGb<~>kqP{JP+VZA9|W1~ z97$?$Ai;VJDsCXG4*mW80m#C+@Htw4nc}(-O1jTf^U|=R{hUjM??K0B7RAayR$|Jk zCCxXFFX9isrp?oiFb6&e3JGz5TK=Nv!sKqcb%V6OX=FL;qI390bjp{{b9wB1o+SkUv)$8)CZU=1pFcY92*;ZJL$nr7MWLJUq+!x z$fj4%`Y2JAt-bD-hZL0$i<|!Be=a0?-ReiUo`~OH1|Y})aKyo`1)*&Pp>3_xhK#au z4)pbiWOn%cb#!%Qm6Zv_>tTfo*Gi_fJ*fNuiJJry1UC=wruV~zGc#j^-rsS3Bos7f z^$iWbYTuvj#rD>vDXq0c)zjf&vy~+`mbW`Z5C(Y8qt>J`8a1VG9cj=c<2CQgX4m zH&mIGuzLqKTe~3g}52HN7v;>UF7l17i(5%j(ONWL$qzf;0N5Mi1QYuJ6D(k#i9oUz_0P1;P_w z(RE|u+0M0$+?C+J@H86Rv7Da73ocWI$CzXIqj_`tZEbwxu7jVn!kwlQIj(eb=MS?_ z&YFsSmQh0VF}#Ge|0&K#v#I7b8he#3?)XPWV!Jo!(dr}Ma-iNL1SCM9MeNXkO^gl9 z6+6)#pMn480@(eKnM7M&TwGKnd1O&RQlZrgpD=7CXrb$c@&o)Av&r(u;JglaNKipbKiq+H!QW$(9qt1>CM-tP9NZ7cTc=LH&Y$T0E?2?<#x zfCEZWhyVat=oMdp9Bq9>R*=DL2vj_to}Lnm#nsj0Khxse!(~L|9}!jkd5@$f_2ZqF z7b}g&1W!rC9UVN72LxnJ(+uO z8@XZ;8v{cefZ^!q=c1ywU|faG^GIG!S>7P&qRfCrZgw%Q2J zEfh9Ttw09*cRep(EyZu}Fe4T@kSX2!FV&|H{0h4%eJ&rFjBx2q!5eh?lbB`NO7yut zAkaOi;7wK7ozmcE$?7FRx$b}cP;Gpq5g&UUi8J-8t*FzAyYQ(Q=3_}dqtF=z)CkVv zq?pgx9W!4!%tFW?zL09^VB@q^d$Rq6Pc1TH(^?;#HfhT?D(E*v?wb#Nik>t3Ayxe6 ztJ2@54eVHlQMu=_Kdcpa2#3dOyeV@we#NXl?wjstPhOkml^t5o@Vx43I-UPSgK0~j zF6t7RvEgIbLHR}TgUFmr)BWSjE*;fM>xF@{CV!S2jFl^_kgzZexclKS9EPF0NUx!* zGnPSCMh4yl1x{-d^b2c)86olOLeHKh?e5wI)P58ZFZ>tvSJG~8K@xfr8F_gc!0n-i z&U#;14?Y8GBs=WDK=pcy-)a2;TwrOCbb6fFr2Rp|lduD3Ac*tQo53gvWrQTtzepaI zp)hPis|rQD%>jNJ7#J888!Ij)g#u3-ln1EItieBJFrAvIxUeQTrSHG)dTTr#!C^DP z&m$?E@V&dbe~43zC?)GU!BQhn9d5g3cng?)bLe}<-}=S0Ki4bXAv&2;CyH0iYin-y z0~5=6=vsjO$jmISLkR@0x;z{X14S|2j9GOlE~Br4Zg>FG!Q7J|;6M*KV@9CTVtM_; z7Zg2k4$8s}4mAJ~4gBJTA9O^pc!QV3z*jjSBn3v5?)SJfr{^2Rnw3_>&s?_Op6<^7 z2mKIlaiG;jZnykJmSDctAn<$Ho{WslhH-Pt=5VP7cf6EPhN~VLPjct@79oW@Yc=+l zs3ut03_rE~V#}U+uiK$0-h5W?Ch_y3EGjbg&%dA0)6?yQWNG``mB+u}_V>!?^BiIIQ7au>!U3TZ7k1O zD)9uf(eUL{gZAP3{rvg!X<#sGW;hSg2BjuxKC46^4muqU!f$SYqq)0haIm&5yJC;n z!#g2KSuI6ORR{Vd==@5+A`fwggIW4>m@>=<)9=D#1C7AK+FIzBFNDykA&7FgG*Y`T zRXTvC1rTfj?@N9-fDmn@zRy)0aCcyNs<}mL`8G}eV>CSJVr_@!0jP;yoKciU%+ zEGLw$|2*2KS6*FDa?H5vEdO0k?Xq| z+>bYbg~$xuEOd+j_s{}x1Pmc$KUE_dg#nBqGj0?qS#Y6*CYp|)p9pX_xB|q0Kfu#c zZawuGK$=?v-(NCA!A2OzZ-6qn++y?te2bD<=#)yVfnx=`X9OYeaS9XBK9Vf$>gX+a za^PS<;65+s4b}d!1kono(ZM{Uxz3|TIcQ?^8gH}V#Up*~yZMl(4nxRy2c7S0jx=i% z)&!4u84j(!e>}8>8K+Ib9x3n`4(Z*&l{GS@nru$uZ)mGw5cE&;_*}jTWZ$ z`I+4X*7y(Ra^BywmCOjPNKxsk^ol>9b-sS^H0xP!NCN?PB1NRD=GxBX>5yGZBCqKD zdZ^EDvqpXczK+f6=c{b^O%}|{4fLFoA_)^)$uE?qWD;JdC1y@+y)(bQUsq(<>Tsua zlQLm)HWdZ^Zb~l=(apBf{vGUy1%i+EPR56kG2G2ZuZLfyIxKSB{ryd@^?WR3xOooQ zlo0!q-;5ib-jMgNr`spFM=P-&ipGmC)0m=N&da~OLni#HBY1C>GsVSag*342<+fMK zL#&Fi$H7msREl%VCeR)fExx8xdV@*4BblxuBq>Jc`?;Jl za666ad2*aS8s`3QTonv!`>#23d4HAkl<=@^J)Eg| z0H8L6Vn9rK_b-Q}QvB1;DJe?eq6ttNxT5$%wFUG>!0VhF82{;m2p&D0kO<8oP?ivV zGrSy(1&IahX#q_Di-;FKxiHtm*hK0|2gMPF_Bq3-<9$M1x#W=@p4|N{|CWZVy9<9S;aBR- zWn(M((@=qVO;7ecZ{gnDLUEUEk8CXO;G^349{nQ>(njx4JYSGPJvdBC<@w~u1zrQAwqw{E~*fSz8H6`byi7&?0j?hxX zGv(29n=v*!;-4+8z_-8T@4#kBOXd2TX7Upw>~)DT_QgWe{9Ze%tH*x66;q9Of?C^# zm3(2t_Ub}O#jNiu6^DHc3J*iNQKLqx>%Y${Z%&^mA6fc2mQQ-+hjwEdU!LN8MMB}= zFHdaS^1#i-xyowTRB%o|8Sv&Vf2WlBqml3B-o*tCF*g^**u}$A_X8_ z&s$cLhD2~3 zV=@C(z3M|0o}<0sx@#Vvlvmpmv>6{zVaKkfC0%I}%6ZqIG1IFd`NzX{6Ea>#QMq&Q zMq1e1{0Km5f~?Z1rFmc0KV4t8E- zLq|`NjTW5)y?*Nae(lwkW*g{PlO$gIA9c(%)3m|f?JBH&U+Gf|&8OJHJOO>IB5!_X zhPYXsi+|i)VjwO@?ybc%wNpT*3Lf_>%w1)VI1!%e$VfdB|9pBk?3l-B=)%5s>TzNKeP?vZ$tD?cZcyEy)LkY-7^L3(>xf2WVNNro<52FXIeqOb)g(O7H%@p zPs!d{{3kukBC2iZ6#0nqlW-~z#>3bNbF9bm`nf!wC{n|r3X@GB2I?XCUB+q=)+HFQ zCgMni8}ZZkk)z9w`02FI!JI}$g27=!+WIab%lP{5fQ^OIH`V{PMqigKQr;2@O42CF z$fl%>wDP}Cj7t+du+;6!OIet%@512dV`JbThm6vQ7c@m++5e*OHp9tQOKxbZ#hByV zpQSCU{%OUF$4O5q7Uxu03(buvGKMA{W`{f5;vIGRx^r4yDi)77N^nPuyK20!w=R=IQ_Ves+-pnRg<4Qeb9j*f?(Pu_w~1k&iUWdU*mi(Q4PgFv z0HD4gm0>pw093}J0tPE+)B)}Utm0Pm{TLGKp#hvny%l!Jho?YKg>5e^)`q|$6!KWH zMWLaesBMn;=>eR-e^vkS!b!4Lz#(9qF2 z(U75`gQ*Cf*hlM>vf_bhyk&_|&J;LY5whalJL-n~bxtcqU`09B1OFl=#By=@8|B9Py$ z(9iJSc#32`vi;nW!4gDFqOXz4{!~6QsltOn&_xyPWRPF5XLxw{jZM&bW{M~@;P6ud zM#-vOS^r3g^8f7SK3_1Gde0~t82$(F%C_rwzvG9$UP4)Rv#E*YdBDGA@AG%hrZCji zlP9Ow^|t{{V`0o8r#-5J#LRvo1V)@QtnBQ0yKNytfS5xz=Tv)NU z6_8Xg{R5Kaj=-UfX1+6TeO{F3IvRMdD@xu!!EYa$y!|tJU~kdfMZr0e^a1at;%E+5tm8zI9!D~!Y?{01pe41@OVf#J;UK2JatjnOy5}6UUArN`35P_C zRJ9USrGlG|Ok_egnsKL*q=oO)cP^u_fik%)JXv%HJ>?-BF7xD}PistZ-}|aFwb+pY zAFPSdS0x3p>M6HBl4)@b@b|yp_A~r~Nrw;*4@neOKOlen5r4ODq=B!IQnf`&Zad+- z8p%U?A=7#k8~Ull$ImV1XWd*~iE!ewS2W`(AIoa#=&;0CuOQGDjynqUv6{3NnU>wI z$rEBoS1*6WiXvvmnFx<~v|5xe^p3;_f|Nzix=JWR9MT-_AE5hIH_vo5};5K8-R zT6I(mb-k)UbV^T1$&<9Pj4y7S`(>>$2Oa$03wP?S2}1EXE%JwF5+8Jrog1f5`Oc;< z`YJq;elncF_d=?8*l$1s);6-M_G4x4k;usOSidx0t38>i@u^Sby3y_)%ongaUAr}# z!GOQzhuI%(&i_^Wez&gynXTzP2T%$gy!>5fl3aV}xmzg2%>9I@eK({bp;aB8&i>@femk%{B8 zmekj57L0LL`oGvTs1w*Lv#Qsgl_&kYN)8)89JY}QThJ_1?D&Z)H6UX$^_8eM{is1U zM;euP20x)@!fvg3N!IVHW?dpp-0~$!)0f0t0|^hU;_}&({Qr#>@1k5g=)K>`02bj~ z0C~Y&3t>X2Dwct;M~RM#`U#-%x+@kURu>kygUm8uKc2#`GN!`GyB>G|y$Nt-g6|Xi zLs%(*i!^|*fk6uRg10~_Fe3{~CQzS0efffr+L7?t+#iWi1S$-OD)1gWkbvzFpdnz1 zBQx?}biGwn6>Qf9DuQ%}beDj1BZ8EqAfSRsNGc^QUDDmsDIrp#bW1nVN;d-19l}}r z{l-7e%{g2#2JY>SC)S#41~VZ#qtuKJSK>09yMcM=fmPAWz1lxt`T)Mg!}B%rV<;~# zgWybwD+_vO^TI=hhH3$usup59`lCK*E3DGC%sLlY(}MJz!t=WT4co;vr!EZhp*2QZCbEj zbgHm7?t`583APq$vPnd5PX4Zii`=3Xt#^*0-&IJ&T~jfmt_`^qWyYev|La$HtlOan7Z6~7*B%Hu!3GqszHL?vmgI#+@&7g2@8<&Ogr z1Uci7#2nrJ9e=x<;c#etTQ`O9yoKr`t-|7TcAnLh3G;^F!qQskr>3MFBaDX{4w0XF z?zrH2#t5?~*RUc2wk%jM-?~nCd50 z=ZKXmLmPW5>c?oO3@5Bi#)|eAQ?d(6l?p2Ju#bHmnm=k7t>4~?wmsgXv z0a&9UINV%ZxSx;r2rl>yTTzDj73sb~|BGm7W2S)NY{HlPIgw%fyYH4h58Ts6FL)D_ z#e}~q?IQ#1UVi{fPf7SPn<4l<3yW3CG57-}Ge*8B#aNf&Mwky@SzD`vdO21k_j2 zdc8|7rlc9^WPzG(h7D)W?)@WuVp_aKWrl6#_vSegD}VZiq%o5<6&1RdqflH^N;gLv zZO&OlkK>_pj)^zR6r~+O>^Z;P#IlEo2@MgA7LT(M+6sA5@@bi;K*oV2&9gp&b|H(w zlJ$Rgd{zEV+&)hhxqj$Qiqmz~o%``XEJJ3nF>^8RGfBhIuX_|t$Kq`t8Vj4Ud{s5c zW;(gZtmPkOJ?Of5LEB=z7uD`YuCtE2ozK%a)SP^N<>5#@XJXzixHPiq@a*QDw$3Q8 zgxA?-)%T4qPi*%(JkH{W>nvTl1-3)Z#e+6di4zjtVczR4zcHzRuMqF695m>^CuGon z9{5pHC+DGOvd_NXTiT_bJkHGWx9OCAuLONY-_$ZlM5&@U?*t#l?j~KDvu5DN9lpt6 z^<$A6uDN_0D104Rmf`7pw=9bOvms7_sxDz;w&R=XuO`#aKeXxmeB|ugCV%+Z4y$YO zdEoWPix)QaO1r0zhx>gxqDPOKHn49diS?69%a!$T7oV-{Y_~kSh^*sy25oTy>zrK zbcl00Q58W^T`c^8<#JHFt{#7~>}RM%DXqgf%gyTXWZ?1_zVvg!`2-oA&jE>sEEzz%8&W)2B>9-+056rz^R$7oi;STD6kX%*ZX<>F8zm`zm zSQ;RA`S3CDHM$b)N8&x`V}YiVJ^U77912_+*d1_R+&8<|2Lb10mg8R%enrAy5Ik@Y zmj^8^cScx5gego};rf9IF3=E+HeD-qB7n65^Dpr7C!nA}WVzVaHbiDbydFbCMU4fe z96(m0t_Mso#D|egT3XuX+kSMOf>TD46~O{`<0qfGbd!?VI4L&XEPrJyJdMNunE#@v z@XMDRc=i7aWFCa+nG|eoQTLPk(5b+)sQbygl~i6&VLB|A*-W`C_rF-XpxCa1d|Q*^$$!`=NKoUYXWxseZdkxFi?aRBY;vTo^c} zOqxB{`QvB&AmeggwTWtXaOVylzfsGh&Oz<&z!h<&<6EV&gR{L8cX*$*511YNM8eKE zx}9mQw7gVl&|w{-lv^8qk<0Cbz8x_JprH)a!1tG}K)@k4T z>r&%ua7LNEcw*w^cCV~VyPb?%YNd{wNdA11Ud3dt&BfwdXwmqhO6D96@1MS@IabSl zPn&+0wM~kQS2dYRR=-nqH-Pq1b^p&$f?ye+`QD?)Dwm`NtSNgx9XER{mDhe2tjN>Q ziUnWl>-JBChgDKl6%#C*seUro$Hh+Xyq7xTdw-Uj|8VEm{~ujG_eS*CtaWLJ0Xl77 zs$y90s2LI?*ap%sUm5lyx9D2H<{J1j{0S!CCRdW~>(sPz>DT+G`8McmU+Mj$zp}J^ z8$)40Vv4^La^#~S+krgY0~mNFIv;bvWY4U-W% z2C33(B8`(%`6R>lNt_rM+iU;aN`CVlZu1{_thr{CjVrx)aTmV9?f%v2(GM!2bC-qc_A_jZ`Y@zL-#)U2n7=96&=a8_*%EuQA z4t)rkV>E;F96!$SM~m>9rGfL?r425+>01_-&Nl0Ey{nk=;3J10@YwGMU6mUCdjP}b z=n>AIV5oZg_HC{yDYc||_}^HgM1f2KLV2JDe#*^70at2p?P(We)%}jp?J_e5z?lg@ zT!iKn_6By&#dNQQ1rnp#Z$rMs6*t7n6rN(oHeE4uOTNAa@^Tz#fJ>h}NX>}DBA-8z z5TK=ET#o#h5!Gz=d9jPNKCaG)A@%)0$-JSWTpl@rG-uY`EoEdNjyZp%y^2=n6i;{M zw2e6Jv5SxDbX-zzcBSf1+VMmme;&E~=XNK>`iOZ4*Y(5Cyo-JVZ9qNQ09+I|sBH@f zOokwc%+;+RnO-%9sAM>)5w?}kqb^WAzH>b3DwFvQOTn2kuF3g%)a(9Z;6DLRi;$2| z8klEr20Dv@`J(PvzTI+XIS6#h8vrMTzrrZaZbOkyu=h?By$=jw;X`~`YtL+JSicS9 zN^DeQAU>(~I0fj%HTVB~*T3fk$Nx|H`KWN8fhp+IW@-M1v)Q(e zCWv~+U12zHI{xMP5$Ml>q^90}bb8ul_I>#K8#Uk>hyd_iSy`!C|4XXzR>APt`p94K zV*;&Us|jBSOw!8ofElC#jXQ*exLD&*i^W51U)JabKuPAYw#9)i1Z<0gSPD8Pc@qwe zbpLJuDpp!H-8g2 zmp#8+TCbinO@12I+hTY+tA27_6XYpbvO?=D*%6uTy}s|O6@~7|$>?&Wa1+Cgb3-_X z^+8#NumrR7Bi#x5ZuZ>Qr(e)EGk@AlRfGV;WcaBd8Fmg`-slGKD8NYtdqaj@3T4Ei z65({u5KXADuXx7cf(;}r0s!QI#fUJ)2ajWLxYkrx{{fV#p1wXAXe`g4uT00fA7HCb zZ7Xx#u4R{DBaPDdlZanB3O-ecNwob6I&iLlmZJfW1BAQ7%NceuFtG3m2!O@V@va)4 zPzr*+A+jBzn6I>)M35VGD&9)LcZP(71n$COu3;_~^};S3Y26PUbJy9%xzWuu;-_Zf zYr`oRZ`V#am6mtW)yd2^z*d0m}Wn2sUaNz~chX_M+J1ueOD(vja{~V732_Gmt zB94FXed5qjjx7swb7lK?fk&VK5-zN}_it^ktL#-BqB?ig61{0)n@=Zwsu}!9EWK2H zU9aRbLv(2GaS%HS>`(y=LHES{j=6iCYUJ(5Mi~avo?@9RVssH~jIUf{fH zJ(O(-g9eQf-r*W}J9Y)6$yZ*w)*ASnRp?xgL|(=dX;mC+Nq!)W6s}RKU>9z36r;Wy zCU^f;$}eF~&TWP^!~!#AVqqbqq*R95=oT8Myk19kO9 zdvQJHq8GjCOtXY~7n7q@u7u$f6ny#d+B%BmZHuE(>T+^Eq@+*)5eEQe$L*=- zGsinCJ#mmWz-w0l7#kFYFo;2eH&YO;{nf=K_LXyBD zMKO_`V{a-Vzk!4$KX~uW);`A8qiRZqw>x8}lNN82Slhm3t99Jr&I0o#;B&&)U~~^0 zu&KbkS9Q_wpe}2!o1259{`0#Zba827g?ZE_y zxLMep1y2Pw3?2!*hiENr1E83K%^CQW!{gGXz@DKanv~g}deFZKS%hlP^4ZzUPu`C- zWyE~MaO47E6!1zI1BHqvpbW7~wa2n{?#$Ez1tN_eXcKoib)JHdaSa0?7qFZ809+$Y z9i3i~aDXo=en%D1Aqu{wFk&mySdf5513Kwn-%P_swYzVWyyKdkwY-d{-ZM-8)U9L& zH4d;XV<8g^piKZPQSRKa1MUeN?1*+?Xb2C`Zn$kbs;sApCk-_;Vl_*7)D&)>(2=|j zNRtDSP0nW~-L`{f=_H`L$`*`@>Lr%$32l6@-+J;y(83Y%hPNC5T_YN*yq zao7JvtpcNU;)q)!w|{|&>f}`E(#WAFk3;U9XPjze z1&Xt0TmeOj=o&I@@GJaW2gOh*M_~MpX0d!v!p<@~AqxG+hRC8~^9Yf5Avy}AU~;aJ z6HXcOkO4x{VpP6&I3#IYt{og4jPEQAdlxVHb1JCs+c{UK6Y-VrVTY+f%T63FUPKH|%s`k|=o!QbfS^<-Ngcf#VIf z7u;jFj~SkVWqOAlSjjIP({H~4x52E2xjX)RC_fNx&WJ)LAz?M1R^<^s3kL}KD4D_~J)Pj=ym{rV#K>fg-#ZOFtENhgY2XzMoG}>S zenrTgZQ|e#HT!M{ljvIj1j`a2PCA5S1&%*B3zH?iB@p&6m^m{Me)>o;T+7xwnKt)X#Ys-aU?>kOS%=#2}?z zf>0`$#;x_3ZY5!4>v&0Q57o6MufRCK@-CgEWM=i8o3aYS4OmO_SWgL*7lVe=?o-kEjM81EAerNZSufx^j$f3>bO+&8Sh{640~Q#r>f zisV?2+m{_Z+E7wgmRf5*Tfa;-H#vP?s@bqF4c{?9g$21ox97+V?LR5gykIBWWmQeX z--^3_S3RBH`V2+t3tW4U+Z{#o5XK!~%MIZ8BN*y~Yn&zz~w@Dq;P z9J*&<^8?2mtZnU*7sG8`pq8Ear~DSAG9(XI;>BH8jPi!x!aZ2K?dQ=CYX%X%WKkKL zkN^XKJK%Ke5#(jct4d>qu&hU@u68k{)ZP51qLxrMxVP5I%v^M-PP$<>si zhex!JtE{_f@T^?waxAgFoqKe=k9;PW&pCHocycB&QO2YBbTs-Kb(;8FvvD^p($wEi zWCw8PGXH5wJXnzb+-KcBs8QpIV|f_m;L2r(*7b2#F{m46=FC|d?XD?m$t z&`ChVQ#oX~Aaq}lR|z&`YXPMC_?Pr@>|_cu#s2=|FN&UHZ+wH`%mK4ug@iR{jf1E7 z7wjc6ZfRr6N)i%tIRZP<Sc?6Dec-6tmL7DV$L0cENy| z!<6C^^9VNj^tgO{*4q%5{;@WFu@v9E?dwNk{9QZPc&eCK$OE#NyTP15$LUwS$D)d2^n`TqxIIas%#1H zBI~1~PV`H?w=W53rjxnSr->%0UQIsp>@g`hnCL-hM&9!o2=(qIWSc9hyg^PsEHcja`kZwGB zxS`^<^Xht9<-+ke%3yDmdw)i(7Vnx1Z>nS3S#=?#40jX-S6iRpmrhi+@|&s3h7sX} z&cWHrD0~*UiWF2-h}-xVJ|MA@y;h=;x~-JKaqyEVnH&BGku+Me4jC+18e1EM0R+99bI2?B4<{(OpR=iz~C6&gs{7!FBETJY{y zEohsc&xH1hABMpQwHYq%-NOgTDlBkkl7W#RydF>${e53wBNMNSQvZyCFoj8;t)l0- zOH2{pBhjNx`{iC46;+s5!{GzjL7Ljy_jZgq;G9Bn!R!YfkW z(>xH0^WIew(by~)Ib%!bKE5AUD$HCakhjOI<>ba8ru)wD(H~>bD}lg}74R-7xFA>w zWXN~8R{TSwqM+OgX)VIPVC#RQq=cF2aex7fir2U^n8x+LAobAy@|dKF<#Ly|O6UJa zfv3U^a!Fh8gqB@&ABewPdJ%>?TE?k}Vl%$x#EP7|@OQm0ZDfGMR$u19sXGgw+lom3 z^|kq&*R_z5zy2ikY9oQ~F%7>LwX27+;H^j^$^AJX9fxAE*2od-*|>)1y-*a_vc{Ni zb`zGVt73dcsiZQ3a((@T;De3&_E6?QOYmHKwYg%JH~ymVOh@d9g_p-SEGfO4Ry9GN zPx0e7&RD_}M!|0he7s*P=eUO(4o==li+jJ)FCmfnFtR=A>Fs~W7$IY$mVX*Oz8Rbp z&#}gq`UUa;(=QO4#wR-gHSa8 zJ_FGO?*nBG?^XMEc!j6khZvcIU)ZzUu!e9pWuNsuxPu!NlrXd=mB>LC|pGVTu6g&e1**a6# zy`1;|T?mI)KgkKA9ZRr!y|ZFs)OH&K{oRwGxzlee!9QauPOiV>T~pvtJh{cj{c@|( zPkeAx)T$`c@W5xV0%Mpkd$)`FhEJ27_|-X)%`9(xhFu8;E;^m|o0S-w?X*|!crj9i z4cE%lPvw6sp^<+ai>WfyxqU^lM%ki1sJ1(>v{tHKfsRghParhXbHqh_SvYFZm!?bE ze5cenH$y}$9T!ehSdmGZk15!hPi5@_#Q)0Szn|F_`Iwl<>c)beq1d_)H4lK&fX2c! zhaTdAz#?xS3^kCCGbF(k%LL+`j0Us76O0cQ2Sh+%j0M&ArwR|5rY1s!)X2zYD{1?e^NC-rl7jXtc!6uZTooDg_`QM8DwDO8i}6W5(NvIWZ`Vn)6p+ zJAP??rR%Dp*nyf?r#ld$q0ezHTDocfJYQH}qqLpZMwU4{3a%D}fik3LmV)wy3TPY% zxqiVY0S(O{$1=7fwph<; zCa2jD8)-6^hog#{4I08J>~v7DU-ckp3b}!wErPYGV!7#j{;T0QM{fE>Qa!7LvMR|V zlG>pt!+tEZKgaE?#eMpn^Rc-5oAJ}GqdGPw6H`rZ&O0$tul_kqn7!FQw`_j%$TB(e z!?M7{6UyK7*ZOx&OkxuDY4Dt8y0)C>I$o{!W$*CNs!{eHsj2d;Yz&>P)eGKt{ld*E z?A;al2`7r$F1~c-w2kF3a|!9n$&iJ7cSh|pt(n(;h2N?gc}%^EBlSs_8?XB!`-Inb zf30^m0~2q$c&>(bMY09Loq}HSSd;PJ%{2TqFUiR7z-IH%j`ZKs80+OtpU8E6AoJE2 zg>>jd~(8k=JFZpRFc=t917!WW(mVvxQWv+;keX$z$8_F~tY&G<=b?#OmGB=4LBT zUwR~;vW&&3hSpZWdE>zgpUY2G2mRFEEQezOqgQd6CbOj%hwmHsW6-&E(=1Mhmgms! zYVsS>yqvDu-_BW27R*H+nVT9=?zy)yn5(F_zlv>{(UAV;5J&txAerhAb4}8Vqj3uT zMtLu98QCDZ*X8Ji%FtKGf6g8UPu;8jIo&?x6z3j% z_OIW$Pk34Q1gGix{lg+j*MJ9Zy>Xo5b{?mwLeyJ5f8~-*O8&(P@1Vbl8}-OE9x7TU zJLwufQ}amX{?^2NI4T)!>%vHRnz5HO-M#F>HZXg0tF=@gN*P$wta$h}3v#!1GTk^_ z)oZXY96G>IuogZS15@&G3!FXi`p;;k{ zpY6ale1d!+5@`3Fhg5f^zx}F}kNo>!AVdNY8 zh@m5){O_P&d)#S+a92HMx~7uFHqHnSzgkE`o~hFG8kcXChC9VF5|{(|zbJxqGBt7e zy{AO2KcGd1D4Jq}W}45ZHCX8A_|yNLVVKDSW(DHXFip3Dz~Q)_m3VID!t_?LI6$-j zjmHBD7uXQUTygR864cMm(b?sd(}+Y6M9+joMRcUf{)4AwANVm&O#3tUtbK50R_*?b z=$4e*{2qH-!QjH&rB_#tY_o zTW{Z%Gom>8C$Ax!9&z>_4AH#*IlMe8r)r z?sadW{nz0Rh2zuf`>i^XJGvP^ii}%RgeAjz1wZ(G8&^IneUDxiWf_Kw(*u@0@4fC8 zIX~mi&6IR%E;V%WQht|_uC!;aHu>!KEs2s#9yPA7M|UMfXqv=_`q)*@KgjA|7qO1HPt#*n>(J$EW=m18NtI3h2WN!Buf@&%Ug;+x2S}K=*72NuG(0|h z*#2lEa7UcA|1{ag^*DiwfmXZgn>TKD$NBp*D_+teDdnBczJVFNE(IALf7Wv))QYhM zh@`bQZ>c%RiFU)Nk>O%TpRPxcEXz^f@4qTC$z7NlU{*##qp z4H&c$v8c3IRDN?kGy{A+iPsnvdh6HV?1UI(K(no&fDZmpxiB_nldAqN7eM7`E;f-H;e94S+E6+pngX5Y&ijws@+|tdf3|aM@ zTYAUi&nYQo0EH3J8sX(n9r!cE`UoKWPimPG(5&-{7k~K@iZI?vgRV(kobVzcBV%x} zEl5s{Uh8)H7^MO}Wx(<0$t{>!Ak0}nNQ#Jj^N^F4erx_x`r#8^A&rzT_q7~K0c^3a zbeIMHx8y}g+4l{IBlEJA)C-kdzHXvcZ1QHiGhRAa_=!m}bAE8!^qmqqAPs=C6Y%XB zXb+J%amwZ zL7tj5rY>77_}c6}H+I62G;L&rPT3Vcr7Wn`-{Vd{-C3>p?d7OKDYd@o;Wfi4nG^X* z@?W>%JSDl10*UXl!DW%oFL{(dlk)khp8WfQUT2O;G)2wgASWR>wm&V*@ry<*rBFCX zXeZ?$Bu~==f84Z7#BLY~=Vf9>nd_>=`H1N$0Z5KRiak$Qg2_4|I(FPh5%S zem-qeW~W0pe9>C^0vNWS?15hJ41Be@dsi7<=(_``%^$4LASrFfohYZE$$K@`+j16m zMb3==NMHBkh$7Tdhyp0_QQ2f1tP_~H@RDJhn3+L-wld%^Ktup=39@edI*jN=M3P|o z1FdRsWaQ(bal{4m<2B6g5d1(`L)@63L~Eqa>7&^rJyOKC6?Wd)Ojs^S1aE|lq>5J}Y6^F)zyAi&1fjzfIE+9o3>dwC zU|=ZCBQW^~)l+n0V$kqgCZJCG)YXZB!3>}S4!^n0XQjKdbc-vr@836QN&{ztbV&N% zhZI}_T3Wm@)&SC0X&hAK*?MPZb^|t)1Em}$+%JH*>SF13N&j@cJw5WKwXLDb1MLB6 z#9}iR%|ZzG<_*#(6FM;}+&$Xv@AK<%ncNNU%O_*IEX{ovY!`m|^X`;v$Id%{bKlh$ zx)SU-Ru$J@3~Htw8y}}M0TZaK!kbd_&%#0^m)mRiBckvmD9~^N{HUCqd{864+%8Nj zD0R7`+F=IdYxt{6XVY!-;6%Gqb&@uiCsG8bUZ{jl#xWiXVA71|YcTCHpYgF-h z)MIk&X&CLbS46{p4OQ9A0A=#)V#PMc6@AQ7S)v~fZPR_|cm*CAcf;-g zh2|Y23~<#DT4^zWNYblPNK5Nss*H4;?boVW6A!l}_cZ-3G!z?NrTm03CtB6b_}T+B`p%n61B# zHIL4q?NaX8w}Wi!p^S=>?_cZh5y~WTITGjx4pv|dFHDw`&dRX=sw_(r+1S;?W|B46 zo0zi&*N;4`k}nSDhFj!ALGDb!8J=}NcC4Bt-%AY_R#~-Ny~1z2aI_;})t2A?_=M`G zV_u@VSNr+?mwz$Gl4({Sgudurj@dC8v!&i1P2%H|X0S%FUEbZvzg#r+sN2)LYi&71 z%^!%5Jz)F9RXdRa=c#;po3HR>! zgvS+C(bTSavf*y=%_=l18mBu)synhZD_q4m6H6klAL96ipHH)dew{-8`(R-toj`!| zCtgO+47bDYYkR&bwK;}Q?MRXK6I^$S!ja)Q$uGVYGr z`auHhBCu;1eoZW$rR!>!q>}%5e09)P6VorY2|D0bNF?M-1`jaAF93G+_d6auwp%Y} zB%9vBb~=BfW)u`V88-Ilg;*E$4hp)Ok#EJr505G#q`mkB2b{wQ)&VR?#E>~xG42mI z_yAc1kZ%A_UpBauYY)O`{Ws_$pwrC}j(?iJK)^zR94}G76*lrqVZI(S45q9tattVR zYbokt@;VvBHV?nWPi^l!NF7)k*Hw%O?5VX#6kdwQpMX&~+aR!1VniTLLKMb~>NClF zrkFq`C&NaCoU`E2&{nhhuW@8!?&s(ZWs_U5L+lA3Sqr8988#N6fU$tlS=Ix<;$UzC zL>M^9+y3Vy1atBzz#RD<)>O)8EYrb$47Ds|i)8VJ8%hYWT}#DosSebh4gYcn?opXpcYROh&9E zN}?6+Wr3TA?K=*hXUp;WXF-u`!!KvaFM1tWEN(jegq<0R>*D{RA(LG!h=>UqMy_OK zH?RND$8x=HDy;C_CkT|wG(3%cex-p`n4y-qsGOLr#amI|aW&0x zHFK=iuWS>r=3%RCy@+{zEPY$^;(g0{BDTJV1#&jgUEX8V%rM=c+o%%l%Y|63S9AOR z*Z&M3Q!;)QHS=q)ugPOWlUbe0Dvd5ne8+tDQ7$*&fxFh(``WNe)W&XOyrI0(u2GM8 zkGaVL@=`YEEqiSa>XNM{W`~&MVQ(^7G_}bFqfDxJh&jq*rv5WpP9nj2`t{hB*{kFg zQNC{rP2)x{fB)q)+KlZa&Mz!g{0HVJvvq;{^MW?}y$7$~h1g)+2@>Vh*SsckKV@2I zyd!>T#^C*DwXICPjf__?;j`C9c!*9*y|;%yu9|8KzD}QS;|)nbN?n_!i;W&hr#|)D zm(Jna4h%F5kQu$$N6^4Q}#pZygy9Jq`gc z;h*(d_lCUBZ#x8y@z!+k;$EICml9;Gu?4b4sjV9<8?01KQ(u`rW~ctJP5jDBF42p- zZCY^F0qse<2u)o*a&?1PM7_+qz4r}Iue}6AeFd)TraALWMB>5B>|^`B?_k877?SC{ zRykmBP>(CoLM|jRoQR=Zf3P-@r`xv{8r>$t){G^BMWyt(>dIX$GoXP2XP}JUg|7Cn zHHL=6n|t7t(I&OvimP>3=@_%QrvvNUV55AbF0XrZl^s#`i58G3zF zq=D+m`^3)S6oZ_aw2LMEo>VrQ)T795hbxl?`zsN79w%LP*tDne65pN?ecu03O~79e zlG0s2&uL^LgMm^a6K1%-3g9(fh*6TlfkJzc|tyI?gp>=Vlmzsg&FT|p5yHa@!lGOW!Jq*?%%Ezcb2a!{omNIFE%E(DJt&t zWw|%46w}A}qvK@JUqouTUhcg0@-QQ0e0&q>?Gf8n=Je=~!u9cZGx=_HV6#eXK&nk` zM1j}Ok)e3mR|cQieh-9BdJiS}_8d|31=N1r5wG1NVe!XOEJfot^*Fj?lV7)d_01f^ zyU~-3t>+)PsHLbxb>qN?F1H1F<;ORN53he!`Uvkf1jLM9w_eWQe3)GpHxI5^ebRL4 zhN~zqu~@EXj!o;t>L8&#MBl-Q#vSl6N&n?QD;g0Hpc}&}yeMx@pVOnf-P_;yaBzI6 zU%Ix_Djk2?zWDiOsPeDm3j29E9g~mcF7`$_&iVZT&SjjP23szY?E9{-R-Eb%?x_=U z=6bK13ObVB%d6d=*kXZs46uVAN6|ci(1$le83+s5uF1(nAXos^1*SfX3=AJ&x(Ek9 zTqg_=KmkmG4x`1h{pDL8N5k^BpzU}-zCE{KL3&@yS_?Bfj+TSAKL4POu-9&Nb``^> zVXk3SW1<$4qMfVbP9{zG0BMFz1|*k|+Dc#@+yd4E$vn3~lPP{(RTgE;#;Zj={4Lx?>|%9-9mH_-trUbC#$n=wjW5=r;Nc4i1pl%Jk}mz_w;v zsZw)Q{5j=Ku;I#kv+bGI%m>1iT~BZ%C4Q%mkr4f^uzdIL-9vVEnwmYlR-IyQ98i7E zzVzCYd8|kJfJ<-L)1GSe6K}f9m<MW)wQjd0 zC_uQYcJn47hj;s_Sl3$tLJ*cfwF$%r1cepin865){T3~m_;TL)$mGp^xm1AZg>AGCahm&f8AHcRiTW!OHM zy{lF;J+1=8c3_VHE3+HUz>!=gVo%_60vR5dfy0*a`We6rZaw0zJ|(>c#rgo3>aR zp>Nv-w7g{=sV}3vq+gDdgeyL6D}7eVXW_lp$NfHsQaB)cGUfGBsAYqiQ*()U=8Bs@ z-Ifx{7t@c|em7e|txH(^cZ#sLjv#}xB zD#`vGz*SO0tv&kgL6f_iXW12(IF;ugReMNgul12tt){bLRRh0MK^tvkPdu9%kXb|s zB*G-%i1@>W{}eQ_&`h)9No@Q=za2AXsZ=%@Mq`{IOK1LaMD&E|w&MB=eBbzn6_j08 z^K&g^i&8oucorc&N3TmMeArw1e0M*Qe3@MAHQlEj_b}wy%#|(EsNv`1zKGBEgljph zQ9V49FF0Q%)r+~d>e5l7$LSG;=Hsfm!FGZN&DRxa!Zgis!+w|iBL4^^IA~k@l4%d? zPb#g_XE7v!|Ipf}B^+lnsJC%qKnbYza28Q`F?bB@3nLXGv5>OFXN)PgDWT+|X4g}9 z>tb*yJK3)RnZ1>TaAmw!OM4~Q~p z0I`xf;S`Gac5&{?HdDvwFZb`3)FRc>v*Ip<7Y&8gq6AY~+S-4{?!x0(;!8p7)EQ3x zsL?hyJ)QGJ60qz0G7<$NTgvQn7@&U6BHj?evQ$w-k7xY%WJ3qPcVKW36`#mp>$udp zp+4<%?FX11m!gjSN3T_u?)j{#UK^YCZ=luJ_<5+SyMR|o$nIU`KC`EtrQ<)pf{vnN z_o3@f&8ONno|%9dmoKj{M!KeR6@EJQG4TZ%Ncr8m%)naHl5G}dG=kl)!M()v*3^aKHONc{I! zNMJ3QG1R2T=!b+bu{XFm+0YRD(dkSe4cs#^Xn89jCY{7DaR8M{Ht6gHG!Yt(D&L;h`bvXKs2>(EW+~(#c075{>=u#8`%%<(F z0H0AiI*rQ$l4_cW6wtYz?us&z`dY2GRjfStA}Fii$lPChcyw@b85rLDK#c`S%`h;d zp^_^>*>x`maLElv=wX0Y(%sw3AW-2&>EGS001PqcXH*aAgFPaiIBpM!xE#>LUhnL9 z(f{wg-hPgP=%-Vj2Ed*{yww{+UGhbHztMNqap~zLWVe~S;(jptKNfU+V4Xb^xbkbS z^X$MeT-eizG<2)%)Wck#6j+YH8HQM-&euFYznLn7iz`BbHG^ehI|+`VVZL>m|Py z-|2HXz52UGcsE;r3=&)rs91dfocV?iXi?c$E@RwE6&!9!wfVnNLuaTEpHXS@%58R^ z3Q!3xC~ruyKX$^*L=V_kEZmN@7Ak4&KiJVUYcNVseW$jE|!TbaSINEYIw ze!`ss7&zF-cDxnPfcOgd)8<$qWJrN$7w8}t;l$*cX1A$!9OzcRm`kl(z&saGiesoR znogM1kXh>>i5S2dBi3guyh3INND`;MKoIeRlxHu%IcHUd^N35 z9ZbHP4$1lX{_+o&%QR25Dk8ztnITzzQ=C?}JYdpWIJs8xnQ&DVi=O2qk$s!$hGLzr z_I~GC4DF|QnYPrG;hk8*J_2lI*IFf0bb57Zy;f0ak@&seuRa;f8Q8S_T-kOY@nYKb zTfiZPt6W6|J3BiYzJ0TS6d)qEcNJV``aXF7*YQB9S5obz{-^vf0+>5oHW zSw~+SBxw+y$G?xYcWyWmDH@-m8*Zh{?xnQh&D&bkc<9G2%h1pg`Gv-Hn#=~;}b`z|5Q6SVn1 zPUHu0uZM+WDa}k4N~~X54lK;ul$jU=6cPO&m63x*w^7X2*6MeGA^76heDsp}1>|Q%0nHJR;PpWd6 zESOrp)NXE7k@EYP=NE@IP^@E<-Fo;V;*%NL?$9+yCiUK@4llJ+pCZ2$KMt4Ud4p(! zo!a!s@0Z&3OmXRX$3OvxA%CMMz}8W`~;@vBW30lEq#5AWxGN11$R4atyg?(GVg!IdlEoIp)?{C5B9K z6@^f7g0Dql;)@ndvy1O=gsR^tYHo|M9Pxt%(A9mHTY_4Wu!D4~4Dj8;AAhR({!&39 zIHzSgns=`P_t`Eb;&9s1LI3i4ABEG`Z$9^_8PeF!Icp>GD?4YiPxpUtJff}=MkV;% zn85so*lVJKDgV3g;Kd6Rq348Sn|ckN>nIs#UkF->1^>DU5qL)&oD5JzNQMmUSP%{s zv`n~R>xqr34qUq(tH%tCJYQu}%TV}WbNC_;Ba-3AYdQ(`8rN#Y(=Tecs-}a6M`M$} z=dxb)iwZr=9L!yBT5X;yyo-~6(Zzjskm&L*eFp7jt#Ikf(zg;Ut>gLzw25XEoA4rx z+LncgA53YQdU^v8_~H@sgN3sVsPbn0WW3^XUd3bXEz_KU1$Qa0nbw z{9fQQtsJ{y3db&6YsXi-tNDuKj1b@r4z3|Ck{T`0Wr9N{Nt^^8t%U9oYD{D~C}_Y+ zl(bSRMO%j}9B62U4>BzuM^=9@kco`B<|4^#=)qg*{nSHUI`LZ5H4*)oBf*(1`>oQJ zxTK`P6%{GzdO5apvn%7z8jI#JEY5rP6g{XoqLURV4 z`9z2Dn(9Zk&AqrL>+tWfu^IM#%QB9fpQ4*8g^JC`YY?S8) z{3Ur&e-%sd44q4IF+a~9-d(P*V!Yp-=@b3dEj&Xe@JY9ty1aU4CVA>ea#gm}8CoFW z12YLU3*XH)6A4d_#_1qnz^ev%oqpYW&FE>eiRohf8sB=SZz*kWHrdld!ksFFHyh;w zjgYXgVDhK?xk+<-bAcJRO~kgWi30wQlBL=WXPcO1VlVZ!IqwtE-QPrydX-;Tlz-p- z9zk0aUv(Y60=S^;%hTg?!nkf ztH%Z?q@g;Y-wn(zT2OS*(1vp=Gvd1@7v!FWC}}Q~qDK`1& z$}JLe)XScJxl(6@G`-1Ix`XEKLy18BZ5_f~9L73JvBMMg%W6^8t9Ez1{9z8Rw1vCs zj*Y9lA9XnNPT3~VzE~H#*keD;kwHhtrsvD1{r;WlSc_}@B>FIec5(FO^MFhD0oTEQ zdpy)0TSHhB#{{OhXsemw%-ubVUqL0zhSJf`a7nc=;PkRKAkW|?aKB3N2%iS2rV(vS zy=L#7TSpbcFG-&T1?0BtJvR5DiOJ^Oa|x90M%~*N2TMbhG=q28y_ZTJ1di~R3jRPs zLfHxzKPaKLwPiTfqam9j*R!SL%&Fr&pp~@!Qa)MMS|{?RZzY+{b66f_k@n|@7v-}x z8&$&H3HODnEeCMG2L_vr#|K*fQ7}{32lrxW*f$%~)!f2!E{sqkEfv8eKH{(LC?lS= zeZUx<248SqbNyk9ZO{Y5u5n%^>yPdcHATujnvW<5ng2ccQ&izSLLCioq==A)j4pa3 z;a-JhO!YFvhLT$eQ6j>75VA<{vx|m#0yM8_Qz@o4 zc9Mgb8YfFgHUl<(Ms|dCU8Qh>{GwethOo(xykq*K@7#Re@0lr~urd5PVn*gR%lVO? zDSytR>@(7;qRrn*>g?!kuKl{}w#!dCMQG~eNoTptpZGy-3XlvFNx zm*Ax!N6@s^toWM$oPIl}w6kF%NoN9$Ji`X&!CqOq%*`^gNa}TnRfBqKRQCDDj|Yxh z5o+uL3skb6UIR)6x|0n0Hu-KL>c&M2-oKWmJMT4+-MGyQHm;%k77TYKRgvlY>}+5$ z-SJX1_mMM_hp0pYNiu~-ree|nS=+~kk>s8-8{^{bg3iIivxFY^u z36X!kxJc9|VFk=ARJTq}5)V#k1%K6O9<6OV8B<_u<u|K@{u0BQn zwq8Hd&MeQ2E;|~UC*}){k7$dm#Rpz(JPyDVVk5^zA4La(o$IR4{ zM3V9j#E~EP2JjIK<4{HyV6!)4JHY>{UObF!7?c%IfCtkC&_K$HaG!v002F=8`XeRC zm3R$sE7%s>nU$6aklTrejq*yXFOVB zO)f%yqmlOAtK9INx`etjxx_7vIW6UFo3-0iz3JJ-k>dWdt_?xNWZ)3=F!3mZ^E9ohs+98skyQj1#4UPd?Z>*NfBO{NvY1NL5whDxW zIENRkQ3wVsbdCG?%h0&A6`dR{N%SdH@wXMU_s1{pvGAp~8sEgdPs`JnaO_rc?@dek zIsl|NR!0mOwM&khh5`=4uN6m6)XvNu1qNyTyrM0aVu{>z{=TvsFVAGs>I+|ctvh_n z1T{qFtNO{#qHyktGdDqPU-+5Jq$zTMZ1C_;^aj}ayhsgPv!pSC~(pu&6>SQi*+O^-r z+)PjCz-4cvamT`)LutE+TAGya1oxbsWv0@BIh=3Q#FbCG^OCEF2cB5{E+vjm*`*=KVu532W( zA|4|afdtL)M#EXPn!V<`77^%>6WEPE zF18^w<0=hq+z7@w4GqYgUxU*zBIYC`Kmq>aKSJy;pgdytymo81dH3LZq(9beIbB_E zd}KW2ySaE@hE-nDyerQBN&VfUYo2qt&Tnv#h4sx*)w7-vj8d`wOf-g6-D;FHTH!d6yK^y`SX{4wf}^TYZZAr>jp)KeN0;41Emk z_Pf~2X@~d(>J=sSo4ymPVki0$0mxy|yr=)(e5)>2Krd)j$Bw%uX&hD3D~egR!ZC=$}NdH(6TkoeA=jfp&t@T#h3BaN2}{w&9K~eqUnOwLAm2(3$gAd%f5)8 ze z!Gn)w`3@|XQPd2opzi9#~P|0V)5>>e-U zN@t&v{pbm^S=FI_-{*G-E{VLWAf zIn(*Fbhc?}b!i{Z&U!+xMd`fmZ1aQgs8G(6#Ew6EHGC<3QHm>v34D=ZOtUA;C!fq7 zOk76uqS%Yi(|G@TIqU2u@;K}nucDsq2fEXLT1M_qJ4glHlkN|-+{o8)D~F}j^A-$R zkX@;9_@il4J5M_<{B=g7oZ|fB>25hOb>ziRl$@DK#hRNxs@|G%do^^As_4zR=$yi$ zrt+KJB#L{XwQMnMSRnfV;x|ap=_T#p0R#*f(3|X^Pptlp6G^-RI@}SY)!~9=Ah1$l z?|_I60tV;nFk(s2S;M0XQxHheK|>3=2-vv@!4nPQDaZ9+OPK3U5(ik4Fu}=@h^iGF zM}1sJ?Qpph)g8whP4xqM-)MfLrk82y}6x8^g90+ICw^2ccKkC+w5E15^nF*svl z$0E2Njh9ChNo9{z%dUEG^Vz@0M%cSZ+0LQ~CB{CLkXm0$E=o6MY|corNh}W!UurHg zBe=TMYnVQ~!=NQatHi?=hU<0`=jwbR%41c}8|~B_@nf#v7I$ayD(af=Y>)7zg<7j; zmg8PapOlXuPa?y!ui$#~!sC?lE&ATU$-T}q2U87vz1x~qR%cosS{KA3&)A)MM}0Hz zDLH)+Yz=%(D^)NchJ<%`|gV+tk#OkB#4kk9luE7e=>Hk^&z$}QqQVnyB7^X6~si{Au4B|U#)Rd7mly*A;w zxPq6qz`iLwEA%>CpyNv0<^Fo}%}SbLraGhj(JJ?~$-S&?@5e+)Az>(Fu8 zxcE+HY-e0|>t_Ju{?8xJn=7|P+_XQqX{X4I*GEO!+};Pl)(^1C7hY<9qmGKqR}A`$ zR+o40of6jTh4UeHoNw)~B#f2yB{V>OtuELiWKgOuM2iv`*@TTGGvh@n>F7U zLn{+-HlfiILwbap{75spX;1LlYiVS_*-A$a>yJTSXd?X86+DJUnBv>_xJu^3w=CB97fRjL)cqfYw^{|SvZO3#df7F{CUIeIc-sC1AkIZu8qCdvlPNUOIR`@nYA+D zOr{%;`t}fJPE^dr9~P=JMu&NByz0_J2Xs*)>QU4NVTTWMtp?MjhoHT^Js32W>UUzg z36Q=KIHQ6Zj{fi=I~NzMOU6Tsa03qm+byz-i?LN>5iU|%MRAdl;1Nj}L3O`zo-}Hx z-?H+KHDo+O!*Q9j7mj?`0S|o4`G&hZ15=q-`L{l1d6rcP{UM)J9J*L8_Jx`I5BF163c>bE;J7$^Xlm+5S&{t;?z}$v_i}^MPxHIt zi2;Hy6nbN}*mRmcc}ZjV-H~+@n8;5QKS7&cnE#f$`NSiV0u5Sx3Zv=+Lzw0px|`@{o3nXKf(mpZ5lF*H}fQ)65K55$8S-4b98oo4Z)B6`MG?;@Ly5mDb0Kpd6VmioGpdN9a8(cKHNs47H7@d>s}*uE;&BM zqk1L1BMJ*tvDX+6Ej$t%&z7GqyKmZ>_iwL$-*rpqtd%}ny~D}I-HuI2a^rfb?1$%>VmXnc}eLSE zW$d&DrZ7jIRhxrjwvU|eOE^v>8*UQr(Vgz!vo;7B+E=O+9!8O?YkJti7uUQqzrVHX z_xteQw!*IwR0(U19;p$M)bsJ^{2JHi+`oK?`Jc;wxXX3ci+6f1o$#K&nqXvzlK3_Y zHJi4YkY!uV0#^9JTqDBh@K3P%?&6lRK(~u4MLjcVD=DRwe#!5j3F2J;^0J-nN3=@? z6x}(zGF%1vTpLd+`TnIdvmSn#R^=IQnM-@B8$+b8jgNA=gg2pZH1e2lzni%#j|}tg zTDZ2#L-lKh%w@1=LPhaG6r?7u`(^f<Q}+&Jl6`|sIhZBv`wL#)_HXr~k~Nx$q6!3!|KQ9_ z{vo}FZbo}eVA;c4fR-c5UDd25_)xV4GdJMllxBlq@A}#UVX7c2ivb_yglQe9g5su> zQ#BjycX0q9b9R=vrNytWS=*o|w{SidtK9j1h$#5U1_y4M{yU`Hp1-rqNX1#HX_kCR z5=-(YxWC4vG->5shp@JtLr246yYAnUjKU`-UQRGhz1@+P3(};ekJUz*!|wzYMijl< zy(f3R-@b!_uJMfJk&wa9?Rm$8BS*)5AJ>-(C6@fV)hBr9TtO;k9IP8tJw-|W+{z~$ zbCs)Vh4l`XDy|dkPB?RY6CUTJ=4eU>2`6#;g-1y_=hyVHn!9AQi`_H|hl2-~*NqH; zH+_QthSa+!HO{!TQ0sz|CYUkPygr;7ULRE?Rjp_JT8e$Mok;kG*7Ujd^%(Qner^B1 zL*t5K=V{~9$Pb?l6o~g&cb#}DuX`Sz_WpQzy{TXM)38(7>GDk{_rw{^0ROpUg{4~O zInmA^qQWt^vjoqjI|;=Vyk1qpJJVMb;j;qW1A?x+oE8@^-W*wi)Kj_jPTpw$E1F>> zap#R-&-Q{!wniM{f*GT+Aiatf1IR?**mwW z@|SO}H%{DkTTL(bk{oTPSCew4&Mb~!I1J*Ub7tQ%4r}PbO)FZg-&O0(vUn&TcOmI< zSnt{u?&H*%eY(^7g{uCaYQ?M@FE8oYT`RjxF}I0N_bl4L`=T@iuNmfw65WEZ}j5(hC!eS_vtXJxw~a#H1wfO52*NPXe}~*@q86ia|CiCCD3l z(B$(_KmwQ<;7w`(F3>MtzI^81T>>j4=Sk7#6_ZvTC}X$_9Z%y(HtE8LFnAlEH$#U5^CTc0oR!m>QQD9SG&E1M@z+Q+hPa61x^7iSRmFLB@-U7STpZ(TB zQb8a6o;H4`8dGc^2@A%w5Q^7^2asCs8;>^-Tg z_er)`o-_9!wlsyGKBN|!{l>TrJ;jND*mE;7dv#}d2=yu zyA;a#T9&9t0PxA8y8Z4LE(SatxVEs1I)9L$`dk0N*mfRz!+0g8Q1tnO$gW$J$I&Bd z781d~;9hv@CSahNRZ@Zz-YH^iOuIQzvvlP{di=Ff@a9kn6RrhDs_@pR=tUSG}ZmQu^&0xdhThk zw{L3lZGlYF0asF`ymIW*M=TFQa>Cvn>C$-@iXzLE?C#y@o>*OXH48Sro=v@3RY<#H zom+fZV!v)Ej6_g%$#VR%@pO{tcg1XTXdIC)4rbh0GE(-j403hD<0K&@>@Du5k@>6p zjdPx#Cay0ZFkk*d+qz`Nlm798F7@J3SR6)+;SZd~E^#&~eNzRCh#%?HcSQvwLn^Gj zT-wh}ZigSe#~GS&5hZxnP*D-v(eWBa7}Qs`w(Kb>DP1M`IbQ$i=ccEn!R};xdmCg< zpffQwF#$2@v&n`QdK&}Pj%^19f`-whF^?zuKta?6oB$c=wq?}<3d9l(c;7|KDJb|} zTboo^$OLD_8Hl*{BSvrhGw&7rVq_s$`}AvicV`!HQ8@b3}* zhS&$*aaK_xD@LIR%3Qb6x4UmL<9S@%Q>>z7FD6$$0tYoK#}RCrB5Cy0O;23*{j()! z+)rjDqC_EDyM)m{;?>kyTMgcSo^XyPGI+&9?5$5H1phEN&i6z9!QS;nqe#yN^Fdyk zXP6?&LxVqjx`zd62c?xCtFOXNJ|kl`B-=O-m{N4v%inc0PY`@w`BNH4U;OOYj|z5S zJGyhnkNDpuq9%JJ;ia96Uk}{lJlF~4JlL}$COf&gjM2Usd7Txve9^EM_?jn>uDUe%}$G}kFEP~KAhrzz`GTf_u;p{z#;j@=BEBLqF~oITyf9p zH_dYH&~=&_bgETURpD1o7vN79SH+7GR`Ot#GWI~U`CD+}{1z_2(_XMyvst(Vs)?-J z-23?2BDJ%HZDWz|n=RUnYiH#g9XV;D>8Wzc=6995J+|~omkKK9kg{-|fLSkj=D?YC zc!FF~F$!V0ooC<&+oz0|ZVPUQ9C)^k?=`-Up74K085^(hHN9h*w}$k&%vPYonLa-b z+Mhyn4XHw16l?<+aa5bhywUOT{bNy)D5731p10F@gBBEwMefE6qqgfIivJ^K3-Bb*y7Pf$(tkT zGa4|ST06_heoop`XJo70F|3t8(XV_HImJ7F!6bb8NoFLxa|+bAGpUD*of;bfqPKF!SX;Uj2iJUPs^J9y6i|%8*dlaX9h;pCPPW&GdBSrRK&Q7cxPGa;5aIw;|ifDDl6Y9WglGY z{+fv9qlmw(w!|oZZnNN**{4B87Q@xv-X3IA2@^TUd&K})1%iA<#Y-qp2&>rRW2`uq zAy2rREG!V-i9>L)8X3`mH+bvrNvTdV5~5UTFyT#Td-4P0W(fq+%H2O^d*DTbQ@Y&=9=O+%JRv^3=dxO0NCw`TjvlPAvx5KcO+t*rpd z!jD0d3EllB{yNBgB+QludCl&tgCp+e;a|uPfGTmYb^KO4SNT!n>&9ftAfSpnQK3 z*)A=M6!>azqaipbNJ35yrD^S;7_v`+TJ1Hxjnd)sdUw8&R5cdiC=6AhedXiAo)ZWK zl#pJ<#l#HG&lef+L5R1KrR+$ z6{y!LvW#V@cMqs2+Hfcn`f}8rGx2t}Hr6DtT&rqd&16gyX~R{V|61ZiD4(FRY>;wV z$L%tTir#bW!Bj)*g@Ha~GaMiyqvCr$A;!E1)|Rac)^Al+e?dYx*fr=m zIjP+LjY&5Ut~GcLD2K1zkXG<(q(FBZsZ5ox^@3L(f-I6*oG)JdgbeLZSc~I`hlc4T zIz`tfuO55~_dRn#1Zwx`t^4T21d!5X6&1e-ql&S2l%sf3iHPGHFO)b3*0@;wjeMlzT3R=G4jiob_iGd zyS}~v{nAQY<0Unw{~fqi+g{+9u0DSv{X1rDXk7hDl_*~Ml{;z9mzNtHqlLLoTuuVs z+WZ|0&YT+dz!dLjL#nrrXF42*QbpcVvhT?BwLkT{($CzBF;^qX?Kh0bQR5sRvfc_V z@$>V;6~@EM3zcFeTkwh=4B_Fnp+8Z9Dh3Ui)YFBi+&&7ro)UTSg1RBfCW?AvQT5_p zI2;TE$xi-FRPUZi4#$|gD+hn=}03{5DAFBKiR zd|z?hQMgjgd0vK+m&2<`sTjfe}s(ZfaZUHz@AvPFxV1ZWUv>jd&ds z;O*H`Nm8)0<0kcGVF^-Pj{)5gtPQX|+Yjz8nUNRpNLgLNn|jN?HGQkct~C5ti42f~ zBxV@!=Z$UTdPUGg9@{ySKQYXJmy-zc(GizE+<%Jb*oXPdyV9+( zkfkz@GrokD_u4J?n=H+U_9$ib_kIt2ECX^6xC!@=2su@E!dd^j?nt^%n+A z5@kx8cPvxZ-kt-}$iYpJ-gNZ{Yn`;$#q5!VYB_^%MaW!HabyhHLidlys2c*l6t8;I z3kxw-V_^}N1rQ7@4l)Bzg*J*Io{B=Y?J)rf3EY{3gM&@r!vY}=*-)89%gHpv;ewyP z+Hr&t5FXG*9X{w&EEE%dOg_qvDsP*#4mUS*it<{U` z<=l9vl}ulYxQ2N-r&+}mUiu=O{0sMgfK-%QSwTTWT|Ehh9hIi@B0BAoibk2q9-V2g z*K|ot^4ST?AgxzMu0vbZXX&g|b+b&zcrR&El=}vMr2K@C2@sbW^mN>#e(A9hkyMi8 zKA-?=!)rVhgC;f$UvkQm#h*Q+3?=fE37x~97Xk`mVq+KWc)mrwZ3!4TCJKCOVs4&U zRi$!&6=)#$5ri>%CMKiV8hdEs&CGrb$4>pu%gq&vHJq{Jglnxzg9#kza|;Wz3Jb#) z7Ib0!*gOIskUZp$$3QWE!O6)Wl;)d+*A&-qNDZJkTseT1!IcBs9k`!{hK7W+wMk1D z*?D*#G=aGrF57%3iL1Gk_RzRkutb{-<0f%Iek@stZ@_&=0i{ZgavZ~&j%pG~X94-2 z;)Kdnt?$FF71Kthzo{&Fifz&owjE?yjDSjjU`hLGOnYc>5HUN+Wk+uoHIk^tB1H5V zSU5{e3=By)3=(ZvV17D^FU6ep(}x!Kzi(VXzbOZ{r(AEVwl{8fC zZu^ znQ`22>$O=psNI441BC$g$p6kai%LO}faS=Js(8E4D2?ISGkkpfuP`|pxWz3VAo}Xy zVCi2Ry@=i#@#uNw;hRx(y?ZZetANX58IikWh6>LY_<0c!63Xk%r{YiF8xt z2n=*VvxPuD#@v+y8jNZ^QK}7^npQCWR~L(}f%6tuXt#Fz<(y3|!Nxi{xATmwnMsau zke+CSz%7z+S_;)~_UkxDetv#`cXm*|W@j4**(fL~ekV$H8R@Hxs?;GL5IAxQ*!z2M z)0kZJJvYSkZyz4j_lO zCC>u?48w$`fbYr;4mW!JnTUs-wDDj)fmOb1Tx<=Ip)T5)IT zwn7B^0@AAw>Fy1!*d~i-kbE3cy}cfyD!au)9A+x$X$Dgxo#`4pX(pDI4Sz0RM)j)h zG<<^${;?q-#vB?Ov!A2Y)3JsR1n_q%ePL73zWRWMS1F13ZU3=Z;39D9Z^q{;oz+Z`m%JCDS#f29Q4UcIV z^hz~S@hTx^{HW9sHH|jkKi$2EYNt|{x2_**o3^SWsleD~zFO!HwBb&X3nLZm@8g^Y zb4*ujF5+Y^8{so1jd!CcKk(C7MsKy>8hu7o_TWBQTY(G~Z&BlQ&9I}s{x@OK`{j}I zyT%Y52Ocl9ZL+c!RIxEJKP_i@5al^vY4cGO;}b`gg4$8L4YTQhyTuWAO9^^bkjl@g zpJbTvZ1-mttVeL!O0wNAVNW!Zna!>VqY5=9Q>YCN+^66wZ`Vt3=ss2X>M;DeS;ad}37Pa6-0 zP}$RAno%kdX>a_eLuviAC(A_xU5KHDgMt7~Q`XfQGY-=h$&hgwkN%RV@M--Aia7*M z3jia6vkboeBe1i?ciM;-0r@@9C-g+ml|2L=8$-6^ZAgucBX1~+%-w(lX- zZV*q+Se}#xL{+oBDBm_Y;TcwGfl0D5DTc&IgsBf~zBF)zZM{@}g{n-&)0Y+d0#6#u zddQZY-!pt@OZ0U;Nq#6g)oK!-o7jcuWto_$DNLqRTcH9PN4$!oA1~A-ddI=S#btsY z6GsmMcLHK!bl2J6(>=H#ZLAKcsKmp8C>!#5u@ExVSW`>OmL&q^x&U)k;XaBOG$e|O zaFRoGGJcW#va%P9_up%1gowwQ=I6Dt52Xg&L5fx_Zfkr8rSywO`#k6)zt=+nfGZI; z#E8zd0|Nsz#t2mEPtRiFRF!S*EX1QM9=S}`yK+ufT7ga!F`B{e z7)ccf3+7u;cwsgD0>&ZxO(p1gBMlSok-9}BClkXYTMghgbi)tsQ>?mPp4ueK(F&x6 zL%#PPhgx`9yGmpLafP$LSy&rZQ&SURAKo4@I-PB;`hE1G=*ls@;^~nV9v&X7pJE=( zPaT)Hc{@BD3rbigQ|q?fzCthUIm~>zD?@fZk(8&j{&KnjS?^aSO$U1t*__Mg4Lh<` zZFnW9B)j>uJV*f4{JXs^7S&OD1p}7OrN^VIz;h(qlvT};kp=6v_j5+2^aIh8=Ba?h zzKn<2V)elYjW_&Ku zQ79g*{)m-)4*njCsdetsow11t+s%SXx!fYZslj1F&F9abcN>>C1A-=^kw4ni+nZTd z_MR14%#V5DhhgdDqv7=Q^xKkzZ$?tn(!x8Jns-m4@qwAH)6i=%e`Pbgd&Z01)urmKqO`$bk@G zdK+OnNG_Cj8wCXgIot2l?KYI;qO!SY7r9d^Y?QH4)A(<#phzLK2CbPyGIZreXsePYQN*?(VyapjlY zxMUR7N&*o)&b&8%ekg_lu%uNEc+BB7{rh(gU#A(+3M4o<%ASclM}D_{V+0wl@e#%8GM$GFF1tX2v(@c*3Q9q+M;c1JDYy#JNHYLIq`FUd~E%?LZ zjVE)iaKjuP9)1p~tgF+YH!fB&v9f~K(ooBzQCpG-BO*05Y<_o*v+*nxXTEdW0Wk3| ztRT;VZ9;m8bPk#*A6E8AZhksr4>6bZr`1)Xl|;s#t=U?<#`ytAd5ZZ6`-Ubk-WnhG z>sbK{bP)1Fhf=lIK?HNcy$(gpC@LFZi^9oqKAtqbEzA}K(kr+S@_M%cRDIu#nxm-M z@+#E#63ab+V`zMc61#=q)WxqtMP2uiIE8Ubu1W z0`~m%#l0~yNldw!j+zl%rCy9ZL7SNl1<1wpt17Sx0iPF^vtB<}+T01ng0@<$LCR;; zlJax+%u1vqCJ_xO3sKu0(-2(Y4N5Khw%e5ghl3&x_~9QQN2ObOgR~e$&*zuHBEYVg zL@ej9_F_~k>w+5#izYfSn)Z!fD5u`m_R4*I?II0Pz@!4jNt}@Zu>c4Rkp{%goiL&0 zY9Un}7KRCjOc?!%WE7l+mjG4jHZ@; z01D^De+yX=Yyj;bzY!Pr|2Vg9SK@Z|5wU{+zynT(!s0;P@ze*H zQ&M0P51l?7?varZpNY+tsSiaffLnG#s>{g*c>2s(w_@1Y+ZO{Q#KXe_EntxJ#NWSk zFsk*`IQcVuC? z_4P7zG%lyef_BEyeqJqr+drxD$&`J--G!;JGQWjYVhr{`&u?0u9HS>bS~o)-nU$o) z{ggOm`sFH&V6nJ=z8QUcxe}x$c|{FUuCBcB!=)NGY8D$1y(Xw4u{%A|(={1bmG*X6 z0XJCjAMm_BSgQ~BC-tbh<3xfg%43v?T0@BCv?g|akT<`2rr^aw?d|lCuV2vZ%9!|R z?N6pD*E?CF0cX}BV*}J5!B+xD-Yht+`Gi6kLOKEGfq{SwhTLQNFEsLR zO^S2979UAMdOAtnoTJnEy3JTnfC$ypxNsB|VhV*Uc97>M;v}m@QTYz$4_$)1 zMCd{e;OPK68Cc_>V`4sI^+(JO4WnjXtP}i4J$2>j(;d`T%!!Sb`9dfZ{Y z`0elaPk30isglI$>lA84#cc+O^c1-gE!b%6iVwwFG}@MjY{N&w>>-)gseUe^TGR9D zh}<3zqI01bO*VSo#1w0qu^_z$=uwABx603#`*qk=;?3!R_P^JTWHJfwP5W{c~5ZeU{Oze9;TKQJ^m+VnxZKBTv1oBZ5O^AJ-L>7b7A<8B~D;7rkq8B(ise;@H)05k+yd$9Z3nCfGs8{36&FXswc0-03+u6ttohweIPE4w6tD{fp~xSYf$nw$7J?Mb_umhgF(5z! zd}aYErK6*})e$-aUkWgDZhk%$H@9lwedMK%yw(LW4vxqF5tDJl35CHAlbGvk+pA$8 zExyOtl!i5aMpTvGtaoHbOckV`wG$?D$rC@T_duqg_cOXvkY||bFy{J|CaY{I^hr29 zF*YaBM0NEi9)WQD5t*+Jc#0y-@%1C??^C(4dC_$ccG#7b2Q6sKbZcb>KcKNd*T}6h z3-}tzdGjU8jy-tSBW&l@){R8C#%tAxV1dSJHopE3H(pwDFG!Y~7>fRNGu+D&{N0pB z6h7!SBQo&k_u0Cwn-cE)k|bNUj+|WlQR-ckfx!))*T$Y)8ULP5tJiaE2>yyx-c-D} zBJ{_s<(eDnu?`vdY6Hy*HEG-fcvl{`%HY59PKLV2s9N%Q^w935#a#aAkb(kG4uv;z zLVus1pVJ#a^4%94QSPV(MOd_nJmmj8Yq_=(rwtg)@YEFHaNH*z#RJeUKpP0-6P@X( zSd9}tt@-)I1uzc)>_TJ$G*vcUSpXpOC7wScb$UAsAShNUSbah{{QbKN9$W+nc+3Qq z60U7nIR||Z_waxyig1_`eg8?x%6?Bz7m~cWUXFeP*DQQ;5Dq=eUcgAY^A?aSs9^9# zPJATr^w5qfk|0=7@c%IVec%b8MvbGe^C zxnljFel+gE*0JQ>@F|^wBE&+?8iM^kvav&r`fV@o_#xE6Ms9OM0e~?aQMF`+69!2f z#~D}VunY=#FkpYDceF8fx&Fp8qo#%s_H!`pK8jYnK zzly0NtC~Sdp{!tuqikL#{*2f4Plm>yE9G<&TU@&!7ad<}+@&9n(iqWfv_A39?54}2 z$u(3pf)@lz@X*9WcCSc8eH0Y{e^>`9q6r-x9RZmM*rSt(iS8MWk^H*+g}C_(6b-G-WmEJtOZpMCn2P-Z4wNXFC zlj*fc1HI(7q`yvXaw1N1E5%7wD`i(t7xr5Wh}pT8t{ zNlUO*A1f`!Z%pe|GZNZ;)Q_g0`x}Wfahqbi-N)=7WhSAX)>>DlWn3huVMqP!)s_$v z!Zh7mGgj4a_w}&stYwQdsn98oztz>%OXZv&xH^)}T}$`$12N0gfT<(z(*^+)@g<4= zpT#+3X^aP4Z}=MqjQ1Xb{eW$hFJox!0)`}$&cUiNhBQpS!!Q0}^E_uzsU78M_Pe+4NV?jUfSxQS-Nh0*I6c(Qo9+6S z^Z}Mts<>Q(sFo2<7Cn1?Rh7XB2fj0g&z+PqlHKK*KUkDYMAC_4azf2WjJP+(g!>i4 zGdud$2Iz~?#ReT2*k#Vrc&4jSJ8P{;HkgRCaduy||w5A6^(5 z89hrI>rk<`FBe8^o44N2|%6%)eA4cgeZpgMzZdFvh~F{({5PNA`P zKs8JuU-C6Jsy%jW)4qNC=7Ttkwv4c(U3^?#Q2|L0!f+fZDJjd!%R#dZ-UzpbMQMs$ zcdErDoiPT7hcl}DSAZr|InC{QoR9OZ3o7*FuxO$yKXuUD6vUK2n;0Fm6%R6?+SGiQ zZH8?~S)LtwfrA!_T_{7f(qn!IZ?mtGc^{SVh_Ok9(biY@=u+Po=7r&pzRtr zG5DpC|ADHFP3bu~)q8KCbpg;oCTnnsYY_1Y03A_I#dxYWrKP3WIT3pH?!5CK05`sU zC(`=yWsl?MtQ&W4kvxIs?77Oo3&AtXn$)Y?1CDP9);WO@J8pmMb)axkdI8+k`Esc9CR%BVi#QY!u!h!MTZ z)ko{Xq8D^09(-GTn(%Lb`a_oToJlTOn!@;(lv06)%<69fweSX!c_*(xcR9m_{V(|+ znT+3^qZ%6!Sg}OceaBQ{>Y=pAi5_3dCeW+vZ^|{&*siBCkj}Ser1hM`Hx9eWTXpM-c0TKDnJo9%kSnFs=oPlJgTbkvTnP)#DbG=q%3duDD$Z31!qsVhd8>>dS8pz&lk$UhGPV10P6A0|F7p4z}Z4aPh82p}lnt7r&xiua0&^i(aEX9u9qNg9r2ae~94TJORE`zN@! zAyM#0j%SpDf~l32@d@A_h1zgzV9;pqdlt0^U)#PEE=cmRfA@s4hYfay5>ahHDw5LF zdT$juFeh6iCz20$D% z7g15(RFYfsjoO^4VX4C+7GHxHVQb;8(1piAaW@Uqk)OEbu2pKNv^>{i>3sVvbxwIA zmw4$`?R;(6!?rm*aYXxXF2E+1vzr-5>Z=zV@{AXJDI8waiv53cAKe9x3JjZzi;Gxe zNiNsWZ3hK0&(s4d2di#ItvV;Ei-nGFoKK)x#jjGS+ShFDze*+La}PS1tMoCnq1hL3 zBUNAY=vk_L!}0_j2R&>*F1xX_p~=F{b%L|{;)>I{^s-*%?d(8Qnj_2eK*^cVCL*FYz2;Go*>0pWVd}G2ze@cnW z+MXT3%Ip&oB4h5{qf$&4nm$6ihnah>aPmFT_>aatT34Ch50rm$bxOVN;LD#>l3(&h z_KAoQNb9vjGmS+r**}O+W09G!7E-*SS&Z(5t$Z*2FRFMXbi33R@t4xEQ^jh-5@~6j zQNB-J*VZTV4+yn0AJ6$cRbY`|!PhZ22x!=et{Om(jNneQ8#yIO=80?b{x03sR~8X# zN!du|znpGabB;uQk(?SCM$I2R(~c$)?i1rYY0xK4+gkIf9gPwB!eyNZ4Tt#1rh7zO zen&o*saH?yHO*vjoPrLvq6H{hD_vC0Vl^Y&rGHP@=@^Z3`UTw@Eg7DH?g zO4ubzdnv`cJD*IvQ9fzAa(>KT%04-v6Y?Q|-fJ|(IWyZhB0=GmkOLbfzrZ<-9!7%v zDyyd!PwWe{gAzRbfzMvH?zp=C-Q#$fMrn@tk%>mtHsEj z+H58?9T7*Y>cuRm@}ykhaK5Ol*!K&o(S>&ImI7+3^|BClwIbrYzCDOvl#p4OwAxD5Z0Zu->@}SNNe_Mv&O^uIt05J!efAjfXYjIDX}yXmR(XJZ=;)ZAKj51L593rXg%?h%=nn^S87v%+fBumO`2aw zQuPTX_ssR^o70{v?bSplK~L@(r2g=|LYWxn+#3|}xmDQ-{JV~^KL;OCabXKeTt4Lt zU>YZ0d$MM4<EYxHSxaCp&6^P_Rq4X(4J#5(Sv99D60g65-MlfQK)U{q8wk z3}lOB`v)#Wk~L>mtM#rg2@6&(X@**#Z!5o3YK;k?R&R+EO#7@8v2g9-cgI8SsdK0T zxxH+>{~ctXtlSIYG*NjCtebzR&3~#IHNmtctBH>Cn-CJEJ9-F>&M%4f%o(D+Oyf+i zqVVe>vfS7+m1w!8%^l-+YJ`*xmgkG?s<9;EIzPj`if{SQCORE`c6l4Y#V5)V@0O}; z=~DIRgkzQC(OxZ$Us`e$`ZvlDWN+jpZ}haIKLTT7X`b=#76WM|SS&({bt1UF=qdCH zveIL}_U|l3LW+2$2RhS`PG6!xmqyLI6qER0?JuyNp)8Es@7AB}1GFr|YWdk4(1s|` zej3e-0X5utqIRymbU*+t#F(vAtpFB_a!-XRVFX!UEDisn$VjYS(4Q*k!Wqu)~q{F{ABRW#Z{j1;fmoPBK-g z3M08__9>&rUYTfCpHY%<#(E0yHxj<^Fh0p(h`pFbuthiv%_-=tz0+=XgOj(3moZ&U zjBYJ@wkX^a?5TX&;?xvWjO8PmkV-}^BZTDjDabCKiqxCjplK!aKo&Xo;(qn$eCMmd z7nZ1U8tm9q_^3E9yoBrMz0x?^B2C1tNQ8#cV$J3A!i4>K7A?NDZ`0}z>BSmS;gWK7 z=IU>w%Z=jN=pmC+H{A8sk?J#a)_wVlyxCw;LZ+(f)NuvdrQ_Pp1r zCWqbColQ0`9Az3p;)q78Xqzg`n6yaqOY75D)5Y<(k=^fhD)X@aKc2oatjacPTM>~| zx5sp&*TfA|>5j0s<0}0wSG$YtPL4%^#10XOw;4`--*B zTEO_-fKw|BEXpi1UnaW36>pm&b#Bkx07QS)kh#0-1i~S(EN2LE51~(i9>mV{OLDNo zUU|dr@@O|@yM}g&&CvWXz)dZm>duZ>)AjfEnps)~U0s~8wyd^)$*p<4X9DTB zy`Uu<0KNh=7~g)mLO#7q!#oK1fXD>F9oQp~y#Rm)z?g#AcXoCbq{>({+ChDJFm4Aa zHU~)uGxuA7-Jm-M8S2uj22kNFe=N*ud(O}5WHYNQ54sO1l^BSXoSivMDg>SbVJXXl zIr%~p(55%NInyM4vDgZ(8&boS#9@T~O!zeTcfATs1$lsxF6z$z9OHHIa7b=eL2T^d zl$dr5w{E<-@X62jZ(Y1!03X?CQt`kwsLts|Fx;Ljm`GMQq(x_ z<{rL4R6uAX9~q5G_(wN2rQ2L*3`SF3BJ@l=t^O8Wu!!hoER(2R1&GJjf3Rg}Jo7%z z#w4Jp9aY-jl2yIjKdFf{`YvQdgnqgyx@pDiM_D%@dt(_NA93&QGF3*F8UNVpXemua zzLKY;SN!-|gY;+@n_`7g3j~%f&)!rep0V`|cEsCvWh6h=Y%EsMqIiKydSC0aBP}1_ zZL~Y2;c7Gta=!F<&dU#j5J8FB0B=`I&Tp?UN@uAfwN zO1vLd)9!AC%dGllCh#4e@Zu_WS`V9<7(86l##*BvFMaEt9dohs_#4QwQ6pK+;Ls}j z;%s+sX_nzf(QeDXYuwkI?($s!wcY@qulu7)c8Uw#JzvoCFFj?9Lj8x%`(hWw^s4S0+f0Q<(Cdvr~3naZj;cv>`Jow z$@#`VnY7tkYM{Cls9O|jlX>k(E73We&XN7lq^I+p&2O$WbKcycY+Bn(wP!^!3kw?P zkvu%#Lx;D2{dVD*c|y!k#oO{&!kIM|+Mxp_~*5*k#0Cdc;?KU@#-! zYIXZdu|LJ2pYRqH!6d%W(NpjA_0E9)U@j(&mcd;tHsde9h?hy$xHuUCes18VRs+S= zk$F7&VpckuA?`~~cHEo3LAKUNA(l7O2X_~~DC2jBpch!ir6oOCvrBtz5Y1cn?Xf`~ zqRLFdqL=>@+d8M?cezEwre;QwOb_v8#o>e=vFQPp`an-sl4q*@G}B|9BCc3WA#S~o zn7(b`UO#&%bfDF(V@C715;065rd<40+qcsyHa-1ubz#)hspzNiWm2KJRuOT6G)?}G zd4q{(-`OaC>`-!X%JQg{su`ix&se_`nYHC+r)Q!|JN?NVD8o2b&8t-ZCTPK`p@O?~ zso(Yj~ikIE2=aR#nM(wK;b&y33i%cQ%)t)pw!1_o*_Vq zk$0^_u(Q@yD&RrCJ_(aS^w_N5yXrtXRDyN7ve-%n9G7!hR)rW!8SzP##XlQF&7aNg zI<|b!CSJ8kc=WT?*E*KEca_b!)!cz%U6hAiA;z_|DKIMlkAb@KJ8z9!CXfF<*#K5@ z6&q(%950jLtD2ai=(yuoS=-=KJ&MZ`%eH*qjL^Q|NnU?X@Y^%AE;*mV z(P8~%O!jC*Wk|}e=B`g62?Xl3d}luK@iJ0U#QWQ1&nb$9Jpgnb}%AtX{^$-m1}#_xo>?*>?WuqB`un%Wrl?-7zup7ax10cAu4 z-lazZd@TF+HH>dqQ%qPx_{+y#FD~hZ_9hD+^b?%{@^Ay z^>rM0a?zRs_0kc#hP{c+z0T9@eT4*tl!+rR%)=bQN&%@W%H& zM89%pA5O@p%(~ftKnN=o)FxRc;-$I{bbnZ$5t3>=$lqG&(?KQcj`Ry?{=Sla?Za_R z%8|74c+PMIUf*L04y%oN0;^wZpQt5F&+aD3s{(liu8?mYj2!4XOi*{wE4?$PwoAGJ z>=a0d0VXQ~lnzc-ja^#+JIJL`T0O`;j!tX)Zta(}kb_jcPYCak5I1++tvh#_w+dj8 zIUs@nCVnw0#|_j_LFxT8U=btM1=G?YIrYvbHqdm10twB^8Ucz|q-g8n3SdFd5f@uh zXjLSU#!v@Lu*~1NbH{!9*5SOr`3t#loitpfxcox2wCFbZ_=2Kx%ca@ztmz@4cwrzk zzt}3Mwc>xwbXl3|`Z217xycgKHmDc#5?z3}_OS{KPEjj3rmb}o&oyq>&(&>Lihslq zm^F)Y2qc^QI~N+FVw#lxPKZ7QE*AtSmwx{eUuT4|6d2zDn|n^IRNRz{@Ng`Wx>mlU z_rgON2@8TE`-bdmwW=rhAa_L@${cs6!$*x)|AP*d5S#*PqWeLbx};8@*#7w+4~dC~ z|6TaE7i5?m6dR0V*4lY9sFLW>O$IjH0CY5>IC)2wqlqU+uZy!5{Elnt3dOpwhpNiz zYU^faDN8p#h8EYd(h_PtDw3LFjwj4}LfGayN?#d^E6uU`sYN2*CN*;l@AD4WF$7=Mld}$2SRjO7Ar7S7SE?Xg9vyPQfE+Bx?{RQn+7vjfTD(jNcr6( zB|h}mFh~THGSI4l2n3?<^TH+Ijz-GApaTghgej2tZ(vdUbPGzUr0fV{{^dbK%VgeOW+PY7`EL1;F_@5Ko6`J2H!v{ps%41(j#aarL*!Gt z_k2KZdKHkLa{OhJ*61~w}m zryqiM$W@Y((=Va zN;SItS5e0be^J9jn!TKkk^(rM+uo)v<6^!xsWK1t!pVD^gHCgw*BTqXV>1)+bN9*h z2V=C-@(lQhRb||2T+7dM0L##_uxyt4Ba#gd4M7?PCZZ<4vXS_A?;cJcQ~c3+HrO;(gkXI{I*>b;Q3Lz!0Gha@QMKh zaCyv_pMSVwfv6V}m|uW?6D-K5eR|DErX6W6f|N5FIB=3(gmq6 zOdkpo)`J2RIf)vy!Gvh0@`V>Y&K@ab<359Vn2MDwV4bw`i& zj{QlxCx0hxx$Jg55j)~8p6LCsZBAxp^f^w{EJ-z!8o(F$BDFR#9EsXgA#n{%wqU1# zn1CiL&x zeP>e2l+1+oKX`Pu;xAC%JXA5cPbcg5K;!Ph%*s>x)P`p_6=J9g%W|buVjImnF+pDv z+Pz{1B%bJg2Z6m|zCsLAHIVGP0P|OoPJX-!vd9Fy_4W0VEJ5HfLD~cyl}>ISaJw+2 z05ZB@zR9oU2u|zOo;V5#zYHh`a+r}OXb2T4)_J>IzfO7mH>Cv^8|`V8`E95ltyt))_+;fn@c}XOPB!qVKhVbJK&S+`VfYjREPp1vdlx=v9bu~u zyWn%|w%0ZO&TJM-R}2-EwdFgVE!T&bLel71bLfggF=W<{=bS+irp#x%+kD{! zYdybjo$mYrpFLrF2oA3@K6^1@6K^Gk|8?x>8N8NK!_Rl+G`?J)Z_m^@-nZzAG=p#4 zUV7vA@8@uv!ajitWqfk76U+ymfaPjeG2^1c(L|l)#yFOZ1;N|fB zC5qm1Y>WCuzsq*wwWzPGw^82h+-i1y`>L-Boq|!j+fX*>)J)Ne56=##9D79tsfLw3 zhJn2E3V_NIZqmoQWw@E3$1Bp~BV=9uayY3t@p*40=S5J`;mIeyt@!wnn{B(C8B?{8 z07wN;908YdOEuIZic;*|@$oCDoMBRH=+{N%-w17|1#FE4aWHH_b@2(RGI0OGTm(8! z&KMX;UEX|d4{sBGkPog!Ky?z4I5Jqi*BilPhFofJ0#N#2zSwO%P=$5)$E#`Uj3edA zQBgSrd4N@qU4gxWBBh^*k^Vn4XW^DHY8P6*tpY2tom{l7=-oUTVv7YuW<>3keiQ5R zP9bLptq(@2=vTD(XJb_KTj%E%Sp!p@PM=E_BwV}(YqNZiOB57Sq3u$zsGCHBhsy@=Z(aU zYuIZRQTD-e#Jood-rnvN43X)TeJ&Ud3*9svgZCsZ!6MyRY1U3E=Jn_X&`fLKrRmr{ z;(`AQ)Mw*`YBo!4B{&tG>jy{pbpz#3Bdg#g!F30F56EK>awfpgabTEObxWz~=mKGg z^NYg`jT^gPw>iLD9l5y21g{+mrsLiDCtRefpo~<|AOwNZ1gAGps>K0%pm{QMZbC)_ zz^=FNwFk)|>{2*r!OmCgDRb%PpNdX@Z1)060}-g!B|nG0trJe~vC8W%dYr5?S*a*r z#Q!fU4&5^GmMzd&A9n6X&1?zhF_7P5-f7YGeTtBjU*RG66hsn!O47E@`_6Jq_ad@f zu4S0bUxQXDF?N*dZI}k_zGP0r*IeAqpNH=vTmZL`2ZafF^X(!Uqnj$V+)(K3PGdHk z&zIp^aJ6mmvc@UWTPxBi(!!tdSQKW0TR4gu_$HTIWdei`?%n|u0{kE$JNHmgic?ga zoSahO!9Ada$b*YxV5MjC<=E(GYZx9K;K25_sa(kK3>5G`H%9Y~Jbx+c6pY2xmz4DX zC6`OjGuGf%CUt=pWMXOx9~~7b25oA30N)t|kt1zZVq%$KXqhZ>gWjU1rU+4ZlcV7` z58q)01cHEO@Xu0vJ&3$@0zZLppQxZFsTB%2475PAw{O1nqze3f2G$L1G9Fnz9`HLj z=)apSdaTQ4B6_|r`!|xinDiJ~{my4UZIO4w-OR8>tt!nTykgI+#QQ2|WpMe*G{E)*pONz~{ zHzca#hNybBspAxfC^Y}O(N|IAFAZ>(-bV#jfxj#mG{!|mibR~KS|5~eM#@wL#nNFv zteEZxyIGoqzc{ROzPunV)Jlk#fCQH|SjH2tYI5zN9%>K04-{&qa<=$`m~IQ(!>xbB z)7C+PF9J-%dd%*blqb8^yazvaH%`UvmLOrJqXqdx&XT0X-N9K?AE* zkrNfvTKAJx;Xi@jH^`H8^MFXsA}HK+I+%NUVnP-UCm8QxhDHX8So>an56yp2CzNbu*Mq&>tPFFsk;o*P&J%>CvZ-Or;Tw1p+hX2eF0QjYS zUm9W>N$=;hw}YB%i1zB`*EgdMn@#dZeHhFa2Bv6|kAzZgBlNTG&#rVF$ov-YcG?`C z_CA69axEhx3P$yt8pBPpv}CE+^fb1(v5#MerM|jfFcTlJO$EUqO!x>($tr+J3FUG@ z0H)^u1dgi|Noq+jTEa|aIwqz&P^ikt+`0kX2RzQteGgFIfR)nlXak0)Ye$K`Xp{^S zLKc3ktWSsnf=*)KK!;-yh?zWbNW>DnyZU;!xJrAtm22JI)xeMGJ(&jol3*c-)ZEe!$ML$ELR^414GcJ;@Jk`=?c9XEw5@EQtAZJmfJqg5TgLnO zBL@?MRD%GTYNOu+M6E#pA?~a=(T92FZ~@6L=e+ zh65xZr0IiQ;CKbi@=v$y3km?bkfBi4$G>M94~9hG)++9b0}(o$V9MC|Xu$wUg76B; zDV7! z*&E!M7*E()wog>Yv`Qs;z=Ek8{GwimVz zRMxF*iG8YP3)7HvTo6yiSLU9W3|QfvGj zVz{x+HlBQs(b+9j?9N8~$&w8X%-rNwka;9zYraXhKq1dl!0dPEM&fXqdVGp(F9RC# zn31Opw7Ar`8%e_s@Gpk+F6843MjRbDPZ9UXj^ittk}2kzefrCLcqfk?4;Gz)g0#e_gUb)3W4>RlXGL^t_*?BSLn=<;XiXm zwf0G~b>9p5x@!Clbr{C>1iTlEypl9p&W-3tyj2IgtY7*)V%RO4l5!rf{}=uFaiZUV zwZ(+q<7#0A>a~aQH*&J?I9h$Qt>`blmp^Q8IyYfn6OEF87Jj>=ZkXSkBUqq-IDHU} zR3fluee(nhPch?OVaX|?*z8q=QMjr-$h6koEKlLL1>|Q)zNtBPRY-Q5r@-z-r-=@ChKq02+5~ zJm_iRPug($st}?yv$ZN{RwCwh_{;PyCtH%Fn5VumqjHefP->zyg(}rh(#fwx5U$cAkW9F-S$@%9&Ypert{FCj!zxurg6}U)2`w^LackBc9>5bZ5b3PO2C~{hx)g3{fA^fEPd~MKSJ)JZZqg2xy4=;>0Um)Nk9!(z2P`pQjp!Y(HSBNEii>$P05pmE^%SVxS zx=$OEOrJq&>S9qvg z8_mJ)ExEQA!5v{7JUa6+deKQ_Uqt%yKJhH)i8T$!aw=pyReK$6Rnm`~TjYqKwtr+c z!^m}jGmjk_w!FN&%2*C=N;!?xY8M(iy74cx&XmycHbv4y&;a1%72K(KPl}XE&G61G zKUI-WLvx)2Sz3~6ikY_E%-K-NJqMM+Gl z|3DBOJxlGOP`hEysb-4ydP`h$-xsCiA4qEtX%yuj$Qo>rIK5e@n)Tb=rLnAKtrrq&&O-_FPtqo#x+N2lRshNYn>)j@&+hTEPqlzZ`R>c|v zH}99A?nJ-rnQdFv61l+1Q#pxIOxyFij!VQPJ>riKLLPUVil8Ld!7|b?N=!GPs)F)g8cS$Bu- zKS%=p($e_C59MEbe%+OQk6n#9tRu-gBQ^4S$RsG|hBCFq_fdBS2e63s-O81xwhs*6mG6`SuVB!Nss>j{NUz*)`8(a7^E5` zvZa0_-B8lKw{+5ZezZO5)*;+#cT+U$qP(f;s?nGUu`2!S8-~;z~z8J!Na;5GIzu#G8)@G-S6cri3cA|^Hc4W?qJ5i?} zSTn70V3Z_7=opye;hYwFp{3fpQCF5QvuTtsT2*GBE;oh+?Tbq71DBjn#&t0?Kc zfi?&ZV9(!G-5!ug?ClsMc1#KYA^bSCPQ)!PG_Kn>@we?%6Tx!op31{nCU6(iqs6@I z_m%Jusu=90W?6`cMMAaosx^5-qtm7~+Il2|g{pBz+8wcTP8O#RUdO)LE~~+L@K|0G zKXkf+Dpoo+kJ&6FJx*%p;0&-aOahJ4$fkUSh9-~GY+nC&F zG-wEm(!VLBwR9cSWkrO}MVj=HvSfnn)R3+W37FVTq04gFg8GY2^i_RZV%6$pwi^-C zlK#m=G1{~9Y-Su8*BQ*=(n#pwLeblKYn^NC6eU2iVn20WZT5t=mW9g@>_%yY>EAZo zNTQg@5gsxTLnpEH@0mD>NBU)|o0};GGX+%WBVm>q;Jm;b#EcOf~t>&%eJezpU0=xXlH8*1!5WU4NB)|LJwG$mz7Hv$?Yyx^Ri- zZkE-L%O3uqe2t>qR4R@ZM79&rdZ=LC{sr5=%YPP6w8GS{Bs{KjE8HALd6L;%f-8QU zur3G-yEK0RvJWI?^{ z6@Slg$wg2bPa#a|+^wc7g!zhNg#24uzinS4%ZIMAYoCAxk=}tEV)>dlfym)?Kok@l znG9r@-f_{gTT@cDK2sN4WCH*xD8WNIvUExUPgHhHaZXqmpz6y1JR}N1XCHi$>o zj{7i1CpIe}Z^q3JDMbXBQjalZ08ih@k-~nW%E&H|Ql2X#y*5&k z-t41X^AV9onM>E5m%=zW&%m$<6)VodAV&W6Z40Rbds zue<45Z-n{?V_9y~Lon0GfIgh~Y`oM}5YaJ=Aa9VoXcS%)`vUY$aL_6`{(=<9Uo;*# z`~G;%0pvFzBr&Rc1FN=H>Is%ozC8EKKEmjKYXTJ4GsBg} zt{B)cKT|F`zhsXJ!rq+97^C z0dVVA6~{WU1aCFrHtndwI?VEoYrDsQuk?@iXT01k`k0n??^BpG>OHZt-EThdX7S8_ z7kT-9K$Z7zpHOFW)5WIXUvi1BXK2XadZ#aDLa!xu3iJ7TMhR#_RI*i6AHVtQKS8i1 ztav72km}j*%DBt9PIW{he3GK2A(6#eL8|7eDV&w}i}@mJdHdu#Y|+yf^&Q$&%6~fE z4Bw#FYF*2n6SuT=HNEE?Db9ABKc2Nyo&HHSw4JL8RQD2YKRCEkAb^{}pHtSHvE9<+ z-DCoR;NA!+D728^Ud=CHOCleL5TgrVcYQUA9O}yhW5+Uz@-&&*dh_qp!6LR+7H$VMB%(Kb!=WVoeu7WpXtc5SAk zj6{~~@-3Wv7qO}eKq{xjC^nQ*v$L|0@j2mn$Qo2!ufx+I&zLE0MR}Dy{EnmMi!y}- zSeS0~vgq%G-5>Dt3QPgr?7Ey5T)3Z@^w5H=~g@ z=CWPJq`chOJDuP2s$=^1&0oS(0_(TZv;J1MGxg$yIVqXJyRRr-@p|`)mXls^?nZ>Av@o)J7i35UjJ#a#fHmUQm`r z5RaxNbygP8YyN(vDIaaw)0;qVWj$77iJ^D-H{}-3BV`T3$v6D{IC%oNK^kWe=mHkd z_5}h*Bn7LHFRua65kz~d7V?&6{vFedo|+sTIoPi2`9~+479)O+gb;yj{84gmGw`aO3=R5>X8{^W5-zb zv1rqh!z&6|;?2BF!~B&Xy^v8KdBZwm3KEAbmaDTkeDUo$7Kr7E7TMi6GO3|OyfU#k z`i-Ff(;0CpnjN#TXYnEvG;Ldr(0hThPYo_0uW06boPB)){QN++L0V2;4HLINL=Tnd zmT3+X$QtPF7~Q|}z-6Cx={s=0fB>+FA|`=qdf~UJc9%HI~#YNl&q?dA|I0xW~ zG+a1PQ#19|qVJ>ObOjw(o`6Kcy9EWfq@t+=aJ8?5N?4}Oi&F=~kd&KO-11o@v+CS( z8W1|Hy{JVP3}K!!FYMyy%E-m?IvUY$y9sADwQU4_24EeASsXwLsiE#o0wvwx8}w~# zHQkUh1R*WZhy3A1e$R^is>TmFXK0Ej1TXGdg(3=kVrKy5?=`vx`~2HZY8kU?rQ z(!vTKT)Fo)O5yjWF*(sFdfZgW6xNylY}pIFD~-_1mzmX7OF}ub-gt&M5mM2lZRanE zAL1APT)e)+ebR-}mAYoHy0+Ywo|>T9I(|>Q?}@Xc&3ZZJg0IDc^s7NJ&yaY<^xCi) znSRgKh8VlE>Rv_i4u*quv&DTB&OP_4XRY*K-E*{jjQ;X4Q2BH)c}slw)EF9XB+w0WZZev(BAEkoGF_24E~iy zuXsR%hCy>D5=X6yb7HbH7D7Es<;&{7ZUVQ!1MrGQg2fZsjrDcj8_NZNSAz>|a zzi@c|%Hq?S*PuC4?reKBeW_VzD7O5p`(`qP^yqz)EJYzRCVzUFngt@*qjSz9 zEK#5G>mCApPbRdsP+5N$uC1wY`T6Z;Ehc>jW2F6_7?cYO_x69>(L4o~S6oSwkxTzG z&gk>NfW;=DrAH-L@H=~7pZ%Jb_+Z)bnDS!BQF)ZnLHuc3ps816fpgP0f5Mj%mxlKm zJKY?v+YB4-x|H%ECI%1#Emw=D=WhonZ>|%HWH+YowTP!(Okak*J*3|#T~j;p4Qz@0 zl0F&pLs1tW7gO`R!+(FmaFRl3rZ@H_)A-ta$G>`SmIOylOpuoV)dIXy6=3)99OkN* zAooyaCR~PD12%B`f3vGds&0i~ugR9+&r!trzVfyNyO^Ss_mI*EsP8}I@*B5@jD8=H zREg2Zb(G0y0obRgymMtA>H@az%=B~zJJJaLnk)rfp|C>}RbRg7qZMV#bc4v*+OJc# zb0c|~+lmKHa%?N%>y&j~5_ks{RQ@%NjIKsij50m4yFQtp6B;N46_|}6Oms&!?RE2*&W!T2_dNEd!{ZDhh-1;)7bYD&UO|M@TbmtRA7?qua-NMSsg;bL>4bQ?9AYg5SjK zhTY;c=Hv5Mkz71^w1a9j7Z-2q2CwOpkBs^%~@%93*$*bF!xNvA9+;8#J)i$tJMBhiP(hvVKITOZEVbMJJ~Kl0*K;f>dGoiYt`%42doF_dUth8ei=7x% zyc)pP!dtjq3Ck-C?KztX+w94ayfSSN1vt%QWCE??S8N;8X^lhU^fXI5?)y!UnBy>w zdf{fUm}8iZ6O{E#rPTBHDNUIADh;VE;|xAtJ&>PshBSU~#m3TR$(nxyDhgjy+yq$z z;P=)?Onuk!D!z%}nSt~#XWwl@hz)<+N{xI;tO4xUPwDn=nEy_tYeUJe&Z%7*W`m8_|@t{VxyRR)_FN5E}!trXfqvj9aZK&UuWpciIx2% zW;MU2#jus{pc9urA5f&F+hxHsC+oehn6AdG+aU(Y7kS6|B*_J*ba5H?Wgn31nwoO{ z52F?u=IXyN6h=?`?8^(AnVQ&9Vf| zgFzyAaC64!pc;>yHLfEN8wV7VP{Hs#8}WLB)O*zjfHwPki+{^`FrX$N(iB2M^Dr*} zLhyC{Tc^FzH`@(BEd`n3#R@Tkz!4#7o_uKy!tqL`s>St#7YBm^)h{>4D(emxP}@qn zd+56RQpcOu56&k14$(rA)|E@m#N-_}#a`x})tW9QYc#acQO{P=SNqXM$!&YaLS=Dm z;^|pW9s=Al*>TpSXt(*Q-EhPz&h08svCVa+F$XaiA0bCTUfA@ytXxAv9<*V8uR3hF zrVix(K@>z@SV8B8;Aqy&mB$CX1%Cw4C0=|c%wd-%@aB;7dL*g8!3?koo81{@Qug!c zwKT>!#jkPF|E0x91P$67nww30eIb2Q6G|I!tc^H6dl;UV%;e$&zD2! zVw_Ef_(0#ojERsVPCpgm_gjP5t~=K$CSC|Y;Y->ysd3fkM)U6NkKGTV{Ty=KczJnQ zX#svYO2%gxc|_I&VV0Na7<6wz5}xmNw16kSLz+oaB2}@Y5Jo8Ph;E_!!`VxNld|=( zf0i!xL@4dMS~G^-jc&rt+AN^FBnil;Q&yYaCMPF9YoYbiZc&AyXattmv1@(YD$QLYzwj?(`Am64gKFDSVgg3X!Nuv zMSpo;Fs=<5S2x)xm3R3WokYLkm1C-a6g|lm5vwjTqPu;iBNaLGW?-O(A^~P|XAXr~ ztz2lTuz_#`fcXqnn+nb2(GXhF;Gc)LeZLseO6v}E5vj~wX+(=p`I)!=ZWhVLzVayN z)^Kxa4pQGN=|`LinU)V@JANT~)BlSW)CDH_=j$=2|EQr&s z7ZckL#JpbW%k{#y?C00Kl}3lJn~sn~n2akWFjfIl50O1q?|BgP^8d%>l5ovhj(TxR zUKVe=2*OkWi?ISFWMT4*>-+ZfG>A7su*sxB-w85#;AJ~jb=0-BJHEVF@qpNg3Ex@M z^m!)%p!pHSe|15424;sf5KTN1llMUDykPwcfq0-_+51Ii8FW1TVkhH}q4ncv)m2)K z@YHcl_vW-l$YD@HZbw+OXyg?#{4(+2q7Z>Ih@GeWT%udp|GSYS1d3D@GB}nLIbAl zhZCd-OkFec|ESR0ihpi&LtWq2)-iPZmac@U?qq(XiQxkRmfwQ|rv#H>rO~4iiQ#!5 zKhwS!WP6T~^(QMI9WZ;e!Hi;UYr?g2Uq z>`uGSbA!#k7lIytsG*#Ef%Vm(X8D{hMzjqka?G1_Zv?C7|GPyW&l^Sfb_f>)Of)Pk zAJsUf;Q)7~K@Q!9`Z-n9^V8DBU!nU{acf7)w4!f2NLEDl8VRz8Ns@0#YkT3hjX(^o zwi-~LI7M@n%I8GwW1AIHxl-Qk|9 zp{*?oLT?yH^!q3)DKtt~x$nQsF!%Eo&JYJS^Kc@>U+m42lkkQ6k3Gm`$CJ4(X`^V! zor7ebt?f^kB5xSP54|#j_aSEXi!sx5c`cnu&2^le*#QB#v@o{=WVYYzT8#|32L^79 zZMd7_{^K0T5aLzNiC0A9xut|IT;YbA2@?j$L0Bg{cE@@sLkWoF&Hw+rbj|7w^G;cQ z=XQK|g7TPQTfyH?@h6V z<8wmWM@xjKL4_U!IuKY?%;9f=d>u*P1SvxiLpPW}+okaj!?xkeYg^er#812d*CAlt zS_f!$ZZ!}fOaJ`RK-M2>E%-`JgaSK+bhDJL^^Fr2iatq;>{U^(NiH_yp3<6GN80&E zO(%i!t0jjIqayFx4UMX6L zCeX%S{u{`ElNKC15N7njvliyonI%bHy+BCTh!(1jg&w%AtZ?eAuVh2ddY$8uyIk^Cx z+P5Va7npSxo03u(vSr(nIJa|Ld5sz%DOn~>him$?$%GS7vo=t+gvdELmA`vwKuRI) zF_zDV3PaX%F`vj!@4*};;Y<(FEUtVl&WB&-cTFfCgCR(T+80(|ka0SmF0ERD+hBCW zd!yHEZEw=PSWji?s@F&_mTN5A(5Z1%9nW^G%!(7eZ+IJvCqG?Aq88amLK`JUY|f{8 zbP|roBFL@mLiUe^*v25g-k#wT9v%S5hL^Z7s z@z-gMOH&$Lb1(n6goJjOSzWXcxNMs5Uh|mEKez0?ED9I8;(J5fF{i*shCD7;x9*Al z6g3QtOaUb^=>1;Rh0;KYzW%7;^&bDA4PAEiz6D=}O+1Y6)P%%xE>cU76(WyckcETn z4rV^QeEAX%1sFfa2vVdYOY%pyxS*Dcye|+v-|)KHVVVP)xuHlhUZknmUKA$y zdXA1|=dYnDd1#=L&L0Ifw91p5xj?(EiI0|+mdLQxcK%?lM=zFT;jd{B28($g-$4Gf z&UrB!X;CoMo*F%wCV?EE_Hs2-@r524x!*A#{N*&-qIn$%RR5#DUYBmj(skUv7(L1L zRns5@Q62A%Bgz{SmQ}Q?m34K*q3w9DBAeNH4_C&r>5gF*wJ_`f;+=PL&@9PR)Zar3 zY??*9bUgRuYY^WTp7ovp%seg4j@0j?vyArx`GaC2zLlfOaCg5IhU8(*NBZ-;w?OJs zZS_@2#gA1+f%?pnbf2NDc~3C{BcJ`w8$VujKiJ|drd^kT z)(lP#Ao$_h0QtK{|KRlW^sOSHK;VCn`eMNOu`b+o65qq4*o#vyb#eYx`nGB6aFmDk z4MtWBfvl5~*zdgnX=jx|42>8Z2rxx)zS75s8W;Tl8=v&b{yFi70_JdotOe#r%8^Q! zB}dOH&)p6D+2ym?Q1Ty(&t6%ZPdp_(bJ!rKJ%f{4=(J`B41!*d-%?iSH>rVE~>XL5LBu^PcA0^ z^eZk1>Mj`@;B7o#Md7*loi^x@*G(FcG9!uZ$>ZgO;9^hdw>mWnI^F%Jh*Zb!ocoCD zmKk(@)GAablslr{iSc8IKM25*Mvu=>`dZ|BfZ_Kio0DKe`Q_XL?Up~dF-=dOw56+} zW@i8ClUZCxe2#H@HQ1B1PdsB&DM_2x>{>q&XNn3h;efgcC z!+*UL>2IY&n98j`;CuiG7-`G)YoUU33M_)~>s6SWAa;t+CKhSDfZ7b?p1lx+A2pUR zt`PJb<`&!)d?21j;sSezfEXZ4^_=EHzAv&oYoPftSq?hZkNa+L(QHCd{PANhcscY; zs3eI3%UiC!a$f7FP8E&Zx0-E~Qnhhbu;*snuh1RgCLpNkPAB^i7lf7Xdwwz?oj1@$ zd+p@t$VZVdDr8WlYSf^U#N8xV)N%p}gNV_L%@|(ZatoYW5%k{~G$kwX7Y4ZS4a;`f zuRLn6!uVygIIe0LH4np+J#hT0hfPe|ZX9*cPCb1fg8R6@L88Gty5nHZ)@OF4OBUoR z6A!(hV1cF$Vmo5M%>b(4Qm^;2v>5A9A`Kl^G_UVX$+c%`@+Y1O;nlSy1g85BulS$6 zd8TK6@%#J1qR-BD%gWAGA@vY9mER^mV@yN1!u4p@sgT`b@4-EZ){uGj(d_X{ykTDp zjO_aY@e>o+90uq2FCX|mc*2zJbium4@bSaPe)}@~O2$b;kEO{I!HMK={3q7|RwODv z5B#x5W(wb=Hjz`p=l$EB(6xf;xnnny341C*#*q&1!E(&R^uxRc8MjT z%1fM~``*ZT$AKuxH!4rz^Hm4$-PU$nqPjVxu({E107l;_#h@SXb z)jWQS^b+tG>Ng9BY~2J?;d9%hoSCKd^Q_n?wBtQ02|xUmXCB1|nz%Q$Q^>vV>%ZMG zwDv0Nm8@Cp%=3tXUf~hfr@O8v9uE5jUg`Y8%`YPc6CzIn)-S5cn&4S-+bOI&&42Mo zH@-h$`|;3f_xZ}kYuB(=`Gpy+yfG|~caea7->5*T1;$+G1;1QKlXUBhCh zd%D(cIRYr)fYcZ)YFvm;z@Ojbas(tBMZgnNk2{bH>n{ocj%Pr843r>E%OLOM)r_eq zT1vk>JviL#15Rl`&0L~YNqLC32M+mvOo#mb>XAxklWwenhH0~&{oEXk_?JnKYpt66 z7Xfqx&>sdV9f(bE4pH&uF{!F$co;c|NbJS?x!z;%&Z1D&=SnCy33%B-F17(pM zwRvPpaMl5tN2;d2_jm9+cy(1cxZ^F-t^dvRvG+mdR>n}sWgqV*?O~+rq=lWE8x=4h zW8>Lx#OjZjRoG&@v}V_Pu3B5)B@e;T{X_Ek^kf+0>~Y&sTwdj#m~8HQ<=ZfNhp4># zee5vJw{JAjd=|oT`3ak0hk48MAgzAmIP-kU(RZUxq&XEaQ?O?l3GZwq+Bv?5TpT}~ zbXi`7K6^uSr;z3R0RVBYuY8R@EQurLvD~6@H&^~pm;Wj4YJ`eZUZ0YUNT_gwsa39?Sw}k05^qau%Cw`I7I}B)9Mifg@mBM^#y3N8Ep@f( zPEtho1@=$REt#ph91enDdrQN z8`XLji1eM1!gBy*q|m30RuKDE!{jYzcvg3+G1*EdKWzOlv4hOc;dMD%DD@9{GSfUL z6*B+7PmGnq$#Py_e^nQwa@0ghb!W`AZp*DH@RFxQl`#>X4Gtej=npO&{&Z{6&S4YW zyh^ITgpGoctZFmvupuq!&Cx%Q?W241)U2~L$*0-um=}j3VCKk;{*^(F8af(036+0$ z*nOY9w#PF2h9m#Vfyf0RSD%|!cMgQd&@{FxpYDgF!ZE(*NfC#Mxv{DOZppq-Wye`( z$5b|VLG4uSsIk$_iN>)z^~Opy*dV%!98=9X_WwE(G3>~* zX{{*WH!4dxWm%EH7ceFTc!7UAW9CWh0sq8Oz3lYBcfG4h6~xrJDck8_88tjlL6(kg z`c{1M%$4t)E@bQU=Oozvi){wcEcilR>~;=8uGIc>1}fk3liA}^rmfX$(;n71mD*UHoo(m1-2b-o6t+{WQ$aAc8#J~ zD()kNKusUodZNFbeEZvG@OAPgWOU$kuQR zbn@!-UA+;CP-`8|T=DrX)8@>;qt_Odc5+Xl+;-;blc3r4_vIX#oA|)4&G^P~sm=z~ zPLq~O#w3pGNJ&p{vIW1e)+hAWuVvFv4m+;`0b=A`{!2mq^j~qQhU32UlC$CR&o=@* z>=zpoCQ&Z&j!!(=Rh#wUw^LF_1)8TQpNDtw@^6Oc*=ipT6|ny)Q4VD&HEtT=KAI5q z+$d2$O~Rk1^LzUYZF{Opk8ZcQIFDc7lsl;QP7Ah{C#oDUEG6;jGNmoGpwJzRqEt&8 zc013sjO4m98!TuEP;t=3-vqB#RzX(>49UGj3V!*kyR<3IVfC1=>v;0_Bk_rLU749u zt&{4DE(QTN5%;HeWu748X}Z1OWY~D&b|Npfzn#w(!&T@`7)8iX@5=WbWmjtgUYeu` z$;T(k%qY24gGhvl{niI{iXNxZlCny7B9S(CNWR4MD0wn5=@}CcgEg`s}qmR9ert?)s} z%lr};gMb`%p5=g!ti6W#-Py{-4jRqTj?FVCdrkgdKHCoRA->@vB~Q#%A9C+X2!vQ_ z|B^;rX@8ruC0Etbk_Cl57^wiz5yVvkeKQL<7*U}kfIdAnkQCP6ZU6&GFfd$ATLVZv z>Z{He06Qao^ngw@PD*}cSlcw zI}zP3B;O|vU$-oDy$47JyE|B%QV0sd)q4t_iau2^kefMCx6VkrxP`j-(mYGS| zWjzE6zr1c#G6Yzj^XLORQ|BXMQn&l^=F>C*f|utP|4IfryS4o|KMymoiX!l}bFFQcwW$Q>_BFXws!?Qex2rbegkrX9gcL57v2$-pu}Taj|8}J)077Ys;bIZf!|$t*c`*U007% zKHiuMwXZtrcWQ0fE-&#lWfIX(?WhfH9iDyBRY2Ii-GzNFED3DTe}Po!+qLt@=5h6X z8Pat!AsKr-!l%f(XxGqlf1q}JKGN2kug-XJzh&Nz+Ru2P;CT72xw<+E!W`$X({N0} zXev#sHXfAEI~(!trb~0rEbj}gMP3{iYnC>T{@uSk>NorPq@;zk_STi@4#iLZa8=?)>U~wJlH%> z5Qk9b$G04QdfaHxboZ`G)-RLYwqI>Z&rG^vI-GPW9kxJIxt8YZ(iH5yIM7?~S@lV4 z&b4kR{DM;~5-#%D^>6Kq!Tiok;!Uqlu#CPNs8hTWSQcC>x0YL~3B7$5ZZ1}mzS{6S z{}3tvG^U?twUDF3>6y{-l)Qabzp6cPE7Abtm6@wX`r`TyAn5 zgw?FC5j?T%oXzTSY2im2J9?`4@Z4UIibneU=BDk}Y5R+CZ}jM~RLre5hnL=aLzUsN zoQp(I^npVnNX9$)ZVV#Yt3cYCbLK6S9*DOGR8P<^eVsag_#n=T|FH`=a#ba|vp%7t zi>q2%{vv!+z_TKWpA_Hm3or2!;M5!jXp&Vve8@QlS(8W?OBlY97d0E`S=%U+p^lbP zlut`y^@ZW7m5klw9AeZOViXBtBnqB6Of8jp>E=}aL$U}QL=x1Hl4*;57KfCXaT@p& zWY+)WXs=(v;-HiSNkM>;A~ol*(K@4NU(M&8b9Ks=Mpd;WV31KHFu4%%uOS|jw{Jh) zw) zTRa{>2G}Fl)nfCdoZ=MDBAGTh7uH+@HY66Mkm5I5fpAb0;Inv<>fg!DXm z46)o5R?4+go)>aDsy|~r?oa!!E7#=#hK;QJ*!FMhHz|DhTGc`GUF97YiTbCVb$DNH zq{thQFMdFf2QFzR%KGF+#2z%Im@fkKn63HYY=rj3vfu{@TwS^SY+Y`)7Q6px0gmtQ z)D9W%g(Go44=c~MNcH^6cT2eWa6gJ6ofkTHy)DA4o+5LeQAhtYJkS|DkG$Wol6U!Q zSiiZ#)9Wr7-RnT!r*V(uhXxMYZsPJsM-0uKsJ+pe+_co}Wc`k)4qt;hUow(x)#Zz1 z!H^~Evh{aT2r{-GL&z;}@eAHNZelh`EZ^}b$ZUOiQVd>x8q1VjPGY!~Q-0}M@l4Qf z77k)5^NxWpSD-`G;x%XNm}hXi46ke0oQ*_yc(dXh)c$N{gQXiKzn?m$5{52^+(eyu zfIFfVoHky6pvAc~WZg6USp0wq=j_#8umK(Q$%7;rh2NXS$&)aTjoFiM(=RC^v|%T9 zdid6dP2>}MW1+XBlI^FF6OoFG@5wLSXn6Th6$W~A_D_aHSJ~*bsOIM zKDemxDFgPXEd#@$FXDSFW$TTG%ihR?Deatf57oEWP+Py#R6O+b;<1btk$BGZ=Ee9g zX!pOe2Cohk>0vJzH^v^cXw1ycJ#CZs>N_w+;uSz5H8hH5Uk$21o)5U*4zgT$F7PhP zbXPi)Y;Wm5^JJQ=j?EOdb>&MyEbv|zc04|770vF@ z1Fao}XDRDAHko5{9W&=Sn(WIC3)-Ju9{aB^TeTU&0=bVRS3Y{x$xqa^<`eE<{12?r zH19%t-ls$> zoRsF$@$hJJ-Cr7-YaCgg5IB&%%7*sOHiWUlgsW$X6cIh1ye%(V+igfLcdQ+MB2-@( z1Wi2Uj*a9W4Otv&+=OZiEL0LK<4NORVc9=ZGgR9WM#~AH9$mMmRv2jE9Bun@cFtvTo)vC^V(;KBu7p2cZLjoJWGL# zd`XqYee5%P+xiioc1s?jy4aGsYF$QTk^}RCeQjRi#`BRC zq_4?o^}M-~t55y~4rW+=ERxfSdN|^1^FD$9$Z5vvYKYqPk!nThxpPQvr718X#Dtll z`A21@t)YTCe0q(2mFd`buctIezVjJe;p5$Ze|Z$g4Nv$aJ`%o!v=vrE-^%nRlHb9zsSw zO8P_v~$80D5j5xa=>$3$DS}XR?XRaS_#Fob9yKO!@ zAKcA)X0A^$wXO5$e~m9`#XB))iZNFWsPhj1ofQB~Kk+{C+;0V3Kmag~T(;{ot5OBw zOn`s)nu$8s0A4G@-fuJG*lpZvQyfT<;h0(^h6P{A z=kK82%DYN&l%N0Y+UDeT z&~2xosh+ww^Ma`@ESHB>6_&U~7HZiA5N?to;=f6L{UAHSkj-7nbKVs5$M`|7@lOG} zDRq#?*ZoVCdvfE^Hs;;BYJXRFYqP3M z|86PxIgz=ienoitxu?4uVx({4@PdNY7AC|<4lnMh z95Tt#gU-i*_n9gG+8bEgEg+g@T6}Czp*b=`BFh`FpA;@CJyzMgG%Mb`5P9I3MTMVP z%Md6iFh!7R`)Q&o_f)YgWeFJ1B9ifN)_v*yl5@O`OKvs=RdQ=YRBJ16Hvdf1`c5x^ zAY6q&0?EhY237aT7Zo|>4teFy1X|erL0FfQl~8gW6Ne<2o-=!bg%$^^dj7<|wC2q@ z;#``=RsUs8_3>3LlA`(Qk1x%9Z`fKq9LlApUFO@2jmD9fEQF5($7bP;eP7sq(%5g7 z={Hn9#ze-m?P2g1X}y;r^4c}!q@k+*B7W&T2;{Y&hhJ*lAdXy|KEzWVPHNJGEw#zlJfkBTGYW=|f zKs}=jFzlIDLEP1{CE38!E7BD|3ITgt2s6WuKW&r)VGFmC!e$KkOFhnuIRh}~1*tB! zRBQPuu1(86ZIxE!Ou_IDV(*xldw=pdLh1JGKEppK#tPt;6OW}wIzLQO$^~H1=f!+6 zS|5Ky>anuhI0f~=8UxzNg<~>A~|i3>}=TW-kk5Km7mlQz!_m__n9nRufC3F&`24k?K~?z*9vl6udi@z1uy z{Gc{>?B8B}R;~dpavFBfPhJSZ*$x4q9ekO_nHrHtpIUp_J`8IHmZm98usd=3OpCl>HCZ1VN~~yFvu&>Z4b#P(7QhkuzSEwu zsdBq?R#jz*EO~n&i>=2ejPF55*H@3Tw&fC8_=2^!zmlENn;%W&N5OiC0=$*9A^Rm; zr=}~uGsQf|gzRX4``(C>&C0dH;+Tx9!9_9(zT2n;kn`zRBbpcBJ2?5$pzc{PJa442 zc0F>fjCrClXe1e5UR8ZTb=CGNTD+vkZJiG)&h*7?Fl$Mc;7k}5!rR_1uUNkz#JPC~ zJxVG>TyzV!+LsxOK_y-79g^w){5xpd;gIPGP9{_@JQ#^tVw(>)>Q9Wcd}i%;R%+U8 zeMi3MraHM&ObAs`1tc9MC+6mwjrIe{$|?-RB-K(jx11b^>ICt|dt66c=?olL_XihZ z1l8rjXp2iCA4EHE51QK2C!r5*?y%*y{DzX8FJULXFr%_0U*(&ED*<6sQqxt8&WpGH zolI-jxcFPTak9LnYw{A=$8r12DzYzwuW9Dw`Uk)Cu%p z0|0L*Kmq#sOpg&a@&nHRxpY5GU`M;UL=cfmay2{zP&NC{+iVkhn7j}2G8jP8A^8Xp zN^wRRg~&!7YMc|)bD(E$z?}wioBiT?oSO7F~f!k$Q@!i*^7FnMkn zKF8{iI8`{z;Dr6`<1m>&i@z=|E{0y%iHAR2Jxm!`Eg6iuWc#?OWBu4M=tLb5olAwl ztmncn=ICMapeM5U^1KB`!xLbN&{U5kj|gu@?%ApR4FXOtvzu+BG> zj+@Y+Z8D_w@@(^^XRz%BnqkJavp}6?b2JUF>$?ZZwmv_)>V%oD`;Iej*Oj1M<5s!W z*)Ql%B$*U1SEjd*g;=S-#rp~2ELx^vA6MaIjBgL6`j6@$(MF!3S01PQM~d}3Z7*;) zeW~nwmet_tJA8jI%x?6UuXgXFNNHUxc-OB_Fzxaj)z!bNQJ!)={ZA1LHw?HHG*z-Em$4pl$vx4WKlh!BP zRi6i@)_eQ|7oit@PB$@_zQKv|jawXsC5X<=<Ej!-C4Q8r$d{-C-V`RTdlqEr@7(R*>2r4yg1u2> zzq-dU$)3nsc8jh*q1tz6zT8sy@SVudc`5M%0y^%wgH4yOkgB1c_4-mIzptJEQcyPt z&jYq2Ns!L7vAGFiVzcnIknor##l#>@w545KYV}1;bqpo?Nf+>GO#nH_f7wpv>sNO0 zHSns=dPhjHsw1%!RrYO)0jL;IA~saoXnhTGA)zd@ESZolOfmm1=J8F4aW2XvyFU0V zj8;$N@|x!3__3$XKC;jotRLjL-$jAtOP*jwFwWWZZ|C2G(t^kAgUud<4+kQj$kNnN zEEZ$)gp)qL;ZpmtK>Qn)Xx18`PV=av@xX!tvzJesr?f?M@ND$RLNbCgeClc;e!sK~ zqABFiS$Q1eMgY=3BP@m@03F40COw68Q(D~!wXBLw=X;TGm~aK0^0Etw8M7NkliJSa z6xZwJ*_!6 zsUBHmv7TPEufBk!gKDJEiHELhEgCNm9-rmJ>0M5oM-Ot;!E}Hjhqe`m{D){59Rab~{wfG!hsg1>ID1OlA zviPR${!Ocr9Pt-jH;yd)T2eH||j1n{T#f%a>Qw4g*ZE0NAt9@6{w zWXW8X;m)&iI}rh|QPSR$yNNAvuRNQ-w%8^Tfc7rU`4UBTSUU zd749!CMP;c>0tv_(*=J zOZlg0lCwm%D=3NavxwLQVT(TR+Vso9eB&BW8iqR_akH0M|>im^8E{-sbZ}KE#g8q6fn1<5E+@frSQyNdUq!aM%!w z7GwOo@=(>*{#CyU;ABAbBbq>sA1%FQ>jUhS`sC(%e!8MUrhLU#6mUFaDHPIooxw;? zMi>2mkeilK4xBaiIaNEV9HfCK(NOTrr`K_rbHu|%YdbaQN^4Y|@$mIOuXgyl|c zQ~hGK#9|e5A3d^?{UW+=E$~E`PRsmN`Uv;Ef+RSD#UEWb8yVciw*z-Z=hLU7+JN@@=bNgxBu5BP7L_2<`T}zcFR18UJqm7M-v}g8|6uI5)jO8RQCb zUE8`AWo2c*Kxsj&@%qv3JN|r@ocn3l{(K^@dc8{0@B7cCdAETOZCQ~0oNr%3w6A#p zmQJ?v)@c_WvHtM6>5O5g%I&rbdWn2?18WaWsa{;VcEx!0pYebIS#HrS1FtLgdJEl2 zdsPo&{bj`aZmH^*zBP|6ie?Q*g6A)N?ighcQ}<3I-nBcvjYt5Y`27Stg&_7L;-4^J z9r7imr$+<3k!(UxULLiSgQ}`3B#?xp3%K4Bd<=qtLKlU?uy~8VyLs54bF5tbE*RfE zfqK0l%?^~P;y_gY4iQ_PHU|I#r3@vffJT_rL$~9P1qM{uh^+L!!RGF+I_hrvrzGUH zw?dcoN_!}`LxD+1WPH+v@67u|znMBcRaf?_1y^{+J&ugR3%$gv)K!cZ(@!YuTl#0f zbGv!g8*sEju8$*y!kMRDUGL z&%QSM-f-%=O&|-lT4R%rdA>=6hjCrSL>~`KS-&{zqb>C>XB$)t~-lfJ$1N1D@gS~Znc9&b(QU6DvGZ&p5AzHxKK}C=TX~PKf7xA z(zAhf8s+VWYCfhi9;Iil>(`Xwi^(UqAxur46#}jYgeVw>7-3IX&d|tp0=*(i-C{IQ zQh?AWAkw1YYMQW+R8zwOib#-w6BiekHKjOoKM>lD`@s{d9LFt%8%nkX=AVEfZC+s^ zuscd5nKlD0xlf1ZYjzHho6;O3gri7G42=WIDL`dud>B@sgHkB=hoPi>s@5r$Z-TK& zV&@7>=i#yPuF)xdW0@QupFJ!_5BG&tJkog;afXGIA{X~Ih30nZUh1Wk4i1e%KzmXy z+Jy3=oO{e85GT>u9OHMRKg}Nt7`59XjFRkY|L75^jlhZdRq^13$G-^qc1&Aj%hImo zI7)EA3asbC3FJYvh`XgOEl3CmL7_2@C>w}v&a+L)^uMzK}U`e zoH!vL%o!b5-8h2s-OHNrpeBvvjY3C12YeXgC#bNPm|pWt?}9x-+T*Hhs<~ztTA-lC z`4C!UPWwK^4|r!FEWA(YsM+M^t!s4RzkLfL^SOl-Eyk1(#<-lf;Rg9~P|~>=w26qI z3k|WhVCY8u|N#?wwLGNSh^RLynQ*kG%d!wo3u`g5oDb} z44YNu>>DokUJ^1^TVsz+zqdW?h?($)U5IiC?aH1f%V0=Z@u{BO3UT$RsFn4VE8u50 zOMq3jWwdF@W2E4pUYe%lt5t~!jg0vUja1l1!X-^gxg{-mk{xsSGN9us6ZqI>$Vmt? z)`1UzTDKcUiQkECwt`d<=4hz_3y5*WK7Rbh`T+)f#3h}aIDsJRRmcoF$6trS9&C2U zIm*++0hY4MfR$WWm=raX%I~s&U_cBsLpH=;0VhB(4W{Pge0&5T3iq{01wpI;js>1G zh{C^nGWU6TdF0&OxRsTaKow1fjhAtaiO{{)2E^-UeIm~o+-Zsw8M05Ddrm;ZV+{P| zKdka@Z{0-H#p$K7Y%Ttp#A&?LMBxiLTU5UJ%p%tXYAe6~g3r;$Lw*mL54 z+K#00okEvqB&ZPv#&sMZyoQ4a#MATYwht`y6+j~nVJi({3}k>G z?KNo03+O84h5mK?L_tQmB~q%^2@LrGIY9V=h=?dFFE0)(P}}tuIj%*#NtKeFmX?MDUUBf|gR~sc@BP`~sFQdam4@u8tl)KpcoxfZ z_s&vhgb~aMdVT=wXD1MH{^G}N%sgE6KzTOU5*S3Fl%y_i0kw$~UpEpnO7*WLB{(W3 z;%RLeiUb&K&mm`XNh9RXa;;JxC;`)bL{#pU9aLe{qEp%Hm~!!w%5!~C*%Iht>0v;F z4q%I@V4wWFi~|Y!Oo?y653{@4=Bx5Ye92rQaK-(I5s=bhAh4$qRH5TTKZ-x1g90b# z_wP3#T^V24i+9Cs69o`ca{;&tgc1M}_ba_tmO6JL3_ypa5c6(2G*2Y{fQtxR$v_hy zmyrK5H3C+t+d|Y#oBM2N}@q1`2s+ED9R|CvP3{2>Kab z-3kLp;k$oWU`YgK+Mf=+1iW6itg~G26(KR-<^~U2RyZW`914CN267ZxkwAi}!y5Gc zI~|pSx)q_qe7k+6cj;d`Vw7XkK2;oOmPW@6=M}&QkdMO&iFl}c!)O%afS3Phlu$00 z>W{&jOI-v?S<(M|vOXDrSyfz0y{KdjGz54y>VX>_ke;8Q_>P06=)dCmBFh ze8q!l;=hmr@=yZO%j;TIdb=Z`^QQ}CMT1oO(R0OA$#P zB4cKyik|~Mw~>S=dW#@BF*jmiV6R`ELH+kbht)5^<$%@%5@Eg}Gl8%pQ7T;Tm)h9UMdhe~3ki8+4gL*(;j>VjQyL zM4fOcfOxdKTQWEf{Lfz_?wxj3+Uwm-5FkqQ+eiV_2EayMP2v!@Z&yH)&-dq31ct2d z5R`AXE-o&>n)ehCn0PK27-<(N+zLg%DR(I^-6|`k-N>Xm3lzZryfk*OV8$STOo*QYeHpU$j$a(swPl7^kKf4 z!WI=>yUL~`O2fT0i3H4O$sgFDfHORTChh0znC$+`n;PXJG=EMRT}J*?Jri`Oz)wXD zAu_A1Gq5mE{`AHa?G1tx{Qn-8HEcNdB0CEUzaFEGMf*0Crsc9n+97s8Nrr#`V(P(H zmZ>TN61X%|63AEJ0MVluu8|#N6@(!{57uU2UVc*vQ=R|Ir9tu(p6up0{>- z&1_KqRy4T4_wq(gNrUMwT#Na-L<){ETQ7~u#TT| z9?(lj8oly#4c!h%-vJwr>(6y`^nb)Ho7ODuv-7*7dFq;R%pMEJ5kB$BsGwr5^b>Ur zCJ%VsykKCI4C6wl9Y^_OGgU{eNv#|H*gbqhjeX(3Djg{Z2-s4}a$nOHRK zOg1@t0Qv*1hzQwGG7H)+(Hw?5S}z>w90Rr;M0E_yPBNKlvo>GMY!sKdg?7 zJwA{y-#<3i2Yf%!viaa`08h9UBQC&(cN$f$fsjE_;5|~+(?bPo9&iG6t$%@`*9jn4 zFH!AEp#H;XyjW}NbJ0IDFw^02lIh|Ed(ujPw435&=^$zUCF?siHNHRe?}9xRSWJrtMv@y zYiKTTlou5j2M(Bgi23X|!u6$T8DcHjlmWYfO@1x~{c;r;A|v>Nj;Y?}CULz%AiYdh z4}@6jy)zVx)n@WRS3L*U%TC40r9{VjaB%R$K85k0#UpwKh6{$FPZBvl!%b9l8-;5Q zzrc`SXa@4g9K9X;U4?-0`{w5{OpTB8>~qGUgQn8;vn%&P6`Ey2v(T0E){pR~ulu6i z2$<;q`Rrak!0Ec&% z$l;pzU}gPl0~nzqX(!=ap|}%)EQ;@S?ahapjEJ1h`yYeC6EBVM`d zwpBFJU9$)biZivVBXV7Fr-OKLV->8yd=gkLTRZ~#cWJOnfuGT!QcFckYqxMxtJ(qs zm{r+*0d8`Tu&O&#uP9wjO$|D-Ct!H^J$8( z(Ui?m-9A|cSSm;^9P|2%$#m?m%SVJbyIKfRaiC;-t=j6K-P7bqBP$*fndn7fVpgPbTg~%pIir0(0jZX2E=^#gc|9lDky+O93Q>b%FbT6Tyqg+; z=3o7$g_%AeBi93jSyH`yJ5a7KX$lJgY9F7?i3_td?!hid>-s9k!AODbMQf)AD{6EI z5gtLkhXiWaL&)0CAht!_b|M0ze3ponu46mQj_2cMg%D)Qyw=Bf7$Mrm*lcuet2Y8Q zEzm(cxZD~b9ZS+Wy;C(924J1MyZ7IOlu`T7d6xDV`BTfiLCnJ`juIX%`&4le@U#+- zP8}zzDON{m0jdab3~KT(39q#r_`|`z9RdQvE{xpXGLDKG3_-MXPtoA;u%w)v9E7Um z;J$lYbTxm#b6PVK?O-Z66qot8t6`5D8?EwR^ePxXKi+|DIu7$7bT(t<4y2h(Mv3IX z2eDGU%LPw1h%=@DX9XZch>#Dt43Ad)mXT3L-|9Dx?Che{HBS{EvcT*1G_jD`fsJXk zSu<|~HfQ;5KJq`(WQ)v1#>GOr`zbWd_1ablGny_>Q~L+|Y!~Q^GaAaUo^9NO(crCS zC5&dpNB-d%s4n|wUnI-Z)c7FgK`Co8=?bU|;71S*~K)^P7U53^X*bU}piGh z{U``Z1UHC+$nLem8X4JY^sn^2Kt>UVSoXRY@o6h=;2*ylztU#JW&Bi&{EE{6#~r9J zZZJA`0Xl6kN5qu;f&T@FtH9=<_DupVkRnT{rHsYOj;JH6FFhgy&80NVGkn?#Wfyv0 z!6bdIVPc@yHys^amj01DL9S_7h`8`s>w!?A(6|CzsAC2gYWuHbZ5=sW>?32! zDpwrFgpgv&TDNE=E8>D-<_%M9ABcwL{A@P8PrhdQ9r=5OEgDdPK~%3e_6&XUFY_Z0 z=oEY6X3TIz$yl14nij(N&$S9e=ntezXx| z@SsZXs6%*`T@&)YV^eNkH3{#%*U-v*A#cckGdY*wBLk4gxTTz}M&j?~42 z#HEYkdp-W-z*4EZUkpcHOa3teJWkk`%4BK5V1xoZQ;a+|nXV^Q!{-?MShaLNZ zj60{U=!_omPCQ}Z!_Ge2ts-q=juGMx6R+Y|^f;#pa0>ik3r~bbp^xwyLOArBpc!Zr zfExuA=^vOBkm1Dz`~MKLx^~BbE>bV~U-HZ7lgJJUxXryb;nJddS=LRk!@a^O!C?xx zJVk+f4qPk%kw(d~&9F3;;Elk_$i^AIwNfUuC+zmio6S6=VhU8UO-F!~d671+qsmgp zyd<d7M3SHrBjR6wSg~7YI6fxhE-z{T^p<7%f~-^<^E!w`&JJ5F=HX`1zPh z(0h^dJ-(r|_&ITjaPQlhdUyr%pW$;W+>9%;8inNrFf1c25SED2`1k3M2j6`=_q}cL zRx8id-3~+ayV!Y2;Us*K`EI^44mwhdOG5im&0XCL$nJwmP@pY{4G{|Jb10xnZEnVj z9PiuhO4Gwd{@52S#|#T5&1v5}qdyk%Sb}mJB&)Rt#e9TAwyER+N$j-MtNCA_oAnxn zg@RhvGiWIrN2sKUz#rE^dY>xfq`ljpzMNiWrk*j1m<Ifuq4Jqk0~jjLpXOe{;vXixX&%Drb`a(Aq~D`M z#gt;5()mtZW7#XYdru+d#KcjDE4ZBB=Y)Qoe76|iYgx39%#H8lD&flUuPKm0yWU{A zY4)_U+_xjWTU`RK}GJC|9fP#vbjHLBA=YS%z zkEn{J&->F@cg{^)`?d0Oh}^WomyxN^iYp82_&Gfp2dA}sx6^6$+q^ZLC9Vx$H}_*_ zw{4NWRB1xh>yy;_jk_0$g1(hX7>{>AfmG_%C_o@d`b~lj1bod}h$$v1iwq}vxU;#= zvH#=AnT%8JPJn3l4x{b!)3}lbCs}X@zx4u#U8ns-c zQh;yTds=X5)$x#+f^w>}`wFG{VI!$Jt z96+nDHVO)&^>?&;Tr?5mWg-7*_Y42SayZi{(eej;6uXlo`Y-1>eLvLu+LnWu;NjPP zx3vb#*p2pVB`fs}FOv2u_@??(m`q(Yos9~qIUMg5b{HI@UCi^{?$nL=mE&6D#dN8t z>38=KvVPfxt>4RMQ7yLOgl=6ZKRsDI;^lFMxW82FomazMZ(+Zx6M}+D4|SV6c1QHp z{=#)QpXkt2>f5t9i`>hiFQ7UxERwrEM8|s3k@@Aqys6?k*wdkFN}q~Dq7$M`KpMLB z`Qhqz*;SM3WQ&01xd` zdmyHLM3IUoATxcy+*vsf3cL*B~FBgcg zwY9Z<+NutuZWcGs+SvyKTF<;4+Py{Zo*u~B`YH>I`w5eS0YS@nc#bX3R9iI;9XcWg zOL9hx34uFWCF+-fonKJ<2Wz$RMbhO$uoZ3zvJ1PQf!<6VJ(E!+&AIIS^<|3?oIyp6 zO0fVwMhT8}C{5y3W9MEtO2Ktn2LGahhefFby{Dc6pNz1_VDa?CU$K6tuSb|TbPchI z&SpE&pL1Wk)CX5aU>$x9PhsX1^flk61t z{Mboq$N3Z)Fprx8hb;d4>dhmCFUvx06`FGzS?ARl(V4G%Y{!hqV0lDRqYiNh1~mMY zs?PzJ5%OsC3+$>7Q{Yx+%?^#_u;@W}{EK&N~;ico)Lx5fFY7 zYt>lc0OAAAsi!M-pxZtnxtkaHJ%#qrcbQV5!oP^18K2xUU{&*OzAubD*pL1LShB%3 zjWZx35OTKJXZ(132C6k^2;jwr5=~?8H>RisDa;a&18zypG# z_mt)4(2376vj72`lt2Nmihs6{rK+|_c(Utsu{UTBzXV@V?1UHLioNOg01?AY+_Odz zh;v>7@N}REemxd}EEsUQ1|x4oAnWf&;(vL(Xn6XY`*ni-2O@>^j5CNi{_WWL>Bit4 ze)06aOw!p$Q789}#vTBDWY%jER8Pls?@Kb@kx}+DW`}cOB*~{7kEqGr$|-zyb^L`A}+!D4tK z!Oq1eeeP#J6Nz|?I>r?$d++HUuWr-~2&TeLl0^Ijo>imMgc*7+upS`Vykde=t13>O zk8UlL-uXJK(U_2Ad#A*2RGl5>iyZoz^6Y_I=UbLGl9|D?k#o%9ORKx5eofEz! zk{wrt4*bmI<2C<(mfFa&+}u+J9tuDQ_+5YH1Pe%Jm=pK!X|Zk~f7Fvr#8D&ucS@pp z{HgjrNE4pqMRMd8m_1$-G(mG#Ts*)iXu*b?<~e%6MBK=TvQD`qJNugFZj=hijeNd+ z*Kg8eik1_AC4lS@5UA0LrwK*%2L42a<0*Exl3l-T%d`_-Rwm$Nw8bA}7-V~HKI8qz z7t~sKCB<^t2^tTe3G)q~2eeB;m5E>(Wuq_d-(w^Ons5$>b8K6-Z6juXzbEl8ad+{_ z6vr4`S(2E~Rxxxm=>>`%EtpSa@XhikU-Et93hTZ<*h>@b6F>4%3T)Mu^Y2? zJ+e28_0EslyeBg^Dic!ynXL={C!Wz=dEvyEMfrsKjIFTACV3r#Um)IFI-aeMF?#soMGDAt=zy8TRZpeXa)cVFWF*c;(mn}ZSk<@t$}QP~4xY$a|m z8K-ygY)T;3@URZeNH+(Rzh`mGJ}^>TsjEx2uY2;SmxBJ9u)_Dx%f$s zOfIAdfTp|q`WQ^Z8iQ$q`hj(TR5UNqrF!z~MU=?O;sW3?`b8PADHkrcYrTuK!=v=R*@6-@+j2%J2)Zqxd`tgadi-(3lfQS=n^&0?FV6`n z(*$>a(QHT59+~-Gcg1aL>2c1d=xwbVg6YbFgxxO+>{*Wcd%toLs)p+)zkAYIgrnEY^lN4VPf(`|I6 ziAsi!Jc~%xd!!<&0Yc5}g>1!r(}min0bil^%Myw420GqR*ZPkxm9KtRgaK0&szs&O z|Hsx>2W7c-{nFhb2+|;kNJw`}w*n#^l8OOHcXy+7NlGb5cXuiXNK1Drc^2>ceRIwq zXV0EJ?me?de4hKhuC;!dS!hjmIQNf&_r+efpvQ}l;c@nV%$@?@anD|$E!8C$B4}_L z9Lu>kbR!R+xs|V>iDA zJb+CAU>-|S=G=@g1^N7AObnsA7sQDGV24cd=;gQPbV(S5sFhtkSr_eL+)|O~*y#fl zerRcJZ%|%Ny`06d7W~e41sfVTLZ!S?OMc0@SY-lhBRRVPQ#ux+AdB2G2Cr{9VTo5-vI4Y3BR)Svh3sWofZe z>(>9tMEe>Fc#N5c?>eIVahsLDCraU$^1*mHv0b@#`S1LY=DoX4yt~bALPO#oNQMMC zkS$Mf;~6Vs6-LQLTzSx{YxY$v=*Q$DJLC@)Xne+(1L7q!Dd~kMG_0hwh}U%Ov4`#= z#wWRl*wFh3pBlN>lgSI(vMiYncX|ZWAMEhoXaoxcU0*9_2(5Z?}H8RsDDnThcf$b)`hm7-sag00FNLG z1bG^98pq<>ukE!&Ih+Tu>FLRK75ypg(8?-T3|v|=J>HsBdrO8qjY0c()tEIJ6h%;L zs9ea9_hII$Dld=s$w*|G)8DlAB0q0!3-v$Oia2qBi#nD!SYw!zn1HW?c?0T;4c-!( z{kUT;B0?rS_u)=!aG?4G{dTSEjHYms``eYigkcH-mY{2okF zo(-i_iuyHr6*m8DyD>1ZCI7mju)EnPscy88Z6Nc&2pBNK9EC#+0&y&MjJ&QKdzfd{ ze(}Lrbi9{Us+C(xGlpL_8FqYpoSB=Uj*Yo439D*kQd4m4d+Ls*YAp4d3XF_94UuxI zB0ji-g&zeD*p6%WRcFnFeXC#BA~oMk%1xF3d;jIe`0md=#K1H&$-TpbzZdrZ)hI>zL#BdfK&0ta7&mOK5^`smi*KlPytN8e47ZR9J>o_>n2zebq)Q~bOT9-$2*E2}^@8(FIcm66joOitvC?-RwTxglLJm_c^YpGLtQT}fnzhY(1 z_MrQOtEbM1B6ppoBPEEtW0T-GEK;h$1!YKaH%Ig(qR(sZf&!L7H3x^Ya}M2|tjDS7 z&xIN}e5U;#s2~xdqK06bRp=U5&p^RO0)IA&j9~81c}S3ecce{iZ_hp#93hDibRXJXkoIixyVd=|BdSpJob zP#OzRpqbQ9l#Z^dPR12NF&SK$V&C9*7zllT-lg90q5!p3eDz%H1VzoK-D*aK(2F}t zy8H1Yt_h+d?d&8~7<=xbh@XFCevZtEkmL1>*G8$h8?k8!2}Ldur3#+Es4gUo0J&49r_gCEU0U1KZ6Dl| zo2Oedo_k1nV0TLQ#q}?{$mQzVn`bVB7Tfr7E-dUWf2AAVwtoul(hx98_Df0si}OG& z(PVH?L1IU>;WR&Je@Sj;zu6Brm@+=F;=N&uoZ7D#S{X^CG@SFn>av%_*N^w2oNc%G zqivIo$$3bA_D(LRcki|zo1kS{h$jxGde=J*xAbvjbmuxFJfe0#XrrIKS?Zp0%)~?< zZX)?0a)B=*V9l8%`r6b_HiwY%HD&0=-g&f;jmUiiLHy?WR}^Dv3}n*WeRFkGA4KZ$ zjIQ>W?}Z5A=D!%~CT;d0kD3*l;~;JHHKW(9lsM~x5LQmB5!&W?cQK=pJ;=W`pk~v1 zB>$*HZ^1{jlv$U}I!rX_YUXwI1tEhC7vY4jIy(1$@FuF2M~!8F-116mMpM~g-`DPw zW1lD5+IIjazA~lT=mebY_QHC;tcUq|iK;XCkHWqcU-0p8ddAz8BnD5(k`06gb32}6 zl=TP)kuY`Syuhz=^rJc@Ti%t$RT(Z@^BPx7XzaDu45Z~k@mDm868ojzKksc(S$@Dm z@nrRRHU|l5(DrN!e$tPREwGwyuH6t`$uJp2PQ;|8fmje1HZWmM!LU=qvXyIN9ttk^ zzi+-6HtH3Q1Mv+T+rhyB^pWoUd_Jsd;gD}UDw+uNsz8DyB_k+5()=Jp(-a8Rx_*9s z{v6Op0b^sJiW1vDr%|@V*>DJVSiCTl{n;d$ zfE#-A{>)j|w5?{Q;I=7^Id0kvt@yz7PpZ1))L^(aOG?d1nroq*Dcv_BE8h_NH|lme zgSon?X?FI|N}~iqUT zvzmWuEF&i5-xV#d4Wd%+#|C{N}yhY=X0<58$o%f*E?oUH!nq}IVUEPaMP^U{rQfOFO7^@|;0H!-Ca z8_5y_(Jq(r3nNBkU36h;JC4I^JW^3!a$ZEYh!-W=OxpKGPH&@D` zBZ0tK!%aI1N*%F3Xy7u@rC30nBSgboOmB6#(o1#t{qaQgR0BDijlE`x_Cpx%Nn?G) zf>^YUze7FV8;EZ)9!?hp1~)W0;CkpZq=vBI0h#j}N{U(4ziflf^*YVKJQh}3QQ8A< zjHw!lFVl)8;@mF>S|Cuo9gnjVtweB(<+E(=R;WX!MO77sg&2LweZtN7Q|SSdH6Q(l zuB9rXSWtSuh4V+(jqcRo%v&Ahgy(Nj`E>{-=U&EVfw>HoEe*MA?`ZwYK%msO|UlM!6sDs-uM3LEYzAQ zhnmGlCim6TQjHutJ7WO6a-eYD8((Fh%m-(SAg#W8)t)<%YF@cS;J54-oU zUx`;U^|IDQ>l8m`;)65OKXYaVzaPR0``TCCV$)`$3~?vO?i4Xm`v{yX>i=1&``*U& z<`VGx6VS5~{$%mIxjIW|b}9Ub2|E^`{C&u zfoTQtWc&&4hOmY%A?h8f_JW$4sVJQ6wP<#=C@6Po*>FIM=I9t4Hh8z-r03Jr2uNxynmar(-NO7NKUB)++nO2~qn-by~b^Y48 zA^7W@lGS+`l0l(y0h`tg((9AHV1DrWoSXaihwM!=8CK7A_KgutH>kmxcecIG?49fR zFheeq7Jr?25fxGSXyH7XI@|m8@pQx~wPL}Mq~VNAxyXfzU)c*IQbHJRhy@aqD`RpS zZ#^W@P&)WJL{j0dyA{ZT81X9`^p3P`ov@;S=oB<6eJ%|PDV0dF#jn7@#s3hAXfj0Z zdY?Ku1duG#2I8tPKqdcc8;gyLOE_eca>R#=#4iZvNv(dQ!RLBTWgAD_<ipWkF3sTubrd?9A0pVA zfU60i?4-~KISIlv4hNrcF}O$D7uDqnebwc0MA68u@-m`dvQ=b#Pnr@OUDkWDndhf8 z&dLfCBTFXqH8|8itngSmG1}QP&P;gR@P1?2<_vF+HzO`Cr(m&ey5^Pr09o>S&emq% z-?y%Wk=eWENGd~mY_<&-?J}GEc%zMPUAlA4Mq!v*3ZckgfI>o%?Kx%2{+<)TT<$;9<4la z!KH3=63InDHeEh9HSS~4=UhhJeu|4Vf#I@~Q?q(WC?CpCFPZGUmEo9K_c;fRl{V8i zkLvH_ds)r_SH%8nVEn*o8o+T z{g?5GCb7F;Vw{PKp2%1Tm-|$W?jL9PR#UA7dnm}{SIQA{LnsOwvS&F2j*H8hu)h5Y zpPilcK2^AzojCURH1W|3wCb6iq{VR5;ugeA6h-D8f~D||86dudVDke$=b|btIk|}3 zqB1r_`{_Zr)pxHfa&0~Kc*gsL8{%cJd1aMZWWtg!2tDvLBoOztv^IQ_=|gv_we?fq z;20;akB>Kl?AY7mht|Xcy8&)qD?$$gE;7+Ha~SUX`Z1B-seJ9lqbDWMi)H*HE9bVx zKXucKlz+!ngz#zefd$*uTcSI2PIT;7ze~z%47(q`i2P1!6<^`eu6o?-ahIm_VMn*U zsQ-XpKj+ORv-(&v-JPb*sJCvX&vYBs?yj_2%CbL7usBU|Qu6M`%Tb`BcwyHt^Luw)^^z2^ra}%D#P#5uFIzF%lS^ z_}mDxD1qmMHr@v5h!6X}#=aI675zP|u8lqLWwlN!*pER@&u9g7X{eROYjnvvPi(&c zJyEX9rYP*n?LT@$G_p22j}CC55~&)A7%C0fg7JwYPMqHFmcY-r?XYri0nfnieH?Sn zfLQSoST&f*kf#|f%)167wOdAR9aEc_Xw#0Rxw$ByhrLy80j>$MF_Uk<-gMoyM6hCV z4?aLeATxBP^XB^?d?-wJWCfKkD7flhp1AL~yys_J15!?k?4W(u!Z5|v3;EK|B(83Q zWYXrAD^DtS1iERW#eW0v5d@n^ZyC`q_~K%0H+u z_|I=bhqm#H#d7xw8~$4E1hDl(nsSJ|!IK<~Xh|e2S3bg(;b~i z9WmMKoUz!2hrfuqi1E)|Y-u{@i3O3W4YYNBA~QZ0z3DR3qS)UEJrQ7JBu7dMUS;Jk za>wLkuC9GXD}=e4%Q8_@OiPc73Fa>3bSZxkgyOsNB1rxP4P z5)%=T(LmmEYZ%SVm7#m6%d5`0%>MBOR#_^XW0PCYW~V2{evdB{npur3SLw2Og1oZ% zqr{(gf(_jbp~X|MJ;dw8qt^sd6^i?26no(X^Gct600uc!D0mkcQ|!wVq>3#NmZTyi zaZg*(?~3^60(U6|O%2%y#bssPQR3GaU}1zR7=T%B?Q@`uUmHq8xFi+$@-AR#u02wx zWH?uR`|Ts50LO0&gDRZ9eqyDhE6+=)^{%8P>LX&_GyM6Q^Amqgq&g@#^FnFUq>SNQ zY<%vZVp)^H;`#bu9r+h(q^c_lJo>6tkrBkrdMut#zdvnERZW{NXT;n*Y-0+-sCWwY zJ|_a_s>W>xA<{`bA1z|hWDJ|ao_1SGm73keUIAQ$?4X6|c~$9wy2*XX&voqGoNQE< zPh8&Yt7Chk2{d0)G>vNQr*J!{>WqJwI*A~wqWK*>9)U)VgGL^nZ#I{lBd~i-D2}P{ zA7O2a0!CGJ&@r;$g8jfq7e*99gEy^kbb!VkUwx~^XypwCh`4SzZ%S1<#5?6Dz-r)XhOI>0;qHIw&kQ~3Cl|~GM3)ew$bZ@F_)y;f^Y75jk?wuW~%Eg>+?3A z-3@srYP)rwgOe7Dgfuip6fu11V_l?=#9+cAUqQAY(NbTkqzJd z?y>*RmF`bh_5L&K2a0O$O>~xtRJN!{`#fw^-0UA<<*ph}iyPm&wLB#KT7mUo;8B<}Xrz;$G{o zW}}ES=3M)d3@TkGkH<|ews)O)HVb=#kH3vC%rv6Tcxs<*goYwP_nsc|UYD-%)iWHt z_-lM}J|YIW?;c~HC4<982vAXuY0tyi%+r4P$Kv7bAj-@!ur_HpG#lH*{C@7ydQ=qs#OwOS84_>G~gwP)EH>r%ni;^Pnyj4t8&A41e~# zuu3Idb?iQeI^T2K^b1ut{b$ash4Oyel!gsOLsD+?-_(5#s?l3qNn%veWW{pv z^mv4&+VfB>z7dGO(Q(e444+k!3Q$K$t6Q&-QNMzxzt!j4+mnWV&if3=cl*{v^o1=c zVow{xEK;J-$}qTkK7K%Fw|O2Az`fi3Rjt|E?Du1lo9ovsnC;ob#uOw0HrvT1z>69^b7uEwcyKft+oQV1C&ru#4Ui0(aBk(jqN(-NQTk~pBR^WW-_s26k#89jx z(@_<*oTte_^J^aoe*CzCd1Bp3{e2n68eJS;-!F87LyzsL3YZu|H~(v6c@}gI}Rwmzr&<|x!J5?prBuQp%RGQ zMV8PIh)p7fQMlG}?A0hI`Sg<4;dL1y!HCo_qp!R8a$L3c;+jTnPX)@k$zAr3J{x{s zZ9)A%m&+qJU2*GiU5Yb&8vmGyIR7x$cRe!QE{KwT>fo@2@`TB(i-x8x*KBY3gVz;O zM6gs_JXytJ4!ZI0oBMYRUUjeZzl33cFS|ioCiKp_-Emv_%whTF!P}|ozi9f2?`eK! z@S)p#T=KAo;(LDg_HHRe7llIxmnPYcwut)B*p`O^p7KNvBRs#>%mV)47bN(VzH+gQ z;$P*}+zAXDw{_)jbj?2H3PpjF2wubqCdT^_j0&EG{t5<+<-B*@WMu;$t185T@u#3h z;V#at?&uaXhvG)C;30U!+|dQYW6|>Fe3@5^J%Bz>$Lfj)rOVl5!At}t5x4qjTm+S7 z)HPH>=Dq|dl=+PJxEqW^ScMGzk8fQ?fZ3b0p}-6RCofDwC~jxFCoixcH_N>G12_&R z`DL&aXsrNdzpe4UbBC4P6*Tj^j~vn*x@dm>&HffQ>h*0TbSsCid;5jQIscsFlT(p2 zUX?9ZMidJ*bi(w-6qRj5#(@E?Ph)%92~T=w)W}&Ge(7+$-rseL-0qKD^@z- zr^^a?8O{}@g5Qo)y&uIR2+}aEkeHbMs#h{gI}Arw2Xr7_4e4;R{c`GY{?**u3KA@0$0y7v0p>F|xUESc>S$$B(we2{8 z&nWBbSIxqitrNgIRC-foJ&`XBy4SzMm>32zv@Wj}s1F4K&b=FPpcJ6T0aq{q! zX-gu(*o}d4OyGYJUBG0(-0kc*-u&$8(-ts2czxr@VC)<0UOqtas4x7CH+Gbmy2h6P zU^}o8yZ`jwubsGp!iO|A8c*a^g>H0WB3dTz)~Cs>PCx{MQd93u{P>a8)HGLy?{>D` z8Qm&9GBM@f?id6cl&-`qEiVxZKmX+3&f_$n2#>7+t=tix+~?7bS(5U!notrg0bdxD zxW_j^|DSByvQzzNz8X!yz$f7k?67i;PE4SD`-KzKj`&Z;oeB13c+{bOsFMkLeHj30 z0`O()~+lz{;o_(KhS}dJ8WnoyxQd93lJ`*pgSUooI$N;t}I2R*5xGAW`nwD02 zTZk%&ROS2S8*Ctr{QF;(Y<%L?)YNo-w!W^06=9)rT=l3}NiEIhj0O=ANfKp4qQCWK z>032*r+ry+IQGyD?rZ0Fl(AkH>|&mm?&Q81?6mOL8>|*%&IL$IXW`(ufzVMA_M##$ z@q!i{dLbcs^92YUeQYuNWZ(fMuOW7X6}QpgJ6o=P*O}@`yNbEFM5C}rc^02Ok!D#3 zV&~-sGev6?KXCQa!K9+(aUk&de*;J9=hlwyrq!O!e#_|pC$?5Ep8-wp3K`H0rI-Qu zG!>jk0&f;1lyqj0BYrThU{2WdGuiAcE-oT0kh1ECm|aT;L-a%8M`ks%VA5-=wFyN^ zyCwW!ft{6C`!3f}>+?`)N}b;bEFbVk;8LV-=?N)=j1n*tI0&V!2!of|u2dg-M+ixZfJ}g2)%^DBs|%(b+D1eCG}R#HbZ4Y&mTZV7faa%m1%9f?^b+BJ^MGD1zM*~aAjI}G z4~6&IKmO32Up}vw}nhg5?c*z8zPUjv9<|ePtMm5~T_1!^=9BD4h4`j%?J63j_!C()wslPfyQ& zj~_24vIwdbSk6IdOczev$_l9;%Μ)7G5i>Uw&iu&T>5;v?kdltS;jnJL4dMHa!_lIY@GW9eL>)7(_4eD&9+m zaN1yvcyto?T{1oWF}IfRRAi!WA2{d=h5pOv79#dm z|K?Rzl**U5S2G#zE9p$hG`_U7aJqWuH{(hYyxG=mlwOUb5>|BS{fAB@t3gkS0Tq4- z6LJ6fR#j9`)%R|E>_o=Wg8x?wuz)Hlp{$Ivw6r9moG7Cq1F?lDk`fSUxKfb(`p8J` zy3HuEnUN@1E_**ZCdLQK0|91{CP0beBh+lM;wefdhM87VhEoXlm6&Ad_*nq=H7^Kn zAi{mc;`Ni1#}KfFPZUnT1GEzyIKWmDpdbP$FjfUqR4+Z zJe84PVW&Qb&m^n6XOcY5^Mn_@$0PaJY>)S|V2I|`%5Gsv5KCy0JZ;s#e@9|=Up6$f z7WEl@;_3HMv0H__nTAdk(!nh8QQ&QMcYU1IlRAC&4UhZjTxmmaftcrhkT*VbhwskY zHN$<|?Rk4GdIZK8+(oyauybn$@sKg0P}5q4AMG*mE6S1@so-~5*WE96M%;cc@kq(vu0QcdPrXDH>g5&miGBk_? z0xV>LVEAK1OoX_aJoB_U^R?KbW7I1S>~Fu=1ifEo&{C9D#qYceQuaG_;GJmY#neD7`vvTj?D`Nzayq=(u<~w6lX(V1fF?jI) z5itL9@K|4)+G3=cJJ*_tG{#D?;U$mn%Uvti%Wt}lW)Ue7H?pW?beTx(MPTx>Y?a|W zWL|j%Z`pAvT+!5fxA*?Wc)m1=EQkd0vsqj(MHd^}t=f{lynnn3_GQD3nM3!H5pO;v zlxL)3K`W|+dkr4{I zt_S3}u48p&HtCJGg|WEFcH^H1F-PAVk~`R!$xSnRWz=d1`K1`P4LszKjAEf`V;J`9 zbrPQYcR*hG`HQ6V{$kY2=w?b|ubjg2e@8vI)U_Qpc_S9cxOl0(UC#+nHl){>Lg{hP zkP&VJDuVE75_!Dfe9V1IA4(6e6db#oi#Y|B!|Av1oMBl~r3egAnUhnWB}%skb(kBl7MlZU?na#cBvvnXH0j|I(lUO`J?C;_Kl=K*~We|{+{9DfA8P! z8-W^)PPj)ZQ-Za&K|TXg$KY%Xc+~#f9$fGwup87t(r;ef29k>v{4w8(4KdtZ zaerAp+A!sr-5giR$l>)9XK%&OdZ*mwn{WB=*GZ+L8o%ZJg}^8B<0ZWnQHfyf*Z9qe zdw4c3<`3{&!>2y4iau#p!kD_wT+@1wxg-*+A>Hr8WWVDi3?_oytrhzoKnFoY!8HG_ z!#x~VQqAw)Q=q3@qv!k`kSOrvd_OPdW;x~z5=qwOqE|Ryb8g@4;S9`suEu|PNi)~D zOlT)&k9UWI99vqx|EGpuV4@`MX+wNuCE2OzI=2zShnXbj%h-&AZQjYnKSrAmQj(4C zEE;jFF8g{ld0X7gSWGBfGdZKwdb^ArU@1?f<}YIu6fon}f-fFDtZFXm#S~uG^tXFT z%nEN!b4IW^-3GT=kLM};Bc_*$ zK#-YC`hgR>^}*=AwjBy*Hlf7sXxrVive=ig$=^rJsYeJ@B7fpAm_4anMMC_f#^{@> z$3kely|>E9BM%-pg-7uiZ@*Jr2L|b2MOF1CuBX6DM@-?E&YpLy|*ee^GBAncjNyK(k-Q3#;u*( z?xdirnFV~vU{)Si&BVsSVr&b@Wk|7uvj<*rAc42%8Y$tjy7dz4>XLiA`UZYvACS9 z>TFh;&pE|Po2ZXyDlb?on4)^^VRbG+dPs*ci_(<|Jt?+1rmg+x?O;WE&nk<3;>B z?@buw9;UVU_HX2#tnOpk55lX8HWSRv*M6w~II6l8}=a zl!t1N#H5tyjBx=eGO!NZrhg@|8$e>w?VWb_?uMjp#VeB#b|j{t+IN&k0K)V>4fj65 z?XamYzfw#It!NXWBez2AFy?f&A{cc9B@qrND>UFYg&F7*KfJ>rmG0>1KtQN-+`1nl z3d1&t#3k}o({3rHF!nkOp)@>LT45OhRtdDJahER%Xr8c5BtJfsSLF32dDt=<;||-U zN2%I5PsXLTo>%2_@~adkP0ZE~Tk+yT0*vR3)t)XGfe0JLGzyf-?K)Ahcaro)UG^S) zK5ja+QnW(yT9%x9d#p=FMy531Ti;4!Ib~i-5TOIvP+&z=Ld5P#X7t>gTBE>LK`wF3 z8dOr-Sz=x;S?gZtnS6M#jQU|Y0)1fQpQo~4AfF$_b`*x2atRG*xqAtk44>qIk-K)nM7BfAn^_-0?+h3aLBzM0d zCZA7G7bGNi{bgJ^{K8k~=ao;~YkpOlhpkP1%nPN00a{fEvq1+xns+<-W+?wQZw)1R z2-c?2FqCKsWV`;_pGsf+JM8u$#|!;a2P_i%A>*?MPj~U{F3&J8)Q?VApFWiM<-cFLq)X{!w!im@J#@_ zr2n3N>=YDSurb514^kWB-*t{4Qy_}zz(RKrUcX709uR9s<|57E)n4Copj_!nf#Kx0 zlCV6S$$x~?RDt^MJ!hJ}4T7m#r@v)!Hyji_P$$lGAlC)*)*(34ADg`f!Iw5OJ6q~d z$(%3cpcS3QxI2=nX1@&^aY8tM?we)Q%7B{97Z=1#DGwJF?TW@n%AG5z_c$I}_@_SfX z=e%UW$@qwPnRBrbLnOAAr26>yq_3czCZox(%2CH;sN|%{XLFR=V^=R+R!hj(#g)SW z3pap$I=q+WGPJIAEz6B%|7`W2v-2|NfY+`KIca62l#p7>R;}>aml*IhW@*q8_U#-OW0dvivzc}^A z_%}drX4LA8a|y{|MTp-+G{AFJ=d} z#MM(5e7cJ`)2wJ-r)Q-n_6S{DD>h%S@Xm@ouKFJS7udhxY=ghy?C}N%EaXX5e)s7b zmaDVFEkS>kicusv`hxplWR?2wB!8{$N4*gI*Uk9Fc(zZY1K*=&k)OA)+KCeLGFcu; z%{qGuS2i1qTz~tAfA5jF_x#xvzTBBeRMw7E438-WW4pqc$(NhS1o;FhvSyd3T*0f5G4#RilOYGWHZPl7={!T^5L(8Lq z{`XMHGg}D;i(VBWUsp? zv+C6fwfzFW5g()dV75`LIQ-eq>C&$5UU|W-9FB>o?l(VA5k#n#*5ya}b74_3rs!+Y zLl11TbFt7;n)s!#kTgp;e&4ja+Hd%{r&xoA>okvCHr6m zwW#ZxGJu$`oc|=~P$X1~>q)dX`l-gQzkeK`ScwOVQ+mA%d30pKs5~2f2;UO5weO2@0=&KGk<+2087P8nA z!$5?X3sVCP4L$-m7$63j1d4*QCMRhgD?^0v`?aCYZ^L3?YGVITVABEM2(pcj-^5}$ z$ENuVn89`2(<2{c`6E0bndhBj>eG}lwFaVhdV2a>bc%c4`0|yw&fEQk*83cgu1tbZgAMrZ_(Xo-b)`W>X7c>O z>Gr?BS(6^l7o%At$W~--qK5zMDQtA+f`XdGiarHf3s`EbiFmhBrNq?!y*vAoD)fPE z-C}ywe1{5`< zllJ`0Bv0Lk*{|b);&xPRwoGQfd=S$9c4L<2?ZJ3|g|P!O0$p(Uj$eo8)ku^(ksEE1 z0S3Ll($>63FDk5m5Q*?^P_v1@Y{Q*ovSy4%!AMQqpHovl3un*x_;BomVJLHh zXQtX-&cJ|5$Z321Z6i4RA<7J_#Xy>L5Ax4LUqa+9o?O;=dYhi#ba|+B3q8a65xVeX zU^Eb)2CwD)JrAnV9@#reaQ^D8>u+QVe+(20QC@C_nXO*W_X zq{KLO6@||~DOckgA+txx{gTupyVK`@KpnA}vOdXvJ<2@b;Mvd+T&N4l|1zw-5ubX^ z4{tGi6d(j(cMQ%B2H%M(SgsH#qzaHvI%Xl#sKU4v32-AwrBmKa0;-oD7zJjvbU?m> z05Jzl6I2g?PUi`ryDV5w0ZstP3^{dmdW!8sl(h%P`F64BA&0UA1 zkY2rH`f#|u7h;~7XJiu{8%tj}mYI zj}_eWfNopj{wy1fRRx7bAy=kkHAv7XC@MPqb-(Hr5F5vvA8IXKRz(!2k&g>GHnf%y zk}X;D9Hs|WAs>CqgthBu5XP{KR=!$>7~I$7JXQpdKw@fYTK@XC&~3Q~ta9+Fz>g*c z4+cDqz#-qJcy@GlD%>~A@Vc&@u>AAq5BP9>AX*8uIY9mfVbW|3=sg0F0~w}Fb(nV= zLqnWr-OY2aCnz>NUCzr(fQ(MAA5J+c>ExzHIwhI`HTj?BE*Oanl@bO;w(fjgS znpQ_o?ZnSjwCDZD$;^7<$3o0_D?WiImXN!vuRqInGQkt&i~WCD0r`c0C=Xazyjenl zpW%n3uluU01*{xQCeeRYTtF8o+C!{|EU98(UGDoOc7ZlYyx4-yGF!6m{)TMdJkl?= zl);yfd+m~IvXz#WwxglOrfW!u; zffn{QLg+SvU%kvn27^&(3Gg~UT8#Da{3uhJ!XI_dQXs=agD?$k6(W`3PFfhQj%jm&JX|TFEL2Qi-N!HmhMRduYFC8fVJUEOH1T} z8)1v;8Ky@itMnT@k2z~bf@yDQs@vxc<#WVOAaY}pl7iZU@Qd&Jc?ZVHo_~;$L4kcv z)O{iPD`3AM{y;zg8LDBfcj~7|4wJNejn;ZOKTA)B)PocRXJPX@D2Om&1jnu9J%#EB>iV8n6@|MYtWQWPkx72~9rE0;bXL|0@ayhuB}VL*=!baWc> zQ_Vm6J6LLBH2g4OURSC;sf*4>b3X6$=LcfN$JBosZLN?-$ktusP9!rY_^)j=`R>>p zYCN`o>i1Lrb=s@UwN5~J8F6(oo4lZy4`g_ZULU;A<{wBfoD4m&l7Y+oKc8+Z2X`3c zn%v6C)I?9>iy{OBpEFtq;Zg>!uUo;BmIeFEtPp`DD>t_m(NpVe40W24(80JQ?T}QH ztjVp2duv@lpo43u0Fbh4a#mm=$k<*AUoA5)enKFGrwM4S&?_?iNWTPl*?xrTKu(T(-QV3 z|Bk~83>SL-bQ@*Gu+Y#;*0?8}RTS|(VuJhGm}AX;&ze_9<}l3JtqRcZ+E35gLcZ0Q z8!ez1fGmP(8iAH-Xk>WAY*trOP5Mas{hbn^piv&Z0eCgbWh60G_QDThrO_df6}lk` zi;m%t2E_%4`P3Uj$WRdBB9ns|b%d((Jgy69Hq}r>`=CZ3JwYXA=pQC-Q5Ya0IK+e} z7J{HOIcl_)(UgjG;dgxO04s%yp!X>{5ImYZ9AoUR_ktnR?l<0lS$X_Nm>MLXgN?z7 zeY^97hQeE)zVb13Mecmuj?YbCFYh9Y%Y$g(z+Tz&-7xBA_@vHFw0puP<#v;`60L`H zbC9pXS1S*%*S>4v1|nwDpic*w4zvFEt}NepD=ugaNkq#FS2bR|Cn^;T`}sdSXeAaP z!y6791aaQNyEVvw;&oS%NQP-IWaxL7eaf+W&V2(J1b3V z|FuHeT1j(LTl^H~{~_usgQ|?S_MyAGI~0)aZV&-60O{@$knZkOxse1+G>W42EwOzN2a`>BJMz0rmS%@OH}n%SH0hyoO>b1l zfrG=OQi}UF!yZndwA+?D$GlyiusCJl1n!U92ks?bQ*jyBDY};{0WT`;`qa++?wDPg zov;|u2@S?V={ntLbO4mGjL(K7L=v7_6F*mIDKP~_&IA4%og~9bp@aq7#WRTW=vCm&DA4@qAQk|0n zdFJvouxp4Z!MOYwu8Ou`gha0!Cq^4LwuE((VsV%+bX73?KT3Qvg8W<>wsDYuE5MD<>;Dt*7tB(mOns2y;2I!H^4LoEC%(` z=`QqCKXNhb8Vf?Lg36i5UyAU-&b@QK;(@2@>8BfL^ViyqpHXirMWflR`ZiO2V`X>a z2+Q&jF@_`FUmpi561&{c^`o?r*XZBiubK^andU;U1FBvUb;_?q^0(b1>a%;J5BRe6 zWgJp3drcf%WXh zf9g8B+3YR9eopC_-2s?FOI`{y3v_*y)<7tELZr|xFf1t1@u!ps?UV)?OR zJA7yV%B=7|^`zuSa@?f3h=>pqiD+MvKeIp_QL-mdVMO)w&E&>R^v=sisQVJC@v z`;oU6iwrirVi$6pYtZ21j_rpBf{D2caB<_p$h_$aLy^xRg%LR>Q;*KRXOuzt?D3}k z5Rbt>MCLAUI8R0m*fU?8g-glTB@Ct$a)Ib4PGIm+S#0EZX&Rr95CA$&8X!ynr~{>m z?59s8K(P)EuxI?54h*Z}a=K=b>Q0x={RV6BLEP$Rd zb$#6U_21K0B(&C@?@QiUZJ1i`qwf)$cNsHBk|AYVyWvIqvZEwfa8ACta$S9^zh@S3 zn149))u+m8TPaz?c&oE*@*TIhmhc2wiD(@>N z;wAs#YSMXE27{%Y7uPwbCt~NnmUF4{nqSR+$xNRRGNCdVp5^ z-Y#9|(}nA=^6>C*aJOH$B~hT=k`xNza90CzpdHz^&m?tgih^iZEcNXtDSQNmUtHwzdO7DVJvqBl zus@xwS>)l;oB)}$M(JPDSaYR2*i2UwyXq5^Ocf-+kUKwN z1+oP(9xBP6?w$=?!o^|5Ze4% z-8I(gChRyUGR(Q|vOYFa=;r(I`9evlN0tIY4X=jMGX8L(QuawBL<=IoX4Hpex+la+ z;=KTOY~(?gn-8bmx`J3l2GO||tjGTLnGjvL4Wq4*$3kH686GW~31erD9 z9u%DzV+wgpX@&A71MCf8cPW6K3-ReU_vJrnzPRA^;68HvO^;iglOwU9Q1hMFN>zM6 zZreGg*gx)ALj3Esk zl^(w?t!^eGpt+4^%d)?&Lmx)FqQ(EV=$%y}F3ddf^|{x3tJDk(dTi{5USjr$0?;x7 zSw}$!6EZ9?a9BGfw`MGJi9zI2Pa{-w82?rHqsqVGDf({Ade>_xg9{GJcM3clQ z`|CpLTZKlJo%O&_4_fP=Bwh9y(IlI4#vS{2(EU=pF$Q?#{j`{5LS^|m@WS2Z@rPPX zdESXf^+dAl8W&)K%i5xc8uk*1fC|TIYO$61zQS&vAVF95Z~_8$6A_DvD4iS~p$pcP zG;D0;10erJvojA|qk+aCvE4V+4TXhx%j$BhK5`X~!j_CM|3|1T|6R%IoEWiE+Wx$YTsI7WmEl=>S zM_G~q8|Pz}AFG?2n}tH)kfa4!3BYj{H`N@3;Blp56A>|q0&k0F*jLiR;a-_1M$?v( zFq%Net-ullu&H8@!~Uf7nvNchO%_*er*nrQYyQGy?HG9ce*WQ%i~*^x`-8O~=oM!X z3&p#-`)Yo5YjQt z>d~`uE+6=N)DzCG8mA)>%$6LNE#ll#l@? z|Itbxm}UVwV_`FB9HuH;JT5y;lXV$01Mqf>z(#$p%W8vzxEDULx;@~8RY})>n{few zLV$|-%p1vz>tUJHXBuENTq|W=Fvr)%JTyp}*2@bwycH5YneyWwwd5(>)T&#E_D8L> zQnX``ifWO=lc1lVl+5XlCQpk5Qb2+9OvXv4*VyncaH$`%s13JJw#Yqz8P0&BFUNsw z8~ezPm>tUVV0JHz2CqT}IN>)KEg8tvI)4G1#m*U7T2q)mMle=#Lv`Uw!Yda01NmnO zt~rZYlGBIKwVzY_n$R2VhJhB!N+0NZ(HGKm1$0IukpWHjaY!_qFWLL5*^?y}LI)OT0E2$P5D>o+~R_7&(}CD_OyJE>WF}rTV?1uA&^*~EHh^GL?eUhv&91awEgdxYT15RJQG^RHs>4JccuYv@ zRhuYpFLq*!tKe1w&`)^cAO6O!?JK3^pXc$0LQa?!0Z=$Wo0s2E?LO9tKW{N8xUv%* zU_OaW8^kA^A1uw~W0I&W?_hkMCz&UC`lsOVE8ubgUplP!oQ>#NKQzIPlW81-?*l=1 zg9 zA(6XLbDSjq286}x~0XqcU)6sL?{aA$#ic$aV)LYYf9TJqI8%9=Cs_JAHVl>uoh`gE{eFP_CyeHtu zcPwMuiA>-3r&)cWP8wMyf8}GpB|gDp)Bzgqc<5Y2u#DkYzkgUU{^vCXjK{|tCj2JP zT>Wf^1W50Ca-M#Zc(uMrF7bjMC`s6mev;6WanaT$`r*ki^N>b^>fdQ+NNp(AhEXwy z<1C#)JggTULq8xYW9@ZL(%yRTdJ^767^*1RI}IsV5W}KK=I{>7<@^o2KwYbXf($=E z(LpT+kL%oO8Oc}ioI4H{7BtY-fEzf$ zlDgEes}$|T(-?t4P#K3$e?@0@-+4k5)}LXuVe`MJWF~(VI;o+MxP23i?(R>4Gx&W}j z4lR5+zb}18=;Rye8X}rq3++5P9e==DquYHu)M$$=^_g6I<(6OX$r*#sc0NM6G->UJ z;+4S-C!SryD-;UIhZ8r{z{Z}91J{4Kgfx3T%iayVc0`0rx70~yrkkb7c3mymSpmnb zsl-z-?fbgdYpd*=`O{=orq+BjueALZ%~-qr(JK0slS5#UyVj~TsI6uXQ<|vZ7G0mB zbPrd1M9(91esoFvoL}1)fkds3MG|M<}17~W% z#SIZ6zkr|0c4pUaUCMvFn0qGk0^6(7r%y#CWizB0f=nautwZ(}wBgvswb}IQEDF(( zy~u)d+4lZneV3K<9ie?;eYMB_b5_oITnn3d5hE62(^B5$v+cE1iu_(&0yYZHmvfA^ z9e(73iyx48Jl0#_G?l3XJE+TjH*^-Z9o;Ry{B$3oQis7qhANSIRI1UVU>C^Jx4zE~S43_uy>-OG9^`gD z1}`4@p)_9A>%0Zdm@wD@=Zr0}yp_>`*7KPSj$tIpD;FZD9?OBFT-n`gZ*#n}1vRU! zC_|E({a@rG8bb-{H_9h+udhl}Iq#&{{$?X$l;fCmCCA0BZL?E@w7NWIqig88jxR^I z8aoWiwsL!#pf|Jw>6!WlK2A^;nf#TW4bsUM08z?hd<*%l>dT(qUJqF&IBFc3T79z8>6bSx)Z5tRQ!-s)wf+_C zi|YDP=)y^_M>6*B=N=1qbH7fUwp(pN`(NLRX}^lyGCACgv@tp_kjy@)DS)+pJ++i3 z6-}cUz_wOG^A%2#FcdCqzVe6GI{ap?4C;%yK5mDfFepm|DR#dbzH%(N5AS$<>(7&26{1H z((yz+F4pvz$OL_#3qmroLxz+ZOCVDL(-&6T1;8!#xY+Xt10WK==0<@-GBW@gyB(I- zn+(P;1|>M0DEmv?#dv4^Ba-3yp~>qF4pKZO6+pjV3jCm^-TGgmfkGhw_6*wnU;ueB zNr6yMzU!jKzgE}M3-TP43_#qa+6#LZ)_qUDq}oq4NWFUu~sgk@|HQ8%026^us%S2o-LE-`yAs+oq$@~q;vM&Lm495r~2Nn}mz0;=ev^r7qb6$rOlm_A z)m)O5%OCp)(nUJWY7DpkeDj%B4TsQ@z2i++ld%<)$Y}59 zkylVs@<)l;*DLqF>lo*O(c3@(*Dt2<8czQ83? z(o3+G9Z*zM^gL^Net)f-5C1-jXK`$?lxy*PdrQEYmzYe{lC@V&edu?uxG9hL- zCp6y8_biSWtJD{-6dCaTjm-F3#lU@s*M`)kM(OPRA^KPEtXV$Ni{3139&k4TbUu&y zd5#gp9Dz&IWd+5?<6^)K`)bWW+z=lPR8Sxe0mwBVJOcN_AaVQMJ6w24Xqa=aRQWvo zF@0L=bNV(w@b`sm6XfIz@)vuh0NeFioc3H(o zFCC6e`>#FZ#N&Nk?1;tyQmXh`i|WEl6nGxhMeqwjTjXY#j@z>+`SUQ4C20NUPI=RZ zpRG~c(T^aa;%%|84h5^!ev>*ODd!EC%N~nb%>5G*&?F8aaqB5dkmdwV%UtwB#&8| z9eJR?#Nim}!HU*+g7@a(Nt;A!=nMbpd*i392Azr;pL^cu{|ywSOC>l|^uuSu$-jnG z0OAkO&`XJI;vqrgV#ve6&x+g-i!nPGKfn~r0}I;kC%0MM+X$BZKdKKUa_idcBqYs@qtT~A)6IgJ`1OJ^GIrcwD8?vhT8ytJp%Dphh$yU%QC zjD!19R5a5wmgze2zW?btJB5WMktgsgkD$U6RDu?C?A*PZLiZ5oKC8*qIGn= z>m(~gxcI|TG1IZ!V`g{^vKdCf%)()iLCfAGx`e^MU-)~S(48&%*33KO9iu8t61Z|{ z<5@LrF?w6OfS|vFfDV8q|7atmhF|?0V*bas2n7Y7f`YAuMd9|B1Z*8&hOEq6ipQmU zAxfWHCGuk1fF-5b&1rf4_O!q+9i#U)flcJAV%!LmD=%d*PU=JW5N&OF$wqGLJ_c^6 zW_PfpKgr;|AwadeZ`q|A^0gL@X-con$&oJSdut7QEG&uQn9k1&i%(QCF84!ntdp)oT==ZAfB zBn;ASjMP_GnTV(7U1%7piLfT3lZ@!k`Vwpcf^8fyC14*&CbEq};=L^D*umntU>FCp zO?=)r6>ka~s~&p3SC*6{w4Rg4Cf@uhW&doXB5@x*21_dnOa@9mxSqvWvMfkD8k4ZX zH2y$1{my8`L)+n7bt0O!#26|(y=I+NU%T#ZrK7BAr8`Ny1PAvmrXBgC7ng@zY&)9xi}(6}^AR*p$DD@- zf1nrBV>||0Z~-PsxTn72+cO54>IIzF`zo122ofu8$g25%Y<*P4I1Yg*zgyoONH^*p zy%1MRE>bo`4wrspnzbCJ((mNftqcl+k0R!jifA_FO06;(z(o$Aj&}i@zk1gV^J*E;6s zr3MS}8zmnL?S$-ed>Y^ze z-@Pr5EjGAHP8coQDe0OINSZQvC&Zi^{wa^-^-L)sFZ>0Q9snYSU(+Dr00o8~=?~VC zX-<1gc;H<;r}%MMl9ikyCN2*9vFHIIrRDd6s~An@SiU{&EvhaKWy)8nmqF4>m3=oj z6k`>-!nKDv(nWt;I8ByVn;U)+`)Bg=5gkxJu9_n6UKppii~Zv{=4=g_0XqW$XZL!+ zQNv)E8?DNzJ>Sj!wO6~ZNM{p@!qF_zuIsxr1)8wN`xng)U8|ZG-Lyy6hu({?B~*5w z6)+O+=d#vD#e#~vD8A0Wd4RANMgo6j1F4j7E3B_h7Hr1u11k_`JHCw;nC z;E=HZL0PZ397&YXxvgpx#JqtaFc>F(AjMi3MPoTS4|<&t!YDW8SCOnEURYG5lqPEP zXd_rAVa%7_0zxe3s;@ro0&N94DNjEgm5bfLuu}dwvu7n|(V7U-P5?De$gHqg71?GFaKN@a%2zfW%=a34A7%}4QW;dw3D}B9HNFVdXJj~wVbaLv~Ah^mAIbJOA&StRvR^q1{GXYz@tzcsor)N7%z!QB!?t0i7(Zabx(251y9 zvWKabLRJp)NUbecz$j;Sbb3}k_-%VAWcdXl|DZz!kPv+*Q6bQ0@kLlNUb;an^E zJqa*gVYideNq@q*GC(7h0VKITbC7&IVGcrvCU&n@eZlaa6O&wEY+^b|;tf5(xL_ba z;?i(FB3yWPVK%J)(KZWCzfp%WY$tZutZQdD9S9|#DMf#qNc6-X{*aI zuiWVe#m6n*Ix(~8>UX7lrU~WuO>hqO6E^H6%O%^_0G*n#REjfN>720>+BNvC~7pX__#T}@&6W1g8 zA$^tlsTk3(gzB_cyt3Wz+Aa(kO*&{i=r@@TWT|-W@|9g%9(xqd`W~?pXRm0@^dgaL zlSPVLl*CzK^5&n~Md(%^iJN1;zGnTNQrH)W)nN7Z_7CDO<2PXw^0}cmTK9M96Wpz0 z2XE7EuzR&{H=3C#q`LC4lN+%;t#z+2H=8(xI7x8~*TaO^aenQdOLUXFZc}=RT)n&y zucULPXzJo|;age#@`j7-$NU2yqb?t$rBPmzh!oX(7GB5WkgVT$*lyvAw9%PD6!|># zH_r=99LSnj1cm)@CxLf#*qH3by+0mw%)D*AKwSF!SNUPp6n+kLT_*DlctsyK&R$vP zj$wUVZY=&kDMpFj%+lK03k#^d z3yzzkbtH3yJ`5F9i=Wm7nPx_Xs0@k3{EqQm-`DvN8g(T?FaBzM@D-*Q7h=&9ev6cG zi<&4@XUtQ!kbC}hS^yzO`YAb~qUL{G0B4=WD=ZVUt!x6ZufXSk)2JI6Fvg8RD?HH$ zOcntNpP0{vI0Ta-f<`(-foem)kk=pC8BBfM?vCpj*zi1@NQ?iW&3AuPSHHy{3(FC7 z-g|3VqY12efK$He7k8`Sa%9+Wj~y(xk$Cqw?vtEw_zmIUYeUEZ0`E*4=6oVbfNPj5 zL!QlW8ZXeeK96!IUIAVyEhbo#5NZ$|uBN8O%*J+U@g8XMM{8l_`wSt>u9xZwi0jN`LP5rBWQ&S)Ig+3G8)@c=rF%|HPTbXtU*CG>+ zTvlT=#^$sSP=5>C3q8#8ph`XDI-Qxao8vn{T&gaMI?>;pvY$(-#-QkZqH0~JLa|P6 z4f4fAme(B!ZP=!AHU6mkGE4syL!JGW=+*I-R2@4L#W5}Rd^N?=$5$DS*_{=;@8TN` z*b&95@DFRm`x1AIn!P56-CBwXB`6+;w4*Nz9~@5PSE)U_h}<6ZjmPq@*@hat1+6N) z(LM>g7<4g>u2-muj#EMoWV{^CE#5peBW~(nlUuiWA@GM(_8e93yXEgiA$#q~+svs& zvt&J?ua@bD5};K`9hcmFUJg8j*2|kN7nlE##JHK7$s56dOwXN9@G7`dlZE`SUn{L+ z;qV>eWX&|{Dj&ound*hc%ZEmC+(u9e#MfgqjUop!x!QDK6}>#&jL9BF-g$Lj)dC^r zB2E5+7-M@&^v+H*2z61!>z5^6(uy~Xi^yj-vsc0&^qEqtr?6!#td+F2e_9{npE@-3SbclLhr?RouAjIO?ouQfh%}dE>>`iOwVw1B1#!-VCK?DBIZY;Qg?0=O&i8`fv zjm^i`cK|n6yL5?XYEnr# zdUrH2A@Na+$I*%jSwI!uripA06Z}r}$V#Oit&L^yn6qZhl@EKHg+=q*>r?;sI$HBB z0$I5v^EbR&cb_cCmyzLzee1M6^*@Pv@*-7_;}-pv?Jwdphf#cQOMgG?GROZ0JU)c6 zH6Agb{0^XA1K~D^Z}n$EcSam7w*@GBTYsOWyIx;}GBsALcG4@|X+vui?@C^b<-Xto zkP!2V?~}L4SwEfRXp?SzN5jtdQqBT6zd`oy?DSW?lh&<$;o5d_oS!UXuW@Fnx6f3} zeP)eD?`PvbCWjR2w?>8cde|ac7wKvyd>a(;lv^tm3yzR(GrWtqaWiBw@SsN^%DP9a zZ+%1y8M(u#R4Khm<`fx-);*kne%cXRx^u9HNGK#f5~o|0R;ph{&Y-Y%Rbd0+z{9~?=(0;glXK^jZ$R$~WN~Xvz{e0~iA7%o=PP zN^frWGZQbs;+_zIy=2<$UjOw@znDPJZ66FQk1`B)Gc)FCD^>e>vnCxAk|*2-EiZt+ zp(4?V^yqrR(#>gU)|+zcfandpu)V(R`^HLVRTb&La&dR6oAbY$md-yEcMmgvAeQ$K zMT=j?(OFoE+jRt0-uJI!MD}b{+wI@bBg2Br6}W+f0`nHEcVRKO3JW=t`vt$MPDl#f z7x0hmtt}F4zW}8IhjDMz_x=UEgCpeX!^vGDEfd!!G(vV zzuhZ8`>(GsF}pKVu&@}b#>yLT`sYcySV~yB)LE5fCu@&YOr_%=6m6_&TjbH zyF(80iw7tRbZLeP+J$8mMLaL3)Afj! z*Ih=lvUe{=+<0?GTusBe@B6T?@sYF>$J%W4oX%glVRo|k2XlQL-U_UR>_>tdPo3iH z1Hz0-@yX=db57e~#i_3kl^r6A(&v)e+G8_>2v>?30w^-hQ(Z*K1V#!S-M-G9s--h) zu|hfj>ZdhZ_$l}KwXhk_qweMiMLrKY&H3I8USAt>lfq($O006i3b?~EjfPZN*k?Bu_Kd}91`s@HFxH{-7)!<`Z9;Vx7jPq z3{6?0e_I)-6)|(Nuz$|m0GZRXE^*!_gc|vo^2CLN8|1;17bJ3q^tx!Nd!HE#C`9D) z#|L3}lq9g<3%ye%EWmP8>zTXXx+R82IV5Fd9F1eG`rMTlWJ*yJeHotp(I%co|5~N$ z+YSZ$2jNODUli4U?u0T5TcYyqG3?8XNni1UJhJ5;tyZzwXcxAaQ%FVpEnKcWdS^AO z81q_t<7f2>t1YeGYm+LU*8kwOut0$I^}fDA<894RsO4x>F~NxE0gOhkWE+E8i=_ zfrERK=>FfaS5(QL?uTDK9ILG*delxxfOd?Gfw1%iTE&ITNp1KxkSc;E5Gl|61M*tP znYvzlDsC3QqTa9y)o| zwUM@Y-7xqZdgIwqzT|fNi)!O#gSb+r&_#Bl5?F*~$zP(<2=Ru$@NqV_Zhwv>^S*Tk zIw-j3ARM57; zk!jJ~?9pxBw7X9MTgRi6e*j%Yc3{JH%5s<{A*9HW9r;ov4v>t`vY)jSfMpKnhwmP- zVgs~E24^OU`nY|uRLOsglyCaoPH$C|O zlRJfiA#C^UKk>v{LZ`9x$?6pq&u)BRtp)I%5-=d?x4uUPB5;CpH7$D)!!*RomLqNv zWeRO8^l{u%;FhJdtmcvN&D9j-mvwIWs<=(aO$w_lKCASd%a?gd^RjwA{*#Vhi# z$M|n^52bjg54XA57m6|`#4TP~5bOz@&h1I)JyBEDxwtg-D{r{eZ5jdlF>0N+cXU6U zV)zS0tKsH_b@uqiqOv5ZWrTDDh!xdTY!-F=-f!jIvD*xf$m`m3BW8UjC&z`In&w5m zxUm1|9^~91Zr6xwD9Cd94=MF}mY1*-|g`DzozNTLbveIF(?co!+0v{Z#GJ93}x+xK^xAIM|-Foy= z>{}4veuwnikuP2o|MhAkR4kGFaV<_=L9{W-C3T%cU_^++wO0pnU=2JQGe4){|8b&=w}f~dk2 zCuw(IoYpbZJ5|0ZA=QTbJ9cmfOP9)ztOk*XGyazy79YPZJB3pt_Y1?hY2FK*sm(l5 zdm?+yd+wc!+BKVHW+OjGp*#Vaxe+1khFX>)-UCR@j+PUl*Ocqi%HdIY1N1`6^Vrb} zBd5Q}w`%_&&z-8JX*^2&CE-2k{mwH01cDRL**072Ozv`wZjm@f;c$IHtwjN0VKOju zqO{HuaRUCgkV|6)OyapUmi)d(xFpKWK~}_&D~rkoLO&KNU%m)U|0Uu zz_vftTph2geKo&xh4qIwRxps(p%K95M29t#{u168;oqoEtGtOqrv-y0`=Av|;QQ+z zbl&5j=vVgZQi99LcL#>i$VT|mU#8>K=W_ONF$~j+X-sH1;rZbEA0LrN2Zc{YhDPl8 z8>8LqH`6sAw%*S{qqm9p+zfQJrLPu9Vft*4aO>?l z&A4BT$q^dt^G-rBkNJqQAXk?))Q!;4=-`RU3Hf7Fk94IzH=YQGG{jwz!Tl`%w z484qJe|vdap8~=J0B7Olss zii#b;rYMIQ6?h)4U)=#~_8!WKANKJUIBeA2yYekI5(NTdnt=Y`G(S!!)BmxopnyKg z1cU;C?UdL&E(P3@Mg(`s|FxxzzhSu}iX~uC=g{B`AfaW#MwY3a3hU+S>!X&3sz2 z9`KF{aP5jhqQ!}qOU1r4s5Lh$|8^^6ac%(T>rM8J#xb+PzVX%I;NZtAxcbF%+CKvu zVc1xX<55TGE~V8CVp>>UxW!~BpTs-gKV7W-v(z{0iS9|^zd~v_JJrf?9{HV;WH|Fg zjWDv>99;i5J2K{{r^9yexkE>%Hnh!JypIyj^wMFn$nBvzJ7@3cWIakSdGqRVzu~SY z^fccY5}x?^_`tPlBQ*5%tX#bNifAe5$vQn$e)Uxs+oUsvQC>H4hRZBz^UYn&*!2_! zv(OXnQk@Gtq1fFky@r=f+SIr|9R{A7keskugR;dgV1#;}hHGIUmUgVa?`51Bh&Cc3 zdud=K-aQgD9BpBTWd4TN$bMa72+Q^A8jr8pg@{~VJa+Nb7vq)SNQYVd)gDZ|&mURQ z$u(zZ-nRV0{k#z@9>9IgCqi~5`zppT<(C`D2-%GPI1zgngUfM?_>q1jzm-t+s3-f* zNQOTNcVq0mwVsSIkwdqB$j@WEsx|r5Nmmr|HEJ%4jsOJd7pADA%zD05(b`;&EW zh#+F6IRb_+`sXY67QK%;S-^P~Fa@gr-TR0G?HsT@2jmAhKyd&QLG`Sal{0lEEo_2! zY{?8+{Tl;VZEeDtEec`KbLvt`oQS-jLQDR4NAB-XkVh@Pn;!QJLXL$^3G)4>JR%aZ zJPCn3{@Y1-Kpw9zo0KGLDMI;Y=qf^B2ge{sIWd!wc-}OT!4V+?;^=lI3};-q8kyEw zAIWgz;j-$+4;?EUpNn~$2p6^#8g_rECa0(@J&_PMyx%o!*IpyHm8y~%n*|-|JJ}>t zx0aQv)fbPS0>fw?gCX|oRvqNH5R*J%PhwyC&wUOM)z-@yZOnZ9NMfoX$NAq4|m;9g*HYP4p~V_AtNIywY9Yd zKnej2mmx~#s+<8JRiqQ&t7bFzClD`#(WT*C;WsvUrCb&^ww{qp;qke-ej^H5xIZBU z$!sxH>^b(j-Wm;O=94S03Op+z*!jJ_V%Em(YInij}q0r=DT9G_D zI=!iHPP937Zre#0p$8`S85mFl^U#r>>X?Mj(Go2y@^W$^SnbgeOgMpViqIQlY|=&T zDua%IU@`elO?;qVP`SHyy}mi9%txlBBwG-_rSNIBqObUU5+W%H`-vljl5<$RY<6l& zX8BdiI3d+7h*W%bYXSHS2P|zuLfNIuEP8Imx7UK|8gS)KX0RDTI7Rl0ypIHyj=T)FH*j{oY_;vR>V z8ebR6oIT#Vx6iT{Ek7!p&om=m-Couc7v`p!-1{2#(_dY0q#BZws6XS}aPbmAtM7qA z$-o*(jYb$k{rs&ZP`cRtan@}+$h2Wl6ejI7douvLb@_~kaxNvZ$xfJP->e1ojwDI; ztxeeFaXDCSBO^1)oTJHbIe#>9S&Zrk!=|<9v<8t^U}Y2NYwPQ4d@g&LwaP6%O%qSY z$Mm9rCK2k<0@fvn+%w!#10i_8PibsshS2Oc!!Hs=OnOBlTjdM^99~}DXZJmAZ8D&p z4Fc;FtV3!*3IxemQRH}vxUJd1@xAufuM*aXiy1_}Kd50jrqf;ERRNH`RDr3yg_^4v zRQoo%QCx19bqMgQ*(-}~+{U=3!$%_5j`3yTLk1k5;-FW!Y@f*c*G%1WG}Yt+J9uVt zqd@ojFQH>^Wt>Vq!OW!lC-P6Qx)9I;u@(CtPEQ(6eUPy2mkp;tG^*3h>+zZR-d+24(x`NF7B4?LZqYb-x=Wp zC7=BYfkPl*$@Jz*hibCYIdaPR8px8jBYz zl(F*3@US_rg;k!cb3yzoSatxb#1JMzN1TTtUrZc))%3-F=_KOAnc$2J1Gqf z4eKQCcOfz?=?e657P<-X@wo@`hgv*orEV&Rl=v_9Tx-LUWifr(N#4+7=TKCJwUVKK zHeJOD0nU{3bWQ~s&|nC5W|j<*I~Hwh9O;7Ed_5A0bfc4`nl09ANH z?k3*hWfqgwITV;RY-Wu0xdo+dHMZ^XvJ#{uMv~2&@|dwH;Reh!b~o+-uGY)#6l54q ziBe#0Z>qNuPUVAsAsUYXEXV=g3s%;%nU1tH+~7zaK}w>p1kw~qYwgO2oMU1lefaN0 zQ*-mue)8rcrkbiK08tcpRakhWDpdRj* zRe8F4_7*KI#g|FgPO91$RZg{kG4P`rYHRUJ)yuh#0$nd*rjjTx_!oHdJ%A3%r;Vy8 z+ec_&aSau@i{b=^6Sp+!QhJ zGb$X9DFjFNIThnzy7Xg`kQXAaSc=d$nonT};9u6O)TrwlFNM$2gY zxsr?40|^z;iDSRj!Tm6Ns3*TQc#nF8fA5w-u;}hL>maCm=j!dVE9#8&W8R~|4e$=} z;7m%5&Xej%u{Yy{a2(b=; z88vS#a=JKXAN1cF-6b3eA4fsjBXESlU=(G>K21^mU3k#W&Q9?iDq>*TXJfy=2EB)~m3?`#k$HCfxW!nfl`F0uYgEja z8)$0S=HOJhy}hjxqYrGL^}s4KLZ^PMfk_^j-PzZ%zIiofp2d+jC*##_86UUt8H6)b zkAjB+sVyMedx*>I>hH4QX(4c$jRLRr+DmMBNGrZQ`P5Bh@8IBh^BePfQb3)+&!7Y| z?S5C_;YBqSL$CPEw*Wf4m&5m9-O+}#Bui`JcmKHYx@%S9of zdqm9_pXkp~Gcy@k+lUb>H8-dDwcohh+6h@aEc-dv_h1VGS1|`3@sTWSh_@{4WfE`D zCtTz(yG+_1cdkgcq1iVO>qz(mUE*b`>s0~pz%w)I%n_YFPJ6Qct0(dR7-=Gm`nx%f zSiRFnyE0jv?S>F>f}trB_uqA7&;jib%z7Q9u5CtO4sGN0`5(<6-;hbj{k81>Qg=QS5K%oB5IgN;Y{1uf=*`=N_!}yjk2V<=g47Oh)!^{rkmBj2=?XTUi#l>lR53R-zUaYA*(a%KHL`l#GniexY{SuCR(Q~|I zE7d&9P+fFGpqR28?y0RI$%LZ2D8}B}I&k&Gd^7iV!gTE7u^iyG9}Ei&$buVn&f|0w zH`i4+EGTj1Mq$hm2lwzD#A3VHI*fHN3VtHN_BIB~q{8(H&ong~t9(>~*5A(|lI?Ts zH7Dus)9~(mLZdIaBu8Ufv2LGGKheLv2gZB&jQJN$`Cr5ljFR^iE{AkfAA(a@13(U5 zHF%&SFdICPw=wKD1c7eEVB3M?#zu1_K^W~cgu#>W7A}74FYq?+1vVhFta$D-wzB5MANDxVMLGF{_Sl41Q~swv@v zKG->y!IH;#7k?lI^ZnpX|ET`Pgd(r5LhGD}O}Jm{drJ!ia9t@mg=>FiMc1!*u$1@> zw#X8uD1KD^$*S1LbpW+otNX{ZvS<0)q5L$Q&{VWKESyKIwIYjQT!JPj3Om-*xc0cm zBRd@F)jha8mW0RIu*7Y1SeLR{sQn?2WDh zsZTPHt=`KmR;meF%Lth;vvOKf0=xmn@?Q*!;9QpnUI4L)-}mW^a~y`JZ#C3h-e46P(1G)T7k8wn+PSy9ohuYU;DD_>hMS^ zNw!+=%iA}mwSU>Y+-tCY+)KYuC55b4(Een#HtSxWRaR@{_Oe7sAiPSyjW76X^Wj6^ zw&RZ-+Ci?;bdcLjO7$Ym$TNZd+4CNh)X=*JaH%0cY?c~uAXPvf4btjCMka{lDm{pN zuo@O5MlYdUx%tiZ=_h;wJ*NhXPqZi{*22O9u&+sVw5gfz?B^*zg+kp#Quu61>5{sC zHXo8Z3=Kzy<}ri&vHSl2sCvt|sMqd$7!VYtyBh_hK}s4a1qBq4?nac9F6ov=K{}*c z1PSRbN$F5hVnn*0J@@(jUp((P_&LMOcdpob?G>j74u5-D9>p<($EnoAEv^lO8`4SN z>))sX()$XGFSzP|5$e7DX%24%-q5SkQ$cI73z2FrV~a9rgQL#vHq7vB9%SE)Tguvv zmU;k$kjTfQtMertA|L8+b;zJ0ygRT)@&9;u#}CA&Z4Vl5d+JC!;AP*XSXGk`RMX@U z7ny31_uxxPzgo~9QF!v?4%lm{w?xD-TQ{_BUA`rkp1X#R9O~lHu!!(kpt`vv>}-j* z8e=>ci)nQA_jmse%04VF&MLPG?G!BUX^F(DXuYv1{21wVl&TqQVxe%^vw3T^^u zcx}>s4+e#h(aj6)>&tK!OmY?o(BAK#BG}%Wsbw#THlVBG;Aj5`*Zu_6Iau|8Y9#ou zvjvgseFq(lT^CMPE=4zu@IApHqc5np_vmIL*j z<@ybOXs$ok){=l6?{jG>GUE|~<{(OAAF4BCW-5e!1ZT5O*E_MuN0R$X(fdKZ2K3>O zJ{Mg26U+`DOr@TQ+b{Xq&}=pWbrh6c^pE3;<{^ME8(#I>+nqLeY*D_K+lJ7%`NMvL z1fI8h3sI0Pl?7^jL+?WpAf{&4PwAZZ#`yo3CnF|C`n7)als-3eF7*Z=AAo{(5b##Q z(Q%InIfMk}&CuFgkmMV~enf^#nV2v_sb0eFO8g>pNUmX$NuRF95ON^pl18+cdL2dxzRm2 z{CQY$X&W(BH0nKpy+QjK16EKfmM@(ErP71dx4@?B>8UWeC3ic{{h^=fl!2`+yTs+c z7aG`g@y{OKP8GC!2Ql}T&tz@rMy@|iJ%WFJ-Ew)9vVL^FcuiaI=`AG`Fy>mjVDvqQ zVMOD{mBW;8G@7N_^RR?zH{`DCXH{jbP#EMwa+R%8f-+6&W5RnsIcrh)=#INL7zVFLbTWs zA?35&N{-KjO8SmENVHr-v=4HmwWZXeK54-$)0bRVy)VLeQQc@T|r`oFrrzru&>(YxH@l2^x4GYM@9#X-6z)ygFI+Y|N>a z??&lm=26YBiO<|VO#@mc`$=NiB#D~dA3bwz-%Ad6Cx(wVGd~8i)jcOH{8C=Z!Pp_h zG3C4T-)Y(%rn_b?Fw)3#m-y(=Y2)2Fl_PTMCGo;rVT^^KLq17~w2h%pOy5%sK&Zgz z#k~-4P#Ru;QBSV`-%+&jRO<9MS8n8<{i=PlGfkkRg)Xzy)QTo)PFum32CgXuUut=t zRZ78;$C>F`tOVKanS6cGrsI>41Nl@k1^h^s^HX29!J+MNIPtPuOpw;&PR@s0z8-!U z<4<&RUHCmh1st=&WSd9a9kJrgDFEjAQ>_I#pGZge?L(8%Y`Ir|yfh(z7iu_6Ikl0| zQB*m#HcSGaAun<5q@P-$k=et<%n5k(PkbS@1o@qN?lmQ|;U#RP6T3Sqt&o_;bR*s8 z_|R^ue*5uWhOGB#?9nC zQ+h+RjAXB@dgD$$jW+fFX%78W{FN99L9xi!k+=i>x&_y2<>Cdfs32R#0chql7EQ*;G;H_wr)D#0W|sTEE%zs+ViEOGt{o9V`a+bYhS>!Npl-(S-)k5J;rbaHCy7VBmh z?L?$rnMx?iv1^ICoOct-%jGgdRlj}}Pq0Y4MSz_ zw1=w(}oZ{Fp zn4axuQw?@2J%CdSCG(yy6%`dK_4!jMfz>XL!g+%*#iQcMIgfVdBghQgvbuM)%8Vy@ zjc}EAOLAIpm5xesByq)49B{sE6&3SX45Wnq4!%>fu(7HAWP^agWWKSn@pBhkXDvadudUT33D7~kW>F^YdKC-hF%lv`x=gZc;A@1R0bmJrq`@8~*894GZlAd@`RHa$l*r1_6DNu2+v$Iq5u;yMuZ%J9% zkiDMRXYBHn*a2`RY2A4nG8k|@V90_{jdVvF4#k{ksKfuq5m+;WI(|;I$4o5lFh$kF z!*3v79Od@S=1{u$z(%GCOvMf>ZiK7Ng1g4Diq2y#(_E^Wz%8|C9PyT_F{ zd8Qlv^FAfIZ~40N^N|AJoM^fm=Luto0NcIpZNnNoKZLhyOctkS(j3nCBRmVe_O57} z2#DRP9q!G%^7#B9+^MxCZ_vh;p7YDp(o>e_jx1C`^8WAvtKmhn6sLiKJAdG6rEoB zB>s>yn}Nf%eNq9N{C-(G`1pdGx%Aq{yjoI5x6>?( zSjep-4OFD4*hotfi>NBIbfDKELD+SC4A#dJcbTI4ytH?q1Ffb`kMB3UZmLbYNx=XNX8=>7g(teFjH#3>&26z!m(mgwHcYGY%c~JK z5c!9KQc_7HgN>N z$qbJPABu{8U?+dGlaP@y9D1WFpMHfg4(kaku}Kp5c>1K*rvFRzE;owI`cS;4njsUK zv;I9L0G^PUYgkx6DW7+x*_JmYC3+${$uw3cUi{ne)S>gRy4A2a`S+(8nm-BvIiBrG zF5OX7-D^N2&#!Pl=(B)|oas+!&)-uot-9AV@Ld-$8o7l|5pqjj%=Hvm!nO5rC-k-L ziX|FG4i_7nnsy}E>=%Z1!8ZtO*#`mwt?LJs9QI%cG{t=V?`-T5JwE0)++L63`g#hf zf8llMatRAC=%d2A!zGj8DJA+PX_UNH(J*#o7tfJFM^<#wZKzkVxyC0XvgdnxHuqdN zhgD92Eq9mjjp$BZb>blLH>3S%8N=OtezAu++0jQldPXG zX=dIT{w^?XtKA+Mnh^`+Z0YOXV5?iopkWBa6$IE>p{O|4h zzu&H3U;3yZDRmH=V@G|0kC&K+)!oa|CfK06mX_Dj&dm*Q(IN&EEh z_$t2lZ@nJ=8Jqk1J1p_*bpzIWZgUgov)d_euS(rE`vc}qnDp=hPDTQLmKcBM<@?Od zi@(4V?yo=C#u^;0_UUJ}<#swZJ3A~B9 z?~%}hd+Ff^>(L6c=TsFRN-n-_A+9`K$i3643Sz`C7X$8oJz3TnIQco|W_r13?`lAE z)Igb-?48@hWH7aW0F`XnSuB17=wp*sC=8ykKMm5 z-U&h5-XFsvTi(>KuJrm{eA1nfWOUKu8?i!ng-dsf=)F%m)sXRQ1>!P)R{{mSSTMWF z>E2L=CtAg}1_%4u?FGd%f?Z#0af9nInMf^PEH3>k!I^*eER3Zdak@&q%oWGoX(yv| zR}=N&O+V|VEOFm-xL~#vr8TS~X5a7;_`^)RaN$|P;Ymd`DHe8jwP^oa=dqDD6^gU* zp5@+Z&UfWGMBBF8K@=H6$&x7j+W~H)*T(tRZEo(?gA2!B-iO$)FMb|aZJ-&gd62(@ zzNCvO9S@^YDz2^i&LK&*dpj2)t;HhWvozsYjOwTd(UKR-cpHEIbiDCB*L{KK-RCep9R(OkY}gRx4%Nr{QrhPhnhyo7tlx%)66n$FC9C!74fZF%j6ts2$& zSw_o_C9KzIi;iz{Vtpum_S@@)xBu-e4`3xPH)o{z4-Ka7_hvB*bBWPuKc%WzmXPE_J{lw2TmAWsuHEy!!zkXf)`~Lu#)}>-%!?&dvFrYR`=Mit)wT?YXhW}S z`UUUPog7jL6dr7rUxS_XmY;&^ldsWQ8ci63^9E&lHOR8|=k`|n7U!Z?BK1#3&hO%e z;I8(s)3qs;9_*jDT;nAfxY4d9&4m}Fw_u25Nct-*T1oc^l98pS`xOZ-%4M<#sY#n&%P#$SP08CV-|I6mgD?tKou^{#yUWYl*cPv_5(Pr0 zwFsjnDKMOdx<0Jyg;LR;u<0=o@|>PWIP@M5ce<0!M_xBCdkX*CJ4Te>UlgSb+R?!A_m(cS0% zXJjPxFvx}6S$V?8ae;t=OeRr8+?1e=vD#_xrXVa;VQnTvvEY=k-8=LUplg=<5&g2{ zu}=qw>G~(v^l*GY;y7GHRr>aKei%A(uCB8*w`vf}kzC&c*)rR&gu*B^jk=neiZhst z%Mn7Q3sbV?k*r6m#6bk+&e1bE;BEpa-Ukgll&8gPock10Jw+kU2X1UF&0L(JEK@9g zmaJ00&&g@7`SeA;a%)!<6%gSgfItH1Do@;IlJ4tP>w-_gZ<=KR1$o`-da^o zN?^94IMhVQE^&dDQRazB7IR?QIZg)tt>6QLB}PD^M%(GSwu%t z_)-|Wl8Dqd6rgn}IN7CsWVq{x?>B^`)2+s|BPg^}5?mQ}iw0Utiw)X_5908*{^gui z(>Loo(KvR|CWcjtob;aE5N_ON|MRe|l*CqufF)8A^|t>!Ccd=DKrqoOsqF<#>f0EV zhpWT*4_$DFhQz)^KdxV2dF6SlNa);wby=2JU(atO#H3!bYpR8JE?V#RpGaan)lAqO-!ij!<{z=SJn`F6T8&u4W)|?mBwPy2mzgIb@faZl{tB%R z{7u;=Yo#U`#ZYsE6db5mBtl7r0D$38VuwBO!8T1%%(_;t%-rjn&Tn9LR$7f_S2#S+ z7Zg{*i4&nLZeKPshtP))zvG-A9N~;K2_|Sn>UFwh^Tx?h#efSP3^+(CyOMwZ{=`Dv zDG2@e+uga|xk-uBUqqk=t@>jOF6Iy~0{4vDpUUnEuml8_DX(kq#f8@i>>qXY3Zq4_ zpBALCd^U2$_8)grSyVh{Sn!)wOqNU#ABp<$g1<`Z$Wr)Tb_Y-|tyZSU)g+ORM#h(% zhxUXCY*WpznoL|9NuXlc(aOZv9xM`g*bSd)sV$VtKQt>9Oo*QTS;8$8aK-qqo`t1ihAn zgjRcNglvi40cFu1Z3t(%21C=vo-hc1vdXvu34hox(=D`>53em=-J@soMhkWdj}OYC zcb$LY9=A$*vFgj+ep~;#bt^+ZqTpeb!y3ivw`0-qpt2Dj<*zGjjb8oO6Br-gziNLR zmSuWJKsN?IPCgPPn!1(1g7*P+dpsfj)0fiky+vo`qB4kAPm zTe^cF<@2n`2@m!2kReJoMNa-2tzMb>lB|(p}k9@AmQf3#Qud-XU zSO0k3m|OyT*SG_7 zX+O4o<4(|f6eF!4_RN`B`U(REc;2YXS65eqHwWeW>$)6Z;=!5~zP>nssq82WOhDgM?ErKZM!;w!uMjcX_uX|| z-zAmEzB6!V63MO)^Q~R8%jaf-0_myaQ|b(keyi871+f{qqm`62*^EjxPp<-#I1cl5 zm1~xcnUXw3@M1?IT+$JGbk%&XTd6lBxEBs;n^*OX?mjl*s&a1XQrh*P(u~X`=W%?_ zF!UkKPu#_2`rA5@I(wM#!HlQsLl(NRUmt|Uh2nLm33?2j@8%0M25VfOh+ZM~5MTe? zCzD4dPv^fe7CjqdH>=CLobbZ_j%dEokFUg+s9NOXisUYg`9mrG6w!2(P8fqJG?zP8deX zI|k>=zDVL1&((V-6t_$7y|I3@ZapU!mS;(kZT?s)GvxK1p)}A@oaNTls1Ln5;;Hnm ze3Ro~!?va4?`_=#AwbYQLI_>oyV#jmf)9j*AeE2_0IAqV5Xk@&Q&v`1%ojybJ=6rS zux{bengzZ$xS%w!&UF|Ska>**8TS;?qPxOy+yQNoUO<54`eI#D?Zt}#7~>&BxT-iz zfxnGRpM&AzKJY>K9b;KI{icZx#VICdRPOh)+g%tUoWwHa(5&ZeFMNEbYvpZ+4MowJ zu+W%3kiQz~dp=FIRDDanA^m2+VQ}a|0%DJ^m!EsfVtL$RyiWC)#-qIas>Pf608IYgJ&U z`>wQSCx19uy1z3Q!8PQ~=+O831o6RhzwHyA>2{5rR)SFDx!%_4*Ubj&>B9jR)}~{T zlKG9!^S#F4L|^TYnf4n-yWVbIFMma+0_uPt{Pc?V>SQTs!|N3H!o-fhf$$IOX*oO3 z^1*d$cIUC_gzjGJxq*m*kImit%~=l?O9;yB5ZCyYO|hYD5w9OTp1C%7Ml&=uWT4~8ighuy1HIjz*jJ5&o*S;M` zC=kPKEnxqzCQlVdPIEIYlJSB9^17#qwNKJ+y$O00UPsz0v$$P)<8YAaxkauKPtTU> zeRp2=q!ap*g75q1@@8n-J)8!O!X;kJ>`XcCf9)uGZ_^bAu64(|I7j!;q<1jd=E9r; z=zB;OzO3K*S-eSPqjwr2s#E0N6QFkW$CAIqM4|QAU`VrXyA898km2=5 zmX<6q|HsF^>Cj2?(g^&{Q2cJ(!iDB;AWeh_Mq5_a*2d?PFY+D`x_87E#rA!yPo_xZ zW#o+1!>t9I9N~rMPh&>Zi7}oPyvWnS#gJ7RItFS113qTO;h57063$8vE&Sg0^-;=kzwZ*A>%jMY zW|mzYcS$bn(pH+}fzGspq3%R>D&C*G7+(T&c9_#bHMKYn)&PF@00$so#_J_rgw~N@{icMMIMR=P!VVIKRkzKX(UtSiTV?@mDLd0GDT z{;K$#s|znzvjfK4p|4onO^$faR3tPle?f{3B9duiz#ud)sPNBuAQ5+G#-bZ#1k0=T za9t%tJDou^mGAYPBFbi(f3hR4GDnPkpL)e{Z9|ef)i0KsEfLpg-w=PIu4RwWwCL~d%i6l|MKEg;vWYV>z9xX6S?&hK~}qm ziL3K>S=&kR7Vd`A5#jNQdjc%QTqqjV$kC6yHL)57V&t%NWa;U?F~@lw{D!f25X_8^ zq=t3BsZ!INJ*%N-{4uz9%p@d~9BT%qZN}0Sw~0|aV1ksts=e81+OYP~7Rk@9muEbs zkBb00F=VDP10e_xqBcP{B-jGCKXTPUm*aW7E5}3Lu{-}w4RRQMe-28Doi#4Ozrl52 zW~YMo+%}QGSXKuL#(BZolHMA=b?d{{n7YzZQK4c|>#4;JZuXCgvQbv0sMPTkIi5`m zSm;=bcKd%v78Y=Ud&X$dD^ru7AKZ&S&kyAx(~p74@l7dpy^jcAe%y2o?8)ZgPd zIXQ7rz&_~4+-sqRhG3f&1FTDbaIXUXj_E?9`^R<8`sxwg3bSDuU;4PY!@EfO;NqhV z0o6uY$CsN?w=S#_ZiUm>_jA@|HaDkh+gl|r&jJ-p{?Vg={NH!Vdm=IlIeB;AYo+f+ zWMI*KQ+xxo1(W%I=w5n^ndhzU?ZG%GejoTPO<=Z*%qauGpmoY?U2dPa$*)mcakjMP z7DTfc_>*|OX=>lbCG7{c0!*HSdA1RGjN&Jm%+ZVQYgeCm?`>@O0h<)!u#r@afLnNN zuoEle99IuLel8>@CGoKfe$=g5r48q)zA5i%=X{C7EX)6_+T4>-GQfPE16w(?rBKSv z>mh-~ufy%~mgrU1S_n*9zG2Vg*N%eg&~1Db8ix503M0N}DX<*(>Cl}z3z`(C@_2{H8GM@L5~bG^)uWqf=jfI{Hu_KU=xDKzkL&83BzSs+Fr zX{+SOU)0=TP$$ZB$$D%|F!t7Lk;n3_G;bsdJkp`4%{u%+!l0lwq4F^GtP=4 z1q7L*;$m(6oTkx7B~IHoafJnT9}88?BNd;}i;al;N|3%|GT6R*M5iRRfAvmIStpc? zu_<)9wMCd*QT9Cv$ISncy1(;~2UvXc-(c))@A;&R)tF6+iJq;)UvyqI@q4~ zMzdatDeoPE{r~rjAgkH z8~gD?{rbfx2q0mAn(vb+$n0!^@bw8m8E7n!3}4Xo0M?^nizTIz(Olrl> z2cTrZcr!)7mh|nP>DINx4;X$`0GRx#LT|cSt}owc(8b?-+J>Z}_45IaIszkTBvd5j z+UMePeR5c^2k$2~J$+~ zeN!|)puBq>{j>z`p0MLDOcZ+f1TYpwoQH75a{ss z4Q`N(Fi@m`arf-cTs;xW`}p_(K+QPx8)~MyFM!Fsi&lXvl{`uyfRvfMBYGx`3ZYv` zCT0v=NR}BZu?(=do{1r32l;>$w1J~lWtk;uB)@w`7$f!EltPtG^8DxU{+!crVKbd{ z{y%+qb3?$0LB7g=WIl6d{6ThdY!k4mgpg4;&{t~M$_8TL)y+mh_FX2dVsN@v?#;)3 zwW9TQX9nYO91qywA;0iAW~AW$6;s337e6^ zSP%|MFYv{CC4PF!0s=i^k%cM_QBhsF{Ms*HvU76-cXHB8o}XXL5b+XX-&BJ@U{LpY z0ZZ!D8+VG2yrwsqydlvr5J{XPAtCv?98C2dWb4QTJV-B<0*<)F;76pg1_p1d3!ZzO z3r(I5XPX74uohwXZ(Laqqk(o}w)BA|bH#)~t?!0(7akFpTmFWk%^WRj%&TGp(H~~k z%SCd$SK&}ZstQE%{n0=!xQ8Roa5ZpbjlPxouD-6e7It=`et*+X*AiH#F9`+~1+3gZ+3DADc$rgsyn(hQLY)BzEvhq7 zWCG1F%)qt8Oq6NiD+U4AtNDh+ypeqIv4~AGxD22PU7;VHgP9>xfy4}`op4Fh^ zTiZ8kN$;3x7Si7OyzJl*sJ39z7Sp+i+OT@5oFPG@B6;aNts}=2bFe<#4G&+VGT_8b zVO+A)he6x9UJv-nm^TB!U9ioen(HzTv{zv3TRE4myb>lq{i9+tzH< zrGD-fS&)jtmF(foNE$2ww+RoJ=!f2k1q%5J&VSFdPv4N?dBukRS%m;TzMWvE0-ZN8 z(AtZQJ;hqHCYs5KKfFXS*X{y#F0e>id9r0+DJ7XJgBBI~@caagYGxv&Ql*%pXmMd7 z8pybVQXk=aKNV8=8eVl(gYloyT!?6$q!BY93 zZ6}CGGC{o$T+gIW{e68V=%Fd|J&`Wx+LfUlu^j!NQGY;4*>i;Q?e;o3nl zDwAt9dY`yVmtVWq5zptf=s_yV__LSFo}*%#PBBL4(CwkH_h{ipE3z`B8^dFJMXS6%tuSd0ScfUpXl`jNsn zIe*-JzBqb6v9UJd1|1_N{Ra9QP&#oUwm?QT{4t56U+Kl8-u>g#k7@?lhLyQ*2 z1&VQIta!1LqkyyK=Ay%?NrNJw25!yWdd*;N!VB@cY2HUap)fVI6>BJ-wQFd;lI%(2 zxedVW{;5|{E%3hn@E_{KT`=<|Qxvm~Vx!kT)^YHgquh<=r=>@OJ#2}~t2_k2Bz!HN zj-G8B@h1LiiqZ72Oqz555Iz}G64`b~j$TRV%X9gijXZpzcd==)6BfCSHz)#J=9+M> zj_osbpQnDo^Z*Jw4Isw)`udjwlRk1lR)_N%+M&_KN0tARJ3hh&L(Q$aX7>ns{x@t9 z5fRY5ikmH3#2w11=(V0S0H>+@2A){v!}7YJ(dI!ieVG%KE_qGa%En8f0Ek*S=lCu1 zrZ;oq7M*DU6jZH7s^$d5rM3Q=?h+ftnrU?OVpjYkJI&z29R$VWybH|fkmY%G0>LUV z;&4Nh_g=Jc5|D9`XI4XlPHnQz!ClpsDLp3~fOwFKoeB5mi}AuSn7$i$?wD(&nvuu? zt#Z6Z`G((i**9DmGP)hlyP~4Vtq*ohug0srsHRD^c?)=@(U086D1LCbRd|GFf8TUA zff!@q&D{9?l?d)FoelEd0*d;LLHcpTEB@zc4Ezq29!_2etMYPFlEd++dLc7&RulS; zOiB*?DI0oibZy!9kMRkBlokV_>&M6T9RGNO@#vy~@_bZWWq}lqHle0t@)5-Z(-Y!7 z!jU!mC(8>$bX26pz~S~(^&VqwH+x9-bE1g2oC4wl&r&%9SsR58pehdZprUY86Tk7! zow!Mtqi7pB+DEn*-F=KDCHK)fmJN19ef@{hmJP0aImRFv25K=k)nqsP0M8exqdke5 z*yNm8_~I{l+q5kQ+XHX2z=xB-VJbwb@t}PGjV4q(4+I5+W7VO$Ns;h=_{DBE7@jxw z-mCWIFS}o*&xu~BX_Pj-^lD@WKJpby^20ki&zN8NvaODyn+m>=Z%mIgd9_LK(PB!5 zeVfX6=#Tg1w|7=%%EhlZUI&Z2c!IEs=Y1%BzW~P*Y&wa#hs`-5NVzArIkb&WUCG5+ z!}-h}(=Vkc1%FnRyOG?V$LQa2B|_B~Vf6@B=y zZHB3iWHe8X`W!?kLvuM`G6Gc=(kFS1_HLJ0_8EvYOf%+R3jv$H>_y|Z1k8n|$XoTR zY7}8k7^~|~SMPIiS*(Mr{)cQ$9i2W!jwkvmhfsQemJ_JWPze7NW?LX=>8i2(b?3+A z%LP zTd@1I-#*~K^KkD83D&a*;qvr@rI1WdlS35HFSA{D_p>tC3JlY7|ITpH2yt6kYWxFVIK|n%~@c#Y& zCyD>=kD#n@NVD%&S0OasuR)zJa!%hI6-sPw=0AvIGd-&eX- zMgP?Ujtlgh%5K^hQHBxZOC*bmQ1%zOWV!=>0%Fd$WqEl)o$J~__hGUQ&2$o`CKs?F zhAAf;)l8u$yrUj!`f3z4P}P)|TORyA=E27dWc&Q@Ll5=G7><6>X_*`pkgJHHzED;D zKAC)mIBncNy&9ZZ@7_bWk)IwwHv`@iyaRUFC>F}pH$+_Tpkz9Zweb8o+5IY!t&Gw= z*Irh?MwwI}QRhWquBWs~8b-xg{K+cQb$%cJF*TD2(fL$W(du#RbukG)=Eyyq&-$EI zAv46Y*?S%~IOAhlEILlVR)2-^FjmOzPt`1laLR@U<+7w&@Q!d)Fkbo`5Aq*-v#l&Z@n zA58W15fg)lXj=e8)9;nvhsB8Hr1kY_Y`X!|yn7czA|qyy9U?(M81}cHJA|WO`20_` zh7(KZ&KPBUC!tE8HCx9;LwK2;zm#9^M=LcMbAY$!IAV{QP{Ip^yxsYw^Vx?I7KR zCPh+a3r^~PiWFDnRtToJBJn2QAGP<&VrZ>9k~gtJO*6NZ@J6+g$!l|5qMM0&$E;?e z0{u@sg-1f!urBc;{?8tTw7GqW__Fu3ZG|g`_DyxE`h6@i{H!F zqB{>G$EaP$lU4~EtCEJX7V7nB*T#sKxphCFngFsuiOq$@uNM)*%ck_BB30-s`i)S@ zr_>U@DQz=d=?Jrb_z5Bv_`$OwMTuOyet-41bl|Nhf4)#eZW{cJGMkkvm;$|4M` zO5hb?b})8-Z?hmHxO=q~G=}YED&qC-zY|}!mG}Yo5~_uFFicjaki3aPe!HWtlPJam zrWf6f?g#2lpnK6B^QEw?;sKyNz^iRtVNsGD3SD8%D*e8SxOZ)T}K>~)Ne6ESCg z4JLgl;_%-UHTQ9bVy~-5MBwNQQ;y#pV#AfbUKwX%(Gsh&)e$FRh5WN{)#cr}gE7ga z`AJn@yw3jteb{_n5UA#0r=ms-+JVm6L4OpcJb&7MP}ehdCBQbk-{}{V+#b)I&8Xng zp+2VbW~NqZ5gafBwPi*`mZZCmK$P4 z%sT2AJSh()|JkeOAA9XM$*}?ITZYwY;z;x9FDnA#m9LPMri*od8^oLU zL{K-6s7c_d=8*!E|nvsH6S~~l&yrTSI&zDgbYfWCE?VIx68`^|&jIE&? z;mlKvs6_$_qbA{NMRFDRy^82LNfU|M8}(Hb(zeZ&TA5uoS)Eg>w8Y4(2Q>eW3*gqNz9*eAv$i@tax%brOFlf+dA#VZh{%USIR|G*lhsOMT`0Os z*ukL~+lOYu7}b*j&y_SVWfI^~aE&SAc;oithOk9_Jg~C>p#zqR5v+2gMibPEkr1_x z(mk1O_=1-z0lLEKz?DP?-gMQsZ{JQgvZaWQ*mWv6R=Xcqph&s6)ZSMWMABrAKH#Bn z9!a|#?=23mU=diXr~5V7UAGDKz0Hjt#;MNEIvuIaviu2I)LN>xFdl)m@5JU^C0j?q~*)M@3n9Mf#^eWWFu*Q_0`!x2AL3h*C zwPfQ(o<|~KoSm@sUuLt}=REd1HUGOi(Rdy_9c2q3>`X!N3!(ME1S1-3_Wl-smVIeo zxhVn(K~?5X{~4f`j*4;LFVcCf%}8K!Q!K09atU-z(tt9Chlgt|!Z7kadkS(;gzMmQ zK`vX2d6h=%-&%fGI|cA0P>3M)Uegc3w@IA1l*#IuYz4@0`F?wVlLgE`*+H)W#R{sF zxt%P2SUvQViPY~W#zAF1ZRzl*CjY_9CoQ-r+|4J9#Cf6t6s6`MNvbYeUY9$>qZRAH zB8tE-hTQgHLdU%>>rh7Bdq%9tVb+y`KR?Y%iC6U$#U}TpyE( zc`5MT+x0@NGw|R{Kdm}0{Orc$Fn4-#xcG88*{NISKYtQzd$2NDDaRHVY4?F}I-je9 zjO`>Qa~buIokE9=GOd!Yw(EISM~uUTWJju=FV9Xr=No9iR@JYY9ptk_o#!$H;0j=7 z=ngDNY5pA{XNV;@62ev5K?=h=GUzR@0g^89)*~)61dX)E`T{mCNn`#on_(s~)lnIW+_Q^n1plbm4#}rH-iuO)xf#w_t z>0kh62uwwP-z)wgENVgQ_OL)<{BK5HPMJ9IuTPlwXOb07`6+F4|EM?qro7fh_oXA~ zmu2kga6TIKo8kAw3#--3^?nfGruBP&o2=J-^3T^5jdE`MZA$ruDLk{kwx6?;-9pjR zRGlf-a{u_PCk}29578ESeEoPZ%yr|D3m6{<=V+EXjbHb8o3Hk_7vNTT& zzTV1nvRqbozjw7*ecifHf{b9#BF%dZ-nLcAD z=hEkXa+%cry_s*0$By!~n|Dk7(Ww8-luwl0L379prvj6FC)zTvD$0hG^9vkuo9m;cp{3oP%A$i1zv#GaYV5{Y$H}P4=iZMN#y*2ZdUJ6 zKmQtOcLPp=z|w)$0^-9I2x|e4bpw>|VB3M>OeEu#MqG`K2SB5BLvAQDmA7VegV_ir zp?fy#9dJ)PPmlS{L=(l%{1_J6-7nnHtvxz^KN=4v4XQY*1#&M5U-Z5ZNypC~_Ojmu zL!Cr4Nrx5L336zuK*iD;&~1VVzz8Y2jx7y4JA16_mU=FWYrNNB>G^2I`m|H>|I8TR zu2!2^FU^7>0}|W=v+fJuqh_(j!u!70Ple*XB#}OUQo@$Zm{1|8ej6iuTDtxDhvy_D z6eRZTF7lR;(w98ykm_J@(Yth$%kU9f%8)=0792qt`C)A#j- z(nh5I_}(TO#_qY8`^P!Yq+#vdoy*4K>d-E0hQ(4%c6bB7$#QkVr*8+_V9I|ZJj(wrO+Z-Q7|D<9f>Nc2v(0l& z?`gP|&~JSVF$%>^Pm##LxAO7eKXqx&?#6zuep#^ERap6XKsQK+PU#ovpR)_6KdP3? zUADIvO9sQPU291L_O3q?%I|d7EjKN$(T}~4M15TN_0`epiI)l!O6H^od)&F9e@ha9 zQj*0h4=>`Hc*8C(4=biCo@_}j-!JH#gCtr9{|2fRTO<9Hkim-x#wpw zQ>}HjaPGFv?b@n!KqvOLzv<-o3ios-x~z3Ym#+OK!5HEsm3@C9!#AquU4g?C7L`33 z%8ZK9RPf>S{2M_7ul4JSl|i!ok%<6NK}#((i^hG7Nl-`D+Ag$zV!8;hziHFra(!)# zh%X-X7_4?%wCN1-^+i9p$9#;|qKI*am{rxDD?rN#Ux=ZQkK;s``S}wgo^T8XUM-A( zTJqR9X=VAsNENZzyO{3`i{6;HkByHbN%ep?wHoml9d1py?bfgJxbEuB48frY(PKY; z{4g_*dpEn2&&Brq7aF$^))(B$BjfeR_X^-`NEvnW{Aee~wlyifAG@2~A(xhFPd@yW zQPzZwH>UL4Aw@kJK^#$bs?9MozR{zp672vWIu*SlWK`hj8sA9p`8@rN3MT&c6Aw<% zLrAMr7)a$gZc|WE!Kge4`s5bpV=*z>7?mQAO2=O_u@NrqObPTq9O)0Tr?!!^FhFzY z`l;At?vj5{tS`$gPA=kJYnCsWKk>w7@YCAPF*50Kf%R;?9XIqvWlf61fGR>2?Tu2R zaW|v<9YaN7y3%(Z2Pqz0wpvh5P78TuN3MR~`Rnof z1H0c^N2%0eY9;L#ExL#n6~pi2MC|s*bIBEXkAuEGe!2X2pO5&B#DZGE4obKnjr>?T zC#%Q;gXygQ;I!idg0*7#%r$67hJ<2_R-7`7FNJIod&#@)0`1wJa+4KFbk}%FEfF2% z*B3W^eCLUk8yRo4ctZ>0Hfqs$xNt8GyGq&K3Ta;Pv5EHU+gZf&3*YW3oQHZJ1PB9l z)H-q6f0ZcfNi0O#-outxZ+CS&{zMn+yls2Z?LD;;9;W>j0Te+<)j>@BNSuD^{@EUz zIyH#>-m?x8{x{uD{RZ;SGM>6^w{u#_u=h!NQyB&shLByo#p6gWFujPpg}C^%DSV_M z;bX;Y_j$y!;`R%csP5nK`E6-$N7?GiF+246h=K3YUXg`HIC1e6t!~laMU-hN8H39^ zisUG2|9@d)yxw}*GU#lonNqFVQg3`%?~uqk#J$rjK78>P6m&)pD6N&rG(||6^tJl& z&=v**GfuN1P?P$=|;_&_*%{3B42?OHkLN_uly5Vd0qi1?iPqO609Y_ZzM%8GilcY2X;p zbfe9Oz2xqo>fLA3lN*{B6P5wW`a`juo-qH{+$)Bhh$Ul|qU`n^3M z-5nyG5&{O@AT6cR5<@CT2?)|9-62W~f=HKufPl2LB8{|wv~<6Fe&_wK#Rrz>IL~9)GA)Y#EpXUSCD}DU&|=Ew1A!o;koa!^OB+)OvA7L(u+HZc;DrCi?fEt z^y&r&?XTnIf(((7>*{akUfEsFd#ck)5ws~;nYkRwoj;#K^m(4p^JY@;85I1grWd6GapNp1 z8}GT`zN7o3<W zg*Rh+_q48nGy#Srm=IAaeR<3(sf0wMq^52&6UYYs2I3_lQB9w=ZF2eRcV4V`@dg!QocInY#q1p4Z>uA#XA1BK@?bRCv$Ml!=CV6<;LK z>r0;2OlB+7i@qrvFoPvttyy??2f1h!i>aD6kl3h$A;7eG`}>!)?@x$}jU{MR4|zm6 z-2L!^2Y#=2C>t=<@jh?WpK+yDPHTTqUF6d}b2Py2YQ-?1^rr52ROY>iurCC)m(~P- zM@wZchuske$6K?yH@@s!PrqBUnMBJbRH}6hE$Ru-D`3mc&-c(czw)b&L*@gV} zgSs)xPCh?x>h<1rFSq>SOu6=M@!bKx%Sisw@tw_l9s9FCS3CO~=5hG}Vob6J7$1L; z3f!<&68HZs#3b=t>h~!NZ)Rhtb)H}Vnl4J)SeY#@*O(mk%2WrjjUygi3aXH84 zLySs0Yy8fy|5DxZ{NG(Tgo}|IM`A}-Hii#*L?ybBfI>Zc+-77&f*DVOi4}wf6K52S z79hoklij9=tnAcAZ`?E!SYjcV6GAEA9*YOAPwucX-xK{01@GSnTEAg=o~SPq$TZZ_ z+xSZ{yY|rKdt53R-A4zTB>d1eG@u2+*j=RvVKPq|`J~;jHEtW3!Xl$IvwGPuy^{e=>?*D4wh$`avj;U0uK{-Z6{Vr9bgb zYxhk`E~CDGT2cCxQ}P=ZYxc}EkhT2#Ls1b%j zx`sbgF35Bxd{~&RbDwEb_~jh8chJ>YG5Yh&vU22sb1Kz2mIVv#n{5s+0ldck(Ti-O#@BN(MYIrIbx`<8#Ooq^-d;HCNhH^sXlr zurUq!YR(vdblndhqBtdzgmQ>xPl=g{#=y z%QNS5tvd4OH(AZY5>pd9>BlQ`ad+E(2nZAtzm8VMBoc8Fuy#@!viYa-i&;a1qy`Ie z72CtD-^$5;E{o`7&!^itV=vmx>d18-Iz=0;=T}hG3P8uidqwSmAO4%5(B7C=zav8b zd7Rl--nVkTi5_Bk3@Ypp3lH=T?xGuUhdrh@?MQUwonY<8q$$FVgsB|(% ztZ?NDw`__F;(t(qW%qBip%ec9Hj5##S^BVuaZP z(Mfg;cj?D316q&r7i3YtlG4=*X~YuaV`TaWwFzv#;}zB#i0^u_j-QBS4K14MJi7M6 zd2GYuyxg1itNt$0=w~CRuIL~(F(UaNtJ=!XrlPZmo(Z~+m&c3?w7oVTAIcf(a3>)q zWwo|AEDt)K{ls4GOMb04s$x~x>N>}x3*s!^lN;-41#@C*%|EHweO&9j2at_t{T#eC zBlCW0x;e$vmJebDen)t5++*JE!QMH#a%d->Eo)7fx)JaY1?F4`KToHA2gy>&o9hvhLCe{q#tvj}a z1w@aRZ`@mY&inepFKcdH(z~>*WwVxk(m$E8Onx`EVdtR=^Vh4w>m~0n+KU>}rXOa@ zz!J>)Ht!@;Hilv`+Za(8KonFc_nM1Y4{<_{>i@zVe?&b3@*Z@)0OW&G&lk-Q_c-;o zf&mk8d2lwROUxy;$Hi= z(CT^j20_0|@2szV)^`A>vGoE3Id2wsSawoPO|1@PEce9Zf0XH>jE@dq%h;KW^x{vyivq>!hxJFIey}B+nwKs(zNc& z&)J(~uN!7N|IOOWttx3Q#7u3q^g{YECIfJ=qZ zS^z=;At9j@L&EZ>eG|8N^)Z!lhqGb02jgQ{)dFe#b*F!eAUJ9VsED$;!!6cTCv}Rn z(TV`(g6awesKHeg3H#_9WPaXny4&;OA*o+P%Tw{=ThquP#qA{jI3LvgBe|5SO>)SKNY9pvd) zHyB13R-83qre0Cg-6$2SRw8aG4}Y;UUNks&nu8RKu;ftvfYJNoc?dS|%A z^z&Ef9?o}upStx;q~v#zkdg)f4hA++i}sQ4QOjs10=|VRzCdC*pG3d>a_?*>614E>{@OTg`SV!dPBd-F~ZOWTZAG z4I#5Ixj@lZ+#2q@Ri_bi<_xc-MpP9~(kUoS_j4zMQJuRXWehF@|9(h=)MgqJiduXL101K4Sp& zfSz*Lf>&GiG)w&DEyA6?0TrOT0)KgBQIYz)WazBklX@G>$+QhmDIC{wJAO{nkCo(s zdL$UOci+X86=i;<@>I(=>l*uc6k)bRQuS?NnDS&`#78SY0@ybri#}lbn}n#NmcjS8 z{`2#j?*IX*D}xMkAX_$x?El~+joeyNS>9LpHz6|Ap>7_l5%w5bFWLR@$7JE(6d3lS z6}(lbbBl2Ho3wD=10!8aM@Np*E~pg&RX5DbKu8P0 zsyD!Bd9`*zr=9XDgO4tX?C0AHvf_n428(2K-%I$4fTV&l7BhAozCXeeQRngW?sZkF zr!|IAw0E%j`}=|C=~^gBt8Yr7QCQ1<&$h(tX14s72-MUoSM>kgO1)_NtSM8< zpK>EP?QDSL4pISA^G4Vc*48-ufKwu@fG}&SrBhtYnS*Ag5%({Xj(Xj1e*e4D#LF<~ zdQm^SCFjTz_)0{7|KvdXf2SrMMY`yIO6VMTC}ZD8$1Rwy^8fkgKx1WXt_DJ6q3eV+ zmjH0imX$>#%mUMMrU$KYto1MzTak-M|>C0GpuK~=!41%cve1K1K48+*i@ z2*f1t+oc~F8*5v3$pKa`oSDZPRfD#E&_Fw7x?p>Ovj@CLFvu?6S(EK~NR%6vhDrOtYD$bC||u z4kX3}N$X!NTxCt4zQz;0t@!=mQ^DEeA<^^z8dCS%D8iJ+@q2B5@HT`OE8Gbrt-pQe&duG2z^R8veqFaHnM|@hBCNXqb88v> zMAedrU*`TqfhMD>`jb~{AF7`x_9!%yuivbc?r|imds6gEu0;2&n(sq(f=loQGh=oe zVR>C$YANSX4#nQOY^j${>@=~*vMx7G{rG7FCX@Bi*GG*kRJ^43(H-{YsutpTP4ax? z$laq({*Mdr84iW!?7w+8CW121q-~zuXG);O$#BK%I1SC9b5}>~&^2ldYF1Xk$OUlQV zenA7b{e0@62L3OWn|PiH?&3 zvvD|*1A!;+Kj!x6krEk2lZob7iZLUdutP8t&fPP*A=2z8ylR;c3Z3FF(d~ANQva=E zET_kECzH+7Zv&p${!@8hA5rm3d1a>(WXNj9(_kPgG1bH{@#J`w<}g_`ZGuv>ZtlPo zWNx&mVC+$Vgq-!=fzMu}6Ylup5W4OklPdUq<=b(bn_d_b3Y`%VNAdPQefs9;cQ$or zbOqdhqChI}Xx}(HRPP_v#HRa0e3H5SYvS7IKzD&D89aCw7ZKOphXiX43<>LaS8`%*X3k!N6Q_JES4ww1@W^z(ec11uHO^XndO=T!aZ!9CJVCK% zx#ck?UpA%|o-;XV*q0?e%Q=-sa}y5%hKVE^^=f3j3=K0|@_#DF$fk%4Y}|cn3`=G+ ztxMXDJxPL+Ff+$}Da1LGr81@YM;W{Znf7F5l#!YKTTeTrNtjt*@~=kN>o}*5dr8|d zQ>h3vG^WHMKfd zx99?W$sIeeKcDTMUQVC6&9yIBTXI*BtF4C(Z z%uZvq$o|v0K#i3@e{|Nfpfm^iG+f&!B>12@y2h0Cn0H75JMpc0=>sW&nnzl8ksX0B zl7+iH2j-Vhl(+1`?Vnp-uC@OKQ_Oe(XC4x7UY8ivLLT}xAb+FqKPH@{CH67B2M-AQ zx4J$bja$T;!z@hJbPxF!p+aBrWyYBp_cNiC%vXJ{jN}#j?1Dhjs=c;B+S!%*yQVAn zQ1L)X^z@G!!%PiF(@lNXA%!7W15qU{h&ykL3tpc#AaXJ2Sjr~H0HMjZ&4oGrZvQ!Z_7;Aie{e7cO|&Af>DP2a-}1uG2GqXUOX`cmQm3S$M zc4Y(-^YN1>GkmxAbh%B+sDU*vWsoV?!!bfXiff-0$Rtzn6Z8Ifx4~Lizv$BjP1n0$ zpER3PKky}V4fL;jNc#(sU0i)P`*AE24yy0~=A^=+2O7>e76N(`1ZV^b{4ixva#G`4 zZ-^(jVTk(kr#6H{!O{ih852w#z+eP!7ZMD#S529n{zyI#cnxn}g6>SSGIl{%>Q;|@ zW%SY0Xh{yGxU8qHzjQwFEaDtyj{iK;!2H3)Z$ECAn%z*^^&$o5uX7!)GF>$Zad9$1W_w%lphH1*V88@(-e1-J%UusI_}64|Mi-O!i=4yGS(Z}yt#>-N z!#AZx)$)Ghv4}Ho%Cp;geVIFCBV8?ZO8|IJnc}+mD7`ZN1^i3h1OBr zv=7_J*Pzz7Y=P~6GOy?&B%UNUabA{jwQG*aY#sRTUwjomw1*BRNn#b%I>KldGio63 z@78lCksByz<|$V4z+7QLCRu;`$V1}MKqjW!w-dfHb23ddg++!#Mg?#Td>C}V!hx46 za?tUxMBM4slkoR#*5oh5GJqi~4}#OX*9p}*5;f@;802B71}X_~P*Yt!c$M034Uc)! z+J-Tzr>;Yp&z(1~xNSbW@>P>bmM80aJI}CUE-CO6u19xrdIx%hXYF-4OLW&0pWq{mZ*d}y z<7AMF6jiraL0HYj!`H#to08M+5V{%3ANS@>D72s2C!lQ8($^0IK1DOdKMYG><&@iB z2M482anfkU_;sxh4N*zc(BIU!vUK$@Fk2Qusr7Vrf>+b1*5N)_8K`rRgAALLjxG|! zw&RY*_W1zv;sbdU-gI+4>uh-A#=&=%kuzc=6J9q-ad(CdpI{k#{`k{#Q4`%tccf2+ z*E`38s<>WdcA=y5S*_BF(gZY@b#7rsm#eaCo5@@6R!YB_38W9N5dF~0j-|Nwe&BAZbuh~J12*53@J{A@h zM7nR7o5+p32rx-D3&C9{%u)MnUeLy3P}D2l)8wnSKe%kWPD@^YTGMYKSJ+ z%EY!eFQ=eu3W3?m@VwK|qC;)k7g16WPqT1;!h~+d@Cu{Sa^Y;gnH+$Zd5{qwv1Z|* zMz}8Z3P<>FvmhQtw&3URh9$3YUrj=4>MAEAt+>i##|nMGE9IXwf?(vSn5XQjGOpv@ z>&_4v1D;aX5OMMG0U*}|3HLYaDrO!YeE5zr5vojS{nm`sa`0I!w6W8FUoBlKfZ?f+ z$Vm07%h7wSu=|3@h!Nsy{+`KjQYoELttYYqrD8fq5qH96E_(YPa9x$1NdEjeD%YC8 zj10EW>~0w}5IB=pzm?XH71SzoB!zZ%X6zHAgDS8G%t2CHVf2*;wJ%Qs0ewf{6t|NV z8`qP0dV0=~rQaE9WAe1+y>%;pEpK_Uu(UM)p#Ap<9Rb!eQ8t{w*agSiZ{ZVOo2z!Q zubnEy*8mg^@J}r0?GlifJNL8ta=@m$uuJ0uG$0*1Yy9T}`uCP#$WoBb25+SdA zO+u|vkVr;{d`^Z>jM$%l0`M(#nkaCg#}XA`4gw%J7R@X4;bF(c#UTJ5gNFu)pV?&f z(?_>hh|5~So6NT&5_))^>5S53JdM>aM9`1kvqxCT4SAoHcsQ-d+*Xb^=Kbw=9BCf7?!JQL3sMQ~W)Ky7PNgwt?D7~Vo_bw*D zdGkMsn3az=Il zg6~ySg-n=JKDF^IH{PSwJpQX`6}^tTj;CwB9kEQ{bt^6;oI*^#8<4r|m5`)Gi>!mY zXzrhTNDi;^TFqJjg<9Pm@HZykaGQD@+4LKWT`}%|MWL1x+M;(DDI2EA$sEL}+~-5n zPcLE0N3Jb>Xo-o|`#4S`>>YJoFeHi?sdn3zorB}ls_`%P_~nN`Z>1gXUKa#R1BNq%btqwleK&u<7*wkNYTw^sfdFZ~8%;RqJi)a;# z$*Rw9{rWt8aa&7kMr*r9(&u^i!OL5iwO_SlZ=aF6-n&LO(VSx(6fl&A#0a?-CL3x+ za)xUoWsb1EWzwiW?Wy-)DXrx+5ct>|*Wt?=2OxKQyvjeDz|{>UhKKt9V&HARwffuJ zm~=w+JK|&HgN zd={W{28V>83}^sI!@*w{po$F-mjw!v@97$+rKKfGk)2ne+Rr>CebYiHO)qX5cqbzvHo%dB2D}9_T1glgV}U4eL&}>5VJrD{8}<>LeN0kP3}P8XgoK3u zAJK})^1Gx3ts<)Af#f<;z{kVFaA4$148%;Jw%qsi^&MI##j%vYDDKmF(SFo=Np)8G z>)F~XwE%sTZ7!U13uoKSU9Cab>|J-={yy?R+t*Z;Q?ujwtC2x$wP9F71uqj~QWgq7 zfO5jJgyaEKX$7aD_Dj;k7l6k$6N|1KyaIC|u21QaHx0psD1Yw(y0Di7!^TVlHCzTX zQr;quxGABE1)XIC>@WpIMdbGHCauCpVbRg|_4R285kAm+09glRiUGRv+%s-oUIJiQ zL8?_J{mbb9IA+bkA`*V(Vy4_`=sT{No3DY}Yl|zD-_7KkDbt zm|b?x9;i!Y2ZgcPG1YWIz3uHS0jVkLcla%d)%sPq&NM#FnoTj~G2G;L(57$aIa zmQ30-r)^|5fAhIc1zsgX~ z5Lqrye7v}@{{6|w^Vwe8A8tQ2Lw&|CZbi0!?eCa}=o}J$MOlurT!a}=y8R(6d?YDL z0zlH!&+IImxRXz-Cd{DK+rYY6g!OOC@;0iU1wRWE1gw}F0GA+qWit+;x<&9yfdw0t zR-;|K_tUC~rCZ;zJR}NP-%!vw*tQT{ zzl#$cjXGI^H45b6YS{XE=75R;kvb~@e9w>sv!OGflRn~cpmElcarse$S=DKe6<<<9 z{T*G!))8V%oiR%Riv`)gu0l;7n)(tMPkBC7MVlRJ=&j`O9C#((k0WFK|1GlCTqtxj z=jr{yt!)mqtWa?`pUrF|5PrvPjOWTs*)LLnj?<0h0$xXt3YQjx!UFoIDL~* z$)wq2_;FMCsEptFr}Nyp^%`CXM&JmlZm_R#~Ua&xh zDITNsJ;Ex7)X3{nKie*maHMyvzpt+yl*VjRHdcSfYWcq!HH?O0kgcb&zeJ#XOS)iT z$pvYW>j}7@LuaU4^egmyu6GBI-mfHnkFAJCoa#0iK`j<}G+>{TMn5x?`bAGE` zi+)htueF3r+1mZ{Cyw>DYch?vY1qAx|Jx8^0if);A>xDpvl^nc7cd8dY0tiWB!47W z9c{xx$$J_*h`qG3Vgccq9-z40{aaM#Za3Bb>^?!Tf?3I9#BrXVjVuH5nxt)I9>Lj; z54;3|{_R%`Ss4Ik5W7(~i9&-6RVgVbhEh49Z3nOluxz-aft=vtZ9n^jnaE%{7(?;i z^=SB7!EZnwMhtGXb7%3adg%qoYq)_jW#Sd%av)w(fN}uf!oquT9mt%$v0uAr0V}Ez zwyd&c2DdfURR}4j-(ZB?;^xLvHtD(Piu4%-WQthonCapIW^_9EOk^18Wo<^>9bH{# z2OZc;R%6lhWxt+G=EA!QC+N6q7Qg zCr>hB>5`2zww!Ru*1P@#_2BdDOQ>;4zHC z^z|~Ib#9Z`))=B3L&9QW6k(3!A9Jh%u2Wo4ZdRCrggsjA5iq#ns%UO$bGdM(+DKiR zT$;JDY>Av=bI{i zEJx7L(?6(MCe^C|P}B5oxr?qY)#1iOP!_ zT}{te!+<(}z&JOcrIz-uWDch#-iOUC0VsKv#kPU|D$n+6N+ zL6Hm~K2($+i;CpIy#{U@kYhr2IC>PlI29BZP$!RG07eA)q}p0q{xF6*uII*+Qxcf= zIXfJ_RJo9`>z8}x1LyH-dg9y^5~un|pNBrP92XOLv1hS-hV!Anbei=8N!Um1^Ah|9 zx(yoiP0ac;^eNqPDlMe$6U%M%f|7ybdgA@GYEkg4yp<@kl4zxEgH$}Y`z0D@@{dG7 zNbg~o@8=Ea;U_a(bwk4@L3})>t#+mx^6L{R_u(t0=8a*HjDVEpEX{{LlD-~?21_>9 zEPmfGmeSO_fti<<>4E;>;J|m;E7vO(T6Vl>ih%;`s!~H203bv}-_WDWDeeTLscDgf z-K24;#5BAX+9o@roo;N;f<$ZoENIzH@`)tz;H6jwfJYxn&5^s3?MozS_<|w9ap(#R zbFLdKO(w)JN`R`V=Ai#5Gdv#r*MANgfIyUQ*VDgD_s?> z_xTLj)y)cIv@TO^6{YHfg#JhH)~m+W}4jgbv)_ zP)HSUSHZOmfY}@0Zt^f!YIYyUDwD?;Dm(eX`fnrXAl8AI)84J~JY4dBKLAnQAjdrcwV7>(vCC zkYeS_iM@bsiNdof2!FijBg$=nzxmbOa8^6^6nJ>&CFDH2!C&iCFKO*P~ekqpJ0 zwux61R--p6KhohPe62y5JX6>8PzS;t$tsyl@rCYp`_m6t2{c~aN^ z-GD#5^;r#Q+rtfMb}b+)d}J~rE|cxv7w`jsT=FpOT*VVo?_d%z1d{jhX(FzG^PJ0o z1qU6P8ZdYCDfLNFhXPY})^x&uyHzH_m9&Al;ze8L%9*9gtxSoy$bb&=mPYS$DbCW* z$R%tvy49~dILlW?Tt1j;r~fW`J1!EgH3;Pdb}I(1VRLh0o4nrQ823;!!pzAr3S*i& zGSeZx)5WsfEogGTVRR(%XW(4`DwO&( z%1-dC^TcDW#}E07_*QVE1PT^)iV1Q%;PzamRIvXYb*ZgwTX&4SJ39Kl;xkph{Ojy2 z#cRKWXS0`<-}wC9=YSz0vvSd1h!m1DWQIcNZU3aBe0_O$Y@UsQspw zvp12G6)24JjJtRUWM$aR((i$i?rzW4B+>8&adj*SHclvYc1drU@I)G*^S)m1IB^W# zcdsAl9PyHgf4rm0mR1qD65Llst_xle?xlU-opkRAX@Fu0=!| z;E@Ktaid-;d9D91bT(3^O9Hk8(38%p>3NqrhccGr%JcYR%{t*NU_TQl36TGbUCH&A zdw7JNN#W{k<($~;kQK|ShdjP2(Y?ij<(CT|#LM1yCDu^SzrI-@{AYl*1X>@woMrbL zXNM1q%OaIbkMxc=8s6deqUX&-yZL#MT~&^?#d$Gidi;5MKEHLtlI`&R{{2r?EIMjU z*fq~t1&%_R=WFz?0y!DK(7vr8@;={a^KhOS#<%l9t&r9rcDERxi+iVFe3(;+YYt?X z#!VrG!K~PE&poHx+<3d(lrg0yGd$r;2~rXx!hO77_F7AYWi$*{x>S=mV2G@A-4#>k zJ53gmvHd1S4y!zcCr@9erumh%mDNe3D967+Fe?4<;e!AT0bJHeODxXjc}&(P@(d$A zj=Cv=7M(C2kMB%ow&GAvMN~~h!KkXev-8`UE6c501DuypP_nw!)ZG(X z=Z_~|$cNDhzE>yWotIb60m|QTkJ@x$H-j_KvmE3s9jU0Pb5YglAjwX^zwmFGOT2L9 zsJRWwJ3JYG0bHS^N60fZ6%}ZL??br|Lv(qRdOcH5U?(sJF$65(E5aJ-M*mI z!D>fCWY$@^*zS@Tey{i=Esbm`=G+;2{LKDh(C|jKtofvN?bcIc@%6#+A?28w`MFP_ zPMds=+0`LWt-Exl#eA;A=6-FLRhjddUdD^h-EUV@;)@fJ^^>%q`pC@I20CtMGK67o z1yWn6)b9gH-kN(Xq2|CBB7TScAD4axdm`5TzC^PlHM#vKrV=m4jF(Kr$tOgG7c&b| za?1ZEk!*5moeuALER`BL%@o^Pu1rVGmY&zMetlp>=pRhw#E%xSaTICtVB^Fl3js+02@k+ zCpDQz@3hg1oekFxml^YrP>ON=8UnB^sEUYydoxJt5=kN%N<@!}d55^PkIS0Q&K0J` ztSSC;DOcGaG9fnFcw}`jsyezcVKLfij|VeBcI}w)<7?G zos>`(mFwHVyX`33TFa@VQ)zj9eYE4K-;Wyl(p!XeePn`cX2MU?>K+1vdf1}9Mzuuu z=d1P(luk}4E!q$d zr)!(ps`8P>A!Wr6!LH8rqftrk_ zkw~&ZgDJaHA9Gyn->Q$kJ63cfFR&^U9p-Z{%eKR>8jy=?_ANHPgZmV({++N6oo24{ zx5CzA=RD{dU*bAn)Aakscy+9F?%A5GJZchb_aJDe^=y0oa^lVWr+r!F2exfhgHm*^ z4ZnZz?<7x!P25ph_?J#Qq~{xcPh115w42#`Jo>md&2a%N>e>^qIhb{Sp|98sE`+~S zB^5s~k+@jtaxSW1Fl|1ixjgL`tPU+;^0D{3yY)NdqF_8cv7vTx#hU+cL&_@IT(zU(&lzI^j$XEx}$%(2WWKT(iZuNXx`Ue~6y z6h<62;+xZMl=OnBwQtpIBHaN_d~7hCV))c}V_l>Ty`}KKJzbP}a`BS(DC( zwXvl3N}SjMF@$dFVIaqPMQ0@)S+K_o;;pZI^zd3h_0*G-*$TH z_SW9)e!FH6#%mI2^LsMzG)~X-e?xU5-#iU3@oZgIpwcRYgyw4M(;%Z2tMA{>SzFbv z&JY=f+l)-%bCJlb&whiKcWbp0L*9u9_rSx_2Qs0LV|9x@MAW3JQKj{Swcmk^wGYG< zO0f(bP}mnP4A^Y{cvd586jedITx^A-#sEz(+&T|Cra;z-ky{F;EkJ&R*4b5zKKcp# z$3J^>4~m&F=AqXbom~1nU#`YT!oE;S2tBqaujI7VAi_oDkg@F0ax7n zMYb1x@=-1LmKm)^TID#3Yb2~>?WfkwdoGY21l2AK22U&rS=;HDHT2&qr{6@3{Nn`M z6U0ZWw@f%AA)zyx?LR`g`^p3Zem%Pn!vZkByFVj~ZlDNsXwC*2#hAV6!7lac-~TW`9=6kKOz8i6S#e zvj6@henarrBTj{uBvN3T%KOrQ_H^(SMWkb9V3aF){>%vlmDT6$)x>*Tj?pbc&K9Ib zP4rWm9Sak+KGc&Ix%0p4uRVT_Fg(n5Euj8ZSFu`v{)qZX1a9ITfmg>)QSah{LaJWp zez2$hUU)MaPrJt=?&*za?M~co5@hy2ht~(q8U$%aB3l(vZBFu7*1!F*^FR7^zW&x` zi%;$}lS_o(v3)QiPe`2*-0USYWsy z%a5LhR(!Ff(y|`%$y8c_kUnA&f*W%An=tx6yg?rxwmn&<9ub{ovO4vX^4EXs zirpRb(q__vl3s2LKrfIOKEzCGmta=F8k+x+41?8zpDN)@LRcC=fCc~@1ouwx-Q_ek zrWV;e-Pcj_$Pz~O^*~2Ln z@AUm3a)fyWke-jlFi-Ob-bEhDbC?jX5!wmqu`*ygf?o%-<=38f%PT8q`-3r02pa#5 zQc{x&vg_eBQW6&uAD1Pkreh#)8A!;=Q8>A8_WYi#f5C~+Vu`JpHV-5n#^_ zurr70i`qSVcD^KZJ^Fq7>(u1v@#Ezr@4_bb`zgvb@`oM*6Xkgep*;s3Z!CJTDs>Y6 za*mvdH(bXep0BQ4Vm`0K(7vg+I4@k~)1*7Rvb-a+ zxMOn|hP7&iawRwOmW)3l6XMemNh3Fu=#b@pvuM?iw!0pV-C2vAG8%p#(OyBm>EBm} z;;YT62k9TSYi2~eWAv5TAIK1KnR;$3;XzV@_%v1;@+lu{-jMHow4$NOhlD!XENrvn zrag9littX<#%0_W>NT_rl%UxgQ1l-%jgC;nPrO0bb%Zivs92E53`0 zzbuwhF7{RC;2E`IhrLihkM8R`LGvb$@%Os`8C3&pxtA4YD~8XA|5aLBTSs6i{aI#j zK9PKrn7Bwy@-Ae;{?Pg9!nG^lB8d%x3NwiYIdi4|M)9{#up8%MSNEabqepC_x-D6E zDa9RX44+Oj75!43;yeqAdNKU|zWx{2o4|*fHDPAbeN>E@3vp5*PiVb^=#&dL_|&7cCXnI-UtVN{nee; z-(@Ff3S5GI9&bc1zYvGiSuwMBy}k6~RwDT|GIAXKz@d0%`Fj4at(Jj-%Jw~*#S^(_ zO&^A)vV!1FKx;uwXdx>I74AOobliR&>`?|uDJerhxFp^^YFM#dR94=wOXB~@#pJfpOIP?vt>b0xx5&jYp_m)7@u zxpvJ)D*8pzEdy0>unoWOY_FKKPDWbKZ_Um|s*!)2H@OnaJYkV}xw&bp*J1XpsSJ5( z>rVS9ER5dimH8td0kihx)H@5ReVG}tE55Z9epf?b%f!)?j>2&gIzP#ZCE_id&mv#W z-G0D!+RkAngEZY2wbC1PUv2g{InkWUaN%TVw7{s=KH%~_x>+2T5#|&edTsOhbHwEE zZ>(VAfz~^bd^9cJKk2YC&!0a|N44x*0dBk3FUBpL8g~KkJ0}x-{8&Z~-Ee9$`%{^c z9g^j5zD9>tth7e&5yx10CNPzvg}S~e~Pn37<8;^Jgu%ZA9D zKqj;4I_F1kt*2Pr_FqW?hc9*+^DS{^pLz0gL4$D`mVoYbneVa!5ol{~^gPh(58j+dnSur8#zxqOLl4LfKC6z>A94REF^sJb928h zH8`*PK=JTWWSv`Cnf-GVPz8X$2WsI9Wo;@b^V@&~S?)A1$7}RC>U)2$x-zre5=k#3 zQdY@$b^OpEA(=jO+*oCkKP5YP%Fm(JXe1$;Q2D`O{rMtJItV1lKoEdu9nrd8gi zF3%HAEVrMu%4{&Sl-;-KL=RS+49^kA<*N96?3{3hd)E9xymd?XFcoIs(BMCKknCo( zwc(8+SH7g{$PwW0pvss95@xW(*7$c0!5M;1b1ie%?M{K3L&VH}MB7+liJLMsxbCs^ z$z$W{l;?aJeSa;@KlQWxM-kp04CYUo6qD=gEuFMP%^qAjx$EWYT}|1z+f$lmE3sZf zrF4NKG?dv9vYp9`Bo%w&OMZ{TzR!~j+%jodesj<<5PhPrx7yy)V@-gIJ*m0T#-K-| z*ndu#pHCblz(v`1u#3hibiVKi=NbPYHhi{&1x;ZidfiLjw8?&5huD5MB5ECYH zuVuKX`Fu&gk-7C@qf?17Yma!=9v%u3@wfkTb9}q0U8#$XnL@8zl7A zaDI~o=!?bE$OSUN`XYjHZo+-&2Nh&O(+HUXwZTq~uI9%nX)SeJkmO$!s6&5F zkY@XUSz$Gydki(7VKB@s02>`%{|rnqAd><|LbnK7WkTRQ3jg+CO^zir)c6zmgwBLW z5dWTT$^cjXhGZyxVp3rL;2p;8uw>Z?5vGc^yh%&udmN(|$=Kp=T%LC`KN4oH7YsJk z`&6Q(LQH0=!JV(mR4%)Ha`x;cv$%Mg^>5azMHrC+tOY#8)eAWSNm=v^O&n~JAR@W2 zi{Lw10*E^+{b>b>&oB_+yH1p$p>&6!JPR_jSYQOn70_sO6+PB$-*%69I-KX>S*~fb zZ-LZBPOK=w6gAzj2+SG?wLcmkVUAw8-VpR*oxkV^X_^v3?vc`0TE{{yOZonpYW&qN zp}JhRRj+>}zAhedPHXykpB-EMq%N?|+Rm6uORkImoBcLr`itcvCRD&nnc$HYGq)f* z_CG!I{zGyapSGq{-=}xUkTK>Lp;_j8{5QM59#KU_>&a411pBOyng7hOX5;91U-CPM zwE+esTl%SvsRTay^#6~iuMCL#UD`%KL`1qlx}~K-P+CbP1nCxmrAxZIq>&IokdTya z=@wB!xzzI4JTG7Vp~AX*|L&Q&<_ao9Z~yEwm39|GE(zscnLlyAIlKQf*_Agw z3w)JCDsaoUdz_O?2DcgAL3w(A)KF5+Dw=ezVcDK$E|Ak&%Sbba63zFJa*%qO=Jn`) z*K&D=IR0yX;qf*tFS4S%t5=c@lwvWTc-_CLVaFxN-~6#_5c-qpwNk_4Q%w3Av!Wwu z*0XuH$@T3}e{an_>ay-EWy9Pr-c%NrcY`tLokrKXYc5=b#a~faG8s>Dnq z#lk_4+Q6nB_h#gwLi4Uf-kh&w$c(S#TTYUo9m+dWciAM^4$u$;hC)OsImvLOg>!FjJ7A z`oKLA$4*NQi0gwT@jtIm%VQhP$Y`jfQCR5%I}J%Gf{h=@J&~L9-TBz^avm6gk<>0I zE&YjX$cO^l&`jKfz98vPoKd51jc&X7Yzvf>lt@QnV8*fA0YeCA8lb2H-<-w6H~7GA z1ESXRn_WPdgW~kT&M8_hcVwn~VLfmJm9Zvl5REpo2LW8HO z%5tO|w9d~!dbplFOX+C+4*Yk37V5G&{v_sI@8dPyf-yQ^k@vf9S5JIU&6WUlZUG-S zKrN+Pmp+C>2Y6zDgZ6YYa)>SML2OSTgcW7i8=sSQ00Yh&Ri(NDja+VBLrG zx&wkB>}Zj+D`a$mq^|_5(m=iBzdc<$P&1})J$KK2C0_nbQ*6q$q^BVbO_xj7xdQDs zEIMoB;JCt-$Ep2fb^YD0w->v~rfs)HKPLz_4>en*=L!+9h#&!lz-A8%`htCJk_gA@T(8?*>QfCdjWxoCX%?kMy_TUGOiUJ9F;h=6d-T3|y zDG}iGMLeNJd37~6u`lI=>3cT1%1X?CL4IJd_<7we*UJ7j!q<>f%Rr3u;i%z4ftXLk zx=#gZtLHfuhmJw0z^;D8{l9+~3L7X6RGT75UANmx4V$7m3I-}1hg|B;8bmn`oQ-#P zEI~wXxKxXFr((_jI!3bf@F~xeJN>-1Zn#HO9;AX2R8ogq;wzqIZH4)k_`1$Q)b#^0 z4%B`Zd_Pi|etWEXT%5>{EU=7RQMF3&#r*B^nW=OYA^8+WJLqS3`dV*YBxJmMcC!A! za&qi~qG5!ivtMAs_$p1pxu!KL#o$%jj(+3*MI`rfbo;5h+ljHb<8h-}Mj4sm9!ZH$ zN~ZYDI>!xq?8_b5t+{>m3$OWIhe~I1giZw}vEbj6zxsHOUvP9a_NyBEF5`$F48~S8 zF3Mi`xJwiz-`u;RBr#5Az~{$3m5p>*j`sCiX*bMWED) ztY;OpxS>aa3#ZG9d-A)VJFzDk6{V%68HC8l$=jUjs{0||mte!i0hM>4@VRaJxl#F=;EvOPOJJ$=49#IFp6mj^_Kl-aWl z9?b?o?5Vs`c=FJ7i|p+D93K;%2O>8hA3hl?tJ1&u-*-i&QkW%>MD*q5W$h++UIU*~ z^YI?ged&0@YeL5CAzCm0lO&4F)^oC^Pa%wOs02PGu*VpvW2+pZV`CN9LC|mUMW{=3 zNpIgfyhm1JI6Lt8q^##+YTf>5ON8o=m*UH;k>nacB%Ga{d910$zRHMSLAKlyWZ|(f z#tCCwJ&3pd;p^)wi9DRZc>UiF{ek6h=d@jGbZIG^VCPy z+F&9K?l_T^hA>sWwpAMIM2<&SS3YuWdBqven=*maGcBl*hv|)UaDdc38;5$f`}a*5 zp8#P`B~vsQ@B*No2k8@r%bItIW7;cJ4pCFY`Vs$i*|Wk*?*$F)b@4y1n$X z`9QLTP>6DgQU+TC1zSA-5x2V>@OfZ7tSyVLvXARLj%3-OdURQ zf#EPG8%2`>naMy5G^rpHM9hT2hdj$aMRFHSO7`YjY; z_kWBHDj8bIOx9TnjP#lJ#p?UKpAg%mg_0FurqU{bDS1Of!(;PldbapNIC}(0*Wd?v4m3S5Qte>jlVv~ubiAVI7C%=gfVmgHHXvt?9vBxwM1an z!VheS(Xp|PqX+hMzr|>>*%LN3w36*Y$Nk57rae=HV_nbRrl<}sSPJ22in+S*-P|VD z5!*0Wc3oDoR8kLDTJ2wTA1^bE9UXo4rM7k%U~c4EW5CA^j252zbmWW8XwRG9DVw0Y zxrgUhg(6qqT6F>UGoI|P>$&5WQ21j=@)$U5Bl}_;YVk0Tc>sx%2zmZp{t$Kd^i+V_ zE<@M>wI`OQr$F*@<4HC;kk#+rjqdvj;8&ay^O)8V#HNAWJK1e?Js+8GAOC$5{8_G^ z3~#qOj(NoWn?G+pe_-`&+IhH8wH7P!)6CwDfb+96Xlb-*4S4NcWMzYyz7>4>U6jZS zaXo;u0be}Ro8>qiUWyzK=#?L&&^2CK6g2&6xB9%LJljA2e_Vh+VVyS&3mmVg(C5#o zox5;zYOncmoU@dy)*&RU6Lw|gw$p<0BC&YS0tJb&*nCgqMNC%Mc;(!R5sTh*0Dx$f>Q z8ZW$u4d4v_{jkFVTJFL2Msw<4E0Y zc8;%H%jx>luHcyNYRy3H=J}a0KI87p(I!9c*h4~1u0@M${nzgDc#jc05)#9MYx8}D zjsl?R;ODPwUzI~|Cgh#M{Uq-22)Dutg9CMMFD3>C#&eDk zg>a@w>a4*}@6~LBOX@?< z8q*LSe6!W1?ys^KStaisZV*mYn6Z5~&%3$(nEXlKNapKpjnz}@fPk>E)G9hs6t{78 zL?yHBToXAO8k)`Q$tik(VH%8n<-XV3?ateK>b%k$RPymW zyZ`0DLJ7U*dHfnhulQbh0@WCC+DxsZfZV=8mBX?evH=YY46NJ8f42vEVBXCU#b~N@ z6&W>^%8~G+F49y)(r|hyhACxTq8A*auiWW0PbmfTnB5Ioyb6S2M zCM40mOwAAX_rDQY5@eHz;n4cm6u(FIQuwgo7jDlm(FZtW{ZH55U!e(AEsNh~Z1mCC zuBY?wZ16hgm+;Z6(8j*6RbsSn{^?2@_hC?RX+!_p{b3$#cse_FkC8TjD9lH6)kDX! zT6`*%=z)(1-1?bUj?ZB_t$-jK0E`suU@}b}>2knqtZJ zBAA_>1?FX-v67iQ!}BkVjgKgqE{$>HZU+4UJ8c%E_#wk%1;Hj#iF#ENXX729Ote+FAm*o8zBNdHvlz%(nMwCm316T+Fv4 z3pd@iCgY_7E{5hW?gAy&WQs6PO+FXqxKvGK3l0!GVGA&w_Viy6-M-ewF_29WQh~NHYQx zUTBJzx+(3Q*x)yc(|5G`o^gYu{?>x+pe4com(guXZmvjt1uuS9i^^B&Ns z9KYGmqQ&?<^2Jc|UV-3mp?Ejh<{vzndL8`udH-o-9LP-@@E?y|(oN$aXo9SwTvtz> zym<5qL=&SUUy#^A-`ceI_0E=mRwYq*`>X9Rlt$CGo14*~jljNpH>bGx4p1+Uiy>z%y*K%vu;{>qSG2I0{zBn>#yuL-qQS z7;YP*--E12)iepo%aRHA3G45fqmJ=I6gh{3o5p*06%>y1xNbR}<}g-*Cseg7fgF20 zG?yb2O zpGWM=Qh7SN@MO?g;Wj>EKo#SeIB{xJs-yTT1Lnr;$h*qu|986XKpJ4=zqY&FT;A>Aua) zaSe5^2tIg=?RxlN=*udJ_+oj<9M(+4<5(f+rc8K8o_<|8{hZuJsO(m-_7 zd@kk=XF7kg9n2+>)_J>UcpkuisT#?{b8~gAd^A8r8kUG)Umt(-B&d!X9P9a#yWpgv z_#FaSB9`>^B7e!?;N|5p zRkyXYo97fgDzUzq=NX-k)>uEXQu{r+#4Cyi1!AKsqNhH_>%O8_fZhGUl@MkE#t2#4jA06$Bgoiumwy^7~)aAZA7Rg@kn1&{1F_=x)y? z+*nyB&1kPI@<%oT|=uJ&cU6Z6PNd0rt%+udcddgu`R1Jeg zk^3k0N^5Z`)6d=H$y>B6QnFtkQ*nQct=)WQ4>p&Tm)rSG*caZjcqMPYSExN*cYCtM zGTUNiPe1ALHu>B4?BSU-V0KJm z`@ZWre-0A^D3blPIYJ;~9I0agZ8#bVWHaK~+S)1^{OV~!{T(ui$EmxukZ9YFv*d;M zb&&@|o1aU@T$cDLj@^vmE`%$Q_Md?ym=0>c_Rx9v!I8bRK*_=ZXNw~@4DY?GT(@^GudLiaKu`43T1(aC+}w6DTMa9hqEl@OggI%(w=SIKSsrz**I zi0A&Cq4B30J*6can)P+pGG8@ZV@fr%_&-4MY+{+Bxl~SE@Q>v(whkUB z7~Bxf>cJQoVKWdC^`xMb@Yy!4O{z(CK zeOTABL;jb@9nq#O=L{n4pVM})0ZUne z@$?g~xd7~Lxy^D6Q=4qM5<>K&K1=n}|ODD3lN4*1`g!LdNx{fa*VCIYf` zQ!TZbfWoh_KK~d{#x{qj`j%vBJ`IlrmQRymUc4KFxizvYsDvgdDSTQau`8sqc%acX z{v6BR|I^!tO6nx+WBe81Kl$(I=%s>ug&n}ui;Kp9QW-fr6M?E$*nN+d(i%XDD*l{X zZz|0Q*L4IXu2S74NO>i<=KdaF^@6)kC=w8=dKP~8Dms$%r_m9y#J{GwuQQz*orK=5 zx!(#l)BAQs97o{VvAO;(fuCemVffYw1nBVo!}MOeW!31{2fav?_6p~^#%8Bsg3Qt$ zmx0&ApArklR6NGyKYsm6hpNCeS!W{-Mph`ym~;(VQ3oXH|8%}IB#x& zhD&P+AdVCp@&YDtjqIkF&%GTL`+Ilhzi{oJ1Pg0BN4a_DZ9Z|9PpZyjo1$arV5UTj zN6yPHWxd_r+`v1Rm!ofp4j0bMlC6}bb{^;XUjZgb6i_;9>!xX3juAr&j#0+Ar^hwg zx2H$-YT1(Gf7WZ0&$-9xg&4nYXrQ=v?_P3=0T_;PbctmfWXSW7b^iYSiAhBXx^-(Y zJWS-SFW+hj+7aNjN$Kjo>%wAVc;1Z|ETEA#hJR% zZwN12-U??x@q@CfKOjtAKER%WV5aThu}5~6X_i^Whe~5I&?69p2RX-_NMJ8ZX;nHe zBvNF}Z(dvx^!UtsALo>nakETKZ?%m`=Wozed6I~Bvu!i{flnsI!qv-bb(t4@0|ZxOedMt{$m`Wz9C7<%>ihSv0msRS zi72UR8BhWYYycHQ`8GQ{#6b zrf4!-rK5_LoXX)19usy_5;?x&WSh8QZ&v+ca-r23?Qi(Kj*(@{2d8R!dRwmq@_wbH zT}b5XTJre@o(ecv6R+eq_m%jvNhG|A{BMhUH`?k>g!dym(Mumm&pWS4%bzFXkfe%d zpA}+pq$w+*T^axS{DCu&T`ts^?Dy~A<*bGq!*{A#idoeEnIBrk0G@{&8Q8WP8yZrB zA9;H}+$;lkP4I@@64T)UNV)Q1uP)KEdLy1N(0n4nAQWiwW8h*Opge`Q>g05in!R50 zU}3FkHlzOLX93mUc=6^eXz@3Qq%jmEzg4}fG?8O$d&8-g-PB|b)N{w@nNwg6@NouhPBo)UeI z>O2(~*46t3{Vpv*IP1VOO2+I|{5y5!28ah_4gSVtuWW3-psUsd4!)KXDEK$Mj9&2X zuef_8-ya6JDaKRg6TbAo;Q!)9ih1)Vs@7vohqReqo7!8<3FuFm6L_#0SQzn)k~C{3 z#tKI!T-b9Lsnapvgc@(XVrV(Qjs1}0xn&ah0XacB+zy~_@L-~&h|Xu7`fyvaIW6Jg zBhiN%zi!eK(+!;DQOMV*Rma~!efqNoQYWic_YgGnQ*@lhI|u3(u=DfJ;2egm5Pgn&&_kJxu$*x3*-& zCE^Q1N62pG<$rX0An23*cSo-BU}G>1YxETnXbGawmx!O@J0rk12CA)`latf0{&X89 z*M&Y=u@4GS8}#|XD$EIcr(V#uw-Z8;*mnyKk`82~o}RrY03_dQFU1R`NnXkD0a{sJ z0Syv~Zc$ZL!lAr3km2qWDto@g90UQw%e5l#afctM-RNrD?^Tzk`llF1(fWw+1IpMQ z{e%aAoGsduAyJziR#O_u>n3Yr8lzvIXApG#1M@vwJJLmh8WWkim0E|-18#k8{3F~N zy79og32-D1F7kPdq0g^~SJz%DuNx%YH9L@6elJ-ORfpKXcw2D#{=B6`S;#)UM{j3i z;6@4?K7WjVjD`5|(yqfVM=wlnpEWs+vNNh997D#@%Fd~n?8R)Fb6%W(v4%F!{?iF? z(S1Wp{InH;t0j>=Lg1d%KSt?9o)Jc^Nnu4k(;&IJ6rDdA^7NT%Xz$zV?IZtGZXq6? zwqNmmOza{b1~-Odm3ZF^Zfr~TIfAo_R%oJw0rTifaJ5g1jt*luYl2b-Xwkoa|BU+H zGC{ip#tCL-@k-PNT5})57UaV5i5;h0Xul4>R>T<9>_sTU&;l(nga6iw+G9CBvk$_f z!sZk7swi=Xr1<`c+lpV<|DAfdZ+uMA4&kkY#x`L@xSLSOtc&*L7M{CfFs8iG*XQ8@ zk&!&u9VR)lm4W~>f+(K}5%));JvKi391QK{IDo?_!ai&AE=YkfV5-XUfk&8)0CtBI zQG!cm#HSbd0*K=fC4(!9+D6>M_YEW&E9-r-XIlrSc@yDJNyw*^ui19UnmO=C#_CQ* z96X~fq)__CdpC|dhnfvJ82Xtp0^QJH#@V^{BJj7#p}*_oUsx&HqMb@b|LO$OXMbCD zarj+|u>-_`YNcuGU$3A18kuYRmgk(xC0D*{P1CVf*5oGGEX~$q7sB^6Sv^PA_y0~&9Toov%2^~p&@>T-2mNc0eSvZs$}-w) z+0ed2rzKvH&QL3pT*NK3gKr{IAhE57Q|Uo?-_u&zT9pweUJ^9AB?u5Xzwz{}@VR^g zOsD+b^GhH2%xD3aG&5sl5)f2XlgoW(!KU%@W&cWemk9~|bH)d^o2tXyl`zC^O3MX& z8keb&aNReNNyF|idK?gLM!0G(E#BAvX>NGnR~&wf>p`t)502t=h;g}%Lx?f*FhB6H3}y$(yclihJms{B_Rgy*u~! zDO0PG!Aolvx0h4YBSb|Hl@u5WVz}`%I-7ofl_RlUs(y5BHABP_efEOxokOH!B%dYL z{_(r_FCWydDDqRtf&!y4)>6z>ss9entnRrFtlE_o?Q5L z8oT_W=;@m^>`XDFYOC*b$PiEI@cHk8g!#sXxI5f|iplrOCd6(N^ewJE9^1L=R7%71 zz=Lq0Bl`Iob4w7%r*eJYN-t z!93S%+rAU3N;ul%3w*7Gzno$Msv_-L(c zU~7<2>4oss>aCH5G#W2L-{-S2Jy$EL`*ycj20goP5eSPskJ9wLbUWFby8im2)faPp zgm!<-Zef18c(1SZ9FP64%7+w%a;R<#Es|eZTr)au0TJzF#XDY?zO9Z zz()#CC(5g>;nwz%ZmE;Tp6qVi)>EF>k)gc(VYH+N1^=ml{xdJzq1#v^VNCxqrt6K* zXD)tEXQrhh`Js|Zw{+g096|j61v#Ei)Zb*g;;y+pt2U1ovkWPJitb0mS*62R0R1p2 z+&ce_X5aV6_43mpg9TlszyoDMp;>1_By$}Mb)kjkxqtKdxJQbNW(nal=70oQzXVD} z(en-A%6SXim2vXU<_x|V5B1+iIwu{gHo_ZnKO5oaOw{@O`qvUu(u9bTKl~%m= z4+JAYMI5&mxlXxlqreO}7g#I0Mi1=Vq#vaWm$6?PBnKlcw%hng*!;MouM*=;WSSr` zN~@)|mee)wb48JAZNGPZ_zjsQIakBP%Om`KG(Vl9zCQ==n^~uKov-^>cWs#Q&WXzy zsHZM0Ug=TvLWAk^_imE|AC2PWglTDzMtjC)MWqFX%H`FuupEa>3+_OGb;a5m?fCrp z&5J!x+$CHCPwaUg1Pa;i;}u=Wws2e@eBJByOw6W6lG=wCtP)O}JQUeefeBH2y*)HP z=3;c`bwk^3Nom*9()1dJQA&Qn+0{KnG@a+a@%p0I}-p*tG=Hp_BN*WNc~ znvNJwe&l9%Q#3`a`wAHQ^N;(YvOJ48?Y_0&o85Bf{QA)Fn=?h*l&jZncDC$(NbA?0 zY1Z2S|aWEhdcG>-3hX%Pkd$^H)l@zv~u7n5Z1# z+BQ3_uKC)Kx=5(*LPX~sq91kVdxh9fMV7jndo$y!6WxeRg>dE%`4F!Fa|NKfFzdWr9A%6ep%S&($^7Gk5Zd7vkZB z!f)@^`>KJ!;BReWihHV3yU8+bTGW6ze$Xc~@kH=A3Ybc?@e1$D%5agiqxSk>KnaYI z*qbsTFdE=TkgR5v3@c=TvyL9{dPzQnR!&ye)>cU@@7FP@@z2xXiV;Mv0RH*P*0#EH z!{JVaczwh(Yz`grslySPY2f4DkyFMCBtP&}Un^DIPyb{)=N%KPxH>MXu(r02fjCOi z*WGv5`xBCfWifQzr_1$sUNf$16SWQ1jc}CtD1poIad(()?~eS&j8Hx$SF-Lso#_3v z2L%rtsTp30D>~-vo#{Fx?D8Mg-s_wiov663?GUcse4I*rrvfSPDB;<+mnK+$Gxybj zG9h{2|HVGd^(6w`j^~;8PZPc4irvNDdSk|sJi3Kf*q_hk9W)m#I4n4xNs!z`w9~}g zd$egM|MBPFQtbeyJAQ;csu?T+Dj#T=STl9~J|17(9M3+*Z@DzIThdr=HQH=HUdWO3 z%K6bJ%cA?~`8jH#U6i@G z*}(@aj7?XZOb!k+Td{j?Xh|Of?`2Sz=8k@nZQlX90V|P7sc2i!>iEOs8PjLKZx~%) zT_U$5sg=~xa-%Hm}G zzahDja)Rmk;XO3IGkih-xbO@&au9@lis@NqGfTisy7>emjUx{k#uJ8`?`?Ej5;KH( zjt>td{Q`P|`-1{@$QF{W+pA|Xc6xVVpdoESk8o8z zT(9n@IUn;I-!T>S;1pmz3s+$7yPO zb||n0QI^SQvi)T#sXOi{E58!GKHgj-xi~94cc|v8nxOUBpNd^7bB$R<%f58ma^JC> zJi^^94?8TRReAoNHu~>ywz}if2hVs2$tW2n%6+hna3UfP;?^%|B^-7&C(koSmVLbr zSW@@nTRY{;`sEEq^#(I}1N_z31Mi9Qz4jnUF>RFVO!zaH~D?#TtlYVLQcH zSa8>rUYY-4Vt+>lM;=HdDoVWAW~#OIyM49GhU(dZTEEPEHGs6 z+(w_gFL)uoe=TeFfoe)8w=S4Hm-af-?UHbPc6pzc3>zfaSw??trYeK+2{rNy3vYLH zbN~-#veNtxKyq*U7^QOXiiL!Q!+_gVBX4eJG-my^u{|Me`Oa}M*nBSh#TQNHg<(){eN3!65Q3W2uyGB332lqy~KEtM%tp8Gr%OATqle>YR?gUk;oq>mMq zg;U))wVDR>UsqJ2p$SLR6M6haAzEnQ^o=pvjot4^DeG>=Eq%<3vNF<6h0A0A8pX7? zCt$OzC&~2LMN!(F*Zlvk_li?O7n>0x?3RPw)f0wjP2*vt4aE5m(}uHw$Pp4EmtO9P z2b-8zR#vcIbBWMJv$LXg`q}$$DC@+}r-HhlkIs7QPa!7$&*|3=SKpuHZlSXe^jPsK z69Oy;I2d(o=*>%}V;=TtK=T0}hxJudOpFPTY)-&@7N=}tAW=YK$c>-E<;xP?0N$e( znz0JhrKHyZ(>5~BO+X*meKXMrvIwlUBIhDCz%ZpyMGn`UanyEwryLrEMV8!Zu~I*o zjUAiuS=H}sWYJo@rk`LGodZ7~;9agE6y6_!+5?!}c(73T_6?9y~lMdtvz)y0R(%(pe_Vv){V6;cGm2R_(B zf;~FlQoz{<$p*t|bFr$x2n}iq3RDylrzZji)G9an{cjgjX_j!ZkI8Bb=P;3Q)2voH|M-(q&!&uzWq{tt@a!m~`Fgn+ z{SFJ$RYPye?3OFc7MJyxGY7c3_*RpFA-&5dJ)V+&v0mG>r5-1`C+|du8IQI$PevzN zqPIs{{78NN-V^tl&kfV`Cc>Q&#Pv8OKH2M_>7wau@%P-h#;TE=4>ABHnx|Gsm zbCGVr@l`;8>G%0*>*BPet=zOfn(*E<*YMLq6oCoD$bbo7Kknu_3~BT?W@O18uq0v zXRm?#2-@NSG#Z*9;4ebueUOi;NQlLLxPEV3Q#BZ`ljxHi9Jytm60GGqyn- zCxOr2MN5RGljvKp-|#FVgQFT|fpQ>}9qE7EQoG*BPkPZu-^is=QD`>xzVjjT(SUDl zLf)}_D}w@408w3zEN zk!2(;lx*BI8_9auf1;w1?WVd!H*SSMSd^O3FDlyhEscel0t9Zc@?&qW{A1aF=_T-L zQ*~Y*{`-gYN!xqV41}XwJPso-mtn5(rLD#wPu)|zb)ig*DU|h3Bgz5y_I#Z zb3RS6y^=+JMO;qJV83yH;~w`yXm(N8!hvBA%kESdB(vV1{Mm?&`*8ox#lPKo>!IPK z7eN=bu8Y3cQ}PvyId+THMYqZiTd(kI2XFd`C(fNQZMpx!=kVMNJ;^@yLO*o7ynS_b z-uiV>-8Em_$zfU&(PgQ1xYQTAS#jiDyL6;_f!m>r6_0g1E*0qLox&y(ewz|BmggH;UMLYWBZx z>_Cw$&-?T7nkXn?WR0Q5Dpar8VVUNi`>KlUq;Ne(;Yy9Vg+@=;`=-jH&Ps$)L8k>p z-8&i~HrL$TTI`N-P(J`nO9koiOKaSJ>D#sjc@8m;M2UKGnu$>I3?l4JTA zz-~_X1k*|NlV(_1Kj_TIt!8saSsDDzR%h6seE1pdu4f_A(Ge4;aj#bW?G|b-WF9?S zrJ9$tTl@+EvcNeYoR9B*;>jtQJPOB+v*mTp&tP1F zoNizUFNlf_6TC8bATA#Diod_)1#8HJZS8D#NdPNZrTcu!Q1JAmHGQQAFKwgHr1Roe6Btx zF(%@usBpZ!ZPvDtlVHYNciWG>IE9nXmOJ4MVl(XNMYPad7}_nIa#Pr70qeJdl^-T4 za5pry-9xa~rQnaGsumZ%WlIa7wqDr zqN8CUgAPq{iGjL$0@M31v!ac`uSbNFUbG|bX{=|AtGH&Rbnsw*biw<{^fw;fd)9Ha z8ZLzm?c+RkmRZ-??$xSk?zAXb8Ln*3lk?z`SsVQ+Zj%3F1LwpOyE7F+G8&sXl6~?H zmj|CVHIWx3IrY&7EveO-X$2Vwa&SS)lAoWSX!G))QO_-oxy9g+H!rMx)eH_^z{w*1@& z#(M-$f2zW^0NDN7CQ!2IV5AUgJ-zD}UjN$4s-jm)Mjmc7qAuXEv+FOcKlGh~x9eeXgpeZ~Y;|+2G4ye0#WBxvr;XHs3d@ z-km3iqbJOrp_C?^v#w6~DY3o|K!W_0@KCibddPAjbb9_gGCcARMgD&f%p3=Yc*O~x zxz5?$<6I!ythVbNb@Qj#?eN{`dxT}lLnb3HFRxYkD=Bk}SM@NXXmH8nW($bDc{3zYKC_~%2p zV@*@_na>*exxXA+sWcmKlW=JJ!8Mm03`w%!{oz;72VsUwV7>WWV0~TL`Nj}>!ZsJ9 zlxt@1ctA|}F?rjc*DTgTq?Pp#h3@&Na$_(%&D0}2Me9SX{y*Ec+pH54*^fV#eG>n! zD0EEe{4Lx3h2`_S3?kwR-?X=F(&Jf5_-PR$UpA$q&{o!zp>fY?*qkWqfhTm~;8Z(K z1C&x}G%QPPRuY?=o1?I;0FN3V+rpyk$B!RKP8($6_2kA%^!K8%5Cj@hHX`3ir2iRY zgT2in4Nrh3F_KA5P=Q4-c=};y$~%s(8LP4z&0E#74&zHv1s%*M<0XMr?xCJs!GdlY%GpDz*B<~{3;`D_oTaff+WYh`> z*K4S7F#=hEafnwWlRD;-Wj#4`Gc%XW*_F3(ro@eTrI;|oB)l-DK7OpURZp)<-{NlRkwOSQRres*FfR~2c`b_N+F)BeYzMq8&*Pn&C`dQwh2sgKLy6Gf4)tg zdIG&3&~4DNHC9Rt5N&9ym=k`JOb~FPub4gJu8~G(ZN=HxiQNdXS;;_*;Hu#DoW(E) zz(59rein4hiWN!-l^?2Y8snnE+F9=ZdZh+0roD+K3tFV5@X+7?ASx z^o*bv5f<*9q1j5jt0VV8Y4KkhH=5!2cE?{cGlj}cxY*Ff zic_~{0YYh5+1bAW)9f46XQY4}iZkq^LDA|C!aaCBm2VC>*=JQ8-=1rWI=zebN_!;7 zqrx=GdSpe00boU{X6(p%2MBfB*N_7-FdBij)xi)ej(quFN~ALs{A{}O9bFQtpFjWr zdQ4{VTWrLG#L>%W7H7-~?ZLN0DgvsHiW==2#sd*KZ;iVmh>^Un3NEFBdjSeZefZ)Np^Z5xyx@R*5byG54tz}LGa;)`vF|2AfRGkqwT+t zQ~m?cRw9MhKm!GR%>syQ8z$+eOxQUC_`AC1sG6SF6U0MNUp&`vuj=+V;jTtRW9rvw1h#831z(-crnBDTnFb7H;+c|K_vjmKpkqiKNSWB| z0#^o3TNqj3xkXCAf!PKb9BsiyWodZUOU+-Sm$G-XZ!Ca{NZQbl2D(mte0#n-L??`aFr z{@YyECGv=hMnK}365Ht8+xd-N8((d`?j@OEw-LxBQ#;?ea|b*sI7uY`l@H%HHru9W zf0ahB|8b0Si`s9@yJM(lQ;ltkPpDLQ{m%lr0rSskrw$%91A`b)0nXTW+;!inUx$AP zp`d`a_nD4PIK*>Ze@=64{lJ=dn{M)bxhYZ_a{W9}?eBTjVn%QIi4s#TkDA3)i2MD8W=&Da0GBK=w zuXykD2H(VO7R7NwzE&oOFu(t=Jv)cmgW$2V)uKz}sf1i6ko~^?Z$VhD>;|_6knID| z|22{`f-_$Xe?`k-C{JG}PJL?n#i)yvhK43qIT1cwx2dOPVaS=5X^I*%u(yU^4@tI zQOT=$84;2C`?UwMSOOItpfcp9kC&f62vW4#_?1C+91t&oP+-%XSLyjKvTGLn@wqsV z7r`IH_E<@DA}pbpjON(V-X3`UPj_{TPPCd1`^}^TE$EHAYzz=bHq6!%nsM>7DxN*t zyHZ@3xI+6i`Dl)hh1b%X-t-VYhSLnn|G^Ev|2~pjOc@V7NkBuFj@i%ac)0dLAY5$dk^n}E(+?N$qpBE=)&G-{BPd80r_*eSs%gHuj+wnDTaLS+JV%>XeKq$ z)S}u`_F^vlC5M8``;i|z2m@vK!?<4Ns9^$~FL_}em`JeBU`rg)*3Y#AL0@C{PKAgQ-LE9U*OE zWAwb2X(D~UYc4l}^!kKmpXBOE%Sol>hl4w}(wlii`1^||DZn!M&liK@w?Ma2qA%{^ z!*eE)TsQaKsgS0jFhevMlrhC9gQ};vnHb z;!(h~LP0^YuV8{83t3D@|KM_XehDym{~OnKU-O0a$VX00Spb>uq^o_-DUqybQkN`h zW~N?0w&NjFihrCTP5=Lhdh4*LzAs*QND)x!P`XQ6TDkIy#1JHC2+z- z2(i#&-p7wW!=nkV99>n0hujyL27M#(dUv$E923`@BDa*7h|z__4;LfOaE|!0Es)27 zC#bNZB5p`D<=?TNh9=6pP@IQ{=i{7jV`HNL*dt$b{74qZeKjgFo|Nt0&|12<_7L7J z|NSJSQ$uk1Yt@il8%2~}vW}E(X0b=^)Yg?o&u25F*sU_>oeO@mnue~<0^}^(Y`*GSo~0UzzNvmsR3Wu$m9idogQmVrQ@O z$W@z-${B()*Tzpvrp=v4nwG7wR1raZ{_E<4*Iud?0RWNg&*bT|R9;gv;ibW(54BE? z;-QWKJ*wWF>BwHA4LcapF?4x~J8QP)XpW!%XRjSJC9ISE2B?X0^1N#gEut;Lz^no$ z3o9}aD|yyvYA2slHlUbr*5?R7DBu7>6%bnDt3;GC-mLpgbO) zo)`!)o_GH?G&KDDKSK&56!G9*KzsldVR~_&Dv2n5+`LaoZ7-@$Q1gj+UkA4?wxmtu zgU?-FrSxvouQ(l00iDejD%S99fXcke4Fn3rM1ZtTnm(h2!$>b#9Ced;6^_&QthiD0 zX3QkLaU8D_2-i0W7C)Q3O_ur{-`{6xJ}vjDh08KhhPN7@lyRHbEV%GF5LI~Cm;il2 z;Kz;XU&nJlrLT1Gr=zs2Rw9LvixvTa3A)8I=3WzYD7PhlRoi@pTmx)8zE&q-w|>8& zxQjjgiPt#ozt+6mv~hw7D?u9G*OeP|8V>54wE#{5R`rQU{#$DL+UuD+u=ZE3-MC@g z&DVGk6nR|Wm~2ike&qA6dc=R*9l?efe(EO;YIqC;934ab5qqyjt7aB=2t00+RIzLa zGPT$&#wUGZol zs!mRzOkpv>S*#CzHn^!^(Ae!$8&rirt&2PT(xfK2$s|7^lyNYnDJF$|QtWF#q-1>< zbSP^ZS?^a9s61*pDu1*@GcZFEsx#27N)#z1dgwjvsp2a~P6^n?pdcL+VI&@jjjQ`Y zO%iv&cT<71s-mL{bnpkzd( ztjSV%k>@73_N)VB_y?P|7y8H)Hmhy$TaJgTi4P@f8;mbwVd@dyHoN+On9<9cL8chw z|Ipigtf^7M@+cAN(ZqOBvh=Wuc@Yi;bkYZoq9YK8Q*u*THVB zg;^!xN$gt1DeHsl&)Mbsp&UL!jsYL&rw_fn&Vk52J38|9Pe;N~s7 zG7TF8##{C7cXRco`Qn{aTh|q_9VY)X6oDj9qqA;(W23r`{*^p|3EsiI1+Xp<#cNsf z6!4vJ7n}#h(n2gS*R%LIKm98))MU;o{1eF%n^}#wca?JdpCD;T{P0t zAyP1k;ZU;VJm*@z<%7vBPqv37Hlvll568Cd`f#CyHF0>$l)jvWty2o$vpQK)R6XxN zPXv7}?#{OQBq%JbXE_*h&Y`Jy8d$&k=Fyef(}?gIyoS2om1}vw?=#buh^~A)fM(>w z(*(aqc%dNTub0?5*hzd-(H%zEA zxg~s-pcU&={wd_IzTKUuVEZKhgf~P+Yg?+RQ5Uq_0M8QPzvRK_pYP{Lbw#-)2FtK- zL_f_M31Lf$1YF@RZrv+?H4g#i2xp{o_q)XDil(v5JsYKrUnZdA1w%AH!0JXk-E+JE z;6TjL^m*cM;zu6P@(lQ=F+A*oGYbv?i1edE=SJp0o`JeJ(vJppl)QygI~kr;|D%sR zq1WXhX#;Mjnc!m<-Mls;R33T4Ss89m4KHpppT=25jwGk`DyrCAM!)f4;Ag1kwGqntD%4V` zB2}psx57z(ywWt?d#xHV(>3-Moz)cX#as5#&S$d12`|<~%&98OT{p~TpW95u<8tL5 zeTZr|f;80vs#q&`7~US5pU()Gk_bf(qTphk&WZ{baD6aQUL;c)L+Sp&)RP?Dj{UF8 z-6oTd>@CxXz%KE=>-@tjcVDpVcZ4I@@1bTyA|H3$j^wfAvOFvbHEF@?Co4OJE^=4_ zDynr^C2R;w#D>T$=+4ntMzIlB?0OuvkgbKE9}Un#b-&Bry|MXAH|i3S<oPuYTuG(zEd20OjY7w#*vZ#Aa`9lR!6B0a%E;b(&LWU1A+k1{?Rd3R z!}BNrPs)e;K!AKQ>aib}{QZ~`eRJ~>l&+2CC#X-VN~|2-{8nL6K%WL+5M=}d`llb0 zv;2gRKZ1#1_KRQ4J0p4N_|J{~BAk_3#^lA1I~lL-W{n5L z4x}va+Zl^<1Bm9JenRf^f=}SxBT1)5plJ5qXOmoy&(h!$SDKYh>TpxYU-2It2K04QC z@jcJx%ELyl+u&)aW()4T=tO*UmiyOv;C-x*I`L~scK3P_oFz`ODtJGsNIqV<5H6`A zIGN8!&l5*q45%{vPFo`BvaBK#=jP)20+g-jf*&b>-v3#ZQzcM)eX0yCQj0VrD%dhw zXLd;cjH0B+H=zFV;)N>(AP}4g&r#By0t=8+!nr=@Q#TzN_6Qti@TEFSau~%^OxCrD z5d>}5vQX|ZA;fszsbRN&()rIBG}z$MsODlGg*=-d;}6U~y)o2cA=z|SS0?}=4+Ijx z{>!sn2QF+_TtGJ|LcF8(z|LA6M7t>>eoFoZC!linONaVf-7wn?SNcv^EjsVX5AD zZ@%0N3+|!wCEPnwlwyRlAXXVhWb;Efg@b68?ALn4LTjmp1ZTAKf>9$hFQ)SvYeQQK zM>%(WX$Gjm+RItzaM#$lsoTng2Nj2vvEutMrd$b>>V_32ksvqmlBS5x zhq7rLB6#P*o*0vgJKiUht=`OM>hBm=>O+e3X9v$uq@wzrvXPxJE&G-D@;CrW*t|Lw{ik9z~NANZP48rsM1|K^0gCr7rHm$5`Y)*1s2<3U( zqapnI|LWvHk%?r}vx2uxPfHn+g5&Oc?)rW9h_rfR+VLM)BP?tp|L+4gmKNKaeSRKp z^{0=rzrP{5`uFdcR`r$p7uR@^oV&btWXpVH0yS|j7!5yaEdKawD@v+O?{!|bHajHo zrJ--s^xfQ5Z%4wD^5+{BZmgvD80eP#NUti&y;2f?oqQ|6bgn$1RH|g-LN<5)fK`4X zchK~ShsQU|YqC#b7R69ezBwa9rpEy ziZ;JndP3RpH%}6`?#&$9*w{cB9)n#5jkOI&v*=Utu3GiHi@r=1R(5m4Gd?DeQ8hPb ze_~Ih{AJ~kn`Pk=XI%)!ioYA&GzIFui#@C$u^E8k+9#T!8O%G+@dmU;ghPIYO zzUsP3GW#<=^03omM?u!hiY&fJ0dsY!R->sVd&Wb8?=&_sJMEJYb?eZytTDEOiQpFs zxtkgC@a+ob3=@YxJOdDSWE1sM{hIT44rd6^tZX?c`*T0AVQZ%Ou3?IMsL2~;FRSUi z#~qw^&#ha=*?bhEuNu^Fj0x*JeE7I81l#&4#+!%6qE;h+x4eq?)eOa&lwQtS{A|Jr zr6NL(Uw`_KITh!dGuP4->JyPimi+e$S_}EB#U6)Ce!V<+U3o1{n0-5>C(h4k-$Te~ z8+Jz0y5~r3C4TAf!VM?^^iZLD3~A+n8^AuU?w666NJ4Qm4Uu(#H5EIcaWl^I-O~c_ zEN^XXrHbpuh93X`{%I~8swW8l`0#j~Yjd_%OT61DAKzvBg+~CEwat1C>u~tybMkmC z3O9$h3RXFT3-KO|{esy%A;1;8_ARlbzVAgfZ{atuZPI zp^!*heEwOXE3EF(0@1UnKQ-CEjHPwfyCQlO%q|GY1~ac|xn5m+Zp)8aW#nK8bx3s1APJk$)TR{lEAz3)zepdA4jc@ZiRMgvvg`LCYRAc9M zpcNl;mNcXhJ1E;ku0-}ML#R>@&_q1WAqc9shEN-w{J46@jG|52TSA6TdZ@uvoV+UI zcE-}$L*mM7(=A^8QVv%dCWKfG=i9c;+~0=M_dpQTcj|{ZKxtiFw&OoKpk5h43^(14 zNZ9+E)ythuun>Y{^uYpyBH1+fvy{c!Yab5D!1D5Mp?-+v!cf(}e+y*9SEETK?U_CMgNS(7L)d#Mg1=^B~WAf)Hw?#_zX&hG`c3^rqL$ z&59I%FxYW+yss@$g0NCJhc+Es)BW}I(G*oT|1DM<0_~PO!WJLa-Ie&oE$}{yy+8c2 zdR@A0!&=r)QkpzZ`uK`|#sFc0?8=G!>amlkC2b0_evnD_jOVmMqNkQUtIU4Y3^ZhNY>l- zVahZFpB^05LAw;ed$Xpk%2U!Mc1vfM;%tvtwp4a3`8G?i7uMhJrVZxCHCtZK)cJUx z)LI+&zb|66dAl+CB@a`xov5dgRWR7Fj%AGnH^z$9jYeG8vghRD!@}O{z1Pux_g?>% zYRwIZ^jsTk60)=P_-+TTw}HlA2};)xXtvpVuZz-CaNp6yWJmeDWG5>#^9$s&;xUF0 zKLUXUl{j;%ar5&#=wJ(xN_~TIWJ2+ngEA&(6uO&&o(RgEf{!1QQ{Gingw1YsHy?f< zSkna&KR`kmOf9gF04^wU<5(IeR1PYPwGb7z{sYK7PyVMugGGJJRC7Ck@7=6Z#aF+A zPjWWLzU6{ix=888h5`;e+)yV%wvFdcaD}*Ar)4pz>qA!m3=7;kVGLEk7I)zv;7^o( z(M~Md?^nceQ}c(mEz6uK67Cb2r0~9%(JJ{Ou5cZ=33F-4%A@}17@5QrELqmgv2vI$ zDmZ+_6ATS^Et^7$r_-?QV+v>cK$%?UZd^lZcEjLM^(@|AK)NHn)9Vx;U%46-I;oUw-TdiYn!MM?gq!6uZ?CzSvrD+Hcx zn;;7PogEy4%gQ7+uIG-3%B=&kA2uCPdN<=%A>KQ}d&<^{Vsvly@iU9_%gUn8hvKg- z#6yM^ShEprMQLVHQ0=1=5kYb4{ilx|BPP+otTL^uEB_z+@G(#(y@FL*AQvj;T#G&7 z3s{(#VEdG~+|$N&J+s(hs?L@9I*k}G^qBX@jGX8U48V;1!>e&x3?haFbnRfUw3Bq* zyfd&4)#++r|A3_cHN+Go(!0mcw)jgrrpdCXf&UG@+}-!FeUx;QEXDR*mp8Y2lzsE3 zXNsj~aZ>IysMOrhM&`U26OK)=71T9Pb`?x7GfqeHRKNWu$1g#azqfSyB!%3qrkyc_ zMl9Wcs@sAq^eoZGg)*gW8F%z8weFj6Tmf%vj!aSdZSl{an&iFJ)U{nU`EhC4ez7Rf z;lsL;^rH&Ruu6X+M;%wB*K19nBiM?KQ(*p}-6Z*W)T_IK5go;(LZGQepr-HjEiP8x zs&ApvJisC256}@wlGEo{UyTpUfXOo<1srHL5W*?g zI1cc8j7nb_~IDChfikIWS2)yxytxX91Qp;t=EuC7(i zgWenT_s3k0SJO7EzrcmNSHH>VHBV@R--M*Lc>U_v@84*E*!fvzar@4pPzJe@9LPd^ zaeWUq@Rg%F0y78}2{hZb#~q}P6xHBHLgl7x0Xmsq-98X*jn-%Y?&pb$dJ*%^#)>-Sn|v}!Z#AR_-h&wj_0;$LBZ(j zK@_S&@MDjR@y4->uNwbPRs2weiNm$Ii7)(N0#}rj{ybr*EX8~2gCxurbalUY_$qv9 z6o-SGu06kn?(g62p7wTTs+ys0YXK?Y7$-C0C_?#Wax!uzKFJr1RiivXP87@?DhYI| zOJ9id+Ezn6JH@VCU*Y%lS*q+a(_puHDUjg{ai!Z&ts}vemk}LXzyb4NfXLXiaRWj3ME`xhCU$H0^RC!9d@S=g8{ z>5R2(OvA7;;6#7}1%mfJ<1suLWWq;O^FRX@lbq+uef##yVh8wK5Z_{*Di7FjgI$5c z&6AT8bPm-9PpG22!oe9rT-;%Wt2+tLc3bH$u$-L@o*N$jb~|5nBa!G#55Vd`@`6sk z0?iS?s36k$0Ll1s0jFCH|2mUi2!Agq?@!;zfHG|w!v|AwPG;zUBZn+cf=wY{;CIoV zk2yyHe^x^6yR2%o2Fh>vP%5^R6MhVfR2BM7Dub&yJUzYRc7DH>NqEZwMLqoMQkkNq z8s|FkN?Uum9EFN*sF>kq`<%d#cx4?n>&p(Y*Ah!c24WxL&zJ`HkN4;T#n&45U99tN z>>T`kwAf4d@R`ln)TNhb!@_fdr-y$nFY_Of4I3lO9f%bRnXX*l3<>lRbT46=Vihm9 zI5e{v#Oc`bs!p?l;ka<_9xxffq;d_m(3b?1@;YR5K0mXkntZC)2RJK(zOD=TKV=Cg zS8Xu~<%xc>%Mwi8N%<&*+JU{l`|`GQpc(a#%Dp}5Ur(i{gce#RH^%2xCP9jV-gka? z8@whhC^w|Ir|huRwfDj8p5RH`?*z_uFLI{L5xB7c(tsbpuAhO^QLt!ekUXAG-8G;z z4yewx=RQ956`HlP@0qiY#KchIECF^axSVE5v5_gC-*Q9^Kh!E-Ng~nwm+Lu!L@4KK z0e#LK=6wumGb(Xt-|%W@hwvC~65tDJrSi6w-g+#NZR@PtXRWK!6ve`roB!_p`}>rn zDlf#m1Z~nRR=C(w*+r3<3jshe0Q?H;lsHoH%^PJctsBfFeV7qTTnXa_0IJ+-)aJle z((#~>?oU^zThPkUFC^P08-u)U=a90pL6Yc$BTx((h7XQ*@w&0%dxykcQU-l<)50|~ zbBiwAIj&c?)&sF9nUcUq-8W9w9zl;vw9cft;?jVXIE+Yir&7 zIv~)c=h8S2=P2NVol2CZXe~KfUe|iI%2(yJ^)Oq0erl>z&*Qj(^WbI1pT|FyCV8mJ zvy+T<_Fmy=-_dJPO7D6=(j#vewiC~D9p6+`GBRzvy)kr;??A8~!-`36GqA!vsWR<1 z>SXv=#zRg;K^(|Q6O6u+FejilrRqRt)xi<V0!)=Z{IL-nbk9H2N*mhabsyT`fq7AOxlsqHhndsV)pqSokQ z`C`XGO()1Wkf9p2_FE*2K%61b=j<8FyE@9=0|Z?`Q`0}T_cAA0=B>7F`-jy3f-hS) zMHNIQ<0xbBUYqdLTURWw7w~y463Jbs86=^Rl!_pn+ckV0H>O2@sh^xyAePm>XoOoC z@)>g_mPTnUA_xEGur>b+Ofrg@Z%qX1+PZ`^;=(~!_e!f3sA#A#m-T2RV}ifwtdOVi4}8*DDaYZKQla-d;BK`m=%#|}zqtovh-qIV6dW*7yaU1E_m zIuu%w5Hi>^lOC6*CMm$=Eq-))t9kJu=NIKIQhkH2+qH8chpm_A#cHj@jP0SxPE20j ztHY&!EF_Ouh|Gt1n28nwA1Coy*_rh(sqX%p`ShY{Un7>M{>TR$Rb7@1uV$^pXeN;N zsrnd5Bm{RW)I4OyhX&**@apScqz|o{@ZejI$lIp$)|TKIvgwEMlmT}T?I96sEqFD) zM<|%aqj6pW6?~#8SF^4_{ZaV-Sl3{FU{+REzQawR4Jw)C$(0_BlvGw~QR+7WKo33( z32vHq$JP{5b?{F}qgUajPDqtfBtgd=L6UJDjHsfzr{KP_`=etM?P-Ur1UWhAIJ+Gm zQLg*9JgZ2BYW7xeE-Qa|;aJGO@}NetF<-a&Fw{>tPNnbISHM0?&hx}1hNCoD;0 zcv1qq6_NsUi5kk>y{d}SnXes26%Vp(vAL0*|3;Y87CK0AR65)jEetKF!mq?Lj2MK} zE5Y5Oxg@KXN;ghcvgMC`gz%~G*(IY@cpONd7=E2OThq$LLWbm=dZlr#UC+LD2-CaC zhGY18T=6Rz52|&Io9pcuG*h)V`k0S7_EOt_1Z#@KFwD2o_XU$(FA;(xU(BzRzOVh! z3MFM>?{x*C5^S>_lNFo9san3s;C5lb3}M_GtW}kVh*6iqL1RpT?mDb1>?HMXBy&Lw zc=gSg0|-I@Z35a7($|+E@3j*I;T7DZ*TWN3SwNea?>l{PdcJun*oDpi;Dh^1Ro&j- zpHBbqhK{R$v{!;cZf`92B02l>t$%tnq%GIcQTi-Im3X6fE$6{#ED(K_DQP~AiM9Gf zK%Mq>D)DSGvASV~uUkrjYPg5QP(&KQg7ETdVQ(B}utnLZ6%$8)UD}SMBzq%bR?;6E zGSYc1YxbQu*x0;@t#^WUl;Sd?c94Ur`or2i+!nh2;qHMpV@epJ5tc9{D6S;$I*@u> z+khNRP9pLO3PS1?bcr0LHFY5N32YWy&EL4ZT_9H(_>GFj?S`wXOM7*k82y#CSLnPE zFeL)LVm!9~pd;oWIS>?l(-#;BPzZ&&Yl+;j!RDvu$kZC+?U(P2b}~mPP!ZdkC^D(c z^>}lO8yK+HJ@|t%s(;;YO?=fQp_CPAOTboL8VPoO1<`%+RTCfwt;}`-fC|3!j zYOq8{G8z%&1kLGLfn=fQrwOW?DuKT_{`^eMZ~7cg((`km{px3roU;2Wre}R`Mb<*V zObbTbE8q+PeccB!k~b^5gy@}me&AqnG0!2DSmG)hj-1ri&?lKUyL(YrP8p?1UeAFgXn9Y+rQ{}AHv2e)G@oaD|vj^`);Bqn)XO|18adzsMgb;yc5$9^Cn-4ciA$_pM~v`GX#32eF! zD2?P#NT7PrBUL1yZCn3wn6anfGBEhzR?l;faK`wUN?hvr^QGg*?amFsNqy>sAq5JJ z#>&e0yUbB<72OgWO(e1>{Cm;_lE!~!#{42$wwTVKWGe1>{-8#)WbDqI{pAOy1U4Z$ zb`l>%KL#0IuKczhVzC8G&l_)>A<+mNoiYDJIzoP}RLhASZFF2HA7cgr`QqA5ZD%pn z=e*j60HG9MskN@40amM&bngwOH5y_j27eLgDF$y^*DS>d4U1K5!4ovgK5Qw4M|>7p z0<(ZdN}?Tv=GooJaxE<^WJ?r%o6XRS>w^bYaJ9YvjXyxQYEWlL9W91#z7Qvgw15W; zYJS*wR7=L-eHAZT-x2X*n0QpHSuOC!v0m!u+mGxDEF^O9h;77ApDSWc-zOa*b=AvI zF+7Z(biXY_?SlYu*>iajRtt}~+S=NUiNq8d;nwx1l2&%L^kV{-Lo?a^-ete-Z@Qz( z(G;q?)|(Afoe`>v39S$ZiL&sf=-o6lH*|3DdCN;^BL&hsO%bP%XRv10efVr|smpsU z!*1$Xf-0IZ_dfa&e(m(J-Rn}q`p9O8wAPi^q`*?0==rxXSqSMlXzN$BKXyzat94g# zOugnFi;yXE#?gl1xnNRscS^U^q)*4>lTS~hCVj+xnABOEVyvM9k|5wB_IL{XiDcWW zEsT#c8aVak>+n8cG}KTj>#lx4$KbX<+>jEc)Z5<8->HIT5rkM&OVS-@Jyp=7w!vtv z>ohc?D}O9){^|C%&Sj>gAgQPB9@olqkjX=KFK7mkG|>cnO+*NwP0=k8WS0uo@*{Zc zxr>IIf9Ciuy44#6>D?dW>FFE1-&Ow9mV##N#~-E1a#bG5g^W*j-+Dmyc6Ro4(%4N# zATdpIA`&|>TR+L9U_6=DM(d+y6{(cI*B_pX+Z&CW4F)t67M86M*d(*RwzggtaD=hE z;;1d)kSI!DYe*uDc;-1QPtP3r-nrj#>uFQ_ z?`ycBAZ`U0hGg|hLjCtCYId$M7=i=pb;#28mZz3h6_)%^f^Lr`xt<%Cv zPj({lm%7)17sut!kBtchohK-%uYxVO*a6~iVz5aL5oPWumRHD81hqZS_vah89Uh;q zcUr45B>q%vESr6ahB5!UeQ^Y~e~NBDaNmE+@f_R00Ew`-kSgw5j+co;%hSZ&Uf@LC zh3SLnNJBI0K5}d-|F`UE*DOrrKHl07+OCe8e&iW@B22yFmb>|KLG|i~e=Yi#uWOtUiPUt3cHFc)kO zLg7gRuGwxKI@Q{hQyiQnfxnZtd&m>KF=K zWJF?P^?d-Yb`Fxl5z2=n*^K&_P=M4I}%j@c_QE?`M_fY>bNjp_VaAb zlA;aci3hH?`|-3BV0>s&aw;u?01u7^XGEK0WpkJXf2r|1VK%>1zJO~3D=Ul-X(BOd z;UI9FeB9i7K;Rm?;4yYzSKK=9r)shdYof972DJad0sng*#l(2ix6?!} zUT%2jw(V8W{MS5#k{z&2h@fu=T^w@+1erkd8U|))5Fx`vfLG&?0Tpy|^86sAvWyE{ zANU3aPkN+2peXuql00jxd~<1!t-;A1r3)mi8f)&q0Vk8QfXf$uze$x__8B2nsJcdf ztn%x!#4cnttY`ix*N#%$xq8U+C5(wPZB$?c0YPPH%xgzIF(RvdLn99%GH! zR{q$HF>Xk(i#VbVl00FbM}v_q{->}pQ2hW~uns6_G}t<4y~8XWzILWd2EwpF(( z+0nLN4REN}!7M$`b2cAq!!)L&pBa7Di-)IGFomK+(C;oVxCtg;&u#d*0G9=x1gN-` zn*bHwFln8rFdg9W63mDh9#wt)bLr*}A@|5v{wk^9K#an!&Uncln2kqt^XbE+8`4L4 znmKTZDd<(%8tcrgPm=~S6MN-QEQ!eche)x=H#^YNCGPJ`>@fvdZy`TD2@lG>?GnLo zg&pRMi=t(3c5$RrQ|*eNo>VD$)wejvc*XF8P&`eqKvFwNi4cHgGi&Dd;D`r+3xp(S z#|sSRLl0zJeW{6m=ZWx^D_4p<1CcCfWubWV@ev0?8xBLyl0~ncc$uoA%UJ_2b_F^l zQi&=>$d@{`=slZ}O*3&M?R1}XtPNC;UYk>lDI>omtNf+ck_nqW;qt{NbOjR{vX3MM zQ)ei`l4{7ShQ?|0X1#FK;Q$D@-i>vgGjNBX>#l|)F-^a2p0=iVLji5Y>E5V(7&t)r zpu69)z4R&5(eVdJ0p*A)h$gOq8Ge389rU(x{>K;o`&kRt&gwt_l$D{#cE5?#uYI}8 zn-+cE48NW_zLluXD+eMYLsgiQIo|iy;U>lHhuFCzJxjPRNmMq&dA#rb%QATy+qS?QDz-Lu&MTm|!zQTvqy{&3 zIFyuUru`mfp~Y9UXHg=uJ#V^U8aE3Z$$ixqq&&}0+PVD< zeAMMmWPY6O zkEp=H`|k6T*49T46ol|B`t63&ckYkKR2m1q)sO0{gWK=)y(#0|ar}XW6p;)34Y2L7 zVGG77GJ)@;;S{6+up37sl@~*CvSPceVUYTRC8$2a5h&O0j5qP9HRlmfyF$c>|~@d2G=DNt3u_CJ`0@)muY zqg0?0Lkp;4Z;1-xjBot21ePla|VgFk?W_eyBykAR?r-z(7CmodAh<)T8V|ECTKfloApKydF@GK{l-!A+l zdnyl{bw$%3HK{+n%ys7nvwPq4v@j4Gl0jGdg|sv{tPGXnrB6yL?%(8-#AYSNmJ(p) zItIfP49l{izIGnaI(#1jH1kipVtknxZAF)ve@qxZ?YcNW^`yS+^aatL?}K+`WtN?E zGPsl`E*R*ssb~-pTHPGkFA=@~;iOb7*kWgm`w<<=2^Dnm4*o9LIr^tKB9)@nzP$G< zpwK#FG(xtz`XHt|h)>}Pjp5;7=c7>C6*ad##R(4!5u0o75qS=ZenPt!JrKBwchq^@ zcJ?UGT~vD)V^MDhqh<#q4$;QHyK{&+qtq5gUU=bi%-W`u($?aY_Xx}7m($`wbTI3F zaEtTe54)ht46L)i18obj>x?sFvj42w(pvD&axE9XKwSTIzgpV5zlM379*>VNGKc|} z6LaC8bwLrysi*Od7ma&b5bojpK~EXo2{@EQmwMo&ii z`HymXd4WiZK%vOJfRoAJ`rkDs`VJIC!g>@KvHl7u{JIn@(B8a-B>m#AXu=T{0+aS& zpV=r6I=bBVoc@H@XCqY_z~rJzd;=R3pm{VL!5kRmieqYEpxm^#rE}t)v&j^(ugvxo zcsg}zb7(z%_Sb&=`dSDR8*=&MykM%D3n&1vg4Sn9c8IaUZ@qVcTJgirA4w!^5M1b|g!Yjz z*OO2c7)K|N&=BIff;)19hq9cZpQ&1rctE2>5?&;(Kzx1+V|m})L#JTFP^}=rI8kg( zkN`Wu1D*(EOFGg~!#1XLdrV7)7ScbI0a|4a>LrQfNr8OoCA>YWw#G4?m@0s5Hgx^7 z-c#yKA40Rv!L&iyaETh=?GPG~>WE-tk_LI#wf8D9p`mb|)4jI~rjX^@1my?jJ~d{Hb7#bJ@KHIL<|04wl2_wAz*{F&kO^d9h|T5 zJ$j}ijQ@<6xj;vk5^&7V&JK*yYKhuQZQ;hP^_VM9Al61Ef6)~RezW5iC25h?B^jD2 zYJCstW@lz(XHY3hP?_ab`K1uCn7+uf&K=c@w;U#h?#UIu;L#A(_1oQ$lpaZW| zP!SXO2%{)ZB^|1(itg!nae(>SUc(5g_aPQ&sU+=r7>Fjp5KHds&nj-?I!c8x|ukbArWq1tD52Kwize&_Db)S?JrbPte%{ z>PnHGscN1uSLmZmaZ7PnuNi7GbTs`tyPfaNh@cU_H6if9+#$GmTY)RZQt@>GM>(J0 zu8mm$lE_msul_h8-p~5;(P+z(Tm96hjf%#&_wUeK8#t7;$#9?^vhsJfw{L?f*l<1r zpcf7f8_bk3v9hujci$+iPM=!_e8?6 zc>YvJ!#Hg>T5)(zZtkwW4@$*i{LWCnOO(=6mW0wkir2j*ZkiY1Gv#4Fw_>y|avTmnzX?v+i$O*4 zV?mIsB9zCF13VnwKB}DL_n0r}gqGCH`O+M*8>zo^o^&2RQCgYSaZ>cL`J*SP>@UQ0 z;+=>~mCNjBelJpVhW^+0ca<%fIEO_dpy}%=e^}}SU<82(P-}QFX9(V(G9eLT0S7*C z+mL{hA6p+m+-n)mE_GB@YgN=u>Xf{e{TsV2 z@-h9If$EIPN8#|6jU8zu2K6*ZGSJuvZ9N(OOIU)ut!HkC!u>&TwrtIO0)N2Zc${iE zv`%nRVS$7h3|pH%u(vOu6JMwp*%W{u6257Jef8hVT8^byJ$+xb59`_0dRfu1XmN)$Q3G)BkWLz=b&WAO-cZ};&$B*FNCETQhAS-Py`Sk5?& znMs(pdeL{0djEhn_?iF5F}6=av^#o2S9|U_;qreW5N^hnI+Oa~k9{;#5+u1GDKUj0 zH0`L7CN!DAJzJ9r<|bgD@b!S{wAVlUiEhJE?=k5JsHm0Z`h!RlMJ&W43=$rlZC+cgLb5_4O@TiQFqt|iA$u7|346D($MGC*&F z)?2Cr$|txq zTP7tFK3QQJ4RNgNbRBlP59?eQVZ2_aNw1>OUGr+fN6NgmW2@cS?{1aooK>1&W@%BD z()QfhBvWy~adF2!>X>i4l)P5AQPMWhQ@$hllQ56r+L`N8J2}GQFiTeOhPF?30nyQ9 zV^G@VwTJ}*7Ewg&dGA{l%>RLBq06C0JZ%2rQW^YX*oLsD0!*JQ0C!FkKjvXB931nF z&#Eb(y1iTn-6m5)8+!b&zWxi462ZD&8+DU<2H8w^_G4w5T=r48@@bJ&~A(sFIjyigh@7#Ax9=S%N7n@G8N+9Yw# zw{ZlkO^`#-Bj@|J(pCKW_xc6~80e5~c-n!b?wYfUpkY+Ub9;6bH}p&Q+=o)r%E@=? zY3FF^g-JJXgiomJk<>`Kh>^JDZ7|Vct$W-IxGePvWJ`LKs)~(bdE*Th5unG=%es0wM+FE54Q$%!l?B_Hnd|v@7LAbL3{#H3n3nJQz?`L_8#Hw;P zVyl6C73RYYC#opBrPr@SI==4xl=ZX)Ib-*|pF2gZ>eM@$`TOUWA@^<}D|dPIV3OsX z{(*sf4*aZV9(*ZTQIf)FMPP!6Ae5ufn}bW*afQ6sojy#rf>!?%#~I;XrJzeTEwp{`eh( zOfV#z0N{$#!yTD85^%89Ke*Ql9G-YMC`e-E#e*P=Nt?%H^79 zYoz(v5+$N-g2a@0gU~{sv-pT#@XJk5l%q#{!!Z&4@3Zm7lz)xwelia(U(>F9)1Q8B zF(|*QAQ%cwSiomffkDQw9|waa*Cb1o^XGeEdnu^mG~VOB^%pp$jJ6^H;x3L6FE^I+rto&1TE(vYD|^vh(cs)eFePSa;DU}2XOL~W z)C9a+XuIX~bl=36{uFLk82&V$AD`}j-$`cDf1xy{E_?;GhTvBh6=KRx8!KxvrRsBd za4~I9yh0>5_L65e>wcS%GlK`kA~*KQcg*ecJ46qHrqb#N&2SZo5Svf1c+Ul1y~ULF z?DIZ33NL7@ky>Oo$91lty1hXpSBO?71_ZQh$);bwIQamQKt!ROf{n}CD=f(mnJP@0 zwoIz}6eS_cZ-uqU>wCHMBG$3Jzy%0E&EVb8e#k4pM7R@>Y6WVoL^4*sqTjKzy)y!k zgqh!-lCk^OUe=dA8V4hBAx>`)#tA$^tcTY`=Yk-g-0$H`m3w7^Z#Zy-?vONxwpIXX z0K_q9(dvGL-P-;At`ZXg8vv9j>cPA<^!P8)D-%#m1)c7(KJKUxXshr5D1%DHX?Q^J zRR(9gawpEtsJ^lBz-T9*dCP0$`ImD@X@N;u4W3tGdSZTT+;LkbW>m-F8y1yRt5RjF zH>tSlKDI5zIKa?|JlzZ$;bODJB(*6Ve58n%zUA(>#w&eWl3!Egs~Es?U@jx%YdsT$ zk&ba6vaagp&c*hHBx71rlE*&0slDi^#;tYEEYA5of%u2*p-=yTV|8#MwT=a51Q&rnHvLkT*Uq;FRaw zklLkk**Xjwks(M9K-eF5i-C-X=B<9e&LVAk0p!GmU#Asp+6GkB%i>PFChIFpYj&!) zD9{xS^kc;|y1nKgQT1Bl zi3`=?x0U?&hkwRy2rtL@X|qA*oqJ&bsXJKM%WlVYH@X|)+^ZolJH}mUcZr=NQN&U4 zAnx)yBQI}L?PKaUKfwcxbCww%fg!B*F!uPG-;06xyC@@`0LDmWT+(42B zDj);r`((Gtpml$(*d)gB&8LozWA#ye6Gpt-5mADf2AfU$!@!q*1y{sD*I@#l@b5;@ zI%21l#o018?RDS6h=~)b|KkM=39xNY&w=p}2ay_l-@pLqPo|r(pO^ReH&8%>aaYvI zi>@gzCvuBC{XY163p5NsC9)-kc(VZZc6Q4e7yzr5P8@B0Om;o|iE&j#UVZQ3 zNX%5eSHy?a_ZYDZ%)$BwpT^^8Bc5a-DevQ;nw>@3fwFWNZR=&S2ll-VV%OuVg|BOh zoSjUz+GYqSiosTGnhta`d$kf#a56{ZHiH*`(gc+(*b&Q zD4Wp4a#6`9`zJBno3mV`2JXl0^tKA1|h_-8;W_1Uy*MN z-WmQ#d0_VSy(FqTTDj1u+V&bc-M}=+7*&uyNEJnN$oREE4vd++rcF3F?G+Hl^fG(g z?~`P1NeED=Fv4u{;&dm?jB-}QU5^E-Ls%aR{1N~51k&*}|0Sv3ZTeLZZ4^U0vkdBkj{*ctZbieMmsR*7Jhv#Pcdqn-1QHavNW${e z)YPl{k!kbxem(?mGrUZTJ%=0hxclvCD&Z&8jt|02-V3=?XH8oZ3ty4Fsyx=apDRAc zPT!VdelnUyn3%bdpjNc#)tHd`^of)3v%^~Vjg-*V$8ScCRn`wPi+_M-?(XoYnAr60 z$wuX^hQI&sjzQcbV4V%Zz175M#OTKzi93N1bXD^HZke7A*}>&HCZ~*L^Vt0L8%jFp zX}26vn4k=SIFJ0+R{FBCr7!skQ!qIUNE9?&`J+1boabUUTxJo>1|S7$8!b{taxG3{ z7upq;m2C^xwYCQ8j_C3H1o-M`_tE?(c*KE3D<=Aq-cM%YM(P9M{!`DqOmA-W-{w|W zK+Zq)Fg?pjW2GWQdG;J;(Q{8X*AIJ^)k01tWYvMV0bV<8{qDEava<5>sfs!~aEmJF zkiGVrahQS}?>~S3q)QGZFsr1KcSCwBgjvD${&9FJ0FembtwXCEpixEyLEsYZjk|w| zhVLGl0Du3;-X8D&$Yw#~56%bpfPgZ<84iLVFak!QmGUq^()^_*3xCsLijUJoBvI` zXt0-Bmo&R@9~QrY{`=qea}H(4grTy8lr1J18G3jvC{o&UFaELv2?w9-$6|XZJbq#u z<<%~babYb;4UIS40Qd&H&*;C~=(Y9#G4&nbSnu!u56Pa{BRhobWUKHXE1T?1Rx%>H zVawjD2$5tL$;jTJ%(6#9R`&YepL5RlfBmkj>s+VK)pI=0^ZvZ=`+nW8aT_E7Qvt`$ zKktb?acZ19=A};o_rFYF03)f7Z{N23`T1!JQpj`R1VE3%$MB}8`P1^OyL?nYB3j(I$EUg5P_jGrs z?yg(^_!hmV!qb^e&Hr>z>iVZpFk<&(vGaM@2CsF>7gUIfO7&+L6~HMZtQarZag!N} z5f059@bEjMyTv&gk*oUY_;v5;f;Y=3uafPI30KgTAh6VJYye{mg{IdJqUr~CjiMX{ zjm$kDoWnbQj&Dltbl_q|$}d={1m6BZHy-ZDehI^zAy7V%Q~wc})e411B$I#)}%FStcf6B878Mo!Pka7W|9uEVo{Ya< zyUoDg&im?wr4f7%|nIp4qkWO_ppn4G(Vo|=peKLJaGNdQjr(iauMEQBkE37R)* z*b^X?g3q{9s99G>r>w3^>mAGuHjnaNe($mtTK2KgNj9d1{0w+sUyd$CqsIeoot`z} z$h@p9VB-Wt7FQeQ_{b9>MB69q*09+aiEb|RG4$DVshi8}@ht89F(z+V(qtFg zLs==PO+)ez>if_3U8y-EN~4e1d4=9(pmn+9qP3*3rqSss7GOccuZu1pXS;HDq*PVZEjzvKFGkd` z((-L3O+&hQpc}v|~4Av(mCLTbQ zk{cGOs;UxDr~{os@Xer$u>~)Opyz{55yAlw@1EKG8t4FEO(b*JrGZKF^MW@4y{;~z zd`7~sfqn#foU7lxhMx2+5v*qo`N@yS?P5Tmf_n z*nmJ@3l;+6Z%t00Q2{0kTsd%KwK08DLvW zd}4Jd^(ZCu=ni$Kd7&wFOFvZ zk>K>jchbqB4D$f$0{md9n-8f7vGeb!!hW}v+UBuCA*!IoG1vk~T zsHa(WJuwLeg}is)lprzGG>-CK_(Z>wF31?A#5s9o6PY!dJn3~}_-y+AMcKFZxflkL z=hocraZEVnjUuD4@AQax{OHk#KMi2Y4!mQ|9KU!Q=iCEXc))-@frupLQ~GRyZfwj) z4F>>}h7r$iI}R;AgivI7GzB9Ox)FS2z&nI`21|A!9^|)x`2E}wNgrhmSKL7?@w-;` zkH=3bcAn(~u6Rs*yT_Vx8n2(aVwPt<;I*1$Xl-xhW+i{SDeU#7<3bQw#D}^n)~AGK ziPr(zNtJnTgrry!>`^??1CkW@EE#>4AiqM&Sr29*7k;F6+h{qABB;DSNfW8Qw7b>d zLb|_iU9x)+<$5A5W$~t%yYeNwK35$-tZQGr!hmno!twr}AGL_AN0s$q62uQgHg`nh zUOtn2dpi6EO!`8VMDnD+y^%IdmTVe}*Ut2n61PP^yJsP4bW!}@@9>^@=8t8XDkf3$ zsgszQl&kYX$7TT(X0=;d#-s)ir7^t8=y}oQAT-C)cVn}4+kvRD(N0<~z?_VGDWz%N zpX6I(Emcm#dElUIF2Cg8-;KF0+4M<{0Z%V{sa}h-E0Yl*X@0-n^c9b(a6Ydo$NsQn zsasOk?0IGP(wD`DK$fK-=$E4IX?Zj=wY)bM_NTTMe1drW?zpv`ZdkHsD0s~X8k>ny zZ+t#Y!*geaPwe%E{(<+R@qz14AyZe;$hA(E){HaCRl28lyO*s@$5#xD9})dIICCHK zwFZ}OSwA|vPi-|FvO9F=1g=by&pJwT_-p@saR}UZv{pYb&~RqVDo(wR??YBx{AG3b zUUG(d#Ty8Z{*1N1w^xzMYAJek(5COj#c;~j67yV4QF=3q!;~Xq^@>AZh14vH3-i`l z-^Co$k%7h*kDYBdY9a1wt1VXKn`g9LRyca6%Zszu6kv{r1*EFR4~5Re%WMyO->8BwJQNons+BptJf8B~QG+6H475;$ z{`Li%hN!^EzyR0Wk5K-GXnA0TAp65x!y@%3+dWY?VeGc_jifn9?#@`3wsqE)<)Tmi z*x0!sQjto|GR4FFGgqjei7sp*lHYu_xt3UpoOfFCfw#S~Z=cl0V+ATPsl%?G*q(zOE46{N zZ0S>#dGNX&-}nh~ayOuCpl*T+2s8+=mmWXiSd|D^ziX81nDkN925$JMl2;q{4Lhxc zWXf`SxAYo`Y>!iDuX4+QRa=pfL1|-TB^gM%W)hpi^@usoF@kuRKZ{0v2-sCN{!zv# zyJ{L0bG8#WJE$1=#C6I;=IHF${i9P`$5v$NtF7&#%Lh~6cX7{q^vv7V5BB6@Eaopv ze>n&eUyZepJ`~jO6jr9(2{7%!cf9z%W=Yb6^fjMy^{W(En;2&|R4jGMkNuJL)MI&b) zP2U>W6kIz!Oxb#>{{nZNaIVeu=X0}l$q=A%DUoT4R4@%pjR$>=mTm3de*0jVBrwEG z>aX*U8b6wWlb!DdjE5{2cRB^7Y3;{DQ?+JKSj^8#0P4_sglHi2xN<}#^!4Pn-q3MO_&D>`5iFKg~PZP zSNoEzBQWp3)UA%~pRhc0>hBK2E_MZH9VCp-;M_Y`O_M^DNxO(bM6IP* z_bs@v!L~)Khyor|WPAkH4K3n-Rr+xbUys_xCvQ(5*mdTA#|yNsA|#zm0a2*~x9d+! zjA8V}x!PqNzfJ}mBEa2h4KMlB19Oq;M=J3ZfB_A%tfSjE_-(P6yR520zi#lD;V`i& zXKtKqyiL8A&3S%s-I`jjQTIfqEx{o|=(qa))Uhr@_RoJbMkyiaqMWP@u`+rfV208)yI_~ja-{9 zlFi1(I$FCWCWVe0avv9bTMYK!+HL*}8IH9ZsqkVd{kR!=m+4dx!n*0C+`1CH4}Dq6 zays({=JxhYL)RV8gZ4h%H~*Y&cLQcE#J_0x?^dNRF>Pyk&T79NFdSd)3D6JA~wY=@D&=Z7+S#~yJi>3j#b7=& zF8RP>0gr3io4E08?YMmzuR`hMZ1`YuH8A+l%pdQXnNG)~)VAc0ha>k-4&9yYKWn!! zcxm7_o+w?BNju>%^*<8DQqP^oee1(Mr=!Fqn=qqM`Yv&(==HmN>PlEpQ@wv`dcc)@ zAwQMl`Ev~gHBaDaC&+w1R&4<#oU~Q@)DVyfE6v2 z)@o>%mH&zQYYgYrV<6-J9_0C!y#XPU5rR;HOFYD|V;>w<}8L7N`)Un4K!MR+B9Ti-lu`1bqq4z#;RjSm?-S$ruyGoUf&9re)*CEb7*ck6u40^&zs zs3ydXuQ8hT8@oyE-^`P4c4AELx{)!|kt6?phM3{xx&@Z&4cRA!IQ?zc%+B7tz`_iA z(oMgaq+uM?Vgz@T16$MJy7z)zV947ormaPlK4oTe+{+}gL6?{PF+ZHHVQKT|bK2U72^ zi%EFdkPb{AGmQo8IUjtuzRb*DP7+RrqG=K)kq!Q!VfI{7wkzQ#Di&wj=A0zw(40{s z;Ece7aGCWaOuc>9itT~zOnC5M_@e4l^?lF;vy(D4=DvL!`S~LlfF{;;R%@0F;#`KUgssV2vJd$SWw#WPnVYNi28wtksor^~fg9(0$(<(=YgdNigz z>F?nx#TdUT6?psFJ+X!2SelD3U7(K!Y4h*zZw=|uN?MMJSl<)ugmDF(qgc?CYw$WdVDDNg0!dbFUFHj*50ptX;B1JIXy9uM~>Nu}V(rDkf&z7Y>NNpGgpJKYnXF!Ud>^Nzaph zr4vM5x(Q?%@S;gcuC5S>UuOc6XA>xqLxHJFbvJXv6&94AHb5{3!3gQ*&u_Ee1uX~c z2ojzz(UM{RrI)LCJ-mW9Z=vupHJH;aNK;>~;x6l>8w~-h#@;<&9*7a5v@V5>&<8I) zQ6iqiSif>oS$MvKQ5C)POjA-S%}$Y{a?KjOOWrEotp1}ZjlHOV{J+ZWMV)rip>$@k z7T`}EqLQ5JwZ+i2>!IOi$eP75Mi-{y`0V7n1+a))(E#j-N48Q zDQ@AYgAa0I@2V*Bt(Z2B8$(h;D>`iz1-_;<-HoF@BMHFIIh~R8-pn@3b7haT`kArG zlrdy**w(1{%Y(ln-1IMzUEJlV$o?BcTw;N=$y6!}Pe`se-+6iLd17<7o`cBUJssoYSYUpqsQlkJzngSrd1dzu;v6T<7V5|+Axe(?W#u0Je-WYEF( z5N?-}Q`lw!ECY4c95S6?mV5tsj}8SAh%$0B-OFk>uCd|t3Ya95Q#?pNlhqx&tS|Ed zU7GeKO2}$!m-k*3?)350K1U+?@jo*K?pX0VV@ib8vXZ*J8y4Zm$A6^fj@Eeslz?Py zdcE$%EHOo?1Lh9DpI#J9X%(&)A-Qpc>f(Kl+G+23BDB!m$1V!A(e&~zQec;?0KguS zdIe+#2m<>>(FO*2C>l@0SH6fK0bp>WE`Ow_G zEe04;-Y7;l1TkB^F& zj2Q^YPW>RnMCM$WymL<1>^yg2?gB&}yh~FJ`N;aEc2xI1-EO4LHF_CUCCu8;_mXQ~ zF#6y)Om*p)Dy9Q;Z5{)mKdyn0CVC(jyL124cu)51Q8wqyLBP&BecLekvcNilR4vDg zd|C&qQodVXayF&q1Q zj$~%SbshclM!@qN|6KgTk{tI3duk-+s|5Tr&&vl!6lH^diyK;3cEn0OARRy4(Uj}_|T2k%>IzfWpg>}z%6@DN+Jz7?;dx~RavPCa=Nlhs;jDi-w)E7gzx1RE!Y4d zCU^gYftTkQTtH3}RSsWz1hNi}{ABc~v#^;4v}I*x{(4}gBwtk#3TyVaDeL}X`MPst zbaZ}DktvCL%+CzT+nJnnyia5;!Z9bDd*qX^R6osYUxX{ke<*ZY*~>(e0%W+f{X7zC zH|R8YE>7~Xu-92?7Vrah0^|+3ac|n*5$x#z5Lm%fgph6==wS50K!N-$gB158R92Ax z!q|bBPdp=QJ9~`WjC?|1d{bUdhG_Ko1YnmJ#qQ$bf`&wGp(Q%&D<5bD?RBU4*0{6v z^zbYvwXeKBJmQieV!Au_;#2a+f;G|vIE28LiSmeBr>yYj1yRv#iNxVC7Zq?SKC`%u zfU+F~iM|`3^&dqqQC@Fmno6Gfmizl3~e$mf$R3{>!dqW z35NB_F_mwrUolscOP;ZI5hq_p>#tG%UFQGdOWk*C%E4}at%7VVVDUPGkmJr6%@ zz>N6@+OKs3{EFe1+`PXl>rSEBjNPR0bm`yq)vitR$ro`?(x6@(7Fm4dT8=0Ve&?m`_Ah zw4ye-%Np2GK*s=H1VLFXXc7Mj`-|oA#YI#!#_O0vavqNljf0=~>?d2nv*Qz2_YhGAsNwXYrbO(NYlH zOD=`CJ|ny-G#>r?`jM$EORL%Z&w1e4wEfmDW z!U(~KpnWN(kFP=pCm49##RG_{!+%D`%#&y>Zl+qXv z9ZrjjyOmhdKE)mETkoOsPJ`cnrmH0Q9~Yn}ELP+#Cw0%Om8s#jpoV%Wrt^ai&cwop zb21kCT^#!5O6;sZd3t#%SLhl?95!Z$N3=~AuuHh`Cm)3$2;DORNDvX|`NyxzqMuc% zE_K0v0FmMxIiLl)_O3V7^L9!xbDA7TYwCrL2dFMaEtRf4>UWy@DuZ6lKI8x7%KeP` zTzcAWF#lmq%Rh!oNu3?OrDrrU3*qz4uF@|)-(E|a{gA%7A?r{1hh!=&ES`j7bpFV3 zK>qj2#=NWbU zyLz*J%Ue|>^TTaXUk_Vte24^qWeM+<8{fwvj2Jk7VAldPAi>24Q}sTmAdrfyq;x%_ zn1Ln<)zR@IV*;)nEP>(&n|zWHxdzxZ*Pq%S-a5TrsZ`GPp3rZpr@ z?@U)YkSDknB+@nvK;?tXPxBFRSj#cNU5KR4a%ccTjUe!QZvc=4OxqG{@)7j*gUk1; zw(>`J%>cv)5>eW9Gcg7<7|tTp_IJUpjqc^eL@c!AF1Y&qe^D-I7gQUtRfJzi(O1_Q1j3 zccO!X(ZDC?Q`@}Rkuu@Y=RVXjyIE9()-mQCR*kKzl=^d4qu6fE7pbEHlSSd*dE0{v z8QYRi*{ntw3<>WCwlay;-m7T{lHHHaJaEfk;dw!Rt#HwJ*2d#no1LiNMJDfK&V#i^ zXF2YS%YiPwR`!d=EvDYH7wgBisD{SLUK8iXdJa7I?bYgVU$NWde^k;&-LSR)@c}P3 z=~A*eANGUInP0}KkH$wgTZ}Fo%c0v|GF`3NGW#V z_~uqr#JAelOJ3aK}Z39Nrp_` zKG8>7`uZK#t{^$)rFuHOaf6-Wx|$ppg(VhmptFPk?a-}E*}oZ<*fza)zV;t6Ke@}U zYP}uN%IdG^XtGqt=X;iTN)pT2B5-5^{3+k+pSt*@#-=lf$8>XQoflks5XPxIWUpECA=$u;GIYo4g-6*Hb z*t-RBe(UPbettSI1LaAWPK2(bY=3KDcEZtnT;|C}&E>R<^P;DBPQsH1MvTeklqq=| zE>0ibCaMd3%o*^}eXVweX3ynFq_koCj&YZN>0v0xUk1cj9cpleKY*r`q@)&LaHpc%xdS&0!g|h{1((m)Q@$Rs$pGIKJ!kHJo8EmxoM5>Wddg zWH1SGg>zoDHWxiTjec0!1R-!>+d&2dlc7~&25^!EKx}SEg+Iyk(f{au0P_I*0?f+vDY35zy(lNdK0dK% zSj^9_-kXYFMD+#aD8{_|JZidXq`$sfy744U*zQH8wN(q4-8*~d+@3S#4x@j>182(V zsXoZ}b8jbsX*f`4?3S2e!Q2F7ff)XPtw^wM6DHtY9r}ifhG?qu4D_?tX5W+7UacVzx7P1!c`rEk*lKaV>z5C$5@Tg~%P40C;$xP2LP!AWW zZ>%Uxa%qqW4YSC&f@u~IQ?Bl?-~DJWd?}1y`&k4QD>$J#(>7eTwle)zCm{K1`Tg7W z@;A<`M@26K^6KJ?F61vN-y8aKX)&&g8vi*^jmCFQu1a+=r+d5!lVr3F2$#R>Z&@D4 zr6Ll@rF!zMNE;#DR$&}`-IJQ_=>f}&)+J@2j=<_!HP(4s>FE_bBmN zWd|}8AR7+s=*=M%0(QXN*ONy*u5xpCcjlasgg@+>P<9%15@CoQ>N@Ir6WHdNuuXP( zSusyjHCu}I=40jy_f+iFHD$GAPAu>nr&6Ppw|uq&DZ>(TuRlpi3e^oXDHcp!mF!+b zW#CpIX$|zEPBc$4ZPGuNneffrP>1bv^XdL{%%dqE97L3~WC5uW8Zw9MVExt8i35B~ z$S{GS1oTj#vxX}I=!`EL8)dHk9Nx8sI)vNg%Pk|Q-10Hec1sEe>)d}4&Y(h~%@Yo~kvj<`f z237WC9wv}iqKJHhdwF-*o`v3Lu=(UOkWVQ1dCf2`Q_!d-M*|a&@&OG+7_fKYsY$(v*xXR!3y|& z;w6`^YTcI86P0xd$GRc3rTvq0^Vw*X!!YcufSfnXx`DyAdKWq>cxZ;QZ%BBTS^n;M z-v&WZ?IQ0i7NSg(lQ(?X=JY)uJjk!74=+<|aWWPZ5F+{nLS*MkM*2IdsR9@S()*dO z98#r2oKl;e7{U*<1&QXjY6tv@V?eVkb|--ebXuloo&2~0#oZfQ-l=#IrK^ZCP-Peb zOu*my7q#wOmO3XwZ3796DhCpZ0}Q0S+wUh(i#v(2`pTT1`+kYHd;ovSl<>S$haoIX z^jb=18w)d7By<3LC9mGd(d(VYuMiCE5p{jitHZ9Jn#4b)zx&+D{q5uAD*H@z&d7$g z3@va@F%-SnH!4us9k&fkHZ;xfoj?lNN)XAmt7#u+@KRN*ih0G zu}6A7*W>tXfxNg+)=`khiXtwqT_6ny5} zeKmc3RXp-xFMjticUvve>YnPH=|?q0E^hUwM~{$E%F{xMbdNS+_RH1BbXVE>H5+;r znfxiXyrtCm9_xv`$|YS0mwys{2$61(bHz{(;W?FCozu(Jgt|~D9NeRScc$>%$KPI8 zY|OuDD{Dy0i5@(7!&f0vd_CQ|{QhqlM)t3Z*l>NhwdDb^6y!M03+pkbv$C+dLnxO9 z-uAwP<2lAMdhLV9c#tNVy27#Yxe2rq<2{!ZG%;gmzkYxP(&0W{D6%vGnq6pt10rky zWlAJ#6?A#_)M_eKk^BvI1KHOTpNdX`ZeXH~TIX$sQr>1O_W}<-o-=kYg9B!4IxlS( z_^}v%8Z0o@uCE6pW`T*S@N}k%6L}w<*@(Ug&8XP66z41aZ1Jkk#!Ax*1Z8FFWaz5H zx{F}{2fcBFSgS;QEgH}P0z+xnDJfC@u(j9aDzwmG%r-U*H*75Mt6{|I{0(#eNCCe)UN1WTGO@OHGN(U%$o< zo%Fs@Ph^U_V;!ODGc1-Tt};(S!*T{x!UFXy?rQ@Z`<3T53&f3lo7QpN8x$Jxq%3VI zMP}a&SS#(*0x!#uOwCH)BFAN8t_-wx_$6VMOqru+dB*Ca$UM~7yPuJ?T}m-lU?Eyc z^G3A=H3Ph4ZvafvvPy~Qu?mO75xTFgE^Psz=|pS>mldVXDAZcy0b>LqpmBVHaP}|3 zI#bx)LMD&xkrt@FGP{6sU%ufuQquekj~s-}n4v8g&=r6u+tu_=gDq1eUZ zJLiZ3sA-5+2Eb>eNCSC|>+028(C18w>^S!-QUKczVZVRcXov*49I`cdmRK;SB}oz+R-VAl6LdtIz^*??kF3!ClGU671} zywR^ekdlvcxJ=~^)_B=J%|T%CN3F1zHa*GFhi!_KqFz$Xu1DU7D;M=X{o1!eHsL@3 zhm7`C(YOVj(fO?za6CvAwj;LfO>OQ?h3!)3_PklKdlFe}V`iN(AINNJ8@`$Si9P%_ z)6z=c%=ND1vkq1=?gTqj(KDK!cAI)p0>3wou*_<&fa^%U;7ywXy*z&9S-<3lfEKRO zv_M=8S>bRRnOYG9<&vqfD>f*Tv0l)Xd5<*UH8rAeI-0;?Zn@uYqDZ?iH!trgnc}xl zpm6|T=<*?>8so5mwW1ExxP6MT_le5y<@5xXB{rAHR2;sTx1 zKw1=cUOY_^v zh~@d&sZabJA{bxhJ7R>>+O&HhFdI=HEf3^$K-j$-@WvT{&<8Zl)?oPHpLul&qr^F5ab~x|CE@xsdS)xwnbk?sKi-gmYQj=`ceHSL%>S6@bt{TBCuzRs1(tHUn=N zlmp5M3GX++sDDVw2yQ;G*|UPbiR8TjqO6`H6AiZuVm$?4ErIPob~LC2db6eJ;e8@z zlyGAJ$YD4MRG9hJaPotZ4-L-?a{}mqeW1E(1INs%U+Zza@^?iKllp=8L?;Ji!{Jsv zzxJQvRDZs|zIcqj^2s}AAc08*p0jrra~lHujU}pe?dQo~ucsNC9Z!rVdiGN@0#}P1 zm5m`_{kQ!36Q?ocmWg(2O3Eb@p&Aw-t5cY1Ho zM~kP`A~PNwFv!pwD>?>ZS(*Q_m);|ogn-gU1`~u6F}UniG)^cuv%$snd(J}w3B~Mw z3-$L|TnVGz(PTtzuR%xjc&g}-G3on7x?L{%cgcQF_REDB-K*h;B-Am6wQ3eVPU5L90TKSB9(D( zV>$*zd{E#mmU`2W#E~G6VPaMpoh1U)*iu+K!!EU}rA^gU^2Pk<*DbT1%z=Fq>8h+N zfe*tnR5=8#+6e)HL!n@y@&p_O5sxRQ9d!=m~pWe)BsY+VKIO)st=dND>~l zombRA^9H<^K9lp$x%u7!Ud-`z-R3+~`!UEJajEb|#83gVYtot&N(I2hDE zyuzbM*=NK1a#-_7c2t6_bs17Q;63C!tSF?NX??qVc5&jDf@I2S#jgNBUP5-MHuANK zN<`!IZkz4AB+4jo0FVJFZ_i6tk-gQ#Z4Id{Ecp9~$sQQ0T{Eh=IqC@(_EK#8696PC6_O7pJd0%ydGnV2Wz5iN{0#@J#7r_#c;p-cLwnGL z!Y9!N0fHVT9q`npNy!(Z^4JlbH5>_pnFRmiJsACj#=AyS4fM+|`@CsKP4lr@J8L*Z zXG*p5$GIUjW6STJrfKu&d~90V5V}ymqmf(>G-7w>|pP zc`7_f9k3#(I9vj%Be@srt)~3yKZX}h05$=qQB^Xyk;GbJi($1$bVJIh#;00KvC;z& z8^mW|s5^m$UU8`Q%i6b5!VTTmY`TFcwk6uvWKCv|#2BE%?D{Gs{P3>|-A&M3LUFKI zU=KV}qZ5)>r&Wm0JG>BOf9C1sb%*UsSN4@OdFQuDJo>qyScM}3Fe+kAJP0!&P;wFX zbv2PbwYYDuy-53}QDvmtFv6-1N4S0fDCl)slnp`NmW!@0%Vhce0!@~s8&uBK#xJ>L zC;g&DXx~Y+iei%8sx48hIPu9r*B2Rx?iTm~@irI%m=Izc7~n%l#ielh5M^osiT;D? z05CF!i(Qf@C!56(Of=jUI>|unR(N<-NZ2UpyQGoYJ3r>htyt#<%O;E_V}iQ{7F-2` z&HzjP_vz+b`lX|Vl_njn@JV4 zjDrGU?(I>}5mA65llX_(%eEi(moJ6Ajqze3PpcXLI? z;B#a*42wBBIjQcP-lA^@XtO~2W(MQXYmA|ek)z#JTD*k^Vn{souewr?F#Z=Rrj<`! zmoH0tF~=#HgAXC15?T;+5dBIAO-+|IFE{sX@F8L##&K+f_y&<^9JOHqF|jbD4}_JB zTy}79FyXN3cqk9Y5TnT#ra`LJ^sX*2jkEnf>!2ZMR@zgkU* z7zqP_2yGd$uB5)@u)e;!+5!fruN!+_kB@@8sJ%!!8WZEDrY2d2uaLM6-CIAS&Le|f zxf7Gli5*>A?Vz1; z4dnj3g_LFo2h{ulY_kuRI+`-5xbQLUO#ge% zpFxgP>sNJrh854!YEW!D9dMncsRBQlC0Yj@p+My7)Cboo)F652kD*eqQwa#6l|Mb% zZ;S6s1$5n^B7C5y!Hyu0sMK4UK2Q%mtvg!MZE=M^Pn=yvV}V$<|3F;TU)eUW`1(^O zS1G99(9NSI8vL5-Xjg${IMh1;&*Vg#>X^1)9C|{hJX-B*<}2-6MUf@m$hY!!ABQpiKrhCfoUHl2tC2SNwtr>S5@@DaUnImdq%CO^Ob}^T`flfB0UA4 zw3MVqI1croo~P~4E&2g|TwlH&HRxs0_3R)0@i9}i<)0C=kNN+OnDkT+j{Z8RW=T+zbE@|` zKL4)==fMN?3lOww7opg8!SwhR zfLMV1`WotYp-0<}{}QH^zWK95I(gU0{VWYyUlRXcatOyh$>;77{1!%2znKe6H(BEZ z5*5CO7M~@3mrV)yd*~Qc=)LTQjfSL3Q#DbQ2tY~FQ`%=M-5<0Mq$Sz_0ob`}!J3H~=HL4+mrv4_3 zUBs6quwXVvB1cQ24Ci6!;$_EefJ<2h&^7w=>B2rB>5Uzkmf4t-r>p zvIE$}(3V3JpSTjN8TuE!GwNjJ@VA7vM2fQ~ry(Za>4_MQwR=zTj+xBmT^#;^T?nnjq)64@gq<4f3jFr88SX@4WMF5=q0v>5(Xojx^K4$Bkxr42FpaLL1 zUPBY|*Rf&OM?WtWHKDQ_RdUVR$MA9jJT0E>+&YacC%$1NPQRZzE%l5x>{XXo_gn-} zs|gP^fT9sPS};0tZh=tdp7!QHm)Z;bFgd1_m%V)$6KhC!i5FZjL>dnyBeE&MJcMQ9T9uF zU|KcX5~|k1N}m-HbA_;s5HPx!fd0^Ax?5gH{h>Zo%W|SGv<|(NcK$y8j3BTX3~3}( zbUCne0_+IbFAAYB7s^Au0Bv~)!ipXu_k@7a>L(2O7gpvos~!VU0^oBbXs6O{0F#Er zv52@(Jqx@UZi398Lp3!BthR)Og{Hz?1Zc2O@El+7W07=V)VxX^fc>6 zgNA}As$|KGhQyzN?_;BF0^puU`F%YTMZ+e>X}AawK+Yv>^N^3fs9+rw|9tZ$}BbOeT~Dso?O_+~#U z%TbebNDl!QY=dNJeS`0~-I8STrhRa3>>V%u^BqL*U*R+U7(_tL2Un)W?{7sZ_ZZ}& z?7j&Izfo;#?Yu1XCJd{R`hL^cK#RB2koEne*Ud0}JlI}P-qRA-6OYR7+sii|X|GR9 zq2&UK@ZB!e^O*1KEx5JlTET>?O!J|lG+0v}JSou<{N#rO;b0j8qil`KPka#a1JFZ3 zNy!4m6BP5y{aJ{i8(TP>G_ZxF6_m${DJix5COYwK>S50{B&0QsiMcx=g`CDSS6V=z zsrwHoFXOrnt_NqoS`Pb_OC%I$NI|K(NSc|V%l;VZu4czUr@r3t1lOr|pS!t}e7@n- z;XTfdI|WNZOnoD)0nVo2(-!n2H=^iegbqiyO8{j; z@15Mmz{7Xa&Mnk1_&;+F{RmcV(=E$^yxQ=H}oCFkJfkV8JSCk{n`qN zXzlzkGHqZ!Qd8{H;yAOp+m(#V{`I3a+3yN*B&Qq(RBQV$q(vS8B~Ts4>o}BnCERQ|q}K^@fR5zG6KPKV-*6T} zq-}gB`eixuD7EkR0>R0IGp7YCAq~32=r!-eto4;5+Rl<&-!|hAY}SEgQMZD2F<~9; zSW>{OSIumFxOAuHGd1)}aE+tW!*u{Ul48U`6}*zeB7851`X@^LtRAKspb~_Gb5Y^< zwCU96U_hCeXukjFr$N29AVL|1+5$`z1cyXeATSLeyTm120U-?~BL~Eo(80li<-Nj_ z)Zg>_$8LXqh;xuQr@p)7IJcBo71LS~q{-@7vUC?sKWx0yi^KOkO}~_M`L*uydF_c; z33!eTNbN)w@s2Mjvy*F-(u*giI8=bQ9Ad8m56GH?L)kThG8CYziWU|u5P5n5rL~rti~2oGzlPe7dg`70z8eA)Fl~_|2JK+d=b3f7I2=`NJpSmd&&~<} zNMMIJhF7CGi359N)?3LrVYhj16opB+5k@R-?njFZO*vi) zTgdfEROLWI4s!-@VE3aojFcd_^Wm3<-yAtYIq}(MM}Z|eWnC#^2T)gv*vWuX2OhB1 zk!8j0O3nmjs)zmn0y>pKd<&un1pNYHRTM1GiiG4L%rC~6mgOC!@oR^e6tM9j1(OR8 ziC3!<3GZQkMr}?wzQ>?(qnrHkBXf`w-eh`#1q_P4nfy`N6TTNWF5ddme-I@oBr2Qe4XEY;E0o!G@hBIl#{^6x0KxpdxLE8h0m zw9R{Fm+ga%xZI^tz2LIrYg7bc9DuHAFT1)6B&aAUZKO1w4FEzgwh>jj3bMy02y!V0 z7P4xN%w^Qq$w>(Lb8-4vM+rvi@X_}fzJJ8-=LdXBtDGfKrZ!R&NF$A9=O5q&|~5hgmL!1%%MZ&y*<)NXld z>Z1IiXwn;hU!!wKEJJkuDnAYWVl%9AwK4ov%DHh9Ifjsdf6>ygKVps#m~Q?+*tEj( z*Wy0W!ypWy$*gJix1ygD-^WQ=(%YveYbK9}pnG#fldV|1w*|8g4x5%0bfH;s|3cVgC+y?MM=*K`|gdJjHN(yrGKpO%@ z4*+2YYYy?WDGqRbnXUPyHJ$^1Db#TT~YqO)_rIxt(DwGlK3q>)88umg>pJ(7dP*q*cLW=vk zI<0iI7ehp#&UE`3HVPdCX9mXDS%?Y26u3n6ZFz?!>CP+YUYo-3I8e6n%E9>XndA=I zr;D>~?4UErg*w~oy<-~R-;+J`N{W7GJZf^!=pn{#-nd0D?e=bVyvCS(EJ6iB;3?Aa z`k-i=r$K*)Nn>--Ioys?PsX!7<=u<%e%yCnjjuZ$ zRzb7aGl1i^)A~UfxGwNBLscfje-2!eL9mb!jRhx-mcAR)NZ2^2NDzxy6cR>b?D<{S zX=e^>=fE=3x)zN-ypZP&vB z)foas^bZ=`#TekxsV2x-UVJGS+cQycFS6|@)j=0k>u@DKcr;8ic%eXqn}~#sl^=W+ zKx8kje<;RFrf!;delCLraT_ZV9DXyT@4rdeGjLz^MrYMeRa$ zL2=-fHGt=TWqtT&zMrSb0Q2A8_IG0Z*8B2Wx=tkD-JhxCo1`hRzdm(Ta1R?;)=I~H z`+o}A6>EOiVvj@YIIP2IJ;jxF{4{ghKewEr zRt^piCvdPiXDR^Oq(zW<@-D;-7T{f}A#23D)Wym+e&{iWR_FNeMwhvpM};$4Iq=;J zGF|Cbp@%G`e;I0+ncf9UJTHt%99P>Q!qPqax$=;WCGmS&(|bw1bFu zfW!sYbF+CYSuAUpL$#WBANIry_S0KS?qL&=+gFAnyT6GC_R&vaW&V`ek%G4 zl!!<(EykceTs(6uHSL&h7{H3>@*LHl&%lUI_tEPrb5a&&Q_FYSPKl$d*p*-)7M7LO z3zx}ji3VpmP>EcZ<@wkDACM-HebTKcv|0g^2d*5HIqbvXRN2l~!r~4Nt{jm1f!Ki* zmTu-~2~!!m0liU0#^M_4$&jVjxOw$$zH`C1YTQ+lO?QcJ2I@AJ=Pt#1iwQDrC|N0B zIN1&UhlCtc+4kq^@;e4{cYe^G?ZF%GTa0Omg$kxvrTK1Cef z3(rLFTMy<`T;rdV`K;BBdLFkF9s`TWIS7ihC5}4`GqpzR8XFVBv;dL947ZM9>+tB2 zaDo8-mip|-RTF^9U?=<)T&|Es>{bU;M@r*~4Ums|)2|c2)M@vkwTre2dODb5hFxGw zYiDbQ_Ah=|(&KnNmMVsq=`8)Aq`+b5jU7mt5Klj0YqHBz@$;5K=P(q(yw}TXS8q|( z)t~HCBQe#{B3PGH5uI6WWGIK4#|=RHkU9)QkiA;V|6}SepsHNEHc)s;i2@=Gf}|3H zv>;L1a%q8E~B_0->L}o#9Mt;(3`=f;eyKsZfpe6xw!% zR8~p&3H#4_N7lyIHvUI9-t@zs48EA2-%qt@fIoF=-1mv-Jx^YDo@H3?(_>YPPl|Ky z3D(^u*sKnN=HfPgguE2K=$bs}?SSJAo?XD!10`pUN!}${pC6nHIFaCZ1M0t=+043^ z%|37wgD+Xi``;{5RCi(Cheiwb$0*YO`9Fx|GB|Anmf|-K%XU>YwTgW?qZoIr5y#5v zK3t*G`|ai-`(+uYM)y+lR*i^P6|wBzaJ8@Ioo4zJ{~rw87@j_goiTO+Mn+kXMQ`(4}8^$UrBY`d@Tt26zFn8g({(wMSF86+aI);GtR~oNnynJLyP2{b zA5+P?ed;y_PAzI@_CI=h4uwQT-PgT_$AK58 zxqOk>(yG4mk3Gf1>_jQp@IUyRoq(O65S#P(&ZWbjPkp`c*xnHi+J<_HsXQh$H{fENH7eqcLE0pko_ zN+wO1)DfILRqt+k!OF>KI7QexL`kI#av*XfiEy#v!&9K~J~}!|Vz^B$>M0}_MNfRQ z+^x~c0M_Ot?XD7<}TZZ!kRUbpTuj}b)p5GDpr!H*tPLEGST8isQ;+7YyNf4*@ zz;>QN3|Gk95O#{k2L5A*e95{QyAsM${zpsEMB=Uk2R_X>)1DLlCv}Y`7XN*%ml$^W zDrrrxZRFRmE@bH9X+lXZg5Qa>36_um_6ON=S&`KzR%-ljtHhieW>3C-68WNbi`J_R z?pceU#u-wb8m*ruhg7Gf7=w$b3w{|_xJg-5O0sVoSB)$jE?3`_PZ0uOsjr)bpz-2_ zsfD&R06z%@O(B3nk{n|LFDIiyN(i)0YZwW;GzZDzTB2}(kg>O;OaJp_gOWxNDn-%u z9bSaqRA79r^kpYQ*_(8LyIY{#sZIwjO?iel((K&6BkT#apxofZ3x+NTrYx@O4G_v? zxI*sD`{_Hh>#&6RR;Bp-N0z4K8%a+eV}hRUn~Fv<$keO(pze?dHWYdPkfRzb!A$>5 zOv?)FRVIq#-YX_p_dOSZ(kl!+=0kY-BN0ob1WwyOyO8f3bUTX8QG9y0q#oj+B|twmkrV}BhCy5a+sO*1wZFPvKDtKjk zZyRIK&10Q@KlAXC#FN?X{wG4FLvzZgg;n8kLZ#)oH}Q2{^z~D2uZwQC@I{`Jj-%^) zr{U*zZ&qc$M60(tEZ_Zlo?4r}Z0#BJ)pm=;y?RIH#AM6ele_AFBrq#u@v;?rtyKI@ zL#}&KeO!^ZR$58O&!wz|etzH!E_lL7cq5^e1@RAnTezwq6KtzKb$$W!02nQSLCkOHb(ve-;QxM$ErN2K|Yp zH&C38)VzT}^;!TH0m%quRc?=Ri-pMYR=_X8VQDnO29TWg-D4oz#8}J&AFh4TmKMOa zx;lZu=QmF2fn&?|?`O5u7@sZ>c3@&F!ew>%5)cQ&RoM}WbPEeoxGSK(-K2F8?-m@x zlFBUDm%xw2XpGT;nY398h5|s8Cxvw<;@5~dGd}#XuN(ax`H-Uc4nsi=UHo9xPnFYz zL7FKCwY8+*0{iu{f)Yv7^a6>Yd{Jy?QL=oO2?fQcn=8xD@A;m%t5|V*$<^%mbr{S5 zVH=n`(pwz@c^510#A>hej8ph zK38M5gu8+anTz+FJj^iPHXIvN3GTM4H(&8w7#@c24SP8-Q02OD32i9hnE!XMt&QpD zo3C}_Tm&|f^uA{7ir7AV9IyU{u}FlqJ%lqYR%S{j$C?4t_^A9p3J04?V~zzy?()SA zTUb^zeWw@e9O!vIyEqoKfuwQ@X8IxJNHoxQ-JY(tTS{sB9!lsfu9hII52m(MM`e5x2ZFp0Y zaIp>KX!i4^NJs#vP#!S}c9uxvH{gJ8z4eNsF(yutGRQ$|!jZ4UWTn7K5gnV{G3*Mo zW#!+FGWhTadI68g&u1CrkWp4v)=GZi&=2d*JE7dlR1abUBv~CZ>wERHEYj#dsGe#i z3x1EiaIXAQabP^u+*SAX3z@=D0Wua*t;s~-szR+05frv@>fZTR9w|y1Z1T}}{g7Zn zisIzYA1~<9VKu<%c4Q4j8zPSY83wK`%Ji<~e0CQR5>W}|g$+-57YO{Y?GEpW_q8f8 z108BqVAY|@tKwBP&vWGX6A|2Va9JCbeoLYm$7*b(H@+(rf*e0_TWS*%!6!WM+o$uv znr~eeee0!}p7eiQ0I4oB>Yl4+s|TJf8#h886yavBt6On7tmVF%SD0X5XzezM+xU0K z$!<-0{mAn!zpOJ#c(A6cu(WM}=-6F|PW7Omx$33*YW6zRD%O+mO7GvbEX2YE<;mPA zMcltBv!|N)sb|2-HCt@nHq$tr4WAE;C-L%qBi!-z_W6z97ZVxwrWB?++ijm8&3E%W zzs|uZXE=es(|y;I!*np|1BJSq?fLPvEj5BYsWashpMLh0mI|`S`#8t()zde$&J1K z;RCbhL45c!$C_%Juo2db!`n{&*W`+WPuxx@E;i?9LN*_mw(C~k9%$K1opd{Rsn+yA z8Iuv_?(+_8Bai(%`0zrnrAlD3Qf|BFy)~106*!#mp-R|$B$S2LtFP)oPrDxue8F)y zLUC1ceo<-h5=0QF)5mw_M!mt1+;i=HJvzu|qDIG1*}3m2K$tD@4Uj-^k<0*oy#+eJ zvQqMdrvSTB&_Vx41$#HCUX<^}kv3>otAcYF0CGTi+OBy+y#qZ1@b^q2=X*PGdacDp z&=g^hgHkTfL%SagSR#-ZT7Lf?L};CP{mI+W-@ zl!tqpM&FpKu-AlD#51Iq6UGzX!*JD(i}K6JKaxAdA|;Cb`qFoYy|!(@6uZ zcE=#?cwfLTt^}OeOnh3N5FCPcrW2Vbt28#<{%&MOQ|mh)a+k?m+)E$TBtB2yWzCzf zBm*u>mkhoqRW|1O-gx-$+n2ez%>P!r7W(u={jXeQI6VKJhA-nxFOes#v<-^!pXt5n zN+hdG`p)WhTi6+EyR?(j-U9Ka_#81msG+3feS_!U$#?!MB5mib&N0z*?#5)%?)8=$ z5+;8HZff`Duy8QHuGM~KF`mm=EaiSE2~D>|lH@u5_!5z*&_`FV+8ZtZb_Y*-PCxB` zJ3HU&{eXj*>)Q1y$Lr}=TfXEtGodlc6U)1vXI2-c(?ln8y6SWXO>TS9x%JPhix_2}Z+FL{lFPPc|LL8K zUOZcvxz>0xUmW+Y=Xh_$Cbe5R@e%4w?80fw>kg9grOz~@V?!hA6v@C&f zGuw$A=7$UU-UH0c^7s#|n&Q{je_b z%dr6D8CLfCk*(e%b{$dae_9|WA*&a{vqr}gp+AQIJ&IkKkYn=Z8Qt53eCrWedjQ)h zhCjQb^oHJwqs)=gS*JvcJ!XKSfWBeUv;i&&Bc2yONtPymI3^Rw0<`CEp?g>j_+xp2 zZriW`gR4a7O$PdIHhiBDi5Q>?)~p2#k7=yRT~_>iLo{K!?|R1zC7F>=J^hnm{eV7^ zsl~#599B_;f#B4I5?rEEM2BWsION6vg9J_y^f)l6bN3gOmi7qlHu{c^j)u4iN$`l< z&1n6|A)wQ?`Vgs40%x8{Aw>cNM2CWmtWmKtjUwhAFcKZb-SbwoENHbkjP}hA%5J?b^Mdyx_U}bA?Lm<5%v2# zJRZ?wY|pdmmi9ON6AQ{$2M+EwU9Lih3(GFc?sNqzomxT`3xn*0 zDy~0gN03|hl=M>QIB&`^ry__SlP!d))*3)7?9+NMyeOCogIFfVGG0!bigyM+N7sRN z46W^e{l^kkF|m1vli}xv8Yh2y_I5Ylc0Iqm;48Y)oA3Hy% zG^E5V5BS{)TRip@-_7mePhNb(DysiWT6BW+by-Ti2(Xz7$PEj%{6+i)*`+t*#N)$1 zk+zfS>D^Mi(<2i~knESnnqjE5B-xju2zldc_Z8R75!p2iQ7vCT9)G*NzU%vm zV|4DveOIk#tF^T6*5{4JjP%-*CK*mdTQ*&<;`jcy<{wyNkIm$Z2;gvE#2MW*^)&Ck zd6^}umln#vd8g0sa(&>DuQiY4@GQjsA(6ox>ax#25|rX~PB<1>?s8L{+OA{>snJuf zjtQ8FoT*l~Rig38-TGV#J7d6IP_9K|w6sf~cEAh+Zh$pPGgy>C3x$#>Km&`itCvgU zYui0u-f7tFw(&fqgoPeT3?sYj$L9kZPRMkQ6hd(KQrVNsaqmFT8xTAJJ`4$16rBpY zUvA@q7JZ$A$l|hhbR#K{Vs^bJcVg2di*QyHR7n1~7xMt$JsabN0hXRLn9GZ;v6P$D zaljET#-b#zinU_!lYSO#;@C}>JEZT&VmHsL8p~*E##gXKci(E^L$_R?zi@%r3k$6$ z6;A?&gDYTr0P&{i2(TOAGU+Ea^^{(CF$HTfP{gA^oKsLoi$FkttksJ#-nEJT-clty zyK)t&Xa-Z(2#f2InTn=AS&r;#HXEE2D^>V%rPW45RQfQpCRnu%*X|Kp{qT7ME7cTYn&bEMSXy)@<+ylmS=RoKl%Ho>G4k4!GLaUt@4@hX^v~6Y>JLS)u*-XV=>%SoN$^>DUPyOkyoZocP zRf@lWc9YX!2?OCvu3~%l)d*9^EAt}TjbNMh9v|hsm&-WPzc|u1CR3L)9a-(Bi})_) z$rRELkB66Etv**=PR77(4!S<>L9LL~NTBC&(gF#6SAH~}2gCZmdt6<}J23BST(<54 zz~#PQN$I9df3kbEPe3f+c*miN{}yhISGM;f1gE^^FM7kT<5!YZ>gdh%()7A`pMBz^tIAhw@*d4+qQwuD0UTm0 z>j$p$jNS7D#)!C7OI(UKAxJ(#DbG8`CUJfA&t^V`9}C^?V5bzTrc)IY3T47e2A{_= zUZ^ynV>KBH<%dNCUkFV11cFs8OC}p2CIuHfQ=;!q7dHe66c8E|7?%_;6(n%~4PNiI zbfTNiI&e$|pADtTh&407)&X(LvIX`G4CG-{0__KMpOJat8!05)dG7Ivaf3!aNUjQk zq9T%3b77V1L~8CE0g;!++<7TXxw8e;QoIWEXfNEmi?)EZTV=bKAyjc7DF@(Qr0EbT z5R1Wz*tESb7qTm!!H2rzUC@yVz%l@fkfThIa4q=2#DP4rIQ&uNyzvy>wJY3Nvg$J9 zwc*B%ypi1~)iG6`!Hj`xr}urZw8~cV5nqOiAJ0AKM{kDW|iFzFod^Cq84-hv?+TS?) za|#@23!M#^+3Jtj&uBT_eXFPDAF~~9I1pBwR$S0^@uJoCz_HOO3-RH+`9n4T`9jkX z7TXz5EK50ez9n@@q7#;l)1QTeItQ-Ub7YDl2C1Pg7oj+-a{Qvu zzx(RZgbupyL^;$A7@lE6l+oCj^p{`2f7OKWJb>N>7$ktXqLbUD%>Y0EvkmY`Ds4A1 z+<_Koks$JY9LTx=BEd)GEOzGO94%TLTT${a2|r8z2^}REethA4d5;kJ97j0QB6B3R zw<>Dq&_3MJH%{(*nRPlnqM|R>Mw}r+`Az1z)O;7W$-K$EYqP-3R*{HN#UstEW`tc` zr8a|7LhoHqcf~)V3CFG2@zoI82`>9FG14#S-MZcfzBvFu|4W}~wkLNS!9S@9)zdh! z)xsO(nwu_fv=qfh%-MrZS1qxG=RV=p(UwYli`Ug2v2FML=~!#jhR45vb+o@YR4=&j zqykU-D2wxSSUoLgi<58u3Qy>=fox>vg8pbr!SMTN{_PaZiA>uoj{Q~!!I8#7f|fbb zw!Zvy*jDg-}O4ZwspGJQ?`UN-$SGz`f2{E znLt7Gv}J#=^pd<3H-_P2)s1trde`V*tXEKS!*SaFzE`X8)t$DGYQ46EmsYwwZIAHz z-OUlhqJK(Ve*4XuTb>9w?Whx26LB4vw)hB&?zvqY<~9fF3AJ1w6r}E+_dI)k;=4X} zGJ2EgCPLO$zUTUj+Ok^CErOL4@l{;0@9MR-b$d_Pxne1{pNHrZa+%Jp69!+z7 zl+B@G7WQVT%ElGX><^fx06hlQ0w6pf`8TBOfB@!O(15Xp0|{#x)bd5(Tq;Llj644v zD5tjH;RnBVcKm_Z3$z4KfCBIZ1cESQ?!Mla&Ba847{E6e8xKRRwZY>A*4hA5S7fQ` zV@j07iAl8v-0pjl!n-m6iw^l|cdG;}aA8Qn$MW5Z2eF4KW6tIi(2e_BkSKgvN&Ki1 z$y#6^B0F+&Rhi77b{?x>7X5v2&67{-4)>vU>;Pu!a8K3F4k-9gxQ1;LNT*oiC6u`| zPr2((%f}`rdJe`N@4`wBwOL&cw=CKB{MdN_=V_F9Qd<7AxLQhp`;}8rbAkk56(>L; z9OOvIeL#4d^C&v2-YXmRmS4@D3I5SJw<|iS45T;?y6_5I<&if$k69l%|6pnrSzNfPccl@hb7|;28b6K<|qB<-8W5f zg$_$kkO^8v&Bv4{FO05#&YI;ZM{^cF5URQAvHap}-R8`H@kpfnh}wPi!yA{)El&0< zf5pZ>(|K2`^NSre|FBcF?{??5T-}^WoeP;4u@w|C|1np%C*|(CEUMqvaOKycN7Oxg zb;u>;MVo)G@}{Xf6(yyokK>T;Nv&J^*UjMYg+070Z#|oH#>Hw9pKWbxe5M@QlR`tCHI!ZH)ZW>aZMQt-9Tq>?amOR_4nd`(!>t@ z0&uU*zP&q@pwaibQI%3IGh$roje#VoXQn%gf^{E8u>2m_vOEzKobrh08P{OL0v3$^ z#~EPdqa=0}CO9b8`KOK#J=AU?1X3!ZKtKo6z6U0Dpcwh}3&B~h`I0f}8@px8h*VG& zvG+&~!yp4dL*rr9RufBpWUuujI}To?Y0=#(dJ;$TM<4X6$ljY9q%s@30$mw}D~M^iKli;93|Jkxf&f-Is~b=K zQ=cdfS@-f4-o^)IFY&ylOXU-8$?Qkc$mcPo@rMqqo<8jA6?urK6y7h|b0f}W(e`Y) z;~VY6F#d1RAA`z<(z7Huz{(7TO~?RczKJT1L6PfvVj477Fvj&%4vQozUcZDaM{z~P z$nPAlYARM5itVS^!Y8lhmsbRWM1swsDfnG z36v*fzH2FVe8?Z|r&@1JwVtVUBeE$rUAP{oC#A+NIm@WFfSM(oMUNFe-v8JA>wrdN z?my582o%f07^gy9H;0SqKiCTte-TJPl1boEoW0GtW|Y%>E5usET%0z=|6p|p2*i=n zofV9J1R=b9Nc!g99SV!<`6b$a(?5qtZCZtM@pxGW8MAx_t_c+2zY>UJnEEL2bg}REYXW) zL_{Jh@OOZTF+K$4{2O}$)$e*##eVc=Y%tuUGS5KjGF#;*v+1(R>iVba#bgrtef_k> zG`bTM>GCGZS1`dp9jn46lrB1UvBd4o(82MK;9+-fikHBlHOWsH@G{%mk9XCK!h(gE?i#j zM3GqET3Zo3ZgkJgQ2KQ+0pHn?$Dt_Qe8Pii1ytshVCi@AiF|`)x+g+6+~2WK)R}kH zBH_0b`UVxz_R@<-UQ{j4)H_;fz5Y`RM-IMeL@RIQv-DS5RlLn|J&7U6S|f|X6}+pK ziA`0hWA0})BMB-Mef_-J{7@x_~gVp|MlsE~+76jR`?gZ?~aq;ix zv5_cBEG~r7JLj*c6`R40ic-Py9Po~5Wn9d^-+Ty-B&_(s+Y0xY^rKH97ajX)@6>uJ zDldLDP>h&$vFSY0>zubNzgtTD|FS847hk>)yoR#009H^0T9mW?W)?k~o-S>0pqxVWi5gs@)vBtd~s( zN+evMsGa-( z8{G+%Xb9y(-1qrDtSBI*TD2cuJUdWk@Zv$TAU%pX|5LI!3uU1Jn{x=T^C{Vc#tCJS zG+v^csP-0y!N^MQKLX_8J$Td7PTUF!k_j;zzjS7B3lFm12i}qAF2+X4qJO-u@Z}oA zPf-bP#h>8!1e?@R*QPn%d4&W%!heCie5}jPjoo?CuB8tI)#pV`?{VO#s6@=x{$#J< zGC67h{1s5x_%(daX9+Pt3#dCe&9cz?`8f8+R9+LD{seKaO96(|H|{I4<)O@?@kW-K zuP>^l@3mjxja->3cBvS(|8jm9%f-BAcGM6r_eX#vemB?i^|J@Y zi2t9a0}HMc5qV>#;0xhGj8DImK|h`eiM7L`S3-#8Cjpl%$o}=xV2h%Dp_Dfd1_adw z=B;250++)af%fySOD3;DijQ^uIt4+!%f2Cm5I$}BrJUE$KnV{Jz6KCgLxaXvTrfMh z7CmX2!DD$Q@9AJJtK@a!-dtrnX_)e-<#CKd{B^aIa&9aOPo-2aXLIj>RV4t9|9?+P zJu!>}3S&m!-!{#3Yl*@5j~Hl_rP6BBxhwOHYD^OTwph=^S7T;paseNGY>g3Lb!rO8 z#oLM24V2qNa0{hB%k+`#Ut9|e_~8M5@^?cn~N>l z#x;FQjOJh{R>`d}ZPu{=@hn1@NFlA*95?j(4P)MLm1AI?fjoJX{?>kuc2ylhYOn;h z|EQ>S2z4}#vA;O6gCP=*N|K=St@;w4N=`5`fxSCCgK@r!%XC*st(Pk79vsjk!5S^+ zRihf71<*Nf{b!r79SayHv@qOqbea1l7Fyj>6{V#d42A@Kt)EP;OUd=0;0fY4^m zz-|lpo3K_0ek_qF8$V2BiK8!(C@qN=csZ!zb%@8MSBb|nKlf2bnpXeaP~i6Sg>#uL z!E)J3$5w04fpJbv?N=!mU+MJhmS$E&GlUA>BYrnC;w7E$Gz`D;q^@vEzir0BnGxl1VS!_;8SB>Puy09)2jY<&a|kORQKn-U2~jkfzLU-K5>uj=o#gaxC|(^C(O_n`OC1z{A#<122!TOL%TX3Ug&`a zu}MypHP2*n?dp=)v{+bEnL?yidrL<23Uk-N`Vh>0`Y&9djDcN0#9_tj5gBTYZg_uDc7rBJOrT~ux0=V zLX9OTV-d|C*Pd!kH8x)GTvQUD76CCAYBcytKn$Xp)LaH*WL&K*-ruhAWV*) zJ%ZW|l!I1m3l_`0zO^8dIE#b4GTX&aR0DVU&qjKa6dPkS<3ss;U9j=Hsyd3+-nhn% zoWwm;)G@fh_JGSOw=peG`Bl%{w12H*^hSH_-GLawo=C%~Fs(0JkKgD<^HwWiJx_j6 zMNiB9LPn~iD!G+YF0F%Q6?q+r*o|*Sen3YT*2(P<5isbW!$;{HU%cqc za6+Q8uGus+hT;+vJ7#96x_f%KxVe4c{m`+Dd}k+6@9zOVL>>Gsr*a+k6|vvCXdPlc z6SJ(Ws+3yaBkT+0qKk_4TpwU3=JtDm)H0SD(Rx5U8fIO>7M%p80utgt>*_PM(K$Iu z{&S6liz2d>=$*vbZCAUaEloJ4%b;}Wa&9@-jTxWab^>l_Ldq3=;LF=LUZmWvg8_SQ zDVXCoA$3;>ou0yDs+ew!sL1R#{a1Gy)fv&fQYZ*N8)tCQP*Y1iNyb8~-88G6cH|+o zoMn}fdqNiEH{>?=aL;g7*}Xw(^E1V_CdPWPe!jb%hsKEneqXNzIR+;ZEqw(^vFXga zF|{Ot%%#{CsIELfse~4T{O(;$+qSO4_6-SjGbU|*(D(dDcMX@Icm0U}V}0KRH}$@| z-c7s6s8DqLsTuCWi;;SNsfifROgrw+k>mTdyabRN{t3LEC#`EFw6l9!N9`B|Vl zy=0i-axa?}XaImh&a8TS@HG?tyWXy>+S-ZluEvfk=4@OKt*A$C25<~Cv_6b|WW-YT z#rT(Dr!}jJ$kcoK#6uXcKq%HHPy@SWFt56|djT6vB94}eitq1BZhuxN4F7_7YP0>; z+`JQpBd~+wxBt1k%3FB@@;ewA88!5sN4Wd8T&$v5`NqH7e~cGsObSSo3@0$I^VKZV zZ-bXlQ%8QdbKnC87`TEX4g@uT+0=Ko+~{WswU(SXo;rV&(8$mw72{+pG#)P5XiWsf z1~*694-ZJsgMNtZ#yv_wF2534Mjj)16XMlpq|BPB*!NoIl70xPA%$)$=Gm8$Fo@s# zz%2C&@-xfdF=Gv*R%>cuXV2}aL_L!txxOq~f*%Gbc;IRk*r<2Seie?9VP_A#S#}XO zHTX>wrM-kwYva5^LPh!33A)a`+^7(G$Vy0vA-kwv<$1qv=Y4V>G?cC(yfm}$f>u)T zK&R~l*TAVkm;-0ZFyg6{T@9&Ewlswv_Q^i`yP~RbBVl_Dwt?97;pH22XD!*;_c_)E z+I+z)0&-kIp$&gqRJAF0i!DX(4P`ODmpdLOkvYmv>U$2rz8{uj)3mdo76lgloawI+vS>Wd&&1}x@{)`b^ zGt&kNB>lGb=&*rR&96FouZ&@eMEUoCQIeZ0aWu!JA5vN@LQKYDq5Rc-1=Sx^tB76? zvGJdeV*12eFww5vBMhW^<|mEKC4wQAgnn_ALCBD^zZTv{OFec|-daV0?oeg(eV_*m zR@WSdRDS7)tw|6el`p@o75p?qtK`Ep^vey@?gO7CuZ8NelXK}^I;1YIE_tR7`xUYf)+`5~WGnQO>S++y(Q3mIi{>kI8!9-TFi>>F-g z0R}Yan8AS$VUHP0+uRzoPm1*Uy>`wtJ^Ls#fQjS4b^bISjYq~+!o{heE${!=Y@OvyFG3|1UGiOI$HyK zmXUYL;LS!21BmhF4CW#)42lt!P1e$)r~`INw@d#1R@BCiF%`R>(YtQzg{gt52<705 z%dbTupVg%z;w^F`%!0R4I4jtA@1Un(lNsu5keUsGq84gyKH*fmD_4iMkMcY6%mYG@n2aXEwN>rn0-Kd{)_|{ zYbuzmRAt}9g^jn@64;pt*ZvhhZ(`Y$MlKv-Ztv%AZVl{_=m^kRj+pcAl3%q@bje#y zs2dWce%PgT9Qe!12GM^{LeX8P8H4 zzCZq`C|q$W;W2v0foHQcB#E#`R#g>%3-f-l!ysqY;GSiHodnafbVsV*lJg9~n4Na^R_|Dl~FHG6$y;GSG8RymosiC}os(zY-9vdxUMkNpR@z?VkUY zU@jarSTgpGgV0qjCJI)^D0cSfxBn=+q~#iHJ<$pSe{mwB3ITZxzvR5=>U3FDYamdV^skUmsH|Tubi0t zha5-%=WVUvwt1Kp-PZ@!vQ-=3Q`EdSx>6YELzLPF(8Ldy`&sRHyZ!oe)nD&wly7$1 z4^Vge?sACgAx&SfVsw>+RxGTN;aogg_YE0X%bKgxO5;iwQ9@YIigqvfcCP|=ll!W<*88^y-=?lp9 zA4Zi1CVT(u&~)nJl9RhmP98sCW#xd!=W??6iK803J}y4K4T$d7jQ{PPpPj+2tCatx zsu5^X9Hv0SHEhXMVwpYkw#LdueYv?*Kc0V6k|LHg;21U~AyteiFXvKT4+LK`@J{cT zoYY8ZY;*?=ep5uy8_LkoBKf9+_W-rHEQj*T*1oC1MR{(j)ETZ`uXyP<<%H~&?}_R& z9@2q~ipL&jyxv8QgWtas!v~zw+1%I=1KNOt5zPJV@cFp-_yA!J1V!TiRHiBg8re!L zke1Q0?X(POAilG2dmz0~L`aAO!MLlp@ z0qwgc4NhIm_g!%OX*^j9c3poEgqAqy4;dohA%2rOoawnHkgowcef<6p^LgmXQqL^g z$pKsxut40J`?i^i6dpr9F>nfdh#68s>N<@V_lH_x9l6V<;*Pv{GQqcu`Om1Uiyi!w(VG15^ zoQ8cQcpU+3#3?L{GR<65^H@Vt08lr%?F5Yn6~Y#N2$~N#lBkv(p1=725I=8m}IqJ%i)`vkahZ=HFF^-_NLTC=WypyZ)027dXK+_jNJ_&&G z$@g3l6RjP?{cqyf2so5x>Jis7CWt(HhOe66Bntzp_Ry_2rt`oSGjdiTAOpq5Fe z6&LS()9l2QT@; zRN$RLECe#8Ku9DI+kVlJ<_7ri?#|Rj)u|xJcJMuq8{m~@Dv{!jM^;j?U_h4!bqFR% zreOX*BjKK3{succM$MKsS|wyK3zRF9E*-rHD&Bgn#JhBRf1Mrc&rgj!nZo$Zst7(~ z_S9gz7Nr{Dg2Zu;pf}MVk@Z+!4>_Yj1&BYg`iW ztcBgafoaw_XN40Q``6H@Xax|EA8uC4MChiGJT2a#*kdRBh8V9Whc^w0xu6wd!feVju z0(PVZp#iaS{;Oe1;oZeR=hF)=%J*3T z0RxJrNGiv=;Cv2wS+u%*?VD+TAa8>ERoSEfsOOy;?t*_T%sG-l8n$-f8xGOPVtXp9~E4Lf`LH@eEw-GYXejj*pB zoX9{t+la|>578syKLVTFG1|FMK6i8>gbhHGuiG`#2RrG1Ix&O}--uqGEEkDDe$l>* zJz5_+*-S4AypY1aM#h~4fgA8mgLyQkeE^e($kpkvxT@+bTd?U`iObcKCBFOQZ-C|&EoPYC?w;sZ}zJd@%OKiLx9LMQa%?nz3&L_SJ(=q0kR8~$sa!q zd|+i|WzBciw7*t}2`+)YB4@bV5Q79?Nn-GU61@R!uz+oOx}+g-CY9x1Xv!8~mV3k- z*>ZJx4kjbViP0G--e%SW5 zc0J_Nlm@fu3Q1}#{B>ln(VN8AB0m3+6U#6ue|EDeM#U3cN?aEG$TW1mfPF9umz>?9 zy02HNhf^-g&AM*iVR-bzTyAx`{>x@5xs?n?v9^pb6DiS>OMkaBXrf^-AJpDN|A6T3 z8Q|n|qSn<-W=pxP$4+%?`Qr2tNO>r`XGB_hIl&J{$H(->vZz`N=rst%vxgD($#VAy z1(y5g0sF-OfC=^MdG8F?nN~dM+GJ>$uPRC3n%UpC8?I!3C=}|x}oXdgHm7(?nM7H$YSG&wVk0YPT z=Yr9tsaVj|lqRW-_vj8}Fn6dwc$O;)L;;xtjamsvM$%3Ld|MpMfjvrth}11$pgYRHhTbc9oR zDjCs0=r8u1EyKCgit`?DSAv=9>G%_0AHHB?Q*fVwT{Faq07tDz#-8+n7u`xc?$QPC zdv?p}9y4O1KFx(^t-nCZ8ms3?0j(ikXsAZr`$uPPZi=h#4C$i{EBd8hD`FM59=Iny z^|ZCB^x(FXq0*+$y8(AAUm_ly2Ee0W_y%C=|>o$y&9 z&4cx|?4btbJh%iO`_7#of8kye?UkIBnyBK0Iuqi zn$*9oU%$LFn2`ms9J%%-jE#iS{Yh-0&U9Z@j6=-tR(y{j@H6e-jDEOw`8{!fK7joG zXf7DUK>H091h&qwTHQN!M@{Yp1#EluiZsYSp|puK?~VEzUS8{!d2H~9+Hj>N2Q$cT zY;?3;r?LKtAXl`!QSwCa@A`Tn%~Ntm_w(;&^rlrM6g1+$O0UjJ!JRPP!4-}KUMR#JahJv`O8 z>2jtw&ewu&eUFJ&ovnI>v+^3lPToMX)jw0lTf<$iJVU=+bj<}28fitMO~c87Juxg; z*GislDzcGA5#DXwx_AptLFuoUbE%cwT|P^y)fNqPyX z#(q0(VN%Lst4oB~a?x zdm|$Wpvr*{AIb{`(&3;lb^S15;=%k8G5T5ANyH1U^W-u8wS+$7tWB2a7xxH5($bxn z6%iiD{!K+<*}*}BtqWIK?D&Qgm3<*PUTwu|UYr5W^LE$od_9k3igNw-=5TNYc{h=L4;dzjU1@6k_kFa~2OV5O+_nzYT_%(mz#oY+^+--`>Q&PG zn)C`*h;VchJuPZRd&vA@eN&`W-Onl9=pw5ozw7>tnYg{w^DnE^Xz%Z(aFUzN?3(X& z7${D8k?sZdFG=vdvkS;fWTDN+K+|Y?@0e1-T9bbt{Rc~qZw?oM*ZL-(<@50zmlCx* zmd&N_^AjbZ%FYsh?|N<3WXGKIaEj_hwi0S=DXCk#14~`7$+`iR-Wxh^_$PyzO_!_g zp?TNVw}w*re_^^o z{axW32C*+Z#pjxu6y4q32WzogDI>l`qNZnGb<+4ba9#`!iEsXl|Dd5Jq*q!3-TX#^ zTuU@aUzhh=O)D3p)n8vjAO}sxx~@iO{t=+EpzcMzfWi&BN?CdNCFtRwDk$J9W15e@#IG07CZ zAM|XT!;FC{xo}0oCjx*w6HYfOMFG%`{go&MdUD(h_!+Q4o;?K9&Thc{;qAh~odxfl z@zMFqqgs?zr(yuVMR38k@zSj_jJpm!Qo@drWe zPN#pIMq}Go#aVf9`|hLD=o}Ulmh?v{2!&trFB3ARSXB{mSSeTKFdNtEyhHz1`6MDV zTrt7GC-QT_s;P2bVwd69AoCxOV0Hxnno3<~^!{6A*J;2c{TrZ{dRu@CW){2W={GJVHM*9Z_;{6Eco8#Y`93$pw;#Yg^2e81r zyDv)2>MobBUrDrIl8wsvqV3ApH3-23VKV{|MkAWyPz)iIP->^k=r$kyZiwf)U3_vf zZ`>9lXs2(@toU~&C1&Su<7M+QNSyGVR>EgDc%p7*uMn@2~p{|z4c%I z9sN>#=jinEJj1z#O3(>+#GDv#zR08Sq6U~Q6jfED*AE%&kQ%S`%q_*n*4AH!QbGP(g)LUg@raH7mwoHKJ>OyU z@WwP<<1>>5BWHBciqEE+y=`n*w?uu0|IKa!#Ij~2=Jt8`n9uK2#Qx2dX5MMjW`q4~)%n{*JcDAvx z33DT1AEHaWjFAP zdq3>djatF1D1X6R@(r}>8t(Ub9Q&0%4ul<={6U){L~2E`lZ33rxy<0YRuOa3X9a{1 zztC!#PmOYYsYndfC$As~W!DM!HvCD*>VBZP6c>j^ucwM%c$nVH-TE0E6KB{YRQ2=r zdSU$Ft+V6`y^ZKuq214_y1JpDUTa)_KOsULUK#i7BOnTZLyxoDpx6^re;3cY#qa3= z>qaxk)u&@op90sSIL+zTWrp|OJuk*617kc<(mzPWAJ%2A@Fq(dX=^906&Uw)owRi*+qKv^Ipr z__^lrO@9zlYhp~{Nb z-rdSWag{Yv`Y7aqztdosU5;Bp#1TB5Lt@^WFCu|&Sv03-)xlufE0fG=zx z8JqaUWy-FScHoxD+-D1TEugLX5qh41srWYDM43*ky=#(VfJCZOGX2|T;nrL%fs#-F zGx2UQun|FDtDLgtTTp<4o8Yi7+SkGJEH~)D0zMk|{W;Uha);`rsl9EA&`?Ez%3&Hu z5vd645}>=oTftZeNjbE)dtcxw7;Wt{@3~a`<>TecD5xR5)W01ra>nfM?|)#}zYnxA zK^NEQ>FI2-tc^2sF{v_Y5qEOvxEq|deKMFs-07!Kk{}o+p}9d_6^g{*X<~Xl`OP~l z>=r;x&;_9Azeg|Efl9W2Ut-XXezf{OY!&vxytw!~lR@(1 zX~*AI)k2QYJat0$M}4o@USwWg(G`{CJ@wnubRE=N+FPNoHZl#VxF6FNry&txfs|X& z@uC^}bCdz!i`meh#n8)2@|W3MBFWS9pGUbTt@(m(|)%F@u|m%iSPqZ2$?sGH$sAyAl6Hx=pg z8}mBR*LuXOo-GyMB*o!XATzqfz9z)3QpZc5PU==-iFv|EU#<~RF*G2fCTtwq7R%rV z_5r9}Ay`y`$so#85N3D>GBx7+Opl*%@aTKG8?PGwjdR5cF#2_T4y<*}nY&NG9NP(q z6#}_zq5FdKt~lexho;k0W!J>f(E|n>RPMv(&5F9O*rxRVxBvvu)&a?N9UTEQi>L4+ zRo&*4O-*?U!XOI13k1c!c#q-=T=QXcD2J;|l z^!-57 z&Lf>et%&dA$Hc@S9jTD1@oTyu>WSo>faeBPXh5`VV>vOhclg|5Vood{VnLvqgHdrt z7G-Mk5_bks8Q#rz`!GklQ^U|7bMaD3n$Bn)DN4Dxm=Td>%FcXv*`nE? zHEz~ProIYgd6}@3mGgH;qva1AxC0hl#@J(#AIT&7aIMH|T*xf~o=ko&ofdW9M5}cP z?KT;RE{S_tXpg&Jo)Y4gQ5b#7m9;eIly=-mwq#viJE0finotX^%G z>X#$^qT#mG!aadMQe{DXE%Sn?i-BZyWxaj$bpI(n=F8)|vL7~?wm{(wF}H9!n#MtRHL%Dv3bJ8udaBjFMsBwk8` zO%}1V|Ji87Eg-YG{T@wK4W|1sT00J-|HZB&s+bo_6Ycbl*1~`+)GIXlg_+s3^8WzxPf&}+!< zVYv^@$E^#Oi_P$ZkUMNYci+-xR=3*R(621%6bWcKxwnb1`6OyHaz#q`Z#J-@{Be+- zBeSdDUXyq{`ATp6bJRaVW?!)!n}|&t@HVMGF3+|3(FGa(vrg?;FONtd`&h=B{wpB4 zG}~!v;)|h7^uA*;zi6A>V02q}tB~zW-?_K;u7c5GBW$B?Yxu(lEdf_n)GH#QLPu<8 z^BbM%b3)?ZQ`;AIIEtO$I1)Vg&4M{6o>;FyP5Gtw)7(9Db61kziyaOvwr8WVXt1y-1q3uY+UlCNlCuxX}(r7Pc=#%!+c-e0m%nkbP)ub8vb-iSx@-RI0M7BWS<5t+x z^7ytbcS`+L2_kNxg0syd@54-#6_n&KoS6*+{Yeiv08!G@_kiWQVdL1CPV?znChQ0s za&aiZ0b)j4;4=qfU$~iy0+wnCA>*w%bQ;kyYlq=-0<91^0LUe!e2ed;r_}zS5HhL> ztlcB~Tsm)v_;x9sW)ML7eUO;^t5^bA#>H{Z~rEsCnG?L%$EA(8&*JW zIYz4s_Q|SHdk{TlMNS*4h;cyMDcjEedH}sIj{Cl3`;}iWtEY;3fUGa}$(SGFT)KUI z8@=_FhQm`jp>}3J?28HFg;qawf=g`dScD%5%AJZ)afJa27Z_f49|-%{@T=c^`sa1C zI|pXZ`L(QfgQLO zBT-H~8X6XVpZkSZ`dx>L?}*7~c`f+@gv)_h^!enL3V{!~o9_-{_}7ux$<+0kK4Sjuu_w(QGGd=bZ#fwJE`zP0uo|Hj#ga%lhR+}!eK5+Pdy`B1w{zj- z-(VkCQb)GCc@!=`z8KR$OrSV)T?G@iMN)B0$l3L8b++p-mG#S|hf7-hA1D7cO27x+>?K9ZFmg!V=VnFrBl70Zz%(nix;>ezSR7s{JWL7WNG!6k(p{Rs+4N<)blXx z1&+!nwPK+?Ly9qeZw+?3j?78>An9mw+gzQcjEu~1f0($_0s?{b2j3DC@+q{3OC(K@ zQ?df1MS`2T1wY72` zFs(t-nV~U_{H^eMzoS$6OGH2E=0~=Q)Pq#Q1zzJ?>$tAk#3ALHrLh`pO%~od_9HQp z@TxQBcx+hCNY%;o?b}Y*m&a(mX6nGI0~$mXcVuA{mvdJ zwH>$IIz2g6ICcRf8t5T_dO$;roO15ELqIZs`nxf1&1ho3UfqOT%3hM&Fd#$bLm-Ep z|NeEa%N$FnIEXmzUzDRmMYJW$jK39ju!C|)87ly3x;~OoMsWThhB1dIMb)^ z+HJqI58U#w*C^=92 zh1T!vSp4MlFQvyH`rQ?Kp@nE`mB(9F-ZD86o7HRcTgp&cVce#@JsqjEvCZqF5M1vI1pq zKEtt68wGG5v~V$K?K?aC2teF~Fmh29k?0^Pu9}cOxn(j-5D3U{q#RPPml-2%*)txr6&10tt20S|@4Y9KuA!h;OSNiIZ7}RIz360)bY4C6=6>o1STXP~ zT)ZkPD{ZYLF{(|qV4FmS;4a+E#W-ujb?_D2al4N|n`QGF5o*V3K)c7YP=+D%8api$ z!B%%rlq|PH+n!m8uw}`-Ld$NaNo=1Nv3}gQ<(jvrpJz|9p}vyZHv6(f?M;Df8rtoT z%Z`4Yl%a&**KJSsylyq3W4dW|b=z0BT4CFU+VG2sh0eGLNL`g+2a1^nHF#90bpFEH zIU7V5T&~@J9%lJ4@N+Rjbf{~$zpoF^dgH|ERyMn!`0=>U^urc%`>BeqAAdAY97m#_ ze7?p%<9l$-kGIcJUFiH;!e%5_`FbIfl4@DVd+3RcH@c$jdHlwvqL~?LN%-lBZpLA9 zVe_rD?2LrZABMFwEo@%ku44@^45pus%y`T<2mcBihoT{zyFytr*6j>QIdoDc8a|<+VA?_c%BS_u#@%?tz9k1v1kp78`WINMAFgj0B8NZ6 zZQl_0Nf*=1N;dv~2d#e`k`bKWiS1@`X9kJGA5TOX&7G zJCUH&`^w6@5JeS<^>v_pxM_7%T27yOs;{jP}m?G=5{WtfBub<`t$d%GNdlSH32e9h_goM=^-sl zU7Kc_dYp|3p~fK=I3*#k)6^%d0;vIR)I>4LbIrTt>B z79Zhzt*M>GddraIpc1Sq`6JVROAHQW_uQBDqPSX;wI#axiFPtp%j3R;*Hp_Jx^+tq zA6;5fC~~HeMAUV^P~6X(yw-(l=cDYI?JXH+^t)@ae9b5Jue`VKztsHwmR>9p`&-ZK zI6B+cUbUbMQv7- ztr3OuK&5oK7d!@PCp&)1!3V>*Fjt%^s1P$UAgB&j+8BEMQj;w#C8nnO`(`QClt%4d z97fpp-%5h%{~g-6|0#KAAfczH1EDVF{JoQ@zzCc5SkHEN@uu?$1c_Ep%OjkZFn=Ga z$5JwX3*_Dhu@#DmQ)SU%yyF9wN^Ifz`m%7S?K3DSjbh^#M^NlWRAG&l^Q1 zH}<)BGX(afWn_G*^t-f&N1gJ_VZPM|DioXGConukg6?a|<+co$&+Y6i0>1vHx{IVj zgRuad%vA=8R(c@m0UBZB)+Aaawh?y}ikTmg_+QkczX3|8{T7;NJMHsm+2!qTQvL7x zVrt|fOfxY5`4G9A-K;ugTj(yW-$c{|R#Ep5 zw0y}Vnc9H6WKCoiWBpqDm)NXRkc|emwDoDEQ~ziF z=YkhFlbtZb+VB1bOPlfr+^>$@I?XcO*S^5H`xao&g9ku%IWJ@cDDZOIrRWM~o< z78b6#PWIR=RAot+ntJm_W!8D1Y46Wy20PgJyiKEsCGM3TSaq(p2gm$3;18}c2M98p z2NyrvE@^yW7A)8ZC+W^2p&up0$%0{27Zf!h7=vMd>u$6of6WVE$-s9m;}N(?Pds(d zP*{itb6(%JTw;x27;Jj)XU+%1lS%2R>o(mQ!W4pvfl1&4YMVR3$?1K6KM%^2%Xt); zORj}wEdQoW7WP2|w2(P3&@Ao-1Q;3m!Edhh8kU=bqxj$SXkYeVE*5-u!m zqdhp)Yh!hS!9~OwqTPXn-LJ3d4Q==0M~a=FVzvT3jaw$-vFGInb|&-MA&a)xIA-S~ zW=u!l)ZI^$>54Ga(;DM>rLJqu7EU`ymd*+PcK$MpSbXkxp?0yT$C}1v7*tnf zOL1{AKhN%QM*5)D&%q*rEbp6zE@bbG9>=d1sy{NvD2}7bY(KcAt*aA0LDoL)MAG@} zjw9)MPjTI^A#Rgb!3z1Tzi6QX07HP516JeNfX5pHB%XH*x4^tO z1sW8RO12(Hvh&x`@8?4r3rTNjsgpFpC4qaD8vXb!{0|&bf$Wg9&*!@v$?d+jyW9ik zpUW&?*hT(hqp>bq0kgC&+Si316;nqRef5<8Pzv@qyR(@LJ-Spi|Nf2sC{to>671v-A0xDdl2ssd<4!j4X{}$nzt09{6L49`-)Dz$QJu_l78aFlbw|^< zsN$Y!jet%#5bglsZKNUdsnqc8r2LHn1#!k8Z!t#{$W&?)*_2H9+RX()&O$=?!iy1$}^+{;E ziy7v>X%wY@SUC|WzZ>$7UYCTwS&~Jt#V+Lc@^1a;@^PIHs=V~vLk(wS6s?)(s@{)` zUGx*q?y{tByV=mT=^9LbazE;EEe6dL!o`Kv08+K4^)ChKTEzl?ZC^9U_&6bqW%pTp z4_1F_he!v1@vlS53!_9oIoU+1c7~tG=!^N^wHAgyfWfYt3 z9uhPXxxIe*ME&;dY8z{+VijG=OiTogev^Hbt2GXr%9}VT$*`3jY{3UYe)qWaw)a*$O+HdqFVt$5tx(WRs+bnc>`)p5M{FbF17!VR z1mk|O+w*qS9U{qc5RtJL$oXi(7;@;0!d@!Lgv;M8=?|8-EKT zltGH;yFnUJ2*@GlW2+_l$S$XiQO2VL?*6v4{*Q(v+JmTAlsCmfjA;ATn&(X33GAO= zaz|45@c{J#l%a1Ysf#{^^SU2o8swM@YuOCPKbDO+{E`^%?GdrA5KK(3lp!enyYDf+ zsmCfA1dH<0B^Y?|28j=7sEXqc>>>NaCAcmKjsn=5w z4C4U-{qR9BQqNS2!OdsUk3H2`=gO^hKO4AmKzz4t+tp8vR}FqH{?xjnXkI8?p!vv% zqPvGriiYEpu;bAF5BtaNf9rXB8A!egf{<1*Lr+7mRv@tVEkF{#Aaw$HI&5qpUF!|H z#lgB3YDSdU5gr6jp>?+>K|Qeh#zm@Y`hx2njPi`wu}A0mOhZ4RTg?iiwGkwgtma z&H}EcPNoIJ0|IhXfnQrl;l;%YyDS7Q_w4LA z#aMhbc7uUS%5Q7Vjs|tf_mDZ-WsN!;biZ}pw^Zeuac#OR7$hwB;4lr1 zWDT9>bxh7-5D zj}&QB4Pb`?B@Y0JyRmfE$DIoc@RaDk{T2qjDl&KvOyb|$i4qVI^8mO}Q4!Ns0KvM1 zfLx1A)pQ}sv*mdE6lO}4Hl%Iew*l&2`45om<-SOouZr8JPwKjN)?duGBH87@8#YdQ z$!rlm;>%`Sy73v@`&CH4A481|nf0g$aMy{5^}AT6q@wD8a9EL;*?#U5&;3miSsR#> z0%pTyDkN2W@0RyWMC@11I;e+o!jR;>?a|d!aLrGDtSvInFjTB2rMO1$! zmY1}uN$r0-O?K1r1`+OSLn7-YP_-b^8b}#>%al&$Pf|PK{y-XP^!}UO+uNJ}odqJD zat6Z)42;d96Ec8`dh1^k!o0z+Z)wHS;kmC4S8`ZbZTjL0a&c zR}ZG*wwim&DYm5k*_6hp2e+CWe11T@;~}8p9w}5U-G8U>T^{FtH3w`uh!@GWY*Fd) z?~2qc!kB4+8E=}qCX{y`k~=nKcX5GD3Z1r%<>SHAu9UWi`l#w^O5yq;UH-$GKwJ?l zf`Kr+E@igF4v?)9$M)zL8is?;8FD4SWC3A5$pC)}pMjj=!NmnIgNnISlK-cQ=!Fb5lPc$Llas|I z4&;UYlr6#gO<_`B9UdJpm5|vhCH;MyYMSuTR#>Kn!tN|uOo-y=L}_X-Z|gq=i(<;; z_jOgtf`y#hb3nOW-biywcpUwMgDqImTNP>dv~Mn zQ=C_dY7!LZ$TU2VL;)iS&PZ-%N<<2V9q40$Vvnry!A2fP2|5oRfFhalHw3oO*$`W~ zIt-cmrDu{gTKO*1%=?2<(Ae0R;P=i#5aF8jjl}u^zjYz^aQkq&83=#h0bvq#zve<^ zwyMJZ9+=0K7^h-W#q;$41UjI_e0U~9tn?8j(5PWYKdGJ3hPEBBd1NdE&}1c2Ecu#u zP{Ak^6%oY}p~M95TH;vjFerBaYZL!K88KLJB=RKoK{A#F#6)oW(j{-h`3!%tGJ6iS zGyQ*?GM|4mQcEBr>|}}gcJ9Tl!Z)3PKjfRFIAlyT*aw0i>V-Ha*}FA}enR43+0$hC zk|m(Mc#fqUDyj#>C6o2!ULirxdH8?AjRVckj~_oqfBlMvkzVmM5 z^Pi&RrOVwkj#d#Z{au~fYmeFMYU+MJrs1I+;w%&p`mQ3tAQ<|GZIc7R?Q+9u(RVSK zWe!(8@)mPJ@cng-6c2Pm^NkQW$63{MCaRLQ5F$I+PhHUSqCi~;aCgo8kkLz0G=ZFH z3*!h;8(6D*@4icnJO^+PK*8{E9MD2g4nK;ZfsTXKr;540TU!ouTD!@Ja)tqw1+%k*!CylhcWlZOKrn4H>-}NU z0*9xXw%okaNGXB}w6VpMnK~)%ta6cC#Ma}~d;$ZcP61J%YDCH+U^Z*uz`$U?S`YRJ zaF3arnF- zg85kGGT|$<*O{gshS@e1YWWpf4=aJj$BS{{h8i#8yS_QA2UZ|3<+Ijgr!H4-wX;0DjxVV)UaxPql(mu zN`CuJLQUC3>)0)ZNAwxBN^@~E1j!OG8ibF`@!nGdei%@pkx3OzO_J+Bv>j@J-Dz`p zJ5ioXM2fi7X38Eq4*&_)62?JKkU)hnuU>c>vl%Y|9JKyw^lzRreISDTuhS(j&vBEa ztHu2l#c!Vh)=PO0({5x?1SkxU)4$q>d+3Hl&**H{7~6)!4_On{0CxanOf&~Z6G#jF z&8Gm20p>eg>pv$8YZ?NU3@<&4gb{E&@DJ!!6FyuY9o44OQBAR1S|c+o;9*Z{$0dKh zC>Wq$1zp?=4NXm?Var{xIZ;mO|Ki*O?k4bS0zD7B-}rc3z<|VN&d|ticbR+-DZ~OP zB(S7G8}MM=q@<(ZG?h!Bm-)Iay}dz&&#q=UB**e$t9E)_f21Y+1`wufp2ixLl=Bvw zK(hwvM(zIlZK|`W2@S}7zklyEeS!Rki3#|~j=<-8D0ja97FI850{Ohu;rIjYQNH9t zx%a~4q`ecHhhJcRAFoPD64Zs6|91bN!YqGkl3SIrqO2@wa->0he*5z#Z}ZuoQVl~= zw_;?hCR`p+##y?$rZfuociUB?p4vrIn6-hT(aJ*LIZ;Z&=%~FH<7S9Ku6y?UZT_1Y zZPOk-SQu8=9uph`Kuz#CmbiZaB*e9k{jolPxTA*Xw;?J99Ab61uu=YItqCHM{9+R(8>j(%lgRr)_cP$gcVOW z>_uTUhA-{ya;BbEc;wgwp@j@2*zTJo@}7MAHc2u(A@UFDB6Sk3R_a6xCb#U!e#VXbx_YJhKtD;Q-FKG) z7}w(7_~r3@EHXqH;j~%aumnnnyRhs1@%Sc_)+i9Axg+@XX$HLguApIno0PdfY>x?t0mH z6CQKc(tsBHz90|6!yVwVFJLlKmGmSs`{rIs%FK=V`yu{&9>EDQ-QKQW3tcICEO95< z_nGMaWB&wu0$dyh^`5~!4ipJ)s0ff;R`2Wc`JRu@e>z>9>}Kt&al$+P(%Xv-q=7e~ zp*j#G4{Fn`nOZE!;`Jcs!($|l>fW$0wnir62n#O&rw3`b2`BN5Ri;s&Zu-Lcu#Ls? zkH~~)aPU}K=mdHx za5gD*BBKXkv*doM_3SSYa9Y_=DIFI~HnN1qPsCD1GDSo)Gs_GbnC+s+%&gP;4MI5p zCSD#I!ozHzl8WIIP2r%*@vIBs&$n+8QBdh|i!=})pek=-d`zLmin8q5=|{u( zMd~JFT_c`7oJH}H$DX)F^mnDk3r`8wJz!fOfh`E~^4$RrJ|mP`aO^;322dFk-Cux4 z1YIrAlY#8~62t?biv*n$+#ztFh{~QIvVlizC3)uUjZs5Fa;|ut8O>>#g8uDx>4a}J zAaT}{EK)d1hp@_BN5Y(mwKjk6kPvuSpuvHePTmDz_)u!Y(A8lW8F)5m4iq{# zErD~%N2(m~>&3{k%$%3)A-iTVbXfm)tC@80>t-GL`t9|x=X2mf8JYuO4UnHy@L0rM zF!)bgeI*Aj@?YtH1Q;F$Pu6p{>wKzj?0WBZ8S1y`7&%lE=$;)WDVO_aE=86ZG*IRx zU-g@~A*vQez1c|H<_Y0`XTBSg2N`}1^S+XxNZ$p%fsi^W6lZYnBd1t^guI8c5dhcc z4Zq&Ng#sP4g|l- zCy?4|5F-WN!aYy3HqJLTkUc5f@R2SM9u4KCbD+@86K+$c-ug0a?Ffffv*rtyrs)bD zHaThTq&zOtKC1*yAJ5Uxd|lh0$kiPyybG!W4Pw~>@1=`al|ncvPuj6$;+SdL@?p~0Gy4r3HC~T? z+P9>N?Saton4d42ZorHajx<2CKm`SM1WLfKuEk~|i5JbgMQ~TkTMD&Lo?PYOkwvAk ziUEo_Sxqp}wPA{Ga?p^gP2q1nN!p-8BcGg5W+ zbx5Jze)ho-A%V2{06+*5-{5bi9M}IetPK(UL0utFC=&UkFc$Gw8$`fW9t@Oo{Pmd(S$RVwA`rom zC?nJqh?!UOP?3WxTo0k|4l)Q0jyTKz`k}$CssPmi{4f;yRdy4%T`!}~@-VK9#tsu{ zZ$4Se;AI7S)3fvQ_}i$Tm1Tc zT^%W$Twn+SRSoo5uo<>;^4Con-l2e!kaMwb2G!SxLe+jJ?%LOfp{i0k|hxYhL6C(1_9qnw&g2ntK%==VV2o^rYhsy zR&c6N^Zkbrto!4b@1WJU9J6cq`J~||bnQVfEL=bqCXl5Issh-$d2~JH%WqwD%U2%MnD6Q-YL0f?0Sj;zusK ztz*f5pU`rz4&E82oAakxE}~hf#e(|*tO5{5ft4(T#wiG`VO{$Otf%QezSxeGepkW= zYZQ5HdBy9`01B2R<8!Cy?J&4o{QUSF7Mu3)tj9tC)+qSwf%FfWEodDP25DsB>6C2f z5g|Ig5~-3W;w+Ijgm%s5KJb5c8(X^Pnq?-2SSf!VB4>j4H0Oc;xy2;z%gZUeUVN+7 zN?_*d`!SJosi*2o<9V3&{aGkaA-6NcK+vD|-UTZS2g-3H^1Hab-LLQXoW0UwaPrOB z75sMi&X$&*Vb)~WNx$x*{%J1sxm?EvEVsDAURnG0T19H)|bSiqbr| zX|Ni8-l&|)Oe8TaCG=jq2dHmY>uxvA*w3Gl5W);3wC0ohp7wQR^un^KH;JuI9skD# zkQ~>yV|l>dCl#n{oq6FlR%3RFt^w8&L37tO49uAu%J=z3A;qqbF(Aw&zs>Xcu5-ld z;c#j_1>fFl5B6?wS8w1b=B6`-b=n3&oybUWW} z%eDT=`28GsMXtX%>4@uRCMPlV^r!+L!BqRrO{jHm;&Gafp3ynR7MQqrvp; z#$kz(Q&VgMk}%rs#bF(9$-7@p8>o}8HF&V!IXLe(Wud(+h~~gXg-Jf*U?M=DeZG0? zYt~ahxQNhEV5{~H4`VtF#!~a;l7!)NL#9Z9?Ai&ClrZDrxTl6s?^7>RICBhZfpoB84b$ysdXM~9IGYe=E~-2DuyJ} z<7`mN-+8q$ny=Wyzo7IT+aiBeDJIJzwDblkZ|@g%^^M>8Qi_`Y@{$!+; zYBix#2YUTgZ^|{L! z%!~%7c6xgHQ%6Voaf@`>XuVR!k1?2~i)1))>qJ7#OJ9>xK?IzV^YEvGD7f?!#-+2y z*VNQ7*y4i9QzC;LS=iaR`roaX7;QF%w~_zzlp{h<8(L$a3R`%IukS#SjIlbgoo!-m#&EEuMG}rkWx?( z^3yD@O8*uK1h&8Y${OVMm*n-9_?n#nIbaDuvrH z5LU)gjO4$(J@o{ob7&e1jwj(!LtYvPe7+8*3k6x7pDf1)^{X<~z&mJP-yahU`5tre zMqX_{FgZxGN1-BD`JPbNQ0TZ9Aa}_@;tvkbFQLpt-cgSq)1v3|CA~B;xs{NZ2pBQu zQzF0xkyajd3iz1L?~d=_kWz!OWZCE*y4}8wc+nf99}upBig_ef{#tQPF&>^uHe{{p zfK599g#a%jXJhjFkwgle$;mx08Z5!eeVkO|U%HXpmw|nB0BXvM|V5CCtp1S(t z{wYHQ6YMq)oUbymL;*_1b}&&Crl!1(QM-3f;pIz#L$tg7^55cY2^&&OKZUuES8cal z)8XLYPzu=$?x>b20)_X^t1QIE@82Eq%y;9Gl7zn%ybp|}E++4bXz5nj4TtwltPf$t zRRGH)dd6#$3u3SXJs;lEA%)+RR&x}g;z&QmezfJo$;pYV z)5^IdJYZf4wE{5(#a$fnw;QI;HMcK$DLf)$b?e|$(}rxQ=F`GP?i*(64w^fUo2oD-QFOb(`lg{>2;10T(T8di38$U`UBFC{ZHG&}*nUru6Ss5riMb>)Lm z1yET!!COPK-dp#<11R5un7+ijJp(gxXxQ%md;bKQQ$<}}lBMNkVlpyi#rMDA5QP=> z4i)Jg^z$bkn3Hsd5;Ajf*%vA6hUku0sVbo?iM7RtRgt{7BT>tkxBV(k%!8VsvCjYG zsrEpu&zw1JGHOS$&eyL4uzkZg7241=3dgYF*w;IN92r=w!5}h5emn5(#tcHfM<{yn z?;HwK;#V1PhlPjBYiQs@zX~D^RqHCFa%c>q%Px!xE3=-$)tMQ8)$4og*}_kW-#a*1 z^p)}_uY`nzyiUiUjS#%;5+M`aP&?viChMiwx0SK*DA*El3OES(9mh#ZvT@l@iM*sBu>2+tk$heOYlPPej znAzE}V8#?!<{GU_w{;$xEdNiMk)6Gg4%q*AM zqK5u8l~|2}&yqJ&j9+xjgPTR%Arp4TlT~IEp1&9eJaIEljbAG}koaN`yP$*xC&!rZo3$jn@1Ss)eRc9q8231@3eA{%kaanXwQuz^jgQkSz zR@_x8{Cq+J$seu_u+i=YAPu%)Ty8GX@9)N^o~OdPvKn%^hLkj{;SPB#8Z9e-4&5Dk zV<}WVb2DFhlNTzJ^Q`1&Rn{#3F6A1p@9<|2I5`@<_u+QDSbn=~?6Ics^cCZ}Z8WQ{ zs`zWUUv){Eol}ijj?$>*hbftvcz`58lQ`>h{A;?^$4i^`_Rufbr=Wp~2)z+}Apxiq zOhMKdzJx`8?7L~t@=Hw<{ePC}9j#lzOo70?D!wuC>~@E?2$jM!tQ)HbiCAq0bwB^6 zNqGnYqv4%9VSG^V)(v#j2s~qn(>EWI2qb!t_nECCyKwmw4)bWDcfPHLB!IOW~^Guz8+!MdSF=4$gdbpI1M*%^h5?Po`2KMDSaCqYQ)=U zd&}|5ANK=mkv%pO)QwEX)-eZX>I&|lK0?a(jU_67-b>%*;o0m|g)6%QI4Q8T z{~Jhd(CRIL(07k9QgY?D=74oVXS%!*P7CurqPb$wAozyUU5y1B6&o@ajt@_(ohVcvOX zlg+>XHIgF&4j%cd!;^U&xF(+p^XQ+y2_+}#wW#Jukh_q)Z*LsukFs*5z3p)2qmiSy zCe4JWmI|8#o)efQ%!27@9l`iYqGTSEABKE;46P&-1aMjYc7c)y6%IZTKEoNcQz%MG zX7x)&!kljN@rBMf6nD_|N`~}qf;ufV6{}>vLl{R<)+q1h|5UP}F?kq7G}b}>X2AqD z%dBoBgM~FU&b>5fDGqU7yeMq}wz&g|xcOEj=qMnx3y_l&m8F9lP#>Rhuzzs)zv6RN3LL5lMSZbR8LntV&ChHq_ZOI@6THBNMD&kZ zbBD#JbL;mqUDsN|T1l!Da9@VsWVhH`>5FJX^I6e!aoLX>`V=O`*K+X9f4;7Hzs|o! zRz}8G?h2hZ)xLdJwVmWwu=W{TnT*(|H*DnlnNr<2W?0k$JIwuA)Pzwf8Qp+fq`H|c zmDxPv-`e}cHv2U>{PBzfsFKN|R4n6eg@lB>hg&uwer46UF|hYXzLGXA!G=;CvL=Im zgqh_;`RRM*k{^MgvMp?Y@-VJ0blMEaYl_MfZS=jnwMd5RoS}kvnsxwhM>T=;RexXS z?^jDr+pUR1><^{6`UZJD72m!Vza?*FC@+sPVG39g@{qT%U|Ji_#)Eqm9wNm3SPrHM z7}Px_Mwds<%YjBC@*35DJ=c#efu-ZAFcdJlJft-o00i8%v0+ovQ3BBnQh9)lzVVFH z(A*xytkT;rvDED5>?M{ZoS8H403HZpHq`V)Cl8z?t9P(~~d#fnuNP-a@W=FRQ47bYH5->fZU~biF;;KGxO>z0Y!>f+_EBA@`x%M^2#= zS=2Ye+)v#9`|cEb!h_gzy#+TU=`-|rx2lXd9--EMSAIKRC$95Qx77AYy%E{>c%llq zdDQ3|mg)+}w;!)t+io(jvSI|t`K$Ouz|*$3FSQ8UAe1>_c=~`Z)ut*#p!q`T=-)5( zPU?3>MM$g*Spot#vGwxEIEqG$0D8;mPcNBYYj~gieg~~8nPbZ#`H&=hCPc!e0j8y^ zs~Z;==a0N?L`0AU9&9(5VMBOir!vLkV5nf?&Yl=Fy9+@1gbl?TOy$!w*v2O&p7Ao9 z$+T9i^pBWXo~w!<4xN3u0~X4=aFrkp&7cZ_{Rt%%(oGmDZJ2x$*3KXm;eXl z%n#d=JD=i4@?z;TM|rOcObfzye**rpz!|BloN@SAl{NMT*F%;x&Y83R*?WO9%zEE~ zRa$|h1MV;3xUSG5z|Vt@2;*^sQ91HXhiY}V1B;$x&Jk=>;AudshO~#Mr!}Sr^syb} zm=Txf$5@ZoH=~-TzRw5Hm9~@>erT zIz@uR@F7TBJp{bTnAS=)nJqKt5%5Y2g_6tSTalWore+uT@Wv%3q7{yX0WC;}OvZEy z``dpi?mMrvci2a_+>VkoI9sTbw7gS!jLF{w=kGt-zxmth#^aFJHUY%o#qrEoUZ8q~}{XMPt>!bbMaoM-8t9jNCyJ`Ymn+ z)(RXTaAqUbamc3xT02H)%mz|;f;b`o;DIvrp}h@^Rv)^802Jxd*}+JQ4ehhDKlzJxT#j)NsF7G^Op2SPKZen_giJ_ z_aQVotLq(;giUFw>I3~986>+N&jcP>SnGe_YSs9<9y?*z1(+^pc)i#0i?9M!oc!Qm zbnHNXP~8IQftinw5ITB<|F0&uLO^z46bR{vXi&0}^Oeb>HHv&|ALS_N?@78!iM}r@^yLYAbw?!FvvOw}Ym)H*aMelrnyJ z1@17&%4M|(lSt{a`m*hNELl`kG%8qoT1tEHesO<9HuTpELeKex>pn_m-#62Os2y61 zefy9g0=*GT$xuPm4^yzEvAgsW+#|bzFQ|+E2S@qh4g=GYbfPiwu-I5a-FX{Lig696 zunb{5k=oGaW+|wI&@XvYLuYI26f|Y_+J5wLG={dMUSvb^W_K*TECZn^k4`R}Q~cD? zuY%avu^KBLYU!q4TsprlOA2K;P?rA>Ejc5@Tr~3~$T%#4at&=EKBTz>Ap`K}eGZ7B z1+f{*fGFNjm?y*G!z3z7F6s7933z0%L(GM#s{wOCvKipDscLDdfYo{H*&nVCfk2j; ze~>7#bQ64U@NTkcfX9=Zv$Npd{ywt1g*h0Uqw*VKei2_w>i@GEqJn^}fbyI#f(gXh zA|(K8-h~zjd3BUd-~w?St{M1+wL{sQeiWT zwSnP*ufb(NUn=0R0O`pI7Ag|M{x{o*I3u5jCKoVL;c;)cyF?z_J$)(%9WA`@3E|ru9C<6<02@LX zr;@8KY!^n4LvknQ9theAs^fxM1(b$J>Rx(!ICRJi zGvjBB2%~Lu=;om8nG_RPe6U$-bhhs_l}_-Q$joFhA*<+lxaCq{ASz_EGfPU+K(?EF zhF)2xalvcM17%`nf};8(w8mHLmh{xag4G_O%a1rn!)!>1<|OsFPNY`n)Ri_9pNiPn z*ys**i`Y+&`rh$6`aeva1yGi2*S6`94r!2*P+GdXlr|9Q1_24_4(Ucgkp@ZW5G19$ z6^nR~Mx?v`^}O%*{qxQ2*|TTwJqQoieP3&x>pafo0LBqkR@U+3$Sm>0NVAC` zuN-k=QW9!TLI!(M--)eX0ipi9qdFh#xv_Rv&QD6cU*cbPKqL^cf4K5e9I3P?$Xron z`yaitXGjW~u>FYU%pSypk~v%ybBUZND2wRWybEW%sT_|O!9U}6R;WMoWC z7s#x(Phk<4Ys=xcXJD;+1r;8(O-3-p{X%Bngpv^d2WA@E3)~jN(`mY#J4|$rlnreI z1H2OtnOz-bKfg&umIz245dgNZj~}^)GsMWUVHYMo7J6&;mK)di=O3+O8F3=>s*jTrUx# zCJw88V2&!M{jkw_j%10~gy=#dYD~|G@X$*V8K(!wo~LC;9&814l3v6`nI1Ioz5qEi zDLFZ$)%Tbgs_SPB4!QmU{QODc;@4mWg^7=+DU?C_4Gz>aw6sX?Ep$~Z_wJE_5{vwj z$Yu;5EeNVi=+|_!J?-6cGJ2`XtW@?=Sf*85NxVZUj^})o}JjoV7JHjy1UOLFu@o_ zRzlj_S-56ZGPb5e``)p|^%2qTw$c3I>TE&s;f~D_Y-z{O>(uOsc=^ph#kJ_j zPr6%%V7E^jy#G*9Y{NX}?L8a9AnBrh*3J<)dhkF7!%oeOO%9lzf!ND&=T08&7#I$a zZ>L07T=4Do4-Djbzkm({igvTJi&}Oa!hS%m&>cGETC~H z=kuFeQ9C-4sWWiz2PePa_xNk?Ue^j6S4*4ZGsJ5DJad#@Xiqsk2H0crz=7i8JgO4= zD6@bSo40Jh65>@mz&{b8C4Gvq-Lg+8w2yhVqHIpnU^m5CYy2-47iG__0fp#W0Ha0S%WM;@jNQF`xg0o8_;SlvkOvKqt1pc zwz1DPYCvp*{WqmL2xT-9wvY=BV zCa_XW%il0T_#n2Uv{(*>w)doZr z1zJN9NoB=n`sZ!UIPE@GWDO2}xCy)xwEUrWR!a!#dnQ3pz&CY{@%zwI1HUTTEd*N9 zQ?(=K!R{grb@e>Y$2QF7Unwiu{p)Y~q6zo%n0tz`w zg&qZ-NLg|vRyHJXd%mQ-ON{ngQietk#1<6f zXcM-W(bn1BW)qg3x0VzZ_6YiOksI4S@l7}>F@jx9^|H$137*=EPru-Z>OYMb$6);} zPSi$cUHeWv?b`;%Eu5@db#q06t2(lI?K-=RFdG^A|F^9J|dt(ZB2;`u^){XliJdX`UnTc;8_^v&Y$%*J&&F_nKFJ z!hhaKjOno($;amB{|bnC?&O2@wTQciH_P|trf~p`jEOKkCs}e*&W9w428-ub*ndT? z>1M>O|AhQ#s?wmZJWjxoU;DI;9U?-q^hrDM8COcrq_ZQ9m-#EB;@udPD{(v_dCWBy zk4RzSr{4!N|B;R`iGk+gL!>q~#DU%P`HzuLBTm=TNpN8?Ga0R9Xu48<(lMO`|F&3dICy z7^4@1d~_zsncF@s{YOy!4P8Xzwuo#6!DyUZ^7zMOzZ&e1tG9EU&lEs*L>i`{wSs#Bv|mtm zb78UvSR}Fl^=X9;6XlHXU8I&Mh@Wy{prV4)@wWKD`I~;-M~{?NNl8013l4o+p(=mT ze%bJmjR^HRfW=J$b^f2?uYRXbdXsO)ViFY;rPY$xe)ArlkF$){sgQ;D2y)%A1MNi$ zuK|x*^~u-{wxASAB6spu%^|<325fO4>+JVpja{dfMWOj9*YVVRDzw#b$*= zK`jkOE4SCy#>;~F6C&-HoUBlak3M2HuI%d4F~r{x$8~wR zZ7q4$72~3*EI$0>UiI4OsU-TCstD%w1I>@NRg9S~j5ygFH~Ph(_1fBCj(Sn;Yf zty(W7J&DkiQ3vh7kr`gsR)KgT^Qy9TDT)+NF)=Zt+jnb*xQAb>GD}}H)49m*v9c!W z8dayGuoB&-_TF(H$le*`_6fG2)dXb^EQC-mf+Zz_rvNqxN$s+l`MSBfBD)^2yMmmi zTOJC0Oz9~ZSRyhlghfQUwvjkC5+sTkTg^X&Ey*!3GW%SW#u{Z$pLAVs-Y6Vaz%x*o zekdZhm&A$-o}5JZPdC7rhawBB?qii%;ELj0SAw37vSA%fYcHv7EG=TIRdsGI45ey=+pc+7iRqA z-d{6HBSMnB9~OJ!JeMCP6>){x4!o?Pi>FSc^p1x*8l0X~ueCT_V-58t?RD>5o6gA< zY`0#WZGwcM-|8cT1gv3DqYC?AdqSWcL~a!W+PEL;6Gq}7Mw~`q3E*agJvzv>cDlM~ z@TMr4=YmZw6Jl}jn~=~NtO_G38_0iIl}~POta(47j^*6%$eT3><=LlhgwO=e9Uh)S zwjcSJeLX7L2N*1Diie%sL^+flNzwBQgUf-PPi*Gm+gqv`cz;XK`(~R&g9kup={me|-DoyzPztNYN*~bZ2)pv;T70z=6XL?>R#ozSQ<5+KkKvQ-A87YR`+R zgN^yc{ew4N7xnVw73X*rnsaelciLZ>%u9qRR@&eN`1EM#{}QB?@pX|hzMdiwz1;6} z{`$4O?Vf>Vxz?9jSFJ$*T>lmaB`dMyoGRscvB^7X5O* ze|+tHe*IA;1q-uAe^X1RvHoK_(FHou$bIElZ<30p?nn;BF2O%{F2<%R&Hto&?rtbh zcAod%`Q3Wro{_96qxfn>?Jwqg7mHyeDkM zR3mc;{SMP^I!hmLCRh5?Rkk)^T}!kRy!d@((zx$<%wMD0)%b04RkKx?TJs>MtZegp zQ8RWc)!_Fy<&D=4tnN>8cfM}fCh1YfsCPe2n~$5Xyu_aK-PX7;Qxl91++~s6y_Ncy zCMG7}P|n1J^Bg0v|Guo&zKpZy7o+cVzL){~XUZ-6MP($%S7TL+l()=ZaxTGO81ME6 zOh(!fj^C1MZ3dLP7tAS`IzmsHBTk5MK6rmgVU?T~CUN!NqcvM+TQJ3>1+%!MxHvnU(m%H4o=lY3_O-V)?eEQU@)e{ws>!cp;zmlJP2 z)VPuogdCvV;F5}=;Fbr}4bDB(c|z4JhjRaB%(IxDP%bt3&0A_UJvG-UAI|MnVf`ZB zOaC$ScM5f7>k*Bj4u157Qiqf9k2sF|e-p2~p^${pC#aEmAXS;ee%V=m(zZj*`S>w5+yFadQ-9!ABB>a~MpgLMeTOq`fji3zS7QSM zAk#{MdjebHgP>em4*m;dJO$9_K2;sJ z-lF)+(Jm1>fn^~bVD@*|)G~h5%Y(zoGN7j0(e5Vh^Q#cwDgCkATSY1-MK4Wvv2w3v zZ|B|#xsAncXD#uRFY&e4-gfc(t9va6f9dsI2mH9MCQ`gknP`7+X7z?wWsJwlxvu>1 z=R*9@OQfkxSVYhF3Dlfp#;enVd7bB{FIo;31OE)( zUp33JV6wK^&~dEGrLC`&it=ppT$rhpihoZr5@E5fwbPmI%imK^GuAQ_JvSG6vO4u# zTjUb8rNQTBg@H2(&r`3P6`9#l{3v;`_o|!+KD_n)f?36T#(%ZlfH(fMo}jr|UYd@t zU~#xsDRS{)x1%owuU}UDug5ZRWut4)+r5T9===PT9RA!9jwvD?{U|N|%fG=p;W&}< zzqFP+LCP$9tAwS6p&f zn?g8yGP+Nnm=W?rN|gH zB@(axd_CE`HjKK@JKA;F?v2@Q^83|@R)y71iRs2~A+Bnrd4kI>bk}xs0ov3p~QDA z!bZ;wgKjvxW&Cpq+xm1y>mavc*R1Vc%mWI=xMylCXxiUWwZ*A1rs+Km${&M_3oggd z!sTUBj9@3{s2fdmujx=yYIfI`D4OiGf8r3=^uf?NndPB$2xEtN^|a>zR#u*53c31+y$sqnFoUJNi*xxhjM3^|Zw8~!#M zfYkza?l}l6fGDB23SK;cGb2sJ_LTZzprg(7}zdQPd5&=x@_bAL6#f;$_Kjr zQ4*Ub9LhrMD!0CwwT+jE{S89Pk)c=*Aqoc$OGpJUl+@GyTciNVTd67K7i89jDAZ~u_) zl%i6mnL6C6tV+@XKbegcwcNPw2*t=>vaSV z#D1J>?g>*XcaJt=Z(bY83!$k;-|Di8QlM*|D0zFm?^*R{s9fK7>~x3nn)GzlB=Y@C zz*n0)-~Y(<;2oRWIT3QD)bib4<#BM>`Z6IK@zHHhnyx2J;yC9|M_Hq_tN7&9R8m%e zxI@APHvN`UqkFCF(_*`M4%p(T?~=swX_xM=vfh@Uu9L;Ui=3~PdUx*z%IwbPc<>Iq z=OTO~DC}k1%Cnj#%R`?OI*nO*H7XIgjEjDSk>Vg86BDEJ@F8U7-GOU^8D>2ooWUF^ z6nrc2a12=Hk@QCv;_uS4s*!?4!_w+DV*rJG5|+VPNvd>FgEl)Yp=pjVUGook3VI=z z#Z2HGBt}L-jR#xRZmedw5u%KrF|98zEsF3u(85x9`x;8ZrH#b5+i|Q*szUUAU=9Qw zB_z)u^~*j-*;l5+b7rPO;V?SzBEff}oKFJRZ2!>El;8{<(N+~gzCe|jBFv8^k;Qp{ z?tWd+>jz(^9|)g52fI??imG~{jcWvn{jxUtbu%5JuU!JCzh%W)6bT_Qg76IKNa+_f z(pdM^)lKUSlCrWiE35sR3kt&8+>q{GP#1wmDpk8tOjc-LK)6fGO4%>3F{M$_iloHJ zDJV+qv67sRxPA!z?9YV<#M~_)hv$1!d z5G5kQe=Y-CB5ysTJ|r!tFZOhP-?tFpk(~R9pxjY*@x&!k8Gp2G*O*JZg(1*yf&b!g zi27tVvq@D}kTj(i)v?^4My^#oU*^u5jN=Gt>Da)Z0;d!+#py;hQIWnIW%bwhECga_ zNvN7p%rzxw@tl090?%{kXalH1HHhXS(vCd-EQDt-{8W)v*b{2+D%+;`Gn=;ErPQlk z`4Lbd6#muU!87nl+1XvM*1A4SmBzLiG@Ykm7n!bmF%2icQpEit#Fc}j^ZSSw2Tn-N z8j&35wzeILDWdb0pj11F9@-Ba+g)t3f9_Wzw_RhVF}HD2H2O&G{V;Bl9%Y<}F(&CF z$!(Z$k8FdjQQ+P^kmrbLY4N~Xp+jEsM{D7G?`;W*XOAB*Ud!E8BjF@S)h^WgD9K8L z|D&$T(HHc!JP?{{tw%aR42IR;@u0DP-XRjgnihWfU*~$F`7F+*fwAlHo+!h_nS8J< z)|$35SQLaln5o?ogVEmNqIs!%Z+cXdArFi~khVnt#08}ioaa&Vze>g)l&K!^emF~f z|6IW?tN)$wZaYbB&B%L^h}RnH#IpZ9n~pPoZyPYBWr$76h~I9f;b_uS#`k&Z?*L*u8^#rMT- zNp6~4LADPb2<1LGqj6Wc$zZM;gt~RorV(?Qc*DG~L*VueU8~!KYwx?{Ot}h|BRigC zFWwug=Jr~h`M~jlCdN)okA%$P%S#vhBS&tlpDjeKb4DlTCgguVNk%SHK4Z#~qF>Fn zy76;CMF~|(O6msf1xjg_!7H01lQ0t*f3iZp=J5)tuit0=86N-kMCWN;FpQkCUZlHM zc4QUA#31_nC)RRdg7QWzwPtRg^vf-_ME{{O7?*VVPV1xrJiq@IbEM;VrnLN$5}EW z?8x#2t$3vbvyeA8DzbMU)>S|py zM_*co;s2o|ayffwy3I4quK8uUMDN`g>jTl7efCsT!;9~1)j7C1>g~}>lB7bJu)PTI z%8v(_n3#go!*)-}s`hH%ql*2A_^dV|@AT^P=!+ZQd+b4#e>B35_fjpHFW|xpY0&maR) zRxh(l`x2&P9EB5kBSVFD5SKZ%{`xBxTTSd^jefIv-kt|dsDW3reT_>M=euQov{@z@ zSgWP9yT2c4EfhZ5_1Z~yBfivN%JQnCD;YBBsu{LLnGm&^V8$8y*sD-B8or&rF3NpI z>_Pt*mF;+$)-gSNy$E|}ur|S5VRD(z-y|a6#w11w5OLF&Y!w$1<`<_8^NZo<<~s|I zRvC{v9=BAtqQ50w|0!BMaqiE3NBWeC?ekPj2m=4EwbAjs{@L-fouSRZ;EHn!1suQN zC%4Y=+8QpX0u3D$AHEdGIR4n-b~hkl(`mE&+E40vIr)Ahl+fY4*oP% z+3|G1Vqx~2-emxKXF_n+YfRpo zj$x@GMQNkt_~op~B*Ll(zaUmB`lkZ^7vhvx)Kk5Gv}m(CQ}%tF5u!vidAuD$`=LYh z5}PXe?z`_BuiIaQoF+B%$@Ik3PzJ_5X{p`Wp>K-Bw#;MBTw0R~>1s2kBE`R?y%yDf zaW3=w`0}ykY-+?32TG)TXQN98LYf&T3eS1W=w8*@?k|_~Fq6gJr%|FiUrcuTn=KNL zhet$tl{Xt1LQ8Vbe(zQ$=@IG-F!l0eOS>>@#%y;sXlz!`1`z0~2Sk}{k9OZ~aO2|? z!$S4EId{6R>?gxd)U?`dQS*{w@tae3ZD4ljtug+t@p|lki4i|6oR0>5tJbzX7m{us zcN~+kf3)M@+4B7I-k&~oY_e;pEZ9WyRer9VJb5hyC#KW0fX)z`NP)zUeJAkI-vTKPM{Ie^O{f)#LbQBc5M_8nGP zLEUb^mj+}0^7Z`36hfyF=q(=a3k*cIgjfdW z?=o19^Hmb;1dHP!Vm}`K5$oNXYQ#ualHUF19f7dV5%gsriSDH4Fd5j{hh_ZUF7UIA zx2mTbc)=R@So7%G`Da?f5=H5Cn0=st2(NyX?1Y+E;}_w0%6Nt8D%*^JA)PbZU*4HqV#>4)Mm@_up<8)o(kXN0Os%^#omjPBsb1fp4-`XU4Erkj%S{kczs?|`Y0F> z-RJajaijT(#r8oqV&qd`@KY=}cY7b&6(*j`d}%tYT)2Ak#C+Xd=1cb69IjR@!SVIh zC;jPjo`3CU*v^KZiDmpsa8uk0nQzm`KD<$JvhLqloR1@O?S={UHu`bG#<|AH?5J1^0+MyR8I2Gx{CLrp^)x|Mr8Fm zQs9;a`rWK(JDl-4t@Wo`&=NN2IDCE;EMqhmmv?Ox{9)z_f0Jme?**PoAAPFcboIkZ zqKE_(T)=`Y=pBtCv9tWhhsN04Nng1{;rjnni10=2Hv6eH*YS+#*Z9F+;2)aRL$9Sv zeZ$k=GSz44YW@})e;VU&{8)X7FR>^eYs7$s{qIUiy4U{<=W3^NdqM~y82jUrE#Nri zO{4w#*T&O86J?{L5wD6JJ3})X^!<7*^TSM=yp@voN!yJWc&U=HlU^jN8A&D8fC4PFMy^hh;)C4xaT*O!!*Hih- z0AXQaaQ@RM#rl1GTvm$iBv$;cJhtu#wQ>Z&TOt&s*96Kofc=9&c`hg_vRLenN@P|0 zTH+(h*v2+Ztp3YXRF$j-ceU^RXtbW#B32MpC(5B&ICD3B!q}T74 ziiu#vnU2U{Jv@E;CzL(W52D|L{N(E^btS|yHSIg;M)eb$j}Q^J>|IHv(}^(|G5yo! z{<5c@cB6RBZ61ajduMR1Id=a1Aa)XWdfV@?Q$8%I1tPtBFBx{ zL9gz*6(9T^U%rC44!Xp0Jjqu2CXWrLO&qRRG7@>SrGt+O^AZlY^d z`i+9o^2c{Uj0=*USO0h`BSr1GQc3&3UlFMBcqMo9jL{#^>A;C}7wBB{#Qr1$tJ{r7qng(D-H2dZkG^zIBQ zJ`U5+>JX3Sn~rNXN+zy{61zpU>VP}mp5_b$^cWwOtC5~LgzC`%I~Qq%b(qh5{&np#TS|0P~8#{ zHlH;mQE%S$vZ!*D_%U_ zR~SHo)-x$)1qk=FL*3`k>1VbqiW1wN+B_lY6k=ZO0ZyNMgQixsv85%yuafpnU!0lN z$+3rwqgOr*4wUwnacbIgy>i>aR?25uzhYL!|cmo7J(tTe<(`-_(EYFIMQ>89 z>)(jiER)^@n25JDb|v}R$7|#92*EB%hAq+x+;66PaWBQbU%Vbp(+A}c^5zI*Bpt+- zHv~wnn_GT>VwExe!kq5btpvzELE5D2>vir`LV6>Z1cBzaXTahZ3r+Il70=X!fAdol z(V$c8ttCP*^*_e@*gLi^@{$I!n&*-XT8XE)=kMv{UzOP8hrPOS?hj0RY1W$kQ@XN& zE+>xf7oA3b_YM_0-ftwXhD{C!fthrRy)`6Y{owzRgvU+o_m`k`;&Jr~0s#sF3l+(yxOPbdI8 zgE3&V_pWK6g~(rfomQN>bh)Y+uOn*pt-%z*?iTik1-u`2TK{@B^&C0}jrcZ>JoL(Y z#ccr-m06T3=};8g=&T~lk7}6B{4Cc*eJ9IZ_t5>9nP^#H+yNe)+8-#49O^WiPhwPx zM=C2w$Z2@e?%IM0;S)_=q8mnNTxmg=YTS=62bAU3k$&DwgOsJital!nnVBgvbE5ss zx8xj|2w}|0$uX79vdkdsj2M3Y_dA8^ud@8U0QD}btu0@FMh(bICD6l=g5V4mIBZ|%JFB;RTL2(NW0j}zF0-P`gTT7 zK*e2X1`_w`zTG6VnaHfc!@mk89>2CcfL9k37M@HT3aa_poUW$N+A$=dPj%?^1u^;1^1VpZ!KC|A$~5L#bKwvJGArbtf);5wYQhh$kp_ zQ)~3k%zp9aUs_Cv5^*V)y_-1aO!UpWkWa}Mw1(JUX|J7V(RWqQU})fZs`nf^RmCjy z4)`CC8AkAi$^{Vv&KTsPLLBkU+txrF6h3$t0;_#dt*5{h8NF3lPek101&KB&w*pQO zL7Soe5%FyQp`QUV?*dE$3`fK_Z@wz#n4R6TRRV$t7;|JoCV11qR@zhN)0{;!e%@Vz z+L{?biJ0ED?XYDE2}d`e400&;5(1%!@rEo51L9UKaD?5yts@_H`yXZL12O|{&asezr0-I zX!h19QzS=pO@VF!zqz6|GSm+^jHc;C zK$403KxERKmj6?j}K@hlyRSa_;ZpA=m;!+$c!QQku|zY2X$)8jcus{{B?g2d zy5y|P>b-+4(nXLMo`DP$lW#>`m?0QCDJ?AmKBMlwzCZOd#`#ugAsWrhQrG_h#Xg>0 zt>&`!*-qj_HzqL7zFg>5EcTfCbvPY5$F51Rwk6pjO0uC9BlA_=+T!a=r0($6-FsB)yh&o88-c>39Idp zPEupk#r}xy;0jBFk=tR>{^X<;u)I(H($G%Vk2Pjq9lPemiku3u63-KT2w<KfAi?@OmAO813p%+30K=6qE37*R1@ zWsNJiHNSoPhI&j~r5HZ7USQ_%rvDa2sMNbOyi?jcf#vTp>M!(nFh)y?trC-y(H5w1 zS=2~@L$sY1G#6(&kkD8WdF9H!h@noX`4x0wZ9_xJJG~YhHdR5wDr@4*2@v|iaBMSp z7@WlXff@Hhb)oRBJ4$x1hx8n|YITa`m8zBi)d0`B?7&YXa$o4=YW)J-;4j-Gfc#%~ zds1$owC@jbz`J`xyi>k@(k9xhYRvZ-wW3ovqQm%BAdJy=zNDg}7v5EH9Cv}~F@wqv ziOGU}7IJW~L_b>ZTWls^-e=?tw%x1hzIER#EiJ9f=@a}l#E@Jy(|?`s&b6k~^`8hK z2yT$WOP%n_t5mwH7{hlnv)SyxTLj z|FAJanI6C1qfjCde$U;{c}Gmjv&rz_OhaT=Q7g||?0e#_Yx*;W1e<9m{5sjY)JiMK zs`Ux+$vzf85vi}{FAuUJ$QeB83_sSJqK6? zstCXc3B0diZi9Ldw+%o9=*>X*cK+GzuocW}ZKX$g^O0dYP|tzH4aNu5eboEmWO28W zd$&B`Cc?BUSy^43S|yERN>%}5D_Ss0kaWtO#U*hTmQZ_^YDNrgur)ZfomwN~`|Xaw z%@^kGEN_W8cZIknIPBK&@h$`imrs7_unpGy-zqB1MW(!uS7&wFLmP#0pU&tMrF9%OI|f*g_Nktxhvnw0=NJ9&;Cqh zqTAaLf%F-dg?+t1YJYDJo)AJhQR0`pVmI3%uYb+D*SdBfU9JC(1qaU88RjTw;r@bH z_L``LD4ddc%ApdprYnYjC%ThZR;%u}Nh1t-p{rfTHT$A4)H0TG1h6pJ&fy-z5o?H3 zW`O}&@%kh9NRfQLo^_Kz;`1dV8-aJuQyL=3b408}0!(Mmnd3_Jq+s%9C?|5rFTJ_{^UkLZR}?%L!~U zT(E@br%H{K_AFN=+Wz6;;lcG%5+#;xT}PkS4^7B_k6^qW)y4Xou~lH+J7_qI#RMPA zi@w);fD(%-iRAodZ6h z{;wCHS#4arcY4F6$j`&fOQ2FM)Ualb;~n345m}AR%5Yxe9tY zIHN!VfL~N{rvsg7)IZ zyseI?viYAeo}gvMPmC6nKXY+VnqF1^mKWybebFH_cPuMf6eIciVz6p`nc^2BBWNf| z)K!s!=mSB!p;Wau-HW|MN9-)YKSMc>gkfd{F%m{9uBJ;(mHls?$kblC#zPyoM%5B3Rkv}Lv zY!AkUJJ>d$`)Li$NmF`U8#1TmX{|Ms3MuKj%S%fb^vp{uE9O@hrw^YeEe)*P6|n&f zNskl@1t^*%NP7XIZL&WS;&s7q8_uc*9$Z+|c7>ut4hHsIRBY;tT#V~BmnOs4re^0H zYD3TUU}7-g`?OpGyuQuOU4T!K5L4qBSJ_7M}iErQH0G>EHI*Mu#t@t?g zBkouB6LHr4kB|Yk*b{?YU^XQBbH(!if7Y62RcyTS?e7Sl{!P*oM7!ONofHORYP?Qo zRZNfSGDUOZ6h5IUyn2^Ey#$e@$`tMhTO(VwyEa%wb?a%*zVEx=e&Xr+KEru1x5wqs z**?FDu^?s=_hUw_xzG>rD#dGI$_4$}f}hXU);0)s(*V)7R&oS8+97XP2fW229g$pr z-2ndt0|u56doE79Hf_s<>72KubIU*<8E$^AlX57&K~6zau5#PAnHh!T0@Q-{aCOrJ zhw|53N0~t0JG_J)e$-$>a0$fqz*_-iw;~MDcV*B$msH*ob9~a&WDCv`@KyqB$HBn? z--ikid6~Z=>0qb`elc)C%Y_jJCdzf#E0VB}`_ZIEnh`zKH_x{Hz2jyASA3T5KQT~9 zAbbJ-4vwq`>4M1QRbVAmA3RWmU>|5M;B`g5`HaVvgO}ypfuf)s{NhNxHF3ohfYTdKkEcT(+F2jI=S!?s#j06MP&YY{L8|&rwaqWmdEvYK2so=r zIexD~M4G01Ju%k>bB5?*EC$m(8p`UASj1(I6a-%cEs*I?wtH7uv9OMJzQQXZiW(js z&cTrd<0;Bn#%r_=0DYiqN`x>UwPdWf0e0ng0eearAWuzAql>3R6`^stlV*gAu;n5Y z3BQk(wMK=HC@5(9a#biy)NELRYLM4bh{rESxY`O`ACR#?A1G(a;DrPM90mpFnW4Nu zR9+3f6hJ+t$h}D2@7Pbt@gn6 zI6ki?Rej}y7A!l?6qDM(YIhM3NNo28?DxRML)tMVoSB@_Q{1Kk4_oG$Ckph8RUBMg z$P``zzTCnd2^yqwD2sm_WhKpzk_FLI>G;2)PJvQ`gp7<#pqxR;)GPOEd^2PtfIAR2 zUq{<@+Ra3f9eU80+uBjTlQx???{;~m@?zCYiFMOc(DZa~8PL5CMe(}r_m-ibgb9Qc zvU{-7+6{Me(V}R&==7`7{20~J@>_i#BC`M?*+Ht9gPwe*lsg8@-XsK^N8Sz$K%v9I zQdLvSjW73?o>$wNdgbsD8RP|UCU`lP(%3~rJW;$$mKZaT5x2l`%!)#Fy0o;EQ(PSR z?QT>m2#z801GOu37;Gt=#q`rZ4J)*EkMEk*iF(iET8I*l`jrXVD5F_wGGasE=0*_*~^$3l+0J2?dY@~u?RBT>r835)=>t zg>)IiY!yoUHABsRBJUpjPgfyyNbunh{aS*kc~&3W$lV|B+)hW|LG;~e#`dx;OGv!T zmDlV(5;Nj$JIOnf63ZalbfzM#Q6heZ=EItT@95LSRU0YjEvU{rL*}&jE+qx%NfwXh zOisPRVjV*mB!l4@op1!x^65cjG|T0KE;+4KXGhNdEa$DlQu)8U}I=ic5R!{c`uDK>5uXORk3o#7_!4|%miH7Jp%qKqH@RsgSPDyj>S19B;x zNdW-?WJ<673ag-Mk3l7z7r0rCy%3mF?tp)$;F7Vy5i1aHpCM&;j-&!q?U6{#@b6~T zKadK0>Srcmc0la-K5F-gt-9mSI!?uJ%R^~;Iy!PrP8FqAZy=(O5$&flxHnz{eoicJ zD@Y=jYWEiNKgq$)_zK1KctXy(ojiGA$m}~DcPHBN^jhjSUabOgs==zK%Svt96BLBP zOf6(zk@hzfJJ!Ky)=+$=Pgx!jh84oCh6y^)d?gtkO<4`T=7i{zk~`br;@!A5uJjE0 zGj4lAoLbV^DuzRMYk^&3HP=kVb-zd9oI$t1oa z_J%AA=eI?szmvqPKv?*rd0N1V+ULhWRYL6{dSwfn4CU0+qS8%>?;Gf9X|V<8@+|Z* zqw-N$F^o|0%$w#DC|f+POFFg;8_KUQCbs0G+o*+%Ic*#xvAu#+zeEwzZlNR*-D@C; zk%0#fA1Z=d6G8*5=32brCI%;;7;K*4ZJ*cFoo%j%^>;!nzh< ziovUP^X}s+&=VhZ6$4()+G^ZXvr_2XKy*WnCy)0=-&j(J+}eQsbi0z+-m%uOP4{(< zA`RL5+l$m|BnF*C8L1y@C(GmHeOa{A)LjLn`h=wzIw7fm%_6&+B>v8 zy$^E;Oe)Wm2c$HX#2d z5*KttO+_M&IP1`se==lgS`aA$CkY}ZJj~cr0z2O|@Fh)2i-7AM5N_yF;bVjs1d32YtFyiFH?*Z!6;?h!PZtl2yqoSe6B?VzSl%pr;!Lr@_KrZqwy~8CF&R?}X`epap zR@^#akOiA;F^}fQdK!~iD-007-c#X4>kC=Ex z=Xr7SqFmuj%8`la@u%mLN80`~bn)*m@k&3V{`6kn`y=*`HlaGdGGgmEb71Kzk_bIv zAj5m!!zcdaGETDL3XPP#i`FQ2HMDubf!6O2Ck~p_vxRBJlivLC63+Zh_kPtd->b3< z3cE_7s|o42T)a9trApA*RJ^z~ytt;<_SfIrJ`I7YcVK zRz<6jTbDEtME}v@I&#Xmb7SX`sd*rEZO}YwUZ4lsR1j%s&2{`DzoS$Yrt078TkFU9 z;-|*j?MEgfr{x_VWFH4MJ?&s#@Xr+B(iaVZfRBwlgZni&%;r^*iaUy%GEBP0Pj5bJ zqvV!jVtdJ~u3y*u`lp(0fpGHB3=6iR#QK)U%(rhffXl-3_O#KN2?B`3c+&14BC4R9 zdecu@sN0!T-S@|A>t}u8KW@Ww2F*M z^(+(`dGw%G^^c7e!CD@OsB-`G>^{Q)n@^7Ije+Gz-*M7_2t0}=bA)ZsyJJf}3!TPe zg)&@f9!D3KYV~Lsz9d!~5=++n@j;cl)8Ez?o96-^zlE3LPwQ-x#2G+Dg$^G6Fi=T~ zts-3ptZR`TRUmdjtNW;5&CI%&hA(GdZrGFzPZ_dT0RK4jukh}IM(NR0Oc&+^D}%^Y zrp_6>J=NHzytp3y3YCSonHGxb?1SG}HkK~rVUQQsg^u%TRQzfEl?6Zb7RbN!hE$fl8%01*BxRp6lwmz$!xcQdO zvdXr>vdRhsqK94JU)2qN@*JY80CdA=SO%lZ&P#EgUH5N7ppb(XdRrDgdm!c9&zy{B z*Nm27-uFDDiN@;Wdo>%cmHg*AxC*&meY%^gE1uOxk-TW86V*wAl8ExDl1SrtbZ8+S zJ!D}PuaEl`0WIS2$->M~?zuZh7OKq2Ipa@vL35ECzDKOu`EHjY9So+t5e(|YCnPu4 zlW`@{B=LsP0|@=lck&a3(okL;&E)s&caWR51v~h65XioZI@d=Z#+1hVhhqHidK1M* zW9gHr+1uRAgTWMm+#@G8{4Gnxh zKsc^U2AKxj`KnYS={^)BtSl~A`$d;A+1%|BrJ>fT$Na1s@6^8hZW_*%NV}&gL-_1t z+}|lY;4paHRw?-0)JgdxjCT0r^*KbzjGq#=pC2Q}Xj5+3WZ>FlV%m&Ed~`|lW6tnS zb#=O;WctG4CL(#p_ux`mvH@f8R*0j5_G!&6BFueL<7LM=~hA#yQR5PDnkiV568a)%|x@2nE^bybId{RK!yii$ z^?}L+KI!lbZO3-7uXcaNP|)O+TO|r@P{jEZkMacZ4FYlu>4jSymb!Vt4%Hp+IDV=k z&Jf1rafP)($7k2=IoI2I9cz5%TE;fOy`-y^r zCNG;EWN;F`TTq(Z!Ck$j@?PtO4#U9qEBto_Wl*p|P%4bWz~X`Qu;u*K#tS5bSU~&J zp|rd9$(=7b4MODVPLm)V>GG(bHQ|Bv`fS8+Evpcn*RY(R@ND9Ht;#7MXsrTnD-Ry! zb3k5L(Ucmn*)X+*Qa8Mx<@iZ;BMDwHq_hxk1?494D>&Z2f3LrDg0Ob`Nu9y_&{uO3 z$UF_cpvW{eTe-*m7OC^wHjvT&0JK7|`ap1sLdDQA&lRP`B&dvn zVOCVq>Ub-xEUlvr4-pucr_VRAwiV2$~5z2Z1w$WTf7_n~EBIx)ExUGNUx<5XZ8GRAPimPWmyofQ{rD_9F_w9oH+_+!O5I!r*9 zInE=wY)fNM6x9{1N+#Z_mNFM#o|;I4Fz69W=pz$Qo3d{}9xt8F1Ix*&tHd$d0gELL z(aHEi3ZMVS(>I5O9k$;u*D_WuyS8ds%UI?bs%0%~Y1y`I+qP}nw(WjT@B4j!{nb_1 z6+WK__kGT}&%sj!9i}c24!uHDpcb~UD97Xa1C;o{Z|f)hON1QX*i<@GfPo`BGF*FA z0KxW8h9n}b4ND^b+l?#xDK&>NBG7DuUJ3ZtmMt6s3~}Sk_Epc}x}+ochMyo0U)6*M8B_y<{KU=$IwR{$)_7dBM>1ilO|O)D!aU^(AS|R?piVdkbP$W(`6k5k4)9Pf_)+wAXbsSChJXHS z?&YLVad5=sDdsTibO(l-s}&FQ)}$>z1&LZLlex3{##v(5ylhb?o^FTK9hVRj4{Z^k|?^vg11%$YWtr3_Q0Td0($TSnatr zFSoc~hWfJ=)sapqQy~oCx*>k1qe*KAqlu7uZ$sFjZw7o};thZ+7F{7S<$l{?WA9lW zY@G=6h+s=49k0InG4l6Mm`B{Mka4!w2AX-rpO}*MaZ`gBh9d%Y)$6y+1OXotRgS#C z`OzBN%lVb@SI%-~b6%dr%t>(d;`sR2{(e|ypL2Isydc~np0L6B_Bc0$-Uu6jl>x2{ zG|2!=E(fk>pqBwTO`RJGWLke0s)K;fD)=@yy_V2}zKs}oO^1XVKvf{h1CPihlb;hi z+uJAxs0l;ad^OxBjhHb5S@-=I+eq%AAObOegp~fnrgekx<|Kx=N1Z9TOmzl&hIyJU zDNit#f@AH&(;A(|EsRCOQhjjRmzJ6EXK={@FP7bVTueOC*9oEYbazlW0SOCWXZ82S zvp%i?)Qj-C37B&NTlxSH1`h`2bDTMV&IKHC+1<)@j-)YvET~*CgC5@*PVA{6ioy0J ze7AQRKeDc{zuHH0k#aFRW*X0@DF z3sQhi)t;@h$Rh^N=f}0rO~=SHObtIW45lRpMSBTHd&{%cuI=h#+7#DzDQef;KLRrr zf2f@!8c%o3uCWUdtcm1m!D)rJnUHcfFV9G}aJQefMF+63EOdDN{lcq>VYzg@DKc=r zaPMUcO7LEveqXI=z4A1&?wf`E18X8;o=x8YyXdWLlr!~~IU|m8G=nR6tWCY332oTf zg_A_idoX^~v*A-Q761*dj#1|XzLt!6WIW8ivFY~sjr`%reiQlth33a3SdLp)Z4Q@05?gzfJN~-;srFQ zptA%T8~o)IkV1KIWdSB91jbar@<6FBTn6n$RD7$7+5h5(nwje1YE6CQxK>2z3as zxp=@o-qDyB(8eYsV?uX#Hki4E*faLI<`Wm6T(Ol)geW+6Lp5>Z7s;#Rifq)Ezo2-m zAS8-)ps|SAAe&u)2;pb3+@ufcPJs@4csleYpIJqq;sIyDQxt~5VSM^Q0iU;5W2av54bNOu6f#34N$ z{4aMPW9EnzywR!93rGuHjijk^Zc1V(M`VV-kdXPVff`scBvdJ~|1 zEZ6nhKN01`VHZApE5wNgKRPxBq7rjTOGDe++Xe4FV8nk0Hwl>f%A5BG^$DU3-t~U3GVezSjJ0v()N(@a=&I5t)w7Vf~2Y-eZ z|0&1EE=9KfEfFW8vz6H8!Ft*za=;s&J37ZT(b=!aQ{!iwHLELf9WC`_I4p0HPFt(w z`B91L^ke(tQEoSiPV=CmqX=NI8Tha&@^K@>4#Zkju+F#BpIIenGW}->G++-Qf9yxr z5aqsU7B=Y6@Dl3h@eyKHYZLI12{U`=e>XuGe}R)83;bkgg_Mv^(fqAPu8>R(8pPw3 zQ+a%XT3a$VlqSj%JvyrXWr;l+P+RKnvyZp5OH4j!NmKQu)%L1YmeZ4c#H;uvJ0si7_xfEI8lnUfI`P;Ifob|nnCCP_2$3hMq7yCG1 zkRdEmHYpVSll=h0s+US1@;uu!y7a9W$rtxgvgz%2d|Q|O9QGy|TMLHR*yvcWOcBG| zF*oXxDq^cjRk*mU;UjJbvOX=Jyo_DoN}6Hi79nukLYa;aUB0~4aOs|WNnPol3cLZo+kptwLQ4Cc7_hQXP6m2h!`Uo<8Cq( z3txE1Tf9^L)+T~;2ntpQXmra@{u#T(8JrHz@4a~H5xgSfLNLPojXvQ0PYWQ~EkUET zWhz9P(#!#`tmDRiJwSQWk@s}ojO0k@Rhaum{ix(XN%*Z~W#x=~Wr?oz$xFi&$%zlm zwyNmLq8V6Kml?`5&gq?6LIcC?tEn}yIHA;*>-Dj^?`85UBiOYt^mVnf#0dT_H!44^ z9BMn2He}8Fl_@0I&(i}+8gQcEeg`bb>%?Y$J}3oH&R$5cVStQdG%3^jmzih6H{wr6 zrDkYP836EBV?;Pg0_9)+JUfu60y$qEv;UHHk%y_)M`~=vH-MM|;!1B%aqOc9U|pgB z+M=F~wFx=?u8p(T6$k1`K-Ru7%C&9k>NXQWuFzal63e{l@r@0M91-XK%FqI$t7>a) zUD*qMD1HqrS#KoK0sa2uBYQ#+FyeZ(mIIffkd~;ZcQJv`f>>&Kc{t!;fQMT^&ju5r zxB3Y;xeN6deD8@|H{q3-Hi46d55@}nPQ+fYInynpW+Y!JM#0Sse0zmivpfeO&nv

{@kT#s{tA1Wc}8qDCuu6iP`QTpLSl+w-?1;JnyH%Y4vTO zsyt(tJ(XHF&T^lTJ^doUYXb@7YDm3^k>C*7i~r{pP^(Xo5+?0~Z;uFOy_rwz<03J{>ms@7I+)%=$x&00mk+<_zfJUoJW-tu)X^LOkk1* zD5AR;BC3r2r7wCMYm7{BlwM8I#c8nJ897~h|PdacC}QBwTwiY8til3V@}>CQF8RngVQ zEhW`JyAOR!Tb```%P11w@HHVa=kx6bssrJgFUsF~18Qkdn!Q@mf>bWw6<%}|^=Vjt z&$eFH$j5e5ch}ow@RlImT=AZC$I`zz0?8oM;Gt(Xh_l+k5UBPP#(!s$UfXF5*!FCoPNsSNlgJ+vxY459K9JqLR z(pr$`K?P7npseqW$|D>1Cw~}cz^wy8(mrPz9Fx8)Gb}LWdnJK}d2dwJ?e{3J8osNl z78pHGbPqN=XvhN%<@H8lpOQgVc9# z9+UxDB^6EgK2+Vfy9zvBQ27Hj!nX_VB-!r`3e!A(ld$fA)4^=%bd1m5Uzo$j+jsE` zp1h3@CXDv`S2*nmf}vUB^YtquF2yOBu8l|fM%8hgL~jl3!P5n(!MBVW1UsNM6g|og z_&*ZVi`)ZRlOt_j0GvgC3C3YDIElJe#(&dn4ymtFVnr~&uTYTg-ey4?cqmC4e|dnT zX-=pVW02gnL(EoCQZo3Mlspn?)@-plnmJ(rNtxhjIKVFX0s;E`eM7Jza9vHNLbuc)X9{-(^=go~gA zKz#eF5@6K}M;+d8G0RH&?#Yx?p|fS z3m*Y&0~)kY_KEu(^wD4!(UoO`q0UVv1=Mt>DxE)>m!ee5VY*?j42ZLH^D&2f z^si=mW;ZVT9s{4$$Ovj4o-pIZApoG1YfU!a%=J2@PL1Ax&7 z0AkzT554I`$yDdOEhDOvlx_vReL+G0Nth$Wog?3r=PP0ADHrX>R43DX+r@AWTt;$R zzTksYl<7=nmC-=9f82=JqU7Gjjm!^x{djHyzI{6&)zs3d6+S2x>;MI2<-Vet<^FQL zf!EvxFlmqtJd8qVCblfXl9FTq%=PMBi3h<%pc_PiruX8rlldN+t;}0h0|zJ%qrkTe%yyvE9$h0Um<4j?JMk-qp(;GS`9KUEn49 zV4l}plR|G-* z{W=T!reUsyV%V zDlbPS9=P8FWE-H4U_XqFj=BzPt%DN3W6cb3zwpqI;8s@Q`@f837(_9K$_7hOg~&mH z1ld^6cQ`l&JCzinKazfpa9P&&>4P9-=t>TM&~XXm9@+JTPz-0C4qHq5v8eApHX(_gA&fu~Xj` zJbc7Q02T`g^#ONo2N5GB3)A!Fp9#)%MU9n)X^+1%q$rm!li#t78^MB9 zHRN2yVd`VRT>=XhkR}8R%0Cz&{sv;aSTq&~l1+)HtK|(3xA@K8N zuByN>EYG{*ClRs=@GA~Og@b29fO;}(5VgC~ji64_E}xs7%{7YiWhI-BJVi|Xt%8PP zffr^2Ec?NGE3-QI!Nyudmow%$C7m>eL7M?1`Xp6L*xTRVzGY$FM;YT8EAn633yhYu zrZr;?*Z9C|Ftkq${AG(h?Zc)c2w;?AMYX0Ir`33H_IA)9MDdY4?cFe{J!9kLReO@P z;~|O$34J*$KSR(#uGIc+3#i8pC$jTaasbE#syXlkNH?MvsZ{P>iNZ9h{pknTIImM1 z%;N^bZqnu@lbI*cIJ@q7*7Z5B0vj5G1H1sRi}&{T*I%AbUS1y< zAcc9r>;jlWGzWO%tMJLr?k@M?hf=l{HXREIB9!KppAOXYKfXrJXqgUZXl0DQ2Fv23 z0etB`K&l}86JNYH4cP}@rq=W>`SLa50h9sYFobH4#gC#K(2vxgE7t>7P5z)60&Rya z_#IQPW>X+YgRd}KDgzxfO!-iTEK23N2mnn56)Vt!ffWqE-LDD?Y1a^EsvU7`C^jwC znr%XtxW_kg?C&wQf3-g%v_Et}Tu}XNpBZ9sxsxKY+e8O!5BOz>;L0K0@PB{tEyJ|e ztTHJ1LuB%`TpJsGWB84w5u>@q@@W49_z zWS48MiEo%03iqC@m^J#WFWEFKP0q|f`M|H4W$Wp4(0B&69|Dd=4**B6vgl#19Rju% z^fOS^k-`4L8$RpxZ4FyceKzr$NFG;@Y3U1tCsO9_hZk-#Z8}L%hn@h+t13Al-2uh? z>$DMwF9J6&5GsS48{{g2V-RdVc0#rz@%I0P!^0qUKxV+>L#_?hR*uSEdrHJwr-QI< zQCsGZ6TROA9Mq-XTv6SE_pIu0RAuA@-W0g1#e#4ZzkSOTEgru|XGUy7D3O-;9jyX2 zD%2)Uxe;~sdW{197nRP~saQM&Ah1o8&6z(2ewL7$z(x!#+q^wp7vPBH1%(}Mfe&8+ z8?wnQUXnx~1i0%yM!$V)Y!R3@t1%qkdvR5S-$l8_q z%MiyI+5^n1_aPE+%dPWoMMW~W$j|Y-q+$-x^z`-gycXp$%dZ9-L3wlUgp`9oftDw? z8%27~$wt*=AKGGiUZa>W0tWSYPQXe6VmRUBoVvRA4NHKZ|Myzy4E2d(iY~GiHa8~& z(Fb4%U`d-{=z&xeXkz{R(Rm5BNvHo2^5W7vy`IHjf+ko}^wGd|R0PDFu_pB}BXc+6 z(q?P@+ZiUT?)%58AYQ4j?BVvK<2U1ZtiqR&Z#97bPfc|Noj4d{vm&>NV|@$N?;?tWG~X@<5Q@fqx8dT`7`53=2Ph z5S&v7xP3HsVapwD{n&;`C}M%g-i>4NCH^fw6@RO&aF_mNR>Rkn>100}kTk=11jT;P zh*?k(P^AUy1SnTPWgYn?0Q;wg z(&*^uc$Aa_OO#KG#fkhXUnM-rQrQ3E0eOTDh^+u==71?IoBIRu$H6BuUekmh2!c92 z0p<>N_Ql;dp`5fdK{ZCO5c8Y~MeK`0kc;?{n!H-g1RFH_M@ERzkVilP>fIqDozSMq zL8!aJ2re(4vrbGO!Ot-IJrQYv+@f|>I$3kfm;H#;De4-pC)VaB(k$ouU}qLAGHC~5 zdncOliq~xeFgzei1jX{?nIjO7lGl>~L2*T|dxtA1Az5+vQW?+V28sGsFIdVBb83*?96qdwoY?vO)0r>}l%l^>syI0@N^$3tD zLKzYNeNG|`)8lzU(E!W;_0|-}y#*hkOLhw0jT1*<=`X!U6w;#Ji1S}UzCbE-;xVdH zjLJVEB23H~kUn^ig$sDxH5**?8Bpfd4k+e1Ahb< zuR#NgY~&4wc;=Ka>ysuUMQC!gIUwRch0z$_?ni-|D0MjncLZ+16a`1rGN2|a%S)4y zGn|Z|3Rfb6?w~GMHc&SOP_SUsz%MyD89idnKnx7#0u+PX$UY`ne>vHOu493j^85F#)`L3`bGO7rJVFS0F^|fv_xADpEg)*N>LXuSe z5eh09BrycsYqtU3^Uor3b;|9TdDeQ1 zx67-L3sQ2j6tLa^8)U5`LdYzVT82TU&I#KFdhzu%>FA#J^~H@dpoh%^vPBS-1kfcQ zeFq16KIRAz^sl0Q$et3P5L)hn6}r##TI*5T{f+I7!lI*)*-Wx5ESL{$ML{tGhSmua zfLr3}CAY)O0^P-(V`NNBL`+Q2`PM*2W58)B0^SiW3&E2rXD|wfGq_1}uZ;cPo8uZ% zfS{UansAOko&Q4n$o{I+Uk-oSAmRC{e2)8f^L8%KJGhn~aJ$B5WFr!lx=O%c0FWi5 z|6<2H(W%{A$&iDmVP3>*Jgfqn`9Qd%*BC03xmvB6*0;M#Jb^H2&nqfl`v_*Hx^U`W z_7PvoYEf~nM1G}UhgIrr`uznGPhTF{9E_s;v^3I8x{#iX&y>^4I<@tIo2&)L#CeYy z2+Vmjm97GNnCU04G20QcpI^Bl5a-2Zw(6R%4C_N6jROkV*g<2PvLOQ+rO_S(mLSko zYI*;8capYp^78@6Sg6t9b9cZ+AtW9OB>CQfTL8KP3l#vf10eFN<9f{Cp+x+|y1_?A z>O1OMKQ{01z?RU+>HJY703>HFn$(zd_KmqJ2tt4HQRj2m%5i+~DYz>%&D1Mnq(b9qjm|k) z#TohA!z^9?T5f0oY9aj0YNmNyCi&J!wOZ3bHYtwnYGfQq#jz71GB!^)gn|j*5(xzZB

+>mK{X{^SZ-j&LS--QbOWPO zKj)Sw-xDdTK8s!yse`}WYmK4@5xTQMlRdN7049Mxy`xO&yVZvOi#L2r9*UEJajXP9 zfVXsf-0T*{bTLG{EXHx$5C209d+jq= zOc`sz&k(5;U^Um%K$JrM!r^u)k^XD!;S!TVrG9fg&O1^S#dyjJpaMY(M+}j=3;K)e zs7tR40gquk(*i=BNWl8NknUV}|JG?@i*`T(`+PYZfa|l%e`w#S?9P3qV6}@l@zxN% z<+t!yDU0x;14>!DpBWK+_p!oZOU+wk-R&$!J4BMd-7nR;$-)Na?H>(f5MKFn_E_ z^58J}HIkQ3vBxmgPyzRFpyIMXa$|sdy*}`^3p7Ue1~uK7h_rme2Pilo(>3F=K@+|H zwtO0E73hbW`>Ik8CNXPt=gk#{7rcGrHjaenQ?Se@3wnhwe&9iQcKmHI;=z35J)~2{ zX14_9#ONxM`8JyH!`pZ%fh^%%K2=brMzcB7UXO$<8ybJY?qs7nL#Sb+P+b5lG2{(b z@BaLrh}GZ1=iI1oyWg_)sIMEX&aJFE@uu502~fiN%|Pj!OB>Vjw4$@xkAQTaUnQ{j zI+hIk*B&Rcnj_bI3dR{}HcsJ%WKUL`!4LdM)5HnZsEl&aYI{F`5KLiWe&>Wx!~ouy}tV6p=^n&gf{= zwRrGy!?xI38TnvW$k{sD<=K`bmn=DcS%JRK75GF^b|9ZuX>SM;M)$ly+KO=kgv)Z5 zcF`iM4pO+q2kyEm=41s)FD~N!5o^7nUr&J z6!Wwf$o>heg(pQ|S)`<}h5bGGe|_Bm0KGtJ$pb(e{`c11vGv9wR;$f<3ZUVT3eIX2 z>p~jnIwXS1TAd6rvLB{1-XSw?SI+vn=e>Q@ORm-m&tUI>G;$whaCf*@@09#09GJIw zfT187P=_)x-FSF#?d|Kkyt*2(1-W;;veP4nLT^h+w3`jv@xY`Sw+}@3r^8UGC&dL_ zeQ^O1ZbWHFsQ$rJf7p-P?M9rfzQ%2{=67)5iVwE#y$|52PuK;hhlQc`ULksH;}z4< z=?`=%*?JA84kf?Lx}xF~!soq2q!S=Fm=M!&RuE4&;8r1N_9F?6@<8h}0^>FzyKllm ztfD_TFn}LDW6w$&2+{K}s5|jOHLZ)psTr*;W_>s3G+YLw@$qq*Hlg?L%|7s-UIQR0 zm6#_MW9$Ox;SmWpa@z00R92?8ba#PvLy4e()c}xfdZ%{>@>WZDfP5d!10luV{cIRL zf&a_TCuxMmrRI}I?{N@D8sKjM{vA|6kM@su&OG9u9m^VRTH*`aKpf2Mvr-Mb5kz~ za+X8}j)zlfw>Ni`0&J>8fq28;p#FDyTfOprovU_GU?4IYT0cP1RmYW}`AfIQACSG> z-q|s5=;Gj-usz%NLJV1V7yY?s{lg1R@mFXTIKs$Mf%p5^iBSXv1?5nKTAhxKT_{^d zfWwzC2nhWP3kydZZ%sS!HBhRY7f~j-YG-%DX`r#SN49fT>g+wIQDi`^%urBC2~|Kq zU9qa^PFh2ukDryRa?)ike9|}xlGCRPv_EK@b9FxFuc_&i!^J(pvu+&yE@Uj#4lb|k zYUn!lpcSv?tdB+sTbg#o6S&$YW%_`IPM|fhB9MuxVuKpJy!wc@c+DF6?t%-WE9`7H zFFGO1KO-)!PwMX8#m)WJZjao!dfV$H73k=16em$Tc;&r@RvL>N5N0u!_PP?xzQ3oCi}RPkz|)Kz>Pd?P!y3u12v}Zu8;@E`vhu^`#RdY zj3%P@*)UX4N6@`#i3s%SqMhrszO_)pwdtg>T*B6$2vMEKx<*OZGcVDdEwYy$N-`QAqxf#9L@4r^|cl433P2YXW2CWX^UDJ%Z1o z@jBO^-q^or4{95VWG8vLwqwt*Qs)SXL$}=$FGh_0!*(UgdLowN-A#$Mn>sYJJ3@(t z+$nJH*%8k>*P3f|IX-utvT)tyh-b!dFys+r<5Np*+`bJaBdjWVBR3tzp+a2xOcog| zn4PrXibhrv7Ff9Gt(M!htM!7<(l8eNiMoYwQ2l!6KF37@ZG~eUhd>;#)=Vb^oqQu_`}!JQPC# z+twkb+Y{9JJ(UN2X6)aj!NkzT=aRe2sbpQPqJkHcS30puJlWOKtLH?XNO?XfPf+NN z&Yj6fjedKZ9hHFv`uA7FG)R|UhX>=POkQA$IQY7P2quxk4aJxS6Lq<=7NYn*L3FIN zxpmojAZ&XeI0S-0oz>fP^p)34_K4#;jIFgM91*D7-2wkS%}2x1Qv-sx;b|kE_@d#E z$jb99W1O7JO7WN)IMpUAiY{-qjH_P`hdhpuY>~d${XrlPn6JwLkbEy=lNS4bHtum9 z-0$>XAwPndFVewV%?CZId}DNcRrl892#Il@?;iw@WLsd$5%FQ=sMotnqj_O&BZ<~n zCwa&_&G%}_E6T&3E23q~RjB;*hbV`|=Wa!Y>^wQX=?9g1>goB*Pz7|;VQnQr415gA zL`A7)O#4p-l@oB~O7<9scs=-%Bm$|)y#2h9*w4&Vwy7+m&M5KCc-;@1bm%TV@jr{q z=?!*>IUhNVho5VIoQSx;FS_y5M*`okyUef2yueg?tfu3_rY)oz5ONhYcq@Abr zRuD^TkaZM~Nvg2cm|bOOgnQu7Y4ujQ55~=zt!}d5`YrhDbEsNVYB;YM${qFw(q<1J zZ6Q1!HN8!Zk1`LqPS9lbOfFZxG@G*DYJNG&_PTZXCaX1?MTavp#$a?#eJmh1?60%@ zbm7Z`d&n#fScsH6H!NJ?{xlGwNW44n=SzleM02go0!bl=cV zt0hIgJz6X|Zhc^bkNR5r;mE5I(V~AOg^>I#O}0$|(HZZ9(3>?dGAN za6T-ARx7$%3_B=_f_HxJlq5|}`g+oMe@Xk9&cVP5pW1bC+xjarht0r$;Rs&!uyaSf z%s~)Bh@2y>1R;N&>ZuY0w+6M7lY^vT1tpIY-hI1qf7iZL^APgARwKDi#k~}z+>C5I zd73D-G_KZyh&#&Uj=fo;@5_|0r%|b}mTRkq(s2Qa4K0=pxT-~`1*0JzRaG$)0<}~T|EteK`M}a}=`XJ2^P^0eGAX5WFLAQ6bpBO-fz5YINvOn&>SvnR75Q zp;srlL~(S^bhg|tLEDaXa7vyFpn?%x%J<^ypBK;~b{YNJkKvCnV@{oeV6&Z<9t0tT zcmmdX5FGtVq3_(5d&^Uy?G)B{wV{lgmhj8>Fp|%rha;s2!E~Dj|E}-Rd{JLM z3LZ?vd#!odLqo*B(UfZ2obp|V&}>mZ#&#UYgQ4vR^3w6(>RG~ly3(myeiX$OcfCT` z{*7!p6F{r-?%4XBoY6}yKWz_!uil5Z%C~3LnKh|q_+O2@p7ud1bP5Q@NAF{Den$d_ z&SvO=4$OS35g|SL@Mg6VxJ3WU(anqb?w6~Ses?ABRnLL^PtU8 z`uBN>^(*^R#xczZo|rv*5F|kmLWoSVwXVPLsKGo6`GM8$y-(Pp}~_@!$+`b$UHur)qpp?X^wL`pILgcVwoer2Q`Ng5+2Io>qpaH2B<={ zB2^e(<9*9>=Y54CYj+Z`a3+h_&t&8`cX;IBPVvCH^j^4xzRgP~Phd1A4eOXBIhqn3 zm^S|`?SemJ@U&O>@#88sKcgq1*s-Hu0X#`a6tsXt;&WZ%^Q1B_JUm=(fG&i1;qBP5 zgCSv*L_#eEH{0e)xM`;za+Hq%NU1LqSpDt@7;|QR96aG`a9T41i9n zq5i~;930ms!^i0L`s2VrAkL)3Czd$C0K}XBtCTqhLI{zLbgvAk(@|*jP~6^XNyOUC zJluDfxN8Zw8g?k^P2Sqoa0_mMqi@ON`kr?g>OnB77_(?yzZM-MYkWX^|F-;i;V1T8 z`;(gw5{4PP@_AFMXieIdX@#%eH~HMTkiLx{1EI4SqmiHz9#3+(Y5Q)q zo!4QR445Av34#zpByx3i zE?&GyWMt%XIS;@d^m;w6uCDm``Ptm>tL>-x`+7Jhu7o|K$a1$zB|ND3k~;$%R>;(G zdcFP_(4Umm3#O|Wl8V7}h(Wbcgb+d`DqXsCc8Bm>8rnD@yF+-9q}%3eNILwqa-H?V7nJVL3$8`5JHIOh3OQ6 zG#4l6tZcN_lc4JL`a>qKN#{9Mo?Ck@gb+fAj5GhiZa$AN>Cx76^GS+UlLZCHN5W)9 z4GA}GGJn*j^V;+=sVF9orUkR-m~0b52q8oQYl(oZ>lK+d`UWeHhIC$=J{rw?5R!Ti zl2$HQYs}4q)h0fsmmH+k%V6!b5JCv?B1@~MAR`M4%##Gugur}zTB{kyrD-_HVgLEe;f@MNrD^P2RgoczM3eyAuE6>eIvLJ*ILZq;^ zdeGLyL!*`Fww5y5T;fP#4?2B-yaP!Pgb+dq+tN!Ak{;8Wk69%{)0Uf3($;V69gnt!=4>7|eSPFnMj>3t*Zgm|u=L zBTE`(ctHpugvc0M!wja+(@GF*B@AgL2Fct0ANk@iAs@VCfB*mh07*qoM6N<$f-gZa AL;wH) literal 0 HcmV?d00001 diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png new file mode 100644 index 0000000000000000000000000000000000000000..960985a3da5de40db2fd6a07798a92982e8210c6 GIT binary patch literal 622 zcmV-!0+IcRP)nDoho~R zjB8~LeZ6peJ)L6>2k_&=n%IY>HU3mtp=@SNT3E4eo|PwGkO(It>n3E}Dy!>j#y>{l z`GRwwL+X(}*5AxU9Tf?p>D;?uCAuM}M1<`5hKqooRkMjMP%W8@h0Yc7M?3lpr35+Q zu|>e_5AP^@;8WQ%s`YQ7>Wu-)23<-M9Uu)8l>3MaChy8ae@GXRm>?0Kd}BFQ0soD2 zpCw{M__=SAAUE7TqYQlyU z$0NbFj2-8m)k&6bf%t((@VGbU3*#R=F>W58#6|grn$*BO^lFd~J?NY>WDp^GkYPR* z>`9i9G5wH3QjPf%>j*LQv-GTn)X*zUVuB5bKqn_5HsqOp#!2mMduHYG<$FchpklRB zzv4Oh9X7*#N>J)oJXJJ8K9a}e{=sT#i-E+cyuUcFcKE0G2Dxi{(s;$-S^xk507*qo IM6N<$g4(MY6951J literal 0 HcmV?d00001 diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png new file mode 100644 index 0000000000000000000000000000000000000000..07b1e33132e430476307b9d5f57d35b9da9f0097 GIT binary patch literal 1632 zcmV-m2A}zfP)92ut1VzFk$_)z#&eG=!_GtF`6j<#qh~zSU~2zr4KgTEpk0R5Zi;5%0L} za19oI4fJ6fr!CXFVZ*QMT4OAvf^@W|D z&ugRef;NuH?PFDGS0a3KbJGRU3sFiHWjO=)aqmOzzpk#Xe(!WT-zLiphr{2Vo}LoU z;+)O#s4KHGxeFQU{tnW2%JO=`*VorQ+|Ct6xtyf9F1NO}22umiqV=Vvr2|othi&&Z zHa7kc1yqD#uP4S{eHfw48Px1^tToh_&pxEzlK^pJ*C!G{kg!s+mr$*vnCs;vNE{O9 zfcJdmG?H!wzKE*zr>6*|da}8>`B@eKvof6e%3$C>_-xuq*o^9xxK*k0gb@SEe(kul zt?j5Ja43!yjV_cdK_dKadG)<*$d^r5eJf$wt;c)vj^H8?B26EG>Ti2DqtXGD85tYN zX0R{I;6L^xBC^VQOb0FtWt*N?iL@$gdxRUM_ZRw zC#t$+3)CTPB$(RX8DBg{PMDDkhtbnjtK>G$OSV8A(&+0K+jDL`$&vr(giUnOhOCM= z1+pBpxycqFX>_!!EswtT#i7=?a*l;cZ#&xPixPAKBqG@YSSBw_+ghZKc5;&~2zMQo zFGYcX$1^Y1*mecdUmedA{wWAsI|`flCIK`|wt$*6`rLQC`d6iC)V2~K`&|&(0xHs; z6(!-H7H2Os^j&@eK>EK@!i$b8|1cnG`b%4WGW<{YzYY>^<$ZFF3O`1`>xHK~aF&HH-s``*cqOn#c$3xikCb%1|hC znj-9r?fQ-&%b>ksRNoo9&b0L~uskd3v2K;%q36UT1E;BFbLRBW6Tw&KQIr8!ZyO`c z44^}qRl7Vlgv&;qBp6@*iS5M1c3iq<(i{?7BXy5L2**)Qt_@NHfs_nuyEEHEE>XTE z1~cJV#jt53FdLT(*L##?@B9Z z63hnlnVh`N@Xn>tN!X}1^C+AyBU6ZAm!}%XBnCLZ&^%vQcx3WVoPG9&cqWoZd^qHY z*E!Nop%a5nr!($^W?8E7GXkpbo^}ma6E>X*&5kx?X7hdLaXjceted@TB6?KL8LLE? z&?b5=0IAtFR@#SGCR_l-dxm%-i$!LL2Dgkh)Fw{d%h`K*CC_Z3q*RO&%y7@RABLv% e%RdB(iGKhQFK?{O$+Euy0000 +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class GearDisplay +{ +public: + GearDisplay(); + void drawGearIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + +private: + int current_gear_; // Internal variable to store current gear + QColor gray = QColor(194, 194, 194); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // GEAR_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp new file mode 100644 index 0000000000000..9b3ecccf6301c --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp @@ -0,0 +1,157 @@ +// Copyright 2024 The Autoware Contributors +// +// 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. + +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +#ifndef OVERLAY_TEXT_DISPLAY_HPP_ +#define OVERLAY_TEXT_DISPLAY_HPP_ + +#include "rviz_2d_overlay_msgs/msg/overlay_text.hpp" +#ifndef Q_MOC_RUN +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#endif + +namespace awf_2d_overlay_vehicle +{ +class OverlayTextDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT +public: + OverlayTextDisplay(); + virtual ~OverlayTextDisplay(); + +protected: + awf_2d_overlay_vehicle::OverlayObject::SharedPtr overlay_; + + int texture_width_; + int texture_height_; + + bool overtake_fg_color_properties_; + bool overtake_bg_color_properties_; + bool overtake_position_properties_; + bool align_bottom_; + bool invert_shadow_; + QColor bg_color_; + QColor fg_color_; + int text_size_; + int line_width_; + std::string text_; + QStringList font_families_; + std::string font_; + int horizontal_dist_; + int vertical_dist_; + HorizontalAlignment horizontal_alignment_; + VerticalAlignment vertical_alignment_; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + + bool require_update_texture_; + // properties are raw pointers since they are owned by Qt + rviz_common::properties::BoolProperty * overtake_position_properties_property_; + rviz_common::properties::BoolProperty * overtake_fg_color_properties_property_; + rviz_common::properties::BoolProperty * overtake_bg_color_properties_property_; + rviz_common::properties::BoolProperty * align_bottom_property_; + rviz_common::properties::BoolProperty * invert_shadow_property_; + rviz_common::properties::IntProperty * hor_dist_property_; + rviz_common::properties::IntProperty * ver_dist_property_; + rviz_common::properties::EnumProperty * hor_alignment_property_; + rviz_common::properties::EnumProperty * ver_alignment_property_; + rviz_common::properties::IntProperty * width_property_; + rviz_common::properties::IntProperty * height_property_; + rviz_common::properties::IntProperty * text_size_property_; + rviz_common::properties::IntProperty * line_width_property_; + rviz_common::properties::ColorProperty * bg_color_property_; + rviz_common::properties::FloatProperty * bg_alpha_property_; + rviz_common::properties::ColorProperty * fg_color_property_; + rviz_common::properties::FloatProperty * fg_alpha_property_; + rviz_common::properties::EnumProperty * font_property_; + +protected Q_SLOTS: + void updateOvertakePositionProperties(); + void updateOvertakeFGColorProperties(); + void updateOvertakeBGColorProperties(); + void updateAlignBottom(); + void updateInvertShadow(); + void updateHorizontalDistance(); + void updateVerticalDistance(); + void updateHorizontalAlignment(); + void updateVerticalAlignment(); + void updateWidth(); + void updateHeight(); + void updateTextSize(); + void updateFGColor(); + void updateFGAlpha(); + void updateBGColor(); + void updateBGAlpha(); + void updateFont(); + void updateLineWidth(); + +private: + void processMessage(rviz_2d_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) override; +}; +} // namespace awf_2d_overlay_vehicle + +#endif // OVERLAY_TEXT_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp new file mode 100644 index 0000000000000..1270f15ca80ae --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp @@ -0,0 +1,141 @@ +// Copyright 2024 The Autoware Contributors +// +// 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. + +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef OVERLAY_UTILS_HPP_ +#define OVERLAY_UTILS_HPP_ + +#include +#include + +#include "rviz_2d_overlay_msgs/msg/overlay_text.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace awf_2d_overlay_vehicle +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; +}; + +enum class VerticalAlignment : uint8_t { + CENTER = rviz_2d_overlay_msgs::msg::OverlayText::CENTER, + TOP = rviz_2d_overlay_msgs::msg::OverlayText::TOP, + BOTTOM = rviz_2d_overlay_msgs::msg::OverlayText::BOTTOM, +}; + +enum class HorizontalAlignment : uint8_t { + LEFT = rviz_2d_overlay_msgs::msg::OverlayText::LEFT, + RIGHT = rviz_2d_overlay_msgs::msg::OverlayText::RIGHT, + CENTER = rviz_2d_overlay_msgs::msg::OverlayText::CENTER +}; + +/** + * Helper class for realizing an overlay object on top of the rviz 3D panel. + * + * This class is supposed to be instantiated in the onInitialize method of the + * rviz_common::Display class. + */ +class OverlayObject +{ +public: + using SharedPtr = std::shared_ptr; + + explicit OverlayObject(const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName() const; + virtual void hide(); + virtual void show(); + virtual bool isTextureReady() const; + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition( + double hor_dist, double ver_dist, HorizontalAlignment hor_alignment = HorizontalAlignment::LEFT, + VerticalAlignment ver_alignment = VerticalAlignment::TOP); + virtual void setDimensions(double width, double height); + virtual bool isVisible() const; + virtual unsigned int getTextureWidth() const; + virtual unsigned int getTextureHeight() const; + +protected: + const std::string name_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; +}; +} // namespace awf_2d_overlay_vehicle + +#endif // OVERLAY_UTILS_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp new file mode 100644 index 0000000000000..aff475aba2f33 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp @@ -0,0 +1,124 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 SIGNAL_DISPLAY_HPP_ +#define SIGNAL_DISPLAY_HPP_ +#ifndef Q_MOC_RUN +#include "gear_display.hpp" +#include "overlay_utils.hpp" +#include "speed_display.hpp" +#include "speed_limit_display.hpp" +#include "steering_wheel_display.hpp" +#include "traffic_display.hpp" +#include "turn_signals_display.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#endif + +namespace awf_2d_overlay_vehicle +{ +class SignalDisplay : public rviz_common::Display +{ + Q_OBJECT +public: + SignalDisplay(); + ~SignalDisplay() override; + +protected: + void onInitialize() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateOverlaySize(); + void updateSmallOverlaySize(); + void updateOverlayPosition(); + void updateOverlayColor(); + void topic_updated_gear(); + void topic_updated_steering(); + void topic_updated_speed(); + void topic_updated_speed_limit(); + void topic_updated_turn_signals(); + void topic_updated_hazard_lights(); + void topic_updated_traffic(); + +private: + std::mutex mutex_; + awf_2d_overlay_vehicle::OverlayObject::SharedPtr overlay_; + rviz_common::properties::IntProperty * property_width_; + rviz_common::properties::IntProperty * property_height_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::ColorProperty * property_signal_color_; + std::unique_ptr steering_topic_property_; + std::unique_ptr gear_topic_property_; + std::unique_ptr speed_topic_property_; + std::unique_ptr turn_signals_topic_property_; + std::unique_ptr hazard_lights_topic_property_; + std::unique_ptr traffic_topic_property_; + std::unique_ptr speed_limit_topic_property_; + + void drawBackground(QPainter & painter, const QRectF & backgroundRect); + void setupRosSubscriptions(); + + std::unique_ptr steering_wheel_display_; + std::unique_ptr gear_display_; + std::unique_ptr speed_display_; + std::unique_ptr turn_signals_display_; + std::unique_ptr traffic_display_; + std::unique_ptr speed_limit_display_; + + rclcpp::Subscription::SharedPtr gear_sub_; + rclcpp::Subscription::SharedPtr steering_sub_; + rclcpp::Subscription::SharedPtr speed_sub_; + rclcpp::Subscription::SharedPtr + turn_signals_sub_; + rclcpp::Subscription::SharedPtr + hazard_lights_sub_; + rclcpp::Subscription::SharedPtr traffic_sub_; + rclcpp::Subscription::SharedPtr speed_limit_sub_; + + std::mutex property_mutex_; + + void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void drawWidget(QImage & hud); +}; +} // namespace awf_2d_overlay_vehicle + +#endif // SIGNAL_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp new file mode 100644 index 0000000000000..317e45917f927 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 SPEED_DISPLAY_HPP_ +#define SPEED_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class SpeedDisplay +{ +public: + SpeedDisplay(); + void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + +private: + float current_speed_; // Internal variable to store current speed + QColor gray = QColor(194, 194, 194); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // SPEED_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp new file mode 100644 index 0000000000000..00eae077ff2ac --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 SPEED_LIMIT_DISPLAY_HPP_ +#define SPEED_LIMIT_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class SpeedLimitDisplay +{ +public: + SpeedLimitDisplay(); + void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + +private: + float current_limit; // Internal variable to store current gear + QColor gray = QColor(194, 194, 194); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // SPEED_LIMIT_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp new file mode 100644 index 0000000000000..a8291064c3807 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 STEERING_WHEEL_DISPLAY_HPP_ +#define STEERING_WHEEL_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class SteeringWheelDisplay +{ +public: + SteeringWheelDisplay(); + void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); + void updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + +private: + float steering_angle_ = 0.0f; + QColor gray = QColor(194, 194, 194); + + QImage wheelImage; + QImage scaledWheelImage; + QImage coloredImage(const QImage & source, const QColor & color); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // STEERING_WHEEL_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp new file mode 100644 index 0000000000000..bf776d27dfa94 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp @@ -0,0 +1,62 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 TRAFFIC_DISPLAY_HPP_ +#define TRAFFIC_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class TrafficDisplay +{ +public: + TrafficDisplay(); + void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficSignalArray current_traffic_; + +private: + QImage traffic_light_image_; + // yellow #CFC353 + QColor yellow = QColor(207, 195, 83); + // red #CF5353 + QColor red = QColor(207, 83, 83); + // green #53CF5F + QColor green = QColor(83, 207, 95); + // gray #C2C2C2 + QColor gray = QColor(194, 194, 194); + + QImage coloredImage(const QImage & source, const QColor & color); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // TRAFFIC_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp new file mode 100644 index 0000000000000..cd659883c9ca0 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 TURN_SIGNALS_DISPLAY_HPP_ +#define TURN_SIGNALS_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +namespace awf_2d_overlay_vehicle +{ + +class TurnSignalsDisplay +{ +public: + TurnSignalsDisplay(); + void drawArrows(QPainter & painter, const QRectF & backgroundRect, const QColor & color); + void updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + +private: + QImage arrowImage; + QColor gray = QColor(194, 194, 194); + + int current_turn_signal_; // Internal variable to store turn signal state + int current_hazard_lights_; // Internal variable to store hazard lights state + QImage coloredImage(const QImage & source, const QColor & color); + + std::chrono::steady_clock::time_point last_toggle_time_; + bool blink_on_ = false; + const std::chrono::milliseconds blink_interval_{500}; // Blink interval in milliseconds +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // TURN_SIGNALS_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml new file mode 100644 index 0000000000000..b19b384d020b6 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml @@ -0,0 +1,31 @@ + + + + awf_2d_overlay_vehicle + 0.0.1 + + RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin + (https://github.com/jsk-ros-pkg/jsk_visualization). + + Khalil Selyan + + BSD-3-Clause + + autoware_auto_vehicle_msgs + autoware_perception_msgs + boost + rviz_2d_overlay_msgs + rviz_common + rviz_ogre_vendor + rviz_rendering + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml new file mode 100644 index 0000000000000..8a5af0abcf0dd --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml @@ -0,0 +1,5 @@ + + + Signal overlay plugin for the 3D view. + + diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp new file mode 100644 index 0000000000000..153e106f04264 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp @@ -0,0 +1,98 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "gear_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +GearDisplay::GearDisplay() : current_gear_(0) +{ + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void GearDisplay::updateGearData( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +{ + current_gear_ = msg->report; // Assuming msg->report contains the gear information +} + +void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // we deal with the different gears here + std::string gearString; + switch (current_gear_) { + case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: + gearString = "N"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::LOW: + case autoware_auto_vehicle_msgs::msg::GearReport::LOW_2: + gearString = "L"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::NONE: + case autoware_auto_vehicle_msgs::msg::GearReport::PARK: + gearString = "P"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: + case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE_2: + gearString = "R"; + break; + // all the drive gears from DRIVE to DRIVE_16 + default: + gearString = "D"; + break; + } + + QFont gearFont("Quicksand", 16, QFont::Bold); + painter.setFont(gearFont); + QPen borderPen(gray); + borderPen.setWidth(4); + painter.setPen(borderPen); + + int gearBoxSize = 30; + int gearX = backgroundRect.left() + 30 + gearBoxSize; + int gearY = backgroundRect.height() - gearBoxSize - 20; + QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize); + painter.setBrush(QColor(0, 0, 0, 0)); + painter.drawRoundedRect(gearRect, 5, 5); + painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString)); +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp new file mode 100644 index 0000000000000..053d0f6e981c0 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp @@ -0,0 +1,556 @@ +// Copyright 2024 The Autoware Contributors +// +// 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. + +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "overlay_text_display.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace awf_2d_overlay_vehicle +{ +OverlayTextDisplay::OverlayTextDisplay() +: texture_width_(0), + texture_height_(0), + bg_color_(0, 0, 0, 0), + fg_color_(255, 255, 255, 255.0), + text_size_(14), + line_width_(2), + text_(""), + font_(""), + require_update_texture_(false) +{ + overtake_position_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake Position Properties", false, + "overtake position properties specified by message such as left, top and font", this, + SLOT(updateOvertakePositionProperties())); + overtake_fg_color_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake FG Color Properties", false, + "overtake color properties specified by message such as foreground color and alpha", this, + SLOT(updateOvertakeFGColorProperties())); + overtake_bg_color_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake BG Color Properties", false, + "overtake color properties specified by message such as background color and alpha", this, + SLOT(updateOvertakeBGColorProperties())); + align_bottom_property_ = new rviz_common::properties::BoolProperty( + "Align Bottom", false, "align text with the bottom of the overlay region", this, + SLOT(updateAlignBottom())); + invert_shadow_property_ = new rviz_common::properties::BoolProperty( + "Invert Shadow", false, "make shadow lighter than original text", this, + SLOT(updateInvertShadow())); + hor_dist_property_ = new rviz_common::properties::IntProperty( + "hor_dist", 0, "horizontal distance to anchor", this, SLOT(updateHorizontalDistance())); + ver_dist_property_ = new rviz_common::properties::IntProperty( + "ver_dist", 0, "vertical distance to anchor", this, SLOT(updateVerticalDistance())); + hor_alignment_property_ = new rviz_common::properties::EnumProperty( + "hor_alignment", "left", "horizontal alignment of the overlay", this, + SLOT(updateHorizontalAlignment())); + hor_alignment_property_->addOption("left", rviz_2d_overlay_msgs::msg::OverlayText::LEFT); + hor_alignment_property_->addOption("center", rviz_2d_overlay_msgs::msg::OverlayText::CENTER); + hor_alignment_property_->addOption("right", rviz_2d_overlay_msgs::msg::OverlayText::RIGHT); + ver_alignment_property_ = new rviz_common::properties::EnumProperty( + "ver_alignment", "top", "vertical alignment of the overlay", this, + SLOT(updateVerticalAlignment())); + ver_alignment_property_->addOption("top", rviz_2d_overlay_msgs::msg::OverlayText::TOP); + ver_alignment_property_->addOption("center", rviz_2d_overlay_msgs::msg::OverlayText::CENTER); + ver_alignment_property_->addOption("bottom", rviz_2d_overlay_msgs::msg::OverlayText::BOTTOM); + width_property_ = new rviz_common::properties::IntProperty( + "width", 128, "width position", this, SLOT(updateWidth())); + width_property_->setMin(0); + height_property_ = new rviz_common::properties::IntProperty( + "height", 128, "height position", this, SLOT(updateHeight())); + height_property_->setMin(0); + text_size_property_ = new rviz_common::properties::IntProperty( + "text size", 12, "text size", this, SLOT(updateTextSize())); + text_size_property_->setMin(0); + line_width_property_ = new rviz_common::properties::IntProperty( + "line width", 2, "line width", this, SLOT(updateLineWidth())); + line_width_property_->setMin(0); + fg_color_property_ = new rviz_common::properties::ColorProperty( + "Foreground Color", QColor(25, 255, 240), "Foreground Color", this, SLOT(updateFGColor())); + fg_alpha_property_ = new rviz_common::properties::FloatProperty( + "Foreground Alpha", 0.8, "Foreground Alpha", this, SLOT(updateFGAlpha())); + fg_alpha_property_->setMin(0.0); + fg_alpha_property_->setMax(1.0); + bg_color_property_ = new rviz_common::properties::ColorProperty( + "Background Color", QColor(0, 0, 0), "Background Color", this, SLOT(updateBGColor())); + bg_alpha_property_ = new rviz_common::properties::FloatProperty( + "Background Alpha", 0.8, "Background Alpha", this, SLOT(updateBGAlpha())); + bg_alpha_property_->setMin(0.0); + bg_alpha_property_->setMax(1.0); + + QFontDatabase database; + font_families_ = database.families(); + font_property_ = new rviz_common::properties::EnumProperty( + "font", "DejaVu Sans Mono", "font", this, SLOT(updateFont())); + for (ssize_t i = 0; i < font_families_.size(); i++) { + font_property_->addOption(font_families_[i], static_cast(i)); + } +} + +OverlayTextDisplay::~OverlayTextDisplay() +{ + onDisable(); +} + +void OverlayTextDisplay::onEnable() +{ + if (overlay_) { + overlay_->show(); + } + subscribe(); +} + +void OverlayTextDisplay::onDisable() +{ + if (overlay_) { + overlay_->hide(); + } + unsubscribe(); +} + +// only the first time +void OverlayTextDisplay::onInitialize() +{ + RTDClass::onInitialize(); + rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_); + + onEnable(); + updateTopic(); + updateOvertakePositionProperties(); + updateOvertakeFGColorProperties(); + updateOvertakeBGColorProperties(); + updateAlignBottom(); + updateInvertShadow(); + updateHorizontalDistance(); + updateVerticalDistance(); + updateHorizontalAlignment(); + updateVerticalAlignment(); + updateWidth(); + updateHeight(); + updateTextSize(); + updateFGColor(); + updateFGAlpha(); + updateBGColor(); + updateBGAlpha(); + updateFont(); + updateLineWidth(); + require_update_texture_ = true; +} + +void OverlayTextDisplay::update(float /*wall_dt*/, float /*ros_dt*/) +{ + if (!require_update_texture_) { + return; + } + if (!isEnabled()) { + return; + } + if (!overlay_) { + return; + } + + overlay_->updateTextureSize(texture_width_, texture_height_); + { + awf_2d_overlay_vehicle::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage Hud = buffer.getQImage(*overlay_, bg_color_); + QPainter painter(&Hud); + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setPen(QPen(fg_color_, std::max(line_width_, 1), Qt::SolidLine)); + uint16_t w = overlay_->getTextureWidth(); + uint16_t h = overlay_->getTextureHeight(); + + // font + if (text_size_ != 0) { + // QFont font = painter.font(); + QFont font(font_.length() > 0 ? font_.c_str() : "Liberation Sans"); + font.setPointSize(text_size_); + font.setBold(true); + painter.setFont(font); + } + if (text_.length() > 0) { + QColor shadow_color; + if (invert_shadow_) + shadow_color = Qt::white; // fg_color_.lighter(); + else + shadow_color = Qt::black; // fg_color_.darker(); + shadow_color.setAlpha(fg_color_.alpha()); + + std::string color_wrapped_text = + (boost::format("%1%") % text_ % + fg_color_.red() % fg_color_.green() % fg_color_.blue() % fg_color_.alpha()) + .str(); + + // find a remove "color: XXX;" regex match to generate a proper shadow + std::regex color_tag_re("color:.+?;"); + std::string null_char(""); + std::string formatted_text_ = std::regex_replace(text_, color_tag_re, null_char); + std::string color_wrapped_shadow = + (boost::format("%1%") % + formatted_text_ % shadow_color.red() % shadow_color.green() % shadow_color.blue() % + shadow_color.alpha()) + .str(); + + QStaticText static_text( + boost::algorithm::replace_all_copy(color_wrapped_text, "\n", "
").c_str()); + static_text.setTextWidth(w); + + painter.setPen(QPen(shadow_color, std::max(line_width_, 1), Qt::SolidLine)); + QStaticText static_shadow( + boost::algorithm::replace_all_copy(color_wrapped_shadow, "\n", "
").c_str()); + static_shadow.setTextWidth(w); + + if (!align_bottom_) { + painter.drawStaticText(1, 1, static_shadow); + painter.drawStaticText(0, 0, static_text); + } else { + QStaticText only_wrapped_text(color_wrapped_text.c_str()); + QFontMetrics fm(painter.fontMetrics()); + QRect text_rect = fm.boundingRect( + 0, 0, w, h, Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop, + only_wrapped_text.text().remove(QRegExp("<[^>]*>"))); + painter.drawStaticText(1, h - text_rect.height() + 1, static_shadow); + painter.drawStaticText(0, h - text_rect.height(), static_text); + } + } + painter.end(); + } + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + require_update_texture_ = false; +} + +void OverlayTextDisplay::reset() +{ + RTDClass::reset(); + + if (overlay_) { + overlay_->hide(); + } +} + +void OverlayTextDisplay::processMessage(rviz_2d_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) +{ + if (!isEnabled()) { + return; + } + if (!overlay_) { + static int count = 0; + std::stringstream ss; + ss << "OverlayTextDisplayObject" << count++; + overlay_.reset(new awf_2d_overlay_vehicle::OverlayObject(ss.str())); + overlay_->show(); + } + if (overlay_) { + if (msg->action == rviz_2d_overlay_msgs::msg::OverlayText::DELETE) { + overlay_->hide(); + } else if (msg->action == rviz_2d_overlay_msgs::msg::OverlayText::ADD) { + overlay_->show(); + } + } + + // store message for update method + text_ = msg->text; + + if (!overtake_position_properties_) { + texture_width_ = msg->width; + texture_height_ = msg->height; + text_size_ = msg->text_size; + horizontal_dist_ = msg->horizontal_distance; + vertical_dist_ = msg->vertical_distance; + + horizontal_alignment_ = HorizontalAlignment{msg->horizontal_alignment}; + vertical_alignment_ = VerticalAlignment{msg->vertical_alignment}; + } + if (!overtake_bg_color_properties_) + bg_color_ = QColor( + msg->bg_color.r * 255.0, msg->bg_color.g * 255.0, msg->bg_color.b * 255.0, + msg->bg_color.a * 255.0); + if (!overtake_fg_color_properties_) { + fg_color_ = QColor( + msg->fg_color.r * 255.0, msg->fg_color.g * 255.0, msg->fg_color.b * 255.0, + msg->fg_color.a * 255.0); + font_ = msg->font; + line_width_ = msg->line_width; + } + if (overlay_) { + overlay_->setPosition( + horizontal_dist_, vertical_dist_, horizontal_alignment_, vertical_alignment_); + } + require_update_texture_ = true; +} + +void OverlayTextDisplay::updateOvertakePositionProperties() +{ + if (!overtake_position_properties_ && overtake_position_properties_property_->getBool()) { + updateVerticalDistance(); + updateHorizontalDistance(); + updateVerticalAlignment(); + updateHorizontalAlignment(); + updateWidth(); + updateHeight(); + updateTextSize(); + require_update_texture_ = true; + } + + overtake_position_properties_ = overtake_position_properties_property_->getBool(); + if (overtake_position_properties_) { + hor_dist_property_->show(); + ver_dist_property_->show(); + hor_alignment_property_->show(); + ver_alignment_property_->show(); + width_property_->show(); + height_property_->show(); + text_size_property_->show(); + } else { + hor_dist_property_->hide(); + ver_dist_property_->hide(); + hor_alignment_property_->hide(); + ver_alignment_property_->hide(); + width_property_->hide(); + height_property_->hide(); + text_size_property_->hide(); + } +} + +void OverlayTextDisplay::updateOvertakeFGColorProperties() +{ + if (!overtake_fg_color_properties_ && overtake_fg_color_properties_property_->getBool()) { + // read all the parameters from properties + updateFGColor(); + updateFGAlpha(); + updateFont(); + updateLineWidth(); + require_update_texture_ = true; + } + overtake_fg_color_properties_ = overtake_fg_color_properties_property_->getBool(); + if (overtake_fg_color_properties_) { + fg_color_property_->show(); + fg_alpha_property_->show(); + line_width_property_->show(); + font_property_->show(); + } else { + fg_color_property_->hide(); + fg_alpha_property_->hide(); + line_width_property_->hide(); + font_property_->hide(); + } +} + +void OverlayTextDisplay::updateOvertakeBGColorProperties() +{ + if (!overtake_bg_color_properties_ && overtake_bg_color_properties_property_->getBool()) { + // read all the parameters from properties + updateBGColor(); + updateBGAlpha(); + require_update_texture_ = true; + } + overtake_bg_color_properties_ = overtake_bg_color_properties_property_->getBool(); + if (overtake_bg_color_properties_) { + bg_color_property_->show(); + bg_alpha_property_->show(); + } else { + bg_color_property_->hide(); + bg_alpha_property_->hide(); + } +} + +void OverlayTextDisplay::updateAlignBottom() +{ + if (align_bottom_ != align_bottom_property_->getBool()) { + require_update_texture_ = true; + } + align_bottom_ = align_bottom_property_->getBool(); +} + +void OverlayTextDisplay::updateInvertShadow() +{ + if (invert_shadow_ != invert_shadow_property_->getBool()) { + require_update_texture_ = true; + } + invert_shadow_ = invert_shadow_property_->getBool(); +} + +void OverlayTextDisplay::updateVerticalDistance() +{ + vertical_dist_ = ver_dist_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHorizontalDistance() +{ + horizontal_dist_ = hor_dist_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateVerticalAlignment() +{ + vertical_alignment_ = + VerticalAlignment{static_cast(ver_alignment_property_->getOptionInt())}; + + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHorizontalAlignment() +{ + horizontal_alignment_ = + HorizontalAlignment{static_cast(hor_alignment_property_->getOptionInt())}; + + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateWidth() +{ + texture_width_ = width_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHeight() +{ + texture_height_ = height_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateTextSize() +{ + text_size_ = text_size_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateBGColor() +{ + QColor c = bg_color_property_->getColor(); + bg_color_.setRed(c.red()); + bg_color_.setGreen(c.green()); + bg_color_.setBlue(c.blue()); + if (overtake_bg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateBGAlpha() +{ + bg_color_.setAlpha(bg_alpha_property_->getFloat() * 255.0); + if (overtake_bg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFGColor() +{ + QColor c = fg_color_property_->getColor(); + fg_color_.setRed(c.red()); + fg_color_.setGreen(c.green()); + fg_color_.setBlue(c.blue()); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFGAlpha() +{ + fg_color_.setAlpha(fg_alpha_property_->getFloat() * 255.0); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFont() +{ + int font_index = font_property_->getOptionInt(); + if (font_index < font_families_.size()) { + font_ = font_families_[font_index].toStdString(); + } else { + RVIZ_COMMON_LOG_ERROR_STREAM("Unexpected error at selecting font index " << font_index); + return; + } + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateLineWidth() +{ + line_width_ = line_width_property_->getInt(); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +} // namespace awf_2d_overlay_vehicle + +#include +PLUGINLIB_EXPORT_CLASS(awf_2d_overlay_vehicle::OverlayTextDisplay, rviz_common::Display) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp new file mode 100644 index 0000000000000..e65a74415fb6a --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp @@ -0,0 +1,267 @@ +// Copyright 2024 The Autoware Contributors +// +// 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. + +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "overlay_utils.hpp" + +#include + +namespace awf_2d_overlay_vehicle +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject(const std::string & name) : name_(name) +{ + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + if (mOverlayMgr) { + mOverlayMgr->destroyOverlayElement(panel_); + mOverlayMgr->destroy(overlay_); + } + if (panel_material_) { + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + } +} + +std::string OverlayObject::getName() const +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() const +{ + return texture_ != nullptr; +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] width=0 is specified as texture size"); + width = 1; + } + + if (height == 0) { + RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] height=0 is specified as texture size"); + height = 1; + } + + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition( + double hor_dist, double ver_dist, HorizontalAlignment hor_alignment, + VerticalAlignment ver_alignment) +{ + // ogre position is always based on the top left corner of the panel, while our position input + // depends on the chosen alignment, i.e. if the horizontal alignment is right, increasing the + // horizontal dist moves the panel to the left (further away from the right border) + double left = 0; + double top = 0; + + switch (hor_alignment) { + case HorizontalAlignment::LEFT: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_LEFT); + left = hor_dist; + break; + case HorizontalAlignment::CENTER: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_CENTER); + left = hor_dist - panel_->getWidth() / 2; + break; + case HorizontalAlignment::RIGHT: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_RIGHT); + left = -hor_dist - panel_->getWidth(); + break; + } + + switch (ver_alignment) { + case VerticalAlignment::BOTTOM: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_BOTTOM); + top = -ver_dist - panel_->getHeight(); + break; + case VerticalAlignment::CENTER: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_CENTER); + top = ver_dist - panel_->getHeight() / 2; + break; + case VerticalAlignment::TOP: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_TOP); + top = ver_dist; + break; + } + + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() const +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() const +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() const +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp new file mode 100644 index 0000000000000..f2a82dd386b37 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp @@ -0,0 +1,501 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "signal_display.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SignalDisplay::SignalDisplay() +{ + property_width_ = new rviz_common::properties::IntProperty( + "Width", 517, "Width of the overlay", this, SLOT(updateOverlaySize())); + property_height_ = new rviz_common::properties::IntProperty( + "Height", 175, "Height of the overlay", this, SLOT(updateOverlaySize())); + property_left_ = new rviz_common::properties::IntProperty( + "Left", 10, "Left position of the overlay", this, SLOT(updateOverlayPosition())); + property_top_ = new rviz_common::properties::IntProperty( + "Top", 10, "Top position of the overlay", this, SLOT(updateOverlayPosition())); + property_signal_color_ = new rviz_common::properties::ColorProperty( + "Signal Color", QColor(94, 130, 255), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + + // Initialize the component displays + steering_wheel_display_ = std::make_unique(); + gear_display_ = std::make_unique(); + speed_display_ = std::make_unique(); + turn_signals_display_ = std::make_unique(); + traffic_display_ = std::make_unique(); + speed_limit_display_ = std::make_unique(); +} + +void SignalDisplay::onInitialize() +{ + std::lock_guard lock(property_mutex_); + + rviz_common::Display::onInitialize(); + rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_); + static int count = 0; + std::stringstream ss; + ss << "SignalDisplayObject" << count++; + overlay_.reset(new awf_2d_overlay_vehicle::OverlayObject(ss.str())); + overlay_->show(); + updateOverlaySize(); + updateOverlayPosition(); + + auto rviz_ros_node = context_->getRosNodeAbstraction(); + + gear_topic_property_ = std::make_unique( + "Gear Topic Test", "/vehicle/status/gear_status", "autoware_auto_vehicle_msgs/msg/GearReport", + "Topic for Gear Data", this, SLOT(topic_updated_gear())); + gear_topic_property_->initialize(rviz_ros_node); + + turn_signals_topic_property_ = std::make_unique( + "Turn Signals Topic", "/vehicle/status/turn_indicators_status", + "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, + SLOT(topic_updated_turn_signals())); + turn_signals_topic_property_->initialize(rviz_ros_node); + + speed_topic_property_ = std::make_unique( + "Speed Topic", "/vehicle/status/velocity_status", + "autoware_auto_vehicle_msgs/msg/VelocityReport", "Topic for Speed Data", this, + SLOT(topic_updated_speed())); + speed_topic_property_->initialize(rviz_ros_node); + + steering_topic_property_ = std::make_unique( + "Steering Topic", "/vehicle/status/steering_status", + "autoware_auto_vehicle_msgs/msg/SteeringReport", "Topic for Steering Data", this, + SLOT(topic_updated_steering())); + steering_topic_property_->initialize(rviz_ros_node); + + hazard_lights_topic_property_ = std::make_unique( + "Hazard Lights Topic", "/vehicle/status/hazard_lights_status", + "autoware_auto_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, + SLOT(topic_updated_hazard_lights())); + hazard_lights_topic_property_->initialize(rviz_ros_node); + + speed_limit_topic_property_ = std::make_unique( + "Speed Limit Topic", "/planning/scenario_planning/current_max_velocity", + "tier4_planning_msgs/msg/VelocityLimit", "Topic for Speed Limit Data", this, + SLOT(topic_updated_speed_limit())); + speed_limit_topic_property_->initialize(rviz_ros_node); + + traffic_topic_property_ = std::make_unique( + "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals", + "autoware_perception/msgs/msg/TrafficSignalArray", "Topic for Traffic Light Data", this, + SLOT(topic_updated_traffic())); + traffic_topic_property_->initialize(rviz_ros_node); +} + +void SignalDisplay::setupRosSubscriptions() +{ + // Don't create a node, just use the one from the parent class + auto rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + gear_sub_ = rviz_node_->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { + updateGearData(msg); + }); + + steering_sub_ = rviz_node_->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); + + speed_sub_ = rviz_node_->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); + + turn_signals_sub_ = + rviz_node_->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + updateTurnSignalsData(msg); + }); + + hazard_lights_sub_ = + rviz_node_->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + updateHazardLightsData(msg); + }); + + traffic_sub_ = rviz_node_->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); + + speed_limit_sub_ = rviz_node_->create_subscription( + speed_limit_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { + updateSpeedLimitData(msg); + }); +} + +SignalDisplay::~SignalDisplay() +{ + std::lock_guard lock(property_mutex_); + overlay_.reset(); + + gear_sub_.reset(); + steering_sub_.reset(); + speed_sub_.reset(); + turn_signals_sub_.reset(); + hazard_lights_sub_.reset(); + traffic_sub_.reset(); + + steering_wheel_display_.reset(); + gear_display_.reset(); + speed_display_.reset(); + turn_signals_display_.reset(); + traffic_display_.reset(); + speed_limit_display_.reset(); + + gear_topic_property_.reset(); + turn_signals_topic_property_.reset(); + speed_topic_property_.reset(); + steering_topic_property_.reset(); + hazard_lights_topic_property_.reset(); + traffic_topic_property_.reset(); +} + +void SignalDisplay::update(float /* wall_dt */, float /* ros_dt */) +{ + if (!overlay_) { + return; + } + awf_2d_overlay_vehicle::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(Qt::transparent); + drawWidget(hud); +} + +void SignalDisplay::onEnable() +{ + std::lock_guard lock(property_mutex_); + if (overlay_) { + overlay_->show(); + } + setupRosSubscriptions(); +} + +void SignalDisplay::onDisable() +{ + std::lock_guard lock(property_mutex_); + + gear_sub_.reset(); + steering_sub_.reset(); + speed_sub_.reset(); + turn_signals_sub_.reset(); + hazard_lights_sub_.reset(); + + if (overlay_) { + overlay_->hide(); + } +} + +void SignalDisplay::updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (traffic_display_) { + traffic_display_->updateTrafficLightData(msg); + } +} + +void SignalDisplay::updateSpeedLimitData( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (speed_limit_display_) { + speed_limit_display_->updateSpeedLimitData(msg); + queueRender(); + } +} + +void SignalDisplay::updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (turn_signals_display_) { + turn_signals_display_->updateHazardLightsData(msg); + queueRender(); + } +} + +void SignalDisplay::updateGearData( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (gear_display_) { + gear_display_->updateGearData(msg); + queueRender(); + } +} + +void SignalDisplay::updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (steering_wheel_display_) { + steering_wheel_display_->updateSteeringData(msg); + queueRender(); + } +} + +void SignalDisplay::updateSpeedData( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (speed_display_) { + speed_display_->updateSpeedData(msg); + queueRender(); + } +} + +void SignalDisplay::updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (turn_signals_display_) { + turn_signals_display_->updateTurnSignalsData(msg); + queueRender(); + } +} + +void SignalDisplay::drawWidget(QImage & hud) +{ + std::lock_guard lock(property_mutex_); + + if (!overlay_->isVisible()) { + return; + } + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + QRectF backgroundRect(0, 0, 322, hud.height()); + drawBackground(painter, backgroundRect); + + // Draw components + if (steering_wheel_display_) { + steering_wheel_display_->drawSteeringWheel(painter, backgroundRect); + } + if (gear_display_) { + gear_display_->drawGearIndicator(painter, backgroundRect); + } + if (speed_display_) { + speed_display_->drawSpeedDisplay(painter, backgroundRect); + } + if (turn_signals_display_) { + turn_signals_display_->drawArrows(painter, backgroundRect, property_signal_color_->getColor()); + } + + // a 27px space between the two halves of the HUD + + QRectF smallerBackgroundRect(349, 0, 168, hud.height() / 2); + + drawBackground(painter, smallerBackgroundRect); + + if (traffic_display_) { + traffic_display_->drawTrafficLightIndicator(painter, smallerBackgroundRect); + } + + if (speed_limit_display_) { + speed_limit_display_->drawSpeedLimitIndicator(painter, smallerBackgroundRect); + } + + painter.end(); +} + +void SignalDisplay::drawBackground(QPainter & painter, const QRectF & backgroundRect) +{ + painter.setBrush(QColor(0, 0, 0, 255 * 0.2)); // Black background with opacity + painter.setPen(Qt::NoPen); + painter.drawRoundedRect( + backgroundRect, backgroundRect.height() / 2, backgroundRect.height() / 2); // Circular ends +} + +void SignalDisplay::reset() +{ + rviz_common::Display::reset(); + overlay_->hide(); +} + +void SignalDisplay::updateOverlaySize() +{ + std::lock_guard lock(mutex_); + overlay_->updateTextureSize(property_width_->getInt(), property_height_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + queueRender(); +} + +void SignalDisplay::updateOverlayPosition() +{ + std::lock_guard lock(mutex_); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + queueRender(); +} + +void SignalDisplay::updateOverlayColor() +{ + std::lock_guard lock(mutex_); + queueRender(); +} + +void SignalDisplay::topic_updated_gear() +{ + // resubscribe to the topic + gear_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + gear_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { + updateGearData(msg); + }); +} + +void SignalDisplay::topic_updated_steering() +{ + // resubscribe to the topic + steering_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + steering_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); +} + +void SignalDisplay::topic_updated_speed() +{ + // resubscribe to the topic + speed_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + speed_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); +} + +void SignalDisplay::topic_updated_speed_limit() +{ + // resubscribe to the topic + speed_limit_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + speed_limit_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + speed_limit_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { + updateSpeedLimitData(msg); + }); +} + +void SignalDisplay::topic_updated_turn_signals() +{ + // resubscribe to the topic + turn_signals_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + + turn_signals_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + updateTurnSignalsData(msg); + }); +} + +void SignalDisplay::topic_updated_hazard_lights() +{ + // resubscribe to the topic + hazard_lights_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + + hazard_lights_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + updateHazardLightsData(msg); + }); +} + +void SignalDisplay::topic_updated_traffic() +{ + // resubscribe to the topic + traffic_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + traffic_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); +} + +} // namespace awf_2d_overlay_vehicle + +#include +PLUGINLIB_EXPORT_CLASS(awf_2d_overlay_vehicle::SignalDisplay, rviz_common::Display) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp new file mode 100644 index 0000000000000..8212758c09a9f --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp @@ -0,0 +1,109 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "speed_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SpeedDisplay::SpeedDisplay() : current_speed_(0.0) +{ + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void SpeedDisplay::updateSpeedData( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in + float speed = msg->longitudinal_velocity; + // we received it as a m/s value, but we want to display it in km/h + current_speed_ = (speed * 3.6); + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +// void SpeedDisplay::processMessage(const +// autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) +// { +// try +// { +// current_speed_ = std::round(msg->state.longitudinal_velocity_mps * 3.6); +// } +// catch (const std::exception &e) +// { +// std::cerr << "Error in processMessage: " << e.what() << std::endl; +// } +// } + +void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect) +{ + QFont referenceFont("Quicksand", 80, QFont::Bold); + painter.setFont(referenceFont); + QRect referenceRect = painter.fontMetrics().boundingRect("88"); + QPointF referencePos( + backgroundRect.width() / 2 - referenceRect.width() / 2 - 5, backgroundRect.height() / 2); + + QString speedNumber = QString::number(current_speed_, 'f', 0); + int fontSize = 60; + QFont speedFont("Quicksand", fontSize); + painter.setFont(speedFont); + + // Calculate the bounding box of the speed number + QRect speedNumberRect = painter.fontMetrics().boundingRect(speedNumber); + + // Center the speed number in the backgroundRect + QPointF speedPos( + backgroundRect.center().x() - speedNumberRect.width() / 2, backgroundRect.center().y()); + painter.setPen(gray); + painter.drawText(speedPos, speedNumber); + + QFont unitFont("Quicksand", 14); + painter.setFont(unitFont); + QString speedUnit = "km/h"; + QRect unitRect = painter.fontMetrics().boundingRect(speedUnit); + QPointF unitPos( + (backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height()); + painter.drawText(unitPos, speedUnit); +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp new file mode 100644 index 0000000000000..fcc1df998798b --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp @@ -0,0 +1,105 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "speed_limit_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0) +{ + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void SpeedLimitDisplay::updateSpeedLimitData( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + current_limit = msg->max_velocity; +} + +void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + // #C2C2C2 + painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + painter.setBrush(QBrush(gray, Qt::SolidPattern)); + + // Define the area for the outer circle + QRectF outerCircleRect = backgroundRect; + outerCircleRect.setWidth(backgroundRect.width() / 2 - 20); + outerCircleRect.setHeight(backgroundRect.height() - 20); + outerCircleRect.moveTopLeft(QPointF(backgroundRect.left() + 10, backgroundRect.top() + 10)); + + // Define the area for the inner circle + QRectF innerCircleRect = outerCircleRect; + innerCircleRect.setWidth(outerCircleRect.width() / 1.375); + innerCircleRect.setHeight(outerCircleRect.height() / 1.375); + innerCircleRect.moveCenter(outerCircleRect.center()); + + // Draw the outer circle + painter.drawEllipse(outerCircleRect); + + // Change the composition mode and draw the inner circle + painter.setCompositionMode(QPainter::CompositionMode_Clear); + painter.drawEllipse(innerCircleRect); + + // Reset the composition mode + painter.setCompositionMode(QPainter::CompositionMode_SourceOver); + + int current_limit_int = std::round(current_limit * 3.6); + + // Define the text to be drawn + QString text = QString::number(current_limit_int); + + // Set the font and color for the text + QFont font = QFont("Quicksand", 14, QFont::Bold); + + painter.setFont(font); + // #C2C2C2 + painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + // Draw the text in the center of the circle + painter.drawText(innerCircleRect, Qt::AlignCenter, text); +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp new file mode 100644 index 0000000000000..b4da8ff5f8ffb --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp @@ -0,0 +1,123 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "steering_wheel_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SteeringWheelDisplay::SteeringWheelDisplay() +{ + // Load the Quicksand font + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } + + // Load the wheel image + std::string image_path = package_path + "/assets/images/wheel.png"; + wheelImage.load(image_path.c_str()); + scaledWheelImage = wheelImage.scaled(54, 54, Qt::KeepAspectRatio, Qt::SmoothTransformation); +} + +void SteeringWheelDisplay::updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->steering_angle is the field you're interested in + float steeringAngle = msg->steering_tire_angle; + // we received it as a radian value, but we want to display it in degrees + steering_angle_ = + (steeringAngle * -180 / M_PI) * + 17; // 17 is the ratio between the steering wheel and the steering tire angle i assume + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + QImage wheel = coloredImage(scaledWheelImage, gray); + + // Rotate the wheel + float steeringAngle = steering_angle_; // No need to round here + // Calculate the position + int wheelCenterX = backgroundRect.right() - wheel.width() - 17.5; + int wheelCenterY = backgroundRect.height() - wheel.height() + 15; + + // Rotate the wheel image + QTransform rotationTransform; + rotationTransform.translate(wheel.width() / 2.0, wheel.height() / 2.0); + rotationTransform.rotate(steeringAngle); + rotationTransform.translate(-wheel.width() / 2.0, -wheel.height() / 2.0); + + QImage rotatedWheel = wheel.transformed(rotationTransform, Qt::SmoothTransformation); + + QPointF drawPoint( + wheelCenterX - rotatedWheel.width() / 2, wheelCenterY - rotatedWheel.height() / 2); + + // Draw the rotated image + painter.drawImage(drawPoint.x(), drawPoint.y(), rotatedWheel); + + QString steeringAngleStringAfterModulo = QString::number(fmod(steeringAngle, 360), 'f', 0); + + // Draw the steering angle text + QFont steeringFont("Quicksand", 9, QFont::Bold); + painter.setFont(steeringFont); + painter.setPen(QColor(0, 0, 0, 255)); + QRect steeringRect( + wheelCenterX - wheelImage.width() / 2, wheelCenterY - wheelImage.height() / 2, + wheelImage.width(), wheelImage.height()); + painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleStringAfterModulo + "°"); +} + +QImage SteeringWheelDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp new file mode 100644 index 0000000000000..233da1f36b4a7 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp @@ -0,0 +1,113 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "traffic_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +TrafficDisplay::TrafficDisplay() +{ + // Load the traffic light image + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string image_path = package_path + "/assets/images/traffic.png"; + traffic_light_image_.load(image_path.c_str()); +} + +void TrafficDisplay::updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg) +{ + current_traffic_ = *msg; +} + +void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + // Define the area for the circle (background) + QRectF circleRect = backgroundRect; + circleRect.setWidth(backgroundRect.width() / 2 - 20); + circleRect.setHeight(backgroundRect.height() - 20); + circleRect.moveTopRight(QPointF(backgroundRect.right() - 10, backgroundRect.top() + 10)); + + painter.setBrush(QBrush(gray)); + painter.drawEllipse(circleRect.center(), 30, 30); + + // Define the area for the traffic light image (should be smaller or positioned within the circle) + QRectF imageRect = + circleRect.adjusted(15, 15, -15, -15); // Adjusting the rectangle to make the image smaller + + QImage scaled_traffic_image = traffic_light_image_.scaled( + imageRect.size().toSize(), Qt::KeepAspectRatio, Qt::SmoothTransformation); + + if (current_traffic_.signals.size() > 0) { + switch (current_traffic_.signals[0].elements[0].color) { + case 1: + painter.setBrush(QBrush(red)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + case 2: + painter.setBrush(QBrush(yellow)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + case 3: + painter.setBrush(QBrush(green)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + case 4: + painter.setBrush(QBrush(gray)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + default: + painter.setBrush(QBrush(gray)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + } + } + // make the image thicker + painter.setPen(QPen(Qt::black, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + painter.drawImage(imageRect, scaled_traffic_image); +} + +QImage TrafficDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp new file mode 100644 index 0000000000000..a9b780cb4eab2 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "turn_signals_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) +{ + last_toggle_time_ = std::chrono::steady_clock::now(); + + // Load the arrow image + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string image_path = package_path + "/assets/images/arrow.png"; + arrowImage.load(image_path.c_str()); +} + +void TurnSignalsDisplay::updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->report is the field you're interested in + current_turn_signal_ = msg->report; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void TurnSignalsDisplay::updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->report is the field you're interested in + current_hazard_lights_ = msg->report; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void TurnSignalsDisplay::drawArrows( + QPainter & painter, const QRectF & backgroundRect, const QColor & color) +{ + QImage scaledLeftArrow = arrowImage.scaled(64, 43, Qt::KeepAspectRatio, Qt::SmoothTransformation); + scaledLeftArrow = coloredImage(scaledLeftArrow, gray); + QImage scaledRightArrow = scaledLeftArrow.mirrored(true, false); + int arrowYPos = (backgroundRect.height() / 3 - scaledLeftArrow.height() / 2); + int leftArrowXPos = backgroundRect.width() / 4 - scaledLeftArrow.width(); // Adjust as needed + int rightArrowXPos = backgroundRect.width() * 3 / 4; // Adjust as needed + + bool leftActive = + (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || + current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + bool rightActive = + (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || + current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + + // Color the arrows based on the state of the turn signals and hazard lights by having them blink + // on and off + if (this->blink_on_) { + if (leftActive) { + scaledLeftArrow = coloredImage(scaledLeftArrow, color); + } + if (rightActive) { + scaledRightArrow = coloredImage(scaledRightArrow, color); + } + } + + // Draw the arrows + painter.drawImage(QPointF(leftArrowXPos, arrowYPos), scaledLeftArrow); + painter.drawImage(QPointF(rightArrowXPos, arrowYPos), scaledRightArrow); + + auto now = std::chrono::steady_clock::now(); + if ( + std::chrono::duration_cast(now - last_toggle_time_) >= + blink_interval_) { + blink_on_ = !blink_on_; // Toggle the blink state + last_toggle_time_ = now; + } +} + +QImage TurnSignalsDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst new file mode 100644 index 0000000000000..e7672ee001955 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst @@ -0,0 +1,24 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rviz_2d_overlay_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.3.0 (2023-05-18) +------------------ +* Removed old position message fields +* Contributors: Dominik, Jonas Otto + +1.2.1 (2022-09-30) +------------------ + +1.2.0 (2022-09-27) +------------------ +* Rename package from overlay_rviz_msgs to rviz_2d_overlay_msgs +* Contributors: Jonas Otto + +1.1.0 (2022-09-11) +------------------ + +1.0.0 (2022-08-30) +------------------ +* Create OverlayText message (currently same as jsk_rviz_plugins) +* Contributors: Jonas Otto, Dominik Authaler diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt new file mode 100644 index 0000000000000..9e9a8f277fd53 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(rviz_2d_overlay_msgs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif () + + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/OverlayText.msg" + DEPENDENCIES + std_msgs +) + +ament_package() diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg new file mode 100644 index 0000000000000..db3cf628b895b --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg @@ -0,0 +1,31 @@ +uint8 ADD = 0 +uint8 DELETE = 1 + +# constants for the horizontal and vertical alignment +uint8 LEFT = 0 +uint8 RIGHT = 1 +uint8 CENTER = 2 +uint8 TOP = 3 +uint8 BOTTOM = 4 + +uint8 action + +int32 width +int32 height +# Position: Positive values move the overlay towards the center of the window, +# for center alignment positive values move the overlay towards the bottom right +int32 horizontal_distance # Horizontal distance from left/right border or center, depending on alignment +int32 vertical_distance # Vertical distance between from top/bottom border or center, depending on alignment + +# Alignment of the overlay withing RVIZ +uint8 horizontal_alignment # one of LEFT, CENTER, RIGHT +uint8 vertical_alignment # one of TOP, CENTER, BOTTOM + +std_msgs/ColorRGBA bg_color + +int32 line_width +float32 text_size +string font +std_msgs/ColorRGBA fg_color + +string text diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml new file mode 100644 index 0000000000000..53396c64aa156 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml @@ -0,0 +1,24 @@ + + + + rviz_2d_overlay_msgs + 1.3.0 + Messages describing 2D overlays for RVIZ, extracted/derived from the jsk_visualization package. + Team Spatzenhirn + BSD-3-Clause + + ament_cmake + rosidl_default_generators + + autoware_auto_vehicle_msgs + autoware_perception_msgs + std_msgs + unique_identifier_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + From e49d63c75a1e4dd36fada537f82f832479dc1e08 Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Tue, 30 Jan 2024 01:06:53 +0900 Subject: [PATCH 153/154] chore(traffic_light_fine_detector): fix_default_tlr_yolox_model_name (#6163) chore: fix_default_tlr_yolox_model_name Signed-off-by: Shunsuke Miura --- .../launch/traffic_light_fine_detector.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index e7a68ea9b9e94..d08378f3dd129 100644 --- a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml +++ b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml @@ -1,7 +1,7 @@ - + From 4c3ea87e1bbd7db5b2c8004b310bcfb15afff37c Mon Sep 17 00:00:00 2001 From: eiki <53928021+N-Eiki@users.noreply.github.com> Date: Tue, 30 Jan 2024 03:00:03 +0900 Subject: [PATCH 154/154] feat(radar_tracks_noise_filter): add unit test (#6113) * feat/test_radar_tracks_noise_filter Signed-off-by: N-Eiki * change file name Signed-off-by: N-Eiki --------- Signed-off-by: N-Eiki Co-authored-by: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> --- .../radar_tracks_noise_filter/CMakeLists.txt | 7 ++ .../radar_tracks_noise_filter_node.hpp | 1 + ...est_radar_tracks_noise_filter_is_noise.cpp | 95 +++++++++++++++++++ .../test/test_radar_tracks_noise_filter.cpp | 26 +++++ 4 files changed, 129 insertions(+) create mode 100644 sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp create mode 100644 sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp diff --git a/sensing/radar_tracks_noise_filter/CMakeLists.txt b/sensing/radar_tracks_noise_filter/CMakeLists.txt index 517a3f1df72f1..133a057cf1a12 100644 --- a/sensing/radar_tracks_noise_filter/CMakeLists.txt +++ b/sensing/radar_tracks_noise_filter/CMakeLists.txt @@ -28,9 +28,16 @@ rclcpp_components_register_node(radar_tracks_noise_filter_node_component # Tests if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(radar_tracks_noise_filter ${test_files}) + + target_link_libraries(radar_tracks_noise_filter + radar_tracks_noise_filter_node_component + ) endif() # Package diff --git a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp b/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp index 9b329da3e5579..2ff4025cc64bc 100644 --- a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp +++ b/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp @@ -57,6 +57,7 @@ class RadarTrackCrossingNoiseFilterNode : public rclcpp::Node // Parameter NodeParam node_param_{}; +public: // Core bool isNoise(const RadarTrack & radar_track); }; diff --git a/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp new file mode 100644 index 0000000000000..aa08260dee2e7 --- /dev/null +++ b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp @@ -0,0 +1,95 @@ +// Copyright 2023 Tier IV, Inc. +// +// 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 "radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp" + +#include + +#include + +std::shared_ptr get_node( + float velocity_y_threshold) +{ + rclcpp::NodeOptions node_options; + node_options.parameter_overrides({ + {"node_params.is_amplitude_filter", true}, + {"velocity_y_threshold", velocity_y_threshold}, + }); + + auto node = + std::make_shared(node_options); + return node; +} + +radar_msgs::msg::RadarTrack getRadarTrack(float velocity_x, float velocity_y) +{ + radar_msgs::msg::RadarTrack radar_track; + geometry_msgs::msg::Vector3 vector_msg; + vector_msg.x = velocity_x; + vector_msg.y = velocity_y; + vector_msg.z = 0.0; + radar_track.velocity = vector_msg; + return radar_track; +} + +TEST(RadarTracksNoiseFilter, isNoise) +{ + using radar_msgs::msg::RadarTrack; + using radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode; + rclcpp::init(0, nullptr); + { + float velocity_node_threshold = 0.0; + float y_velocity = 0.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = -1.0; + float y_velocity = 3.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = -1.0; + float y_velocity = 3.0; + float x_velocity = 100.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = 3.0; + float y_velocity = 1.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_FALSE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = 3.0; + float y_velocity = 1.0; + float x_velocity = -10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_FALSE(node->isNoise(radar_track)); + } +} diff --git a/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp b/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp new file mode 100644 index 0000000000000..55032a7b62e79 --- /dev/null +++ b/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}