From 0f9feb3bcc0aeb5ec3b6f103a3bb6b65ed2ff197 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Sun, 28 Apr 2024 20:19:49 +0200 Subject: [PATCH 01/67] Added LQR and PID DP --- guidance/dp_guidance/CMakeLists.txt | 27 ++++++ guidance/dp_guidance/README.md | 15 ++++ .../config/dp_guidance_config.yaml | 4 + guidance/dp_guidance/dp_guidance/__init__.py | 0 .../dp_guidance/dp_guidance/conversions.py | 59 +++++++++++++ .../dp_guidance/dp_guidance_node.py | 88 +++++++++++++++++++ .../dp_guidance/reference_filter.py | 72 +++++++++++++++ .../dp_guidance/launch/dp_guidance.launch.py | 16 ++++ guidance/dp_guidance/package.xml | 21 +++++ motion/lqr_controller/CMakeLists.txt | 27 ++++++ motion/lqr_controller/config/lqr_config.yaml | 5 ++ .../launch/lqr_controller.launch.py | 19 ++++ .../lqr_controller/lqr_controller/__init__.py | 0 .../lqr_controller/conversions.py | 59 +++++++++++++ .../lqr_controller/lqr_controller.py | 39 ++++++++ .../lqr_controller/lqr_controller_node.py | 80 +++++++++++++++++ motion/lqr_controller/package.xml | 27 ++++++ motion/pid_controller/CMakeLists.txt | 27 ++++++ motion/pid_controller/config/pid_config.yaml | 6 ++ .../launch/pid_controller.launch.py | 19 ++++ motion/pid_controller/package.xml | 27 ++++++ .../pid_controller/pid_controller/__init__.py | 0 .../pid_controller/conversions.py | 59 +++++++++++++ .../pid_controller/pid_controller.py | 43 +++++++++ .../pid_controller/pid_controller_node.py | 85 ++++++++++++++++++ 25 files changed, 824 insertions(+) create mode 100644 guidance/dp_guidance/CMakeLists.txt create mode 100755 guidance/dp_guidance/README.md create mode 100644 guidance/dp_guidance/config/dp_guidance_config.yaml create mode 100644 guidance/dp_guidance/dp_guidance/__init__.py create mode 100644 guidance/dp_guidance/dp_guidance/conversions.py create mode 100755 guidance/dp_guidance/dp_guidance/dp_guidance_node.py create mode 100644 guidance/dp_guidance/dp_guidance/reference_filter.py create mode 100644 guidance/dp_guidance/launch/dp_guidance.launch.py create mode 100644 guidance/dp_guidance/package.xml create mode 100644 motion/lqr_controller/CMakeLists.txt create mode 100644 motion/lqr_controller/config/lqr_config.yaml create mode 100644 motion/lqr_controller/launch/lqr_controller.launch.py create mode 100644 motion/lqr_controller/lqr_controller/__init__.py create mode 100644 motion/lqr_controller/lqr_controller/conversions.py create mode 100644 motion/lqr_controller/lqr_controller/lqr_controller.py create mode 100644 motion/lqr_controller/lqr_controller/lqr_controller_node.py create mode 100644 motion/lqr_controller/package.xml create mode 100644 motion/pid_controller/CMakeLists.txt create mode 100644 motion/pid_controller/config/pid_config.yaml create mode 100644 motion/pid_controller/launch/pid_controller.launch.py create mode 100644 motion/pid_controller/package.xml create mode 100644 motion/pid_controller/pid_controller/__init__.py create mode 100644 motion/pid_controller/pid_controller/conversions.py create mode 100644 motion/pid_controller/pid_controller/pid_controller.py create mode 100644 motion/pid_controller/pid_controller/pid_controller_node.py diff --git a/guidance/dp_guidance/CMakeLists.txt b/guidance/dp_guidance/CMakeLists.txt new file mode 100644 index 00000000..7cc58d78 --- /dev/null +++ b/guidance/dp_guidance/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(dp_guidance) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + dp_guidance/dp_guidance_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/guidance/dp_guidance/README.md b/guidance/dp_guidance/README.md new file mode 100755 index 00000000..3dbdad76 --- /dev/null +++ b/guidance/dp_guidance/README.md @@ -0,0 +1,15 @@ +# Dynamic Positioning (DP) Guidance + +This package provides the implementation of DP guidance for the Vortex ASV. + +## Usage + +To use the DP guidance launch it using: `ros2 launch dp_guidance dp_guidance.launch` + +To run with custom waypoints (replace example waypoints with actual waypoints, and add as many prefered): + +`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 0.0, y: 0.0, z: 0.0}, {x: 5.0, y: 5.0, z: 0.0}]"` + +## Configuration + +You can configure the behavior of the hybrid path guidance by modifying the parameters in the `config` directory. \ No newline at end of file diff --git a/guidance/dp_guidance/config/dp_guidance_config.yaml b/guidance/dp_guidance/config/dp_guidance_config.yaml new file mode 100644 index 00000000..02db4e76 --- /dev/null +++ b/guidance/dp_guidance/config/dp_guidance_config.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + dp_guidance: + dt: 0.1 # Time step \ No newline at end of file diff --git a/guidance/dp_guidance/dp_guidance/__init__.py b/guidance/dp_guidance/dp_guidance/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/guidance/dp_guidance/dp_guidance/conversions.py b/guidance/dp_guidance/dp_guidance/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/guidance/dp_guidance/dp_guidance/conversions.py @@ -0,0 +1,59 @@ +import numpy as np + +from transforms3d.euler import euler2quat, quat2euler +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +def odometrymsg_to_state(msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + +def state_to_odometrymsg(state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg + +def state_to_posestamped(state, frame_id, stamp): + orientation_list_next = euler2quat(0, 0, state[2]) + + posestamped_msg = PoseStamped() + + posestamped_msg.header.frame_id = frame_id + posestamped_msg.header.stamp = stamp + + posestamped_msg.pose.position.x = state[0] + posestamped_msg.pose.position.y = state[1] + posestamped_msg.pose.position.z = 0.0 + posestamped_msg.pose.orientation.w = orientation_list_next[0] + posestamped_msg.pose.orientation.x = orientation_list_next[1] + posestamped_msg.pose.orientation.y = orientation_list_next[2] + posestamped_msg.pose.orientation.z = orientation_list_next[3] + + return posestamped_msg + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py new file mode 100755 index 00000000..a416f952 --- /dev/null +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 + +import numpy as np +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Pose2D +from vortex_msgs.srv import Waypoint +from nav_msgs.msg import Odometry +from transforms3d.euler import quat2euler + +from dp_guidance.conversions import odometrymsg_to_state, state_to_odometrymsg +from dp_guidance.reference_filter import ReferenceFilter + +class Guidance(Node): + def __init__(self): + super().__init__("dp_guidance") + self.declare_parameters( + namespace='', + parameters=[ + ('dp_guidance.dt', 0.1) + ]) + + self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) + self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1) + self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1) + + # Get parameters + self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value + + # Flags for logging + self.waypoints_received = False + self.waiting_message_printed = False + + self.init_pos = False + + self.eta = np.array([0, 0, 0]) + self.eta_ref = np.array([0, 0, 0]) + + self.xd = np.zeros(9) + + self.reference_filter = ReferenceFilter() + + # Timer for guidance + timer_period = 0.1 + self.timer = self.create_timer(timer_period, self.guidance_callback) + + def waypoint_callback(self, request, response): + self.waypoints = request.waypoint + self.get_logger().info(f'Received waypoints: {self.waypoints}') + self.waypoints_received = True + self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + response.success = True + return response + + def eta_callback(self, msg: Odometry): + self.eta = odometrymsg_to_state(msg)[:3] + + def guidance_callback(self): + if self.waypoints_received: + if not self.init_pos: + self.xd[0:3] = self.eta + self.init_pos = True + last_waypoint = self.waypoints[-1] + self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0]) + x_next = self.reference_filter.step(self.eta_ref, self.xd) + self.xd = x_next + self.get_logger().info(f'x_next[0]: {x_next[0]}') + self.get_logger().info(f'x_next[0]: {x_next[1]}') + self.get_logger().info(f'x_next[0]: {x_next[2]}') + + odom_msg = Odometry() + odom_msg = state_to_odometrymsg(x_next[:3]) + self.guidance_publisher.publish(odom_msg) + + else: + if not self.waiting_message_printed: + self.get_logger().info('Waiting for waypoints to be received') + self.waiting_message_printed = True + +def main(args=None): + rclpy.init(args=args) + guidance = Guidance() + rclpy.spin(guidance) + guidance.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/guidance/dp_guidance/dp_guidance/reference_filter.py b/guidance/dp_guidance/dp_guidance/reference_filter.py new file mode 100644 index 00000000..1ad4f344 --- /dev/null +++ b/guidance/dp_guidance/dp_guidance/reference_filter.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +import numpy as np +import control +import matplotlib.pyplot as plt + +class ReferenceFilter: + def __init__(self): + zeta = 0.8 + omega_b = 0.05 + omega_n = omega_b/np.sqrt(1-2*zeta**2 + np.sqrt(4*zeta**4 - 4*zeta**2 + 2)) + + I = np.eye(3) + Omega = 2*zeta*omega_n*I + # Gamma = omega_n**2*I + Delta = zeta*I + Ad = np.zeros((9,9)) + Ad[0:3,3:6] = I + Ad[3:6,6:9] = I + Ad[6:9,0:3] = -Omega**3 + Ad[6:9,3:6] = -(2*Delta+I)@Omega**2 + Ad[6:9,6:9] = -(2*Delta+I)@Omega + + Bd = np.zeros((9,3)) + Bd[6:9,:] = Omega**3 + + sys = control.ss(Ad, Bd, np.zeros((9,9)), np.zeros((9,3))) + sysd = control.c2d(sys, 0.1) + + self.Ad = sysd.A + self.Bd = sysd.B + + def step(self, r, xd): + x_next = self.Ad@xd + self.Bd@r + return x_next + + def get_eta(self, xd): + return xd[:,0:3] + + def get_nu(self, xd): + nu = np.zeros((len(xd),3)) + for i in range(len(xd)): + psi = xd[i,2] + nu[i,:] = (self.rotationMatrix(psi).transpose())@xd[i,3:6] + return nu + + @staticmethod + def rotationMatrix(psi): + R = np.array([[np.cos(psi), -np.sin(psi), 0], + [np.sin(psi), np.cos(psi), 0], + [0, 0, 1]]) + return R + + +if __name__ == "__main__": + refFilter = ReferenceFilter() + ref_pos = np.array([5, 10, 0]) + t = np.arange(0, 100, 0.1) + N = len(t) + x_d = np.zeros((N, 9)) + + for k in range(1,N): + x_d[k,:] = refFilter.step(ref_pos, x_d[k-1, :]) + + eta_d = refFilter.get_eta(x_d) + nu_d = refFilter.get_nu(x_d) + plt.figure() + plt.plot(t, eta_d[:,0]) + + plt.figure() + plt.plot(t, nu_d[:,0]) + plt.show() \ No newline at end of file diff --git a/guidance/dp_guidance/launch/dp_guidance.launch.py b/guidance/dp_guidance/launch/dp_guidance.launch.py new file mode 100644 index 00000000..710ed9e0 --- /dev/null +++ b/guidance/dp_guidance/launch/dp_guidance.launch.py @@ -0,0 +1,16 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + dp_guidance_node = Node( + package='dp_guidance', + executable='dp_guidance_node.py', + name='dp_guidance_node', + parameters=[os.path.join(get_package_share_directory('dp_guidance'),'config','dp_guidance_config.yaml')], + output='screen', + ) + return LaunchDescription([ + dp_guidance_node + ]) diff --git a/guidance/dp_guidance/package.xml b/guidance/dp_guidance/package.xml new file mode 100644 index 00000000..33547bcb --- /dev/null +++ b/guidance/dp_guidance/package.xml @@ -0,0 +1,21 @@ + + + + dp_guidance + 0.0.0 + This package provides the implementation of hybrid path guidance for the Vortex ASV. + vortex + MIT + + rclpy + nav_msgs + python-transforms3d-pip + geometry_msgs + vortex_msgs + + python3-pytest + + + ament_cmake + + diff --git a/motion/lqr_controller/CMakeLists.txt b/motion/lqr_controller/CMakeLists.txt new file mode 100644 index 00000000..cafa3ad4 --- /dev/null +++ b/motion/lqr_controller/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(lqr_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS +lqr_controller/lqr_controller_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml new file mode 100644 index 00000000..ac497221 --- /dev/null +++ b/motion/lqr_controller/config/lqr_config.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + lqr_controller: + Q: [20.0, 20.0, 1.0, 1.0, 1.0, 1.0] # State costs for [x, y, heading, surge, sway, yaw] + R: [1.0, 1.0, 1.0] # Control cost weight \ No newline at end of file diff --git a/motion/lqr_controller/launch/lqr_controller.launch.py b/motion/lqr_controller/launch/lqr_controller.launch.py new file mode 100644 index 00000000..03ceca10 --- /dev/null +++ b/motion/lqr_controller/launch/lqr_controller.launch.py @@ -0,0 +1,19 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + lqr_controller_node = Node( + package='lqr_controller', + executable='lqr_controller_node.py', + name='lqr_controller_node', + parameters=[ + os.path.join(get_package_share_directory('lqr_controller'),'config','lqr_config.yaml'), + os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') + ], + output='screen', + ) + return LaunchDescription([ + lqr_controller_node + ]) diff --git a/motion/lqr_controller/lqr_controller/__init__.py b/motion/lqr_controller/lqr_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/lqr_controller/lqr_controller/conversions.py b/motion/lqr_controller/lqr_controller/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/motion/lqr_controller/lqr_controller/conversions.py @@ -0,0 +1,59 @@ +import numpy as np + +from transforms3d.euler import euler2quat, quat2euler +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +def odometrymsg_to_state(msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + +def state_to_odometrymsg(state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg + +def state_to_posestamped(state, frame_id, stamp): + orientation_list_next = euler2quat(0, 0, state[2]) + + posestamped_msg = PoseStamped() + + posestamped_msg.header.frame_id = frame_id + posestamped_msg.header.stamp = stamp + + posestamped_msg.pose.position.x = state[0] + posestamped_msg.pose.position.y = state[1] + posestamped_msg.pose.position.z = 0.0 + posestamped_msg.pose.orientation.w = orientation_list_next[0] + posestamped_msg.pose.orientation.x = orientation_list_next[1] + posestamped_msg.pose.orientation.y = orientation_list_next[2] + posestamped_msg.pose.orientation.z = orientation_list_next[3] + + return posestamped_msg + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py new file mode 100644 index 00000000..7c7b6f67 --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -0,0 +1,39 @@ +import numpy as np +from scipy.linalg import solve_continuous_are + +class LQRController: + def __init__(self, M: float, D: list[float], Q: list[float], R: list[float], heading_ref: float = 0.0): + self.M = M + self.M_inv = np.linalg.inv(self.M) + self.D = D + + self.A = np.zeros((6,6)) + self.B = np.zeros((6,3)) + self.C = np.zeros((3,6)) + self.C[:3, :3] = np.eye(3) + + self.Q = np.diag(Q) + print(f"Q: {self.Q}") + self.R = np.diag(R) + + self.linearize_model(heading_ref) + + def calculate_control_input(self, x, x_ref): + tau = -self.K_LQR @ x + self.K_r @ x_ref + return tau + + def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: + R = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + self.A[:3,3:] = R + self.A[3:, 3:] = - self.M_inv @ self.D + + self.B[3:,:] = self.M_inv + + self.P = solve_continuous_are(self.A, self.B, self.Q, self.R) + self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) + self.K_r = np.linalg.inv(self.C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py new file mode 100644 index 00000000..18bebe0c --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 + +import rclpy +import numpy as np +from rclpy.node import Node +from geometry_msgs.msg import Wrench +from nav_msgs.msg import Odometry +from lqr_controller.lqr_controller import LQRController +from lqr_controller.conversions import odometrymsg_to_state +from time import sleep + +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) + +class LQRControllerNode(Node): + def __init__(self): + super().__init__("lqr_controller_node") + + self.declare_parameters( + namespace='', + parameters=[ + ('lqr_controller.Q', [10.0, 10.0, 5.0, 0.1, 1.0, 5.0]), + ('lqr_controller.R', [1.0, 1.0, 1.0]), + ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65]), + ('physical.damping_matrix_diag', [77.55, 162.5, 42.65]) + ]) + + self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) + self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) + self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) + + Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value + R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value + D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + D = np.diag(D_diag) + M = np.reshape(M, (3, 3)) + + heading_ref = 0.0 + + self.LQR = LQRController(M, D, Q, R, heading_ref) + + self.x_ref = [0, 0, 0] + self.state = [0, 0, 0, 0, 0, 0] + + self.enabled = False + + controller_period = 0.1 + self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + + self.get_logger().info("lqr_controller_node started") + + def state_cb(self, msg): + self.state = odometrymsg_to_state(msg) + + def guidance_cb(self, msg): + self.x_ref = odometrymsg_to_state(msg)[:3] + + def controller_callback(self): + if hasattr(self, 'state') and hasattr(self, 'x_ref'): + self.LQR.linearize_model(self.state[2]) + control_input = self.LQR.calculate_control_input(self.state, self.x_ref) + wrench_msg = Wrench() + wrench_msg.force.x = control_input[0] + wrench_msg.force.y = control_input[1] + wrench_msg.torque.z = control_input[2] + self.wrench_publisher_.publish(wrench_msg) + +def main(args=None): + rclpy.init(args=args) + node = LQRControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/motion/lqr_controller/package.xml b/motion/lqr_controller/package.xml new file mode 100644 index 00000000..55cd3a82 --- /dev/null +++ b/motion/lqr_controller/package.xml @@ -0,0 +1,27 @@ + + + + lqr_controller + 0.0.0 + This is an implementation of the LQR DP controller for the ASV + vortex + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + nav_msgs + geometry_msgs + vortex_msgs + std_msgs + numpy + matplotlib + scipy + + python3-pytest + + + ament_cmake + + diff --git a/motion/pid_controller/CMakeLists.txt b/motion/pid_controller/CMakeLists.txt new file mode 100644 index 00000000..0c45a92b --- /dev/null +++ b/motion/pid_controller/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(pid_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS +pid_controller/pid_controller_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/motion/pid_controller/config/pid_config.yaml b/motion/pid_controller/config/pid_config.yaml new file mode 100644 index 00000000..f04befd2 --- /dev/null +++ b/motion/pid_controller/config/pid_config.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + pid_controller: + Kp: [23.04, 34.56, 4.032] # Proportional costs for [x, y, heading] + Ki: [2.76, 4.1, 0.48] # Integral costs for [x, y, heading] + Kd: [28.7648, 43.1472, 5.048384] # Derivative costs for [x, y, heading] \ No newline at end of file diff --git a/motion/pid_controller/launch/pid_controller.launch.py b/motion/pid_controller/launch/pid_controller.launch.py new file mode 100644 index 00000000..465924a9 --- /dev/null +++ b/motion/pid_controller/launch/pid_controller.launch.py @@ -0,0 +1,19 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + pid_controller_node = Node( + package='pid_controller', + executable='pid_controller_node.py', + name='pid_controller_node', + parameters=[ + os.path.join(get_package_share_directory('pid_controller'),'config','pid_config.yaml'), + os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') + ], + output='screen', + ) + return LaunchDescription([ + pid_controller_node + ]) diff --git a/motion/pid_controller/package.xml b/motion/pid_controller/package.xml new file mode 100644 index 00000000..1cb6be76 --- /dev/null +++ b/motion/pid_controller/package.xml @@ -0,0 +1,27 @@ + + + + pid_controller + 0.0.0 + This is an implementation of the PID DP controller for the ASV + vortex + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + nav_msgs + geometry_msgs + vortex_msgs + std_msgs + numpy + matplotlib + scipy + + python3-pytest + + + ament_cmake + + diff --git a/motion/pid_controller/pid_controller/__init__.py b/motion/pid_controller/pid_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/pid_controller/pid_controller/conversions.py b/motion/pid_controller/pid_controller/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/motion/pid_controller/pid_controller/conversions.py @@ -0,0 +1,59 @@ +import numpy as np + +from transforms3d.euler import euler2quat, quat2euler +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +def odometrymsg_to_state(msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + +def state_to_odometrymsg(state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg + +def state_to_posestamped(state, frame_id, stamp): + orientation_list_next = euler2quat(0, 0, state[2]) + + posestamped_msg = PoseStamped() + + posestamped_msg.header.frame_id = frame_id + posestamped_msg.header.stamp = stamp + + posestamped_msg.pose.position.x = state[0] + posestamped_msg.pose.position.y = state[1] + posestamped_msg.pose.position.z = 0.0 + posestamped_msg.pose.orientation.w = orientation_list_next[0] + posestamped_msg.pose.orientation.x = orientation_list_next[1] + posestamped_msg.pose.orientation.y = orientation_list_next[2] + posestamped_msg.pose.orientation.z = orientation_list_next[3] + + return posestamped_msg + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/pid_controller/pid_controller/pid_controller.py b/motion/pid_controller/pid_controller/pid_controller.py new file mode 100644 index 00000000..1207fbb1 --- /dev/null +++ b/motion/pid_controller/pid_controller/pid_controller.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 + +import numpy as np + +class PID: + def __init__(self, Kp, Ki, Kd): + self.error_sum = np.zeros(3) + + self.Kp = np.diag(Kp) + self.Ki = np.diag(Ki) + self.Kd = np.diag(Kd) + + self.tau_max = np.array([100, 100, 100]) + + def calculate_control_input(self, eta, eta_d, eta_dot, dt): + error = eta - eta_d + self.error_sum += error * dt + self.error_sum = np.clip(self.error_sum, -20, 20) + + p = self.Kp @ error + i = 0 #self.Ki @ self.error_sum + d = self.Kd @ eta_dot + + self.last_error = error + + tau = -(p + i + d) + + if tau[0] > self.tau_max[0]: + tau[0] = self.tau_max[0] + elif tau[0] < -self.tau_max[0]: + tau[0] = -self.tau_max[0] + + if tau[1] > self.tau_max[1]: + tau[1] = self.tau_max[1] + elif tau[1] < -self.tau_max[1]: + tau[1] = -self.tau_max[1] + + if tau[2] > self.tau_max[2]: + tau[2] = self.tau_max[2] + elif tau[2] < -self.tau_max[2]: + tau[2] = -self.tau_max[2] + + return tau \ No newline at end of file diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py new file mode 100644 index 00000000..4c3e187c --- /dev/null +++ b/motion/pid_controller/pid_controller/pid_controller_node.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +import rclpy +import numpy as np +from rclpy.node import Node +from geometry_msgs.msg import Wrench +from nav_msgs.msg import Odometry +from pid_controller.pid_controller import PID +from pid_controller.conversions import odometrymsg_to_state +from time import sleep + +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) + +class PIDControllerNode(Node): + def __init__(self): + super().__init__("pid_controller_node") + + self.declare_parameters( + namespace='', + parameters=[ + ('pid_controller.Kp', [1.0, 1.0, 1.0]), + ('pid_controller.Ki', [1.0, 1.0, 1.0]), + ('pid_controller.Kd', [1.0, 1.0, 1.0]), + ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65]) + ]) + + self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) + self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) + self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) + + Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value + Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value + Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + M = np.reshape(M, (3, 3)) + M_diag = np.diag(M) + + ## PID TUNING VALUES ## (OVERWRITES YAML FILE VALUES) + omega_n = 1.2 + zeta = 0.75 + Kp = M_diag * omega_n**2 + Kd = M_diag * 2 * zeta * omega_n #- D_diag + Ki = omega_n/10 * Kp + + self.pid = PID(Kp, Ki, Kd) + + self.x_ref = np.array([0, 0, 0]) + self.state = np.array([0, 0, 0, 0, 0, 0]) + + self.enabled = False + + self.controller_period = 0.1 + self.controller_timer_ = self.create_timer(self.controller_period, self.controller_callback) + + self.get_logger().info("pid_controller_node started") + + def state_cb(self, msg): + self.state = odometrymsg_to_state(msg) + + def guidance_cb(self, msg): + self.x_ref = odometrymsg_to_state(msg)[:3] + + def controller_callback(self): + if hasattr(self, 'state') and hasattr(self, 'x_ref'): + control_input = self.pid.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period) + self.get_logger().info(f"Control input: {control_input}") + wrench_msg = Wrench() + wrench_msg.force.x = control_input[0] + wrench_msg.force.y = control_input[1] + wrench_msg.torque.z = control_input[2] + self.wrench_publisher_.publish(wrench_msg) + +def main(args=None): + rclpy.init(args=args) + node = PIDControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() From cff5d3c837d4c166999b2f25b7e640761d80674a Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Mon, 29 Apr 2024 08:11:07 +0200 Subject: [PATCH 02/67] Fixed topic names and initial tuning --- guidance/hybridpath_guidance/CMakeLists.txt | 1 + .../config/hybridpath_guidance_config.yaml | 2 +- .../hybridpath_guidance_node.py | 6 +- .../hybridpath_guidance/waypoint_node.py | 91 +++++++++++++++++++ .../config/hybridpath_controller_config.yaml | 4 +- .../hybridpath_controller_node.py | 2 +- 6 files changed, 99 insertions(+), 7 deletions(-) create mode 100755 guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt index 0fca6d21..e16c1b10 100644 --- a/guidance/hybridpath_guidance/CMakeLists.txt +++ b/guidance/hybridpath_guidance/CMakeLists.txt @@ -21,6 +21,7 @@ install(DIRECTORY install(PROGRAMS hybridpath_guidance/hybridpath_guidance_node.py + hybridpath_guidance/waypoint_node.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml index 72488399..20fa168f 100644 --- a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml +++ b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml @@ -4,4 +4,4 @@ lambda_val: 0.15 # Curvature constant path_generator_order: 1 # Differentiability order dt: 0.1 # Time step - mu: 0.03 # Tuning parameter \ No newline at end of file + mu: 0.15 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 36255f0d..2022db46 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -19,11 +19,11 @@ def __init__(self): ('hybridpath_guidance.lambda_val', 0.15), ('hybridpath_guidance.path_generator_order', 1), ('hybridpath_guidance.dt', 0.1), - ('hybridpath_guidance.mu', 0.03) + ('hybridpath_guidance.mu', 0.2) ]) self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) - self.eta_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1) + self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, 1) self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) # Get parameters @@ -33,7 +33,7 @@ def __init__(self): self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value self.eta = np.zeros(3) - self.u_desired = 0.25 # Desired velocity + self.u_desired = 0.2 # Desired velocity # Flags for logging self.waypoints_received = False diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py new file mode 100755 index 00000000..bae7f73b --- /dev/null +++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from vortex_msgs.srv import Waypoint +from geometry_msgs.msg import Point +from nav_msgs.msg import Odometry +from transforms3d.euler import quat2euler +import numpy as np + +class WaypointClient(Node): + def __init__(self): + super().__init__('waypoint_client') + self.eta_odom = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, 1) + + self.eta_received = False + + while not self.eta_received: + self.get_logger().info('Waiting for eta...') + rclpy.spin_once(self) + + self.client = self.create_client(Waypoint, 'waypoint_list') + + while not self.client.wait_for_service(timeout_sec=2.0): + self.get_logger().info('service not available, waiting again...') + + self.send_request() + + def eta_callback(self, msg: Odometry): + self.eta = self.odom_to_eta(msg) + #self.get_logger().info(f'Received eta {self.eta}') + self.eta_received = True + + def send_request(self): + if self.eta_received: + req = Waypoint.Request() + wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1]]] + req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list] + self.get_logger().info(f'Sending request: {req}') + self.future = self.client.call_async(req) + self.future.add_done_callback(self.get_response) + + def get_response(self, future): + try: + response = future.result() + self.get_logger().info(f'Received response: {response}') + if response.success: + self.destroy_node() + rclpy.shutdown() + + except Exception as e: + self.get_logger().error('Service call failed %r' % (e,)) + + @staticmethod + def odom_to_eta(msg: Odometry) -> np.ndarray: + """ + Converts an Odometry message to 3DOF eta vector. + + Args: + msg (Odometry): The Odometry message to convert. + + Returns: + np.ndarray: The eta vector. + """ + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles + yaw = quat2euler(orientation_list)[2] + + state = np.array([x, y, yaw]) + return state + +def main(args=None): + rclpy.init(args=args) + waypoint_client_node = WaypointClient() + try: + rclpy.spin(waypoint_client_node) + except Exception as e: + waypoint_client_node.get_logger().error(f'Unhandled exception: {e}') + finally: + if rclpy.ok(): + waypoint_client_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml index e7ac823a..14a63975 100644 --- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml +++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: hybridpath_controller: - K1_diag: [10.0, 10.0, 10.0] # First gain matrix - K2_diag: [60.0, 60.0, 60.0] # Second gain matrix \ No newline at end of file + K1_diag: [1.0, 1.0, 1.0] # First gain matrix + K2_diag: [40.0, 40.0, 40.0] # Second gain matrix \ No newline at end of file diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py index d74f1abb..e9a88bde 100755 --- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py +++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py @@ -20,7 +20,7 @@ def __init__(self): ('physical.damping_matrix_diag', [10.0, 10.0, 5.0]) ]) - self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.state_callback, 1) + self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, 1) self.hpref_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.reference_callback, 1) self.wrench_publisher_ = self.create_publisher(Wrench, 'thrust/wrench_input', 1) From 4a1110f7b888947c148152fd155e8b6e7b3ccce1 Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Mon, 29 Apr 2024 08:38:22 +0200 Subject: [PATCH 03/67] Made LQR and PID ready for testing --- guidance/dp_guidance/dp_guidance/dp_guidance_node.py | 10 +++++----- .../hybridpath_guidance/waypoint_node.py | 2 +- motion/lqr_controller/CMakeLists.txt | 2 +- .../lqr_controller/lqr_controller_node.py | 4 ++-- motion/pid_controller/CMakeLists.txt | 2 +- motion/pid_controller/launch/pid_controller.launch.py | 0 .../pid_controller/pid_controller_node.py | 6 +++--- 7 files changed, 13 insertions(+), 13 deletions(-) mode change 100644 => 100755 motion/lqr_controller/lqr_controller/lqr_controller_node.py mode change 100644 => 100755 motion/pid_controller/launch/pid_controller.launch.py mode change 100644 => 100755 motion/pid_controller/pid_controller/pid_controller_node.py diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index a416f952..29a87a13 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -8,8 +8,8 @@ from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler -from dp_guidance.conversions import odometrymsg_to_state, state_to_odometrymsg -from dp_guidance.reference_filter import ReferenceFilter +from conversions import odometrymsg_to_state, state_to_odometrymsg +from reference_filter import ReferenceFilter class Guidance(Node): def __init__(self): @@ -64,9 +64,9 @@ def guidance_callback(self): self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0]) x_next = self.reference_filter.step(self.eta_ref, self.xd) self.xd = x_next - self.get_logger().info(f'x_next[0]: {x_next[0]}') - self.get_logger().info(f'x_next[0]: {x_next[1]}') - self.get_logger().info(f'x_next[0]: {x_next[2]}') + # self.get_logger().info(f'x_next[0]: {x_next[0]}') + # self.get_logger().info(f'x_next[0]: {x_next[1]}') + # self.get_logger().info(f'x_next[0]: {x_next[2]}') odom_msg = Odometry() odom_msg = state_to_odometrymsg(x_next[:3]) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py index bae7f73b..ff8dc843 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py @@ -34,7 +34,7 @@ def eta_callback(self, msg: Odometry): def send_request(self): if self.eta_received: req = Waypoint.Request() - wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1]]] + wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 3]] req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list] self.get_logger().info(f'Sending request: {req}') self.future = self.client.call_async(req) diff --git a/motion/lqr_controller/CMakeLists.txt b/motion/lqr_controller/CMakeLists.txt index cafa3ad4..874cf293 100644 --- a/motion/lqr_controller/CMakeLists.txt +++ b/motion/lqr_controller/CMakeLists.txt @@ -20,7 +20,7 @@ install(DIRECTORY ) install(PROGRAMS -lqr_controller/lqr_controller_node.py + lqr_controller/lqr_controller_node.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py old mode 100644 new mode 100755 index 18bebe0c..9261d41a --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -5,8 +5,8 @@ from rclpy.node import Node from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry -from lqr_controller.lqr_controller import LQRController -from lqr_controller.conversions import odometrymsg_to_state +from lqr_controller import LQRController +from conversions import odometrymsg_to_state from time import sleep from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy diff --git a/motion/pid_controller/CMakeLists.txt b/motion/pid_controller/CMakeLists.txt index 0c45a92b..dffae1df 100644 --- a/motion/pid_controller/CMakeLists.txt +++ b/motion/pid_controller/CMakeLists.txt @@ -20,7 +20,7 @@ install(DIRECTORY ) install(PROGRAMS -pid_controller/pid_controller_node.py + pid_controller/pid_controller_node.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/motion/pid_controller/launch/pid_controller.launch.py b/motion/pid_controller/launch/pid_controller.launch.py old mode 100644 new mode 100755 diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py old mode 100644 new mode 100755 index 4c3e187c..700a9d4d --- a/motion/pid_controller/pid_controller/pid_controller_node.py +++ b/motion/pid_controller/pid_controller/pid_controller_node.py @@ -5,8 +5,8 @@ from rclpy.node import Node from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry -from pid_controller.pid_controller import PID -from pid_controller.conversions import odometrymsg_to_state +from pid_controller import PID +from conversions import odometrymsg_to_state from time import sleep from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy @@ -67,7 +67,7 @@ def guidance_cb(self, msg): def controller_callback(self): if hasattr(self, 'state') and hasattr(self, 'x_ref'): control_input = self.pid.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period) - self.get_logger().info(f"Control input: {control_input}") + # self.get_logger().info(f"Control input: {control_input}") wrench_msg = Wrench() wrench_msg.force.x = control_input[0] wrench_msg.force.y = control_input[1] From 539d0252f2e90f88f5b92343e0025fca95b439c2 Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Mon, 29 Apr 2024 12:36:01 +0200 Subject: [PATCH 04/67] Added QoS profile to seapath/odom subscription --- .../hybridpath_guidance/hybridpath_guidance_node.py | 8 ++++++-- .../hybridpath_guidance/waypoint_node.py | 9 +++++++-- .../hybridpath_controller/hybridpath_controller_node.py | 7 ++++++- 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 2022db46..5fe957df 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -7,8 +7,12 @@ from vortex_msgs.srv import Waypoint from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler - from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy + + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) class Guidance(Node): def __init__(self): @@ -23,7 +27,7 @@ def __init__(self): ]) self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) - self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, 1) + self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) # Get parameters diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py index ff8dc843..d9e024bd 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py @@ -7,11 +7,16 @@ from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler import numpy as np +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy + + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) class WaypointClient(Node): def __init__(self): super().__init__('waypoint_client') - self.eta_odom = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, 1) + self.eta_odom = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) self.eta_received = False @@ -34,7 +39,7 @@ def eta_callback(self, msg: Odometry): def send_request(self): if self.eta_received: req = Waypoint.Request() - wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 3]] + wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1]]] req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list] self.get_logger().info(f'Sending request: {req}') self.future = self.client.call_async(req) diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py index e9a88bde..07c69eaa 100755 --- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py +++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py @@ -7,6 +7,11 @@ from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry from vortex_msgs.msg import HybridpathReference +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy + + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) class HybridPathControllerNode(Node): def __init__(self): @@ -20,7 +25,7 @@ def __init__(self): ('physical.damping_matrix_diag', [10.0, 10.0, 5.0]) ]) - self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, 1) + self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, qos_profile=qos_profile) self.hpref_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.reference_callback, 1) self.wrench_publisher_ = self.create_publisher(Wrench, 'thrust/wrench_input', 1) From f9cc00178362089ab109e52f2adcc62a5567d62d Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Sun, 23 Jun 2024 13:34:19 +0200 Subject: [PATCH 05/67] Added QoS profile --- .../dp_guidance/dp_guidance/dp_guidance_node.py | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index 29a87a13..b329a483 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -10,6 +10,10 @@ from conversions import odometrymsg_to_state, state_to_odometrymsg from reference_filter import ReferenceFilter +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) class Guidance(Node): def __init__(self): @@ -21,7 +25,7 @@ def __init__(self): ]) self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) - self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1) + self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1) # Get parameters @@ -32,6 +36,7 @@ def __init__(self): self.waiting_message_printed = False self.init_pos = False + self.eta_received = False self.eta = np.array([0, 0, 0]) self.eta_ref = np.array([0, 0, 0]) @@ -54,11 +59,13 @@ def waypoint_callback(self, request, response): def eta_callback(self, msg: Odometry): self.eta = odometrymsg_to_state(msg)[:3] + self.eta_received = True def guidance_callback(self): - if self.waypoints_received: + if self.waypoints_received and self.eta_received: if not self.init_pos: self.xd[0:3] = self.eta + self.get_logger().info(f"Reference initialized at {self.xd[0:3]}") self.init_pos = True last_waypoint = self.waypoints[-1] self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0]) @@ -73,6 +80,9 @@ def guidance_callback(self): self.guidance_publisher.publish(odom_msg) else: + if not self.eta_received: + self.get_logger().info("Waiting for eta") + if not self.waiting_message_printed: self.get_logger().info('Waiting for waypoints to be received') self.waiting_message_printed = True From e611e5c24ace2ef8f2630803a0288d3837c064cc Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Fri, 5 Jul 2024 20:57:52 +0200 Subject: [PATCH 06/67] Added yaw publisher --- .../hybridpath_guidance/hybridpath_guidance_node.py | 12 +++++++++--- .../config/param_joystick_interface.yaml | 6 +++--- .../config/hybridpath_controller_config.yaml | 4 ++-- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 5fe957df..971791e3 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -3,6 +3,7 @@ import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose2D +from std_msgs.msg import Float32 from vortex_msgs.msg import HybridpathReference from vortex_msgs.srv import Waypoint from nav_msgs.msg import Odometry @@ -28,6 +29,7 @@ def __init__(self): self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) + self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1) self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) # Get parameters @@ -65,7 +67,11 @@ def waypoint_callback(self, request, response): return response def eta_callback(self, msg: Odometry): + yaw_msg = Float32() self.eta = self.odom_to_eta(msg) + yaw = float(self.eta[2]) + yaw_msg.data = yaw + self.yaw_publisher.publish(yaw_msg) def guidance_callback(self): if self.waypoints_received: @@ -77,9 +83,9 @@ def guidance_callback(self): pos_der = signals.get_derivatives()[0] pos_dder = signals.get_derivatives()[1] - psi = signals.get_heading() - psi_der = signals.get_heading_derivative() - psi_dder = signals.get_heading_second_derivative() + psi = 0.#signals.get_heading() + psi_der = 0.#signals.get_heading_derivative() + psi_dder = 0.#signals.get_heading_second_derivative() hp_msg = HybridpathReference() hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) diff --git a/mission/joystick_interface/config/param_joystick_interface.yaml b/mission/joystick_interface/config/param_joystick_interface.yaml index 965e3dd8..30c786f2 100755 --- a/mission/joystick_interface/config/param_joystick_interface.yaml +++ b/mission/joystick_interface/config/param_joystick_interface.yaml @@ -1,7 +1,7 @@ joystick_interface: ros__parameters: - surge_scale_factor: 100.0 - sway_scale_factor: 100.0 - yaw_scale_factor: 60.0 + surge_scale_factor: 300.0 + sway_scale_factor: 300.0 + yaw_scale_factor: 180.0 diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml index 14a63975..ef6650ff 100644 --- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml +++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: hybridpath_controller: - K1_diag: [1.0, 1.0, 1.0] # First gain matrix - K2_diag: [40.0, 40.0, 40.0] # Second gain matrix \ No newline at end of file + K1_diag: [0.5, 0.5, 0.5] # First gain matrix + K2_diag: [20.0, 20.0, 20.0] # Second gain matrix \ No newline at end of file From 7050c3b75037a93e375becedf8734ebfb5bde201 Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Tue, 9 Jul 2024 19:42:57 +0200 Subject: [PATCH 07/67] . --- .../dp_guidance/dp_guidance_node.py | 9 +- guidance/hybridpath_guidance/CMakeLists.txt | 1 + .../config/hybridpath_guidance_config.yaml | 2 +- .../hybridpath_guidance/hybridpath.py | 89 +++++++++-- .../hybridpath_guidance_node.py | 146 +++++++++++++----- .../noisy_odom_publisher.py | 57 +++++++ .../hybridpath_guidance/u_desired_node.py | 48 ++++++ .../hybridpath_guidance/waypoint_node.py | 2 +- .../config/hybridpath_controller_config.yaml | 4 +- .../adaptive_backstep.py | 39 ++++- .../hybridpath_controller_node.py | 84 ++++++++-- 11 files changed, 407 insertions(+), 74 deletions(-) create mode 100755 guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py create mode 100644 guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index b329a483..483b5074 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -37,6 +37,7 @@ def __init__(self): self.init_pos = False self.eta_received = False + self.eta_logger = True self.eta = np.array([0, 0, 0]) self.eta_ref = np.array([0, 0, 0]) @@ -68,7 +69,7 @@ def guidance_callback(self): self.get_logger().info(f"Reference initialized at {self.xd[0:3]}") self.init_pos = True last_waypoint = self.waypoints[-1] - self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0]) + self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.eta[2]]) x_next = self.reference_filter.step(self.eta_ref, self.xd) self.xd = x_next # self.get_logger().info(f'x_next[0]: {x_next[0]}') @@ -76,12 +77,14 @@ def guidance_callback(self): # self.get_logger().info(f'x_next[0]: {x_next[2]}') odom_msg = Odometry() - odom_msg = state_to_odometrymsg(x_next[:3]) + # odom_msg = state_to_odometrymsg(x_next[:3]) + odom_msg = state_to_odometrymsg(self.eta_ref) self.guidance_publisher.publish(odom_msg) else: - if not self.eta_received: + if not self.eta_received and self.eta_logger: self.get_logger().info("Waiting for eta") + self.eta_logger = False if not self.waiting_message_printed: self.get_logger().info('Waiting for waypoints to be received') diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt index e16c1b10..8b4fd5ac 100644 --- a/guidance/hybridpath_guidance/CMakeLists.txt +++ b/guidance/hybridpath_guidance/CMakeLists.txt @@ -22,6 +22,7 @@ install(DIRECTORY install(PROGRAMS hybridpath_guidance/hybridpath_guidance_node.py hybridpath_guidance/waypoint_node.py + hybridpath_guidance/noisy_odom_publisher.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml index 20fa168f..7279c745 100644 --- a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml +++ b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml @@ -4,4 +4,4 @@ lambda_val: 0.15 # Curvature constant path_generator_order: 1 # Differentiability order dt: 0.1 # Time step - mu: 0.15 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind \ No newline at end of file + mu: 0.25 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py index 9f2a6155..a4a4db3c 100644 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py @@ -66,6 +66,40 @@ class HybridPathGenerator: path (Path): The path object. """ def __init__(self, WP: list[Point], r: int, lambda_val: float): + self.WP = WP + self.r = r + self.lambda_val = lambda_val + self.order = 2*r + 1 + + def create_path(self, WP: list[Point]) -> None: + """ + Create a path object. + + Args: + WP (list[Point]): A list of waypoints. + + Returns: + None + """ + self.update_waypoints(WP) + self._initialize_path() + self._calculate_subpaths() + + def _initialize_path(self): + self.path = Path() + + self.path.coeff.a = [] + self.path.coeff.b = [] + self.path.coeff.a_der = [] + self.path.coeff.b_der = [] + self.path.LinSys.A = None + self.path.LinSys.bx = [] + self.path.LinSys.by = [] + + self.path.NumSubpaths = len(self.WP) - 1 + self.path.Order = self.order + + def update_waypoints(self, WP: list[Point]) -> None: # Convert the waypoints to a numpy array WP_arr = np.array([[wp.x, wp.y] for wp in WP]) @@ -77,13 +111,6 @@ def __init__(self, WP: list[Point], r: int, lambda_val: float): else: self.WP = WP_arr - self.r = r - self.lambda_val = lambda_val - self.order = 2*r + 1 - self.path = Path() - self.path.NumSubpaths = len(self.WP) - 1 - self.path.Order = self.order - self._calculate_subpaths() def _construct_A_matrix(self) -> np.ndarray: """ @@ -280,6 +307,12 @@ def _calculate_subpaths(self) -> None: for k, (a, b) in enumerate(zip(a_derivatives, b_derivatives)): self._append_derivatives(k, a, b) + def get_path(self) -> Path: + """ + Get the hybrid path. + """ + return self.path + @staticmethod def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> float: """ @@ -295,7 +328,9 @@ def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> flo float: The updated position along the hybrid path. """ - signals = HybridPathSignals(path, s) + signals = HybridPathSignals() + signals.update_path(path) + signals.update_s(s) v_s = signals.get_vs(u_desired) s_new = s + (v_s + w) * dt return s_new @@ -314,13 +349,14 @@ class HybridPathSignals: Path (Path): The path object. s (float): The path parameter. """ - def __init__(self, path: Path, s: float): - if not isinstance(path, Path): - raise TypeError("path must be an instance of Path") - self.path = path - self.s = self._clamp_s(s, self.path.NumSubpaths) + def __init__(self): + # if not isinstance(path, Path): + # raise TypeError("path must be an instance of Path") + self.path = None + self.s = None - def _clamp_s(self, s: float, num_subpaths: int) -> float: + @staticmethod + def _clamp_s(s: float, num_subpaths: int) -> float: """ Ensures s is within the valid range of [0, num_subpaths]. @@ -506,4 +542,29 @@ def get_w(self, mu: float, eta: np.ndarray) -> float: w = (mu / np.linalg.norm(eta_d_s)) * np.dot(eta_d_s, error) return w + + def update_path(self, path: Path) -> None: + """ + Update the path object. + + Args: + path (Path): The new path object. + + Returns: + None + """ + self.path = path + self.update_s(0.) + + def update_s(self, s: float) -> None: + """ + Update the path parameter. + + Args: + s (float): The new path parameter. + + Returns: + None + """ + self.s = self._clamp_s(s, self.path.NumSubpaths) \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 971791e3..f97da507 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -2,14 +2,15 @@ import numpy as np import rclpy from rclpy.node import Node -from geometry_msgs.msg import Pose2D -from std_msgs.msg import Float32 +from geometry_msgs.msg import Pose2D, Point +from std_msgs.msg import Float32, Float64MultiArray from vortex_msgs.msg import HybridpathReference -from vortex_msgs.srv import Waypoint +from vortex_msgs.srv import Waypoint, DesiredVelocity from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy +import threading qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, @@ -28,8 +29,12 @@ def __init__(self): ]) self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) + self.u_desired_server = self.create_service(DesiredVelocity, 'u_desired', self.u_desired_callback) self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1) + self.s_publisher = self.create_publisher(Float32, 's', 1) + self.w_publisher = self.create_publisher(Float32, 'w', 1) + self.wp_arr_publisher = self.create_publisher(Float64MultiArray, 'waypoints', 1) self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) # Get parameters @@ -39,74 +44,128 @@ def __init__(self): self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value self.eta = np.zeros(3) - self.u_desired = 0.2 # Desired velocity + self.u_desired = 0.3 # Desired velocity + + self.waypoints = [] + self.path = None + self.s = 0. + self.w = 0. + + # Initialize path generator + self.generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val) + + # Initialize signals + self.signals = HybridPathSignals() # Flags for logging self.waypoints_received = False self.waiting_message_printed = False + self.first_pos_flag = False + self.eta_received = False # Timer for guidance timer_period = 0.1 self.timer = self.create_timer(timer_period, self.guidance_callback) + self.lock = threading.Lock() + + def u_desired_callback(self, request, response): + self.u_desired = request.u_desired + self.get_logger().info(f"Received desired velocity: {self.u_desired}") + response.success = True + return response + + def waypoint_callback(self, request, response): - self.get_logger().info('Received waypoints, generating path...') - self.waypoints = request.waypoint - generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val) - self.path = generator.path + with self.lock: + if self.eta_received: + self.waypoints = [Point(x=self.eta[0], y=self.eta[1])] - self.waypoints_received = True - self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + self.get_logger().info('Received waypoints, generating path...') - self.s = 0 - signals = HybridPathSignals(self.path, self.s) - self.w = signals.get_w(self.mu, self.eta) - + new_waypoints = request.waypoint + + for point in new_waypoints: + self.waypoints.append(point) + + self.generator.create_path(self.waypoints) + self.path = self.generator.get_path() + + self.waypoints_received = True + self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + self.first_pos_flag = False + + self.s = 0. + self.signals.update_path(self.path) + self.w = self.signals.get_w(self.mu, self.eta) + + wp_arr = Float64MultiArray() + wp_list = self.generator.WP.tolist() + wp_arr.data = [coordinate for wp in wp_list for coordinate in wp] + self.wp_arr_publisher.publish(wp_arr) + response.success = True + return response def eta_callback(self, msg: Odometry): yaw_msg = Float32() self.eta = self.odom_to_eta(msg) - yaw = float(self.eta[2]) - yaw_msg.data = yaw + self.yaw = float(self.eta[2]) + yaw_msg.data = self.yaw self.yaw_publisher.publish(yaw_msg) + self.eta_received = True def guidance_callback(self): - if self.waypoints_received: - self.s = HybridPathGenerator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) - signals = HybridPathSignals(self.path, self.s) - self.w = signals.get_w(self.mu, self.eta) + with self.lock: + if self.waypoints_received: + self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) + self.signals.update_s(self.s) + self.w = self.signals.get_w(self.mu, self.eta) + + pos = self.signals.get_position() - pos = signals.get_position() - pos_der = signals.get_derivatives()[0] - pos_dder = signals.get_derivatives()[1] + if not self.first_pos_flag: + self.get_logger().info(f"First position: {pos}") + self.first_pos_flag = True - psi = 0.#signals.get_heading() - psi_der = 0.#signals.get_heading_derivative() - psi_dder = 0.#signals.get_heading_second_derivative() + # pos[0] = self.eta[0] + pos_der = self.signals.get_derivatives()[0] + pos_dder = self.signals.get_derivatives()[1] - hp_msg = HybridpathReference() - hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) - hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) - hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) + psi = 0. #signals.get_heading() + psi_der = 0.#signals.get_heading_derivative() + psi_dder = 0.#signals.get_heading_second_derivative() - hp_msg.w = signals.get_w(self.mu, self.eta) - hp_msg.v_s = signals.get_vs(self.u_desired) - hp_msg.v_ss = signals.get_vs_derivative(self.u_desired) + hp_msg = HybridpathReference() + hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) + hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) + hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) - self.guidance_publisher.publish(hp_msg) + hp_msg.w = self.signals.get_w(self.mu, self.eta) + hp_msg.v_s = self.signals.get_vs(self.u_desired) + hp_msg.v_ss = self.signals.get_vs_derivative(self.u_desired) - if self.s >= self.path.NumSubpaths: - self.waypoints_received = False - self.waiting_message_printed = False - self.get_logger().info('Last waypoint reached') + self.guidance_publisher.publish(hp_msg) - else: - if not self.waiting_message_printed: - self.get_logger().info('Waiting for waypoints to be received') - self.waiting_message_printed = True + if self.s >= self.path.NumSubpaths: + self.waypoints_received = False + self.waiting_message_printed = False + self.get_logger().info('Last waypoint reached') + + else: + if not self.waiting_message_printed: + self.get_logger().info('Waiting for waypoints to be received') + self.waiting_message_printed = True + + s_msg = Float32() + s_msg.data = self.s + self.s_publisher.publish(s_msg) + + w_msg = Float32() + w_msg.data = self.w + self.w_publisher.publish(w_msg) @staticmethod def odom_to_eta(msg: Odometry) -> np.ndarray: @@ -129,7 +188,10 @@ def odom_to_eta(msg: Odometry) -> np.ndarray: # Convert quaternion to Euler angles yaw = quat2euler(orientation_list)[2] + # yaw = np.deg2rad(yaw) + state = np.array([x, y, yaw]) + return state def main(args=None): diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py b/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py new file mode 100755 index 00000000..5d4395d5 --- /dev/null +++ b/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from vortex_msgs.srv import Waypoint +from geometry_msgs.msg import Point +from nav_msgs.msg import Odometry +from transforms3d.euler import quat2euler +import numpy as np +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy + + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) + +class OdomPublisher(Node): + def __init__(self): + super().__init__('odom_publisher') + + self.eta_odom_publisher = self.create_publisher(Odometry, '/seapath/odom/ned', qos_profile) + + timer_period = 0.01 + self.timer = self.create_timer(timer_period, self.odom_callback) + + self.get_logger().info("Odom publisher started") + + def odom_callback(self): + + noise = np.random.normal(0, 0.005, 3) + + msg = Odometry() + msg.pose.pose.position.x = 0.0 + noise[0] + msg.pose.pose.position.y = 0.0 + noise[1] + msg.pose.pose.position.z = 0.0 + msg.pose.pose.orientation.x = 0.0 + msg.pose.pose.orientation.y = 0.0 + msg.pose.pose.orientation.z = 0.0 + msg.pose.pose.orientation.w = 1.0 + msg.twist.twist.linear.x = 0.0 + noise[0] + msg.twist.twist.linear.y = 0.0 + noise[1] + msg.twist.twist.linear.z = 0.0 + msg.twist.twist.angular.x = 0.0 + msg.twist.twist.angular.y = 0.0 + msg.twist.twist.angular.z = 0.0 + noise[2] + + self.eta_odom_publisher.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = OdomPublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py new file mode 100644 index 00000000..033afd22 --- /dev/null +++ b/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py @@ -0,0 +1,48 @@ +import rclpy +from rclpy.node import Node +from vortex_msgs.srv import DesiredVelocity + +class DesiredVelocityClient(Node): + def __init__(self): + super().__init__('desired_velocity_client') + self.client = self.create_client(DesiredVelocity, 'u_desired') + + while not self.client.wait_for_service(timeout_sec=2.0): + self.get_logger().info('service not available, waiting again...') + + self.send_request() + + def send_request(self): + req = DesiredVelocity.Request() + req.u = 0.3 + self.get_logger().info(f'Sending request: {req}') + self.future = self.client.call_async(req) + self.future.add_done_callback(self.get_response) + + def get_response(self, future): + try: + response = future.result() + self.get_logger().info(f'Received response: {response}') + if response.success: + self.destroy_node() + rclpy.shutdown() + + except Exception as e: + self.get_logger().error('Service call failed %r' % (e,)) + +def main(args=None): + rclpy.init(args=args) + + client = DesiredVelocityClient() + + try: + rclpy.spin(client) + except Exception as e: + client.get_logger().error('Error in DesiredVelocityClient: %r' % (e,)) + finally: + if rclpy.ok(): + client.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py index d9e024bd..59b5df38 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py @@ -39,7 +39,7 @@ def eta_callback(self, msg: Odometry): def send_request(self): if self.eta_received: req = Waypoint.Request() - wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1]]] + wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 0.]] req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list] self.get_logger().info(f'Sending request: {req}') self.future = self.client.call_async(req) diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml index ef6650ff..f01ea966 100644 --- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml +++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: hybridpath_controller: - K1_diag: [0.5, 0.5, 0.5] # First gain matrix - K2_diag: [20.0, 20.0, 20.0] # Second gain matrix \ No newline at end of file + K1_diag: [1., 1., 1.] # First gain matrix + K2_diag: [40.0, 30.0, 20.0] # Second gain matrix \ No newline at end of file diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py index ff95a51c..ecc0008e 100644 --- a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py +++ b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py @@ -4,7 +4,13 @@ from transforms3d.euler import quat2euler class AdaptiveBackstep: - def __init__(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None: + def __init__(self): + self.K_1 = np.eye(3) + self.K_2 = np.eye(3) + self.M = np.eye(3) + self.D = np.eye(3) + + def update_parameters(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None: self.K_1 = K1 self.K_2 = K2 self.M = M @@ -27,6 +33,7 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda # Extract values from the state and reference eta = state[:3] + # eta[0] = 0. nu = state[3:] w = reference.w v_s = reference.v_s @@ -54,8 +61,34 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda tau = -self.K_2 @ z2 + self.calculate_coriolis_matrix(nu) + self.D @ nu + self.M @ sigma1 + self.M @ ds_alpha1 * (v_s + w) + self.eta_error = eta_error + self.z1 = z1 + self.alpha1 = alpha1 + self.z2 = z2 + self.ds_alpha1 = ds_alpha1 + self.sigma1 = sigma1 + return tau + def get_eta_error(self): + return self.eta_error + + def get_z1(self): + return self.z1 + + def get_alpha1(self): + return self.alpha1 + + def get_z2(self): + return self.z2 + + def get_sigma1(self): + return self.sigma1 + + def get_ds_alpha1(self): + return self.ds_alpha1 + + @staticmethod def calculate_coriolis_matrix(nu: np.ndarray) -> np.ndarray: """ @@ -66,7 +99,7 @@ def calculate_coriolis_matrix(nu: np.ndarray) -> np.ndarray: [0, 0, 5.5], [82.5, -5.5, 0] ]) - return C_A @ nu + return (C_A @ nu) * 0 @staticmethod def rotationmatrix_in_yaw_transpose(psi: float) -> np.ndarray: @@ -118,6 +151,8 @@ def odom_to_state(msg: Odometry) -> np.ndarray: # Convert quaternion to Euler angles yaw = quat2euler(orientation_list)[2] + # yaw = np.deg2rad(yaw) + u = msg.twist.twist.linear.x v = msg.twist.twist.linear.y r = msg.twist.twist.angular.z diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py index 07c69eaa..86718344 100755 --- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py +++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py @@ -7,7 +7,9 @@ from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry from vortex_msgs.msg import HybridpathReference +from std_msgs.msg import Float64MultiArray from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy +from rcl_interfaces.msg import SetParametersResult qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, @@ -25,31 +27,61 @@ def __init__(self): ('physical.damping_matrix_diag', [10.0, 10.0, 5.0]) ]) + self.parameters_updated = False + self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, qos_profile=qos_profile) self.hpref_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.reference_callback, 1) self.wrench_publisher_ = self.create_publisher(Wrench, 'thrust/wrench_input', 1) - # Get parameters + # Debug publishers + self.eta_error_publisher = self.create_publisher(Float64MultiArray, 'eta_error', 10) + self.z1_publisher = self.create_publisher(Float64MultiArray, 'z1', 10) + self.alpha1_publisher = self.create_publisher(Float64MultiArray, 'alpha1', 10) + self.z2_publisher = self.create_publisher(Float64MultiArray, 'z2', 10) + self.ds_alpha1_publisher = self.create_publisher(Float64MultiArray, 'ds_alpha1', 10) + self.sigma1_publisher = self.create_publisher(Float64MultiArray, 'sigma1', 10) + self.tau_publisher = self.create_publisher(Float64MultiArray, 'tau', 10) + + self.AB_controller_ = AdaptiveBackstep() + + self.update_controller_parameters() + + controller_period = 0.1 + self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + self.add_on_set_parameters_callback(self.parameter_callback) + + self.get_logger().info("hybridpath_controller_node started") + + def update_controller_parameters(self): + K1_diag = self.get_parameter('hybridpath_controller.K1_diag').get_parameter_value().double_array_value K2_diag = self.get_parameter('hybridpath_controller.K2_diag').get_parameter_value().double_array_value D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value - # Transform parameters to diagonal matrices K1 = np.diag(K1_diag) K2 = np.diag(K2_diag) D = np.diag(D_diag) - - # Reshape M to a 3x3 matrix M = np.reshape(M, (3, 3)) - # Create controller object - self.AB_controller_ = AdaptiveBackstep(K1, K2, M, D) + self.AB_controller_.update_parameters(K1, K2, M, D) - controller_period = 0.1 - self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + self.get_logger().info("Updated controller parameters") - self.get_logger().info("hybridpath_controller_node started") + def parameter_callback(self, params): + self.parameters_updated = True + for param in params: + if param.name == 'hybridpath_controller.K1_diag': + self.get_logger().info(f"Updated K1_diag to {param.value}") + elif param.name == 'hybridpath_controller.K2_diag': + self.get_logger().info(f"Updated K2_diag to {param.value}") + elif param.name == 'physical.damping_matrix_diag': + self.get_logger().info(f"Updated damping_matrix_diag to {param.value}") + elif param.name == 'physical.inertia_matrix': + self.get_logger().info(f"Updated inertia_matrix to {param.value}") + + # self.update_controller_parameters() + return SetParametersResult(successful=True) def state_callback(self, msg: Odometry): """ @@ -64,6 +96,10 @@ def controller_callback(self): """ Callback function for the controller timer. This function calculates the control input and publishes the control input. """ + if self.parameters_updated: + self.update_controller_parameters() + self.parameters_updated = False + if hasattr(self, 'state_odom') and hasattr(self, 'reference'): control_input = self.AB_controller_.control_law(self.state_odom, self.reference) wrench_msg = Wrench() @@ -72,6 +108,36 @@ def controller_callback(self): wrench_msg.torque.z = control_input[2] self.wrench_publisher_.publish(wrench_msg) + # Debug publishers + eta_error_msg = Float64MultiArray() + eta_error_msg.data = self.AB_controller_.get_eta_error().tolist() + self.eta_error_publisher.publish(eta_error_msg) + + z1_msg = Float64MultiArray() + z1_msg.data = self.AB_controller_.get_z1().tolist() + self.z1_publisher.publish(z1_msg) + + alpha1_msg = Float64MultiArray() + alpha1_msg.data = self.AB_controller_.get_alpha1().tolist() + self.alpha1_publisher.publish(alpha1_msg) + + z2_msg = Float64MultiArray() + z2_msg.data = self.AB_controller_.get_z2().tolist() + self.z2_publisher.publish(z2_msg) + + ds_alpha1_msg = Float64MultiArray() + ds_alpha1_msg.data = self.AB_controller_.get_ds_alpha1().tolist() + self.ds_alpha1_publisher.publish(ds_alpha1_msg) + + sigma1_msg = Float64MultiArray() + sigma1_msg.data = self.AB_controller_.get_sigma1().tolist() + self.sigma1_publisher.publish(sigma1_msg) + + tau_msg = Float64MultiArray() + tau_msg.data = control_input.tolist() + self.tau_publisher.publish(tau_msg) + + def main(args=None): rclpy.init(args=args) node = HybridPathControllerNode() From 1cd14f8bb4f0edd655a4928b0e5f78c44f3d612b Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Thu, 11 Jul 2024 18:22:31 +0200 Subject: [PATCH 08/67] Added way to reinitialize waypoints through service call --- guidance/dp_guidance/dp_guidance/dp_guidance_node.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index 483b5074..b0d754f5 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -53,6 +53,7 @@ def __init__(self): def waypoint_callback(self, request, response): self.waypoints = request.waypoint self.get_logger().info(f'Received waypoints: {self.waypoints}') + self.xd = np.zeros(9) self.waypoints_received = True self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets response.success = True @@ -77,8 +78,8 @@ def guidance_callback(self): # self.get_logger().info(f'x_next[0]: {x_next[2]}') odom_msg = Odometry() - # odom_msg = state_to_odometrymsg(x_next[:3]) - odom_msg = state_to_odometrymsg(self.eta_ref) + odom_msg = state_to_odometrymsg(x_next[:3]) + # odom_msg = state_to_odometrymsg(self.eta_ref) self.guidance_publisher.publish(odom_msg) else: From 09110faf381e42f6512d4a6a288f50bf6eb08f56 Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Sat, 13 Jul 2024 13:28:01 +0200 Subject: [PATCH 09/67] Enabled controller tuning of LQR and PID during runtime. --- .../dp_guidance/dp_guidance_node.py | 12 ++-- .../lqr_controller/lqr_controller.py | 25 ++++---- .../lqr_controller/lqr_controller_node.py | 57 ++++++++++++----- motion/pid_controller/config/pid_config.yaml | 6 +- .../pid_controller/pid_controller.py | 37 ++++++----- .../pid_controller/pid_controller_node.py | 64 +++++++++++++------ 6 files changed, 128 insertions(+), 73 deletions(-) diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index b0d754f5..317332da 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -7,7 +7,7 @@ from vortex_msgs.srv import Waypoint from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler - +from std_msgs.msg import Float32 from conversions import odometrymsg_to_state, state_to_odometrymsg from reference_filter import ReferenceFilter from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy @@ -27,7 +27,9 @@ def __init__(self): self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1) - + self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1) + self.yaw_ref_publisher = self.create_publisher(Float32, 'yaw_ref', 1) + # Get parameters self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value @@ -39,8 +41,7 @@ def __init__(self): self.eta_received = False self.eta_logger = True - self.eta = np.array([0, 0, 0]) - self.eta_ref = np.array([0, 0, 0]) + self.yaw_ref = 0 self.xd = np.zeros(9) @@ -62,6 +63,7 @@ def waypoint_callback(self, request, response): def eta_callback(self, msg: Odometry): self.eta = odometrymsg_to_state(msg)[:3] self.eta_received = True + self.yaw_publisher.publish(Float32(data=self.eta[2])) def guidance_callback(self): if self.waypoints_received and self.eta_received: @@ -70,7 +72,7 @@ def guidance_callback(self): self.get_logger().info(f"Reference initialized at {self.xd[0:3]}") self.init_pos = True last_waypoint = self.waypoints[-1] - self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.eta[2]]) + self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.yaw_ref]) x_next = self.reference_filter.step(self.eta_ref, self.xd) self.xd = x_next # self.get_logger().info(f'x_next[0]: {x_next[0]}') diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index 7c7b6f67..d9507b67 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -2,34 +2,37 @@ from scipy.linalg import solve_continuous_are class LQRController: - def __init__(self, M: float, D: list[float], Q: list[float], R: list[float], heading_ref: float = 0.0): - self.M = M - self.M_inv = np.linalg.inv(self.M) - self.D = D + def __init__(self): + self.M = np.eye(3) + self.D = np.eye(3) + self.Q = np.eye(6) + self.R = np.eye(3) self.A = np.zeros((6,6)) self.B = np.zeros((6,3)) self.C = np.zeros((3,6)) self.C[:3, :3] = np.eye(3) - + + def update_parameters(self, M: float, D: list[float], Q: list[float], R: list[float]): + self.M = M + self.M_inv = np.linalg.inv(self.M) + self.D = D + self.Q = np.diag(Q) - print(f"Q: {self.Q}") self.R = np.diag(R) - self.linearize_model(heading_ref) - def calculate_control_input(self, x, x_ref): tau = -self.K_LQR @ x + self.K_r @ x_ref return tau - def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: - R = np.array( + def calculate_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: + rotation_matrix = np.array( [[np.cos(heading), -np.sin(heading), 0], [np.sin(heading), np.cos(heading), 0], [0, 0, 1]] ) - self.A[:3,3:] = R + self.A[:3,3:] = rotation_matrix self.A[3:, 3:] = - self.M_inv @ self.D self.B[3:,:] = self.M_inv diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index 9261d41a..6ffd5420 100755 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -8,6 +8,8 @@ from lqr_controller import LQRController from conversions import odometrymsg_to_state from time import sleep +from std_msgs.msg import Float32 +from rcl_interfaces.msg import SetParametersResult from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy @@ -27,32 +29,49 @@ def __init__(self): ('physical.damping_matrix_diag', [77.55, 162.5, 42.65]) ]) - self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) + self.parameters_updated = False + + self.state_subscriber_ = self.create_subscription(Odometry, "/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) + + self.LQR_controller = LQRController() - Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value - R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value - D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value - M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value - - D = np.diag(D_diag) - M = np.reshape(M, (3, 3)) - - heading_ref = 0.0 - - self.LQR = LQRController(M, D, Q, R, heading_ref) - - self.x_ref = [0, 0, 0] - self.state = [0, 0, 0, 0, 0, 0] + self.update_controller_parameters() self.enabled = False controller_period = 0.1 self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + self.add_on_set_parameters_callback(self.parameter_callback) self.get_logger().info("lqr_controller_node started") + def update_controller_parameters(self): + Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value + R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value + D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + D = np.diag(D_diag) + M = np.reshape(M, (3, 3)) + + self.LQR_controller.update_parameters(M, D, Q, R) + + def parameter_callback(self, params): + self.parameters_updated = True + for param in params: + if param.name == 'lqr_controller.Q': + self.get_logger().info(f"Updated Q to {param.value}") + elif param.name == 'lqr_controller.R': + self.get_logger().info(f"Updated R to {param.value}") + elif param.name == 'physical.inertia_matrix': + self.get_logger().info(f"Updated inertia_matrix to {param.value}") + elif param.name == 'physical.damping_matrix_diag': + self.get_logger().info(f"Updated damping_matrix_diag to {param.value}") + + return SetParametersResult(successful=True) + def state_cb(self, msg): self.state = odometrymsg_to_state(msg) @@ -60,9 +79,13 @@ def guidance_cb(self, msg): self.x_ref = odometrymsg_to_state(msg)[:3] def controller_callback(self): + if self.parameters_updated: + self.update_controller_parameters() + self.parameters_updated = False + if hasattr(self, 'state') and hasattr(self, 'x_ref'): - self.LQR.linearize_model(self.state[2]) - control_input = self.LQR.calculate_control_input(self.state, self.x_ref) + self.LQR_controller.calculate_model(self.state[2]) + control_input = self.LQR_controller.calculate_control_input(self.state, self.x_ref) wrench_msg = Wrench() wrench_msg.force.x = control_input[0] wrench_msg.force.y = control_input[1] diff --git a/motion/pid_controller/config/pid_config.yaml b/motion/pid_controller/config/pid_config.yaml index f04befd2..f600f9f0 100644 --- a/motion/pid_controller/config/pid_config.yaml +++ b/motion/pid_controller/config/pid_config.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: pid_controller: - Kp: [23.04, 34.56, 4.032] # Proportional costs for [x, y, heading] - Ki: [2.76, 4.1, 0.48] # Integral costs for [x, y, heading] - Kd: [28.7648, 43.1472, 5.048384] # Derivative costs for [x, y, heading] \ No newline at end of file + Kp: [130., 240., 60.] # Proportional costs for [x, y, heading] + Ki: [15., 29., 7.] # Integral costs for [x, y, heading] + Kd: [160., 300., 75.] # Derivative costs for [x, y, heading] \ No newline at end of file diff --git a/motion/pid_controller/pid_controller/pid_controller.py b/motion/pid_controller/pid_controller/pid_controller.py index 1207fbb1..1bd218e1 100644 --- a/motion/pid_controller/pid_controller/pid_controller.py +++ b/motion/pid_controller/pid_controller/pid_controller.py @@ -3,15 +3,20 @@ import numpy as np class PID: - def __init__(self, Kp, Ki, Kd): + def __init__(self): self.error_sum = np.zeros(3) + self.Kp = np.eye(3) + self.Ki = np.eye(3) + self.Kd = np.eye(3) + + # self.tau_max = np.array([100, 100, 100]) + + def update_parameters(self, Kp, Ki, Kd): self.Kp = np.diag(Kp) self.Ki = np.diag(Ki) self.Kd = np.diag(Kd) - self.tau_max = np.array([100, 100, 100]) - def calculate_control_input(self, eta, eta_d, eta_dot, dt): error = eta - eta_d self.error_sum += error * dt @@ -25,19 +30,19 @@ def calculate_control_input(self, eta, eta_d, eta_dot, dt): tau = -(p + i + d) - if tau[0] > self.tau_max[0]: - tau[0] = self.tau_max[0] - elif tau[0] < -self.tau_max[0]: - tau[0] = -self.tau_max[0] + # if tau[0] > self.tau_max[0]: + # tau[0] = self.tau_max[0] + # elif tau[0] < -self.tau_max[0]: + # tau[0] = -self.tau_max[0] - if tau[1] > self.tau_max[1]: - tau[1] = self.tau_max[1] - elif tau[1] < -self.tau_max[1]: - tau[1] = -self.tau_max[1] - - if tau[2] > self.tau_max[2]: - tau[2] = self.tau_max[2] - elif tau[2] < -self.tau_max[2]: - tau[2] = -self.tau_max[2] + # if tau[1] > self.tau_max[1]: + # tau[1] = self.tau_max[1] + # elif tau[1] < -self.tau_max[1]: + # tau[1] = -self.tau_max[1] + + # if tau[2] > self.tau_max[2]: + # tau[2] = self.tau_max[2] + # elif tau[2] < -self.tau_max[2]: + # tau[2] = -self.tau_max[2] return tau \ No newline at end of file diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py index 700a9d4d..dfff3a08 100755 --- a/motion/pid_controller/pid_controller/pid_controller_node.py +++ b/motion/pid_controller/pid_controller/pid_controller_node.py @@ -8,6 +8,7 @@ from pid_controller import PID from conversions import odometrymsg_to_state from time import sleep +from rcl_interfaces.msg import SetParametersResult from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy @@ -24,40 +25,61 @@ def __init__(self): ('pid_controller.Kp', [1.0, 1.0, 1.0]), ('pid_controller.Ki', [1.0, 1.0, 1.0]), ('pid_controller.Kd', [1.0, 1.0, 1.0]), - ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65]) - ]) + ]) - self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) + self.parameters_updated = False + + self.state_subscriber_ = self.create_subscription(Odometry, "/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) - Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value - Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value - Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value - M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value - - M = np.reshape(M, (3, 3)) - M_diag = np.diag(M) - ## PID TUNING VALUES ## (OVERWRITES YAML FILE VALUES) - omega_n = 1.2 - zeta = 0.75 - Kp = M_diag * omega_n**2 - Kd = M_diag * 2 * zeta * omega_n #- D_diag - Ki = omega_n/10 * Kp + # M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + # M = np.reshape(M, (3, 3)) + # M_diag = np.diag(M) + # omega_n = 1.2 + # zeta = 0.75 + # Kp = M_diag * omega_n**2 + # self.get_logger().info(f"Kp: {Kp}") + # Kd = M_diag * 2 * zeta * omega_n #- D_diag + # self.get_logger().info(f"Kd: {Kd}") + # Ki = omega_n/10 * Kp + # self.get_logger().info(f"Ki: {Ki}") - self.pid = PID(Kp, Ki, Kd) + self.pid_controller = PID() - self.x_ref = np.array([0, 0, 0]) - self.state = np.array([0, 0, 0, 0, 0, 0]) + self.update_controller_parameters() self.enabled = False self.controller_period = 0.1 self.controller_timer_ = self.create_timer(self.controller_period, self.controller_callback) - + self.add_on_set_parameters_callback(self.parameter_callback) + self.get_logger().info("pid_controller_node started") + def update_controller_parameters(self): + Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value + Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value + Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value + + self.pid_controller.update_parameters(Kp, Ki, Kd) + + self.get_logger().info("Updated controller parameters") + + def parameter_callback(self, params): + self.parameters_updated = True + for param in params: + if param.name == 'pid_controller.Kp': + self.get_logger().info(f"Updated Kp to {param.value}") + elif param.name == 'pid_controller.Ki': + self.get_logger().info(f"Updated Ki to {param.value}") + elif param.name == 'pid_controller.Kd': + self.get_logger().info(f"Updated Kd to {param.value}") + + return SetParametersResult(successful=True) + def state_cb(self, msg): self.state = odometrymsg_to_state(msg) @@ -66,7 +88,7 @@ def guidance_cb(self, msg): def controller_callback(self): if hasattr(self, 'state') and hasattr(self, 'x_ref'): - control_input = self.pid.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period) + control_input = self.pid_controller.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period) # self.get_logger().info(f"Control input: {control_input}") wrench_msg = Wrench() wrench_msg.force.x = control_input[0] From 19a7ff090c246ace3c591df387568464588c8c6d Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Sat, 13 Jul 2024 15:48:53 +0200 Subject: [PATCH 10/67] sitaw merge --- asv_setup/config/sitaw/land_polygon.yaml | 8 + .../config/sitaw/landmark_server_params.yaml | 4 + .../config/sitaw/map_manager_params.yaml | 8 + asv_setup/config/sitaw/seapath_params.yaml | 7 + asv_setup/launch/sitaw.launch.py | 51 +++ asv_setup/launch/tf.launch.py | 21 +- .../dp_guidance/launch/dp_guidance.launch.py | 1 + mission/landmark_server/CMakeLists.txt | 16 +- .../include/landmark_server/grid_manager.hpp | 96 +++++ .../landmark_server/landmark_server.hpp | 27 ++ .../launch/landmark_server_launch.py | 26 ++ mission/landmark_server/package.xml | 3 + .../params/landmark_server_params.yaml | 4 + mission/landmark_server/src/grid_manager.cpp | 196 ++++++++++ .../landmark_server/src/landmark_server.cpp | 186 +++++++--- mission/map_manager/CMakeLists.txt | 60 ++++ .../include/map_manager/map_manager_ros.hpp | 83 +++++ .../map_manager/launch/map_manager_launch.py | 26 ++ mission/map_manager/package.xml | 24 ++ .../params/map_manager_params.yaml | 8 + mission/map_manager/src/map_manager_node.cpp | 14 + mission/map_manager/src/map_manager_ros.cpp | 340 ++++++++++++++++++ mission/waypoint_manager/package.xml | 2 +- 23 files changed, 1164 insertions(+), 47 deletions(-) create mode 100644 asv_setup/config/sitaw/land_polygon.yaml create mode 100644 asv_setup/config/sitaw/landmark_server_params.yaml create mode 100644 asv_setup/config/sitaw/map_manager_params.yaml create mode 100644 asv_setup/config/sitaw/seapath_params.yaml create mode 100644 asv_setup/launch/sitaw.launch.py create mode 100644 mission/landmark_server/include/landmark_server/grid_manager.hpp create mode 100644 mission/landmark_server/launch/landmark_server_launch.py create mode 100644 mission/landmark_server/params/landmark_server_params.yaml create mode 100644 mission/landmark_server/src/grid_manager.cpp create mode 100644 mission/map_manager/CMakeLists.txt create mode 100644 mission/map_manager/include/map_manager/map_manager_ros.hpp create mode 100644 mission/map_manager/launch/map_manager_launch.py create mode 100644 mission/map_manager/package.xml create mode 100644 mission/map_manager/params/map_manager_params.yaml create mode 100644 mission/map_manager/src/map_manager_node.cpp create mode 100644 mission/map_manager/src/map_manager_ros.cpp diff --git a/asv_setup/config/sitaw/land_polygon.yaml b/asv_setup/config/sitaw/land_polygon.yaml new file mode 100644 index 00000000..52cec359 --- /dev/null +++ b/asv_setup/config/sitaw/land_polygon.yaml @@ -0,0 +1,8 @@ +63.437946025438066 10.507175268806458 +63.43787563495969 10.507121157686353 +63.4378360402396 10.50719002638467 +63.43763366637102 10.50697358190425 +63.43760726967404 10.507101480915406 +63.43713432473124 10.506752218231092 +63.437239913116464 10.505876601923934 +63.438056010214204 10.506461985859616 \ No newline at end of file diff --git a/asv_setup/config/sitaw/landmark_server_params.yaml b/asv_setup/config/sitaw/landmark_server_params.yaml new file mode 100644 index 00000000..52e3ba35 --- /dev/null +++ b/asv_setup/config/sitaw/landmark_server_params.yaml @@ -0,0 +1,4 @@ +landmark_server_node: + ros__parameters: + fixed_frame: map + target_frame: base_link \ No newline at end of file diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml new file mode 100644 index 00000000..e2c48285 --- /dev/null +++ b/asv_setup/config/sitaw/map_manager_params.yaml @@ -0,0 +1,8 @@ +map_manager_node: + ros__parameters: + use_predef_landmask: true + landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon.yaml" + map_resolution: 0.2 + map_width: 1000 + map_height: 1000 + frame_id: "map" diff --git a/asv_setup/config/sitaw/seapath_params.yaml b/asv_setup/config/sitaw/seapath_params.yaml new file mode 100644 index 00000000..a5406a83 --- /dev/null +++ b/asv_setup/config/sitaw/seapath_params.yaml @@ -0,0 +1,7 @@ +seapath_ros_driver_node: + ros__parameters: + UDP_IP: "10.0.1.10" # Source IP of the host sender + UDP_PORT: 31421 # Port at which to receive UDP semgents + use_predef_map_origin: false + map_origin_lat: 0.0 + map_origin_lon: 0.0 diff --git a/asv_setup/launch/sitaw.launch.py b/asv_setup/launch/sitaw.launch.py new file mode 100644 index 00000000..1961eeea --- /dev/null +++ b/asv_setup/launch/sitaw.launch.py @@ -0,0 +1,51 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + enable_tf = LaunchConfiguration('enable_tf') + enable_tf_arg = DeclareLaunchArgument( + 'enable_tf', + default_value='True', + description='enable tf launch file', + ) + + seapath_ros_driver_node = Node( + package='seapath_ros_driver', + executable='seapath_ros_driver_node', + name='seapath_ros_driver_node', + parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','seapath_params.yaml')], + output='screen', + ) + map_manager_node = Node( + package='map_manager', + executable='map_manager_node', + name='map_manager_node', + parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','map_manager_params.yaml')], + output='screen', + ) + landmark_server_node = Node( + package='landmark_server', + executable='landmark_server_node', + name='landmark_server_node', + parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','landmark_server_params.yaml')], + # arguments=['--ros-args', '--log-level', 'DEBUG'], + output='screen', + ) + tf_launch = IncludeLaunchDescription( + launch_description_source=PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('asv_setup'), 'launch', 'tf.launch.py')), + condition=IfCondition(enable_tf), + ) + return LaunchDescription([ + enable_tf_arg, + tf_launch, + seapath_ros_driver_node, + map_manager_node, + landmark_server_node + ]) \ No newline at end of file diff --git a/asv_setup/launch/tf.launch.py b/asv_setup/launch/tf.launch.py index 0051d517..f5b1ffe4 100644 --- a/asv_setup/launch/tf.launch.py +++ b/asv_setup/launch/tf.launch.py @@ -36,9 +36,9 @@ def generate_launch_description(): ) # base_link (NED) to seapath_frame (NED) tf. - tf_base_link_to_seapath = Node( + tf_seapath_to_base_link = Node( package='tf2_ros', - name='base_link_ned_to_seapath_frame', + name='seapath_to_base_link', executable='static_transform_publisher', arguments=['--x' , '0', '--y' , '0', @@ -50,8 +50,23 @@ def generate_launch_description(): '--child-frame-id' , 'base_link'], ) + os_sensor_to_os_lidar = Node( + package='tf2_ros', + name='os_sensor_to_os_lidar', + executable='static_transform_publisher', + arguments=['--x' , '0', + '--y' , '0', + '--z' , '0.036180000000000004', + '--roll' , '0', + '--pitch' , '0', + '--yaw' , str(math.pi), + '--frame-id' , 'os_sensor', + '--child-frame-id' , 'os_lidar'], + ) + return LaunchDescription([ tf_base_link_to_lidar, tf_base_link_to_zed_camera_link, - tf_base_link_to_seapath, + tf_seapath_to_base_link, + os_sensor_to_os_lidar, ]) diff --git a/guidance/dp_guidance/launch/dp_guidance.launch.py b/guidance/dp_guidance/launch/dp_guidance.launch.py index 710ed9e0..1f9ad872 100644 --- a/guidance/dp_guidance/launch/dp_guidance.launch.py +++ b/guidance/dp_guidance/launch/dp_guidance.launch.py @@ -14,3 +14,4 @@ def generate_launch_description(): return LaunchDescription([ dp_guidance_node ]) + diff --git a/mission/landmark_server/CMakeLists.txt b/mission/landmark_server/CMakeLists.txt index 3fa085b5..74d383a1 100644 --- a/mission/landmark_server/CMakeLists.txt +++ b/mission/landmark_server/CMakeLists.txt @@ -22,24 +22,34 @@ find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) find_package(rclcpp_components REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) include_directories(include) +include_directories(${EIGEN3_INCLUDE_DIR}) # Create an executable add_executable(landmark_server_node src/landmark_server.cpp - src/landmark_server_main.cpp) + src/landmark_server_main.cpp + src/grid_manager.cpp) target_link_libraries( ${PROJECT_NAME}_node - pcl_common) + pcl_common + ${PCL_LIBRARIES} + Eigen3::Eigen) ament_target_dependencies(landmark_server_node rclcpp std_msgs vortex_msgs + sensor_msgs + pcl_conversions nav_msgs rclcpp_action rclcpp_components @@ -52,6 +62,8 @@ ament_target_dependencies(landmark_server_node ) install(DIRECTORY + launch + params DESTINATION share/${PROJECT_NAME}/ ) diff --git a/mission/landmark_server/include/landmark_server/grid_manager.hpp b/mission/landmark_server/include/landmark_server/grid_manager.hpp new file mode 100644 index 00000000..93327dcb --- /dev/null +++ b/mission/landmark_server/include/landmark_server/grid_manager.hpp @@ -0,0 +1,96 @@ +#include +#include +#include +#include + +namespace landmark_server +{ + +enum class ShapeType : uint8_t +{ + SPHERE = 0, + PRISM = 1 +}; + +struct Circle +{ + float radius; + float x_centre; + float y_centre; +}; + +struct Point +{ + float x; + float y; + float z; +}; + +struct Polygon +{ + std::vector vertices; +}; + +struct Pose +{ + float x; + float y; + float yaw; +}; + + +struct ShapeInfo +{ + ShapeType type; + Circle circle; + Polygon polygon; + Pose pose; + +}; + + + + +class GridManager +{ + + public: + using Grid = Eigen::Array; + + // GridManager(std::vector grid, float resolution, uint32_t height, uint32_t width); + GridManager(float resolution, uint32_t height, uint32_t width); + ~GridManager() = default; + + const Grid& get_grid() const; + + + + void update_grid(int8_t* grid, const Eigen::Array& polygon, int value); + + std::tuple world_to_grid(float x, float y); + + + void handle_circle(int xc, int yc, int r, int value); + + void draw_circle(int xc, int yc, int x, int y, int value, Eigen::Block& block, int xmin, int ymin); + + + void handle_polygon(int8_t* grid, const Eigen::Array& vertices, int value); + + void draw_polygon(int x0, int y0, int x1, int y1, Eigen::Block& block, int value); + void draw_line(int x0, int y0, int x1, int y1, int8_t* grid, int value); + + + + + private: + Grid grid_; + float resolution_; + uint32_t height_; + uint32_t width_; + + +}; + + +} \ No newline at end of file diff --git a/mission/landmark_server/include/landmark_server/landmark_server.hpp b/mission/landmark_server/include/landmark_server/landmark_server.hpp index 799bbd7b..1cfa1d7a 100644 --- a/mission/landmark_server/include/landmark_server/landmark_server.hpp +++ b/mission/landmark_server/include/landmark_server/landmark_server.hpp @@ -2,9 +2,14 @@ #define LANDMARK_SERVER_HPP #include +#include #include #include #include +#include +#include +#include + #include #include @@ -17,6 +22,10 @@ #include #include +#include "nav_msgs/srv/get_map.hpp" + +#include + namespace landmark_server { @@ -63,6 +72,11 @@ class LandmarkServerNode : public rclcpp::Node { void landmarksRecievedCallback( const vortex_msgs::msg::LandmarkArray::SharedPtr msg); + +Eigen::Array get_convex_hull(const shape_msgs::msg::SolidPrimitive& solid_primitive); + +void get_grid(); + /** * @brief A shared pointer to a publisher for the LandmarkArray message type. * Publishes all landmarks currently stored in the server. @@ -76,12 +90,21 @@ class LandmarkServerNode : public rclcpp::Node { */ rclcpp::Publisher::SharedPtr posePublisher_; + rclcpp::Publisher::SharedPtr gridPublisher_; + + rclcpp::Publisher::SharedPtr convex_hull_publisher_; + rclcpp::Publisher::SharedPtr cluster_publisher_; + + /** * @brief A shared pointer to a LandmarkArray message. * The array contains all landmarks currently stored by the server. */ std::shared_ptr storedLandmarks_; + + nav_msgs::msg::OccupancyGrid grid_msg_; + /** * @brief A shared pointer to an rclcpp_action server for handling filtered * landmarks. @@ -89,6 +112,10 @@ class LandmarkServerNode : public rclcpp::Node { rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Client::SharedPtr grid_client_; + + std::unique_ptr grid_manager_ = nullptr; + /** * @brief Handles the goal request for the `handle_goal` function. * diff --git a/mission/landmark_server/launch/landmark_server_launch.py b/mission/landmark_server/launch/landmark_server_launch.py new file mode 100644 index 00000000..72a1d29a --- /dev/null +++ b/mission/landmark_server/launch/landmark_server_launch.py @@ -0,0 +1,26 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import (DeclareLaunchArgument) +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + default_params_file = os.path.join(get_package_share_directory('landmark_server'),'params','landmark_server_params.yaml') + params_file = LaunchConfiguration('params_file') + params_file_arg = DeclareLaunchArgument('params_file', + default_value=str( + default_params_file), + description='name or path to the parameters file to use.') + landmark_server_node = Node( + package='landmark_server', + executable='landmark_server_node', + name='landmark_server_node', + parameters=[params_file], + # arguments=['--ros-args', '--log-level', 'DEBUG'], + output='screen', + ) + return LaunchDescription([ + params_file_arg, + landmark_server_node + ]) \ No newline at end of file diff --git a/mission/landmark_server/package.xml b/mission/landmark_server/package.xml index 65fe7f54..0247154e 100644 --- a/mission/landmark_server/package.xml +++ b/mission/landmark_server/package.xml @@ -18,6 +18,9 @@ nav_msgs tf2 tf2_geometry_msgs + sensor_msgs + pcl_conversions + diff --git a/mission/landmark_server/params/landmark_server_params.yaml b/mission/landmark_server/params/landmark_server_params.yaml new file mode 100644 index 00000000..8455dd85 --- /dev/null +++ b/mission/landmark_server/params/landmark_server_params.yaml @@ -0,0 +1,4 @@ +landmark_server_node: + ros__parameters: + fixed_frame: world + target_frame: base_link \ No newline at end of file diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp new file mode 100644 index 00000000..44273f69 --- /dev/null +++ b/mission/landmark_server/src/grid_manager.cpp @@ -0,0 +1,196 @@ +#include +#include + +namespace landmark_server +{ + + // GridManager::GridManager(std::vector grid, float resolution, uint32_t height, uint32_t width) + // : resolution_(resolution), height_(height), width_(width) + // { + + // if (grid.size() != height * width) { + // throw std::runtime_error("Grid size does not match height * width"); + // } + // grid_ = Eigen::Map> + // (grid.data(), height, width); + // } + + GridManager::GridManager(float resolution, uint32_t height, uint32_t width) + : resolution_(resolution), height_(height), width_(width){} + + const GridManager::Grid& GridManager::get_grid() const { + return grid_; + } + + void GridManager::update_grid(int8_t* grid, const Eigen::Array& polygon, int value){ + handle_polygon(grid, polygon, value); + } + + + // void GridManager::update_grid(const ShapeInfo& shape_info, int value) + // { + // switch (shape_info.type) + // { + // case ShapeType::SPHERE: + // handle_circle(shape_info.circle.x_centre, shape_info.circle.y_centre, shape_info.circle.radius, value); + // break; + // case ShapeType::PRISM: + // handle_polygon(shape_info.polygon.vertices, value); + // default: + // break; + // } + // } + +void GridManager::draw_circle(int xc, int yc, int x, int y, int value, Eigen::Block& block, int xmin, int ymin) { + // Translate coordinates to block local coordinates + int bx = xc - xmin; + int by = yc - ymin; + + if (bx + x < block.rows() && by + y < block.cols() && bx + x >= 0 && by + y >= 0) { + block(bx + x, by + y) = value; + block(bx - x, by + y) = value; + block(bx + x, by - y) = value; + block(bx - x, by - y) = value; + block(bx + y, by + x) = value; + block(bx - y, by + x) = value; + block(bx + y, by - x) = value; + block(bx - y, by - x) = value; + } +} + +void GridManager::handle_circle(int xc, int yc, int r, int value) { + int xmin = static_cast((xc - r) / resolution_ + width_ / 2); + int ymin = static_cast((yc - r) / resolution_ + width_ / 2); + int xmax = static_cast((xc + r) / resolution_ + width_ / 2); + int ymax = static_cast((yc + r) / resolution_ + width_ / 2); + + if (xmin < 0 || ymin < 0 || xmax >= grid_.rows() || ymax >= grid_.cols()) { + throw std::runtime_error("Circle exceeds grid boundaries"); + } + + auto block = grid_.block(xmin, ymin, xmax - xmin + 1, ymax - ymin + 1); + int x = 0, y = r; + int d = 3 - 2 * r; + draw_circle(xc, yc, x, y, value, block, xmin, ymin); + while (y >= x) { + x++; + if (d > 0) { + y--; + d = d + 4 * (x - y) + 10; + } else { + d = d + 4 * x + 6; + } + draw_circle(xc, yc, x, y, value, block, xmin, ymin); + } +} + +void GridManager::handle_polygon(int8_t* grid, const Eigen::Array& vertices, int value) { + // Determine the bounding box of the polygon + // float min_x = vertices.row(0).minCoeff(); + // float min_y = vertices.row(1).minCoeff(); + // float max_x = vertices.row(0).maxCoeff(); + // float max_y = vertices.row(1).maxCoeff(); + + // int xmin = static_cast(min_x / resolution_ + width_ / 2); + // int ymin = static_cast(min_y / resolution_ + height_ / 2); + // int xmax = static_cast(max_x / resolution_ + width_ / 2); + // int ymax = static_cast(max_y / resolution_ + height_ / 2); + + // if (xmin < 0 || ymin < 0 || xmax >= height_ || ymax >= width_) { + // throw std::runtime_error("Polygon exceeds grid boundaries"); + // } + + // Draw the polygon + for (Eigen::Index i = 0; i < vertices.cols(); ++i) { + Eigen::Index j = (i + 1) % vertices.cols(); + std::cout << "i: " << i << " j: " << j << std::endl; + int x0 = static_cast(vertices(0, i) / resolution_ + width_ / 2); + int y0 = static_cast(vertices(1, i) / resolution_ + height_ / 2); + int x1 = static_cast(vertices(0, j) / resolution_ + width_ / 2); + int y1 = static_cast(vertices(1, j) / resolution_ + height_ / 2); + + draw_line(x0, y0, x1, y1, grid, value); + } +} + +void GridManager::draw_polygon(int x0, int y0, int x1, int y1, Eigen::Block& block, int value) { + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (steep) { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) { + std::swap(x0, x1); + std::swap(y0, y1); + } + int dx = x1 - x0; + int dy = std::abs(y1 - y0); + int error = 2 * dy - dx; + int ystep = (y0 < y1) ? 1 : -1; + int xbounds = steep ? block.rows() : block.cols(); + int ybounds = steep ? block.cols() : block.rows(); + int y = y0; + + for (int x = x0; x <= x1; x++) { + if (steep) { + if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { + block(y, x) += value; + } + } else { + if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { + block(x, y) += value; + } + } + if (error > 0) { + y += ystep; + error -= 2*(dx-dy); + } else { + error += 2 * dy; + } + } +} + +void GridManager::draw_line(int x0, int y0, int x1, int y1, int8_t* grid, int value) { + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (steep) { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) { + std::swap(x0, x1); + std::swap(y0, y1); + } + int dx = x1 - x0; + int dy = std::abs(y1 - y0); + int error = 2*dy - dx; + int ystep = (y0 < y1) ? 1 : -1; + int xbounds = steep ? height_ : width_; + int ybounds = steep ? width_ : height_; + int y = y0; + + for (int x = x0; x <= x1; x++) { + if (steep) { + if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { + grid[y + x * height_] += value; + } + } else { + if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { + grid[y * width_ + x] += value; + } + } + if(error > 0) + { + y += ystep; + error -= 2*(dx-dy); + } + else + { + error += 2*dy; + } + + } +} + + + +} \ No newline at end of file diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp index 2a8875b8..7959f323 100644 --- a/mission/landmark_server/src/landmark_server.cpp +++ b/mission/landmark_server/src/landmark_server.cpp @@ -1,4 +1,4 @@ -#include "landmark_server/landmark_server.hpp" +#include using std::placeholders::_1, std::placeholders::_2; using LandmarkArray = vortex_msgs::msg::LandmarkArray; @@ -27,11 +27,22 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) posePublisher_ = this->create_publisher( "landmark_poses_out", qos); + gridPublisher_ = this->create_publisher( + "grid", qos); + + rmw_qos_profile_t qos_profile_sensor = rmw_qos_profile_sensor_data; + qos_profile_sensor.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + auto qos_sensor = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor.history, 1), qos_profile_sensor); + + convex_hull_publisher_ = this->create_publisher("landmark/hull", qos_sensor); + cluster_publisher_ = this->create_publisher("landmark/cluster", qos_sensor); + + tf2_buffer_ = std::make_shared(this->get_clock()); tf2_listener_ = std::make_shared(*tf2_buffer_); + declare_parameter("fixed_frame", "world"); declare_parameter("target_frame", "base_link"); - declare_parameter("world_frame", "world_frame"); // Create the act. action_server_ = rclcpp_action::create_server( @@ -39,8 +50,81 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) std::bind(&LandmarkServerNode::handle_goal, this, _1, _2), std::bind(&LandmarkServerNode::handle_cancel, this, _1), std::bind(&LandmarkServerNode::handle_accepted, this, _1)); + + grid_client_ = create_client("get_map"); + + std::thread(&LandmarkServerNode::get_grid, this).detach(); +} + + +Eigen::Array LandmarkServerNode::get_convex_hull(const shape_msgs::msg::SolidPrimitive& solid_primitive) { + pcl::PointCloud cluster; + cluster.resize(solid_primitive.polygon.points.size()); + for (size_t i = 0; i < solid_primitive.polygon.points.size(); i++) { + cluster.points[i].x = solid_primitive.polygon.points[i].x; + cluster.points[i].y = solid_primitive.polygon.points[i].y; + cluster.points[i].z = 1.0; + } + sensor_msgs::msg::PointCloud2 cluster_msg; + pcl::toROSMsg(cluster, cluster_msg); + cluster_msg.header.frame_id = "world"; + cluster_publisher_->publish(cluster_msg); + pcl::PointCloud convex_hull; + pcl::ConvexHull chull; + chull.setDimension(2); + chull.setInputCloud(cluster.makeShared()); + chull.reconstruct(convex_hull); + sensor_msgs::msg::PointCloud2 hull_msg; + pcl::toROSMsg(convex_hull, hull_msg); + hull_msg.header.frame_id = "world"; + convex_hull_publisher_->publish(hull_msg); + RCLCPP_INFO(this->get_logger(), "Convex hull size: %ld", convex_hull.size()); + Eigen::Array hull(2, convex_hull.size()); + for (size_t i = 0; i < convex_hull.size(); i++) { + hull(0, i) = convex_hull.points[i].x; + hull(1, i) = convex_hull.points[i].y; + } + RCLCPP_INFO(this->get_logger(), "Convex hull: %ld", hull.cols()); + std::cout << "HUll" << hull << std::endl; + return hull; } +void LandmarkServerNode::get_grid(){ + while(true){ + rclcpp::sleep_for(std::chrono::seconds(1)); + if (!grid_client_->wait_for_service(std::chrono::seconds(5))) { + RCLCPP_ERROR(this->get_logger(), "Service not available after waiting"); + continue; + } + + auto request = std::make_shared(); + auto result_future = grid_client_->async_send_request(request); + + // Wait for the result within a specified timeout period + auto status = result_future.wait_for(std::chrono::seconds(5)); + if (status == std::future_status::ready) { + try { + auto result = result_future.get(); + if(result->map.data.empty()){ + RCLCPP_ERROR(this->get_logger(), "Received empty map from grid client"); + continue; + } + grid_msg_ = result->map; + + grid_manager_ = std::make_unique(grid_msg_.info.resolution, grid_msg_.info.height, grid_msg_.info.width); + RCLCPP_INFO(this->get_logger(), "Successfully received map from grid client"); + return; + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), "Exception while getting result from future: %s", e.what()); + } + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to get map from grid client within timeout period"); + continue; + } + } +} + + void LandmarkServerNode::landmarksRecievedCallback( const LandmarkArray::SharedPtr msg) { if (msg->landmarks.empty()) { @@ -48,44 +132,67 @@ void LandmarkServerNode::landmarksRecievedCallback( "Received empty landmark array"); return; } + + if (grid_manager_ == nullptr) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Grid manager not initialized"); + return; + } for (const auto &landmark : msg->landmarks) { - - if (landmark.action == 0) { - // Remove landmarks with matching id and landmark_type - storedLandmarks_->landmarks.erase( - std::remove_if(storedLandmarks_->landmarks.begin(), - storedLandmarks_->landmarks.end(), - [&](const auto &storedLandmark) { - return storedLandmark.id == landmark.id && - storedLandmark.landmark_type == - landmark.landmark_type; - }), - storedLandmarks_->landmarks.end()); - } else if (landmark.action == 1) { - // Find the landmark if it already exists - auto it = std::find_if( + auto it = std::find_if( storedLandmarks_->landmarks.begin(), storedLandmarks_->landmarks.end(), [&](const auto &storedLandmark) { return storedLandmark.landmark_type == landmark.landmark_type && storedLandmark.id == landmark.id; }); + if(landmark.action == vortex_msgs::msg::Landmark::ADD_ACTION) { if (it != storedLandmarks_->landmarks.end()) { - // Update the existing landmark - *it = landmark; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Requested to add already existing landmark"); } else { // Add the new landmark + grid_manager_->update_grid(grid_msg_.data.data(), get_convex_hull(landmark.shape), 200); storedLandmarks_->landmarks.push_back(landmark); } + continue; } - } + + if (it == storedLandmarks_->landmarks.end()) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Requested to remove or update non-existing landmark"); + continue; + } + + grid_manager_->update_grid(grid_msg_.data.data(), get_convex_hull((*it).shape), -200); + + if(landmark.action == vortex_msgs::msg::Landmark::REMOVE_ACTION){ + storedLandmarks_->landmarks.erase(it); + } else if(landmark.action == vortex_msgs::msg::Landmark::UPDATE_ACTION){ + grid_manager_->update_grid(grid_msg_.data.data(), get_convex_hull(landmark.shape), 200); + *it = landmark; + } + } + std_msgs::msg::Header header; + + // header.frame_id = get_parameter("fixed_frame").as_string(); + // header.stamp = rclcpp::Clock().now(); + + // // Convert the Eigen array to std::vector + // auto grid_eigen = grid_manager_->get_grid(); + // std::vector grid_data(grid_eigen.data(), grid_eigen.data() + grid_eigen.size()); + // grid_msg_.header = header; + // grid_msg_.data = grid_data; + grid_msg_.header.stamp = rclcpp::Clock().now(); // Publish the landmarks + gridPublisher_->publish(grid_msg_); landmarkPublisher_->publish(*storedLandmarks_); posePublisher_->publish(poseArrayCreater(*storedLandmarks_)); } + geometry_msgs::msg::PoseArray LandmarkServerNode::poseArrayCreater( vortex_msgs::msg::LandmarkArray landmarks) { @@ -94,8 +201,7 @@ geometry_msgs::msg::PoseArray LandmarkServerNode::poseArrayCreater( if (!landmarks.landmarks.empty()) { poseArray.header.frame_id = landmarks.landmarks.at(0).odom.header.frame_id; } else { - poseArray.header.frame_id = get_parameter("world_frame").as_string(); - poseArray.header.stamp = rclcpp::Clock().now(); + poseArray.header.frame_id = get_parameter("fixed_frame").as_string(); } // Timestamps for stored landmarks may vary so use current time for // visualization @@ -173,8 +279,7 @@ void LandmarkServerNode::execute( continue; } - vortex_msgs::msg::OdometryArray filteredLandmarksOdoms = - filterLandmarks(goal_handle); + vortex_msgs::msg::OdometryArray filteredLandmarksOdoms = filterLandmarks(goal_handle); if (filteredLandmarksOdoms.odoms.empty()) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "No landmarks found matching the request"); @@ -199,32 +304,29 @@ vortex_msgs::msg::OdometryArray LandmarkServerNode::filterLandmarks( goal_handle) { const auto goal = goal_handle->get_goal(); _Float32 distance = goal->distance; + uint8_t filter_type = goal->landmark_types; vortex_msgs::msg::OdometryArray filteredLandmarksOdoms; for (const auto &landmark : storedLandmarks_->landmarks) { - if (goal->landmark_types.empty() && distance == 0.0) { + if (filter_type == 6 && distance == 0.0) { filteredLandmarksOdoms.odoms.push_back(landmark.odom); - } else if (goal->landmark_types.empty()) { + } else if (filter_type == 6) { if (calculateDistance(landmark.odom.pose.pose.position, landmark.odom.header) <= distance) { filteredLandmarksOdoms.odoms.push_back(landmark.odom); } } else if (distance == 0.0) { - for (const auto &type : goal->landmark_types) { - if (landmark.landmark_type == type) { + if (landmark.landmark_type == filter_type) { filteredLandmarksOdoms.odoms.push_back(landmark.odom); } - } } else { - for (const auto &type : goal->landmark_types) { - if (landmark.landmark_type == type && + if (landmark.landmark_type == filter_type && calculateDistance(landmark.odom.pose.pose.position, landmark.odom.header) <= distance) { filteredLandmarksOdoms.odoms.push_back(landmark.odom); } - } } } return filteredLandmarksOdoms; @@ -237,13 +339,13 @@ void LandmarkServerNode::requestLogger( const auto goal = goal_handle->get_goal(); double distance = goal->distance; - if (distance == 0.0 && goal->landmark_types.empty()) { + if (distance == 0.0 && goal->landmark_types == vortex_msgs::msg::Landmark::NONE) { RCLCPP_INFO_STREAM(this->get_logger(), "Received request to return all landmarks."); return; } - if (goal->landmark_types.empty()) { + if (goal->landmark_types == vortex_msgs::msg::Landmark::NONE) { RCLCPP_INFO_STREAM( this->get_logger(), "Received request to return all landmarks within distance " << distance @@ -253,12 +355,14 @@ void LandmarkServerNode::requestLogger( std::stringstream types_log; types_log << "Received request to return landmarks by type filter: ["; - for (auto it = goal->landmark_types.begin(); it != goal->landmark_types.end(); - it++) { - types_log << *it; - if (std::next(it) != goal->landmark_types.end()) { - types_log << ", "; - } + if (goal->landmark_types & vortex_msgs::msg::Landmark::BUOY) { + types_log << "BUOY, "; + } + if (goal->landmark_types & vortex_msgs::msg::Landmark::BOAT) { + types_log << "BOAT, "; + } + if (goal->landmark_types & vortex_msgs::msg::Landmark::WALL) { + types_log << "WALL, "; } types_log << "]."; RCLCPP_INFO_STREAM(this->get_logger(), types_log.str()); @@ -274,7 +378,7 @@ LandmarkServerNode::calculateDistance(const geometry_msgs::msg::Point &point, try { // Lookup the transformation geometry_msgs::msg::TransformStamped transform_stamped = - tf2_buffer_->lookupTransform("base_link", header.frame_id, header.stamp, + tf2_buffer_->lookupTransform(target_frame, header.frame_id, header.stamp, rclcpp::Duration(1, 0)); // Transform the point cloud diff --git a/mission/map_manager/CMakeLists.txt b/mission/map_manager/CMakeLists.txt new file mode 100644 index 00000000..aa641539 --- /dev/null +++ b/mission/map_manager/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.8) +project(map_manager) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(PCL REQUIRED) + +include_directories(include) + +add_executable(map_manager_node src/map_manager_ros.cpp + src/map_manager_node.cpp +) + +target_link_libraries( ${PROJECT_NAME}_node + pcl_common) + +ament_target_dependencies( + map_manager_node + rclcpp + geometry_msgs + nav_msgs + sensor_msgs +) + +install( + DIRECTORY include/ + DESTINATION include + +) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME}/ + ) + + install(DIRECTORY + launch + params + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/mission/map_manager/include/map_manager/map_manager_ros.hpp b/mission/map_manager/include/map_manager/map_manager_ros.hpp new file mode 100644 index 00000000..8a506fde --- /dev/null +++ b/mission/map_manager/include/map_manager/map_manager_ros.hpp @@ -0,0 +1,83 @@ +#ifndef MAP_MANAGER_HPP +#define MAP_MANAGER_HPP + +#include +#include +#include +#include "nav_msgs/srv/get_map.hpp" +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + + + +namespace map_manager { + +/** + * @class MapManagerNode + * @brief A class representing a node for handling maps in a ROS 2 system. + * + * This class inherits from rclcpp::Node and provides functionality for + * receiving map messages and storing them in a map. + */ +class MapManagerNode : public rclcpp::Node { +public: + /** + * @brief Constructor for the MapManagerNode class. + * + * @param options The options for configuring the node. + */ + explicit MapManagerNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + + /** + * @brief Destructor for the MapManagerNode class. + */ + ~MapManagerNode(){}; + +private: + + constexpr double deg2rad(double degrees) const; + std::array lla2flat(double lat, double lon) const; + std::array flat2lla(double px, double py) const; + + geometry_msgs::msg::PolygonStamped processCoordinates(const std::vector>& coordinates); + geometry_msgs::msg::PolygonStamped readPolygonFromFile(const std::string& filename); + void mapOriginCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + nav_msgs::msg::OccupancyGrid createOccupancyGrid(); + geometry_msgs::msg::Pose calculate_map_origin(); + void insert_landmask(nav_msgs::msg::OccupancyGrid& grid, const geometry_msgs::msg::PolygonStamped& polygon); + void handle_get_map_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + void fillOutsidePolygon(nav_msgs::msg::OccupancyGrid& grid, const geometry_msgs::msg::PolygonStamped& polygon); + + + + + + rclcpp::Subscription::SharedPtr map_origin_sub_; + rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr landmask_pub_; + rclcpp::Service::SharedPtr grid_service_; + bool map_origin_set_ = false; + bool use_predef_landmask_; + double map_origin_lat_; + double map_origin_lon_; + std::string landmask_file_; + std::string grid_frame_id_; + +}; + +} // namespace map_manager + +#endif // MAP_MANAGER_HPP \ No newline at end of file diff --git a/mission/map_manager/launch/map_manager_launch.py b/mission/map_manager/launch/map_manager_launch.py new file mode 100644 index 00000000..d7ee6dfa --- /dev/null +++ b/mission/map_manager/launch/map_manager_launch.py @@ -0,0 +1,26 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import (DeclareLaunchArgument) +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + default_params_file = os.path.join(get_package_share_directory('map_manager'),'params','map_manager_params.yaml') + params_file = LaunchConfiguration('params_file') + params_file_arg = DeclareLaunchArgument('params_file', + default_value=str( + default_params_file), + description='name or path to the parameters file to use.') + map_manager_node = Node( + package='map_manager', + executable='map_manager_node', + name='map_manager_node', + parameters=[params_file], + # arguments=['--ros-args', '--log-level', 'DEBUG'], + output='screen', + ) + return LaunchDescription([ + params_file_arg, + map_manager_node + ]) \ No newline at end of file diff --git a/mission/map_manager/package.xml b/mission/map_manager/package.xml new file mode 100644 index 00000000..6db8a033 --- /dev/null +++ b/mission/map_manager/package.xml @@ -0,0 +1,24 @@ + + + + map_manager + 0.0.0 + TODO: Package description + jorgen + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + geometry_msgs + nav_msgs + sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mission/map_manager/params/map_manager_params.yaml b/mission/map_manager/params/map_manager_params.yaml new file mode 100644 index 00000000..61b315b2 --- /dev/null +++ b/mission/map_manager/params/map_manager_params.yaml @@ -0,0 +1,8 @@ +map_manager_node: + ros__parameters: + use_predef_landmask: true + landmask_file: "src/vortex-asv/mission/map_manager/params/land_polygon.yaml" + map_resolution: 0.2 + map_width: 1000 + map_height: 1000 + frame_id: "world" diff --git a/mission/map_manager/src/map_manager_node.cpp b/mission/map_manager/src/map_manager_node.cpp new file mode 100644 index 00000000..420dca0c --- /dev/null +++ b/mission/map_manager/src/map_manager_node.cpp @@ -0,0 +1,14 @@ +#include +#include + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp new file mode 100644 index 00000000..303a68d2 --- /dev/null +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -0,0 +1,340 @@ +#include + +namespace map_manager { + +MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) + : Node("map_manager_node", options) { + + // rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + // auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; + qos_profile_transient_local.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + auto qos_transient_local = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), qos_profile_transient_local); + + map_origin_sub_ = this->create_subscription( + "map_origin", qos_transient_local,std::bind(&MapManagerNode::mapOriginCallback, this, std::placeholders::_1)); + + declare_parameter("use_predef_landmask", false); + declare_parameter("landmask_file", "src/vortex-asv/mission/map_manager/params/land_polygon.yaml"); + declare_parameter("map_resolution", 0.2); + declare_parameter("map_width", 1000); + declare_parameter("map_height", 1000); + declare_parameter("map_frame_id", "world"); + + landmask_file_ = get_parameter("landmask_file").as_string(); + landmask_pub_ = this->create_publisher("landmask", qos_transient_local); + // traffic centre + // map_origin_lat_ = 63.41490901857848; + // map_origin_lon_ = 10.398215601285054; + // office bag still + // map_origin_lat_ = 63.414660884931976; + // map_origin_lon_ = 10.398554661537544; + // map_origin_set_ = true; + // auto grid = createOccupancyGrid(); + // auto polygon = readPolygonFromFile(landmask_file_); + // landmask_pub_->publish(polygon); + // fillOutsidePolygon(grid, polygon); + // insert_landmask(grid, polygon); + + // map_pub_ = this->create_publisher("map", qos_transient_local); + // map_pub_->publish(grid); + + + + grid_service_ = this->create_service( + "get_map", std::bind( + &MapManagerNode::handle_get_map_request, // Callback function + this, // Pointer to the object + std::placeholders::_1, // Placeholder for request header + std::placeholders::_2, // Placeholder for request + std::placeholders::_3 // Placeholder for response + )); + + } + + void MapManagerNode::mapOriginCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) + { + map_origin_lat_ = msg->latitude; + map_origin_lon_ = msg->longitude; + map_origin_set_ = true; + map_origin_sub_ = nullptr; + + landmask_pub_->publish(readPolygonFromFile(landmask_file_)); + } + + void MapManagerNode::handle_get_map_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + if(!map_origin_set_){ + RCLCPP_WARN(this->get_logger(), "Map origin not set, cannot provide map"); + return; + } + // Your logic to fill the map data + RCLCPP_INFO(this->get_logger(), "Received request for map"); + + // Example: You could fill this with actual map data + nav_msgs::msg::OccupancyGrid grid = createOccupancyGrid(); + auto polygon = readPolygonFromFile(landmask_file_); + fillOutsidePolygon(grid, polygon); + insert_landmask(grid, polygon); + response->map = grid; + RCLCPP_INFO(this->get_logger(), "Map sent"); + + } + + constexpr double MapManagerNode::deg2rad(double degrees) const{ + return degrees * M_PI / 180.0; + } + + std::array MapManagerNode::lla2flat(double lat, double lon) const + { + const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) + const double f = 1.0 / 298.257223563; // Flattening of the earth + const double psi_rad = 0.0; // Angular direction of the flat Earth x-axis, specified as a scalar. + // The angular direction is the degrees clockwise from north, + // which is the angle in degrees used for converting flat Earth x and y coordinates to the north and east coordinates + + // Convert angles from degrees to radians + const double lat_rad = deg2rad(lat); + const double lon_rad = deg2rad(lon); + const double origin_lat_rad = deg2rad(map_origin_lat_); + const double origin_lon_rad = deg2rad(map_origin_lon_); + + // Calculate delta latitude and delta longitude in radians + const double dlat = lat_rad - origin_lat_rad; + const double dlon = lon_rad - origin_lon_rad; + + // Radius of curvature in the vertical prime (RN) + const double RN = R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Radius of curvature in the meridian (RM) + const double RM = RN * (1.0 - (2.0 * f - f * f)) / (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Changes in the north (dN) and east (dE) positions + const double dN = RM * dlat; + const double dE = RN * cos(origin_lat_rad) * dlon; + + // Transformation from North-East to flat x-y coordinates + const double px = cos(psi_rad) * dN - sin(psi_rad) * dE; + const double py = sin(psi_rad) * dN + cos(psi_rad) * dE; + + return {px, py}; + } + + std::array MapManagerNode::flat2lla(double px, double py) const + { + // Earth constants + const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) + const double f = 1.0 / 298.257223563; // Flattening of the earth + + // Convert origin from degrees to radians + const double origin_lat_rad = deg2rad(map_origin_lat_); + const double origin_lon_rad = deg2rad(map_origin_lon_); + + // Radius of curvature in the vertical prime (RN) and in the meridian (RM) + const double RN = R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + const double RM = RN * (1.0 - (2.0 * f - f * f)) / (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Calculate changes in latitude and longitude + double dN = px; // Change in north direction + double dE = py; // Change in east direction + double dlat = dN / RM; + double dlon = dE / (RN * cos(origin_lat_rad)); + + // Convert delta lat and lon from radians back to degrees + double new_lat = (origin_lat_rad + dlat) * 180.0 / M_PI; + double new_lon = (origin_lon_rad + dlon) * 180.0 / M_PI; + + return {new_lat, new_lon}; +} + + + geometry_msgs::msg::PolygonStamped MapManagerNode::readPolygonFromFile(const std::string& filename) { + std::ifstream file(filename); + if (!file.is_open()) { + throw std::runtime_error("Could not open file: " + filename); + } + + std::string line; + std::vector> coords; + while (std::getline(file, line)) { + std::stringstream ss(line); + double lat, lon; + if (ss >> lat >> lon) { + coords.push_back({lat, lon}); + } else { + std::cerr << "Failed to parse line: " << line << std::endl; + } + } + + file.close(); + std::cout << "coords size: " << coords.size() << "\n"; + + return processCoordinates(coords); + } + + geometry_msgs::msg::PolygonStamped MapManagerNode::processCoordinates(const std::vector>& coordinates) + { + geometry_msgs::msg::PolygonStamped polygon; + polygon.header.frame_id = get_parameter("map_frame_id").as_string(); + polygon.header.stamp = this->now(); + std::cout << "coords size: " << coordinates.size() << "\n"; + for (const auto& coord : coordinates) { + std::array flat = lla2flat(coord[0], coord[1]); + geometry_msgs::msg::Point32 point; + point.x = flat[0]; + point.y = flat[1]; + point.z = 0; + polygon.polygon.points.push_back(point); + } + std::cout << "Polygon size: " << polygon.polygon.points.size() << std::endl; + return polygon; + } + + nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() + { + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = get_parameter("map_frame_id").as_string(); + grid.header.stamp = this->now(); + grid.info.resolution = get_parameter("map_resolution").as_double(); + grid.info.width = get_parameter("map_width").as_int(); + grid.info.height = get_parameter("map_height").as_int(); + grid.info.origin = calculate_map_origin(); + grid.info.map_load_time = this->now(); + // 0 represents unoccupied, 1 represents definitely occupied, and + // -1 represents unknown. + // Set all to unoccupied + grid.data.resize(grid.info.width * grid.info.height, 0); + return grid; + } + + geometry_msgs::msg::Pose MapManagerNode::calculate_map_origin() { + // 63.44098584906518, 10.42304390021551 + // 63.41490901857848, 10.398215601285054 + // Office traffic centre origin + // map_origin_lat_ = 63.41490901857848; + // map_origin_lon_ = 10.398215601285054; + + // Map centre is (0,0) in map frame, origin is bottom left corner + double half_width_meters = -(get_parameter("map_width").as_int() * get_parameter("map_resolution").as_double()) / 2.0; + double half_height_meters = -(get_parameter("map_height").as_int() * get_parameter("map_resolution").as_double()) / 2.0; + // auto [lat, lon] = flat2lla(half_width_meters, half_height_meters); + geometry_msgs::msg::Pose map_origin; + map_origin.position.x = half_width_meters; + map_origin.position.y = half_height_meters; + map_origin.position.z = 0.0; + map_origin.orientation.x = 0.0; + map_origin.orientation.y = 0.0; + map_origin.orientation.z = 0.0; + map_origin.orientation.w = 1.0; + return map_origin; + } + + +// Helper function to draw line using Bresenham's algorithm +void drawLine(int x0, int y0, int x1, int y1, nav_msgs::msg::OccupancyGrid& grid, int value) { + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (steep) { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) { + std::swap(x0, x1); + std::swap(y0, y1); + } + int dx = x1 - x0; + int dy = std::abs(y1 - y0); + int error = 2*dy - dx; + int ystep = (y0 < y1) ? 1 : -1; + int xbounds = steep ? grid.info.height : grid.info.width; + int ybounds = steep ? grid.info.width : grid.info.height; + int y = y0; + + for (int x = x0; x <= x1; x++) { + if (steep) { + if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { + grid.data[y + x * grid.info.width] = value; + } + } else { + if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { + grid.data[y * grid.info.width + x] = value; + } + } + if(error > 0) + { + y += ystep; + error -= 2*(dx-dy); + } + else + { + error += 2*dy; + } + + } +} + +bool isPointInsidePolygon(int px, int py, const geometry_msgs::msg::PolygonStamped& polygon, const nav_msgs::msg::OccupancyGrid& grid) { + int count = 0; + int n = polygon.polygon.points.size(); + for (int i = 0; i < n; i++) { + const geometry_msgs::msg::Point32& start = polygon.polygon.points[i]; + const geometry_msgs::msg::Point32& end = polygon.polygon.points[(i + 1) % n]; + int x0 = start.x / grid.info.resolution + grid.info.width / 2; + int y0 = start.y / grid.info.resolution + grid.info.height / 2; + int x1 = end.x / grid.info.resolution + grid.info.width / 2; + int y1 = end.y / grid.info.resolution + grid.info.height / 2; + + // Check if the line from start to end intersects with the line from (px, py) to (infinity, py) + if ((y0 > py) != (y1 > py)) { // If the point is between the y bounds of the segment + // Calculate x coordinate of the intersection point of the line segment with the horizontal line through py + float intersectX = (y1 - y0 != 0) ? (x1 - x0) * (float)(py - y0) / (float)(y1 - y0) + x0 : x0; + if (px < intersectX) // Ray crossing from the left + count++; + } + } + return (count % 2) == 1; // Odd inside, even outside +} + +void MapManagerNode::fillOutsidePolygon(nav_msgs::msg::OccupancyGrid& grid, const geometry_msgs::msg::PolygonStamped& polygon) { + int outside_value = 50; // Set occupancy value for outside (0-100) + + // Iterate over each pixel in the grid + for (int x = 0; x < grid.info.width; x++) { + for (int y = 0; y < grid.info.height; y++) { + if (!isPointInsidePolygon(x, y, polygon, grid)) { + grid.data[y * grid.info.width + x] = outside_value; + } + } + } +} + + +void MapManagerNode::insert_landmask(nav_msgs::msg::OccupancyGrid& grid, const geometry_msgs::msg::PolygonStamped& polygon) + { + int value = 100; // Set this to the desired occupancy value for land (0-100) + for (uint i = 0; i < polygon.polygon.points.size(); i++) { + + // if(i > 1){break;} + const geometry_msgs::msg::Point32& current = polygon.polygon.points[i]; + const geometry_msgs::msg::Point32& next = polygon.polygon.points[(i + 1) % polygon.polygon.points.size()]; // Loop back to the first point + // std::cout << "current: " << current.x << " " << current.y << " next: " << next.x << " " << next.y << "\n"; + + // Convert map xy to grid indices + int x0 = current.x / grid.info.resolution + grid.info.width / 2; + int y0 = current.y / grid.info.resolution + grid.info.height / 2; + int x1 = next.x / grid.info.resolution + grid.info.width / 2; + int y1 = next.y / grid.info.resolution + grid.info.height / 2; + // std::cout << "x0: " << x0 << " y0: " << y0 << " x1: " << x1 << " y1: " << y1 << "\n"; + // grid.data[x0 + y0 * grid.info.width] = value; + + + // Draw line on the grid + drawLine(x0, y0, x1, y1, grid, value); + } + +} + + +} // namespace map_manager \ No newline at end of file diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml index b5dde0d1..018e9192 100644 --- a/mission/waypoint_manager/package.xml +++ b/mission/waypoint_manager/package.xml @@ -12,7 +12,7 @@ rclpy geometry_msgs ros2launch - rnav_msgs + nav_msgs vortex_msgs ament_lint_auto From 18024e5ce81bb5712a4b79a6b4fcc8636507acaf Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Sat, 13 Jul 2024 13:49:23 +0000 Subject: [PATCH 11/67] Committing clang-format changes --- .../include/landmark_server/grid_manager.hpp | 123 ++-- .../landmark_server/landmark_server.hpp | 26 +- mission/landmark_server/src/grid_manager.cpp | 342 +++++----- .../landmark_server/src/landmark_server.cpp | 220 ++++--- .../include/map_manager/map_manager_ros.hpp | 65 +- mission/map_manager/src/map_manager_ros.cpp | 617 +++++++++--------- 6 files changed, 706 insertions(+), 687 deletions(-) diff --git a/mission/landmark_server/include/landmark_server/grid_manager.hpp b/mission/landmark_server/include/landmark_server/grid_manager.hpp index 93327dcb..78f09706 100644 --- a/mission/landmark_server/include/landmark_server/grid_manager.hpp +++ b/mission/landmark_server/include/landmark_server/grid_manager.hpp @@ -1,96 +1,81 @@ -#include +#include #include #include -#include +#include -namespace landmark_server -{ +namespace landmark_server { -enum class ShapeType : uint8_t -{ - SPHERE = 0, - PRISM = 1 -}; +enum class ShapeType : uint8_t { SPHERE = 0, PRISM = 1 }; -struct Circle -{ - float radius; - float x_centre; - float y_centre; +struct Circle { + float radius; + float x_centre; + float y_centre; }; -struct Point -{ - float x; - float y; - float z; +struct Point { + float x; + float y; + float z; }; -struct Polygon -{ - std::vector vertices; +struct Polygon { + std::vector vertices; }; -struct Pose -{ - float x; - float y; - float yaw; +struct Pose { + float x; + float y; + float yaw; }; - -struct ShapeInfo -{ - ShapeType type; - Circle circle; - Polygon polygon; - Pose pose; - +struct ShapeInfo { + ShapeType type; + Circle circle; + Polygon polygon; + Pose pose; }; +class GridManager { +public: + using Grid = Eigen::Array; + // GridManager(std::vector grid, float resolution, uint32_t height, + // uint32_t width); + GridManager(float resolution, uint32_t height, uint32_t width); + ~GridManager() = default; -class GridManager -{ - - public: - using Grid = Eigen::Array; - - // GridManager(std::vector grid, float resolution, uint32_t height, uint32_t width); - GridManager(float resolution, uint32_t height, uint32_t width); - ~GridManager() = default; - - const Grid& get_grid() const; + const Grid &get_grid() const; + void update_grid(int8_t *grid, + const Eigen::Array &polygon, + int value); + std::tuple world_to_grid(float x, float y); - void update_grid(int8_t* grid, const Eigen::Array& polygon, int value); + void handle_circle(int xc, int yc, int r, int value); - std::tuple world_to_grid(float x, float y); + void draw_circle( + int xc, int yc, int x, int y, int value, + Eigen::Block &block, + int xmin, int ymin); - - void handle_circle(int xc, int yc, int r, int value); - - void draw_circle(int xc, int yc, int x, int y, int value, Eigen::Block& block, int xmin, int ymin); - - - void handle_polygon(int8_t* grid, const Eigen::Array& vertices, int value); - - void draw_polygon(int x0, int y0, int x1, int y1, Eigen::Block& block, int value); - void draw_line(int x0, int y0, int x1, int y1, int8_t* grid, int value); - - - - - private: - Grid grid_; - float resolution_; - uint32_t height_; - uint32_t width_; + void handle_polygon(int8_t *grid, + const Eigen::Array &vertices, + int value); + void draw_polygon( + int x0, int y0, int x1, int y1, + Eigen::Block &block, + int value); + void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value); +private: + Grid grid_; + float resolution_; + uint32_t height_; + uint32_t width_; }; - -} \ No newline at end of file +} // namespace landmark_server \ No newline at end of file diff --git a/mission/landmark_server/include/landmark_server/landmark_server.hpp b/mission/landmark_server/include/landmark_server/landmark_server.hpp index 1cfa1d7a..7c3115a6 100644 --- a/mission/landmark_server/include/landmark_server/landmark_server.hpp +++ b/mission/landmark_server/include/landmark_server/landmark_server.hpp @@ -1,15 +1,14 @@ #ifndef LANDMARK_SERVER_HPP #define LANDMARK_SERVER_HPP -#include +#include #include +#include +#include +#include #include #include #include -#include -#include -#include - #include #include @@ -20,13 +19,12 @@ #include #include +#include "nav_msgs/srv/get_map.hpp" #include #include -#include "nav_msgs/srv/get_map.hpp" #include - namespace landmark_server { /** @@ -72,10 +70,10 @@ class LandmarkServerNode : public rclcpp::Node { void landmarksRecievedCallback( const vortex_msgs::msg::LandmarkArray::SharedPtr msg); + Eigen::Array + get_convex_hull(const shape_msgs::msg::SolidPrimitive &solid_primitive); -Eigen::Array get_convex_hull(const shape_msgs::msg::SolidPrimitive& solid_primitive); - -void get_grid(); + void get_grid(); /** * @brief A shared pointer to a publisher for the LandmarkArray message type. @@ -92,9 +90,10 @@ void get_grid(); rclcpp::Publisher::SharedPtr gridPublisher_; - rclcpp::Publisher::SharedPtr convex_hull_publisher_; - rclcpp::Publisher::SharedPtr cluster_publisher_; - + rclcpp::Publisher::SharedPtr + convex_hull_publisher_; + rclcpp::Publisher::SharedPtr + cluster_publisher_; /** * @brief A shared pointer to a LandmarkArray message. @@ -102,7 +101,6 @@ void get_grid(); */ std::shared_ptr storedLandmarks_; - nav_msgs::msg::OccupancyGrid grid_msg_; /** diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp index 44273f69..492c129d 100644 --- a/mission/landmark_server/src/grid_manager.cpp +++ b/mission/landmark_server/src/grid_manager.cpp @@ -1,196 +1,200 @@ -#include #include +#include -namespace landmark_server -{ +namespace landmark_server { - // GridManager::GridManager(std::vector grid, float resolution, uint32_t height, uint32_t width) - // : resolution_(resolution), height_(height), width_(width) - // { +// GridManager::GridManager(std::vector grid, float resolution, uint32_t +// height, uint32_t width) +// : resolution_(resolution), height_(height), width_(width) +// { - // if (grid.size() != height * width) { - // throw std::runtime_error("Grid size does not match height * width"); - // } - // grid_ = Eigen::Map> - // (grid.data(), height, width); - // } +// if (grid.size() != height * width) { +// throw std::runtime_error("Grid size does not match height * width"); +// } +// grid_ = Eigen::Map> +// (grid.data(), height, width); +// } - GridManager::GridManager(float resolution, uint32_t height, uint32_t width) - : resolution_(resolution), height_(height), width_(width){} +GridManager::GridManager(float resolution, uint32_t height, uint32_t width) + : resolution_(resolution), height_(height), width_(width) {} - const GridManager::Grid& GridManager::get_grid() const { - return grid_; - } - - void GridManager::update_grid(int8_t* grid, const Eigen::Array& polygon, int value){ - handle_polygon(grid, polygon, value); - } +const GridManager::Grid &GridManager::get_grid() const { return grid_; } +void GridManager::update_grid( + int8_t *grid, const Eigen::Array &polygon, + int value) { + handle_polygon(grid, polygon, value); +} - // void GridManager::update_grid(const ShapeInfo& shape_info, int value) - // { - // switch (shape_info.type) - // { - // case ShapeType::SPHERE: - // handle_circle(shape_info.circle.x_centre, shape_info.circle.y_centre, shape_info.circle.radius, value); - // break; - // case ShapeType::PRISM: - // handle_polygon(shape_info.polygon.vertices, value); - // default: - // break; - // } - // } - -void GridManager::draw_circle(int xc, int yc, int x, int y, int value, Eigen::Block& block, int xmin, int ymin) { - // Translate coordinates to block local coordinates - int bx = xc - xmin; - int by = yc - ymin; - - if (bx + x < block.rows() && by + y < block.cols() && bx + x >= 0 && by + y >= 0) { - block(bx + x, by + y) = value; - block(bx - x, by + y) = value; - block(bx + x, by - y) = value; - block(bx - x, by - y) = value; - block(bx + y, by + x) = value; - block(bx - y, by + x) = value; - block(bx + y, by - x) = value; - block(bx - y, by - x) = value; - } +// void GridManager::update_grid(const ShapeInfo& shape_info, int value) +// { +// switch (shape_info.type) +// { +// case ShapeType::SPHERE: +// handle_circle(shape_info.circle.x_centre, shape_info.circle.y_centre, +// shape_info.circle.radius, value); break; +// case ShapeType::PRISM: +// handle_polygon(shape_info.polygon.vertices, value); +// default: +// break; +// } +// } + +void GridManager::draw_circle( + int xc, int yc, int x, int y, int value, + Eigen::Block &block, + int xmin, int ymin) { + // Translate coordinates to block local coordinates + int bx = xc - xmin; + int by = yc - ymin; + + if (bx + x < block.rows() && by + y < block.cols() && bx + x >= 0 && + by + y >= 0) { + block(bx + x, by + y) = value; + block(bx - x, by + y) = value; + block(bx + x, by - y) = value; + block(bx - x, by - y) = value; + block(bx + y, by + x) = value; + block(bx - y, by + x) = value; + block(bx + y, by - x) = value; + block(bx - y, by - x) = value; + } } void GridManager::handle_circle(int xc, int yc, int r, int value) { - int xmin = static_cast((xc - r) / resolution_ + width_ / 2); - int ymin = static_cast((yc - r) / resolution_ + width_ / 2); - int xmax = static_cast((xc + r) / resolution_ + width_ / 2); - int ymax = static_cast((yc + r) / resolution_ + width_ / 2); - - if (xmin < 0 || ymin < 0 || xmax >= grid_.rows() || ymax >= grid_.cols()) { - throw std::runtime_error("Circle exceeds grid boundaries"); + int xmin = static_cast((xc - r) / resolution_ + width_ / 2); + int ymin = static_cast((yc - r) / resolution_ + width_ / 2); + int xmax = static_cast((xc + r) / resolution_ + width_ / 2); + int ymax = static_cast((yc + r) / resolution_ + width_ / 2); + + if (xmin < 0 || ymin < 0 || xmax >= grid_.rows() || ymax >= grid_.cols()) { + throw std::runtime_error("Circle exceeds grid boundaries"); + } + + auto block = grid_.block(xmin, ymin, xmax - xmin + 1, ymax - ymin + 1); + int x = 0, y = r; + int d = 3 - 2 * r; + draw_circle(xc, yc, x, y, value, block, xmin, ymin); + while (y >= x) { + x++; + if (d > 0) { + y--; + d = d + 4 * (x - y) + 10; + } else { + d = d + 4 * x + 6; } - - auto block = grid_.block(xmin, ymin, xmax - xmin + 1, ymax - ymin + 1); - int x = 0, y = r; - int d = 3 - 2 * r; draw_circle(xc, yc, x, y, value, block, xmin, ymin); - while (y >= x) { - x++; - if (d > 0) { - y--; - d = d + 4 * (x - y) + 10; - } else { - d = d + 4 * x + 6; - } - draw_circle(xc, yc, x, y, value, block, xmin, ymin); - } + } } -void GridManager::handle_polygon(int8_t* grid, const Eigen::Array& vertices, int value) { - // Determine the bounding box of the polygon - // float min_x = vertices.row(0).minCoeff(); - // float min_y = vertices.row(1).minCoeff(); - // float max_x = vertices.row(0).maxCoeff(); - // float max_y = vertices.row(1).maxCoeff(); - - // int xmin = static_cast(min_x / resolution_ + width_ / 2); - // int ymin = static_cast(min_y / resolution_ + height_ / 2); - // int xmax = static_cast(max_x / resolution_ + width_ / 2); - // int ymax = static_cast(max_y / resolution_ + height_ / 2); - - // if (xmin < 0 || ymin < 0 || xmax >= height_ || ymax >= width_) { - // throw std::runtime_error("Polygon exceeds grid boundaries"); - // } - - // Draw the polygon - for (Eigen::Index i = 0; i < vertices.cols(); ++i) { - Eigen::Index j = (i + 1) % vertices.cols(); - std::cout << "i: " << i << " j: " << j << std::endl; - int x0 = static_cast(vertices(0, i) / resolution_ + width_ / 2); - int y0 = static_cast(vertices(1, i) / resolution_ + height_ / 2); - int x1 = static_cast(vertices(0, j) / resolution_ + width_ / 2); - int y1 = static_cast(vertices(1, j) / resolution_ + height_ / 2); - - draw_line(x0, y0, x1, y1, grid, value); - } +void GridManager::handle_polygon( + int8_t *grid, const Eigen::Array &vertices, + int value) { + // Determine the bounding box of the polygon + // float min_x = vertices.row(0).minCoeff(); + // float min_y = vertices.row(1).minCoeff(); + // float max_x = vertices.row(0).maxCoeff(); + // float max_y = vertices.row(1).maxCoeff(); + + // int xmin = static_cast(min_x / resolution_ + width_ / 2); + // int ymin = static_cast(min_y / resolution_ + height_ / 2); + // int xmax = static_cast(max_x / resolution_ + width_ / 2); + // int ymax = static_cast(max_y / resolution_ + height_ / 2); + + // if (xmin < 0 || ymin < 0 || xmax >= height_ || ymax >= width_) { + // throw std::runtime_error("Polygon exceeds grid boundaries"); + // } + + // Draw the polygon + for (Eigen::Index i = 0; i < vertices.cols(); ++i) { + Eigen::Index j = (i + 1) % vertices.cols(); + std::cout << "i: " << i << " j: " << j << std::endl; + int x0 = static_cast(vertices(0, i) / resolution_ + width_ / 2); + int y0 = static_cast(vertices(1, i) / resolution_ + height_ / 2); + int x1 = static_cast(vertices(0, j) / resolution_ + width_ / 2); + int y1 = static_cast(vertices(1, j) / resolution_ + height_ / 2); + + draw_line(x0, y0, x1, y1, grid, value); + } } -void GridManager::draw_polygon(int x0, int y0, int x1, int y1, Eigen::Block& block, int value) { - bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); +void GridManager::draw_polygon( + int x0, int y0, int x1, int y1, + Eigen::Block &block, + int value) { + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (steep) { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) { + std::swap(x0, x1); + std::swap(y0, y1); + } + int dx = x1 - x0; + int dy = std::abs(y1 - y0); + int error = 2 * dy - dx; + int ystep = (y0 < y1) ? 1 : -1; + int xbounds = steep ? block.rows() : block.cols(); + int ybounds = steep ? block.cols() : block.rows(); + int y = y0; + + for (int x = x0; x <= x1; x++) { if (steep) { - std::swap(x0, y0); - std::swap(x1, y1); - } - if (x0 > x1) { - std::swap(x0, x1); - std::swap(y0, y1); + if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { + block(y, x) += value; + } + } else { + if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { + block(x, y) += value; + } } - int dx = x1 - x0; - int dy = std::abs(y1 - y0); - int error = 2 * dy - dx; - int ystep = (y0 < y1) ? 1 : -1; - int xbounds = steep ? block.rows() : block.cols(); - int ybounds = steep ? block.cols() : block.rows(); - int y = y0; - - for (int x = x0; x <= x1; x++) { - if (steep) { - if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { - block(y, x) += value; - } - } else { - if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { - block(x, y) += value; - } - } - if (error > 0) { - y += ystep; - error -= 2*(dx-dy); - } else { - error += 2 * dy; - } + if (error > 0) { + y += ystep; + error -= 2 * (dx - dy); + } else { + error += 2 * dy; } + } } -void GridManager::draw_line(int x0, int y0, int x1, int y1, int8_t* grid, int value) { - bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); +void GridManager::draw_line(int x0, int y0, int x1, int y1, int8_t *grid, + int value) { + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (steep) { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) { + std::swap(x0, x1); + std::swap(y0, y1); + } + int dx = x1 - x0; + int dy = std::abs(y1 - y0); + int error = 2 * dy - dx; + int ystep = (y0 < y1) ? 1 : -1; + int xbounds = steep ? height_ : width_; + int ybounds = steep ? width_ : height_; + int y = y0; + + for (int x = x0; x <= x1; x++) { if (steep) { - std::swap(x0, y0); - std::swap(x1, y1); + if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { + grid[y + x * height_] += value; + } + } else { + if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { + grid[y * width_ + x] += value; + } } - if (x0 > x1) { - std::swap(x0, x1); - std::swap(y0, y1); - } - int dx = x1 - x0; - int dy = std::abs(y1 - y0); - int error = 2*dy - dx; - int ystep = (y0 < y1) ? 1 : -1; - int xbounds = steep ? height_ : width_; - int ybounds = steep ? width_ : height_; - int y = y0; - - for (int x = x0; x <= x1; x++) { - if (steep) { - if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { - grid[y + x * height_] += value; - } - } else { - if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { - grid[y * width_ + x] += value; - } - } - if(error > 0) - { - y += ystep; - error -= 2*(dx-dy); - } - else - { - error += 2*dy; - } - + if (error > 0) { + y += ystep; + error -= 2 * (dx - dy); + } else { + error += 2 * dy; } + } } - - -} \ No newline at end of file +} // namespace landmark_server \ No newline at end of file diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp index 7959f323..1cc35ed8 100644 --- a/mission/landmark_server/src/landmark_server.cpp +++ b/mission/landmark_server/src/landmark_server.cpp @@ -27,16 +27,20 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) posePublisher_ = this->create_publisher( "landmark_poses_out", qos); - gridPublisher_ = this->create_publisher( - "grid", qos); - + gridPublisher_ = + this->create_publisher("grid", qos); + rmw_qos_profile_t qos_profile_sensor = rmw_qos_profile_sensor_data; qos_profile_sensor.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - auto qos_sensor = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor.history, 1), qos_profile_sensor); - - convex_hull_publisher_ = this->create_publisher("landmark/hull", qos_sensor); - cluster_publisher_ = this->create_publisher("landmark/cluster", qos_sensor); + auto qos_sensor = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor.history, 1), + qos_profile_sensor); + convex_hull_publisher_ = + this->create_publisher("landmark/hull", + qos_sensor); + cluster_publisher_ = this->create_publisher( + "landmark/cluster", qos_sensor); tf2_buffer_ = std::make_shared(this->get_clock()); tf2_listener_ = std::make_shared(*tf2_buffer_); @@ -56,74 +60,80 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) std::thread(&LandmarkServerNode::get_grid, this).detach(); } - -Eigen::Array LandmarkServerNode::get_convex_hull(const shape_msgs::msg::SolidPrimitive& solid_primitive) { - pcl::PointCloud cluster; - cluster.resize(solid_primitive.polygon.points.size()); - for (size_t i = 0; i < solid_primitive.polygon.points.size(); i++) { - cluster.points[i].x = solid_primitive.polygon.points[i].x; - cluster.points[i].y = solid_primitive.polygon.points[i].y; - cluster.points[i].z = 1.0; - } - sensor_msgs::msg::PointCloud2 cluster_msg; - pcl::toROSMsg(cluster, cluster_msg); - cluster_msg.header.frame_id = "world"; - cluster_publisher_->publish(cluster_msg); - pcl::PointCloud convex_hull; - pcl::ConvexHull chull; - chull.setDimension(2); - chull.setInputCloud(cluster.makeShared()); - chull.reconstruct(convex_hull); - sensor_msgs::msg::PointCloud2 hull_msg; - pcl::toROSMsg(convex_hull, hull_msg); - hull_msg.header.frame_id = "world"; - convex_hull_publisher_->publish(hull_msg); - RCLCPP_INFO(this->get_logger(), "Convex hull size: %ld", convex_hull.size()); - Eigen::Array hull(2, convex_hull.size()); - for (size_t i = 0; i < convex_hull.size(); i++) { - hull(0, i) = convex_hull.points[i].x; - hull(1, i) = convex_hull.points[i].y; - } - RCLCPP_INFO(this->get_logger(), "Convex hull: %ld", hull.cols()); - std::cout << "HUll" << hull << std::endl; - return hull; +Eigen::Array LandmarkServerNode::get_convex_hull( + const shape_msgs::msg::SolidPrimitive &solid_primitive) { + pcl::PointCloud cluster; + cluster.resize(solid_primitive.polygon.points.size()); + for (size_t i = 0; i < solid_primitive.polygon.points.size(); i++) { + cluster.points[i].x = solid_primitive.polygon.points[i].x; + cluster.points[i].y = solid_primitive.polygon.points[i].y; + cluster.points[i].z = 1.0; + } + sensor_msgs::msg::PointCloud2 cluster_msg; + pcl::toROSMsg(cluster, cluster_msg); + cluster_msg.header.frame_id = "world"; + cluster_publisher_->publish(cluster_msg); + pcl::PointCloud convex_hull; + pcl::ConvexHull chull; + chull.setDimension(2); + chull.setInputCloud(cluster.makeShared()); + chull.reconstruct(convex_hull); + sensor_msgs::msg::PointCloud2 hull_msg; + pcl::toROSMsg(convex_hull, hull_msg); + hull_msg.header.frame_id = "world"; + convex_hull_publisher_->publish(hull_msg); + RCLCPP_INFO(this->get_logger(), "Convex hull size: %ld", convex_hull.size()); + Eigen::Array hull(2, convex_hull.size()); + for (size_t i = 0; i < convex_hull.size(); i++) { + hull(0, i) = convex_hull.points[i].x; + hull(1, i) = convex_hull.points[i].y; + } + RCLCPP_INFO(this->get_logger(), "Convex hull: %ld", hull.cols()); + std::cout << "HUll" << hull << std::endl; + return hull; } -void LandmarkServerNode::get_grid(){ - while(true){ - rclcpp::sleep_for(std::chrono::seconds(1)); - if (!grid_client_->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_ERROR(this->get_logger(), "Service not available after waiting"); - continue; - } +void LandmarkServerNode::get_grid() { + while (true) { + rclcpp::sleep_for(std::chrono::seconds(1)); + if (!grid_client_->wait_for_service(std::chrono::seconds(5))) { + RCLCPP_ERROR(this->get_logger(), "Service not available after waiting"); + continue; + } - auto request = std::make_shared(); - auto result_future = grid_client_->async_send_request(request); - - // Wait for the result within a specified timeout period - auto status = result_future.wait_for(std::chrono::seconds(5)); - if (status == std::future_status::ready) { - try { - auto result = result_future.get(); - if(result->map.data.empty()){ - RCLCPP_ERROR(this->get_logger(), "Received empty map from grid client"); - continue; - } - grid_msg_ = result->map; - - grid_manager_ = std::make_unique(grid_msg_.info.resolution, grid_msg_.info.height, grid_msg_.info.width); - RCLCPP_INFO(this->get_logger(), "Successfully received map from grid client"); - return; - } catch (const std::exception &e) { - RCLCPP_ERROR(this->get_logger(), "Exception while getting result from future: %s", e.what()); - } - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to get map from grid client within timeout period"); + auto request = std::make_shared(); + auto result_future = grid_client_->async_send_request(request); + + // Wait for the result within a specified timeout period + auto status = result_future.wait_for(std::chrono::seconds(5)); + if (status == std::future_status::ready) { + try { + auto result = result_future.get(); + if (result->map.data.empty()) { + RCLCPP_ERROR(this->get_logger(), + "Received empty map from grid client"); continue; + } + grid_msg_ = result->map; + + grid_manager_ = std::make_unique(grid_msg_.info.resolution, + grid_msg_.info.height, + grid_msg_.info.width); + RCLCPP_INFO(this->get_logger(), + "Successfully received map from grid client"); + return; + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), + "Exception while getting result from future: %s", + e.what()); } + } else { + RCLCPP_ERROR(this->get_logger(), + "Failed to get map from grid client within timeout period"); + continue; } + } } - void LandmarkServerNode::landmarksRecievedCallback( const LandmarkArray::SharedPtr msg) { @@ -132,8 +142,8 @@ void LandmarkServerNode::landmarksRecievedCallback( "Received empty landmark array"); return; } - - if (grid_manager_ == nullptr) { + + if (grid_manager_ == nullptr) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Grid manager not initialized"); return; @@ -141,39 +151,44 @@ void LandmarkServerNode::landmarksRecievedCallback( for (const auto &landmark : msg->landmarks) { auto it = std::find_if( - storedLandmarks_->landmarks.begin(), - storedLandmarks_->landmarks.end(), [&](const auto &storedLandmark) { - return storedLandmark.landmark_type == landmark.landmark_type && - storedLandmark.id == landmark.id; - }); + storedLandmarks_->landmarks.begin(), storedLandmarks_->landmarks.end(), + [&](const auto &storedLandmark) { + return storedLandmark.landmark_type == landmark.landmark_type && + storedLandmark.id == landmark.id; + }); - if(landmark.action == vortex_msgs::msg::Landmark::ADD_ACTION) { + if (landmark.action == vortex_msgs::msg::Landmark::ADD_ACTION) { if (it != storedLandmarks_->landmarks.end()) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, - "Requested to add already existing landmark"); + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Requested to add already existing landmark"); } else { // Add the new landmark - grid_manager_->update_grid(grid_msg_.data.data(), get_convex_hull(landmark.shape), 200); + grid_manager_->update_grid(grid_msg_.data.data(), + get_convex_hull(landmark.shape), 200); storedLandmarks_->landmarks.push_back(landmark); } continue; } if (it == storedLandmarks_->landmarks.end()) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, - "Requested to remove or update non-existing landmark"); + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Requested to remove or update non-existing landmark"); continue; } - grid_manager_->update_grid(grid_msg_.data.data(), get_convex_hull((*it).shape), -200); + grid_manager_->update_grid(grid_msg_.data.data(), + get_convex_hull((*it).shape), -200); - if(landmark.action == vortex_msgs::msg::Landmark::REMOVE_ACTION){ + if (landmark.action == vortex_msgs::msg::Landmark::REMOVE_ACTION) { storedLandmarks_->landmarks.erase(it); - } else if(landmark.action == vortex_msgs::msg::Landmark::UPDATE_ACTION){ - grid_manager_->update_grid(grid_msg_.data.data(), get_convex_hull(landmark.shape), 200); + } else if (landmark.action == vortex_msgs::msg::Landmark::UPDATE_ACTION) { + grid_manager_->update_grid(grid_msg_.data.data(), + get_convex_hull(landmark.shape), 200); *it = landmark; - } } + } std_msgs::msg::Header header; // header.frame_id = get_parameter("fixed_frame").as_string(); @@ -181,9 +196,9 @@ void LandmarkServerNode::landmarksRecievedCallback( // // Convert the Eigen array to std::vector // auto grid_eigen = grid_manager_->get_grid(); - // std::vector grid_data(grid_eigen.data(), grid_eigen.data() + grid_eigen.size()); - // grid_msg_.header = header; - // grid_msg_.data = grid_data; + // std::vector grid_data(grid_eigen.data(), grid_eigen.data() + + // grid_eigen.size()); grid_msg_.header = header; grid_msg_.data = + // grid_data; grid_msg_.header.stamp = rclcpp::Clock().now(); // Publish the landmarks @@ -192,7 +207,6 @@ void LandmarkServerNode::landmarksRecievedCallback( posePublisher_->publish(poseArrayCreater(*storedLandmarks_)); } - geometry_msgs::msg::PoseArray LandmarkServerNode::poseArrayCreater( vortex_msgs::msg::LandmarkArray landmarks) { @@ -279,7 +293,8 @@ void LandmarkServerNode::execute( continue; } - vortex_msgs::msg::OdometryArray filteredLandmarksOdoms = filterLandmarks(goal_handle); + vortex_msgs::msg::OdometryArray filteredLandmarksOdoms = + filterLandmarks(goal_handle); if (filteredLandmarksOdoms.odoms.empty()) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "No landmarks found matching the request"); @@ -318,15 +333,15 @@ vortex_msgs::msg::OdometryArray LandmarkServerNode::filterLandmarks( filteredLandmarksOdoms.odoms.push_back(landmark.odom); } } else if (distance == 0.0) { - if (landmark.landmark_type == filter_type) { - filteredLandmarksOdoms.odoms.push_back(landmark.odom); - } + if (landmark.landmark_type == filter_type) { + filteredLandmarksOdoms.odoms.push_back(landmark.odom); + } } else { - if (landmark.landmark_type == filter_type && - calculateDistance(landmark.odom.pose.pose.position, - landmark.odom.header) <= distance) { - filteredLandmarksOdoms.odoms.push_back(landmark.odom); - } + if (landmark.landmark_type == filter_type && + calculateDistance(landmark.odom.pose.pose.position, + landmark.odom.header) <= distance) { + filteredLandmarksOdoms.odoms.push_back(landmark.odom); + } } } return filteredLandmarksOdoms; @@ -339,7 +354,8 @@ void LandmarkServerNode::requestLogger( const auto goal = goal_handle->get_goal(); double distance = goal->distance; - if (distance == 0.0 && goal->landmark_types == vortex_msgs::msg::Landmark::NONE) { + if (distance == 0.0 && + goal->landmark_types == vortex_msgs::msg::Landmark::NONE) { RCLCPP_INFO_STREAM(this->get_logger(), "Received request to return all landmarks."); return; @@ -378,8 +394,8 @@ LandmarkServerNode::calculateDistance(const geometry_msgs::msg::Point &point, try { // Lookup the transformation geometry_msgs::msg::TransformStamped transform_stamped = - tf2_buffer_->lookupTransform(target_frame, header.frame_id, header.stamp, - rclcpp::Duration(1, 0)); + tf2_buffer_->lookupTransform(target_frame, header.frame_id, + header.stamp, rclcpp::Duration(1, 0)); // Transform the point cloud geometry_msgs::msg::Point transformed_point; diff --git a/mission/map_manager/include/map_manager/map_manager_ros.hpp b/mission/map_manager/include/map_manager/map_manager_ros.hpp index 8a506fde..f8065220 100644 --- a/mission/map_manager/include/map_manager/map_manager_ros.hpp +++ b/mission/map_manager/include/map_manager/map_manager_ros.hpp @@ -1,23 +1,21 @@ #ifndef MAP_MANAGER_HPP #define MAP_MANAGER_HPP -#include -#include -#include #include "nav_msgs/srv/get_map.hpp" #include +#include +#include +#include +#include #include #include -#include +#include +#include #include -#include #include -#include -#include - - +#include namespace map_manager { @@ -44,40 +42,39 @@ class MapManagerNode : public rclcpp::Node { ~MapManagerNode(){}; private: - - constexpr double deg2rad(double degrees) const; - std::array lla2flat(double lat, double lon) const; - std::array flat2lla(double px, double py) const; - - geometry_msgs::msg::PolygonStamped processCoordinates(const std::vector>& coordinates); - geometry_msgs::msg::PolygonStamped readPolygonFromFile(const std::string& filename); - void mapOriginCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); - nav_msgs::msg::OccupancyGrid createOccupancyGrid(); - geometry_msgs::msg::Pose calculate_map_origin(); - void insert_landmask(nav_msgs::msg::OccupancyGrid& grid, const geometry_msgs::msg::PolygonStamped& polygon); - void handle_get_map_request( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); - void fillOutsidePolygon(nav_msgs::msg::OccupancyGrid& grid, const geometry_msgs::msg::PolygonStamped& polygon); - - - - + constexpr double deg2rad(double degrees) const; + std::array lla2flat(double lat, double lon) const; + std::array flat2lla(double px, double py) const; + + geometry_msgs::msg::PolygonStamped + processCoordinates(const std::vector> &coordinates); + geometry_msgs::msg::PolygonStamped + readPolygonFromFile(const std::string &filename); + void mapOriginCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + nav_msgs::msg::OccupancyGrid createOccupancyGrid(); + geometry_msgs::msg::Pose calculate_map_origin(); + void insert_landmask(nav_msgs::msg::OccupancyGrid &grid, + const geometry_msgs::msg::PolygonStamped &polygon); + void handle_get_map_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + void fillOutsidePolygon(nav_msgs::msg::OccupancyGrid &grid, + const geometry_msgs::msg::PolygonStamped &polygon); rclcpp::Subscription::SharedPtr map_origin_sub_; rclcpp::Publisher::SharedPtr map_pub_; - rclcpp::Publisher::SharedPtr landmask_pub_; - rclcpp::Service::SharedPtr grid_service_; + rclcpp::Publisher::SharedPtr + landmask_pub_; + rclcpp::Service::SharedPtr grid_service_; bool map_origin_set_ = false; bool use_predef_landmask_; double map_origin_lat_; double map_origin_lon_; std::string landmask_file_; std::string grid_frame_id_; - }; -} // namespace map_manager +} // namespace map_manager -#endif // MAP_MANAGER_HPP \ No newline at end of file +#endif // MAP_MANAGER_HPP \ No newline at end of file diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index 303a68d2..b2b9987a 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -5,336 +5,355 @@ namespace map_manager { MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) : Node("map_manager_node", options) { - // rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - // auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; - qos_profile_transient_local.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - auto qos_transient_local = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), qos_profile_transient_local); - - map_origin_sub_ = this->create_subscription( - "map_origin", qos_transient_local,std::bind(&MapManagerNode::mapOriginCallback, this, std::placeholders::_1)); - - declare_parameter("use_predef_landmask", false); - declare_parameter("landmask_file", "src/vortex-asv/mission/map_manager/params/land_polygon.yaml"); - declare_parameter("map_resolution", 0.2); - declare_parameter("map_width", 1000); - declare_parameter("map_height", 1000); - declare_parameter("map_frame_id", "world"); - - landmask_file_ = get_parameter("landmask_file").as_string(); - landmask_pub_ = this->create_publisher("landmask", qos_transient_local); - // traffic centre - // map_origin_lat_ = 63.41490901857848; - // map_origin_lon_ = 10.398215601285054; - // office bag still - // map_origin_lat_ = 63.414660884931976; - // map_origin_lon_ = 10.398554661537544; - // map_origin_set_ = true; - // auto grid = createOccupancyGrid(); - // auto polygon = readPolygonFromFile(landmask_file_); - // landmask_pub_->publish(polygon); - // fillOutsidePolygon(grid, polygon); - // insert_landmask(grid, polygon); - - // map_pub_ = this->create_publisher("map", qos_transient_local); - // map_pub_->publish(grid); - - - - grid_service_ = this->create_service( - "get_map", std::bind( - &MapManagerNode::handle_get_map_request, // Callback function - this, // Pointer to the object - std::placeholders::_1, // Placeholder for request header - std::placeholders::_2, // Placeholder for request - std::placeholders::_3 // Placeholder for response - )); - - } - - void MapManagerNode::mapOriginCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) - { - map_origin_lat_ = msg->latitude; - map_origin_lon_ = msg->longitude; - map_origin_set_ = true; - map_origin_sub_ = nullptr; + // rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + // auto qos_sensor_data = + // rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), + // qos_profile); + rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; + qos_profile_transient_local.durability = + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + auto qos_transient_local = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), + qos_profile_transient_local); + + map_origin_sub_ = this->create_subscription( + "map_origin", qos_transient_local, + std::bind(&MapManagerNode::mapOriginCallback, this, + std::placeholders::_1)); + + declare_parameter("use_predef_landmask", false); + declare_parameter( + "landmask_file", + "src/vortex-asv/mission/map_manager/params/land_polygon.yaml"); + declare_parameter("map_resolution", 0.2); + declare_parameter("map_width", 1000); + declare_parameter("map_height", 1000); + declare_parameter("map_frame_id", "world"); + + landmask_file_ = get_parameter("landmask_file").as_string(); + landmask_pub_ = this->create_publisher( + "landmask", qos_transient_local); + // traffic centre + // map_origin_lat_ = 63.41490901857848; + // map_origin_lon_ = 10.398215601285054; + // office bag still + // map_origin_lat_ = 63.414660884931976; + // map_origin_lon_ = 10.398554661537544; + // map_origin_set_ = true; + // auto grid = createOccupancyGrid(); + // auto polygon = readPolygonFromFile(landmask_file_); + // landmask_pub_->publish(polygon); + // fillOutsidePolygon(grid, polygon); + // insert_landmask(grid, polygon); + + // map_pub_ = this->create_publisher("map", + // qos_transient_local); map_pub_->publish(grid); + + grid_service_ = this->create_service( + "get_map", + std::bind(&MapManagerNode::handle_get_map_request, // Callback function + this, // Pointer to the object + std::placeholders::_1, // Placeholder for request header + std::placeholders::_2, // Placeholder for request + std::placeholders::_3 // Placeholder for response + )); +} - landmask_pub_->publish(readPolygonFromFile(landmask_file_)); - } +void MapManagerNode::mapOriginCallback( + const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + map_origin_lat_ = msg->latitude; + map_origin_lon_ = msg->longitude; + map_origin_set_ = true; + map_origin_sub_ = nullptr; - void MapManagerNode::handle_get_map_request( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) - { - if(!map_origin_set_){ - RCLCPP_WARN(this->get_logger(), "Map origin not set, cannot provide map"); - return; - } - // Your logic to fill the map data - RCLCPP_INFO(this->get_logger(), "Received request for map"); - - // Example: You could fill this with actual map data - nav_msgs::msg::OccupancyGrid grid = createOccupancyGrid(); - auto polygon = readPolygonFromFile(landmask_file_); - fillOutsidePolygon(grid, polygon); - insert_landmask(grid, polygon); - response->map = grid; - RCLCPP_INFO(this->get_logger(), "Map sent"); + landmask_pub_->publish(readPolygonFromFile(landmask_file_)); +} - } +void MapManagerNode::handle_get_map_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) { + if (!map_origin_set_) { + RCLCPP_WARN(this->get_logger(), "Map origin not set, cannot provide map"); + return; + } + // Your logic to fill the map data + RCLCPP_INFO(this->get_logger(), "Received request for map"); + + // Example: You could fill this with actual map data + nav_msgs::msg::OccupancyGrid grid = createOccupancyGrid(); + auto polygon = readPolygonFromFile(landmask_file_); + fillOutsidePolygon(grid, polygon); + insert_landmask(grid, polygon); + response->map = grid; + RCLCPP_INFO(this->get_logger(), "Map sent"); +} - constexpr double MapManagerNode::deg2rad(double degrees) const{ - return degrees * M_PI / 180.0; - } +constexpr double MapManagerNode::deg2rad(double degrees) const { + return degrees * M_PI / 180.0; +} - std::array MapManagerNode::lla2flat(double lat, double lon) const - { - const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) - const double f = 1.0 / 298.257223563; // Flattening of the earth - const double psi_rad = 0.0; // Angular direction of the flat Earth x-axis, specified as a scalar. - // The angular direction is the degrees clockwise from north, - // which is the angle in degrees used for converting flat Earth x and y coordinates to the north and east coordinates - - // Convert angles from degrees to radians - const double lat_rad = deg2rad(lat); - const double lon_rad = deg2rad(lon); - const double origin_lat_rad = deg2rad(map_origin_lat_); - const double origin_lon_rad = deg2rad(map_origin_lon_); - - // Calculate delta latitude and delta longitude in radians - const double dlat = lat_rad - origin_lat_rad; - const double dlon = lon_rad - origin_lon_rad; - - // Radius of curvature in the vertical prime (RN) - const double RN = R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); - - // Radius of curvature in the meridian (RM) - const double RM = RN * (1.0 - (2.0 * f - f * f)) / (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); - - // Changes in the north (dN) and east (dE) positions - const double dN = RM * dlat; - const double dE = RN * cos(origin_lat_rad) * dlon; - - // Transformation from North-East to flat x-y coordinates - const double px = cos(psi_rad) * dN - sin(psi_rad) * dE; - const double py = sin(psi_rad) * dN + cos(psi_rad) * dE; - - return {px, py}; - } +std::array MapManagerNode::lla2flat(double lat, double lon) const { + const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) + const double f = 1.0 / 298.257223563; // Flattening of the earth + const double psi_rad = + 0.0; // Angular direction of the flat Earth x-axis, specified as a scalar. + // The angular direction is the degrees clockwise from north, + // which is the angle in degrees used for converting flat Earth x and + // y coordinates to the north and east coordinates + + // Convert angles from degrees to radians + const double lat_rad = deg2rad(lat); + const double lon_rad = deg2rad(lon); + const double origin_lat_rad = deg2rad(map_origin_lat_); + const double origin_lon_rad = deg2rad(map_origin_lon_); + + // Calculate delta latitude and delta longitude in radians + const double dlat = lat_rad - origin_lat_rad; + const double dlon = lon_rad - origin_lon_rad; + + // Radius of curvature in the vertical prime (RN) + const double RN = + R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Radius of curvature in the meridian (RM) + const double RM = RN * (1.0 - (2.0 * f - f * f)) / + (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Changes in the north (dN) and east (dE) positions + const double dN = RM * dlat; + const double dE = RN * cos(origin_lat_rad) * dlon; + + // Transformation from North-East to flat x-y coordinates + const double px = cos(psi_rad) * dN - sin(psi_rad) * dE; + const double py = sin(psi_rad) * dN + cos(psi_rad) * dE; + + return {px, py}; +} - std::array MapManagerNode::flat2lla(double px, double py) const - { - // Earth constants - const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) - const double f = 1.0 / 298.257223563; // Flattening of the earth +std::array MapManagerNode::flat2lla(double px, double py) const { + // Earth constants + const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) + const double f = 1.0 / 298.257223563; // Flattening of the earth - // Convert origin from degrees to radians - const double origin_lat_rad = deg2rad(map_origin_lat_); - const double origin_lon_rad = deg2rad(map_origin_lon_); + // Convert origin from degrees to radians + const double origin_lat_rad = deg2rad(map_origin_lat_); + const double origin_lon_rad = deg2rad(map_origin_lon_); - // Radius of curvature in the vertical prime (RN) and in the meridian (RM) - const double RN = R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); - const double RM = RN * (1.0 - (2.0 * f - f * f)) / (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + // Radius of curvature in the vertical prime (RN) and in the meridian (RM) + const double RN = + R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + const double RM = RN * (1.0 - (2.0 * f - f * f)) / + (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); - // Calculate changes in latitude and longitude - double dN = px; // Change in north direction - double dE = py; // Change in east direction - double dlat = dN / RM; - double dlon = dE / (RN * cos(origin_lat_rad)); + // Calculate changes in latitude and longitude + double dN = px; // Change in north direction + double dE = py; // Change in east direction + double dlat = dN / RM; + double dlon = dE / (RN * cos(origin_lat_rad)); - // Convert delta lat and lon from radians back to degrees - double new_lat = (origin_lat_rad + dlat) * 180.0 / M_PI; - double new_lon = (origin_lon_rad + dlon) * 180.0 / M_PI; + // Convert delta lat and lon from radians back to degrees + double new_lat = (origin_lat_rad + dlat) * 180.0 / M_PI; + double new_lon = (origin_lon_rad + dlon) * 180.0 / M_PI; - return {new_lat, new_lon}; + return {new_lat, new_lon}; } - - geometry_msgs::msg::PolygonStamped MapManagerNode::readPolygonFromFile(const std::string& filename) { - std::ifstream file(filename); - if (!file.is_open()) { - throw std::runtime_error("Could not open file: " + filename); - } - - std::string line; - std::vector> coords; - while (std::getline(file, line)) { - std::stringstream ss(line); - double lat, lon; - if (ss >> lat >> lon) { - coords.push_back({lat, lon}); - } else { - std::cerr << "Failed to parse line: " << line << std::endl; - } +geometry_msgs::msg::PolygonStamped +MapManagerNode::readPolygonFromFile(const std::string &filename) { + std::ifstream file(filename); + if (!file.is_open()) { + throw std::runtime_error("Could not open file: " + filename); + } + + std::string line; + std::vector> coords; + while (std::getline(file, line)) { + std::stringstream ss(line); + double lat, lon; + if (ss >> lat >> lon) { + coords.push_back({lat, lon}); + } else { + std::cerr << "Failed to parse line: " << line << std::endl; } + } - file.close(); - std::cout << "coords size: " << coords.size() << "\n"; + file.close(); + std::cout << "coords size: " << coords.size() << "\n"; - return processCoordinates(coords); - } - - geometry_msgs::msg::PolygonStamped MapManagerNode::processCoordinates(const std::vector>& coordinates) - { - geometry_msgs::msg::PolygonStamped polygon; - polygon.header.frame_id = get_parameter("map_frame_id").as_string(); - polygon.header.stamp = this->now(); - std::cout << "coords size: " << coordinates.size() << "\n"; - for (const auto& coord : coordinates) { - std::array flat = lla2flat(coord[0], coord[1]); - geometry_msgs::msg::Point32 point; - point.x = flat[0]; - point.y = flat[1]; - point.z = 0; - polygon.polygon.points.push_back(point); - } - std::cout << "Polygon size: " << polygon.polygon.points.size() << std::endl; - return polygon; - } + return processCoordinates(coords); +} - nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() - { - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = get_parameter("map_frame_id").as_string(); - grid.header.stamp = this->now(); - grid.info.resolution = get_parameter("map_resolution").as_double(); - grid.info.width = get_parameter("map_width").as_int(); - grid.info.height = get_parameter("map_height").as_int(); - grid.info.origin = calculate_map_origin(); - grid.info.map_load_time = this->now(); - // 0 represents unoccupied, 1 represents definitely occupied, and - // -1 represents unknown. - // Set all to unoccupied - grid.data.resize(grid.info.width * grid.info.height, 0); - return grid; - } +geometry_msgs::msg::PolygonStamped MapManagerNode::processCoordinates( + const std::vector> &coordinates) { + geometry_msgs::msg::PolygonStamped polygon; + polygon.header.frame_id = get_parameter("map_frame_id").as_string(); + polygon.header.stamp = this->now(); + std::cout << "coords size: " << coordinates.size() << "\n"; + for (const auto &coord : coordinates) { + std::array flat = lla2flat(coord[0], coord[1]); + geometry_msgs::msg::Point32 point; + point.x = flat[0]; + point.y = flat[1]; + point.z = 0; + polygon.polygon.points.push_back(point); + } + std::cout << "Polygon size: " << polygon.polygon.points.size() << std::endl; + return polygon; +} - geometry_msgs::msg::Pose MapManagerNode::calculate_map_origin() { - // 63.44098584906518, 10.42304390021551 - // 63.41490901857848, 10.398215601285054 - // Office traffic centre origin - // map_origin_lat_ = 63.41490901857848; - // map_origin_lon_ = 10.398215601285054; - - // Map centre is (0,0) in map frame, origin is bottom left corner - double half_width_meters = -(get_parameter("map_width").as_int() * get_parameter("map_resolution").as_double()) / 2.0; - double half_height_meters = -(get_parameter("map_height").as_int() * get_parameter("map_resolution").as_double()) / 2.0; - // auto [lat, lon] = flat2lla(half_width_meters, half_height_meters); - geometry_msgs::msg::Pose map_origin; - map_origin.position.x = half_width_meters; - map_origin.position.y = half_height_meters; - map_origin.position.z = 0.0; - map_origin.orientation.x = 0.0; - map_origin.orientation.y = 0.0; - map_origin.orientation.z = 0.0; - map_origin.orientation.w = 1.0; - return map_origin; - } +nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() { + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = get_parameter("map_frame_id").as_string(); + grid.header.stamp = this->now(); + grid.info.resolution = get_parameter("map_resolution").as_double(); + grid.info.width = get_parameter("map_width").as_int(); + grid.info.height = get_parameter("map_height").as_int(); + grid.info.origin = calculate_map_origin(); + grid.info.map_load_time = this->now(); + // 0 represents unoccupied, 1 represents definitely occupied, and + // -1 represents unknown. + // Set all to unoccupied + grid.data.resize(grid.info.width * grid.info.height, 0); + return grid; +} +geometry_msgs::msg::Pose MapManagerNode::calculate_map_origin() { + // 63.44098584906518, 10.42304390021551 + // 63.41490901857848, 10.398215601285054 + // Office traffic centre origin + // map_origin_lat_ = 63.41490901857848; + // map_origin_lon_ = 10.398215601285054; + + // Map centre is (0,0) in map frame, origin is bottom left corner + double half_width_meters = -(get_parameter("map_width").as_int() * + get_parameter("map_resolution").as_double()) / + 2.0; + double half_height_meters = -(get_parameter("map_height").as_int() * + get_parameter("map_resolution").as_double()) / + 2.0; + // auto [lat, lon] = flat2lla(half_width_meters, half_height_meters); + geometry_msgs::msg::Pose map_origin; + map_origin.position.x = half_width_meters; + map_origin.position.y = half_height_meters; + map_origin.position.z = 0.0; + map_origin.orientation.x = 0.0; + map_origin.orientation.y = 0.0; + map_origin.orientation.z = 0.0; + map_origin.orientation.w = 1.0; + return map_origin; +} // Helper function to draw line using Bresenham's algorithm -void drawLine(int x0, int y0, int x1, int y1, nav_msgs::msg::OccupancyGrid& grid, int value) { - bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); +void drawLine(int x0, int y0, int x1, int y1, + nav_msgs::msg::OccupancyGrid &grid, int value) { + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (steep) { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) { + std::swap(x0, x1); + std::swap(y0, y1); + } + int dx = x1 - x0; + int dy = std::abs(y1 - y0); + int error = 2 * dy - dx; + int ystep = (y0 < y1) ? 1 : -1; + int xbounds = steep ? grid.info.height : grid.info.width; + int ybounds = steep ? grid.info.width : grid.info.height; + int y = y0; + + for (int x = x0; x <= x1; x++) { if (steep) { - std::swap(x0, y0); - std::swap(x1, y1); + if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { + grid.data[y + x * grid.info.width] = value; + } + } else { + if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { + grid.data[y * grid.info.width + x] = value; + } } - if (x0 > x1) { - std::swap(x0, x1); - std::swap(y0, y1); - } - int dx = x1 - x0; - int dy = std::abs(y1 - y0); - int error = 2*dy - dx; - int ystep = (y0 < y1) ? 1 : -1; - int xbounds = steep ? grid.info.height : grid.info.width; - int ybounds = steep ? grid.info.width : grid.info.height; - int y = y0; - - for (int x = x0; x <= x1; x++) { - if (steep) { - if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { - grid.data[y + x * grid.info.width] = value; - } - } else { - if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { - grid.data[y * grid.info.width + x] = value; - } - } - if(error > 0) - { - y += ystep; - error -= 2*(dx-dy); - } - else - { - error += 2*dy; - } - + if (error > 0) { + y += ystep; + error -= 2 * (dx - dy); + } else { + error += 2 * dy; } + } } -bool isPointInsidePolygon(int px, int py, const geometry_msgs::msg::PolygonStamped& polygon, const nav_msgs::msg::OccupancyGrid& grid) { - int count = 0; - int n = polygon.polygon.points.size(); - for (int i = 0; i < n; i++) { - const geometry_msgs::msg::Point32& start = polygon.polygon.points[i]; - const geometry_msgs::msg::Point32& end = polygon.polygon.points[(i + 1) % n]; - int x0 = start.x / grid.info.resolution + grid.info.width / 2; - int y0 = start.y / grid.info.resolution + grid.info.height / 2; - int x1 = end.x / grid.info.resolution + grid.info.width / 2; - int y1 = end.y / grid.info.resolution + grid.info.height / 2; - - // Check if the line from start to end intersects with the line from (px, py) to (infinity, py) - if ((y0 > py) != (y1 > py)) { // If the point is between the y bounds of the segment - // Calculate x coordinate of the intersection point of the line segment with the horizontal line through py - float intersectX = (y1 - y0 != 0) ? (x1 - x0) * (float)(py - y0) / (float)(y1 - y0) + x0 : x0; - if (px < intersectX) // Ray crossing from the left - count++; - } +bool isPointInsidePolygon(int px, int py, + const geometry_msgs::msg::PolygonStamped &polygon, + const nav_msgs::msg::OccupancyGrid &grid) { + int count = 0; + int n = polygon.polygon.points.size(); + for (int i = 0; i < n; i++) { + const geometry_msgs::msg::Point32 &start = polygon.polygon.points[i]; + const geometry_msgs::msg::Point32 &end = + polygon.polygon.points[(i + 1) % n]; + int x0 = start.x / grid.info.resolution + grid.info.width / 2; + int y0 = start.y / grid.info.resolution + grid.info.height / 2; + int x1 = end.x / grid.info.resolution + grid.info.width / 2; + int y1 = end.y / grid.info.resolution + grid.info.height / 2; + + // Check if the line from start to end intersects with the line from (px, + // py) to (infinity, py) + if ((y0 > py) != + (y1 > py)) { // If the point is between the y bounds of the segment + // Calculate x coordinate of the intersection point of the line segment + // with the horizontal line through py + float intersectX = + (y1 - y0 != 0) ? (x1 - x0) * (float)(py - y0) / (float)(y1 - y0) + x0 + : x0; + if (px < intersectX) // Ray crossing from the left + count++; } - return (count % 2) == 1; // Odd inside, even outside + } + return (count % 2) == 1; // Odd inside, even outside } -void MapManagerNode::fillOutsidePolygon(nav_msgs::msg::OccupancyGrid& grid, const geometry_msgs::msg::PolygonStamped& polygon) { - int outside_value = 50; // Set occupancy value for outside (0-100) - - // Iterate over each pixel in the grid - for (int x = 0; x < grid.info.width; x++) { - for (int y = 0; y < grid.info.height; y++) { - if (!isPointInsidePolygon(x, y, polygon, grid)) { - grid.data[y * grid.info.width + x] = outside_value; - } - } +void MapManagerNode::fillOutsidePolygon( + nav_msgs::msg::OccupancyGrid &grid, + const geometry_msgs::msg::PolygonStamped &polygon) { + int outside_value = 50; // Set occupancy value for outside (0-100) + + // Iterate over each pixel in the grid + for (int x = 0; x < grid.info.width; x++) { + for (int y = 0; y < grid.info.height; y++) { + if (!isPointInsidePolygon(x, y, polygon, grid)) { + grid.data[y * grid.info.width + x] = outside_value; + } } + } } - -void MapManagerNode::insert_landmask(nav_msgs::msg::OccupancyGrid& grid, const geometry_msgs::msg::PolygonStamped& polygon) - { - int value = 100; // Set this to the desired occupancy value for land (0-100) - for (uint i = 0; i < polygon.polygon.points.size(); i++) { - - // if(i > 1){break;} - const geometry_msgs::msg::Point32& current = polygon.polygon.points[i]; - const geometry_msgs::msg::Point32& next = polygon.polygon.points[(i + 1) % polygon.polygon.points.size()]; // Loop back to the first point - // std::cout << "current: " << current.x << " " << current.y << " next: " << next.x << " " << next.y << "\n"; - - // Convert map xy to grid indices - int x0 = current.x / grid.info.resolution + grid.info.width / 2; - int y0 = current.y / grid.info.resolution + grid.info.height / 2; - int x1 = next.x / grid.info.resolution + grid.info.width / 2; - int y1 = next.y / grid.info.resolution + grid.info.height / 2; - // std::cout << "x0: " << x0 << " y0: " << y0 << " x1: " << x1 << " y1: " << y1 << "\n"; - // grid.data[x0 + y0 * grid.info.width] = value; - - - // Draw line on the grid - drawLine(x0, y0, x1, y1, grid, value); - } - +void MapManagerNode::insert_landmask( + nav_msgs::msg::OccupancyGrid &grid, + const geometry_msgs::msg::PolygonStamped &polygon) { + int value = 100; // Set this to the desired occupancy value for land (0-100) + for (uint i = 0; i < polygon.polygon.points.size(); i++) { + + // if(i > 1){break;} + const geometry_msgs::msg::Point32 ¤t = polygon.polygon.points[i]; + const geometry_msgs::msg::Point32 &next = + polygon.polygon + .points[(i + 1) % polygon.polygon.points.size()]; // Loop back to + // the first point + // std::cout << "current: " << current.x << " " << current.y << " next: " << + // next.x << " " << next.y << "\n"; + + // Convert map xy to grid indices + int x0 = current.x / grid.info.resolution + grid.info.width / 2; + int y0 = current.y / grid.info.resolution + grid.info.height / 2; + int x1 = next.x / grid.info.resolution + grid.info.width / 2; + int y1 = next.y / grid.info.resolution + grid.info.height / 2; + // std::cout << "x0: " << x0 << " y0: " << y0 << " x1: " << x1 << " y1: " << + // y1 << "\n"; grid.data[x0 + y0 * grid.info.width] = value; + + // Draw line on the grid + drawLine(x0, y0, x1, y1, grid, value); + } } - -} // namespace map_manager \ No newline at end of file +} // namespace map_manager \ No newline at end of file From c204914bd2ac0f41924911c2b865b907d35ab99f Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Mon, 15 Jul 2024 16:52:06 +0200 Subject: [PATCH 12/67] Working system after physical testing --- .../dp_guidance/dp_guidance_node.py | 4 +++- .../dp_guidance/reference_filter.py | 21 ------------------- .../hybridpath_guidance/waypoint_node.py | 3 ++- .../pid_controller/pid_controller.py | 18 ++++++++++++++-- .../pid_controller/pid_controller_node.py | 2 ++ 5 files changed, 23 insertions(+), 25 deletions(-) diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index 317332da..1ddbf8cf 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py @@ -41,7 +41,7 @@ def __init__(self): self.eta_received = False self.eta_logger = True - self.yaw_ref = 0 + self.yaw_ref = 0. self.xd = np.zeros(9) @@ -52,6 +52,7 @@ def __init__(self): self.timer = self.create_timer(timer_period, self.guidance_callback) def waypoint_callback(self, request, response): + self.init_pos = False self.waypoints = request.waypoint self.get_logger().info(f'Received waypoints: {self.waypoints}') self.xd = np.zeros(9) @@ -64,6 +65,7 @@ def eta_callback(self, msg: Odometry): self.eta = odometrymsg_to_state(msg)[:3] self.eta_received = True self.yaw_publisher.publish(Float32(data=self.eta[2])) + self.yaw_ref_publisher.publish(Float32(data=self.yaw_ref)) def guidance_callback(self): if self.waypoints_received and self.eta_received: diff --git a/guidance/dp_guidance/dp_guidance/reference_filter.py b/guidance/dp_guidance/dp_guidance/reference_filter.py index 1ad4f344..45b243f3 100644 --- a/guidance/dp_guidance/dp_guidance/reference_filter.py +++ b/guidance/dp_guidance/dp_guidance/reference_filter.py @@ -2,7 +2,6 @@ import numpy as np import control -import matplotlib.pyplot as plt class ReferenceFilter: def __init__(self): @@ -50,23 +49,3 @@ def rotationMatrix(psi): [np.sin(psi), np.cos(psi), 0], [0, 0, 1]]) return R - - -if __name__ == "__main__": - refFilter = ReferenceFilter() - ref_pos = np.array([5, 10, 0]) - t = np.arange(0, 100, 0.1) - N = len(t) - x_d = np.zeros((N, 9)) - - for k in range(1,N): - x_d[k,:] = refFilter.step(ref_pos, x_d[k-1, :]) - - eta_d = refFilter.get_eta(x_d) - nu_d = refFilter.get_nu(x_d) - plt.figure() - plt.plot(t, eta_d[:,0]) - - plt.figure() - plt.plot(t, nu_d[:,0]) - plt.show() \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py index 59b5df38..c0e60668 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py @@ -39,7 +39,8 @@ def eta_callback(self, msg: Odometry): def send_request(self): if self.eta_received: req = Waypoint.Request() - wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 0.]] + wp_list = [[self.eta[0] + 5., self.eta[1]], [self.eta[0] + 5., self.eta[1] + 5.], [self.eta[0], self.eta[1] + 5.]] + # wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 0.]] req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list] self.get_logger().info(f'Sending request: {req}') self.future = self.client.call_async(req) diff --git a/motion/pid_controller/pid_controller/pid_controller.py b/motion/pid_controller/pid_controller/pid_controller.py index 1bd218e1..29624c66 100644 --- a/motion/pid_controller/pid_controller/pid_controller.py +++ b/motion/pid_controller/pid_controller/pid_controller.py @@ -10,6 +10,9 @@ def __init__(self): self.Ki = np.eye(3) self.Kd = np.eye(3) + self.upper = np.array([40, 40, 40]) + self.lower = np.array([-40, -40, -40]) + # self.tau_max = np.array([100, 100, 100]) def update_parameters(self, Kp, Ki, Kd): @@ -23,7 +26,7 @@ def calculate_control_input(self, eta, eta_d, eta_dot, dt): self.error_sum = np.clip(self.error_sum, -20, 20) p = self.Kp @ error - i = 0 #self.Ki @ self.error_sum + i = self.Ki @ self.error_sum d = self.Kd @ eta_dot self.last_error = error @@ -45,4 +48,15 @@ def calculate_control_input(self, eta, eta_d, eta_dot, dt): # elif tau[2] < -self.tau_max[2]: # tau[2] = -self.tau_max[2] - return tau \ No newline at end of file + return tau + + @staticmethod + def cap_array(arr: np.ndarray, lower: np.ndarray, upper: np.ndarray): + new_arr = np.zeros(3) + for i in range(len(arr)): + if arr[i] > upper[i]: + new_arr[i] = upper[i] + elif arr[i] < lower[i]: + new_arr[i] = lower + else: + new_arr[i] = arr[i] diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py index dfff3a08..a8927ea0 100755 --- a/motion/pid_controller/pid_controller/pid_controller_node.py +++ b/motion/pid_controller/pid_controller/pid_controller_node.py @@ -60,6 +60,8 @@ def __init__(self): self.get_logger().info("pid_controller_node started") def update_controller_parameters(self): + self.pid_controller.error_sum = np.zeros(3) + Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value From 4f63bb1407fcd5b278636a1522880126d2b3bd52 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Sat, 20 Jul 2024 22:37:02 +0200 Subject: [PATCH 13/67] Wall update Added functionality to map walls into the grid. Wall width mapped to grid is customizable through param in landmark server --- ...lygon.yaml => land_polygon_grillstad.yaml} | 0 .../config/sitaw/land_polygon_office.yaml | 16 +++ .../config/sitaw/landmark_server_params.yaml | 3 +- .../config/sitaw/map_manager_params.yaml | 2 +- .../include/landmark_server/grid_manager.hpp | 56 +------- .../landmark_server/landmark_server.hpp | 2 +- .../params/landmark_server_params.yaml | 3 +- mission/landmark_server/src/grid_manager.cpp | 133 +----------------- .../landmark_server/src/landmark_server.cpp | 68 +++++---- mission/map_manager/src/map_manager_ros.cpp | 12 +- 10 files changed, 78 insertions(+), 217 deletions(-) rename asv_setup/config/sitaw/{land_polygon.yaml => land_polygon_grillstad.yaml} (100%) create mode 100644 asv_setup/config/sitaw/land_polygon_office.yaml diff --git a/asv_setup/config/sitaw/land_polygon.yaml b/asv_setup/config/sitaw/land_polygon_grillstad.yaml similarity index 100% rename from asv_setup/config/sitaw/land_polygon.yaml rename to asv_setup/config/sitaw/land_polygon_grillstad.yaml diff --git a/asv_setup/config/sitaw/land_polygon_office.yaml b/asv_setup/config/sitaw/land_polygon_office.yaml new file mode 100644 index 00000000..90a4898a --- /dev/null +++ b/asv_setup/config/sitaw/land_polygon_office.yaml @@ -0,0 +1,16 @@ +63.41489120183045 10.39705552070149 +63.41497265487507 10.39783732516908 +63.41538547016246 10.397651181248118 +63.415431749532054 10.398250978326427 +63.415048554100395 10.398449531842004 +63.41512075073269 10.399185834462275 +63.41495229164123 10.399305793877936 +63.41486158248972 10.398540535536645 +63.41453021396673 10.398747362115373 +63.4145246602717 10.398631539231284 +63.414280296625655 10.39877218130482 +63.41424142039905 10.398197203415958 +63.4144598670836 10.398110336252891 +63.41444505719146 10.397990376837232 +63.41478938520484 10.397841461700548 +63.41472089017681 10.397167207053899 \ No newline at end of file diff --git a/asv_setup/config/sitaw/landmark_server_params.yaml b/asv_setup/config/sitaw/landmark_server_params.yaml index 52e3ba35..92002733 100644 --- a/asv_setup/config/sitaw/landmark_server_params.yaml +++ b/asv_setup/config/sitaw/landmark_server_params.yaml @@ -1,4 +1,5 @@ landmark_server_node: ros__parameters: fixed_frame: map - target_frame: base_link \ No newline at end of file + target_frame: base_link + wall_width: 0.1 \ No newline at end of file diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml index e2c48285..1fa57cf9 100644 --- a/asv_setup/config/sitaw/map_manager_params.yaml +++ b/asv_setup/config/sitaw/map_manager_params.yaml @@ -1,7 +1,7 @@ map_manager_node: ros__parameters: use_predef_landmask: true - landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon.yaml" + landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml" map_resolution: 0.2 map_width: 1000 map_height: 1000 diff --git a/mission/landmark_server/include/landmark_server/grid_manager.hpp b/mission/landmark_server/include/landmark_server/grid_manager.hpp index 78f09706..31ab72c7 100644 --- a/mission/landmark_server/include/landmark_server/grid_manager.hpp +++ b/mission/landmark_server/include/landmark_server/grid_manager.hpp @@ -5,74 +5,28 @@ namespace landmark_server { -enum class ShapeType : uint8_t { SPHERE = 0, PRISM = 1 }; - -struct Circle { - float radius; - float x_centre; - float y_centre; -}; - -struct Point { - float x; - float y; - float z; -}; - -struct Polygon { - std::vector vertices; -}; - -struct Pose { - float x; - float y; - float yaw; -}; - -struct ShapeInfo { - ShapeType type; - Circle circle; - Polygon polygon; - Pose pose; -}; - +/** + * @class GridManager + * @brief A class for managing the grid map. + */ class GridManager { public: - using Grid = Eigen::Array; - - // GridManager(std::vector grid, float resolution, uint32_t height, - // uint32_t width); GridManager(float resolution, uint32_t height, uint32_t width); - ~GridManager() = default; - const Grid &get_grid() const; + ~GridManager() = default; void update_grid(int8_t *grid, const Eigen::Array &polygon, int value); - std::tuple world_to_grid(float x, float y); - - void handle_circle(int xc, int yc, int r, int value); - - void draw_circle( - int xc, int yc, int x, int y, int value, - Eigen::Block &block, - int xmin, int ymin); - void handle_polygon(int8_t *grid, const Eigen::Array &vertices, int value); - void draw_polygon( - int x0, int y0, int x1, int y1, - Eigen::Block &block, - int value); void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value); private: - Grid grid_; float resolution_; uint32_t height_; uint32_t width_; diff --git a/mission/landmark_server/include/landmark_server/landmark_server.hpp b/mission/landmark_server/include/landmark_server/landmark_server.hpp index 7c3115a6..9ce86854 100644 --- a/mission/landmark_server/include/landmark_server/landmark_server.hpp +++ b/mission/landmark_server/include/landmark_server/landmark_server.hpp @@ -71,7 +71,7 @@ class LandmarkServerNode : public rclcpp::Node { const vortex_msgs::msg::LandmarkArray::SharedPtr msg); Eigen::Array - get_convex_hull(const shape_msgs::msg::SolidPrimitive &solid_primitive); + get_convex_hull(const vortex_msgs::msg::Landmark& landmark); void get_grid(); diff --git a/mission/landmark_server/params/landmark_server_params.yaml b/mission/landmark_server/params/landmark_server_params.yaml index 8455dd85..c334181d 100644 --- a/mission/landmark_server/params/landmark_server_params.yaml +++ b/mission/landmark_server/params/landmark_server_params.yaml @@ -1,4 +1,5 @@ landmark_server_node: ros__parameters: fixed_frame: world - target_frame: base_link \ No newline at end of file + target_frame: base_link + wall_width: 0.1 \ No newline at end of file diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp index 492c129d..b727165e 100644 --- a/mission/landmark_server/src/grid_manager.cpp +++ b/mission/landmark_server/src/grid_manager.cpp @@ -3,113 +3,22 @@ namespace landmark_server { -// GridManager::GridManager(std::vector grid, float resolution, uint32_t -// height, uint32_t width) -// : resolution_(resolution), height_(height), width_(width) -// { - -// if (grid.size() != height * width) { -// throw std::runtime_error("Grid size does not match height * width"); -// } -// grid_ = Eigen::Map> -// (grid.data(), height, width); -// } GridManager::GridManager(float resolution, uint32_t height, uint32_t width) : resolution_(resolution), height_(height), width_(width) {} -const GridManager::Grid &GridManager::get_grid() const { return grid_; } - void GridManager::update_grid( int8_t *grid, const Eigen::Array &polygon, int value) { handle_polygon(grid, polygon, value); } -// void GridManager::update_grid(const ShapeInfo& shape_info, int value) -// { -// switch (shape_info.type) -// { -// case ShapeType::SPHERE: -// handle_circle(shape_info.circle.x_centre, shape_info.circle.y_centre, -// shape_info.circle.radius, value); break; -// case ShapeType::PRISM: -// handle_polygon(shape_info.polygon.vertices, value); -// default: -// break; -// } -// } - -void GridManager::draw_circle( - int xc, int yc, int x, int y, int value, - Eigen::Block &block, - int xmin, int ymin) { - // Translate coordinates to block local coordinates - int bx = xc - xmin; - int by = yc - ymin; - - if (bx + x < block.rows() && by + y < block.cols() && bx + x >= 0 && - by + y >= 0) { - block(bx + x, by + y) = value; - block(bx - x, by + y) = value; - block(bx + x, by - y) = value; - block(bx - x, by - y) = value; - block(bx + y, by + x) = value; - block(bx - y, by + x) = value; - block(bx + y, by - x) = value; - block(bx - y, by - x) = value; - } -} - -void GridManager::handle_circle(int xc, int yc, int r, int value) { - int xmin = static_cast((xc - r) / resolution_ + width_ / 2); - int ymin = static_cast((yc - r) / resolution_ + width_ / 2); - int xmax = static_cast((xc + r) / resolution_ + width_ / 2); - int ymax = static_cast((yc + r) / resolution_ + width_ / 2); - - if (xmin < 0 || ymin < 0 || xmax >= grid_.rows() || ymax >= grid_.cols()) { - throw std::runtime_error("Circle exceeds grid boundaries"); - } - - auto block = grid_.block(xmin, ymin, xmax - xmin + 1, ymax - ymin + 1); - int x = 0, y = r; - int d = 3 - 2 * r; - draw_circle(xc, yc, x, y, value, block, xmin, ymin); - while (y >= x) { - x++; - if (d > 0) { - y--; - d = d + 4 * (x - y) + 10; - } else { - d = d + 4 * x + 6; - } - draw_circle(xc, yc, x, y, value, block, xmin, ymin); - } -} - void GridManager::handle_polygon( int8_t *grid, const Eigen::Array &vertices, int value) { - // Determine the bounding box of the polygon - // float min_x = vertices.row(0).minCoeff(); - // float min_y = vertices.row(1).minCoeff(); - // float max_x = vertices.row(0).maxCoeff(); - // float max_y = vertices.row(1).maxCoeff(); - - // int xmin = static_cast(min_x / resolution_ + width_ / 2); - // int ymin = static_cast(min_y / resolution_ + height_ / 2); - // int xmax = static_cast(max_x / resolution_ + width_ / 2); - // int ymax = static_cast(max_y / resolution_ + height_ / 2); - - // if (xmin < 0 || ymin < 0 || xmax >= height_ || ymax >= width_) { - // throw std::runtime_error("Polygon exceeds grid boundaries"); - // } - - // Draw the polygon + // Convert to grid coordinates for (Eigen::Index i = 0; i < vertices.cols(); ++i) { Eigen::Index j = (i + 1) % vertices.cols(); - std::cout << "i: " << i << " j: " << j << std::endl; int x0 = static_cast(vertices(0, i) / resolution_ + width_ / 2); int y0 = static_cast(vertices(1, i) / resolution_ + height_ / 2); int x1 = static_cast(vertices(0, j) / resolution_ + width_ / 2); @@ -119,46 +28,6 @@ void GridManager::handle_polygon( } } -void GridManager::draw_polygon( - int x0, int y0, int x1, int y1, - Eigen::Block &block, - int value) { - bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); - if (steep) { - std::swap(x0, y0); - std::swap(x1, y1); - } - if (x0 > x1) { - std::swap(x0, x1); - std::swap(y0, y1); - } - int dx = x1 - x0; - int dy = std::abs(y1 - y0); - int error = 2 * dy - dx; - int ystep = (y0 < y1) ? 1 : -1; - int xbounds = steep ? block.rows() : block.cols(); - int ybounds = steep ? block.cols() : block.rows(); - int y = y0; - - for (int x = x0; x <= x1; x++) { - if (steep) { - if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { - block(y, x) += value; - } - } else { - if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { - block(x, y) += value; - } - } - if (error > 0) { - y += ystep; - error -= 2 * (dx - dy); - } else { - error += 2 * dy; - } - } -} - void GridManager::draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value) { bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp index 1cc35ed8..8fc4f175 100644 --- a/mission/landmark_server/src/landmark_server.cpp +++ b/mission/landmark_server/src/landmark_server.cpp @@ -47,6 +47,8 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) declare_parameter("fixed_frame", "world"); declare_parameter("target_frame", "base_link"); + declare_parameter("wall_width", 0.1); + // Create the act. action_server_ = rclcpp_action::create_server( @@ -61,17 +63,49 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) } Eigen::Array LandmarkServerNode::get_convex_hull( - const shape_msgs::msg::SolidPrimitive &solid_primitive) { + const vortex_msgs::msg::Landmark &landmark) { + if (landmark.landmark_type == vortex_msgs::msg::Landmark::WALL) { + Eigen::Array hull(2, 4); + float x1 = landmark.shape.polygon.points[0].x; + float y1 = landmark.shape.polygon.points[0].y; + float x2 = landmark.shape.polygon.points[1].x; + float y2 = landmark.shape.polygon.points[1].y; + + // Calculate the direction vector + float dx = x2 - x1; + float dy = y2 - y1; + + // Normalize the direction vector + float length = std::sqrt(dx * dx + dy * dy); + dx /= length; + dy /= length; + + // Perpendicular vector with the given width + float wall_width = get_parameter("wall_width").as_double(); + float wx = -dy * wall_width/2; // half the width to each side + float wy = dx * wall_width/2; + + // Calculate the four points of the rectangle in clockwise order + hull(0, 0) = x1 + wx; + hull(1, 0) = y1 + wy; + hull(0, 1) = x2 + wx; + hull(1, 1) = y2 + wy; + hull(0, 2) = x2 - wx; + hull(1, 2) = y2 - wy; + hull(0, 3) = x1 - wx; + hull(1, 3) = y1 - wy; + return hull; + } pcl::PointCloud cluster; - cluster.resize(solid_primitive.polygon.points.size()); - for (size_t i = 0; i < solid_primitive.polygon.points.size(); i++) { - cluster.points[i].x = solid_primitive.polygon.points[i].x; - cluster.points[i].y = solid_primitive.polygon.points[i].y; + cluster.resize(landmark.shape.polygon.points.size()); + for (size_t i = 0; i < landmark.shape.polygon.points.size(); i++) { + cluster.points[i].x = landmark.shape.polygon.points[i].x; + cluster.points[i].y = landmark.shape.polygon.points[i].y; cluster.points[i].z = 1.0; } sensor_msgs::msg::PointCloud2 cluster_msg; pcl::toROSMsg(cluster, cluster_msg); - cluster_msg.header.frame_id = "world"; + cluster_msg.header.frame_id = get_parameter("fixed_frame").as_string(); cluster_publisher_->publish(cluster_msg); pcl::PointCloud convex_hull; pcl::ConvexHull chull; @@ -80,16 +114,13 @@ Eigen::Array LandmarkServerNode::get_convex_hull( chull.reconstruct(convex_hull); sensor_msgs::msg::PointCloud2 hull_msg; pcl::toROSMsg(convex_hull, hull_msg); - hull_msg.header.frame_id = "world"; + hull_msg.header.frame_id = get_parameter("fixed_frame").as_string(); convex_hull_publisher_->publish(hull_msg); - RCLCPP_INFO(this->get_logger(), "Convex hull size: %ld", convex_hull.size()); Eigen::Array hull(2, convex_hull.size()); for (size_t i = 0; i < convex_hull.size(); i++) { hull(0, i) = convex_hull.points[i].x; hull(1, i) = convex_hull.points[i].y; } - RCLCPP_INFO(this->get_logger(), "Convex hull: %ld", hull.cols()); - std::cout << "HUll" << hull << std::endl; return hull; } @@ -165,12 +196,11 @@ void LandmarkServerNode::landmarksRecievedCallback( } else { // Add the new landmark grid_manager_->update_grid(grid_msg_.data.data(), - get_convex_hull(landmark.shape), 200); + get_convex_hull(landmark), 200); storedLandmarks_->landmarks.push_back(landmark); } continue; } - if (it == storedLandmarks_->landmarks.end()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 5000, @@ -179,26 +209,16 @@ void LandmarkServerNode::landmarksRecievedCallback( } grid_manager_->update_grid(grid_msg_.data.data(), - get_convex_hull((*it).shape), -200); + get_convex_hull((*it)), -200); if (landmark.action == vortex_msgs::msg::Landmark::REMOVE_ACTION) { storedLandmarks_->landmarks.erase(it); } else if (landmark.action == vortex_msgs::msg::Landmark::UPDATE_ACTION) { grid_manager_->update_grid(grid_msg_.data.data(), - get_convex_hull(landmark.shape), 200); + get_convex_hull(landmark), 200); *it = landmark; } } - std_msgs::msg::Header header; - - // header.frame_id = get_parameter("fixed_frame").as_string(); - // header.stamp = rclcpp::Clock().now(); - - // // Convert the Eigen array to std::vector - // auto grid_eigen = grid_manager_->get_grid(); - // std::vector grid_data(grid_eigen.data(), grid_eigen.data() + - // grid_eigen.size()); grid_msg_.header = header; grid_msg_.data = - // grid_data; grid_msg_.header.stamp = rclcpp::Clock().now(); // Publish the landmarks diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index b2b9987a..be5eba87 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -17,7 +17,7 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) qos_profile_transient_local); map_origin_sub_ = this->create_subscription( - "map_origin", qos_transient_local, + "/map/origin", qos_transient_local, std::bind(&MapManagerNode::mapOriginCallback, this, std::placeholders::_1)); @@ -28,7 +28,7 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) declare_parameter("map_resolution", 0.2); declare_parameter("map_width", 1000); declare_parameter("map_height", 1000); - declare_parameter("map_frame_id", "world"); + declare_parameter("frame_id", "map"); landmask_file_ = get_parameter("landmask_file").as_string(); landmask_pub_ = this->create_publisher( @@ -187,7 +187,7 @@ MapManagerNode::readPolygonFromFile(const std::string &filename) { geometry_msgs::msg::PolygonStamped MapManagerNode::processCoordinates( const std::vector> &coordinates) { geometry_msgs::msg::PolygonStamped polygon; - polygon.header.frame_id = get_parameter("map_frame_id").as_string(); + polygon.header.frame_id = get_parameter("frame_id").as_string(); polygon.header.stamp = this->now(); std::cout << "coords size: " << coordinates.size() << "\n"; for (const auto &coord : coordinates) { @@ -204,7 +204,7 @@ geometry_msgs::msg::PolygonStamped MapManagerNode::processCoordinates( nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() { nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = get_parameter("map_frame_id").as_string(); + grid.header.frame_id = get_parameter("frame_id").as_string(); grid.header.stamp = this->now(); grid.info.resolution = get_parameter("map_resolution").as_double(); grid.info.width = get_parameter("map_width").as_int(); @@ -319,8 +319,8 @@ void MapManagerNode::fillOutsidePolygon( int outside_value = 50; // Set occupancy value for outside (0-100) // Iterate over each pixel in the grid - for (int x = 0; x < grid.info.width; x++) { - for (int y = 0; y < grid.info.height; y++) { + for (size_t x = 0; x < grid.info.width; x++) { + for (size_t y = 0; y < grid.info.height; y++) { if (!isPointInsidePolygon(x, y, polygon, grid)) { grid.data[y * grid.info.width + x] = outside_value; } From 447125b32639c8252b62b5bf5e0ba3b9b1132e56 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Sat, 20 Jul 2024 20:37:43 +0000 Subject: [PATCH 14/67] Committing clang-format changes --- .../landmark_server/landmark_server.hpp | 2 +- mission/landmark_server/src/grid_manager.cpp | 1 - .../landmark_server/src/landmark_server.cpp | 69 +++++++++---------- 3 files changed, 35 insertions(+), 37 deletions(-) diff --git a/mission/landmark_server/include/landmark_server/landmark_server.hpp b/mission/landmark_server/include/landmark_server/landmark_server.hpp index 9ce86854..fbb90d2c 100644 --- a/mission/landmark_server/include/landmark_server/landmark_server.hpp +++ b/mission/landmark_server/include/landmark_server/landmark_server.hpp @@ -71,7 +71,7 @@ class LandmarkServerNode : public rclcpp::Node { const vortex_msgs::msg::LandmarkArray::SharedPtr msg); Eigen::Array - get_convex_hull(const vortex_msgs::msg::Landmark& landmark); + get_convex_hull(const vortex_msgs::msg::Landmark &landmark); void get_grid(); diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp index b727165e..a46d3688 100644 --- a/mission/landmark_server/src/grid_manager.cpp +++ b/mission/landmark_server/src/grid_manager.cpp @@ -3,7 +3,6 @@ namespace landmark_server { - GridManager::GridManager(float resolution, uint32_t height, uint32_t width) : resolution_(resolution), height_(height), width_(width) {} diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp index 8fc4f175..899213bb 100644 --- a/mission/landmark_server/src/landmark_server.cpp +++ b/mission/landmark_server/src/landmark_server.cpp @@ -48,7 +48,6 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) declare_parameter("fixed_frame", "world"); declare_parameter("target_frame", "base_link"); declare_parameter("wall_width", 0.1); - // Create the act. action_server_ = rclcpp_action::create_server( @@ -64,38 +63,38 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) Eigen::Array LandmarkServerNode::get_convex_hull( const vortex_msgs::msg::Landmark &landmark) { - if (landmark.landmark_type == vortex_msgs::msg::Landmark::WALL) { - Eigen::Array hull(2, 4); - float x1 = landmark.shape.polygon.points[0].x; - float y1 = landmark.shape.polygon.points[0].y; - float x2 = landmark.shape.polygon.points[1].x; - float y2 = landmark.shape.polygon.points[1].y; - - // Calculate the direction vector - float dx = x2 - x1; - float dy = y2 - y1; - - // Normalize the direction vector - float length = std::sqrt(dx * dx + dy * dy); - dx /= length; - dy /= length; - - // Perpendicular vector with the given width - float wall_width = get_parameter("wall_width").as_double(); - float wx = -dy * wall_width/2; // half the width to each side - float wy = dx * wall_width/2; - - // Calculate the four points of the rectangle in clockwise order - hull(0, 0) = x1 + wx; - hull(1, 0) = y1 + wy; - hull(0, 1) = x2 + wx; - hull(1, 1) = y2 + wy; - hull(0, 2) = x2 - wx; - hull(1, 2) = y2 - wy; - hull(0, 3) = x1 - wx; - hull(1, 3) = y1 - wy; - return hull; - } + if (landmark.landmark_type == vortex_msgs::msg::Landmark::WALL) { + Eigen::Array hull(2, 4); + float x1 = landmark.shape.polygon.points[0].x; + float y1 = landmark.shape.polygon.points[0].y; + float x2 = landmark.shape.polygon.points[1].x; + float y2 = landmark.shape.polygon.points[1].y; + + // Calculate the direction vector + float dx = x2 - x1; + float dy = y2 - y1; + + // Normalize the direction vector + float length = std::sqrt(dx * dx + dy * dy); + dx /= length; + dy /= length; + + // Perpendicular vector with the given width + float wall_width = get_parameter("wall_width").as_double(); + float wx = -dy * wall_width / 2; // half the width to each side + float wy = dx * wall_width / 2; + + // Calculate the four points of the rectangle in clockwise order + hull(0, 0) = x1 + wx; + hull(1, 0) = y1 + wy; + hull(0, 1) = x2 + wx; + hull(1, 1) = y2 + wy; + hull(0, 2) = x2 - wx; + hull(1, 2) = y2 - wy; + hull(0, 3) = x1 - wx; + hull(1, 3) = y1 - wy; + return hull; + } pcl::PointCloud cluster; cluster.resize(landmark.shape.polygon.points.size()); for (size_t i = 0; i < landmark.shape.polygon.points.size(); i++) { @@ -208,8 +207,8 @@ void LandmarkServerNode::landmarksRecievedCallback( continue; } - grid_manager_->update_grid(grid_msg_.data.data(), - get_convex_hull((*it)), -200); + grid_manager_->update_grid(grid_msg_.data.data(), get_convex_hull((*it)), + -200); if (landmark.action == vortex_msgs::msg::Landmark::REMOVE_ACTION) { storedLandmarks_->landmarks.erase(it); From 08afdbaa2764b952750f70516d5a37c0ed920119 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Tue, 23 Jul 2024 21:13:31 +0200 Subject: [PATCH 15/67] qos fix --- .../landmark_server/src/landmark_server.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp index 899213bb..70ce01f3 100644 --- a/mission/landmark_server/src/landmark_server.cpp +++ b/mission/landmark_server/src/landmark_server.cpp @@ -12,23 +12,32 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) // Define the quality of service profile for publisher and subscriber rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), + auto qos_best_effort = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + // Set QoS profile + rmw_qos_profile_t qos_profile_sub = rmw_qos_profile_default; + qos_profile_sub.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + qos_profile_sub.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos_profile_sub.depth = 10; // You can adjust this depth as needed for your use case + + auto qos_reliable = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sub.history, qos_profile_sub.depth), qos_profile_sub); + + storedLandmarks_ = std::make_shared(); landmark_sub_ = this->create_subscription( - "target_tracking/landmarks", qos, + "target_tracking/landmarks", qos_reliable, std::bind(&LandmarkServerNode::landmarksRecievedCallback, this, _1)); landmarkPublisher_ = - this->create_publisher("landmarks_out", qos); + this->create_publisher("landmarks_out", qos_best_effort); posePublisher_ = this->create_publisher( - "landmark_poses_out", qos); + "landmark_poses_out", qos_best_effort); gridPublisher_ = - this->create_publisher("grid", qos); + this->create_publisher("grid", qos_best_effort); rmw_qos_profile_t qos_profile_sensor = rmw_qos_profile_sensor_data; qos_profile_sensor.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; From cfc4853e28d6164ef8186c94fd0c644dfb0b56c7 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Tue, 23 Jul 2024 19:14:00 +0000 Subject: [PATCH 16/67] Committing clang-format changes --- .../landmark_server/src/landmark_server.cpp | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp index 70ce01f3..c4b896bb 100644 --- a/mission/landmark_server/src/landmark_server.cpp +++ b/mission/landmark_server/src/landmark_server.cpp @@ -12,17 +12,19 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) // Define the quality of service profile for publisher and subscriber rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - auto qos_best_effort = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), - qos_profile); + auto qos_best_effort = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - // Set QoS profile - rmw_qos_profile_t qos_profile_sub = rmw_qos_profile_default; - qos_profile_sub.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - qos_profile_sub.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos_profile_sub.depth = 10; // You can adjust this depth as needed for your use case - - auto qos_reliable = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sub.history, qos_profile_sub.depth), qos_profile_sub); + // Set QoS profile + rmw_qos_profile_t qos_profile_sub = rmw_qos_profile_default; + qos_profile_sub.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + qos_profile_sub.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos_profile_sub.depth = + 10; // You can adjust this depth as needed for your use case + auto qos_reliable = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile_sub.history, qos_profile_sub.depth), + qos_profile_sub); storedLandmarks_ = std::make_shared(); @@ -36,8 +38,8 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) posePublisher_ = this->create_publisher( "landmark_poses_out", qos_best_effort); - gridPublisher_ = - this->create_publisher("grid", qos_best_effort); + gridPublisher_ = this->create_publisher( + "grid", qos_best_effort); rmw_qos_profile_t qos_profile_sensor = rmw_qos_profile_sensor_data; qos_profile_sensor.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; From 0131dcb25b0fded374f5f174f57239c006bb3194 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christopher=20Str=C3=B8m?= Date: Thu, 25 Jul 2024 19:10:57 +0200 Subject: [PATCH 17/67] Add sim launch for freya MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Christopher Strøm --- asv_setup/launch/sim_freya.launch.py | 61 ++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 asv_setup/launch/sim_freya.launch.py diff --git a/asv_setup/launch/sim_freya.launch.py b/asv_setup/launch/sim_freya.launch.py new file mode 100644 index 00000000..f87f63b2 --- /dev/null +++ b/asv_setup/launch/sim_freya.launch.py @@ -0,0 +1,61 @@ +from os import path +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + # Set environment variable + set_env_var = SetEnvironmentVariable( + name='ROSCONSOLE_FORMAT', + value='[${severity}] [${time}] [${node}]: ${message}' + ) + + + # Thruster Allocator launch + thruster_allocator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + path.join(get_package_share_directory('thruster_allocator'), 'launch', 'thruster_allocator.launch.py') + ) + ) + + # Joystick node + joy_node = Node( + package='joy', + executable='joy_node', + name='joystick_driver', + output='screen', + parameters=[ + {'deadzone': 0.15}, + {'autorepeat_rate': 100.0}, + ], + remappings=[ + ('/joy', '/joystick/joy'), + ], + ) + + # Joystick interface launch + joystick_interface_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + path.join(get_package_share_directory('joystick_interface'), 'launch', 'joystick_interface.launch.py') + ) + ) + + # Vortex Sim Interface launch + vortex_sim_interface_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + path.join(get_package_share_directory('vortex_sim_interface'), 'launch', 'vortex_sim_interface.launch.py') + ) + ) + + # Return launch description + return LaunchDescription([ + set_env_var, + thruster_allocator_launch, + joy_node, + joystick_interface_launch, + vortex_sim_interface_launch, + ]) \ No newline at end of file From 3e3b7dea16501c74c1332a708c31406bf18d0589 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Fri, 26 Jul 2024 15:22:25 +0200 Subject: [PATCH 18/67] docking task control logic for the asv to navigate through the initial buoy formation of the docking task --- mission/docking_task/CMakeLists.txt | 77 ++ .../include/docking_task/docking_task_ros.hpp | 158 ++++ .../launch/docking_task_launch.py | 17 + mission/docking_task/package.xml | 28 + .../params/docking_task_params.yaml | 32 + .../docking_task/src/docking_task_node.cpp | 14 + mission/docking_task/src/docking_task_ros.cpp | 681 ++++++++++++++++++ mission/map_manager/src/map_manager_ros.cpp | 22 +- 8 files changed, 1018 insertions(+), 11 deletions(-) create mode 100644 mission/docking_task/CMakeLists.txt create mode 100644 mission/docking_task/include/docking_task/docking_task_ros.hpp create mode 100644 mission/docking_task/launch/docking_task_launch.py create mode 100644 mission/docking_task/package.xml create mode 100644 mission/docking_task/params/docking_task_params.yaml create mode 100644 mission/docking_task/src/docking_task_node.cpp create mode 100644 mission/docking_task/src/docking_task_ros.cpp diff --git a/mission/docking_task/CMakeLists.txt b/mission/docking_task/CMakeLists.txt new file mode 100644 index 00000000..15bfb0ce --- /dev/null +++ b/mission/docking_task/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(docking_task) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +include_directories(include) + +add_executable(docking_task_node + src/docking_task_node.cpp + src/docking_task_ros.cpp) + +# target_include_directories(dock_localization_node PUBLIC +# $ +# $) + +# target_compile_features(dock_localization_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +target_link_libraries(docking_task_node + tf2::tf2 + tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs +) + +ament_target_dependencies(docking_task_node + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs +) + +install( + DIRECTORY include/ + DESTINATION include + +) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME}/ + ) + +install(DIRECTORY + launch + params + DESTINATION share/${PROJECT_NAME}/ +) + + +ament_package() diff --git a/mission/docking_task/include/docking_task/docking_task_ros.hpp b/mission/docking_task/include/docking_task/docking_task_ros.hpp new file mode 100644 index 00000000..6502f01c --- /dev/null +++ b/mission/docking_task/include/docking_task/docking_task_ros.hpp @@ -0,0 +1,158 @@ +#ifndef DOCKING_TASK_ROS_HPP +#define DOCKING_TASK_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace docking_task { + +struct LandmarkWithID { + geometry_msgs::msg::Pose pose_map_frame; + geometry_msgs::msg::Pose pose_base_link_frame; + uint32_t id; + }; + +/** + * @class DockingTaskNode + * @brief A class representing a node for handling dock localization in a ROS 2 system. + * + * This class inherits from rclcpp::Node and provides functionality for + * localizing the dock in the map. + */ +class DockingTaskNode : public rclcpp::Node { +public: + /** + * @brief Constructor for the DockingTaskNode class. + * + * @param options The options for configuring the node. + */ + explicit DockingTaskNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + + /** + * @brief Destructor for the DockLocalizationNode class. + */ + ~DockingTaskNode(){}; + + /** + * @brief Main task for the DockingTaskNode class. + */ + void main_task(); + + /** + * @brief Detect the closest buoy pair and set a waypoint between them + * + * @return A pair of landmarks representing the closest buoy pair + */ + std::pair initial_waypoint(); + + /** + * @brief Predict the 6-tuple formation of buoys + * + * @param landmark1 The first landmark/buoy used for initial waypoint + * @param landmark2 The second landmark/buoy used for initial waypoint + * + * @return The predicted posistion of the 6-tuple formation of buoys in odom frame + */ + Eigen::Array predict_buoy_formation(const LandmarkWithID &buoy1, const LandmarkWithID &buoy2) const; + + /** + * @brief Generate the reward matrix for the auction algorithm + * + * @param predicted_positions The predicted positions of the buoys + * @param measured_positions The measured positions of landmarks + * + * @return The reward matrix for the auction algorithm + */ + Eigen::MatrixXd generate_reward_matrix(const Eigen::Array& predicted_positions, const Eigen::MatrixXd& measured_positions); + + /** + * @brief The auction algorithm for the assignment problem + * + * @param reward_matrix The reward matrix for the auction algorithm + * + * @return The assignment of landmarks to buoys + */ + Eigen::VectorXi auction_algorithm(const Eigen::MatrixXd &reward_matrix); + + /** + * @brief Navigate the ASV through the formation of buoys. First travels to waypoint between second pair of buoys, + * then navigates through the formation of buoys and returns control when asv is 0.2m away from crossing the last buoy pair. + * + * @param predicted_positions The predicted positions of the buoys + */ + void navigate_formation(const Eigen::Array& predicted_positions); + + /** + * @brief Calculate the coordinates of a gps point in the map frame + * + * @param lat The latitude of the gps point + * @param lon The longitude of the gps point + * + * @return The coordinates of the gps point in the map frame + */ + std::pair lla2flat(double lat, double lon) const; + + /** + * @brief Utility function to get the heading of the ASV assuming identical orientation between odom and map frame + * + * @param msg The quat message derived from the odom message published by the seapath + */ + double get_freya_heading(const geometry_msgs::msg::Quaternion msg) const; + + /** + * @brief Utility function to set the position of the GPS coordinates in the map frame + */ + void set_gps_frame_coords(); + + /** + * @brief Utility function that runs until waypoint is reached. + * Function returns when within distance_threshold of the waypoint. + * + * @param distance_threshold The distance threshold for reaching the waypoint + */ + void reach_waypoint(const double distance_threshold); + + + +private: + mutable std::mutex odom_mutex_; + mutable std::mutex grid_mutex_; + mutable std::mutex landmark_mutex_; + + rclcpp::Subscription::SharedPtr map_origin_sub_; + rclcpp::Subscription::SharedPtr grid_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr landmarks_sub_; + + rclcpp::Client::SharedPtr waypoint_client_; + + rclcpp::Publisher::SharedPtr waypoint_visualization_pub_; + rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_; + nav_msgs::msg::Odometry::SharedPtr odom_msg_; + vortex_msgs::msg::LandmarkArray::SharedPtr landmarks_msg_; + + geometry_msgs::msg::Point previous_waypoint_odom_frame_; + +}; + +} // namespace docking_task + +#endif // DOCKING_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/docking_task/launch/docking_task_launch.py b/mission/docking_task/launch/docking_task_launch.py new file mode 100644 index 00000000..20434096 --- /dev/null +++ b/mission/docking_task/launch/docking_task_launch.py @@ -0,0 +1,17 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + docking_task_node = Node( + package='docking_task', + executable='docking_task_node', + name='docking_task_node', + parameters=[os.path.join(get_package_share_directory('docking_task'),'params','docking_task_params.yaml')], + # arguments=['--ros-args', '--log-level', 'DEBUG'], + output='screen', + ) + return LaunchDescription([ + docking_task_node + ]) \ No newline at end of file diff --git a/mission/docking_task/package.xml b/mission/docking_task/package.xml new file mode 100644 index 00000000..25f54ea0 --- /dev/null +++ b/mission/docking_task/package.xml @@ -0,0 +1,28 @@ + + + + docking_task + 0.0.0 + TODO: Package description + jorgen + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + nav_msgs + geometry_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + + + ament_cmake + + diff --git a/mission/docking_task/params/docking_task_params.yaml b/mission/docking_task/params/docking_task_params.yaml new file mode 100644 index 00000000..c57119cf --- /dev/null +++ b/mission/docking_task/params/docking_task_params.yaml @@ -0,0 +1,32 @@ +docking_task_node: + ros__parameters: + map_origin_lat: 63.4411585867919 + map_origin_lon: 10.419400373255625 + map_origin_set: true + gps_start_lat: 63.44097054434422 + gps_start_lon: 10.419997767413607 + gps_end_lat: 63.44125901804796 + gps_end_lon: 10.41857835889424 + gps_start_x: 0.0 + gps_start_y: 0.0 + gps_end_x: 0.0 + gps_end_y: 0.0 + gps_frame_coords_set: false + dock_width: 4.13 # width of available docking space + dock_width_tolerance: 0.5 + dock_length: 2.0 # length of available docking space + dock_length_tolerance: 0.5 + dock_edge_width: 0.13 # width of the dock edge + dock_edge_width_tolerance: 0.05 + dock_search_offset: 0.2 # offset in forward direction to begin search for dock + task_nr: 2 # possible values 1 and 2 corresponding to task 4.1 and 4.2 + models: + dynmod_stddev: 0.01 + senmod_stddev: 0.01 + + + map_origin_topic: "/map/origin" + grid_topic: "grid" + odom_topic: "/seapath/odom/ned" + landmark_pose_topic: "landmarks_out" + \ No newline at end of file diff --git a/mission/docking_task/src/docking_task_node.cpp b/mission/docking_task/src/docking_task_node.cpp new file mode 100644 index 00000000..ca84d750 --- /dev/null +++ b/mission/docking_task/src/docking_task_node.cpp @@ -0,0 +1,14 @@ +#include +#include + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} diff --git a/mission/docking_task/src/docking_task_ros.cpp b/mission/docking_task/src/docking_task_ros.cpp new file mode 100644 index 00000000..9cb36c3e --- /dev/null +++ b/mission/docking_task/src/docking_task_ros.cpp @@ -0,0 +1,681 @@ +#include + + +namespace docking_task { + +DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) + : Node("dock_localization_node", options) { + + declare_parameter("map_origin_lat", 0.0); + declare_parameter("map_origin_lon", 0.0); + declare_parameter("map_origin_set", false); + declare_parameter("gps_start_lat", 0.0); + declare_parameter("gps_start_lon", 0.0); + declare_parameter("gps_end_lat", 0.0); + declare_parameter("gps_end_lon", 0.0); + declare_parameter("gps_start_x", 0.0); + declare_parameter("gps_start_y", 0.0); + declare_parameter("gps_end_x", 0.0); + declare_parameter("gps_end_y", 0.0); + declare_parameter("gps_frame_coords_set", false); + declare_parameter("dock_width", 0.0); + declare_parameter("dock_width_tolerance", 0.0); + declare_parameter("dock_length", 0.0); + declare_parameter("dock_length_tolerance", 0.0); + declare_parameter("dock_edge_width", 0.0); + declare_parameter("dock_edge_width_tolerance", 0.0); + declare_parameter("dock_search_offset", 0.0); + declare_parameter("task_nr", 0.0); + declare_parameter("models.dynmod_stddev", 0.0); + declare_parameter("models.sen_stddev", 0.0); + + declare_parameter("map_origin_topic", "/map/origin"); + declare_parameter("grid_topic", "grid"); + declare_parameter("odom_topic", "/seapath/odom/ned"); + declare_parameter("landmark_pose_topic", "/landmark/pose"); + + // Sensor data QoS profile + rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), qos_profile_sensor_data); + + // Transient local QoS profile + rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; + qos_profile_transient_local.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + auto qos_transient_local = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), qos_profile_transient_local); + + gps_map_coord_visualization_pub_ = this->create_publisher( + "/gps_map_coord_visualization", qos_sensor_data); + + map_origin_sub_ = this->create_subscription( + get_parameter("map_origin_topic").as_string(), + qos_transient_local, [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + this->set_parameter(rclcpp::Parameter("map_origin_lat", msg->latitude)); + this->set_parameter(rclcpp::Parameter("map_origin_lon", msg->longitude)); + this->set_parameter(rclcpp::Parameter("map_origin_set", true)); + RCLCPP_INFO(this->get_logger(), "Map origin set to: %f, %f", msg->latitude, msg->longitude); + + // Set GPS frame coordinates + set_gps_frame_coords(); + + map_origin_sub_.reset(); + }); + + grid_sub_ = this->create_subscription( + get_parameter("grid_topic").as_string(), + qos_sensor_data, [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received grid message"); + std::unique_lock lock(grid_mutex_); + grid_msg_ = msg; + lock.unlock(); + }); + + odom_sub_ = this->create_subscription( + get_parameter("odom_topic").as_string(), + qos_sensor_data, [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received odometry message"); + std::unique_lock lock(odom_mutex_); + odom_msg_ = msg; + lock.unlock(); + }); + + landmarks_sub_ = this->create_subscription( + get_parameter("landmark_pose_topic").as_string(), + qos_sensor_data, [this](const vortex_msgs::msg::LandmarkArray::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received landmark pose array"); + std::unique_lock lock(landmark_mutex_); + landmarks_msg_ = msg; + lock.unlock(); + }); + + waypoint_visualization_pub_ = this->create_publisher( + "/waypoint_visualization", qos_sensor_data); + + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + waypoint_client_ = this->create_client("/waypoint"); + + std::thread(&DockingTaskNode::main_task, this).detach(); +} + + +void DockingTaskNode::main_task() { + // Sleep for 5 seconds to allow system to initialize and tracks to be aquired + RCLCPP_INFO(this->get_logger(), "Waiting for system to initialize before starting main task"); + rclcpp::sleep_for(std::chrono::seconds(5)); + RCLCPP_INFO(this->get_logger(), "System initialized, starting main task"); + + // Set initial waypoint between first buoy pair + auto [landmark1, landmark2] = initial_waypoint(); + RCLCPP_INFO(this->get_logger(), "Initial waypoint set between landmarks %d and %d", landmark1.id, landmark2.id); + + reach_waypoint(1.0); + + auto formation = predict_buoy_formation(landmark1, landmark2); + + navigate_formation(formation); + +} + +std::pair DockingTaskNode::initial_waypoint() { + RCLCPP_INFO(this->get_logger(), "Initial waypoint running"); + + while (true) { + rclcpp::sleep_for(std::chrono::seconds(1)); + if (!(this->get_parameter("map_origin_set").as_bool())) { + RCLCPP_INFO(this->get_logger(), "Map origin not set, exiting initial waypoint"); + continue; + } + if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { + set_gps_frame_coords(); + } + // std::unique_lock odom_lock(odom_mutex_); + // if (odom_msg_ == nullptr) { + // RCLCPP_INFO(this->get_logger(), "Odometry message not received, exiting initial waypoint"); + // odom_lock.unlock(); + // continue; + // } + // double heading = get_freya_heading(odom_msg_->pose.pose.orientation); + // odom_lock.unlock(); + + // double gps_start_x = this->get_parameter("gps_start_x").as_double(); + // double gps_start_y = this->get_parameter("gps_start_y").as_double(); + // double gps_end_x = this->get_parameter("gps_end_x").as_double(); + // double gps_end_y = this->get_parameter("gps_end_y").as_double(); + + // // Calculate the direction vector from the start point + // double dir_x = cos(heading); + // double dir_y = sin(heading); + + // // The point P should lie on the line from the start point in the direction of the heading + // // Find the intersection point where vector from P to end point is orthogonal to the heading direction vector + + // // Set up the equation for the line in the heading direction from start point + // // P = (P_x, P_y) + // // Line equation: P_x = gps_start_x + t * dir_x, P_y = gps_start_y + t * dir_y + + // // We need the vector (gps_end_x - P_x, gps_end_y - P_y) to be orthogonal to the direction vector (dir_x, dir_y) + // // (gps_end_x - (gps_start_x + t * dir_x)) * dir_x + (gps_end_y - (gps_start_y + t * dir_y)) * dir_y = 0 + // // Solving for t + + // double t = ((gps_end_x - gps_start_x) * dir_x + (gps_end_y - gps_start_y) * dir_y) / (dir_x * dir_x + dir_y * dir_y); + + // // Calculate intersection point + // double intersection_x = gps_start_x + t * dir_x; + // double intersection_y = gps_start_y + t * dir_y; + + // RCLCPP_INFO(this->get_logger(), "Intersection point: %f, %f", intersection_x, intersection_y); + // geometry_msgs::msg::PoseStamped waypoint; + // waypoint.header.frame_id = "map"; + // waypoint.pose.position.x = intersection_x; + // waypoint.pose.position.y = intersection_y; + // waypoint_visualization_pub_->publish(waypoint); + + if (landmarks_msg_ == nullptr) { + RCLCPP_INFO(this->get_logger(), "Landmark pose array not received, exiting initial waypoint"); + continue; + } + + int result = 0; + double x_waypoint = 0.0; + double y_waypoint = 0.0; + std::pair id_pair; + + std::pair landmark_return_pair; + + while (result < 10){ + + // Transform landmark poses to base_link frame + geometry_msgs::msg::PoseArray landmark_poses_map_frame; + geometry_msgs::msg::PoseArray landmark_poses_base_link_frame; + std::vector landmarks_with_ids; + std::unique_lock landmark_lock(landmark_mutex_); + for (const auto& landmark : landmarks_msg_->landmarks) { + LandmarkWithID lwid; + lwid.id = landmark.id; + lwid.pose_map_frame = landmark.odom.pose.pose; + landmark_poses_map_frame.poses.push_back(landmark.odom.pose.pose); + landmarks_with_ids.push_back(lwid); + } + + landmark_lock.unlock(); + + try { + auto transform = tf_buffer_->lookupTransform("base_link", "map", rclcpp::Time(0)); + for (size_t i = 0; i < landmark_poses_base_link_frame.poses.size(); ++i) { + tf2::doTransform(landmark_poses_map_frame.poses.at(i), landmark_poses_base_link_frame.poses.at(i), transform); + landmarks_with_ids[i].pose_base_link_frame = landmark_poses_base_link_frame.poses[i]; + } + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + } + + // Remove landmarks behind the drone + std::remove_if(landmarks_with_ids.begin(), landmarks_with_ids.end(), + [](const LandmarkWithID &l) { + return l.pose_base_link_frame.position.x < 0.0; + }); + + // Sort landmarks based on distance + std::sort(landmarks_with_ids.begin(), landmarks_with_ids.end(), + [](const LandmarkWithID &a, const LandmarkWithID &b) { + double dist_a = sqrt(pow(a.pose_base_link_frame.position.x, 2) + pow(a.pose_base_link_frame.position.y, 2)); + double dist_b = sqrt(pow(b.pose_base_link_frame.position.x, 2) + pow(b.pose_base_link_frame.position.y, 2)); + return dist_a < dist_b; + }); + + // Find a pair of landmarks that satisfy the conditions + for (size_t i = 0; i < landmarks_with_ids.size() - 1; ++i) { + const auto& landmark1 = landmarks_with_ids[i]; + const auto& landmark2 = landmarks_with_ids[i + 1]; + + // Calculate the distance between the landmarks + double distance = sqrt(pow(landmark2.pose_base_link_frame.position.x - landmark1.pose_base_link_frame.position.x, 2) + + pow(landmark2.pose_base_link_frame.position.y - landmark1.pose_base_link_frame.position.y, 2)); + + // Check if the landmarks are on opposite sides of the drone and within the desired distance range + // Base link is NED frame, x is forward, y is right + if ((landmark1.pose_base_link_frame.position.x > 0 && landmark2.pose_base_link_frame.position.x > 0) + && (distance >= 3.0 && distance <= 8.0) + && ((landmark1.pose_base_link_frame.position.y < 0) != (landmark2.pose_base_link_frame.position.y < 0))) { + // Calculate the midpoint between the two landmarks in map frame + double x_waypoint_sample = (landmark1.pose_map_frame.position.x + landmark2.pose_map_frame.position.x) / 2; + double y_waypoint_sample = (landmark1.pose_map_frame.position.y + landmark2.pose_map_frame.position.y) / 2; + std::pair id_pair_sample = {landmark1.id, landmark2.id}; + // Check if this is the first valid pair of landmarks + if (result == 0){ + x_waypoint = x_waypoint_sample; + y_waypoint = y_waypoint_sample; + id_pair = id_pair_sample; + result ++; + break; + } + + // Check if the new waypoint is further away from the current waypoint + // or if the new pair of landmarks is the same as the current pair + if ((sqrt(pow(x_waypoint_sample - x_waypoint, 2) + pow(y_waypoint_sample - y_waypoint, 2)) > 1.0) || + !((id_pair_sample.first == id_pair.first && id_pair_sample.second == id_pair.second) || + (id_pair_sample.first == id_pair.second && id_pair_sample.second == id_pair.first))) + { + result = 0; + break; + } + + // Update the waypoint and the pair of landmarks + id_pair = id_pair_sample; + x_waypoint += x_waypoint_sample; + y_waypoint += y_waypoint_sample; + x_waypoint /= 2; + y_waypoint /= 2; + result ++; + landmark_return_pair.first = landmark1; + landmark_return_pair.second = landmark2; + break; + + } + } + + } + //send waypoint + if(!waypoint_client_->service_is_ready()){ + RCLCPP_INFO(this->get_logger(), "Waypoint client not ready"); + continue; + } + geometry_msgs::msg::Point waypoint_map_frame; + geometry_msgs::msg::Point waypoint_odom_frame; + waypoint_map_frame.x = x_waypoint; + waypoint_map_frame.y = y_waypoint; + waypoint_map_frame.z = 0.0; + try { + auto transform = tf_buffer_->lookupTransform("odom", "map", rclcpp::Time(0)); + tf2::doTransform(waypoint_map_frame, waypoint_odom_frame, transform); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + } + auto request = std::make_shared(); + request->waypoint.push_back(waypoint_odom_frame); + auto result_future = waypoint_client_->async_send_request(request); + RCLCPP_INFO(this->get_logger(), "Waypoint sent: %f, %f", waypoint_odom_frame.x, waypoint_odom_frame.y); + // Check if the service was successful + + auto status = result_future.wait_for(std::chrono::seconds(5)); + if (status == std::future_status::timeout) { + RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); + continue; + } + if (!result_future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); + } + + geometry_msgs::msg::PoseStamped waypoint_vis; + waypoint_vis.header.frame_id = "odom"; + waypoint_vis.pose.position.x = waypoint_odom_frame.x; + waypoint_vis.pose.position.y = waypoint_odom_frame.y; + waypoint_visualization_pub_->publish(waypoint_vis); + + previous_waypoint_odom_frame_ = waypoint_odom_frame; + return landmark_return_pair; + + } +} + +void DockingTaskNode::reach_waypoint(const double distance_threshold) { + RCLCPP_INFO(this->get_logger(), "Reach waypoint running"); + std::unique_lock odom_lock(odom_mutex_); + double x = odom_msg_->pose.pose.position.x; + double y = odom_msg_->pose.pose.position.y; + odom_lock.unlock(); + while (sqrt(pow(x - previous_waypoint_odom_frame_.x, 2) + pow(y - previous_waypoint_odom_frame_.y, 2)) > distance_threshold) { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + odom_lock.lock(); + x = odom_msg_->pose.pose.position.x; + y = odom_msg_->pose.pose.position.y; + odom_lock.unlock(); + } + RCLCPP_INFO(this->get_logger(), "Reached waypoint"); + return; +} + +Eigen::Array DockingTaskNode::predict_buoy_formation(const LandmarkWithID &buoy1, const LandmarkWithID &buoy2) const { + RCLCPP_INFO(this->get_logger(), "Predict buoy formation running"); + try { + geometry_msgs::msg::Point previous_waypoint_map_frame; + auto transform = tf_buffer_->lookupTransform("map", "odom", rclcpp::Time(0)); + tf2::doTransform(previous_waypoint_odom_frame_, previous_waypoint_map_frame, transform); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + } + Eigen::Vector2d direction_vector(previous_waypoint_odom_frame_.x - this->get_parameter("gps_start_x").as_double(), + previous_waypoint_odom_frame_.y - this->get_parameter("gps_start_y").as_double()); + direction_vector.normalize(); + // Sanity check that first buoy pair is ish correct + Eigen::Vector2d first_left, first_right; + + size_t found = 0; + std::unique_lock landmark_lock(landmark_mutex_); + for (const auto& landmark : landmarks_msg_->landmarks) { + if (landmark.id == buoy1.id && + std::sqrt(std::pow(landmark.odom.pose.pose.position.x - buoy1.pose_map_frame.position.x, 2) + + std::pow(landmark.odom.pose.pose.position.y - buoy1.pose_map_frame.position.y, 2)) < 0.5) { + found++; + if(buoy1.pose_base_link_frame.position.y < 0){ + first_left = Eigen::Vector2d(landmark.odom.pose.pose.position.x, landmark.odom.pose.pose.position.y); + } else { + first_right = Eigen::Vector2d(landmark.odom.pose.pose.position.x, landmark.odom.pose.pose.position.y); + } + } + if (landmark.id == buoy2.id && + std::sqrt(std::pow(landmark.odom.pose.pose.position.x - buoy2.pose_map_frame.position.x, 2) + + std::pow(landmark.odom.pose.pose.position.y - buoy2.pose_map_frame.position.y, 2)) < 0.5) { + found++; + if(buoy2.pose_base_link_frame.position.y < 0){ + first_left = Eigen::Vector2d(landmark.odom.pose.pose.position.x, landmark.odom.pose.pose.position.y); + } else { + first_right = Eigen::Vector2d(landmark.odom.pose.pose.position.x, landmark.odom.pose.pose.position.y); + } + } + } + landmark_lock.unlock(); + if(found != 2){ + RCLCPP_WARN(this->get_logger(), "Buoy pair not found in landmark array"); + } + Eigen::Array formation; + // Create formation with index + // dock + //<< 0 1 + //<< 2 3 + //<< 4 5 + // start + Eigen::Vector2d second_left, second_right, third_left, third_right; + second_left = first_left + direction_vector * 5.0; + second_right = first_right + direction_vector * 5.0; + third_left = first_left + direction_vector * 10.0; + third_right = first_right + direction_vector * 10.0; + formation << third_left.x(), third_right.x(), second_left.x(), second_right.x(), first_left.x(), first_right.x(), + third_left.y(), third_right.y(), second_left.y(), second_right.y(), first_left.y(), first_right.y(); + return formation; +} + +Eigen::MatrixXd DockingTaskNode::generate_reward_matrix(const Eigen::Array& predicted_positions, const Eigen::MatrixXd& measured_positions) { + int num_predicted = predicted_positions.cols(); // Should be 6 + int num_measured = measured_positions.cols(); + Eigen::MatrixXd reward_matrix(num_measured, num_predicted); + + // Initialize the reward matrix + for (int i = 0; i < num_measured; ++i) { + for (int j = 0; j < num_predicted; ++j) { + double dx = measured_positions(0, i) - predicted_positions(0, j); + double dy = measured_positions(1, i) - predicted_positions(1, j); + double distance = std::sqrt(dx * dx + dy * dy); + reward_matrix(i, j) = -distance; // We use negative distance as reward (shorter distance = higher reward) + } + } + return reward_matrix; +} + +Eigen::VectorXi DockingTaskNode::auction_algorithm(const Eigen::MatrixXd &reward_matrix) { + int num_items = reward_matrix.rows(); + int num_customers = reward_matrix.cols(); + Eigen::VectorXi assignment = Eigen::VectorXi::Constant(num_customers, -1); + Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_items); + + std::vector unassigned; + for (int i = 0; i < num_customers; ++i) { + unassigned.push_back(i); + } + + double epsilon = 1.0 / (num_items + 1); + + while (!unassigned.empty()) { + int customer = unassigned.back(); + unassigned.pop_back(); + + double max_value = std::numeric_limits::lowest(); + int max_item = -1; + for (int item = 0; item < reward_matrix.rows(); ++item) { + double value = reward_matrix.coeff(item, customer) - prices[item]; + if (value > max_value) { + max_value = value; + max_item = item; + } + } + + // Find the current owner of max item + int current_owner = -1; + for (int i = 0; i < num_customers; ++i) { + if (assignment[i] == max_item) { + current_owner = i; + break; + } + } + if (current_owner != -1) { + unassigned.push_back(current_owner); + } + + assignment[customer] = max_item; + prices[max_item] += max_value + epsilon; + } + + return assignment; +} + +void DockingTaskNode::navigate_formation(const Eigen::Array& predicted_positions) { + RCLCPP_INFO(this->get_logger(), "Navigating though formation"); + std::vector prev_assignment; + std::vector landmark_ids; + bool first_half = true; + // Buoy formation with index + // dock + //<< 0 1 + //<< 2 3 + //<< 4 5 + // start + int result = 0; + while (true){ + landmark_ids.clear(); + geometry_msgs::msg::PoseArray landmark_poses_map_frame; + std::unique_lock landmark_lock(landmark_mutex_); + for (const auto& landmark : landmarks_msg_->landmarks) { + + landmark_ids.push_back(landmark.id); + landmark_poses_map_frame.poses.push_back(landmark.odom.pose.pose); + } + + landmark_lock.unlock(); + + // Transform landmark poses to odom_frame + geometry_msgs::msg::PoseArray landmark_poses_odom_frame; + try { + auto transform = tf_buffer_->lookupTransform("odom", "map", rclcpp::Time(0)); + for (size_t i = 0; i < landmark_poses_map_frame.poses.size(); ++i) { + tf2::doTransform(landmark_poses_map_frame.poses.at(i), landmark_poses_odom_frame.poses.at(i), transform); + } + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + } + + Eigen::MatrixXd measured_positions(2, landmark_ids.size()); + for (size_t i = 0; i < landmark_ids.size(); ++i) { + measured_positions(0, i) = landmark_poses_odom_frame.poses.at(i).position.x; + measured_positions(1, i) = landmark_poses_odom_frame.poses.at(i).position.y; + } + + Eigen::MatrixXd reward_matrix = generate_reward_matrix(predicted_positions, measured_positions); + + Eigen::VectorXi assignment = auction_algorithm(reward_matrix); + + + if (result == 0) { + for(Eigen::Index i = 0; i < assignment.size(); i++){ + prev_assignment.push_back(landmark_ids.at(assignment(i))); + } + result++; + continue; + } + bool valied = true; + // Check that the assigned landmarks matches the previous assignment by id + if(first_half){ + // Check index 2,3,4,5 + if (landmark_ids.at(assignment(2)) != prev_assignment.at(2) || landmark_ids.at(assignment(3)) != prev_assignment.at(3) + || landmark_ids.at(assignment(4)) != prev_assignment.at(4) || landmark_ids.at(assignment(5)) != prev_assignment.at(5)){ + valied = false; + } + } else { + // Check index 0,1,2,3 + if (landmark_ids.at(assignment(0)) != prev_assignment.at(0) || landmark_ids.at(assignment(1)) != prev_assignment.at(1) + || landmark_ids.at(assignment(2)) != prev_assignment.at(2) || landmark_ids.at(assignment(3)) != prev_assignment.at(3)){ + valied = false; + } + } + + if(!valied){ + result = 0; + prev_assignment.clear(); + continue; + } + else{ + result++; + } + if (result > 10){ + geometry_msgs::msg::Point waypoint_odom_frame; + // Calculate the waypoint between the relevant buoy pair + double buoy_left_x; + double buoy_left_y; + double buoy_right_x; + double buoy_right_y; + if(first_half){ + buoy_left_x = landmark_poses_odom_frame.poses.at(assignment(2)).position.x; + buoy_left_y = landmark_poses_odom_frame.poses.at(assignment(2)).position.y; + buoy_right_x = landmark_poses_odom_frame.poses.at(assignment(3)).position.x; + buoy_right_y = landmark_poses_odom_frame.poses.at(assignment(3)).position.y; + } else { + buoy_left_x = landmark_poses_odom_frame.poses.at(assignment(0)).position.x; + buoy_left_y = landmark_poses_odom_frame.poses.at(assignment(0)).position.y; + buoy_right_x = landmark_poses_odom_frame.poses.at(assignment(1)).position.x; + buoy_right_y = landmark_poses_odom_frame.poses.at(assignment(1)).position.y; + } + + waypoint_odom_frame.x = (buoy_left_x + buoy_right_x) / 2; + waypoint_odom_frame.y = (buoy_left_y + buoy_right_y) / 2; + + auto request = std::make_shared(); + request->waypoint.push_back(waypoint_odom_frame); + auto result_future = waypoint_client_->async_send_request(request); + RCLCPP_INFO(this->get_logger(), "Waypoint sent: %f, %f", waypoint_odom_frame.x, waypoint_odom_frame.y); + // Check if the service was successful + + auto status = result_future.wait_for(std::chrono::seconds(5)); + if (status == std::future_status::timeout) { + RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); + continue; + } + if (!result_future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); + } + geometry_msgs::msg::PoseStamped waypoint_vis; + waypoint_vis.header.frame_id = "odom"; + waypoint_vis.pose.position.x = waypoint_odom_frame.x; + waypoint_vis.pose.position.y = waypoint_odom_frame.y; + waypoint_visualization_pub_->publish(waypoint_vis); + + previous_waypoint_odom_frame_ = waypoint_odom_frame; + + if(first_half){ + first_half = false; + result = 0; + prev_assignment.clear(); + + // Wait for the ASV to reach the waypoint and then find waypoint for the second half of the formation + reach_waypoint(1.0); + continue; + } + else{ + // Return from function when the asv is close to the second waypoint + reach_waypoint(0.2); + return; + } + + + } + + } +} + +void DockingTaskNode::set_gps_frame_coords() { + auto [gps_start_x, gps_start_y] = lla2flat(this->get_parameter("gps_start_lat").as_double(), this->get_parameter("gps_start_lon").as_double()); + auto [gps_end_x, gps_end_y] = lla2flat(this->get_parameter("gps_end_lat").as_double(), this->get_parameter("gps_end_lon").as_double()); + this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_x)); + this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_y)); + this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_x)); + this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_y)); + this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); + RCLCPP_INFO(this->get_logger(), "GPS map frame coordinates set to: %f, %f, %f, %f", gps_start_x, gps_start_y, gps_end_x, gps_end_y); + + geometry_msgs::msg::PoseArray gps_points; + gps_points.header.frame_id = "map"; + + // Convert GPS points to geometry poses + geometry_msgs::msg::Pose start_pose; + start_pose.position.x = gps_start_x; + start_pose.position.y = gps_start_y; + gps_points.poses.push_back(start_pose); + + geometry_msgs::msg::Pose end_pose; + end_pose.position.x = gps_end_x; + end_pose.position.y = gps_end_y; + gps_points.poses.push_back(end_pose); + + gps_map_coord_visualization_pub_->publish(gps_points); +} + +std::pair DockingTaskNode::lla2flat(double lat, double lon) const { + const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) + const double f = 1.0 / 298.257223563; // Flattening of the earth + const double psi_rad = 0.0; + + // Angular direction of the flat Earth x-axis, specified as a scalar. + // The angular direction is the degrees clockwise from north, + // which is the angle in degrees used for converting flat Earth x and + // y coordinates to the north and east coordinates + + // Convert angles from degrees to radians + const double lat_rad = lat * M_PI / 180.0; + const double lon_rad = lon * M_PI / 180.0; + const double origin_lat_rad = this->get_parameter("map_origin_lat").as_double() * M_PI / 180.0; + const double origin_lon_rad = this->get_parameter("map_origin_lon").as_double() * M_PI / 180.0; + + // Calculate delta latitude and delta longitude in radians + const double dlat = lat_rad - origin_lat_rad; + const double dlon = lon_rad - origin_lon_rad; + + // Radius of curvature in the vertical prime (RN) + const double RN = + R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Radius of curvature in the meridian (RM) + const double RM = RN * (1.0 - (2.0 * f - f * f)) / + (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Changes in the north (dN) and east (dE) positions + const double dN = RM * dlat; + const double dE = RN * cos(origin_lat_rad) * dlon; + + // Transformation from North-East to flat x-y coordinates + const double px = cos(psi_rad) * dN - sin(psi_rad) * dE; + const double py = sin(psi_rad) * dN + cos(psi_rad) * dE; + + return {px, py}; +} + +double DockingTaskNode::get_freya_heading(const geometry_msgs::msg::Quaternion msg) const { + tf2::Quaternion quat; + tf2::fromMsg(msg, quat); + + // Convert quaternion to Euler angles + double roll, pitch, yaw; + tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw); + + return yaw; +} + +} // namespace docking_task \ No newline at end of file diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index be5eba87..3c25644d 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -37,17 +37,17 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) // map_origin_lat_ = 63.41490901857848; // map_origin_lon_ = 10.398215601285054; // office bag still - // map_origin_lat_ = 63.414660884931976; - // map_origin_lon_ = 10.398554661537544; - // map_origin_set_ = true; - // auto grid = createOccupancyGrid(); - // auto polygon = readPolygonFromFile(landmask_file_); - // landmask_pub_->publish(polygon); - // fillOutsidePolygon(grid, polygon); - // insert_landmask(grid, polygon); - - // map_pub_ = this->create_publisher("map", - // qos_transient_local); map_pub_->publish(grid); + map_origin_lat_ = 63.414660884931976; + map_origin_lon_ = 10.398554661537544; + map_origin_set_ = true; + auto grid = createOccupancyGrid(); + auto polygon = readPolygonFromFile(landmask_file_); + landmask_pub_->publish(polygon); + fillOutsidePolygon(grid, polygon); + insert_landmask(grid, polygon); + + map_pub_ = this->create_publisher("map", + qos_transient_local); map_pub_->publish(grid); grid_service_ = this->create_service( "get_map", From 64b5b8848a438f83a05d78579ac6bc9cee2a95c8 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Fri, 26 Jul 2024 13:22:50 +0000 Subject: [PATCH 19/67] Committing clang-format changes --- .../include/docking_task/docking_task_ros.hpp | 78 ++- mission/docking_task/src/docking_task_ros.cpp | 589 ++++++++++-------- mission/map_manager/src/map_manager_ros.cpp | 5 +- 3 files changed, 388 insertions(+), 284 deletions(-) diff --git a/mission/docking_task/include/docking_task/docking_task_ros.hpp b/mission/docking_task/include/docking_task/docking_task_ros.hpp index 6502f01c..cb80ff59 100644 --- a/mission/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/docking_task/include/docking_task/docking_task_ros.hpp @@ -2,31 +2,32 @@ #define DOCKING_TASK_ROS_HPP #include -#include +#include #include -#include -#include #include #include -#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include namespace docking_task { struct LandmarkWithID { - geometry_msgs::msg::Pose pose_map_frame; - geometry_msgs::msg::Pose pose_base_link_frame; - uint32_t id; - }; + geometry_msgs::msg::Pose pose_map_frame; + geometry_msgs::msg::Pose pose_base_link_frame; + uint32_t id; +}; /** * @class DockingTaskNode - * @brief A class representing a node for handling dock localization in a ROS 2 system. + * @brief A class representing a node for handling dock localization in a ROS 2 + * system. * * This class inherits from rclcpp::Node and provides functionality for * localizing the dock in the map. @@ -63,70 +64,79 @@ class DockingTaskNode : public rclcpp::Node { * * @param landmark1 The first landmark/buoy used for initial waypoint * @param landmark2 The second landmark/buoy used for initial waypoint - * - * @return The predicted posistion of the 6-tuple formation of buoys in odom frame + * + * @return The predicted posistion of the 6-tuple formation of buoys in odom + * frame */ - Eigen::Array predict_buoy_formation(const LandmarkWithID &buoy1, const LandmarkWithID &buoy2) const; + Eigen::Array + predict_buoy_formation(const LandmarkWithID &buoy1, + const LandmarkWithID &buoy2) const; /** * @brief Generate the reward matrix for the auction algorithm * * @param predicted_positions The predicted positions of the buoys * @param measured_positions The measured positions of landmarks - * + * * @return The reward matrix for the auction algorithm */ - Eigen::MatrixXd generate_reward_matrix(const Eigen::Array& predicted_positions, const Eigen::MatrixXd& measured_positions); + Eigen::MatrixXd + generate_reward_matrix(const Eigen::Array &predicted_positions, + const Eigen::MatrixXd &measured_positions); /** * @brief The auction algorithm for the assignment problem * * @param reward_matrix The reward matrix for the auction algorithm - * + * * @return The assignment of landmarks to buoys */ Eigen::VectorXi auction_algorithm(const Eigen::MatrixXd &reward_matrix); /** - * @brief Navigate the ASV through the formation of buoys. First travels to waypoint between second pair of buoys, - * then navigates through the formation of buoys and returns control when asv is 0.2m away from crossing the last buoy pair. + * @brief Navigate the ASV through the formation of buoys. First travels to + * waypoint between second pair of buoys, then navigates through the formation + * of buoys and returns control when asv is 0.2m away from crossing the last + * buoy pair. * * @param predicted_positions The predicted positions of the buoys */ - void navigate_formation(const Eigen::Array& predicted_positions); + void + navigate_formation(const Eigen::Array &predicted_positions); /** * @brief Calculate the coordinates of a gps point in the map frame * * @param lat The latitude of the gps point * @param lon The longitude of the gps point - * + * * @return The coordinates of the gps point in the map frame */ std::pair lla2flat(double lat, double lon) const; /** - * @brief Utility function to get the heading of the ASV assuming identical orientation between odom and map frame + * @brief Utility function to get the heading of the ASV assuming identical + * orientation between odom and map frame * - * @param msg The quat message derived from the odom message published by the seapath + * @param msg The quat message derived from the odom message published by the + * seapath */ double get_freya_heading(const geometry_msgs::msg::Quaternion msg) const; /** - * @brief Utility function to set the position of the GPS coordinates in the map frame + * @brief Utility function to set the position of the GPS coordinates in the + * map frame */ void set_gps_frame_coords(); /** * @brief Utility function that runs until waypoint is reached. * Function returns when within distance_threshold of the waypoint. - * + * * @param distance_threshold The distance threshold for reaching the waypoint */ void reach_waypoint(const double distance_threshold); - - private: mutable std::mutex odom_mutex_; mutable std::mutex grid_mutex_; @@ -135,12 +145,15 @@ class DockingTaskNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr map_origin_sub_; rclcpp::Subscription::SharedPtr grid_sub_; rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr landmarks_sub_; + rclcpp::Subscription::SharedPtr + landmarks_sub_; rclcpp::Client::SharedPtr waypoint_client_; - rclcpp::Publisher::SharedPtr waypoint_visualization_pub_; - rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; + rclcpp::Publisher::SharedPtr + waypoint_visualization_pub_; + rclcpp::Publisher::SharedPtr + gps_map_coord_visualization_pub_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -150,7 +163,6 @@ class DockingTaskNode : public rclcpp::Node { vortex_msgs::msg::LandmarkArray::SharedPtr landmarks_msg_; geometry_msgs::msg::Point previous_waypoint_odom_frame_; - }; } // namespace docking_task diff --git a/mission/docking_task/src/docking_task_ros.cpp b/mission/docking_task/src/docking_task_ros.cpp index 9cb36c3e..60c1a192 100644 --- a/mission/docking_task/src/docking_task_ros.cpp +++ b/mission/docking_task/src/docking_task_ros.cpp @@ -1,6 +1,5 @@ #include - namespace docking_task { DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) @@ -33,88 +32,99 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) declare_parameter("grid_topic", "grid"); declare_parameter("odom_topic", "/seapath/odom/ned"); declare_parameter("landmark_pose_topic", "/landmark/pose"); - + // Sensor data QoS profile rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), qos_profile_sensor_data); - + auto qos_sensor_data = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), + qos_profile_sensor_data); + // Transient local QoS profile rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; - qos_profile_transient_local.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - auto qos_transient_local = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), qos_profile_transient_local); + qos_profile_transient_local.durability = + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + auto qos_transient_local = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), + qos_profile_transient_local); - gps_map_coord_visualization_pub_ = this->create_publisher( - "/gps_map_coord_visualization", qos_sensor_data); + gps_map_coord_visualization_pub_ = + this->create_publisher( + "/gps_map_coord_visualization", qos_sensor_data); map_origin_sub_ = this->create_subscription( - get_parameter("map_origin_topic").as_string(), - qos_transient_local, [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - this->set_parameter(rclcpp::Parameter("map_origin_lat", msg->latitude)); - this->set_parameter(rclcpp::Parameter("map_origin_lon", msg->longitude)); - this->set_parameter(rclcpp::Parameter("map_origin_set", true)); - RCLCPP_INFO(this->get_logger(), "Map origin set to: %f, %f", msg->latitude, msg->longitude); - - // Set GPS frame coordinates - set_gps_frame_coords(); - - map_origin_sub_.reset(); - }); + get_parameter("map_origin_topic").as_string(), qos_transient_local, + [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + this->set_parameter(rclcpp::Parameter("map_origin_lat", msg->latitude)); + this->set_parameter( + rclcpp::Parameter("map_origin_lon", msg->longitude)); + this->set_parameter(rclcpp::Parameter("map_origin_set", true)); + RCLCPP_INFO(this->get_logger(), "Map origin set to: %f, %f", + msg->latitude, msg->longitude); + + // Set GPS frame coordinates + set_gps_frame_coords(); + + map_origin_sub_.reset(); + }); grid_sub_ = this->create_subscription( - get_parameter("grid_topic").as_string(), - qos_sensor_data, [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received grid message"); - std::unique_lock lock(grid_mutex_); - grid_msg_ = msg; - lock.unlock(); - }); + get_parameter("grid_topic").as_string(), qos_sensor_data, + [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received grid message"); + std::unique_lock lock(grid_mutex_); + grid_msg_ = msg; + lock.unlock(); + }); odom_sub_ = this->create_subscription( - get_parameter("odom_topic").as_string(), - qos_sensor_data, [this](const nav_msgs::msg::Odometry::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received odometry message"); - std::unique_lock lock(odom_mutex_); - odom_msg_ = msg; - lock.unlock(); - }); + get_parameter("odom_topic").as_string(), qos_sensor_data, + [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received odometry message"); + std::unique_lock lock(odom_mutex_); + odom_msg_ = msg; + lock.unlock(); + }); landmarks_sub_ = this->create_subscription( - get_parameter("landmark_pose_topic").as_string(), - qos_sensor_data, [this](const vortex_msgs::msg::LandmarkArray::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received landmark pose array"); - std::unique_lock lock(landmark_mutex_); - landmarks_msg_ = msg; - lock.unlock(); - }); - - waypoint_visualization_pub_ = this->create_publisher( - "/waypoint_visualization", qos_sensor_data); + get_parameter("landmark_pose_topic").as_string(), qos_sensor_data, + [this](const vortex_msgs::msg::LandmarkArray::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received landmark pose array"); + std::unique_lock lock(landmark_mutex_); + landmarks_msg_ = msg; + lock.unlock(); + }); + + waypoint_visualization_pub_ = + this->create_publisher( + "/waypoint_visualization", qos_sensor_data); tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - waypoint_client_ = this->create_client("/waypoint"); + waypoint_client_ = + this->create_client("/waypoint"); std::thread(&DockingTaskNode::main_task, this).detach(); } - void DockingTaskNode::main_task() { // Sleep for 5 seconds to allow system to initialize and tracks to be aquired - RCLCPP_INFO(this->get_logger(), "Waiting for system to initialize before starting main task"); + RCLCPP_INFO(this->get_logger(), + "Waiting for system to initialize before starting main task"); rclcpp::sleep_for(std::chrono::seconds(5)); RCLCPP_INFO(this->get_logger(), "System initialized, starting main task"); // Set initial waypoint between first buoy pair auto [landmark1, landmark2] = initial_waypoint(); - RCLCPP_INFO(this->get_logger(), "Initial waypoint set between landmarks %d and %d", landmark1.id, landmark2.id); + RCLCPP_INFO(this->get_logger(), + "Initial waypoint set between landmarks %d and %d", landmark1.id, + landmark2.id); reach_waypoint(1.0); - + auto formation = predict_buoy_formation(landmark1, landmark2); navigate_formation(formation); - } std::pair DockingTaskNode::initial_waypoint() { @@ -123,7 +133,8 @@ std::pair DockingTaskNode::initial_waypoint() { while (true) { rclcpp::sleep_for(std::chrono::seconds(1)); if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), "Map origin not set, exiting initial waypoint"); + RCLCPP_INFO(this->get_logger(), + "Map origin not set, exiting initial waypoint"); continue; } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { @@ -131,9 +142,8 @@ std::pair DockingTaskNode::initial_waypoint() { } // std::unique_lock odom_lock(odom_mutex_); // if (odom_msg_ == nullptr) { - // RCLCPP_INFO(this->get_logger(), "Odometry message not received, exiting initial waypoint"); - // odom_lock.unlock(); - // continue; + // RCLCPP_INFO(this->get_logger(), "Odometry message not received, exiting + // initial waypoint"); odom_lock.unlock(); continue; // } // double heading = get_freya_heading(odom_msg_->pose.pose.orientation); // odom_lock.unlock(); @@ -147,32 +157,39 @@ std::pair DockingTaskNode::initial_waypoint() { // double dir_x = cos(heading); // double dir_y = sin(heading); - // // The point P should lie on the line from the start point in the direction of the heading - // // Find the intersection point where vector from P to end point is orthogonal to the heading direction vector + // // The point P should lie on the line from the start point in the + // direction of the heading + // // Find the intersection point where vector from P to end point is + // orthogonal to the heading direction vector - // // Set up the equation for the line in the heading direction from start point + // // Set up the equation for the line in the heading direction from start + // point // // P = (P_x, P_y) - // // Line equation: P_x = gps_start_x + t * dir_x, P_y = gps_start_y + t * dir_y + // // Line equation: P_x = gps_start_x + t * dir_x, P_y = gps_start_y + t * + // dir_y - // // We need the vector (gps_end_x - P_x, gps_end_y - P_y) to be orthogonal to the direction vector (dir_x, dir_y) - // // (gps_end_x - (gps_start_x + t * dir_x)) * dir_x + (gps_end_y - (gps_start_y + t * dir_y)) * dir_y = 0 + // // We need the vector (gps_end_x - P_x, gps_end_y - P_y) to be orthogonal + // to the direction vector (dir_x, dir_y) + // // (gps_end_x - (gps_start_x + t * dir_x)) * dir_x + (gps_end_y - + // (gps_start_y + t * dir_y)) * dir_y = 0 // // Solving for t - // double t = ((gps_end_x - gps_start_x) * dir_x + (gps_end_y - gps_start_y) * dir_y) / (dir_x * dir_x + dir_y * dir_y); + // double t = ((gps_end_x - gps_start_x) * dir_x + (gps_end_y - gps_start_y) + // * dir_y) / (dir_x * dir_x + dir_y * dir_y); // // Calculate intersection point // double intersection_x = gps_start_x + t * dir_x; // double intersection_y = gps_start_y + t * dir_y; - // RCLCPP_INFO(this->get_logger(), "Intersection point: %f, %f", intersection_x, intersection_y); - // geometry_msgs::msg::PoseStamped waypoint; - // waypoint.header.frame_id = "map"; - // waypoint.pose.position.x = intersection_x; - // waypoint.pose.position.y = intersection_y; + // RCLCPP_INFO(this->get_logger(), "Intersection point: %f, %f", + // intersection_x, intersection_y); geometry_msgs::msg::PoseStamped + // waypoint; waypoint.header.frame_id = "map"; waypoint.pose.position.x = + // intersection_x; waypoint.pose.position.y = intersection_y; // waypoint_visualization_pub_->publish(waypoint); if (landmarks_msg_ == nullptr) { - RCLCPP_INFO(this->get_logger(), "Landmark pose array not received, exiting initial waypoint"); + RCLCPP_INFO(this->get_logger(), + "Landmark pose array not received, exiting initial waypoint"); continue; } @@ -183,14 +200,14 @@ std::pair DockingTaskNode::initial_waypoint() { std::pair landmark_return_pair; - while (result < 10){ - + while (result < 10) { + // Transform landmark poses to base_link frame geometry_msgs::msg::PoseArray landmark_poses_map_frame; geometry_msgs::msg::PoseArray landmark_poses_base_link_frame; std::vector landmarks_with_ids; std::unique_lock landmark_lock(landmark_mutex_); - for (const auto& landmark : landmarks_msg_->landmarks) { + for (const auto &landmark : landmarks_msg_->landmarks) { LandmarkWithID lwid; lwid.id = landmark.id; lwid.pose_map_frame = landmark.odom.pose.pose; @@ -201,10 +218,15 @@ std::pair DockingTaskNode::initial_waypoint() { landmark_lock.unlock(); try { - auto transform = tf_buffer_->lookupTransform("base_link", "map", rclcpp::Time(0)); - for (size_t i = 0; i < landmark_poses_base_link_frame.poses.size(); ++i) { - tf2::doTransform(landmark_poses_map_frame.poses.at(i), landmark_poses_base_link_frame.poses.at(i), transform); - landmarks_with_ids[i].pose_base_link_frame = landmark_poses_base_link_frame.poses[i]; + auto transform = + tf_buffer_->lookupTransform("base_link", "map", rclcpp::Time(0)); + for (size_t i = 0; i < landmark_poses_base_link_frame.poses.size(); + ++i) { + tf2::doTransform(landmark_poses_map_frame.poses.at(i), + landmark_poses_base_link_frame.poses.at(i), + transform); + landmarks_with_ids[i].pose_base_link_frame = + landmark_poses_base_link_frame.poses[i]; } } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); @@ -219,44 +241,63 @@ std::pair DockingTaskNode::initial_waypoint() { // Sort landmarks based on distance std::sort(landmarks_with_ids.begin(), landmarks_with_ids.end(), [](const LandmarkWithID &a, const LandmarkWithID &b) { - double dist_a = sqrt(pow(a.pose_base_link_frame.position.x, 2) + pow(a.pose_base_link_frame.position.y, 2)); - double dist_b = sqrt(pow(b.pose_base_link_frame.position.x, 2) + pow(b.pose_base_link_frame.position.y, 2)); + double dist_a = + sqrt(pow(a.pose_base_link_frame.position.x, 2) + + pow(a.pose_base_link_frame.position.y, 2)); + double dist_b = + sqrt(pow(b.pose_base_link_frame.position.x, 2) + + pow(b.pose_base_link_frame.position.y, 2)); return dist_a < dist_b; }); // Find a pair of landmarks that satisfy the conditions for (size_t i = 0; i < landmarks_with_ids.size() - 1; ++i) { - const auto& landmark1 = landmarks_with_ids[i]; - const auto& landmark2 = landmarks_with_ids[i + 1]; + const auto &landmark1 = landmarks_with_ids[i]; + const auto &landmark2 = landmarks_with_ids[i + 1]; // Calculate the distance between the landmarks - double distance = sqrt(pow(landmark2.pose_base_link_frame.position.x - landmark1.pose_base_link_frame.position.x, 2) + - pow(landmark2.pose_base_link_frame.position.y - landmark1.pose_base_link_frame.position.y, 2)); - - // Check if the landmarks are on opposite sides of the drone and within the desired distance range - // Base link is NED frame, x is forward, y is right - if ((landmark1.pose_base_link_frame.position.x > 0 && landmark2.pose_base_link_frame.position.x > 0) - && (distance >= 3.0 && distance <= 8.0) - && ((landmark1.pose_base_link_frame.position.y < 0) != (landmark2.pose_base_link_frame.position.y < 0))) { + double distance = + sqrt(pow(landmark2.pose_base_link_frame.position.x - + landmark1.pose_base_link_frame.position.x, + 2) + + pow(landmark2.pose_base_link_frame.position.y - + landmark1.pose_base_link_frame.position.y, + 2)); + + // Check if the landmarks are on opposite sides of the drone and within + // the desired distance range Base link is NED frame, x is forward, y is + // right + if ((landmark1.pose_base_link_frame.position.x > 0 && + landmark2.pose_base_link_frame.position.x > 0) && + (distance >= 3.0 && distance <= 8.0) && + ((landmark1.pose_base_link_frame.position.y < 0) != + (landmark2.pose_base_link_frame.position.y < 0))) { // Calculate the midpoint between the two landmarks in map frame - double x_waypoint_sample = (landmark1.pose_map_frame.position.x + landmark2.pose_map_frame.position.x) / 2; - double y_waypoint_sample = (landmark1.pose_map_frame.position.y + landmark2.pose_map_frame.position.y) / 2; - std::pair id_pair_sample = {landmark1.id, landmark2.id}; + double x_waypoint_sample = (landmark1.pose_map_frame.position.x + + landmark2.pose_map_frame.position.x) / + 2; + double y_waypoint_sample = (landmark1.pose_map_frame.position.y + + landmark2.pose_map_frame.position.y) / + 2; + std::pair id_pair_sample = {landmark1.id, + landmark2.id}; // Check if this is the first valid pair of landmarks - if (result == 0){ + if (result == 0) { x_waypoint = x_waypoint_sample; y_waypoint = y_waypoint_sample; id_pair = id_pair_sample; - result ++; + result++; break; } - + // Check if the new waypoint is further away from the current waypoint // or if the new pair of landmarks is the same as the current pair - if ((sqrt(pow(x_waypoint_sample - x_waypoint, 2) + pow(y_waypoint_sample - y_waypoint, 2)) > 1.0) || - !((id_pair_sample.first == id_pair.first && id_pair_sample.second == id_pair.second) || - (id_pair_sample.first == id_pair.second && id_pair_sample.second == id_pair.first))) - { + if ((sqrt(pow(x_waypoint_sample - x_waypoint, 2) + + pow(y_waypoint_sample - y_waypoint, 2)) > 1.0) || + !((id_pair_sample.first == id_pair.first && + id_pair_sample.second == id_pair.second) || + (id_pair_sample.first == id_pair.second && + id_pair_sample.second == id_pair.first))) { result = 0; break; } @@ -267,17 +308,15 @@ std::pair DockingTaskNode::initial_waypoint() { y_waypoint += y_waypoint_sample; x_waypoint /= 2; y_waypoint /= 2; - result ++; + result++; landmark_return_pair.first = landmark1; landmark_return_pair.second = landmark2; break; - } } - } - //send waypoint - if(!waypoint_client_->service_is_ready()){ + // send waypoint + if (!waypoint_client_->service_is_ready()) { RCLCPP_INFO(this->get_logger(), "Waypoint client not ready"); continue; } @@ -287,7 +326,8 @@ std::pair DockingTaskNode::initial_waypoint() { waypoint_map_frame.y = y_waypoint; waypoint_map_frame.z = 0.0; try { - auto transform = tf_buffer_->lookupTransform("odom", "map", rclcpp::Time(0)); + auto transform = + tf_buffer_->lookupTransform("odom", "map", rclcpp::Time(0)); tf2::doTransform(waypoint_map_frame, waypoint_odom_frame, transform); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); @@ -295,7 +335,8 @@ std::pair DockingTaskNode::initial_waypoint() { auto request = std::make_shared(); request->waypoint.push_back(waypoint_odom_frame); auto result_future = waypoint_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Waypoint sent: %f, %f", waypoint_odom_frame.x, waypoint_odom_frame.y); + RCLCPP_INFO(this->get_logger(), "Waypoint sent: %f, %f", + waypoint_odom_frame.x, waypoint_odom_frame.y); // Check if the service was successful auto status = result_future.wait_for(std::chrono::seconds(5)); @@ -315,7 +356,6 @@ std::pair DockingTaskNode::initial_waypoint() { previous_waypoint_odom_frame_ = waypoint_odom_frame; return landmark_return_pair; - } } @@ -325,7 +365,9 @@ void DockingTaskNode::reach_waypoint(const double distance_threshold) { double x = odom_msg_->pose.pose.position.x; double y = odom_msg_->pose.pose.position.y; odom_lock.unlock(); - while (sqrt(pow(x - previous_waypoint_odom_frame_.x, 2) + pow(y - previous_waypoint_odom_frame_.y, 2)) > distance_threshold) { + while (sqrt(pow(x - previous_waypoint_odom_frame_.x, 2) + + pow(y - previous_waypoint_odom_frame_.y, 2)) > + distance_threshold) { rclcpp::sleep_for(std::chrono::milliseconds(100)); odom_lock.lock(); x = odom_msg_->pose.pose.position.x; @@ -336,51 +378,70 @@ void DockingTaskNode::reach_waypoint(const double distance_threshold) { return; } -Eigen::Array DockingTaskNode::predict_buoy_formation(const LandmarkWithID &buoy1, const LandmarkWithID &buoy2) const { +Eigen::Array +DockingTaskNode::predict_buoy_formation(const LandmarkWithID &buoy1, + const LandmarkWithID &buoy2) const { RCLCPP_INFO(this->get_logger(), "Predict buoy formation running"); try { geometry_msgs::msg::Point previous_waypoint_map_frame; - auto transform = tf_buffer_->lookupTransform("map", "odom", rclcpp::Time(0)); - tf2::doTransform(previous_waypoint_odom_frame_, previous_waypoint_map_frame, transform); + auto transform = + tf_buffer_->lookupTransform("map", "odom", rclcpp::Time(0)); + tf2::doTransform(previous_waypoint_odom_frame_, previous_waypoint_map_frame, + transform); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - } - Eigen::Vector2d direction_vector(previous_waypoint_odom_frame_.x - this->get_parameter("gps_start_x").as_double(), - previous_waypoint_odom_frame_.y - this->get_parameter("gps_start_y").as_double()); + } + Eigen::Vector2d direction_vector( + previous_waypoint_odom_frame_.x - + this->get_parameter("gps_start_x").as_double(), + previous_waypoint_odom_frame_.y - + this->get_parameter("gps_start_y").as_double()); direction_vector.normalize(); // Sanity check that first buoy pair is ish correct Eigen::Vector2d first_left, first_right; size_t found = 0; std::unique_lock landmark_lock(landmark_mutex_); - for (const auto& landmark : landmarks_msg_->landmarks) { - if (landmark.id == buoy1.id && - std::sqrt(std::pow(landmark.odom.pose.pose.position.x - buoy1.pose_map_frame.position.x, 2) + - std::pow(landmark.odom.pose.pose.position.y - buoy1.pose_map_frame.position.y, 2)) < 0.5) { + for (const auto &landmark : landmarks_msg_->landmarks) { + if (landmark.id == buoy1.id && + std::sqrt(std::pow(landmark.odom.pose.pose.position.x - + buoy1.pose_map_frame.position.x, + 2) + + std::pow(landmark.odom.pose.pose.position.y - + buoy1.pose_map_frame.position.y, + 2)) < 0.5) { found++; - if(buoy1.pose_base_link_frame.position.y < 0){ - first_left = Eigen::Vector2d(landmark.odom.pose.pose.position.x, landmark.odom.pose.pose.position.y); + if (buoy1.pose_base_link_frame.position.y < 0) { + first_left = Eigen::Vector2d(landmark.odom.pose.pose.position.x, + landmark.odom.pose.pose.position.y); } else { - first_right = Eigen::Vector2d(landmark.odom.pose.pose.position.x, landmark.odom.pose.pose.position.y); + first_right = Eigen::Vector2d(landmark.odom.pose.pose.position.x, + landmark.odom.pose.pose.position.y); } } if (landmark.id == buoy2.id && - std::sqrt(std::pow(landmark.odom.pose.pose.position.x - buoy2.pose_map_frame.position.x, 2) + - std::pow(landmark.odom.pose.pose.position.y - buoy2.pose_map_frame.position.y, 2)) < 0.5) { + std::sqrt(std::pow(landmark.odom.pose.pose.position.x - + buoy2.pose_map_frame.position.x, + 2) + + std::pow(landmark.odom.pose.pose.position.y - + buoy2.pose_map_frame.position.y, + 2)) < 0.5) { found++; - if(buoy2.pose_base_link_frame.position.y < 0){ - first_left = Eigen::Vector2d(landmark.odom.pose.pose.position.x, landmark.odom.pose.pose.position.y); + if (buoy2.pose_base_link_frame.position.y < 0) { + first_left = Eigen::Vector2d(landmark.odom.pose.pose.position.x, + landmark.odom.pose.pose.position.y); } else { - first_right = Eigen::Vector2d(landmark.odom.pose.pose.position.x, landmark.odom.pose.pose.position.y); + first_right = Eigen::Vector2d(landmark.odom.pose.pose.position.x, + landmark.odom.pose.pose.position.y); } } } landmark_lock.unlock(); - if(found != 2){ + if (found != 2) { RCLCPP_WARN(this->get_logger(), "Buoy pair not found in landmark array"); } Eigen::Array formation; - // Create formation with index + // Create formation with index // dock //<< 0 1 //<< 2 3 @@ -391,33 +452,39 @@ Eigen::Array DockingTaskNode::predict_buoy_formation(const Landmar second_right = first_right + direction_vector * 5.0; third_left = first_left + direction_vector * 10.0; third_right = first_right + direction_vector * 10.0; - formation << third_left.x(), third_right.x(), second_left.x(), second_right.x(), first_left.x(), first_right.x(), - third_left.y(), third_right.y(), second_left.y(), second_right.y(), first_left.y(), first_right.y(); + formation << third_left.x(), third_right.x(), second_left.x(), + second_right.x(), first_left.x(), first_right.x(), third_left.y(), + third_right.y(), second_left.y(), second_right.y(), first_left.y(), + first_right.y(); return formation; } -Eigen::MatrixXd DockingTaskNode::generate_reward_matrix(const Eigen::Array& predicted_positions, const Eigen::MatrixXd& measured_positions) { - int num_predicted = predicted_positions.cols(); // Should be 6 - int num_measured = measured_positions.cols(); - Eigen::MatrixXd reward_matrix(num_measured, num_predicted); - - // Initialize the reward matrix - for (int i = 0; i < num_measured; ++i) { - for (int j = 0; j < num_predicted; ++j) { - double dx = measured_positions(0, i) - predicted_positions(0, j); - double dy = measured_positions(1, i) - predicted_positions(1, j); - double distance = std::sqrt(dx * dx + dy * dy); - reward_matrix(i, j) = -distance; // We use negative distance as reward (shorter distance = higher reward) - } +Eigen::MatrixXd DockingTaskNode::generate_reward_matrix( + const Eigen::Array &predicted_positions, + const Eigen::MatrixXd &measured_positions) { + int num_predicted = predicted_positions.cols(); // Should be 6 + int num_measured = measured_positions.cols(); + Eigen::MatrixXd reward_matrix(num_measured, num_predicted); + + // Initialize the reward matrix + for (int i = 0; i < num_measured; ++i) { + for (int j = 0; j < num_predicted; ++j) { + double dx = measured_positions(0, i) - predicted_positions(0, j); + double dy = measured_positions(1, i) - predicted_positions(1, j); + double distance = std::sqrt(dx * dx + dy * dy); + reward_matrix(i, j) = -distance; // We use negative distance as reward + // (shorter distance = higher reward) } - return reward_matrix; + } + return reward_matrix; } -Eigen::VectorXi DockingTaskNode::auction_algorithm(const Eigen::MatrixXd &reward_matrix) { - int num_items = reward_matrix.rows(); - int num_customers = reward_matrix.cols(); +Eigen::VectorXi +DockingTaskNode::auction_algorithm(const Eigen::MatrixXd &reward_matrix) { + int num_items = reward_matrix.rows(); + int num_customers = reward_matrix.cols(); Eigen::VectorXi assignment = Eigen::VectorXi::Constant(num_customers, -1); - Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_items); + Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_items); std::vector unassigned; for (int i = 0; i < num_customers; ++i) { @@ -436,7 +503,7 @@ Eigen::VectorXi DockingTaskNode::auction_algorithm(const Eigen::MatrixXd &reward double value = reward_matrix.coeff(item, customer) - prices[item]; if (value > max_value) { max_value = value; - max_item = item; + max_item = item; } } @@ -459,24 +526,25 @@ Eigen::VectorXi DockingTaskNode::auction_algorithm(const Eigen::MatrixXd &reward return assignment; } -void DockingTaskNode::navigate_formation(const Eigen::Array& predicted_positions) { +void DockingTaskNode::navigate_formation( + const Eigen::Array &predicted_positions) { RCLCPP_INFO(this->get_logger(), "Navigating though formation"); std::vector prev_assignment; std::vector landmark_ids; bool first_half = true; - // Buoy formation with index + // Buoy formation with index // dock //<< 0 1 //<< 2 3 //<< 4 5 // start int result = 0; - while (true){ + while (true) { landmark_ids.clear(); geometry_msgs::msg::PoseArray landmark_poses_map_frame; std::unique_lock landmark_lock(landmark_mutex_); - for (const auto& landmark : landmarks_msg_->landmarks) { - + for (const auto &landmark : landmarks_msg_->landmarks) { + landmark_ids.push_back(landmark.id); landmark_poses_map_frame.poses.push_back(landmark.odom.pose.pose); } @@ -486,9 +554,11 @@ void DockingTaskNode::navigate_formation(const Eigen::Array& predi // Transform landmark poses to odom_frame geometry_msgs::msg::PoseArray landmark_poses_odom_frame; try { - auto transform = tf_buffer_->lookupTransform("odom", "map", rclcpp::Time(0)); + auto transform = + tf_buffer_->lookupTransform("odom", "map", rclcpp::Time(0)); for (size_t i = 0; i < landmark_poses_map_frame.poses.size(); ++i) { - tf2::doTransform(landmark_poses_map_frame.poses.at(i), landmark_poses_odom_frame.poses.at(i), transform); + tf2::doTransform(landmark_poses_map_frame.poses.at(i), + landmark_poses_odom_frame.poses.at(i), transform); } } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); @@ -496,120 +566,137 @@ void DockingTaskNode::navigate_formation(const Eigen::Array& predi Eigen::MatrixXd measured_positions(2, landmark_ids.size()); for (size_t i = 0; i < landmark_ids.size(); ++i) { - measured_positions(0, i) = landmark_poses_odom_frame.poses.at(i).position.x; - measured_positions(1, i) = landmark_poses_odom_frame.poses.at(i).position.y; + measured_positions(0, i) = + landmark_poses_odom_frame.poses.at(i).position.x; + measured_positions(1, i) = + landmark_poses_odom_frame.poses.at(i).position.y; } - - Eigen::MatrixXd reward_matrix = generate_reward_matrix(predicted_positions, measured_positions); - Eigen::VectorXi assignment = auction_algorithm(reward_matrix); + Eigen::MatrixXd reward_matrix = + generate_reward_matrix(predicted_positions, measured_positions); + Eigen::VectorXi assignment = auction_algorithm(reward_matrix); - if (result == 0) { - for(Eigen::Index i = 0; i < assignment.size(); i++){ - prev_assignment.push_back(landmark_ids.at(assignment(i))); - } - result++; - continue; + if (result == 0) { + for (Eigen::Index i = 0; i < assignment.size(); i++) { + prev_assignment.push_back(landmark_ids.at(assignment(i))); } - bool valied = true; - // Check that the assigned landmarks matches the previous assignment by id - if(first_half){ - // Check index 2,3,4,5 - if (landmark_ids.at(assignment(2)) != prev_assignment.at(2) || landmark_ids.at(assignment(3)) != prev_assignment.at(3) - || landmark_ids.at(assignment(4)) != prev_assignment.at(4) || landmark_ids.at(assignment(5)) != prev_assignment.at(5)){ - valied = false; - } + result++; + continue; + } + bool valied = true; + // Check that the assigned landmarks matches the previous assignment by id + if (first_half) { + // Check index 2,3,4,5 + if (landmark_ids.at(assignment(2)) != prev_assignment.at(2) || + landmark_ids.at(assignment(3)) != prev_assignment.at(3) || + landmark_ids.at(assignment(4)) != prev_assignment.at(4) || + landmark_ids.at(assignment(5)) != prev_assignment.at(5)) { + valied = false; + } + } else { + // Check index 0,1,2,3 + if (landmark_ids.at(assignment(0)) != prev_assignment.at(0) || + landmark_ids.at(assignment(1)) != prev_assignment.at(1) || + landmark_ids.at(assignment(2)) != prev_assignment.at(2) || + landmark_ids.at(assignment(3)) != prev_assignment.at(3)) { + valied = false; + } + } + + if (!valied) { + result = 0; + prev_assignment.clear(); + continue; + } else { + result++; + } + if (result > 10) { + geometry_msgs::msg::Point waypoint_odom_frame; + // Calculate the waypoint between the relevant buoy pair + double buoy_left_x; + double buoy_left_y; + double buoy_right_x; + double buoy_right_y; + if (first_half) { + buoy_left_x = + landmark_poses_odom_frame.poses.at(assignment(2)).position.x; + buoy_left_y = + landmark_poses_odom_frame.poses.at(assignment(2)).position.y; + buoy_right_x = + landmark_poses_odom_frame.poses.at(assignment(3)).position.x; + buoy_right_y = + landmark_poses_odom_frame.poses.at(assignment(3)).position.y; } else { - // Check index 0,1,2,3 - if (landmark_ids.at(assignment(0)) != prev_assignment.at(0) || landmark_ids.at(assignment(1)) != prev_assignment.at(1) - || landmark_ids.at(assignment(2)) != prev_assignment.at(2) || landmark_ids.at(assignment(3)) != prev_assignment.at(3)){ - valied = false; - } + buoy_left_x = + landmark_poses_odom_frame.poses.at(assignment(0)).position.x; + buoy_left_y = + landmark_poses_odom_frame.poses.at(assignment(0)).position.y; + buoy_right_x = + landmark_poses_odom_frame.poses.at(assignment(1)).position.x; + buoy_right_y = + landmark_poses_odom_frame.poses.at(assignment(1)).position.y; } - if(!valied){ - result = 0; - prev_assignment.clear(); + waypoint_odom_frame.x = (buoy_left_x + buoy_right_x) / 2; + waypoint_odom_frame.y = (buoy_left_y + buoy_right_y) / 2; + + auto request = std::make_shared(); + request->waypoint.push_back(waypoint_odom_frame); + auto result_future = waypoint_client_->async_send_request(request); + RCLCPP_INFO(this->get_logger(), "Waypoint sent: %f, %f", + waypoint_odom_frame.x, waypoint_odom_frame.y); + // Check if the service was successful + + auto status = result_future.wait_for(std::chrono::seconds(5)); + if (status == std::future_status::timeout) { + RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); continue; } - else{ - result++; - } - if (result > 10){ - geometry_msgs::msg::Point waypoint_odom_frame; - // Calculate the waypoint between the relevant buoy pair - double buoy_left_x; - double buoy_left_y; - double buoy_right_x; - double buoy_right_y; - if(first_half){ - buoy_left_x = landmark_poses_odom_frame.poses.at(assignment(2)).position.x; - buoy_left_y = landmark_poses_odom_frame.poses.at(assignment(2)).position.y; - buoy_right_x = landmark_poses_odom_frame.poses.at(assignment(3)).position.x; - buoy_right_y = landmark_poses_odom_frame.poses.at(assignment(3)).position.y; - } else { - buoy_left_x = landmark_poses_odom_frame.poses.at(assignment(0)).position.x; - buoy_left_y = landmark_poses_odom_frame.poses.at(assignment(0)).position.y; - buoy_right_x = landmark_poses_odom_frame.poses.at(assignment(1)).position.x; - buoy_right_y = landmark_poses_odom_frame.poses.at(assignment(1)).position.y; - } - - waypoint_odom_frame.x = (buoy_left_x + buoy_right_x) / 2; - waypoint_odom_frame.y = (buoy_left_y + buoy_right_y) / 2; + if (!result_future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); + } + geometry_msgs::msg::PoseStamped waypoint_vis; + waypoint_vis.header.frame_id = "odom"; + waypoint_vis.pose.position.x = waypoint_odom_frame.x; + waypoint_vis.pose.position.y = waypoint_odom_frame.y; + waypoint_visualization_pub_->publish(waypoint_vis); - auto request = std::make_shared(); - request->waypoint.push_back(waypoint_odom_frame); - auto result_future = waypoint_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Waypoint sent: %f, %f", waypoint_odom_frame.x, waypoint_odom_frame.y); - // Check if the service was successful + previous_waypoint_odom_frame_ = waypoint_odom_frame; - auto status = result_future.wait_for(std::chrono::seconds(5)); - if (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); - continue; - } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); - } - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = waypoint_odom_frame.x; - waypoint_vis.pose.position.y = waypoint_odom_frame.y; - waypoint_visualization_pub_->publish(waypoint_vis); - - previous_waypoint_odom_frame_ = waypoint_odom_frame; - - if(first_half){ - first_half = false; - result = 0; - prev_assignment.clear(); - - // Wait for the ASV to reach the waypoint and then find waypoint for the second half of the formation - reach_waypoint(1.0); - continue; - } - else{ - // Return from function when the asv is close to the second waypoint - reach_waypoint(0.2); - return; - } + if (first_half) { + first_half = false; + result = 0; + prev_assignment.clear(); - + // Wait for the ASV to reach the waypoint and then find waypoint for the + // second half of the formation + reach_waypoint(1.0); + continue; + } else { + // Return from function when the asv is close to the second waypoint + reach_waypoint(0.2); + return; } - + } } } void DockingTaskNode::set_gps_frame_coords() { - auto [gps_start_x, gps_start_y] = lla2flat(this->get_parameter("gps_start_lat").as_double(), this->get_parameter("gps_start_lon").as_double()); - auto [gps_end_x, gps_end_y] = lla2flat(this->get_parameter("gps_end_lat").as_double(), this->get_parameter("gps_end_lon").as_double()); + auto [gps_start_x, gps_start_y] = + lla2flat(this->get_parameter("gps_start_lat").as_double(), + this->get_parameter("gps_start_lon").as_double()); + auto [gps_end_x, gps_end_y] = + lla2flat(this->get_parameter("gps_end_lat").as_double(), + this->get_parameter("gps_end_lon").as_double()); this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_x)); this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_y)); this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_x)); this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_y)); this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); - RCLCPP_INFO(this->get_logger(), "GPS map frame coordinates set to: %f, %f, %f, %f", gps_start_x, gps_start_y, gps_end_x, gps_end_y); + RCLCPP_INFO(this->get_logger(), + "GPS map frame coordinates set to: %f, %f, %f, %f", gps_start_x, + gps_start_y, gps_end_x, gps_end_y); geometry_msgs::msg::PoseArray gps_points; gps_points.header.frame_id = "map"; @@ -628,10 +715,11 @@ void DockingTaskNode::set_gps_frame_coords() { gps_map_coord_visualization_pub_->publish(gps_points); } -std::pair DockingTaskNode::lla2flat(double lat, double lon) const { +std::pair DockingTaskNode::lla2flat(double lat, + double lon) const { const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) const double f = 1.0 / 298.257223563; // Flattening of the earth - const double psi_rad = 0.0; + const double psi_rad = 0.0; // Angular direction of the flat Earth x-axis, specified as a scalar. // The angular direction is the degrees clockwise from north, @@ -641,8 +729,10 @@ std::pair DockingTaskNode::lla2flat(double lat, double lon) cons // Convert angles from degrees to radians const double lat_rad = lat * M_PI / 180.0; const double lon_rad = lon * M_PI / 180.0; - const double origin_lat_rad = this->get_parameter("map_origin_lat").as_double() * M_PI / 180.0; - const double origin_lon_rad = this->get_parameter("map_origin_lon").as_double() * M_PI / 180.0; + const double origin_lat_rad = + this->get_parameter("map_origin_lat").as_double() * M_PI / 180.0; + const double origin_lon_rad = + this->get_parameter("map_origin_lon").as_double() * M_PI / 180.0; // Calculate delta latitude and delta longitude in radians const double dlat = lat_rad - origin_lat_rad; @@ -667,7 +757,8 @@ std::pair DockingTaskNode::lla2flat(double lat, double lon) cons return {px, py}; } -double DockingTaskNode::get_freya_heading(const geometry_msgs::msg::Quaternion msg) const { +double DockingTaskNode::get_freya_heading( + const geometry_msgs::msg::Quaternion msg) const { tf2::Quaternion quat; tf2::fromMsg(msg, quat); diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index 3c25644d..968afe6b 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -46,8 +46,9 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) fillOutsidePolygon(grid, polygon); insert_landmask(grid, polygon); - map_pub_ = this->create_publisher("map", - qos_transient_local); map_pub_->publish(grid); + map_pub_ = this->create_publisher( + "map", qos_transient_local); + map_pub_->publish(grid); grid_service_ = this->create_service( "get_map", From 168772e10745b658ae83f78f3458d2d5529ba5ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christopher=20Str=C3=B8m?= Date: Fri, 26 Jul 2024 17:16:53 +0200 Subject: [PATCH 20/67] Add controller to sim launch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Christopher Strøm --- asv_setup/launch/sim_freya.launch.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/asv_setup/launch/sim_freya.launch.py b/asv_setup/launch/sim_freya.launch.py index f87f63b2..6d053248 100644 --- a/asv_setup/launch/sim_freya.launch.py +++ b/asv_setup/launch/sim_freya.launch.py @@ -51,11 +51,19 @@ def generate_launch_description(): ) ) + # Controller + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + path.join(get_package_share_directory('asv_setup'), 'launch', 'hybridpath.launch.py') + ) + ) + # Return launch description return LaunchDescription([ set_env_var, thruster_allocator_launch, joy_node, joystick_interface_launch, + controller_launch, vortex_sim_interface_launch, ]) \ No newline at end of file From d9c848c94bde927fd106199e4cc042e4e447538b Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Fri, 26 Jul 2024 17:45:20 +0200 Subject: [PATCH 21/67] dynamic buoy prediction --- .../include/docking_task/docking_task_ros.hpp | 5 ++-- mission/docking_task/src/docking_task_ros.cpp | 26 ++++++++++++++++--- 2 files changed, 25 insertions(+), 6 deletions(-) diff --git a/mission/docking_task/include/docking_task/docking_task_ros.hpp b/mission/docking_task/include/docking_task/docking_task_ros.hpp index cb80ff59..1cb8825a 100644 --- a/mission/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/docking_task/include/docking_task/docking_task_ros.hpp @@ -100,9 +100,10 @@ class DockingTaskNode : public rclcpp::Node { * buoy pair. * * @param predicted_positions The predicted positions of the buoys + * @return The ids of the last pair of buoys in the formation */ - void - navigate_formation(const Eigen::Array &predicted_positions); + std::pair + navigate_formation(Eigen::Array &predicted_positions); /** * @brief Calculate the coordinates of a gps point in the map frame diff --git a/mission/docking_task/src/docking_task_ros.cpp b/mission/docking_task/src/docking_task_ros.cpp index 60c1a192..f0657f6b 100644 --- a/mission/docking_task/src/docking_task_ros.cpp +++ b/mission/docking_task/src/docking_task_ros.cpp @@ -124,7 +124,7 @@ void DockingTaskNode::main_task() { auto formation = predict_buoy_formation(landmark1, landmark2); - navigate_formation(formation); + auto [buoy_left_id, buoy_right_id] = navigate_formation(formation); } std::pair DockingTaskNode::initial_waypoint() { @@ -526,8 +526,7 @@ DockingTaskNode::auction_algorithm(const Eigen::MatrixXd &reward_matrix) { return assignment; } -void DockingTaskNode::navigate_formation( - const Eigen::Array &predicted_positions) { +std::pair DockingTaskNode::navigate_formation(Eigen::Array &predicted_positions) { RCLCPP_INFO(this->get_logger(), "Navigating though formation"); std::vector prev_assignment; std::vector landmark_ids; @@ -627,6 +626,25 @@ void DockingTaskNode::navigate_formation( landmark_poses_odom_frame.poses.at(assignment(3)).position.x; buoy_right_y = landmark_poses_odom_frame.poses.at(assignment(3)).position.y; + Eigen::Vector2d direction_vector + ((buoy_left_x - landmark_poses_odom_frame.poses.at(assignment(4)).position.x + + buoy_right_x - landmark_poses_odom_frame.poses.at(assignment(5)).position.x) / + 2, (buoy_left_y - landmark_poses_odom_frame.poses.at(assignment(4)).position.y + + buoy_right_y - landmark_poses_odom_frame.poses.at(assignment(5)).position.y) /2); + direction_vector.normalize(); + // Update the predicted positions + predicted_positions << buoy_left_x + direction_vector.x() * 5, + buoy_right_x + direction_vector.x() * 5, + buoy_left_x, + buoy_right_x, + buoy_left_x - direction_vector.x() * 5, + buoy_right_x - direction_vector.x() * 5, + buoy_left_y + direction_vector.y() * 5, + buoy_right_y + direction_vector.y() * 5, + buoy_left_y, + buoy_right_y, + buoy_left_y - direction_vector.y() * 5, + buoy_right_y - direction_vector.y() * 5; } else { buoy_left_x = landmark_poses_odom_frame.poses.at(assignment(0)).position.x; @@ -676,7 +694,7 @@ void DockingTaskNode::navigate_formation( } else { // Return from function when the asv is close to the second waypoint reach_waypoint(0.2); - return; + return {landmark_ids.at(assignment(0)), landmark_ids.at(assignment(1))}; } } } From f406191d166e9e86d09d6e438454fc0ac0e3ef24 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Fri, 26 Jul 2024 15:46:08 +0000 Subject: [PATCH 22/67] Committing clang-format changes --- mission/docking_task/src/docking_task_ros.cpp | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/mission/docking_task/src/docking_task_ros.cpp b/mission/docking_task/src/docking_task_ros.cpp index f0657f6b..f79e483e 100644 --- a/mission/docking_task/src/docking_task_ros.cpp +++ b/mission/docking_task/src/docking_task_ros.cpp @@ -526,7 +526,8 @@ DockingTaskNode::auction_algorithm(const Eigen::MatrixXd &reward_matrix) { return assignment; } -std::pair DockingTaskNode::navigate_formation(Eigen::Array &predicted_positions) { +std::pair DockingTaskNode::navigate_formation( + Eigen::Array &predicted_positions) { RCLCPP_INFO(this->get_logger(), "Navigating though formation"); std::vector prev_assignment; std::vector landmark_ids; @@ -626,23 +627,25 @@ std::pair DockingTaskNode::navigate_formation(Eigen::Array Date: Mon, 29 Jul 2024 20:23:24 +0200 Subject: [PATCH 23/67] njord task structure Added base njord task node that other tasks will inherit from --- .../include/docking_task/docking_task_ros.hpp | 171 ------- .../collision_avoidance_task/CMakeLists.txt | 73 +++ .../collision_avoidance_task_ros.hpp | 27 ++ .../launch/collision_avoidance_task_launch.py | 17 + .../collision_avoidance_task/package.xml | 29 ++ .../collision_avoidance_task_params.yaml | 20 + .../src/collision_avoidance_task_node.cpp | 14 + .../src/collision_avoidance_task_ros.cpp | 36 ++ .../docking_task/CMakeLists.txt | 8 +- .../include/docking_task/docking_task_ros.hpp | 102 +++++ .../launch/docking_task_launch.py | 0 mission/njord_tasks/docking_task/package.xml | 29 ++ .../params/docking_task_params.yaml | 0 .../docking_task/src/docking_task_node.cpp | 0 .../docking_task/src/docking_task_ros.cpp | 431 ++++-------------- .../maneuvering_task/CMakeLists.txt | 73 +++ .../maneuvering_task/maneuvering_task_ros.hpp | 27 ++ .../launch/maneuvering_task_launch.py | 17 + .../njord_tasks/maneuvering_task/package.xml | 29 ++ .../params/maneuvering_task_params.yaml | 20 + .../src/maneuvering_task_node.cpp | 14 + .../src/maneuvering_task_ros.cpp | 38 ++ .../navigation_task/CMakeLists.txt | 71 +++ .../navigation_task/navigation_task_ros.hpp | 27 ++ .../launch/navigation_task_launch.py | 17 + .../njord_tasks/navigation_task/package.xml | 29 ++ .../params/navigation_task_params.yaml | 20 + .../src/navigation_task_node.cpp | 14 + .../src/navigation_task_ros.cpp | 38 ++ .../njord_task_base/CMakeLists.txt | 85 ++++ .../njord_task_base/njord_task_base_ros.hpp | 73 +++ .../njord_task_base}/package.xml | 2 +- .../src/njord_task_base_ros.cpp | 266 +++++++++++ 33 files changed, 1297 insertions(+), 520 deletions(-) delete mode 100644 mission/docking_task/include/docking_task/docking_task_ros.hpp create mode 100644 mission/njord_tasks/collision_avoidance_task/CMakeLists.txt create mode 100644 mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp create mode 100644 mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py create mode 100644 mission/njord_tasks/collision_avoidance_task/package.xml create mode 100644 mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml create mode 100644 mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp create mode 100644 mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp rename mission/{ => njord_tasks}/docking_task/CMakeLists.txt (83%) create mode 100644 mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp rename mission/{ => njord_tasks}/docking_task/launch/docking_task_launch.py (100%) create mode 100644 mission/njord_tasks/docking_task/package.xml rename mission/{ => njord_tasks}/docking_task/params/docking_task_params.yaml (100%) rename mission/{ => njord_tasks}/docking_task/src/docking_task_node.cpp (100%) rename mission/{ => njord_tasks}/docking_task/src/docking_task_ros.cpp (56%) create mode 100644 mission/njord_tasks/maneuvering_task/CMakeLists.txt create mode 100644 mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp create mode 100644 mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py create mode 100644 mission/njord_tasks/maneuvering_task/package.xml create mode 100644 mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml create mode 100644 mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp create mode 100644 mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp create mode 100644 mission/njord_tasks/navigation_task/CMakeLists.txt create mode 100644 mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp create mode 100644 mission/njord_tasks/navigation_task/launch/navigation_task_launch.py create mode 100644 mission/njord_tasks/navigation_task/package.xml create mode 100644 mission/njord_tasks/navigation_task/params/navigation_task_params.yaml create mode 100644 mission/njord_tasks/navigation_task/src/navigation_task_node.cpp create mode 100644 mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp create mode 100644 mission/njord_tasks/njord_task_base/CMakeLists.txt create mode 100644 mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp rename mission/{docking_task => njord_tasks/njord_task_base}/package.xml (96%) create mode 100644 mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp diff --git a/mission/docking_task/include/docking_task/docking_task_ros.hpp b/mission/docking_task/include/docking_task/docking_task_ros.hpp deleted file mode 100644 index 1cb8825a..00000000 --- a/mission/docking_task/include/docking_task/docking_task_ros.hpp +++ /dev/null @@ -1,171 +0,0 @@ -#ifndef DOCKING_TASK_ROS_HPP -#define DOCKING_TASK_ROS_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace docking_task { - -struct LandmarkWithID { - geometry_msgs::msg::Pose pose_map_frame; - geometry_msgs::msg::Pose pose_base_link_frame; - uint32_t id; -}; - -/** - * @class DockingTaskNode - * @brief A class representing a node for handling dock localization in a ROS 2 - * system. - * - * This class inherits from rclcpp::Node and provides functionality for - * localizing the dock in the map. - */ -class DockingTaskNode : public rclcpp::Node { -public: - /** - * @brief Constructor for the DockingTaskNode class. - * - * @param options The options for configuring the node. - */ - explicit DockingTaskNode( - const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - /** - * @brief Destructor for the DockLocalizationNode class. - */ - ~DockingTaskNode(){}; - - /** - * @brief Main task for the DockingTaskNode class. - */ - void main_task(); - - /** - * @brief Detect the closest buoy pair and set a waypoint between them - * - * @return A pair of landmarks representing the closest buoy pair - */ - std::pair initial_waypoint(); - - /** - * @brief Predict the 6-tuple formation of buoys - * - * @param landmark1 The first landmark/buoy used for initial waypoint - * @param landmark2 The second landmark/buoy used for initial waypoint - * - * @return The predicted posistion of the 6-tuple formation of buoys in odom - * frame - */ - Eigen::Array - predict_buoy_formation(const LandmarkWithID &buoy1, - const LandmarkWithID &buoy2) const; - - /** - * @brief Generate the reward matrix for the auction algorithm - * - * @param predicted_positions The predicted positions of the buoys - * @param measured_positions The measured positions of landmarks - * - * @return The reward matrix for the auction algorithm - */ - Eigen::MatrixXd - generate_reward_matrix(const Eigen::Array &predicted_positions, - const Eigen::MatrixXd &measured_positions); - - /** - * @brief The auction algorithm for the assignment problem - * - * @param reward_matrix The reward matrix for the auction algorithm - * - * @return The assignment of landmarks to buoys - */ - Eigen::VectorXi auction_algorithm(const Eigen::MatrixXd &reward_matrix); - - /** - * @brief Navigate the ASV through the formation of buoys. First travels to - * waypoint between second pair of buoys, then navigates through the formation - * of buoys and returns control when asv is 0.2m away from crossing the last - * buoy pair. - * - * @param predicted_positions The predicted positions of the buoys - * @return The ids of the last pair of buoys in the formation - */ - std::pair - navigate_formation(Eigen::Array &predicted_positions); - - /** - * @brief Calculate the coordinates of a gps point in the map frame - * - * @param lat The latitude of the gps point - * @param lon The longitude of the gps point - * - * @return The coordinates of the gps point in the map frame - */ - std::pair lla2flat(double lat, double lon) const; - - /** - * @brief Utility function to get the heading of the ASV assuming identical - * orientation between odom and map frame - * - * @param msg The quat message derived from the odom message published by the - * seapath - */ - double get_freya_heading(const geometry_msgs::msg::Quaternion msg) const; - - /** - * @brief Utility function to set the position of the GPS coordinates in the - * map frame - */ - void set_gps_frame_coords(); - - /** - * @brief Utility function that runs until waypoint is reached. - * Function returns when within distance_threshold of the waypoint. - * - * @param distance_threshold The distance threshold for reaching the waypoint - */ - void reach_waypoint(const double distance_threshold); - -private: - mutable std::mutex odom_mutex_; - mutable std::mutex grid_mutex_; - mutable std::mutex landmark_mutex_; - - rclcpp::Subscription::SharedPtr map_origin_sub_; - rclcpp::Subscription::SharedPtr grid_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr - landmarks_sub_; - - rclcpp::Client::SharedPtr waypoint_client_; - - rclcpp::Publisher::SharedPtr - waypoint_visualization_pub_; - rclcpp::Publisher::SharedPtr - gps_map_coord_visualization_pub_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_; - nav_msgs::msg::Odometry::SharedPtr odom_msg_; - vortex_msgs::msg::LandmarkArray::SharedPtr landmarks_msg_; - - geometry_msgs::msg::Point previous_waypoint_odom_frame_; -}; - -} // namespace docking_task - -#endif // DOCKING_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt b/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt new file mode 100644 index 00000000..73e37226 --- /dev/null +++ b/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.8) +project(collision_avoidance_task) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(njord_task_base REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +include_directories(include) + +add_executable(collision_avoidance_task_node + src/collision_avoidance_task_node.cpp + src/collision_avoidance_task_ros.cpp) + +target_link_libraries(collision_avoidance_task_node + tf2::tf2 + tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs +) + +ament_target_dependencies(collision_avoidance_task_node + rclcpp + njord_task_base + geometry_msgs + nav_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs +) + +install( + DIRECTORY include/ + DESTINATION include + +) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME}/ + ) + +install(DIRECTORY + launch + params + DESTINATION share/${PROJECT_NAME}/ +) + + +ament_package() diff --git a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp new file mode 100644 index 00000000..64c70ab4 --- /dev/null +++ b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp @@ -0,0 +1,27 @@ +#ifndef COLLISION_AVOIDANCE_TASK_ROS_HPP +#define COLLISION_AVOIDANCE_TASK_ROS_HPP + +#include + +namespace collision_avoidance_task { + +class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { +public: + explicit CollisionAvoidanceTaskNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + + ~CollisionAvoidanceTaskNode(){}; + + /** + * @brief Main task for the CollisionAvoidanceTaskNode class. + */ + void main_task(); + +private: + geometry_msgs::msg::Point previous_waypoint_odom_frame_; + +}; + +} // namespace collision_avoidance_task + +#endif // COLLISION_AVOIDANCE_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py b/mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py new file mode 100644 index 00000000..40e7e2fa --- /dev/null +++ b/mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py @@ -0,0 +1,17 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + collision_avoidance_task_node = Node( + package='collision_avoidance_task', + executable='collision_avoidance_task_node', + name='collision_avoidance_task_node', + parameters=[os.path.join(get_package_share_directory('collision_avoidance_task'),'params','collision_avoidance_task_params.yaml')], + # arguments=['--ros-args', '--log-level', 'DEBUG'], + output='screen', + ) + return LaunchDescription([ + collision_avoidance_task_node + ]) \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/package.xml b/mission/njord_tasks/collision_avoidance_task/package.xml new file mode 100644 index 00000000..5d19b351 --- /dev/null +++ b/mission/njord_tasks/collision_avoidance_task/package.xml @@ -0,0 +1,29 @@ + + + + collision_avoidance_task + 0.0.0 + TODO: Package description + jorgen + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + njord_task_base + nav_msgs + geometry_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + + + ament_cmake + + diff --git a/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml b/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml new file mode 100644 index 00000000..4c7dd0b3 --- /dev/null +++ b/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml @@ -0,0 +1,20 @@ +collision_avoidance_task_node: + ros__parameters: + map_origin_lat: 63.4411585867919 + map_origin_lon: 10.419400373255625 + map_origin_set: true + gps_start_lat: 63.44097054434422 + gps_start_lon: 10.419997767413607 + gps_end_lat: 63.44125901804796 + gps_end_lon: 10.41857835889424 + gps_start_x: 0.0 + gps_start_y: 0.0 + gps_end_x: 0.0 + gps_end_y: 0.0 + gps_frame_coords_set: false + + + map_origin_topic: "/map/origin" + odom_topic: "/seapath/odom/ned" + landmark_pose_topic: "landmarks_out" + \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp new file mode 100644 index 00000000..5c42dbea --- /dev/null +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp @@ -0,0 +1,14 @@ +#include +#include + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp new file mode 100644 index 00000000..2c3f5668 --- /dev/null +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -0,0 +1,36 @@ +#include + +namespace collision_avoidance_task { + +CollisionAvoidanceTaskNode::CollisionAvoidanceTaskNode(const rclcpp::NodeOptions &options) + : NjordTaskBaseNode("collision_avoidance_task_node", options) { + + std::thread(&CollisionAvoidanceTaskNode::main_task, this).detach(); +} + +void CollisionAvoidanceTaskNode::main_task() { + // Sleep for 3 seconds to allow system to initialize and tracks to be aquired + RCLCPP_INFO(this->get_logger(), + "Waiting 3 seconds for system to initialize before starting main task"); + rclcpp::sleep_for(std::chrono::seconds(3)); + + while(true) { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + std::unique_lock setup_lock(setup_mutex_); + if (!(this->get_parameter("map_origin_set").as_bool())) { + RCLCPP_INFO(this->get_logger(), + "Map origin not set, sleeping for 100ms"); + setup_lock.unlock(); + continue; + } + if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { + setup_map_odom_tf_and_subs(); + set_gps_frame_coords(); + setup_lock.unlock(); + break; + } + setup_lock.unlock(); + } +} + +} // namespace collision_avoidance_task \ No newline at end of file diff --git a/mission/docking_task/CMakeLists.txt b/mission/njord_tasks/docking_task/CMakeLists.txt similarity index 83% rename from mission/docking_task/CMakeLists.txt rename to mission/njord_tasks/docking_task/CMakeLists.txt index 15bfb0ce..c9298cc7 100644 --- a/mission/docking_task/CMakeLists.txt +++ b/mission/njord_tasks/docking_task/CMakeLists.txt @@ -19,6 +19,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(njord_task_base REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(sensor_msgs REQUIRED) @@ -33,12 +34,6 @@ add_executable(docking_task_node src/docking_task_node.cpp src/docking_task_ros.cpp) -# target_include_directories(dock_localization_node PUBLIC -# $ -# $) - -# target_compile_features(dock_localization_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 - target_link_libraries(docking_task_node tf2::tf2 tf2_ros::tf2_ros @@ -47,6 +42,7 @@ target_link_libraries(docking_task_node ament_target_dependencies(docking_task_node rclcpp + njord_task_base geometry_msgs nav_msgs sensor_msgs diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp new file mode 100644 index 00000000..e4977d4f --- /dev/null +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -0,0 +1,102 @@ +#ifndef DOCKING_TASK_ROS_HPP +#define DOCKING_TASK_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include + +namespace docking_task { + +struct LandmarkWithID { + geometry_msgs::msg::Pose pose_odom_frame; + geometry_msgs::msg::Pose pose_base_link_frame; + int32_t id; +}; + +/** + * @class DockingTaskNode + * @brief A class representing a node for handling dock localization in a ROS 2 + * system. + * + * This class inherits from rclcpp::Node and provides functionality for + * localizing the dock in the map. + */ +class DockingTaskNode : public NjordTaskBaseNode { +public: + /** + * @brief Constructor for the DockingTaskNode class. + * + * @param options The options for configuring the node. + */ + explicit DockingTaskNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + + /** + * @brief Destructor for the DockLocalizationNode class. + */ + ~DockingTaskNode(){}; + + /** + * @brief Main task for the DockingTaskNode class. + */ + void main_task(); + + /** + * @brief Detect the closest buoy pair and set a waypoint between them + * + * @return A pair of landmarks representing the closest buoy pair + */ + std::pair initial_waypoint(); + + /** + * @brief Predict the 6-tuple formation of buoys + * + * @param landmark1 The first landmark/buoy used for initial waypoint + * @param landmark2 The second landmark/buoy used for initial waypoint + * + * @return The predicted posistion of the 6-tuple formation of buoys in odom + * frame + */ + Eigen::Array + predict_buoy_formation(const LandmarkWithID &buoy1, + const LandmarkWithID &buoy2); + + /** + * @brief Navigate the ASV through the formation of buoys. First travels to + * waypoint between second pair of buoys, then navigates through the formation + * of buoys and returns control when asv is 0.2m away from crossing the last + * buoy pair. + * + * @param predicted_positions The predicted positions of the buoys + * @return The ids of the last pair of buoys in the formation + */ + std::pair + navigate_formation(Eigen::Array &predicted_positions); + + /** + * @brief Utility function that runs until waypoint is reached. + * Function returns when within distance_threshold of the waypoint. + * + * @param distance_threshold The distance threshold for reaching the waypoint + */ + void reach_waypoint(const double distance_threshold); + +private: + mutable std::mutex grid_mutex_; + bool new_grid_msg_ = false; + std::condition_variable grid_cond_var_; + + rclcpp::Subscription::SharedPtr grid_sub_; + + nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_; + + geometry_msgs::msg::Point previous_waypoint_odom_frame_; +}; + +} // namespace docking_task + +#endif // DOCKING_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/docking_task/launch/docking_task_launch.py b/mission/njord_tasks/docking_task/launch/docking_task_launch.py similarity index 100% rename from mission/docking_task/launch/docking_task_launch.py rename to mission/njord_tasks/docking_task/launch/docking_task_launch.py diff --git a/mission/njord_tasks/docking_task/package.xml b/mission/njord_tasks/docking_task/package.xml new file mode 100644 index 00000000..e9d0a452 --- /dev/null +++ b/mission/njord_tasks/docking_task/package.xml @@ -0,0 +1,29 @@ + + + + docking_task + 0.0.0 + TODO: Package description + jorgen + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + njord_task_base + nav_msgs + geometry_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + + + ament_cmake + + diff --git a/mission/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml similarity index 100% rename from mission/docking_task/params/docking_task_params.yaml rename to mission/njord_tasks/docking_task/params/docking_task_params.yaml diff --git a/mission/docking_task/src/docking_task_node.cpp b/mission/njord_tasks/docking_task/src/docking_task_node.cpp similarity index 100% rename from mission/docking_task/src/docking_task_node.cpp rename to mission/njord_tasks/docking_task/src/docking_task_node.cpp diff --git a/mission/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp similarity index 56% rename from mission/docking_task/src/docking_task_ros.cpp rename to mission/njord_tasks/docking_task/src/docking_task_ros.cpp index f79e483e..98b44944 100644 --- a/mission/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -3,20 +3,9 @@ namespace docking_task { DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) - : Node("dock_localization_node", options) { - - declare_parameter("map_origin_lat", 0.0); - declare_parameter("map_origin_lon", 0.0); - declare_parameter("map_origin_set", false); - declare_parameter("gps_start_lat", 0.0); - declare_parameter("gps_start_lon", 0.0); - declare_parameter("gps_end_lat", 0.0); - declare_parameter("gps_end_lon", 0.0); - declare_parameter("gps_start_x", 0.0); - declare_parameter("gps_start_y", 0.0); - declare_parameter("gps_end_x", 0.0); - declare_parameter("gps_end_y", 0.0); - declare_parameter("gps_frame_coords_set", false); + : NjordTaskBaseNode("dock_localization_node", options) { + + declare_parameter("dock_width", 0.0); declare_parameter("dock_width_tolerance", 0.0); declare_parameter("dock_length", 0.0); @@ -27,82 +16,7 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) declare_parameter("task_nr", 0.0); declare_parameter("models.dynmod_stddev", 0.0); declare_parameter("models.sen_stddev", 0.0); - - declare_parameter("map_origin_topic", "/map/origin"); declare_parameter("grid_topic", "grid"); - declare_parameter("odom_topic", "/seapath/odom/ned"); - declare_parameter("landmark_pose_topic", "/landmark/pose"); - - // Sensor data QoS profile - rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; - auto qos_sensor_data = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), - qos_profile_sensor_data); - - // Transient local QoS profile - rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; - qos_profile_transient_local.durability = - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - auto qos_transient_local = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), - qos_profile_transient_local); - - gps_map_coord_visualization_pub_ = - this->create_publisher( - "/gps_map_coord_visualization", qos_sensor_data); - - map_origin_sub_ = this->create_subscription( - get_parameter("map_origin_topic").as_string(), qos_transient_local, - [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - this->set_parameter(rclcpp::Parameter("map_origin_lat", msg->latitude)); - this->set_parameter( - rclcpp::Parameter("map_origin_lon", msg->longitude)); - this->set_parameter(rclcpp::Parameter("map_origin_set", true)); - RCLCPP_INFO(this->get_logger(), "Map origin set to: %f, %f", - msg->latitude, msg->longitude); - - // Set GPS frame coordinates - set_gps_frame_coords(); - - map_origin_sub_.reset(); - }); - - grid_sub_ = this->create_subscription( - get_parameter("grid_topic").as_string(), qos_sensor_data, - [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received grid message"); - std::unique_lock lock(grid_mutex_); - grid_msg_ = msg; - lock.unlock(); - }); - - odom_sub_ = this->create_subscription( - get_parameter("odom_topic").as_string(), qos_sensor_data, - [this](const nav_msgs::msg::Odometry::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received odometry message"); - std::unique_lock lock(odom_mutex_); - odom_msg_ = msg; - lock.unlock(); - }); - - landmarks_sub_ = this->create_subscription( - get_parameter("landmark_pose_topic").as_string(), qos_sensor_data, - [this](const vortex_msgs::msg::LandmarkArray::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received landmark pose array"); - std::unique_lock lock(landmark_mutex_); - landmarks_msg_ = msg; - lock.unlock(); - }); - - waypoint_visualization_pub_ = - this->create_publisher( - "/waypoint_visualization", qos_sensor_data); - - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - waypoint_client_ = - this->create_client("/waypoint"); std::thread(&DockingTaskNode::main_task, this).detach(); } @@ -110,9 +24,26 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) void DockingTaskNode::main_task() { // Sleep for 5 seconds to allow system to initialize and tracks to be aquired RCLCPP_INFO(this->get_logger(), - "Waiting for system to initialize before starting main task"); - rclcpp::sleep_for(std::chrono::seconds(5)); - RCLCPP_INFO(this->get_logger(), "System initialized, starting main task"); + "Waiting 3 seconds for system to initialize before starting main task"); + rclcpp::sleep_for(std::chrono::seconds(3)); + + while(true) { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + std::unique_lock setup_lock(setup_mutex_); + if (!(this->get_parameter("map_origin_set").as_bool())) { + RCLCPP_INFO(this->get_logger(), + "Map origin not set, sleeping for 100ms"); + setup_lock.unlock(); + continue; + } + if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { + set_gps_frame_coords(); + setup_map_odom_tf_and_subs(); + setup_lock.unlock(); + break; + } + setup_lock.unlock(); + } // Set initial waypoint between first buoy pair auto [landmark1, landmark2] = initial_waypoint(); @@ -131,15 +62,7 @@ std::pair DockingTaskNode::initial_waypoint() { RCLCPP_INFO(this->get_logger(), "Initial waypoint running"); while (true) { - rclcpp::sleep_for(std::chrono::seconds(1)); - if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), - "Map origin not set, exiting initial waypoint"); - continue; - } - if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - set_gps_frame_coords(); - } + // std::unique_lock odom_lock(odom_mutex_); // if (odom_msg_ == nullptr) { // RCLCPP_INFO(this->get_logger(), "Odometry message not received, exiting @@ -196,37 +119,29 @@ std::pair DockingTaskNode::initial_waypoint() { int result = 0; double x_waypoint = 0.0; double y_waypoint = 0.0; - std::pair id_pair; + std::pair id_pair; std::pair landmark_return_pair; while (result < 10) { // Transform landmark poses to base_link frame - geometry_msgs::msg::PoseArray landmark_poses_map_frame; - geometry_msgs::msg::PoseArray landmark_poses_base_link_frame; std::vector landmarks_with_ids; - std::unique_lock landmark_lock(landmark_mutex_); - for (const auto &landmark : landmarks_msg_->landmarks) { + auto landmark_msg = get_landmarks_odom_frame(); + for (const auto &landmark : landmark_msg->landmarks) { LandmarkWithID lwid; lwid.id = landmark.id; - lwid.pose_map_frame = landmark.odom.pose.pose; - landmark_poses_map_frame.poses.push_back(landmark.odom.pose.pose); + lwid.pose_odom_frame = landmark.odom.pose.pose; landmarks_with_ids.push_back(lwid); } - landmark_lock.unlock(); - try { auto transform = - tf_buffer_->lookupTransform("base_link", "map", rclcpp::Time(0)); - for (size_t i = 0; i < landmark_poses_base_link_frame.poses.size(); - ++i) { - tf2::doTransform(landmark_poses_map_frame.poses.at(i), - landmark_poses_base_link_frame.poses.at(i), + tf_buffer_->lookupTransform("base_link", "odom", rclcpp::Time(0)); + for (size_t i = 0; i < landmarks_with_ids.size(); ++i) { + tf2::doTransform(landmarks_with_ids.at(i).pose_odom_frame, + landmarks_with_ids.at(i).pose_base_link_frame, transform); - landmarks_with_ids[i].pose_base_link_frame = - landmark_poses_base_link_frame.poses[i]; } } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); @@ -265,21 +180,21 @@ std::pair DockingTaskNode::initial_waypoint() { 2)); // Check if the landmarks are on opposite sides of the drone and within - // the desired distance range Base link is NED frame, x is forward, y is - // right + // the desired distance range. + // Base link is NED frame, x is forward, y is right if ((landmark1.pose_base_link_frame.position.x > 0 && landmark2.pose_base_link_frame.position.x > 0) && (distance >= 3.0 && distance <= 8.0) && ((landmark1.pose_base_link_frame.position.y < 0) != (landmark2.pose_base_link_frame.position.y < 0))) { // Calculate the midpoint between the two landmarks in map frame - double x_waypoint_sample = (landmark1.pose_map_frame.position.x + - landmark2.pose_map_frame.position.x) / + double x_waypoint_sample = (landmark1.pose_odom_frame.position.x + + landmark2.pose_odom_frame.position.x) / 2; - double y_waypoint_sample = (landmark1.pose_map_frame.position.y + - landmark2.pose_map_frame.position.y) / + double y_waypoint_sample = (landmark1.pose_odom_frame.position.y + + landmark2.pose_odom_frame.position.y) / 2; - std::pair id_pair_sample = {landmark1.id, + std::pair id_pair_sample = {landmark1.id, landmark2.id}; // Check if this is the first valid pair of landmarks if (result == 0) { @@ -320,22 +235,15 @@ std::pair DockingTaskNode::initial_waypoint() { RCLCPP_INFO(this->get_logger(), "Waypoint client not ready"); continue; } - geometry_msgs::msg::Point waypoint_map_frame; geometry_msgs::msg::Point waypoint_odom_frame; - waypoint_map_frame.x = x_waypoint; - waypoint_map_frame.y = y_waypoint; - waypoint_map_frame.z = 0.0; - try { - auto transform = - tf_buffer_->lookupTransform("odom", "map", rclcpp::Time(0)); - tf2::doTransform(waypoint_map_frame, waypoint_odom_frame, transform); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - } + waypoint_odom_frame.x = x_waypoint; + waypoint_odom_frame.y = y_waypoint; + waypoint_odom_frame.z = 0.0; + auto request = std::make_shared(); request->waypoint.push_back(waypoint_odom_frame); auto result_future = waypoint_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Waypoint sent: %f, %f", + RCLCPP_INFO(this->get_logger(), "Waypoint(odom frame) sent: %f, %f", waypoint_odom_frame.x, waypoint_odom_frame.y); // Check if the service was successful @@ -361,18 +269,16 @@ std::pair DockingTaskNode::initial_waypoint() { void DockingTaskNode::reach_waypoint(const double distance_threshold) { RCLCPP_INFO(this->get_logger(), "Reach waypoint running"); - std::unique_lock odom_lock(odom_mutex_); - double x = odom_msg_->pose.pose.position.x; - double y = odom_msg_->pose.pose.position.y; - odom_lock.unlock(); + auto odom_msg = get_odom(); + double x = odom_msg->pose.pose.position.x; + double y = odom_msg->pose.pose.position.y; while (sqrt(pow(x - previous_waypoint_odom_frame_.x, 2) + pow(y - previous_waypoint_odom_frame_.y, 2)) > distance_threshold) { rclcpp::sleep_for(std::chrono::milliseconds(100)); - odom_lock.lock(); - x = odom_msg_->pose.pose.position.x; - y = odom_msg_->pose.pose.position.y; - odom_lock.unlock(); + auto odom_msg_ = get_odom(); + x = odom_msg->pose.pose.position.x; + y = odom_msg->pose.pose.position.y; } RCLCPP_INFO(this->get_logger(), "Reached waypoint"); return; @@ -380,35 +286,40 @@ void DockingTaskNode::reach_waypoint(const double distance_threshold) { Eigen::Array DockingTaskNode::predict_buoy_formation(const LandmarkWithID &buoy1, - const LandmarkWithID &buoy2) const { + const LandmarkWithID &buoy2) { RCLCPP_INFO(this->get_logger(), "Predict buoy formation running"); + geometry_msgs::msg::Point previous_waypoint_map_frame; + geometry_msgs::msg::TransformStamped odom_map_tf; try { - geometry_msgs::msg::Point previous_waypoint_map_frame; - auto transform = - tf_buffer_->lookupTransform("map", "odom", rclcpp::Time(0)); - tf2::doTransform(previous_waypoint_odom_frame_, previous_waypoint_map_frame, - transform); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - } + // Compute the inverse transform from the stored map_odom_tf_ + tf2::Transform tf2_transform; + tf2::fromMsg(map_odom_tf_.transform, tf2_transform); + tf2::Transform tf2_inverse_transform = tf2_transform.inverse(); + odom_map_tf.transform = tf2::toMsg(tf2_inverse_transform); + + // Use the inverse transform + tf2::doTransform(previous_waypoint_odom_frame_, previous_waypoint_map_frame, odom_map_tf); +} catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); +} Eigen::Vector2d direction_vector( - previous_waypoint_odom_frame_.x - + previous_waypoint_map_frame.x - this->get_parameter("gps_start_x").as_double(), - previous_waypoint_odom_frame_.y - + previous_waypoint_map_frame.y - this->get_parameter("gps_start_y").as_double()); direction_vector.normalize(); // Sanity check that first buoy pair is ish correct Eigen::Vector2d first_left, first_right; size_t found = 0; - std::unique_lock landmark_lock(landmark_mutex_); - for (const auto &landmark : landmarks_msg_->landmarks) { + auto landmarks_msg = get_landmarks_odom_frame(); + for (const auto &landmark : landmarks_msg->landmarks) { if (landmark.id == buoy1.id && std::sqrt(std::pow(landmark.odom.pose.pose.position.x - - buoy1.pose_map_frame.position.x, + buoy1.pose_odom_frame.position.x, 2) + std::pow(landmark.odom.pose.pose.position.y - - buoy1.pose_map_frame.position.y, + buoy1.pose_odom_frame.position.y, 2)) < 0.5) { found++; if (buoy1.pose_base_link_frame.position.y < 0) { @@ -421,10 +332,10 @@ DockingTaskNode::predict_buoy_formation(const LandmarkWithID &buoy1, } if (landmark.id == buoy2.id && std::sqrt(std::pow(landmark.odom.pose.pose.position.x - - buoy2.pose_map_frame.position.x, + buoy2.pose_odom_frame.position.x, 2) + std::pow(landmark.odom.pose.pose.position.y - - buoy2.pose_map_frame.position.y, + buoy2.pose_odom_frame.position.y, 2)) < 0.5) { found++; if (buoy2.pose_base_link_frame.position.y < 0) { @@ -436,7 +347,6 @@ DockingTaskNode::predict_buoy_formation(const LandmarkWithID &buoy1, } } } - landmark_lock.unlock(); if (found != 2) { RCLCPP_WARN(this->get_logger(), "Buoy pair not found in landmark array"); } @@ -459,78 +369,11 @@ DockingTaskNode::predict_buoy_formation(const LandmarkWithID &buoy1, return formation; } -Eigen::MatrixXd DockingTaskNode::generate_reward_matrix( - const Eigen::Array &predicted_positions, - const Eigen::MatrixXd &measured_positions) { - int num_predicted = predicted_positions.cols(); // Should be 6 - int num_measured = measured_positions.cols(); - Eigen::MatrixXd reward_matrix(num_measured, num_predicted); - - // Initialize the reward matrix - for (int i = 0; i < num_measured; ++i) { - for (int j = 0; j < num_predicted; ++j) { - double dx = measured_positions(0, i) - predicted_positions(0, j); - double dy = measured_positions(1, i) - predicted_positions(1, j); - double distance = std::sqrt(dx * dx + dy * dy); - reward_matrix(i, j) = -distance; // We use negative distance as reward - // (shorter distance = higher reward) - } - } - return reward_matrix; -} - -Eigen::VectorXi -DockingTaskNode::auction_algorithm(const Eigen::MatrixXd &reward_matrix) { - int num_items = reward_matrix.rows(); - int num_customers = reward_matrix.cols(); - Eigen::VectorXi assignment = Eigen::VectorXi::Constant(num_customers, -1); - Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_items); - - std::vector unassigned; - for (int i = 0; i < num_customers; ++i) { - unassigned.push_back(i); - } - - double epsilon = 1.0 / (num_items + 1); - - while (!unassigned.empty()) { - int customer = unassigned.back(); - unassigned.pop_back(); - - double max_value = std::numeric_limits::lowest(); - int max_item = -1; - for (int item = 0; item < reward_matrix.rows(); ++item) { - double value = reward_matrix.coeff(item, customer) - prices[item]; - if (value > max_value) { - max_value = value; - max_item = item; - } - } - - // Find the current owner of max item - int current_owner = -1; - for (int i = 0; i < num_customers; ++i) { - if (assignment[i] == max_item) { - current_owner = i; - break; - } - } - if (current_owner != -1) { - unassigned.push_back(current_owner); - } - - assignment[customer] = max_item; - prices[max_item] += max_value + epsilon; - } - - return assignment; -} - -std::pair DockingTaskNode::navigate_formation( +std::pair DockingTaskNode::navigate_formation( Eigen::Array &predicted_positions) { RCLCPP_INFO(this->get_logger(), "Navigating though formation"); - std::vector prev_assignment; - std::vector landmark_ids; + std::vector prev_assignment; + std::vector landmark_ids; bool first_half = true; // Buoy formation with index // dock @@ -541,27 +384,12 @@ std::pair DockingTaskNode::navigate_formation( int result = 0; while (true) { landmark_ids.clear(); - geometry_msgs::msg::PoseArray landmark_poses_map_frame; - std::unique_lock landmark_lock(landmark_mutex_); - for (const auto &landmark : landmarks_msg_->landmarks) { + geometry_msgs::msg::PoseArray landmark_poses_odom_frame; + auto landmark_msg = get_landmarks_odom_frame(); + for (const auto &landmark : landmark_msg->landmarks) { landmark_ids.push_back(landmark.id); - landmark_poses_map_frame.poses.push_back(landmark.odom.pose.pose); - } - - landmark_lock.unlock(); - - // Transform landmark poses to odom_frame - geometry_msgs::msg::PoseArray landmark_poses_odom_frame; - try { - auto transform = - tf_buffer_->lookupTransform("odom", "map", rclcpp::Time(0)); - for (size_t i = 0; i < landmark_poses_map_frame.poses.size(); ++i) { - tf2::doTransform(landmark_poses_map_frame.poses.at(i), - landmark_poses_odom_frame.poses.at(i), transform); - } - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + landmark_poses_odom_frame.poses.push_back(landmark.odom.pose.pose); } Eigen::MatrixXd measured_positions(2, landmark_ids.size()); @@ -572,10 +400,7 @@ std::pair DockingTaskNode::navigate_formation( landmark_poses_odom_frame.poses.at(i).position.y; } - Eigen::MatrixXd reward_matrix = - generate_reward_matrix(predicted_positions, measured_positions); - - Eigen::VectorXi assignment = auction_algorithm(reward_matrix); + Eigen::VectorXi assignment = assign_landmarks(predicted_positions, measured_positions); if (result == 0) { for (Eigen::Index i = 0; i < assignment.size(); i++) { @@ -587,6 +412,11 @@ std::pair DockingTaskNode::navigate_formation( bool valied = true; // Check that the assigned landmarks matches the previous assignment by id if (first_half) { + if (assignment(2) == -1 || assignment(3) == -1 || assignment(4) == -1 || + assignment(5) == -1) { + valied = false; + } + // Check index 2,3,4,5 if (landmark_ids.at(assignment(2)) != prev_assignment.at(2) || landmark_ids.at(assignment(3)) != prev_assignment.at(3) || @@ -595,6 +425,10 @@ std::pair DockingTaskNode::navigate_formation( valied = false; } } else { + if (assignment(0) == -1 || assignment(1) == -1 || assignment(2) == -1 || + assignment(3) == -1) { + valied = false; + } // Check index 0,1,2,3 if (landmark_ids.at(assignment(0)) != prev_assignment.at(0) || landmark_ids.at(assignment(1)) != prev_assignment.at(1) || @@ -703,91 +537,4 @@ std::pair DockingTaskNode::navigate_formation( } } -void DockingTaskNode::set_gps_frame_coords() { - auto [gps_start_x, gps_start_y] = - lla2flat(this->get_parameter("gps_start_lat").as_double(), - this->get_parameter("gps_start_lon").as_double()); - auto [gps_end_x, gps_end_y] = - lla2flat(this->get_parameter("gps_end_lat").as_double(), - this->get_parameter("gps_end_lon").as_double()); - this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_x)); - this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_y)); - this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_x)); - this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_y)); - this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); - RCLCPP_INFO(this->get_logger(), - "GPS map frame coordinates set to: %f, %f, %f, %f", gps_start_x, - gps_start_y, gps_end_x, gps_end_y); - - geometry_msgs::msg::PoseArray gps_points; - gps_points.header.frame_id = "map"; - - // Convert GPS points to geometry poses - geometry_msgs::msg::Pose start_pose; - start_pose.position.x = gps_start_x; - start_pose.position.y = gps_start_y; - gps_points.poses.push_back(start_pose); - - geometry_msgs::msg::Pose end_pose; - end_pose.position.x = gps_end_x; - end_pose.position.y = gps_end_y; - gps_points.poses.push_back(end_pose); - - gps_map_coord_visualization_pub_->publish(gps_points); -} - -std::pair DockingTaskNode::lla2flat(double lat, - double lon) const { - const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) - const double f = 1.0 / 298.257223563; // Flattening of the earth - const double psi_rad = 0.0; - - // Angular direction of the flat Earth x-axis, specified as a scalar. - // The angular direction is the degrees clockwise from north, - // which is the angle in degrees used for converting flat Earth x and - // y coordinates to the north and east coordinates - - // Convert angles from degrees to radians - const double lat_rad = lat * M_PI / 180.0; - const double lon_rad = lon * M_PI / 180.0; - const double origin_lat_rad = - this->get_parameter("map_origin_lat").as_double() * M_PI / 180.0; - const double origin_lon_rad = - this->get_parameter("map_origin_lon").as_double() * M_PI / 180.0; - - // Calculate delta latitude and delta longitude in radians - const double dlat = lat_rad - origin_lat_rad; - const double dlon = lon_rad - origin_lon_rad; - - // Radius of curvature in the vertical prime (RN) - const double RN = - R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); - - // Radius of curvature in the meridian (RM) - const double RM = RN * (1.0 - (2.0 * f - f * f)) / - (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); - - // Changes in the north (dN) and east (dE) positions - const double dN = RM * dlat; - const double dE = RN * cos(origin_lat_rad) * dlon; - - // Transformation from North-East to flat x-y coordinates - const double px = cos(psi_rad) * dN - sin(psi_rad) * dE; - const double py = sin(psi_rad) * dN + cos(psi_rad) * dE; - - return {px, py}; -} - -double DockingTaskNode::get_freya_heading( - const geometry_msgs::msg::Quaternion msg) const { - tf2::Quaternion quat; - tf2::fromMsg(msg, quat); - - // Convert quaternion to Euler angles - double roll, pitch, yaw; - tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw); - - return yaw; -} - } // namespace docking_task \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/CMakeLists.txt b/mission/njord_tasks/maneuvering_task/CMakeLists.txt new file mode 100644 index 00000000..c5027532 --- /dev/null +++ b/mission/njord_tasks/maneuvering_task/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.8) +project(maneuvering_task) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(njord_task_base REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +include_directories(include) + +add_executable(maneuvering_task_node + src/maneuvering_task_node.cpp + src/maneuvering_task_ros.cpp) + +target_link_libraries(maneuvering_task_node + tf2::tf2 + tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs +) + +ament_target_dependencies(maneuvering_task_node + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs + njord_task_base +) + +install( + DIRECTORY include/ + DESTINATION include + +) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME}/ + ) + +install(DIRECTORY + launch + params + DESTINATION share/${PROJECT_NAME}/ +) + + +ament_package() diff --git a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp new file mode 100644 index 00000000..c1992fb3 --- /dev/null +++ b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp @@ -0,0 +1,27 @@ +#ifndef MANEUVERING_TASK_ROS_HPP +#define MANEUVERING_TASK_ROS_HPP + +#include + +namespace maneuvering_task { + +class ManeuveringTaskNode : public NjordTaskBaseNode { +public: + explicit ManeuveringTaskNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + + ~ManeuveringTaskNode(){}; + + /** + * @brief Main task for the ManeuveringTaskNode class. + */ + void main_task(); + +private: + geometry_msgs::msg::Point previous_waypoint_odom_frame_; + +}; + +} // namespace maneuvering_task + +#endif // MANEUVERING_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py b/mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py new file mode 100644 index 00000000..da8a4eac --- /dev/null +++ b/mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py @@ -0,0 +1,17 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + maneuvering_task_node = Node( + package='maneuvering_task', + executable='maneuvering_task_node', + name='maneuvering_task_node', + parameters=[os.path.join(get_package_share_directory('maneuvering_task'),'params','maneuvering_task_params.yaml')], + # arguments=['--ros-args', '--log-level', 'DEBUG'], + output='screen', + ) + return LaunchDescription([ + maneuvering_task_node + ]) \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/package.xml b/mission/njord_tasks/maneuvering_task/package.xml new file mode 100644 index 00000000..52812f8d --- /dev/null +++ b/mission/njord_tasks/maneuvering_task/package.xml @@ -0,0 +1,29 @@ + + + + maneuvering_task + 0.0.0 + TODO: Package description + jorgen + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + njord_task_base + nav_msgs + geometry_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + + + ament_cmake + + diff --git a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml new file mode 100644 index 00000000..07648529 --- /dev/null +++ b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml @@ -0,0 +1,20 @@ +maneuvering_task_node: + ros__parameters: + map_origin_lat: 63.4411585867919 + map_origin_lon: 10.419400373255625 + map_origin_set: true + gps_start_lat: 63.44097054434422 + gps_start_lon: 10.419997767413607 + gps_end_lat: 63.44125901804796 + gps_end_lon: 10.41857835889424 + gps_start_x: 0.0 + gps_start_y: 0.0 + gps_end_x: 0.0 + gps_end_y: 0.0 + gps_frame_coords_set: false + + + map_origin_topic: "/map/origin" + odom_topic: "/seapath/odom/ned" + landmark_pose_topic: "landmarks_out" + \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp new file mode 100644 index 00000000..0c56e330 --- /dev/null +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp @@ -0,0 +1,14 @@ +#include +#include + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp new file mode 100644 index 00000000..8b6b0ddb --- /dev/null +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -0,0 +1,38 @@ +#include + +namespace maneuvering_task { + +ManeuveringTaskNode::ManeuveringTaskNode(const rclcpp::NodeOptions &options) + : NjordTaskBaseNode("maneuvering_task_node", options) { + + std::thread(&ManeuveringTaskNode::main_task, this).detach(); +} + +void ManeuveringTaskNode::main_task() { + // Sleep for 5 seconds to allow system to initialize and tracks to be aquired + RCLCPP_INFO(this->get_logger(), + "Waiting 3 seconds for system to initialize before starting main task"); + rclcpp::sleep_for(std::chrono::seconds(3)); + + while(true) { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + std::unique_lock setup_lock(setup_mutex_); + if (!(this->get_parameter("map_origin_set").as_bool())) { + RCLCPP_INFO(this->get_logger(), + "Map origin not set, sleeping for 100ms"); + setup_lock.unlock(); + continue; + } + if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { + set_gps_frame_coords(); + setup_map_odom_tf_and_subs(); + setup_lock.unlock(); + break; + } + setup_lock.unlock(); + } +} + + + +} // namespace maneuvering_task \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/CMakeLists.txt b/mission/njord_tasks/navigation_task/CMakeLists.txt new file mode 100644 index 00000000..1fe4ebaf --- /dev/null +++ b/mission/njord_tasks/navigation_task/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.8) +project(navigation_task) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(njord_task_base REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +include_directories(include) + +add_executable(navigation_task_node + src/navigation_task_node.cpp + src/navigation_task_ros.cpp +) + +target_link_libraries(navigation_task_node + tf2::tf2 + tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs +) + +ament_target_dependencies(navigation_task_node + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs + njord_task_base +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(TARGETS + navigation_task_node + DESTINATION lib/${PROJECT_NAME}/ +) + +install(DIRECTORY + launch + params + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp new file mode 100644 index 00000000..408a650a --- /dev/null +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -0,0 +1,27 @@ +#ifndef NAVIGATION_TASK_ROS_HPP +#define NAVIGATION_TASK_ROS_HPP + +#include + +namespace navigation_task { + +class NavigationTaskNode : public NjordTaskBaseNode { +public: + explicit NavigationTaskNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + + ~NavigationTaskNode(){}; + + /** + * @brief Main task for the NavigationTaskNode class. + */ + void main_task(); + +private: + geometry_msgs::msg::Point previous_waypoint_odom_frame_; + +}; + +} // namespace navigation_task + +#endif // NAVIGATION_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/launch/navigation_task_launch.py b/mission/njord_tasks/navigation_task/launch/navigation_task_launch.py new file mode 100644 index 00000000..c2dd17c7 --- /dev/null +++ b/mission/njord_tasks/navigation_task/launch/navigation_task_launch.py @@ -0,0 +1,17 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + navigation_task_node = Node( + package='navigation_task', + executable='navigation_task_node', + name='navigation_task_node', + parameters=[os.path.join(get_package_share_directory('navigation_task'),'params','navigation_task_params.yaml')], + # arguments=['--ros-args', '--log-level', 'DEBUG'], + output='screen', + ) + return LaunchDescription([ + navigation_task_node + ]) \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/package.xml b/mission/njord_tasks/navigation_task/package.xml new file mode 100644 index 00000000..a41c7cd7 --- /dev/null +++ b/mission/njord_tasks/navigation_task/package.xml @@ -0,0 +1,29 @@ + + + + navigation_task + 0.0.0 + TODO: Package description + jorgen + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + njord_task_base + rclcpp + nav_msgs + geometry_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + + + ament_cmake + + diff --git a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml new file mode 100644 index 00000000..70b3dad0 --- /dev/null +++ b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml @@ -0,0 +1,20 @@ +navigation_task_node: + ros__parameters: + map_origin_lat: 63.4411585867919 + map_origin_lon: 10.419400373255625 + map_origin_set: true + gps_start_lat: 63.44097054434422 + gps_start_lon: 10.419997767413607 + gps_end_lat: 63.44125901804796 + gps_end_lon: 10.41857835889424 + gps_start_x: 0.0 + gps_start_y: 0.0 + gps_end_x: 0.0 + gps_end_y: 0.0 + gps_frame_coords_set: false + + + map_origin_topic: "/map/origin" + odom_topic: "/seapath/odom/ned" + landmark_pose_topic: "landmarks_out" + \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_node.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_node.cpp new file mode 100644 index 00000000..4fe227ad --- /dev/null +++ b/mission/njord_tasks/navigation_task/src/navigation_task_node.cpp @@ -0,0 +1,14 @@ +#include +#include + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp new file mode 100644 index 00000000..4f365b8e --- /dev/null +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -0,0 +1,38 @@ +#include + +namespace navigation_task { + +NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options) + : NjordTaskBaseNode("navigation_task_node", options) { + + std::thread(&NavigationTaskNode::main_task, this).detach(); +} + +void NavigationTaskNode::main_task() { + // Sleep for 3 seconds to allow system to initialize and tracks to be aquired + RCLCPP_INFO(this->get_logger(), + "Waiting 3 seconds for system to initialize before starting main task"); + rclcpp::sleep_for(std::chrono::seconds(3)); + + while(true) { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + std::unique_lock setup_lock(setup_mutex_); + if (!(this->get_parameter("map_origin_set").as_bool())) { + RCLCPP_INFO(this->get_logger(), + "Map origin not set, sleeping for 100ms"); + setup_lock.unlock(); + continue; + } + if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { + setup_map_odom_tf_and_subs(); + set_gps_frame_coords(); + setup_lock.unlock(); + break; + } + setup_lock.unlock(); + } +} + + + +} // namespace navigation_task \ No newline at end of file diff --git a/mission/njord_tasks/njord_task_base/CMakeLists.txt b/mission/njord_tasks/njord_task_base/CMakeLists.txt new file mode 100644 index 00000000..2666fae7 --- /dev/null +++ b/mission/njord_tasks/njord_task_base/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.8) +project(njord_task_base) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +include_directories(include) + +# Create library target +add_library(${PROJECT_NAME}_lib + src/njord_task_base_ros.cpp +) + +ament_target_dependencies(${PROJECT_NAME}_lib PUBLIC + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs +) + +target_link_libraries(${PROJECT_NAME}_lib PUBLIC + tf2::tf2 + tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs +) + +# Specify the include directories for the library +target_include_directories(${PROJECT_NAME}_lib PUBLIC + "$" + "$" +) + +install(TARGETS ${PROJECT_NAME}_lib + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# Export the library +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + vortex_msgs + tf2 + tf2_ros + tf2_geometry_msgs +) + +install( + DIRECTORY include/ + DESTINATION include +) + +ament_package() diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp new file mode 100644 index 00000000..bfc573e5 --- /dev/null +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -0,0 +1,73 @@ +#ifndef NJORD_TASK_BASE_ROS_HPP +#define NJORD_TASK_BASE_ROS_HPP + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/pose_array.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "vortex_msgs/msg/landmark_array.hpp" +#include "vortex_msgs/srv/waypoint.hpp" +#include +#include +#include +#include +#include + +class NjordTaskBaseNode : public rclcpp::Node +{ +public: + NjordTaskBaseNode(const std::string &node_name, const rclcpp::NodeOptions &options); + +protected: + void setup_map_odom_tf_and_subs(); + void set_gps_frame_coords(); + std::pair lla2flat(double lat, double lon) const; + + void map_origin_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + std::shared_ptr get_odom(); + void landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg); + std::shared_ptr get_landmarks_odom_frame(); + + /** + * @brief Assign landmarks to buoys based on the predicted and measured positions using the auction algorithm. + * + * @param predicted_positions The predicted positions of the buoys + * @param measured_positions The measured positions of landmarks + * + * @return A vector of size equal to number of predicted positions(buoys), + * where each element is the index of the measured position(landmark) assigned to the corresponding predicted position(buoy). + * If an element is -1, it means that the corresponding predicted position(buoy) is not assigned to any measured position(landmark). + */ + Eigen::VectorXi assign_landmarks(const Eigen::Array &predicted_positions, + const Eigen::Array &measured_positions); + + rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; + rclcpp::Publisher::SharedPtr waypoint_visualization_pub_; + rclcpp::Subscription::SharedPtr map_origin_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr landmarks_sub_; + rclcpp::Client::SharedPtr waypoint_client_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + geometry_msgs::msg::TransformStamped map_odom_tf_; + + mutable std::mutex setup_mutex_; + mutable std::mutex odom_mutex_; + mutable std::mutex landmark_mutex_; + + std::condition_variable odom_cond_var_; + std::condition_variable landmark_cond_var_; + + std::shared_ptr odom_msg_; + std::shared_ptr landmarks_msg_; + bool new_odom_msg_ = false; + bool new_landmark_msg_ = false; +}; + +#endif // NJORD_TASK_BASE_ROS_HPP diff --git a/mission/docking_task/package.xml b/mission/njord_tasks/njord_task_base/package.xml similarity index 96% rename from mission/docking_task/package.xml rename to mission/njord_tasks/njord_task_base/package.xml index 25f54ea0..12b43f79 100644 --- a/mission/docking_task/package.xml +++ b/mission/njord_tasks/njord_task_base/package.xml @@ -1,7 +1,7 @@ - docking_task + njord_task_base 0.0.0 TODO: Package description jorgen diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp new file mode 100644 index 00000000..4b2e5cf5 --- /dev/null +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -0,0 +1,266 @@ +#include + +NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, const rclcpp::NodeOptions &options) + : Node(node_name, options) +{ + declare_parameter("map_origin_lat", 0.0); + declare_parameter("map_origin_lon", 0.0); + declare_parameter("map_origin_set", false); + declare_parameter("gps_start_lat", 0.0); + declare_parameter("gps_start_lon", 0.0); + declare_parameter("gps_end_lat", 0.0); + declare_parameter("gps_end_lon", 0.0); + declare_parameter("gps_start_x", 0.0); + declare_parameter("gps_start_y", 0.0); + declare_parameter("gps_end_x", 0.0); + declare_parameter("gps_end_y", 0.0); + declare_parameter("gps_frame_coords_set", false); + + declare_parameter("map_origin_topic", "/map/origin"); + declare_parameter("odom_topic", "/seapath/odom/ned"); + declare_parameter("landmark_pose_topic", "/landmark/pose"); + + // Sensor data QoS profile + rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; + auto qos_sensor_data = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), + qos_profile_sensor_data); + + // Transient local QoS profile + rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; + qos_profile_transient_local.durability = + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + auto qos_transient_local = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), + qos_profile_transient_local); + + gps_map_coord_visualization_pub_ = + this->create_publisher( + "/gps_map_coord_visualization", qos_sensor_data); + + map_origin_sub_ = this->create_subscription( + get_parameter("map_origin_topic").as_string(), qos_transient_local, + std::bind(&NjordTaskBaseNode::map_origin_callback, this, + std::placeholders::_1)); + + waypoint_visualization_pub_ = + this->create_publisher( + "/waypoint_visualization", qos_sensor_data); + + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + waypoint_client_ = + this->create_client("/waypoint"); +} + +void NjordTaskBaseNode::setup_map_odom_tf_and_subs() +{ + // Get the transform between the map and odom frames to avoid the overhead + // from continuously looking up the static transform between map and odom + bool tf_set = false; + while (!tf_set) { + try { + auto transform = tf_buffer_->lookupTransform("odom", "map", tf2::TimePointZero, tf2::durationFromSec(1.0)); + map_odom_tf_ = transform; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + std::this_thread::sleep_for(std::chrono::seconds(1)); + continue; + } + tf_set = true; + } + + // Sensor data QoS profile + rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), qos_profile_sensor_data); + + odom_sub_ = this->create_subscription( + get_parameter("odom_topic").as_string(), qos_sensor_data, + std::bind(&NjordTaskBaseNode::odom_callback, this, std::placeholders::_1)); + + landmarks_sub_ = this->create_subscription( + get_parameter("landmark_pose_topic").as_string(), qos_sensor_data, + std::bind(&NjordTaskBaseNode::landmark_callback, this, std::placeholders::_1)); +} + +void NjordTaskBaseNode::set_gps_frame_coords() +{ + auto [gps_start_x, gps_start_y] = lla2flat(this->get_parameter("gps_start_lat").as_double(), this->get_parameter("gps_start_lon").as_double()); + auto [gps_end_x, gps_end_y] = lla2flat(this->get_parameter("gps_end_lat").as_double(), this->get_parameter("gps_end_lon").as_double()); + this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_x)); + this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_y)); + this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_x)); + this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_y)); + this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); + RCLCPP_INFO(this->get_logger(), "GPS map frame coordinates set to: %f, %f, %f, %f", gps_start_x, gps_start_y, gps_end_x, gps_end_y); + + geometry_msgs::msg::PoseArray gps_points; + gps_points.header.frame_id = "map"; + + // Convert GPS points to geometry poses + geometry_msgs::msg::Pose start_pose; + start_pose.position.x = gps_start_x; + start_pose.position.y = gps_start_y; + gps_points.poses.push_back(start_pose); + + geometry_msgs::msg::Pose end_pose; + end_pose.position.x = gps_end_x; + end_pose.position.y = gps_end_y; + gps_points.poses.push_back(end_pose); + + gps_map_coord_visualization_pub_->publish(gps_points); +} + +std::pair NjordTaskBaseNode::lla2flat(double lat, double lon) const +{ + const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) + const double f = 1.0 / 298.257223563; // Flattening of the earth + const double psi_rad = 0.0; + + // Convert angles from degrees to radians + const double lat_rad = lat * M_PI / 180.0; + const double lon_rad = lon * M_PI / 180.0; + const double origin_lat_rad = this->get_parameter("map_origin_lat").as_double() * M_PI / 180.0; + const double origin_lon_rad = this->get_parameter("map_origin_lon").as_double() * M_PI / 180.0; + + // Calculate delta latitude and delta longitude in radians + const double dlat = lat_rad - origin_lat_rad; + const double dlon = lon_rad - origin_lon_rad; + + // Radius of curvature in the vertical prime (RN) + const double RN = R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Radius of curvature in the meridian (RM) + const double RM = RN * (1.0 - (2.0 * f - f * f)) / (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Changes in the north (dN) and east (dE) positions + const double dN = RM * dlat; + const double dE = RN * cos(origin_lat_rad) * dlon; + + // Transformation from North-East to flat x-y coordinates + const double px = cos(psi_rad) * dN - sin(psi_rad) * dE; + const double py = sin(psi_rad) * dN + cos(psi_rad) * dE; + + return {px, py}; +} + +void NjordTaskBaseNode::map_origin_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) +{ + std::unique_lock setup_lock(setup_mutex_); + this->set_parameter(rclcpp::Parameter("map_origin_lat", msg->latitude)); + this->set_parameter(rclcpp::Parameter("map_origin_lon", msg->longitude)); + this->set_parameter(rclcpp::Parameter("map_origin_set", true)); + RCLCPP_INFO(this->get_logger(), "Map origin set to: %f, %f", msg->latitude, msg->longitude); + + // Set the map to odom transform + setup_map_odom_tf_and_subs(); + // Set GPS frame coordinates + set_gps_frame_coords(); + + map_origin_sub_.reset(); + setup_lock.unlock(); +} + +void NjordTaskBaseNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + std::unique_lock lock(odom_mutex_); + odom_msg_ = msg; + new_odom_msg_ = true; + lock.unlock(); + odom_cond_var_.notify_one(); +} + +std::shared_ptr NjordTaskBaseNode::get_odom() +{ + std::unique_lock lock(odom_mutex_); + odom_cond_var_.wait(lock, [this] { return new_odom_msg_; }); + new_odom_msg_ = false; + lock.unlock(); + return odom_msg_; +} + +void NjordTaskBaseNode::landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg) +{ + std::unique_lock lock(landmark_mutex_); + // transform the landmarks to the odom frame + for (auto &landmark : msg->landmarks) { + tf2::doTransform(landmark.odom.pose.pose, landmark.odom.pose.pose, map_odom_tf_); + } + landmarks_msg_ = msg; + new_landmark_msg_ = true; + lock.unlock(); + landmark_cond_var_.notify_one(); +} + +std::shared_ptr NjordTaskBaseNode::get_landmarks_odom_frame() +{ + std::unique_lock lock(landmark_mutex_); + landmark_cond_var_.wait(lock, [this] { return new_landmark_msg_; }); + new_landmark_msg_ = false; + lock.unlock(); + return landmarks_msg_; +} + +Eigen::VectorXi NjordTaskBaseNode::assign_landmarks(const Eigen::Array &predicted_positions, + const Eigen::Array &measured_positions){ + int num_predicted = predicted_positions.cols(); + int num_measured = measured_positions.cols(); + int higher = std::max(num_predicted, num_measured); + + // Add dummy items to allow algorithm to converge + Eigen::MatrixXd reward_matrix(num_measured, higher); + reward_matrix.fill(-1.0); + + double epsilon = 1e-6; // Small positive number to prevent division by zero + for (Eigen::Index i = 0; i < num_measured; ++i) { + for (Eigen::Index j = 0; j < num_predicted; ++j) { + double dx = measured_positions(0, i) - predicted_positions(0, j); + double dy = measured_positions(1, i) - predicted_positions(1, j); + double distance = std::sqrt(dx * dx + dy * dy); + reward_matrix(i, j) = 1/(distance + epsilon); + } + } + + Eigen::VectorXi assignment = Eigen::VectorXi::Constant(higher, -1); + Eigen::VectorXd prices = Eigen::VectorXd::Zero(higher); + + std::vector unassigned; + for (int i = 0; i < num_measured; ++i) { + unassigned.push_back(i); + } + + epsilon = 1.0 / (num_measured + 1); + + while (!unassigned.empty()) { + int customer = unassigned.back(); + unassigned.pop_back(); + + double max_value = std::numeric_limits::lowest(); + double second_max_value = std::numeric_limits::lowest(); + int max_item = -1; + + for (int item = 0; item < higher; ++item) { + double value = reward_matrix.coeff(customer, item) - prices[item]; + if (value > max_value) { + second_max_value = max_value; + max_value = value; + max_item = item; + } else if (value > second_max_value) { + second_max_value = value; + } + } + + int current_owner = assignment[max_item]; + if (current_owner != -1) { + unassigned.push_back(current_owner); + } + + assignment[max_item] = customer; + prices[max_item] += max_value - second_max_value + epsilon; + } + + // Extract the final assignment for real predicted positions, ignoring dummy items + Eigen::VectorXi final_assignment = assignment.head(num_predicted); + return assignment; +} \ No newline at end of file From fa84fd011a6b8d12083d35d304c3a2256b83284e Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Mon, 29 Jul 2024 18:23:52 +0000 Subject: [PATCH 24/67] Committing clang-format changes --- .../collision_avoidance_task_ros.hpp | 3 +- .../src/collision_avoidance_task_node.cpp | 3 +- .../src/collision_avoidance_task_ros.cpp | 13 ++- .../include/docking_task/docking_task_ros.hpp | 2 +- .../docking_task/src/docking_task_ros.cpp | 53 ++++----- .../maneuvering_task/maneuvering_task_ros.hpp | 5 +- .../src/maneuvering_task_ros.cpp | 12 +- .../navigation_task/navigation_task_ros.hpp | 3 +- .../src/navigation_task_ros.cpp | 12 +- .../njord_task_base/njord_task_base_ros.hpp | 41 ++++--- .../src/njord_task_base_ros.cpp | 108 ++++++++++-------- 11 files changed, 137 insertions(+), 118 deletions(-) diff --git a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp index 64c70ab4..557b1532 100644 --- a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp +++ b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp @@ -19,9 +19,8 @@ class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { private: geometry_msgs::msg::Point previous_waypoint_odom_frame_; - }; - + } // namespace collision_avoidance_task #endif // COLLISION_AVOIDANCE_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp index 5c42dbea..9cf7ead4 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp @@ -4,7 +4,8 @@ int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = + std::make_shared(); rclcpp::spin(node); diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp index 2c3f5668..dd6742a3 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -2,7 +2,8 @@ namespace collision_avoidance_task { -CollisionAvoidanceTaskNode::CollisionAvoidanceTaskNode(const rclcpp::NodeOptions &options) +CollisionAvoidanceTaskNode::CollisionAvoidanceTaskNode( + const rclcpp::NodeOptions &options) : NjordTaskBaseNode("collision_avoidance_task_node", options) { std::thread(&CollisionAvoidanceTaskNode::main_task, this).detach(); @@ -10,16 +11,16 @@ CollisionAvoidanceTaskNode::CollisionAvoidanceTaskNode(const rclcpp::NodeOptions void CollisionAvoidanceTaskNode::main_task() { // Sleep for 3 seconds to allow system to initialize and tracks to be aquired - RCLCPP_INFO(this->get_logger(), - "Waiting 3 seconds for system to initialize before starting main task"); + RCLCPP_INFO( + this->get_logger(), + "Waiting 3 seconds for system to initialize before starting main task"); rclcpp::sleep_for(std::chrono::seconds(3)); - while(true) { + while (true) { rclcpp::sleep_for(std::chrono::milliseconds(100)); std::unique_lock setup_lock(setup_mutex_); if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), - "Map origin not set, sleeping for 100ms"); + RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); setup_lock.unlock(); continue; } diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp index e4977d4f..1967113a 100644 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -1,9 +1,9 @@ #ifndef DOCKING_TASK_ROS_HPP #define DOCKING_TASK_ROS_HPP -#include #include #include +#include #include #include #include diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 98b44944..1fb1dec8 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -5,7 +5,6 @@ namespace docking_task { DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) : NjordTaskBaseNode("dock_localization_node", options) { - declare_parameter("dock_width", 0.0); declare_parameter("dock_width_tolerance", 0.0); declare_parameter("dock_length", 0.0); @@ -23,16 +22,16 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) void DockingTaskNode::main_task() { // Sleep for 5 seconds to allow system to initialize and tracks to be aquired - RCLCPP_INFO(this->get_logger(), - "Waiting 3 seconds for system to initialize before starting main task"); + RCLCPP_INFO( + this->get_logger(), + "Waiting 3 seconds for system to initialize before starting main task"); rclcpp::sleep_for(std::chrono::seconds(3)); - while(true) { + while (true) { rclcpp::sleep_for(std::chrono::milliseconds(100)); std::unique_lock setup_lock(setup_mutex_); if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), - "Map origin not set, sleeping for 100ms"); + RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); setup_lock.unlock(); continue; } @@ -62,7 +61,7 @@ std::pair DockingTaskNode::initial_waypoint() { RCLCPP_INFO(this->get_logger(), "Initial waypoint running"); while (true) { - + // std::unique_lock odom_lock(odom_mutex_); // if (odom_msg_ == nullptr) { // RCLCPP_INFO(this->get_logger(), "Odometry message not received, exiting @@ -180,7 +179,7 @@ std::pair DockingTaskNode::initial_waypoint() { 2)); // Check if the landmarks are on opposite sides of the drone and within - // the desired distance range. + // the desired distance range. // Base link is NED frame, x is forward, y is right if ((landmark1.pose_base_link_frame.position.x > 0 && landmark2.pose_base_link_frame.position.x > 0) && @@ -195,7 +194,7 @@ std::pair DockingTaskNode::initial_waypoint() { landmark2.pose_odom_frame.position.y) / 2; std::pair id_pair_sample = {landmark1.id, - landmark2.id}; + landmark2.id}; // Check if this is the first valid pair of landmarks if (result == 0) { x_waypoint = x_waypoint_sample; @@ -239,7 +238,7 @@ std::pair DockingTaskNode::initial_waypoint() { waypoint_odom_frame.x = x_waypoint; waypoint_odom_frame.y = y_waypoint; waypoint_odom_frame.z = 0.0; - + auto request = std::make_shared(); request->waypoint.push_back(waypoint_odom_frame); auto result_future = waypoint_client_->async_send_request(request); @@ -291,17 +290,18 @@ DockingTaskNode::predict_buoy_formation(const LandmarkWithID &buoy1, geometry_msgs::msg::Point previous_waypoint_map_frame; geometry_msgs::msg::TransformStamped odom_map_tf; try { - // Compute the inverse transform from the stored map_odom_tf_ - tf2::Transform tf2_transform; - tf2::fromMsg(map_odom_tf_.transform, tf2_transform); - tf2::Transform tf2_inverse_transform = tf2_transform.inverse(); - odom_map_tf.transform = tf2::toMsg(tf2_inverse_transform); - - // Use the inverse transform - tf2::doTransform(previous_waypoint_odom_frame_, previous_waypoint_map_frame, odom_map_tf); -} catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); -} + // Compute the inverse transform from the stored map_odom_tf_ + tf2::Transform tf2_transform; + tf2::fromMsg(map_odom_tf_.transform, tf2_transform); + tf2::Transform tf2_inverse_transform = tf2_transform.inverse(); + odom_map_tf.transform = tf2::toMsg(tf2_inverse_transform); + + // Use the inverse transform + tf2::doTransform(previous_waypoint_odom_frame_, previous_waypoint_map_frame, + odom_map_tf); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + } Eigen::Vector2d direction_vector( previous_waypoint_map_frame.x - this->get_parameter("gps_start_x").as_double(), @@ -400,7 +400,8 @@ std::pair DockingTaskNode::navigate_formation( landmark_poses_odom_frame.poses.at(i).position.y; } - Eigen::VectorXi assignment = assign_landmarks(predicted_positions, measured_positions); + Eigen::VectorXi assignment = + assign_landmarks(predicted_positions, measured_positions); if (result == 0) { for (Eigen::Index i = 0; i < assignment.size(); i++) { @@ -412,11 +413,11 @@ std::pair DockingTaskNode::navigate_formation( bool valied = true; // Check that the assigned landmarks matches the previous assignment by id if (first_half) { - if (assignment(2) == -1 || assignment(3) == -1 || assignment(4) == -1 || - assignment(5) == -1) { + if (assignment(2) == -1 || assignment(3) == -1 || assignment(4) == -1 || + assignment(5) == -1) { valied = false; - } - + } + // Check index 2,3,4,5 if (landmark_ids.at(assignment(2)) != prev_assignment.at(2) || landmark_ids.at(assignment(3)) != prev_assignment.at(3) || diff --git a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp index c1992fb3..e4083b08 100644 --- a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp +++ b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp @@ -18,10 +18,9 @@ class ManeuveringTaskNode : public NjordTaskBaseNode { void main_task(); private: - geometry_msgs::msg::Point previous_waypoint_odom_frame_; - + geometry_msgs::msg::Point previous_waypoint_odom_frame_; }; - + } // namespace maneuvering_task #endif // MANEUVERING_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index 8b6b0ddb..0c5c0c49 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -10,16 +10,16 @@ ManeuveringTaskNode::ManeuveringTaskNode(const rclcpp::NodeOptions &options) void ManeuveringTaskNode::main_task() { // Sleep for 5 seconds to allow system to initialize and tracks to be aquired - RCLCPP_INFO(this->get_logger(), - "Waiting 3 seconds for system to initialize before starting main task"); + RCLCPP_INFO( + this->get_logger(), + "Waiting 3 seconds for system to initialize before starting main task"); rclcpp::sleep_for(std::chrono::seconds(3)); - while(true) { + while (true) { rclcpp::sleep_for(std::chrono::milliseconds(100)); std::unique_lock setup_lock(setup_mutex_); if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), - "Map origin not set, sleeping for 100ms"); + RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); setup_lock.unlock(); continue; } @@ -33,6 +33,4 @@ void ManeuveringTaskNode::main_task() { } } - - } // namespace maneuvering_task \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp index 408a650a..8cbe29f0 100644 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -19,9 +19,8 @@ class NavigationTaskNode : public NjordTaskBaseNode { private: geometry_msgs::msg::Point previous_waypoint_odom_frame_; - }; - + } // namespace navigation_task #endif // NAVIGATION_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 4f365b8e..17c04458 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -10,16 +10,16 @@ NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options) void NavigationTaskNode::main_task() { // Sleep for 3 seconds to allow system to initialize and tracks to be aquired - RCLCPP_INFO(this->get_logger(), - "Waiting 3 seconds for system to initialize before starting main task"); + RCLCPP_INFO( + this->get_logger(), + "Waiting 3 seconds for system to initialize before starting main task"); rclcpp::sleep_for(std::chrono::seconds(3)); - while(true) { + while (true) { rclcpp::sleep_for(std::chrono::milliseconds(100)); std::unique_lock setup_lock(setup_mutex_); if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), - "Map origin not set, sleeping for 100ms"); + RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); setup_lock.unlock(); continue; } @@ -33,6 +33,4 @@ void NavigationTaskNode::main_task() { } } - - } // namespace navigation_task \ No newline at end of file diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index bfc573e5..93966be6 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -1,26 +1,26 @@ #ifndef NJORD_TASK_BASE_ROS_HPP #define NJORD_TASK_BASE_ROS_HPP -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/pose_array.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" #include "vortex_msgs/msg/landmark_array.hpp" #include "vortex_msgs/srv/waypoint.hpp" -#include +#include #include +#include #include #include -#include -class NjordTaskBaseNode : public rclcpp::Node -{ +class NjordTaskBaseNode : public rclcpp::Node { public: - NjordTaskBaseNode(const std::string &node_name, const rclcpp::NodeOptions &options); + NjordTaskBaseNode(const std::string &node_name, + const rclcpp::NodeOptions &options); protected: void setup_map_odom_tf_and_subs(); @@ -34,23 +34,30 @@ class NjordTaskBaseNode : public rclcpp::Node std::shared_ptr get_landmarks_odom_frame(); /** - * @brief Assign landmarks to buoys based on the predicted and measured positions using the auction algorithm. + * @brief Assign landmarks to buoys based on the predicted and measured + * positions using the auction algorithm. * * @param predicted_positions The predicted positions of the buoys * @param measured_positions The measured positions of landmarks * * @return A vector of size equal to number of predicted positions(buoys), - * where each element is the index of the measured position(landmark) assigned to the corresponding predicted position(buoy). - * If an element is -1, it means that the corresponding predicted position(buoy) is not assigned to any measured position(landmark). + * where each element is the index of the measured position(landmark) assigned + * to the corresponding predicted position(buoy). If an element is -1, it + * means that the corresponding predicted position(buoy) is not assigned to + * any measured position(landmark). */ - Eigen::VectorXi assign_landmarks(const Eigen::Array &predicted_positions, - const Eigen::Array &measured_positions); + Eigen::VectorXi assign_landmarks( + const Eigen::Array &predicted_positions, + const Eigen::Array &measured_positions); - rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; - rclcpp::Publisher::SharedPtr waypoint_visualization_pub_; + rclcpp::Publisher::SharedPtr + gps_map_coord_visualization_pub_; + rclcpp::Publisher::SharedPtr + waypoint_visualization_pub_; rclcpp::Subscription::SharedPtr map_origin_sub_; rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr landmarks_sub_; + rclcpp::Subscription::SharedPtr + landmarks_sub_; rclcpp::Client::SharedPtr waypoint_client_; std::shared_ptr tf_buffer_; diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index 4b2e5cf5..3ce713f9 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -1,8 +1,8 @@ #include -NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, const rclcpp::NodeOptions &options) - : Node(node_name, options) -{ +NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, + const rclcpp::NodeOptions &options) + : Node(node_name, options) { declare_parameter("map_origin_lat", 0.0); declare_parameter("map_origin_lon", 0.0); declare_parameter("map_origin_set", false); @@ -19,8 +19,8 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, const rclcpp: declare_parameter("map_origin_topic", "/map/origin"); declare_parameter("odom_topic", "/seapath/odom/ned"); declare_parameter("landmark_pose_topic", "/landmark/pose"); - - // Sensor data QoS profile + + // Sensor data QoS profile rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), @@ -54,46 +54,55 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, const rclcpp: this->create_client("/waypoint"); } -void NjordTaskBaseNode::setup_map_odom_tf_and_subs() -{ +void NjordTaskBaseNode::setup_map_odom_tf_and_subs() { // Get the transform between the map and odom frames to avoid the overhead // from continuously looking up the static transform between map and odom bool tf_set = false; while (!tf_set) { - try { - auto transform = tf_buffer_->lookupTransform("odom", "map", tf2::TimePointZero, tf2::durationFromSec(1.0)); - map_odom_tf_ = transform; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - std::this_thread::sleep_for(std::chrono::seconds(1)); - continue; - } - tf_set = true; + try { + auto transform = tf_buffer_->lookupTransform( + "odom", "map", tf2::TimePointZero, tf2::durationFromSec(1.0)); + map_odom_tf_ = transform; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + std::this_thread::sleep_for(std::chrono::seconds(1)); + continue; + } + tf_set = true; } // Sensor data QoS profile rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), qos_profile_sensor_data); + auto qos_sensor_data = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), + qos_profile_sensor_data); odom_sub_ = this->create_subscription( get_parameter("odom_topic").as_string(), qos_sensor_data, - std::bind(&NjordTaskBaseNode::odom_callback, this, std::placeholders::_1)); + std::bind(&NjordTaskBaseNode::odom_callback, this, + std::placeholders::_1)); landmarks_sub_ = this->create_subscription( get_parameter("landmark_pose_topic").as_string(), qos_sensor_data, - std::bind(&NjordTaskBaseNode::landmark_callback, this, std::placeholders::_1)); + std::bind(&NjordTaskBaseNode::landmark_callback, this, + std::placeholders::_1)); } -void NjordTaskBaseNode::set_gps_frame_coords() -{ - auto [gps_start_x, gps_start_y] = lla2flat(this->get_parameter("gps_start_lat").as_double(), this->get_parameter("gps_start_lon").as_double()); - auto [gps_end_x, gps_end_y] = lla2flat(this->get_parameter("gps_end_lat").as_double(), this->get_parameter("gps_end_lon").as_double()); +void NjordTaskBaseNode::set_gps_frame_coords() { + auto [gps_start_x, gps_start_y] = + lla2flat(this->get_parameter("gps_start_lat").as_double(), + this->get_parameter("gps_start_lon").as_double()); + auto [gps_end_x, gps_end_y] = + lla2flat(this->get_parameter("gps_end_lat").as_double(), + this->get_parameter("gps_end_lon").as_double()); this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_x)); this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_y)); this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_x)); this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_y)); this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); - RCLCPP_INFO(this->get_logger(), "GPS map frame coordinates set to: %f, %f, %f, %f", gps_start_x, gps_start_y, gps_end_x, gps_end_y); + RCLCPP_INFO(this->get_logger(), + "GPS map frame coordinates set to: %f, %f, %f, %f", gps_start_x, + gps_start_y, gps_end_x, gps_end_y); geometry_msgs::msg::PoseArray gps_points; gps_points.header.frame_id = "map"; @@ -112,8 +121,8 @@ void NjordTaskBaseNode::set_gps_frame_coords() gps_map_coord_visualization_pub_->publish(gps_points); } -std::pair NjordTaskBaseNode::lla2flat(double lat, double lon) const -{ +std::pair NjordTaskBaseNode::lla2flat(double lat, + double lon) const { const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) const double f = 1.0 / 298.257223563; // Flattening of the earth const double psi_rad = 0.0; @@ -121,18 +130,22 @@ std::pair NjordTaskBaseNode::lla2flat(double lat, double lon) co // Convert angles from degrees to radians const double lat_rad = lat * M_PI / 180.0; const double lon_rad = lon * M_PI / 180.0; - const double origin_lat_rad = this->get_parameter("map_origin_lat").as_double() * M_PI / 180.0; - const double origin_lon_rad = this->get_parameter("map_origin_lon").as_double() * M_PI / 180.0; + const double origin_lat_rad = + this->get_parameter("map_origin_lat").as_double() * M_PI / 180.0; + const double origin_lon_rad = + this->get_parameter("map_origin_lon").as_double() * M_PI / 180.0; // Calculate delta latitude and delta longitude in radians const double dlat = lat_rad - origin_lat_rad; const double dlon = lon_rad - origin_lon_rad; // Radius of curvature in the vertical prime (RN) - const double RN = R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + const double RN = + R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); // Radius of curvature in the meridian (RM) - const double RM = RN * (1.0 - (2.0 * f - f * f)) / (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + const double RM = RN * (1.0 - (2.0 * f - f * f)) / + (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); // Changes in the north (dN) and east (dE) positions const double dN = RM * dlat; @@ -145,13 +158,14 @@ std::pair NjordTaskBaseNode::lla2flat(double lat, double lon) co return {px, py}; } -void NjordTaskBaseNode::map_origin_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) -{ +void NjordTaskBaseNode::map_origin_callback( + const sensor_msgs::msg::NavSatFix::SharedPtr msg) { std::unique_lock setup_lock(setup_mutex_); this->set_parameter(rclcpp::Parameter("map_origin_lat", msg->latitude)); this->set_parameter(rclcpp::Parameter("map_origin_lon", msg->longitude)); this->set_parameter(rclcpp::Parameter("map_origin_set", true)); - RCLCPP_INFO(this->get_logger(), "Map origin set to: %f, %f", msg->latitude, msg->longitude); + RCLCPP_INFO(this->get_logger(), "Map origin set to: %f, %f", msg->latitude, + msg->longitude); // Set the map to odom transform setup_map_odom_tf_and_subs(); @@ -162,8 +176,8 @@ void NjordTaskBaseNode::map_origin_callback(const sensor_msgs::msg::NavSatFix::S setup_lock.unlock(); } -void NjordTaskBaseNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) -{ +void NjordTaskBaseNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { std::unique_lock lock(odom_mutex_); odom_msg_ = msg; new_odom_msg_ = true; @@ -171,8 +185,7 @@ void NjordTaskBaseNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr m odom_cond_var_.notify_one(); } -std::shared_ptr NjordTaskBaseNode::get_odom() -{ +std::shared_ptr NjordTaskBaseNode::get_odom() { std::unique_lock lock(odom_mutex_); odom_cond_var_.wait(lock, [this] { return new_odom_msg_; }); new_odom_msg_ = false; @@ -180,12 +193,13 @@ std::shared_ptr NjordTaskBaseNode::get_odom() return odom_msg_; } -void NjordTaskBaseNode::landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg) -{ +void NjordTaskBaseNode::landmark_callback( + const vortex_msgs::msg::LandmarkArray::SharedPtr msg) { std::unique_lock lock(landmark_mutex_); // transform the landmarks to the odom frame for (auto &landmark : msg->landmarks) { - tf2::doTransform(landmark.odom.pose.pose, landmark.odom.pose.pose, map_odom_tf_); + tf2::doTransform(landmark.odom.pose.pose, landmark.odom.pose.pose, + map_odom_tf_); } landmarks_msg_ = msg; new_landmark_msg_ = true; @@ -193,8 +207,8 @@ void NjordTaskBaseNode::landmark_callback(const vortex_msgs::msg::LandmarkArray: landmark_cond_var_.notify_one(); } -std::shared_ptr NjordTaskBaseNode::get_landmarks_odom_frame() -{ +std::shared_ptr +NjordTaskBaseNode::get_landmarks_odom_frame() { std::unique_lock lock(landmark_mutex_); landmark_cond_var_.wait(lock, [this] { return new_landmark_msg_; }); new_landmark_msg_ = false; @@ -202,8 +216,9 @@ std::shared_ptr NjordTaskBaseNode::get_landmark return landmarks_msg_; } -Eigen::VectorXi NjordTaskBaseNode::assign_landmarks(const Eigen::Array &predicted_positions, - const Eigen::Array &measured_positions){ +Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( + const Eigen::Array &predicted_positions, + const Eigen::Array &measured_positions) { int num_predicted = predicted_positions.cols(); int num_measured = measured_positions.cols(); int higher = std::max(num_predicted, num_measured); @@ -218,7 +233,7 @@ Eigen::VectorXi NjordTaskBaseNode::assign_landmarks(const Eigen::Array Date: Mon, 29 Jul 2024 22:23:51 +0200 Subject: [PATCH 25/67] auction algorithm fix --- .../docking_task/src/docking_task_ros.cpp | 5 +++ .../njord_task_base/njord_task_base_ros.hpp | 2 +- .../src/njord_task_base_ros.cpp | 41 ++++++++++--------- 3 files changed, 27 insertions(+), 21 deletions(-) diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 1fb1dec8..3c0fe6ec 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -399,6 +399,11 @@ std::pair DockingTaskNode::navigate_formation( measured_positions(1, i) = landmark_poses_odom_frame.poses.at(i).position.y; } + + if(predicted_positions.cols() > measured_positions.cols()){ + result = 0; + continue; + } Eigen::VectorXi assignment = assign_landmarks(predicted_positions, measured_positions); diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index 93966be6..4600cba6 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -34,7 +34,7 @@ class NjordTaskBaseNode : public rclcpp::Node { std::shared_ptr get_landmarks_odom_frame(); /** - * @brief Assign landmarks to buoys based on the predicted and measured + * @brief Assign buoys to landmarks based on the predicted and measured * positions using the auction algorithm. * * @param predicted_positions The predicted positions of the buoys diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index 3ce713f9..c868d6fd 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -216,16 +216,15 @@ NjordTaskBaseNode::get_landmarks_odom_frame() { return landmarks_msg_; } -Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( - const Eigen::Array &predicted_positions, - const Eigen::Array &measured_positions) { +Eigen::VectorXi NjordTaskBaseNode::assign_landmarks(const Eigen::Array &predicted_positions, + const Eigen::Array &measured_positions){ int num_predicted = predicted_positions.cols(); int num_measured = measured_positions.cols(); - int higher = std::max(num_predicted, num_measured); + Eigen::MatrixXd reward_matrix(num_measured, num_predicted); - // Add dummy items to allow algorithm to converge - Eigen::MatrixXd reward_matrix(num_measured, higher); - reward_matrix.fill(-1.0); + if(num_predicted > num_measured){ + RCLCPP_ERROR(this->get_logger(), "Number of predicted positions is greater than number of measured positions in auction algorithm"); + } double epsilon = 1e-6; // Small positive number to prevent division by zero for (Eigen::Index i = 0; i < num_measured; ++i) { @@ -233,19 +232,19 @@ Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( double dx = measured_positions(0, i) - predicted_positions(0, j); double dy = measured_positions(1, i) - predicted_positions(1, j); double distance = std::sqrt(dx * dx + dy * dy); - reward_matrix(i, j) = 1 / (distance + epsilon); + reward_matrix(i, j) = 1/(distance + epsilon); } } - Eigen::VectorXi assignment = Eigen::VectorXi::Constant(higher, -1); - Eigen::VectorXd prices = Eigen::VectorXd::Zero(higher); + Eigen::VectorXi assignment = Eigen::VectorXi::Constant(num_predicted, -1); + Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_measured); std::vector unassigned; - for (int i = 0; i < num_measured; ++i) { + for (int i = 0; i < num_predicted; ++i) { unassigned.push_back(i); } - epsilon = 1.0 / (num_measured + 1); + epsilon = 1.0 / (num_measured + num_predicted + 1); while (!unassigned.empty()) { int customer = unassigned.back(); @@ -255,8 +254,8 @@ Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( double second_max_value = std::numeric_limits::lowest(); int max_item = -1; - for (int item = 0; item < higher; ++item) { - double value = reward_matrix.coeff(customer, item) - prices[item]; + for (int item = 0; item < num_measured; ++item) { + double value = reward_matrix.coeff(item, customer) - prices[item]; if (value > max_value) { second_max_value = max_value; max_value = value; @@ -266,17 +265,19 @@ Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( } } - int current_owner = assignment[max_item]; + int current_owner = -1; + for (int i = 0; i < num_predicted; ++i) { + if (assignment[i] == max_item) { + current_owner = i; + break; + } + } if (current_owner != -1) { unassigned.push_back(current_owner); } - assignment[max_item] = customer; + assignment[customer] = max_item; prices[max_item] += max_value - second_max_value + epsilon; } - - // Extract the final assignment for real predicted positions, ignoring dummy - // items - Eigen::VectorXi final_assignment = assignment.head(num_predicted); return assignment; } \ No newline at end of file From f5248f49ad3ee83c4043d58ea61d67987c6085d0 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Mon, 29 Jul 2024 20:24:20 +0000 Subject: [PATCH 26/67] Committing clang-format changes --- .../docking_task/src/docking_task_ros.cpp | 4 ++-- .../njord_task_base/src/njord_task_base_ros.cpp | 13 ++++++++----- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 3c0fe6ec..9a543216 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -399,8 +399,8 @@ std::pair DockingTaskNode::navigate_formation( measured_positions(1, i) = landmark_poses_odom_frame.poses.at(i).position.y; } - - if(predicted_positions.cols() > measured_positions.cols()){ + + if (predicted_positions.cols() > measured_positions.cols()) { result = 0; continue; } diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index c868d6fd..597f25e8 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -216,14 +216,17 @@ NjordTaskBaseNode::get_landmarks_odom_frame() { return landmarks_msg_; } -Eigen::VectorXi NjordTaskBaseNode::assign_landmarks(const Eigen::Array &predicted_positions, - const Eigen::Array &measured_positions){ +Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( + const Eigen::Array &predicted_positions, + const Eigen::Array &measured_positions) { int num_predicted = predicted_positions.cols(); int num_measured = measured_positions.cols(); Eigen::MatrixXd reward_matrix(num_measured, num_predicted); - if(num_predicted > num_measured){ - RCLCPP_ERROR(this->get_logger(), "Number of predicted positions is greater than number of measured positions in auction algorithm"); + if (num_predicted > num_measured) { + RCLCPP_ERROR(this->get_logger(), + "Number of predicted positions is greater than number of " + "measured positions in auction algorithm"); } double epsilon = 1e-6; // Small positive number to prevent division by zero @@ -232,7 +235,7 @@ Eigen::VectorXi NjordTaskBaseNode::assign_landmarks(const Eigen::Array Date: Tue, 30 Jul 2024 13:42:24 +0200 Subject: [PATCH 27/67] utility functions for task base --- .../src/collision_avoidance_task_ros.cpp | 2 +- .../docking_task/src/docking_task_ros.cpp | 4 +- .../src/maneuvering_task_ros.cpp | 2 +- .../navigation_task/navigation_task_ros.hpp | 21 +- .../navigation_task/navigation_task.pdf | Bin 0 -> 99398 bytes .../params/navigation_task_params.yaml | 7 +- .../src/navigation_task_ros.cpp | 87 +++++++- .../njord_task_base/njord_task_base_ros.hpp | 71 ++++++- .../src/njord_task_base_ros.cpp | 198 ++++++++++++++++-- 9 files changed, 356 insertions(+), 36 deletions(-) create mode 100644 mission/njord_tasks/navigation_task/navigation_task.pdf diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp index dd6742a3..b4c349ec 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -26,7 +26,7 @@ void CollisionAvoidanceTaskNode::main_task() { } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { setup_map_odom_tf_and_subs(); - set_gps_frame_coords(); + set_gps_odom_points(); setup_lock.unlock(); break; } diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 9a543216..66776708 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -36,7 +36,7 @@ void DockingTaskNode::main_task() { continue; } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - set_gps_frame_coords(); + set_gps_odom_points(); setup_map_odom_tf_and_subs(); setup_lock.unlock(); break; @@ -406,7 +406,7 @@ std::pair DockingTaskNode::navigate_formation( } Eigen::VectorXi assignment = - assign_landmarks(predicted_positions, measured_positions); + auction_algorithm(predicted_positions, measured_positions); if (result == 0) { for (Eigen::Index i = 0; i < assignment.size(); i++) { diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index 0c5c0c49..40965cf0 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -24,7 +24,7 @@ void ManeuveringTaskNode::main_task() { continue; } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - set_gps_frame_coords(); + set_gps_odom_points(); setup_map_odom_tf_and_subs(); setup_lock.unlock(); break; diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp index 8cbe29f0..a8c16080 100644 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -17,8 +17,27 @@ class NavigationTaskNode : public NjordTaskBaseNode { */ void main_task(); + + /** + * @brief Predict the positions of the first two buoys + * + * @return An Eigen::Array representing the predicted positions + * of the first two buoys + */ + Eigen::Array predict_first_buoy_pair(); + + /** + * @brief Predict the positions of the first two buoys pairs + * + * @return An Eigen::Array representing the predicted positions + * of the first two buoy pairs + */ + Eigen::Array predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); + + private: - geometry_msgs::msg::Point previous_waypoint_odom_frame_; + + }; } // namespace navigation_task diff --git a/mission/njord_tasks/navigation_task/navigation_task.pdf b/mission/njord_tasks/navigation_task/navigation_task.pdf new file mode 100644 index 0000000000000000000000000000000000000000..9c0357a1226a91a5892fe7cd459b1474ace73d85 GIT binary patch literal 99398 zcmd?R^;=b6*glA@prRlkt&}uMH>e;D(hbrL(jB6rfOH8GA|>6ON;lFC(p`tndEoeY z-}%m8FxSlaK`&tMwbx$j$@{+7A%89KitZUb8!GwKbY~AL)1#-4EVPYLxw%mpG8}mVq8BgQAhHjp3tbER5`^48nRw28K3|o-wj8qcXfO zvax=xXZ6y;?5%~lp1I8JYs$c1=lcaiYPJVo~9$z}0f(M^*Y3 zmdGN(${M}ggL_?c_Gcr@Wm#o*#amUFEqS3gm)`w7Qj0TTu%By-oZz@^pl=?#Uww8| z{dGd)y1>LWl=BaNmEB~yL6rN4SZBPT4bG+CXO}L1|7gNn*n#k#E8D{M6%3+_UwItU zKRTp;jE?*P`zFtx+y>so4=91oHgm}PkU#%_>t912K^ZIE-^)MInmrXSBVT{?CcC4_ zq8FN$W#_f*tH}4|z>eH6a?G}5y*oQrr0yf`Lh8VS{7cijoRWE-C#8!^xIXp4*p-b( zM3s*nM38^nSNioVz)8}ohmEmMnoZrc)-nq}j15)eA@a|83^OYsw`hv-BC)z1C&Xzp z4GszK`npm$nEe|&`7#>F)rD>nn&OWBuP zlAO%yFcyyj8{gK@5?5mu-*QBbvE|$JVL*(HOx~c4yr`1{_MW-cig1wanb%ysc4+c%mSOd`Oc}1^UUDNjIvjn2AIaX-{8vUtS6# z+yYCa*>m7Javr8zoHm6MHd6SFTU6;k2aJcTE#)dEKmIehqhtRGJVu7DB7tiXO$i2u zYUzRjf+ga?usmIH_XMgq2zQiz_ln*b`!+rIXZ) zhGoc9%TI{BoWH@c%+Wtsk+Ve~q-46Rq?H&#!akQf*@$$9*MXUX2eM&agmoC+9q!+R z$%310??}+nX0D3Kn{_#F`H-7*-iRqBCffUPW`mCDYhPO;jy7~VY8vOBtK!BLOfpZ zz5f{b7W~bKnz%yMop512nMm^>?Ie6#JXRO!pUb<%-KNRHPUXgXo%N!MVZw<^rA}mY zN8&aUh;NfSS1#^;RR8<4^~<^IZD<_0PP@4zo0%k397`{(mhObIm93w&H{`ms+{|uJ z_sSmL@tq9&cT*vF4eUkenFQu~;!S@?xisS|xn)lR@>mubwfnL(A9!2zSd`8U@?nk@ zv#s)$Nq(!U63Rw%Gepi<_`c2I^TxjjGMPwWS+68> zC}f!F8dE7sY|55MoEdrIAfqpepT~kT&yzCTsIMk3Vo7$BBC@LQS)PW?jpq!eAYMw@ z;+Fn&$;1w4t-td{Tz2YFkW$8|!`4!aCoyurP>!X>aoJlNd%HRHuy&MBg?>-Peu{O@ zd&-@>{F5AOxgf8ILB~`@jIZ~ZA{HrpS8P{8{|F!vRjoeW)Rh5Afmh$riL4z4@ z|J;9#$lRE1x6E7LD$iC}Ah}qHu&5vnQyG4cJ(i}}d5b>(ihtJNX2Rr+|I7w_ z=PN#e`VT902`ADZ;no3T=>+BCygq!@p|HSZ+|L3(dN=(AD2nN%b0va*lSe)U1;s@aEKqqJnW7(Q`ii?uThAPh1}=n^yZMX1 zxs1#b9xhbdqp|~;6Sla8O<>H8T8N6f8dw!%$YER_2u_WFN%gqwMr01jK?+DnkZtO> zoSFK(v5)g#_};>StGcYEu1JmT@j0?>Y`gB{tw2hY=NE`}pLLwHO1Y2t(vG&AXuLCW z=k3sO=H%>!XM4B^1nu`PPimedJIWb#jaQlOKuo@?dXK_qn_vu}| zG?B!--p2vLWX5cU$?Li99~dw$W;jDExg1Q;()YkyfRpow{r&gCy;}PfH9hE8vn70~ zg2xZzN`~wJ5zvPVGqpJi zXj?9y%2uGC(K*DZkZx?BKuQnKP@AY3WZp?FVo*1mTw(d>8{Tzn(cH45e}e$0Lgiu_ zfdOC|gzH7*wShuCU)pFJ%gk?cB3^IEAj;44zCbs8m zj$ie?-73~P#m^+Gge-1qFkOt*q&asrHiue;(f4PHT-bGA5*<=Z4Kxko(^>18;mOid za+-l2w!>ol009zPT1}JRKAA*E^gDgi4> zpeJMSVK*?-eC1faHGa5yz09HkS;{9QIEoYM3tT=`7+ z(jR*v+PgH0`Kj-6m0OxamvKW=`;{Dd6<1Hm^;&n|>*M|`KYaKw(oY2sivYoXGX6p* zE^~ATvE`?E%S{bAdMB#CBe!MW?u-8V{LfA;1Ln^dW*#II^NLQUIum~C=wo8;kA9ge zplu{up%>?E{r<?wLiRD&&TP+!DA>`8sJY5)@tuYi%%28t)F6&L zMqE)`JEm&aPlymakLn%P^Fe5p=qCp6^1;gW4++i3tuy>|9D{__z%TcIzy9Cn1l`J3 zFVaX;Dz?Q|cgpYy!>ghU{i8}u{^)SLM!5>hxY%&qdX)Ao;jsbJ!$A2=OM_H3MMtl8 z)FH}ZZ}zjL6ZF}>mKeS}(Ur+AnR8`_UQuzLauf`T2~;%Sk?u?TA=;I(+4Yn1;}O~) zv7$R89k)1tEo25vRm~T3ylYfkJLEV~hjP<=#62gcXrQe|tz0iVjBCOS+hJ*3ytC$cVp4@$JAf~(3W<{j#n$Ukjkv)D?KAB|v35>h! zixQ=*{oJ2EHiFs2o17>7eSQW5QZMJOg{BY5<#(4QFXBfkqz&kAjG|1C4qVG7KrXyx zkhjI?-tpY)_#$Dl7TTp;J4!Aax7S{1Fa9P_1_z%=jZ>-o2v!=4Oi@~-HXk{|aacNq z-~$ywK|zre$Gl8rmVB-ZB#l^NCzXx7i#6bPyHB+Df`=|g5L;Rbs_40o0zwp3_bY~7 z=(ta3nd45V=1Xm>o6$%+?r0Jyq=~vlu9e@)71Yknb^PmNt9mDPdEI&2Ym()naPT-L zpE^!IT)iQj|FBZ;P4ABZ_U!lWw=|5aupJKFc9PX?dN~4=NEX@J6O01>K&?nMubiB2 zF#Wo!-NOjH``&+{ftV^d7*>C%%mFu7DY$+i^B3Z{b}AN4!;BXTY9**Z#3wqP=myxB z3k#quq8f9!EkR4&Z^Y8CM=EB5+*~!hqNjCG2l)O`oi%1reTbVb>_p>`dLTWyc%zb{ zMBhO>iP7<~Y9nl0x%r941}E}1yPrK}0a;aA$qB{Vcku0ru-|SKP1?FS`XtW4&i$U& zoUsZ1q1$;yk)U-z(pprIBuK6EyLe%PEVtdKXuV{Ui>=yd&VXvb?`0`%+4dl3IPq*E zElUR8-P<3aQoyt!F>4~l#*f4@=5q*dtm3=MCw?)| zch~Ekcb*pKfu~6qb@$_|=%RkoT%zB^cMXqn37Y2@|8yD$ev{@udx>GS%^L7 z?HE!FUorVKKE4(4$<(83#{2je!`%~58byqUj6#vkg)%Mk6m4xYyzWyAY?DGQ|C54xzwJUnZlgtk@L zgJ`82Znc0P$-2%WxxvX-N-n+GGEHr&_=38pePxg(({zWz#DDDgXADhn@D-^Eoa2W` zFL{RnW|utiH#lrrKiukb-PDVsyVGMuH#L@0Fy1bW!$W6HSlF0zUlXbKKMu(2GyXH3 zaEQ2g9h95o>~qrV%`~V(oVwUH!g^)1CdC_EZeklU))rcIL)fkqN;nKOA@1Mtr9{8Y zRFTr|mi||W>o!6FV_9Km@A;!j5>yqWJRmK$wd3*|a>b`Mv<$iI!g!;ivQ5eWmCmjb z<-I>{jTThFUv-c@d<(*FETJ)}%VSrj+62QN=26KgJfLW;0!PT`=Zd>Prw7 z-Q{yM81xK;=5z@^(nsJ2`rg!iU%z&=0TZ@2TTAN1h0&ZBE5JYgw`lbU@@13l4pS?C zqE+R1hywI!*cH9!{d?{s?&R98QUyUVfDl$}rlpC>Umiq36F@(*ANJo&W|Zm<6lE)S z!?T8;`jRE)WKOSmOlw&>I%!Nb)vQ^xR#SZquo#mvg|$NL6c0cuJmhTbb=+$|867L?-c zigU#xdi;>Z)Hs;CEq7Hv)UP-5TcWpGZ}q$H0A(% z-D|tVmFJN>|Tjxa|c^G4gN?KM?W*#1KR+xEYsjkbUR=^DHe)pTS^gr?^4## z+{}>+QI!L4k)~Fi=`S2=LW|+$jcz!Gb>8%dg!4#$ZtB?%ojzTPh5L2*EV-=e{?s8A0;F z3c9Z|{r&;6m$~UG+Q{l57uGWNOW>r^y!nXB~}MyyOkk!r<& zD`?y}(`7}rE+~TCssXMMtpnykg3;PYLI(XX^A4J zWkpmma;fmiLfcD0BBf)(iv@ecaMRcM8O`L+EUjcFB42?Y&~J$&#`l`zRD!nq&t@`X zL)6f4Y(raY^S@G2+A^^+jM`1w6q`jrd3eih7rAR&cLC_*-yVZp=gSzNtXp;n-Js7H~@Io?-OYHEKIbu!s=zuif>m!qLEN4?b}=M1K;wDLwd1C1qI_~=27f~Pedj} z0>)OQwoIShn4-gwFXI3SPM4lpb$ZhaiYbIP@8pGSP<~62*xY@o&ygts00P-QE)rU3 z%7=Xt-x<|0)nL!hVj;;bu_^Q_C9cG>nfq%qaRsW)97^qo_76f?21>c+%gsTVh-p<-fg zljnnx-TG}3AoJFTzVOe5#8ucQ;6U0h(>rOMSpM9*E_Q_@y`uEvdhvwVFWSBj}KGcd>%T zjHcg+JWP+%srAR_F=$wuJ}6S5Z*srY``u!OwN%ieZMoYkao$ipNFC2up6TPNo#u7A z!+_j6-JU?lcK|{Q{P-edA(F`WX{&IwNL|Jp7kh8i5m5{s4l9l6w;{vqvsc_Cky%yh z&7y=}Z96)Bj|o$Hsd@2&y#ac7i!^-uYGyfS4yS z(rx+QG1_RnzJJ6xSnqYBF*p z?!JiqT||B;Zn?){#dDs*|3OhG5p)xM7JQS_Wt$6&=fJdNS94&#)B_QVYkbJKhC=&= zec*e~Jsq_4l~3FNRoscinS>4~rvw0!f3O_1Ax9E>l+wBWRq@`!WF3;fNe7q=`z*G8 zG!53s?vY|HHvF^C3XbPbmJ9LUnCm?WYRQ`jNqLoTF4j*01=)iNH{?<}5TBN>%k4pw zS;URf<@@`GSB7HfdVNL)5o=GeX6l_f3=um<0w|53BuMk)Trl8pmKtL{ z3AZ>@*|>sSx-KW^4xt!>D$8V4uO!~vk(FzSvN)qV=j1zw8{)-}<-+B-`fy07hvFKA z7v@2A@hlH)G+*bxWvfS3vSVWMnQ;*Cs6$k77)Ht_W)9+0YNz99_MS3qhi2*=&=Se~ zkQt*QDp%G)n%1QCYqF{xF3WQ$k7Dc$+BS^63&&rIuhFq=u#t|J+Ap1Q;2ho5C{NZgg|saR2lTw zoD>2P0hWbyog^8CUQr0t%o0`;@uM;uA;>WTCqCBmXcivLLo)S=$PR*+Ckazl!ZG<{ z65Uhw;WI*6A}67uc)eHN$*X}gQh-M$vf_E;FBjwXL~h(C%?!^F%PF9u$M9wv$=CTf9HO``M^=S(T!4Iq znI|I6z>pc-AYxF>PbEW`vl>OoC9D3>j)8@F(aF>PD`Yo%L0i{2(#Vr6WWao^ak)$W zuhc(ywkn9gWzMbq9TR|x226jYS%ZW}jRq?qOIaUR{4yX4P45#wZ%C-bX?<+)iXKr@ zDK8+9{uRjAc8l}x>-caWGkjTcWY}^A(nj5on5s0673Oa;hR^J(byYTV(!ott;(g#& z|C9L~BT1-+mb?0}D=EHW6=8`b4v9L{VIWBCAW1;x{bK{bOci=D68<)YTV{=1JXeVrFO>+iuQH zF{j|ic_(Jgx&~?`hSyrar9uI*+N#i+K?E?HY{klohmL0)vX@7B{-J?a^`2Xie4Gd* z48~=;kRtp5aSiZp-Vnu7(*t_$HH6)?Nc(!-YSEQs{3zWh0g>4> zY9j7Ctt}pRF`3;E&$p$GgI56G(oHJkc~Sx><#wsTbaTPVpaIZgL+qig8N$GcO)I$_ z%vq3lJ6(*xe<=rQA<}RGXtw@58p+@Ms-nWS@YEsr=q~j1R~f1#&QhKhO*L7-}=+H-AggQ zo;gxpJfUuC11LTNY3JN-`3Q@{sDE?!2*1UNcq^@|=F5xO?)v~!j0}tjYGFW!mpOis zPtL?}v!k@Qc3PO-kQ`uYqRv_3)_&I+wCtGkuZJTL*&TFu?3MWl{i#eOH&-RNPb&11 zP_og>)XmKX>ONNzuP@82pAy%h0pkf7vw!M@be$P{$Q18V*10SWvY=>URIh~cj3h{D zqN^_6r9Bo?ZOe@t1NP?O{0?^it}=o2?DbV3Pn(93nEj&((Ypcei@9f=T4?Xh2wJ=a zeyn_ey&LULc6jgq9!Dq!r|fh^rE)I-mbPU^IY~*qk#>{UhJ_m4UL0w?O@SnfW*d>8 zd>`c6j_kBbRwOJoKoX}^E?uKcY3j0eti+eXlQh9>l{1MJ4A;}vV$rG(2RQp01J=dj zKRSYt>l!frkb7VvBhZZXB~h-DwzhPl(cT|mpQJVptsgP(4kh++-I1WnXKw(28)&n( zR_i;+h@1G=aZ>c?aejW}#!u(1&!Tc^Oyg#6m#OHhU41DSZB_5=eVqROZ`R~4BvE?#R}p3 zfDR(fjJV)jQgCR-_jTi8B-RqUX)I#S0-}0f6qQNnjVUPBzg?2eXMbB7`@B)hy7HX* z_(zT@TdPDU6-s}~cnNT_ij-Pu-+WWyId#IY-{T|hFkN)wUf(L$Ikk}qeTU|Qh6SWw z1~xhpm2_(@M7NRL)v->ePr#;Ak9$CX5H=~>OuUc1=aSc$MQcUmEufMqGJox_!T;lwZg@JLIlLEM3!J%=X^&Dn>$ytYvbqozW~hvjX7hz50N}`@}uiBzy%*8F7GgEI*U<< zC>r%@Xr=7&gwD+gDja&?snegu7bGY84% zkf@UW-`?=69>kUK^jX`Fyz5D4T~aD~6|z+r#kRzqab}a9w9H^QfvOzWMx`$5Y=o+*XO zcP6u8jPOm3E%yz}q7K(B-0dgZBlH|Vwv7eN(KVQ}#;iRb&_O-`P-bL>(?Fgd&L{et zS${IVi7QH3E0#ZJ;@LY(6{hB2Hkf^-w9*5O-!~N%gK)$}>tI^#v`+lrjJ6V>+B;{B zK?!B6#$?_8S4MuxElKg;`k%n2Tel9RS;2NLLv36gq zrU~nM?Ch}WN3Odu_*T2#lS$_>rl}I@=azLf(V$xvR~fE*^aE#`|8z?2yoct@M&~?v zG`kCwyr`ndPZ1(;$>dSmzJy?d1HxQN8F4afnsu{Y$H6r9>YHwl4w@j45VQUYd!lpX$udTM3W^W{}k;yWxYRafJJonVuQ4OOxY>@crJMEyGy|MP4OMuSCgoi z6X&_sdL^#U*HG@=3Xt7K1qls=^>Dt zrmy|PfQnhcngnH^;_9JM+Cc0aPa`&+G66D2fZHH1Zwyep#P@*oImI!ceQ922 zT|r#j9SJ;pJbF_tV4gzxe$nehi=&MgH}L9?iPQNO;4@p-`xDnrZP}yKpg4Yl^irzQ zp0u=+AByt>(B2ZWv**@|4#&Rni1#*kzAz8OL0l6eCr@!RP5F!1_7j(ncoJmjwf^{5 z0Qp4ZOjUwF{csc+ohzny-JGCL>FR#mRCUUE%cJro`U%&Teg|gc#<1&)vXf`l~G>lk8*dTDHTc@gVx z6`>k|YCbCUtL>dH2G4V9zkh<^KbIz1YNvL$W7ipj=c;&XLTrQMahk{PkYyh}_KzqK!F z71XO%u`5j`98&uS5xxl>ICRQX`BjI+rX&)*O!~Q2fTn7o97N6kqMH%vRX;(j zov&Xz(Rc`b-L*< zf}K#_3?;q`_=U3dihjRZkp{JLsYVpKU)l9nbWT$HlR9BrM>qDl!w}#Tnn`75-R@rC z+`Re+oN=}8DT{Gn*$rniG0@}#JQj8;-I8!l33A8I7Ng8WAH`}C()9PY-8#p@0TCmW zu_aUnT^_LPQ6~Oj>LQ@VljUQD*K*pLiQQu4t+$rKPAFC@c58J6c_KgRM8be9Z&Hf? z&0+LdItMdN{_$qV(72hDBcd)gW(nCZK;;5Uew>UAVKvvwzlBQ6^K&nSV$8l{wMiP6U12~)ru;n6^bR1|QeD%F}h6t?L@KgvN* zQ?>C9rHc_TOpS6q3!W=Fw!I0e88%`1V~s#p=0JN?L{4p(#R@$wKsmQG(u!oNu4RlK z$d>=gml5K@NG*Wq5Af|`8#sbTFRxHX!g1T8?PN=$JXcwO`gq3t;BcmsqQ!6_WCYO` zcbaNE%IZg7)Y9In#65^MX8g2*7T{c^yLtezZWukXCucg4Gt+s%FnQP=Xz!(>#gT%M zpXNFd{?=m!F4Ky{qw@aNv3mH`OMJEEwSgQ)+p!f^2gw#zZ(oBjrFe~x+*P4R=Xk3I zeWw{J4BhBfuk8Rj=~Vx{`08u!ewUio^$&>YR1;F=veHY700)o!s=By@pR8tvI>ITr zGfD$e99qz6?(tMpP3+^@Jd{gy`vzT#9Z~69XuqV6QZQjJ4Btt8LqMLCz4oYw2au|D zrTB1f!Ta7$@!`zvP-z=B`^e+lF0eKj4SD%$Sq{a#q@Z0Js_5#0-a?wUf4J&Po@9H* z{nT#))GVyUc?`0duX?gvlQU%2_9ExSp{p{V!U2co0Uhm$PS}$n&{KIAE6;_JxP~`1NOD*Y} zYLtMC>@i2n>ZT>%=X@z(j>;d=HW;Y~gCfmy;C$z}7cl3zfx#u1FH4nN=EWBDNjf75 z)M8>SP8+H?`b>U5_C}U&LNES_frEm1S4UUx3o>l?xAWS$b#*_4R~0Jfy`o!I7-Bh^ z97B_e*7*AB7+0gtr6YZKwpJ<;U-OASK$yT(Fg0g0)W-u^NK?nw0u~gnw z$2w-Y&X4vX+L>`|oLg_V(c#W$#+DP;GBYBkNsqGnFv`o=u{vTg=wj5 z<#I>?CB-YNkfiCY!NX(pJ~h_a>i9jtgJ3jdlf9M94@8L9LWqgZw1JOZ45)Mpp+8qm zo#I{j&ccm;%rS0j9T9}4x)U>4HW5lR9h2g)vdQNtwDB9P1I5rKIQj|g?$Cyy7CWtz)61d6F@xQlaL@T%Z0jHmVle>uQ`MWs}rXlD|7pgoD&p#QFg! zqM7;$bR%`nR|*Bye{`Pvxd9RDBCF`>grOiA27Z)^T*gS+dT(DVtHw5$CC&EAG)tpJ znfaO9;O{`FwOeg=y17%mOXcI5X`m@h9CCl#me4_po!n_g>odr*#B7r`paHf)(vg0e zOc1ouvRo4fXo$Rdm&QO1W&18`eJh8>0joqrtmzy#ASV(UmFY^0X-2P-fc-9)2?Kq= z1L_&+EvR!Nf_%H<;xNLNoQMx;xN55|rtD$YR9}iJ#-HvcK6c}lwF1pgbBB%%Uqxk!@FrWIO}?HFsQ z9iet>HJv~2ey_$*>dIC?^#xp*T@p18gK8xJTHxTKpR_}{o zHs1B%@F`Afe&WJhmyxKkVX;&g!X=|Q-5#K`Z<)~mEZoNFh6K_I8w8!cc}o+?yfj%Q z&|L}gXH<)dRuRj%%wR_c$=M$dQw1ril5QnA1yuQ|H`L*j5aY_oI6 zgn6ApP7B4Of|l`b;g*E;hT7S=w28w`ed8Qt+~9C2ihiOTw5<@d&kFFv)_c7V-3N-N zPt42K4{S3KhafP+d-yFWfQqrrv3J>wPPjTZS$QPF2v`iIh&wd)Cy6eov({8`(mMt% zD?5nhr}Ln+qg2c{ijc;o-AW?G04Fksl=kAJ;(1v;SJ6~i7`RFmuz90M%ps@m?TJ;Hi&XBbu=HaLt_cf zy|0tp^*VcLW7WDTy|=B22zO}NaPMN%$Yk_-ZalnHUJj7HB9P(RFPlT>i?qjB(@G<( z*#hsNe0SwWS6gJX3W`ttxHTq$$a*}`6NhgV{1doZ{v(&)(N92c4-B1T?Oa&ucwNQk zn5v-Etg3i%MNR4uU-ItlKLV`Qv6MT}wNQm8#@*&npCeT;Pe?cTo-qNSGN{^Ogy^?B z^4!H>Z}lZQn{_Dp>oliAhGJTuIz1emLfS3}iflk_-Ybe5W>~nwWKy8G-YW4qnc1XP#X5_Bzn##P#l0gEAl zFcllQ9=rtrF0G;2tDXrka#Ioq^8rNJ$OkMgqc3k@OG2|}j|e4@1iJS>VhI3x=)W zHa<6xB?@5oT+o*QzemF-e>Zm8LQj#gJzRO7`&=#0y{itkn6vz|9>lTCbh;sK_hh{! zs0T9Ts#ljPtCJS{LTo!oF}!VdSqgsH|fZwNNA?)h@1oQ zr!Ov}%-n~PICQ-T>{-VCl6+M@Buu^VGiiWp+i!8kZmlIB-_FrKLJ#uZz6pp&7e*;G z%2TnnM5SFBff9#L>jpDk$Mm~i@7Ri*$~AFQYRmJ}EH$m1$z9Kbz6~av?6{t9OBpx! zn7GYxoNo~=u66+EReiUG#oP{jr!T7GC9N9}8t}g1v~Vv<@6;}G_3?mMc}ZO_2qBzw zP8}RcTH}Z@E+hg9-2b3+3})`OM%;D$w(o7`?U%9VDjj8KUhdQU0f47)E=!H!S5aoj zjP<|L|0t8JhAoz~JDY|Maja}7c!Gro3~AEq z$TzM12#TFV6mu3b)_KTxor4g;s1{nw^)kZ5LB6H2;r3F1{IHH&5FbO^> zBMW6CC!)r7d9N1xL+DSrCkflHSV5b)BrSH<9vgvF$+@?_qgr+@;m^yJ_(JtVK1j|DI@{tJqw$48y!u z;f#SSfZGKxiu_$uvVh!(J$wu(Q$v|4Gb2NWGF!aW``uPnwKwNzwFfpVeviSHs4q!< zH+KaT_Rv0t>|7C|_oh@U5V&gRV8f*5Z5=?V*!MGMye{1*kAOPtS`Z zz}RXUo9Df(87tJD%333#skE{x_m8??&fV~ec0M$6r#v@DhgCZ1Qu9N{Omap?5W>uV ziluTO3nrbYhGNa$-Mf?;g4zk*L z5#GX9_1__`zu?c0BCH}2HiVUACo|jrg^7&Jns$jHbL%{wo>Mr|vU06AjzFWVY3_s%$A_@=`; zU$%CcRei|2Ar^LQ@g3fBCuw;ZJ5^XL*XOn98__faJ$NM6Tjp)qCt@fb$AUqj87^q} zrDS<3(~^g2f`tN6_NX9IKi6|d;yi^qyEbs>RdRR>ewalG$i=rftmxV$fsF!1WRbpU z9&i9~Tu8|on5{?Et*|rJp1zRDn9;LGisH_iN0hmZgGl=0U(k6ouL}R%<_LhE$?Uph zzHP=Eg~3+`&sdU#Bmf^6AgGQUI)x&=6P+s|?$6e;_2qz+*dB^+909>Shl#gSTK+IS zHSTr%IkJ1^8cZzIG}I_tu^XQHhpaX*#dum0#Etji2q&CH1C&Q4!=|s-_nH!v0plj4 zD>c(lcv2TDXME1?`;nr5Z1Gt2({t;7fy8ArphR;%7FNc_i-6o6+Gz55sCk;!VTW$3 zK9osRf}Mm?(RwP)43~Zvgkjpv!-zYacRQYb5Q#%vjV>F2zIqK{F%!FM5mrHh7qf^Z{ zI&6{elqoTU17tvY(kXunbcH}5q|ecC2PN=a0}o52oFv^CYBbu0IkwmCTJ@d6zYUWZnPW1DV5`{p47&3J^rJ(7QGH8uF0=2OYw||vP|B{ z6-}c6Ne!HyK8+L9o128 zXyUBuxKoyLj_=^ zQO%=3HpR@kGq-bofo|Ofx3sB7cqjI!ivcKC6Ty^>cF&O`aQA1om>@RTL2Q`LJ+p$& zPS5$=zwbuhf=g8yS#Hs&S=@Uq&cKQmuffG0Sy(6 zlWhh)7Vw5HJ&GJ>&k0J$T}Si88Dpc1t7H|f>Gm$)LitWu=C1-8kkrVyl+~I=2+Ju$ zHAd>%fC#a~NQ1}%sHiBdPv;D;;U~2@Zc&K4&;0qzk;+92ET`|${PY??r_fP5KfjN{WHQ5z z@_h`>AA=_8y_%w?5Q|N#esA|r<}vnK?KsfoXeANp()IME%VkS!L;_(2hkQIb$@Vy; zK>1Dhsk8I#(B|!LAZ^J~KtSuk87jp!Nt18e2O1x|^`fCHIXxiiT7pT2Ok2`I3t85m z(bxVJJBLpWCH81d6&X)9)AUQDD=8%64opeF(`MRi;!cYEv6wL#6@QLto4>V*BH{wI z>=LJwH(*bVK84OwD)(Esxz9^%nmq^wK)@4vwHJaGM}p0RMj`~xMP>Cj8My|}zvB~= z&x)HHheYZJqAA!6Q)8pdC9x2{0wFStWn8=w_Rv+R7)U}&KU;?*D2c=VKH%5(E)Ym6 zgY=cJf#7)wtK&hi%xqc(&u;&wGjgeidskC3|CbLTpB;X=P4ni zQ>|J(YCd!H`xtFCsWP`OycbUcS47%y*J#?$ce#;p4vi!%X**r|^yNV{V`*$`d~T5P zz4r0^Vo7gx4YX07q=|X}v40GchB;-fb`v6grgXtnE7Os3ZNxf0!1wPXbP! z?-P{vTUUrhRnCr24!OZBZ=Et;L2*%Nu}1(c-#qR&o=2F=PD@L_im?RiOnLJ~CvPmK zgG7#Z9srH9{eit-rLbIpOG#-aPnjvu*^ zl_1tMB$R&=knA`P+9x;%7!&gZUN#5WXIJV4y)Do=u)a8D@+5K z5#8%_5y0tl9J@$11YJVeiU7PE4z2bd2MHb&ae*$9gi3kiyncpH;1y7VGUeuUOJ`pm zGOD|LAx=S3sVE$i(6+W?^N=x?`uhWbo*Lv!dX?e7KYkZ>;sRhEAe7q@ZK^xpDy#;i zJR^{LKac2Jphh_Mp_ApYhi^KF9a8xxX=}pq)FFaXvMpq7$mjq6;9q_v09K&2N51N{ zcjv3mJcqCxaxwx#de+(xc@;i}#g_E=7b1Tb8iz*+qjMml@F-%*ul!+@(!vrx;MV6D zA6&=-VOTpUozvo!mE>>XZXhZjkT*b$yw?#)|!M_#~CY+Ib37$}Q?L2>m}V z%TdU)sswcUK6g?x4^a9b$bG!MnE2l_FCLAVp*+kEgrCMnD8(L}5=~bsF6>GP`F{+g zZ>~2F(k-i4;8Q?Nb<360=>M4hRTGsE1RQ`>2FZ?-*-`8EZ5tbb7jcUJ_g6G5U>GZ) znD>@p$gFai9sns?>Ue>Pw~s9l>| zp|TM5ze5HoqFrbAv9%Gn&eIUrVcdGNeuZRd)wqlYyG$RY3vZqNH?e7Ouy4eZn;LW| zE*$5ZKGT$bZF)E~bvVZq-+h2~UP)m^LtQl4MYb=qVjo*}8>QySmbR#K$ZZ@>viVD| zYl6TkHk1WFNao7z%sK-6!*8``F8%75e9?V~~comW~~x+O=ykzb$Ik1^3VT5fnq(UREYIZ?gL_tIbh-44pu1F-bc;C<(_w54}c z`jw5uoh6MglxAp6ytjNv3W=g>ZaTqN2idqxc-bvI_9b!j4#M24#DMQy=;DoEbCtI& zE0HVz%tdojTei2P9{)laO;#sTfHoWtPeq<`%+!UU7|re0Z;(c5KIB067=696NB2ar z-P3SrUQS8>8@by=LbUO$tVrZ6eFodrOzbkAwD^R0&gr0s14@kR>^(AtW>vFxXxe_<(8z$zYl7=0Q>D`WJ zyH(!6Er+rZ$@6Vcg-2MyRNl3jI8wt_rAgIt49O*X4S zLD*#9NuOiWf6Bu`x$id5)|U0zoGf-htEy(Uw4iQUppY@&+wBh8>rXGE@P-v%;_~;4 zWKCJjS=CT>zs&JKxlgH(K0*WGcWJxe>g%WASWTsMaZZYI^=x?*CRg%DW$tJGOhf&k zOHOh2fXZR0CIz#Ut-vR83I9W#U5!J()C~{r`M-&*b7{2W##3ru=9efRf|t*z{VpDB z`H5W~SU89gwG;jf-{hw?(UE0DljC|!ag{5Ui;xJ2*Gb6kfHh}{c!71aQ}9z z(*g+!|09da`J%34Y2%Ifi}0g)8Ei6I?6_2lT)u{t8x+%D0yXdw;bVT-oT zD`uNkzdSIO`ZNT#Fk)1rwP6!Ak|o6$vF@2eu3bzH}=PR2m%|k zAOA0^&O5BhrQ6%M-HM99RumN^*bwPTl`bN^N$(&cy$MK3fPe)M5D;n7rFW1T2vwBc zd+#McfB+$c07<^de$RQ&_2nNg@iIKm%&b{!)~t2^s5Ktz`;|N{uGqI2Gvv} z&9ok{cNlY!rwSMY-u)|$OLDtT>?3K34NnL|%4i(06*PzR8>KE!lkN{MK=MHU8vFS8 zpDg#?#Gu08~(R2(_lGRng>sgGALR)~1Wk^qt!96n8}5 zP{2FbW?X^-zvkgno!ME>ks$IfGhWu0i3b9JH}dxw|chH6bJ9Grlx+ zeO@`7J^nQN+6Y2*j#iZZ7@>>z-YseTOaS`spoS)34e+$&nWPz zx)ICpHRj^MhV^7SIb742MV5VEN zNQJ($*B&LQ&q_D04P@V~C>^yUbyQCX2w+DbQ=%qL6Nf`zZ?QMvokd;6r|bM0++dcH zxC`FIeeH)BqSpoAmA&u0x@x-k&)Z4I<$dQqA@+KH|F*DcgrE=fGd<};V-<=A`aUWu6|&Or50?tzMiN{=N9bJly@b(Dj7+kS)!Kk%a%|;Ji11T*oDb z^U)?kwi`{MR;0h}=&2SY-5@^}CfrWfCappXX{V$(bRMX+L?`ZNj*p%%US>S+B7WfI z=>;MG4lr87$akpFOpa|0{%rrW#v(Rl+%d8jS>iOmd`77y?aZ~e({!eib=;e7g{k!0 zw=0gWzdWu}$I?X4dTuq<3onK*Se<>=Sh3ghQM<4@zXTx{W&>@gs?p5U92$x3LSv_1 zlpv^f?Z8q#_h*d(0kT-Ld2@Hrr3emEzLp3)!u&83(pqM@&Gp^-@iVhGCPm}w8jJ#g z1Hx?*p~n9lXjC1O2}y2ikg4lUPb~#%Xivb9CnJBEL5739 zQIU1)VFfky=@rg{#k1(;)U@46yw@xvD~{{!d|A(=)uqvcE6+jmAfcly;J!X{Wv6uq zmUok5?QNSCu}y&fZ$1cBB^6lR|J?o(S~r74Dfs;!J<9apTeUn_%G!}FIo(JumL<3A zjj^6^y{(c#pLJlk32M$KD~Zi%O;s3t)?2;%e*`EifI4GI+MewK)w3YrK8g{GGtox` z(uR#`pPv8u;2|Y#Jx=_cBTcYRA?~bu66{66RESBpdtnt+Rp_YBO%W!gyt;I{CUL@c zn_b22q${9T{!Y_<)S1e;)Z%#f<`Yhvw3-;W%eWV2t@NDqr917y$$tCph(*!$o=n$l zyIdkO?*;fZs;_vbrzcNsQlNq)$3c(&G0fl6m;Q6MD)ySeH{*=d+>^00d4uaC1;rm& z&waI|9!nulbmnXMytZ;5a==Wzu%zgT;c56Lt-EVOl_b;KFK8aBm8+ukKX84# zx-(wfDa-Vxh;H2w(|;a-R}y8ckl54qHJ9VM+f()iF+)U9TaBelR+2ry=OH z0+0Thq8pTV5a02CppX5)$N_6XJ<|c*MxgT*I+MODK-MWtOG-xBHfsWF`l_-b5ilnH zQ)&Z2cb4)Ec;N=TQ~e{#Kvr*zG<5gYtsUtvV30g?FIs8-ol%ZbgugK6aHRY#9l27c z;TH)P0v;L^rcu6W`X=_VMbLP3%b4iV9~{`xS}%5wQCJJ$iHwsD4yUhRA=kK~DljbO zk%sl#pr(!TU;Vn!+kr|Yc;%SxN!(%G6?X({{XOsq&)^DuA0@WJ>*9StmF29>?R{15 z@8^8)DRl<7^qo!?xzkCxn%a#K6csCXgQ-^fQk|qt>=L8R-`81jvRo8v+&SfDXdfMS)>I+lj*7q4yE3Ft&(_h?s!w*JICB1ZY@W_GDJ5+{cK<5%M%l zz;K5rvF7;}0oq1w*6Gq)Pr_7^F4}LTw3&fx`UpxYI{k}k60vjI?XDh$;2b68p|>r$ zrfbg0BxPVOnMqUGo^gY$SbS*b{%~WL!%6z>>e8oG`t48~e82r68`!ETX8S>(&iAUZ z`b=mV+P}ihvk4|qR^IS<(BtG+)7Znt>-FAL9SYUpO6c)08Z7Ebdg#^MS{F&Ta9{(z zS63uDe*Yb5_&48zwh{2E<2&Zxa%#;>Cyzb?>zohdQ}vv#Lgxaf{CDZ?Bb^xYdrPPy zLauVk)=Kh?hsYg((IAA~2f(|p7)SAULKZ=&;!q)J-v%DDdv=?gtB1ZTQ~?@+$<)yA z3s1rp&iTIXaw#&7G?CsYLyz~#?!ZDsucp7KzRFJ1kvzKmPRPe!nzXBx&yO3shbaiR z!X4%@Tl}qKnd2LsXQ|Tu5siHI?IQxSF`Zjs`NYAmwj?z_#VNK#O+le&8n5HJ*g8V@ zu)y&b{BVhEO%rNq5xd&O8dg72Ie)Fo_^LCntI@y@Pd4}alKC?uMcyBo{3LR=y|X%w z^BH!Y`jptsr~4^Ui{C#h8PfA?zGbjcuT)RFT8!l+nPJ|q2OCrpw*J)Cx=v=;K9>Jw z>ZK?P9P(@oxlph!cbqlK?$DPMR}t)0cet%QaYKlIBwBF=`LGK+S-Www^@D6e$<^yy zGrBnv@k5)E)dh|19IxSt!j`_V``JC0SMEjq$(q{roaDRHH2Du`v2P1|(uZzjLTtr5 z|0dgxE(3GpMXVpH#HY`HF}t>05AUy5VeXT>Ua!6&V8}{-JzkQgUOI6)YLJ^&7g;lt zfi@4HHn$tw7+3$;#4IfCsk#0>?}!$a`hty(#*C9{#R0u+wl%lcBv8WK1gTg-IscwF zcM&H~K|2t?QK`mP8~sgNj%#eX=-VibFjF9HMiqiUX{;M4@+$kc$PL}gh7r8%ULB+- zqsA9Zjg556ujNh$E1K0$jT{AJT~0f$(pU(cVpEm7B~h`Ne(!gvhs_^?&FRF<(F973 z&R|L@aZg1{%eHy)vp@cH%)Bf|Cco#f=sy1=eHYUBuUjmd1<5Fn-LcQ->CWdL-W?Np z()}jh?ovlPuz=Zjn{#tK^N=r3eSCX=$u8O)I+0-Ad&Xb1WX_Zc@Aa;&i~Cf=bA9(% zFN(n*Jb;ngN~>8KIlV2Yeprn?te^DVKTIv2hg{M&b)gSVokDD&ofJrn^^d6r^j0Ly zjh5!MeRRikgD>1GvqO9iO`N{nQmHHxKWiyBBA_$7pKMrHUkpYV0%3UzEemNt$K3*qGP(d~{xp^>Lob@l!G$_VQu8QSkI6c=ugk z_KJk5q-=q4qjW+9kG)sI0Fqo;t;VHTDfyoyq%EskDj#}s=pt^U^ONGZYx6?hG&zDw z8Mec6WeyqSq&yW3?aRdSPCj;;7~8IE{u4uM=1t(?aLJ!~uc+7PwC+FFqL(~j)UFz9 z{rRAKGzC7ywbK;HCh2OvY}Bh)0(TsBPLeW=jF^#h(HmZV6ya3tkUm%8ykQhgR}!kE zUV+jzqvkooQpQ#&CFW=)uT}ev(jxIpt54`R4+JKR4r%(TrCf%0r!?FdsnCO`> z*u{K*ylfyp+{;E1a8cE^+@p4fME$A8bZqwzVeUTd%KrqDzhwutLFv%bn~mz5UpFw& zF^BO~T=Z=b;pWZRY`y26e6*aK_~D^ff0A6!{+NR^jV|BaGAC1nD*krHcYW2Ws1eUU zfr2S-=T;>!eL0pNb^UEB8Nitge4fFd_hDBb%rcF)Jiq>{!@+a@`^IUrCOA|I3-q|b zAuLf!Sa=fu1?Y$wFRQVvP=Jc#@C19dibgr+!&pJo%kpF5;hV-}nI#YQQWggN?eDdO zKI(_=eI+JRE{spQ9pcaZE)?1{C=QR;X1u7nRO(ll7i=?nai8E~p5hN)veP7huNaOAL+u8BO6!D{)gfuUF~5OkwEY{+8Yg zPe*q-qTppxCk>6S-bB@*di{Fb{NFK1u5v`{yfuzGN~_aygG3o(_SyxESN|t7{Wa|? z*7wP;oy04u!fM1cn_8>taozrr%uc7?Q4JvvnJ&mu(~hQ@xun2XgZXsrB*QvJR__D0 zG3x$NEc`{!Rrr2*Bip_mrUeh%fFCrS!(!7Ad*^ zB+f5HB%{JyZ<(74T@0@$b}%I1AcLT!7CUBXZK;dA>mqPE(bRvv%taxLKR@oyjZWII z(ZVq+Gyix7=XP^=-z{5rH3gK+SW3emde4OQq~5Tk5Wc3`&VPQ> z@ADt|)q(>zmW}Jn^>|qGIBLtg~Orx!VtR7RRZt zc7CVdH?|PZ91I~@kMSeo!a13gN3iQ1-H40zQ1r8!#APd!>TTI!n6`W2{xa#2VX|$Gu87N;oa~+m$PFf1$kz- zRm`ME*q1%pq=UIrMX=vjPZP$33no-wOIyt)aJvco?{fl+X?;X@PB$j(-Ma^RXJ5Be zOl++zuC-l&EeV< zvGJ1A%5$m59ilsAIn_Ka`tW1!%Vh9Vc_As)H#2r4i!q(hxtcGN!#Kl-CI*^B8BOmlq}Nqx&{$p?_C*1d5&tc%eJw2Qd`wb*UYoyi>>a! zz4*Z)>5UOpfUoV!w+jFc!SB|bDIz}9^fAp;}=9kO>y?(_fLuJ7A+ z>@syZE8=3So&9oCx4ohZI${`U76SJp!fBMQfBCdCJKSi0H;VK7fY;3v-OWLZ(DnN~ zTUG_LVQO1bC5dGxnw!p#Lo4D@e5qPfs<_j8b{l#!^wOhuQ9IE28uh&N)WBS26t17_ z^N`GBqQGLKq%vaa%W>4S@l~V7f4ST`Lx)`;S5#vS6buJ5erkWm^rT5MH=UyZlp~l; z=n;%dZ2qmh8S&!axggHK1dwkO^ya^B(sGSGqLMSc)R5N1Ub9#^*tPiha;ag}r^bg% z;C{iL&w0N2X*U6?B^1>&(5TeQ%V30O8ULY4D?q5bYaUe9U^3L(e7jmQLtWDpc;&Jv zt3Xc=SsSiq5M=)M;>_sz)$Gq7;hUu?`u$oj+XmcI19ni-5j^DxJdR)m!bl*u&jxFJr zxL&K@+Xf!PGCBeKYojWNv%O{qmqc(zZeq)mKG)cns6FzE(nz(VSE4Nv-V=Y5I_-l{ z$z82jDI>ugc-JnMZnW1ecOyUjp;>}u;`cW-8Z1#AKU8spC;IKHZVkoUG$4)2GyAqo zizwVRzXceLpls&vQp&|;2b&wj;=zoYo8efV`gIin*#!7qn)aY7>SOh3(lf~%4r6qI zLh6a%;VB`UX{4;0KVGHvURi3RE`gsX=khK=^ysWMy8dvwve*l=lg;8t`_AgBsdn`1 zDo(HO0uv`E0Yy2=eGRvzF~hp4eYEy5PsXAPf?gBZt)a_Jc0Os14c+k^cJ^6(S5#IX z$`x~YIf=S1FJ+dFFt13VI+0XV1m=G)Un?(*YRqBZJXbk8dT*HyJH+jrDu(KGOK_ul zRt>U2z_E7&FDI%*uNTCUP`lQ#)hkk=Y?VM?(LRY2Tb z6v6E(dA#rgEbk?)d4G=r=Q(PPa;?$^Gwj_`koq|Hi>X{e#(WJq|dzF{jEV~XLwk8pEa(Hl?zhd^s?ajx_H5gRNE;W@~*uVF~ zMnTCamC%VDO{0b!WXF%!A4Q)?kquK%vNgVInELe24z)ml?Uv1<5x}ntD73(CHXNL*ttvkp!Xfq=1SeLmL z>#A7tdp`wF?rZd~PZ;0_o^nQoz7)Dj)BWOB%G;EBw zT@$i@S*LiHMzU7b+F+vmMYI@khTW+)O)fz8{~Hu~svph&9nCXT*9`vq>wp{j?`u<2 zy?pg=xI>vCa9a#;4d7jDZ-GCay{C*a6%~(c-^YF#*F|#6)j9bG^;hbzQgwja2JkOn z+Uec+Bo@O_8}06gL`mOa`i6VFo-}G(Oumg?FIygAu$c|Z?Zu+fZPIq3OTkt?rRP%p zN)(kDK|*`r-`jf1RLevJveg^9F8w-kYQl${A}wF~GL)`&@luEKOgIbqgsmQgk42Bi zGDEcNUKmt82|bP$_Wa(In}*(tGAmpD(3pt8?I#XLUi75`!41V~z;sfJ{&STKTm0pJ z%+ctu;Zu#AUVM#3B=t_%ia9m>TB+K<81-%M%2L@P^8SZnAD_v?L(Zx8I&ZR2_W5%$ z%uG72-nI>$%;MZr^Iog@omb|4ApUob%BUW7{CBQ*HSb-W=aq!z>K}D!bj* z9%gc&R~YWeE#F&-z7}ZX%q`_oNStwEvv$dUYH69`dPS)HQA|N(_#;WdpSG+nNoJ`9 zJvMJ&1zxehh|uiz2+$~CgvY4tq1D6~4UVf1a6 z(QL@2&LhR&otmER!l28q*SX6YU52I!|5AQ4=aGb9`Fit!mgHE>`dY3|d9FX<%w65! z;PIObhMoprP0iCX{OY$)PGWCfGjKYcf^cc@&SDWuME8sEda6rw|6K-e)S?b{SY=%K zs7p`MN51*N7&8AX-}|%N;=#^YWaG^H#ND{;BD14>QPs_xCFA#>jdU94675r}cZL{C zf^hLSIfM-Cr#f1Z;Wi;94$AmTdHfIR+txpq%YFlH@r@#+2vE#IMsXbT4%l6t)o zKVY|jbX4E#vFoh#X=;5ugWM>1P2+amLdD^(iDlF%?cwX1Q z&#wC|%9jtgrlwUoT?5RAMeEj_3HSMX}=_fWv^;`#tFu7 zeShzYeDwH~`uxgctBUQ>}qvNSO01^>o!(DWLdiQWY___)SS|n?lb$r zQ=H(KQm*>MHpst1#`&<<=GJ(sg@H|~>u;;pDIeWURxK+7146n?>+$gf?$Go0!npTX z1i_duIL)7-m5P_j{O|u7Tsb2X8Mc0mak6%KlAQH8`KFzypXpwLv9iXb{hYMw$eecW zD*v!&m_4nrsIT28Oit!t$h>(i{G_#8-$kpc4$n$HLYtpNvLlaOFanc^=k4d#aMnzt zrmrSPs#NL8Q<>Ld zin77|XFoa+91OkpWDb|FQ=&%SDnb37f{UIR-TQcak%V>n|r&m z87|JM>aB&i!Ds%^508F#LoPws{R_H>zocoTq(Q&o37RuJ^zH4NKXj~Z-}*;Z+%4nF z_(tM657@Q;ec7z^1aHHrnA=R`0*#~bKx@aJ-;sT8o5-u~2)*R0-Wr$B>eIT%#W%)p zZ8DW{9g}`5uwLKKs$_+e9NxFDxox9vjV4&lys|5!lL}{dDtLOYa%ACIw!LL$+&1!R z!tTS(zf56Y&4K*Jfz&AiV0&W6Zr5>mOLqM8-+3q9R~ijh;5a=kvhf zEk>kO$*ydmY*_&)-CEpkp1a9rNlO%?G2< zZ%j{LfC9){>*cGqr_nb3!|W0oX;KYtt6jD3C|^H6(-EF~_m-wznuw@0f3te;D5*`t z9->en`MMtN;mxrWn^}7gq9(d9Jc3(9Fza;StMdJpH5Vd};7;JZJ*R7;^!rI5u5+HV zbnL3q0p-Wz)lHyAwac9MmxC;&x&$=%03#mo7BNwoO<>U2?AF3UbPA~#r zV7$ZwR0rW#kPti5sJZ?0Sj9;OF;?M|?rliHh}FiRP%Z@23x;U*cNP^yH&YY-fQ~G{ z^vEH}eO1f?V3j>lH?Xbh?e2dCwGIE{urxW`7xHpfLjaTZ1~yCIR%KGZxT87`Osm6`cHLiVC z8}_41G`_Cpuc@mbd9c|D!eQ`ul+fl!x{|k`L?-KqL3Ptaks1?i@*x_3Y0R!pdCYqX z`LQfAO-dyVYf|$zU5$Cvi!DJ_uhz{&`qV)OhUlbEY~)Ke)E8F#OBF%|-t?^1*#I-Gb6KjQ+ywHpq%=i7_95fj<6? zwoef*$Wz$8l>T2Iq5ZYEA@3Mdr#mt#s_~))J#Z9HgD0E5?qj5`>gqm>fi%gDjcViu zYKL8%Q<^8IO66%*OZprtKDfH-&^9=@2NoTibV@*j8E=5$wk9h!hI5mWlDIINa}6i0 zVK;`Q4_m|HsWbQtMwl!Oyx@y*JKH&!JRw-Id=lZLK5{-x)tkt>X;A`MVW9NloNQH; zb0v_FoNYfde_EQfKOCsVxv%x?;qe+3&0{%gNn2Z6F?_jt#YP+=-H#t0 zulf5|vM$=|I0VU6T(1gxA(T{IJ2rt+u4;FAJL{!Jh&41!Dt|tDAOk!Z>o)-V(3F#G z<^K!=T?e2e6_CUW2ju><3a3EW)Ww$th;IdS0wCj?ZF4~Ed+(af=s0Y<==lPn@HMaZ z67)BRPronmw6*(yBiuT>*cjnF+vs;fBA(donAUrjWoKvauL?R&{A7h6-Tj@6Ds@1t zmChA0Ax9^&-yi}!-Mz+Hnq({wkJHXtrUIQB0AYb2#33F3VqNi2>bz^iYQu;F<+Z{9 zpz3r&{#07=>c86z$HIvF>;o<^fDh_H=hZC;xrKO?Vx7va!Z%f->AztC#otNM>uas! z{X#q(uN;14GYOIK{^!;$L*zTVkQZ8cFVhDG2I{;7*6Few@eqlK8MskXm+kWT?jj?f z12fo0`e)?=?sG9<(Sr79{o4(?F0E1JVP3W)=Jon> zlFxEmhs0!O8;_rSjIAC*fFZ=`AJrxsE&K@ea>OMX`CJ|6+eCE~zEbHO0&{RvC zCZRmRo;=dM?zKV~x9HH@A?xs{MwDN~JZ+?ZdMIp!0ryky%6@d#%Gd7-ptr*MseU97 zWXbINO##0iiP3PnK^D4|kdjpu4M(#_x)jGIb;Au(-w!nlYL%o9_uF(^mlr~vU5(os zv8{ZIsIP0vBP_)+)Hj{1mTs*`=$Dub>La}7-_Tx`kGOq{nfc^sZ@CAue~e4236s91 z2eCJBd2dyrJ9`6S$8Y5j-Dw;Xc%B_Xy|=P1G;|3!3wnbrsIxN#ZFmT>1#b; z%&qNVN;DD#H0+I_u$FPdJK!i{u3Vlr{<1qpMsm+azST6k%uzf2;Li`Ta zHLWAX$hiJC-Czf|R0vI^U>dR^Vd@!TeKgn^fG-NTH3VSj*#hG)bI;-5ZZn=ZhPLxE z2baIbb-l|^iZV=ZGnIc@ta26E@c~rU9PwXk#|xILd*CRGbD`ocilatQ01LEZ=pyw! z)-HeCq#H|$mYy}DvD#%VU5#nfZL2#f;g zX>M5(G=+`GB08X7=P7C!HEt?{->$Q1*(HV*13(VoS%Ig%R^Raf*!I!YZ_a8P2CAd+ z3XXuqTKS1mjoTlRoV%cfk^oJNf&!0&+oWiuP97Zf$i{#4ANLg3u_8!?%c}>k%%1fy z;w_w+7u^V!E|11$*$oa3vMI+5Twb_DL(~0JrGJ^d-|M#e<2;F$Jkr%}#=@YQWJgJN zui<=g&dZUu6}th`jfKF5w2%k&%*fb3-;6u_0h|hydA}@rcO6g?HAe)#<)LW6cuF;` z2o+Kr=V?QhA1>0^2*p!sz;{KXu-8m5;#k}BPJwL&V%Wb^;0v%_Oj}{3zSmso;LvuB zbL5>A;S$DT8wGaa!KTM~E@w&Kqun}8qqytRpM33HUQ=Yu+FY(inqIz^95&R=)3eca zS^ijZrk>a=zW)_ITGmDzKV6MuT4S7jvsummyM@~aR%KKTO5Vka zzTMoC9l0^3ivaTu8tO~|taqp|_>74;ohbcoEl0$*{j2QFmizn|{>^5G(tpd6ea%)R z&V9{c{$efaq1MO12pU&EyoQ`lMWQ@+?Nu<^LdkfCcLx!|TP*kNQM}F)re%DGzcSf3 zv=gPtr^DUdg}Q}$+tY|NQTH|C30`~tq*r7A(FYTiC-%Qf3dO`(4WSG60Pgq8hP4Sjlxg@X1%!7%g8{3l9t9{< zK;Yw+yrhbV$v^cSl#0uv`ftwaJFjwgy0vTChMKP3nEQ7cYQZ!-;}8X6Y118eE~j&8 zeplbE$t>j8l~9)m<0W)qgP=|uKk6nrFefr)Jz=E8&YEIP(^fol)E#>nwrtV_pY82c zdl((k`r60mq`A4-rID~!+C)0uM$BJjU{DBWva+_$@fDq%oOGHjUj@$C4no=-MNfoD zk8UBG^A2x=4RABk%w~MPKM&0*TXiPYMvoN}koDxm7Byq>}cfCjY&C-NYYjAVPTZ; zt08~-`kZ$ceqn*<7W1(0PnD#lrHw6R^YbH?S+)4qc_-#(M-!)#l-vEs$LfIb2QWgQCo%GADM(WDUl-UH(jBRI%Kba99WXFl z;}?028yV^qi$R&1uT-ezG}=zy(eNFsyJGYvC6BW^Nimm1rSG~Qt97`X^4(KU&I}^- zS?Z^_9;w^B_-y;;^V}!JY0)HRF;>AH22OhnhaLa4=;=aL5`M@=C=wzm=C-0(s8`(6 z+dEtznx&PWnP~}rs-7m5Vque-g@L0*JEwoF306FGW%hF>DKr7xIec~WdWK8u-Qno$?OT2@fD zRj@R>{ohH6ySGsc;tSwHK`7GmyII={C`BEL`8u^AF)!}RddPpu8%}*0Tx;wUR2=KE zH*Qy3w4)gLyKG+4XOG`+1PemBup4|4%pxUl;_+yqK3}PGmw%<*aNG3s34Y$+pTPf7 zVYcz)vF-Acx`)GHE-Iau2j8B`qv29ueJ4aMhE<=|tW5xAM{${440XGNx}m9QRNhX1 zbbAH_5UP0f{_uVIe6HNidABoXR^b-V_w+wECc^JD;o3o=L6Ta=Uav?rqoTIz%9|3b z!;5n?H?J5o{rCNJ038ez`)+nZGcZSKH5AFW4^Gld>Ys;JB$Ss1Ev{Q{Ki;_UaA$$T zp5G_qWQ~rY;-jo4X^S z4C7a4iJXxkiWi|VOg&T8j_rJaFtsZU72h3^>>c0%wWb`}Pf%saxw8uiKNM z0lwEKAzNh9Q6o)K8h$ukVyVwOp7>_umD1p$nMa#^0&Z9JBJ@jLE&X1&VJ(JUx6ge^ z4{S7;+;vTqL?Q?5lT?D8AVyTLBsXo6?^<}=$DBf!5sMDTU*^+OpKUq-dq+})#iP})k#I&;{LnduPqqrMj@YOP9ax*GR? z?3bd`;i>j$#~ew|1(zUqNX60p`Jg0QE07@0b6(-Ic=44gBeSgEzPSsg{$_RHXx-KT zlbT4C^mko5B_FHr36zKq=FPRV@XU4}(t6&!_8ln@spj8AKsR9}yj$_>6i;v)>&&_4 zrQ)0)?7W)K2Hd!9T0NkC2oz>~XsWKd4jf8;1I`s4J@~oU3lDbS7A^R>x3hIhC*vFDB%J%6QCeR<5!lq^hI~?({9ji4ID0@&0qKokH^+v>LmP#P1*-p zd4fK5t3NJiDzK)+b@xp5B-B}-n2%#X^3hDP-@N`y1hM+$DgAO{G_VOl2V_fGoqzG3 zX1**W+elkP)QKn)jXh#%B1oa2O_i|J+X%Tn*^|p^Z-6Z7k%KXSF;b<)E$yy}1?q4_ zc4VN}FMqoYs_FB5_ZqiL?6bDyf#B=vGH zLVN0bddyCGyJnlbAxFcg-FbkLV7TeWzT5M0p(0Y&@)kml>k&{S7L6(0XhiA%@2hMuuVe=`?%FY z1br&9X2L<`ccY3UvxZQy$nU(l=io#ZR*fqXds^S}`EeKoV)q=9>Aa)MUhFlsPb`pZ zvUh_t`J+&p;nSf{(-hjB2)7%Nj?4NWc%dpU9e*GK;`WWwW`KwXqM9Ysyd37+6l2mo zbcdjL=GFHmD2VXOYS^j5`ezs6PXm6;;YSG*4>9~}l>nM@?m58W1DR_%6Jfqp-pd7BBaE&CI2|h9!&FAY^}eUhL*{W6Rsa~mq@)b4Bw&Qs!lTmFT2!hVY0%L!-hH)M4Z37}Ls32!#5nXx7;Hj4?t zcGP*=AEM@4C{g(APz@{kvP4e+^mGfaM4{t~2j5M$g>icy8(K-DHUzA)!!1P_+zT&H z#ujW)g5eH(W^s@-Cs6)g^C2m^LCzl)K`quEV_kRb8U$i=ZnMqRyD5# z`alzMm`k!ZdMsz>T^55dGoGXspcL5u(Lcw`1)M2O`!YBCqw#G{XIQO*<|(3%xv4ao z!!fK4oFjHs0EA9a@4AscTb_oc5Uj&A6LJC_8Rb(MPTvMdM1q^OMGk3GZo{OP`DVrm z&Qo|Hpc!CRV>|s`;4>`1H#|8v;8+9&%{J2~DuG}O81i_EG|$XIc<({@M%{s}qI}?H zWcbhI%95jl-c@Ty9#MGoYV!ppeSh7x;X#F&vF@6v3RhX1=(dtECkbT_`aJ$e8JoKu zS|2}z((YDlZO2M24nE#!ME?5H+h^==P>Z&~H#9+6kO0%3NbWQS-R}S=sD(#CLEV-T zjJeg;NR`EAK{@z*mpA8Re6~`Jcl(%7_nCt{Kc!1hG(uy?8BocG-vr zb~$-FsqHh{Vi#BJLh#uY=zsk7momp?K(g>b-%8Bd9=#v;sOc;P8HsJa{tNy0R{^k_ zy`+RCzyr?XP@2VsaBYHaD>i@=##XBo>$42AFr-@aVWqY&3f#P5Cw8~X(Klvv zh4ye*BtE$@`)GwH`=MjO(+A~CFXeDo7FUBl#ofGV0I8^G`gOL4MlM1EklD4^`rVceFXFx(}^JNK@ZevhViQ!02Y?KMCcv^UA01UfED>y zXvX}4*&CU!y~*vU-KxaiIxdllM_Lq?IuJ>+`#Tkh*SxnIGndW2T`#$OPxYaEf|`+om_>1Gjbajsr0jWi z%m9abf=SS=$GX_0LJAANL>iTEFt2Kt&fqHANz!2R2oH!x0O98G1C%D59IoiilHUpa zBo+OPu4h;Ln}r4h@F{?p4eET_<*@rJgy#UpRs^8SKs_TS3D~YgpNBSGsv<$pIM*Xh zJ|!3sG0N=md=!c)$p!Gw09{%7RS*K;ssLCCP$QCXH#2={DHJZf?$nQ{^GCPSBmbdf zB)=knh?Gxhh%=D(81j7xR4|X_A6`dS0m7ih?!7a<+-|8Kl%irz!6Ni7K9#@awqy6Q zC-cWJ{x9{pLrK^ypq>`uiH!iGRx(XSICdjwP3Siw!jE3{MF)8s{IC%IkRn)g48!A} zE|zb9Lr1X2ng;?_#i7_c-B@hiG1it**FUEVssh_7-Yy*%LCfBBpx#8`s6M*q2{?hV@i|)(nqg)PG4vvjg$w5` zUIKjsz;g`CF5s)XnqTzUFQM=LjXru&;!fZmymlH0wEL*OJ)9utF4$Z=dI!>Y5(Yc<{JhWm<3)3OMda#}5( zYTU|f#^%M+(P-=iKc@GQg5tY1`*ps@cJxAb`-=iBEy_MgnCk$@UF|P+6@90?8Hy4* zfPLG~x+nt-3+JeVc7swpkj2x$&anb0&X@)WlK`TSW;*uWBlC*q+l)eq9zZFjG;;^y zqBl`(wA=^4NFe?ynXFIR@73ql^t)c|$}KW$BkfQk69!I<&?o@f272xs^QlArtTBpK z*@Z%hl`!_Rq3?Y^fz43)79u+}14Y#7!Q`wDau=5{A*-e`s+O-W%v&EU5qFJ8p+3O zBdf6UU8ScLr;%6peW8;*(1%4c_U1VS5%zG1c;V~pmHcmFV9Ntce|*x;tyC)oZneJq zaf&q8Jsf9U_Gf-Xt7P;67+NmjB;VdI`d%wC(zx*vzbC+70=>pdH+a(PH3=KAfh{2r z4gW2z*~|_)xK}j)rR|n$5@h7=sh!Bk5#TEZWrskg4aj00{Y|$*1NOmadwA-fHy}uN zH3NL0WFMxY)QolwA-R5!{A*RvJycVmSy{HGXj+a zFjLtDg0^FR@4)_)=*ePO?j{+c%FC$Y7#g3#3KI)s2Trz2uS&C>A{^m-FhjPffLIFp z^YCalgxJSHaIrk=J-<0=#XDADU3CBhT}@H&N_zxaD7E2Fl2`q+fVSC2?ms#Wnp01K z?3f9GMTQ_s0mr`qZtoQgu@VPc*8_c{LUMppK213^iXDstB?#LfXTWat91JRDLPGd& zmmaB#Mrw}hdY&a7mr6A{V#oW8`RvX+anS67JIwh#f55G~C>G5x+ON|4peOXtn>MGr z<}XR`UfPh7ksnUS4n@{WR-?%h2P;<;h20^5m?6&>z9}}9e=@NOGT0|9O}79!AHR;< zb580Wb1VSjtu_NchX=S*AF-kA=G+|tSu2rw{Psz1d(CP=39VZLaDKTFrOY0XoKiD% zHMS$Lr|mM;5r50F8TSQg<3g*_1BQ#7N$*3TsHiYK3{jH*eI9kkFbL=>y+r%DdUbk0 zZj3qw{?G0_2W-akbiI6g2#^rN>BIP>94a16o5G|xQBN=%nhrzAb7)@P`O|Rpp%+(y z&+VF`Q>Im~dmlTYI->MM=vUTHZBj|hSBnh`IMkvyHuarGT-66IJ~U%|32eIO?-b}$ z`~2tkV!=TV$XInDMtZ7MJE2ntX>o4LO`^cFV^b})&Bqe)H*H=y%7aVqqZW`W<<|Q% zZa1qi-|>K$djsfylw`&Om%uQLqk`T7N%EYHq0H0_1%i{{O0u;F&IgFEl`IA~6s@%Yon+xqYOWf;}xsNLG-v zMVkS(@4&=JG&b)9djbB}dv1#U4B)H*jlo-i60lh2%A^}^pmuG0#b%zCK>c*8|r$uN~yfGk=Q9>iDGPj!=>h^qWd<&xp5~&1Xh&eFK*oQ&!X2 z0MAR3@|0n6-XAgDmYAC)NZA$_5;w#AOo7G1DKp22( z_^udD4S){==z8Nj;yE^evgf)=7JwEFBOlzac)pq3?g%1gN}6VS!Ru5nUNel_Db(fw z#YqGx_5K5Ckncs5d)tZbA*W>I#u1gki8bQW7j}}Wd71SM=7&WdY zWi#_udP!S9u`~4qXTjYc;3h?Wc19)q!mTCf>&doKkaKr9?*zau8K?G{W_D=jR|Q}Z zaTTzW>N1X}!dlYg?@0FU9$uD(0MX8B>k7Hhr>khgU9qU%D z#l7HG$+V?f1;X1X#W(3ph}ex5O3haQQ1IxX}s6LC;FGXgjh8s3)jzy4b9f{7sy;{q$>6+DOCx-J%g5CKN9%m=) za?;|V1Q5|WQh~sy>u;0f2w51tDKV(Aq3TaqHVCI-aEK8XWyxPw_oej<;L{f9&l*4ga6_;(RK z0uoID34n{f1u&||^uksS!YP0dc|<(^i)#OZTk2mb!wKwGp^#NX->$bueCqd@$?q=p zN#W4`QfG00Esu@KqRpXUuipF&0pSoSxjxGwKaW+{D9f>AGvwYrq)FoD2aIL$jh=wAp2t10aI zo-R{$eAh}Bj}uEbf?2}7qrMr)p-qd$=o8mp?h`xlg)Av%&&=&sd#|u@boinY}T@;Co0pf&j-f=dnSFLQ7%7Yug2r z-{YO?Yu+rk2KAkIw7&oanXb1PXIxeXt(cpP8kHn%9fGX~;tB*J?a!}87u@>(@il}~ zMcL4f3-^QPPJiN#&r2xrR6_$2GE$ORF<*WR)u zKn{N7G9vKUdC=$iW@I}LOzp8SS5|!?K)SC;9DVpUUQlo9v)Jp;;R5Y8e*Cf-%8E2< z^i)YO;UkXA`^hq_G8=AkY*AeRmw}id=LTaB^!(&^5ZvL!Ns;5}g$2BlK5*UMp$cN~ zs&S)X^#)fi?gr8)_vl6-fKI&YvU!Hk@Ci8D1Q& zZTRdb?7X#_4PPStuDXJc;w$$NN}d`b+T3#lWP+E1r1 zGvp6AK6%X^&dz+b1^TH#H5`B-`Nj8A_TP6E7HVzgC&ciD09h~Hu!|(TXYZj!S-;RU zw(D1X+0GK$Z=OUNjZESrFCDK(F7}hRPS%uBR{xw%l_|6cK3&WRhY%`YcEL5zF5sRmRvr6|}|V8ooOp107FL5VBPnO+YnT zpSK?Ekx0DtUH7gA|6Th1z>ZHM;6e__Yb};x@SuGAR!OV~cz^6%-K%7~#dM)+e?P9h z_E`k!`!Oh10`E}YF!Y2_>Pg1yPm@my!@up%yO1|`kCkm!8$`(83ufG=$Grfyjq}O#X%nO;O|xmpv_aF5^4V) zcb42gXBr?EB}{eO#!E@&#WT%oj$1o)TRUUob>O3UCjWZfS!2yB*-i}=XEh|3x9E-< zCrwQ?pBwzLSm?+4?Ceda%%&{<_CWQEV|;veBG8F}h#cpqBGB#pkV4tIy5k!+>*U6G z#`>aj40}@Ai#n6tK8{~@la#XaSdXd7&2^EG)^6w-qwmhIgZS)uh|7l{nvey*Qf%(8w5DH`ePyv=mW>OF3}gv6H;DP_2JqS&w- zXRp{l!!mB|Tq~$n)qI@5;h+(Lqq7rqNwXryUfh;|*423>Bm6&jb*zZM%JH8S2>2t1lCiJeIVc?VFZK7VXwThkljf1o^rLe*TCq-BX!3HB zAgUf@Qr!v7Zj2NUH;H4j`DHuq(s?;tO(*PauL1k9`d|5sDb!p|#SPhzp-g4TL_?Ta z-I1ib9#1{)?OPWoBBRB7bE;-W3zZdEK(OOSJmze z*(rhaWExsKR9GkQfaN#!OQImGy7BTF;YY31>ueO8h{)s0e&Zs14=w}q;H%gm z^ymIcymvtZy+axw7`)O3q+7=GUP=gHwr3b1iuaxu%0fYGf9`zV(<3-t0Z^b&4= z6S+I!t-EqDseq}+-Aw4af{;Le_&J}J_}ewPhSuG!n8{Fv(k9xqbS#x3(>q9(dOPp2 zWbGvW#{Ed^Q0ql#g!zG~$7b|8^|@!)x+5i6kZ)W%nR;=wU*J#UW4NwWi(*B#pC}!9 z_X{R9V<^vNVXoC5ZwULKBMUPjnim8gNDT4T3wgCy85;9x%)fKpJQj!^Pfe~Ad-sVy zZ}C)iji?8ohno1WOL(TlA(9(?Vr&9n$E(`IIv=6DeKRm9u))n=5@cKJHO=1UE%Ff0 zGokP80!ROkyf+Ppy8Zvgr(C5ZijZWBsBGC~FWIt%ki8PJXE%1)lC>0BhV1(~mMlY2 zc4HkoV;{yc7(2s#PT%XgzQ6nbe|SH+kK>*Pj&k^X&hvcl%jDpCP7mz6|1&4H+QNlFGycD)xHq{ctaCMO|`WgqCmj%AsOXq^dCo6JOy3Y!{E8 z#`s&`j@p6P;pF6sd#}G8h4sg}b=21RprMOaY|&8oL1l|Qs2uM9(qRRAw#>GxXOV1Q zUDFHZH2P*>v2r3{ih99h*@)p%uXh^;5=;fX-bKQq3VAPtfwwg<~un>V>PQu3Z(0XSXXg8}yM+m-i> zCZ&TfM8A3wL(vrK-Ky);-LEe%QzFDK>~1^hH+JaUWRZ@$7c<)j-N?9Zu?$^vTCK5& zbnQkN;=S$jL$+-ZLOuh*d8Qn%DVP>$dY$@MJI_q@NoedMk&KJ0%xm=b`y@R26HD{* zD+l?I6HZo0H`dU zuA9``Zn`v~-(eScq8)fM5QNPdY_r#5uQy9qeCM|Po65EgK6gaJ>_}d4(Z@7NAMwC* z)LXvw`YWg+nMWsMtcQJPuEcNj`RrD8UapHME9+#G!}6=>)i}mqMK%<-eywLzG5|bV z0zelu?Ud6ckCoZe{!!fMR&zA$8MOd9Qh*h2s;xsUkeXO8ZabSFGn2x3({fFCxk%rF zpn@s0!VG$BA+N5ta(a;ZBqS&b63VT4b{H<{=-Ezc8<)Ax%U~P=p$#r6d;@%b`n$%f zICB4Mp~%3&#j!8vn46On)+FPjbg?5{y8DJPGSzvl$?wL-aewLMX^9Zx;euw+-Y6&L zMibP|@XV7a8lA=xK0`@Zw4NhM-el%-iHZLA!$Ge;$}9d$$rRGI60ZY{BUXwSZqO7z zEy1luc2b;j9{QfWdmTn9^VD}uGKDpDP|j02*YXb|gMp9^{Pd$+BPa?3TNl)2*_g?M zn+7i8Hiw<)(sJ6e{qf>A*`_8H8p4DvKq#Z* z6f(Xv6BWYv3nufW_~rO8z{sRYR`GQaMmOM8B-CrBk{-tWNuayKF=Ih>}wB=KE_^QH0d1{xvJDTK8B_^97w83jmt z0KU(%>48OD^0`-%tz9~q-aS?}N>Z}@X?PZKHLRv_ZI-&!dH}H1QH@RycD)Zip2jhG zexLt=2=;0sTA8AR80Z|dd(4npFF3-KesFj#sCk5^+@4D{sfSDCMfZ#0xu(b_y2PH3d3poxC%XifT5c?`L&L+pk%{C?vrBDVBJcr^!n*7E^I<_~q<43(XDBI0# z%wE4?NfqbbgNK{W_GBalQkaO{KIA{E!~x8^ThdYjkHh9yZH*bH$s7;99UpcgL#be- z`~ybB!!kW&o;W+Dpl?Rl>@U%C=sxn?8LEiHTDgw=g_|zx_)&~J86xP75oRWu zmzTuMryt{{+ijY%3nh4LvklB_#%X2(u)+!C^B#6p-1XEMx^>sM=olpRY;+d~y8p6J65MTmT^s-PS#jmDd6ABsvZ^fgjK zv?RqTwZ4pq_aJ)n1MAFVJI#Z)5{nEC9)Dl>V4CIhn@2Tb!S06n*t$v6h!XFy@aDMD zE^rvtXA7R`_Ka&6l%vYJvnn;Vwk;e^!+q14+5BJ2?<>hzj-%c!STp1(bH%o&r`n2O z1>BIk1u2Eq6zuoJDPL_byvyrV>ds}_0Y;~I%4x_1k)`jY66-0FKQz^^I>`35Lj3jY zy)W%H>gvYsp4dZp(|N$4VkEw%jAd-QbF3|csf#)-F+p+F;jI6)Xk_3s^PU40ez%mp zVH=*r&uGb_23ODKtB7*H24k)lUit$O^Ss%40)d?GeMj4B75M?if7T0K6?&(Yocj?oPF3W?WETk;$YUXL0a+z5AdZAY` z_~5hNu$a-g&Y&)wUUIK$+p){&OshI6O3z{Kz-ecwizoh>vp|W2pzGu^h>+C`7)7An z%4+io4KvBstUE>V7JlK9?(s~X-xA|maS0ni#jU27G>Wm3iCk3;>w!yXQpjCm=+UtY zcQ={DUPIt7R2ARRy;G79&`_vDl0=%5&y!>zE5-n?u17OFe0F45Pn@tBeCKOgF6k9 z<2s6jomQN3_M86t6R({(#h6`e5nX4cjlki`>SJktDt=y6kPhQEFgTP-Wkp6Ix1L1P{?)Ercb#2QC6uHWUE>^af!96;lqLrBV zK)L#UKlj{>tH3Mog1D z(fSR2nByxDT22)4IiFy3(yX7Oc>`WvIj)X=Ya+&Y#YZOKnm;m7><#ZEusuV=!b5t= z9&@uL2+x(DvxNMD>P1|wu%1M|q7)B)@G?f|6jpXnLZ5V+Qn|C0%s9O0V{E44{5vlE z@B>eGmt?*bRLI6QkC*mn3!Cmo3dytu!K=ur2?Nd5DHc!AtaXn}$k@8zM@J_9HF7N# z{`e){#cC&Xx2?El?^m2a99DQ}Se)jrW}DHz+{*a`bsP`#4!yleQqAHYhb1d8?AdFy z(lwd^Vyq>GMP38P_X8&8ci=8(8$Tcezgt-@YpM<6%@cOqO^2sEvrQ}rKR*%nXjd<$ zHm)pAz=nZc&Nm5I!6g{-`jqW#4cEjw@J@JfVrdMETTSvyMzp%S=4i~>DPg2Pa1TMM z<-CY<#o1R!IV$x4LqyMgxrpbo$~6%XM3lc47$dm;QvbDPK=D9z!AZa9gD%JyO%pDX zI8Ra*Lf35~7SaNIRy3Nm}2+ZRN3p|z^p*L*n7H={H2C=C zH$G~Swl`MaClSwotvSTvCBO3@{|JT?qdMh{3*KmF?7cck`HWQGj)e~qZZ&nP8&|+) zmBD81(IG=`v@`};rCJ*^Q_Mqk@)Y66an{95sSJ2GlhitV?MCRqJ7dvc2DMav4i`t^FJYW}OXO$EJmn?R@V7{sJ_WZivE!JquCr3|8<~k!m$9;J_Y;I;^VUpbQ|ca?+TLjK=&5r}931T&e+Sr)Y*SrDrtTlCIPF2QU3X2@-WdIT zBXJxZS}o}XMb&DT$jk6TKCvZVQXPLgRpu%K%(+Z*Xvb?OZRgHUs3e{dpQMC@fS`0d z%Ne0O%nHgd(R>Ki>40$A^;-AdUfyCm<-z+@fHX~Q<3J#!b#JD;IXbvbhz%MP32yr= zP}uUO@1kx`mW24q07shBox^NgS351|foIn08MuUMcKaexRpT83M^-le%fM&FH)(&0%j5szW_$izLGID^s$T(B##QJ$>o`Z1`)7)dJO?Js$EGoDTX{=AozC%f5YlC9n=^OG2 zW5tIi1k57xa#YvAutoCLg+rFZ;LophT-+q z(CNl3Y3odFdM#p4ANdr;S*tTu`;E3jG*i_p~KUrUFOy?|dPq?=M2%rn7?8KgnVqKFYJS(Cz8R z-`?@wxZsIn+E+L9GD4Yo2?fa% z8}x$<(+WUN1%S#MyR!?^(o*M^p{Z7M)$Vsv-#n?Ys*K$!!F|J-1MyFsPFWUxZlKmA zYYmqCt3bnN#B*^9yu8PBtJcSEteHX1bm9=dv!?z|uUZ1}9ktWQKmbg4oGEWDyVP2h z4E<4Q0QnTvSw=^&aJct8+ady!vkMJ(y;n4c8}~M}gwWa3u@(23{V4d1 zjYi{+bbJ3r$fsnlALAc+8?tU1E-_q}myz%ORPX{XJz(j8SfzR5eEyXJCcs`u&!Om9 zPRf_W-$N++R{Y8c6QAqu=6fxV28MRF-4l`@PsG89Q2QsnHq3+Z;E(rY20l(xmNE_9dqZn-T5EB*_a0;c zlz-Mw7)T36CGM$Oeo^MtJAPYi{kf&k8xci~2%IFD3TsG}Eq{dhS#lI-Uy+B&^u(s{ zz2p?Io+p;Rq(O2IVRyFYG(oM2ahnYpk@*BNYe$Fa=iT5@s9|z^u={A6@E36zs`$OU zQ!CErL(dpzXw5$xK1RzydeZ^YpXDM zmnSC1*I5L^{fwh!(Co_t!=tO}_GcXA(aN$eF5qs6d?02JS%a3^Bf2}ravZ0Q=Xj-1 zo+~0ehus_G2khWgv&7wL@enE0pH3eBwHmLG)ndGTInqzk=S#b)YIi5f9^<)M1BCni zd%y#wN+L0oq6CCq%}n`WU70>|6v=N7n!QQp&?tLCEROITaqSkLP5<`qd$lsjBcGQ_ z)IW^px9ZQ1_Pt^A;!SeSo8M4r(ctdKXr1Xj;&K182dV{{Lnx^K7M<@ZFGb!YsOqAB z+%av(#_EMa&--2T(F;IF{C5}e0^F~b(?TaXx!@Q+dQcn@>d?=%Eumil+2@Cb0%FY( zT;mODBoHO#2{)#t!5W`4p*O7tu5*#AstL|_>tre7tyopw{X&bfGh#A&&Uc}8OI5p< zFmR_}-)vjxIEDVS%hE@1_G6KM$mxIW)^^{B?@kf9Ie&70+q~758?J0Qa$h2heGYr- z)HXYLu0yQFLpO<41t}7U;d`**Fx!ypHC-3%waz`_W4PT+>MING$lh;CiCTaDct=wu z<#*ps;QkTGuXo^NcOQ<#YBGk?w+m^uPGoz)$-0lgGOUmZIxTg=n@>?P&XTr0$#sV?gTzhRTy z^cZc(&bsGcP5t>{RMFv$>j}T^=X2U1#n(br4c%L~=JmqS?~JP%Tp8m*Y%jHw^^(4P z-gd(0o#M1aUn7bCc^Ii|`Ue*`hL;Y|=`564k4<8n$r*w| z-1EMg?7z7sSUkM;B6D9NB6FT*%L){DcTTV8nK%$ySN8i2p6lhJ9V?Pjk4l6SSi|?` zd9_TADg+WHrqV{z5n&?FFYL<`&)MdjQyx@#Je~wfa{pP5_tnp(MBrORB*(5{d20l1l49KUnP2^v13Ud|qz9MxmV@ z`l~i!s|}qIzs^v#-4@Xtfl;RU{PLrU53}gH^i+R~Ixt6HJylgz9#ma#IG+A6SK{Jz z(f}8+XxF)eb++hAv?&a2Tq31-(+h>LN5v#bS&ww+WTdODiZi(y3<)xVBQg;wsnR{Q`7Z!CUFn^u)m_2}zQUQ5|C_AT2sk%Ol^UaGhe3h6$Dt6GA zqOJGfT8ZtO3Rq8}OD_2rtfD7N_+8 zg?HO+QlpN_S9m-CWw}Tc!SZNo@da9G`!<1HdK&iQp7#qY)s=SRLV8t&*)0kRi~0gI z_Ol}nbAxPGU*R|(i@e?}KF-Ql)R6Mbm;dfmxg_emdE#}^vx$Gbl}s;}VF$`BFycm! z<2d|ymdGUK03?Yrutxr$5lcGVkA;(Sf(Fi>s`!lQJjpqo%dGSC6bP25Z`B```h_-p zZ?%zb9Nd8tQ8%L{B%>Q<`FgikCJoUVLT`tn^L8J(VQs zKvmw;nXX|mwJ|MWgRW7xQLgy`ja19S5IhamHRAB?fJ@xv6EGlf=6|PJ(77X2*S2fB zEwt7MM#+0IvO#~|sruvSvsmHIYc;JJ6trHctW^t&DMITa(lDLur3}wOLBaTxvgLHW zC5=UwM8w(?RQ6=!isWM4j`(FdkTntIAX63-zzzgjw&`88n8h=9ri1`BxRgrstMM&D zW^u3%-P`uHo5oXNjpJ9wx7aiix062u>wEmh zo_e;E=CJAk8EE45bFmNAc*foIcv^zLAWr!SILQC8of zy!c%5%(aY?Y#$hYB4=aByCw`rF%sov($~9^Azmzx#c(?)lqc z@-oE#LKD-m{*aOfrQS$%@)asfT$ge~4o@$~?lLd{KH-I|Bn)Grf^ z>=TjYRweDAq)yjNLNZl_mBAk&&AwJY*6EAB&E*e zZxr}C=RD1wLPYw|LD=+a8pPL7&Yy9h8i*)3r4_AU3g?`8~7k3Rk! z#CJ6GBaf|)AUDI%veW(Il`7p3lQ-+%u#)F>>S4bD#o5-+;1o(WxO8h?M&vYG(CVsr zs+l-#{G^Q53X!j3u3f6*WsfySrf6(8pAiDalvs^#e_9=uQuW}{uU!q3?UG_@DdQ-q z;e&&H3G>PLeoVk-@Q!DvJ(`!?LQgkxkr}WZRO7vVU-Pa+m2I>C z?xFYn0%?ITPXD_%%;QGA$M7FR&68E)KsdXY_=_zzNuMUywAhNdb&>G{O@{@y9-4bV z!&L48-})u+9Z-&2W72%_)*(za`bCkQW{oVJ&IEP|R8V&aG-Tq1*zI)u1#iCOU6JJn z?Y&}ZAccE`h>Fz>_6+tOl+IlFO9N!Rxk>t#$cF7+HjqI)R|YWzAdgZA=vq50E;e$M zJfPQ54`pg88&WWOgxf7l(bRE%%|TzFO8I@EBaBlP8^;y2^y#T|Nnf+L zw8~COvyLXuPFYvsM3k*8UNX8e6sYck(C;ci*8dC9;)48tu*(^BedkHz2l7Za?u(Hx zNwQM?5R&=S)WOZ4m-+1=koT!=S@7}1PMzh6?;D@PPWWx-%B+gzZs?Hp^WAsmaHx3? z12Ja%Ptvsz-ecD3Hx1+w5tqIcNWI>)SHms~nju^lS2aaTE6S1(Yaf;r&lG0<-K2hX z)%pAvJ#(f&GCOyLUsc`juIlK--?$E>@eZ;^?hM575@M<**)t|HWumaH*VoWQo{_WW zRcv`ZBEMjz9j;@%^_lha`y>!r=HW7s8FBjNNRHUfj4YU~?pWQ>Fy9Nr9LGNstT(b553Kf3su}?NLj5og#ANr&Hx7KSZE*}TUpa!RD8c& zDK+C&`;arKt#`|UL2vgGjbgZt{hQ<#?!$y~qqLHJ-wKVfZF*1jO{+9f`kjS^gn@c^8Ky; z-;;T6cwX~@XYgueu2WC_fUa+{QYF=|a121QIzJ2NLr?CR2N?9Ws~m!rx1 z_GpP(17T^G=f0_Le0t>f(tL~j-1$@|YCkjt@Hx$UEnLY5$Nij=3A11GN72?F0t8iumm>`J#`k}?_K^8 z7+lv(w@Q%;CnUZYic=93G5HF>JM$<`PR~$s{qh_S6h}x)b{R~9At}3P(mrM>^!%R{ zHY6TuA%MEAnRX{!}*J2KM4pnW&%w9#tA&BNG-9Y&(1i*X|(@< z&UY+0722)QPsM@Iz^)PWW3GXQ-Cm`Lnrl$^*ZEk(r|l^ri|4Ezg@`#&27Y^ z{8{X2`S@eX&{tnnc?x}>b8?jdWmbLi%}PBjqa37RizZ?JFd7SgXf7-wQ+Vqexn|@JZJ1e z;j_rJ>$~8v-!9`=4t2vLlDV~-q7hVJHLG%x+#ZoCYb2b zud4QktW8Hnp;&_MHtcbJFEm0Wg!@74E!>+%@<`PuIu5LV@6tL;l75+X*X`N#OgX8+ z^Meun)vn{fS4Y^rJ@j|U?Xs=cK}%kj*lA}lqr%&$T>s`T6odpvJtVX5>Hx0mlchrq z)5%nmemmHoyCFYIptZhur;~GP_zCQc|F0})Bct!EY;Yi>;A+Qa^HEWj#*p{Xy*=rM zA)eLCjnA4Ko_3UFrF~TX1in%U?&2yzK|@d?Y7&dimN-L${`@0m_Grc6V5D`chO47w+-;0zRL{M4(=Ht+vazB-dh@RH zH*;^_Eh0%A01lSZd$ReHlX789Y@`!*Wx1W)#?F!MD2W6T_U7@AfKNwBEqH>+QOc|t zNfa~)LG4rh@f`ttS({x#7QDRUGmo}N=*?T%uwx85> zJPrBDrVj#~yzbTM&fAni5t{}@*Do5_qU_8`)?W2{jrl$b zR%B)M>aQP=o>IzmoTOHMRw$Fuy0nQH0+ozO{F()?DC>+7jz`Fq9cNj8&O?Dvx=JU> zwPgI|n}L+rvCH}_OM=dX-rHU+p8L^zWM2_$9q%S*!gr^P>v2=FguVEi9;Kkz3CxS3 z?|c}*5%9K}e1~}ykh5*E)DVx?F&k3{lp*{yuQ;UC=QjzX<@rr}NS@P0AA^-%e{yLV z^)~!KoBoXKk^VzJ&eerSd_wN`dk12d;~h<|-58EbS>YVN8{}uZV^xsO5z!jvQ&8X*k%`QR`LY zyIeOJY{&#-J!FN{(!cd~Za~x?&yVlVW0xB{ZVNrcvo|tNj+qFnwwAtzyOy2Wi?6m% zE4VdTcy4i{;yD*kFCiNYJ3FTF?haWanE9|30{EhMh2he8Q0s&!f%I5OD)|vpg-q@J zG=K~+YAzSFNgxz0hE7ls?)V2eQF`gky+{QlD&D2ihzj-Wp=kL0O>cYRzEoYBs*dN) z^U-40<{TjC?wjMk9ueml^rXr!NcBQaX1SiU)4A*#Hb`z; zy)zZx-W)j-DR{l})&8CihiH3W6`>>NNm=6qNDR;_=bo~<`I36{$ucItlSnr*BHhRz zG#u$kdycMl7>`#sg1C0b{G$I>#EP{A0Up_+ zwUPCX@udx=Ka8<&m2_I?#oNq5lVNhHVZJPon64f1DlA>AhTEp$%rSgZ5+qfMbqAMC z=BYKP5pt4&W~PKI5UrcUT}NCJTQ9Btgk(mPukntpFB$!L=T%OMMYN$YV0J{E6NOj< zYK|Rq0kT02xP_V2*2nA_axotTv{u)IBEpY!i0D(T4Bi^DFOfOoj3Hvo3kLTgd7yx$ zB$Z8Soa<2Ogoo9sImC$9F} zb~(0kHK3{qBT8K3&R*$WBxN*w@{4ITKI*Wc8FLPOg>VNVONC`-N`UH_Oinqh98e@M zd*gHX%8uJ&vE5z^+|N1kg{*j93!v)vbQ4yc=lSS&ipSQ@^GI|h%RscQ5((x3JC@{~ zemsS!CC^1x)p>X|1_N#VmIO@d?_J@+8NDvII_dUY3@!#6{J{1Tu{*kF|MU+Uh-2M46RJ<-ZA$NdcVPM;EXUAw#YqdMKs z5;9#)G&~*sF`=+1-p=v@*>^z7_UD}>2i<+!cmS%pH_~gQZc#DYV$%5jLz#mi>Cqee zQO~b<8*URwA$eD(91Iy@=8bi49-$o=7?~>X%oig|4D%H(k+4zE9A|Y;LqYHU{fQ5V z3j2U7RM@|QTIyZulHO$0hV^k;9*{7$ByUjr^aPmlOYA#!%Mon7om%mn=#JKg{gaS^ zcSujzG4~;o#6PI(QjPxHQ)Y(LecsKx_2{?>^y??R%02fdE$eeY84KX1m4bmm6Ok0c z=EV2ppskYnIQ4=_7+cTphd1>6C`n>+DPBzTZPGZ=;G6DPiiMAyoYy|v?7NUj0?{hU z3&<&8Ui|{qYe;QCTWqvzdiyogKg39B_#S`J199M=_rr`HnO4XM$aEH;ZNe0M_BIL#Wnnzv?&ybBi9lRDD3tgT5NwG2`FORY~lfnc$yM%&1=t}J^iqq&KGHU?o>)( z`l-yRYhaWEjk-dzT48THo)S(`p^y_Y54}) zFr`{jRjv4WP7uMK-6@18V{Qq*KR)41t-1(NzKuO961X_^&!2+3JXx$j9{s0t1hNjH z|Bwg#8o;nYrMH6Ef&m>VVETziApVE*;>LYLJhV`?NF*Ic8S@_0 zfk23EH`6*kCUO+?B>?;qlIa1Cr(_L&ID)SDIj~&b2fZcBp6Te?%?pRqOdCj^=^_fy zG`o2E727h3I{+0|;Eps%RVbyLfTs{u*+rXH?5(FGPM2Q%EFQ=E*#EYOsPjwPas@IK z;{txnDqtRxDPVq5etvn?BrVl7L-|atBjq4J%Y~pGr{a?mmyCNCeS2Kz{vb0nT;N@M zn_!N?CF98z5Gr;@sQIZG(Bvn968?FuqhOaVyBLBcceaG}Mx zW5}u?>6Oln1s+Xd_Zc$rw%5293GLD%wDjfIv-pYB1bAA=d(dLK`;`~Q$*q+Ke>%zV zPOr_fCq_eDD^g+q8-r`GM+pOf@~@u}*P@A_c5vbFuJ7w^LY#I@9LL+YKw;ObTvF{l zd!7%QuaXfr0M{HYtIg{xcMm=fbJ5Oxu=lvfJV3o+7U+k>=j)F-^@fYY1g|Byd0UeKya*NtB0}zG(JkL-MV`#zOb0V zD|8n0&id67E0)cjYNB2@DtVfEiVwsRs#t#LeK`L2{Yh2$3(U)r17FR%jjzXu7pRVc zM(sHOyl@I`z=j%f(47?4@Y}a!TLm{>sJ&{^)Fu{bA4Gjij*WWq`t=(t3^$hBemqTc z_{J98V3gMT(J0N?R+IBW!~Ws!7vw#-&Z2E?yMAe*T;G#gfZlQqK4f&&uAVCm@rxQ# zYrU%O7vb2~{ViA`L~O0Ed(ah(HPvru%fWURx%AckNM;C8GBw4c-)^xF5DG6Vi9EaO z&)1OdomJWP!pf{9$rc)p;WaW5{aqk(*RU?q9`q*)#$V>z8&H0o^j*3&xt-XMjh^VP zrJwV)ZR%i|?84XNrO-a71DCKdf!j~-#^B5*xn;Hn4|1b|{Jf1_D`$V%z)GMll!Ke% zY5{s}oFy?mGQyTeC@NIq5-@-)?V!={i^m!>6JGM{$lN-`WM5QMRNtAVL;qWZJz0E- zVY!0y744<9ym{YVA4jo3clW42M~zbgxdA4LY{aRLCd4c8j0c+ zi<9DUP%&Py*lh3u`Z3&HvP}vy9+wnf{r#m}XSt0Fhu`o{dL5-oeWwo}r@t_E{aXbG z+?Yt@=IU?fQy6@wj>+Y>_m$!PHsFlSHwx~&C^JP%FW{auPv5E~p*@t6QgxDSZ()C^ zf;qYG-ns)?=C1I!JuvyBb!^M>bhCLwwCmjJ-URA2%YK4n3M&~WOMip)kb%QLl_)HK z42$6)^sCCSZI*#7X$(h6Z;#Be2fSDuatwyV{@#!of+zC@_}_jvQJV+% zGY%b)nqKQDZA)$V1t?{(lbQ!TxgP0;H*}n>Ttbumk9V^>gM2ui(YN1odUcx~-Zt;l zjj0I0S1V4ouq`!Tzyuz24)#uP*>c2lJ@boc=eAo7<(}g4#vWN==@GMUK!NJj(d#?j zuGh<6kA6ITd>R6gA+|XG5Rc`7Go`m^%i8m)S)BzkAG`U3G(0F&D%S2V@);X{H|C88 z=x!TeG_|Yhf%_7ozPMO}(&_JOTM65Kx^&2bwk=r?TFIz$kvw(K!=0xfF+?tU% zi+9s&q=2;sJ%X^gN>DM~PN-3*3GUD2F|1pYOik+JkvLZJFz9RK`9TS!j~Fh2P^St= zbiR0WO4kUtG2A3Jr%>t(zfJKH%<|V(;DL$`wS5H=fabqJfx8i|_B!vET)&v93JaTe z`n_7DH|xOzm@ZCkn!YSav*Hfs9e%MFb2S2i;2B}$eO`epUW@~XjEFrla1nkT7c{3!2Zp1aM~7!*wTFVNsGqukmQ2;EjP^*Eq)&?lB4??z+H4{^!Eo#X zGfVih!bULOp@mAyfg^1#UfNl_G*7MGzkrb=_U~$M-dh-%=x^Z#9biy9zxhFLo&Wc^ z)O6$$+WOxxFy=Y%+EI@96|w%iA|#9Ugys3`kSkvVxV82xs^0dPJAQ@{vYM{F5I8=!&iWK?qy4^N?BHFMRQ zOCN4NAErRoF`SFkpFIC)jsUCl~>($tVgYe4#G3^8Gm1*Zj` zFvT}jd`>X7dKUa>b+ zEal+<^tC`jz{fJs6S400s-hRFeiW##VAUl=XzdWG17){+k#&{8k3X)=ZpmIu9Mmqh zmk8OO*84}K!_2p^u~{}%5o?S(`%ZTZtP=QEu9EZ`zZ35vJJp-ygl5CbuZoCNS6r=f zm$<5@0%nO^!mw&O4;9iJfIC6|xDV6ea81{YMD&RYb(oGh7PfBY&IS8^TbA_1()(iF zZi)cg1BPKS>-8oLCWWzw~wF8ymuW+g2nn3^W z{{-)0=ATz)&YIs07VS|Ty_x8e%2Ho<^5RFm-Db_k8OST@QzU>MmG=~0PJm-jwiDJr zR>4wpDe}beNHtH@wz!g@4G!JRR|sFYXq3w?Bep{0d*7Hyol)BT`$EN_2T-bS+w*Nr zl6EKov=w&gwLD0!*MH5O}(u7pw8#mQwZ= z{e3^f-Q(NBoZQs9=Px>x_WgGJv_OB{w=jYS8;N6v&R*SDr@t`+g1>pxO8`uzvX40Y z0g8mMihC~DT0x*A9wDVa_+vi&g5$sMOW~xP5k6C|Jh25?`PEC6Z#BMe6jSZR*1y2*OP_7$f#yg^UyHq#|#!KqSsUqk|{yQ^us271+2Bd1u3{cPGbVp zg2rAmdOT0MPvWtc7r^oaac1JDhySH+)^LSz0zA*x)uhlV@HXi>=k|_gWJT_H)T_!(Nvw(9+~+=@n^ZeK3;uXu+B;@2SSfx1O#Y z6$W2$&fvdL?B{!VAWPGXg=vltliyY;uvWhpy;;1(=@-f*THuG+23>)VrU zCnoyhWcS3m8W^+ZzT21qzDr3o1rtv_JmQSbF}SD3|P!P3JGSGL|24=^SWR4 zAz|x|Y_c3%Jy1E}Z!sTznN2vO6`bmp0{B8OE3`pAG7r8Sp{m+BY$xMr{x-edrQs85 z8|CnXQBmyoZVEttKL6WR`~L!N%m_r6Gm>}w@^09X^4-xcsC=9MB||nZ{mYHPH8I=4 z4;dz`sdFCRtMFg-pD)}q;#EPuGqDZ>pvnW)vRo)= zWzT1l)fe>TVLG$4KbmD73HLZ9M6tz#p!d0kFm<{9-BwyKY#VVJ+cV}_Ln!_I&SYTt z|a%H;+ofvWug|=QwDAS&(d5;E z+eYV6KPdJKj!Jw@G_)Xk%hs}721>`V46Dw?WkCwV+8!>UMQgSHLE^_VO#4-YQh5=# z8eo|%L42-SG~_ByUNr9b9MB$O3)bvoAH|OEC-!A~|(|v?xHZukeu4SKm3>n+UyT@$S=*pmXsy9^i;I)7bh92Z;ZsxmSM2qg8 zDHtx!yjSL$qFm4vgrDFexv)T{?W{7sdXRBs7J=Vl%g=KS+Pgh|^I+$h^ZwULCNVuc zJ8x^sUUKa{xwxD8X<_F>0G3<#O>f>KLX^G}?I*77V20pO@~@5EJNpGAZXmV!SBR9m zM{&@au{9xF0V;9lt@!r+mhK7|CkUv3v#hLET$9Y0JQivUrvn+KUf+|;pzNGq4sdb$ z&U7L|>TK8R+Z4Tyo>Thm#3Tth+K9gXxz(%@w06{Gl!}nOTk}Z9?vR{OksBr5la6p7 zr$~v1KOL>09U7wy0x}7JAUDrFaiz!5?iJ!PjH2I|D0wi4dmeo|CjkWNLN#5{#M`Yz z?XmGAt#9b#7a`{;6&DEaM z8&x;=f`n!fOhZ1+)Cx>2I*Q)ea1S?s;99iI!1uUG~ z7Afy_8n#If%#W{N^bdCsO24Wb=Gw-CP8pc}DB zZKpLVY}3KVfuE$e3q%L%ONQ)a-sIutuBQ@{7JvJHtlHzU7kg_!!`o>N$w-=Kbr^a^ zT>3_FH7jND07JQ(1Rot4H1g0EzAwa7Khi1{cZnH130VT!@1M*^i5nzMmE9?()UW$3 zyR<-#aOf9N*jpOQvDLQVG^3qsk(G2E_#jB{<$rYIc`_lTl3u8XyW^Jd7$3lGrB;u0 zb^BSBU_JoI{{P$>HSIF}u_MSx0Hm(Es=FApJ<_uxn)&YT8~ zpCgVJ&M<&%lh?HB6qgguNCJs@_m9I>eoRlhOtYoDJqaq2#0uE|dD8cPVKH zAz6iQeBMCLcsfFN_b`RzJty$v-~$5UU(BG8zeyCCKc!r1zYb&RB5}%saenDT8e}@( z0BF&PVjaICNt6KQ1^f}QnEUD7R$Gg}q7y)wDx-ANU(Vtv{XKwEgAXVZYf!y^E>u9-}lGvKjN$AeVyAOSw*5q`T-#bp(5hV|5FzZLjtsxVekecs%LC2 zB452nwD^f`ewW!Z``_tLHyarsF2w3Sv1)(!8@}+t&Pi`@Z-E|NJ(v&jbGJTa*;0q; zRiAwKbuGsIe)_VLeI%eQOFJnaY?)hd<)Ywyl0V%6;`5)@C2iDw#GLr2*LNJ5o08f6W+UB&Nv&K`k&4&-RBlUXnoN$o89$ibZ(P_sNjDuj{l)a&e@e2GZvL>Gyk zDCZfy5^hiD0^?t>!_)A}4R6JfEjf?y#$qvt2Y829YazqgR-@bIo}PyZ;-p^h2~uY^ z!vB^jwRD&zF8I=(eZ;}FDfV4{$+E-iQ$ojw^9ao8lZd;CG7-41ih%ohv`IwngF$y9 zhm~VrVMe`Xng|SRGNGcXZ)W-quTAYw?eHHVjrUQoO|;?dzNHHu z)&#?xmzU$#?3GZ=;ys}ZmAX9lK1ss@O4TAi=!%luQ$*~wcQ+{VCimJ|e)jdUvC!a( zy=yC;{NlU?i#hO{;1eTc<(gunN9RrEKqWl5??KCFI`PPvEKW3Mx!mdi+-R`lgUMTd zP+gi9U^Md0)-imgl}jE$WPD@scYASsYE<2|Lzl%{xR(+@c$HZH;nyRW<#3HniG%Cm z%YdR>SSy(S2KNVS@|e$}p?^_2P=LFsZh_lMlqmlD0A|%3r!f=hmn)borpPTJJ+ z83yzY$+wW&Mn$T|L!-k+6O9q~^oab*I@ zLSnMn2J{^Z<=!=_ z(YAMJQjz#=i&QhZ=>P9}ACr0{50_mHr^3lDRU6?;26HlCX#o=>N+vn^J2I>~zGKjn zI{uBNV95WrAQjAVp}nm#2krfRQc@a*8R#}RG3=n_`>OlBZ$j0t>4#Bg4vi4FnO>5O zB1ML0m;dY);FbV^{4@r$yLsiiRRLTxMaMIc*RK4|wQ@1> zo=6+#suY6WD#>3g(*G!`I_3bq(Ak-x(MN5DxjI2vAYSO@=Hoim=lgpEdJ8v*@EEn* z$%{^BW`Xqmqo+aQ(YW8!lru6WwV)bwb%G*atIXvPZU$>5hm!q(M%A zm5)iXC}HQwKX4G;-(d3P{Rphp{+q*%)k56_TfOAbwjppS8?8*L*0z%r(d35Y^_<0O z8XJ3r&qDv&Epdpf-TqbBL$9>0QQ7 zh#`-*c~9B^5RwSEtu+BqEa__^+}s6i<&&sr`K6;*(rGvT%(4SjWm~yUw?8lC3ovgk z0KMacp8+A$zq|$MTfmoqrol@aTq%3G9V*i&vOLp(mgr0I$XHjqM$LGChBeybgC2R3 zdNbw|$dBd2K;9az?+av>&du>7Ey2s{u-h=)yS$@sBZ09-b9%ye9<-k`ff#bM04 zHJ(&=t{4-#9~j|!n_bE)L*=O_LSVHGjJW=O`V6!Z1FYLXnsHmgPYLu$q3|}M9N%jK zC$km;0*IF<8DX|nbj+K!BMh<6xujNovH4_kG+_1vS`Ap)L@#t2@aNI8^_O@intBh* z077QS>J7|Yp4T&+xo(D%wIu-X@P;JX&WWH;ni&jil>vH-FfGSBhjt)=1=lW5`r#Zy z;@;_fRDZe2*yVSVt#$Nv*3o7`*c=9zD))UL8hAYNN7P3-33#P7%BaeisRuSS$zDQU zw6G&H_+d7Ms{n9cS%1pRph$!wi-KLueWzWH#+*`Jf995afZhCeMnK-r*5jH-4Y;Y~ zP$7NOslv8Z^m(<8@G|%28sRN4VgP9lj8M>U81~E5-ndXf;JBBWSk2R%-&y3hKHY+lb%}3XeA+R4n#mr*FU>4buO0&s@={Po5=Ata zzsfo^Uk0~H!A|%Ozvk@{b0wxG)oa9a42Z9XvF ztr0zg10!SU-MV>2Ik&0U)PZjDr02(#`I5iu+gz!5SYr$0ot-4!lqg)({|_Vq0)eIW zW=!6Ls4%CH^*+i=K{msBUy5feGd;Uh(gOgj3p9h8AT>j>FJ1-GMJd`pi;5FEeEtSm zbanu%wQkVnzCdOkR!%SgP@BY01|$e87hS2hYSpt}Xlt_bKepXL<>hgD0Ki&)?q@WZ z2h9YTIk;pfgOE@EClY8}6f~R)LLk6{AlzAs8n3PEztlY}LoV?_(%MP40dJI5V)mHW z#f6%S0!dtz+S$%!m{qb^SB19a5TfLYQ0^iO2KjLJ>zl-N1b z@x*7w(SeT1huU<;x8lQ5v79neycoZRHv{x7yu9ng+3tJFifCjn>WU{m~oL-xoYW0-;5+Pv4$pHImg}$?VY5vaVXL1R}G`;?Q z@lg4|TPPBeX;8hqjxRV2uWH@sF)w%15-O9p|9>@)HL`yZKtq|H^6&Tssc@HE0yDv6 zV2WqdaX!`50N9Xku~!ra@cAt2rvmi!ek5*VwQXk2qdi-Aic5OONs?90)e^i zAdhT;UUP9r+C5dW>C`Gi6NJY9=?L&90kg$Gpi5%MUjOLB%|D^#FxH64Tb5dt@GHg) z<#Hla^HdM7;)0&Hc4#Ia8q^#z_QhgTtM2=p-{MbH6f9GTp!TyYqAMl6Tlpr}(%EC) zp2z&)+JQO_+MMn?lS^MAql|bc@pLkiGeV< z4;2E4lhm+I+g*DTByC+JanC9mLBd#4V0{1UdgId1=AOYTXv~!Xf+)0Q3kimTQkjMz zX*$*R`V9bHkdy9g^Z`v*F`{5;7$ltWz-*~;M&kEO<#p6Y;4{Q?S2Ih3tJEZxBUcVYy#8v6n$iIWYzV&q#`8W`)M=5FyQ|& z5DD|<|M4gPJJ0@qOfG$UcYXM!gZtZVtG7Wr)PS|) zpHQ(~eG~)?!)9?b(H|dMQo96768@C{ViE$$_L1rhn7IoEOOt$%LMs0hMD4)7032$6 zIHd#>Um8+2OU)d*5BndmFAW)j-cuzsjsSN5(@Sm)@|1)J6Og?>4+aibw?O~>Lv1DS z#&(yecVKfVN+97Sv-r3+OpDSl;H{Uf;A>cwg9qqIJxp2!%m7zh%5dQ3*7+caUqS*( zEjLcS>-%NzXn#ISPP^b$Pls4zOh$~e7*a8iR&y=mZuNHK>YO5xJm}8<4VRo4BZonW za@ou48=egJ|BRPes3%D7ePo=8WJ&?Nu@gFD+`q;t_-`H6n1&=#KWRq=VqHZ%8$8@K z;jXW#P2SO*8j<{eaz{4;0PWAh`r}KTXTFTtBVIv_#^d9;E|!7ohTY%vIwf}fZKSz- z1{{{0d}rjXs&5{27=KXm^*@)v(@&KgjmvdmM^FRWjp~Sazk*SlX!w``&&jYizu-*S z*HL1kx&KU2wP6#8%wv+h0juktKQ!`8qoxZc+TUbPT)y(p^`8**epYPl6lljXE8IRe$(At8!AO&Q z@jt)5m!m42_>aL!z&WNAQeJEC?Y>-AgfwYOzEkZySHss_YfBe+_UY=A1x6MkX2yR{ z4*3UH83%3UBbMMG4Ey@gux&ix~Z8O`+Tf%y%5&1*aN!w+Gb2o~T==1e|XCAYp zVTZu8+iQ7idQ?O#Gk43XW_Qz!og6plRFkhs|tRUi*Is1oJpsS{Em_S>~?Y-6_JaS zM!0E~yDg)dDS%VOz2kpxj?%;Ot6o9dff9FLNfml?sRZ4M-DXpH4!k~8fv@Ro)JC*^ z9F(-sWBQlzUqna$Nie?Po=4TjD7%X1B`|zW*2Wm+)rA8oc>c@w{;X-V7IQHG!{eNd zZlIP8ffyoCGEveg&8&(fQ9$Po*qnO#^~6Vuc5;yujRl_C|LKjYk7+0laz~!rT2pwr z?m%Nwfeh-M#zQ^S)!NzY%qnk3%-zU-#a7aL?z`AYt$O)Ofm}fp)VfIx7IJA^%3uMP z5~h(}YrkvTIa-@F417$-E%ozbu@NpXJ>x`DSvMyqq!eiLSA?{61P^p+SP=O}!;5Xg9pGytU?(In-t!+WL4;kpH$AA61Yrdw z21n=~N-^^995=;OCPtha3%(Yd8+DP=KabcCqdfaY^!xCt&uFPKi)MdJcHfxkekD_S z=Jbh2OL^iie!2p)s3yfZ!gb@1vfSu&z%}l&P<-DvLo-g>ffyZ>Gr5_r0W1)}b~smDz9wp>vVhq%0EB3y^- zIL;+;AA#Zl$~K}u{si8;5(C#8=W;zP_pwai12Shh%LufV#`rU<9HsO}Lb4pA$z^N2 z|GdVHVL9fT5LHD1Dl3|1Wpiu*l?BZHh?>VyHWFb4dl3(uKE#zZU9J;36u^nOt8jv& zLjwTg_ws*jLxfFtd&eFbmhCSafs{ZkvQ!kdUMO~_wzZurHL-S?d@rHkkzd>2gcrT* zPwjNT^n{8kA|m8sVD0Tg!OT0cqKw|IAw}g@@{|S!aN4eDVEZ&vs5q%82Mhsre3d9EJPeIXt~n>?i!&@ z=||Vy)*&$pGb|_tUTrys5w3{wUNTC&G?DaQ_wVnc_cfb|Z5uk7O1PN-eWE}#pd0Be z1P&Gsa5_eh`unHRd6#-B?Nf#o2*UhpOci8IQ>_~K`HZ`5ThA?fR`_h(b)GVAfwVP{ zF#yw5P>a}&+$$GQ0?tuDQv2SbDN3+9uc_<_V?0jM2vAObuKgySKQq7*%eh_MhHRty zH%$u@V>*8}O+??Y(i*=QZTmN)b8p_@$jHFdS*2Bqetdz(Q`hBq&1Kbou+QZKrGd2C z$r`i--YSLAwqNTWj^{#JzCwFE%2Vr6_EXg!^Hpk^o#^&cbD>}28(w}BpAz`-tg27- zH6Z6*^{WAw4uFD`AUvcO9T!&Qzna?{Pxo$DyLatLwum{0c2SbYr4)l*)1bBy;^%oZHmU7&s=WF~EB&4Jf*ilVZ&lnOAA+_Qwd(G29frqDa z(yo9OBbZw(#fh6|Dme0FC{1;g)nG!JlOPZLbfoBsG#*fg7dhr7eG*maqbiHiy?!QK z?V#6}OPuIlyekKGIW<9o<;MZ%#>;?*WyerluR$p;F7xReb8%pZu2^HKjH5=nlm)*6 z-N`(htmblun4Sz0`jMAH9RqMtz{maz0EB{q)uYpnjy3>e@}7SYPwzinbMjXong;FS zdAC*r+a{;6dv;9TI9^d#Wy==zz{x?l1URbkerZbX(JP><1jyq}M@^7d@(`kJ1H~{@fV&-&rifxclKvQv6UpeWTCV!iJizKl#>LRr{p zjdRDSo}2w7m03F=WlU{ppB2%d5iHJbQD)$Jbr%mM&Oze(5`Lwldbt#kl@wP4~~Yyn1R z>hZdm5q6s>GjwhMA6$lZwPj%v?w!ngzTqD!rCt$5E_y}U_NKR|bZo<=q_0e|bQTG19SP-mpPKH9^gj#~oG#U@iJycEOk7@cps^ zNKYa-B>vlFy+~*l6lfwsHMtBj9FZ;8{A;QmJ536xYOq+2(QOesg$09Z0{8#B%R_~g zA-6OGZp(VYCg2F>ioAgW*(tJe++1Sc96pIjpoML|#U033QNx=of+eCDgzJE8 zd}|TlZ2nPIokp9}UjFGw)j3(o%L!W)=MCrby#i*$M4ziaJs=xP!X)hZtq^+A<^nc)7p3Vv%%F8nwVAgdiB81|>G7tEN< z?hu!euUsKKsWCXvz~>n4OgLrsKNB_tnuY5Z8CD;vco}BY35ysXt1WQAYG^wGgJ~l9eK63`lmrzzFbM=xv)8!p zIq<+N2L8;RY+88+#g`19N4ZH>E*uq#Fw_GP5{Ow=xK*pX;yV1SoWi}ei#N=>SyfYI=U-nlkgMoL2q1)y_0~hZ$#X`@xjUvhdDVUQLG&!pjx=(1>p!r|Wc1prv^YeopXHn-RLf5XqV(6hlhBF-SM@!O4p(S-h;FD( zRJ24?6v%Dd(bG(N$({)k4Y_YNp$;5)Rv-! zbaxWLb?495Mw1*BNx0X0$EF*VP|XBD!|odRzt$WCRZ}hysR=A+EIJgRgueWG)A~Rm zwsd+`{g+)O;YvZ^i$#}1gZ(8DRy`wk$*x8~Xsx_erYa8^fvTTKtx*`0k}#I9dOtQh zvB_ZWCs(Y}`4m-MO~h$B3D&v|aPu z6TV2~wn$H1%dvW<*Ng(M!|lCT+=)fJ3pqvlg4cw9oLy6AOM*BictJb^@}eqD9W zIb@zU>zH&jYYpm^T6M9;q=~OrkEItqp-};EoBZ@u3RaSD8Pi~o7BBV18uniGlQh#y zw@<3?v&GN1Z4Z6^3-onrgN$V9p`WkU{6PT67}U&y{;d7Lxyh9Ea{p`-x@TS5@>^hK zAgTTiv&uW;HHgWF6?+k^G)Tk~UDc(+nk=>iMD-c?fQ@IIh9AGvgzFf_Ikzl4Su_y3 z>-&RaX{5CIE_e&i`w#1roEkKpnI_ulckmu^Jc-C5HBE12l@jDxz93P{TJ8*4PZxo~>&#}`j-?O*70P*M_Wsu# z>I;D%E+h^zx7E*nRj+X``g~5F-Fq_Ra<1MeJLf7bEV(f-&8@AD>cVjOY<+-}Gmnwp z0uUU%f&Y%j zMC>Ui_j{!=19V<3uCKmrz@28lgG1(hziS}6tx#yXko1Nh{sMiG#KGzF8IP-HW#c5J zV}n+GF9=JA*`#rMkyEL@vAj%aBH6(kzGE8!bEwb6Yyg9$r3cujuKR)d{1pWbW6crH z$y7Z<&t;dKK;(h^VG4@VuAGolSPk+9C0C|L3B2u zM{jutyk49x_>z^&I#S&|GwePsxv!a+p^~Gxn%HJg5(;ra-imenF`-2eTIY9`AWS?9 z9uK0k5`LRxsQFinhxt>!C2Nq8I^mghFIC+Cc-;&hk2(4hw-G5x1$tfjmPd({YqPmU zs+0|6ApYbV=802kye{z7=vL@T%?Qv^%C(UIj7A0(| zVlBZw#`*Pe3uHC;5AgF5L)PHa<~FZa+X@E6vBOoKms0V#n||)>_(1*%LUbeG-J-!c zTwNZeLi_&P5PXagn%Sl>0_F_sFmxu#+oFQbe;kT64-KzPs#O+il`CQfOH_?~E>JvR zIcpr=tIJyjwnMZY`UV*P}i;BD(WcKO88 z7ypXz@cvx?v($Iu3PY{e*?gy11Gsv~8z~#0=kk+&C1gPO&q%2X$=qy48j|9M#`W#j zs}HB2_KwgTmo+sj`UX+qxCTtB*B+agF~Bt__cN6{i)kJWfobz}rgg*pBcd z=wl%xX1-lN{tmuvKi6wLNw7_R9#{AV z3Q%KU%HrF2FvG&0H7hvQHpjFIu;07)_FpkTCceG$?o))6R|dkbPlzA3C)7`S(%Y-o ze=9TaGfukYo?83%--#VH9ulCSF?yUeK4Ng(dZ~{ZI7YRXa~v(=+M=qr8p>STYUx2R zynSLV$a;}z=NefJga51VMg8fIXLYO~Von?DKRunBd%ztAFay~2XBSknAF1%h7An&d zL_B?lBI|~NNUW!10lexpk=sqL98FbM&7~L`9j&jDF58{f$c9u2w2J=zPliMbOq$Q} zvfcu?f8=+<9JJ3JXY^4j#^j-KhlUGGVBkQe-e%;y-jid@FLw52wF+$%Ny03ees4ru z*X8$SvKg@fun>f{YcV4U zI{r&GfS|G$S46D>oV#mJy8?jy&fBF9K)N=WSuf_!@EpY5)mKz-qkWV6h9+xLRfwC^f+h~|BiotLEUL+ z@$2rzSct6DU*OV_f_6VV$$g3z)*8Ed_I;Px^PQigf8qNq{{NAww;(S;YN5pQ|niJ?u%>==WuG@4mM84{qA@ZAwHgCP*(>z+D>h{{nR6^ zkx(q*qeQ^(v2q)8Z*LO#n4E-v`7SWJb$zt`{tv+ zMH%syo7WH8tm;W=m4ZyRvo44W-MSj^dz6ZUtP$EY0;wBfp(bt8YWhCweI>5r@A0!x z6s{dm=uYQSr^=x2g_u6OiYUChH8RnsP~{Br)8E-th8@Abj??u?Ih#c2I9(c3??Z6G zC#)z_S<48{ZYIZoVx}>g||-aX4jJ zho-iyX!Ti2ZYXzJf!%7xD?NPL3%6DlCWo(mwB_oRB|@U20chfejdj-eW7iGk=FstL ze>0Xk6awow8tpw7k}x}k<$3O9uH}O6Bn5Hag}UN)5ErUtTn}bQ1F@A&WP{Z4YTWF) zY`pzAt^4ev>=IyC?k-oH_x_5MNnc{ecI>g8B0L zy&nV;#+pDe#k4@yf==Pfckvs)!H6yH>Xily^?#?$&g#;&*)lAh5V}@3TalA$P?aiM z_To@iPXdri-;Ua#%Wk(YQA_n}V`ZsN#>Z#^js_q4a`@QT9f}0(hS3La>iN9HHiC&f zT;9J8-RXKr9-0u~n7{;6p~Z_*)(!f1b$q~k2bcZekJbRZPDcq>{P6NJqmwY={+>0Y z86mi|O>d{t2o3_MqP7MzVTf|AE~B%0o2W0)F<*y7pRX(55a;jo7N6+V&-|$nrhx z&o`F7P2x3*KxC4JIuK1qF5i%}f%L^3J}C>mEk-ueDE92=@dHrJfgK4Lj^}lo2P0jW zUCUy?86}xDE206!vm1Q~-rO79MF21W!^V=@p1aaxAm;7VnA27mZixg{8%9 z2z4inbiB%`Rj~Y8f%@s!K+Jp<+-O?H(AplQB$DNCPW-ExkT7FI*M*4Hda&%&f;3wg zg|@HHZL9^nIX?Gul!I;6u;6$uKtbu~Y=%g(oi5FjTv!*c|x%rew zfCTNv3$;}_#`kOD-7uX~puUU09M4}xr^lZ+7;p0=8}aMw>2jq$VG)LF*zLA6iE+2kR>lhcw*Vn|tou~j{%9OrYtE+o=nz)&mH-+Ocu*CD>_BK zaF{yUf3KV2XnY6neZO5gj`K(zPp$JTjWTo%6@|nCpfUOixSfOF=UA{L8fs^FU@Z_7 zxmH*5h=xeuBcada>$U;HmFt*h+lJbOqf^re1v$=Y8~fxjMPrtcjfvia>31Y^rHu&m zM4%ZcuSH;v5aJB*MCtOfmC+ONaTdAh{M-7dtGn!WufNq9aTX_@gZh9>jsbL)m}3ug zs?jf>O88WsWe0nk3*PUjUZHw+#?f!LynglaClRpoJp+&Y{(5iy#~3kc%u!qw#T&^s7!@Tod4zmuR_`;4)tv7Y!F`X(V)G%aMs1 zoL0wja4w2V%Tvpm3tH04pmbr!!9=dG4;tdBk2QN{4^FBFV6Lk18G(Y!KR4@AgKRYI zCqzN%m{wyW3IPmYRLM3f@Y!fy8Fv61B0EWq*YD+G9zb*&sJ5HAn$K?)E(3_>m4E*n zlujF(`eFYlY{lJyTB+A(vUE5%`BC@|)T6L74WkX}0<;J9GwH^ZBzsoc%&-L;vQWq{ zExPGCggwrmYICg|pj+-f{IH%3$6}Wbm{|S_IAmLXa@blxYx31+$36RvLRVWzY=;(` zAzNc&S(Axam_60Y$}}l2xPrwrFa6$J<=@Kt>*Me^i9%gkv%n{`6!5zT*?MDlYn+E$ zqF(rprYAT}jKTbnr(~jiN)c2p#KaYfG#Jr%hvwHmajZsVke9%5|A}UP1WDwXwyX7) zPn74zaL4`N)clyTGN=}Ef!mgBQUWQ(zoo5>Go}}_f3vpmH+P>BrHkL$W37169x8{& zTPg|q00NXI0a9R|*#M{cS#iC#_$|0`LETQm_%O=`FeA#GeW(LmR`|J|VrK}uW?s4n z142=}+D<}D-7lfMed1gcDcEmNQWF?amGSic$mrXY0~QH!^Xo{%$bKeKH-;A^evlj| zVmldKEFNx-6ROqcu`_7dv`R%KZNRPzUeoQ)k_hhQ(=U7S1a1~dl};@_z>zAD$`z!o zC81%d_hWPLr^O3Ov5P#1j+#?kI0ic-eaFPcqK*-=J*T7V<@z|7GsFDQzWk@lcGm)+#SRrJIy7tqwg?e3Npsja zVC_c@n2Uqrr0GU^&K?0!#?l3N&NAaDqRAx@Ytx4fB$rm&A>3PS6K zSWVbejNjS&({klUL1P1aXiz=m{6iW_f3&n^ZRF-DG!kq+*w<*2ZO8_ z(tiEXD}LFaq=?&dr_|*aOUSL&)y@p#a*G?+I(OyYu_iAerV}Ntkk>0R4Q8{k&0^n; z!xUA6jsaL`11`fvZz*li`NH7T2y0Lm?)ZvX30(xG!pFI}8-{A-^He5R>5AMr)CQka z(I)K7MZm+JH*TnV#nO#ihWWq!M4T$82N_fD&UwKudO{{~XQb$62Kn>Dq9m>Jk$xui z3D_F!*a2(x#!!^qiWZ@;ib@ya?rAebRjNjOjM1k~_vM9M^@WTn{!qW7;uxJZmpx$# zA9{z%nXC;Ty~#TM?Pjo9qAcsG-XX2-Qo${aGBj+vW62i{F(xyRLZ_0g1dF|TIaYreNC^3-AUUc!+vV8Sd`ykUBt-m5lZ|JMXMRsEu7wOknR28 zp{$)O&f;{5v#lsG0x8^miYs3vrz>*hC|H9WyCjzwYca1AR5YZGm9p3@nY*hHwOa>y zp4S!Jx7jA`(I*qaK2wEBHYpg)YIkp8wc%}B|OQ(s~CBw{oAY*wetyo6{9nQiz=7VCC9>-)%FE(C)0-iEv5xyox|*;$ViY znPf+R#9=* z-d2K9C6a#Wj%^JA=St@U)X31qptS&RY82;AC&?eus!CNye51HWysU!S3=vc-OA zrJ&Dw4e>%FdwA`z80Rq)zmiGNxd0&)VR)Z@Ihi3Tz4sNa(-!&#eG}-z6r%XzyOqj?izIxJA7^+eb{C2!yd2VE42=;9p^CoPibqdl zlovPdbYab(NBA&AxQrT$MftBpvSuproJGJeqb?_7pai{5ma?6YWv}CH( znh7*XQ*oSQUT*bkO#kj;RENJ%6Qs|FQYD?%#DFydH#JRQk&}oRAL`oJF6G>oJy)qPuD}fJwtEoQS@5e}u z)AWtXynYFrz@CXMJ~`}AxvN?CT1)2GlQR@fNuBu8(?OES?S-lF@Ul=v?UP=qw6cKi z3bB$;F6g+eR1^j~n*K zeC{e_c-J{42lEv#%Dg5Xh4~V+-ml(p+%fRT;l?~{d-Ky;7_E@QXXLCJUVkr!1}W?? z?;>@x6y=hPP?I1pmpzywwymb4+dCi zXw~G$D1W%P*c^Nl-iV^qkfUkaa}1+k_hj=FUsgYh&Fq@Ef3Jk9bipggcomXX_}og1 z`yj4qb6Vsw8%W^V5F83_&;df^$h#9I9~D9VN3rl`MFc1@4*Wa?%qUnxZUHr}DJC)4 zinrK5px)yBg8{gHK#<{7`EKt>9BX3I3;tEet<32SwO4{Q&tl3No8|5%)6Z(pC`)a* zBV4aH(2i$owq#s)%^Ry321JB+mhbj{j9(i|xwGMHdOC(*AbUoy*DUuH%}(VZ*1o*k&LqzO-3rnqP+)bv`Q>xv0Eq%)JLp z6Yoh77L#e^Li+}=mo8uCHu=)A865kJm}wD|`s3bTzQqu-D@g+`R7gVq)`?-xinXM; zA*GI<4Q}p}pb&kr=zf247ZbZ11N+s+6Dz1Xf-l)k$!9BSnOAQIQKHFjw*n7o#B@{{~d^lW?yJs^`SM}jc4JCSPsUx6|2&0R)hVj5mG@Np;7g!b;5zHIq z364B(8NC91lN$HcQX^%68=Mv!)(oOj>B%NG{7V~a7)VBNIoQRHNT7I9>o@(jr)oT_ zr%r2Tam6st5~ZtWJ|_UY0!yuIeKhy`>_cWTv8c)#F0@Vd`c;y!bxxElIs3bD@3Dd6 zR4UPf{wxlOH4d&s>XGTzmr{5fS;0n|xecAiTyE-@6%5wsmqx{QmjETLw}&iOeYI&{ zIAr~Xg+`0ni-l#fFU~D5C`^wc8xut{;`Ya##8NLu|522va!cVVflC+JFV5tgif}aZ&}@dzVYd+| zkHw0ceQH&x>^JoWcRVJ5--7tK;*T*+w?7Daq}MlKHjBBf7>cCMY!Tgew3pufH3(&9 z!5VERt?!khNJxs&cB!4z5sPD{t(81W#S!;|@6@F`!DcORB&+Y|X47PaA~h4ZKGY^^zN@pKJuI5a zb1xfySUk+uH0>B&+&&TJ7~=oiWT0ceXs0R!`YQ-fT=Dn&akEmVATejH~RQws#+{%#vOOvYjJ$j5Z%L~Y!KiOFw*xQS;}?sfgrbq--*kfVWIUUMlDUq>9TF*y zM)qN(o9pD7K{4Br$K}?lHMn6?6^~o*>WxQvL zTS}X3{vKz|%TwJu>$2GJ?d!pDf_T*Wi_?BL8-w;|3}^1eE-)iuI9_bS;ey(q3Z*V- zPZ(LeF9S+nuaMORP@wy(q!oNPXJGL6fU4Q_sX~6Y&mx+t~T+ zMgCEx+f0sDanf?PV{%8Aw8@wL=@N~39FsVAT@9p!J&7WCeBPBfh?HHsB;0|;&TJmB zuQX!G@um5^#?gq}G&xfgIQvco|4Q51x6_7I2?_Jbo7t*=mu`GC^JRV9!q8nh37{XD zSAE3l%x)rTUK3dPAMqvl+l*w=tON>=L#7q5W9%)qH-#{c3iDB(XHY8NxxIPyki-H; zEls-$VISIPinP%POSWP>Yg<@R6<0}tXAw&m5HPFm+*%5{+>vl^#KY0kg|-XKUx8wB zx92A=AhIW^KDy}%X`9y)NgyyT10@uITyO>05@G2t-3TCTxIhl=hv&;ZV;YkRFC6}u zV*5p1LR%4#ESnf#gRA7*JkW5{PlQUPrHywXEg37I&G%Y>qtu*VdAa$%rOk-dr`^Yc z6OSE$@Ifp>4;XCHgQ8N>Plur2nX%jPe{0;nx0aw&e(!+uyMf_}l}2e}TE*eC(_(6qSg4G|EQ4RU9+(s<8$--(iP@n3q3()j`#CI@ zYo?GB0YMexANIvS4a^sQ8TbiBReCw5rOP$u)Nv`bkT9Ly4yX<~3jVbf2A{Iq8XFk& z-QKgNxQJirUtAL!NX=%>=uSm@rhA^Vgv}GzJ)PqN%D#1OAA#`8@)y8N$2j-5TgDs5QzxwH zFTwSz%n({<1;UPogZE?S6Oxy06#5c38Qa=`zy1xX)XPT?GY*5!R9h5zgi9ABS&sr3 z;1qXk{deby7FvKcSgV95u74`J-umyrr&1j%GdXKqn7E=Tz+e5tN^T?ar%#o8FWGA> zA{KCopzKKKrqmMED^91lgE31?zf6<~#;|$wA5lOA<$00%KOX|DR$BqnSY&_#l-wm) zfgc|h;4ZZ`zXc4cyBp5EgtX%c={;3&RJ>dZa5!2WisSeKD}y~2dd$by_d2fPWg^V= z#L0_V?k7Q=118D6n`fqZ;LjmlzzG<*rPeF$^&}O(vPXI6ti;!bec6UgL4hNH?3@Wv z$bTv_OFsau>wX8?g1|3(4ZQ79ssm!y4zKuJr`OgoEif(QJN5C@1JV_zyV1jhn`J)G z>c~F>n3~%kvw=pWMMJ&A!Ns!F1)9XKD?DocKO2exu@TfM{ZoJOPqFE@&%kE&>^o@~ z_6m;g35`yZkq?mOJW8AHUM69p7y%28Z;n5@oNy`n@8cmc@F>Wdh)aHiVcJChCU9vr zOnbm&=sTk;L@&PvFzvS*KwZZS{GIN_moc0J3)aVv6j)=s&A1>o!@9sY*Y!9i7f2gr z=k)(L-rs}3THLpKQBN#?%hCG3Klaopgk!blp< zJ^bn1UA7(;HKybBy~Tb`NS(YY)#Jw^>6{dbVY~j+JRl0Q2>U3W?p3;!&P=Fx_~#RL zk|W+o>hzQa)@DZ-WH6p-fXco)$Q5ge*9U+?DBPwW$)g584b`7JEV{$` zhZi-e8F)k-v^#EY^j#|Er(flkM+R7QFgVtVaSb3$bhjemuAVMD(`|6)iJY9+gQ{d` zO_+L@M*4~46GmAn5+ZgWS|<$V?`mD|RaMOF$R>R#7{(q`+72^QTcdKliXlh;f1;a< z1dnfd+ccAyEZ?nhV+I3{{+3M&m@cVdvd19D;yEW(ql85xKdJV+MDj}4396`G{Vwr|NDxT-OI2Y#sj7W_XRUF}>QCJ<0d5L`HfM{TwN zVLY_4n&_Df>d7$8vTO5jrkPfo&{wRn&37J%4X(73uIxT)N6F)C++uYlwW-1i7s)`4 zo+(C_bPfC3z%GSt(sK3BTE95xP;hx{9N+vwf6Zw2mk)HR%FrHZYceJjhDM4~t!@pO z2W>B*Jtf}jask~rSfzwD%S;b4(ttF{p;p!uSXFFLLg=k0FyDv3GF;HF4eEL6Cow@- z5TZbnvp_@p$Xeg)9lKBXWiPmoPnQq$&Y|`cnQ^+qAdX8yTOU6k2iqt4@LU1A-j660 zR9_L%)bjiMtkIbRFT0YO$`sC2i_6+1ECgSI^AOjD(RG}WwphvMKp#g%6Zal9u8crfzlXj|V)h(TJ=$bd?<9lH{-mgEebr=i^o5pZWZEL7 zY}KQ+_3-W)TieMJj4jsCKzq=^2oWhjB6K&QY~l6DEn+2wC_YZ%+47$7l!#Q4N`2ML zdhK&5!-^{1lqy8)COV~ue0FedfU=~E z$4RX}`G6A#sE3$dw-xSlk1_m09w@W>we`IE!K!+5pYS#1NwZ z-lR*T7~W}{=V#_iq*SCr;kSt@k7~Q|DMP#Jk{LVCZ?`zU-J8i~^SJl^1(TqMXYoc! zN$S^apMKIGTo=i%MBZ&0*sndW9!`E?O1>w*GoW3xrJ0l_&Gjq4Gv z#eAbVy{`*hWE_%ucM|8_ha^_AaupG*Y0&A+1p}rzuO7+#Q~9?kZ&NOJJzIiEv$%wK zIsB$Vm|29=Gt zl+?hWdOK`o4`N8fsq^7=xkP27#unD!69{BLqwm>)*~<-w>3X~BEuU?&G@GR%``@8Y z`z+Umbib4asxC{yB08XBG#gsZHHMS-$EbpkRcY1m*W-pNlU6p*Bs%n!$Vm#ieAZ8FP7*Z{6@S5#mDa(2qQr?|<}^QUSHo!{2Tw_{$Za*9 zx!OVeTMD#X7h)=z$ly0M%g=>(;|<)qRz03nukQ3MOwSW;fKEtQt9SYME_vWxm(;oc z7t;pJGVUZj7kLnj7aD*)J%Ju4^^V(>l^duO;<6+*BHc?Ntxc_b$n~sLA6fNHEOrLcAD3v3+tWIG40R6U22jIs?R_C(Ti}@UA_P>_9L4MIAU(Rs- zl7?$cSQk!CFa6ci_kgeuP_UNGF-j7K;w;yc70#( zU7={LcCQ6{s!6YiZM7%M+b6+lA&%f1b(l@xzixd?q@(#qpSzGJpWz7FX{S`9E$6a1 zpB}cJl|4Jqsgtc%e4}^(8USM{aMAMWpz(e;1AA6LFVJ}WTS;y+tQj(Q2yVf*5h3aT zn=?5{Yw1?x`?l5@$k{u(h_(}4(I0!!wLEgFvNOozqr+0$K~{*qgc^?jMeN^9OiXNS zG0Sg#)f`Ls}MiJSIJ#QT%?y8Ad z)vg>7wuib>n&3V)RDE@{QYiQE=;$$(L-lb(YwH4KY0y|+47Tk_6kOVrMkfJR$~jyf z!H?;Ce1qqUVieTZm+jqKBo~u1mo+{6$fhw{x9=e1T~uz&bocdO2L7SE+PSxLbE$#GMjXjig8~@(s-siHpsFgKrwZ$YrdbbaueMScs|KL#euSi{`o!11fJ^~QMMVWI9RstF_KUbg zlKq%0HuzMOZ0DeTRn`mX_KayMsrVT~w{J={g=mPSow)4oFW$Mwo~h_GF2C1@-}rd9 zXNDsj`9cFSv}db2V;sJ)sy&%)KL3mx%1ARobqYe25A2`{-m7c{eSenzO{w@n_bW(M zsKl_V_N>yy>!UYF53Ahymp{My-g%Cb*GPTL`A7Yyd~*`@yKzPO?}#K8e*3Qe2L1e+ zhfC)(7?WY@vNL<@lMM^KnRJZYcRg#fb8~HNY%-F_=a%~R5_soi=B47}3ew+}9x>F! ztmZ0=tNgKx$4d-q0w*V!%75XLD^fR7Fl*se8{C8vN^CF4A^Z9z@ATQ3%{YAuquZ5l z0ob1ynYmoNR47eeW{fHrex|7+n=ZyJIXOLRE5TV_PvDzM2MO_k>ys{DXU*lI2(QgA zdlTG;-u4rbM{1(8Y<4WK4U1A+FDP`%GgMaTWgUt&o4d8-7O;?E9@`ytbq{wfdb_)` zHBz6Fx{_++$<^Lh$_HGG3!}v;@emH>f@eg;5*l9 zEl7${Jdh8|z>V5e#7vipO`4KLb5Uozc`**?S3#C5et3zYhys z=b+B>FKbW~KZmjlEO&JE^|#lE$GOnZpRT(+wta&S3N}6!5%Tp_x2<93nyIbh^**wY zq3OT(AKkyhsHhz3xii4XLdhHRyxDYlv3`xx6J?vQTJ?mFaa}H)eEsvN-8=U6Hc<>!rvOOK&T7(as8@L_7p#$SpwtEQ~|O zoH7>{X`^BgY<&i0#TzMdl_=d@H!+Opn+0-=Lk|N|*cb=1ACXX1?VM@~5;3aY9VxY? zJ$vaTKk2K?d*p<3ZZ*9ni@l}3Lxo4?o@|2Z@GUEG+tDgoMgT?pXq}EzG(y`T#H*ue zK^3?PMvpr|Li2>I}+N?NOIT8;IlFx25M7^3RuJ z;#!1P$2|5iZH!D|R{Vg6BO{4`q2+TtFbx zakg!?H8Rsg-$&IF_6kI(VRwO6OagAX*A$6d0zuc6%d=pKNhlO-7`f^yw)s>Y~ zJgW!$3yAV#!pdeXHi<=^xUxT~R!|{(m`r4Mn$B7H;q>C&S+UPveV{O1;a*Ht?CN)Xb3cbc=oSZ$;7rRgj>r`_$mjqs?J z!|yT!!l132#4e$Osu~YffBEv|AjVKqQZiRNe{pefe|rT!>x8B|hQV>(@Nrb+R=h|- z-zYA*t}eQ_{K81bxTr$|U*`HCLgRg4mVi7-!$k}KDzt?^iH_8jATk^pn-T2_vQ^XX z_N|#yuRkWzL4T1T0>*8^E1s^l2UJ-v1%X!ASGQmj4N2p8hu|7izvF1OK#siobq*)! zw#5(4m#N!_7g?#ws)y}z)Tth!mub|jGeXtoYHvm41gB4hDT8;stJoE3)*QXw-Yz^d z+^9;j$sg17YC=4;_cf$g+O_r*#N3+AgY4X@gFRyOUpg)Hq+9o9%(lmfsH0(z11=%g zCqAw}1g|N8H_9s_W>bq}$*-5~2L~tKEL|J@YsGRcaT3)gx}5&b4io;~L|Oe&3^$9s ziwc1k>}8^oy23C%Cfaq?hF)KU__rb-<3D%WNHBglUd^!BWz2DhSyiv5X>Of1Dw!dy zBVd}xEqQ->wV$QHfstxcA9;5L>6W=>EImh;B%85WOjDFvijzM30H)lF^t|MDgBQu{)Z>tdM&mn18Ude+K zK2G*~?DRn~*%2viE^v^B9?fRb-YVVLZLZ05AkZ0>Eaw+moz({{Pwg*?E26YGuj z>EzH8US*hW+(-cK6H3UFqhnlIerkRy=ci=;=&jopdCleXd#f7io-L8J zZ;F!du8bjVC09yXpe=F%|sZ5i3>~R1?)c6 zy+Wop)J=$JfpY${iL$jDf2}>IF)p_tHJ$3!*2?KUXeaD{&3%iN7r(o$&$Az7Po*h3 z2W9+zGr1&6w_}=>g~YirBDkDx|7Th-0WW=TyTG4t0>3uMzAQGjYekKd^+EsqQGc#kLrR08PMx#C=s=pZH$x#{MTN?27rZZT&d+q` z_olR;{@l2heCfRWBX?cgX~hI*P6?|3JuzOkA^ly#tGxx9ui#7Z&Z|Y# z&$6}r{8E{Wl0+-5W-r5)K0Bu@x7BAs%)Mv@#MNaPFQ10@XXR+K9WQu@_STmx?t}w) zu2T3-$IM@s1)+vblnd!%WNUpV=oQ-Om%v3_tKmyhi7n<4QQlJ9#7x2_uld{KGO4 z?FjNR!#VZT#bfC+jxata?t}&(>BT?B?z&%#w3I~Nvl<;nTozsRzHJi}#K7 zrhEY|j&CShGH1$hu`rDEfY7feP=vO+@rL zwRjK8#A5kTDYE=6t+#Qp%_wwI@x?5jsVK=S)8PQ=rCgNv+xcO?XH~p)b@{8Dija-! zwru+e)O%a!U&9wU)L;719DLRs3etL1Efz5H4v7`<^W#%DHHn)9B0>EvAG9R+9b@ol^n4c?s;Of);&=&Z!?R_Tfp0tJ_7r@)AhN(xQJ-76W^s93mE zV{z6TrQ6R%iYo}ML^y~)+^8D}z&ebaIpXC^(A-s-ynj5JLh`8gDDpJTyZ^{g+TYE| z&v+}!26UI6YN6d1H+XgD-TR=4<(W6wUVGg*{omC7=GM;KjkD6FSF4n6)43C)gP}jwE@D^zocP+9Xj|VI* zu&u(JGZt2b?)XH9)!4~gW^SN}wo-RVC)_ zt)x2&4=0)nZAL49INcQ*wEqz)yB5RdZL`RJCh)u62(m?@cO~Qp{xxU~9T^`+6Y-uraYUl_Ncd^udQV2RMihqirdJP(rX|&33cz z&YI>>Rn*k0B1Wgi4Hd6U%%J^!>>Jxi@O4AmZ!TJXFqBtEd$6+s+dFk52zf|!*&jwi zE^!~KasCvhZgG_%;#P1YCGCGC&RP|DXGy-v`MMJRIp=uq)8g$oa!2WTpiRwQ{|RU^ zH!jighJUOj6I}BQ%8{M?Tr%`=@(m^h0F;%~hTSA}5438g7?c8BFlaS~1BoPEXt0&9 z;7vzD!GnhF!@#508uw%O?Y$v-xT=ikkaZnkhIpJSTd+*HFjnmlxe!Q*5mYtl>Mb@c zRH=iA|B2kI3r1qq2s(S`Pb-|x{K@8I&rq@|r(OH(yrc_7q(Ic!ap^*>*!~NBPrMUSiv2TFK=RnkM=$@RMoR^muBV#D@zbGjlO4VMjxMy6PuHaId2dz(U z{98z|E0yR#z}I@fxehe%=QQTS0~4F&=6v5B5Z@99enO)KwKsoycYp>1cw=j>R800w z3$eWmtoJQ)^luoQWJQ5>L) zT^+tQ?bh@!s7*Fb8Oy^>deK}rgI)V@ukPeK=XjWpY=#U}GVk5F9ma6VvP-TSDlX)R z4s}Cu-FHIpiVuVrBirQjhmqEOOwAW0*{yA?eUL6cFc^)0=GB8`FU3|_B)qH3+oU}k zMjK9kAh?O*b`vEj_aq9;hj&ias(Lz540v)x$0Y}KEk6$YHCRM0`<_&ycXz?MHf2GV zJntku{yY6oI;ov9-_0v4i#>mZ|3$onAPfc*7T&ACxUCU+7-V~0zEvVphtKF=(^<6D zvCT`kIdhqr-D=n?{dk{tXE`Jg8fw}iF0kiJSsPBtoSq<-YTg+$Ka2%Upzmfy#(I#(~zh&I_*=a zfA*joIiF{+`7_JrQ2Q$(1xN)jgzBJ_w9LVZbZxq0KvDTGn>1|TKay?|$F4=NKtm^n z{C(vGdIBXruHhGdCI5zZ{!hF076_9Vj6`+|!y0 z$Dn+Tn?qYwLp`tE(NmbXN%{!7wRppr%)Id) zw}`jL*}2gb{(?_!E1%*FC)`UM39R)Bga5KY!Ac#U`{W-3)4iQ|U7odj3SO^bRonWs zp03n+vt&s@(3-I@TFzUOnJ$8S-B2p&OHkEMg{Ck3qNIfZjMXyELcOb0eQq|Lfycpz zj|BB-a-y~Zno{+8_}=(Z{Y7ivvKm>2cqv3BHvKs1Zv4*)z@(Xt?t!GJqLULjrRZN( z72qAtL_e|}hrqR;kiyB=XrR}ij6HLLjL1e+_y!a&OZqan0ac$+82Eftbg4qNf%+nf z56k4lO?fZuZ)uk&WuI%|fHg?cu`P`+t#4YJtNFY(ZV6ii>|aAZ)#U8tyi|7G7-hpx{%kTQ%( zQM+{#j%e3Iep06nm^v(!Qf7*;#FV8aktOnyi0@oklp^(8IX4jpQ?Hs!oX|@;5Lfg* zd^Yp^?Q=S)d}@gLa!^*Tv3uUfTulpyC??FumD4;B=L>dY)-P?ORYzoyJ~^+cgB$Gw z%hhR{nP+HPkn1i*eqL}8!J?$g3HKB&+zq3Z>R@k5We*|N4I)_Ey(=5f&b&bkna015@wzDv70!gqFnP#pj3I=%{V!#)*gM3Al}9*Eh?yea#6|r6MSot;`pPN+%*TvwZZW1tCgUrc2Jp;4MTyn#s%48cX4v? zNua`gKz~>5`!ANwOEt?}UwLXzvrY&jPvpWPmq#6gzd2p-NxxcV&bIq!?x!x^bk2h; zoq4tMJmNSQWzgO=5^woWq(0-40avigNLO;PHz`iY&A7&!I_X<zCDx z!A2p(1staL_IyP5v$0xxbs>sp_4xu|Q%@*22537@`zPF;bZ1&ZayFm^zKsDNEm(^n zySJvIM<7);L2JT?0*io#MfAP>uo2yBZMRCZE+-gAzW~0ylq@BT)I_|q)=7KqKYk?|zFeES zDexB!6UhJ9re0F~j$oT8v?W7bAPP;5K5`pe#e&Q5S}z2-@pmHsLVEAyRGNjZUbieo zOI6AgXj9D*3164WIKLf|Cwm7SrhCxkM@J2xr5&cYVW_fxk*Sy)LR$$Pf9$KC9J56- z8QyK9cf2h>#Bbe_I@p{yW!20#ZrQnGRQ!Eyv`BXUC1*bOK5PM`vuLurU=B*(^~x$C z;_!vFmGz**D7^aGx6X*pX__9z*2K(F2XatMzJA4%%JC~Yw^kNrn717_b2;6XU^S_Bw&0o*ZQ*Z1}-yux+{ z9;Ltm?gj$cD&y+Qs9Pjt%uffE?qm|q|2PRy8?4ksdt1~!JrICNxF6f~gfjtE*~d1y z1NMd(*Rz$Q9rluSW8NMUrC6W#26Q7uW5dTUwA$nNXpV<5QJQA~)|U1rG8@U!*=>uv z>65F)vVZ^ewMl)mFqfV%rNk1+UqwxcT%g~IZ*}6HMBkPtDd&;C?pDV#KrK&JCH71&As1u?Is&2-YF*jcy)ry2Q0JqzUt9VuWPI>a}Y>srSMU~+O>SlG> zUDxv%{o1ITS=K{k4(!HYK1WpjQ~8C_ZQrcBI)6w7Tb^j9CgFXq-o+Z!;C{)YVva&0 zGw)J^kJqT>(K6G9O+eELKa}Yjl$Mc^k!x&K{Ub(mNF_>vk!GFGkvpwZb39S0B3row z*;u*wR%C_(V{A&aJmzvDF6Fu!9d(Mn^7^zCqY>Kmqy6BRi{FKk8_;OQEYCxG%^O!jK;ZMpRXv5IWCD>ogG!{Bw9rnO~+NK#?07S zC)W1o?DLK#ycmvk{-vGrs=@AaESD(3tI2N7MbU?w`pbv&6Hhz!UJx`iAw>bd(~0zh zOZ6JYER`?hZxG$$9a}jleTeZUJG@rZ@!G|icL19FhU5DX1A5-}1?Yk|-7*lY3x{N_ zGU;Kx3ogE@YJKBT)ANf3e%78h9JxKEKX;)~jD8&UqVHX3*1<;!_ zP`rwwTqszrK9K{sK*yGh266)<;>2BA*cCr**xo_Tmi5v3F}Z4UGMX_?&dsfS$`(16 zq%*Tr^tzTj^(cvz*lB~$d>neHWfZS%zTh)zp?)4x}XnGcc6jYlx`$T`@QeD+$@`Yk4OiPmVB3V@;ujrl|Gj^2DQO0ScKajk$uk zUO58}>G_dLkfyrq#lWfYawJCr8XnjxS(1Nuki)*KAf z^#ltKjPR%+3^er9++TxZR|xI+QGKtjZLup-3k>htz>MgUXO-T}VwqZgah$8H zY0X?aML(})u;-q!3NMl#zgYEnEM9#RBQ=t~dp-BSYtUP-mG`#HP!3^=z&|PyyNWAv zCDLC!mMv2{Dhpa`IH-qCmBRaE8JlqVdC9p)e3LSb?(GX1h5;AK{aS@t{iu!ls1+8GP*Y7mm3#*0kV z;!lK87Uj9}Q|Yev2UH+4NGFe$NvG!F5`P{)r`>k`q_erlZnj#PH^lv7dbhmq#C(al za;YXq(kcn?=uI{H`XiM>gSMjm&I$c>RO=YT>0gYWKpVTx4BzsMAi>s-I+35ahdGZt z4nHaktkjy9?Tq9k#jx7T6Kp6S0l;h=LBk>mE&?9&YMXx&<~5ube{S zhu}eag|*OP5{$Ix-B5CaM(!jq6`GQSH6+)`?Y;vV$D{%|~bNIfC)5$T(H^b%zbQYFe^!$#& z7u!(x%De=kZ|hKkgi3a`vfSekyd=z42n*(pmiGY7KeqrYqTHR*4B4+4N)|YX@+{dm z9ZhA-V>q%dJT4m^x3zUOlp1sOPJzEz{faOSMje4`GP@LQKiQ&{5qr%ttlHmK)2=Pw z(sWI0c+>lse-wYwnBci4Ir;2}tBhse;J%t-*YuCrTue`{W_e4+EQg8yHYa7)-!EDX z4X`X`82MHD>xVUGag2HBA@#?K{cj z#!NZWM2XL%w|UlX&+oto-v#Wz)Z<9JeBwKIaGp*tg2JXKzBCgTKL7Sq@bS zx{aU8#p{ny^eJM)u3bqN&risLYVsPH{6*GzdnT1boT)foRwKW`P4fKk_e+geY^-XMMSvzOoC-yJJdlFqQS^Q)R56EI)^6;w4( za@yoGEnA-kIuy}&`|)Oj8ZP?IlEbdTD?YakI!Cy??t7Givbn$F7m za$iX%gVk3%Ut)oaj&dX4xJ-;!9hL(k3oFdwAX{!iZdFDUqBP}-B}jrh2@M~#P6>Ic z#n~zFB$JnGYmR=&0qnw*Kt9(D8pPWf4Y@~g-y?SxzuJ#`J$bGXss@xY+IHVqxKlT$ z|GEXZxrp+O-Omk{>I58u)6HXogf;wrclt87=gvsmPx8kEb=<2xL7G-@J!G+NTw5#V zpQpkl#f+$kscAVA1u%7C7bRx6l7;L2wFBMcD#?N7Fw^MDJc1Hn{_MchpYN}=d&?>S*35cTW}k}?cv+z?-*;I|+8G--e^c>cd zC8o(>%xN~TzhPFfgQ3u5=DfWGC-}3sDBy#w%>Rr&40q-GRz`Gxf}?8O{xx z_~C6cHZ?HZlpJ%uIn3*SF~5IQ(qrz; z7AI~!6Xnnf7w~pUIG3e~8W-=-u<3kD_L8OC@K=4} z7I8r;f1M#eV7*Ct$S~uynNEzets~t`5nhfl9Jzt55MTUc+_D$;p_r{K8I`u#K-*)% zv*rG)M5`4OZzHw((}Fac#&`vzMjHC7WI^`FgYbxK!otDpY}ZlTm1rcVlic>xS<)Gr zIM6S(sdONjd;62>jiCYZS2A;Xv|i}2l>@51vdX4e5kL=ZguH0KWKLtAYTNSobDb_t z^TeeV7bSWxHTJJfFt&kZT+zTzfO&ler0{%kH8d3d+MqropC=cpE>=&jL^b=P@=-R2 z<)`&@eJx^#>EyNdE}v$43Qlwlx9@-O2$p4d=Gxl#etiw(o7>#i4UPZ@j2tq^eeROr zr6&ft3kIjvdlv_+`VgNc&tiXlu5Ze3Q@=nX#s-oDi!uI+f{RL_OFNhqcrFd3`sKr= zuVf;w?I-FNF1RZd`yvHA%m5i_^+7R{PnHe!`zk`-x?0k^XFz{11G4mZDg*L)LLe|v zsr#cI&U%RsGN(KP1aJ3)K#OEe>Jl>Wy6wHcxwsH&w73i=9se!9q%jkj^C*bHeD{1a zaFtf4In1)2IKq8s`BC#tu5t66i0<7cEC;=6e#C69+f@nGiY+GMh1Zk?ng`m~0oTd= zxOEnSlCKnSHD%322tG$~>vA!xpgNe!5Z4@__7!`L2lUMV*=GPUTho!dMt-hTz#|0C zV^y(GvA(BcufRk6h&;BR%pK+Sdc%`8R6=<*?g#Axr7f1+BAqu_qS927bEDivwa}|De z>@rq#4OfIB@qv1HNjh0~60&5;r9W>=9~aeu45TkxwVx?Bdz=R*c7a_mfRN(_5F-(6 zX#6Xv`!*^sW&>`gCcB z+D12BsN0WEdx-Iz^KlMow=@pz?=E${n%-`WR4Y9+ea_YTWz zay`;PL|n6nLOhQovBE)Iv4@Bz^^xbd=fc7KOWES!KNbo%$o!I7_V^QJi=1+WUZJ-RlHIaj3N%|MLZ9!a|5; z(JY@K!~$&y<%a@-baYMnw@5rqz<3-n2yUpiz)4Id-;u14QbYnD@Lay3<2Y&wg zo2E3u$z=t{0T{4Zu2`P?0kHR%JT@FeLp^(0p^P6S!cJCIgdpZy7=Gr*zU}Vr%*?=q z_vC+z2TG^Ho+yt{V}!@dk2Xeb8XN~OmY^ske2@&X-QH^carD+fjge80>8ghp1=AiV zELU&ew;3#2b2o%F9w*uWRsV4+`cP+2WFt^=%(GrsUd{0C+}E`ZC;Z&Hv8xhKAlJ$J znDl7(gniDUdk_eeh1WUsY7QUmu?N4xI_)|9PR&S?bpT`!NtRP%?=9^wx2IjcTYjO|o`{GIFTGjc}$weuXXN%rlz%U_a^CdEp07Bo4JPBPG!k2)ps z@l|4?>fx_g&K@CbymlP|Y)-X&1VS|74MJ>avzw`FsM_-H0|({M{*NB4REIdBvkvwO zA`Igr&L;kD%U3_Ii#d6Ug^|BswGnqDy!8z}W+i{&irq1Smm4p4@OmmvF3AMITrPRs z_*WNXBJbWh)tueDb{~8J;ACrDCpnKCBURvJ(H}zlh+<8`bSftzGY<%FtDNvJ21qFbGE(7j~{vdc8Vq>D^m9 zu)xI6WiXnae~Up=d{PX%8)vLGpZaQb)S|?asLf;^g?je@0SJbcLa-r589(nLrh=^9 zF4$<-h2&0Wyp|J@W0Rr72LR~no{E`&NC1f$hSVs(>Ua#?!dU6VE&L}lBEh^~g_oqE zGuV=!PzwquNGlMXnuj&URc=@ByR3JVI^`V#hSF7TTYoka=w<1EeEmV8gAguFVA~Yn z1kd&8lr$xrhwU-sq$y(dEpH6y5CAw+Z;GX#xM`I=EN|S2yukSLpuffEqsPojm4QmAs)Ajeg6&X!L+8?b;P$81(IqHdvUZuBUP+7@30k z2uLv}eL67Ti?Sr~=eHN)mM-@rqI{0B(bgnbr=@-TEGOcpsQ#`FKwaNhR$_Jx3`uU9 zyB#3mw_9c_xL!ieT|MfVjU9?>mfNq1CEv5S_`&fc$gw|V_whfG+Q6m1hoT~c!9%HNRGCyzCJ_>549Mt` z8^s`zgG>|zAXFfIT#b3V^zRq0m+~Ec!fhiwz(Cf>p?LAin85H^k;t_nu`TG^5=seZ zF3&f?%JUh>8D->X zuU22X=*bj=@SZfvd^Ic6xpdf*s#t!xHUw@lac!1;PkYw#0uAwmWPZZW^HX>Z78rr9 z@VmC8zU+HKeE@bsVPNkoecZI01E?3PlM3u<|1(5yp% zJ@d_yuaqNNQ@IP%ns3Zln>@BqDZD{e?UmDE!Im>|h>2`{IdEC2s}B@iGEMBe;s|uy zOH~}Z!tdy%hyE^gjO=(u$du<`NQ1MO(l$H9@xEExvx^*;NcUkA zEcAgGpvMZvz2E*Yo!E^sI?RlBzIwkD9G?O6dc72b{_4F8x35tomc;s6NF{CfGc>p} zCyT7ULAF#M7Es^&<}&z9F$S#z)G!epy2qv~xC9H}1S_PIE&rxhb_12;4LFp03;`yK1I-EOI~R&`5cuF@J*qj3ByHu>7fA@4oai0$@G6YltX zv@b!^OYyIgDd@SKBJT-mkJASkIsNkbYmx4Bz1U5mfhyRXuy!s?1WG^M!l#$~9 zt=%UJH9VMr43iq`;OFV%l4Wjn>F(4P18}a}Q%p|TD06x$Ked|AGA?=&L(Ug+E6tS9 zqH41~PMjqpKK{Cksj-DFJ!VV8$0f${@6>UoOFVlakfyto`%QOU6h%V?nn14h1+M6L zdh${Cjom`f!WHKq1aul6FIAWW6b598wv~(YfuL2CHa_J-A3TPnJ;oLBxT0nHF~18QFH;3~V!XiD#&I8|9jYw0(e2BWlqkJa+Fqgf9IRYh0_9y4-T!PGX1m`^z`TjqzK zW8O9ZJCpCR<>B@|dS3txs`n~sTLOS@gBvtOXJ9QaHlbhNu~AIXHs=%)^w*4cHZXhT~Gu#?TsnyT2<-ep9BwR?gCO~5|SE`cJg;n{bE3Vpv5ZEIG)od5H0W}4_F7f-bM*S{jI5w2m`Mvc)ARR$LLe9XH~_Y>5;HAJ2JY2z^?x52 zev`EEJh2d+mm{zhXR;j)YHJ>-JeMkL3y%jzyret43WQGch)xcBsP6gR^>jkGp(3W(6ZN<%)K%EuGg;PXq@^E+YHLjlwT4 zQw{_}QHTOEwiH3^ETtlDKeYwDOyPQMF`&67Maz`9zeK@qs3+A=(Pd*5Yp%$@J-F_I z5~1CdIyw4969(38R@F#=8xkxc;+UdQU{XRWy2diB&8yKndqR6baoe ztf__+pBMsAp)&6H#fjw8llVX}Zt6ci%u%q-hl?V$V}-MPjL0dsz*7$bfJMO_Y$hNa zwahl?0Rblo`)%D53i^c)20YBlvE?y5)Pns+0}K7I9W zstQw~_rF*{e!>AU4tUf_)agBAP2Rqp!rinBi7|CZtY^`{J2n4Z_)|$^l=tPq9??%= zIUU3Y5{@npEQ6VdsxJ#Db~R)`Xow;I*@n9U$$_)isI9y>ozRMp?XyEmekG*1$MfQo zwVQTI2gRC0PLtCc=CI~*e(lX$46g)cPMshau!56N+?8iG9U!C z3^5UixjA(Cmgl;mNlRpfp$dg``houcS>cSTi@XQlwKB}@HRVg5%*?|fDRW92v1!3E zUr{t>!vFDdT*B_#`SbR1vs@nXO__4%^@Ye+-3@ zFSn|=!I+MW3;Yb8QJOD3NclfMVwU}Ba^-ibfSJ6JvXm=GF%#75|MMOX&M@_G;2T*x zH}&*e>7U{m(!7m`M3TX|dp=Av7&E|MBUAb6=ckvVaKqzc-&I%=ABYq@jU= zM)?2x9}fm1-U!ur0?T2!ugQ>SvQX@WUe9OUKjCgR;IlPd; z2u0j?wLPk@RORRs{;p7v5l~jGE)_x35e zW8V;a{h&H>olpp65tyYwcQyI0PNBtg=Z`<4u~YM3vEw=WX*Aa(!4F!*lDQ1#Vmg!F zy`}Uoi4OaUcDgjuvD){dcE>Qhull_P9_H#sC36)e16uQi?t-M*K-H6^&z*OSG6L5P zW2BAifY?jmP^b=Q?f}lTY^^h!Ih+sOhP%a-cEJxm*vsO$px_sjX2yj$i>ey!-Vnv-(JE=+&|9=CVq;YE zF|SXZiSqIDd!2Sb^WA>;euQ%^&&oAdMEwVgJ&9NDvihp2?)~8&14ZCFQ+?^wsz-k# zt9(|fr&Kbly#4D&RU3{Eh4B9jU0xBgx^skSXMhXCXr6(y93S!~t}zXy#!t27JW1-} z+Ukp{;0|I~GI;g4!oIWDz91ndlqW(0QR$H#D5*(}+L>j?juAw+^elLuRuMzDVW<+1 z&Rs~b%_7R**41Ia^7&`)1|N)}aCz0#id84TD1$|Dmp?e0&s0tAT5?R|surh$%(}OVkC@u35d57)^}V~Kxo-`#0*qBA?byy3up-ET7pRa`~z z8`W7ebFr>sHZ?ymLmU}aW~ZA8muHF3}Is3ZBwH&b!{82$tDX7 zr&GGen{y32KW*0b7u3wKa#YYf)BKGQGI6++pUHcadYU$qb1!kq)Ie@7>d)Usk0n*U z1x}98elRV|SYwmQZ0$CblEE^>3op~~p8_4-t;$sHB$I|^r>g&R)r z_8RKEC_`&StaST;O8rQ_J6C?$$GHUiO?j5ymZs&X z$IEciOy=bnZ>~{Jk{A1xQu(U~+FD~07=qWBhS8%_&p>|oke4w-iN&^W)lc#7pN2rD zUwd~vm}_sLb|>c<5&1DtdJ%HEJEvJyB%R%#-iROM8|6(u4i)9*3UdHN``w zdNs?QzVjvW3DnfYiLZ5GTw_Oi=?cUgjTTFA| z4+r`i8RSMJS;tLB@6%X=R2_g@ZhMDV2&dV20hY?39d->K&uCvb< zZL|2yR37L2vf z{MwnlWD!!u* zzaPp~#f8L@`wT77+f*eSt5)NKXPE;LwXH~+XS65r8a)ifEA=j@HimSL$TicYJ18lv zPq*YHUo8LdxQ2sGN2fhKL$9)P|MOTH&C61JZ-d*wIjdaHmehN^b}_{-vRmicYP^4U zE*d>Z%r!!-7L|~4JUSr!(dS7Hhz;$2buJn+aHfSu4^r%eR$pQ((<#yBycQO+a`20h zPuxfNX2?!1uNvW%h@}17*hO{v2Wl2<)SnA=56^h2q)U^s=ZP)%7r%)aKWc@de_vU$ z{sM=4s9t)$R1A${=zvszuz9^yc*B=mOEaPL_{3w(QF8bkLHaDxL)Vx}1vcTX-k`wb z{FmZn%R35^G|*vSr@*7&UXIOk7xs@NZn-C^E)$IpnV#dkWfB-Wh4?3CWwAK*!?!%z z-!S~4zTT>`45vTX0gCT|w&onFgZyV>nAO8Vms5|@KmT;w%n|E3EdDJbL^bU<|$8*BmqG-r4<(td;vjZks^D<2uKkSu%ZZvhzhdrD2r?YMq*gL3E0-Q@AG`` zkN3y-!+G-L-kg~^=ge6q_s((#$IUHeI8Ymu&!n(J6JDZggNHBb|9D9xuchTU z82tcZo<2RvP3_A*ci@esx;2SopOgTfcmMEG?_BF+?7_;MiyFW&(iD^CQ{NG6jx0vD zdkx*udshBnnrNFST4_s7l#(A-rNRo3yE-a*R&Lbn8I=?p@__TOQ|!zT-7LyLjQdc! zE&8%0$zASPwMLKT1D2t{dm5T0opOgNeCFr%&VTx*$Sl>-*z9y`YZL)q%J3fxcc%tz zO23B4B8O>=G-M~1QT1mqO8V*p#Czi=`ZQ01VYYVvnoFxyK zMRI?HH(2s6Dd|d);O(1Y$EAm|;bxSwM%9{tdMi>Lvp@XgPTiQFmmSPcE=hkV_P&+B zeKVv8!V`Trx+|FUgMWzaqrUep;LyIwFyW%)?|-G*yFDVXixYAeK6Fq7+xLk1w~eUl zdwK>vP54negIuFH6(;-oj$hhR*QHot%>6cp1TQKFhi`OqF-kyS?f_r;)?l;(^@O z0eKFxP2V5376~Y|VSbU4+aWO@E6YXu+Lz$*p71g2$2CFUtVV)ULfCRG1uuRJ)7>D^ zyrth;Y<{#Xyl}xWHZ-8aqx)_|x|18MZ*=zB7}TT~vMM4ov~=Jl zXMeh3uEI9z?c6-ZtJ}TL-hhoKPN1)dBe}F~>~h17D2@~SeT>hfOezzE$EQd$L5sd+ z*FPybI^?}k)^tw#<~t$(wS5_xjd04T#LUY zB|FKBKY4ja&AjlZ!~)5dUkL-Qwe|nwQ+)5%{%g!7$45QaU*7Z#F-f_8Y~|9F^s090 ztBi}P4j9!`g8X2u4e?q}hNcLtt@v_i%dB&MM72IyWi&rHBkFi-K0EAcbdz%QJ!I(6 z9i?nxYCh}7v9WhaR6ADp(!V$ut8u|Cuud?gedSf4%<4V0PjA_0j~w}A!V>JY1E&DK~K7&VpTF`^}QZ8QN^y%+fAiW0{_g`(Tj)rIV$ieQlvP_t8p;+b6aO zBiDL7Cnr7k;T0Dr^E}4JS@|&uH@hx&KphD0YRD=iO|zDEfcXvZLQ3BOr1{#UUuL_T z_3o=ku=OT=?%8+SoI+F%z7Z4&5iS{HrNtamo+cdP?vL3kD5w@2-JDsFzV$wQN_;!g zy{DrBtu#LR^yV*xNo>2zN>jOYhk)dRXZ9MBYjzlg3(FJR9wIMORkF%ONa$d7m`$P6FuH=5LOgIkA* z8*En53D`~Q=Vo&VHzcdu&xJ<8GEfSg^MgBWZ8kcH_Yq8!30*J4N-G4Li(RD z?MY!ohEo)4dai>qU`w#@I63ZqKGMB$QFUNHBdO}06;~(hH5f`(;$hxN-|bO)8)j>c+jJ%sbiMUBv&`nZSQLs>RuM5F;QiKk zlEAfxjgv(~S-+j_lU!+LPxObpFWUZm_=Zi`F4&dJ*aoMjC~U{d>`3Fvv%_2)_4{nE zp?98!{g#h8Hn;B890LBQaQ<{CQbzPyeO?>Up1-7W zX5qSoUxukc_tx-H;hacs|9C~+Dhb$?2ihEV@?dbV+=-4c=NiBgi(7Vky1yd8AXn=^ z-O+D*CuLG>G`9&r3VeHsDHj!t;@ULL7W#Z!qtv^BQozS7B{5{_7d}jD2&y|*vA@Bo zV96K;@#smRz86A0!9D{m&e@X5(#BXp7(WzMJMS zctxPc*c=~qkd;avB=&l)6uO_Av02lGjDJqw*=fEuWLg3ninjAL41;BWKW}LC&gD>q zN?>T@On|i9WToC)5s-PsX}b7hp-8OieYRO$^?W!ET4kW+JQv<$RDH#VHVrVpe>e#3Tr9ESg#@;czWLiRh` zvgxz8OXN9~Py6^DwpH_@b}y%X(KYO9!fG>ZdkdqX$b!%*eHG3hUgfU?hI0Ml9~WMT zUN$L@Y&!SxYT>Dj?gRv!QwUWu;;V?FlCP%ge;#Ty(cQM;wAuyi(5~RZIeGrLZvvQm z9x=FGT&65f;%zG2S5*@r^|74HX&?7>YwWoI1IKnM>AMefmpAmO#zs$w-Mja4WiK^k zRAQb5Ec}s)sG*wm9KZur^1J%IpmS0sO&=5P@8i}-1K&=XWm&eAB@FE)EHa**`BgT? zOj2UP=X?Le)0+=oOi^jp`7|-(vcEz$ji{UvI=)=Gqo4EeV>meX61~BpY`>15S6<4Y zR!J=-U@Z_>jVnM$0yjH)Q|);a7Nyt9u)EXtAOnq$crAn}XYAszW3*>>2j|bpZz_&R znM?~>_)sn?5dZN0c|oGc76&x8-peuX$>eP4-s6-LWLi~BxGmR^Z-t%svaq*rY-{AYTK}1k63xuoz7XXs_3i>LEHB8Ay+c_t_D@}=|SB0ClFsK>7Bm$->Iv*uy2)Fiv?mhVM+L71ui7rCxQ z)+M{+=R0J=S~7WCK-4oE{hNQuwkxoYYY0+Vo=MDiFmE$g_yaE=ePUyx8$BH?&jx|3 z*b}+&ztdbJeT2+%<9dS76xKn9=cm9-?P|G=x{ZF?U(qWO!R8aYvYDJltLron|H(71 zVeh{w6i?t{&)2MB0+CSdW$*V>>$if%F6Co&O`jXOc2QPPozkh85;U`i&z{enj0afF z21R)=8MfGy#z3Y$kMsdV0|O)p%d5= z=9eG*==(C?b%jug6LQs_ydZuEy(juC?ZkBq>zV&Qz72#{L@@DWVIi`Q8-qzFyLj{O zXs(_@Xb1uEuQ)6cjrbj?^%K?( zv<~tmL+TbT6hP-F*^TOAL<@w72v7op)KtS@aVU(Y78V7eai|kQ>Xu{%&7V#pGax>* zcIK|0WD;}z9sy~BN|t;U!ABiW^I?*Gm<)*DI!hryfewB`$luF+76|`0Um$#Rf3m_5 zQa?gkig*{Mix-W;Cq)4a{+$x$iwbz46DExg?bB2} zNTyOem<&fG5(yph1~nlj4YDvcg+K!W$aKCkwA3`T)DUV&$Z_APQ>WB?-P~1~G@2Jf zjYRWyR8$0Vqq!LabwT@#wUG!U3W3o?XlNpl8faw%QUQTb07cd)QfWT_S2+th&CQ<# zi2i4+S$UAXyuM`6gUR&OR#*SGeEuoCKswo72mxV*5Pu{{3ys!9L+;zwVK^*C3k$8~ zLOy@OG=SLo=?@sV*8oQS7Z?tU#sKO64TjSECq5J!_m6T|4WOyt>oVysR4+1p%{;8A zr^$e2bxRrzm;&E0gwzlDxYHoMX8HHV5K+enYiO*AH^LYhX(EuCrW!_CMmR$gJjMiP zhy>J(^dKEWLqn7a0*^33fP5_!28TeInjnpgH85xmgq9&j@5H*BxzL$wa>C&dShUcd KJtpR+LjMbLVc&%S literal 0 HcmV?d00001 diff --git a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml index 70b3dad0..b6f3732c 100644 --- a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml +++ b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml @@ -12,9 +12,10 @@ navigation_task_node: gps_end_x: 0.0 gps_end_y: 0.0 gps_frame_coords_set: false - - map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" landmark_pose_topic: "landmarks_out" - \ No newline at end of file + assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct + + # Task specific parameters + distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 17c04458..a28fb312 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -5,6 +5,8 @@ namespace navigation_task { NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options) : NjordTaskBaseNode("navigation_task_node", options) { + declare_parameter("distance_to_first_buoy_pair", 2.0); + std::thread(&NavigationTaskNode::main_task, this).detach(); } @@ -25,12 +27,95 @@ void NavigationTaskNode::main_task() { } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { setup_map_odom_tf_and_subs(); - set_gps_frame_coords(); + set_gps_odom_points(); setup_lock.unlock(); break; } setup_lock.unlock(); } + // First pair of buoys + Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); + std::vector buoy_landmarks_0_to_1 = get_buoy_landmarks(predicted_first_buoy_pair); + if(buoy_landmarks_0_to_1.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); + } + geometry_msgs::msg::Point waypoint_first_pair; + waypoint_first_pair.x = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / 2; + waypoint_first_pair.y = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / 2; + waypoint_first_pair.z = 0.0; + send_waypoint(waypoint_first_pair); + reach_waypoint(1.0); + + // Second pair of buoys + Eigen::Array predicted_first_and_second_pair = predict_first_and_second_buoy_pair(buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position); + std::vector buoy_landmarks_0_to_3 = get_buoy_landmarks(predicted_first_and_second_pair); + if(buoy_landmarks_0_to_3.size() != 4) { + RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); + } + geometry_msgs::msg::Point waypoint_second_pair; + waypoint_second_pair.x = (buoy_landmarks_0_to_3[2].pose_odom_frame.position.x + buoy_landmarks_0_to_3[3].pose_odom_frame.position.x) / 2; + waypoint_second_pair.y = (buoy_landmarks_0_to_3[2].pose_odom_frame.position.y + buoy_landmarks_0_to_3[3].pose_odom_frame.position.y) / 2; + waypoint_second_pair.z = 0.0; + send_waypoint(waypoint_second_pair); + reach_waypoint(1.0); + + // West buoy +} + +Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { + // Predict the positions of the first two buoys + geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; + geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; + buoy_0_base_link_frame.header.frame_id = "base_link"; + buoy_1_base_link_frame.header.frame_id = "base_link"; + + double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); + + buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_0_base_link_frame.pose.position.y = -2.5; + buoy_0_base_link_frame.pose.position.z = 0.0; + buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_1_base_link_frame.pose.position.y = 2.5; + buoy_1_base_link_frame.pose.position.z = 0.0; + + geometry_msgs::msg::PoseStamped buoy_0_odom_frame; + geometry_msgs::msg::PoseStamped buoy_1_odom_frame; + + try { + auto transform = tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero, + tf2::durationFromSec(1.0)); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + } + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; + predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; + predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; + predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; + + return predicted_positions; +} + +Eigen::Array NavigationTaskNode::predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1){ + Eigen::Vector2d direction_vector; + direction_vector << previous_waypoint_odom_frame_.x - this->get_parameter("gps_start_x").as_double(), + previous_waypoint_odom_frame_.y - this->get_parameter("gps_start_y").as_double(); + direction_vector.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x; + predicted_positions(1, 0) = buoy_0.y; + predicted_positions(0, 1) = buoy_1.x; + predicted_positions(1, 1) = buoy_1.y; + predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 5; + predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 5; + predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 5; + predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 5; + + return predicted_positions; } } // namespace navigation_task \ No newline at end of file diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index 4600cba6..ee32bef6 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -17,20 +17,54 @@ #include #include + /** + * @brief A struct representing a landmark with id and pose in the odom frame + */ + struct LandmarkPoseID { + geometry_msgs::msg::Pose pose_odom_frame; + int32_t id; +}; + class NjordTaskBaseNode : public rclcpp::Node { public: NjordTaskBaseNode(const std::string &node_name, const rclcpp::NodeOptions &options); protected: - void setup_map_odom_tf_and_subs(); - void set_gps_frame_coords(); + std::pair lla2flat(double lat, double lon) const; void map_origin_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - std::shared_ptr get_odom(); void landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg); + + /** + * @brief Spins until the map to odom tf is available. + * Stores the tf in the member variable map_odom_tf_ + * Then initializes the odom and landmark subscribers + */ + void setup_map_odom_tf_and_subs(); + + /** + * @brief Set the gps_start_odom_frame_ and gps_end_odom_frame_ member variables + * Requires that the map_origin_ member variable is correctly set + */ + void set_gps_odom_points(); + + /** + * @brief Get the odometry message + * If no odometry message is available, the function will wait for a new message + * + * @return A shared pointer to the odometry message + */ + std::shared_ptr get_odom(); + + /** + * @brief Get the landmarks in the odom frame + * If no landmarks are available, the function will wait for a new message + * + * @return A shared pointer to the landmarks in the odom frame + */ std::shared_ptr get_landmarks_odom_frame(); /** @@ -46,10 +80,36 @@ class NjordTaskBaseNode : public rclcpp::Node { * means that the corresponding predicted position(buoy) is not assigned to * any measured position(landmark). */ - Eigen::VectorXi assign_landmarks( + Eigen::VectorXi auction_algorithm( const Eigen::Array &predicted_positions, const Eigen::Array &measured_positions); + + /** + * @brief Use predicted positions of buoys to get the landmarks with id and pose + * corresponding to the buoys. + * @param predicted_positions The predicted positions of the buoys + * @return A vector of landmarks with id and pose where the index in the vector + * corresponds to the index of the predicted position in the predicted_positions array. + */ + std::vector get_buoy_landmarks(const Eigen::Array& predicted_positions); + + /** + * @brief Send a waypoint to the waypoint service + * @param waypoint The waypoint to send in odom frame + * Also sets the member variable previous_waypoint_odom_frame_ to the waypoint + */ + void send_waypoint(const geometry_msgs::msg::Point &waypoint); + + /** + * @brief Utility function that returns when within distance_threshold of the waypoint. + * The reference waypoint is the member variable previous_waypoint_odom_frame_ set by the send_waypoint function. + * + * @param distance_threshold The distance threshold for reaching the waypoint + */ + void reach_waypoint(const double distance_threshold); + + rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; rclcpp::Publisher::SharedPtr @@ -75,6 +135,9 @@ class NjordTaskBaseNode : public rclcpp::Node { std::shared_ptr landmarks_msg_; bool new_odom_msg_ = false; bool new_landmark_msg_ = false; + + + geometry_msgs::msg::Point previous_waypoint_odom_frame_; }; #endif // NJORD_TASK_BASE_ROS_HPP diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index 597f25e8..0ba3b429 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -15,6 +15,7 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, declare_parameter("gps_end_x", 0.0); declare_parameter("gps_end_y", 0.0); declare_parameter("gps_frame_coords_set", false); + declare_parameter("assignment_confidence", 10); declare_parameter("map_origin_topic", "/map/origin"); declare_parameter("odom_topic", "/seapath/odom/ned"); @@ -52,6 +53,8 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, waypoint_client_ = this->create_client("/waypoint"); + + } void NjordTaskBaseNode::setup_map_odom_tf_and_subs() { @@ -88,37 +91,49 @@ void NjordTaskBaseNode::setup_map_odom_tf_and_subs() { std::placeholders::_1)); } -void NjordTaskBaseNode::set_gps_frame_coords() { +void NjordTaskBaseNode::set_gps_odom_points() { auto [gps_start_x, gps_start_y] = lla2flat(this->get_parameter("gps_start_lat").as_double(), this->get_parameter("gps_start_lon").as_double()); auto [gps_end_x, gps_end_y] = lla2flat(this->get_parameter("gps_end_lat").as_double(), this->get_parameter("gps_end_lon").as_double()); - this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_x)); - this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_y)); - this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_x)); - this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_y)); - this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); - RCLCPP_INFO(this->get_logger(), - "GPS map frame coordinates set to: %f, %f, %f, %f", gps_start_x, - gps_start_y, gps_end_x, gps_end_y); - geometry_msgs::msg::PoseArray gps_points; - gps_points.header.frame_id = "map"; + geometry_msgs::msg::PoseStamped gps_start_map_frame; + gps_start_map_frame.header.frame_id = "map"; + gps_start_map_frame.pose.position.x = gps_start_x; + gps_start_map_frame.pose.position.y = gps_start_y; + geometry_msgs::msg::PoseStamped gps_end_map_frame; + gps_end_map_frame.header.frame_id = "map"; + gps_end_map_frame.pose.position.x = gps_end_x; + gps_end_map_frame.pose.position.y = gps_end_y; + geometry_msgs::msg::PoseStamped gps_start_odom_frame; + geometry_msgs::msg::PoseStamped gps_end_odom_frame; + try { + tf2::doTransform(gps_start_map_frame, gps_start_odom_frame, map_odom_tf_); + tf2::doTransform(gps_end_map_frame, gps_end_odom_frame, map_odom_tf_); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + return; + } - // Convert GPS points to geometry poses - geometry_msgs::msg::Pose start_pose; - start_pose.position.x = gps_start_x; - start_pose.position.y = gps_start_y; - gps_points.poses.push_back(start_pose); + this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_odom_frame.pose.position.x)); + this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_odom_frame.pose.position.y)); + this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_odom_frame.pose.position.x)); + this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_odom_frame.pose.position.y)); + this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); + RCLCPP_INFO(this->get_logger(), + "GPS oodm frame coordinates set to: %f, %f, %f, %f", + gps_start_odom_frame.pose.position.x, gps_start_odom_frame.pose.position.y, + gps_end_odom_frame.pose.position.x, gps_end_odom_frame.pose.position.y); - geometry_msgs::msg::Pose end_pose; - end_pose.position.x = gps_end_x; - end_pose.position.y = gps_end_y; - gps_points.poses.push_back(end_pose); + geometry_msgs::msg::PoseArray gps_points_odom_frame; + gps_points_odom_frame.header.frame_id = "odom"; + + gps_points_odom_frame.poses.push_back(gps_start_odom_frame.pose); + gps_points_odom_frame.poses.push_back(gps_end_odom_frame.pose); - gps_map_coord_visualization_pub_->publish(gps_points); + gps_map_coord_visualization_pub_->publish(gps_points_odom_frame); } std::pair NjordTaskBaseNode::lla2flat(double lat, @@ -170,7 +185,7 @@ void NjordTaskBaseNode::map_origin_callback( // Set the map to odom transform setup_map_odom_tf_and_subs(); // Set GPS frame coordinates - set_gps_frame_coords(); + set_gps_odom_points(); map_origin_sub_.reset(); setup_lock.unlock(); @@ -216,7 +231,7 @@ NjordTaskBaseNode::get_landmarks_odom_frame() { return landmarks_msg_; } -Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( +Eigen::VectorXi NjordTaskBaseNode::auction_algorithm( const Eigen::Array &predicted_positions, const Eigen::Array &measured_positions) { int num_predicted = predicted_positions.cols(); @@ -227,6 +242,7 @@ Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( RCLCPP_ERROR(this->get_logger(), "Number of predicted positions is greater than number of " "measured positions in auction algorithm"); + return Eigen::VectorXi::Constant(num_predicted, -1); } double epsilon = 1e-6; // Small positive number to prevent division by zero @@ -283,4 +299,140 @@ Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( prices[max_item] += max_value - second_max_value + epsilon; } return assignment; +} + +std::vector NjordTaskBaseNode::get_buoy_landmarks(const Eigen::Array& predicted_positions){ + std::vector landmark_ids; + std::vector expected_assignment; + Eigen::Array measured_buoy_positions(2, predicted_positions.cols()); + int confidence_threshold = this->get_parameter("assignment_confidence").as_int(); + int result = 0; + while (result < confidence_threshold) { + landmark_ids.clear(); + auto landmark_msg = get_landmarks_odom_frame(); + + // Extract measured positions and corresponding landmark ids + Eigen::Array measured_positions(2, landmark_msg->landmarks.size()); + for (size_t i = 0; i < landmark_msg->landmarks.size(); i++) { + landmark_ids.push_back(landmark_msg->landmarks[i].id); + measured_positions(0, i) = landmark_msg->landmarks[i].odom.pose.pose.position.x; + measured_positions(1, i) = landmark_msg->landmarks[i].odom.pose.pose.position.y; + } + + // Check if there are enough landmarks detected to perform auction algorithm + if (predicted_positions.cols() > measured_positions.cols()) { + RCLCPP_ERROR(this->get_logger(), + "Not enough landmarks detected to perform auction algorithm"); + result = 0; + continue; + } + // Perform auction algorithm + Eigen::VectorXi assignment = auction_algorithm(predicted_positions, measured_positions); + + // Extract measured positions of assigned buoys + for (Eigen::Index i = 0; i < assignment.size(); i++) { + measured_buoy_positions(0, i) = measured_positions(0, assignment(i)); + measured_buoy_positions(1, i) = measured_positions(1, assignment(i)); + } + + // Check if any buoys are unassigned + // Should never happen as long as the number of landmarks detected is greater than or equal to the number of buoys + bool unassigned_buoy = false; + for (Eigen::Index i = 0; i < assignment.size(); i++) { + if (assignment(i) == -1) { + unassigned_buoy = true; + break; + } + } + + // If any buoys are unassigned, restart assignment process + if(unassigned_buoy){ + result = 0; + continue; + } + + // If this is the first iteration, save the assignment and continue + if (result == 0) { + expected_assignment.clear(); + for (Eigen::Index i = 0; i < assignment.size(); i++) { + expected_assignment.push_back(landmark_ids.at(assignment(i))); + } + result++; + continue; + } + + // Check if the assignment is consistent with the previous assignment + bool consistent_assignment = true; + for (Eigen::Index i = 0; i < assignment.size(); i++) { + if (landmark_ids.at(assignment(i)) != expected_assignment.at(i)) { + consistent_assignment = false; + break; + } + } + + // If the assignment is consistent, increment the result counter + // Otherwise, reset the result counter + if (consistent_assignment) { + result++; + continue; + } else { + result = 0; + continue; + } + + } + // Loop has completed, return the id and pose of the landmarks assigned to buoys + std::vector buoys; + for (size_t i = 0; i < expected_assignment.size(); i++) { + LandmarkPoseID landmark; + landmark.id = expected_assignment.at(i); + landmark.pose_odom_frame.position.x = measured_buoy_positions(0, i); + landmark.pose_odom_frame.position.y = measured_buoy_positions(1, i); + buoys.push_back(landmark); + } + return buoys; +} + +void NjordTaskBaseNode::send_waypoint(const geometry_msgs::msg::Point &waypoint) { + auto request = std::make_shared(); + request->waypoint.push_back(waypoint); + auto result_future = waypoint_client_->async_send_request(request); + RCLCPP_INFO(this->get_logger(), "Waypoint(odom frame) sent: %f, %f", + waypoint.x, waypoint.y); + // Check if the service was successful + + auto status = result_future.wait_for(std::chrono::seconds(5)); + while (status == std::future_status::timeout) { + RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); + status = result_future.wait_for(std::chrono::seconds(5)); + } + if (!result_future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); + } + + geometry_msgs::msg::PoseStamped waypoint_vis; + waypoint_vis.header.frame_id = "odom"; + waypoint_vis.pose.position.x = waypoint.x; + waypoint_vis.pose.position.y = waypoint.y; + waypoint_visualization_pub_->publish(waypoint_vis); + + previous_waypoint_odom_frame_ = waypoint; +} + +void NjordTaskBaseNode::reach_waypoint(const double distance_threshold) { + RCLCPP_INFO(this->get_logger(), "Reach waypoint running"); + + auto get_current_position = [&]() { + auto odom_msg = get_odom(); + return std::make_pair(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y); + }; + + auto [x, y] = get_current_position(); + + while (std::hypot(x - previous_waypoint_odom_frame_.x, y - previous_waypoint_odom_frame_.y) > distance_threshold) { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + std::tie(x, y) = get_current_position(); + } + + RCLCPP_INFO(this->get_logger(), "Reached waypoint"); } \ No newline at end of file From e7a71dac51e23f185287758d1d67a605aad66291 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Tue, 30 Jul 2024 11:42:52 +0000 Subject: [PATCH 28/67] Committing clang-format changes --- .../navigation_task/navigation_task_ros.hpp | 10 +- .../src/navigation_task_ros.cpp | 57 ++++++--- .../njord_task_base/njord_task_base_ros.hpp | 40 +++--- .../src/njord_task_base_ros.cpp | 115 ++++++++++-------- 4 files changed, 129 insertions(+), 93 deletions(-) diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp index a8c16080..94a21e8f 100644 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -17,7 +17,6 @@ class NavigationTaskNode : public NjordTaskBaseNode { */ void main_task(); - /** * @brief Predict the positions of the first two buoys * @@ -26,18 +25,17 @@ class NavigationTaskNode : public NjordTaskBaseNode { */ Eigen::Array predict_first_buoy_pair(); - /** + /** * @brief Predict the positions of the first two buoys pairs * * @return An Eigen::Array representing the predicted positions * of the first two buoy pairs */ - Eigen::Array predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); - + Eigen::Array + predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1); private: - - }; } // namespace navigation_task diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index a28fb312..9c75f592 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -6,7 +6,7 @@ NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options) : NjordTaskBaseNode("navigation_task_node", options) { declare_parameter("distance_to_first_buoy_pair", 2.0); - + std::thread(&NavigationTaskNode::main_task, this).detach(); } @@ -35,27 +35,44 @@ void NavigationTaskNode::main_task() { } // First pair of buoys Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); - std::vector buoy_landmarks_0_to_1 = get_buoy_landmarks(predicted_first_buoy_pair); - if(buoy_landmarks_0_to_1.size() != 2) { + std::vector buoy_landmarks_0_to_1 = + get_buoy_landmarks(predicted_first_buoy_pair); + if (buoy_landmarks_0_to_1.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); } geometry_msgs::msg::Point waypoint_first_pair; - waypoint_first_pair.x = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / 2; - waypoint_first_pair.y = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / 2; + waypoint_first_pair.x = + (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + + buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / + 2; + waypoint_first_pair.y = + (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + + buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / + 2; waypoint_first_pair.z = 0.0; send_waypoint(waypoint_first_pair); reach_waypoint(1.0); // Second pair of buoys - Eigen::Array predicted_first_and_second_pair = predict_first_and_second_buoy_pair(buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position); - std::vector buoy_landmarks_0_to_3 = get_buoy_landmarks(predicted_first_and_second_pair); - if(buoy_landmarks_0_to_3.size() != 4) { + Eigen::Array predicted_first_and_second_pair = + predict_first_and_second_buoy_pair( + buoy_landmarks_0_to_1[0].pose_odom_frame.position, + buoy_landmarks_0_to_1[1].pose_odom_frame.position); + std::vector buoy_landmarks_0_to_3 = + get_buoy_landmarks(predicted_first_and_second_pair); + if (buoy_landmarks_0_to_3.size() != 4) { RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); } geometry_msgs::msg::Point waypoint_second_pair; - waypoint_second_pair.x = (buoy_landmarks_0_to_3[2].pose_odom_frame.position.x + buoy_landmarks_0_to_3[3].pose_odom_frame.position.x) / 2; - waypoint_second_pair.y = (buoy_landmarks_0_to_3[2].pose_odom_frame.position.y + buoy_landmarks_0_to_3[3].pose_odom_frame.position.y) / 2; - waypoint_second_pair.z = 0.0; + waypoint_second_pair.x = + (buoy_landmarks_0_to_3[2].pose_odom_frame.position.x + + buoy_landmarks_0_to_3[3].pose_odom_frame.position.x) / + 2; + waypoint_second_pair.y = + (buoy_landmarks_0_to_3[2].pose_odom_frame.position.y + + buoy_landmarks_0_to_3[3].pose_odom_frame.position.y) / + 2; + waypoint_second_pair.z = 0.0; send_waypoint(waypoint_second_pair); reach_waypoint(1.0); @@ -69,7 +86,8 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { buoy_0_base_link_frame.header.frame_id = "base_link"; buoy_1_base_link_frame.header.frame_id = "base_link"; - double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); + double distance_to_first_buoy_pair = + this->get_parameter("distance_to_first_buoy_pair").as_double(); buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; buoy_0_base_link_frame.pose.position.y = -2.5; @@ -82,8 +100,8 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { geometry_msgs::msg::PoseStamped buoy_1_odom_frame; try { - auto transform = tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero, - tf2::durationFromSec(1.0)); + auto transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform); tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform); } catch (tf2::TransformException &ex) { @@ -99,10 +117,15 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1){ +Eigen::Array +NavigationTaskNode::predict_first_and_second_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - this->get_parameter("gps_start_x").as_double(), - previous_waypoint_odom_frame_.y - this->get_parameter("gps_start_y").as_double(); + direction_vector << previous_waypoint_odom_frame_.x - + this->get_parameter("gps_start_x").as_double(), + previous_waypoint_odom_frame_.y - + this->get_parameter("gps_start_y").as_double(); direction_vector.normalize(); Eigen::Array predicted_positions; diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index ee32bef6..9bb9cbc4 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -17,10 +17,10 @@ #include #include - /** - * @brief A struct representing a landmark with id and pose in the odom frame - */ - struct LandmarkPoseID { +/** + * @brief A struct representing a landmark with id and pose in the odom frame + */ +struct LandmarkPoseID { geometry_msgs::msg::Pose pose_odom_frame; int32_t id; }; @@ -31,7 +31,6 @@ class NjordTaskBaseNode : public rclcpp::Node { const rclcpp::NodeOptions &options); protected: - std::pair lla2flat(double lat, double lon) const; void map_origin_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); @@ -46,21 +45,22 @@ class NjordTaskBaseNode : public rclcpp::Node { void setup_map_odom_tf_and_subs(); /** - * @brief Set the gps_start_odom_frame_ and gps_end_odom_frame_ member variables - * Requires that the map_origin_ member variable is correctly set + * @brief Set the gps_start_odom_frame_ and gps_end_odom_frame_ member + * variables Requires that the map_origin_ member variable is correctly set */ void set_gps_odom_points(); /** * @brief Get the odometry message - * If no odometry message is available, the function will wait for a new message + * If no odometry message is available, the function will wait for a new + * message * * @return A shared pointer to the odometry message */ std::shared_ptr get_odom(); /** - * @brief Get the landmarks in the odom frame + * @brief Get the landmarks in the odom frame * If no landmarks are available, the function will wait for a new message * * @return A shared pointer to the landmarks in the odom frame @@ -84,15 +84,16 @@ class NjordTaskBaseNode : public rclcpp::Node { const Eigen::Array &predicted_positions, const Eigen::Array &measured_positions); - /** - * @brief Use predicted positions of buoys to get the landmarks with id and pose - * corresponding to the buoys. + * @brief Use predicted positions of buoys to get the landmarks with id and + * pose corresponding to the buoys. * @param predicted_positions The predicted positions of the buoys - * @return A vector of landmarks with id and pose where the index in the vector - * corresponds to the index of the predicted position in the predicted_positions array. + * @return A vector of landmarks with id and pose where the index in the + * vector corresponds to the index of the predicted position in the + * predicted_positions array. */ - std::vector get_buoy_landmarks(const Eigen::Array& predicted_positions); + std::vector get_buoy_landmarks( + const Eigen::Array &predicted_positions); /** * @brief Send a waypoint to the waypoint service @@ -101,15 +102,15 @@ class NjordTaskBaseNode : public rclcpp::Node { */ void send_waypoint(const geometry_msgs::msg::Point &waypoint); - /** - * @brief Utility function that returns when within distance_threshold of the waypoint. - * The reference waypoint is the member variable previous_waypoint_odom_frame_ set by the send_waypoint function. + /** + * @brief Utility function that returns when within distance_threshold of the + * waypoint. The reference waypoint is the member variable + * previous_waypoint_odom_frame_ set by the send_waypoint function. * * @param distance_threshold The distance threshold for reaching the waypoint */ void reach_waypoint(const double distance_threshold); - rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; rclcpp::Publisher::SharedPtr @@ -136,7 +137,6 @@ class NjordTaskBaseNode : public rclcpp::Node { bool new_odom_msg_ = false; bool new_landmark_msg_ = false; - geometry_msgs::msg::Point previous_waypoint_odom_frame_; }; diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index 0ba3b429..7687fde9 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -53,8 +53,6 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, waypoint_client_ = this->create_client("/waypoint"); - - } void NjordTaskBaseNode::setup_map_odom_tf_and_subs() { @@ -117,19 +115,24 @@ void NjordTaskBaseNode::set_gps_odom_points() { return; } - this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_odom_frame.pose.position.x)); - this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_odom_frame.pose.position.y)); - this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_odom_frame.pose.position.x)); - this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_odom_frame.pose.position.y)); + this->set_parameter( + rclcpp::Parameter("gps_start_x", gps_start_odom_frame.pose.position.x)); + this->set_parameter( + rclcpp::Parameter("gps_start_y", gps_start_odom_frame.pose.position.y)); + this->set_parameter( + rclcpp::Parameter("gps_end_x", gps_end_odom_frame.pose.position.x)); + this->set_parameter( + rclcpp::Parameter("gps_end_y", gps_end_odom_frame.pose.position.y)); this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); - RCLCPP_INFO(this->get_logger(), - "GPS oodm frame coordinates set to: %f, %f, %f, %f", - gps_start_odom_frame.pose.position.x, gps_start_odom_frame.pose.position.y, - gps_end_odom_frame.pose.position.x, gps_end_odom_frame.pose.position.y); + RCLCPP_INFO( + this->get_logger(), "GPS oodm frame coordinates set to: %f, %f, %f, %f", + gps_start_odom_frame.pose.position.x, + gps_start_odom_frame.pose.position.y, gps_end_odom_frame.pose.position.x, + gps_end_odom_frame.pose.position.y); geometry_msgs::msg::PoseArray gps_points_odom_frame; gps_points_odom_frame.header.frame_id = "odom"; - + gps_points_odom_frame.poses.push_back(gps_start_odom_frame.pose); gps_points_odom_frame.poses.push_back(gps_end_odom_frame.pose); @@ -301,42 +304,51 @@ Eigen::VectorXi NjordTaskBaseNode::auction_algorithm( return assignment; } -std::vector NjordTaskBaseNode::get_buoy_landmarks(const Eigen::Array& predicted_positions){ +std::vector NjordTaskBaseNode::get_buoy_landmarks( + const Eigen::Array &predicted_positions) { std::vector landmark_ids; std::vector expected_assignment; - Eigen::Array measured_buoy_positions(2, predicted_positions.cols()); - int confidence_threshold = this->get_parameter("assignment_confidence").as_int(); + Eigen::Array measured_buoy_positions( + 2, predicted_positions.cols()); + int confidence_threshold = + this->get_parameter("assignment_confidence").as_int(); int result = 0; while (result < confidence_threshold) { landmark_ids.clear(); auto landmark_msg = get_landmarks_odom_frame(); - + // Extract measured positions and corresponding landmark ids - Eigen::Array measured_positions(2, landmark_msg->landmarks.size()); + Eigen::Array measured_positions( + 2, landmark_msg->landmarks.size()); for (size_t i = 0; i < landmark_msg->landmarks.size(); i++) { landmark_ids.push_back(landmark_msg->landmarks[i].id); - measured_positions(0, i) = landmark_msg->landmarks[i].odom.pose.pose.position.x; - measured_positions(1, i) = landmark_msg->landmarks[i].odom.pose.pose.position.y; + measured_positions(0, i) = + landmark_msg->landmarks[i].odom.pose.pose.position.x; + measured_positions(1, i) = + landmark_msg->landmarks[i].odom.pose.pose.position.y; } // Check if there are enough landmarks detected to perform auction algorithm if (predicted_positions.cols() > measured_positions.cols()) { - RCLCPP_ERROR(this->get_logger(), - "Not enough landmarks detected to perform auction algorithm"); + RCLCPP_ERROR( + this->get_logger(), + "Not enough landmarks detected to perform auction algorithm"); result = 0; continue; } // Perform auction algorithm - Eigen::VectorXi assignment = auction_algorithm(predicted_positions, measured_positions); + Eigen::VectorXi assignment = + auction_algorithm(predicted_positions, measured_positions); // Extract measured positions of assigned buoys for (Eigen::Index i = 0; i < assignment.size(); i++) { measured_buoy_positions(0, i) = measured_positions(0, assignment(i)); measured_buoy_positions(1, i) = measured_positions(1, assignment(i)); } - + // Check if any buoys are unassigned - // Should never happen as long as the number of landmarks detected is greater than or equal to the number of buoys + // Should never happen as long as the number of landmarks detected is + // greater than or equal to the number of buoys bool unassigned_buoy = false; for (Eigen::Index i = 0; i < assignment.size(); i++) { if (assignment(i) == -1) { @@ -346,7 +358,7 @@ std::vector NjordTaskBaseNode::get_buoy_landmarks(const Eigen::A } // If any buoys are unassigned, restart assignment process - if(unassigned_buoy){ + if (unassigned_buoy) { result = 0; continue; } @@ -369,7 +381,7 @@ std::vector NjordTaskBaseNode::get_buoy_landmarks(const Eigen::A break; } } - + // If the assignment is consistent, increment the result counter // Otherwise, reset the result counter if (consistent_assignment) { @@ -379,9 +391,9 @@ std::vector NjordTaskBaseNode::get_buoy_landmarks(const Eigen::A result = 0; continue; } - } - // Loop has completed, return the id and pose of the landmarks assigned to buoys + // Loop has completed, return the id and pose of the landmarks assigned to + // buoys std::vector buoys; for (size_t i = 0; i < expected_assignment.size(); i++) { LandmarkPoseID landmark; @@ -393,30 +405,31 @@ std::vector NjordTaskBaseNode::get_buoy_landmarks(const Eigen::A return buoys; } -void NjordTaskBaseNode::send_waypoint(const geometry_msgs::msg::Point &waypoint) { +void NjordTaskBaseNode::send_waypoint( + const geometry_msgs::msg::Point &waypoint) { auto request = std::make_shared(); - request->waypoint.push_back(waypoint); - auto result_future = waypoint_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Waypoint(odom frame) sent: %f, %f", - waypoint.x, waypoint.y); - // Check if the service was successful - - auto status = result_future.wait_for(std::chrono::seconds(5)); - while (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); - status = result_future.wait_for(std::chrono::seconds(5)); - } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); - } + request->waypoint.push_back(waypoint); + auto result_future = waypoint_client_->async_send_request(request); + RCLCPP_INFO(this->get_logger(), "Waypoint(odom frame) sent: %f, %f", + waypoint.x, waypoint.y); + // Check if the service was successful + + auto status = result_future.wait_for(std::chrono::seconds(5)); + while (status == std::future_status::timeout) { + RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); + status = result_future.wait_for(std::chrono::seconds(5)); + } + if (!result_future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); + } - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = waypoint.x; - waypoint_vis.pose.position.y = waypoint.y; - waypoint_visualization_pub_->publish(waypoint_vis); + geometry_msgs::msg::PoseStamped waypoint_vis; + waypoint_vis.header.frame_id = "odom"; + waypoint_vis.pose.position.x = waypoint.x; + waypoint_vis.pose.position.y = waypoint.y; + waypoint_visualization_pub_->publish(waypoint_vis); - previous_waypoint_odom_frame_ = waypoint; + previous_waypoint_odom_frame_ = waypoint; } void NjordTaskBaseNode::reach_waypoint(const double distance_threshold) { @@ -424,12 +437,14 @@ void NjordTaskBaseNode::reach_waypoint(const double distance_threshold) { auto get_current_position = [&]() { auto odom_msg = get_odom(); - return std::make_pair(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y); + return std::make_pair(odom_msg->pose.pose.position.x, + odom_msg->pose.pose.position.y); }; auto [x, y] = get_current_position(); - while (std::hypot(x - previous_waypoint_odom_frame_.x, y - previous_waypoint_odom_frame_.y) > distance_threshold) { + while (std::hypot(x - previous_waypoint_odom_frame_.x, + y - previous_waypoint_odom_frame_.y) > distance_threshold) { rclcpp::sleep_for(std::chrono::milliseconds(100)); std::tie(x, y) = get_current_position(); } From a09d4e54e472fd0b4f5745952d3bff4758337930 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Tue, 30 Jul 2024 22:55:41 +0200 Subject: [PATCH 29/67] navigation task test ready --- .../navigation_task/navigation_task_ros.hpp | 79 ++++- .../src/navigation_task_ros.cpp | 275 ++++++++++++++++++ 2 files changed, 350 insertions(+), 4 deletions(-) diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp index 94a21e8f..4faa43f3 100644 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -31,11 +31,82 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @return An Eigen::Array representing the predicted positions * of the first two buoy pairs */ - Eigen::Array - predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); + Eigen::Array predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); + + /** + * @brief Predict the positions of the west buoy by using the two first buoy pairs + * + * @return An Eigen::Array representing the predicted positions + * of the second buoy pair and the west buoy + */ + Eigen::Array predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, const geometry_msgs::msg::Point &buoy_3); + + /** + * @brief Predict the positions of the third buoy pair by using the two first buoy pairs + * + * @return An Eigen::Array representing the predicted positions + * of the third buoy pair + */ + Eigen::Array predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, const geometry_msgs::msg::Point &buoy_3); + + /** + * @brief Predict the positions of the north buoy by using the two first buoy pairs + * + * @return An Eigen::Array representing the predicted positions + * of the third buoy pair and the north buoy + */ + Eigen::Array predict_north_buoy(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, + const Eigen::Vector2d &direction_vector_second_to_third_pair); + + /** + * @brief Predict the positions of the south buoy by using the two first buoy pairs + * + * @return An Eigen::Array representing the predicted positions + * of the north and south buoys + */ + Eigen::Array predict_south_buoy(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, + const geometry_msgs::msg::Point &buoy_7, const Eigen::Vector2d &direction_vector_second_to_third_pair); + + /** + * @brief Predict the positions of the fourth buoy pair by using the third buoy pair + * + * @return An Eigen::Array representing the predicted positions + * of the fourth buoy pair + */ + Eigen::Array predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6); + + /** + * @brief Predict the position of the east buoy by using the fourth buoy pair + * and the direction vector from the second to the third buoy pair + * + * @return An Eigen::Array representing the predicted positions + * of the fourth buoy pair and the east buoy + */ + Eigen::Array predict_east_buoy(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, + const Eigen::Vector2d &direction_vector_second_to_third_pair); + + /** + * @brief Predict the position of the fifth buoy pair by using the fourth buoy pair + * and the direction vector from the second to the third buoy pair + * + * @return An Eigen::Array representing the predicted positions + * of the fifth buoy pair and the east buoy + */ + Eigen::Array predict_fifth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, + const geometry_msgs::msg::Point &buoy_11, const Eigen::Vector2d &direction_vector_second_to_third_pair); + + /** + * @brief Predict the position of the sixth buoy pair by using the fifth buoy pair + * and fourth buoy pair + * + * @return An Eigen::Array representing the predicted positions + * of the fifth and sixth buoy pairs + */ + Eigen::Array predict_sixth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, + const geometry_msgs::msg::Point &buoy_12, const geometry_msgs::msg::Point &buoy_13); -private: }; } // namespace navigation_task diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 9c75f592..e78c31d0 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -77,6 +77,145 @@ void NavigationTaskNode::main_task() { reach_waypoint(1.0); // West buoy + Eigen::Array predicted_west_buoy = predict_west_buoy(buoy_landmarks_0_to_3[0].pose_odom_frame.position, buoy_landmarks_0_to_3[1].pose_odom_frame.position, + buoy_landmarks_0_to_3[2].pose_odom_frame.position, buoy_landmarks_0_to_3[3].pose_odom_frame.position); + std::vector buoy_landmarks_2_to_4 = get_buoy_landmarks(predicted_west_buoy); + if(buoy_landmarks_2_to_4.size() != 3) { + RCLCPP_ERROR(this->get_logger(), "Could not find west buoy"); + } + Eigen::Vector2d direction_vector_upwards; + direction_vector_upwards << buoy_landmarks_2_to_4[2].pose_odom_frame.position.x - buoy_landmarks_0_to_3[3].pose_odom_frame.position.x, + buoy_landmarks_2_to_4[2].pose_odom_frame.position.y - buoy_landmarks_0_to_3[3].pose_odom_frame.position.y; + direction_vector_upwards.normalize(); + + // Set waypoint two meters above west buoy + geometry_msgs::msg::Point waypoint_west_buoy; + waypoint_west_buoy.x = buoy_landmarks_2_to_4[4].pose_odom_frame.position.x + direction_vector_upwards(0) * 2; + waypoint_west_buoy.y = buoy_landmarks_2_to_4[4].pose_odom_frame.position.y + direction_vector_upwards(1) * 2; + waypoint_west_buoy.z = 0.0; + + send_waypoint(waypoint_west_buoy); + reach_waypoint(1.0); + + // Third pair of buoys + Eigen::Array predicted_third_buoy_pair = predict_third_buoy_pair(buoy_landmarks_0_to_3[0].pose_odom_frame.position, buoy_landmarks_0_to_3[1].pose_odom_frame.position, + buoy_landmarks_0_to_3[2].pose_odom_frame.position, buoy_landmarks_0_to_3[3].pose_odom_frame.position); + + std::vector buoy_landmarks_5_to_6 = get_buoy_landmarks(predicted_third_buoy_pair); + if(buoy_landmarks_5_to_6.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find third buoy pair"); + } + geometry_msgs::msg::Point waypoint_third_pair; + waypoint_third_pair.x = (buoy_landmarks_5_to_6[0].pose_odom_frame.position.x + buoy_landmarks_5_to_6[1].pose_odom_frame.position.x) / 2; + waypoint_third_pair.y = (buoy_landmarks_5_to_6[0].pose_odom_frame.position.y + buoy_landmarks_5_to_6[1].pose_odom_frame.position.y) / 2; + waypoint_third_pair.z = 0.0; + + send_waypoint(waypoint_third_pair); + Eigen::Vector2d direction_vector_second_to_third_pair; + direction_vector_second_to_third_pair << waypoint_third_pair.x - waypoint_second_pair.x, + waypoint_third_pair.y - waypoint_second_pair.y; + direction_vector_second_to_third_pair.normalize(); + geometry_msgs::msg::Point waypoint_through_third_pair; + waypoint_through_third_pair.x = waypoint_third_pair.x + direction_vector_second_to_third_pair(0) * 2; + waypoint_through_third_pair.y = waypoint_third_pair.y + direction_vector_second_to_third_pair(1) * 2; + waypoint_through_third_pair.z = 0.0; + send_waypoint(waypoint_through_third_pair); + reach_waypoint(1.0); + + // North buoy + Eigen::Array predicted_north_buoy = predict_north_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, buoy_landmarks_5_to_6[1].pose_odom_frame.position, + direction_vector_second_to_third_pair); + std::vector buoy_landmarks_5_to_7 = get_buoy_landmarks(predicted_north_buoy); + if(buoy_landmarks_5_to_7.size() != 3) { + RCLCPP_ERROR(this->get_logger(), "Could not find north buoy"); + } + geometry_msgs::msg::Point waypoint_north_buoy; + waypoint_north_buoy.x = buoy_landmarks_5_to_7[2].pose_odom_frame.position.x + direction_vector_second_to_third_pair(0) * 2; + waypoint_north_buoy.y = buoy_landmarks_5_to_7[2].pose_odom_frame.position.y + direction_vector_second_to_third_pair(1) * 2; + waypoint_north_buoy.z = 0.0; + send_waypoint(waypoint_north_buoy); + reach_waypoint(1.0); + + // South buoy + Eigen::Array predicted_south_buoy = predict_south_buoy(buoy_landmarks_5_to_7[0].pose_odom_frame.position, buoy_landmarks_5_to_7[1].pose_odom_frame.position, + buoy_landmarks_5_to_7[2].pose_odom_frame.position, direction_vector_second_to_third_pair); + std::vector buoy_landmarks_7_to_8 = get_buoy_landmarks(predicted_south_buoy); + if(buoy_landmarks_7_to_8.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find south buoy"); + } + geometry_msgs::msg::Point waypoint_south_buoy; + waypoint_south_buoy.x = buoy_landmarks_7_to_8[1].pose_odom_frame.position.x - direction_vector_second_to_third_pair(0) * 2; + waypoint_south_buoy.y = buoy_landmarks_7_to_8[1].pose_odom_frame.position.y - direction_vector_second_to_third_pair(1) * 2; + waypoint_south_buoy.z = 0.0; + send_waypoint(waypoint_south_buoy); + reach_waypoint(1.0); + + // Fourth pair of buoys + Eigen::Array predicted_fourth_buoy_pair = predict_fourth_buoy_pair(buoy_landmarks_5_to_7[0].pose_odom_frame.position, buoy_landmarks_5_to_7[1].pose_odom_frame.position); + std::vector buoy_landmarks_9_to_10 = get_buoy_landmarks(predicted_fourth_buoy_pair); + if(buoy_landmarks_9_to_10.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find fourth buoy pair"); + } + geometry_msgs::msg::Point waypoint_fourth_pair; + waypoint_fourth_pair.x = (buoy_landmarks_9_to_10[0].pose_odom_frame.position.x + buoy_landmarks_9_to_10[1].pose_odom_frame.position.x) / 2; + waypoint_fourth_pair.y = (buoy_landmarks_9_to_10[0].pose_odom_frame.position.y + buoy_landmarks_9_to_10[1].pose_odom_frame.position.y) / 2; + waypoint_fourth_pair.z = 0.0; + send_waypoint(waypoint_fourth_pair); + reach_waypoint(1.0); + + // East buoy + Eigen::Array predicted_east_buoy = predict_east_buoy(buoy_landmarks_9_to_10[0].pose_odom_frame.position, buoy_landmarks_9_to_10[1].pose_odom_frame.position, + direction_vector_second_to_third_pair); + std::vector buoy_landmarks_9_to_11 = get_buoy_landmarks(predicted_east_buoy); + if(buoy_landmarks_9_to_11.size() != 3) { + RCLCPP_ERROR(this->get_logger(), "Could not find east buoy"); + } + Eigen::Vector2d direction_vector_9_to_10; + direction_vector_9_to_10 << buoy_landmarks_9_to_11[1].pose_odom_frame.position.x - buoy_landmarks_9_to_11[0].pose_odom_frame.position.x, + buoy_landmarks_9_to_11[1].pose_odom_frame.position.y - buoy_landmarks_9_to_11[0].pose_odom_frame.position.y; + direction_vector_9_to_10.normalize(); + geometry_msgs::msg::Point waypoint_east_buoy; + waypoint_east_buoy.x = buoy_landmarks_9_to_11[2].pose_odom_frame.position.x + direction_vector_9_to_10(0) * 2; + waypoint_east_buoy.y = buoy_landmarks_9_to_11[2].pose_odom_frame.position.y + direction_vector_9_to_10(1) * 2; + waypoint_east_buoy.z = 0.0; + send_waypoint(waypoint_east_buoy); + reach_waypoint(1.0); + + // Fifth pair of buoys + Eigen::Array predicted_fifth_buoy_pair = predict_fifth_buoy_pair(buoy_landmarks_9_to_11[0].pose_odom_frame.position, buoy_landmarks_9_to_11[1].pose_odom_frame.position, + buoy_landmarks_9_to_11[2].pose_odom_frame.position, direction_vector_second_to_third_pair); + std::vector buoy_landmarks_11_to_13 = get_buoy_landmarks(predicted_fifth_buoy_pair); + if(buoy_landmarks_11_to_13.size() != 3) { + RCLCPP_ERROR(this->get_logger(), "Could not find fifth buoy pair"); + } + geometry_msgs::msg::Point waypoint_fifth_pair; + waypoint_fifth_pair.x = (buoy_landmarks_11_to_13[1].pose_odom_frame.position.x + buoy_landmarks_11_to_13[2].pose_odom_frame.position.x) / 2; + waypoint_fifth_pair.y = (buoy_landmarks_11_to_13[1].pose_odom_frame.position.y + buoy_landmarks_11_to_13[2].pose_odom_frame.position.y) / 2; + waypoint_fifth_pair.z = 0.0; + send_waypoint(waypoint_fifth_pair); + reach_waypoint(1.0); + + // Sixth pair of buoys + Eigen::Array predicted_sixth_buoy_pair = predict_sixth_buoy_pair(buoy_landmarks_9_to_11[0].pose_odom_frame.position, buoy_landmarks_9_to_11[1].pose_odom_frame.position, + buoy_landmarks_11_to_13[1].pose_odom_frame.position, buoy_landmarks_11_to_13[2].pose_odom_frame.position); + std::vector buoy_landmarks_12_to_15 = get_buoy_landmarks(predicted_sixth_buoy_pair); + if(buoy_landmarks_12_to_15.size() != 4) { + RCLCPP_ERROR(this->get_logger(), "Could not find sixth buoy pair"); + } + geometry_msgs::msg::Point waypoint_sixth_pair; + waypoint_sixth_pair.x = (buoy_landmarks_12_to_15[2].pose_odom_frame.position.x + buoy_landmarks_12_to_15[3].pose_odom_frame.position.x) / 2; + waypoint_sixth_pair.y = (buoy_landmarks_12_to_15[2].pose_odom_frame.position.y + buoy_landmarks_12_to_15[3].pose_odom_frame.position.y) / 2; + waypoint_sixth_pair.z = 0.0; + send_waypoint(waypoint_sixth_pair); + reach_waypoint(1.0); + + // Gps end goal + geometry_msgs::msg::Point gps_end_goal; + gps_end_goal.x = this->get_parameter("gps_end_x").as_double(); + gps_end_goal.y = this->get_parameter("gps_end_y").as_double(); + gps_end_goal.z = 0.0; + send_waypoint(gps_end_goal); + } Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { @@ -141,4 +280,140 @@ NavigationTaskNode::predict_first_and_second_buoy_pair( return predicted_positions; } +Eigen::Array NavigationTaskNode::predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, const geometry_msgs::msg::Point &buoy_3) { + Eigen::Vector2d direction_vector_right; + direction_vector_right << (buoy_3.x + buoy_2.x)/2 - (buoy_1.x + buoy_0.x)/2, + (buoy_3.y + buoy_2.y)/2 - (buoy_1.y + buoy_0.y)/2; + direction_vector_right.normalize(); + + Eigen::Vector2d direction_vector_up; + direction_vector_up << (buoy_0.x + buoy_2.x)/2 - (buoy_1.x + buoy_3.x)/2, + (buoy_0.y + buoy_2.y)/2 - (buoy_1.y + buoy_3.y)/2; + direction_vector_up.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_2.x; + predicted_positions(1, 0) = buoy_2.y; + predicted_positions(0, 1) = buoy_3.x; + predicted_positions(1, 1) = buoy_3.y; + predicted_positions(0, 2) = buoy_3.x + direction_vector_right(0) * 12.5 * 1/2 + direction_vector_up(0) * 5 * 2/5; + predicted_positions(1, 2) = buoy_3.y + direction_vector_right(1) * 12.5 * 1/2 + direction_vector_up(1) * 5 * 2/5; + + return predicted_positions; +} + +Eigen::Array NavigationTaskNode::predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, const geometry_msgs::msg::Point &buoy_3) { + Eigen::Vector2d direction_vector_right; + direction_vector_right << (buoy_3.x + buoy_2.x)/2 - (buoy_1.x + buoy_0.x)/2, + (buoy_3.y + buoy_2.y)/2 - (buoy_1.y + buoy_0.y)/2; + direction_vector_right.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_2.x + direction_vector_right(0) * 12.5; + predicted_positions(1, 0) = buoy_2.y + direction_vector_right(1) * 12.5; + predicted_positions(0, 1) = buoy_3.x + direction_vector_right(0) * 12.5; + predicted_positions(1, 1) = buoy_3.y + direction_vector_right(1) * 12.5; + + return predicted_positions; +} + +Eigen::Array NavigationTaskNode::predict_north_buoy(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, +const Eigen::Vector2d &direction_vector_second_to_third_pair){ + Eigen::Vector2d direction_5_to_6; + direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; + direction_5_to_6.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_5.x; + predicted_positions(1, 0) = buoy_5.y; + predicted_positions(0, 1) = buoy_6.x; + predicted_positions(1, 1) = buoy_6.y; + predicted_positions(0, 2) = buoy_6.x + direction_5_to_6(0) * 12.5 * 0.5 + direction_vector_second_to_third_pair(0) * 2; + predicted_positions(1, 2) = buoy_6.y + direction_5_to_6(1) * 12.5 * 0.5 + direction_vector_second_to_third_pair(1) * 2; + + return predicted_positions; +} + +Eigen::Array NavigationTaskNode::predict_south_buoy(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, +const geometry_msgs::msg::Point &buoy_7, const Eigen::Vector2d &direction_vector_second_to_third_pair){ + Eigen::Vector2d direction_5_to_6; + direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; + direction_5_to_6.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_7.x; + predicted_positions(1, 0) = buoy_7.y; + predicted_positions(0, 1) = buoy_6.x + direction_5_to_6(0) * 12.5 + direction_vector_second_to_third_pair(0) * 8; + predicted_positions(1, 1) = buoy_6.y + direction_5_to_6(1) * 12.5 + direction_vector_second_to_third_pair(1) * 8; + + return predicted_positions; +} + +Eigen::Array NavigationTaskNode::predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6){ + Eigen::Vector2d direction_5_to_6; + direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; + direction_5_to_6.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5; + predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5; + predicted_positions(0, 1) = buoy_6.x + direction_5_to_6(0) * 12.5 + direction_5_to_6(0) * 5; + predicted_positions(1, 1) = buoy_6.y + direction_5_to_6(1) * 12.5 + direction_5_to_6(1) * 5; + + return predicted_positions; +} + +Eigen::Array NavigationTaskNode::predict_east_buoy(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, +const Eigen::Vector2d &direction_vector_second_to_third_pair){ + Eigen::Vector2d direction_9_to_10; + direction_9_to_10 << buoy_10.x - buoy_9.x, buoy_10.y - buoy_9.y; + direction_9_to_10.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_9.x; + predicted_positions(1, 0) = buoy_9.y; + predicted_positions(0, 1) = buoy_10.x; + predicted_positions(1, 1) = buoy_10.y; + predicted_positions(0, 2) = buoy_9.x + direction_9_to_10(0) * 2.5 -direction_vector_second_to_third_pair(0) * 7.5; + predicted_positions(1, 2) = buoy_9.y + direction_9_to_10(1) * 2.5 -direction_vector_second_to_third_pair(1) * 7.5; + + return predicted_positions; +} + +Eigen::Array NavigationTaskNode::predict_fifth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, +const geometry_msgs::msg::Point &buoy_11, const Eigen::Vector2d &direction_vector_second_to_third_pair){ + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_11.x; + predicted_positions(1, 0) = buoy_11.y; + predicted_positions(0, 1) = buoy_9.x - direction_vector_second_to_third_pair(0) * 12.5; + predicted_positions(1, 1) = buoy_9.y - direction_vector_second_to_third_pair(1) * 12.5; + predicted_positions(0, 2) = buoy_10.x - direction_vector_second_to_third_pair(0) * 12.5; + predicted_positions(1, 2) = buoy_10.y - direction_vector_second_to_third_pair(1) * 12.5; + + return predicted_positions; +} + +Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, +const geometry_msgs::msg::Point &buoy_12, const geometry_msgs::msg::Point &buoy_13){ + Eigen::Vector2d direction_fourth_to_fifth_pair; + direction_fourth_to_fifth_pair << (buoy_13.x + buoy_12.x)/2 - (buoy_10.x + buoy_9.x)/2, + (buoy_13.y + buoy_12.y)/2 - (buoy_10.y + buoy_9.y)/2; + direction_fourth_to_fifth_pair.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_12.x; + predicted_positions(1, 0) = buoy_12.y; + predicted_positions(0, 1) = buoy_13.x; + predicted_positions(1, 1) = buoy_13.y; + predicted_positions(0, 2) = buoy_12.x + direction_fourth_to_fifth_pair(0) * 5; + predicted_positions(1, 2) = buoy_12.y + direction_fourth_to_fifth_pair(1) * 5; + predicted_positions(0, 3) = buoy_13.x + direction_fourth_to_fifth_pair(0) * 5; + predicted_positions(1, 3) = buoy_13.y + direction_fourth_to_fifth_pair(1) * 5; + + return predicted_positions; +} + } // namespace navigation_task \ No newline at end of file From d05125e725e06a3f3da506f52c8a409551b10ad9 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Tue, 30 Jul 2024 20:56:06 +0000 Subject: [PATCH 30/67] Committing clang-format changes --- .../navigation_task/navigation_task_ros.hpp | 79 +++-- .../src/navigation_task_ros.cpp | 311 ++++++++++++------ 2 files changed, 265 insertions(+), 125 deletions(-) diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp index 4faa43f3..7e878168 100644 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -31,51 +31,71 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @return An Eigen::Array representing the predicted positions * of the first two buoy pairs */ - Eigen::Array predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); + Eigen::Array + predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1); /** - * @brief Predict the positions of the west buoy by using the two first buoy pairs + * @brief Predict the positions of the west buoy by using the two first buoy + * pairs * * @return An Eigen::Array representing the predicted positions * of the second buoy pair and the west buoy */ - Eigen::Array predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, const geometry_msgs::msg::Point &buoy_3); + Eigen::Array + predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, + const geometry_msgs::msg::Point &buoy_3); /** - * @brief Predict the positions of the third buoy pair by using the two first buoy pairs + * @brief Predict the positions of the third buoy pair by using the two first + * buoy pairs * * @return An Eigen::Array representing the predicted positions * of the third buoy pair */ - Eigen::Array predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, const geometry_msgs::msg::Point &buoy_3); + Eigen::Array + predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, + const geometry_msgs::msg::Point &buoy_3); /** - * @brief Predict the positions of the north buoy by using the two first buoy pairs + * @brief Predict the positions of the north buoy by using the two first buoy + * pairs * * @return An Eigen::Array representing the predicted positions * of the third buoy pair and the north buoy */ - Eigen::Array predict_north_buoy(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, - const Eigen::Vector2d &direction_vector_second_to_third_pair); + Eigen::Array predict_north_buoy( + const geometry_msgs::msg::Point &buoy_5, + const geometry_msgs::msg::Point &buoy_6, + const Eigen::Vector2d &direction_vector_second_to_third_pair); /** - * @brief Predict the positions of the south buoy by using the two first buoy pairs + * @brief Predict the positions of the south buoy by using the two first buoy + * pairs * * @return An Eigen::Array representing the predicted positions * of the north and south buoys */ - Eigen::Array predict_south_buoy(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, - const geometry_msgs::msg::Point &buoy_7, const Eigen::Vector2d &direction_vector_second_to_third_pair); + Eigen::Array predict_south_buoy( + const geometry_msgs::msg::Point &buoy_5, + const geometry_msgs::msg::Point &buoy_6, + const geometry_msgs::msg::Point &buoy_7, + const Eigen::Vector2d &direction_vector_second_to_third_pair); /** - * @brief Predict the positions of the fourth buoy pair by using the third buoy pair + * @brief Predict the positions of the fourth buoy pair by using the third + * buoy pair * * @return An Eigen::Array representing the predicted positions * of the fourth buoy pair */ - Eigen::Array predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6); + Eigen::Array + predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_5, + const geometry_msgs::msg::Point &buoy_6); /** * @brief Predict the position of the east buoy by using the fourth buoy pair @@ -84,29 +104,36 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @return An Eigen::Array representing the predicted positions * of the fourth buoy pair and the east buoy */ - Eigen::Array predict_east_buoy(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, - const Eigen::Vector2d &direction_vector_second_to_third_pair); + Eigen::Array predict_east_buoy( + const geometry_msgs::msg::Point &buoy_9, + const geometry_msgs::msg::Point &buoy_10, + const Eigen::Vector2d &direction_vector_second_to_third_pair); /** - * @brief Predict the position of the fifth buoy pair by using the fourth buoy pair - * and the direction vector from the second to the third buoy pair + * @brief Predict the position of the fifth buoy pair by using the fourth buoy + * pair and the direction vector from the second to the third buoy pair * * @return An Eigen::Array representing the predicted positions * of the fifth buoy pair and the east buoy */ - Eigen::Array predict_fifth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_11, const Eigen::Vector2d &direction_vector_second_to_third_pair); + Eigen::Array predict_fifth_buoy_pair( + const geometry_msgs::msg::Point &buoy_9, + const geometry_msgs::msg::Point &buoy_10, + const geometry_msgs::msg::Point &buoy_11, + const Eigen::Vector2d &direction_vector_second_to_third_pair); /** - * @brief Predict the position of the sixth buoy pair by using the fifth buoy pair - * and fourth buoy pair + * @brief Predict the position of the sixth buoy pair by using the fifth buoy + * pair and fourth buoy pair * * @return An Eigen::Array representing the predicted positions * of the fifth and sixth buoy pairs */ - Eigen::Array predict_sixth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_12, const geometry_msgs::msg::Point &buoy_13); - + Eigen::Array + predict_sixth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, + const geometry_msgs::msg::Point &buoy_10, + const geometry_msgs::msg::Point &buoy_12, + const geometry_msgs::msg::Point &buoy_13); }; } // namespace navigation_task diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index e78c31d0..33cbb9c0 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -77,145 +77,218 @@ void NavigationTaskNode::main_task() { reach_waypoint(1.0); // West buoy - Eigen::Array predicted_west_buoy = predict_west_buoy(buoy_landmarks_0_to_3[0].pose_odom_frame.position, buoy_landmarks_0_to_3[1].pose_odom_frame.position, - buoy_landmarks_0_to_3[2].pose_odom_frame.position, buoy_landmarks_0_to_3[3].pose_odom_frame.position); - std::vector buoy_landmarks_2_to_4 = get_buoy_landmarks(predicted_west_buoy); - if(buoy_landmarks_2_to_4.size() != 3) { + Eigen::Array predicted_west_buoy = + predict_west_buoy(buoy_landmarks_0_to_3[0].pose_odom_frame.position, + buoy_landmarks_0_to_3[1].pose_odom_frame.position, + buoy_landmarks_0_to_3[2].pose_odom_frame.position, + buoy_landmarks_0_to_3[3].pose_odom_frame.position); + std::vector buoy_landmarks_2_to_4 = + get_buoy_landmarks(predicted_west_buoy); + if (buoy_landmarks_2_to_4.size() != 3) { RCLCPP_ERROR(this->get_logger(), "Could not find west buoy"); } Eigen::Vector2d direction_vector_upwards; - direction_vector_upwards << buoy_landmarks_2_to_4[2].pose_odom_frame.position.x - buoy_landmarks_0_to_3[3].pose_odom_frame.position.x, - buoy_landmarks_2_to_4[2].pose_odom_frame.position.y - buoy_landmarks_0_to_3[3].pose_odom_frame.position.y; + direction_vector_upwards + << buoy_landmarks_2_to_4[2].pose_odom_frame.position.x - + buoy_landmarks_0_to_3[3].pose_odom_frame.position.x, + buoy_landmarks_2_to_4[2].pose_odom_frame.position.y - + buoy_landmarks_0_to_3[3].pose_odom_frame.position.y; direction_vector_upwards.normalize(); // Set waypoint two meters above west buoy geometry_msgs::msg::Point waypoint_west_buoy; - waypoint_west_buoy.x = buoy_landmarks_2_to_4[4].pose_odom_frame.position.x + direction_vector_upwards(0) * 2; - waypoint_west_buoy.y = buoy_landmarks_2_to_4[4].pose_odom_frame.position.y + direction_vector_upwards(1) * 2; + waypoint_west_buoy.x = buoy_landmarks_2_to_4[4].pose_odom_frame.position.x + + direction_vector_upwards(0) * 2; + waypoint_west_buoy.y = buoy_landmarks_2_to_4[4].pose_odom_frame.position.y + + direction_vector_upwards(1) * 2; waypoint_west_buoy.z = 0.0; send_waypoint(waypoint_west_buoy); reach_waypoint(1.0); // Third pair of buoys - Eigen::Array predicted_third_buoy_pair = predict_third_buoy_pair(buoy_landmarks_0_to_3[0].pose_odom_frame.position, buoy_landmarks_0_to_3[1].pose_odom_frame.position, - buoy_landmarks_0_to_3[2].pose_odom_frame.position, buoy_landmarks_0_to_3[3].pose_odom_frame.position); - - std::vector buoy_landmarks_5_to_6 = get_buoy_landmarks(predicted_third_buoy_pair); - if(buoy_landmarks_5_to_6.size() != 2) { + Eigen::Array predicted_third_buoy_pair = + predict_third_buoy_pair( + buoy_landmarks_0_to_3[0].pose_odom_frame.position, + buoy_landmarks_0_to_3[1].pose_odom_frame.position, + buoy_landmarks_0_to_3[2].pose_odom_frame.position, + buoy_landmarks_0_to_3[3].pose_odom_frame.position); + + std::vector buoy_landmarks_5_to_6 = + get_buoy_landmarks(predicted_third_buoy_pair); + if (buoy_landmarks_5_to_6.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find third buoy pair"); } geometry_msgs::msg::Point waypoint_third_pair; - waypoint_third_pair.x = (buoy_landmarks_5_to_6[0].pose_odom_frame.position.x + buoy_landmarks_5_to_6[1].pose_odom_frame.position.x) / 2; - waypoint_third_pair.y = (buoy_landmarks_5_to_6[0].pose_odom_frame.position.y + buoy_landmarks_5_to_6[1].pose_odom_frame.position.y) / 2; + waypoint_third_pair.x = + (buoy_landmarks_5_to_6[0].pose_odom_frame.position.x + + buoy_landmarks_5_to_6[1].pose_odom_frame.position.x) / + 2; + waypoint_third_pair.y = + (buoy_landmarks_5_to_6[0].pose_odom_frame.position.y + + buoy_landmarks_5_to_6[1].pose_odom_frame.position.y) / + 2; waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); Eigen::Vector2d direction_vector_second_to_third_pair; - direction_vector_second_to_third_pair << waypoint_third_pair.x - waypoint_second_pair.x, - waypoint_third_pair.y - waypoint_second_pair.y; + direction_vector_second_to_third_pair + << waypoint_third_pair.x - waypoint_second_pair.x, + waypoint_third_pair.y - waypoint_second_pair.y; direction_vector_second_to_third_pair.normalize(); geometry_msgs::msg::Point waypoint_through_third_pair; - waypoint_through_third_pair.x = waypoint_third_pair.x + direction_vector_second_to_third_pair(0) * 2; - waypoint_through_third_pair.y = waypoint_third_pair.y + direction_vector_second_to_third_pair(1) * 2; + waypoint_through_third_pair.x = + waypoint_third_pair.x + direction_vector_second_to_third_pair(0) * 2; + waypoint_through_third_pair.y = + waypoint_third_pair.y + direction_vector_second_to_third_pair(1) * 2; waypoint_through_third_pair.z = 0.0; send_waypoint(waypoint_through_third_pair); reach_waypoint(1.0); // North buoy - Eigen::Array predicted_north_buoy = predict_north_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, buoy_landmarks_5_to_6[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - std::vector buoy_landmarks_5_to_7 = get_buoy_landmarks(predicted_north_buoy); - if(buoy_landmarks_5_to_7.size() != 3) { + Eigen::Array predicted_north_buoy = + predict_north_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, + buoy_landmarks_5_to_6[1].pose_odom_frame.position, + direction_vector_second_to_third_pair); + std::vector buoy_landmarks_5_to_7 = + get_buoy_landmarks(predicted_north_buoy); + if (buoy_landmarks_5_to_7.size() != 3) { RCLCPP_ERROR(this->get_logger(), "Could not find north buoy"); } geometry_msgs::msg::Point waypoint_north_buoy; - waypoint_north_buoy.x = buoy_landmarks_5_to_7[2].pose_odom_frame.position.x + direction_vector_second_to_third_pair(0) * 2; - waypoint_north_buoy.y = buoy_landmarks_5_to_7[2].pose_odom_frame.position.y + direction_vector_second_to_third_pair(1) * 2; + waypoint_north_buoy.x = buoy_landmarks_5_to_7[2].pose_odom_frame.position.x + + direction_vector_second_to_third_pair(0) * 2; + waypoint_north_buoy.y = buoy_landmarks_5_to_7[2].pose_odom_frame.position.y + + direction_vector_second_to_third_pair(1) * 2; waypoint_north_buoy.z = 0.0; send_waypoint(waypoint_north_buoy); reach_waypoint(1.0); // South buoy - Eigen::Array predicted_south_buoy = predict_south_buoy(buoy_landmarks_5_to_7[0].pose_odom_frame.position, buoy_landmarks_5_to_7[1].pose_odom_frame.position, - buoy_landmarks_5_to_7[2].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_7_to_8 = get_buoy_landmarks(predicted_south_buoy); - if(buoy_landmarks_7_to_8.size() != 2) { + Eigen::Array predicted_south_buoy = + predict_south_buoy(buoy_landmarks_5_to_7[0].pose_odom_frame.position, + buoy_landmarks_5_to_7[1].pose_odom_frame.position, + buoy_landmarks_5_to_7[2].pose_odom_frame.position, + direction_vector_second_to_third_pair); + std::vector buoy_landmarks_7_to_8 = + get_buoy_landmarks(predicted_south_buoy); + if (buoy_landmarks_7_to_8.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find south buoy"); } geometry_msgs::msg::Point waypoint_south_buoy; - waypoint_south_buoy.x = buoy_landmarks_7_to_8[1].pose_odom_frame.position.x - direction_vector_second_to_third_pair(0) * 2; - waypoint_south_buoy.y = buoy_landmarks_7_to_8[1].pose_odom_frame.position.y - direction_vector_second_to_third_pair(1) * 2; + waypoint_south_buoy.x = buoy_landmarks_7_to_8[1].pose_odom_frame.position.x - + direction_vector_second_to_third_pair(0) * 2; + waypoint_south_buoy.y = buoy_landmarks_7_to_8[1].pose_odom_frame.position.y - + direction_vector_second_to_third_pair(1) * 2; waypoint_south_buoy.z = 0.0; send_waypoint(waypoint_south_buoy); reach_waypoint(1.0); // Fourth pair of buoys - Eigen::Array predicted_fourth_buoy_pair = predict_fourth_buoy_pair(buoy_landmarks_5_to_7[0].pose_odom_frame.position, buoy_landmarks_5_to_7[1].pose_odom_frame.position); - std::vector buoy_landmarks_9_to_10 = get_buoy_landmarks(predicted_fourth_buoy_pair); - if(buoy_landmarks_9_to_10.size() != 2) { + Eigen::Array predicted_fourth_buoy_pair = + predict_fourth_buoy_pair( + buoy_landmarks_5_to_7[0].pose_odom_frame.position, + buoy_landmarks_5_to_7[1].pose_odom_frame.position); + std::vector buoy_landmarks_9_to_10 = + get_buoy_landmarks(predicted_fourth_buoy_pair); + if (buoy_landmarks_9_to_10.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find fourth buoy pair"); } geometry_msgs::msg::Point waypoint_fourth_pair; - waypoint_fourth_pair.x = (buoy_landmarks_9_to_10[0].pose_odom_frame.position.x + buoy_landmarks_9_to_10[1].pose_odom_frame.position.x) / 2; - waypoint_fourth_pair.y = (buoy_landmarks_9_to_10[0].pose_odom_frame.position.y + buoy_landmarks_9_to_10[1].pose_odom_frame.position.y) / 2; + waypoint_fourth_pair.x = + (buoy_landmarks_9_to_10[0].pose_odom_frame.position.x + + buoy_landmarks_9_to_10[1].pose_odom_frame.position.x) / + 2; + waypoint_fourth_pair.y = + (buoy_landmarks_9_to_10[0].pose_odom_frame.position.y + + buoy_landmarks_9_to_10[1].pose_odom_frame.position.y) / + 2; waypoint_fourth_pair.z = 0.0; send_waypoint(waypoint_fourth_pair); reach_waypoint(1.0); // East buoy - Eigen::Array predicted_east_buoy = predict_east_buoy(buoy_landmarks_9_to_10[0].pose_odom_frame.position, buoy_landmarks_9_to_10[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - std::vector buoy_landmarks_9_to_11 = get_buoy_landmarks(predicted_east_buoy); - if(buoy_landmarks_9_to_11.size() != 3) { + Eigen::Array predicted_east_buoy = + predict_east_buoy(buoy_landmarks_9_to_10[0].pose_odom_frame.position, + buoy_landmarks_9_to_10[1].pose_odom_frame.position, + direction_vector_second_to_third_pair); + std::vector buoy_landmarks_9_to_11 = + get_buoy_landmarks(predicted_east_buoy); + if (buoy_landmarks_9_to_11.size() != 3) { RCLCPP_ERROR(this->get_logger(), "Could not find east buoy"); } Eigen::Vector2d direction_vector_9_to_10; - direction_vector_9_to_10 << buoy_landmarks_9_to_11[1].pose_odom_frame.position.x - buoy_landmarks_9_to_11[0].pose_odom_frame.position.x, - buoy_landmarks_9_to_11[1].pose_odom_frame.position.y - buoy_landmarks_9_to_11[0].pose_odom_frame.position.y; + direction_vector_9_to_10 + << buoy_landmarks_9_to_11[1].pose_odom_frame.position.x - + buoy_landmarks_9_to_11[0].pose_odom_frame.position.x, + buoy_landmarks_9_to_11[1].pose_odom_frame.position.y - + buoy_landmarks_9_to_11[0].pose_odom_frame.position.y; direction_vector_9_to_10.normalize(); geometry_msgs::msg::Point waypoint_east_buoy; - waypoint_east_buoy.x = buoy_landmarks_9_to_11[2].pose_odom_frame.position.x + direction_vector_9_to_10(0) * 2; - waypoint_east_buoy.y = buoy_landmarks_9_to_11[2].pose_odom_frame.position.y + direction_vector_9_to_10(1) * 2; + waypoint_east_buoy.x = buoy_landmarks_9_to_11[2].pose_odom_frame.position.x + + direction_vector_9_to_10(0) * 2; + waypoint_east_buoy.y = buoy_landmarks_9_to_11[2].pose_odom_frame.position.y + + direction_vector_9_to_10(1) * 2; waypoint_east_buoy.z = 0.0; send_waypoint(waypoint_east_buoy); reach_waypoint(1.0); // Fifth pair of buoys - Eigen::Array predicted_fifth_buoy_pair = predict_fifth_buoy_pair(buoy_landmarks_9_to_11[0].pose_odom_frame.position, buoy_landmarks_9_to_11[1].pose_odom_frame.position, - buoy_landmarks_9_to_11[2].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_11_to_13 = get_buoy_landmarks(predicted_fifth_buoy_pair); - if(buoy_landmarks_11_to_13.size() != 3) { + Eigen::Array predicted_fifth_buoy_pair = + predict_fifth_buoy_pair( + buoy_landmarks_9_to_11[0].pose_odom_frame.position, + buoy_landmarks_9_to_11[1].pose_odom_frame.position, + buoy_landmarks_9_to_11[2].pose_odom_frame.position, + direction_vector_second_to_third_pair); + std::vector buoy_landmarks_11_to_13 = + get_buoy_landmarks(predicted_fifth_buoy_pair); + if (buoy_landmarks_11_to_13.size() != 3) { RCLCPP_ERROR(this->get_logger(), "Could not find fifth buoy pair"); } geometry_msgs::msg::Point waypoint_fifth_pair; - waypoint_fifth_pair.x = (buoy_landmarks_11_to_13[1].pose_odom_frame.position.x + buoy_landmarks_11_to_13[2].pose_odom_frame.position.x) / 2; - waypoint_fifth_pair.y = (buoy_landmarks_11_to_13[1].pose_odom_frame.position.y + buoy_landmarks_11_to_13[2].pose_odom_frame.position.y) / 2; + waypoint_fifth_pair.x = + (buoy_landmarks_11_to_13[1].pose_odom_frame.position.x + + buoy_landmarks_11_to_13[2].pose_odom_frame.position.x) / + 2; + waypoint_fifth_pair.y = + (buoy_landmarks_11_to_13[1].pose_odom_frame.position.y + + buoy_landmarks_11_to_13[2].pose_odom_frame.position.y) / + 2; waypoint_fifth_pair.z = 0.0; send_waypoint(waypoint_fifth_pair); reach_waypoint(1.0); // Sixth pair of buoys - Eigen::Array predicted_sixth_buoy_pair = predict_sixth_buoy_pair(buoy_landmarks_9_to_11[0].pose_odom_frame.position, buoy_landmarks_9_to_11[1].pose_odom_frame.position, - buoy_landmarks_11_to_13[1].pose_odom_frame.position, buoy_landmarks_11_to_13[2].pose_odom_frame.position); - std::vector buoy_landmarks_12_to_15 = get_buoy_landmarks(predicted_sixth_buoy_pair); - if(buoy_landmarks_12_to_15.size() != 4) { + Eigen::Array predicted_sixth_buoy_pair = + predict_sixth_buoy_pair( + buoy_landmarks_9_to_11[0].pose_odom_frame.position, + buoy_landmarks_9_to_11[1].pose_odom_frame.position, + buoy_landmarks_11_to_13[1].pose_odom_frame.position, + buoy_landmarks_11_to_13[2].pose_odom_frame.position); + std::vector buoy_landmarks_12_to_15 = + get_buoy_landmarks(predicted_sixth_buoy_pair); + if (buoy_landmarks_12_to_15.size() != 4) { RCLCPP_ERROR(this->get_logger(), "Could not find sixth buoy pair"); } geometry_msgs::msg::Point waypoint_sixth_pair; - waypoint_sixth_pair.x = (buoy_landmarks_12_to_15[2].pose_odom_frame.position.x + buoy_landmarks_12_to_15[3].pose_odom_frame.position.x) / 2; - waypoint_sixth_pair.y = (buoy_landmarks_12_to_15[2].pose_odom_frame.position.y + buoy_landmarks_12_to_15[3].pose_odom_frame.position.y) / 2; + waypoint_sixth_pair.x = + (buoy_landmarks_12_to_15[2].pose_odom_frame.position.x + + buoy_landmarks_12_to_15[3].pose_odom_frame.position.x) / + 2; + waypoint_sixth_pair.y = + (buoy_landmarks_12_to_15[2].pose_odom_frame.position.y + + buoy_landmarks_12_to_15[3].pose_odom_frame.position.y) / + 2; waypoint_sixth_pair.z = 0.0; send_waypoint(waypoint_sixth_pair); reach_waypoint(1.0); // Gps end goal geometry_msgs::msg::Point gps_end_goal; - gps_end_goal.x = this->get_parameter("gps_end_x").as_double(); + gps_end_goal.x = this->get_parameter("gps_end_x").as_double(); gps_end_goal.y = this->get_parameter("gps_end_y").as_double(); gps_end_goal.z = 0.0; send_waypoint(gps_end_goal); - } Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { @@ -280,16 +353,20 @@ NavigationTaskNode::predict_first_and_second_buoy_pair( return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, const geometry_msgs::msg::Point &buoy_3) { +Eigen::Array +NavigationTaskNode::predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, + const geometry_msgs::msg::Point &buoy_3) { Eigen::Vector2d direction_vector_right; - direction_vector_right << (buoy_3.x + buoy_2.x)/2 - (buoy_1.x + buoy_0.x)/2, - (buoy_3.y + buoy_2.y)/2 - (buoy_1.y + buoy_0.y)/2; + direction_vector_right << (buoy_3.x + buoy_2.x) / 2 - + (buoy_1.x + buoy_0.x) / 2, + (buoy_3.y + buoy_2.y) / 2 - (buoy_1.y + buoy_0.y) / 2; direction_vector_right.normalize(); Eigen::Vector2d direction_vector_up; - direction_vector_up << (buoy_0.x + buoy_2.x)/2 - (buoy_1.x + buoy_3.x)/2, - (buoy_0.y + buoy_2.y)/2 - (buoy_1.y + buoy_3.y)/2; + direction_vector_up << (buoy_0.x + buoy_2.x) / 2 - (buoy_1.x + buoy_3.x) / 2, + (buoy_0.y + buoy_2.y) / 2 - (buoy_1.y + buoy_3.y) / 2; direction_vector_up.normalize(); Eigen::Array predicted_positions; @@ -297,17 +374,25 @@ Eigen::Array NavigationTaskNode::predict_west_buoy(const geometry_ predicted_positions(1, 0) = buoy_2.y; predicted_positions(0, 1) = buoy_3.x; predicted_positions(1, 1) = buoy_3.y; - predicted_positions(0, 2) = buoy_3.x + direction_vector_right(0) * 12.5 * 1/2 + direction_vector_up(0) * 5 * 2/5; - predicted_positions(1, 2) = buoy_3.y + direction_vector_right(1) * 12.5 * 1/2 + direction_vector_up(1) * 5 * 2/5; - + predicted_positions(0, 2) = buoy_3.x + + direction_vector_right(0) * 12.5 * 1 / 2 + + direction_vector_up(0) * 5 * 2 / 5; + predicted_positions(1, 2) = buoy_3.y + + direction_vector_right(1) * 12.5 * 1 / 2 + + direction_vector_up(1) * 5 * 2 / 5; + return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, const geometry_msgs::msg::Point &buoy_3) { +Eigen::Array NavigationTaskNode::predict_third_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, + const geometry_msgs::msg::Point &buoy_3) { Eigen::Vector2d direction_vector_right; - direction_vector_right << (buoy_3.x + buoy_2.x)/2 - (buoy_1.x + buoy_0.x)/2, - (buoy_3.y + buoy_2.y)/2 - (buoy_1.y + buoy_0.y)/2; + direction_vector_right << (buoy_3.x + buoy_2.x) / 2 - + (buoy_1.x + buoy_0.x) / 2, + (buoy_3.y + buoy_2.y) / 2 - (buoy_1.y + buoy_0.y) / 2; direction_vector_right.normalize(); Eigen::Array predicted_positions; @@ -315,12 +400,14 @@ Eigen::Array NavigationTaskNode::predict_third_buoy_pair(const geo predicted_positions(1, 0) = buoy_2.y + direction_vector_right(1) * 12.5; predicted_positions(0, 1) = buoy_3.x + direction_vector_right(0) * 12.5; predicted_positions(1, 1) = buoy_3.y + direction_vector_right(1) * 12.5; - + return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_north_buoy(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, -const Eigen::Vector2d &direction_vector_second_to_third_pair){ +Eigen::Array NavigationTaskNode::predict_north_buoy( + const geometry_msgs::msg::Point &buoy_5, + const geometry_msgs::msg::Point &buoy_6, + const Eigen::Vector2d &direction_vector_second_to_third_pair) { Eigen::Vector2d direction_5_to_6; direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; direction_5_to_6.normalize(); @@ -330,14 +417,19 @@ const Eigen::Vector2d &direction_vector_second_to_third_pair){ predicted_positions(1, 0) = buoy_5.y; predicted_positions(0, 1) = buoy_6.x; predicted_positions(1, 1) = buoy_6.y; - predicted_positions(0, 2) = buoy_6.x + direction_5_to_6(0) * 12.5 * 0.5 + direction_vector_second_to_third_pair(0) * 2; - predicted_positions(1, 2) = buoy_6.y + direction_5_to_6(1) * 12.5 * 0.5 + direction_vector_second_to_third_pair(1) * 2; + predicted_positions(0, 2) = buoy_6.x + direction_5_to_6(0) * 12.5 * 0.5 + + direction_vector_second_to_third_pair(0) * 2; + predicted_positions(1, 2) = buoy_6.y + direction_5_to_6(1) * 12.5 * 0.5 + + direction_vector_second_to_third_pair(1) * 2; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_south_buoy(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, -const geometry_msgs::msg::Point &buoy_7, const Eigen::Vector2d &direction_vector_second_to_third_pair){ +Eigen::Array NavigationTaskNode::predict_south_buoy( + const geometry_msgs::msg::Point &buoy_5, + const geometry_msgs::msg::Point &buoy_6, + const geometry_msgs::msg::Point &buoy_7, + const Eigen::Vector2d &direction_vector_second_to_third_pair) { Eigen::Vector2d direction_5_to_6; direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; direction_5_to_6.normalize(); @@ -345,13 +437,17 @@ const geometry_msgs::msg::Point &buoy_7, const Eigen::Vector2d &direction_vector Eigen::Array predicted_positions; predicted_positions(0, 0) = buoy_7.x; predicted_positions(1, 0) = buoy_7.y; - predicted_positions(0, 1) = buoy_6.x + direction_5_to_6(0) * 12.5 + direction_vector_second_to_third_pair(0) * 8; - predicted_positions(1, 1) = buoy_6.y + direction_5_to_6(1) * 12.5 + direction_vector_second_to_third_pair(1) * 8; + predicted_positions(0, 1) = buoy_6.x + direction_5_to_6(0) * 12.5 + + direction_vector_second_to_third_pair(0) * 8; + predicted_positions(1, 1) = buoy_6.y + direction_5_to_6(1) * 12.5 + + direction_vector_second_to_third_pair(1) * 8; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6){ +Eigen::Array NavigationTaskNode::predict_fourth_buoy_pair( + const geometry_msgs::msg::Point &buoy_5, + const geometry_msgs::msg::Point &buoy_6) { Eigen::Vector2d direction_5_to_6; direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; direction_5_to_6.normalize(); @@ -359,14 +455,18 @@ Eigen::Array NavigationTaskNode::predict_fourth_buoy_pair(const ge Eigen::Array predicted_positions; predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5; predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5; - predicted_positions(0, 1) = buoy_6.x + direction_5_to_6(0) * 12.5 + direction_5_to_6(0) * 5; - predicted_positions(1, 1) = buoy_6.y + direction_5_to_6(1) * 12.5 + direction_5_to_6(1) * 5; + predicted_positions(0, 1) = + buoy_6.x + direction_5_to_6(0) * 12.5 + direction_5_to_6(0) * 5; + predicted_positions(1, 1) = + buoy_6.y + direction_5_to_6(1) * 12.5 + direction_5_to_6(1) * 5; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_east_buoy(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, -const Eigen::Vector2d &direction_vector_second_to_third_pair){ +Eigen::Array NavigationTaskNode::predict_east_buoy( + const geometry_msgs::msg::Point &buoy_9, + const geometry_msgs::msg::Point &buoy_10, + const Eigen::Vector2d &direction_vector_second_to_third_pair) { Eigen::Vector2d direction_9_to_10; direction_9_to_10 << buoy_10.x - buoy_9.x, buoy_10.y - buoy_9.y; direction_9_to_10.normalize(); @@ -376,31 +476,44 @@ const Eigen::Vector2d &direction_vector_second_to_third_pair){ predicted_positions(1, 0) = buoy_9.y; predicted_positions(0, 1) = buoy_10.x; predicted_positions(1, 1) = buoy_10.y; - predicted_positions(0, 2) = buoy_9.x + direction_9_to_10(0) * 2.5 -direction_vector_second_to_third_pair(0) * 7.5; - predicted_positions(1, 2) = buoy_9.y + direction_9_to_10(1) * 2.5 -direction_vector_second_to_third_pair(1) * 7.5; + predicted_positions(0, 2) = buoy_9.x + direction_9_to_10(0) * 2.5 - + direction_vector_second_to_third_pair(0) * 7.5; + predicted_positions(1, 2) = buoy_9.y + direction_9_to_10(1) * 2.5 - + direction_vector_second_to_third_pair(1) * 7.5; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_fifth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, -const geometry_msgs::msg::Point &buoy_11, const Eigen::Vector2d &direction_vector_second_to_third_pair){ - +Eigen::Array NavigationTaskNode::predict_fifth_buoy_pair( + const geometry_msgs::msg::Point &buoy_9, + const geometry_msgs::msg::Point &buoy_10, + const geometry_msgs::msg::Point &buoy_11, + const Eigen::Vector2d &direction_vector_second_to_third_pair) { + Eigen::Array predicted_positions; predicted_positions(0, 0) = buoy_11.x; predicted_positions(1, 0) = buoy_11.y; - predicted_positions(0, 1) = buoy_9.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 1) = buoy_9.y - direction_vector_second_to_third_pair(1) * 12.5; - predicted_positions(0, 2) = buoy_10.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 2) = buoy_10.y - direction_vector_second_to_third_pair(1) * 12.5; - + predicted_positions(0, 1) = + buoy_9.x - direction_vector_second_to_third_pair(0) * 12.5; + predicted_positions(1, 1) = + buoy_9.y - direction_vector_second_to_third_pair(1) * 12.5; + predicted_positions(0, 2) = + buoy_10.x - direction_vector_second_to_third_pair(0) * 12.5; + predicted_positions(1, 2) = + buoy_10.y - direction_vector_second_to_third_pair(1) * 12.5; + return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, -const geometry_msgs::msg::Point &buoy_12, const geometry_msgs::msg::Point &buoy_13){ +Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair( + const geometry_msgs::msg::Point &buoy_9, + const geometry_msgs::msg::Point &buoy_10, + const geometry_msgs::msg::Point &buoy_12, + const geometry_msgs::msg::Point &buoy_13) { Eigen::Vector2d direction_fourth_to_fifth_pair; - direction_fourth_to_fifth_pair << (buoy_13.x + buoy_12.x)/2 - (buoy_10.x + buoy_9.x)/2, - (buoy_13.y + buoy_12.y)/2 - (buoy_10.y + buoy_9.y)/2; + direction_fourth_to_fifth_pair + << (buoy_13.x + buoy_12.x) / 2 - (buoy_10.x + buoy_9.x) / 2, + (buoy_13.y + buoy_12.y) / 2 - (buoy_10.y + buoy_9.y) / 2; direction_fourth_to_fifth_pair.normalize(); Eigen::Array predicted_positions; From 4de17faaa5df5df0e80da213fd1ec93fb6d74e9a Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Wed, 31 Jul 2024 12:09:48 +0200 Subject: [PATCH 31/67] Concurrency setup change --- .../collision_avoidance_task_ros.hpp | 1 - .../collision_avoidance_task_params.yaml | 2 +- .../src/collision_avoidance_task_ros.cpp | 4 +- .../include/docking_task/docking_task_ros.hpp | 2 - .../params/docking_task_params.yaml | 2 +- .../docking_task/src/docking_task_ros.cpp | 4 +- .../maneuvering_task/maneuvering_task_ros.hpp | 4 +- .../params/maneuvering_task_params.yaml | 2 +- .../src/maneuvering_task_ros.cpp | 42 +++++++++++- .../params/navigation_task_params.yaml | 2 +- .../src/navigation_task_ros.cpp | 43 ++++-------- .../njord_task_base/njord_task_base_ros.hpp | 14 +++- .../src/njord_task_base_ros.cpp | 65 ++++++++++++++----- 13 files changed, 124 insertions(+), 63 deletions(-) diff --git a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp index 557b1532..d382b697 100644 --- a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp +++ b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp @@ -18,7 +18,6 @@ class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { void main_task(); private: - geometry_msgs::msg::Point previous_waypoint_odom_frame_; }; } // namespace collision_avoidance_task diff --git a/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml b/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml index 4c7dd0b3..af97764b 100644 --- a/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml +++ b/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml @@ -16,5 +16,5 @@ collision_avoidance_task_node: map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" - landmark_pose_topic: "landmarks_out" + landmark_topic: "landmarks_out" \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp index b4c349ec..7bf5e2da 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -18,14 +18,14 @@ void CollisionAvoidanceTaskNode::main_task() { while (true) { rclcpp::sleep_for(std::chrono::milliseconds(100)); - std::unique_lock setup_lock(setup_mutex_); + std::unique_lock setup_lock(navigation_mutex_); if (!(this->get_parameter("map_origin_set").as_bool())) { RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); setup_lock.unlock(); continue; } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - setup_map_odom_tf_and_subs(); + get_map_odom_tf(); set_gps_odom_points(); setup_lock.unlock(); break; diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp index 1967113a..1dc2ef81 100644 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -93,8 +93,6 @@ class DockingTaskNode : public NjordTaskBaseNode { rclcpp::Subscription::SharedPtr grid_sub_; nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_; - - geometry_msgs::msg::Point previous_waypoint_odom_frame_; }; } // namespace docking_task diff --git a/mission/njord_tasks/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml index c57119cf..76f29018 100644 --- a/mission/njord_tasks/docking_task/params/docking_task_params.yaml +++ b/mission/njord_tasks/docking_task/params/docking_task_params.yaml @@ -28,5 +28,5 @@ docking_task_node: map_origin_topic: "/map/origin" grid_topic: "grid" odom_topic: "/seapath/odom/ned" - landmark_pose_topic: "landmarks_out" + landmark_topic: "landmarks_out" \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 66776708..15ef198b 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -29,7 +29,7 @@ void DockingTaskNode::main_task() { while (true) { rclcpp::sleep_for(std::chrono::milliseconds(100)); - std::unique_lock setup_lock(setup_mutex_); + std::unique_lock setup_lock(navigation_mutex_); if (!(this->get_parameter("map_origin_set").as_bool())) { RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); setup_lock.unlock(); @@ -37,7 +37,7 @@ void DockingTaskNode::main_task() { } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { set_gps_odom_points(); - setup_map_odom_tf_and_subs(); + get_map_odom_tf(); setup_lock.unlock(); break; } diff --git a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp index e4083b08..18978564 100644 --- a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp +++ b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp @@ -17,8 +17,10 @@ class ManeuveringTaskNode : public NjordTaskBaseNode { */ void main_task(); + Eigen::Array predict_first_buoy_pair(); + + private: - geometry_msgs::msg::Point previous_waypoint_odom_frame_; }; } // namespace maneuvering_task diff --git a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml index 07648529..d7c44476 100644 --- a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml +++ b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml @@ -16,5 +16,5 @@ maneuvering_task_node: map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" - landmark_pose_topic: "landmarks_out" + landmark_topic: "landmarks_out" \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index 40965cf0..74efef6f 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -17,7 +17,7 @@ void ManeuveringTaskNode::main_task() { while (true) { rclcpp::sleep_for(std::chrono::milliseconds(100)); - std::unique_lock setup_lock(setup_mutex_); + std::unique_lock setup_lock(navigation_mutex_); if (!(this->get_parameter("map_origin_set").as_bool())) { RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); setup_lock.unlock(); @@ -25,7 +25,7 @@ void ManeuveringTaskNode::main_task() { } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { set_gps_odom_points(); - setup_map_odom_tf_and_subs(); + get_map_odom_tf(); setup_lock.unlock(); break; } @@ -33,4 +33,42 @@ void ManeuveringTaskNode::main_task() { } } +Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { + // Predict the positions of the first two buoys + geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; + geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; + buoy_0_base_link_frame.header.frame_id = "base_link"; + buoy_1_base_link_frame.header.frame_id = "base_link"; + + double distance_to_first_buoy_pair = + this->get_parameter("distance_to_first_buoy_pair").as_double(); + + buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_0_base_link_frame.pose.position.y = -2.5; + buoy_0_base_link_frame.pose.position.z = 0.0; + buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_1_base_link_frame.pose.position.y = 2.5; + buoy_1_base_link_frame.pose.position.z = 0.0; + + geometry_msgs::msg::PoseStamped buoy_0_odom_frame; + geometry_msgs::msg::PoseStamped buoy_1_odom_frame; + + try { + auto transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + } + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; + predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; + predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; + predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; + + return predicted_positions; +} + } // namespace maneuvering_task \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml index b6f3732c..dde98e63 100644 --- a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml +++ b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml @@ -14,7 +14,7 @@ navigation_task_node: gps_frame_coords_set: false map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" - landmark_pose_topic: "landmarks_out" + landmark_topic: "landmarks_out" assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct # Task specific parameters diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 33cbb9c0..8954202e 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -11,28 +11,9 @@ NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options) } void NavigationTaskNode::main_task() { - // Sleep for 3 seconds to allow system to initialize and tracks to be aquired - RCLCPP_INFO( - this->get_logger(), - "Waiting 3 seconds for system to initialize before starting main task"); - rclcpp::sleep_for(std::chrono::seconds(3)); - - while (true) { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - std::unique_lock setup_lock(setup_mutex_); - if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); - setup_lock.unlock(); - continue; - } - if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - setup_map_odom_tf_and_subs(); - set_gps_odom_points(); - setup_lock.unlock(); - break; - } - setup_lock.unlock(); - } + + navigation_ready(); + RCLCPP_INFO(this->get_logger(), "Navigation task started"); // First pair of buoys Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); std::vector buoy_landmarks_0_to_1 = @@ -311,13 +292,17 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { geometry_msgs::msg::PoseStamped buoy_0_odom_frame; geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + bool transform_success = false; + while (!transform_success){ + try { + auto transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform); + transform_success = true; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + } } Eigen::Array predicted_positions; diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index 9bb9cbc4..210497e5 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -37,12 +37,21 @@ class NjordTaskBaseNode : public rclcpp::Node { void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); void landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg); + /** + * @brief Runs until the navigation systeam required for task completion is ready. + * This includes setting the map origin, getting the map to odom tf, and + * and getting the odom coordinates of the start and end GPS points. + */ + void navigation_ready(); + /** * @brief Spins until the map to odom tf is available. * Stores the tf in the member variable map_odom_tf_ * Then initializes the odom and landmark subscribers */ - void setup_map_odom_tf_and_subs(); + void get_map_odom_tf(); + + void initialize_subscribers(); /** * @brief Set the gps_start_odom_frame_ and gps_end_odom_frame_ member @@ -125,7 +134,7 @@ class NjordTaskBaseNode : public rclcpp::Node { std::shared_ptr tf_listener_; geometry_msgs::msg::TransformStamped map_odom_tf_; - mutable std::mutex setup_mutex_; + mutable std::mutex navigation_mutex_; mutable std::mutex odom_mutex_; mutable std::mutex landmark_mutex_; @@ -136,6 +145,7 @@ class NjordTaskBaseNode : public rclcpp::Node { std::shared_ptr landmarks_msg_; bool new_odom_msg_ = false; bool new_landmark_msg_ = false; + bool navigation_ready_ = false; geometry_msgs::msg::Point previous_waypoint_odom_frame_; }; diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index 7687fde9..99775fdb 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -19,7 +19,7 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, declare_parameter("map_origin_topic", "/map/origin"); declare_parameter("odom_topic", "/seapath/odom/ned"); - declare_parameter("landmark_pose_topic", "/landmark/pose"); + declare_parameter("landmark_topic", "landmarks_out"); // Sensor data QoS profile rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; @@ -39,23 +39,33 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, this->create_publisher( "/gps_map_coord_visualization", qos_sensor_data); - map_origin_sub_ = this->create_subscription( - get_parameter("map_origin_topic").as_string(), qos_transient_local, - std::bind(&NjordTaskBaseNode::map_origin_callback, this, - std::placeholders::_1)); + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + if(this->get_parameter("map_origin_set").as_bool()) { + get_map_odom_tf(); + set_gps_odom_points(); + initialize_subscribers(); + std::unique_lock setup_lock(navigation_mutex_); + navigation_ready_ = true; + setup_lock.unlock(); + } else { + map_origin_sub_ = this->create_subscription( + get_parameter("map_origin_topic").as_string(), qos_transient_local, + std::bind(&NjordTaskBaseNode::map_origin_callback, this, + std::placeholders::_1)); + } waypoint_visualization_pub_ = this->create_publisher( "/waypoint_visualization", qos_sensor_data); - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); waypoint_client_ = this->create_client("/waypoint"); } -void NjordTaskBaseNode::setup_map_odom_tf_and_subs() { +void NjordTaskBaseNode::get_map_odom_tf() { // Get the transform between the map and odom frames to avoid the overhead // from continuously looking up the static transform between map and odom bool tf_set = false; @@ -71,7 +81,9 @@ void NjordTaskBaseNode::setup_map_odom_tf_and_subs() { } tf_set = true; } +} +void NjordTaskBaseNode::initialize_subscribers() { // Sensor data QoS profile rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; auto qos_sensor_data = @@ -84,12 +96,16 @@ void NjordTaskBaseNode::setup_map_odom_tf_and_subs() { std::placeholders::_1)); landmarks_sub_ = this->create_subscription( - get_parameter("landmark_pose_topic").as_string(), qos_sensor_data, + get_parameter("landmark_topic").as_string(), qos_sensor_data, std::bind(&NjordTaskBaseNode::landmark_callback, this, std::placeholders::_1)); } void NjordTaskBaseNode::set_gps_odom_points() { + if (this->get_parameter("gps_frame_coords_set").as_bool()) { + RCLCPP_INFO(this->get_logger(), "Using predefined GPS frame coordinates"); + return; + } auto [gps_start_x, gps_start_y] = lla2flat(this->get_parameter("gps_start_lat").as_double(), this->get_parameter("gps_start_lon").as_double()); @@ -125,7 +141,7 @@ void NjordTaskBaseNode::set_gps_odom_points() { rclcpp::Parameter("gps_end_y", gps_end_odom_frame.pose.position.y)); this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); RCLCPP_INFO( - this->get_logger(), "GPS oodm frame coordinates set to: %f, %f, %f, %f", + this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", gps_start_odom_frame.pose.position.x, gps_start_odom_frame.pose.position.y, gps_end_odom_frame.pose.position.x, gps_end_odom_frame.pose.position.y); @@ -178,7 +194,6 @@ std::pair NjordTaskBaseNode::lla2flat(double lat, void NjordTaskBaseNode::map_origin_callback( const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - std::unique_lock setup_lock(setup_mutex_); this->set_parameter(rclcpp::Parameter("map_origin_lat", msg->latitude)); this->set_parameter(rclcpp::Parameter("map_origin_lon", msg->longitude)); this->set_parameter(rclcpp::Parameter("map_origin_set", true)); @@ -186,14 +201,28 @@ void NjordTaskBaseNode::map_origin_callback( msg->longitude); // Set the map to odom transform - setup_map_odom_tf_and_subs(); + get_map_odom_tf(); // Set GPS frame coordinates set_gps_odom_points(); - + initialize_subscribers(); map_origin_sub_.reset(); + std::unique_lock setup_lock(navigation_mutex_); + navigation_ready_ = true; setup_lock.unlock(); } +void NjordTaskBaseNode::navigation_ready() { + bool ready = false; + while (!ready) { + std::unique_lock setup_lock(navigation_mutex_); + ready = navigation_ready_; + setup_lock.unlock(); + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Waiting for navigation system to be ready"); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } +} + void NjordTaskBaseNode::odom_callback( const nav_msgs::msg::Odometry::SharedPtr msg) { std::unique_lock lock(odom_mutex_); @@ -308,7 +337,7 @@ std::vector NjordTaskBaseNode::get_buoy_landmarks( const Eigen::Array &predicted_positions) { std::vector landmark_ids; std::vector expected_assignment; - Eigen::Array measured_buoy_positions( + Eigen::Array returned_buoy_positions( 2, predicted_positions.cols()); int confidence_threshold = this->get_parameter("assignment_confidence").as_int(); @@ -342,8 +371,8 @@ std::vector NjordTaskBaseNode::get_buoy_landmarks( // Extract measured positions of assigned buoys for (Eigen::Index i = 0; i < assignment.size(); i++) { - measured_buoy_positions(0, i) = measured_positions(0, assignment(i)); - measured_buoy_positions(1, i) = measured_positions(1, assignment(i)); + returned_buoy_positions(0, i) = measured_positions(0, assignment(i)); + returned_buoy_positions(1, i) = measured_positions(1, assignment(i)); } // Check if any buoys are unassigned @@ -398,8 +427,8 @@ std::vector NjordTaskBaseNode::get_buoy_landmarks( for (size_t i = 0; i < expected_assignment.size(); i++) { LandmarkPoseID landmark; landmark.id = expected_assignment.at(i); - landmark.pose_odom_frame.position.x = measured_buoy_positions(0, i); - landmark.pose_odom_frame.position.y = measured_buoy_positions(1, i); + landmark.pose_odom_frame.position.x = returned_buoy_positions(0, i); + landmark.pose_odom_frame.position.y = returned_buoy_positions(1, i); buoys.push_back(landmark); } return buoys; From 1d5d522f5c3936104c82553c0a665032b94524a1 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Wed, 31 Jul 2024 10:10:15 +0000 Subject: [PATCH 32/67] Committing clang-format changes --- .../include/maneuvering_task/maneuvering_task_ros.hpp | 1 - .../navigation_task/src/navigation_task_ros.cpp | 10 ++++++---- .../include/njord_task_base/njord_task_base_ros.hpp | 6 +++--- .../njord_task_base/src/njord_task_base_ros.cpp | 3 +-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp index 18978564..3602e0ee 100644 --- a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp +++ b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp @@ -19,7 +19,6 @@ class ManeuveringTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); - private: }; diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 8954202e..4b6c75d6 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -11,7 +11,7 @@ NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options) } void NavigationTaskNode::main_task() { - + navigation_ready(); RCLCPP_INFO(this->get_logger(), "Navigation task started"); // First pair of buoys @@ -293,12 +293,14 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { geometry_msgs::msg::PoseStamped buoy_1_odom_frame; bool transform_success = false; - while (!transform_success){ + while (!transform_success) { try { auto transform = tf_buffer_->lookupTransform( "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, + transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, + transform); transform_success = true; } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index 210497e5..8c80ddf1 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -38,9 +38,9 @@ class NjordTaskBaseNode : public rclcpp::Node { void landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg); /** - * @brief Runs until the navigation systeam required for task completion is ready. - * This includes setting the map origin, getting the map to odom tf, and - * and getting the odom coordinates of the start and end GPS points. + * @brief Runs until the navigation systeam required for task completion is + * ready. This includes setting the map origin, getting the map to odom tf, + * and and getting the odom coordinates of the start and end GPS points. */ void navigation_ready(); diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index 99775fdb..d8ed78a8 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -42,7 +42,7 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - if(this->get_parameter("map_origin_set").as_bool()) { + if (this->get_parameter("map_origin_set").as_bool()) { get_map_odom_tf(); set_gps_odom_points(); initialize_subscribers(); @@ -60,7 +60,6 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, this->create_publisher( "/waypoint_visualization", qos_sensor_data); - waypoint_client_ = this->create_client("/waypoint"); } From 06dec3e8832fd034ee3892f88feaabcccf6561c9 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Wed, 31 Jul 2024 17:24:12 +0200 Subject: [PATCH 33/67] bugfix in navigation, finished maneuvering --- .../maneuvering_task/maneuvering_task_ros.hpp | 16 ++ .../maneuvering_task/maneuvering_task.pdf | Bin 0 -> 120289 bytes .../params/maneuvering_task_params.yaml | 9 +- .../src/maneuvering_task_ros.cpp | 212 +++++++++++++++--- .../params/navigation_task_params.yaml | 3 +- .../src/navigation_task_ros.cpp | 24 +- 6 files changed, 231 insertions(+), 33 deletions(-) create mode 100644 mission/njord_tasks/maneuvering_task/maneuvering_task.pdf diff --git a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp index 3602e0ee..8efa88b7 100644 --- a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp +++ b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp @@ -19,6 +19,22 @@ class ManeuveringTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); + Eigen::Array +predict_second_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1); + + Eigen::Array predict_third_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, + const geometry_msgs::msg::Point &buoy_3); + + Eigen::Array predict_next_pair_in_formation( + const geometry_msgs::msg::Point &buoy_red, + const geometry_msgs::msg::Point &buoy_green, + Eigen::Vector2d direction_vector); + private: }; diff --git a/mission/njord_tasks/maneuvering_task/maneuvering_task.pdf b/mission/njord_tasks/maneuvering_task/maneuvering_task.pdf new file mode 100644 index 0000000000000000000000000000000000000000..94df6470062bb5ad4f858ba2095992259920d75e GIT binary patch literal 120289 zcmeFZWmuG5`#x$Sp&}qHEnU(zbV+v(Aq~>akSZYE(j_1uQc^>M(%n4_NDe)~NY}of zSA2f&uH)EW_Lsf?d|>97x$kwaxYiZtd9I;Vm6BoQVB^E2U0xX;$K;}XMd@hz783-* zWY>f^Sx~a;s+hjDFmuCX*Kjv=`_CtG_9j*qnCv<>=5E%M96X%-nC!1DY^`dG&q%6!F&4G3mEgYt7wFOF*2&+R$6`}czd@^HxC_y6nl|KSXXaAX*B zUmP=!8m>=w5gSTd5_D)UFt%E=!7@CZ*()9SP97Ots0>Z`-nMs&zWMNZ+8bH&GHt0Z zIEVY15_OjZ>X6@+ktM90hK+{G$(O!jYBBTs3voVD>kN6E;rl;qC}hrG#|z_-+e6I1 zjgr5m*v}1ke7kI)_id#R{s-7N z%}r}0h2dOktf)O?CMPGHJqa#x!fJbRDr|mbws;aka@)a$c(p(ptrxYMI{}#=744^O z72jVpoy@E+J|+Tz3tHXtu#ho37jy=co?q;TM|ZuWR*(~QQ-&d){&!?bRXP`Pf0tthVOe~fXTw|W!kP?AiT*0`4U#gJ zXtd56&Cw0XWnZ)Rt;tI> zl`!{QMq!DXu!_k`&};%+R|%nAKwW!b3DS&#lS^^3-G0;8%dOZKfrCfJCE zbLTVNiJaqI{o(dn*w{L#WVCMUTqtkbx63-j5A__=xxu;XXxh>xQ($r4t0myEmFjTb zAjBX%k)s6BJEzr*zvU6wFJkPoRMS{UERcB#2gX82<{fU7+)ahJHP_SM*0^X|FA(n6 z(&ZG%9u%!#FHVDnyVy2F9K@#9)P$XjIq%_5Ryzi!op+g1jyZ zc+#o=rNlDKgv;NcYTC(~$q+4GxVg7MJ{i3^$wL@Y{*@y-&_&PH$c5aHC_h==xkaE` z<9tbL4~L*W$&Pv-Q^*`Tu+T|U`$lQn3rhg|&oum?k3z+)T$<|P-y|DM<=jN(i%Y43 zbef%g3?n})fY641Q`f3*!e0ut&g|%ghq*};cecz@{wlz<;kO@a!_a%udL3zZ-#_&! z8Y=j`k~X&(1qOr=A=6q!JHq|;zi^`7$ZaGJrXMmt9$(S`FHj? zLd9R^V#%!H*j!iG9d+&g&O=*jdqUqwft8AoI>F!M;0Ad^rR08ZKLZ`x36Hg(ja+`ioO5(8J%59M2w8pg%Q21yd1C3R zds;6Fdm+_FAc?!Z?}$nL%yccPHF#nI;S9@KIdv zX`lS|K=55%KqOr_PPnVf9K?j!xH+yH1LE}!QEx@6a@DxEk?l4pd!rw)j8wA9?qkb; zKsQo)(QS#3v&LQK-LehI<-E7N`NSf5B0QAa=$q5~9qgQ1r#&0&QhDk|67LvRt@@{a zJvDh>VFHaOSCA*Okjk7Qqi!M`7xLT9GV&@?{;|BlCpF@8fta$+*fKc56NstHcBqU( z!dNUm+RL$fS?=8w!%sqYMEx{Y*uCQu37Nr@d5ZWam0ID64oCirm|!KuQkdr5v>%iphLG!AE2OJrcrBwh`#Tiw&m`2 zl=G-kfl)jxt(j9leLrP29+mQ<#(+3gS-TDxe=|Kh6119(lo{CK^ey-Wn0AloG&zI; zWH8%c&Ffbf-Vk%Z=qHTM= zvwJ2&v<7O3$F&!ZDfAasVY7*3Tg6cP#hZ2RIe%E1xWzq*6q{LH{k3%f->64@U{6TD z=ync&jn~Y!?Z{2Z;oA9L=&HcpjZuvao0zUC-QB z^>tUrv6N!_G5kKRK+ZBl%p(O4x#n$hgjyL>J!E$43EFUyeZ4sB14cUC#jC*!e!be% zju(F|{zoBetx1lXPHGQC%KHk{@m=N!R6ru;oObl`dxVEU(ShD0s`nfTDG`HTsJS={ zOLbpT;++)|r&iAfD^#erBQDMluXe4+iB>W~2Iz$ZAnoEOV~N3Igj_{IC6XnXQhJ1& zeVw>mOKi`X!AHr1-GR7O@*jUc8t9W8O3*r8B~%b1Jb(L3h1wr?d^a5fMqT@MV#x<0 zrT?_O1abPSzfD6DSusfpwVB8X0W9^Wj-Iqs+Mf8x8tCg^KlX0AuPBQ?Iivd}aNE9? z$I_J7N&H&x6WVokCI7YM)8u zF|=gN*Ys9Y4vOOZRr7S>JAjbXdqV~fS2+?)YIaZIMVS0{9NMUXjV0b0rPHh^F=m-| zT{dlSr?b7o;w|36ecF^hOj|RmY7E(kO2td7A5_i4LF3UF`NCj(cNKKI0b)HL#-n0+ z9=vp6K3-r_t4N{?8KtFfh|WBM>4p(qF_DA?$p{0HY9$$oUO&`FGxi$~xAO4E1BWE! zLomJ0mGA^MxJSE$EF)O6dxOcu3e=~m_ag1NZ}2#_Ol2($moCo(+$On7(SMv;sa_|P zCPZsGFkpSDUU)$xq6f5q7o2&cQ}2p{5LEe>@%iDC=Q zenT`duVEg6>EEd*hGq@T>39?5dyI3!2=pY|j~9U@gRY^~UWn9glPg^BLB!HMdA29A zpAstb()Z0}bQlOO*S;I^DKUWghG}CSm_>kLLN)|=7Kvzn0&fmKlpf@G){)iD#@fyw z3$fIf0!C+yeXaM_gsnpsNF4N7m()upa(Bai>hQde1nj~UnJr6@MdFf}N$8t8Z;&jz z$u=Vhx%>t~QY23f){F)wbU#gX{qSFt&FV^SXgPMdeXYk@RZW-{Fg{Gr0@+rYW03P; zlqN4buKz+u2zYVs0*7MdNTcCl3713Z8;aWbUzILdq1=bVZ0!M)?sFyS^4di;M}oUV zdgNJ5SSqv%@!+_`M;%q~G3Isd7Z4?1ho`Zpl_%y_1O_xol47cn^Yp?w> z`et{8wjCq8pIHVnKS?xI*^5$u$wYDiW9wD;YpP6 z9cmxT=>AS>#aXi}5^u1Em&C#s>dl=VYp7l>aM|4#=-GRH+FdEMXSsJ3h}DuyYAs^h zake86V|!3p<;!l@JXAXG*(g!k6$vf07FolIlMUTL?FWDRRh)KrF}UaJtLUfy#=)M( zCIqN~q20WI%FRNuJj&ueZapg>zu{wvvUu4qj9!OMH9PTX1GCQQ$77N!WcGn9R0yiZ zit~~C@Rapx_!JKm_O)J5F2cjbiIo?QScQ_7jI2eEct==@xS4y*okTOe-@{mhJS8j7 zu+y1_R~U0ESq2?Ie@`>c+sf81W7T%WuWi?E+C4k8CQbGewe){;kA5DM+^tQ>(9H4Q z&PsjQtZAsnEqniY_V?$&43aG~MH7q#8LU9zZ#>~$6wF`MDzJn8PgYI`;iARdtm z-@&*=EsV5`kW1PNUIphaV0+f;H1y`{7CrUzf6-_PTgPJeoVTo9ELg&w&4~AS^d$*Z z5#z8?W_O?&y$k)vqY>ev;Gwe;Wx1P|siTpT3s8i$v=qr~v1e=r<-ab!$)`UO~FY-?b?(G|Dae-T^GusXlNJl#8z>Y{(+^-)(>VC`mvwx8RxBQ#7QHmZW zqE^YR(RZt$Mu;Z%mbCBQ(>nJ9zRi8r+GKJ!R7k3Ib*rj|j=6W{wPF}{3(>ifOKc2S zjWsmmx)!l*ysAfY`8oGv<)-YCO>zo3ucY*fA|GhDu?Lw_-vF-aliQOD$Z|W83`;mgXU*zJ=sVcB=&;-4a z<+B)uxTVL+etAgeWQ?80BqeCxTLRU*EE|IpY+Hz28Y_T>l1(j00%o^s$qw#@Ro|0! z%*=R3omzqzRWyzBM+{5+7z6UNsKRSnp*%9xtnqbFaQyLqEf{m`HNdW-ib(K>VeDoBEE1uqHYF-u_k$O?Wa(PZ{)BJ@m{khEm3 z-fQ+}l|_ph@im78OJ1*eQ1mC)A_R`U9kG-!bwPJbL=fzdnXx1e`rv=&AGsich?D&i zY_MYa$biWxry(|cBpp*OLidyKB$aEQcJa;bcjBvpvZOTdR+`jWJPQ4Ch5ts^eieh+i(wdgusOEkT$#Ad_ zc6R%37iuv?6B$yobZUI0A3!q?VzwETVd?DCvAuS`Sx87$nRWA0ezW>9;l_^tN>uA! zqut39t|BQ{JhfugH!g*6uCq zRX#qW*Q-*|?E?ynLOVf;W2i$!c|O7V><4n0je8BV(|){}S26S2ghxPP&3&EQ4QzH` zZ_t>Fq&<@hHIP!gpy=vl%OByG&zotpZ4Umgm}uVx8+#HqSDx`&xsQb;mH8euXat z5(CtBbLNe#Br6@x{uL$JCS`5LL;9u91TpLz8=_P5Y%TLKtWL$k?6_A+taOJH@DDbY zKLhdYZdk@ny!Qp!aW_w6@D%u5vLT^&e$3SWHTP_i!!{jq6DIaQvF(Ic0K48qjJnjT zXzVjjbC_;!m^M*?X|rW<%%#nYUWGX23Xu1uLpch)$?b=)1s8xk>LGM(kHDKKlcd1Q18{`Vob)~M1LGSG+7=K^ZO91EZX) zn~{4tL__RJyh!TWCyKusqS8tOJcGda%wYG6lw z@_5dEvyh?MgWy+j8^wn~>9q9G~anAZs?biW6ixvXjYot3svZ`4{W&GKHzh5DT<8><{^IQmv7 z!ax4^stPq{6gJc0ua$gq3^sr(oQgBV0(%u3|d9g|DpwVh}2{(dT7Qp zoegSa?1}R}s!S3@Jo2Ug~09+%js&_2j^Kj zIkmQ*q^!|a-;9+Hk46=jEy~6^%~t64&R(0$YCNAa9-(#L*J>a=@86#KrCBoYaV5Z3 z$b{{ke@gEPS<(XTpD;Ro-`~~RgBX- zI-`J~ehP&SSOQQ+ePDIo^@+woh)5^l3!Ma1#{Nv0T6>WB7;=DO=YZ#j6e(-IyJ9`Ku{I>uia+Zuz?U9>sYm z4Bfw?wq$kGj)O8QbIbkHPJG9hJbF!!nKTq!ss_^v*(<6%es)_azdMX5u%N=zMs9Jr z@BT36!v^MYgU@Un*iFF$RLS1tWcq}ke(co^4m}Os5OB5uVE(tY8@jty9sn{`3g0QF zGd{X@_&slY#pcl3`>paqYqfzr+FR9^X_SOpHP;31wK?#vA&UPL0>eNJab>3G!VDMN z`ndhYGkjr|ILqyk<>86F~|-%$eQBFQ?XgA9NUZ+H8mFHfK)kd>@&LobR9q1z#RTMF#xx(q)V>mf~#0y&FhZEt%i6p=_5SV-@pH~Z`U}&&8bD^hKO>n?ro6M zF1-(b&Adf_3)$aks^!n?KhI}BV<8!Rz$u}_D?vwdIsU4;*W!^P+nZO$0m*Kp%NFJL zIGvG)Hz`D~@K-iGSS|=qO49`cg!n?Yga~bh;UN0ltNnpuhkEJ>P0gxZ)S>F0B!Z=ykIEqlDkN8ef|Eno&WE>V4`<(Wg!U% zaiyCbZ%3#4rrPTKTfMTG!Gzv(NJwjc~VI>nT~Zz#53nhvnC&H<%0 z;oy09<=54-Q-FaD9V9eLD${4*{%-bvxFJQPK`!~&jkIcH`puW4Tx^EVluniV6S2j1 zH>p{;MGpl=^d6D@_Y?kk)bCNHwy@Ux(o6twVc!(rE{*voJYJANfof}R#qg5~@Bj4Z zPuDc4l-u-LQ?4BWe)XCI=+)O#>8iv12>>zlXzqHxi}&RJ^b7|&{ZSgzDGDC|ncb4H z{ty!g0=#5gX0=0WVl$rbj46)1_upaPj=vzrX?gf0*aNIZ3eC$a?tbpacK2cQi`maF z%K(S!_J1*u3T4x&$$Xy1nAmD>lK8nvz3Ip zMUkJudH0lI>Dgb<&Nno6>-P44UwlJVk8Z2hJNr43ZGW%bKNz(TK@a%%y8rd&|GzVU zi4K(J>}hY@r?-rG1vHSH-8Tv2jp5^E1R^gXKKC}FSqgNGG9rhH$oVn zb~}m73Am5N6MuTmp3upgalKejb<^9fp@qRsiJMC`;qKd0*|0b&`XRi>4RHE6F7szt zMc#3gZMXe?M=d!t5*S~{6LLeuZR^@-WcHTZ-neQuS*dkX)b#!0IbOg8Nf(dAY)k-4 zqv=g^OKXYkxw~kkQy}l6I|--z@|`xBp9nZ##-%L2)x3*rtB~`GQK; zYac_4#?N7&rL+mMV3LrUGPa9X*YIg|=$K2L4mMGfo>wjXPdLMF%hU^)u z4rmbZ8#2&;;a(NHTy>Y?-*AVy0hA>-Ow<=ZmhhkRu%8|RLq6;nq2;2|(Q}yqQ1YNc z@gAP&7u_R12K!ob%PQyx_m*evYq+(5ln1fVrjh~ zOcB$_G}c-YEt^%k-0~dVz6`h^0O9bogQAUF%=}+h1DVJRdDE4m0D4FM*Ou`- z>~QzYXkSgl zn}tMWv^v&z`3av|E%~-R# zD!<{Q^oFS-UE(EGL7z`Mf-cbj7HYJ=Cdk@4<$k2`&2$ihS>no!KW9$1*5=EtTe@?$ zOsEvW+#?{gL{ZEbPi3`DHC=Z`HQKfbSvxNUR4+8s-dR3It?q;->!0<~gV%XMX~(fg z=vT*~w3dDaB@>BO*4?^=0>gV32CX)v!93jC6RlpsTfYsmkuxFJ7AZgqSG~!>1Qu=m z2dlQP@9L!)Q|(2SWf7qnTAiFa;|O3TY4hH()#4q zj_KaM3^Hk;8rd2YYV5a(m%uQ~$10 zuXo_ekyt6dPSC>WQmS2DDiT6gU0K?VWvoP`${(fQ?2m_=pgy^S-8s3q@WP}3QJ*>% zYHINbDF}SrK5Q$mSwgGnHzUxPP>Ldz=s`WrqRM!N9BGL+1vD1=0Da%9_TAa7Xt6;d zSU-F7kk3^i;^S`P&6Z~`-$zu`s9T*Tz$3x$v9^+No4z+j_Vx?+X;R-jXKFD}NJ5Hu zFKp1hCp(X%mo;F)iA${A;IszN0|^SHjS%8LL#7;l-1qX--?@sD_=bp$A@0huhkFUs z46FwL+*8wEe0$*WGpP;$fG|)1+t38K7*Qf^F%axB{cc-LX#@87cCIWekKK59Jm8!z z=SO?KZYpsfm$~wW@YKaJGyKOMPr9$T!|Qdt)^o$>Vb(6@7D#T$l}P&zIwl#U`aWLW z5&d-;fEr#Meq%aHOD><)<@?cK+j|J?`Okg~2S=d%AW25kzG*utZ3!W8OZOPLzf@%G zx*wnA!h)!^D2z&H_E=Q$nYMcQWG%2cCbmN{I&Y;ONTEv^;|awqAzMq#e9qO0#^H%0 zbytNszUYMQ$FoK6yr$?uw5OPSd<@^DI!W3XF9D!$a}AWCIy??wJWF#0i@$lc1X4$a zAw3hoa2E)TORKH`Z#09@Q0Vd$_*ojB2!F;EU~8c%Bv_?Io| zX|z)VYqK**)Peviji(lxV^}aVr(_iHrk`gpRDbo-50K<23CDy^q3l}QnVxJz+?r3S zw?x{UdRYChJ_=y#$#}>q9fw@P{nKp$^-qq7`A#LPL>?7D#QkG4#v?$u1*D#sa1p{Y zvs0Y$0PhVpESemj%zz4jG!xiWOve{=o4cS_h%R`B5r4nh^hS7YdK@2G&5Ize^(`Jb zjTit(AE?h}pSed-pF6>B-7-bCT+D@%!-hfdh?+{~dgjy%KZZ!C_G-*+v9TKGe^Vyi z!j(;;5hFmax$+E#{4RRg)HUB0j>9p<`xI|_m%b1ofel*K2f&~8Ur3-*!*R)~=!aTw zL%4$}R$sJQRy~#YI1ZU8nO+B#wvtjus9tostojCY!xhs|`{Vy;URJ|$?%Q7k(PH@t z(I6Px=rIJM0U7e4;yyxXSZzOFTjJ;B91XU+(wXH&$Rj%Qc*NsU1P&l`omi%vYgRG$ z-p6+U7zZFRN;vCe)-?Xkngq%;PgeJsw+tE7ct86#FLSHrI%Bz`J`PZ z17ed`lRRsSP@z3zL`Y9y7|5Q|yvfs0UuGKXfbL63bkttlm9=Z_-6TAVH4~5iT!z{O zyt8;1i}A2Vy5)Q7QNSet_q`V?qh*e#RnY!=0E0d-9O82G*B&yPaZtGb{?&ha`{P1I zXo`2}S2#Ht4P&(Zq{VmDuab|?CuQ-6&lTwcrpd0x`Ctb;iKvY&2tQ!sV*tG@9COlXP9-EUgc@>|Nce{X z%O%tK#%*l~pu|wPY?mQZNX)WMQ#)?S;66ocsY@-<{mmcWkEb5pY)pO&`L<+rsJry^&kA@3s=}s$k<9 zN#hzCy(;Z@K)mQ;0_1c5B(K3fk;aNc+d#Grs7F8Fe|4Y{wQg*|zE1T>pFHdBC)CI_ z#MI^nbPnh}8(9I#GB3heYzXyqvtfN`#w7||$NIBxL5^Rrxu68Sx&5qrEU#*X_hPq4 z4^ZnVIWkQe_^o0ef^x22>F|6-97Of?d_$O2PNPjOT}#~ zab4azT3YgozMNy9>UbkJyAA2V^fTl1Yg4a084mOSODk)*byM^?h73M46$8^xLxp8k zbenB2zhwe;*FWDT&#+7)vY5=m=vL37dAcv4;~q;i!33gS{Y@eM!%~ z)pa0lr|}v;9zRKb@_RzWR@F|q)-W%lP^lKjB@OjmLUbUib)$&Xv0wBfUYf%huqD?d zd865gbei`!jMC}KF<-9&E<$&uYIsw8J)0h2uCzU(BR$Uj2YdzZ2`x%7GlVf~l!~Ov zWhqDuN!oVBTQ{yJrao zq-4OPQqmuuNBDdy7IfYfseE_*$$W~ z5@ok;FxsdoI4L?b6{tjj6B-!JO$0~&{^7re*Lv5Ytg`F`uS5@%XPnLhNUM`;vH-|F z8!*re<824#jEV9YAFIo7`>R67tqW8dKw)@i28gd;bf%$Wx`AI3`nIW0WuGV8Spe~+ z;Z^Ja{msip@v_{dn1wT;*@o{vZRkjupHE$~YQIF9_P$3kXmkROzX_zX*J!^*mEZlE zNHg~!vbb_7T|*Pb?w;J`gnN#mVWRR{j73Dmr+A!GE2 zZNh#BO(RtkmAQ)JsX#^n+r|$v*;-TFB56RdJe_@-4q`pp3V3$($7MHRxqU70*#W|R z4%%m}6BBL($7C?pbCH!0!wqQxLZJMD)r-nL&6al*(UY;LOHoZo@OyM=W+P5WwTjk2 zyGe3-Hb;+^T(kZlkmMDESl`#j!>+?_a29qz)ii;{wf#D-oeD&;;XGk91&%DAk0=ab zq0Hja)q32_3K86VnqMGKpM({er^2OW;2<9L4C|5cQqpcC?eqA=uexUNQPEys?zKPB z(ko;mES;L#z8gNUgmo3<+6(ONej1HnSFwyH!rh!+)JnFyprca)k@k2OQyPzZyu9irrR!KG55E)UA+2BczM_H2u;;|x~gM!X<(jsjc%Lzj2IvtD)a4cI{73uD(A zCkvdl)o-K}0a6XXQ_VFALJ@zgMvSVz5Au<9pU`{vvw(^h)0};?k3i;zS~3ISZ@doR zyfYo1bUzGass82NZx&uDJ)#fK`-xb!nPG90@V-?+*9I-+>GhVEt2&b(?SrzSdNp$# z#^+;{m9blT5psKcy_-x|q$K5Eq`R4(=qn7Kd1t)^^!1>IT9Uu)J5o$r7`r`mpd<47 zhe#!a=8vd1F8jA{*0@Tkyb$DlAk4NlsOJI0pRb>(lZ|(N!AV=+ z(nIdXx8@N%(wh@R&*u!Z@&dG@Ay(DC#{x)PIBjLEXsIZ-by97ILHMl<=`;G~G%B{} z3VY>prS4a;ohmh{Z}9zh)Z#|(-s|XEn5SjFT1X1j^V_%!F+roZAUJCwM!v;`gAaKt zk9#EZoF{`bx~)e%{_I@_Cz+LRz{2wi%K#R@Og+AaV{=DU6A-|yA`8ZEs>Y*hYuu*Y zEi`4~kSREaM&i<6fb%34jgG)+BGus)iuTQz+S@}$$alPm1N`$lHZWK2)D_Y|ip9I( zDV$90sCh2nbWkgAGO~V?upj@-XzhD(^`+x%3mBM0bH{$oV)km7x>P}4${%0|2%ueB z6cs;ItVs_c$O<3@xgmN=W_#y3>mkW+!JD>)V8>cog3=kEZNDdPHeu30ZV8;G0*Xd+ zt9~uV@7Ep18`-xl!SP(OfXY&0P^{na5iq`s?&SwhJ~PA-QkEhFP#tYU$~|wA_Zp5{ z)D7Yvfy?C7i|hNhXAgcfN(>JLbv<8|Vxf!!H=L}GocZ4mfP*BR zmC54@?5z&6cyaHZJ{Zx-2C#vFu~;hCRe)9I%k(=38oo!hdl0kIG}KB|&)~HGBKyGB zBd%XY09)VjkHJ5`m$&f|Z>SRx_>+1510I3PYy`)}%1Q1>D|kI|Xl;)tTpwyI37Egg zXdH_t2TF}@!|0ve|0K0SNwC!9rQtn(+4~H* z8D)5-C#|?L&(SZ3Fq2O8P<1 zba?!WV2Ju7&Me=Bi`-6^6Q@Qq75^_+ku#k7BeDRHq~lp~720HMC{Y|Fk*+mE-#yMC zamn>`=ZEs-VDH%SvQqMmMF!TNrMq+IhY(eQiW?qRWSvx1V$2Z@$(ne!Ae4Kg2y`Yh zb%|k71F41m3xTM9tsCLDsmXacw4p}aFtgheqTtO;J8gI{`WvsXoM^!H@R9qBNxw3Yk}eR(F+ z4z?)5?HWY5;+Elb_%X~(FR!x%%eH+(v9oAHt(inWj5rQ*#Lg=!e)I3$$oG#eW)gUE z3T2}=)W)<2LSHJ|Cwf%OZm=| z_W#ZnT=f^M7MEH?-HJ7#K~ebvdJCoJ)AsaxV2>QtV;qKyjRm{C1% z7<7H>tsBb1#P_1r^*YC@@_S)%py%<9YE(3omdyw`U3~aqf%)&!oG7pE{$$Ot@ikrC z>8GtA-8|Ladh+vbHmWzKrxba*LVVuu!&s2SNytH6Y)Y1NRlQBD^uCRJC`&j<#lPd& zIH-iVi99ARi%p-iC8IPYx>n0}sp-|A?->+d%T6k?PO)kY`>}daPb-~gISZYTEo$xa zl_Q>*MTz0)upSHU>~S`crOW%|{wY>_cu7`7b7E?0ev~X+-!NHu4pq5<&34;SUwQMx zFbB7d1KdGvktuAsPmlbzX2vSMF{!lVOs_cm#-t@tcMWV7|C)?D-Rix){5nApY_Z;fNod&;erX>&2cSA9ok z%9CI`oHju~Wn!f$kOUmYv!+Urv`(GV)C)bB&I-u6xph)^ymZtNQD0ecIF{8DtUi(& z;X}#;Bwh6d#{aW13Q|_-ETwqKLSAq_b^TOG-8Fbj*1Wutt3mA8tfP0CtGAL$cmj2L z&J~#oUgWCg(Uw}R#eeyjhEe1+KI}S)ru?Lg#)7EAKKZ`mjDpH(VoMYc{5qu%GN!}6 z(}z&roBX(|??R{a_2ZW6c$l`NK!E7EwYbRNL)QD^eOBE)-bS#Vp4YdV?^U!ob%OWQ zr?CYmLFBkmqM)a>I|ogrHR<;ztt%53iMxO4MRmPFZ>!hmv%jeakNwb3X6kW>NP{Uyl!LR*R9=oh1TUrJMO4AhZolADm^wVx4Twx9iCiHk$SS$w#5E-EQmzPEr|#r|syD%t8|d|_pZ z-C}Ej8uqp$%uTFb?3zFjmd!hii+-eao>h^h4$|Fa6yJsa+yx|?iM zw_Qln;|(Q~WWR^&miF9dwK)h+ZB7r-H8*T(&po(XJC*5MZBVmvajZ44Nyn;y-MDeJ zeqxw_)4QVEIlE#Ae`%g*X_=G&4_{KGkFQH)*xjhHSlQ z980Z;yw;{d~e(A}z zqt7|R23hUw27O>dFip^JRwC7 zN{+5}&r#~K=BmvNF#5YDaXmiWJ$i>DJsU^dQH7?u(gR%O&cZee9j~C41j!|(6Y3F~ zt~TscuW{r+3R8+>7c|sR9>>8~^}4cF12zltbQXQ)CP&`=hzDoM7b!fqG=x&Y84gwgmnO+i@bWUcxz@{h zcn-cv0Slu&$~xC6i}1$m2>y=z29D^6o(yQoZKe?beOW>XCM}1YlaQED_l~+*nXgYF zchZF^_vO6dUJyb@l}z4x=fJd~V<>J>exhrz%V3Bb7`K_jhko^sVeK#Us(uvK!}As_ zHd}>ylhN3&md2?q^q6tBulG+pPFP+_vU7xGV|(gCMMZPif4$b>BuKgJXA!Gn80PG9tSt3Q!o ze6E=s+Tl@Z-e0Ah;aY;giGOi&7vWlrAb#=Do4hA)zPn#zXK?`<-aJCB_7YlMUBQco zaZ~AJ)9Al&mb-W>BI*eai;iO_W>1)wktt}_EfNIzw@kmve$+>#O`ZVpm!o_Fk$;Y; zdcI9@_NH6u?GeRQq5bDiSz7XgEYvk75kS}LT`P7&C{ATC)|lht+u0FZQ@<|Rta?^v z9+e6e4YHz04r7&s_zS%1nqv9MFwtSYQLLfyS&>J!XpL!M9ed5)FQ$+d8$4Gi{v-Y@ zg&WXX7d$Rl(RN7sPS-Xl-@zwZOZz^|weCw;$O9e;p45R#1%x3OaCX~y6`XY>4K!oq^-#d@db5XD+fA$-dN(UA z(Wz}}^J&I%Hw;&AD)WZD3-$rzHRuJ&D!zb?$DJ}&!vVaWYwK)>%ANLi*vz-#1wQ1% zzco+h*TRUTNOmiQkm|~>HBMPh_fpDbWM)sLUHwNl73g{LtU@id=6r5EO~S&>Qgmp8 zP03;KbyJLN;DAVhV6R8g;-SE6jG{?0L&K=3L1vv=8{^RfXa_k>;cC@EXn=XcF#eu= zs~jV5+HX3FjLdOXR&rhkyoQ@B7RJ`*-%hjZFA*GwQQispOrbA=&C~c+gAu=~W4_Uu zsy|R1Izz_8?FkDFO=AM2o9FXPvln!O#^S*hVc%)oxbPhq=pjzpX_ z_SI3~CsZs`Bw=Pf@(Z6glb-c&v=Jj$Jau-OYg`sfJG+rAg3eZhP=1v)Nb1Q!Oskq`55;i=&eW&ed8$sU8H)Y!V{4#&5=*I>a4;Ad09`oCwh5u9;7TJVGr zu_MZL!C3P{~=VGu7z!+BVnS_#=WQ)?%^z~O9;wZV|S zZldl>muDQ764+FcU&HDrSy!w(kAs3!(>-~S1IeUp-lHi6bZl`4mC6OEB&A!?U z`_pxMi)m#&h?9cm$R_=Qj=)Wi+hPPg45cOH0LONS?9-1q>{!rlyQEOeNW4$vwS92N zTTZIDNYs%(=TjVUeXH~mBWR9}1K>UXJ!Zk6gqfCYD!rfo8(nYV7ghJ}4dZP~-84uk z3P_iLv`R@!#}LxpIkeIt-7uhZBS^Q@(A_XJ3>`Dnz`(p4f9JgCeV)&I_yb_?&9&CL zu5YbP;XwDU8i5-x?@rEgy4VB)fr##dye)3Z1-L*ii{Gr4Vtm@vzY(PrJ+iOu-Vk!I zDEq2*q`L&~(+)k#@VPSht^?%M9C^~bgbN({c{;0{yJprh3>MK%n1<=LH^9yL z4p9Z?<|Z{1IPrF-NxR#}$u(Wy9vKYM1cUpu^!9k%Fi93wlIm*n{c{itXTeIjJl{Xq zig{Q1im-nPFD2Q`=ext5XJSJ}4W4ZrmO%%wmG%Q5#z*42n*+4C#RDCFD!ia)HrV8SI+xEUAjO{kdob`SaU>GMN zy!66Gh9ts3qj&`=B@oTb+{yUWOQ+zF5-w~~7j5(Xym`17p{-yts^_fMh<*m7eL#ol zy*@TGt258RMCQ6ti4mQzKukkm2~wd&5fx!wtzfW9(XL_JZ<7&q0>9hEwx5ZdjWx3! z9sM79#%mOEqJW`-0C(f?pl2Wl;xa@v9ootgs<%7b5CEtAyT;nR7!+o>oE{$)JTkXc zj%EZ#sr;N>?`dN%ondQ_csJUmu>GGa$C0H3Q%DFhH-YKQOd#6NNu)r^obG8tTi|EyF!@>$we7q0>dLg0LO-;M@o3SJYBd_8Pe_&iZ)w5sEEx>8I zqpqoP)J0-Pk}3?QYbAZi9#e*O_4ni`mX${^9{4Ixj?e>J#HI&WSqUz00lgSzcip3E zWB_sh2N=SVR0MBndHpmtflot{r0nY@%<{p9qVq@T^GVfgG3q;i%$uD+;;l|J3Wm`a z>UJZy3p=Pju*w;GNnV^EWlc=CGhm8EwX_u%%A-iw?Yag{$joaJU2i|qkil`DI%AOR z3wUA)dR&e(`aX#suWHW&uRZa z96iu~-PVFiB_4I^ru~+o=#EQlUY*VvC()c8rIx{1PX?AAB&N$P$%TzmY+bE{dCNqX z(`KSwi5_GTDjxmQq&VwZWbur5t>n?+pbH+pcb z1Q!-$bCWcNN&0zd$8BX!r4kYWRaMFl{x1^^xnV@oQ-9&Cj={z0T91~r<-zBN7soXs z%y0oXSLmw*$BbPgr~Uf5RGj$z!_10blD5VDJLj_?Yi zN!wt;Qv`n)!zM(;y^%A~jejOUj+gJD^NMfR37Rx0e2qj}HSc4!H)YSZwb+t)4cW&U z$;nY#^C+|hlCs;*j0hQ=-eq|m)AZ7O-o|X(SG|+qZ+jspZ4$OffN`#{z;x0hl9BHO zgssLEDQ;?4d!lYGFwQLE)A?==g9B~`iEQ!1oU2F^L_I&#)=MFIT8A{p%0JNCTV%v= zHKkRj9ax|C%)=wbr=HvLPcW$ZB$0kH{Pbz`fpWU9N7#P)e>xeRXl5Z_@P3RNo*A9gYsWpeFUF#eyMgt4nX>)(pSjL^ z>T1D2jrs)&a?&NfNVXjKfxhsKcMta%=AaYtMSIV?71>bKd{>|eh|-MQ4xL%$x95{| z^sSD5aXz>^YSLQBaM7b$jmhB7vq&#$HU6hV#eysPJho4B@FVGYw0yF`CSLEzE$E16 zUL;m7lPeZal8?!~C&-#ag+=lmGpOS67zzH}jRx5I0w621c1TZ|z@{+a7 z_KdicBT1E|fk$Bb7|b`79N26_`VEHHW!=s$qV(zxF%UN;iHip}NdKq4V?7lb2qlCO zV}-RTvAb{Vt{Y|yzv&Eos-A4kyEtSC?CMnoNiQA=g?%4C!0ZlP&P4atSy76W^XM*1?aTM59zBS@BQuF;uNc33i zL=>#f^r_8HQr$n&e^yNCUKJvpWMo(5Mrv}aS~qU4@n}Le zvy>Dh;QA(kmTP~@+GYhs_PAC*Dz*OD?wVamR$b97mxE%jtz;^KmU}bzVwF0C2AJ}e zxitwMy+cNS_JSF4pz}Rf-|vADv%E3N!^oBN%$FgU(P<^AlgOaSNvl>Vy?$e_NQX}w&2eunpErq+q5t(=DzxjS*VzQF4}sm~6ot9JeE z$$qW@ASiq^uGX%4f|er#*#Jn}yVz{^>y_=edT~o#W3M~F3jf$Pk)->`!L@+5*oM4W zA(p;YBM>hZm;&>Qp(LJ{Hv0esXi-CV4lSY9&NjHR!j=JYL8sqxwFM zZOpqY=64l>NiB9?s}pewZVmW%}r`UK@@@TXi~x-GrcS zZ`7o$weCxOyF74*##37foD2QtjrH;h*yazy02?5EMtAFB{Td{UZKG(|f4^aUe)g65m<3zL(yU-ThkHA}z7 zH1NEv->rH75xYz~dn}b(-qZ;r=SvN?u|tt-icHPLW)z5VXl(8GQ|s4RWHo*q;LY$6 zw=8%z8_Aud;-$*cRT`ii|r1eAxE6L0jamc1|MW)Kl+@zmM*=l6Y zE)+jVQPZ@Bl-wNJbfkI&Xi`s;n;F>_v2(z#W48rg64H} zE_6)kI@IS!^nGIPq2C_~272&Q(-;Wh6;6XoFT00!8uD%q@7J|dI7#h2xZDQYlW)CO zd`*wrwj68bp*}wzrZ7k@wa?{gr8RcM*j_-77@PbvWOEq<&1>;~sviJUS#`HY{s^z# z@_85ZEIl9odn`tW8LZvBkxKuvN+KZ2bEz@qHHUfBM`^*}Grk~=E|pK#)3mxvVst}D z(sa&UP2SF2!8th7hNB}x$i5%%F4eh!3?7}qAW|_)_}Gvj(|)5uIO?X!?S?qrn;5yB ze5$LpUonFc02YKq?l6sRV(lEWkgHqZNlm!_PIb1lZcz@cr{6=5kYOnPftI!`NKq&8 zZ#kJX=%!|}R+GTRsg+)&(o0v!RJEL{#J-Vd78(SxZYA&-lVIiah$)R}Zv&aD zp?a!7*M*6xzcQ?yY%G^j;ODRY7*lH(4@i zDs7}~O>goVDB|PW7X_ARPSC}X&s?H?bz(G)Kj2GGBoN^+*x7FMky5C8W`nlke*hc@p;m$HV~W)5p<~h;^c9oM8$nVDp`@!hq>el3PK3c+ zf8p7i`C}e~arQL&?ninO!}mYW-Zge4@c@h5Tcn^#5xx!7HjbpJhr#A~us&F~&N3}8 zCF5$7A1;&3Ju;9aP~#E-G;$ROv|HatF2m2&Dhq4vjGB}ws#sD%7t0YesJ-|e1x$&? z0XVULgeBpXU)FRn@|Oa1-P&zJgNG~a!+~=9ebm35nYxt+fL2F=h==CS60dR3_|f&@~;I53?cpF#Jy$9+C)tMViEs9!i6+zvJ-`esjuz5 zlu!SD?bcQ2OjuL-?OdU0V*>bh23#_g+dL_-H(txH)7Rhe{30nNI3s!>?%fFBls`=O zQeO?>qlFO>@6aagy9TKCR5IH>Ch*93>1Q;1zZE{Eh-YP8!f#neZ&>b5``r6Hm5xS9 z_%cC4m+=2UV*NXEGsTKqCAv6`3U+fY20v${T6Q!nN97Zr`KBdQx=D(eSAH^C@T2Q_ zcpuCEqxga?z0(5Tjbokh2){*Lo|}WbsRReJnrMS3zcw(aH~NYR^wKVU%b!YQW)5)s zMb*i?t|ci9b^j&-zcd9AzTgPV|K>}u!_lAMwSH1dmtXPH`rn>~jsYrB5+Il*!;@+x z&o9$yoe0PiB+iZqnXiwV(K(CQ*?8;bo2h-{qI|Me53k+ozVDQiF12-upV-PW zSQ4y7a~I;azsFV^nM^1gF3v+JaF9TskDdaCz1;AWHbIxAjIN@p=e9pPHf&RcfsJ;R zx?t*|WcjTvebco1Kux1_`WVS2PyRZ@GddCPjuCSWzn>SQt(;_6;^{%6K z-u(3)GDt4bEOw;cQhoUkzu(y>e-^yrKZ4qj-k5erJGPt*bsBq3)~=G-jRPFQ6xnv2 z>fFhIaY(f@ClME27^v=kdNy~<`rI!RH*aU5Tz%d;! zi;jJtj!L$uMO6CkL?M1CQG@2e?>GDSU63N07j*^LnifrfUr#U#{yStcakLwB21_pw zHCHFohi&r+1{kWy68jyrgZd7KcW{tOFovdD`Zq@3hmYR9g)zT>)N*Rl6d7;bTTu#^ z$EowOM$;PjN#EC(F4cU3R!>~G-`>q3-6!(OP0yY{r`w;ES*cE ze$HnRtLFO}=t^AUq||Yad#LxflrYQntxe;Si6NsuAzO!oXqx?w2FfjQ;?1mM_YLU| zUc&t=7Ig!;+yN)haSTGZA<>?=b*@$DU3zfc%g9hh;RqnExJrV3`_e!&Sn+nGBPuNQ<^)AZ{{j{;6odB zy1mne#%2Gr<9lPi>iHZX0EMB;R1GxG(EfeBTUdlY?|L;dEs|AtU>DWq$=IUz0vUzxgZ_|fG5C& zR=0;v`X9f=JmI||c+IT0&>PEBiu|y0KgNE(Sa_^@Ax| z4kqA!8tzvumY|!zDYCI1=dBIP=(1J(0&I}DbwH1i!5W)!0WhD-?Q^v4kNdXe<6C!8 z?TitBSYW4RSfZw^i%dW`IIBpEV_P$Xv(E~>3t+7c68aM<7&XIau5+EV>XznUVgq^!cJy>{pONB=<9u#EYqXoSFB*P zCLi5aO?>LLKN9hA8E{%&#z>0jRhxJVh_+S&B&xom7?Ff%nqYk@E-j+II+xlEGJkqRg2&BFYQQnf3tjEftSme<6fQ zWicZTrWX&%P&qgVG{n}gPpn_^YsLf-V=SgCj@@xqjPocc+(savWqgIo8X35O`$kYc zCYy*mi)^?XI1+1xwyRjVtmS9juUG#@K|cH3lO_o}u$<{1R!F)TkDCYa{E9`sKBA->z9+@D(?3nbrW#fU36b-i$lq zMD1e|s?KoYemGBmeKrNt`1K_$@!GDU*X+-RTQw}nb>^i|pV(2UXrk^$G>{eGjm3Uw z`vE%2?Y!sJ1HjkatM{p`g~lSK_jOD!!_EZW&h#6$_m`I740!yD!16v0{5{s7@jNsUF!~W0JhqBh}Wh7R7|Sk1cZm zz%>37j2v_m$CQfFd+h{Le$%II0>FibFQeIal|i6f&b7jV*fv@3Ltys+kryt`= zE9!5@s+BTREK9Z{%2Pn$KDF}=RJlBK2$e*rC1UCV>n`c=N7c;+6eYE^w>+BRE%6DE zDNqzaiuB%tdrKI;JLdul2(ip@+v69yfXj{`!ijM^H6X|I~;dNcu^qH4Vvv*qFySZ+G z3JyW95ErS_Z28nz`XMOM)Md#@WTOU$B|>N_J-ysTW3> zrK*Zymw8@&5zZv&PkRk`5l^vne3N-vh#Cf*`5l4Y+~lOWb&bl#k_iG)s7gKE{gKsd z=TO`M9!r&XJ#Bi&Nd@6Eo8O%Js@us-BH`nY|eve7m}6FRdDmRs=p?^N5-<1%N&V_>$cg z>GSLLZxodK!?zNKIo-YRL4L{Dk)PNQP4|ym`>(<#j-2vYzrvjMM?lNCy}jb@T65|0 zabz&DvISIE(9}iI1HH31=`Z3nHNGrmGAHKI{)rc>m zQnvk2Qkcx+PW(%?hzX*p_|}eGUF(p&o(1><+?zVAddy`1vb$ode<>I_?D`B*=+$>M?!jV;SpWA z0ztuMyOc62r_5d63ziY!z+Eo%uk$WLyJ_P1_nmbKq0kHN(0Eu7oSOWc5juZUMo!87wUgqOjxS+NV1zKqt1$Hg41)}A8m`3eSdxiZn z)~|^GqhOwSxa4({g5F3?1cf@w-M=_&Xua7mp2{QEjtg_Ff5rqqnRBl{!iO;@vKbm% zvIEm>$*t=`*maWL8$>4n%4)l;^IV+_)$}q-j<@%1M68tC%`kERpNxJ~kWsOi(F?se62sB=%EE5WWxp%n4>9z={y;GeQ^v=X z7oBMP-%0o2`lK!oAqZA@k#* zSPw2&p0LZ zgFWafVV-3Jf7A?h=i(vPAd+;CHugNp%V-imnzqdzkus6WrHpiW}LLVx|?E`=a zYer&M=@HVRX?FBY6H*|cN3j|B^SHVhXzri1dky?r$#}&fSV5T@qHd@Zku2w2-GA-= z1#zIsmWb9Y9z3b>diK+qChAdJa*j+3Xn4^=d=j{8>H zs(O9_!28o2bF)!0y}o9@YX$QZYagirV{Y|_deAl6)I*;{7#SfdS4F)lQL|#1B}CJX z-K1}ThzzB>ihhONDs7Q!6qlYtFy}($)cbBwIWwG`e*x%nbAYqPN@)^US?+1_QZ!JS zC*i$=en0ECHau>;jsN8-TH6#&z%6@eVhNTAc(itbE6Y`N2o>55`gbsWU!DuCi_goeMW}R}p2` zM`Oyv*gsMjOU&iO5H-TFNHGk;Ei0A^O!P)iaB4{qf4PUcx9%jF{V;^s-1O-~eS?Jm z5wjC~{|(TMI2qh&=SVlR&7@QG#fO3cFfa;#wvg4GbO&IlmCkPd*D`BS{ZS$;2C?VO z%8Rg5>E98?KqHWr7s%>-l&&u%UctQnu5F;Bk2;&i-}@LCw3~+ymr0RIH98dDC4DCD z?lt1)hSQdFkujyBR%3SdpCW~eRzg%64GQ{%+8Yu2(MM0slNz{}TPyF6lpI0-S6${Sm$k>LH*pFT`(K}||rNJ$UbCq%nB2Uvk zU$Hzwp*;bqz0B0}j^fWfcSkdyT5M5zg%cwmE%2KmoW*ztlJ)kbsYH!f3DmT$cSvPtg^T+;2FHY6FNdyk_w`4;;ZU3gS|g8+NRCCQSg?(kwpm!8(|#DNJ; z&|GVbkU%ca;DlLOuU*NrUoZ`=Lclj$+D73v=uy)TV__cqI#p&hbgQa8`IrD$zC`~U zj4Z&4ahhw)D@&hpuB_wc`CWZd2rAhdm+{hn&X%NZ@x1dYs6a|lJ2n=R4>CcSNHv~a zic7M#8UoZ4F(=7#3A0F3q!|#jvvR}v?y_&#G4fs7BQ zK75$=MCWK9NgS^AzeqzS47!U%#~{%amORVHZ*42~pWE}&9Oj9aLAr4|Kkb(5(4-#d zC>)8H+4+q0Mxfj8VR?W451jm=FopQ9?F-@aBdqHVPQF#QTCV1`hbc!F;91$wPqW$@ zTCx+`SuR{!kF+_-&ifF~`Yae5aII~q&?86?CY*y)v^`HC0(;ZJ6=`>eG<4G;)%O>3 zP0#~R+N$km_&Q7z!lm6rz@tM)yZMT-9Y^H6ID6Y7iausa8%P7kTkE2 zDa`Q29&QUASXPf4%_s;m5+X^&#l2k!=B&c=0_deFj3P65S?m$%amS%`2o}d z(VQjR_CLcsGb6l3qH{Nn2g0+(5`dS&N%hXA9I};t3qonNJ(mWL7DSbW5~ONodcU&v z{mW;^x*H{zU-R!s9`+M?ff8RJ>1l38D1r_e@fpBWwk?VOA$Jp);`i4wK)tHXTEThS zM^?D!HgYlabgRIu@`hZBF|r{`jBfVc8{Sa-!kFBXlHWBRIYw*=BMb?C<$6)&Te5#pCnbT1*U}vwq)3irieF6&V z{a-KIOZ%)mh7lb8W@dywCU^_yRn<26ZVqIp#gLpD^?CgGn%XirFV|9zk&nxmyb`)y zWT?%;brtmOypkyMIX{>H<)GXo$l&EMS{)L#yZs^|AkJ?b4=gOqO}?d^!*^ZVJrqR&ql24+8qXkRck-#| ziPps6?-k~QM5r3Bh4nq2f{KsR0J|+Al6k5paOGSL?PY>Tpr8DM<*hZ|9IcmLckor8 z8V}P-o!V6wLNe#FR1zwGk<)ToFifGFsDVsCf!*pI{VxAs5;Sa{gX$X{kYDqdraBr=(-P0Nb6zh;jpW!zj>#6+{?Ii zSYM0cZuoo3|FiJ6Muf9OaNo@DrcdCmwI( z()XA)MN7;-N<6BA%qN{X@Fjm5qV7+$=Pj>*Q)ku9(qXkT%C*_3{I?r-nxw={p-}5K zuf+O!XD$y8>KfZm;bH8CcHbIi($?DvW_KiUK*Bm)KY__*V^$bRHk35N9mYwz~bPwXvq;$`6ADZ0MU& z6NP?sVs&XjXL3anwsxt*ocvdj&jfCxS-+k|JXwc|ymj7l*SJj^O5L6rUExbe+RcFTU(%eyselG(xDOHE3s#g6Jjn8l zX_}KYeKk-*GU=|*Bjf^i57}SeOx2^rTq(t@#u*ITH#$Y3J~`X~Z9Bh`*WL=)2>+#{ z#`NN-n|Su3%$wX80KX5J)1es2@~gn2>JXN$LPo^NB8n%CuXNMIo-I~qp?;=t+_3*e zcYv;M!y~NA&)ZAFm@;K0w#3}o1z1g7{fWAWNSRH%F{T`D{SwYv>qf5!tr=1@$-q^M9IIno&ly>= z+y!)gvsL*5dq&ue=%6Q?QLT0|ikabR(%p z^CJUq8`%i&%>TT4?<2bG)TJ`#L)m}aTL~~ZW>;8p&0%EC4@$$E_Kq~3%Hr5zDqvcv zRF@^^?vyn`X(c1;1&j5lvlUxeRa9gpj3^TMr_$SW#m4Bq4sQ)7L&-rL5zHYTjRSyY1JT87WhvlHmT9m z&QD!i$)}!{a_7grCMmu54rEifBDJR!izl0H1ywydM{ZYxVt`={*cM3VS?Pn-C34F| z5zs21x}>8v+m6f`)3&cK|K{xL?kvsqDlUnd+IUo!g|5<^?dpDQb>9~6o1J_9 z_}>jCk8UW_cp^WII~*Xi4hPrXBa%vLr%xL({lirvV(l9DvD1^$83ohhMkba$HmPN| zlvU#4=@X_b&H%8wcMKScZf^eZkB%&=h{)(GIrY5nH3W2-SV#WH;M`DoP5NIEwTz7k zZ=fj|5!W+m63f-efe^xy99E&wpNxlSIhvlwb30zMBqX$3knN&loOiG1K<#t8BF@Z=J+nB7qF%C13qBlg%& zk>6-pgsyi)8T-nf6;))q%TPcl03B)abX1?BVgsY_KTe{EVl}2H!!?H6{78At9+H0h zxBUT>#c~uxXTq^{*Syu)Lk(%yrII4}Sl$-BAyJ6I%5&49zUveKDxTdP!Vrh`f%c3+ zd+I8c)EEL)XvdX(i#u#jgV6kzrzosJJ&+oL9PM$+P?IOJZ_4kCHnq6Yx{vkdL4(FR zIsd~cN-c@)Zi2hK7GhkT5h5ttf=|we^dnu_1Qz8=Dd09`xr7SE+qBfwr+@HLm2R0F zIP@xkYf+Hm*?ncA-!h~L1{lsM7*E^d-TU%nn@w!+ZN8s0sJz4fXrgk|s6pps^xyFm zmwF;3BE_2GEL^X+D{i~D81Lu{(RUqBV_DV4N+WBIOTMKTQb|OnZowZu-&*AM9jJlX z+nH6canulS86DtW#H&r(FqY~M&FEe`S8-1Let@J}a*Z!#3zq3J?Ok};`7XShx^(}XdoZZ` z)(P=JW1D*Yemf&0=v_|HIDu5Ax~^HG^7GcKfT`?eV7^?!&TuQ7V{Jtjq+c|U0bxl% z!kW!88|K}xMJhgoRxH>4a#G#uF%-Qe+~Lo`L4K`wedJ-lRw^y-=rBUYONHAgoh_gg zZMSDX5PmgwAFG!G2FFH~g!d=VJGIqeG}w~1E)J5Jm4zXOQCART!#5)utT(TSIJ^LN?D8e+`U;R_Dih?G6x(Wt%t)glrHkmt zg&t-2ZKtyC@9eHXM1<6Ga%{sQX0FeQn^*XsVI4<3Icg_}PhxC(2j{oc{A;cic4Yw+ z$!td@8TKy0$LW%Hi7J=%4%K#yf;?|BUv0!LULh+;vpl#lz01zgK$c=}mNUMnPW92) z_)T;&^`-UPPiTpv@Emded2|6&jNe-pZ(H`=CSViFB5he#SMcKp!nNd+k*v$ks@qtr z##a>IGE6UD|MKIrFG-SJdI6UKw&pJ>6V~qi#@yJ=Q~ury;mp-U;oL(>Vw~F{jEIUp z7)yj?Wbf9%hikF8c3dn`Cf~7njZUupL)*#ax)bw3Q!e6zGI!hJbNBqWHFFq_4xiMAx|x7dS%{K!U{vqjVR^j z!W#4khwt2+bKi+fc-KQA+QEfu2h|i$Il`7QUt@MyQ1s5;>9x|)*zg3$Kf%=2=B}-| zGpmHj7d7{=nl*b1;`%b+uh!$+Z-KsJVM7@1r#im7_|GsnQ|Y#x{2-!WwJ|Yv4sYmh z5=d8>Y`pys7Bj5NAALqVM=pnDwZjI(@w&xY+?^S--n~L6uPmi;u!0oCaEC>4l9aRh z5N~ukiBj5!gfC}){U~A6fE053x$jUcEEYTxP(+yb^rxCnG2h%o zqh4Tn$A|`HNUM%aR1bkRh2{R)0r~vd5+;aU)`;PZ%)MNZW4HoEH7_+qB^Y+q>f!xJ z1`1vGe>b~>9w)=3#TfJM#ZD$(1a?6=e1H3;%S&|P4^7@snwR1Ex!2j-Z?$#TC3D+i?sTt#Bx#ZMt*qN-k7<2vz!0Bzv6MP z{qZhQAGlYb17d&G;)oHZz;S;4LOw}rG+O?$53d_gb*mVCsxQibD=t5}8;f_E$$HdyQ00*i4_RR0FPN#GWj07^K3T#PFHEoF_fgg_Z&GOjy*|f=u0DZG$d9w{ zhSEWcg~Ha1Jc9!_1@8yOo~0c}o|fM%96sj}A5g+$zcF?QbNrAeT~dq_KeO|@Qpey( z=f3G#NdNOFwb~o7T;9t}GYFZ0i=Mvg@s^Q@rY6<)oa@O6E?oZ+eO36UX4Mw2jHP8( z?;q0fR8Cz%hc&!}Z}qGmcUh;wV$C1YUarjEYlp6^4})D^HVa3Ih-oGP4g}0e?VaW! zGogsk^Aum35Y&FNFR8z$4w8jVTU8sXpVJPF9N4YCHY2xeaC}OxM~(GN&ch~po{tWp zXtJDp(~Kpnwj}QNm?+Sfw2gBRWzt>nC4irN5fG;>@`!!CmG>;GW;EoDlAwpXX$s-Q7u{sp7$|TkYWZY zJ}c#swY!0-MC)6JU+74(zs;&Tn}J*etXV>b1*%DyuO6=&=Pg5{xYPLhUrE1wkwkW9Z9$P+;+Y?r9Z>Y}nDImSr<_;aruIMz z;&JA6s54DK-8MPj#xZR**=OrLH1E8VyEz4e-JtGNw~49lSNDHo&lggzP9ytDothLd zO6kl@XFgJneHWkzPjRH`KA=>T3@J+z_ED1b|U(YR&VvC+; z=M_bniIhz_x0lxe#ZMY>v@9ONxK*F!){GaqX_VjXl0Wu3e~CQ*iZdVmKEMBSeh)b^ zSZske%NSbhTBx`A&^)dtcrPPvFK?pl$E>{K@i0THAs z(xA%1Qj5v$&VNFQ+?FWF6OM@kr0Ua!Hxjt%QAgINU7j; zj?yB>A5HiHM((Z?4Vw5agr48}0-Wdt(eL0~q@4`7Au z^4fA2`2O#xnk%_R@Cj`cSc(Mgb`Q&gNwtkbap~J&@h~W*h5r&zW=_P3?*&I>J#{7) zN*-10?m8^v&nIUu&77+c{vOeuuyq)h{`M86N~gz$@UiPbinBx z_Xt7#R-CBsi@`!3q59v+po+;51JLLVI>2}*wO3os&Lgr-MM7O%dqk-){9Lj7X?U}sY(x{u| zns{YfD(IlPd7EC0i?BH{9>tG{!q)=3rMO3q_UMC#O!~hqyJ+w1oKoC^1v%U@8$^C{}f!FovhDfM7XsDYrIXjJN(TDdTYz+cCab0 zu9hzxme_6f2;y7Cf7(B`a%}v^u)4u(OV z{Dx!l3Ma<7$5kgL^qb{Yd>IXHHm4wd7Ea8CsbR;SqA&Mn3nlv@eNPB}FD z_32y7VSd$mL{B9?0^c-mlG?rxjLUR)P3U`x7rMEjoGKnrQKJQJUR#8b5~*ttc@OpU z?){l>wIPm8i4oJIgu)?y3R3X0~0}%vwmcLYF_oY8bP($idVokZTtW^h6B4p@pBc z;u-rK_?=ep+9q4mQcO4|*00;k=Tz2sjx2f!BBG%fvtOyKP=b!GDvH2!gVc>Sj0o86 zZ2IVxxt$U7S(OFnSTRZ$?k4vf4`I#y@|Vl9tLar4$C#HZY^4nwe0Jck$4iy8UgYAdmaxt*K@wMU&1=)y)9y zeNYsh6{qlz?&uF?43GGpAMGtV=bgFB!Mw`ZC(QMVME6*5W+xq&zN!~wKJ20lbPc{= z<$NV{AMg@gx;W8%1rF;&*W-pnuf~MC)T7LEK&QBkUh4)xdCz68my8$&esDV57Gl`` zn9}`i!3^Km0aM(oGV*Zr$pioB2*7Z>8rHHmcMM?xuFf^m5cqyEjq56soA#$+koo`u z8M1t(T4bA;Dbh^yM*kwez^7uumw#)(3;l|x*WmdrdB)>aAkG${;P=E8rpW+Y`}Vd)aQ`zkOQ`;KzLr3(79v5B1^G#jlmj8#Tui$F43)U^rLh(}ELV;2!F2w^B zC#lY45Ax35GxNyI-b8H`K7V^q3$kZjfYjF3 zI<|eMq!e!a0>Q^~f1ou!A1WNqj5M}DYp~>5ZlBHW7k=g^uArT_L69%LYu;0=pW+)L zs!yH8M7m(S!>evbrpw`eGgHAG2w!zxwvGuQJRA*+fwBX2Zew3G>G>yY`xyhfHc zPYFn1gwsP+^MrUI>Z$r1PUX* zJFk$X-XCfnnM{U_AzXBOBPrt>_Cp>D3`UK-77}3-d%0?h%cSnwY1Ejj-mpgrf*9g5 zwXb>j36+*n)?I0hM9mBfaN}1SJ3Z@{Z;rnL|r>CPHEj|x!RL}>H{9T;< zN!$F%D56#G2aA&_fa2WUvz_t)IVzFIQ!Ka)&SVx*>^QvU_j9OIO%n!#7ajCw={B~0 zmN?JxusvJpoW0MjoPN!6_T#zOD*r!Zz`NEr{;o98mq_u#(O2);Lqm1pC+;YGE;_+tcN{X<31AV$O(XW!iX5GXAg7 z>8UBdFEo5?;d3`e6NUS8FM;vP;VdO=W6ckLS<~m7lG<|~^@wghtuyfoJ|RW?Gc0~Y zZoxNvY{RhmQ<3-f>0w%^2=eN62@$htxkPzlkqAW|ZiYI~li+tdYV|nk^f@LC{2;WM zcY{oDubEO{zqwxm{iTX5^eOOTX1~j3wgKI4ucCB07H1DFl}-Iba*8-c@ww{Nr!4&5 z5)~Vo9d+d+oabv`w$o$L$BAOVThhE^#vk90@6LT?S@&5Y)Xny=$ITggQ>0u`oMF>Q zAFIn4K!CM}YIm4%DI~`*r0J_Ny^LYfk7zZRa&&TZKb5l(eug0IqMJAw@z0-$A?>jB zF&^^0xhQTGFPYQ=SG(u_^1DHSn{7@te40;(v6Ce1D$ z+Wog5^?z9rnsrB{iyMM>C?9n>u;T?^AAjZEE|BQ6>!|PTe+mpPzLmsCB$fyTCw+@` zlUlvsc|eWG!NSah<{k=Vj3hE~noV>qog~CA%BgIGg<+Pr~sfYgQ>>z&WiQQH9ENq8Q-^opmTBF*iUY=-&oB{cvneo$9xA2 z_gz~!2~E^pN|mQ|8B)heW|#M&MZ6L>%WC=Kh}l{l=xjpgh*b6-(McDJrdx7bgYcbC z8`uyqnkE0uIlXCdFrKPh2NQAGFRYOat-2TWhmaxz@#3burT+d#kQp&CMY{fL(enMj z=hE@IZlVR!cYowa_$R9Bu}58d%aujbE`U!I&xv)i!>Ti_-xp(CzLp#sJc`TDt{ge1 zMj$ESypt>RivnRK!@B=5?)$>WH0?Yu6xvrsJzsx! z+HiCe9vh?f=h5ulgCIIi?ucpjzAa(ml!afS0ld7tmd#|?!4L?f*PyDNm(^sg72I9s zM{JRuF{8u^g|<8N)`8R;l2ngM;z@Ol+%U<)_PdV&u zL(zH08h*3^+Rl#xiQj$3w$0aMv=>SOiD=0&*rFG5^n6(gO<$PI?)A=Wb;)I!E~>y1Z-c)yHz>sKc>n2#7B{sS|0Z- z0`T}4gzsAC`Rmd^6*g&7ZPCV!zg3Nibi7grzG{N0AGWZ@Gc^Cf2m%#Js;02ha_34} zJ3FaF);Buc)1qNkkH~uUA6;bhfn)tlWDMb*h=>ckOv^5^ zEP9v;2uar|g+NY!2`@1deu16b^krS{?V9mMVD{n^+s;=WB9W+Vc%(x*t=2 zoyF`@hxeN=5+(&DCAC!2m;YL$6+pA^Hdmbn!!M+rJ)s5&l8lzUIoC|~B-3pfp688& zS^8#mGvy9DBG{YXDbU;T+alRZgs&g!Jp#v-O$#55uIx_A3I^&}@N*m;S-p__Jje{(W{r)B z@w4f(wzgKS(7{dEyni-bsMPOP7CL5d(g^Vih)T#e;XWJjLCwACU#!!bu#Ku7-Pv%X z^svN?Z%Ar2zU%@Sc^gHW3vq|MmUkZ$c(YOK?opq%TJd$wdn)|t`PD%?-N2qypUO0? z(ftF!X%T&1lUOmBHAAZPQeHFE#d@Tv`St}x`r`HOT09e3GFbwFG-bTrrtScWtf?Wp zrcl0~ehNcel_ZaUb4;n+8u!kKLWoYuFqvtnxBUZ~aD08Cdv?4zQr2dP-1}pa ze;I1L*8Fx5>f?}n)KjVnDOoTUK1LR-3mhrNSknUkGacZ5;?#C63eiW8q?GyhZiDY{ z+CFJ^9@o{^+ujgTrjZ2I)KDnF)ddM`lON1kM(qLH@i7s9y&(qInY!H}P{2T-tV>}) zP^B0i-SQ176WwZV??+FXZ7r|9VHvh@0RvNQ4H3D?!4^eLSHfJdQG4LBH(E`pIxZ61 z60*bpQAsb_2!FaM*pFTwZPzpFRu~_JtCuba%j~PqHD=;@oiLm&)|kFs#@as0Cg3!$t2f$a zUHF6NO4Waei%HM6n5Rs<(ovi>E53B$g?rgqwA7w=8=`|sQo@i+1*R(IV-#UCJw}N$ z68r&E?m(9IeoG9y+ihNxW%h>PVg-Q@U@44JK1!E z6sV#J?^Gw1WO`#mkCT&A`)=uximzEb=w?hr`}?#-SnZ~f`7}5@@V@SzrEvI51uVP4 zD5A8MeN1JNz+8-XYnp@LHQdL-DT55llNTGeNgbxN6KE{dOv25epezdHFnmDK!Tat) zzR?vncasEOo#~&tjlXR>C@M47trE*UjEJkk3eHVw7cY}=MAfpx!aaFMMxCQ_>VuqI zE&079@_7?{2L73C*Q)5a-&)Hae#MHe0QL#$VBk^G<9E9HiXCPAJ_u23H3fenhz@bV@BMNs9*Mey5m9 zVdPiZ+mA93c*$&ozOy`X>$#WyNW4gVRO~+9d{kVgp{o71q;WULbG)_zFZe-l5!s!N zQtCr5Ty;UC>1vL(aqN@8e#VYHTT=Evl8p6O>aUzbquNdTZRdAh{8kO`Z#g6Nq^l{t z_{7jO+D5R-6T7!4%tnf$vC_q|cWh3-$=e2CF2YJq_Ya{UR14q~WJ`jc$WBxm5!a<`Vv*Nf!R)F?sgWGh5L^ zVJ%2wUq1w4B45}aii3+g$qlJ78FhPm<(sAv(sbJh|Cc>-fP2+8Z-KEcU@;mj=>3GQ zbGYtZG=5JiA0bYb#^mKFNoQA*?S@K;t&`vBOMXSd{ZxOG-jUm`OHiOcdAYk-ecj zo}p~4tCZQVRh)mHZCEkb3EVG%XVN0{>H2}$7~Mt5Q>ge#h!FdFGr}Oa>JnJL3f>xM+qBwm!Zs`o@*4Tf#x%{{R*gf0dj@-O1sDW|?>uJki%Vw@1O zp&mvS{Wl3t`+_()kLNhVayj|*FCp6Sq1GauqjRb7nl>NJ8rTcpblN9LFbzuCFSnaX zBf8+Lw+zYEn1p08s;~ddBB`Kg_-bfqz`#vIU7fPLykI$CsF}gXXk*HmPBwd$WY?$V zaonElvBBm}}A4wyeyOre#?SErA z@(s~SS@oh-3H`(j(*)-@1H_HTc!<+Lv61vOZQO{w8Kf{)NuC%qf1>&OQd}rV0nA0x z_FaB}ptyq4C8r49RcrBL-}lY0ebWa?5pcS~ovGlpaFp{BDIwlFVVWer=Nunq$MaGx z+3;96@z}ik{_^}lyz80QI#Iu5v0vAk0BsNYXD8@`iS_%?C{?+?>e^QmIE6)~wX1v| z#QCn{bWOaHT>OwB=2V43!z_m{KpmCiG9SLtp5=3t2Rj?r)Q1c??1P4r-)?Xgxm1cf z`XL-r(I60r`s>&Cd^F@&U&I9qp;C*56E5+Ol?Xj5?=f|Wf%eiDmGN-cF26KDeD>}4 zSVZCfnDYD*KDuUOta4?8h-k9pLI=Y?4|Li;^__W?FbI}$esEp`0Bv1xET>oua@A9j zHL<4{*|rl8n;CMUznAN+t`8>H*48$>l$7GKv$KDv2Wo_0)cvjAQZPFV2F9*g^h(Hd zps$D55!O%HT+n9nOY*rhUa9G2ZpJA19J71&M?Y(=D`BAGym|yg4imdt843QQa!j?;Ur?{Wx zM0^p5le)FFwL6kHO(CnMM!;|-mh3H2M6(xzqA+0vk$7l7{t53HqS(rUce@~`aCu(hp>iBVwZ!Hfx>K(%>!vc3N8 zkiEQ~2G=rR#lvlFX?9MfBS3$bR!x0&|KMWQZzh0w*5(hpL?2<7h_!R%N2PTr8n^sK zwQA<$4)nk*k=8Ebnux?JyYqpTNPLsszsqA8$12UFm*Qhngo%4zwyW!N&*?7m-@K{2 zPU7nzI+rp)s$Nk1x`tV4V3#PlcKx_bzC-~mqpf@( zkLrJdtxAxNDE|1uc6^!7$=)KoPC96W3zH||BzV~r%^-Y;?-3qeC zkqk;p!iPYaAuD$|vW*Cc%PfIgA>=%mUHq zg33y{r3cC>@V~)}qg7B394hXk^#q{adqse}v-E5+LozpH!|d;k@rW`af90lDjqbK#sleYPCm5hEEv!qw$z&nzOSQZ$R)PjWCRpOtGmlgPRDAclE3MUhY0Bb>>-Jta9kY ztQ#vBEId8*H%YN!64>RuH!0Y`92oC#mUe3xezOlGrG7e%wCJGQ!L+u|bfOMo0R{2X zK}du`BqiU&!x4;y<^r;i-`{E$WViXC6UIgg+`r)j&(23hFP>ylZ0om4rU^k#86_1`|?M2ko zu`~t)@{#{IWiWlfv1wzKAhC&3-79@4%vDe*$Y!{~Ba;sA5~DS&DA_hPL+_?WAdq4k zI6MGKt4$g63^x(*$2=IV{S?6?U$+q9=RBRfO;1I-hAq&^r?0Run=#$9!2@HUU4pJO z6im1|b!}O%m!N{e!ymKdKrPlhkv?z`eVxWJzA~J4HvW!2y>@BZ77w=_l-8@kODHF$ zYNXte{-i?WkjfhOIvuMvAG-4`#ipbq$O|ApnP52O{#3)H9lKU~!*uU8Kh%}LvI^gn={vlii}3iL0cWJ=HAk~~u!;r-}U(QuAi zr3&ajG}9lR`gT}q+q|WGum~vyB%x~^+>Hc8b9mZ6eVLL6**g&S%OcE)0o`@;es zMllukI>f#X=(T@^<+7D8?f}nZ#1-VNbi|OW7S>-2rk4fm983 znVu+9(l~@eqaBM~N>-K-Ni&!3c7Xue0f+cI!PV3;p?dPYK*^8byCUF{yGb0k4&F*C z4=Ra8$aU>eF*L3+h@Oo)aQL9IUx%LnEk8>iLwi$-8 zbactS1HSDfcmG-T5g@UaVqfWCpWFE?UUF)L9ZnUE1IcN@EBno7P*A1H)7kI%c~dB? zP6<%;t!;zjcayTtqAoHmAAHwxgAG2Djug_Tm&fA@K|w(b42<#_vcwT5M@KYFOz(#) z|ASrTdVDzrcyys0)6&ahtA_ zta8oTJR~x1s`*lynjn<@@4_%CD)f9FY5l00_q_yPOOIPp;BzHo)cx+it?^}swj3%) zKy#7|q)4%5tjEL3T%V;Bx@2`K4pS0X&F=p7c^PtE29{%xG<|bDSbyE?;_RaD0$9k^ z(PcG=0su3d?nV+GjY8N^sxF>!e1y}bO2^}rFW!f9JUAdT3p~ODPdPc+wf)T)>We{I z({}wFZzdQZ$s6*hv(gt3DZDzkUU32?ji(XaF;|R;tsA3Y_a6dLADnQ>*Au z=Z7Etd#CCA-Lv5$yXHb#te@dMNeb-7?%#qtIIWcX_2>Ds3*?Xjm<7&xzf4j0z_xk@ zyGOv91AZZB81?Q^a-(g@;H)sSHlNthWg9$dV>{#!<*3$w=G{PUgLCj3cgBm2V5mbMeuep7!eyN;EYR#>I!P>T+`L_H?DCrRC-2 zMMOkkjT{UoYF`%=6%B1xg2IWMPM6pNNDFIgTcy6x2skln7M#QTw-akVH`??bz1tPX zgFW7bORZig3tRKD$riDnu}lRN@OrINyy7i(EFpLT7cX%nEs>VmNzyq>yl%uiTfYPo z^Fg|s&oNF-0COY2*7;_=twiCI%!V@u^baLDg^fx=RYfghl(*Ey%l1_5Pm18E2iXx> zsU;$AN9Bt~X{=TJi~0{*2*jy0wna1PlYC{9`1JW$Mdk>dhE|ZocgH|o#J$(EXyr}! zvzv5ewqfM0oD7+p?IIzo@W?W$t~gE;`YS2MEo}o6-3?gR@I!zV=Eq}K2uDX$ymG^q zS(Ge&0+j=B{7?fQZA6_Ke`%EuIN7!Q)ss6}^L&8~6dx(X>=e1ui4abq8vl6`1; zLlnh4(n%pk|0=9z@h4n#^S>Ux4!R4R1b;PI#zswtv#S5#q>f$4dc*aU_%ckXvb)|I zIhAyHMR#FiVL>ZDt((eWbp>@0FYZQdtE{a2^vtV&3Fn_NG8>RY9KpTfp4>8QgQO|N zU&ws%X<9%Z zhJi*R8y4)~AHo0L`77~O2?0Ns@QvaHD@USWc1;!)S6ZX(Il$`Fxa{uLuUSoiI&S#~ zqn|6=#M5{QzL#-6GFk2I^fr+yS<(8zaIGp{PYoS^Wd(NjBx@B1EKz*TO^kqxTsBd( zR96RK_BCSqzW0Y4Ykk=62#-Lg2(^G)u^ zq3;IrG=hNNjFsSPyt}=X;Gz3~VSN~S2K~N9k)dp(DKhOg$eIzRBPYQKjB!J2XeqeY zq7cZOQBbYOcwxZd3f4lpOFv=YzyO%!If68e(s}!H^yz#bj zpr@zxNyw;X)j)^wr5zg^8&OeFu`{|s_qt|gW)s01y}iAhHaf`vM3ofeZygu;?fwtK)1j1=4eJC65s6=mgg|d5ci`K4}ska(ZB5u=d?3p}rc@ z5&_JaBXo`|vr=}^eEs2<4Vvi3IdD5#w!E_O(RTvb2dZ|6Zv8f;;Lkb`XgjA$Jj+Ku z%cl#3P3an}rHTrCmFO_u3lN42X3}C*R{7nut;dO$PZ$%|q4(Pztmc#V8#pvhtNgq; zlcc}IEb(_5S{bi|?JVyWnYIRTux7>h(k+bryV6ViQxD%$vobHi;O|1!+#Ge~CY7ur z@}#}4K|8?$fGt$N509TwpLrcvXp)wN8(TX=>3CDQSUcluM24>IOA07j`+&aU7nFg- zQ-=Ql?hIOHIV|gGQXTmG#%q7Hd3itJkUkycKUV55(@TMGkdZS+B%9NhQ7S&LGoXnr zKwo#m$MA>pMS&+OIYh1ei{&K}uLJV?!dhy|H_<@(k{!2l6=H+e)r$HTRq*#0lcKBC zysI7GlVChPRaeqbcSo+<=$B+uj9@1HahKu+q$hCh{C9Zts3Hv@fnyj1I56H`8w8#? z|LK_d@|^KR@u(>pzCF6YBuy_zvm^CkeRbN-I#S4 z`&8cMsCyGu@^<_UQ&c-;TDC>yNpM4MJ4IOZF;C#|iIVFRt|cu#5dqH8v5-E}yR&y; zXe04z#_#P+w@Nn_o){C~XWP1Nekc{y)q)WhY~-7WzyA0zOwUrDbo!%+@BTZXt zIrMxTCyE5k-E3PV%5BONaUX|=Ly82LA)tnXh+bb~V=80ZU7CWstD7g^0vqaPMisK( zFkK3CvTM=G}eotS4S&TeNZ)gu6KS3e#&*^5Ap3k{ zqjZp0GeF6x=Gxzq8H_#%AL*JK&!tPX>d<~7D?H_|+o7sw&ZLp4G5doI15ii8QJ5If zT%DE{Q!F@c9m_Bu;B9uY8{G8+j&W_R5v={juU$7g%XK9&ykw<`nIX_e@sw0cPbuD1 zcB8wb_dA!vQaWU(rkwl?K?=8sW>?1dPRlOAgMr7aL;KYMl{s7s3ED=OD`tT2 z^77Dvf4U;oO=mkHICj)(sg*lH*S0s=oIj`InLnH5bGeQODSMTfEYiu#0OHp%?7smf zo>ranG#{f2TR;W?l5lT(dMwdvX3{UrO`O?(ZE;TO(CX9{=S6Keoz2U|5 zPH74~sMa$72hRKeurehxmFD`#GOOX${mRlpS!PHkC~~<%hf{hQgVJJ6UWC-3V$&Zr zXnNr855)0Q9hi6zKwKor^%@y+*gLU>m%Z$*aZ5UNg3ZR8f2tgtZgiDX9S4Kq=jn2x z%2Vt}5o216(K4!H7eVeA1V23&)rVax?G<>rZApqH_J=&`?X10xhvP;K=xAxb|AUAm zXuRo?W?T`ugAJI&KXS6V{z7u96|P*6kSM0L?_L#_H}y+bSGcLzy~!^w&vgPc*hYE< zkW?yG*9OQsdwyFQN!q47s+MilNu1=9wcC4-tsl-97)44Y2qfN`fHc*d!!UrU5+^Z7ARQK9+868C--}{v?^aWU3Qf)(o#X ztbwfR?aB9v;}I#A1?%380^`ga9+ByLVQod_r*ZDOD9y4=q{#JDgabFi1;poyC_Hji zWIW}r5R8D|crji3XO6cFpGrsB6bp3!KUUU1P7MFHNqxk*z@0^LUXW4%c^Q)aQ zswv&6OMGM5>%9v6v+(vMR%=2|Us}D-B;sSuWHp_MGV}CO=**j@TS~e~1wA`yYH}%u zE*h5E`7QgNqlx6WcMqh4dA@1OP5+*$2RVz;@S|{;nccCW=-Y2H*|B~{uFe+cOm0{v3!v~eGT1p zFH;~kV7%)o?+R^eX0(_8Qw2oAlITz`mac+%ll`P;7)pX*8Ro4n)RzY{{@^dIr-Z9g zcBs1w!alxd+r$vikqdM7=jmxx?;8vZEM>UWLMSnvWOH%>pdP0+tHgUD%el^QguS!2 zmkaLFZaRkePhdX@(z?g9l_rPl19?kJOHEBppPyg&2|dUq_J&~Frq**uPXOK{m_Lr$ z2g zufzNF>O|iBltTFH1c0@!KsyvQA$k$H2c4d+XNcPu>ddd^sAmZh<)3kN$B|Dse*P+Bk zS*Y*g6G!)0}^JG>@*2j)(jpd9Qr%77i?42@1ChxgXtPc*;wgNzN7ZfMc+ zlND#Zx;&mW7hQTWz84yNK(p;1UG}4Jqt)h{c?S{^UWQOB?i@W_`+xBulgD5?YPgxZ ze6}1Ohu+IwuL?h>?q$s*of*Vg2kSA}5gLunC-2HgafyaiQ3O~k6IGRuOVzZEWhI^g zOTT;wo7hFU9uMU_N1%u)ps!&iYW>wdkMi)EIumd*OW>>r5ujcgcZezY?56xnA#&?0 z>gwC#^J$6+sTaZ_ur?EE2!7@IvrF`gaBD(Y)0Zl^2}2|Kj}FLcSuhGaY7C*SWg!sn zxn*M9S7pni0FSPz&AGE&^W+i~L^emi)Pw!;y;x$b^C5E^pICcr24d@{@`DDOed|hU z9?1Ot?Ljh6tt3e61o8`;c5kG1K9a?3ra-cD|xn^8F^5Vlz3EQQ$dh4*p3 z;b-L{w;+K^|7YIMpMM*4S(m%O)r7O&LiLC~mqLH=C}WgX6CL3c=10mjRU3>kxrM~F zmaf@M$nxzJPk_@-@h$#LQWTumV&`qi#%q)lenDD=AK{P?3DsjB{gWL&9j3=21FC6L zkOwM_;+(jw!Bm06d7kDnPIfw~ND36~XT24oy)0iO&Kk}yl-JxY z8ErR=F1bvq6s*P8A6#qyTXRYF$c(@8!2|MtE%!xODeU+8kAc5K=@ zIuy3bdSv2wPTu9W0ol@kfbkx)FQK!|t-_z)3be9tX2%Sd|5%9#AnbWeO_4Q@)lYwp z>Z4{18>ec<1;~{uZ{Z9MmR=ConQ(#zS*PmwB^-h=Xpg|FyQ~%hUECZvEk@vvx|1~iG?;nxa~23|Go49IpY$vB zxPHi5_Q+AM5L?DL)n`)R6$ zS}#u`vA-DgjRlj6ci+h`|Bni>OTJ_~?z}sy<8|e7_zlhLHDQllI|qN#`67qg(TPvV zXYJgK-So{{xpxDqeSnsX%deRsB-X?FRhk&w&kJiFt$t%Y=u68Xim?!3n(GXYlg9;t zwFYa&(mbDL1HK81Ds}O!w(9I`5|Chy({UD^3C{}Fs2W4$1w;a6Pf|=B(}0-(SX%wrr$XlJ{`D!gzt!_-8(L+$6 z;hDPMdL>3%dlzTJYW_QeGZy_rJmGz~NVBpVk|u$@mIs~QU+%Z>-3?Ck3kkx0_@;~p zS(OSBE4fuD9JQ!aoeQLS5#u7ASYK}Q-()>Gfk>ah(i4$y_vUZsLD>^Sz7Sxyj3%tW zzq!zKvF;poRT9IXKcohhrT;kC&}M?n-t?^5l7^imkX>LL0}oY)Fe1Zp<3M2cYu(;D zku{bi#HCovS29iWb=!t{UYO?QTD^Ap@GI3#9L2g!ynSTcJ>?fVi4jQ*fPi@ItG3}F zhd-I~UFz|iDPbh+IKWmSs+BRPnVzgI25u)KmTm8^XX8qMYbB}%eeFzXIQSC>JIdg* zW_mt5-25}z6WKe{gWns%<;T$@dJoG5#_eIgiTI;0b{7iFXpa75s@8N<;IA*hRKO0T`N7pkW;B_xVEn^AyErI~M9eCCr8$f))2`p4J9^w!=I{&mdfiYz;#X|h`4 z`K4e=(%iVt?H(xMezs$b5I-j!Z|6z4&$L?ErewkF96I%hRmBX@CbJ?GhPW7rYr6eP zvc5iVVJjV2(hEpyI`q>k!vgAC8fBHC@p-f%uJW(5H%KU~QnuoLQ{PnUjj;D5sVs6vAFsoCuSf&4y) z^$7p`@?tE|bK%nN-Si*5^a5nL9P=jfc`>IBc4lx+xAW7ukbrFQeJ^2RflGYgF4?r~ z$j41_K6W7_lA;~WvcY|hSBgHp6g2Y>Y;Ij1CiRn7;X{xDi8ANs;(n7Mmxb0B`1=MG z9!$?4wrxTg&OA4|`)9`Q2;Z)}jk_$8vrw={%xW!%D{uFj9t1 z%YAeYUG0SX-X>GG{=JL()^M`xPpi+QeTLu3Z@vhON1^-RQKQlel?$a)UuD4JSI8G@ z_!&(<{~0KhUH~Qo8b3$;G2Z(2!%W`ab$^{gV0<&lUC$rzv1QF(@e|8>mVMu-gOcvy zq?2i5KUq0*EV%Roi;>kGA+PWDly1kHoQ@9T7jZasa>&BB5s_}052P(G>m&268~0-Z z7177_zdR=}@_hD43rLBpr$8=#4UC)6Z)_RSqO9b-%%?GYKR%E$L95OU3)Q{DtgrGvU@5zbwW6Iyi0 z4YjxYDI&d`C@^QhwLdzIm|AXtgwC;vpk~z!=PhAAM5ubv3H7dndgb5>X__(2zE#uv zCI>36c#I^hfqYh`l8X$zTQ1(J80nN>`;c4cTx88WZsnH%kT`;)w1$~C3K!N)VN3w* zN^%o88`{`rIQ8Ca?qS8F&z0!6WB;vNP=2t%Ts5{1@Ln_@s}y7;g)u-9TiqC}7h*v` zjcH7q>)}PRG)yS?o)c4KozLy@vg$oS_X!N1&i*b*lDshyB1Qv@X1zD5J!kf<)7QOm zs5lS;mWaXZIhA}Mh(l@INoP7Z;%Ed++K}-h<#;T`F^s@c`nfgqpeA>no~_{m%K7Sf zb-kPFaxQ>}kDsXE-Ph-Dv+n77do>%sjH!jN^(eb6RGVIO)#DaVZernk=al699JreS zt1Df_O1NdZO1139-Z=EV+v`9Q&vlNo%c|aeCH`^V=*$9G13=^%ZIFVY7kk?x#mKio zSMV|P;SfWs_N2x&GEf$9NbaPJky)>Qgfs_Ep?1UpouRKh`AG-BY$dfBv%7}^( zTXctxrMb*|Af_@h4`nqdp=FEU9^=VZg`6>jb47~CFFQN4)V`u|!qK1xw`e5!!aQY( zPm$h41EYl(gMUrzfhjPU@wNSfP~)syTB%`wN3iM48>i;=R({2Tib_ zQN5{9Fe8T2ukp!yYPtR3R@0#l@Wu{q=tgUk48F1|IQS5u?$1kNTMCU6(m&dwqkjeT z(Mt%{a4<0UdJ*~7j$LVXthW#@jMWg?`wCm%fwKqgBqBT5w-aZ`uhIII@{S)FKLren zsXhi9+MJhu?9Vc}R|aOihXG#V@?U#y@9*%r;F_)Y7Ok|IO^59ap5+T~!&U^ZHAK<0 zx%JxnJlM*crjhB!KU%7`KTQ64&`k4s?vbk7htKZu$n!c@Mm$vk7_GTINAsfsVJJc| z)gX_f$lj?$x(d-!3BaMz@ZzHiv0?r?mJ1h1t}PPJq>ylJ?Z z!Q@+{s_0@B@!u6Ij)@^dBQ@e^(7};-$e$8z?!Iao=* zd%t(iEk9v1AWmownXMcYv7Tc&tS&v0Pq%2BAvedw9Wx|!Tc!Pc1XGxVr8^Iwc?*op z1!UPo@)5g7aDx1JbD#D$hxJ2d3X1u zE-jaELdLXU=U!Lhq;a4JFN+!9qz(I|CHJI0|D+xHq*JoTd0jC=LlNd$3H$Y*$pc~1 z0$w!r(|K?(9d7GyhAT_KU|GXqa)Xn#Oc%e@58Blz9dBNQsumyKKg5KZ>om3eZ(Cw62VqWR#PHmI)Yw>7gs_wCRDHJLB{&IS?RbVEFe6k-h}Kyn?VV z8DWpJd}T9~p@&}wkUY@xeWJMpkltYZ`rfId)lcI*^B2#*8hJ3W;Yh5|R0e@0=paF} zyh!A~-uPRa2t$Kjw&b;^t(PUC8)-44pt?6tn&Qi{?-Wuono+_G*kqXn+cmeO_A5a3 zakToJDAQ9Ayn&6G)(X$RN_$oBs(2I0_}w4UFLfXTO@Qy^KtN6(h(0R8Ku9!r%X7>i zhB0i3(5WuBIC+00%f<*_UOwl*+eIu3=5=BB{F1ZG@}p^zMa76A+3G|xmtU3#aHslD zH=-JZuO5xFT&eZgR?E@?9JWp4mC~=h_b*CcBI|lX0z@>i>G4UVfjD4<-!d?&QT_oq zE~o1>kbAd^|nBhmtB%PG4mIHwekRyM(>mzWxyCzzmOwAda@G>w?(|igj12Dj7gm z^D&|OqYctwgLLkrRaKl@%A2Y~M_F9z2(^p+&+2fgbZQ0{Hx~l!5d|{lVfsTc*5^LQ z@No?m?_9*004_JhY@_RESVn)hvQSvHTDLJbGIv@@(Pv{@O~_20MFkY6$TAMAl#`TaV9~EhyDtHNwl@OjtWyEBk3CM- zq4avXYykWAk6PeK-CJ%{cw=O6Sx)3OU3|sfy`31~r=0?6gS_jk{N83|k@fL}XEfUU zj(T4<33lHJ%0uR!H50PtG16KM07*5}G!1uiUtmUvIUdYw=%AQJLh;&P7Y#Wl%@Qqz z+xI&ssEqG2QTboQX0%fm0-cd%i$0kwBn7AHbdS*NvbK^DxP1+^Uj6k_FSns!Ob&RP z>#96f(N}eO+_xyZ3^0dGmDoq4vfh3|C6(rUyj$_AKJNb)@%Fx6d)KmdLed+fVsvKv z)Z68ZbG_~usFT^Z;#1<;u6sZ)F!UD&!VIf%?XQ@f0O3V*rFKv5W!Ebw@%p50H+l=` zl`>tBg?>IhFBP>0|CHkCW1Prg7O5mtN4=ig4pGL%W8^$Qf*Krt>o^5o%4I96#mS6s zm(qWgnmFSy4}){?SIY&=r=2Hp5*dK_dAmy@9cl61bzP`tAzo3D&ceWQb!`oa>(LXb z$zhDm$jzDoT1M~0MXUa}ZCsW38c z06|$Qwuimf^UrsPw#j!VqTgPN>is|JX2H3rU-_zO)1V&`=g7XmtRKPh+P^aJ`iN30 z9ZURW`b~%nQsHe$%&v`LKE-LV42cIVJij&UwA_zPbgj_QFE8PL&pVNN04Q^dAj<=y z&tdIHGO= zSa}>Uz?F|eSZg`DwzB8;Pzm}-o+iFM0)CkJYq4XtU#I>LUvCu^SJZWB2lo&h0u&zH zEkFUm0>Rzg-77c*3&DfCTj3DgEd-a~?(XjLpSnPto;`M=`t^&iwnuW$T+jdbT*Ie`;EPVK2!n??XT@7uFt&c13Y- zd}*JGl2)i8d(_WdflIe04+7*zl;+KHWED5yP;c#La$_ccRuJ^Kn4G&{xDcdv1$9aa zYM!6}@~X_%o8+E@m+>7ZTgQGcQ@ouD<6I>(+wI4q{=84GkvIk_FOq_6D4z1}6@i?A z9>70YPE(PKW04WBw`Y^g9@8aP?E&OIc{hX;XtaiZ@^|dr;7_LV^*FsOH8We6s17f> zjruxVubIdohz>*CZpb&kQ<5)CxNYLDvZGk;)IN>PhRjPI7}R(RsRbbBNaFzDhn{nR zd<##fJXRL;oyB0>i~)^IMgMj1$I>F@02$rlGQ)?}O42I{aRVe@R)&In_;jPu`Sl<$ zxQY-4!)T5qrXOeD3!#^F_Oe#RbBZc#59>3#z=t+Gn+aUsNy@E*_O$>#-;|K<3XJ?* zy=-Jf#*AHeHGWKK`y4Cw`|O2UF`K6S9e+NrKo%u_OrRWt_ouzFFa)Cb;itv9<=CKv z835C4GwE--682yxl7Ym;!loUgnFgF9O!E5k2U!+p(ZaS->XD`)nYr(sv0O;9cQ_yE zY`S4)8r#a8y0MuFn`9yK1eM)2;-!SL`c6hbwPguiz7-LNbQ)vzq0Q^h|eh8MRtbjU3~v6D=e0ZLnq$Xr?9K0`{! zS3jp|ii!z-OmPLY)~%Z<1Ig|xGrqRszr(-i^rgalwm{#yBY zY;$*NSFePzWVQz;8@{zA7KGe%s&TADx_w`S`%0yHR9!mg#DzGCOid%V2Sv{SFPK;$ zv{gO|sHXJjeu?VjY}#AszRt?M)Xi#{c!>fWU4ZvKH$~hsj9z)vgwha-MwoMoGR%8BqS-Aosi)QQ zM+ZHlsKgYcM8?7h^tgH&(zz}NmJV5&7DVY;>We`0G}(@sywFahdygfFl%B{o#5DNf z7$$UG?2@lg*F#dg-ltXIJ|ALy;$LTl<&04#cloU~RB|JaFRDSw=#TSUEI=)s`!9`@U#jqIr37XO`|o`EbWUk0o6?Kl&Bo=-f?l~QQlo}BAeo}1#~cB74xOK7 zX~c}OEbHZ4I-YK+6B{iO<{lp-P738P9B5fXiz^bt{v)kTq`Y;byDnU+m(D2SPmda% z4B|B8A0)z5WsgLJ=zaH?V&FDdNQ2|Ns^w=zTOAZ^i1acdZUz3(Hutnuow_PbpK3*p zs_gH=6u~EH*goQB><(^>ylrq?#zzb+#@wJar{0WWV+YL>22n=m!m4bExN*5+%+5>P zhjMs`8!=tud;HceZP5sWD;Pf0Cp^G@tb`uUBnbN7w>)t$i(P&`3J)-Oy?+b2x*SLS zMx$~Ab35%PV0kxN?bitW12wmZJ11x?9|^z0p~6;U0u54crgLG*)1%Zd92=)$B6YV%W2D_AYO@65DpqkFFIGE6Qa* zAEuxIIT1WoCpM}~M>m>2D*eeYcg8#7>>f719i($88#-vfNF zqO^S345TcuAzQLGxlbx>t3oK48rx$FHz!U~K9kDzLeY)uIwD^p^E+%i?R9;wCmG$_ zZz{T8C5QMma~2NcG@P%xoLR$uV9tnH&z{&UgzL&h?4kVvCNdyb0ks^W++vV+;se$j zsG=-)u!w~BP8MD7J_oY?tjt9BhrYus`C9c36M=;WEa5Mc_ynw#KA?nj@Ur=(+EKDE z>gVuS$w0tIvuiG#`RfD(jh#kS2ZR_yygTyCif7+5hk68`a+`8MGBBj*Xj$$9BEN zUv?;P`5l^fDX=wv3$O<2z)X%&)p!+Jd`{_M?n>EII-Rg1_D6RKYfy>jjrvl!4CuH_am+Rq;=n;G$|NMmln&V^js0> zo+y8f`Xem%+}IU*0@S~qmdi}!UX^|`wI;jU_$t-Rbu%7>)yxUXZ;rYI2XaSXlbBD6 zy`FxJz+K#AHAY}3WA-i8@I50#7DQ=>={kO-DeZ`ifLR|^NaR*O>MDt2x>|(7CfO>4 z7JtR>@*r-V;8uU^(m$w+_x$Y&+RtU)Nv(?|Yp7hto`u-cBL!R(=Q7WII##^W!|jgf z_UJrPg6I_FzT07~6{We6KKpCP1IY6Mg7UyeC665Qjaw&6=!r=x-9?yfRC+)dgVH?o zv+hAmTvpw7qtt=b6DiK6nqrzaC~GJ{ObKvywbw3o8hB!4AQSO`M3?skot;C3dg?Dm zJ+`M6QYg$a`PRbUW4z^p)1_g3t%(kjAcNS`l*pyglgG`tv{Ku8(;bKh@E2?18Y8;2WI-C3?!Kt@(Cw-22GFbw3BU#yN%G?$s#cnsfla#FikDvGi7ulaUH|Hq1_`VYv%=+ z#O2iRH*;y#p&?%y($1SgGCsH&lX7KGx{RU+D2?{n-+%UkRbea4RgSG+DSj+>ANkXf zhX4E%7@0H2^Ca9BO85cfVwr(?97k13vDN;J`!>oecaE@n-K~V)>@?)6t9nRK@^d-? zh}*i4=H7iSwmH!TfnkOeNCAy<7?t&P8q^ z)80(iFnPnVQPg5sJ^$lkab^nW{hNlZMntS%6nYq?0c=Ewzn8Pogm2|yO%j-d_j=%G z%vj5gBCgTHNS8?CzmzUum-jOE%qg`bbD=sB%*ErL_%iDV^`slDw%wL-NuK{If zX1!ma8#l?NGAgUm)h8R5AiYypus>rPF)k=vPbFNuYoQxx?bzlBFy#4P>{y?(V8YGkA;j9G5E%PE*g*UH!k)Xfq z69m;^iCe!{z%0FPWq<6%e);QmW}xy?Du1S=s+QHzGyht`7N^zhMSw^p66p(a>_=@B zv3C)!gyzH63vP2!E1=I}(Dmcxc38{5kaaI4qsCej8rN)KXVCWA|n$VNK3^LPp5r#=88HdYhGS zQ6CgFoDfNSuQg3mOEgL1_^)jJ?9%YBY;8?F{ZC9Z&E;QAbm)NHiMwAm!gUM_@L;Cb zpn6{%ZZ#l&F!Y0o5B>DoZngZfGNn}w`ofY#FVQwbMgbpTaDnh*k-19z>O~~GUmVG< zAeP(-_Tv6H?5PCvKC*p=jc`~4t$5{E>LVh6g zE`Noy9M7#qvPZWA$(G6hxQHxNo1+Wf+=w8*jI|KaJE0SjN)={YDBZBvNfYAoN--^u zTjm3KMcJ!`v2ppX2dimN_v>n^?6PwjW2WrSDNhGk1fL`27!x_MEhj!jcP?{@|ANcZ zCkzCe$#w9R#@$7p!`VL273IE;KxUW62>x{HZyM?Z{;F|1N_>3`BH~br%(X3Jd=f~fnMRc4i)RJp1T5EoK z?;InqhW&|)PVqSQ@ZrtQ&pC29P=t8bQr}4Y4%~Nw-W5?*U$t@!MlIv()}B2ee-H80 zmN?Lc3^^5i4@-LC5&oKF`=f9e5F{I`l}uV}y2METYk1bJ2#(3O+~lIF3hsw+Ei%@5 zoZW>i4h5D=7M%L?Lb4i1-Sd<1nO3ga%?^PGy?xGul0uQ9L5refP`iDOzeq7Qpg^uP z*tq>24o=eV8Y(@eh^kOL8~j&ji&5GOgLsYa0_@vgO5*S+slj_qSb!TwQxwr6!p@D4k=?7NDb& zH68M^4u-Sga@+o1@GpNi7a2w^6JjBsrQ`>=?;)ca2v{q{V~-gAO2hmM5Nwam@oTe& z%xV@3AXWnA92Ajx zGkc!}AG}jdP!mg)=OOG7p{SN;=D)y6RI>@5FPyfg{MIQ-zkevduhr1XtB23ONo7E~A{> ziHKB>4c)G>mDVN%RGSeJnKcvW9Ib$Qz~$98yfdAK2;ZdC`h<6V)5=mlKH zCX~m>*I!wFTuugtg((L3v8QGCyh^Z~v8U<9zFZr1?vZ(Ge;=`uQ(`(^m;J6@5hI_` z*v@7<#z|!-O*`jO&Tf)~yX;Pl;tYQXa#Bf{?%Y?HROS_t1?IR-|5YH2pjHE#v`>G} zM+u(l{^Hv?ZzkEI`H6cHr~;)Max+DCNjW1SBp_B;Z4Len$xpAyJB(KK*Ot>H9r%}e zcY(R@c=5PhR&QjYFnDZt!^x7g?)Fpa)s*dFSl?7@l~^BijB1SyeU~>Z_W0BB{ko5; zw41(i<6t6k8xpZ6GU9n6_t3-H)F0@XrI3p8v9|2p0hm90wn&-Qf8KbqG1%*hGsukV zKl+!A@W~%T1}A=g1srv&lqlzld4KhyP?|X58b+3*b-V$3Jey(y>)9p5Lm=&z61(B0 zR!;aTn6KE*PQS2gOE2=dmEZVxoi>|YGYI}OBY z(CfZ5$<+qWPEmqrF1$S|gNMVpp?Dl~2W$ois_wIgCMc}@GS_O7eyfl@l()Ku;J0a4 zp$TJLFDIc^gHwCEB07J!@OIX;Xw0Vh1kyV(=P^ypYmeqiU0!2^M>JugpoTpr^25Ic zvf$I~lgNBwSvAyWtyf)s)(Ri@g}Ugt?K67+Jiprz1aYk5sL1>#Ak{6;Br=QQ?fd^!giZgF!R$>UwD^?#syoQAn_1w)g(6LfCI;f@h)zbhK<9u)K97pirr9NL zB0Ur92i*zAn(k}V{TS`{-aA?sLeXiPFWwW+Aq}Yyr}GDxj~FdrmtH=b)$B%E`+4e9 z?0pKsMM88$6<&sDP$pl#WW}5GqMT1C+FK(3&W^x{d4m_Gx1+Sq>W;<81^{%0zxx)r z-lwVDVoE%RyMp2;;1DlbT9q5T%sLO4_x}mvWlc8|zrOu3WxSytTD=K6zVN3UW9b-f z6G;MUK3hD`o>ad>Ktow}?$3TWLN zKK;>-j(?j5YlCcBk!vsG!C{;CeIrR{%tdqC6{dCF{*@`d6J1D3wHCnT$n+J<>nQH# z#7d;ei$rRGP`;aIs%{7Wd_t}EV~>O(#LW}XkC5#2v8?WAI=#JgO_|pDaO_1Q_&j%r z&EniOm2v<95iX=i22jl@aH@Z^Q)miqPBfCCsl!jW4zNA5u}AhD5xfl2tA!G%-%MN# z(sn(%5#OPu>gy{REiGXH4(EmzS~pU*F!CUKuF0EabuHk1Ik#R!7TlbXI@@P=FO#vZ zE<%^VEL88uy;l;>Zk_fDFLsXBj{I_6JBYloio!`!`7>)NCPhCD_Pm*SN1FxkE~mkT zENN#X1v_kKE`A6^dz2^Oe!%|2rN?1lr!$AC{#*_51Xy5!WM%R=GI3OmRbw?${wiKE zl62REM2R@!rcwY+nw`p?8J_HKOyYy@9ru*bvtsDpXbIE&TCC&YwYUfuP?K( zu@z`NKplYylcC47vI*1j{0n-7d{B!~EST!~`S8f(=jx)~Fv3;ArKpFN6xqAacc?AS zeZmcV7|eon1xOMt$@gZ*7Xx$08TC`#DVklS-D6ZUN~G$xk&C?U<*Vw^TxX(|_sIPw zA1)Q`>KWoZlAn_m<=?YD$_Qk%^DBCgTTj7Kd5WJfF+|?uaYo)7?$yE3RvuwA!>j-H zePtr-Z;eMQZ8+H8RZ2I_xo$a*hr@b&=KOkZeu^@Pu!y2oi?CQs#&`YBn~T#0mEzX;b!}_hWm{90^~o97BU5@9o%4ldMBf1f!6LF~7p=-^oCSZYRlcW@oh9 zvKP12JUP#Om{lA%MzAvi%q%?Tj8;y2x?u&N5qTM96j4A)M9sVWbG)q2k5>ueQQ%T+ zD&{=7yvqzWh7>TJ%%08$`ggH*;_`pLoi&pI?Nh%3`ppXgfs2vpf8_#eLn`C{*jZG3 zyxlTB;EPx-h>8%WmkN`Bn>8h>T>`y*eu#jegC`|u5FAFfqh03PT* zt27Be+PJ+dI=zw)VP1f{fSP6!cbjH5_DNxU8UR-rhdcYYD<%NcXS=&KCJb#wsB5^q z$>&6n5hk}$6^|`y4J)04!lxCMtlH%lt)AP@*e)a`Wh<$t+5Bqtqj~)EVT2tWx2SG4 z-;VV3_YD_6@nhco=!s#|T->>$(hPSyNdWGau5jF*LTLg0fujnFnTX53bE+*+#KN%U zgF-~&N48V7r@XdI{~E>j#(T;xy20su=pizD$f_ zj+b`iT=+6Q?XkE6Yomi>yMEEP&^5D{cp`Z~o|@Ovfl*$L19QfMO)ci!bhA~YB;G!c zb8v*RVEsb{>{z}V8=RI}ou*iuy>fJvxf9xSNTA=1@Z;dM^6?0GT25WUCvhu=2>9F| z$eYDnAIbFvExd)aVb%rah-X?VF;4%v;u%-QWGWb7lHB+vGl3L8;li|zz;qw_Ot264 z7{Ho`yQ(Kd^zWSa$ah`9`hY)0ReY&gpX~ipu-E#b9oqJywMJtOGY|B?YEzK7mxbQH z9^K6obWdbXi8j%a$GU##hz_u@k>QWu7nVE9 zSvOI9OIDE}=bZ8=1ET9S-(>8<>Ym)gPt8dUcHFy$dGTm2fnL`u5MdK9tQbda56qE|w?1Zu zucnj?$NlBCG@wqBs%wnjJtsXkfabMuRZV$=Za4Cv9x?hml?&cK-AegDu+;w4-s6F0u!m~72mQ1zu!Y>%^p3Dwb~!|!syMpG&|2od z#ug(4|9{4oQU>j&X$#}|Pj1FR8qPWOh)aP7d%OO72W=`7{8+dBb>GTN)B}@U#a6?1 z*JY>;)YuFHx&ZlB(h@VdX`O8u8S&Y_rVA1u7sBW)!L!j7QgQ24G@+*$#7d~lN7FN< z((?P&b~AGU2%q8l5TxH~f7f3m)2-rVODb8BUffS;O-w537_H=hBJEtN9)JM`MCkcr zh#l(ENlNn&nly~-(=WA;T$d!F&q*AYsvj=Nhmd${YD1-94Db?mkO+-3^aGSV4c}kh z&-RU}@@PuZ3qA(09HaElugA5o3U9hCa#nfTtT+)VV-{Sp5!+J4`_{lmzcycP2?<+H z2AG3@quCr(WnTRCU`zZrL%;u4KFqTJv5 z;vOcW&6XuK?F(ACKw=imR7Hb0%3xA<8exFn`u1wOodoK)ur_{kJ_w6s688mZHWWZh zK1_IoJ+X*QvL5fw@5R1z$2kx-d|+m=nBsx}fs=Mur`rZp=9+IhKq)uzXgss}coLEF zQl{}rAowUu1ZHp>h?2_U(&yo(p<3OGGJ!&m}DT6V~<~+Xu+N z#WH%(t^B`9*gJ;-#Khe7E%@<}VBQ}I?J=lpf`@6&uGzkHm*v${>G3WxtudJoK&n~3&)~@R_;++3f#fj_Y!Pq23hG!hdVW!C*vJ1CW z;a(fTeI3aPg@1f?GWi?>gZd`WPCH>RY|Wu}xTNEqtUI^MNZBLKEqw?bL$z6wcOL*L z3COZ!(XOR-(nvrrKM7-&D(}Kb%H&^KTB>jFQkPag z_-alcWDRsh4XY>V1zqu0K$Y=z zCsuYmh}bm+OoErBxJ95`wDa-uZg`nEt-eIQ%s?LIQ}nYBj{!j`GL|C?e+2RC=CSKl zlHPgjojAMO@t8`QZ z3}QP~F>zUzx& zKMuIcF=Fk*RUVIZ#)FzYfkx-T%35oif0GpD+;QBm5yra7dYs>2$^1*Ge4X;+h_dgm z=|(*YK%RATFF3luN%a7XWw4|Em=a4uxCo5(z$Ge)E(IyYLy_|Xm$010y|=`;zl@_= zn6hW5q1K$(@h}>&8B0G|?xPHH-;OM-$N6>&`#i+B8xut*pTg4!XzyyRLFqDk{sANG z7ILf-l@z-!rQy>2`SSc1fTTKIiX?6sMx~EfhNQXx?4HqE{L$o_WfD!|(aPKCOr5AexMr{W+uWlmq2$S|O=tF3Ja%r30XRB4e6$we&l_ zz2v%bA6t*>j7dCs{M;vw{Aj0i+`QM!{5`7(v-}c%84AdbLJHvwuApIpwK%_g)AC}H zJzVKuR{(0qSg%~vXSoEi1F(@&i|D?t!mie^Hr;259DHd$I|P6T>IT!w_BPK+JZZhk zX?2DAd~L~NNK4eQu^(JjzweM7SJworaAqddwswcI>f_2iF$y}h)icK0H}#I-wH}Q( z3Gqj_+w+NRG0$&6s^85psJC;z`1BdF$>CI!xoL}9@k=nO`R`0}V-&W0r15)GRA|`w z7A{3W82?(6peEqxN}l^xUTdfY+Js%NWijTWnC`9UJJCfpb62+eq36hqjBp6B1rnr> zXb0)m?M${!Wrc}nHVm7~PAQj^U=S5D{Qge87p3!oWw0=q-xyS(2GI+u zV%9CUr!0b%Z;;F)m&i~cAai1FcHuD*)#~X>hg;yXwJ2DeK%5L`r%hW)9)pyyNfCpD zXrp2dGM#iL7zmAQD`S&`T}TFil#-!f*s5oALQXiu#X(N(QANHRW`SGy#pGEUrVGRO z=vw^IThn%D{RU~#C^VAze~VXt1h*`k+gIIJs9-jyJI1NBp5leRGtc3}c$!0jWa2Z6 z^DE@6nkDx{TzE{?Uez~#$_N?V6}bce9nmhy9T>OLjr zTzE-IC@Y%*?1zd)@D}m}uVzx{lT{s(-Bz^_Jxh|K93^7ly*0GEQHF;!Mcb1nn>s;a zA8yJ_dN({laK=tS5EjluwGH7KMzw7_u6mNwEa|p4IBIt*@qBBC#6scMQR{C=hD~hB ztOPEwV&oPMjw}9gRmNz4So^?Le9!$a!{I<$F89vNbSGx7Z%Q2h@9Z1;>H@VJO$Am7 zdO<^q!0R8t3a(+(%#)(GRKLO>_3xtHHoG;SHGmhjQ!)Ejga)I z$rF6_qzP10ptH+4dV)!_fEM@n1I}U)CJg zm%mkWc0p6}$PKhAX!;PNYY&!?Y)w`W)YLkR*{($jN2%7?^Xi9r^IkN2(L-}zJsYhSy&2c`$3i?OCxzesgrC_^vdcZmH~5x`89*|mS!-|mCgYIyj1 z8R_T)Lh*1DxW|>Ktvs<-0-!<~b)_xj zr!lafgRL^OT#0})+Luq{Nx+!Wx@4tM@W;R7HBr^?o3(Wa>kb=9!dzoPS3C%iU6rn9 zs(%lDms52(v3Z29=!{QqCcORpEYPM>G@n6XPBxom*Rf{Vt-o4(oT=JkSAv6+d8_a^3n8F_Ri--Mh722*Kf}C`B8#vTOx-3Z31({oWQvsSO+&a> zz3QG6ulL6GGxRtk2Id*Y}{l z%SdfmF4jBkLSz=OBm#t!htVpvzV^IA`)y#(F}y%|3mT@qaUFK1N7Y-W{;m7w4|dva z`wdH!l{|Spa4hq zX!jn`aR}#*x~+rTT)Wyl9Po+pM+9S_>l4?ZNO^Zbt&4@_Df~@5532W#=Tae=nC3%B zbiXuF2N{@vHT}{k@~G`Zo$$U``2Iv@?E80p48i6?yAhmO z`ItCG+o@8Y9rXp<2w3y1Sb(GACijm{Kg)J3yozU(QrAM2r_7wmGvYyY`>b%^60c3E z#h_4DExjc64>MhW`!eByTZnS$f$LlIU?w?dpM_iH9MfMDZJmv%ev*Ss*kS9UO?Sbz zndFgc3eq><{<8|t>m^I2l=4TH{-TPI5>tk-d#TVyxauzuAy}G_Dfsi3aF|xfq=Wgr zBNtjLeKtRn?);jgXWr&M@CH6rqK+pH+->JZw&b3LMR24|Mn|Q4x?f$HIDY#42mQ|7 zvska&rOGZaOT%$EU)SzT5yh9R>ng;A>|2%$=HH?rCdhAZ`}d~)d2iy7S8FLnD@tTD z8UD^7n(Wq=f314r+r^3U{jdENNq`B9>Do8Y_kcnJEu-(RU0Wz@cmb!!&P6o|u$U0> zq5v&Lcc*6QhfFV@K>-@K<94qeU!QZI*`(DvKeMHsu5tFHL_2|F50uY~g*C>+_Q0 zy=m+`2gleiNcYW3{=g87#A~X(OHNoa+*j z5(wH`F>!EkSh;o$WN#%`Z(EdMc+AgvyO2<2;S?c4(Ggiz_MV}{W=-c!=-Z_E)8in{ z;{7kpg6e8;L|5b&uz#~9lX2LxtgmRk7lZc0-!JL9QADreVcO5MxgE1+Dl|yD zyI?KJ+%a-1&knO{!r6;iJC zbvh!*Q8Ca_uOaJ*m3g6WZgWP9ix4uOp#@pZ+jQ>CyasFjV|c(;o|RL<`dT6)O7@8# zz2reeq#cNQ`eU^Ln4CbY@rgiK{m!3cy&P!gLrce5!3%6kzR|>0efW4Q#z|)+DgSr? z<|^wSldZU`SDf2T+gq9M?0bro~2hPO6WKC1+x8gAy~>V5_jZ zwBbq}(jM>qS|^(AF#`OD(MS>swHOtCJ=sU~td0`Yx6o2yIVdQyl*|R7g7C`hTps=c zE+K4l&<=wAeyN0A+dOX-nxk>cG0ll3}&j#pQ+Epu;B$tc+{ z#HM{S2km5yy~OOln0q0d!=<7J((u0@dUACe=*c@n74QKj_X;`tMmBDvc*hBY51y$C z7N;mW#eu>Smx19v!|U3LAM>`aOU8nK$;tmUad8GPWP>$@{sfK0@}@gIxSQVtxKb{) z@xNc_&;s!?Brkpqpdoo1q}4JMVRs0>e~)WrLvUaX8)O{rFOq{b{D+?U$CQt5gY86( zJ`#SY&3wP)R;G6Puc2$RN?)`1->QgeAHEWW=zYLERUu=zJ=;aGD1Ywq?v~ST9bpmr z4bTrOLn_VL0$AR2c)3ifzne-UE^MI&HwYq>le=EyTX)pazIhKo20nFJH2BSAc0N6K z-9w>B{2sX{0UGmv1cQB}fk^EFpt2Mv#`=Wl-tZwBbvCNVcdTh>M7F*$~z^C zXp}&@?wsCDtDYoAWGUY%+^f9)up>Y(UuVre?fLRo+~qW$6hr{PmXaUMkI@G6#6cy- zj60cjOC~Mwjm{}u3$25qVC5alUQ$d=HmKFLT9<#-p>vot34+HIAL$Zh%r4;RDa$*# zGr$8by-I3!0JTTh+Mpr0Ybsc}X^dH6E$QjR?C=I6U2Jv$rh#XipdDc@%GoLZoKZAo z-B)RCo9G=fvdG%GUp+cmo0AW+zWhWv=4IE$!S^pea*e{*uN~KPOvjsbl^yKU7$(eSDpU)}TfbqXis5Ehe&zyr9t zAYL|QS5g4sY!%JxP4DwO6^_~f#kPjbo`dqe#}KO1&KsQ^QH`;c&L_a)>;>m?i=eka zpz8=EMu*02YFB%J7aw2J9>e}HENowCK7oBz)CcxlUcc1~+gnKz} z#Va1b6#f2}qY$kKsD1?ml^|4wel=idl`k923$3k4%2v67(CDF<7VQjQXIp+Q{#P%3?T(Ha*u;O`a>^*NQkdE7C4Uc&Zq4IckD4R(s<&z~3^nJqTgh3V{ zl=`0aID&n@*_CWaUrYz+g@K=iD*-r|OVyoi9}_MscxF!)Eub*!+K(sCb$>Y=7Gibg2J{-=uSWBFbS-424$ zNC$#UbM6TVfUNL3z)D!USjvor6_*fHbo_?;>9ZOBI~UN|*(;7w7^HS3fLZ{kNftMU z8CsLQRIg{Q|Me?ThF5XkaHbUVSX-G3rM#2g$}#cC9>wv%3BWms@35T*B$vI9waPa-3^k#Y!W)ZreMeBdpUBS1=3YP%-6RusU! zW<{OBFJSJ1W%;J{iAQ=45i_7@@ACn0SAQAioH9X6*zDq#nciU6C>0k#ME@?by-!xc zb=ARg*6}$&iKmj|owBVodSO==Kju!2(mc4IS>fGwnml(}1)4|AO#J$ zqk8_{0yhRFeKRg|pXBn_n(~znH2`|3 zP({wD6#FVu?hI({o7jXT@l>Il=Vq6PLh=|4urn|)eC|q?enRPXZr%Ex^$VFaXk`?t zt5<;0GM$q^(Z{t!Vw|z7({s;2gztInaSKy07T&-(-ybI83AD`Hay?|*FQB;oxzB)iOwX2gX>UlHs6CJ`Dq1mI+?@Rp2 z<9gyp0Up-m$OiXZTf35w_cZevKB4NXF>`+9c|WII;1Fz1frM*=)Ot0tzkFt(_uf`D zpjEqCX8^L{nY!`$(6A-Bb$EBjX0o>EW3$bY`Pt`SE4r%D!25J$oAqTQS(X7z7Wbku zv`rN*ZX3>r7fWmH&+m52o{TsYMhV6}7}7USL1B${%)OZqs`ab?9SF2NLUP8W@V%JaZxKL)-n zIw%F{^@j-?p{&ZMRy!QMyUUgRZsNYn%728~r(wSv3;j+KkYn1L$D$r*5JNq}{u`(< z8lMN4u!&ubWcU2MV?BpJprl)j5vz zxRY6gQd48$m8#sSSo3OcP=;lTbzQ?r7KY2U^Dp<9^s}_n)QeVig%th-Cd4faRF@nv zo%*u7-JB(o?AaMtf3z+E=nFj8m^zg`vA<1HAdesa8(D}~3hVuEW}$lA@4uOa-$i5p z<$>~CgMn<)7m<(U|D}PNwhHgVJc9$O#!D5vo)imJ$%GZ?!=EeU7AL20%0W*IoQp9;q^=>z@m-HOg&P}=(e~SKJ=O6Rz!ppjTmFPm ze-_=Jq#JrwlCN$|Z(VZ|dj)g75|A?Nq@Z}d97C70U^3DX{1MlQn1;2h8B92I2^;3M zYan3Y9W4&3b>5GLI`Oap1#9TPN0&q;**R1pHQ&QX{X<#KEIE&|;0D*ms8eOL>pY zP-SC1e66f-iRR`h-6>z;qp;20&lP_xE%-7uOO57zBp-nAuaAZi?*R zF5X_bXKRBjH@^^sfg`peQ5UKMA>UYAL0I;xh8&uqQe`I1p0)}~N;1ISFYRC4dy8ry zD&g^V_eDTH#Un_*ycSz&2m51SuLS~tZnl9v1ShjfM|E;%McfQE3Ro*sP#{TeYik22 zh;PxJ{8hotzohv@T5TuymvY|B(W!7G7BxrKJf!QGLT?7)>W`eqB*K0HVuTC7ebX#k?eIPv^K4G^23~QpOc-ywf*FjtG z0pmH2^}snH_Tz$h6#16x2T72HH$1Dq@Dw3zMJv-xiV9q`dMSXLu*8?+uj z(;#3B6rpS!;=gAlwtcS_)wvzp`Fd2UnRGW)GuzH%ttb0#8y?2~)n@tpVXKLt>?u$| zaAyZqd|R7tE5x;OdAho!5ax8p^UIp{r)GUD&6MYegZh_&Y}yyFZ{~Xw$6pM(l*Se> z>;-y^CTcVYQDK?-y9SbFp&yf%#!Jb!2QTLHu5pVm82?8h3I+&x)s{p4QK?3KVZXB& zR&F$iq4Tx#zXaM6vvg{mQnBu&{?B1cMn_7ka1P^Cqc>>Wok)%k@&ZZ>LQel;;D+Dl z5dj!u1#II+Yp2r@qQ@wGa|C%_z#?IB6^M9nDxMu}#ICmDAR8lvYN}*{Wgx~uveUSMR`$r7u25O?LJIgVg7#M3 z#w{t zEk_j+2|Mg29zTEdElHiF_5#jnEY006E<|eHtSz&+6|3GCf)zwv_rNomm$eLKSkUec znkhtyR?wPbgoDF@NF|^_nl=1Pc0m8v=0G+UYs>BZc+*fEOpk+nrq_de#Ob^S`s};; zpYu%If7VPB{yT0@Rg8wVvZRmwT&MxctZY4x#MiHv$5| z!Wu*wLj8iAl0xd?kqMAc@S{P2K?Q(FYw6u01S*$QVh+F6eN(;aG7ufEqI48A9H-!z zyi@_@czvBc_mP;PjZ8(Rq_u(;7>UQ3qO>q^epyMBF{MwgSj7{j=Pd0T@*dihkCy|H zD>sH2)&B!La4O(|dlQ*Hk?bNXe&pmOLlu}}4%M~^4QI}u%CHs^bv6dP?L|?2;6s6I zDajheJPDLEy>7}ky-e!W0m{Hky%ruj&RLSH_OrKjSJrGXMLUG)sFEk2RoD<@{%35S ze@|_^q=4IKQ9N1wgXL=;L>63usL%C3vP{lZB7;Db2uH>6UNQQmV|+9Ee5S3;y+vc_ zyo}#z>F@_l)C@_1wh}QRh3Su^0ENzsbW&+F-Zk!iHP=YGiseYsjYw(CLeR6j;7hnD zmuMcruPbW)%m=Beuq&>v)bfNBNr`GH6;8A8HXE3GGyW12wrlFmn^uYi)v4)h119I_ z0=4X`k}SIIkOE#8GjJOW0U1VfRiu|ZqP0>GOHIUnHytU_Q*t z1w-E!B(Rm<1B1Q(*OM2a#)&PEz$-M7oT-7}7`e~mCE+{Jt6D2DjnS(sv8byhfOdLf zghZBzZ-rljUiS!B9(+iE)pO*yC~{LX#&Xs+d5Zcm&6S{8Mofw6!%1Z!=~w9aY7NOO zL*4!Kn2C7N{ZbX}eE zI_JFbgxsZsJERU@j^lRm0xMTFF#VhxR3zpV5LF!CrYwHzob^e&IeH1tIl@##;X(5! zB<636q>9uyfON?|QaZKe{8lBlUtxpv)EoT!UBfQMz9TK0r;8(%mit8mZ>>---t`<0 ze|nrX(@R170Auwtr~!8JA>cY|NC30a&;*BOM`1t%vr$ui+W=$WgXNu=?3FaFab|aT z4$Y2|v$Rs)Z)*oP)JXk|>eXbbSLMM@4IiAI*D|i#LT8uHShl4)DY$A_*tFIx~=%~Qu&_lrL#vez3!z9;fJ@EmypzERiFOxa69 zF3LhUl|C#(g~{U zC>EIQDkJBxS-oLW7KL|pBKuaXL5{c_mn&b8DF0;HmhG)wWF{jm_3s*90 zw>k}$?Vm-^2o)i$_l0p>NJvvoGE5Yl0g>r;!>Q>lI*j`6qtVYZ`V|?64YPvr9zW9 zw3Q9f)@h;#DT}W7Q`p|bshPK30mAP-JuHx7*deqWc?Rf#D?V@XzI1m9_zU{v#kih5 zS>a&{Z}1?6YC%n1EAD%#1`hLo{t~227^r0Ncr)zz)h?GDN##=!-%6d;n7=MD$9&?R z3QF|j`11t$RNi*Hhp|G|trlv%C)rn`G50oGOD~psqq0{v&iXD`lmCY# z+md|rIl>}9uIPVCZ-9{jG#&uCOH1hkgdJ>cL}56iO!oJYI+#18Hn#7r{|w({t#(_@ zp8K49_2<;#>3%k_U)b~lrp9gq? z0U!@Xdw_Pus{gA4AlAU^o6yl_O>bRSRiXCux)S`j0@$*;UkCLZ!gqk$JS)C-U#ReD z*|tYpZdG@xE6Pg96?{P%AR#n*|@fe+I91N#}@zNeK5=O=uW`~38$IY};0I@`KK;=LXr z0dk8j6mtsr3rr6{US}4=X!OKzU$Bmm^4QXj<0j$GV>&ro?kNv%gqA1rb%b1p8Btz{ zRsA{(^8s*O;~4qBu4@H%HP>A(*W+09HCVHvF7mnQI`5`y-~&xu(YlC8Zfi*J*yT`u|0H6$2!JRwBL6ZK6ZT?M^VLN@Yz|L3=-AT z$IF4cFpxx^@A{@eMnIeP`pjq zo<#b(k;(Hms#j6JK+JW3O-y1O>r9e7dN1M-pEH2eU<+@>kl23a{p?}p!*ZNH)*ox+kC%iBBn4v5B}2w@lQbx zehrs^5Qp#t2P5dyy>;t_=|i_;>*rk0EF`lf#|em~R%x?s0X!>k9@L-)0qv_kEWcL2 z_k0*J6X0;ytiGHc!J&5Aw8$Vbcx|hjk?T4(bK2g$4gJ%k5V}YY8`g$WtqhD#lpvzf z2nx|>i564A^Gxmc8-Zt)2^^jOld6ry>E78hK4V@&3Zm67!@JDC#ezxiDv6|*0pk>uavLY( z*uQ@fY2s9TatGY9dKZ}iVVd&n>Ob;ktwbPnpGDt*-3K>Yk3N+isV%gkHIS#@XVpz^ zO+y*h$m=5JfgO71RJ#Djj2ESO1C8Wc|3K!V#LT`b_SNqM;$D;KkH41VTLTlFl=O)C z(^`y2oRtz$zhD+aJ2Z^CFYh^1ru0FG5te~Gs^6+Fw)iJib1=0QR{+usAl(zLP7+RU zuGszB_tY^i*H#@01e6T!`>~`1%}Sk}$8%>e0Q!Dek4K=p;>jz}T}T;t?vQ?aa#avDoQ_1KPA?-`e zFUXALuSL_Gn&XSYqg6L=3>V8cPq=guL^yows0F^-$Bpc=s*DEvv#>&mpD`DX1Y9#1 z+bhCllJm<^l2UXgC+M7{P>CxM!ClT~*)B<8Xfo|AgUYDhxr25JH^WcyOrd{G)LI6( zS!aJlQF~p;F+wH49w0qCipQSTztBH@XxuT{LCueX9~(-oRakg#Vfk-OFKZ;Q#J(T& zo&aoBZ}9s#1O@0JqBg@=>`w*%3Etb)?MZk?)Lw40Cw=w*p$^Ce zm080Kf({(GkN8`n=|1MyFMWt8FLx}Z#Fv^kj-L|LX#8}TMZkvS@@6=0uj7hCu|C?p zEon(gj~Am^7?8>HXrA1WUr#P|Kk_oj19}0eSmR4qZ4HRT=BtFPiWp}}zqRh92(0Wulk72{%|o@D zK!6NK0fWQ~G(}r~08P+2`I$hUbJJh4p8{73@EP)(d;mi5(1em%XWXh%MEzp86SXo9PZw{t8GW&eh zDI$2pF@;&-Rm-lnhvOKM^6}|JN=R8^gUDkCwL5C* zK%e;=my=w;Bt4m~PS5o;+5XGS15aX9DQBfkHzNxIl-H~XJ15Q6FH4Qea!sHpTKaB5 z&hddwRet1bee>;<{*Gwwa*Zi$@ympfGa019N^ z|AoKW*6Mo8)&g|+Q=Jj>zaqWT%)0v=4(x!p%FWVNJdi_w(uyY~?`wZXR*qlZkDt=a zv}|(1Ec%AqT&FW03!|5GA52E$MmCf;k5*FroTlH!N4YGJb3Odp;+cty?cb)UK36e| z>ojyXf~#(#47bJ+#mHNaZ{F8k%eqS==cZbc;h zJE;N(4;q=4glu3ryNKT|@BNThca4VzkcLq%$37Hy=k{3Nk?E4gkzQ+)1l-n`;>gy}tU`Ca~6 zHBSG7_u}C`N0YV|0axoJw29Z$JgAAHPhC^i@|a?;Y=<`a`HRpx!Y=N%HF=adDTnd- za{pRXhecLtj9&uz32}%JDfFG0?unlzrn{4oFMUCnYTNeU*h?W;xxvzpBR|O@gOr^fIf^9^m|nc+qRDL%yGx__S$=h$P&H0RGN&V1r%Iu zVpGGpL=Wm?Xy8Jt9$q@Q z+S?Lclv(XC8<8))T6I79VCtD#H+!)dtZ3ON&AlP52MpXSY_&h_mYCY|9s{E1u1U^Q z#>x3}_Qtt*B74iqF=V5BM*vo%)0xDEMWA_T0+|auQw2eq$Pz7E=CsDqkP9ClATgNq z=8_bOsE9?5FW0I2RWe#+_~sQ%Es*ANg+RwfuQbt*PL3Hf;P zPbR8QEB$KqIHUs0;I|#?Y>0U+;`grkh_N-zl4yzkF<-ag6XmDq&M==K1z|#+t~jPm z|2_oF@&aq|i2-w)V?n-zFxNJNbe=fk+hWtF{h#7zg7!YmX*~-b#6un`0$WMzoML~O z^OHqY)3j9Uv`lGR_Doa!d23hIcI~KBO(8ks7qtkBr$**Hj+6J^6KMQ#GeE90)h<}F zrc2GVb0QZ_2FZc7Mc$0>~@TlkaoDKc4vNHWKbmK9k8(?iA_r1MdW6& z2@bKF<%3IHjNR_-nit?#>euz>uE4yAxt+hVNWZKN2p{Jqy#q#z1>EL%K z!R?LXy}KzAYSB6Y_Sg7SDVpXKGx|CN4_qldU4uf+NTCpM$7!_CW>1VkQSaI0=Y=3< z0|Y`N+S?`A{i^J35KA)k$!m|Ua$q>G@d}7SP-Nw7Gk%YWy|5PZf5CM~=9m19%pI+llD?cAEHvZ0y?hT@-Hbd9g^1oAPFO{Frub z`qkW8Q51auCa&wK*6d1b%5%@d5iHUDI|WdKptsG_T)Z?33(>4g7VWTUVaXSJZufqi zu`dxOIe>zE6Ml`%_SuG-o~4^xOgR^WGu8eWkLStb7l)+F7rk^>qe7I=b} z26qRhb^UOh&;qg~k@5IN64QpK@9S`$y2=n&OD*#(_|*AXOq&qljN(gzE2v;Sb=d(< zww(;j?cV|;80R^v4f;7ZbH;feu2DxE67P@%zTdZc3*E31(GiOao~0f-Na1iV?d$yD z60dAUkJ9vq)DA>*vxSUo(qx{hqRk8~Ts37K2gwgQOfjKN;t`E`Hk=z?R|=1n{mY;l zs!DS(y(9H{s^}+AUnZOpXBy9~ew;{PdVcJLlJde>w?PjqLn0kdMw2(L!%kFDuXoOF zGxacd-w!bNdsj$n0O$}%@HG!kF%+EAi@z z>jWf8Vgml@Tb>f=Kks)uLN^A|<>>%-^a3c`DUClJtz=)Lr6?I}!b*vj^_ajYy7F@$?NqPJG_G|F{-;<@|z!{24N8${^|{%r!?nh zCj<4kB`bE4>hvWzDwo>T7}po|`KQQAhCY;y1U9gA#AYeJz}O?Yw?(} z&GY^-rn`wxp+i&9cKIi{W#;6L)DG$l|D4Irdh{%6IFU|47;@ekng12 z0=uF-Ky9Gyx757NypwM!Z<1sjScPqc`Xwt~HSP!MI#Q)-DP0$*4Gz8w-h{f2R)v>| zY76d9!1RGpRlfX$?ZMno81uJ?m0vRm(y30r5q>sk*t>Jn(8P-4t1?U58HW+_UHOW< zN8RLlq5*8;WR95?!Src{5Q#*$9noxmm|gi0XIWN8gWu>h4xh5me{C0(^j-BW+A`fOhq&1{fxQdR5m_fuLkHRP59b$V4 zQ9~t05#|Yoi8(48!lhLKT32(h$Dcp#_Fl14StoaFN6ikgkHxB#frmw1lkxs56>{K# z^NFaW_u)trLP?Zw>&uOE6oS^$Uj&fS2Pd^trcmy2wg7+2 zIS*j;@|fd+x*@is_*w$Z_q}Xj*$S=4!{{2SqG-BQUGV659OutWf`;ysC7b6f@UBa= z>ll>Qms=8#gH}Ecq%&r_9b=7lhby ze}UEy2}d`Sa*Rqz4A6OUcKb`qCN*`83&uJDVi*_l&Re38PC=qQb`Pt8dR*V~VSIN-0@~`4!2IZy!*j3$K z-FM9!YQ|%scqUoxh_md|{};tD?@{~(aD%=kBRU<9;e9P++yp=_R}!?nM`b7o)FgrRd>0`^a7F-dj}Kdiy6R(vvKh3=|-!<+N?4oG;+`Z&=a zKOQBud%LzusU989MuxHUh1t>Fo_RMWAtMGbmb6TKGh>|bNSv4Zc=S)Er!wFuCDRFE zl43pIny1&1z@+oj>f+_*s_k3aI~<%*|Mk2Dj`+BA)UXUW^Ei}_$!hPQ?ESo3Fl|A2 zBkxB7zctPobX}~qH^`7j=$#+66{oVT`V9!JSNon)#YUbn+9C$mOd(_=qB(t=CQxa9 z)n%=f-8^#S^{+xg>v?g+$ooHK1&hIpBrZ89zm$PMQy`(eXuPqq+Mixzqz-!$kU@y? zftUc@pvd&;e~h^c?J!hJ=VfyC_>Co?-ScFDH<&(*0i)&Ph#a*(>}_j$cD15_vo- zi*|#*b@zaUc9ucM(7cSZ24f9yqx${#a@k-7bGF9Rh+Nb*$yYcq@OOg=pmY|TRO5>h@JiM zxCzjueKzk3cW5cIRaUF`MYttXDp&L1FC7>&t`FBUlWp(GeGjaZ>LGog*5%nRHNZ(G zz`CFq1E!l{b}Epc7&IKtqV3SjNrUIAiP!1Wde)@yzJ@=>IWt~Q#{oA_WS7Rt!aE-c z0XY=4$nwdnKK(8SdkB8GS@HMW*oMOASfw%HK&jcg8T4aupifH5O`ety&@IKLujsH| zXZrH@!|^W>F&Z@vaN~WK;RA_uRtX^6T}>VbNg<3ffiqy)11jy+u`jJLoBm3t*#+Da{ho5#pnFR@xy+DlRT!B$!pI|y8kvWcs!lZ#14bvI@Vv5W18nCDN5fl&B7P_WYAB>CzgqM9-Gi zfCPg@0Ai(OoC^jfBwx1`w-RfYw{Tq{+5dQ~cUfK8s@?s<)Fq*PDf-wnhZ5=Dxsr zfh2P697R%!TdbQAUnj|q){TRa*G5abqIBwHo&%Cvq}h zPYcs`TaOcg8r*ySe>d%Sy%2QwePvWS;)IP^o>)M{$SnJk)-ynk*}7Lh9r8OyIKU>6 z^v&t?oN8CCirnKi0dR$R<$*s#N8u{ZW~O~t%;bk9@`h(?o%)^^8~u?Ze6IBmYXI8d z;~vY~qFXLlWu_vVn|o*FW6>>)=!VqFnXA!ghgU%fKLN}0#?n!1WYb`a-laG*v=lTF zS86c?BgQOY;TYn1t}5NI*JQk!lA^-5MH&t3vkS9OqDlV%N<| z(in-mMp{%h9vc;Q>PtSgth*vj+r}hJC*-)^5T-f9bjvRfk8p7|F{Yq``O~((vFgk5 zYIW!A5{zoGMH>aWgdf6WXmlkrXrZwb(I$V723*soiZX0iyVd69dJQXbwH=RONvIX! zePJvcaBNFiVh0SO)Xa@4)^h0py+`zhwmyeeuQvORKzF&%S0(1;&Yj{ifxWR!JovUG?EV4|3=2?vr`qA^{vu|F*n~qm1)3s-SBWZ6 zCEyFX7f@WYMOLW8L#*|o0}UK_qOaf@{arxlWv9Da`f2I)>5fqgx6IO8SsA)HSak4$ z=a=7lr`IE#6$>SRL*#Itto<8KiSJdMVq(KDj47NKXRz)}f|F#ZGM{O(U!8JnynUE_v@7%PdxyC`pL%u*rB*0f=e8wjpeFmPQRPA%FdC&}OLxMCgLJw7 z?kBRJ8HQjQ1Tk&)aIMYJbQEE1V&{?C7UEXt5*YB@ojM#;#@-fZ_xTPI>wb+>zo9Bq2g#$|cPYfYB zV*4=$oU*cUh8j*lN3e{68M$~FNM?}t**_i2r7P^aNYpa7e4F~kD40Vdk;xkVMqg7| zKd=|&)&!*RLJpj+fy8 zmK!PAj=3hXs6<_=AeF3oTm$}qJ3*H9g@IjTVs~jZ&_cMhoRVgRf$Lx`U~|;J#s)~F zj|fAYgR$pSN;ecmy|G&D4W0Yw5Us%w=s6tsyuK9$&V1FBnb(rylp z3n{egb+yX!Rbu=;6|^a-PB)BK5mqQP8H~%O;zFK!Pv86DG<$fAI(g3|%?6FLK{2#; z)=$OMBg^TzLk~XC!V<`P01yt@8+W*K_6v12zTc*Z;(XaohlGldSH`MFjW;G>xg;I! zuFR1=%5W#id^b)5-C~yP;XNsCuxyv4aqbumB5-+g$Pr{ATlv#Mr4}n1m#xYz2qIvc zIWgR(&af#zHfPS7j5rZul|cf=U7S&24%B^N$Y=e^kzUR^P9fb_9;qz@viWO(gQYjP4}Z6>9Zh|1Z8=+^-YQcwYawy`su|n$fEn z3r^l0anD^p4WOqJ@Lx+KrVYdWfN7I2_*jF{|J?gQzN~E!~pR3O+qJGK590I(7sr`ustGHR+iu@9@UT?clN*ZTWpRklHU__(3?p5$?2`v0Z$jH`u{wr18QW^I!#>f4fnzRD*gev@I%tvnGTy+5wd&v^K?D^ zC{YYSnupbik(S$*=6a@cmaHN=3b@HMk6I_fQY?ZsOA@YWa>D<#aXOIHRBLo+8cyyQ zm9h7*=jNh3U5LAV;)W>JOp#_80J0z)IE^gL4Uk>!_YHR<+;MFx?iH7Aw!qsUrPLhq>lnlb8!Zy zHE<8|;Z6{`uH1AA|E6$Y_TnKu(BHJGCbAVFk;xA0+lXT5f(RDm{ zka|=Wa6dqQnk~C(oky;_y93z89fF?4*u{1{R~Pq3HLI=wpW88jtZemrfct#jP=Q{F zsdbZs9OFJfI238`E~RzKI#4YRq4<|u3B@$RJ*3~o(qX-1Y`Hq%PCoOCvb!;;wg>-Y zI=VXO5xT8rgp(+vc2J->?>BDU3seX7XmuV8?vHlyqz~g5{(gKSA7b70?%`JRwWDUEt${X(>1fR|Te7v0y=@s(Z_ixS$OsqK1vesMEsUMM1V0-xkP5H^K>x|;Fi6jHdCUL@ubYNzJNP^LO#}et`6p_fylKQ}7yLg^nd-uS zX2<|!MdL6>7Qolq1Cywutb4ZJuOPqZhZ`I%+sY-!vkRLvE$ba3!%i$c(r!e;m=>dG zyxFL{LS#qZ9RqThbdPd6y!aT4U}&K(MC!~#JoXh1uuJ&N9D(F82!LG)Wye7=drtK%$>)p*_eS3D`lER1|;q#dayko z(BPE3+#RNkc&rctH?cXC=Mzh@xp1|-?y=APItH`(fXRmm2IsADqSxQ}$wi6w_P^6! zg=adDEquPA&aI*WLwvKrjCunv5Wo!gM+yo(*J-_ORJ7gzB6ZInIn}D4FiBH@rIdZr zWZa8eiLv|M&m1v#3*8V-;CQH00MLs-!xD4@7C~5MQrXYUO(o32v zE$PTj-_PJUJ=CM*@OCvL%Ru=9jV$N*a+ELne{EGLufj~&2tzY~3al;bai0GKc1b=> zApIU5d)At)qA7N!*F2K5P^DY!OHIyKcBGkQ|*TvV(i&} zd`XCG!!1mem4Z~F<1p1uj96Uy=WU`%GRT$&15x>mG~L{%nfT_^VWje2+6b3s07g7% z;SjHij;jTE?=-$NUui;IV_Ucfv$vyorW% zV%(qkmrQlgiNLHFUpP>tjhWHT$MJCIw~^oPi${tL>y0#_ib*fK4t%fAyLEaoEPcbo zXunup1l5~yrX>?N~OTi4u)pe9?K$KY-F3A`M6yHZZ*qU zWviBIUdibs6uvY&)R|H#H$}@u28I0+zhw5P!A2s}w=9Apy7YgO0ljnaiU%!Lrh;hN zlq6FvSKZb20!mrvpB}J*$!&3*hlSSuB|H*uYE2vvHCEa8IT>ONx-rpuMcAVFBEQNf za%<{W*^;JaY4bO$m3eYX6wPgVvLdHjYdp?s&F_1@NIC=)%sE>zS{L=qWsjZ}I?Ziq zDqBmOKLJlv82V05A&Xm`HgCs+K{_#cFg!^@?^KRobq^JIwBgq1$Suw~4)PdS)R?Prhx zoORr#`b(KqEjWK?*l-k<0xS|rgof#k1{hvNrIOFqB>22}PS@WOhGW{|&blLyrzsvw za*RwTdA#PBPJ(lg;i&*DCe~g@qS{yLDJBijx^T2i?SE6{Swf#`03?#|O?5qVe26ub zN;WI1l0ya*cabD@F46B3FNPLxc5^W@_aTY0R5JIWJpXG0({X}U*Wc+o*ihqYqi7fDem1C~w9c_NW@ zHmMiKVT!ca;?LGLr1L2eqttVK^S}BVY38^M7%syw_006g3zH;zoh{h;*#JJ6KigXUjj&qI~TBg*Jh;0A%-GH@AjU9K(Bhpwd)4Q*R}1Z#zl)+6Hsd2G;}5$3ci zZ4|z%WsJ_gQw`A%_c$uJS#9+BspO}mH( zz$ZY9IT2YIwoKA238!!8ec1n5yXt6+cV^FMO2&Q#W})Z$Ga%V$A8lcF%Tg_C7zfl) zA9C3{r_c5a%>(3mT)nm#ErbGWTUdee{CDam+#06A?>|a@8pZwUB;FsQEe6T5O2(-( zGBfPaPx16?1P3N}6x8Ao5A>-Ok<=^*mwmtkPKm^*8R&pvTi%-Ko3hK2v9MQ9Zwgn$ zyFxq5CJ9?~WyX#%Z@*P~>h;8@=Fil>B98kJjez$q;79iG6ospWqQKfW{!G5JC<3na zn@aNA4?y4uepdc?$-ERCYrK|$*&P@ruZpDh2X!%Dv<&W}H?ciwlySSrTRko9Amwgg z(6^x8FKEAg=Xz?g{g_(M3_E;{z1(GBR87N|wFv}Wll=7*Dfs84{9?2YBiYkfQ{@iG z{lcgKg5I^Mu6@GvH-elcueXi3=31wt{-)XLwHW>6gS7M#*VgrgkD(u7TJ5!;+1*)3 z>gmtdkaW0<-PC=ghW)q{nO6)uIA8MZd2|ISC`1FXHa|#{02M3ZWv|%*E`}R@md34@ zPc=(P`!Ps5B*XTng_NsQY4J{C`s8))(49(@fv8aL)0J450jtxZE#4aLC;wiDVU+^63dQ z@)3fH{`ftHD`rH&1K05>TTcEM*159K$Y8d|`{C9)#ib`KkV1R3 zhy4NvMiG>}9>#i+s`&QnSxfLO^3GGb0T%fa^wqFmyey`Hu#~TqDXQF!i`MV~v`e=A z&QR(RYMNb{TffCPeiX8~Ns=3rtuj3ALiPysug@Qva|gSHp|~g>%vn*Cz_AlO$Me zu758pExN&2fE)+#ACy5kWR-Aa)iGrCGqC3ahR<-Ku>$cA2=ii`kDPt_>2!TaADF0H zpf$^COTX4JcyO4ghyY3#z5BIP4Zno)GGb!lga^5S8)mk2l16j}t6lLwm9Kd+17j0y zEkNr+*4+4I+mCwGL`M6Q)6P*Z7Y)RJ$cJNy{=pui-oZk7@=TWf=9x$qY!`(Zne}A( z0|Fwx5hgG zq24mZ<$PQO1_)#`m^5tNd8UvJn2f!$#+cKxi7_f7MW0PB6p)m1d>6j|&3ek@ky}Yq zfOAwC1_51^_rGof*{u;Ts)~~rmo%{OUzen93C$!L>%a8|_M|u*S~QY>IY8*hE%f(u zHIU|jS95)}IBr$&2z-&h-19VGp*;vz$wxHsczmjtTZ(i+qEy3l#S}05fLtPUvVHvaJZ4M>Fyad))>+Canotk`q|_^xgRPd zc9LZsW8*Vgw)%2{n`0i|r|(l@+8P7HY}C4c|CR1%Yrls}ufDMR9)c<5H>Dr6-8#{o zb4MZC`W+*PMmJFe9xS`XW=#;GEJiHzLOHzwSp%G*phSmuz0=>g+W`2I{>45uGT0X2 z>RuvLwE)mRtl||~OMjvL)hSKdBST&>AW6}DZe=U`mqPgdNnJdJzJ;ZxFw1)+%b!im zaQrs~Yltwpl^uF0-ha%30&^^kr~~=hTIivvI&?@n--iQWK_l{0t!}~geS6fuYi@FV zTL_;{01C>-I#h~fpyYAehQSd1wbLxts-Ev$DM=VBe#V*$J)whQx12f?(Q~;Lj6wwTD%6t|gfqaIDZL`HRjh4$l-;}Nfw&%YxN>JKHvEA9uct^Ie>=iLYA8MpjT zJ-ta&#A6zo*F~BeLI!9M5S|KX^&KkL{i#085}%AKIYcA)bewpoEug1_r;Y2WO2TZu zSA8E_8`I_Lva-?;$%4)35$R7mP(YfS7u4!>UH@odV~DHOI_6S-)PKtDJZ3L)zQuXE z+4hm`q>9DQ_(Z<hJKM}hCPr}qUP`sIHtHm86EN z{9kB@Z)eJ(#f$B5(eh5SveLyQoJ;4ZPXsb z?%B|l*~b!X)8m(BTm4yBt}?-KLW(HLX}`K>mtoOXgOW(M zJ5Sn28dgF=LP9;|s%#M=J53+P#6WFUblp8K&us7z+M_d(WAnQh39-gaqn+RHzdhQd zSvNJN_{xn6Fc19wtfgh4foW#J@Y2*_a7TfJBrw9{;up*HOHAVUWg7y*{*xp#KZQ@T zq~53;iSONUM_m9urO(0FdV~pv?YMn&fGIkq}n5A;ljzan=Bb%8|&3EHiLoUn?YO@@K3$oFy{S13GTKvExjNhbPH=r81{y??}f&S^kPk=kQ$a09zQru zQItI_NJ(QRtS<3Ls-0xJz4Z0TCF&(@Y%)Jj!a(HSW`fC#>1OR186U5X3RZzF^xCR! ze?09_aGbB^wc7yy{T*o6mDU_C9wrqACtEyy!k5(?KKL0QS7MA*eXp`GO zIbxa&VQ>!PsN49_qDQt59)igM^{fM0j>Ja~okHpN+-KHhFcQ->XmX|J3 z+sTG5uc-4_yX&9i3e1m)0s7Ybt}>c+LS+yjA#PHL9+6j)WtyqXb9K#okEf2;@C9KP zCbg-$k?<7>__eVS!*YELD&omMzU{4{aKIN%pC$27Fd0@dA2F&g=(Krx7)Q7gOlU~W=hWvBzVcPWcFf7V;ClU& z&x8d#_lk1tW{p7WCu7;gUpq=ff)Eg7;?+^nZmCp0GFB>nU{62n9$3Lg8XH;{kk1^s zI&4Fgk4!x1Tv^VEbrOJFIe+d%-B)qDHA-=khd=nRKouauqA>xi&q}xse=|^MlwF&i zlhApSX{WjE0(SfqqVx3s_j z3QC8xv@{Hz!;lIB($YP2cMKf@(lG)}((KBNm~fD0;>HCc^@up*Jc;eosiAlf9>=r=nE! zm~|!GF=9aFf<3v308$wEUgy)QTrTBQ%?IX;;Zv!yrkaspDFTjX4Xh7hk@=({N(Sr4 z%7%s`gzgfPF~lQjBW8ix{!~y2&1vjf;c3wpD57F&9dro7Nsq_3RH7rzNi8Z^h}x^p z5hfEJheYt=2J90kd2Q;9RJmT-mF-2Q$6Q;WLFRMU&}_~(^-OBkJEv#<3kCO|cJ|Te z>7h#<;*+8mZ{gtFY@o#p*F@v?4D}IC!$DoTAefj}Do+&Mce9O$8JP|P^NESx3=I$E zS0l_V{SVnbGX_7iw#j4+AX|}vi@(x8w1$1Q9JYS@{mQQex@?xr644;3ojmLP8AD9j z6`87CvC|WJB;lQ{WxGL1+xeo4*q9-RZ=K{8PJW~T3gisUfA^X}j4$ga+Teph=pH2p zZou~^du0>)Fzi8$M# zbhGihZq=fa;&&nC+t~dH!u%|1zwdiqyVhm4N@_D&E~)gZyM@tCW;SUu_gZ}QDx!@P zWAX?R)h&p)3@@55_fa(mz;I64EV0dVJVT(tx3oqTxYx%O%VyAlf-^0L9I85yOZ2v_}xyl`#ZV-PAqYQ&&T$*8ueEh#J` z`#vxl{4}R~PJG?mwAhb`Q~_zRpl2lyMmY7&M~%4z-|=>98)x@7?vfrYRft%gjLL$g zPfqdAV^n4QOZBJ)(4%=0-bF4N7(-uhGFh^ne94~{*Z^1+CI+fkv2H~d5+d&s-L?uTX%_?wJYacO zgGA4kR_O-XaD*X_Emx8lO;9^Le)CPIY{^JreBFiLE!A@NzIJ z9`PDNIB%+lPDZt=zG7Dnc_=!SFIb(y_PUW3h>@JvhWhr7|5Q$Rv5Hhpzhq8m-sUm# z0#df{kJ|=S+MBEJa=q6x(?!i(vDOh->P3CAiJXO%42_J5aGI@PpJC|ic#yC1PCK42 zO(vpmB6t%^fBQ>_&P5zFb^645VOdsHTs4PAXO#FhO0+gF=}wELh!VZ&`uGzl*o8|y zQL54uZoo9%4*Rt$Y$sinHgtFuXL3aRcLQLmc#{mOc3Iz4Lum$~Kf47ecW2Ci6;|1F z+x@OEh|X`V2104+*7s5saU6_LL5$jU<3eZSu4BBF6wiY-ZNp+hpelhS*n$=VW}Ld1 zN$hoBrlrk_UO7$X%$4A|y51~VfK36eABbiJ|9Yr0SLt`KB{nMIs@>5^S5*19hj;*lVgIfmC0ApoqbdE5 zN&l=Z11e&tZ|#9TyZaU2j!Y=A&b+JrZk&q9B|6(wY=Zm!L#j-V)1z-b&X*<$k2;w4$m-Xlqzi2tsB!BbIB%C5IK zbd8od1pXQazIyyj`$9QYPrITIvlrP&O(OcygIR4t{2hBa#O!NKK%$M!oEgz`lir+n z?lvn0B;7fEx9=J4dH7n^;G~ycnai9Eb+45q)?ru?p$(pFpN$uXpR7H8_gXS+Xyy;9 zCLDZ$t}A`%v&_bJX0c!TVAv?+qt*ND9Lpy9+jjT;vaAn2ngw| z&H2t|R?RvTBMtlfBr#-*f{|e9daoojZ_lsD$>RwON zm~kxySob1CmhJdsd&80e1BH8)()@nEwI;*#bmz3TOq6wr&Z=|$tJCR7k2J65KZse$aQqo7pD7Ib6mNvqtYIfSJPu5%(*){yKTbhPtws(9cfvzLK9D9QoR zMt@474P96iNCO?VZMIX_#0bq2WgyWv3*Baq%~S-+q-$IQ{iCs1VLKu=Um0t5k`uSu zr2@KZfstuCacll_`?#ZLdU-}3eD-guw$I-=&JM4v1ro+w&S~R(;ov6NA)6M`on9t4 zE+cFD?TCWWXVmZb^ZCC1^cXK#=kA7F#%bv=f^x2+@XP}~RasP?cj2HzH7aGxBp9u% z0#jF^@iNWlXXTFNd^s5Zoi@5z1c>Z0(~g}b-&ob3_YP^P0s-21K*{#JLLn-_x-uzs zkmp0yRo#JW~_up%`=x>Fx*TSSelY*^>?l*a+UXT1E_LYcYIpC7K%>XGwN=_C%H_kGKJ}7RgSN7@zwk;m9YTwlH7%=mHR)U z?nxDkb@D6M)8CB}IBK~qx0c3qu>H?Noct&7FWp7#IsgE(;1OgmmI2Bu-@zEBBu`dP z6-Obt%ayzc?aEcVJKQ_KfBy8R%UUU(W><|e6WiHLEXa}QYtrF-lY1b_F7WhXT{aFznK2I#No=5^HVYM%@k~`kw<)qqL zI`mW8MIx_|OdXW&3BXhtbxT{hHBDYb(oP8tt`OkFKh{)~q_T_E+pJsu*?y-YR+E~l zjGW4paeFSG@q;z(WPAMcD+jqmq0Knrn^XH%0wwysC^4m9FKura_<)AT)Mu>e(L5@K zo2nl0+r9fH+`FtVT?v<~X81O(Jv1P;EgC*>#fJ0o+3t~>Wbf2qPWXJI9hk1G4pOzd z-QbbLzFp>Az53Jpq@WocN&53ha?joWqGb8O~Na~%o- z%H931`)7*~ZnBEI!0+qkQ>e2E-#tJ`xd`HIiWBs9kc0=4V_`AUO4~38t zy}+RX7$#Llnyv~23~B~O*zRN{@jndVg>QpA{}}H9kIOZ*82p3tJ5Z>=sI18_GLDHa zI;&)H*wy%C@Jf0G6w$fApK-j=xq5`U!I9}vOTJ{?1n_bOca1$a0pERm-ly-KGWjaD zN*7s!Tjq2N(bk+Bf_nxt%zZC=82z|jWy%<5wY}u*B4{wZhL9a!XWx3ek93E%G}>Cy z@lP<=&-&FMn=2-4r>@IPDw@mn5*(bB;<#-?=x%NY`;7zJjV#w&&6Zfa1%|Y-0rV2p zNDhSpu|7M~s35`#gk4h%Tdh`fn}J0HKO)2=X?dF23}Fsv8zo8aNUWqc)?BMBAn0)J z$j#l3n8#)+xrz*%r^it<_fef4`$p8J6sKG~2L9(~j(iX|OX;*%kxsqk;>_epvOcDrvT2+)gzsV_Aj0Oj0mX zxoo-x5K!4N|OX7dPmGAT67rwqADt{uj z=!I*N)>0}ea!7dPSQ!n1J)^vsB5Evq36Z{ho8ZbMtjfbfi>CSq2qgXuK*iD8NmrBZ z@G%rIARTm63C*{lO!ejaxiuNSkDm&=`Xuf7pvBfF01S|HqP>rB_c_7QefqG^wza7s zr66ci*m-E!w0ngP9njpV%7|x4^=csyr%1n|Ge0HJ+g~xyW5r^cy4_al#R;c zMyM)0Go#0Gpi)(QqKapG(uoFvRf##=P%Y1k93|axD-c~x*5xK7PjvRt>Pnd z-Di1hEK&1OL(#uX-o-p(&e18@ykx#vmfv5bx!U{B?>@+^ujhLwY?6MV7C~}x$aw+oJ2uN2Im$ecegvh$aoI!Fqs{PIkLQc1 zgv;kqtz?&@je&9=+241oVpG-G15({J|Clfrz#=bUq3|wg597ht9Oi}HUWV0!M`w`= zEo4+rMS)Oo)Kokjp!0v$97AC5cPfl6X_Su^61B*x=><^GgC0e_Xi#Nn^B!P8D{{qC zXgKjwNvYlHuJ*;UEr6SiKjqmQqjMuA%Rq!u)wfdamr8cWGAF@VslLYrI@1&CL1b;~ zRb%$3T$6SiRfQ(XN7?FMS9*>_9uYvjRbqpkJ8jZF z+gMXE=P`Hnx{1(JCbB)Gn@}=@y$IZyl~RigpX3fIN!AMUa$Tqy!PE|3KyI-cG&?Ms zI%I5qBhIEi^!)8-~|b5aGX$>*z>a9j2yIk?wy5N3AhP^Lto|3Yxji#pn#~J zd^Haq37>I9{LIlGX-`U^V-O%&)?TSjcDX8NUHaYpDBoCO9&=zUm0SItmmH5qK|86; z-EF&2td#ZQib^o_A)f-%N5b!;U!K1eDaTrM{SuY!U#(-+$ISAztJNCjy^v$6l%t%= zA+pVe-26AP?;Wej%z~=}k{zcJLf(@iMn&OW z1o=gSOrX?7Dww^|!jC&ypTDp3qmx60>ZEYq2wb)rcwcFo;#z)cSJ>m^EF9p!eBpYr zd{`1!7?27EzJ24e|`o9GdXcR{6Lb_r0vX zWe@A|NY1oNOa5I}5VaEYBB_RHp{Vfnq+y==_HT;axm1sdn=^~!ZP^jLt z&NwzE9C%T+cbC5zy#zt*{gAsMMMLs?-{0)F9y-SmgR?up-W)ehe zx8*%jd_E}Da2ljQSQCCjnyyH>ma+;)>#=l;%6|UPb31(~XK#Sg0UDaJ%+&?)v{-m} zsb$GYxAVF-1~#Y7u*aWcYC6OQd9XAU+OE2>ONk6>9wQJwo{JD{$~`Sy0^dyIAGuM3 z%xq9I;72KuHViS*NnB2i9(v56Ds!h^f^?9^Ohh6gG&7>>*aG|*FQX*j)C;{DL)q+q zdUp}}?z?J$ST*sqxBa!({dmNXSdzXjhg~e1v>|Jk|G`gs+m4fgmR`pNQ;DphYZS&i zD~ixS{pxxtQ;dU&NzXph4bZzZABpK{UiAZoW$sc{OVwj=VCqHVqX=>;b{}{M=G0D2 z@Me?cOS&H<@?KhM9WYyJB`*%gO3YX?W@YOJN5wU_@_yJbWsx>ir}d*jPA#?nW?oVjy7!*$;J(;zE&-pTveg4oLHG|y|x>_P7OOOAGPFM znZO#~@Q3!wXb+1s5VGSd%5iVI&ch4WlLvERC3?AKj&X3cS8Z3;hm}*w`(r^4w7Wj1t#+|j zM*uG5Ks4%4a`0B-0w)Wg?TK|{P(kB&3?pCm054x(l*}Y6FHv;SmzZ=k$~E#v-?xD$ zs%U(0qPkh=I#LV5RJnPU3&VX_5RFC~U5&!~52x$I+(2 zwMkLGR!l+!8>&DPac$4}_Dfdnh_ICv>zC(p=B`#x%pT(z?xcAgNMDOd-oz7+6NvC# z5Ie4|_@y-#t?wzRUFU?olD4XgLdZDhI6u`RgS~gD_lI+%MyJOW%O3*3#BN;vmSoNS zFQVZn{9u8Hs5fNo47K?KAIdD}-?XI@2=npVQc!_EVl9xEjDn?t3m-tt!NK_$k4y>G z4z9}ogB4mivE}H>j|G9}!i6icV8S8L6s)X#B|N;E+^5RvXe5)fwxuc*OFrH@KC4Ji{TW$%o})f>g@}pIYq1B z16;1yS&It)+%I|VLq&-;1->5=9GPf~2pFLE?PZ3|g66c=fs1S486yx*P&L2I(`fiP z>b-I@G!A3EF)y5Gv*SPKqYim56GfpDkBBo0Ye3VJvX|yj zC2;4WQgJ$`InLzZ&$uXpSbT-AQBro*|Pd*fu@_ky=UgRE)u}GJ3{@i};eB z9}Vy9d4I~-A-VzII3yKEONRhD^Y>449;|0Ul1ILjQlSd$$w}|E!izsqVvY)iOCC=P zf@Ox&kQ^eRGygm$EpyN*bfM2HofA|n$Cc%Z06N}{9$95F?4;z+X~|$?<=kr?o$l!r z^9PrmO8vd8U8E8}%TXEU;DzKTc)c>9B#wNU1n=&H_b1^L9y-D?j2x$j=ZwL%aZluL z;YM|&qI#K_<4+VhNhTZD2=L&fQMzLRV9~tbLRcSVj7&@se&DXRz1FZmw)@Q(bRY7w z*sx~fT9{!Xhic{lL&c2clwDM}m9Go)S@6)tYtDrhc(LO+J5D}(nJkcocz|nPiMlFh z?I^TXQeCVN8Q(k3GD1FYj>`XVH_ZZd=jKR%2}yV^V;ZYAAZWN3<&=|zk||I0tP)bM z?9;SO9QAtlZeyGq{@`9o!mT?JbAfM=#0RPo){50G0Ge`Be*r5}_RQ=}dTPlR^|z?9 z5Bf-$nn%)KfdF3;HoM|IfV&?Vp5PZCuas8P*^_5Cct^-Z?I%R~w+(D^33zl2fh@xrO1&VLjJ`;LoHg6=B;u8RSaGKiGl%V3<%_5(2mA4~zOhf7#kFdn`Y z@(n*+0sFw1IJQYEyFUCn`LZnd@sI;IZ?mAepV=VT2L@O-8h7d#M#%Ni4mz9Fx{f|n zMfP*BK#e%hZtE{Mp>W(H$RVl%r5HLD7>_K5zSn77GjblBF_1O{=2y0yzS;Te$H+IW z3BxBJ3=|scy}rMDBlS;SfadLw_eY%ODQCI*(oHP?kAh|^iLVb$njAKtxW;3+aIrKx zv0bN{YF8AnT!Sp44*@E;MVQy4%7i=r7J2Vd9a&WqU09pzn)XVS50RU&EEOSYMZS&^j z=Rfa49ZAS!g)^NPJXhGN`Lue1JT~)90gp}6HE&7B-*uayxyn=9kghNlDzsN;_O49^-d(`0*+|emIAM$3ITm>udT-)WILp&Fd`;wEgeu*q z&X}@ZlB(V>yAbVIHV_+3`b`VeY3=vt*C^er)sDk=HQ6*|l30>75BMCJ0oy19A2ge3E`XUUiE zx~+d~cuxI6J6Sq1CJA)_vMt3gdwN%Ag{Qk1a76U+>bP60V7gUzDPfOkP`1G2UFgk- z{IdIZc?17eMn^Pw8@%S;k*7X9zj7$svm=17+C1|4$a#=_M-lsZ)pRh_muA)z8wnT1 zx*P|KRjrzI0yY)eU*Fb@QZ(bKY}&hf@O%97q1R`F+N1{`m<=RU~NFFdG~tHK*FEWl{8c<4+;Gd>#Q1?Vpilf)?ag zO+W+_2*Eh)z+?()`ZGJ@o)}fVF5*HO;Z~4mul`D@F{@Yx07=|b>-=_ThO_WTSOx9> zh?);x^e^NCrD|5utv6zxqfJ(tpvUZl8?salLyfE9a_7mcRH4yj#x3qztL2=7g&r`Z-2AM*Lc2TyvKDT{hjzuOur05ZpWU||~zg6?Y;)Q;>fY+eW5psp`M zISj5>Cu38(o1Z?~ACBL43fQJt;DOkeQAW}DK^BA;e+NuF!lbNjZmPy7Yj)frfKsJ@ zWj?dmCUW#KGaf;1bQjhQ-RN~4|IzDqTF6Kv=pq`!CcBoZq6y89dzu;E@jfD^0Y(i6 z-s?zyuiddKR$|y&!|#Q*^~9{t*T%A>7i!?%EOPumWv(J4X~}YwQS4FN-Q(O)!A3PI zPvvehDk3P=AMvHxuaj6lV5;(y;0-mu8?GItUY}5tIsApMpySrAMVvqnxsGC97Fm@|#-BpKV;FlwcCY&& ziF~jJa*YTXPSI$9YS+&%)oS3}cxv-%S_x%-Gk>oG9^yNPEFOm1K`}rgr!cJ1U zRJ16!&;v@w9VnnXooEAvz`*h)1EsswKnuzJy`>+XpnNiT27~7;lxh?D1f@RVLul_)eNtYn#@+a%u!xmYk*lh{sQZtKv;q6<9;dW-k|gR!kK@AqvM|%X2-)SQqU460@ z8rm^BdW!#vOaBucT?;tan~%ob!E+$9E}=M%3Q8p>9<3%gXiSmxTScCr;vsOCHPmN| zU-a;Z-00{yy6;o6__OVmf{(p*v@0etQ)lK4J@+xj)3sH46UN>98Nz$iRj9;3!je=? zdv3YJVy*haDFHJ)TEuNPVNBizLd?aqth7QoGQ%*8ygRpML(Fg>GfE)Dv(yqh3*eSh@J?zt)^tbCO9X>c)79*1YkAxts zIb7H{F=oH!yOEyZ02bAf=L;$sG%BLw(%M}HTNzg}1Zv-X$E)ACg#m!NHM0v7&;=6< zTtq`=#4&T;P}O=HMYV+tr*IBo2A{Y5+w3qQLuvG5AU!FNMqlPOA6}5vtWtK|p~=4H zLlYuq-3I!LOALx#^rq~Z72F-p$18*5!>M1dx++vNFnnL@D-O@QliZt%wls`IRqGTc?)_D8a;PhlqtCSmj3o zQ>t7=hV$JGuoq;$CVf46d*mU++zh8EJ}iR!p)Z%qe&HTZj9koB=t`c}GxwfoV}HUX z9cObXM5)hmi*(j!3)fTL`XN+GuSx`~wOW9ZEqjsM_HW6>+ilizv7*Vc1dPl;fnz{z zW_lDimWY;lJm@ffc9udwNIf*EojW*nbED#LkjmC{(`u_-un6yh_A5VgB@~+lEx3Q_ zYsVQb*7yf+?)s1RCBnNO=ZoA3=ct_=_YLhXi1yMxF!?Sg{Oy5n#tnmOA|w`+M()cldd1}s9#USK_e#`m=Uva;b@^MN z{C;~rj_!Gvd41H#2vi<(^U|R*)eLk&y`{=j>f^Wb7Y&Q~7_{Y+Jn5XQwC={@d|CQ0 z4034@9(uhow|esC$(*@Q{9@DoLIhqIFD{J(gi^X__=1v(l`T@NeJdC%?7sKNx04o; zMV7M~FXYTBV>wJa+i*H%!-&uT^2xu;HXpb)_QnCY)djY_--kWZ>%Kq?veiV{a@(Um zZLB5Aru|5-+43(pL+pwC3S&m(oyKLrl+j1~6?A8<*6H zo(rZs17RJs?a7Jq&3jaQtrlbTES|bhn1SugY13z}{IrE6pKoJPRBbGwbJ#=vE}NSm zfBJJ~WqZmM7;9h-2!t;>wX^0rL3}h2Pq-w!Z{y+@SBSa9+BEA0czSSWl>x540_8s0 zu4|EVa_M_z{2pMO!$R+w3cCJIIPx*MfB-h-yB9yMsUgT<7We>FlA)VF5mZP z!Rn~0u-ilOxh2Wu#%erqnuLiQEo|P+-~|JHtu#LHTX4eF8bMIK?babnp*vDiXH)8| zqQj2$*lkCTdS>&&$}$-Z2nech72{OladeKv@;miBn{j%vqfvvx(;W<#+n z{CJLbbt6Owbj(aIVo|p(HsozBovq9GoXr?8a^o zG2+Sh4?bSjYC7jmhVqmxi>>;sv!7HJOKW?9=l|5xoHENh!o3g1nW9T#FPKaD$D;EU z>*@8h&feAu?Zvrtwj!}rH>EmGK7Fe(SGF4(C!}wtS#|v~zU-f=@jPL;Y|Unt+qHCS{1dF6b$8cqtky!q&KuDJ^_4 z{wrUHaG=1wlKo~lfHL2FHUMy=zc1d5w1Q(sp&e3Anv!X&ndNd zj402z|69s9$qOjfUF0zS!m_1t@m3c)=yc{y?3{O`*cjOCH#Ds;3pex?S-3|JUK)If z_^Td z*Wqy(m%`q;LMVM#tg#FduP`b;Cqn((r1koIZ+}S3yzZuTc~iyN4I%Oa4bp6>FJ3R+ zGl6OYFmVK*TBuI)u|M(n`+BJcDWh>=b=FiV?G7E3v&Ui2{So|}j zpALeeioef>zB{>e|22@j@)(sePbO0o74|y1W1lR23G9EnD5A_B_j& z8H|)<21SsiwA7Vj(qwq;ZxU!!BPfSJ|G3#)WB(?3e(Ups-J53@dhz7`_*2>BcXQ=l z#tSB!)EssTD4gBRZ^4*r4fUqq!5o_pOgS<1 zs?qNX^#hu(4zj477r5Ra$%)PQB(*bvq1le4We`hLPgYKZKS59~KU2FJPu5eqrJ-!P9H_w3|Zm(MV(%977Zzktz<)DCPRYs@dHG?Sht z^)&wI1IK02(8 z7@dNjEf#UTP{x)tE}2*?^w-gPjFn&d2kzQ=d-#y4WF=#3XxMl&g*}5m?n(&ai1;Yz z@@(`ZgI7rDWcw|??^xa*4pUGZ?7R58V(#Bs~xO|DE5z6;6f8>|ImY=#BIOlg6upno0Z8J zj&#r`PzVTA`+KA4{$}YAf@hm!f}+CfuaRmIy@iyPOB6p><&VFc3HBNjEjbA(inXW) zljS5|rdBlbHywib@C5tgTOEr=oat-!mV8En$-XlEFCHUiqx(ufZ;d^JBC#8Nuk(e! zQwpA8v`*bTCuXYyD}!J89nIB7vI6)oJf6wRYlT^;pbZ=#pcd zpl?3O2;xI;HrJm_#t;4~q-V1gDW%W}Tw&F8Wx*nY8btr}qf-V!9Qy3>M&IZ{g<~`p5HuX16Q99^Nh#ZS>(DJO0LQBO!@&wFekLrE7KOCBI>o~p4| z@uvsZ>HG5>sjwmRPW(S=-I%?p2z+Hpj6ePC5ylaL8Q48_`pljYN8(jdZBSa7SkI!& z3ndIwBR7E~wpsxTC+e5Z%~awb1B9~`SVigm;{Xqq66K62_;guwb$&$jRA9KKW>~N! z+L2=msO%IP^W;UN>H$}5oJxc&>`%81yN^sSz)|p4)fvep8cS@HZOJN@v0~^O&-(8J z1ucdDiTAQgqia);?$;lYwYf#QVNb@*rlmHHuX&f+7kk()TrX~4C$Y%D>M|@t4x2*^ zuksz_m^PjrjRetxVcoT3v7BXF3BPI#Yw7?h#o`czocS9uxRXqp1z)=#@Fw*Dl^^rf z)%n#m;7CFL$A744uM+HE8`oO_yx^m+s@t3TIR{?iZkYgzP~NVq2y|5xu6CGhxeM7% z6LZD(KO-!4bJI45hHw4|pD4p}>CNm4A1jrfN0tExKQ8b)Dcd*popSJaoh_H7W`VOPpxePDe^1m)3&(cG zL2n^Iaec@m5a0Lg6tKyT9%YlHJ#cqrXN3ETC+=@;Tk!;ZlQx9Ndl)vR6OcWNF*G_% z+rwp#T7C7})(uKauV#1rGlTCK+qBsJ<3FkRMK|e!!$?bP z>7q7Ys!o#`u>3^ci6#mK4lpU6sfJnH{#k}HMDhopyuUg-&g0jR2+Gs?q%nt4`O)Ry zDQWnbf)Q+h^%-a%q=`sgC4di$hwKypbRuA_>$A2>dzJgXg@`2~MZ_B4FfuGzl5|Wk zDAZwR56+GjJAd-W=C5LXC@uCM#X1%7Lt+(yW$T?6wAm$x$T1P#l`1VOrH>J+uh-3F zXm$a>&@=&POkfkJl((RO0!nN&`?Hy_v<90%v~4eav)Vs%`g#7i1Qbon@%p%HlHAk* zEG|vTXM;bR9NM3o@^7s7tj)0c88C0FBXa&sG_liPd1p8TerE{l5O1fYjV@hxUzUsJ z_a5Zc$JnvNrTG$&(3U0OiAwHJo?i?fxUnX=)_l`eS@B8*Ym3*5-370qlk`r{ zPv#4PFErxmLI4rcZF_%)jGh32WTn4xNCLfvBMe~AXQJv@vhCCs^|Mne$QqHZ)7j|K?L-kj1UnM`bSMBtu_2Fb8zBpE>k<@I}P9DH70i8Uy8=cd{3wZ`! zm1)yY!IKR>9@&22*{8DZPD3L4&uRF9H!k@|n`767l;oexykB$GRoPFh5hS#8xe4Lz z&q90_N`0;L&#yi**HJb4L;{Zb&Xc5|z^AOJgc^7Z#yMU{T059IQe!z?Z&_dx+n-O2 zocu4gYCDt`%^fi?<7VMKZWvU3<=7QTwjs5)gDG>r4wdi-s38#OVb3TfI|ui&)GMRw zb;K=;Y>|l0A9K&1l>{AVM7Q2CGsTba$be72yLclnRLzj}UYH_E_OEFE_V!Zrqp0O^ zK(8HX7xSOi&i&2|e!fkWqwaO!H^o3`4ZT=D38N2BIvMJu&pQmGX}$g*ZQ38=rEJ@i zb(G;)?8Dh?x5aP|BA3sAvVE{B*dVL;m4g|0P`y?!c(RMFW+c~L*RTF5T#&kIl56DE zQBUJRSi4l28KP=4FojL&B#pSpF`ajk)sZ5H+ z2+Bl`GmNYPz{C$S-(0>QJt!-vu(O~yZ5`@F(}?84zIRwih2YNItxv5hX_m2OWvA0p z&2|E;f-S$mal6;KBBT18XHKq`{+d;6J8W(@dkBY@SLGMW-By77m=P`W@f}o~0VzGQ zQi-x4fJ?HmUx6n7oc?bvqkX|=$H!xTQ!=11SZumwpBr62G5-ISLN|Cy)OYlmm8R*G z0Qe4!E9ZlOF5J90rME(oKB#xMHpOGa31Ie0)Qjogl|P(=;CYMs_-M%Z3pui!-MMkK z$X+g_y*aX-q11dZf6 zD=$C5*`0)o#-76z@fX=j@_gdnaL?OkB1S~J^ zKN9NHLDyuy@+bq%z2ma)b#1ID3jC$m!;GOHFy&XRqZ-e#I}OdMcSCd)X`2_S?RfTD z7BaQiOE%&>jw{%H%)N$<9Oz? zoTA{?cucbTCf~;p@_+Y6$I26I;V}qkHsfBchIGyaf$J3&R78R@b^m#8X^8>$*;hLi z40T>;3!AP2k6NTiPdWP@DFZIePFbRl+gg{gzM0LnQAzWB+T+!iY={wn34z2o_*5_-Yt$UX9 zm%>SFZ*1Z?7B8D5)R+SR5X9$+i1}m{3N(*U2&*19Z2~5|5f|inkoKBu-1x^Wy8mH7 zXc!6pqtRyfZMSXQRdtua;x`*jS)sP6o+W?-Xvdfk#@75od;j#Jueh&k=r&{s0r9p4 z5uHfGvGbUr8Bf8T^NJ>2voG00QJ_8n^;@IuRf5gVH31>AY3iv+v{+EsuY@O;ut`T1 zj@Ab)rCqTnhSO{}!2(ZA2)BD(d}sP}7dY6-GY5U>{2OqvRLR-6paQyXMovZi3=dFXfXw2w^-d0IldD=oiGp9%Gk1bHKA{jH;H9 zs(+iN;^&P4L1%1*Fvq9Y?-txne3$~vb<{&yo_l6~`cNS5O|?_*UuTYeuOp+$?6Ov+ z9P9WbHukfeI!X*|sNf8jw=9TLuS1ECD2CD9U6vVt-u9}=eQ6n|kd50ln{ZtWI=jg^ z-tT8R)S{dyvS09qg@trVNbNOmz0pOB6vF~UOW0|7|NFifgX&(R)F7uek*>ee=Z?td z|1Ij1y|b3+sQ6og{U`-WFvasG6(5a5@l`t{Hv(`~nN@+}FG!jpa};F*73yf^uMhY) zC=;Fo8N_N!lonflBze3s@^>IOCc7)4(<>*#9XhLykF0_lZKV{>>VjFDl1~zfdx54W zn#|!A;Zo1$KZLt=!hZ($i&27G_59rda8;vFNA50CkK-R3MtXox*_|o{% z=3x*2Ql`_I$9MQB3jS@%9r3D#9{STBV_t2x{iR=fWH^m=Bc^;x8$irIK!`Z*?sne@ z;6(8-8($Z)|2d%KPaw4mvW*>GOn)Mf_ld1j zV}DhGI%oCB;7BJv6%b61Maj~+zlG%KFAZPyA4^+ztcuC4iikuH;w;);(M?Ss_q@1p zZ==?gp^7-ZQ>|VKQc9^3i+Da%RG20uY>aBK4O{KaTG|yQ)eTvj&gN8n9ttJ_+<;HK zG{(oS3{a~7rEkZSEj-I3=?*1AkNr~{A0yp()E?DsrFDwur+b=1JU*L6uy`bqVOdNpb2 z9dDbwclL@?pJp>Jn|z|(R=v2CYr*mkclVmgRM)25n_$uUtRiROe@V>GR33w)B&)3Y z_39j6T)6}~*l0{y`!KCQ^`kOdo^Y>|0UlKkS1anVYUNMa1*WLQ+teO*n6IN>=jU#` z!|8ZFR@M#Bek;Cw)505hX#E&axAoD5X+;mJs(!GETe|{=a=?E)vc1_s*ZlVwzIB<+ z;Raf9Hag8~V*2<qAYIw7!Cl=`M)zv?8F|+K9Qwe0vSb6qmrnRNj?pq3~{&3Lr zUF}xZ5mP4o-C}Rz``BdN56EO(3ykFoB!Yj}#tP00GILsUs7tw}7VPu^*Q_Vl zdu2PU8l&F1(CT^h6Rwoo(;H&(Zju3ahN$L;Xk$0X(PC9K#GrYv;4Y*IBa@^=I3IRG zC^^H z@hp(apys^?dU;dYjn}mkr_#U|j>ArC$;{&hn-miXSu0TeY5!k)?;Q@;_pXhn`$Qs= z2ojN8I0&e8DosjV3c=DzTb1sb2u`PSeTKie;d7ky$_s+FH*WMfZf>oAnAGUa@PZFB^$K856E?r2v|Lafc zrH^r~#E)7XoyF1%lE=rotDWLR0LKh;xhy3&p{aJ2X)IfHd!@6%GQ-Lnzp`SNV&#M`TBnsupQ4#lzkvr{oWUtt$#rygwB7j2;e zfHeh^Y@)y~J6DV2edF{d%DSTU_Bt584=AE6*GGM7RbP(L4=wIGJa`P35SE$zvQoc< zs`D;TJfc&ByL9d*8ppF zwDIe&dyDm_UFcd=yzr64&DxEjSj>wwjn1ng6qMdb+b(TnsMCFeV)3Lj&icF{CIw~H zds83v0|CDxpp4JDR{6jCqx}Mb<%$8TWSULAAD8(2A8dlyixd5_8jMso-`Ibgh=1`K zU#ZCe1e4%jrc>w~iDBwPR0b5Oo>lEvz=78!Wc08}G<$k&(2Un5Q2#@1aF9xX4NDgw zf*Fp|o=f=jDKielI`8k$?R%#4x|OV#go!Er>!_#vMX1JG4^0D% zwh(=JTK2i)dMjOvR}8%@F~yVD4zX-`iU6o07Yu;edhG60Q|0>YPjK(#E(X`f3ww9n z)EN7_Vu`y70+eCAs0Zg_&iFJ9HqO8UJ4Lw^^v}8lZN?UIdoJN%zDM7Z1)T1iy&?Si zn9S+N;|G1i>sa`L4-;CnmN3ZXK;4Qy90L5yQCh}iTl`<~ZTP^rzTB27A^6qs; z$*Pbwxx-ynzNfT^0#@3MNG)OfoLlE^;o;GC^v=506C+A?q)#4ue$lSjU)~tJ=|+21!ZOwg_VJ2KxGBp-@w zrA9VYR@YQao@HR&RkTEFJX8uwlOR3W)}cQv_~rB&L{|=cB>c|fH6+bpSMq97B%1q^ zS`rVHrO3zGj<-;%X8xvO2;ux=L&dvAJz1%~milz?Wahuvv*QyV`M4hu{7|+4_a$16F7g%i|`0t|#d(s>GS2@gQ@I|l5m4k?SS$HVS z5R)e3u4KF)1F(uYQZNHZ;J1NZq%8-t3b1*#DC&|ts+?Vbo`gM@+}`R=?9|c^@jZtR zO>P~#&zW3r@Cg#XdW$V=y_NT=Kz~fT&tl!D)i2Ai*4zeJyBYf%VI-MHe$wDwyc}Vc z_APuMMV9I+V5=v11{qCCeIQ^@{w)u^KEcKMWQdKzR^1yB$}dM2&J);)Vyc0|2cY=S^5$BcyAeWz{(&9&?^1OjB1wo_068g>c39}6x~Wb`hgGlAt-WEG z1Nfm-5wv`pVv(&)&I5)z9z{I4;CGw<0m#!IqY|cjtpF_2u3b4EN@1JWtUKAUtmQJ* z6DWy(J3cfd8Ma=*5uWl#=xx{K25#m|sWpR<+klek&kTe)IZDWh*2aVCuDZxuh(*xi%MuRtxPMGXW z{@K^&n$ln>T_|;J;^a6?Lvjfh)PKZh|FtwJujtaPd}fMlq01P_41CDML}6GfqP+}5 z5V-v4!=?yTHTP>t!6X}_3##1~J=AX`_J=9jpZ?G{SAJ(-EBYpDb@~S0zu%5WZy*!( z2{_rDHa!QGdS1T1{UFU~8(Hz9(=iR^(^-&c6)rtBl&ZNJM}0KcPY|I7@WFhzz!UvB zOyf7YsmVsUP<8~dN70LN4GnQzF4}o?7Ogww*Uv$+W&7l>Hr{q!C2iUcU<^Fnb-GoX z?zI}LKN#=tohH+Q(Hv@CAln41 zov)<=n!R?s_Y$Sqof`@BNE`dGI!KpEQ;v*#ksK@wa6=dRGqZ6VS^$WGw!iNDOY{Em z>KZNZQ5Wni`da9do;ZhXb{cgh`{{`rjLR`>0ON$)^ij|$buCYqxD(d!(L zz){B6;gU!yK+mz?eF!*=w%=uWy-(D&RQI=WdL{q_;k@zUgrfW#T&Gcc{+~-NBlr*K zj9E_J@YH{`fJ(I+i6>J5oHo6`ym-%&pU-$L%u%u>O_aV)gqXc0SeuAx?25V;0H$ni z#2t7u1H1Z0=lI5t1cLCk5<4d80*&4yGe#f~{~5)pf@MY2!6M^hC5v5+OLw9)Im$T1 z?SAL>{_xj6E2o!L5vs8>NMZnvqDF+D7w1t&X|E-MPa1$z14l=MovwG4a>yiGFl5#+ z4BLu!vJ{7kK$P@hM>`2XM8(zb__FmF!5kmea)G)*mO2gquBzdfaQZykeM-jC7#hG` zqoCzcAeCf3bN8n&Qmaz->O@WCE6>*y6lX2rrRHGn&U`1PpQmiF0jF{vy;y(~V|=|4 zq`i%w$X)uY=r+i(7wqwlk}V|SlQm8rBuBqlyfz8=cuJ^mfA?f~1u zp8t)!3U}^=+ML&X_?u$W`%@_IibVU^`vbyhD9Y2El40{+cBAyW1D-E*5PWPhcR#K+ zB;xV_L`_aE974-3%i53S z`H>s)6jMsYr5rsg!ta%QjF!0F;}cokW&ny!1AwFNA{oAWF~VRM|Gc^3Bu_k&HP)E% zaDYjEv_>to^nf-c4Wj_%Vsa>VJ)Pk*@!;CDX+zkn$4OS{?#sBq2yme%ADR^|l9h?H3FK`o# z9jthZ8CmUuq|zs+>|B}n0H(3b_`IvT8yZtP7GDmNv`waZHTRo3gFrog}_iqMYUQqa;AzYGefPDj5j(;v#7P3Q_U z*M3tLTX4HUX*Ejlkc40&7(Mi-LGv2F1!}YD zIH;bS+TJ&)@54dy?c-xXVwdMYz-%)(@N?^>W**AbHYyCm|6+8^Tdp+U9CvUke<`4` z^DDE|pE>6f+J-JCdiYzi(Bk22iGKWEL%R2ouk~^Uu%yO4GuS6rp@r&fEBd8-9RJ}? z0=V}q_oB>weh%^OkVa18l{XR8Wxu%PpR$Mj+C3a)FaUTN%V;Pbyi9(b=725D^21N_ zE7E+>j%E97sAs`z%R?P961mxr467vOKrgCxcay4kzAq>Dp0twlv;5{>oou2Z@HN-V z+*?ph1jNw^opxCUc}06J?IEYN~myW-(<8vR8oQJs96qCb*) zd3AX(ssL?Q_oO)I9k9~_`fEH7#_;)RA;ss>B5W0@@HsGxFE*olUUZ?gvV9~UhvF7o z)I^t)t&BG4YrcLR6}L~^Q0tBjo&#cStqiGIx`~VBoZ;z8L(H+FvJ3Ez~hsGLidl2nSt-6QA?*RhV#weq;w~prvLCtEf z^~1{s#RlOs#hbw;Zg=fCWKC+V9oz~h;RP#GZ;ry6jknOx45k1SsA!7Tz-bZuIY#() zEpdRmc*D0dcYaDVq2bTOYh&!s6vj8*?YCQo^v3uPQ7zZ#P|a&4pw%@Wl_}D~Gt_eU zcQyjbtvxN|ugt#SS6M>9MQrSCe^Ro*eUy>SeIx~BV^7T4OOUVniew!M(MSq_&=N9; z0Rqfl%oj0XlHkxeHYvUWhRMMsLm);fg3LS?+|)n7+09Y9MfFYgf+ZSA1-x;w-j2+t z`?FV-u7#S`<=T-^rN=5WHfl1jRRQKtFWdpTKsu>NZn;rmdQ11qi|lFuLa9W2US0{2 ztJATIv~kZWe122_#N2nKvI2CIWLpCAXzKOW+f%&iNkig#P8I90%}L@63xgCPZk@)J zQRx((J&(U>%fY3#vPJ(nwcpkD73o(MPkIKLmi&>m}#-4eD%H z0W*%108FM_YSw>c`aCsc-(9oja2v0HVN~0yCX88ls(w z*4Bt#R%BKGmnVLZ(;98fFS?D~mOga_Jz#s4#Sd^sWmwo3$Cr0!$NxY9@eFp4)JjVC z9lcBd<>&RBR#06)crsc~XYch|#9Tr1`yvn#Zr`LM!q*V+_y0P4u6>)rxkByDZJHMc(;7J3(u-BiZY1AUTV` zAH4xo5fsX(6I|v=i~dv%pwG_5-5tw15Np~os3-uaH0actbqE4jy7TGggvI?LAuV6$ zb0C2LVk(Obpd}dJRREC97fHyZ=Fv@iLq4!+Z(QA02pvq&?E z9sqxa{f*MR^i_g|oNXgok(NiCRU3h%lA0?&*JcTS|9BDK%Hd6)^T`o#^vtMu+;?D~ z8EGjdYXKtB07g^#APF}cR&erPwf%4)$|J5kRS;;ko}$ms@4+JmOAvs?GJgpo5K>Ae zQ*97q6VLINZtc4I#X_o=JW2iMsV=3C9AS7<)t?Gl!~?9uYIp>8S+9*P$b-Abg1>mA zr7JW9$%tIm#UTi}=3dQ6Q46oXi2(ApU+Y9r`P@zm56dpZ_a?|B%37 z$3pej$Z$sT{nO#EF(!J;%gznwh`ZNn(>xKUkJ~}gV=0;A#q+}wv+wC@2|*&I`L5+z zi@q;+Y2762@6cUF$Tm9UbOjxe4@h6+VMk)+|2!<>4=sl{JhCM9Bc7V5N3aMa6CPyj#O;RoM`{}qZah>vw570$Jtln zEh)_5$!Le&{*0K>9W{^AXWxxETsTKRd$QtTM&!62h{$jaMz6^7d-m|8R1bWX0@7 zbQQyQ<7hp?Cxhx6=k;T=wxzE;>i4pECwKRrVp_?O@ZZVg18LvJHe78_d@4m>g@L)A1R8 zf%v(nb!A`OKR4y1fo}IbJFHhUYWWI%SWQ$paZFcI(i*HcHLNl8d+Nrj*h)5s1R9rE zE)ie@Q8oN>!(@45xZWx+;-KG?+~?>tsT7M>p9gh|c#%Umlr+sV<{4P{9mZwzlC+8H z`1K6hq_$y;5sp_sePSg))qnK7>8YX7H;bO2D?T~osWJXeM&BIqRVTviV2ijL4_)ld z9Wi1A>8>d{;n}l_S23qe$uUk@MsDR&`oH!Fp9knfhh}mlPm#u5*yFXMc(oUGXC@t@ ze$LmduuRtVzFF|2{0PUY&+|glvjoi!f}b0LV2y^g%@U?FQ!{4HD7%PEv>I#@><4xz zF$=m5X9e&2x~%I@X73@^W37wTG_*6iU76Y~0$Yzy-(BH&K*FAuwMjpHtNZnd2lhDg zh!!#ez2+V;TP)Z%6v|QCK!TIfmP+ZpWqR@r<^eHoca1u#ryses!ILfmt4JqYrm0ex zOza?M!`>yVx{hjSOa%<0#U7Ss-yTp~Zp$_wqMP;n(ZK^v8~lZ;z3TxfDS5*F54CFQ zj{abQUd#{B9b)`|1x&>g0i5g#vkr>)9f9SFIY5m z;GBi*n7YoQB0-m_N`(E&@wA&>3-Fj3zuXuN@!=&^Q9dS08{>D!J@ z)t`3$oxo>JTUe)B%IQ3-bNovYtLL+7!g}CaZ?`%3j1}))jN};q*3#NsiB9RL8!|qc zCoT8g@@qG6ID(wj%?=Ff3o>Kxm0Pzu6%_v3^ue#r2=t%M|oU=4ahX}8>>w==MrQM=JTVe2lo zy|!8i2KhjYO*@s7HjP^xCr{MLOdh-87o|5nl|^(ZA4E1%oH(1z`SfhLsNIG;?kx7P zBaLgGP`;l3J@UTKlk*!6?u^-278uQLM&l;zonf@i_gRitKCR7Phf3^v(uAM{r%^%b zo-z~1Kh|4_vTK8L0&dnoUyPlN(b)H)ujkiDISs4AS=u`a+vMxD7jF$E6h8fw(D3b% zQ^NHJhJ+~py%HYu6Welz@yo%p?OpB~pNt;a`)O=XoyRWPW5i51PcmPLCpZxJ3e1OT zC1S5w2O6f-)hM)WrJXO6D3w_e8 zy;Ki5<3z&ke9q3N`)Z%gyg7bf@{53@ZfY@ZAz+t9&l|nbg1s0|4t24*cDW~wP;;Ji(hv&n%-akT&dP9u=4@!xmB;Q?pP-r9-Q<*gr1^aJ9dV*OH-ZXbL$|gGQJ&+7Z&`bF zNn~$zyvRJ#13X1hD)OV=j&ANn?ZI@t-CUM-(_s!uCF|YOWZ4_#>K4_V16kK*)NY*W zPFzw$PQ5*bKgGSquIO^iXSTO?Q5nj3+ZKf?n>sHz2kI7-wP^2=)I&w@m)_VclSX1L zhX=WlenQ>oZl@bZ(T1P`@7oi?zg(3Rpy6U^Fy`be;12X$;Ea>og+6U9RtgZx!jZf9 zd7$cr5G!&uGr=7*aZ8>wO4`~%{TXX7^@p1LV)|MwGvE~d6{nnch9)<^3Yj}n3fsj$ zT`i~ia5S&Pf|8(C_GTa7^;;@AN-z#BiH+cwJFnh2^c$xoz;$v`3hG^F1x(I%OTSBU zOl%(^5uEWGhddtT=rcXy8ngpTT2rmE`uz%FgU{$@uq!Zw)XFDLrze{MR-2n`y!PT} z-o7&uEXuwXKPgz0@66VW&&GaR+U7(sc67Hl;XB3_(+ym(HM+8DNepy4clJDLb_@1K z;pVhPT))q(gJ#4X4;z>u+=mfz$Kj9XKtIG!Wg~8B%-wTWK+h0O595lkjR#F-2ImVi z$0l>Na}{m!tZ>BK)?jm)Em$u@z{nHBC{S4Y%$U?APav9m+bo=cn%{{E)e!Ow5dzL! z`If>5E^jw&;CpAjpR;JruT@!B#8G}^M?GH{QP*q3_3`11zj$EK$DrWr3~W!Q@!z5< zhA#yM>81w<_S)Y%en9*4t>>gu3eR{NTKz6M>yCn1;J7-bf8KwY9#p1VVl?KsZzr{K zyD?2JDu9NfG@{N;wl^@}pYsS67KsXf%#S|5uRM%14uEjHo;!Z7F|GM|HM`&Z1RAx! zG$=il!7NuV87*hr*H>G4v{)VIcQ#wOPkJmf;k0(wmS5w5iq8m}iMr_GeJYFQQ%-UX z$YW;pWzoN z5D6FKl^-y@LG#|-DmzQ0VN%t)VGoU3YhE^YtO@N=8`$%a%Ut#NG>_TvszQLQWWnQ@ z^a762-g$0P;RH~ckm=~DcyQPT6qopZ|Iu&5Rq;Jk`uH^4^=>0iQjOb?G@=5{p<+!D zJYRjQ7+e}ZU%U2;fjifH_b~n^`v5n)@@4zkr+Olhs`8(0laMtqhqoRKnBaODBjHKpN*~twzn8J0w;GGGmQa9Q|#a}M9$`u0O8*4$Z_URGM^6u@0>krbrh$3aZWxg zUV#0%5Nld+Fm%H?W|kUx#2$$#8Q5*;OpnFE<+b=FUWH_*1(s-&`xe*EeIV~TE-n>k z)a|D@Eh32zAgu>TwW<}D2X)szU|!y^CtO=u%XD;SX%O=koxeAo(VgA8Pf}m+vv$vN zT%_SF7N6l7Rx;w^SS~%K(alzQ{k~_0HpuCy*wBGKut%z;VIxi+gb^e?wTBL)9-q}) zjGb*#I-XlpzNTS5B^rm(e=nmnkT5zt>(Y3C@@moy?jVYDzGSSS)RMcr7r@ICp=wfX zy@uUXF=+HzL*nhRKN)HJkczZMCmX;%x8t&ufVzfhd1SuNCIf z3&=6WZ;T%9Zz=73T@lSU6m_%1sl?uHUi@k1u1f8S&feKEiX=fCwHM1y=O*|vY(!sK zayl*Fl$Z}P-KYC$-ljrRzM3(K>ur0{JhS|Jttv+)!C1jjq~2kMrBbCLCr-XN<{i$e z@}{~AyMKH^S=XaGs}D3s@tX})scRyx2xc|PZ@kX~)z>aNIhvBUDHtidk;^)L-J6-I zm{@YFTwV_z+UD=%K3%%l^N0aU)hs8dNj<+w$l;LE4BpV;GyMQ?X3EY+qvf)+@-$$ z;+rG?k2}mQ5?F~&oPJcwH_JFhv#M0}9tq4p0QOax@Q4K!|87Z;^n`&6gi9FGr8~^8 ziP0uHKT6(@yj6F)5r_As3%kDQyVu)9$L_s}2iJX(Xmy$c+vAIJOedNR4eS>1U&eh! zVGCAc-$U!fYMjB2aL{3Q7Q04?_(783hOgDcugNCt#PS9>t|pzDNleo98>CrYeRoW7 zPSW&CY)#2&bO}IV2G~QnY~{=Xe+oDa1u#_j+!hjb0MO#E96=ktYn=|J;goICe4!@D3UC zOy`b`^Gw$$zfB5D51m+JkPL4)PkQ36s_yBo>o{H0?k;wcg6%5OMoBf54pmHf-EJtf zpUa#nYuuy@MIfHfeM0LPRJk6)R#!#aU`Y3ui2)c=CJEA&w#nm#dUq zoi#?^Hn~(6bgFZQXJt_df@mvb2eNW^x1Os&#jY?CiIUi!uZZz?`;=#BUY$lX5_z39?6Lg1UJKXu|&2av>*>+kV9k4qTCZ^w|X+)EJ^KixZ zdUL`$1}XLhb}z%@9`$61?nljirWt%2$pO2&0%JdRW$8qUd{|AQotOB{_}4`>x$GfR zS?KBIb5&I9SFQ2OncCW8D8yr3+;=gnwdtBH6C)UH@T6hQY&F?%8o_d)BB2#FPZ$0) z<>uIp6@M`got9Ton+EuSvEka}tL^QN6;aDMdfM~hE7Qe1jaM9--P{{Dc}dEme@d0L zfh{RFd+>N^X%2T#_mqx%{;hWm)axu2)|-ep>sZd;5UGr{y$s27jt zNk8!+Uhi(}#z~2aky(^vB)??EbsXu+yc(3!o;ptIH?19S&SQwzO8A)uWX+W!Na6&sSwA)SXyZb55O9Ou0@ zP4CzdPBrmbYTw1mdqVaa!Mia{-Y-Hx%# zFkZTJmPfp>_{O^x+?wbJKc^00k0b4$z+2j-Osf5Bb^$QbM2y~}sPK8hV}qbnSLvT6D}qA;b~sA~+>L(v$q zOXWSud_A7zAJeEN*P@QQ<82z(NKVuGF~7z6Ln7jRS@=JHCRVIo1@>@eHqQa*!?q2Mjs0BYF%z*T{Cw41xZ6`VVT+jWaHKw-9 z_s;C`$$2hi8sJ}|c?M-f!t(Ch6Fn~;U69bV%z!u?r!f%R$s2#|HB}o(xHuf;d~?Lo zJ-&})tUcJCU~hF_wcM2>7x5%81n(5k6b;acO!r-mLLD3gllQI1Jki8`G-VL`T zKjR!ngxa0OgH7g64|bKW2+4uehrsuWUyBc2hdiZd7*?bF&H7MTyR{B6S5Ws|gY3#+ z%iFTu$+Xm|WthU2Lg2A?jIcHoiM6!-^=R_BI`_akvZv<1p+EUVIC8;5C?n77MfC~k z(y~~awgN(Mq}RJ&Gm-X3#v0P&bGv&D)(NxwdsuhLY8esIFhZSqh9@o^tt-)DL8uw8 zSMkC%(~CDA&M@hI;sn(V4RYL`Eh#wxEVP7`6WDS~YmYi^cJskpBZVCZKbI4wbDaAO zW`8a@8fZ#$JPwRe<^{*CrPa424$zS3Tz&_kOxh3$a=s&ZNYDfNslHq;J<24hACqU& zo3XBK1u`(t!|uu>^RB!(U3v8JFu#b-m8EJ|E$VCh96I0!p5mYsRT0XnIC(uzJ&n;4 zN=Caf?XNzn(0e19I%|ACp8NTGY9tM75_Ywc(6+x?k4{s)<>WC{MWJU}a${1^#=1*7 z#+2olz$(>K)z0ClUov1|@~?i3zwA82rGOM`u{7*k`ZCx&>E2p2v+NTv+TwMNnkieK z+-xj%Y9RDtHyd^uMg731Z}$;8fEMI>NOKB8;2XWt5esJ+h7B*bC7%)dlzMJ;PBEYVdxa#^np?U1{j_PCmjPus_3JVqEjM@*3bUP8XnjRwAt2M41!S#={ zMM?`+h~tQ|XIdQs4$TY~j6))xL_U6b-G_b*CZsL=oK@~?`}OL{C>`=&E^hlllPRJJ zYUyLFFWn?8XtK?H6gFddPPL-TZnE_({uaxkaW>3rHfs(pgHn%8_6fd-tZccObnEt$ z^W5O=HwB!7`1G17;Jq2)DWfLhku8oWh*&{sDs?J2e5`7|{Vn{f5el0V@BO2Ot0QUh zN3uSA=<3~+Q4tgyXQHSZ_LtZD$>stFTgO?I!n$ZRK0(O~KW z&bpI1U}1e*a~i8Fc__JjLW+EKDJPDt&ih5jq(&l3M<%tZ(2%`l^DDix*Y|9u*F;y} zM$U8-}2#fL<17$TEI-1kP0YWZ~eWfxa^syAV0 z13kCqd~IfBd%o{hUlD-$qH^F#nv|`(fGdPEr%yWP>VbE%w*&)jeiwVtyfeF*u;QaW z@Wvivf?h&Y9ZvN6_>IuY58VF!UY@+m(ylpn;%(NDZ(-3KscR@#+H51WcyRLCealty z^#azy+woU(jRLCW8zR1UUQ$ZfH_lHF_AC~fr(Pn+%7gq0nw5sQ;l1J%*ZdJO)j**T?R#H~x`*{?m{TYi)=p z$y8}Z%8u1P^XyvtWhFZH;D%8gwQ)JkChXS-ES`VCshVPa1beJd^1W|~fYnbRDuyA~ zPTNHweh_2qxVHu5XkM%A+1FC2P3Y@=--=mkiXZveJXh~7xus{AxOh&7wh+d?x{Z!W z?n1Y}+{n1%`0MepkEY?JJ)fN9l5FFQ=-qAJm8W3em<*C7?kg{6gwbVr5h9w};8y58 zccMDg{m@w~R^q)jeEjR}V>gd7gsR*Aq$qMx7?K(~PC}F3Cvv_%3et|7Dp&H(CzRW} zzfh~L_03qCl(54slH_bC!QlkUv4xjM!j%4o)wJY?cklsYA%y09U79pE(ST{LO>2p- zr?aqQ8tT0={3DD8on5MI-=So4`1+>I%GX?N}PKD-6vFR=u9~qTMks`6GE|Rg7)t@ z6{FV9Y<%DB-)JG^&I^`s9OO>oZs&bg=;|KCnxQaL<(FIPp+g|Ajng89I+KDyKbYF3 zlzcq#DZ-n?40z?luu`+1-{mBdb)N#c==b_+Swn}}^t-%vN#S!mne!Qp~N|dwz9O_ZAVcrIQ61;%aH;cyR@KXGhFW&qaUno10r0`20Ip z;9t37V#J)Xwhj1OIL0aGycyO@%@*& zwP4Pc^qg1WM9FK=f~9Wa)tiaIO|xLR5f1AR@*^&g2|fS;e2pWce(PaZI21^9*N|NNKU=|A(h0I^@Bf9G9Xa|1*D zBK<`FJJh z#RO$#h4^H-goTA=dAMZ+xrKmf#n1OjKtPa>hhISYm4wmXxo74I`6DObd4vV|i5VGR ItH~1oAILv!YybcN literal 0 HcmV?d00001 diff --git a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml index d7c44476..15140c8b 100644 --- a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml +++ b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml @@ -12,9 +12,14 @@ maneuvering_task_node: gps_end_x: 0.0 gps_end_y: 0.0 gps_frame_coords_set: false - - map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" landmark_topic: "landmarks_out" + assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct + + # Task specific parameters + distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame. + # If the distance is greater than five we drive distance-3m towards end before trying to find the first buoy pair + initial_offset: 1.0 # The vertical offset from the second and third buoy pair in meters + nr_of_pair_in_formation: 11 # Number of buoy pairs in formation, excluding the start and end gate at gps points \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index 74efef6f..6055a9a8 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -5,32 +5,141 @@ namespace maneuvering_task { ManeuveringTaskNode::ManeuveringTaskNode(const rclcpp::NodeOptions &options) : NjordTaskBaseNode("maneuvering_task_node", options) { + declare_parameter("distance_to_first_buoy_pair", 2.0); + declare_parameter("initial_offset", 1.0); + declare_parameter("nr_of_pair_in_formation", 11); + std::thread(&ManeuveringTaskNode::main_task, this).detach(); } void ManeuveringTaskNode::main_task() { - // Sleep for 5 seconds to allow system to initialize and tracks to be aquired - RCLCPP_INFO( - this->get_logger(), - "Waiting 3 seconds for system to initialize before starting main task"); - rclcpp::sleep_for(std::chrono::seconds(3)); - - while (true) { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - std::unique_lock setup_lock(navigation_mutex_); - if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); - setup_lock.unlock(); - continue; - } - if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - set_gps_odom_points(); - get_map_odom_tf(); - setup_lock.unlock(); - break; - } - setup_lock.unlock(); + navigation_ready(); + RCLCPP_INFO(this->get_logger(), "Maneuvering task started"); + // First buoy pair + Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); + // First first buoy pair is far away, should be closer before trying to measure it. + double gps_start_x = this->get_parameter("gps_start_x").as_double(); + double gps_start_y = this->get_parameter("gps_start_y").as_double(); + double gps_end_x = this->get_parameter("gps_end_x").as_double(); + double gps_end_y = this->get_parameter("gps_end_y").as_double(); + Eigen::Vector2d direction_vector_to_end; + direction_vector_to_end << gps_end_x - gps_start_x, gps_end_y - gps_start_y; + direction_vector_to_end.normalize(); + + double distance = this->get_parameter("distance_to_first_buoy_pair").as_double(); + if (distance > 5.0) { + auto odom = get_odom(); + geometry_msgs::msg::Point waypoint_toward_start; + waypoint_toward_start.x = odom->pose.pose.position.x + direction_vector_to_end(0) * distance -3; + waypoint_toward_start.y = odom->pose.pose.position.y + direction_vector_to_end(1) * distance -3; + waypoint_toward_start.z = 0.0; + send_waypoint(waypoint_toward_start); + reach_waypoint(2.0); + } + + std::vector measured_first_buoy_pair = get_buoy_landmarks(predicted_first_buoy_pair); + geometry_msgs::msg::Point waypoint_first_pair; + waypoint_first_pair.x = (measured_first_buoy_pair[0].pose_odom_frame.position.x + measured_first_buoy_pair[1].pose_odom_frame.position.x) / 2; + waypoint_first_pair.y = (measured_first_buoy_pair[0].pose_odom_frame.position.y + measured_first_buoy_pair[1].pose_odom_frame.position.y) / 2; + waypoint_first_pair.z = 0.0; + send_waypoint(waypoint_first_pair); + reach_waypoint(0.5); + + + // Second buoy pair is FAR away, should be closer before trying to measure it. + geometry_msgs::msg::Point waypoint_approach_formation; + waypoint_approach_formation.x = waypoint_first_pair.x + direction_vector_to_end(0) * 15; + waypoint_approach_formation.y = waypoint_first_pair.y + direction_vector_to_end(1) * 15; + waypoint_approach_formation.z = 0.0; + send_waypoint(waypoint_approach_formation); + reach_waypoint(1.0); + + // Second buoy pair + Eigen::Array predicted_second_buoy_pair = predict_second_buoy_pair(measured_first_buoy_pair[0].pose_odom_frame.position, measured_first_buoy_pair[1].pose_odom_frame.position); + std::vector measured_buoy_0_to_3 = get_buoy_landmarks(predicted_second_buoy_pair); + geometry_msgs::msg::Point waypoint_second_pair; + waypoint_second_pair.x = (measured_buoy_0_to_3[2].pose_odom_frame.position.x + measured_buoy_0_to_3[3].pose_odom_frame.position.x) / 2; + waypoint_second_pair.y = (measured_buoy_0_to_3[2].pose_odom_frame.position.y + measured_buoy_0_to_3[3].pose_odom_frame.position.y) / 2; + waypoint_second_pair.z = 0.0; + send_waypoint(waypoint_second_pair); + reach_waypoint(0.5); + + // Setup variable to navigate formation + Eigen::Vector2d direction_vector_forwards; + direction_vector_forwards << (measured_buoy_0_to_3[2].pose_odom_frame.position.x + measured_buoy_0_to_3[3].pose_odom_frame.position.x) / 2 + - (measured_buoy_0_to_3[0].pose_odom_frame.position.x + measured_buoy_0_to_3[1].pose_odom_frame.position.x) / 2, + (measured_buoy_0_to_3[2].pose_odom_frame.position.y + measured_buoy_0_to_3[3].pose_odom_frame.position.y) / 2 + - (measured_buoy_0_to_3[0].pose_odom_frame.position.y + measured_buoy_0_to_3[1].pose_odom_frame.position.y) / 2; + direction_vector_forwards.normalize(); + + Eigen::Vector2d direction_vector_downwards; + direction_vector_downwards << measured_buoy_0_to_3[3].pose_odom_frame.position.x - measured_buoy_0_to_3[2].pose_odom_frame.position.x, + measured_buoy_0_to_3[3].pose_odom_frame.position.y - measured_buoy_0_to_3[2].pose_odom_frame.position.y; + direction_vector_downwards.normalize(); + + Eigen::Vector2d vector_to_next_pair; + vector_to_next_pair << direction_vector_forwards(0) * 5 + direction_vector_downwards(0) * this->get_parameter("initial_offset").as_double(), + direction_vector_forwards(1) * 5 + direction_vector_downwards(1) * this->get_parameter("initial_offset").as_double(); + + geometry_msgs::msg::Point red_buoy = measured_buoy_0_to_3[2].pose_odom_frame.position; + geometry_msgs::msg::Point green_buoy = measured_buoy_0_to_3[3].pose_odom_frame.position; + + // ASV is at first buoy pair in formation. + // Run loop for the rest of the formation excluding the first pair. + int num_iterations = this->get_parameter("nr_of_pair_in_formation").as_int() - 1; + + for (int _ = 0; _ < num_iterations; ++_) { + Eigen::Array predicted_next_pair = + predict_next_pair_in_formation(red_buoy, + green_buoy, + vector_to_next_pair); + std::vector measured_next_pair = get_buoy_landmarks(predicted_next_pair); + geometry_msgs::msg::Point waypoint_next_pair; + waypoint_next_pair.x = (measured_next_pair[2].pose_odom_frame.position.x + measured_next_pair[3].pose_odom_frame.position.x) / 2; + waypoint_next_pair.y = (measured_next_pair[2].pose_odom_frame.position.y + measured_next_pair[3].pose_odom_frame.position.y) / 2; + waypoint_next_pair.z = 0.0; + send_waypoint(waypoint_next_pair); + reach_waypoint(1.0); + red_buoy = measured_next_pair[2].pose_odom_frame.position; + green_buoy = measured_next_pair[3].pose_odom_frame.position; + vector_to_next_pair << (measured_next_pair[2].pose_odom_frame.position.x + measured_next_pair[3].pose_odom_frame.position.x) / 2 + - (measured_next_pair[1].pose_odom_frame.position.x + measured_next_pair[0].pose_odom_frame.position.x) / 2, + (measured_next_pair[2].pose_odom_frame.position.y + measured_next_pair[3].pose_odom_frame.position.y) / 2 + - (measured_next_pair[1].pose_odom_frame.position.y + measured_next_pair[0].pose_odom_frame.position.y) / 2; + } + + // ASV is now at the last pair of the buoy formation + // Should move close to end before we try to measure the last pair + auto odom = get_odom(); + double distance_to_end = std::sqrt(std::pow(odom->pose.pose.position.x - gps_end_x, 2) + std::pow(odom->pose.pose.position.y - gps_end_y, 2)); + if (distance_to_end > 6.0) { + auto odom = get_odom(); + geometry_msgs::msg::Point waypoint_toward_end; + waypoint_toward_end.x = odom->pose.pose.position.x + direction_vector_to_end(0) * (distance_to_end - 3.0); + waypoint_toward_end.y = odom->pose.pose.position.y + direction_vector_to_end(1) * (distance_to_end - 3.0); + waypoint_toward_end.z = 0.0; + send_waypoint(waypoint_toward_end); + reach_waypoint(2.0); } + Eigen::Array predicted_last_buoy_pair; + predicted_last_buoy_pair(0, 0) = gps_end_x - direction_vector_downwards(0) * 2.5; + predicted_last_buoy_pair(1, 0) = gps_end_y - direction_vector_downwards(1) * 2.5; + predicted_last_buoy_pair(0, 1) = gps_end_x + direction_vector_downwards(0) * 2.5; + predicted_last_buoy_pair(1, 1) = gps_end_y + direction_vector_downwards(1) * 2.5; + std::vector measured_last_buoy_pair = get_buoy_landmarks(predicted_last_buoy_pair); + geometry_msgs::msg::Point waypoint_last_pair; + waypoint_last_pair.x = (measured_last_buoy_pair[0].pose_odom_frame.position.x + measured_last_buoy_pair[1].pose_odom_frame.position.x) / 2; + waypoint_last_pair.y = (measured_last_buoy_pair[0].pose_odom_frame.position.y + measured_last_buoy_pair[1].pose_odom_frame.position.y) / 2; + waypoint_last_pair.z = 0.0; + send_waypoint(waypoint_last_pair); + // Also send waypoint 2m through the last pair + geometry_msgs::msg::Point waypoint_through_last_pair; + waypoint_through_last_pair.x = waypoint_last_pair.x + direction_vector_to_end(0) * 2; + waypoint_through_last_pair.y = waypoint_last_pair.y + direction_vector_to_end(1) * 2; + waypoint_through_last_pair.z = 0.0; + send_waypoint(waypoint_through_last_pair); + //Task is now finished + } Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { @@ -53,13 +162,19 @@ Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { geometry_msgs::msg::PoseStamped buoy_0_odom_frame; geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + bool transform_success = false; + while (!transform_success) { + try { + auto transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, + transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, + transform); + transform_success = true; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + } } Eigen::Array predicted_positions; @@ -71,4 +186,45 @@ Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { return predicted_positions; } +Eigen::Array +ManeuveringTaskNode::predict_second_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1) { + Eigen::Vector2d direction_vector; + direction_vector << previous_waypoint_odom_frame_.x - + this->get_parameter("gps_start_x").as_double(), + previous_waypoint_odom_frame_.y - + this->get_parameter("gps_start_y").as_double(); + direction_vector.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x; + predicted_positions(1, 0) = buoy_0.y; + predicted_positions(0, 1) = buoy_1.x; + predicted_positions(1, 1) = buoy_1.y; + predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 20; + predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 20; + predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 20; + predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 20; + + return predicted_positions; +} + +Eigen::Array ManeuveringTaskNode::predict_next_pair_in_formation( + const geometry_msgs::msg::Point &buoy_red, + const geometry_msgs::msg::Point &buoy_green, + Eigen::Vector2d vector_to_next_pair) { + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_red.x; + predicted_positions(1, 0) = buoy_red.y; + predicted_positions(0, 1) = buoy_green.x; + predicted_positions(1, 1) = buoy_green.y; + predicted_positions(0, 2) = buoy_red.x + vector_to_next_pair(0); + predicted_positions(1, 2) = buoy_red.y + vector_to_next_pair(1); + predicted_positions(0, 3) = buoy_green.x + vector_to_next_pair(0); + predicted_positions(1, 3) = buoy_green.y + vector_to_next_pair(1); + + return predicted_positions; +} + } // namespace maneuvering_task \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml index dde98e63..42e4e897 100644 --- a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml +++ b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml @@ -18,4 +18,5 @@ navigation_task_node: assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct # Task specific parameters - distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame \ No newline at end of file + distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame + # If the distance is greater than 6.0 we drive distance-4m towards the first buoy pair before trying to find it \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 4b6c75d6..d0582135 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -16,6 +16,26 @@ void NavigationTaskNode::main_task() { RCLCPP_INFO(this->get_logger(), "Navigation task started"); // First pair of buoys Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); + double distance_to_first_buoy_pair = + this->get_parameter("distance_to_first_buoy_pair").as_double(); + if (distance_to_first_buoy_pair > 6.0){ + geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; + waypoint_to_approach_first_pair_base_link.x = distance_to_first_buoy_pair-4.0; + waypoint_to_approach_first_pair_base_link.y = 0.0; + waypoint_to_approach_first_pair_base_link.z = 0.0; + try { + auto transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); + geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; + tf2::doTransform(waypoint_to_approach_first_pair_base_link, + waypoint_to_approach_first_pair_odom, transform); + send_waypoint(waypoint_to_approach_first_pair_odom); + reach_waypoint(1.0); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + } + + } std::vector buoy_landmarks_0_to_1 = get_buoy_landmarks(predicted_first_buoy_pair); if (buoy_landmarks_0_to_1.size() != 2) { @@ -297,9 +317,9 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { try { auto transform = tf_buffer_->lookupTransform( "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, + tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, + tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); transform_success = true; } catch (tf2::TransformException &ex) { From 84070377122629f0f7a2ab92057426f18315b178 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Wed, 31 Jul 2024 15:24:41 +0000 Subject: [PATCH 34/67] Committing clang-format changes --- .../maneuvering_task/maneuvering_task_ros.hpp | 27 ++- .../src/maneuvering_task_ros.cpp | 178 ++++++++++++------ .../src/navigation_task_ros.cpp | 12 +- 3 files changed, 142 insertions(+), 75 deletions(-) diff --git a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp index 8efa88b7..45423aa0 100644 --- a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp +++ b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp @@ -20,20 +20,19 @@ class ManeuveringTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); Eigen::Array -predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - Eigen::Array predict_third_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); - - Eigen::Array predict_next_pair_in_formation( - const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green, - Eigen::Vector2d direction_vector); + predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1); + + Eigen::Array + predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, + const geometry_msgs::msg::Point &buoy_3); + + Eigen::Array + predict_next_pair_in_formation(const geometry_msgs::msg::Point &buoy_red, + const geometry_msgs::msg::Point &buoy_green, + Eigen::Vector2d direction_vector); private: }; diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index 6055a9a8..e5f1a0aa 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -17,7 +17,8 @@ void ManeuveringTaskNode::main_task() { RCLCPP_INFO(this->get_logger(), "Maneuvering task started"); // First buoy pair Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); - // First first buoy pair is far away, should be closer before trying to measure it. + // First first buoy pair is far away, should be closer before trying to + // measure it. double gps_start_x = this->get_parameter("gps_start_x").as_double(); double gps_start_y = this->get_parameter("gps_start_y").as_double(); double gps_end_x = this->get_parameter("gps_end_x").as_double(); @@ -26,120 +27,192 @@ void ManeuveringTaskNode::main_task() { direction_vector_to_end << gps_end_x - gps_start_x, gps_end_y - gps_start_y; direction_vector_to_end.normalize(); - double distance = this->get_parameter("distance_to_first_buoy_pair").as_double(); + double distance = + this->get_parameter("distance_to_first_buoy_pair").as_double(); if (distance > 5.0) { auto odom = get_odom(); geometry_msgs::msg::Point waypoint_toward_start; - waypoint_toward_start.x = odom->pose.pose.position.x + direction_vector_to_end(0) * distance -3; - waypoint_toward_start.y = odom->pose.pose.position.y + direction_vector_to_end(1) * distance -3; + waypoint_toward_start.x = + odom->pose.pose.position.x + direction_vector_to_end(0) * distance - 3; + waypoint_toward_start.y = + odom->pose.pose.position.y + direction_vector_to_end(1) * distance - 3; waypoint_toward_start.z = 0.0; send_waypoint(waypoint_toward_start); reach_waypoint(2.0); } - std::vector measured_first_buoy_pair = get_buoy_landmarks(predicted_first_buoy_pair); + std::vector measured_first_buoy_pair = + get_buoy_landmarks(predicted_first_buoy_pair); geometry_msgs::msg::Point waypoint_first_pair; - waypoint_first_pair.x = (measured_first_buoy_pair[0].pose_odom_frame.position.x + measured_first_buoy_pair[1].pose_odom_frame.position.x) / 2; - waypoint_first_pair.y = (measured_first_buoy_pair[0].pose_odom_frame.position.y + measured_first_buoy_pair[1].pose_odom_frame.position.y) / 2; + waypoint_first_pair.x = + (measured_first_buoy_pair[0].pose_odom_frame.position.x + + measured_first_buoy_pair[1].pose_odom_frame.position.x) / + 2; + waypoint_first_pair.y = + (measured_first_buoy_pair[0].pose_odom_frame.position.y + + measured_first_buoy_pair[1].pose_odom_frame.position.y) / + 2; waypoint_first_pair.z = 0.0; send_waypoint(waypoint_first_pair); reach_waypoint(0.5); - // Second buoy pair is FAR away, should be closer before trying to measure it. geometry_msgs::msg::Point waypoint_approach_formation; - waypoint_approach_formation.x = waypoint_first_pair.x + direction_vector_to_end(0) * 15; - waypoint_approach_formation.y = waypoint_first_pair.y + direction_vector_to_end(1) * 15; + waypoint_approach_formation.x = + waypoint_first_pair.x + direction_vector_to_end(0) * 15; + waypoint_approach_formation.y = + waypoint_first_pair.y + direction_vector_to_end(1) * 15; waypoint_approach_formation.z = 0.0; send_waypoint(waypoint_approach_formation); reach_waypoint(1.0); // Second buoy pair - Eigen::Array predicted_second_buoy_pair = predict_second_buoy_pair(measured_first_buoy_pair[0].pose_odom_frame.position, measured_first_buoy_pair[1].pose_odom_frame.position); - std::vector measured_buoy_0_to_3 = get_buoy_landmarks(predicted_second_buoy_pair); + Eigen::Array predicted_second_buoy_pair = + predict_second_buoy_pair( + measured_first_buoy_pair[0].pose_odom_frame.position, + measured_first_buoy_pair[1].pose_odom_frame.position); + std::vector measured_buoy_0_to_3 = + get_buoy_landmarks(predicted_second_buoy_pair); geometry_msgs::msg::Point waypoint_second_pair; - waypoint_second_pair.x = (measured_buoy_0_to_3[2].pose_odom_frame.position.x + measured_buoy_0_to_3[3].pose_odom_frame.position.x) / 2; - waypoint_second_pair.y = (measured_buoy_0_to_3[2].pose_odom_frame.position.y + measured_buoy_0_to_3[3].pose_odom_frame.position.y) / 2; + waypoint_second_pair.x = + (measured_buoy_0_to_3[2].pose_odom_frame.position.x + + measured_buoy_0_to_3[3].pose_odom_frame.position.x) / + 2; + waypoint_second_pair.y = + (measured_buoy_0_to_3[2].pose_odom_frame.position.y + + measured_buoy_0_to_3[3].pose_odom_frame.position.y) / + 2; waypoint_second_pair.z = 0.0; send_waypoint(waypoint_second_pair); reach_waypoint(0.5); // Setup variable to navigate formation Eigen::Vector2d direction_vector_forwards; - direction_vector_forwards << (measured_buoy_0_to_3[2].pose_odom_frame.position.x + measured_buoy_0_to_3[3].pose_odom_frame.position.x) / 2 - - (measured_buoy_0_to_3[0].pose_odom_frame.position.x + measured_buoy_0_to_3[1].pose_odom_frame.position.x) / 2, - (measured_buoy_0_to_3[2].pose_odom_frame.position.y + measured_buoy_0_to_3[3].pose_odom_frame.position.y) / 2 - - (measured_buoy_0_to_3[0].pose_odom_frame.position.y + measured_buoy_0_to_3[1].pose_odom_frame.position.y) / 2; + direction_vector_forwards + << (measured_buoy_0_to_3[2].pose_odom_frame.position.x + + measured_buoy_0_to_3[3].pose_odom_frame.position.x) / + 2 - + (measured_buoy_0_to_3[0].pose_odom_frame.position.x + + measured_buoy_0_to_3[1].pose_odom_frame.position.x) / + 2, + (measured_buoy_0_to_3[2].pose_odom_frame.position.y + + measured_buoy_0_to_3[3].pose_odom_frame.position.y) / + 2 - + (measured_buoy_0_to_3[0].pose_odom_frame.position.y + + measured_buoy_0_to_3[1].pose_odom_frame.position.y) / + 2; direction_vector_forwards.normalize(); Eigen::Vector2d direction_vector_downwards; - direction_vector_downwards << measured_buoy_0_to_3[3].pose_odom_frame.position.x - measured_buoy_0_to_3[2].pose_odom_frame.position.x, - measured_buoy_0_to_3[3].pose_odom_frame.position.y - measured_buoy_0_to_3[2].pose_odom_frame.position.y; + direction_vector_downwards + << measured_buoy_0_to_3[3].pose_odom_frame.position.x - + measured_buoy_0_to_3[2].pose_odom_frame.position.x, + measured_buoy_0_to_3[3].pose_odom_frame.position.y - + measured_buoy_0_to_3[2].pose_odom_frame.position.y; direction_vector_downwards.normalize(); Eigen::Vector2d vector_to_next_pair; - vector_to_next_pair << direction_vector_forwards(0) * 5 + direction_vector_downwards(0) * this->get_parameter("initial_offset").as_double(), - direction_vector_forwards(1) * 5 + direction_vector_downwards(1) * this->get_parameter("initial_offset").as_double(); + vector_to_next_pair + << direction_vector_forwards(0) * 5 + + direction_vector_downwards(0) * + this->get_parameter("initial_offset").as_double(), + direction_vector_forwards(1) * 5 + + direction_vector_downwards(1) * + this->get_parameter("initial_offset").as_double(); - geometry_msgs::msg::Point red_buoy = measured_buoy_0_to_3[2].pose_odom_frame.position; - geometry_msgs::msg::Point green_buoy = measured_buoy_0_to_3[3].pose_odom_frame.position; + geometry_msgs::msg::Point red_buoy = + measured_buoy_0_to_3[2].pose_odom_frame.position; + geometry_msgs::msg::Point green_buoy = + measured_buoy_0_to_3[3].pose_odom_frame.position; // ASV is at first buoy pair in formation. // Run loop for the rest of the formation excluding the first pair. - int num_iterations = this->get_parameter("nr_of_pair_in_formation").as_int() - 1; + int num_iterations = + this->get_parameter("nr_of_pair_in_formation").as_int() - 1; for (int _ = 0; _ < num_iterations; ++_) { Eigen::Array predicted_next_pair = - predict_next_pair_in_formation(red_buoy, - green_buoy, + predict_next_pair_in_formation(red_buoy, green_buoy, vector_to_next_pair); - std::vector measured_next_pair = get_buoy_landmarks(predicted_next_pair); + std::vector measured_next_pair = + get_buoy_landmarks(predicted_next_pair); geometry_msgs::msg::Point waypoint_next_pair; - waypoint_next_pair.x = (measured_next_pair[2].pose_odom_frame.position.x + measured_next_pair[3].pose_odom_frame.position.x) / 2; - waypoint_next_pair.y = (measured_next_pair[2].pose_odom_frame.position.y + measured_next_pair[3].pose_odom_frame.position.y) / 2; + waypoint_next_pair.x = (measured_next_pair[2].pose_odom_frame.position.x + + measured_next_pair[3].pose_odom_frame.position.x) / + 2; + waypoint_next_pair.y = (measured_next_pair[2].pose_odom_frame.position.y + + measured_next_pair[3].pose_odom_frame.position.y) / + 2; waypoint_next_pair.z = 0.0; send_waypoint(waypoint_next_pair); reach_waypoint(1.0); red_buoy = measured_next_pair[2].pose_odom_frame.position; green_buoy = measured_next_pair[3].pose_odom_frame.position; - vector_to_next_pair << (measured_next_pair[2].pose_odom_frame.position.x + measured_next_pair[3].pose_odom_frame.position.x) / 2 - - (measured_next_pair[1].pose_odom_frame.position.x + measured_next_pair[0].pose_odom_frame.position.x) / 2, - (measured_next_pair[2].pose_odom_frame.position.y + measured_next_pair[3].pose_odom_frame.position.y) / 2 - - (measured_next_pair[1].pose_odom_frame.position.y + measured_next_pair[0].pose_odom_frame.position.y) / 2; + vector_to_next_pair + << (measured_next_pair[2].pose_odom_frame.position.x + + measured_next_pair[3].pose_odom_frame.position.x) / + 2 - + (measured_next_pair[1].pose_odom_frame.position.x + + measured_next_pair[0].pose_odom_frame.position.x) / + 2, + (measured_next_pair[2].pose_odom_frame.position.y + + measured_next_pair[3].pose_odom_frame.position.y) / + 2 - + (measured_next_pair[1].pose_odom_frame.position.y + + measured_next_pair[0].pose_odom_frame.position.y) / + 2; } // ASV is now at the last pair of the buoy formation // Should move close to end before we try to measure the last pair auto odom = get_odom(); - double distance_to_end = std::sqrt(std::pow(odom->pose.pose.position.x - gps_end_x, 2) + std::pow(odom->pose.pose.position.y - gps_end_y, 2)); + double distance_to_end = + std::sqrt(std::pow(odom->pose.pose.position.x - gps_end_x, 2) + + std::pow(odom->pose.pose.position.y - gps_end_y, 2)); if (distance_to_end > 6.0) { auto odom = get_odom(); geometry_msgs::msg::Point waypoint_toward_end; - waypoint_toward_end.x = odom->pose.pose.position.x + direction_vector_to_end(0) * (distance_to_end - 3.0); - waypoint_toward_end.y = odom->pose.pose.position.y + direction_vector_to_end(1) * (distance_to_end - 3.0); + waypoint_toward_end.x = + odom->pose.pose.position.x + + direction_vector_to_end(0) * (distance_to_end - 3.0); + waypoint_toward_end.y = + odom->pose.pose.position.y + + direction_vector_to_end(1) * (distance_to_end - 3.0); waypoint_toward_end.z = 0.0; send_waypoint(waypoint_toward_end); reach_waypoint(2.0); } Eigen::Array predicted_last_buoy_pair; - predicted_last_buoy_pair(0, 0) = gps_end_x - direction_vector_downwards(0) * 2.5; - predicted_last_buoy_pair(1, 0) = gps_end_y - direction_vector_downwards(1) * 2.5; - predicted_last_buoy_pair(0, 1) = gps_end_x + direction_vector_downwards(0) * 2.5; - predicted_last_buoy_pair(1, 1) = gps_end_y + direction_vector_downwards(1) * 2.5; - std::vector measured_last_buoy_pair = get_buoy_landmarks(predicted_last_buoy_pair); + predicted_last_buoy_pair(0, 0) = + gps_end_x - direction_vector_downwards(0) * 2.5; + predicted_last_buoy_pair(1, 0) = + gps_end_y - direction_vector_downwards(1) * 2.5; + predicted_last_buoy_pair(0, 1) = + gps_end_x + direction_vector_downwards(0) * 2.5; + predicted_last_buoy_pair(1, 1) = + gps_end_y + direction_vector_downwards(1) * 2.5; + std::vector measured_last_buoy_pair = + get_buoy_landmarks(predicted_last_buoy_pair); geometry_msgs::msg::Point waypoint_last_pair; - waypoint_last_pair.x = (measured_last_buoy_pair[0].pose_odom_frame.position.x + measured_last_buoy_pair[1].pose_odom_frame.position.x) / 2; - waypoint_last_pair.y = (measured_last_buoy_pair[0].pose_odom_frame.position.y + measured_last_buoy_pair[1].pose_odom_frame.position.y) / 2; + waypoint_last_pair.x = + (measured_last_buoy_pair[0].pose_odom_frame.position.x + + measured_last_buoy_pair[1].pose_odom_frame.position.x) / + 2; + waypoint_last_pair.y = + (measured_last_buoy_pair[0].pose_odom_frame.position.y + + measured_last_buoy_pair[1].pose_odom_frame.position.y) / + 2; waypoint_last_pair.z = 0.0; send_waypoint(waypoint_last_pair); // Also send waypoint 2m through the last pair geometry_msgs::msg::Point waypoint_through_last_pair; - waypoint_through_last_pair.x = waypoint_last_pair.x + direction_vector_to_end(0) * 2; - waypoint_through_last_pair.y = waypoint_last_pair.y + direction_vector_to_end(1) * 2; + waypoint_through_last_pair.x = + waypoint_last_pair.x + direction_vector_to_end(0) * 2; + waypoint_through_last_pair.y = + waypoint_last_pair.y + direction_vector_to_end(1) * 2; waypoint_through_last_pair.z = 0.0; send_waypoint(waypoint_through_last_pair); - //Task is now finished - + // Task is now finished } Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { @@ -167,10 +240,8 @@ Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { try { auto transform = tf_buffer_->lookupTransform( "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, - transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, - transform); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); transform_success = true; } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); @@ -186,8 +257,7 @@ Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array -ManeuveringTaskNode::predict_second_buoy_pair( +Eigen::Array ManeuveringTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index d0582135..781a2247 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -18,9 +18,10 @@ void NavigationTaskNode::main_task() { Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); - if (distance_to_first_buoy_pair > 6.0){ + if (distance_to_first_buoy_pair > 6.0) { geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; - waypoint_to_approach_first_pair_base_link.x = distance_to_first_buoy_pair-4.0; + waypoint_to_approach_first_pair_base_link.x = + distance_to_first_buoy_pair - 4.0; waypoint_to_approach_first_pair_base_link.y = 0.0; waypoint_to_approach_first_pair_base_link.z = 0.0; try { @@ -34,7 +35,6 @@ void NavigationTaskNode::main_task() { } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } - } std::vector buoy_landmarks_0_to_1 = get_buoy_landmarks(predicted_first_buoy_pair); @@ -317,10 +317,8 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { try { auto transform = tf_buffer_->lookupTransform( "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, - transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, - transform); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); transform_success = true; } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); From a6a61acfb73cf6a928167e52eccb421d3d4dae56 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Thu, 1 Aug 2024 11:55:15 +0200 Subject: [PATCH 35/67] Map manager sim setup Added option to use predef map origin to streamline test setups --- .../config/sitaw/map_manager_params.yaml | 9 +- asv_setup/launch/sitaw.launch.py | 9 + .../params/map_manager_params.yaml | 7 + mission/map_manager/src/map_manager_ros.cpp | 67 +- .../src/collision_avoidance_task_ros.cpp | 15 +- .../include/docking_task/docking_task_ros.hpp | 62 +- .../params/docking_task_params.yaml | 16 +- .../docking_task/src/docking_task_ros.cpp | 693 +++++------------- .../params/maneuvering_task_params.yaml | 2 +- 9 files changed, 281 insertions(+), 599 deletions(-) diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml index 1fa57cf9..b23c80e3 100644 --- a/asv_setup/config/sitaw/map_manager_params.yaml +++ b/asv_setup/config/sitaw/map_manager_params.yaml @@ -1,8 +1,15 @@ map_manager_node: ros__parameters: - use_predef_landmask: true + use_predef_landmask: false landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml" map_resolution: 0.2 map_width: 1000 map_height: 1000 frame_id: "map" + # Map origin for office rosbag + # map_origin_lat: 63.414660884931976 + # map_origin_lon: 10.398554661537544 + # use_predef_map_origin: true + map_origin_lat: 0.0 + map_origin_lon: 0.0 + use_predef_map_origin: false \ No newline at end of file diff --git a/asv_setup/launch/sitaw.launch.py b/asv_setup/launch/sitaw.launch.py index 1961eeea..71f796fb 100644 --- a/asv_setup/launch/sitaw.launch.py +++ b/asv_setup/launch/sitaw.launch.py @@ -15,6 +15,12 @@ def generate_launch_description(): default_value='True', description='enable tf launch file', ) + enable_seapath = LaunchConfiguration('enable_seapath') + enable_seapath_arg = DeclareLaunchArgument( + 'enable_seapath', + default_value='True', + description='enable seapath launch file', + ) seapath_ros_driver_node = Node( package='seapath_ros_driver', @@ -22,6 +28,8 @@ def generate_launch_description(): name='seapath_ros_driver_node', parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','seapath_params.yaml')], output='screen', + condition=IfCondition(enable_seapath), + ) map_manager_node = Node( package='map_manager', @@ -45,6 +53,7 @@ def generate_launch_description(): return LaunchDescription([ enable_tf_arg, tf_launch, + enable_seapath_arg, seapath_ros_driver_node, map_manager_node, landmark_server_node diff --git a/mission/map_manager/params/map_manager_params.yaml b/mission/map_manager/params/map_manager_params.yaml index 61b315b2..4b7b047b 100644 --- a/mission/map_manager/params/map_manager_params.yaml +++ b/mission/map_manager/params/map_manager_params.yaml @@ -6,3 +6,10 @@ map_manager_node: map_width: 1000 map_height: 1000 frame_id: "world" + # Map origin for office rosbag + # map_origin_lat: 63.414660884931976 + # map_origin_lon: 10.398554661537544 + # use_predef_map_origin: true + map_origin_lat: 0.0 + map_origin_lon: 0.0 + use_predef_map_origin: false diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index 968afe6b..c1129c30 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -5,10 +5,6 @@ namespace map_manager { MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) : Node("map_manager_node", options) { - // rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - // auto qos_sensor_data = - // rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), - // qos_profile); rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; qos_profile_transient_local.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; @@ -16,10 +12,6 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), qos_profile_transient_local); - map_origin_sub_ = this->create_subscription( - "/map/origin", qos_transient_local, - std::bind(&MapManagerNode::mapOriginCallback, this, - std::placeholders::_1)); declare_parameter("use_predef_landmask", false); declare_parameter( @@ -29,26 +21,36 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) declare_parameter("map_width", 1000); declare_parameter("map_height", 1000); declare_parameter("frame_id", "map"); + declare_parameter("map_origin_lat", 0.0); + declare_parameter("map_origin_lon", 0.0); + declare_parameter("use_predef_map_origin", false); landmask_file_ = get_parameter("landmask_file").as_string(); landmask_pub_ = this->create_publisher( "landmask", qos_transient_local); - // traffic centre - // map_origin_lat_ = 63.41490901857848; - // map_origin_lon_ = 10.398215601285054; - // office bag still - map_origin_lat_ = 63.414660884931976; - map_origin_lon_ = 10.398554661537544; - map_origin_set_ = true; - auto grid = createOccupancyGrid(); - auto polygon = readPolygonFromFile(landmask_file_); - landmask_pub_->publish(polygon); - fillOutsidePolygon(grid, polygon); - insert_landmask(grid, polygon); - map_pub_ = this->create_publisher( "map", qos_transient_local); - map_pub_->publish(grid); + + if(this->get_parameter("use_predef_map_origin").as_bool()){ + map_origin_lat_ = this->get_parameter("map_origin_lat").as_double(); + map_origin_lon_ = this->get_parameter("map_origin_lon").as_double(); + map_origin_set_ = true; + + auto grid = createOccupancyGrid(); + if(this->get_parameter("use_predef_landmask").as_bool()){ + auto polygon = readPolygonFromFile(landmask_file_); + landmask_pub_->publish(polygon); + fillOutsidePolygon(grid, polygon); + insert_landmask(grid, polygon); + } + + map_pub_->publish(grid); + } else { + map_origin_sub_ = this->create_subscription( + "/map/origin", qos_transient_local, + std::bind(&MapManagerNode::mapOriginCallback, this, + std::placeholders::_1)); + } grid_service_ = this->create_service( "get_map", @@ -66,26 +68,27 @@ void MapManagerNode::mapOriginCallback( map_origin_lon_ = msg->longitude; map_origin_set_ = true; map_origin_sub_ = nullptr; - - landmask_pub_->publish(readPolygonFromFile(landmask_file_)); + if(this->get_parameter("use_predef_landmask").as_bool()){ + landmask_pub_->publish(readPolygonFromFile(landmask_file_)); + } } void MapManagerNode::handle_get_map_request( - const std::shared_ptr request_header, - const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr request_header, + [[maybe_unused]] const std::shared_ptr request, const std::shared_ptr response) { if (!map_origin_set_) { RCLCPP_WARN(this->get_logger(), "Map origin not set, cannot provide map"); return; } - // Your logic to fill the map data RCLCPP_INFO(this->get_logger(), "Received request for map"); - // Example: You could fill this with actual map data nav_msgs::msg::OccupancyGrid grid = createOccupancyGrid(); - auto polygon = readPolygonFromFile(landmask_file_); - fillOutsidePolygon(grid, polygon); - insert_landmask(grid, polygon); + if(this->get_parameter("use_predef_landmask").as_bool()){ + auto polygon = readPolygonFromFile(landmask_file_); + fillOutsidePolygon(grid, polygon); + insert_landmask(grid, polygon); + } response->map = grid; RCLCPP_INFO(this->get_logger(), "Map sent"); } @@ -210,7 +213,7 @@ nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() { grid.info.resolution = get_parameter("map_resolution").as_double(); grid.info.width = get_parameter("map_width").as_int(); grid.info.height = get_parameter("map_height").as_int(); - grid.info.origin = calculate_map_origin(); + // grid.info.origin = calculate_map_origin(); grid.info.map_load_time = this->now(); // 0 represents unoccupied, 1 represents definitely occupied, and // -1 represents unknown. diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp index 7bf5e2da..2aa18f2e 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -17,20 +17,7 @@ void CollisionAvoidanceTaskNode::main_task() { rclcpp::sleep_for(std::chrono::seconds(3)); while (true) { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - std::unique_lock setup_lock(navigation_mutex_); - if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); - setup_lock.unlock(); - continue; - } - if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - get_map_odom_tf(); - set_gps_odom_points(); - setup_lock.unlock(); - break; - } - setup_lock.unlock(); + } } diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp index 1dc2ef81..49c18349 100644 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -11,12 +11,6 @@ namespace docking_task { -struct LandmarkWithID { - geometry_msgs::msg::Pose pose_odom_frame; - geometry_msgs::msg::Pose pose_base_link_frame; - int32_t id; -}; - /** * @class DockingTaskNode * @brief A class representing a node for handling dock localization in a ROS 2 @@ -45,54 +39,34 @@ class DockingTaskNode : public NjordTaskBaseNode { */ void main_task(); - /** - * @brief Detect the closest buoy pair and set a waypoint between them - * - * @return A pair of landmarks representing the closest buoy pair - */ - std::pair initial_waypoint(); + Eigen::Array predict_first_buoy_pair(); - /** - * @brief Predict the 6-tuple formation of buoys - * - * @param landmark1 The first landmark/buoy used for initial waypoint - * @param landmark2 The second landmark/buoy used for initial waypoint - * - * @return The predicted posistion of the 6-tuple formation of buoys in odom - * frame - */ - Eigen::Array - predict_buoy_formation(const LandmarkWithID &buoy1, - const LandmarkWithID &buoy2); + Eigen::Array +predict_second_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1); - /** - * @brief Navigate the ASV through the formation of buoys. First travels to - * waypoint between second pair of buoys, then navigates through the formation - * of buoys and returns control when asv is 0.2m away from crossing the last - * buoy pair. - * - * @param predicted_positions The predicted positions of the buoys - * @return The ids of the last pair of buoys in the formation - */ - std::pair - navigate_formation(Eigen::Array &predicted_positions); + Eigen::Array predict_third_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, + const geometry_msgs::msg::Point &buoy_3); - /** - * @brief Utility function that runs until waypoint is reached. - * Function returns when within distance_threshold of the waypoint. - * - * @param distance_threshold The distance threshold for reaching the waypoint - */ - void reach_waypoint(const double distance_threshold); + void grid_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + + std::shared_ptr get_grid(); + void initialize_grid_sub(); + private: mutable std::mutex grid_mutex_; bool new_grid_msg_ = false; std::condition_variable grid_cond_var_; - + nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_; rclcpp::Subscription::SharedPtr grid_sub_; - nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_; + + }; } // namespace docking_task diff --git a/mission/njord_tasks/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml index 76f29018..919a27a6 100644 --- a/mission/njord_tasks/docking_task/params/docking_task_params.yaml +++ b/mission/njord_tasks/docking_task/params/docking_task_params.yaml @@ -12,6 +12,16 @@ docking_task_node: gps_end_x: 0.0 gps_end_y: 0.0 gps_frame_coords_set: false + map_origin_topic: "/map/origin" + odom_topic: "/seapath/odom/ned" + landmark_topic: "landmarks_out" + assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct + + # Task specific parameters (not derived from base class) + distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame + # If the distance is greater than 6.0 we drive distance-4m towards the first buoy pair before trying to find it + distance_between_buoys_in_pair: 5.0 # Distance between the two buoys in a pair + grid_topic: "grid" dock_width: 4.13 # width of available docking space dock_width_tolerance: 0.5 dock_length: 2.0 # length of available docking space @@ -23,10 +33,4 @@ docking_task_node: models: dynmod_stddev: 0.01 senmod_stddev: 0.01 - - - map_origin_topic: "/map/origin" - grid_topic: "grid" - odom_topic: "/seapath/odom/ned" - landmark_topic: "landmarks_out" \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 15ef198b..ee968dcf 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -5,6 +5,9 @@ namespace docking_task { DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) : NjordTaskBaseNode("dock_localization_node", options) { + declare_parameter("distance_to_first_buoy_pair", 2.0); + declare_parameter("distance_between_buoys_in_pair", 5.0); + declare_parameter("grid_topic", "grid"); declare_parameter("dock_width", 0.0); declare_parameter("dock_width_tolerance", 0.0); declare_parameter("dock_length", 0.0); @@ -15,532 +18,220 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) declare_parameter("task_nr", 0.0); declare_parameter("models.dynmod_stddev", 0.0); declare_parameter("models.sen_stddev", 0.0); - declare_parameter("grid_topic", "grid"); + + initialize_grid_sub(); std::thread(&DockingTaskNode::main_task, this).detach(); } void DockingTaskNode::main_task() { - // Sleep for 5 seconds to allow system to initialize and tracks to be aquired - RCLCPP_INFO( - this->get_logger(), - "Waiting 3 seconds for system to initialize before starting main task"); - rclcpp::sleep_for(std::chrono::seconds(3)); - - while (true) { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - std::unique_lock setup_lock(navigation_mutex_); - if (!(this->get_parameter("map_origin_set").as_bool())) { - RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms"); - setup_lock.unlock(); - continue; - } - if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - set_gps_odom_points(); - get_map_odom_tf(); - setup_lock.unlock(); - break; + navigation_ready(); + // Starting docking task + Eigen::Array predicted_first_pair = predict_first_buoy_pair(); + double distance_to_first_buoy_pair = + this->get_parameter("distance_to_first_buoy_pair").as_double(); + if (distance_to_first_buoy_pair > 6.0){ + geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; + waypoint_to_approach_first_pair_base_link.x = distance_to_first_buoy_pair-4.0; + waypoint_to_approach_first_pair_base_link.y = 0.0; + waypoint_to_approach_first_pair_base_link.z = 0.0; + try { + auto transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); + geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; + tf2::doTransform(waypoint_to_approach_first_pair_base_link, + waypoint_to_approach_first_pair_odom, transform); + send_waypoint(waypoint_to_approach_first_pair_odom); + reach_waypoint(1.0); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } - setup_lock.unlock(); } + std::vector buoy_landmarks_0_to_1 = get_buoy_landmarks(predicted_first_pair); + if (buoy_landmarks_0_to_1.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); + } + geometry_msgs::msg::Point waypoint_first_pair; + waypoint_first_pair.x = + (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + + buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / + 2; + waypoint_first_pair.y = + (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + + buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / + 2; + waypoint_first_pair.z = 0.0; + send_waypoint(waypoint_first_pair); + reach_waypoint(1.0); - // Set initial waypoint between first buoy pair - auto [landmark1, landmark2] = initial_waypoint(); - RCLCPP_INFO(this->get_logger(), - "Initial waypoint set between landmarks %d and %d", landmark1.id, - landmark2.id); - + // Second pair of buoys + Eigen::Array predicted_first_and_second_pair = + predict_second_buoy_pair( + buoy_landmarks_0_to_1[0].pose_odom_frame.position, + buoy_landmarks_0_to_1[1].pose_odom_frame.position); + std::vector buoy_landmarks_0_to_3 = + get_buoy_landmarks(predicted_first_and_second_pair); + if (buoy_landmarks_0_to_3.size() != 4) { + RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); + } + geometry_msgs::msg::Point waypoint_second_pair; + waypoint_second_pair.x = + (buoy_landmarks_0_to_3[2].pose_odom_frame.position.x + + buoy_landmarks_0_to_3[3].pose_odom_frame.position.x) / + 2; + waypoint_second_pair.y = + (buoy_landmarks_0_to_3[2].pose_odom_frame.position.y + + buoy_landmarks_0_to_3[3].pose_odom_frame.position.y) / + 2; + waypoint_second_pair.z = 0.0; + send_waypoint(waypoint_second_pair); reach_waypoint(1.0); - auto formation = predict_buoy_formation(landmark1, landmark2); + // Third pair of buoys + Eigen::Array predicted_second_and_third_pair = + predict_third_buoy_pair( + buoy_landmarks_0_to_1[0].pose_odom_frame.position, + buoy_landmarks_0_to_1[1].pose_odom_frame.position, + buoy_landmarks_0_to_3[2].pose_odom_frame.position, + buoy_landmarks_0_to_3[3].pose_odom_frame.position); + std::vector buoy_landmarks_2_to_5 = + get_buoy_landmarks(predicted_second_and_third_pair); + if (buoy_landmarks_2_to_5.size() != 4) { + RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); + } + geometry_msgs::msg::Point waypoint_third_pair; + waypoint_third_pair.x = + (buoy_landmarks_2_to_5[2].pose_odom_frame.position.x + + buoy_landmarks_2_to_5[3].pose_odom_frame.position.x) / + 2; + waypoint_third_pair.y = (buoy_landmarks_2_to_5[2].pose_odom_frame.position.y + + buoy_landmarks_2_to_5[3].pose_odom_frame.position.y) / + 2; + waypoint_third_pair.z = 0.0; + send_waypoint(waypoint_third_pair); + reach_waypoint(1.0); - auto [buoy_left_id, buoy_right_id] = navigate_formation(formation); } -std::pair DockingTaskNode::initial_waypoint() { - RCLCPP_INFO(this->get_logger(), "Initial waypoint running"); - - while (true) { - - // std::unique_lock odom_lock(odom_mutex_); - // if (odom_msg_ == nullptr) { - // RCLCPP_INFO(this->get_logger(), "Odometry message not received, exiting - // initial waypoint"); odom_lock.unlock(); continue; - // } - // double heading = get_freya_heading(odom_msg_->pose.pose.orientation); - // odom_lock.unlock(); - - // double gps_start_x = this->get_parameter("gps_start_x").as_double(); - // double gps_start_y = this->get_parameter("gps_start_y").as_double(); - // double gps_end_x = this->get_parameter("gps_end_x").as_double(); - // double gps_end_y = this->get_parameter("gps_end_y").as_double(); - - // // Calculate the direction vector from the start point - // double dir_x = cos(heading); - // double dir_y = sin(heading); - - // // The point P should lie on the line from the start point in the - // direction of the heading - // // Find the intersection point where vector from P to end point is - // orthogonal to the heading direction vector - - // // Set up the equation for the line in the heading direction from start - // point - // // P = (P_x, P_y) - // // Line equation: P_x = gps_start_x + t * dir_x, P_y = gps_start_y + t * - // dir_y - - // // We need the vector (gps_end_x - P_x, gps_end_y - P_y) to be orthogonal - // to the direction vector (dir_x, dir_y) - // // (gps_end_x - (gps_start_x + t * dir_x)) * dir_x + (gps_end_y - - // (gps_start_y + t * dir_y)) * dir_y = 0 - // // Solving for t - - // double t = ((gps_end_x - gps_start_x) * dir_x + (gps_end_y - gps_start_y) - // * dir_y) / (dir_x * dir_x + dir_y * dir_y); - - // // Calculate intersection point - // double intersection_x = gps_start_x + t * dir_x; - // double intersection_y = gps_start_y + t * dir_y; - - // RCLCPP_INFO(this->get_logger(), "Intersection point: %f, %f", - // intersection_x, intersection_y); geometry_msgs::msg::PoseStamped - // waypoint; waypoint.header.frame_id = "map"; waypoint.pose.position.x = - // intersection_x; waypoint.pose.position.y = intersection_y; - // waypoint_visualization_pub_->publish(waypoint); - - if (landmarks_msg_ == nullptr) { - RCLCPP_INFO(this->get_logger(), - "Landmark pose array not received, exiting initial waypoint"); - continue; - } - - int result = 0; - double x_waypoint = 0.0; - double y_waypoint = 0.0; - std::pair id_pair; - - std::pair landmark_return_pair; - - while (result < 10) { - - // Transform landmark poses to base_link frame - std::vector landmarks_with_ids; - auto landmark_msg = get_landmarks_odom_frame(); - for (const auto &landmark : landmark_msg->landmarks) { - LandmarkWithID lwid; - lwid.id = landmark.id; - lwid.pose_odom_frame = landmark.odom.pose.pose; - landmarks_with_ids.push_back(lwid); - } - - try { - auto transform = - tf_buffer_->lookupTransform("base_link", "odom", rclcpp::Time(0)); - for (size_t i = 0; i < landmarks_with_ids.size(); ++i) { - tf2::doTransform(landmarks_with_ids.at(i).pose_odom_frame, - landmarks_with_ids.at(i).pose_base_link_frame, - transform); - } - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - } - - // Remove landmarks behind the drone - std::remove_if(landmarks_with_ids.begin(), landmarks_with_ids.end(), - [](const LandmarkWithID &l) { - return l.pose_base_link_frame.position.x < 0.0; - }); - - // Sort landmarks based on distance - std::sort(landmarks_with_ids.begin(), landmarks_with_ids.end(), - [](const LandmarkWithID &a, const LandmarkWithID &b) { - double dist_a = - sqrt(pow(a.pose_base_link_frame.position.x, 2) + - pow(a.pose_base_link_frame.position.y, 2)); - double dist_b = - sqrt(pow(b.pose_base_link_frame.position.x, 2) + - pow(b.pose_base_link_frame.position.y, 2)); - return dist_a < dist_b; - }); - - // Find a pair of landmarks that satisfy the conditions - for (size_t i = 0; i < landmarks_with_ids.size() - 1; ++i) { - const auto &landmark1 = landmarks_with_ids[i]; - const auto &landmark2 = landmarks_with_ids[i + 1]; - - // Calculate the distance between the landmarks - double distance = - sqrt(pow(landmark2.pose_base_link_frame.position.x - - landmark1.pose_base_link_frame.position.x, - 2) + - pow(landmark2.pose_base_link_frame.position.y - - landmark1.pose_base_link_frame.position.y, - 2)); - - // Check if the landmarks are on opposite sides of the drone and within - // the desired distance range. - // Base link is NED frame, x is forward, y is right - if ((landmark1.pose_base_link_frame.position.x > 0 && - landmark2.pose_base_link_frame.position.x > 0) && - (distance >= 3.0 && distance <= 8.0) && - ((landmark1.pose_base_link_frame.position.y < 0) != - (landmark2.pose_base_link_frame.position.y < 0))) { - // Calculate the midpoint between the two landmarks in map frame - double x_waypoint_sample = (landmark1.pose_odom_frame.position.x + - landmark2.pose_odom_frame.position.x) / - 2; - double y_waypoint_sample = (landmark1.pose_odom_frame.position.y + - landmark2.pose_odom_frame.position.y) / - 2; - std::pair id_pair_sample = {landmark1.id, - landmark2.id}; - // Check if this is the first valid pair of landmarks - if (result == 0) { - x_waypoint = x_waypoint_sample; - y_waypoint = y_waypoint_sample; - id_pair = id_pair_sample; - result++; - break; - } - - // Check if the new waypoint is further away from the current waypoint - // or if the new pair of landmarks is the same as the current pair - if ((sqrt(pow(x_waypoint_sample - x_waypoint, 2) + - pow(y_waypoint_sample - y_waypoint, 2)) > 1.0) || - !((id_pair_sample.first == id_pair.first && - id_pair_sample.second == id_pair.second) || - (id_pair_sample.first == id_pair.second && - id_pair_sample.second == id_pair.first))) { - result = 0; - break; - } - - // Update the waypoint and the pair of landmarks - id_pair = id_pair_sample; - x_waypoint += x_waypoint_sample; - y_waypoint += y_waypoint_sample; - x_waypoint /= 2; - y_waypoint /= 2; - result++; - landmark_return_pair.first = landmark1; - landmark_return_pair.second = landmark2; - break; - } - } - } - // send waypoint - if (!waypoint_client_->service_is_ready()) { - RCLCPP_INFO(this->get_logger(), "Waypoint client not ready"); - continue; - } - geometry_msgs::msg::Point waypoint_odom_frame; - waypoint_odom_frame.x = x_waypoint; - waypoint_odom_frame.y = y_waypoint; - waypoint_odom_frame.z = 0.0; - - auto request = std::make_shared(); - request->waypoint.push_back(waypoint_odom_frame); - auto result_future = waypoint_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Waypoint(odom frame) sent: %f, %f", - waypoint_odom_frame.x, waypoint_odom_frame.y); - // Check if the service was successful - - auto status = result_future.wait_for(std::chrono::seconds(5)); - if (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); - continue; +Eigen::Array DockingTaskNode::predict_first_buoy_pair() { + // Predict the positions of the first two buoys + geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; + geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; + buoy_0_base_link_frame.header.frame_id = "base_link"; + buoy_1_base_link_frame.header.frame_id = "base_link"; + + double distance_to_first_buoy_pair = + this->get_parameter("distance_to_first_buoy_pair").as_double(); + double distance_between_buoys_in_pair = + this->get_parameter("distance_between_buoys_in_pair").as_double(); + + buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_0_base_link_frame.pose.position.y = -distance_between_buoys_in_pair / 2; + buoy_0_base_link_frame.pose.position.z = 0.0; + buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_1_base_link_frame.pose.position.y = distance_between_buoys_in_pair / 2; + buoy_1_base_link_frame.pose.position.z = 0.0; + + geometry_msgs::msg::PoseStamped buoy_0_odom_frame; + geometry_msgs::msg::PoseStamped buoy_1_odom_frame; + + bool transform_success = false; + while (!transform_success) { + try { + auto transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, + transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, + transform); + transform_success = true; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); - } - - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = waypoint_odom_frame.x; - waypoint_vis.pose.position.y = waypoint_odom_frame.y; - waypoint_visualization_pub_->publish(waypoint_vis); - - previous_waypoint_odom_frame_ = waypoint_odom_frame; - return landmark_return_pair; } -} -void DockingTaskNode::reach_waypoint(const double distance_threshold) { - RCLCPP_INFO(this->get_logger(), "Reach waypoint running"); - auto odom_msg = get_odom(); - double x = odom_msg->pose.pose.position.x; - double y = odom_msg->pose.pose.position.y; - while (sqrt(pow(x - previous_waypoint_odom_frame_.x, 2) + - pow(y - previous_waypoint_odom_frame_.y, 2)) > - distance_threshold) { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - auto odom_msg_ = get_odom(); - x = odom_msg->pose.pose.position.x; - y = odom_msg->pose.pose.position.y; - } - RCLCPP_INFO(this->get_logger(), "Reached waypoint"); - return; -} + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; + predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; + predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; + predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; -Eigen::Array -DockingTaskNode::predict_buoy_formation(const LandmarkWithID &buoy1, - const LandmarkWithID &buoy2) { - RCLCPP_INFO(this->get_logger(), "Predict buoy formation running"); - geometry_msgs::msg::Point previous_waypoint_map_frame; - geometry_msgs::msg::TransformStamped odom_map_tf; - try { - // Compute the inverse transform from the stored map_odom_tf_ - tf2::Transform tf2_transform; - tf2::fromMsg(map_odom_tf_.transform, tf2_transform); - tf2::Transform tf2_inverse_transform = tf2_transform.inverse(); - odom_map_tf.transform = tf2::toMsg(tf2_inverse_transform); + return predicted_positions; +} - // Use the inverse transform - tf2::doTransform(previous_waypoint_odom_frame_, previous_waypoint_map_frame, - odom_map_tf); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - } - Eigen::Vector2d direction_vector( - previous_waypoint_map_frame.x - - this->get_parameter("gps_start_x").as_double(), - previous_waypoint_map_frame.y - - this->get_parameter("gps_start_y").as_double()); +Eigen::Array +DockingTaskNode::predict_second_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1) { + Eigen::Vector2d direction_vector; + direction_vector << previous_waypoint_odom_frame_.x - + this->get_parameter("gps_start_x").as_double(), + previous_waypoint_odom_frame_.y - + this->get_parameter("gps_start_y").as_double(); direction_vector.normalize(); - // Sanity check that first buoy pair is ish correct - Eigen::Vector2d first_left, first_right; - size_t found = 0; - auto landmarks_msg = get_landmarks_odom_frame(); - for (const auto &landmark : landmarks_msg->landmarks) { - if (landmark.id == buoy1.id && - std::sqrt(std::pow(landmark.odom.pose.pose.position.x - - buoy1.pose_odom_frame.position.x, - 2) + - std::pow(landmark.odom.pose.pose.position.y - - buoy1.pose_odom_frame.position.y, - 2)) < 0.5) { - found++; - if (buoy1.pose_base_link_frame.position.y < 0) { - first_left = Eigen::Vector2d(landmark.odom.pose.pose.position.x, - landmark.odom.pose.pose.position.y); - } else { - first_right = Eigen::Vector2d(landmark.odom.pose.pose.position.x, - landmark.odom.pose.pose.position.y); - } - } - if (landmark.id == buoy2.id && - std::sqrt(std::pow(landmark.odom.pose.pose.position.x - - buoy2.pose_odom_frame.position.x, - 2) + - std::pow(landmark.odom.pose.pose.position.y - - buoy2.pose_odom_frame.position.y, - 2)) < 0.5) { - found++; - if (buoy2.pose_base_link_frame.position.y < 0) { - first_left = Eigen::Vector2d(landmark.odom.pose.pose.position.x, - landmark.odom.pose.pose.position.y); - } else { - first_right = Eigen::Vector2d(landmark.odom.pose.pose.position.x, - landmark.odom.pose.pose.position.y); - } - } - } - if (found != 2) { - RCLCPP_WARN(this->get_logger(), "Buoy pair not found in landmark array"); - } - Eigen::Array formation; - // Create formation with index - // dock - //<< 0 1 - //<< 2 3 - //<< 4 5 - // start - Eigen::Vector2d second_left, second_right, third_left, third_right; - second_left = first_left + direction_vector * 5.0; - second_right = first_right + direction_vector * 5.0; - third_left = first_left + direction_vector * 10.0; - third_right = first_right + direction_vector * 10.0; - formation << third_left.x(), third_right.x(), second_left.x(), - second_right.x(), first_left.x(), first_right.x(), third_left.y(), - third_right.y(), second_left.y(), second_right.y(), first_left.y(), - first_right.y(); - return formation; + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x; + predicted_positions(1, 0) = buoy_0.y; + predicted_positions(0, 1) = buoy_1.x; + predicted_positions(1, 1) = buoy_1.y; + predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 5; + predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 5; + predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 5; + predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 5; + + return predicted_positions; } -std::pair DockingTaskNode::navigate_formation( - Eigen::Array &predicted_positions) { - RCLCPP_INFO(this->get_logger(), "Navigating though formation"); - std::vector prev_assignment; - std::vector landmark_ids; - bool first_half = true; - // Buoy formation with index - // dock - //<< 0 1 - //<< 2 3 - //<< 4 5 - // start - int result = 0; - while (true) { - landmark_ids.clear(); - geometry_msgs::msg::PoseArray landmark_poses_odom_frame; - auto landmark_msg = get_landmarks_odom_frame(); - for (const auto &landmark : landmark_msg->landmarks) { - - landmark_ids.push_back(landmark.id); - landmark_poses_odom_frame.poses.push_back(landmark.odom.pose.pose); - } - - Eigen::MatrixXd measured_positions(2, landmark_ids.size()); - for (size_t i = 0; i < landmark_ids.size(); ++i) { - measured_positions(0, i) = - landmark_poses_odom_frame.poses.at(i).position.x; - measured_positions(1, i) = - landmark_poses_odom_frame.poses.at(i).position.y; - } - - if (predicted_positions.cols() > measured_positions.cols()) { - result = 0; - continue; - } - - Eigen::VectorXi assignment = - auction_algorithm(predicted_positions, measured_positions); - - if (result == 0) { - for (Eigen::Index i = 0; i < assignment.size(); i++) { - prev_assignment.push_back(landmark_ids.at(assignment(i))); - } - result++; - continue; - } - bool valied = true; - // Check that the assigned landmarks matches the previous assignment by id - if (first_half) { - if (assignment(2) == -1 || assignment(3) == -1 || assignment(4) == -1 || - assignment(5) == -1) { - valied = false; - } - - // Check index 2,3,4,5 - if (landmark_ids.at(assignment(2)) != prev_assignment.at(2) || - landmark_ids.at(assignment(3)) != prev_assignment.at(3) || - landmark_ids.at(assignment(4)) != prev_assignment.at(4) || - landmark_ids.at(assignment(5)) != prev_assignment.at(5)) { - valied = false; - } - } else { - if (assignment(0) == -1 || assignment(1) == -1 || assignment(2) == -1 || - assignment(3) == -1) { - valied = false; - } - // Check index 0,1,2,3 - if (landmark_ids.at(assignment(0)) != prev_assignment.at(0) || - landmark_ids.at(assignment(1)) != prev_assignment.at(1) || - landmark_ids.at(assignment(2)) != prev_assignment.at(2) || - landmark_ids.at(assignment(3)) != prev_assignment.at(3)) { - valied = false; - } - } - - if (!valied) { - result = 0; - prev_assignment.clear(); - continue; - } else { - result++; - } - if (result > 10) { - geometry_msgs::msg::Point waypoint_odom_frame; - // Calculate the waypoint between the relevant buoy pair - double buoy_left_x; - double buoy_left_y; - double buoy_right_x; - double buoy_right_y; - if (first_half) { - buoy_left_x = - landmark_poses_odom_frame.poses.at(assignment(2)).position.x; - buoy_left_y = - landmark_poses_odom_frame.poses.at(assignment(2)).position.y; - buoy_right_x = - landmark_poses_odom_frame.poses.at(assignment(3)).position.x; - buoy_right_y = - landmark_poses_odom_frame.poses.at(assignment(3)).position.y; - Eigen::Vector2d direction_vector( - (buoy_left_x - - landmark_poses_odom_frame.poses.at(assignment(4)).position.x + - buoy_right_x - - landmark_poses_odom_frame.poses.at(assignment(5)).position.x) / - 2, - (buoy_left_y - - landmark_poses_odom_frame.poses.at(assignment(4)).position.y + - buoy_right_y - - landmark_poses_odom_frame.poses.at(assignment(5)).position.y) / - 2); - direction_vector.normalize(); - // Update the predicted positions - predicted_positions << buoy_left_x + direction_vector.x() * 5, - buoy_right_x + direction_vector.x() * 5, buoy_left_x, buoy_right_x, - buoy_left_x - direction_vector.x() * 5, - buoy_right_x - direction_vector.x() * 5, - buoy_left_y + direction_vector.y() * 5, - buoy_right_y + direction_vector.y() * 5, buoy_left_y, buoy_right_y, - buoy_left_y - direction_vector.y() * 5, - buoy_right_y - direction_vector.y() * 5; - } else { - buoy_left_x = - landmark_poses_odom_frame.poses.at(assignment(0)).position.x; - buoy_left_y = - landmark_poses_odom_frame.poses.at(assignment(0)).position.y; - buoy_right_x = - landmark_poses_odom_frame.poses.at(assignment(1)).position.x; - buoy_right_y = - landmark_poses_odom_frame.poses.at(assignment(1)).position.y; - } - - waypoint_odom_frame.x = (buoy_left_x + buoy_right_x) / 2; - waypoint_odom_frame.y = (buoy_left_y + buoy_right_y) / 2; - - auto request = std::make_shared(); - request->waypoint.push_back(waypoint_odom_frame); - auto result_future = waypoint_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Waypoint sent: %f, %f", - waypoint_odom_frame.x, waypoint_odom_frame.y); - // Check if the service was successful - - auto status = result_future.wait_for(std::chrono::seconds(5)); - if (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); - continue; - } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); - } - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = waypoint_odom_frame.x; - waypoint_vis.pose.position.y = waypoint_odom_frame.y; - waypoint_visualization_pub_->publish(waypoint_vis); +Eigen::Array DockingTaskNode::predict_third_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, + const geometry_msgs::msg::Point &buoy_3) { + Eigen::Vector2d direction_vector_first_to_second_pair; + direction_vector_first_to_second_pair << (buoy_2.x + buoy_3.x) / 2 - + (buoy_0.x + buoy_1.x) / 2, + (buoy_2.y + buoy_3.y) / 2 - (buoy_0.y + buoy_1.y) / 2; + direction_vector_first_to_second_pair.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_2.x; + predicted_positions(1, 0) = buoy_2.y; + predicted_positions(0, 1) = buoy_3.x; + predicted_positions(1, 1) = buoy_3.y; + predicted_positions(0, 2) = buoy_2.x + direction_vector_first_to_second_pair(0) * 5; + predicted_positions(1, 2) = buoy_2.y + direction_vector_first_to_second_pair(1) * 5; + predicted_positions(0, 3) = buoy_3.x + direction_vector_first_to_second_pair(0) * 5; + predicted_positions(1, 3) = buoy_3.y + direction_vector_first_to_second_pair(1) * 5; + + return predicted_positions; +} - previous_waypoint_odom_frame_ = waypoint_odom_frame; +void DockingTaskNode::initialize_grid_sub() { + rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; + auto qos_sensor_data = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), + qos_profile_sensor_data); + std::string grid_topic = this->get_parameter("grid_topic").as_string(); + grid_sub_ = this->create_subscription( + grid_topic, qos_sensor_data, std::bind(&DockingTaskNode::grid_callback, this, std::placeholders::_1)); +} - if (first_half) { - first_half = false; - result = 0; - prev_assignment.clear(); +void DockingTaskNode::grid_callback( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + std::unique_lock lock(grid_mutex_); + grid_msg_ = msg; + new_grid_msg_ = true; + lock.unlock(); + grid_cond_var_.notify_one(); +} - // Wait for the ASV to reach the waypoint and then find waypoint for the - // second half of the formation - reach_waypoint(1.0); - continue; - } else { - // Return from function when the asv is close to the second waypoint - reach_waypoint(0.2); - return {landmark_ids.at(assignment(0)), landmark_ids.at(assignment(1))}; - } - } - } +std::shared_ptr DockingTaskNode::get_grid() { + std::unique_lock lock(grid_mutex_); + grid_cond_var_.wait(lock, [this] { return new_grid_msg_; }); + new_grid_msg_ = false; + lock.unlock(); + return grid_msg_; } } // namespace docking_task \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml index 15140c8b..86b8c703 100644 --- a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml +++ b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml @@ -21,5 +21,5 @@ maneuvering_task_node: distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame. # If the distance is greater than five we drive distance-3m towards end before trying to find the first buoy pair initial_offset: 1.0 # The vertical offset from the second and third buoy pair in meters - nr_of_pair_in_formation: 11 # Number of buoy pairs in formation, excluding the start and end gate at gps points + nr_of_pair_in_formation: 11 # Number of buoy pairs in formation, excluding the start and end gate at gps points (NB! pdf is not accurate) \ No newline at end of file From e9749293f9de03bee97b489440dcc846f2ab1a7a Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Thu, 1 Aug 2024 09:55:41 +0000 Subject: [PATCH 36/67] Committing clang-format changes --- mission/map_manager/src/map_manager_ros.cpp | 14 +++--- .../src/collision_avoidance_task_ros.cpp | 1 - .../include/docking_task/docking_task_ros.hpp | 20 ++++----- .../docking_task/src/docking_task_ros.cpp | 44 ++++++++++--------- 4 files changed, 39 insertions(+), 40 deletions(-) diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index c1129c30..0126688e 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -12,7 +12,6 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), qos_profile_transient_local); - declare_parameter("use_predef_landmask", false); declare_parameter( "landmask_file", @@ -30,14 +29,14 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) "landmask", qos_transient_local); map_pub_ = this->create_publisher( "map", qos_transient_local); - - if(this->get_parameter("use_predef_map_origin").as_bool()){ + + if (this->get_parameter("use_predef_map_origin").as_bool()) { map_origin_lat_ = this->get_parameter("map_origin_lat").as_double(); map_origin_lon_ = this->get_parameter("map_origin_lon").as_double(); map_origin_set_ = true; auto grid = createOccupancyGrid(); - if(this->get_parameter("use_predef_landmask").as_bool()){ + if (this->get_parameter("use_predef_landmask").as_bool()) { auto polygon = readPolygonFromFile(landmask_file_); landmask_pub_->publish(polygon); fillOutsidePolygon(grid, polygon); @@ -68,14 +67,15 @@ void MapManagerNode::mapOriginCallback( map_origin_lon_ = msg->longitude; map_origin_set_ = true; map_origin_sub_ = nullptr; - if(this->get_parameter("use_predef_landmask").as_bool()){ + if (this->get_parameter("use_predef_landmask").as_bool()) { landmask_pub_->publish(readPolygonFromFile(landmask_file_)); } } void MapManagerNode::handle_get_map_request( [[maybe_unused]] const std::shared_ptr request_header, - [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr + request, const std::shared_ptr response) { if (!map_origin_set_) { RCLCPP_WARN(this->get_logger(), "Map origin not set, cannot provide map"); @@ -84,7 +84,7 @@ void MapManagerNode::handle_get_map_request( RCLCPP_INFO(this->get_logger(), "Received request for map"); nav_msgs::msg::OccupancyGrid grid = createOccupancyGrid(); - if(this->get_parameter("use_predef_landmask").as_bool()){ + if (this->get_parameter("use_predef_landmask").as_bool()) { auto polygon = readPolygonFromFile(landmask_file_); fillOutsidePolygon(grid, polygon); insert_landmask(grid, polygon); diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp index 2aa18f2e..34ec0602 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -17,7 +17,6 @@ void CollisionAvoidanceTaskNode::main_task() { rclcpp::sleep_for(std::chrono::seconds(3)); while (true) { - } } diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp index 49c18349..7e3ac3c7 100644 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -42,31 +42,27 @@ class DockingTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); Eigen::Array -predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); + predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1); - Eigen::Array predict_third_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); + Eigen::Array + predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1, + const geometry_msgs::msg::Point &buoy_2, + const geometry_msgs::msg::Point &buoy_3); void grid_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); std::shared_ptr get_grid(); void initialize_grid_sub(); - + private: mutable std::mutex grid_mutex_; bool new_grid_msg_ = false; std::condition_variable grid_cond_var_; nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_; rclcpp::Subscription::SharedPtr grid_sub_; - - - }; } // namespace docking_task diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index ee968dcf..e524e674 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -30,9 +30,10 @@ void DockingTaskNode::main_task() { Eigen::Array predicted_first_pair = predict_first_buoy_pair(); double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); - if (distance_to_first_buoy_pair > 6.0){ + if (distance_to_first_buoy_pair > 6.0) { geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; - waypoint_to_approach_first_pair_base_link.x = distance_to_first_buoy_pair-4.0; + waypoint_to_approach_first_pair_base_link.x = + distance_to_first_buoy_pair - 4.0; waypoint_to_approach_first_pair_base_link.y = 0.0; waypoint_to_approach_first_pair_base_link.z = 0.0; try { @@ -47,7 +48,8 @@ void DockingTaskNode::main_task() { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } } - std::vector buoy_landmarks_0_to_1 = get_buoy_landmarks(predicted_first_pair); + std::vector buoy_landmarks_0_to_1 = + get_buoy_landmarks(predicted_first_pair); if (buoy_landmarks_0_to_1.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); } @@ -104,13 +106,13 @@ void DockingTaskNode::main_task() { (buoy_landmarks_2_to_5[2].pose_odom_frame.position.x + buoy_landmarks_2_to_5[3].pose_odom_frame.position.x) / 2; - waypoint_third_pair.y = (buoy_landmarks_2_to_5[2].pose_odom_frame.position.y + - buoy_landmarks_2_to_5[3].pose_odom_frame.position.y) / - 2; + waypoint_third_pair.y = + (buoy_landmarks_2_to_5[2].pose_odom_frame.position.y + + buoy_landmarks_2_to_5[3].pose_odom_frame.position.y) / + 2; waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); reach_waypoint(1.0); - } Eigen::Array DockingTaskNode::predict_first_buoy_pair() { @@ -140,10 +142,8 @@ Eigen::Array DockingTaskNode::predict_first_buoy_pair() { try { auto transform = tf_buffer_->lookupTransform( "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, - transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, - transform); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); transform_success = true; } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); @@ -159,8 +159,7 @@ Eigen::Array DockingTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array -DockingTaskNode::predict_second_buoy_pair( +Eigen::Array DockingTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; @@ -189,8 +188,8 @@ Eigen::Array DockingTaskNode::predict_third_buoy_pair( const geometry_msgs::msg::Point &buoy_2, const geometry_msgs::msg::Point &buoy_3) { Eigen::Vector2d direction_vector_first_to_second_pair; - direction_vector_first_to_second_pair << (buoy_2.x + buoy_3.x) / 2 - - (buoy_0.x + buoy_1.x) / 2, + direction_vector_first_to_second_pair + << (buoy_2.x + buoy_3.x) / 2 - (buoy_0.x + buoy_1.x) / 2, (buoy_2.y + buoy_3.y) / 2 - (buoy_0.y + buoy_1.y) / 2; direction_vector_first_to_second_pair.normalize(); @@ -199,10 +198,14 @@ Eigen::Array DockingTaskNode::predict_third_buoy_pair( predicted_positions(1, 0) = buoy_2.y; predicted_positions(0, 1) = buoy_3.x; predicted_positions(1, 1) = buoy_3.y; - predicted_positions(0, 2) = buoy_2.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 2) = buoy_2.y + direction_vector_first_to_second_pair(1) * 5; - predicted_positions(0, 3) = buoy_3.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 3) = buoy_3.y + direction_vector_first_to_second_pair(1) * 5; + predicted_positions(0, 2) = + buoy_2.x + direction_vector_first_to_second_pair(0) * 5; + predicted_positions(1, 2) = + buoy_2.y + direction_vector_first_to_second_pair(1) * 5; + predicted_positions(0, 3) = + buoy_3.x + direction_vector_first_to_second_pair(0) * 5; + predicted_positions(1, 3) = + buoy_3.y + direction_vector_first_to_second_pair(1) * 5; return predicted_positions; } @@ -214,7 +217,8 @@ void DockingTaskNode::initialize_grid_sub() { qos_profile_sensor_data); std::string grid_topic = this->get_parameter("grid_topic").as_string(); grid_sub_ = this->create_subscription( - grid_topic, qos_sensor_data, std::bind(&DockingTaskNode::grid_callback, this, std::placeholders::_1)); + grid_topic, qos_sensor_data, + std::bind(&DockingTaskNode::grid_callback, this, std::placeholders::_1)); } void DockingTaskNode::grid_callback( From 638f9986bfc865399e70f40193fb03f8545ac2ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Mon, 5 Aug 2024 10:18:44 +0200 Subject: [PATCH 37/67] Controller wont publish forces if the killswitch is on or system not in autonomous mode --- .../hybridpath_controller_node.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py index 86718344..21262565 100755 --- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py +++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py @@ -7,7 +7,7 @@ from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry from vortex_msgs.msg import HybridpathReference -from std_msgs.msg import Float64MultiArray +from std_msgs.msg import Float64MultiArray, String, Bool from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy from rcl_interfaces.msg import SetParametersResult @@ -28,10 +28,15 @@ def __init__(self): ]) self.parameters_updated = False + + self.killswitch_active = False + self.operational_mode = 'autonomous mode' self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, qos_profile=qos_profile) self.hpref_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.reference_callback, 1) self.wrench_publisher_ = self.create_publisher(Wrench, 'thrust/wrench_input', 1) + self.operational_mode_subscriber = self.create_subscription(String, 'softWareOperationMode', self.operation_mode_callback, 10) + self.killswitch_subscriber = self.create_subscription(Bool, 'softWareKillSwitch', self.killswitch_callback, 10) # Debug publishers self.eta_error_publisher = self.create_publisher(Float64MultiArray, 'eta_error', 10) @@ -52,6 +57,12 @@ def __init__(self): self.get_logger().info("hybridpath_controller_node started") + def operation_mode_callback(self, msg: String): + self.operational_mode = msg.data + + def killswitch_callback(self, msg: Bool): + self.killswitch_active = msg.data + def update_controller_parameters(self): K1_diag = self.get_parameter('hybridpath_controller.K1_diag').get_parameter_value().double_array_value @@ -96,6 +107,9 @@ def controller_callback(self): """ Callback function for the controller timer. This function calculates the control input and publishes the control input. """ + if self.killswitch_active or self.operational_mode != 'autonomous mode': + return + if self.parameters_updated: self.update_controller_parameters() self.parameters_updated = False From 591919a5cbcecbab5d41bb1d480f30a8e3897ab5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Mon, 5 Aug 2024 10:21:34 +0200 Subject: [PATCH 38/67] Added comminication with joystick and ability to set desired yaw angle and velocity. --- .../hybridpath_guidance_node.py | 80 ++++++++++++++----- 1 file changed, 60 insertions(+), 20 deletions(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index f97da507..04aa6fef 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -3,7 +3,7 @@ import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose2D, Point -from std_msgs.msg import Float32, Float64MultiArray +from std_msgs.msg import Float32, Float64MultiArray, String, Bool from vortex_msgs.msg import HybridpathReference from vortex_msgs.srv import Waypoint, DesiredVelocity from nav_msgs.msg import Odometry @@ -36,7 +36,10 @@ def __init__(self): self.w_publisher = self.create_publisher(Float32, 'w', 1) self.wp_arr_publisher = self.create_publisher(Float64MultiArray, 'waypoints', 1) self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) - + self.yaw_server = self.create_service(DesiredVelocity,'yaw_reference', self.yaw_ref_callback) + self.operational_mode_subscriber = self.create_subscription(String, 'softWareOperationMode', self.operation_mode_callback, 10) + self.killswitch_subscriber = self.create_subscription(Bool, 'softWareKillSwitch', self.killswitch_callback, 10) + # Get parameters self.lambda_val = self.get_parameter('hybridpath_guidance.lambda_val').get_parameter_value().double_value self.path_generator_order = self.get_parameter('hybridpath_guidance.path_generator_order').get_parameter_value().integer_value @@ -44,12 +47,18 @@ def __init__(self): self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value self.eta = np.zeros(3) - self.u_desired = 0.3 # Desired velocity + self.u_desired = 1.1 # Desired velocity + + self.yaw_ref = 0. # Desired heading, 100 if hybridpath heading, else can be set by service call self.waypoints = [] self.path = None self.s = 0. self.w = 0. + self.v_s = 0. + self.v_ss = 0. + self.operational_mode = 'autonomous mode' + self.killswitch_active = False # Initialize path generator self.generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val) @@ -62,6 +71,7 @@ def __init__(self): self.waiting_message_printed = False self.first_pos_flag = False self.eta_received = False + self.initial_pos = False # Timer for guidance timer_period = 0.1 @@ -69,12 +79,23 @@ def __init__(self): self.lock = threading.Lock() + def operation_mode_callback(self, msg: String): + self.operational_mode = msg.data + + def killswitch_callback(self, msg: Bool): + self.killswitch_active = msg.data + def u_desired_callback(self, request, response): self.u_desired = request.u_desired self.get_logger().info(f"Received desired velocity: {self.u_desired}") response.success = True return response + def yaw_ref_callback(self, request, response): + self.yaw_ref = request.u_desired # xd + + response.success = True + return response def waypoint_callback(self, request, response): @@ -113,28 +134,45 @@ def eta_callback(self, msg: Odometry): yaw_msg = Float32() self.eta = self.odom_to_eta(msg) self.yaw = float(self.eta[2]) + + if not self.initial_pos: + self.eta_initial = self.eta + self.yaw_ref = self.yaw + self.initial_pos = True + yaw_msg.data = self.yaw self.yaw_publisher.publish(yaw_msg) self.eta_received = True def guidance_callback(self): with self.lock: - if self.waypoints_received: - self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) - self.signals.update_s(self.s) - self.w = self.signals.get_w(self.mu, self.eta) + if self.killswitch_active or self.operational_mode != 'autonomous mode': + return + + if self.initial_pos: + if self.path is None and not self.waypoints_received: + pos = [self.eta_initial[0], self.eta_initial[1]] + pos_der = [0., 0.] + pos_dder = [0., 0.] + + else: + self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) + self.signals.update_s(self.s) + self.w = self.signals.get_w(self.mu, self.eta) + self.v_s = self.signals.get_vs(self.u_desired) + self.v_ss = self.signals.get_vs_derivative(self.u_desired) + + pos = self.signals.get_position() - pos = self.signals.get_position() + pos_der = self.signals.get_derivatives()[0] + pos_dder = self.signals.get_derivatives()[1] - if not self.first_pos_flag: - self.get_logger().info(f"First position: {pos}") - self.first_pos_flag = True + if self.yaw_ref == 100.: + psi = self.signals.get_heading() - # pos[0] = self.eta[0] - pos_der = self.signals.get_derivatives()[0] - pos_dder = self.signals.get_derivatives()[1] + else: + psi = self.yaw_ref - psi = 0. #signals.get_heading() psi_der = 0.#signals.get_heading_derivative() psi_dder = 0.#signals.get_heading_second_derivative() @@ -143,20 +181,22 @@ def guidance_callback(self): hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) - hp_msg.w = self.signals.get_w(self.mu, self.eta) - hp_msg.v_s = self.signals.get_vs(self.u_desired) - hp_msg.v_ss = self.signals.get_vs_derivative(self.u_desired) + hp_msg.w = self.w + hp_msg.v_s = self.v_s + hp_msg.v_ss = self.v_ss self.guidance_publisher.publish(hp_msg) - if self.s >= self.path.NumSubpaths: + if self.path is not None and self.s >= self.path.NumSubpaths: self.waypoints_received = False self.waiting_message_printed = False + self.path = None + self.eta_initial = self.eta self.get_logger().info('Last waypoint reached') else: if not self.waiting_message_printed: - self.get_logger().info('Waiting for waypoints to be received') + self.get_logger().info('Waiting for eta') self.waiting_message_printed = True s_msg = Float32() From 7dd07c78b338b1906364c2e0fb5dceaefb41d6a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Mon, 5 Aug 2024 10:22:00 +0200 Subject: [PATCH 39/67] Removed spam, initialized y-button --- .../joystick_interface/joystick_interface.py | 47 ++++++++++++++++--- 1 file changed, 41 insertions(+), 6 deletions(-) diff --git a/mission/joystick_interface/joystick_interface/joystick_interface.py b/mission/joystick_interface/joystick_interface/joystick_interface.py index 306bc82c..bc8c52b0 100755 --- a/mission/joystick_interface/joystick_interface/joystick_interface.py +++ b/mission/joystick_interface/joystick_interface/joystick_interface.py @@ -79,6 +79,9 @@ def __init__(self): self.last_button_press_time_ = 0 self.debounce_duration_ = 0.25 self.state_ = States.NO_GO + self.previous_state_ = None + + self.mode_logger_done_ = False self.joystick_buttons_map_ = [] @@ -160,8 +163,12 @@ def transition_to_xbox_mode(self): """ Turns off the controller and signals that the operational mode has switched to Xbox mode. """ - self.operational_mode_signal_publisher_.publish(String(data="XBOX")) + msg = String() + msg.data = "XBOX" + self.operational_mode_signal_publisher_.publish(msg) self.state_ = States.XBOX_MODE + self.previous_state_ = States.XBOX_MODE + self.mode_logger_done_ = False def transition_to_autonomous_mode(self): """ @@ -169,8 +176,19 @@ def transition_to_autonomous_mode(self): """ wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0) self.wrench_publisher_.publish(wrench_msg) - self.operational_mode_signal_publisher_.publish(String(data="autonomous mode")) + msg = String() + msg.data = "autonomous mode" + self.operational_mode_signal_publisher_.publish(msg) self.state_ = States.AUTONOMOUS_MODE + self.previous_state_ = States.AUTONOMOUS_MODE + self.mode_logger_done_ = False + + def set_home_position(self): + """ + Calls a service that communicates to the guidance system to set the current + position as the home position. + """ + pass def joystick_cb(self, msg : Joy) -> Wrench: """ @@ -217,6 +235,7 @@ def joystick_cb(self, msg : Joy) -> Wrench: xbox_control_mode_button = buttons.get("A", 0) software_killswitch_button = buttons.get("B", 0) software_control_mode_button = buttons.get("X", 0) + software_set_home_button = buttons.get("Y", 0) left_trigger = self.left_trigger_linear_converter(axes.get('LT', 0.0)) right_trigger = self.right_trigger_linear_converter(axes.get('RT', 0.0)) @@ -240,7 +259,12 @@ def joystick_cb(self, msg : Joy) -> Wrench: # signal that killswitch is not blocking self.software_killswitch_signal_publisher_.publish( Bool(data=False)) - self.transition_to_xbox_mode() + + if self.previous_state_ == States.XBOX_MODE: + self.transition_to_xbox_mode() + + else: + self.transition_to_autonomous_mode() return else: @@ -257,18 +281,29 @@ def joystick_cb(self, msg : Joy) -> Wrench: wrench_msg = self.create_2d_wrench_message(surge, sway, yaw) if self.state_ == States.XBOX_MODE: - self.get_logger().info("XBOX mode", throttle_duration_sec=1) + if not self.mode_logger_done_: + self.get_logger().info("XBOX mode") + self.mode_logger_done_ = True + self.wrench_publisher_.publish(wrench_msg) if software_control_mode_button: self.transition_to_autonomous_mode() - if self.state_ == States.AUTONOMOUS_MODE: - self.get_logger().info("autonomous mode", throttle_duration_sec=1) + elif self.state_ == States.AUTONOMOUS_MODE: + if not self.mode_logger_done_: + self.get_logger().info("autonomous mode") + self.mode_logger_done_ = True if xbox_control_mode_button: self.transition_to_xbox_mode() + else: + if not self.mode_logger_done_: + self.get_logger().info("Killswitch is active") + self.mode_logger_done_ = True + return + return wrench_msg From bced81a74bb065b1d590df3a9f78519114536a37 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Mon, 5 Aug 2024 15:56:26 +0200 Subject: [PATCH 40/67] Raised max and min thrust --- asv_setup/config/robots/freya.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml index 19c59c15..77281efc 100644 --- a/asv_setup/config/robots/freya.yaml +++ b/asv_setup/config/robots/freya.yaml @@ -34,8 +34,8 @@ yaw: true thrusters: num: 4 - min: -200 - max: 200 + min: -1000 + max: 1000 configuration_matrix: #NED [0.70711, 0.70711, 0.70711, 0.70711, -0.70711, 0.70711, -0.70711, 0.70711, From cfa6963f7d1800e1cb977a3bf478edcf2097bb9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Mon, 5 Aug 2024 15:56:51 +0200 Subject: [PATCH 41/67] Minor adjustments to heading tuning --- .../config/hybridpath_controller_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml index f01ea966..d8a91934 100644 --- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml +++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: hybridpath_controller: - K1_diag: [1., 1., 1.] # First gain matrix - K2_diag: [40.0, 30.0, 20.0] # Second gain matrix \ No newline at end of file + K1_diag: [1., 1., 5.] # First gain matrix + K2_diag: [40.0, 30.0, 40.0] # Second gain matrix \ No newline at end of file From 783168d8a7981a8219493dfc3ce57dce42b40431 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Mon, 5 Aug 2024 15:57:30 +0200 Subject: [PATCH 42/67] improved logging and behaviour for stationkeeping --- .../hybridpath_guidance_node.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 04aa6fef..cd0df956 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -69,6 +69,7 @@ def __init__(self): # Flags for logging self.waypoints_received = False self.waiting_message_printed = False + self.stationkeeping_flag = False self.first_pos_flag = False self.eta_received = False self.initial_pos = False @@ -94,6 +95,8 @@ def u_desired_callback(self, request, response): def yaw_ref_callback(self, request, response): self.yaw_ref = request.u_desired # xd + self.get_logger().info(f"Received desired heading: {self.yaw_ref}") + response.success = True return response @@ -108,8 +111,11 @@ def waypoint_callback(self, request, response): new_waypoints = request.waypoint for point in new_waypoints: + self.waypoints.append(point) + self.last_waypoint = [self.waypoints[-1].x, self.waypoints[-1].y] + self.generator.create_path(self.waypoints) self.path = self.generator.get_path() @@ -136,7 +142,7 @@ def eta_callback(self, msg: Odometry): self.yaw = float(self.eta[2]) if not self.initial_pos: - self.eta_initial = self.eta + self.eta_stationkeeping = self.eta self.yaw_ref = self.yaw self.initial_pos = True @@ -151,7 +157,11 @@ def guidance_callback(self): if self.initial_pos: if self.path is None and not self.waypoints_received: - pos = [self.eta_initial[0], self.eta_initial[1]] + if not self.stationkeeping_flag: + self.get_logger().info(f'No waypoints received, stationkeeping at {np.round(self.eta_stationkeeping, 3)}') + self.stationkeeping_flag = True + + pos = [self.eta_stationkeeping[0], self.eta_stationkeeping[1]] pos_der = [0., 0.] pos_dder = [0., 0.] @@ -190,9 +200,9 @@ def guidance_callback(self): if self.path is not None and self.s >= self.path.NumSubpaths: self.waypoints_received = False self.waiting_message_printed = False + self.stationkeeping_flag = False self.path = None - self.eta_initial = self.eta - self.get_logger().info('Last waypoint reached') + self.eta_stationkeeping = np.array([self.last_waypoint[0], self.last_waypoint[1], self.yaw_ref]) else: if not self.waiting_message_printed: From 6f6d8fc0acf643a6867442a82ffb660ddb83fd9c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Mon, 5 Aug 2024 18:00:32 +0200 Subject: [PATCH 43/67] Added ability to set stationkeeping pose by pressing Y --- guidance/hybridpath_guidance/CMakeLists.txt | 1 + .../hybridpath_guidance_node.py | 14 ++++++++++ mission/joystick_interface/CMakeLists.txt | 1 + .../joystick_interface/joystick_interface.py | 27 ++++++++++++++++--- 4 files changed, 39 insertions(+), 4 deletions(-) diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt index 8b4fd5ac..c85f4eea 100644 --- a/guidance/hybridpath_guidance/CMakeLists.txt +++ b/guidance/hybridpath_guidance/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake_python REQUIRED) find_package(rclpy REQUIRED) find_package(vortex_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_srvs REQUIRED) ament_python_install_package(${PROJECT_NAME}) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index cd0df956..e0686077 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -11,6 +11,7 @@ from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy import threading +from std_srvs.srv import Empty qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, @@ -40,6 +41,8 @@ def __init__(self): self.operational_mode_subscriber = self.create_subscription(String, 'softWareOperationMode', self.operation_mode_callback, 10) self.killswitch_subscriber = self.create_subscription(Bool, 'softWareKillSwitch', self.killswitch_callback, 10) + self.set_stationkeeping_pose_service = self.create_service(Empty, 'set_stationkeeping_pose', self.set_stationkeeping_pose_callback) + # Get parameters self.lambda_val = self.get_parameter('hybridpath_guidance.lambda_val').get_parameter_value().double_value self.path_generator_order = self.get_parameter('hybridpath_guidance.path_generator_order').get_parameter_value().integer_value @@ -98,6 +101,7 @@ def yaw_ref_callback(self, request, response): self.get_logger().info(f"Received desired heading: {self.yaw_ref}") response.success = True + return response def waypoint_callback(self, request, response): @@ -150,6 +154,16 @@ def eta_callback(self, msg: Odometry): self.yaw_publisher.publish(yaw_msg) self.eta_received = True + def set_stationkeeping_pose_callback(self, request, response): + if self.eta_received: + self.eta_stationkeeping = self.eta + self.get_logger().info(f'Set stationkeeping pose to {self.eta_stationkeeping}') + + else: + self.get_logger().info('No eta received, cannot set stationkeeping pose') + + return response + def guidance_callback(self): with self.lock: if self.killswitch_active or self.operational_mode != 'autonomous mode': diff --git a/mission/joystick_interface/CMakeLists.txt b/mission/joystick_interface/CMakeLists.txt index f27ffc20..c4bcc36e 100755 --- a/mission/joystick_interface/CMakeLists.txt +++ b/mission/joystick_interface/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(ament_cmake_python REQUIRED) find_package(rclpy REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_srvs REQUIRED) ament_python_install_package(joystick_interface) diff --git a/mission/joystick_interface/joystick_interface/joystick_interface.py b/mission/joystick_interface/joystick_interface/joystick_interface.py index bc8c52b0..4aa2584d 100755 --- a/mission/joystick_interface/joystick_interface/joystick_interface.py +++ b/mission/joystick_interface/joystick_interface/joystick_interface.py @@ -5,6 +5,7 @@ from sensor_msgs.msg import Joy from std_msgs.msg import Bool from std_msgs.msg import String +from std_srvs.srv import Empty class States: @@ -92,6 +93,8 @@ def __init__(self): self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) + + self.set_stationkeeping_pose_client = self.create_client(Empty, 'set_stationkeeping_pose') self.declare_parameter('surge_scale_factor', 50.0) self.declare_parameter('sway_scale_factor', 50.0) @@ -183,12 +186,24 @@ def transition_to_autonomous_mode(self): self.previous_state_ = States.AUTONOMOUS_MODE self.mode_logger_done_ = False - def set_home_position(self): + def set_stationkeeping_pose(self): """ Calls a service that communicates to the guidance system to set the current - position as the home position. + position as the stationkeeping position. """ - pass + request = Empty.Request() + future = self.set_stationkeeping_pose_client.call_async(request) + future.add_done_callback(self.set_stationkeeping_pose_callback) + + def set_stationkeeping_pose_callback(self, future): + """ + Callback function for the set_stationkeeping_pose service. + """ + try: + response = future.result() + except Exception as e: + self.get_logger().info( + f"Service call failed {str(e)}") def joystick_cb(self, msg : Joy) -> Wrench: """ @@ -248,9 +263,10 @@ def joystick_cb(self, msg : Joy) -> Wrench: software_control_mode_button = False xbox_control_mode_button = False software_killswitch_button = False + software_set_home_button = False # If any button is pressed, update the last button press time - if software_control_mode_button or xbox_control_mode_button or software_killswitch_button: + if software_control_mode_button or xbox_control_mode_button or software_killswitch_button or software_set_home_button: self.last_button_press_time_ = current_time # Toggle ks on and off @@ -290,6 +306,9 @@ def joystick_cb(self, msg : Joy) -> Wrench: if software_control_mode_button: self.transition_to_autonomous_mode() + if software_set_home_button: + self.set_stationkeeping_pose() + elif self.state_ == States.AUTONOMOUS_MODE: if not self.mode_logger_done_: self.get_logger().info("autonomous mode") From b461023de5e5bf43ec6d4497487ae9960778c10d Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Mon, 5 Aug 2024 19:09:14 +0200 Subject: [PATCH 44/67] visualization fix --- .../sitaw/land_polygon_sim_navigation.yaml | 5 + .../config/sitaw/map_manager_params.yaml | 11 +- .../njord_tasks/docking_task/CMakeLists.txt | 4 + .../include/docking_task/docking_task_ros.hpp | 12 +- mission/njord_tasks/docking_task/package.xml | 1 + .../params/docking_task_params.yaml | 10 +- .../docking_task/src/docking_task_ros.cpp | 225 ++++-- .../maneuvering_task/CMakeLists.txt | 4 + .../maneuvering_task/maneuvering_task_ros.hpp | 14 +- .../njord_tasks/maneuvering_task/package.xml | 1 + .../src/maneuvering_task_ros.cpp | 317 ++++++-- .../navigation_task/CMakeLists.txt | 5 + .../navigation_task/navigation_task_ros.hpp | 49 +- .../njord_tasks/navigation_task/package.xml | 1 + .../params/navigation_task_params.yaml | 18 +- .../src/navigation_task_ros.cpp | 676 ++++++++++++++---- .../njord_task_base/njord_task_base_ros.hpp | 9 + .../src/njord_task_base_ros.cpp | 39 +- 18 files changed, 1083 insertions(+), 318 deletions(-) create mode 100644 asv_setup/config/sitaw/land_polygon_sim_navigation.yaml diff --git a/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml b/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml new file mode 100644 index 00000000..63c9117a --- /dev/null +++ b/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml @@ -0,0 +1,5 @@ +-33.722576743831986 150.67377683501326 +-33.72213785615945 150.6737273159938 +-33.722119332734295 150.6744244880215 +-33.72265441348879 150.6743192836287 + \ No newline at end of file diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml index b23c80e3..036e3b2a 100644 --- a/asv_setup/config/sitaw/map_manager_params.yaml +++ b/asv_setup/config/sitaw/map_manager_params.yaml @@ -1,7 +1,8 @@ map_manager_node: ros__parameters: - use_predef_landmask: false - landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml" + use_predef_landmask: true + landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml" + # landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml" map_resolution: 0.2 map_width: 1000 map_height: 1000 @@ -10,6 +11,6 @@ map_manager_node: # map_origin_lat: 63.414660884931976 # map_origin_lon: 10.398554661537544 # use_predef_map_origin: true - map_origin_lat: 0.0 - map_origin_lon: 0.0 - use_predef_map_origin: false \ No newline at end of file + map_origin_lat: -33.72242824301795 + map_origin_lon: 150.6740063854522 + use_predef_map_origin: true \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/CMakeLists.txt b/mission/njord_tasks/docking_task/CMakeLists.txt index c9298cc7..2ecfb3b2 100644 --- a/mission/njord_tasks/docking_task/CMakeLists.txt +++ b/mission/njord_tasks/docking_task/CMakeLists.txt @@ -27,6 +27,8 @@ find_package(vortex_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) include_directories(include) @@ -38,6 +40,7 @@ target_link_libraries(docking_task_node tf2::tf2 tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs + pcl_common ) ament_target_dependencies(docking_task_node @@ -50,6 +53,7 @@ ament_target_dependencies(docking_task_node tf2 tf2_ros tf2_geometry_msgs + pcl_conversions ) install( diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp index 7e3ac3c7..ebd943d0 100644 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -4,10 +4,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace docking_task { @@ -41,11 +41,11 @@ class DockingTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); - Eigen::Array + Eigen::Array predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); - Eigen::Array + Eigen::Array predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, const geometry_msgs::msg::Point &buoy_2, diff --git a/mission/njord_tasks/docking_task/package.xml b/mission/njord_tasks/docking_task/package.xml index e9d0a452..e3dc286c 100644 --- a/mission/njord_tasks/docking_task/package.xml +++ b/mission/njord_tasks/docking_task/package.xml @@ -21,6 +21,7 @@ tf2 tf2_ros tf2_geometry_msgs + pcl_conversions diff --git a/mission/njord_tasks/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml index 919a27a6..4b675182 100644 --- a/mission/njord_tasks/docking_task/params/docking_task_params.yaml +++ b/mission/njord_tasks/docking_task/params/docking_task_params.yaml @@ -1,10 +1,12 @@ docking_task_node: ros__parameters: - map_origin_lat: 63.4411585867919 - map_origin_lon: 10.419400373255625 + # map_origin_lat: 63.4411585867919 + # map_origin_lon: 10.419400373255625 + map_origin_lat: -33.72242824301795 + map_origin_lon: 150.6740063854522 map_origin_set: true - gps_start_lat: 63.44097054434422 - gps_start_lon: 10.419997767413607 + gps_start_lat: -33.72255346763595 + gps_start_lon: 150.67394210459224 gps_end_lat: 63.44125901804796 gps_end_lon: 10.41857835889424 gps_start_x: 0.0 diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index e524e674..0a15d3fd 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -19,7 +19,6 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) declare_parameter("models.dynmod_stddev", 0.0); declare_parameter("models.sen_stddev", 0.0); - initialize_grid_sub(); std::thread(&DockingTaskNode::main_task, this).detach(); } @@ -27,7 +26,32 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) void DockingTaskNode::main_task() { navigation_ready(); // Starting docking task + odom_start_point_ = get_odom()->pose.pose.position; Eigen::Array predicted_first_pair = predict_first_buoy_pair(); + + sensor_msgs::msg::PointCloud2 buoy_vis_msg; + pcl::PointCloud buoy_vis; + pcl::PointXYZRGB buoy_red_0; + buoy_red_0.x = predicted_first_pair(0, 0); + buoy_red_0.y = predicted_first_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + pcl::PointXYZRGB buoy_green_1; + buoy_green_1.x = predicted_first_pair(0, 1); + buoy_green_1.y = predicted_first_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); if (distance_to_first_buoy_pair > 6.0) { @@ -38,11 +62,13 @@ void DockingTaskNode::main_task() { waypoint_to_approach_first_pair_base_link.z = 0.0; try { auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); + "odom", "base_link", tf2::TimePointZero); geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; tf2::doTransform(waypoint_to_approach_first_pair_base_link, waypoint_to_approach_first_pair_odom, transform); send_waypoint(waypoint_to_approach_first_pair_odom); + set_desired_heading(odom_start_point_, + waypoint_to_approach_first_pair_odom); reach_waypoint(1.0); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); @@ -53,6 +79,30 @@ void DockingTaskNode::main_task() { if (buoy_landmarks_0_to_1.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; + buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; + buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_first_pair; waypoint_first_pair.x = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + @@ -64,55 +114,156 @@ void DockingTaskNode::main_task() { 2; waypoint_first_pair.z = 0.0; send_waypoint(waypoint_first_pair); + set_desired_heading(odom_start_point_, waypoint_first_pair); reach_waypoint(1.0); // Second pair of buoys - Eigen::Array predicted_first_and_second_pair = + Eigen::Array predicted_first_and_second_pair = predict_second_buoy_pair( buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position); - std::vector buoy_landmarks_0_to_3 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_first_and_second_pair(0, 0); + buoy_red_0.y = predicted_first_and_second_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_first_and_second_pair(0, 1); + buoy_green_1.y = predicted_first_and_second_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_2_to_3 = get_buoy_landmarks(predicted_first_and_second_pair); - if (buoy_landmarks_0_to_3.size() != 4) { + if (buoy_landmarks_2_to_3.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; + buoy_red_0.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; + buoy_green_1.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_second_pair; waypoint_second_pair.x = - (buoy_landmarks_0_to_3[2].pose_odom_frame.position.x + - buoy_landmarks_0_to_3[3].pose_odom_frame.position.x) / + (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + + buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / 2; waypoint_second_pair.y = - (buoy_landmarks_0_to_3[2].pose_odom_frame.position.y + - buoy_landmarks_0_to_3[3].pose_odom_frame.position.y) / + (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + + buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / 2; waypoint_second_pair.z = 0.0; send_waypoint(waypoint_second_pair); + set_desired_heading(odom_start_point_, waypoint_second_pair); reach_waypoint(1.0); // Third pair of buoys - Eigen::Array predicted_second_and_third_pair = + Eigen::Array predicted_third_pair = predict_third_buoy_pair( buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position, - buoy_landmarks_0_to_3[2].pose_odom_frame.position, - buoy_landmarks_0_to_3[3].pose_odom_frame.position); - std::vector buoy_landmarks_2_to_5 = - get_buoy_landmarks(predicted_second_and_third_pair); - if (buoy_landmarks_2_to_5.size() != 4) { + buoy_landmarks_2_to_3[2].pose_odom_frame.position, + buoy_landmarks_2_to_3[3].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_third_pair(0, 0); + buoy_red_0.y = predicted_third_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_third_pair(0, 1); + buoy_green_1.y = predicted_third_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_4_to_5 = + get_buoy_landmarks(predicted_third_pair); + if (buoy_landmarks_4_to_5.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = buoy_landmarks_4_to_5[0].pose_odom_frame.position.x; + buoy_red_0.y = buoy_landmarks_4_to_5[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = buoy_landmarks_4_to_5[1].pose_odom_frame.position.x; + buoy_green_1.y = buoy_landmarks_4_to_5[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_third_pair; waypoint_third_pair.x = - (buoy_landmarks_2_to_5[2].pose_odom_frame.position.x + - buoy_landmarks_2_to_5[3].pose_odom_frame.position.x) / + (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + + buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / 2; waypoint_third_pair.y = - (buoy_landmarks_2_to_5[2].pose_odom_frame.position.y + - buoy_landmarks_2_to_5[3].pose_odom_frame.position.y) / + (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + + buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / 2; waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); + set_desired_heading(odom_start_point_, waypoint_third_pair); reach_waypoint(1.0); + + std::thread(&DockingTaskNode::initialize_grid_sub, this).detach(); } Eigen::Array DockingTaskNode::predict_first_buoy_pair() { @@ -159,30 +310,26 @@ Eigen::Array DockingTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array DockingTaskNode::predict_second_buoy_pair( +Eigen::Array DockingTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; direction_vector << previous_waypoint_odom_frame_.x - - this->get_parameter("gps_start_x").as_double(), + odom_start_point_.x, previous_waypoint_odom_frame_.y - - this->get_parameter("gps_start_y").as_double(); + odom_start_point_.y; direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x; - predicted_positions(1, 0) = buoy_0.y; - predicted_positions(0, 1) = buoy_1.x; - predicted_positions(1, 1) = buoy_1.y; - predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 5; - predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 5; - predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 5; - predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 5; + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 5; + predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 5; + predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 5; + predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 5; return predicted_positions; } -Eigen::Array DockingTaskNode::predict_third_buoy_pair( +Eigen::Array DockingTaskNode::predict_third_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, const geometry_msgs::msg::Point &buoy_2, @@ -193,18 +340,14 @@ Eigen::Array DockingTaskNode::predict_third_buoy_pair( (buoy_2.y + buoy_3.y) / 2 - (buoy_0.y + buoy_1.y) / 2; direction_vector_first_to_second_pair.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_2.x; - predicted_positions(1, 0) = buoy_2.y; - predicted_positions(0, 1) = buoy_3.x; - predicted_positions(1, 1) = buoy_3.y; - predicted_positions(0, 2) = + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_2.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 2) = + predicted_positions(1, 0) = buoy_2.y + direction_vector_first_to_second_pair(1) * 5; - predicted_positions(0, 3) = + predicted_positions(0, 1) = buoy_3.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 3) = + predicted_positions(1, 1) = buoy_3.y + direction_vector_first_to_second_pair(1) * 5; return predicted_positions; diff --git a/mission/njord_tasks/maneuvering_task/CMakeLists.txt b/mission/njord_tasks/maneuvering_task/CMakeLists.txt index c5027532..6f3e7a2a 100644 --- a/mission/njord_tasks/maneuvering_task/CMakeLists.txt +++ b/mission/njord_tasks/maneuvering_task/CMakeLists.txt @@ -27,6 +27,8 @@ find_package(vortex_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) include_directories(include) @@ -38,6 +40,7 @@ target_link_libraries(maneuvering_task_node tf2::tf2 tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs + pcl_common ) ament_target_dependencies(maneuvering_task_node @@ -50,6 +53,7 @@ ament_target_dependencies(maneuvering_task_node tf2_ros tf2_geometry_msgs njord_task_base + pcl_conversions ) install( diff --git a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp index 45423aa0..731d5920 100644 --- a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp +++ b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp @@ -2,6 +2,10 @@ #define MANEUVERING_TASK_ROS_HPP #include +#include +#include +#include +#include namespace maneuvering_task { @@ -19,17 +23,11 @@ class ManeuveringTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); - Eigen::Array + Eigen::Array predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); - Eigen::Array - predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); - - Eigen::Array + Eigen::Array predict_next_pair_in_formation(const geometry_msgs::msg::Point &buoy_red, const geometry_msgs::msg::Point &buoy_green, Eigen::Vector2d direction_vector); diff --git a/mission/njord_tasks/maneuvering_task/package.xml b/mission/njord_tasks/maneuvering_task/package.xml index 52812f8d..4703ead8 100644 --- a/mission/njord_tasks/maneuvering_task/package.xml +++ b/mission/njord_tasks/maneuvering_task/package.xml @@ -21,6 +21,7 @@ tf2 tf2_ros tf2_geometry_msgs + pcl_conversions diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index e5f1a0aa..a1106fb3 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -15,16 +15,39 @@ ManeuveringTaskNode::ManeuveringTaskNode(const rclcpp::NodeOptions &options) void ManeuveringTaskNode::main_task() { navigation_ready(); RCLCPP_INFO(this->get_logger(), "Maneuvering task started"); + odom_start_point_ = get_odom()->pose.pose.position; // First buoy pair Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); + + sensor_msgs::msg::PointCloud2 buoy_vis_msg; + pcl::PointCloud buoy_vis; + pcl::PointXYZRGB buoy_red_0; + buoy_red_0.x = predicted_first_buoy_pair(0, 0); + buoy_red_0.y = predicted_first_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + pcl::PointXYZRGB buoy_green_1; + buoy_green_1.x = predicted_first_buoy_pair(0, 1); + buoy_green_1.y = predicted_first_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + // First first buoy pair is far away, should be closer before trying to // measure it. - double gps_start_x = this->get_parameter("gps_start_x").as_double(); - double gps_start_y = this->get_parameter("gps_start_y").as_double(); double gps_end_x = this->get_parameter("gps_end_x").as_double(); double gps_end_y = this->get_parameter("gps_end_y").as_double(); Eigen::Vector2d direction_vector_to_end; - direction_vector_to_end << gps_end_x - gps_start_x, gps_end_y - gps_start_y; + direction_vector_to_end << gps_end_x - odom_start_point_.x, gps_end_y - odom_start_point_.y; direction_vector_to_end.normalize(); double distance = @@ -43,6 +66,30 @@ void ManeuveringTaskNode::main_task() { std::vector measured_first_buoy_pair = get_buoy_landmarks(predicted_first_buoy_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_first_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_first_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_first_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_first_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_first_pair; waypoint_first_pair.x = (measured_first_buoy_pair[0].pose_odom_frame.position.x + @@ -54,6 +101,7 @@ void ManeuveringTaskNode::main_task() { 2; waypoint_first_pair.z = 0.0; send_waypoint(waypoint_first_pair); + set_desired_heading(odom_start_point_, waypoint_first_pair); reach_waypoint(0.5); // Second buoy pair is FAR away, should be closer before trying to measure it. @@ -64,51 +112,91 @@ void ManeuveringTaskNode::main_task() { waypoint_first_pair.y + direction_vector_to_end(1) * 15; waypoint_approach_formation.z = 0.0; send_waypoint(waypoint_approach_formation); + set_desired_heading(waypoint_first_pair, waypoint_approach_formation); reach_waypoint(1.0); // Second buoy pair - Eigen::Array predicted_second_buoy_pair = + Eigen::Array predicted_second_buoy_pair = predict_second_buoy_pair( measured_first_buoy_pair[0].pose_odom_frame.position, measured_first_buoy_pair[1].pose_odom_frame.position); - std::vector measured_buoy_0_to_3 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_second_buoy_pair(0, 0); + buoy_red_0.y = predicted_second_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_second_buoy_pair(0, 1); + buoy_green_1.y = predicted_second_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector measured_buoy_2_to_3 = get_buoy_landmarks(predicted_second_buoy_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_buoy_2_to_3[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_buoy_2_to_3[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_buoy_2_to_3[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_buoy_2_to_3[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_second_pair; waypoint_second_pair.x = - (measured_buoy_0_to_3[2].pose_odom_frame.position.x + - measured_buoy_0_to_3[3].pose_odom_frame.position.x) / + (measured_buoy_2_to_3[0].pose_odom_frame.position.x + + measured_buoy_2_to_3[1].pose_odom_frame.position.x) / 2; waypoint_second_pair.y = - (measured_buoy_0_to_3[2].pose_odom_frame.position.y + - measured_buoy_0_to_3[3].pose_odom_frame.position.y) / + (measured_buoy_2_to_3[0].pose_odom_frame.position.y + + measured_buoy_2_to_3[1].pose_odom_frame.position.y) / 2; waypoint_second_pair.z = 0.0; send_waypoint(waypoint_second_pair); + set_desired_heading(waypoint_approach_formation, waypoint_second_pair); reach_waypoint(0.5); // Setup variable to navigate formation Eigen::Vector2d direction_vector_forwards; direction_vector_forwards - << (measured_buoy_0_to_3[2].pose_odom_frame.position.x + - measured_buoy_0_to_3[3].pose_odom_frame.position.x) / - 2 - - (measured_buoy_0_to_3[0].pose_odom_frame.position.x + - measured_buoy_0_to_3[1].pose_odom_frame.position.x) / - 2, - (measured_buoy_0_to_3[2].pose_odom_frame.position.y + - measured_buoy_0_to_3[3].pose_odom_frame.position.y) / - 2 - - (measured_buoy_0_to_3[0].pose_odom_frame.position.y + - measured_buoy_0_to_3[1].pose_odom_frame.position.y) / - 2; + << (waypoint_second_pair.x - waypoint_first_pair.x), + (waypoint_second_pair.y - waypoint_first_pair.y); direction_vector_forwards.normalize(); Eigen::Vector2d direction_vector_downwards; direction_vector_downwards - << measured_buoy_0_to_3[3].pose_odom_frame.position.x - - measured_buoy_0_to_3[2].pose_odom_frame.position.x, - measured_buoy_0_to_3[3].pose_odom_frame.position.y - - measured_buoy_0_to_3[2].pose_odom_frame.position.y; + << measured_buoy_2_to_3[1].pose_odom_frame.position.x - + measured_buoy_2_to_3[0].pose_odom_frame.position.x, + measured_buoy_2_to_3[1].pose_odom_frame.position.y - + measured_buoy_2_to_3[0].pose_odom_frame.position.y; direction_vector_downwards.normalize(); Eigen::Vector2d vector_to_next_pair; @@ -121,46 +209,86 @@ void ManeuveringTaskNode::main_task() { this->get_parameter("initial_offset").as_double(); geometry_msgs::msg::Point red_buoy = - measured_buoy_0_to_3[2].pose_odom_frame.position; + measured_buoy_2_to_3[0].pose_odom_frame.position; geometry_msgs::msg::Point green_buoy = - measured_buoy_0_to_3[3].pose_odom_frame.position; - + measured_buoy_2_to_3[1].pose_odom_frame.position; + geometry_msgs::msg::Point waypoint_prev_pair = waypoint_second_pair; // ASV is at first buoy pair in formation. // Run loop for the rest of the formation excluding the first pair. int num_iterations = this->get_parameter("nr_of_pair_in_formation").as_int() - 1; for (int _ = 0; _ < num_iterations; ++_) { - Eigen::Array predicted_next_pair = + Eigen::Array predicted_next_pair = predict_next_pair_in_formation(red_buoy, green_buoy, vector_to_next_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_next_pair(0, 0); + buoy_red_0.y = predicted_next_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_next_pair(0, 1); + buoy_green_1.y = predicted_next_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + std::vector measured_next_pair = get_buoy_landmarks(predicted_next_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_next_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_next_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_next_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_next_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_next_pair; - waypoint_next_pair.x = (measured_next_pair[2].pose_odom_frame.position.x + - measured_next_pair[3].pose_odom_frame.position.x) / + waypoint_next_pair.x = (measured_next_pair[0].pose_odom_frame.position.x + + measured_next_pair[1].pose_odom_frame.position.x) / 2; - waypoint_next_pair.y = (measured_next_pair[2].pose_odom_frame.position.y + - measured_next_pair[3].pose_odom_frame.position.y) / + waypoint_next_pair.y = (measured_next_pair[0].pose_odom_frame.position.y + + measured_next_pair[1].pose_odom_frame.position.y) / 2; waypoint_next_pair.z = 0.0; send_waypoint(waypoint_next_pair); + set_desired_heading(waypoint_prev_pair, waypoint_next_pair); reach_waypoint(1.0); - red_buoy = measured_next_pair[2].pose_odom_frame.position; - green_buoy = measured_next_pair[3].pose_odom_frame.position; + red_buoy = measured_next_pair[0].pose_odom_frame.position; + green_buoy = measured_next_pair[1].pose_odom_frame.position; vector_to_next_pair - << (measured_next_pair[2].pose_odom_frame.position.x + - measured_next_pair[3].pose_odom_frame.position.x) / - 2 - - (measured_next_pair[1].pose_odom_frame.position.x + - measured_next_pair[0].pose_odom_frame.position.x) / - 2, - (measured_next_pair[2].pose_odom_frame.position.y + - measured_next_pair[3].pose_odom_frame.position.y) / - 2 - - (measured_next_pair[1].pose_odom_frame.position.y + - measured_next_pair[0].pose_odom_frame.position.y) / - 2; + << waypoint_next_pair.x - waypoint_prev_pair.x, + waypoint_next_pair.y - waypoint_prev_pair.y; + waypoint_prev_pair = waypoint_next_pair; } // ASV is now at the last pair of the buoy formation @@ -169,17 +297,17 @@ void ManeuveringTaskNode::main_task() { double distance_to_end = std::sqrt(std::pow(odom->pose.pose.position.x - gps_end_x, 2) + std::pow(odom->pose.pose.position.y - gps_end_y, 2)); - if (distance_to_end > 6.0) { - auto odom = get_odom(); + if (distance_to_end > 7.0) { geometry_msgs::msg::Point waypoint_toward_end; waypoint_toward_end.x = odom->pose.pose.position.x + - direction_vector_to_end(0) * (distance_to_end - 3.0); + direction_vector_to_end(0) * (distance_to_end - 4.0); waypoint_toward_end.y = odom->pose.pose.position.y + - direction_vector_to_end(1) * (distance_to_end - 3.0); + direction_vector_to_end(1) * (distance_to_end - 4.0); waypoint_toward_end.z = 0.0; send_waypoint(waypoint_toward_end); + set_desired_heading(waypoint_prev_pair, waypoint_toward_end); reach_waypoint(2.0); } Eigen::Array predicted_last_buoy_pair; @@ -191,8 +319,56 @@ void ManeuveringTaskNode::main_task() { gps_end_x + direction_vector_downwards(0) * 2.5; predicted_last_buoy_pair(1, 1) = gps_end_y + direction_vector_downwards(1) * 2.5; + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_last_buoy_pair(0, 0); + buoy_red_0.y = predicted_last_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_last_buoy_pair(0, 1); + buoy_green_1.y = predicted_last_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + std::vector measured_last_buoy_pair = get_buoy_landmarks(predicted_last_buoy_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_last_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_last_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_last_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_last_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_last_pair; waypoint_last_pair.x = (measured_last_buoy_pair[0].pose_odom_frame.position.x + @@ -204,6 +380,8 @@ void ManeuveringTaskNode::main_task() { 2; waypoint_last_pair.z = 0.0; send_waypoint(waypoint_last_pair); + set_desired_heading(waypoint_prev_pair, waypoint_last_pair); + reach_waypoint(1.0); // Also send waypoint 2m through the last pair geometry_msgs::msg::Point waypoint_through_last_pair; waypoint_through_last_pair.x = @@ -212,6 +390,7 @@ void ManeuveringTaskNode::main_task() { waypoint_last_pair.y + direction_vector_to_end(1) * 2; waypoint_through_last_pair.z = 0.0; send_waypoint(waypoint_through_last_pair); + set_desired_heading(waypoint_last_pair, waypoint_through_last_pair); // Task is now finished } @@ -257,42 +436,34 @@ Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array ManeuveringTaskNode::predict_second_buoy_pair( +Eigen::Array ManeuveringTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; direction_vector << previous_waypoint_odom_frame_.x - - this->get_parameter("gps_start_x").as_double(), + odom_start_point_.x, previous_waypoint_odom_frame_.y - - this->get_parameter("gps_start_y").as_double(); + odom_start_point_.y; direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x; - predicted_positions(1, 0) = buoy_0.y; - predicted_positions(0, 1) = buoy_1.x; - predicted_positions(1, 1) = buoy_1.y; - predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 20; - predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 20; - predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 20; - predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 20; + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 20; + predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 20; + predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 20; + predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 20; return predicted_positions; } -Eigen::Array ManeuveringTaskNode::predict_next_pair_in_formation( +Eigen::Array ManeuveringTaskNode::predict_next_pair_in_formation( const geometry_msgs::msg::Point &buoy_red, const geometry_msgs::msg::Point &buoy_green, Eigen::Vector2d vector_to_next_pair) { - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_red.x; - predicted_positions(1, 0) = buoy_red.y; - predicted_positions(0, 1) = buoy_green.x; - predicted_positions(1, 1) = buoy_green.y; - predicted_positions(0, 2) = buoy_red.x + vector_to_next_pair(0); - predicted_positions(1, 2) = buoy_red.y + vector_to_next_pair(1); - predicted_positions(0, 3) = buoy_green.x + vector_to_next_pair(0); - predicted_positions(1, 3) = buoy_green.y + vector_to_next_pair(1); + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_red.x + vector_to_next_pair(0); + predicted_positions(1, 0) = buoy_red.y + vector_to_next_pair(1); + predicted_positions(0, 1) = buoy_green.x + vector_to_next_pair(0); + predicted_positions(1, 1) = buoy_green.y + vector_to_next_pair(1); return predicted_positions; } diff --git a/mission/njord_tasks/navigation_task/CMakeLists.txt b/mission/njord_tasks/navigation_task/CMakeLists.txt index 1fe4ebaf..9d462689 100644 --- a/mission/njord_tasks/navigation_task/CMakeLists.txt +++ b/mission/njord_tasks/navigation_task/CMakeLists.txt @@ -26,6 +26,9 @@ find_package(vortex_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) + include_directories(include) @@ -38,6 +41,7 @@ target_link_libraries(navigation_task_node tf2::tf2 tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs + pcl_common ) ament_target_dependencies(navigation_task_node @@ -50,6 +54,7 @@ ament_target_dependencies(navigation_task_node tf2_ros tf2_geometry_msgs njord_task_base + pcl_conversions ) install( diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp index 7e878168..01852834 100644 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -2,6 +2,11 @@ #define NAVIGATION_TASK_ROS_HPP #include +#include +#include +#include +#include + namespace navigation_task { @@ -26,23 +31,23 @@ class NavigationTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); /** - * @brief Predict the positions of the first two buoys pairs + * @brief Predict the positions of the second buoy pair * - * @return An Eigen::Array representing the predicted positions - * of the first two buoy pairs + * @return An Eigen::Array representing the predicted positions + * of the second buoy pair */ - Eigen::Array - predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + Eigen::Array + predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); /** * @brief Predict the positions of the west buoy by using the two first buoy * pairs * - * @return An Eigen::Array representing the predicted positions - * of the second buoy pair and the west buoy + * @return An Eigen::Array representing the predicted position + * of the west buoy */ - Eigen::Array + Eigen::Array predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, const geometry_msgs::msg::Point &buoy_2, @@ -65,10 +70,10 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @brief Predict the positions of the north buoy by using the two first buoy * pairs * - * @return An Eigen::Array representing the predicted positions - * of the third buoy pair and the north buoy + * @return An Eigen::Array representing the predicted position + * of the north buoy */ - Eigen::Array predict_north_buoy( + Eigen::Array predict_north_buoy( const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, const Eigen::Vector2d &direction_vector_second_to_third_pair); @@ -80,10 +85,9 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @return An Eigen::Array representing the predicted positions * of the north and south buoys */ - Eigen::Array predict_south_buoy( + Eigen::Array predict_south_buoy( const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, - const geometry_msgs::msg::Point &buoy_7, const Eigen::Vector2d &direction_vector_second_to_third_pair); /** @@ -101,10 +105,10 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @brief Predict the position of the east buoy by using the fourth buoy pair * and the direction vector from the second to the third buoy pair * - * @return An Eigen::Array representing the predicted positions - * of the fourth buoy pair and the east buoy + * @return An Eigen::Array representing the predicted position + * of the east buoy */ - Eigen::Array predict_east_buoy( + Eigen::Array predict_east_buoy( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, const Eigen::Vector2d &direction_vector_second_to_third_pair); @@ -113,23 +117,22 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @brief Predict the position of the fifth buoy pair by using the fourth buoy * pair and the direction vector from the second to the third buoy pair * - * @return An Eigen::Array representing the predicted positions - * of the fifth buoy pair and the east buoy + * @return An Eigen::Array representing the predicted positions + * of the fifth buoy pair */ - Eigen::Array predict_fifth_buoy_pair( + Eigen::Array predict_fifth_buoy_pair( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_11, const Eigen::Vector2d &direction_vector_second_to_third_pair); /** * @brief Predict the position of the sixth buoy pair by using the fifth buoy * pair and fourth buoy pair * - * @return An Eigen::Array representing the predicted positions - * of the fifth and sixth buoy pairs + * @return An Eigen::Array representing the predicted position + * of the sixth buoy pairs */ - Eigen::Array + Eigen::Array predict_sixth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, const geometry_msgs::msg::Point &buoy_12, diff --git a/mission/njord_tasks/navigation_task/package.xml b/mission/njord_tasks/navigation_task/package.xml index a41c7cd7..1c06d25a 100644 --- a/mission/njord_tasks/navigation_task/package.xml +++ b/mission/njord_tasks/navigation_task/package.xml @@ -21,6 +21,7 @@ tf2 tf2_ros tf2_geometry_msgs + pcl_conversions diff --git a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml index 42e4e897..fdc8a50b 100644 --- a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml +++ b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml @@ -1,17 +1,17 @@ navigation_task_node: ros__parameters: - map_origin_lat: 63.4411585867919 - map_origin_lon: 10.419400373255625 + map_origin_lat: -33.72242824301795 + map_origin_lon: 150.6740063854522 map_origin_set: true - gps_start_lat: 63.44097054434422 - gps_start_lon: 10.419997767413607 - gps_end_lat: 63.44125901804796 - gps_end_lon: 10.41857835889424 + gps_start_lat: -33.722553423946906 + gps_start_lon: 150.67394210456843 + gps_end_lat: -33.72253671663162 + gps_end_lon: 150.67413245758723 gps_start_x: 0.0 gps_start_y: 0.0 - gps_end_x: 0.0 - gps_end_y: 0.0 - gps_frame_coords_set: false + gps_end_x: 1.1791211357704299 + gps_end_y: 17.485397720327114 + gps_frame_coords_set: true map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" landmark_topic: "landmarks_out" diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 781a2247..895fc857 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -14,8 +14,34 @@ void NavigationTaskNode::main_task() { navigation_ready(); RCLCPP_INFO(this->get_logger(), "Navigation task started"); + auto odom_msg = get_odom(); + odom_start_point_ = odom_msg->pose.pose.position; // First pair of buoys Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); + sensor_msgs::msg::PointCloud2 buoy_vis_msg; + pcl::PointCloud buoy_vis; + + pcl::PointXYZRGB buoy_red_0; + buoy_red_0.x = predicted_first_buoy_pair(0, 0); + buoy_red_0.y = predicted_first_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + pcl::PointXYZRGB buoy_green_1; + buoy_green_1.x = predicted_first_buoy_pair(0, 1); + buoy_green_1.y = predicted_first_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); if (distance_to_first_buoy_pair > 6.0) { @@ -31,6 +57,7 @@ void NavigationTaskNode::main_task() { tf2::doTransform(waypoint_to_approach_first_pair_base_link, waypoint_to_approach_first_pair_odom, transform); send_waypoint(waypoint_to_approach_first_pair_odom); + set_desired_heading(odom_start_point_, waypoint_to_approach_first_pair_odom); reach_waypoint(1.0); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); @@ -41,6 +68,29 @@ void NavigationTaskNode::main_task() { if (buoy_landmarks_0_to_1.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; + buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; + buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_first_pair; waypoint_first_pair.x = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + @@ -52,74 +102,202 @@ void NavigationTaskNode::main_task() { 2; waypoint_first_pair.z = 0.0; send_waypoint(waypoint_first_pair); + set_desired_heading(odom_start_point_, waypoint_first_pair); reach_waypoint(1.0); // Second pair of buoys - Eigen::Array predicted_first_and_second_pair = - predict_first_and_second_buoy_pair( + Eigen::Array predicted_second_pair = + predict_second_buoy_pair( buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position); - std::vector buoy_landmarks_0_to_3 = - get_buoy_landmarks(predicted_first_and_second_pair); - if (buoy_landmarks_0_to_3.size() != 4) { + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_2; + pcl::PointXYZRGB buoy_green_3; + buoy_red_2.x = predicted_second_pair(0, 0); + buoy_red_2.y = predicted_second_pair(1, 0); + buoy_red_2.z = 0.0; + buoy_red_2.r = 255; + buoy_red_2.g = 0; + buoy_red_2.b = 0; + buoy_vis.push_back(buoy_red_2); + buoy_green_3.x = predicted_second_pair(0, 1); + buoy_green_3.y = predicted_second_pair(1, 1); + buoy_green_3.z = 0.0; + buoy_green_3.r = 0; + buoy_green_3.g = 255; + buoy_green_3.b = 0; + buoy_vis.push_back(buoy_green_3); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_2_to_3 = + get_buoy_landmarks(predicted_second_pair); + if (buoy_landmarks_2_to_3.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_2 = pcl::PointXYZRGB(); + buoy_green_3 = pcl::PointXYZRGB(); + buoy_red_2.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; + buoy_red_2.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; + buoy_red_2.z = 0.0; + buoy_red_2.r = 255; + buoy_red_2.g = 0; + buoy_red_2.b = 0; + buoy_vis.push_back(buoy_red_2); + buoy_green_3.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; + buoy_green_3.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; + buoy_green_3.z = 0.0; + buoy_green_3.r = 0; + buoy_green_3.g = 255; + buoy_green_3.b = 0; + buoy_vis.push_back(buoy_green_3); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_second_pair; waypoint_second_pair.x = - (buoy_landmarks_0_to_3[2].pose_odom_frame.position.x + - buoy_landmarks_0_to_3[3].pose_odom_frame.position.x) / + (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + + buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / 2; waypoint_second_pair.y = - (buoy_landmarks_0_to_3[2].pose_odom_frame.position.y + - buoy_landmarks_0_to_3[3].pose_odom_frame.position.y) / + (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + + buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / 2; waypoint_second_pair.z = 0.0; send_waypoint(waypoint_second_pair); + set_desired_heading(waypoint_first_pair, waypoint_second_pair); reach_waypoint(1.0); // West buoy - Eigen::Array predicted_west_buoy = - predict_west_buoy(buoy_landmarks_0_to_3[0].pose_odom_frame.position, - buoy_landmarks_0_to_3[1].pose_odom_frame.position, - buoy_landmarks_0_to_3[2].pose_odom_frame.position, - buoy_landmarks_0_to_3[3].pose_odom_frame.position); - std::vector buoy_landmarks_2_to_4 = + Eigen::Array predicted_west_buoy = + predict_west_buoy(buoy_landmarks_0_to_1[0].pose_odom_frame.position, + buoy_landmarks_0_to_1[1].pose_odom_frame.position, + buoy_landmarks_2_to_3[0].pose_odom_frame.position, + buoy_landmarks_2_to_3[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_west; + buoy_west.x = predicted_west_buoy(0, 0); + buoy_west.y = predicted_west_buoy(1, 0); + buoy_west.z = 0.0; + buoy_west.r = 255; + buoy_west.g = 255; + buoy_west.b = 0; + buoy_vis.push_back(buoy_west); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_4 = get_buoy_landmarks(predicted_west_buoy); - if (buoy_landmarks_2_to_4.size() != 3) { + if (buoy_landmarks_4.size() != 1) { RCLCPP_ERROR(this->get_logger(), "Could not find west buoy"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_west = pcl::PointXYZRGB(); + buoy_west.x = buoy_landmarks_4[0].pose_odom_frame.position.x; + buoy_west.y = buoy_landmarks_4[0].pose_odom_frame.position.y; + buoy_west.z = 0.0; + buoy_west.r = 255; + buoy_west.g = 255; + buoy_west.b = 0; + buoy_vis.push_back(buoy_west); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + Eigen::Vector2d direction_vector_upwards; direction_vector_upwards - << buoy_landmarks_2_to_4[2].pose_odom_frame.position.x - - buoy_landmarks_0_to_3[3].pose_odom_frame.position.x, - buoy_landmarks_2_to_4[2].pose_odom_frame.position.y - - buoy_landmarks_0_to_3[3].pose_odom_frame.position.y; + << buoy_landmarks_2_to_3[0].pose_odom_frame.position.x - + buoy_landmarks_2_to_3[1].pose_odom_frame.position.x, + buoy_landmarks_2_to_3[0].pose_odom_frame.position.y - + buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; direction_vector_upwards.normalize(); // Set waypoint two meters above west buoy geometry_msgs::msg::Point waypoint_west_buoy; - waypoint_west_buoy.x = buoy_landmarks_2_to_4[4].pose_odom_frame.position.x + - direction_vector_upwards(0) * 2; - waypoint_west_buoy.y = buoy_landmarks_2_to_4[4].pose_odom_frame.position.y + - direction_vector_upwards(1) * 2; + waypoint_west_buoy.x = buoy_landmarks_4[0].pose_odom_frame.position.x + + direction_vector_upwards(0) * 3; + waypoint_west_buoy.y = buoy_landmarks_4[0].pose_odom_frame.position.y + + direction_vector_upwards(1) * 3; waypoint_west_buoy.z = 0.0; send_waypoint(waypoint_west_buoy); + set_desired_heading(waypoint_second_pair, waypoint_west_buoy); reach_waypoint(1.0); // Third pair of buoys Eigen::Array predicted_third_buoy_pair = predict_third_buoy_pair( - buoy_landmarks_0_to_3[0].pose_odom_frame.position, - buoy_landmarks_0_to_3[1].pose_odom_frame.position, - buoy_landmarks_0_to_3[2].pose_odom_frame.position, - buoy_landmarks_0_to_3[3].pose_odom_frame.position); + buoy_landmarks_0_to_1[0].pose_odom_frame.position, + buoy_landmarks_0_to_1[1].pose_odom_frame.position, + buoy_landmarks_2_to_3[0].pose_odom_frame.position, + buoy_landmarks_2_to_3[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_5; + pcl::PointXYZRGB buoy_green_6; + buoy_red_5.x = predicted_third_buoy_pair(0, 0); + buoy_red_5.y = predicted_third_buoy_pair(1, 0); + buoy_red_5.z = 0.0; + buoy_red_5.r = 255; + buoy_red_5.g = 0; + buoy_red_5.b = 0; + buoy_vis.push_back(buoy_red_5); + buoy_green_6.x = predicted_third_buoy_pair(0, 1); + buoy_green_6.y = predicted_third_buoy_pair(1, 1); + buoy_green_6.z = 0.0; + buoy_green_6.r = 0; + buoy_green_6.g = 255; + buoy_green_6.b = 0; + buoy_vis.push_back(buoy_green_6); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); std::vector buoy_landmarks_5_to_6 = get_buoy_landmarks(predicted_third_buoy_pair); if (buoy_landmarks_5_to_6.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find third buoy pair"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_5 = pcl::PointXYZRGB(); + buoy_green_6 = pcl::PointXYZRGB(); + buoy_red_5.x = buoy_landmarks_5_to_6[0].pose_odom_frame.position.x; + buoy_red_5.y = buoy_landmarks_5_to_6[0].pose_odom_frame.position.y; + buoy_red_5.z = 0.0; + buoy_red_5.r = 255; + buoy_red_5.g = 0; + buoy_red_5.b = 0; + buoy_vis.push_back(buoy_red_5); + buoy_green_6.x = buoy_landmarks_5_to_6[1].pose_odom_frame.position.x; + buoy_green_6.y = buoy_landmarks_5_to_6[1].pose_odom_frame.position.y; + buoy_green_6.z = 0.0; + buoy_green_6.r = 0; + buoy_green_6.g = 255; + buoy_green_6.b = 0; + buoy_vis.push_back(buoy_green_6); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_third_pair; waypoint_third_pair.x = (buoy_landmarks_5_to_6[0].pose_odom_frame.position.x + @@ -132,6 +310,7 @@ void NavigationTaskNode::main_task() { waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); + set_desired_heading(waypoint_west_buoy, waypoint_third_pair); Eigen::Vector2d direction_vector_second_to_third_pair; direction_vector_second_to_third_pair << waypoint_third_pair.x - waypoint_second_pair.x, @@ -144,57 +323,163 @@ void NavigationTaskNode::main_task() { waypoint_third_pair.y + direction_vector_second_to_third_pair(1) * 2; waypoint_through_third_pair.z = 0.0; send_waypoint(waypoint_through_third_pair); + set_desired_heading(waypoint_third_pair, waypoint_through_third_pair); reach_waypoint(1.0); // North buoy - Eigen::Array predicted_north_buoy = + Eigen::Array predicted_north_buoy = predict_north_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, buoy_landmarks_5_to_6[1].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_5_to_7 = + std::vector buoy_landmarks_7 = get_buoy_landmarks(predicted_north_buoy); - if (buoy_landmarks_5_to_7.size() != 3) { + if (buoy_landmarks_7.size() != 1) { RCLCPP_ERROR(this->get_logger(), "Could not find north buoy"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_north; + buoy_north.x = buoy_landmarks_7[0].pose_odom_frame.position.x; + buoy_north.y = buoy_landmarks_7[0].pose_odom_frame.position.y; + buoy_north.z = 0.0; + buoy_north.r = 255; + buoy_north.g = 255; + buoy_north.b = 0; + buoy_vis.push_back(buoy_north); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_north_buoy; - waypoint_north_buoy.x = buoy_landmarks_5_to_7[2].pose_odom_frame.position.x + - direction_vector_second_to_third_pair(0) * 2; - waypoint_north_buoy.y = buoy_landmarks_5_to_7[2].pose_odom_frame.position.y + - direction_vector_second_to_third_pair(1) * 2; + waypoint_north_buoy.x = buoy_landmarks_7[0].pose_odom_frame.position.x + + direction_vector_second_to_third_pair(0) * 2.5; + waypoint_north_buoy.y = buoy_landmarks_7[0].pose_odom_frame.position.y + + direction_vector_second_to_third_pair(1) * 2.5; waypoint_north_buoy.z = 0.0; send_waypoint(waypoint_north_buoy); + set_desired_heading(waypoint_through_third_pair, waypoint_north_buoy); reach_waypoint(1.0); // South buoy - Eigen::Array predicted_south_buoy = - predict_south_buoy(buoy_landmarks_5_to_7[0].pose_odom_frame.position, - buoy_landmarks_5_to_7[1].pose_odom_frame.position, - buoy_landmarks_5_to_7[2].pose_odom_frame.position, + Eigen::Array predicted_south_buoy = + predict_south_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, + buoy_landmarks_5_to_6[1].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_7_to_8 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_south; + buoy_south.x = predicted_south_buoy(0, 0); + buoy_south.y = predicted_south_buoy(1, 0); + buoy_south.z = 0.0; + buoy_south.r = 255; + buoy_south.g = 255; + buoy_south.b = 0; + buoy_vis.push_back(buoy_south); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_8 = get_buoy_landmarks(predicted_south_buoy); - if (buoy_landmarks_7_to_8.size() != 2) { + if (buoy_landmarks_8.size() != 1) { RCLCPP_ERROR(this->get_logger(), "Could not find south buoy"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_south = pcl::PointXYZRGB(); + buoy_south.x = buoy_landmarks_8[0].pose_odom_frame.position.x; + buoy_south.y = buoy_landmarks_8[0].pose_odom_frame.position.y; + buoy_south.z = 0.0; + buoy_south.r = 255; + buoy_south.g = 255; + buoy_south.b = 0; + buoy_vis.push_back(buoy_south); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + Eigen::Vector2d direction_vector_downwards; + direction_vector_downwards + << buoy_landmarks_5_to_6[1].pose_odom_frame.position.x - + buoy_landmarks_5_to_6[10].pose_odom_frame.position.x, + buoy_landmarks_5_to_6[1].pose_odom_frame.position.y - + buoy_landmarks_5_to_6[0].pose_odom_frame.position.y; + direction_vector_downwards.normalize(); + geometry_msgs::msg::Point waypoint_south_buoy; - waypoint_south_buoy.x = buoy_landmarks_7_to_8[1].pose_odom_frame.position.x - - direction_vector_second_to_third_pair(0) * 2; - waypoint_south_buoy.y = buoy_landmarks_7_to_8[1].pose_odom_frame.position.y - - direction_vector_second_to_third_pair(1) * 2; + waypoint_south_buoy.x = buoy_landmarks_8[0].pose_odom_frame.position.x - + direction_vector_second_to_third_pair(0) * 3 + + direction_vector_downwards(0) * 1; + waypoint_south_buoy.y = buoy_landmarks_8[0].pose_odom_frame.position.y - + direction_vector_second_to_third_pair(1) * 3 + + direction_vector_downwards(1) * 1; waypoint_south_buoy.z = 0.0; send_waypoint(waypoint_south_buoy); - reach_waypoint(1.0); + set_desired_heading(waypoint_north_buoy, waypoint_south_buoy); + reach_waypoint(0.2); // Fourth pair of buoys Eigen::Array predicted_fourth_buoy_pair = predict_fourth_buoy_pair( - buoy_landmarks_5_to_7[0].pose_odom_frame.position, - buoy_landmarks_5_to_7[1].pose_odom_frame.position); + buoy_landmarks_5_to_6[0].pose_odom_frame.position, + buoy_landmarks_5_to_6[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_9; + pcl::PointXYZRGB buoy_green_10; + buoy_red_9.x = predicted_fourth_buoy_pair(0, 0); + buoy_red_9.y = predicted_fourth_buoy_pair(1, 0); + buoy_red_9.z = 0.0; + buoy_red_9.r = 255; + buoy_red_9.g = 0; + buoy_red_9.b = 0; + buoy_vis.push_back(buoy_red_9); + buoy_green_10.x = predicted_fourth_buoy_pair(0, 1); + buoy_green_10.y = predicted_fourth_buoy_pair(1, 1); + buoy_green_10.z = 0.0; + buoy_green_10.r = 0; + buoy_green_10.g = 255; + buoy_green_10.b = 0; + buoy_vis.push_back(buoy_green_10); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + std::vector buoy_landmarks_9_to_10 = get_buoy_landmarks(predicted_fourth_buoy_pair); if (buoy_landmarks_9_to_10.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find fourth buoy pair"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_9 = pcl::PointXYZRGB(); + buoy_green_10 = pcl::PointXYZRGB(); + buoy_red_9.x = buoy_landmarks_9_to_10[0].pose_odom_frame.position.x; + buoy_red_9.y = buoy_landmarks_9_to_10[0].pose_odom_frame.position.y; + buoy_red_9.z = 0.0; + buoy_red_9.r = 255; + buoy_red_9.g = 0; + buoy_red_9.b = 0; + buoy_vis.push_back(buoy_red_9); + buoy_green_10.x = buoy_landmarks_9_to_10[1].pose_odom_frame.position.x; + buoy_green_10.y = buoy_landmarks_9_to_10[1].pose_odom_frame.position.y; + buoy_green_10.z = 0.0; + buoy_green_10.r = 0; + buoy_green_10.g = 255; + buoy_green_10.b = 0; + buoy_vis.push_back(buoy_green_10); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_fourth_pair; waypoint_fourth_pair.x = (buoy_landmarks_9_to_10[0].pose_odom_frame.position.x + @@ -206,82 +491,212 @@ void NavigationTaskNode::main_task() { 2; waypoint_fourth_pair.z = 0.0; send_waypoint(waypoint_fourth_pair); + set_desired_heading(waypoint_south_buoy, waypoint_fourth_pair); reach_waypoint(1.0); // East buoy - Eigen::Array predicted_east_buoy = + Eigen::Array predicted_east_buoy = predict_east_buoy(buoy_landmarks_9_to_10[0].pose_odom_frame.position, buoy_landmarks_9_to_10[1].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_9_to_11 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_east; + buoy_east.x = predicted_east_buoy(0, 0); + buoy_east.y = predicted_east_buoy(1, 0); + buoy_east.z = 0.0; + buoy_east.r = 255; + buoy_east.g = 255; + buoy_east.b = 0; + buoy_vis.push_back(buoy_east); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_11 = get_buoy_landmarks(predicted_east_buoy); - if (buoy_landmarks_9_to_11.size() != 3) { + if (buoy_landmarks_11.size() != 1) { RCLCPP_ERROR(this->get_logger(), "Could not find east buoy"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_east = pcl::PointXYZRGB(); + buoy_east.x = buoy_landmarks_11[0].pose_odom_frame.position.x; + buoy_east.y = buoy_landmarks_11[0].pose_odom_frame.position.y; + buoy_east.z = 0.0; + buoy_east.r = 255; + buoy_east.g = 255; + buoy_east.b = 0; + buoy_vis.push_back(buoy_east); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + Eigen::Vector2d direction_vector_9_to_10; direction_vector_9_to_10 - << buoy_landmarks_9_to_11[1].pose_odom_frame.position.x - - buoy_landmarks_9_to_11[0].pose_odom_frame.position.x, - buoy_landmarks_9_to_11[1].pose_odom_frame.position.y - - buoy_landmarks_9_to_11[0].pose_odom_frame.position.y; + << buoy_landmarks_9_to_10[1].pose_odom_frame.position.x - + buoy_landmarks_9_to_10[0].pose_odom_frame.position.x, + buoy_landmarks_9_to_10[1].pose_odom_frame.position.y - + buoy_landmarks_9_to_10[0].pose_odom_frame.position.y; direction_vector_9_to_10.normalize(); geometry_msgs::msg::Point waypoint_east_buoy; - waypoint_east_buoy.x = buoy_landmarks_9_to_11[2].pose_odom_frame.position.x + - direction_vector_9_to_10(0) * 2; - waypoint_east_buoy.y = buoy_landmarks_9_to_11[2].pose_odom_frame.position.y + - direction_vector_9_to_10(1) * 2; + waypoint_east_buoy.x = buoy_landmarks_11[0].pose_odom_frame.position.x + + direction_vector_9_to_10(0) * 3; + waypoint_east_buoy.y = buoy_landmarks_11[0].pose_odom_frame.position.y + + direction_vector_9_to_10(1) * 3; waypoint_east_buoy.z = 0.0; send_waypoint(waypoint_east_buoy); + set_desired_heading(waypoint_fourth_pair, waypoint_east_buoy); reach_waypoint(1.0); // Fifth pair of buoys - Eigen::Array predicted_fifth_buoy_pair = + Eigen::Array predicted_fifth_buoy_pair = predict_fifth_buoy_pair( - buoy_landmarks_9_to_11[0].pose_odom_frame.position, - buoy_landmarks_9_to_11[1].pose_odom_frame.position, - buoy_landmarks_9_to_11[2].pose_odom_frame.position, + buoy_landmarks_9_to_10[0].pose_odom_frame.position, + buoy_landmarks_9_to_10[1].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_11_to_13 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_12; + pcl::PointXYZRGB buoy_green_13; + buoy_red_12.x = predicted_fifth_buoy_pair(0, 0); + buoy_red_12.y = predicted_fifth_buoy_pair(1, 0); + buoy_red_12.z = 0.0; + buoy_red_12.r = 255; + buoy_red_12.g = 0; + buoy_red_12.b = 0; + buoy_vis.push_back(buoy_red_12); + buoy_green_13.x = predicted_fifth_buoy_pair(0, 1); + buoy_green_13.y = predicted_fifth_buoy_pair(1, 1); + buoy_green_13.z = 0.0; + buoy_green_13.r = 0; + buoy_green_13.g = 255; + buoy_green_13.b = 0; + buoy_vis.push_back(buoy_green_13); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_12_to_13 = get_buoy_landmarks(predicted_fifth_buoy_pair); - if (buoy_landmarks_11_to_13.size() != 3) { + if (buoy_landmarks_12_to_13.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find fifth buoy pair"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_12 = pcl::PointXYZRGB(); + buoy_green_13 = pcl::PointXYZRGB(); + buoy_red_12.x = buoy_landmarks_12_to_13[0].pose_odom_frame.position.x; + buoy_red_12.y = buoy_landmarks_12_to_13[0].pose_odom_frame.position.y; + buoy_red_12.z = 0.0; + buoy_red_12.r = 255; + buoy_red_12.g = 0; + buoy_red_12.b = 0; + buoy_vis.push_back(buoy_red_12); + buoy_green_13.x = buoy_landmarks_12_to_13[1].pose_odom_frame.position.x; + buoy_green_13.y = buoy_landmarks_12_to_13[1].pose_odom_frame.position.y; + buoy_green_13.z = 0.0; + buoy_green_13.r = 0; + buoy_green_13.g = 255; + buoy_green_13.b = 0; + buoy_vis.push_back(buoy_green_13); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_fifth_pair; waypoint_fifth_pair.x = - (buoy_landmarks_11_to_13[1].pose_odom_frame.position.x + - buoy_landmarks_11_to_13[2].pose_odom_frame.position.x) / + (buoy_landmarks_12_to_13[0].pose_odom_frame.position.x + + buoy_landmarks_12_to_13[1].pose_odom_frame.position.x) / 2; waypoint_fifth_pair.y = - (buoy_landmarks_11_to_13[1].pose_odom_frame.position.y + - buoy_landmarks_11_to_13[2].pose_odom_frame.position.y) / + (buoy_landmarks_12_to_13[0].pose_odom_frame.position.y + + buoy_landmarks_12_to_13[1].pose_odom_frame.position.y) / 2; waypoint_fifth_pair.z = 0.0; send_waypoint(waypoint_fifth_pair); + set_desired_heading(waypoint_east_buoy, waypoint_fifth_pair); reach_waypoint(1.0); // Sixth pair of buoys - Eigen::Array predicted_sixth_buoy_pair = + Eigen::Array predicted_sixth_buoy_pair = predict_sixth_buoy_pair( - buoy_landmarks_9_to_11[0].pose_odom_frame.position, - buoy_landmarks_9_to_11[1].pose_odom_frame.position, - buoy_landmarks_11_to_13[1].pose_odom_frame.position, - buoy_landmarks_11_to_13[2].pose_odom_frame.position); - std::vector buoy_landmarks_12_to_15 = + buoy_landmarks_9_to_10[0].pose_odom_frame.position, + buoy_landmarks_9_to_10[1].pose_odom_frame.position, + buoy_landmarks_12_to_13[0].pose_odom_frame.position, + buoy_landmarks_12_to_13[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_14; + pcl::PointXYZRGB buoy_green_15; + buoy_red_14.x = predicted_sixth_buoy_pair(0, 0); + buoy_red_14.y = predicted_sixth_buoy_pair(1, 0); + buoy_red_14.z = 0.0; + buoy_red_14.r = 255; + buoy_red_14.g = 0; + buoy_red_14.b = 0; + buoy_vis.push_back(buoy_red_14); + buoy_green_15.x = predicted_sixth_buoy_pair(0, 1); + buoy_green_15.y = predicted_sixth_buoy_pair(1, 1); + buoy_green_15.z = 0.0; + buoy_green_15.r = 0; + buoy_green_15.g = 255; + buoy_green_15.b = 0; + buoy_vis.push_back(buoy_green_15); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_14_to_15 = get_buoy_landmarks(predicted_sixth_buoy_pair); - if (buoy_landmarks_12_to_15.size() != 4) { + if (buoy_landmarks_14_to_15.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find sixth buoy pair"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_14 = pcl::PointXYZRGB(); + buoy_green_15 = pcl::PointXYZRGB(); + buoy_red_14.x = buoy_landmarks_14_to_15[0].pose_odom_frame.position.x; + buoy_red_14.y = buoy_landmarks_14_to_15[0].pose_odom_frame.position.y; + buoy_red_14.z = 0.0; + buoy_red_14.r = 255; + buoy_red_14.g = 0; + buoy_red_14.b = 0; + buoy_vis.push_back(buoy_red_14); + buoy_green_15.x = buoy_landmarks_14_to_15[1].pose_odom_frame.position.x; + buoy_green_15.y = buoy_landmarks_14_to_15[1].pose_odom_frame.position.y; + buoy_green_15.z = 0.0; + buoy_green_15.r = 0; + buoy_green_15.g = 255; + buoy_green_15.b = 0; + buoy_vis.push_back(buoy_green_15); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_sixth_pair; waypoint_sixth_pair.x = - (buoy_landmarks_12_to_15[2].pose_odom_frame.position.x + - buoy_landmarks_12_to_15[3].pose_odom_frame.position.x) / + (buoy_landmarks_14_to_15[0].pose_odom_frame.position.x + + buoy_landmarks_14_to_15[1].pose_odom_frame.position.x) / 2; waypoint_sixth_pair.y = - (buoy_landmarks_12_to_15[2].pose_odom_frame.position.y + - buoy_landmarks_12_to_15[3].pose_odom_frame.position.y) / + (buoy_landmarks_14_to_15[0].pose_odom_frame.position.y + + buoy_landmarks_14_to_15[1].pose_odom_frame.position.y) / 2; waypoint_sixth_pair.z = 0.0; send_waypoint(waypoint_sixth_pair); + set_desired_heading(waypoint_fifth_pair, waypoint_sixth_pair); reach_waypoint(1.0); // Gps end goal @@ -290,6 +705,7 @@ void NavigationTaskNode::main_task() { gps_end_goal.y = this->get_parameter("gps_end_y").as_double(); gps_end_goal.z = 0.0; send_waypoint(gps_end_goal); + set_desired_heading(waypoint_sixth_pair, gps_end_goal); } Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { @@ -334,31 +750,27 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array -NavigationTaskNode::predict_first_and_second_buoy_pair( +Eigen::Array +NavigationTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; direction_vector << previous_waypoint_odom_frame_.x - - this->get_parameter("gps_start_x").as_double(), + odom_start_point_.x, previous_waypoint_odom_frame_.y - - this->get_parameter("gps_start_y").as_double(); + odom_start_point_.y; direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x; - predicted_positions(1, 0) = buoy_0.y; - predicted_positions(0, 1) = buoy_1.x; - predicted_positions(1, 1) = buoy_1.y; - predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 5; - predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 5; - predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 5; - predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 5; + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 5; + predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 5; + predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 5; + predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 5; return predicted_positions; } -Eigen::Array +Eigen::Array NavigationTaskNode::predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, const geometry_msgs::msg::Point &buoy_2, @@ -374,15 +786,11 @@ NavigationTaskNode::predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, (buoy_0.y + buoy_2.y) / 2 - (buoy_1.y + buoy_3.y) / 2; direction_vector_up.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_2.x; - predicted_positions(1, 0) = buoy_2.y; - predicted_positions(0, 1) = buoy_3.x; - predicted_positions(1, 1) = buoy_3.y; - predicted_positions(0, 2) = buoy_3.x + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_3.x + direction_vector_right(0) * 12.5 * 1 / 2 + direction_vector_up(0) * 5 * 2 / 5; - predicted_positions(1, 2) = buoy_3.y + + predicted_positions(1, 0) = buoy_3.y + direction_vector_right(1) * 12.5 * 1 / 2 + direction_vector_up(1) * 5 * 2 / 5; @@ -409,7 +817,7 @@ Eigen::Array NavigationTaskNode::predict_third_buoy_pair( return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_north_buoy( +Eigen::Array NavigationTaskNode::predict_north_buoy( const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, const Eigen::Vector2d &direction_vector_second_to_third_pair) { @@ -417,34 +825,27 @@ Eigen::Array NavigationTaskNode::predict_north_buoy( direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; direction_5_to_6.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_5.x; - predicted_positions(1, 0) = buoy_5.y; - predicted_positions(0, 1) = buoy_6.x; - predicted_positions(1, 1) = buoy_6.y; - predicted_positions(0, 2) = buoy_6.x + direction_5_to_6(0) * 12.5 * 0.5 + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5 * 0.5 + direction_vector_second_to_third_pair(0) * 2; - predicted_positions(1, 2) = buoy_6.y + direction_5_to_6(1) * 12.5 * 0.5 + + predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5 * 0.5 + direction_vector_second_to_third_pair(1) * 2; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_south_buoy( +Eigen::Array NavigationTaskNode::predict_south_buoy( const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, - const geometry_msgs::msg::Point &buoy_7, const Eigen::Vector2d &direction_vector_second_to_third_pair) { Eigen::Vector2d direction_5_to_6; direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; direction_5_to_6.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_7.x; - predicted_positions(1, 0) = buoy_7.y; - predicted_positions(0, 1) = buoy_6.x + direction_5_to_6(0) * 12.5 + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5 + direction_vector_second_to_third_pair(0) * 8; - predicted_positions(1, 1) = buoy_6.y + direction_5_to_6(1) * 12.5 + + predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5 + direction_vector_second_to_third_pair(1) * 8; return predicted_positions; @@ -468,7 +869,7 @@ Eigen::Array NavigationTaskNode::predict_fourth_buoy_pair( return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_east_buoy( +Eigen::Array NavigationTaskNode::predict_east_buoy( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, const Eigen::Vector2d &direction_vector_second_to_third_pair) { @@ -476,41 +877,34 @@ Eigen::Array NavigationTaskNode::predict_east_buoy( direction_9_to_10 << buoy_10.x - buoy_9.x, buoy_10.y - buoy_9.y; direction_9_to_10.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_9.x; - predicted_positions(1, 0) = buoy_9.y; - predicted_positions(0, 1) = buoy_10.x; - predicted_positions(1, 1) = buoy_10.y; - predicted_positions(0, 2) = buoy_9.x + direction_9_to_10(0) * 2.5 - + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_9.x + direction_9_to_10(0) * 2.5 - direction_vector_second_to_third_pair(0) * 7.5; - predicted_positions(1, 2) = buoy_9.y + direction_9_to_10(1) * 2.5 - + predicted_positions(1, 0) = buoy_9.y + direction_9_to_10(1) * 2.5 - direction_vector_second_to_third_pair(1) * 7.5; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_fifth_buoy_pair( +Eigen::Array NavigationTaskNode::predict_fifth_buoy_pair( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_11, const Eigen::Vector2d &direction_vector_second_to_third_pair) { - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_11.x; - predicted_positions(1, 0) = buoy_11.y; - predicted_positions(0, 1) = + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_9.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 1) = + predicted_positions(1, 0) = buoy_9.y - direction_vector_second_to_third_pair(1) * 12.5; - predicted_positions(0, 2) = + predicted_positions(0, 1) = buoy_10.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 2) = + predicted_positions(1, 1) = buoy_10.y - direction_vector_second_to_third_pair(1) * 12.5; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair( +Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, const geometry_msgs::msg::Point &buoy_12, @@ -521,15 +915,11 @@ Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair( (buoy_13.y + buoy_12.y) / 2 - (buoy_10.y + buoy_9.y) / 2; direction_fourth_to_fifth_pair.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_12.x; - predicted_positions(1, 0) = buoy_12.y; - predicted_positions(0, 1) = buoy_13.x; - predicted_positions(1, 1) = buoy_13.y; - predicted_positions(0, 2) = buoy_12.x + direction_fourth_to_fifth_pair(0) * 5; - predicted_positions(1, 2) = buoy_12.y + direction_fourth_to_fifth_pair(1) * 5; - predicted_positions(0, 3) = buoy_13.x + direction_fourth_to_fifth_pair(0) * 5; - predicted_positions(1, 3) = buoy_13.y + direction_fourth_to_fifth_pair(1) * 5; + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_12.x + direction_fourth_to_fifth_pair(0) * 5; + predicted_positions(1, 0) = buoy_12.y + direction_fourth_to_fifth_pair(1) * 5; + predicted_positions(0, 1) = buoy_13.x + direction_fourth_to_fifth_pair(0) * 5; + predicted_positions(1, 1) = buoy_13.y + direction_fourth_to_fifth_pair(1) * 5; return predicted_positions; } diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index 8c80ddf1..32bf3c3d 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -6,11 +6,13 @@ #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "vortex_msgs/msg/landmark_array.hpp" #include "vortex_msgs/srv/waypoint.hpp" +#include "vortex_msgs/srv/desired_velocity.hpp" #include #include #include @@ -120,15 +122,21 @@ class NjordTaskBaseNode : public rclcpp::Node { */ void reach_waypoint(const double distance_threshold); + void set_desired_heading(const geometry_msgs::msg::Point &prev_waypoint, const geometry_msgs::msg::Point &next_waypoint); + rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; rclcpp::Publisher::SharedPtr waypoint_visualization_pub_; + rclcpp::Publisher::SharedPtr + buoy_visualization_pub_; rclcpp::Subscription::SharedPtr map_origin_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr landmarks_sub_; rclcpp::Client::SharedPtr waypoint_client_; + rclcpp::Client::SharedPtr + heading_client_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -148,6 +156,7 @@ class NjordTaskBaseNode : public rclcpp::Node { bool navigation_ready_ = false; geometry_msgs::msg::Point previous_waypoint_odom_frame_; + geometry_msgs::msg::Point odom_start_point_; }; #endif // NJORD_TASK_BASE_ROS_HPP diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index d8ed78a8..06ab1bc2 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -38,6 +38,10 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, gps_map_coord_visualization_pub_ = this->create_publisher( "/gps_map_coord_visualization", qos_sensor_data); + + buoy_visualization_pub_ = + this->create_publisher( + "/buoy_visualization", qos_sensor_data); tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -61,7 +65,10 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, "/waypoint_visualization", qos_sensor_data); waypoint_client_ = - this->create_client("/waypoint"); + this->create_client("waypoint_list"); + + heading_client_ = + this->create_client("yaw_reference"); } void NjordTaskBaseNode::get_map_odom_tf() { @@ -442,6 +449,11 @@ void NjordTaskBaseNode::send_waypoint( waypoint.x, waypoint.y); // Check if the service was successful + geometry_msgs::msg::PoseStamped waypoint_vis; + waypoint_vis.header.frame_id = "odom"; + waypoint_vis.pose.position.x = waypoint.x; + waypoint_vis.pose.position.y = waypoint.y; + waypoint_visualization_pub_->publish(waypoint_vis); auto status = result_future.wait_for(std::chrono::seconds(5)); while (status == std::future_status::timeout) { RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); @@ -451,11 +463,6 @@ void NjordTaskBaseNode::send_waypoint( RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); } - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = waypoint.x; - waypoint_vis.pose.position.y = waypoint.y; - waypoint_visualization_pub_->publish(waypoint_vis); previous_waypoint_odom_frame_ = waypoint; } @@ -478,4 +485,24 @@ void NjordTaskBaseNode::reach_waypoint(const double distance_threshold) { } RCLCPP_INFO(this->get_logger(), "Reached waypoint"); +} + +void NjordTaskBaseNode::set_desired_heading( + const geometry_msgs::msg::Point &prev_waypoint, + const geometry_msgs::msg::Point &next_waypoint) { + auto request = std::make_shared(); + double dx = next_waypoint.x - prev_waypoint.x; + double dy = next_waypoint.y - prev_waypoint.y; + double desired_heading = std::atan2(dy, dx); + request->u_desired = desired_heading; + auto result_future = heading_client_->async_send_request(request); + RCLCPP_INFO(this->get_logger(), "Desired heading sent: %f", desired_heading); + auto status = result_future.wait_for(std::chrono::seconds(5)); + while (status == std::future_status::timeout) { + RCLCPP_INFO(this->get_logger(), "Desired heading service timed out"); + status = result_future.wait_for(std::chrono::seconds(5)); + } + if (!result_future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Desired heading service failed"); + } } \ No newline at end of file From b9f431cb95a53c72b6c43bf8b693558a3893fe72 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Mon, 5 Aug 2024 17:09:42 +0000 Subject: [PATCH 45/67] Committing clang-format changes --- .../docking_task/src/docking_task_ros.cpp | 26 +- .../src/maneuvering_task_ros.cpp | 275 +++++++++--------- .../navigation_task/navigation_task_ros.hpp | 5 +- .../src/navigation_task_ros.cpp | 37 ++- .../njord_task_base/njord_task_base_ros.hpp | 8 +- .../src/njord_task_base_ros.cpp | 3 +- 6 files changed, 171 insertions(+), 183 deletions(-) diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 0a15d3fd..31b523b2 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -19,7 +19,6 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) declare_parameter("models.dynmod_stddev", 0.0); declare_parameter("models.sen_stddev", 0.0); - std::thread(&DockingTaskNode::main_task, this).detach(); } @@ -61,8 +60,8 @@ void DockingTaskNode::main_task() { waypoint_to_approach_first_pair_base_link.y = 0.0; waypoint_to_approach_first_pair_base_link.z = 0.0; try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero); + auto transform = + tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; tf2::doTransform(waypoint_to_approach_first_pair_base_link, waypoint_to_approach_first_pair_odom, transform); @@ -122,7 +121,7 @@ void DockingTaskNode::main_task() { predict_second_buoy_pair( buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position); - + buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); buoy_red_0 = pcl::PointXYZRGB(); @@ -190,12 +189,11 @@ void DockingTaskNode::main_task() { reach_waypoint(1.0); // Third pair of buoys - Eigen::Array predicted_third_pair = - predict_third_buoy_pair( - buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position, - buoy_landmarks_2_to_3[2].pose_odom_frame.position, - buoy_landmarks_2_to_3[3].pose_odom_frame.position); + Eigen::Array predicted_third_pair = predict_third_buoy_pair( + buoy_landmarks_0_to_1[0].pose_odom_frame.position, + buoy_landmarks_0_to_1[1].pose_odom_frame.position, + buoy_landmarks_2_to_3[2].pose_odom_frame.position, + buoy_landmarks_2_to_3[3].pose_odom_frame.position); buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); @@ -262,7 +260,7 @@ void DockingTaskNode::main_task() { send_waypoint(waypoint_third_pair); set_desired_heading(odom_start_point_, waypoint_third_pair); reach_waypoint(1.0); - + std::thread(&DockingTaskNode::initialize_grid_sub, this).detach(); } @@ -314,10 +312,8 @@ Eigen::Array DockingTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - - odom_start_point_.x, - previous_waypoint_odom_frame_.y - - odom_start_point_.y; + direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, + previous_waypoint_odom_frame_.y - odom_start_point_.y; direction_vector.normalize(); Eigen::Array predicted_positions; diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index a1106fb3..a131bd28 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -22,32 +22,33 @@ void ManeuveringTaskNode::main_task() { sensor_msgs::msg::PointCloud2 buoy_vis_msg; pcl::PointCloud buoy_vis; pcl::PointXYZRGB buoy_red_0; - buoy_red_0.x = predicted_first_buoy_pair(0, 0); - buoy_red_0.y = predicted_first_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - pcl::PointXYZRGB buoy_green_1; - buoy_green_1.x = predicted_first_buoy_pair(0, 1); - buoy_green_1.y = predicted_first_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); + buoy_red_0.x = predicted_first_buoy_pair(0, 0); + buoy_red_0.y = predicted_first_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + pcl::PointXYZRGB buoy_green_1; + buoy_green_1.x = predicted_first_buoy_pair(0, 1); + buoy_green_1.y = predicted_first_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); // First first buoy pair is far away, should be closer before trying to // measure it. double gps_end_x = this->get_parameter("gps_end_x").as_double(); double gps_end_y = this->get_parameter("gps_end_y").as_double(); Eigen::Vector2d direction_vector_to_end; - direction_vector_to_end << gps_end_x - odom_start_point_.x, gps_end_y - odom_start_point_.y; + direction_vector_to_end << gps_end_x - odom_start_point_.x, + gps_end_y - odom_start_point_.y; direction_vector_to_end.normalize(); double distance = @@ -67,29 +68,29 @@ void ManeuveringTaskNode::main_task() { std::vector measured_first_buoy_pair = get_buoy_landmarks(predicted_first_buoy_pair); - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_first_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_first_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_first_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_first_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_first_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_first_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_first_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_first_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_first_pair; waypoint_first_pair.x = (measured_first_buoy_pair[0].pose_odom_frame.position.x + @@ -122,53 +123,53 @@ void ManeuveringTaskNode::main_task() { measured_first_buoy_pair[1].pose_odom_frame.position); buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_second_buoy_pair(0, 0); - buoy_red_0.y = predicted_second_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_second_buoy_pair(0, 1); - buoy_green_1.y = predicted_second_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_second_buoy_pair(0, 0); + buoy_red_0.y = predicted_second_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_second_buoy_pair(0, 1); + buoy_green_1.y = predicted_second_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); std::vector measured_buoy_2_to_3 = get_buoy_landmarks(predicted_second_buoy_pair); - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_buoy_2_to_3[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_buoy_2_to_3[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_buoy_2_to_3[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_buoy_2_to_3[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_buoy_2_to_3[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_buoy_2_to_3[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_buoy_2_to_3[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_buoy_2_to_3[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); geometry_msgs::msg::Point waypoint_second_pair; waypoint_second_pair.x = @@ -186,8 +187,7 @@ void ManeuveringTaskNode::main_task() { // Setup variable to navigate formation Eigen::Vector2d direction_vector_forwards; - direction_vector_forwards - << (waypoint_second_pair.x - waypoint_first_pair.x), + direction_vector_forwards << (waypoint_second_pair.x - waypoint_first_pair.x), (waypoint_second_pair.y - waypoint_first_pair.y); direction_vector_forwards.normalize(); @@ -212,7 +212,7 @@ void ManeuveringTaskNode::main_task() { measured_buoy_2_to_3[0].pose_odom_frame.position; geometry_msgs::msg::Point green_buoy = measured_buoy_2_to_3[1].pose_odom_frame.position; - geometry_msgs::msg::Point waypoint_prev_pair = waypoint_second_pair; + geometry_msgs::msg::Point waypoint_prev_pair = waypoint_second_pair; // ASV is at first buoy pair in formation. // Run loop for the rest of the formation excluding the first pair. int num_iterations = @@ -285,8 +285,7 @@ void ManeuveringTaskNode::main_task() { reach_waypoint(1.0); red_buoy = measured_next_pair[0].pose_odom_frame.position; green_buoy = measured_next_pair[1].pose_odom_frame.position; - vector_to_next_pair - << waypoint_next_pair.x - waypoint_prev_pair.x, + vector_to_next_pair << waypoint_next_pair.x - waypoint_prev_pair.x, waypoint_next_pair.y - waypoint_prev_pair.y; waypoint_prev_pair = waypoint_next_pair; } @@ -320,54 +319,54 @@ void ManeuveringTaskNode::main_task() { predicted_last_buoy_pair(1, 1) = gps_end_y + direction_vector_downwards(1) * 2.5; - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_last_buoy_pair(0, 0); - buoy_red_0.y = predicted_last_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_last_buoy_pair(0, 1); - buoy_green_1.y = predicted_last_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_last_buoy_pair(0, 0); + buoy_red_0.y = predicted_last_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_last_buoy_pair(0, 1); + buoy_green_1.y = predicted_last_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); std::vector measured_last_buoy_pair = get_buoy_landmarks(predicted_last_buoy_pair); - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_last_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_last_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_last_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_last_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_last_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_last_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_last_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_last_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); geometry_msgs::msg::Point waypoint_last_pair; waypoint_last_pair.x = @@ -440,10 +439,8 @@ Eigen::Array ManeuveringTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - - odom_start_point_.x, - previous_waypoint_odom_frame_.y - - odom_start_point_.y; + direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, + previous_waypoint_odom_frame_.y - odom_start_point_.y; direction_vector.normalize(); Eigen::Array predicted_positions; diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp index 01852834..e224c3d4 100644 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -7,7 +7,6 @@ #include #include - namespace navigation_task { class NavigationTaskNode : public NjordTaskBaseNode { @@ -38,7 +37,7 @@ class NavigationTaskNode : public NjordTaskBaseNode { */ Eigen::Array predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); + const geometry_msgs::msg::Point &buoy_1); /** * @brief Predict the positions of the west buoy by using the two first buoy @@ -118,7 +117,7 @@ class NavigationTaskNode : public NjordTaskBaseNode { * pair and the direction vector from the second to the third buoy pair * * @return An Eigen::Array representing the predicted positions - * of the fifth buoy pair + * of the fifth buoy pair */ Eigen::Array predict_fifth_buoy_pair( const geometry_msgs::msg::Point &buoy_9, diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 895fc857..9398ecac 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -41,7 +41,7 @@ void NavigationTaskNode::main_task() { buoy_vis_msg.header.frame_id = "odom"; buoy_vis_msg.header.stamp = this->now(); buoy_visualization_pub_->publish(buoy_vis_msg); - + double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); if (distance_to_first_buoy_pair > 6.0) { @@ -57,7 +57,8 @@ void NavigationTaskNode::main_task() { tf2::doTransform(waypoint_to_approach_first_pair_base_link, waypoint_to_approach_first_pair_odom, transform); send_waypoint(waypoint_to_approach_first_pair_odom); - set_desired_heading(odom_start_point_, waypoint_to_approach_first_pair_odom); + set_desired_heading(odom_start_point_, + waypoint_to_approach_first_pair_odom); reach_waypoint(1.0); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); @@ -87,7 +88,7 @@ void NavigationTaskNode::main_task() { buoy_green_1.b = 0; buoy_vis.push_back(buoy_green_1); pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.frame_id = "odom"; buoy_vis_msg.header.stamp = this->now(); buoy_visualization_pub_->publish(buoy_vis_msg); @@ -106,10 +107,9 @@ void NavigationTaskNode::main_task() { reach_waypoint(1.0); // Second pair of buoys - Eigen::Array predicted_second_pair = - predict_second_buoy_pair( - buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position); + Eigen::Array predicted_second_pair = predict_second_buoy_pair( + buoy_landmarks_0_to_1[0].pose_odom_frame.position, + buoy_landmarks_0_to_1[1].pose_odom_frame.position); buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); @@ -198,7 +198,7 @@ void NavigationTaskNode::main_task() { buoy_vis_msg.header.frame_id = "odom"; buoy_vis_msg.header.stamp = this->now(); buoy_visualization_pub_->publish(buoy_vis_msg); - + std::vector buoy_landmarks_4 = get_buoy_landmarks(predicted_west_buoy); if (buoy_landmarks_4.size() != 1) { @@ -218,7 +218,7 @@ void NavigationTaskNode::main_task() { buoy_vis_msg.header.frame_id = "odom"; buoy_vis_msg.header.stamp = this->now(); buoy_visualization_pub_->publish(buoy_vis_msg); - + Eigen::Vector2d direction_vector_upwards; direction_vector_upwards << buoy_landmarks_2_to_3[0].pose_odom_frame.position.x - @@ -412,11 +412,11 @@ void NavigationTaskNode::main_task() { geometry_msgs::msg::Point waypoint_south_buoy; waypoint_south_buoy.x = buoy_landmarks_8[0].pose_odom_frame.position.x - - direction_vector_second_to_third_pair(0) * 3 - + direction_vector_downwards(0) * 1; + direction_vector_second_to_third_pair(0) * 3 + + direction_vector_downwards(0) * 1; waypoint_south_buoy.y = buoy_landmarks_8[0].pose_odom_frame.position.y - - direction_vector_second_to_third_pair(1) * 3 - + direction_vector_downwards(1) * 1; + direction_vector_second_to_third_pair(1) * 3 + + direction_vector_downwards(1) * 1; waypoint_south_buoy.z = 0.0; send_waypoint(waypoint_south_buoy); set_desired_heading(waypoint_north_buoy, waypoint_south_buoy); @@ -632,7 +632,7 @@ void NavigationTaskNode::main_task() { buoy_landmarks_9_to_10[1].pose_odom_frame.position, buoy_landmarks_12_to_13[0].pose_odom_frame.position, buoy_landmarks_12_to_13[1].pose_odom_frame.position); - + buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); pcl::PointXYZRGB buoy_red_14; @@ -750,15 +750,12 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array -NavigationTaskNode::predict_second_buoy_pair( +Eigen::Array NavigationTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - - odom_start_point_.x, - previous_waypoint_odom_frame_.y - - odom_start_point_.y; + direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, + previous_waypoint_odom_frame_.y - odom_start_point_.y; direction_vector.normalize(); Eigen::Array predicted_positions; diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index 32bf3c3d..d0c55429 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -11,8 +11,8 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "vortex_msgs/msg/landmark_array.hpp" -#include "vortex_msgs/srv/waypoint.hpp" #include "vortex_msgs/srv/desired_velocity.hpp" +#include "vortex_msgs/srv/waypoint.hpp" #include #include #include @@ -122,7 +122,8 @@ class NjordTaskBaseNode : public rclcpp::Node { */ void reach_waypoint(const double distance_threshold); - void set_desired_heading(const geometry_msgs::msg::Point &prev_waypoint, const geometry_msgs::msg::Point &next_waypoint); + void set_desired_heading(const geometry_msgs::msg::Point &prev_waypoint, + const geometry_msgs::msg::Point &next_waypoint); rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; @@ -135,8 +136,7 @@ class NjordTaskBaseNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr landmarks_sub_; rclcpp::Client::SharedPtr waypoint_client_; - rclcpp::Client::SharedPtr - heading_client_; + rclcpp::Client::SharedPtr heading_client_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index 06ab1bc2..3f592be0 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -38,7 +38,7 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, gps_map_coord_visualization_pub_ = this->create_publisher( "/gps_map_coord_visualization", qos_sensor_data); - + buoy_visualization_pub_ = this->create_publisher( "/buoy_visualization", qos_sensor_data); @@ -463,7 +463,6 @@ void NjordTaskBaseNode::send_waypoint( RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); } - previous_waypoint_odom_frame_ = waypoint; } From b98b385e313e4ec11ed99f619cd9c9639b8c61b6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Wed, 7 Aug 2024 16:21:08 +0200 Subject: [PATCH 46/67] Set mu to zero such that the vessel can keep constant velocity more easely --- .../hybridpath_guidance/hybridpath_guidance_node.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index e0686077..65521a4d 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -60,6 +60,7 @@ def __init__(self): self.w = 0. self.v_s = 0. self.v_ss = 0. + self.mu = 0. self.operational_mode = 'autonomous mode' self.killswitch_active = False @@ -114,8 +115,14 @@ def waypoint_callback(self, request, response): new_waypoints = request.waypoint - for point in new_waypoints: + self.get_logger().info(f"Received {len(new_waypoints)} waypoints") + for i, point in enumerate(new_waypoints): + + wp = [point.x, point.y] + + self.get_logger().info(f"Waypoint {i+1}: {wp}") + self.waypoints.append(point) self.last_waypoint = [self.waypoints[-1].x, self.waypoints[-1].y] From adf847a81730c68a8749e92597ac323d82f85091 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Wed, 7 Aug 2024 16:21:43 +0200 Subject: [PATCH 47/67] Added new list to waypoint node collection. 40 meters straight east --- .../hybridpath_guidance/hybridpath_guidance/waypoint_node.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py index c0e60668..5be5c27e 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py @@ -39,8 +39,9 @@ def eta_callback(self, msg: Odometry): def send_request(self): if self.eta_received: req = Waypoint.Request() - wp_list = [[self.eta[0] + 5., self.eta[1]], [self.eta[0] + 5., self.eta[1] + 5.], [self.eta[0], self.eta[1] + 5.]] + # wp_list = [[self.eta[0] + 5., self.eta[1]], [self.eta[0] + 5., self.eta[1] + 5.], [self.eta[0], self.eta[1] + 5.]] # wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 0.]] + wp_list = [[self.eta[0], self.eta[1] + 40.]] req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list] self.get_logger().info(f'Sending request: {req}') self.future = self.client.call_async(req) From 70b32a1e9bbd9c5fefafc73d36b1ff898c5e77c4 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Thu, 8 Aug 2024 08:59:06 +0200 Subject: [PATCH 48/67] colav task --- .../sitaw/land_polygon_sim_maneuvering.yaml | 4 + .../config/sitaw/map_manager_params.yaml | 11 +- .../collision_avoidance_task/CMakeLists.txt | 4 + .../collision_avoidance_task_ros.hpp | 54 ++ .../collision_avoidance_task/package.xml | 1 + .../collision_avoidance_task_params.yaml | 19 +- .../src/collision_avoidance_task_ros.cpp | 665 +++++++++++++++++- .../params/maneuvering_task_params.yaml | 12 +- .../src/maneuvering_task_ros.cpp | 4 +- .../src/njord_task_base_ros.cpp | 22 + 10 files changed, 772 insertions(+), 24 deletions(-) create mode 100644 asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml diff --git a/asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml b/asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml new file mode 100644 index 00000000..b7b44874 --- /dev/null +++ b/asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml @@ -0,0 +1,4 @@ +-33.722464875303054 150.6733186790048 +-33.720864277234156 150.673353826611 +-33.72115033638088 150.67502276015122 +-33.72271743627459 150.67461276680072 \ No newline at end of file diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml index 036e3b2a..fb8c56b2 100644 --- a/asv_setup/config/sitaw/map_manager_params.yaml +++ b/asv_setup/config/sitaw/map_manager_params.yaml @@ -1,7 +1,7 @@ map_manager_node: ros__parameters: use_predef_landmask: true - landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml" + landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml" # landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml" map_resolution: 0.2 map_width: 1000 @@ -11,6 +11,11 @@ map_manager_node: # map_origin_lat: 63.414660884931976 # map_origin_lon: 10.398554661537544 # use_predef_map_origin: true - map_origin_lat: -33.72242824301795 - map_origin_lon: 150.6740063854522 + # Map origin for sim_navigation + # map_origin_lat: -33.72242824301795 + # map_origin_lon: 150.6740063854522 + # use_predef_map_origin: true + # Map origin for sim_maneuvering + map_origin_lat: -33.72213985487166 + map_origin_lon: 150.67413507552098 use_predef_map_origin: true \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt b/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt index 73e37226..aed2ab9e 100644 --- a/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt +++ b/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt @@ -27,6 +27,8 @@ find_package(vortex_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) include_directories(include) @@ -38,6 +40,7 @@ target_link_libraries(collision_avoidance_task_node tf2::tf2 tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs + pcl_common ) ament_target_dependencies(collision_avoidance_task_node @@ -50,6 +53,7 @@ ament_target_dependencies(collision_avoidance_task_node tf2 tf2_ros tf2_geometry_msgs + pcl_conversions ) install( diff --git a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp index d382b697..16e11993 100644 --- a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp +++ b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp @@ -2,9 +2,20 @@ #define COLLISION_AVOIDANCE_TASK_ROS_HPP #include +#include +#include +#include +#include +#include // Required for tf2::doTransform +#include +#include namespace collision_avoidance_task { +struct LandmarkOdomID { + nav_msgs::msg::Odometry odom; + int32_t id; +}; class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { public: explicit CollisionAvoidanceTaskNode( @@ -17,7 +28,50 @@ class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { */ void main_task(); + /** + * @brief Predict the positions of the first two buoys + * + * @return Eigen::Array predicted_positions + */ + Eigen::Array predict_first_buoy_pair(); + + Eigen::Array + predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1); + + void track_vessel(const Eigen::Vector2d& direction_vector_downwards, + const Eigen::Vector2d& direction_vector_forwards, + const geometry_msgs::msg::Point& waypoint_second_buoy_pair); + + nav_msgs::msg::Odometry get_vessel_odom(); + + LandmarkOdomID filter_landmarks( + const vortex_msgs::msg::LandmarkArray &landmarks, + const Eigen::Vector2d& direction_vector_downwards, + const Eigen::Vector2d& direction_vector_forwards, + const geometry_msgs::msg::Point& waypoint_second_buoy_pair); + + void vessel_collision_heading(); + + double calculate_angle(const geometry_msgs::msg::Vector3& twist1, const geometry_msgs::msg::Vector3& twist2); + + bool has_vessel_passed_freya(const Eigen::Vector2d& direction_vector_downwards); + + Eigen::Array + predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1); + + Eigen::Array + predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_red, + const geometry_msgs::msg::Point &buoy_green); + + + private: + mutable std::mutex vessel_odom_mutex_; + bool new_vessel_odom_msg_ = false; + nav_msgs::msg::Odometry vessel_odom_; + std::condition_variable vessel_odom_cv_; }; } // namespace collision_avoidance_task diff --git a/mission/njord_tasks/collision_avoidance_task/package.xml b/mission/njord_tasks/collision_avoidance_task/package.xml index 5d19b351..9e2ea608 100644 --- a/mission/njord_tasks/collision_avoidance_task/package.xml +++ b/mission/njord_tasks/collision_avoidance_task/package.xml @@ -21,6 +21,7 @@ tf2 tf2_ros tf2_geometry_msgs + pcl_conversions diff --git a/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml b/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml index af97764b..db084e8f 100644 --- a/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml +++ b/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml @@ -1,7 +1,7 @@ collision_avoidance_task_node: ros__parameters: - map_origin_lat: 63.4411585867919 - map_origin_lon: 10.419400373255625 + map_origin_lat: -33.72213988382845 + map_origin_lon: 150.67413500672993 map_origin_set: true gps_start_lat: 63.44097054434422 gps_start_lon: 10.419997767413607 @@ -9,12 +9,15 @@ collision_avoidance_task_node: gps_end_lon: 10.41857835889424 gps_start_x: 0.0 gps_start_y: 0.0 - gps_end_x: 0.0 - gps_end_y: 0.0 - gps_frame_coords_set: false - - + gps_end_x: 0.05243377001522731 + gps_end_y: 37.196639612524166 + gps_frame_coords_set: true map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" landmark_topic: "landmarks_out" - \ No newline at end of file + assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct + + # Task specific parameters + distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame. + distance_between_buoy_pairs: 5.0 # Distance between buoy pairs in meters + vessel_assignment_confidence: 5 # Number of consequtive identical assignments from landmark to vessel before we consider the assignment as correct \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp index 34ec0602..9d3f7ca8 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -6,18 +6,673 @@ CollisionAvoidanceTaskNode::CollisionAvoidanceTaskNode( const rclcpp::NodeOptions &options) : NjordTaskBaseNode("collision_avoidance_task_node", options) { + declare_parameter("distance_to_first_buoy_pair", 2.0); + declare_parameter("distance_between_buoy_pairs", 5.0); + declare_parameter("vessel_assignment_confidence", 5); + std::thread(&CollisionAvoidanceTaskNode::main_task, this).detach(); } void CollisionAvoidanceTaskNode::main_task() { - // Sleep for 3 seconds to allow system to initialize and tracks to be aquired - RCLCPP_INFO( - this->get_logger(), - "Waiting 3 seconds for system to initialize before starting main task"); - rclcpp::sleep_for(std::chrono::seconds(3)); + navigation_ready(); + RCLCPP_INFO(this->get_logger(), "Collision avoidance task started"); + odom_start_point_ = get_odom()->pose.pose.position; + Eigen::Array predicted_first_buoy_pair = predict_first_buoy_pair(); + sensor_msgs::msg::PointCloud2 buoy_vis_msg; + pcl::PointCloud buoy_vis; + pcl::PointXYZRGB buoy_red_0; + buoy_red_0.x = predicted_first_buoy_pair(0, 0); + buoy_red_0.y = predicted_first_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + pcl::PointXYZRGB buoy_green_1; + buoy_green_1.x = predicted_first_buoy_pair(0, 1); + buoy_green_1.y = predicted_first_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + // First first buoy pair is far away, should be closer before trying to + // measure it. + double gps_end_x = this->get_parameter("gps_end_x").as_double(); + double gps_end_y = this->get_parameter("gps_end_y").as_double(); + Eigen::Vector2d direction_vector_to_end; + direction_vector_to_end << gps_end_x - odom_start_point_.x, + gps_end_y - odom_start_point_.y; + direction_vector_to_end.normalize(); + + double distance = + this->get_parameter("distance_to_first_buoy_pair").as_double(); + if (distance > 5.0) { + auto odom = get_odom(); + geometry_msgs::msg::Point waypoint_toward_start; + waypoint_toward_start.x = + odom->pose.pose.position.x + direction_vector_to_end(0) * (distance - 3); + waypoint_toward_start.y = + odom->pose.pose.position.y + direction_vector_to_end(1) * (distance - 3); + waypoint_toward_start.z = 0.0; + send_waypoint(waypoint_toward_start); + set_desired_heading(odom_start_point_, waypoint_toward_start); + reach_waypoint(3.0); + } + + std::vector measured_first_buoy_pair = + get_buoy_landmarks(predicted_first_buoy_pair); + if (measured_first_buoy_pair.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); + } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_first_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_first_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_first_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_first_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + geometry_msgs::msg::Point waypoint_first_pair; + waypoint_first_pair.x = + (measured_first_buoy_pair[0].pose_odom_frame.position.x + + measured_first_buoy_pair[1].pose_odom_frame.position.x) / + 2; + waypoint_first_pair.y = + (measured_first_buoy_pair[0].pose_odom_frame.position.y + + measured_first_buoy_pair[1].pose_odom_frame.position.y) / + 2; + waypoint_first_pair.z = 0.0; + send_waypoint(waypoint_first_pair); + set_desired_heading(odom_start_point_, waypoint_first_pair); + reach_waypoint(3.0); + + // Second buoy pair + Eigen::Array predicted_second_buoy_pair = + predict_second_buoy_pair( + measured_first_buoy_pair[0].pose_odom_frame.position, + measured_first_buoy_pair[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_second_buoy_pair(0, 0); + buoy_red_0.y = predicted_second_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_second_buoy_pair(0, 1); + buoy_green_1.y = predicted_second_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector measured_second_buoy_pair = + get_buoy_landmarks(predicted_second_buoy_pair); + if (measured_second_buoy_pair.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); + } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_second_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_second_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_second_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_second_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + geometry_msgs::msg::Point waypoint_second_pair; + waypoint_second_pair.x = + (measured_second_buoy_pair[0].pose_odom_frame.position.x + + measured_second_buoy_pair[1].pose_odom_frame.position.x) / + 2; + waypoint_second_pair.y = + (measured_second_buoy_pair[0].pose_odom_frame.position.y + + measured_second_buoy_pair[1].pose_odom_frame.position.y) / + 2; + waypoint_second_pair.z = 0.0; + send_waypoint(waypoint_second_pair); + set_desired_heading(odom_start_point_, waypoint_second_pair); + reach_waypoint(3.0); + + Eigen::Vector2d direction_vector_forwards; + Eigen::Vector2d direction_vector_downwards; + direction_vector_forwards << waypoint_second_pair.x - waypoint_first_pair.x, + waypoint_second_pair.y - waypoint_first_pair.y; + direction_vector_forwards.normalize(); + direction_vector_downwards << (measured_first_buoy_pair[1].pose_odom_frame.position.x + measured_second_buoy_pair[1].pose_odom_frame.position.x)/2 + - (measured_first_buoy_pair[0].pose_odom_frame.position.x + measured_second_buoy_pair[0].pose_odom_frame.position.x)/2, + (measured_first_buoy_pair[1].pose_odom_frame.position.y + measured_second_buoy_pair[1].pose_odom_frame.position.y)/2 + - (measured_first_buoy_pair[0].pose_odom_frame.position.y + measured_second_buoy_pair[0].pose_odom_frame.position.y)/2; + direction_vector_downwards.normalize(); + + geometry_msgs::msg::Point waypoint_midpoint; + waypoint_midpoint.x = waypoint_second_pair.x + direction_vector_forwards(0) * 10; + waypoint_midpoint.y = waypoint_second_pair.y + direction_vector_forwards(1) * 10; + waypoint_midpoint.z = 0.0; + send_waypoint(waypoint_midpoint); + set_desired_heading(waypoint_second_pair, waypoint_midpoint); + + std::thread(std::bind(&CollisionAvoidanceTaskNode::track_vessel, this, direction_vector_forwards, direction_vector_downwards, waypoint_second_pair)).detach(); + RCLCPP_INFO(this->get_logger(), "Tracking vessel"); + reach_waypoint(9.0); + // Run until the angle between the twist vectors are between 60 and 120 degrees + vessel_collision_heading(); + auto vessel_odom = get_vessel_odom(); + auto freya_odom = get_odom(); + + Eigen::Vector2d direction_vector_freya_to_vessel; + direction_vector_freya_to_vessel << vessel_odom.pose.pose.position.x - freya_odom->pose.pose.position.x, + vessel_odom.pose.pose.position.y - freya_odom->pose.pose.position.y; + // Project the vector do find "height" difference of vessels + double downwards_diff = direction_vector_freya_to_vessel.dot(direction_vector_downwards); + geometry_msgs::msg::Point first_avoidance_waypoint; + if (std::abs(downwards_diff) > 5) { + first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3; + first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3; + first_avoidance_waypoint.z = 0.0; + send_waypoint(first_avoidance_waypoint); + set_desired_heading(freya_odom->pose.pose.position, first_avoidance_waypoint); + } else { + first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3 + direction_vector_downwards(0) * 2; + first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3 + direction_vector_downwards(1) * 2; + first_avoidance_waypoint.z = 0.0; + send_waypoint(first_avoidance_waypoint); + set_desired_heading(freya_odom->pose.pose.position, first_avoidance_waypoint); + } + + while(!has_vessel_passed_freya(direction_vector_downwards)){ + RCLCPP_INFO(this->get_logger(), "Vessel has not passed Freya yet"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + freya_odom = get_odom(); + geometry_msgs::msg::Point second_avoidance_waypoint; + second_avoidance_waypoint.x = freya_odom->pose.pose.position.x + direction_vector_forwards(0) * 5; + second_avoidance_waypoint.y = freya_odom->pose.pose.position.y + direction_vector_forwards(1) * 5; + second_avoidance_waypoint.z = 0.0; + + // reach_waypoint(1.0); + + geometry_msgs::msg::Point back_on_track_waypoint; + back_on_track_waypoint.x = waypoint_second_pair.x + direction_vector_forwards(0) * 15 + direction_vector_downwards(0) * 0; + back_on_track_waypoint.y = waypoint_second_pair.y + direction_vector_forwards(1) * 15 + direction_vector_downwards(1) * 0; + back_on_track_waypoint.z = 0.0; + + auto request = std::make_shared(); + request->waypoint.push_back(second_avoidance_waypoint); + request->waypoint.push_back(back_on_track_waypoint); + auto result_future = waypoint_client_->async_send_request(request); + + // Check if the service was successful + auto odom = get_odom(); + double dx = back_on_track_waypoint.x - odom->pose.pose.position.x; + double dy = back_on_track_waypoint.y - odom->pose.pose.position.y; + double desired_heading = std::atan2(dy, dx); + tf2::Quaternion q; + q.setRPY(0.0, 0.0, desired_heading); + + geometry_msgs::msg::PoseStamped waypoint_vis; + waypoint_vis.header.frame_id = "odom"; + waypoint_vis.pose.position.x = back_on_track_waypoint.x; + waypoint_vis.pose.position.y = back_on_track_waypoint.y; + waypoint_vis.pose.orientation = tf2::toMsg(q); + waypoint_visualization_pub_->publish(waypoint_vis); + auto status = result_future.wait_for(std::chrono::seconds(5)); + while (status == std::future_status::timeout) { + RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); + status = result_future.wait_for(std::chrono::seconds(5)); + } + if (!result_future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); + } + + previous_waypoint_odom_frame_ = back_on_track_waypoint; + + set_desired_heading(second_avoidance_waypoint, back_on_track_waypoint); + reach_waypoint(3.0); + + Eigen::Array predicted_third_buoy_pair = + predict_third_buoy_pair( + measured_first_buoy_pair[0].pose_odom_frame.position, + measured_first_buoy_pair[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_third_buoy_pair(0, 0); + buoy_red_0.y = predicted_third_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_third_buoy_pair(0, 1); + buoy_green_1.y = predicted_third_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector measured_third_buoy_pair = + get_buoy_landmarks(predicted_third_buoy_pair); + if (measured_third_buoy_pair.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); + } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_third_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_third_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_third_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_third_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + geometry_msgs::msg::Point waypoint_third_pair; + waypoint_third_pair.x = + (measured_third_buoy_pair[0].pose_odom_frame.position.x + + measured_third_buoy_pair[1].pose_odom_frame.position.x) / + 2; + waypoint_third_pair.y = + (measured_third_buoy_pair[0].pose_odom_frame.position.y + + measured_third_buoy_pair[1].pose_odom_frame.position.y) / + 2; + waypoint_third_pair.z = 0.0; + send_waypoint(waypoint_third_pair); + set_desired_heading(back_on_track_waypoint, waypoint_third_pair); + reach_waypoint(3.0); + + Eigen::Array predicted_fourth_buoy_pair = + predict_fourth_buoy_pair( + measured_third_buoy_pair[0].pose_odom_frame.position, + measured_third_buoy_pair[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_fourth_buoy_pair(0, 0); + buoy_red_0.y = predicted_fourth_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_fourth_buoy_pair(0, 1); + buoy_green_1.y = predicted_fourth_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector measured_fourth_buoy_pair = + get_buoy_landmarks(predicted_fourth_buoy_pair); + if (measured_fourth_buoy_pair.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); + } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_fourth_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_fourth_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_fourth_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_fourth_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + geometry_msgs::msg::Point waypoint_fourth_pair; + waypoint_fourth_pair.x = + (measured_fourth_buoy_pair[0].pose_odom_frame.position.x + + measured_fourth_buoy_pair[1].pose_odom_frame.position.x) / + 2; + waypoint_fourth_pair.y = + (measured_fourth_buoy_pair[0].pose_odom_frame.position.y + + measured_fourth_buoy_pair[1].pose_odom_frame.position.y) / + 2; + waypoint_fourth_pair.z = 0.0; + send_waypoint(waypoint_fourth_pair); + set_desired_heading(waypoint_third_pair, waypoint_fourth_pair); + reach_waypoint(3.0); + + geometry_msgs::msg::Point waypoint_end; + waypoint_end.x = this->get_parameter("gps_end_x").as_double(); + waypoint_end.y = this->get_parameter("gps_end_y").as_double(); + waypoint_end.z = 0.0; + send_waypoint(waypoint_end); + set_desired_heading(waypoint_fourth_pair, waypoint_end); + +} + +Eigen::Array CollisionAvoidanceTaskNode::predict_first_buoy_pair() { + // Predict the positions of the first two buoys + geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; + geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; + buoy_0_base_link_frame.header.frame_id = "base_link"; + buoy_1_base_link_frame.header.frame_id = "base_link"; + + double distance_to_first_buoy_pair = + this->get_parameter("distance_to_first_buoy_pair").as_double(); + + buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_0_base_link_frame.pose.position.y = -2.5; + buoy_0_base_link_frame.pose.position.z = 0.0; + buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_1_base_link_frame.pose.position.y = 2.5; + buoy_1_base_link_frame.pose.position.z = 0.0; + + geometry_msgs::msg::PoseStamped buoy_0_odom_frame; + geometry_msgs::msg::PoseStamped buoy_1_odom_frame; + + bool transform_success = false; + while (!transform_success) { + try { + auto transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); + transform_success = true; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + } + } + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; + predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; + predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; + predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; + + return predicted_positions; +} + +Eigen::Array CollisionAvoidanceTaskNode::predict_second_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1) { + Eigen::Vector2d direction_vector; + direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, + previous_waypoint_odom_frame_.y - odom_start_point_.y; + direction_vector.normalize(); + + double distance_between_buoy_pairs = + this->get_parameter("distance_between_buoy_pairs").as_double(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * distance_between_buoy_pairs; + predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * distance_between_buoy_pairs; + predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * distance_between_buoy_pairs; + predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * distance_between_buoy_pairs; + + return predicted_positions; +} + +void CollisionAvoidanceTaskNode::track_vessel(const Eigen::Vector2d& direction_vector_downwards, + const Eigen::Vector2d& direction_vector_forwards, + const geometry_msgs::msg::Point& waypoint_second_buoy_pair) { + std::shared_ptr landmark_msg; + int32_t prev_vessel_id = -1; + int vessel_assignment_confidence = this->get_parameter("vessel_assignment_confidence").as_int(); + int current_confidence = 0; + while (true){ + landmark_msg = get_landmarks_odom_frame(); + LandmarkOdomID vessel = filter_landmarks(*landmark_msg, direction_vector_downwards, direction_vector_forwards, waypoint_second_buoy_pair); + + if (vessel.id == -1){ + current_confidence = 0; + continue; + } + if (current_confidence == 0){ + prev_vessel_id = vessel.id; + current_confidence++; + continue; + } + if (vessel.id != prev_vessel_id){ + current_confidence = 0; + continue; + } + if (vessel.id == prev_vessel_id){ + if(current_confidence < vessel_assignment_confidence){ + current_confidence++; + continue; + } + current_confidence++; + std::unique_lock lock(vessel_odom_mutex_); + vessel_odom_ = vessel.odom; + new_vessel_odom_msg_ = true; + lock.unlock(); + vessel_odom_cv_.notify_one(); + continue; + } + } +} + +LandmarkOdomID CollisionAvoidanceTaskNode::filter_landmarks( + const vortex_msgs::msg::LandmarkArray &landmarks, + const Eigen::Vector2d& direction_vector_downwards, + const Eigen::Vector2d& direction_vector_forwards, + const geometry_msgs::msg::Point& waypoint_second_buoy_pair) { + double max_velocity = std::numeric_limits::min(); + LandmarkOdomID filtered_landmark; + filtered_landmark.id = -1; + for (auto landmark : landmarks.landmarks) { + double velocity = std::hypot(landmark.odom.twist.twist.linear.x, + landmark.odom.twist.twist.linear.y); + geometry_msgs::msg::Point landmark_pose = landmark.odom.pose.pose.position; + Eigen::Vector2d vector_waypoint_to_landmark; + vector_waypoint_to_landmark << landmark_pose.x - waypoint_second_buoy_pair.x, + landmark_pose.y - waypoint_second_buoy_pair.y; + double projection_down = vector_waypoint_to_landmark.dot(direction_vector_downwards); + double projection_forwards = vector_waypoint_to_landmark.dot(direction_vector_forwards); + + if (projection_down < -1 || projection_down > 20){ + continue; + } + if (projection_forwards < -1 || projection_forwards > 20){ + continue; + } + if (velocity > max_velocity && velocity > 0.1){ + max_velocity = velocity; + filtered_landmark.odom = landmark.odom; + filtered_landmark.id = landmark.id; + } + } + return filtered_landmark; +} + +nav_msgs::msg::Odometry CollisionAvoidanceTaskNode::get_vessel_odom() { + std::unique_lock lock(vessel_odom_mutex_); + vessel_odom_cv_.wait(lock, [this] { return new_vessel_odom_msg_; }); + new_vessel_odom_msg_ = false; + lock.unlock(); + return vessel_odom_; +} + +void CollisionAvoidanceTaskNode::vessel_collision_heading() { + RCLCPP_INFO(this->get_logger(), "Checking vessel collision heading"); while (true) { + auto freya_odom = get_odom(); + bool transform_success = false; + geometry_msgs::msg::TransformStamped transform; + while (!transform_success) { + try { + transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero); + transform_success = true; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + } + } + geometry_msgs::msg::Vector3 freya_odom_twist; + tf2::doTransform(freya_odom->twist.twist.linear, freya_odom_twist, transform); + auto vessel_odom = get_vessel_odom(); + double collision_angle = calculate_angle(freya_odom_twist, vessel_odom.twist.twist.linear); + if (collision_angle > 60 || collision_angle < 120) { + return; + } + continue; } + +} + +// Function to calculate the angle between two vectors +double CollisionAvoidanceTaskNode::calculate_angle(const geometry_msgs::msg::Vector3& twist1, const geometry_msgs::msg::Vector3& twist2) { + // Extract linear velocities + double Ax = twist1.x; + double Ay = twist1.y; + double Bx = twist2.x; + double By = twist2.y; + + // Calculate dot product + double dot_product = Ax * Bx + Ay * By; + + // Calculate magnitudes + double magnitude_A = std::sqrt(Ax * Ax + Ay * Ay); + double magnitude_B = std::sqrt(Bx * Bx + By * By); + + // Calculate angle in radians + double angle_radians = std::acos(dot_product / (magnitude_A * magnitude_B)); + + // Convert to degrees if needed + double angle_degrees = angle_radians * (180.0 / M_PI); + + return angle_degrees; +} + +bool CollisionAvoidanceTaskNode::has_vessel_passed_freya(const Eigen::Vector2d& direction_vector_downwards) { + // Calculate the relative vector from freya to the vessel + auto freya_pose = get_odom(); + auto vessel_pose = get_vessel_odom(); + Eigen::Vector2d direction_vector_freya_to_vessel; + direction_vector_freya_to_vessel << vessel_pose.pose.pose.position.x - freya_pose->pose.pose.position.x, + vessel_pose.pose.pose.position.y - freya_pose->pose.pose.position.y; + + // Project the relative vector onto the direction_vector_downwards + double projection = direction_vector_freya_to_vessel.dot(direction_vector_downwards); + + // Check the sign of the projection + if (projection < 0) { + // The vessel has passed freya + return true; + } else { + // The vessel has not passed freya yet + return false; + } +} + +Eigen::Array CollisionAvoidanceTaskNode::predict_third_buoy_pair( + const geometry_msgs::msg::Point &buoy_0, + const geometry_msgs::msg::Point &buoy_1) { + Eigen::Vector2d direction_vector; + direction_vector << this->get_parameter("gps_end_x").as_double() - odom_start_point_.x, + this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; + direction_vector.normalize(); + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 20; + predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 20; + predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 20; + predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 20; + + return predicted_positions; +} + +Eigen::Array CollisionAvoidanceTaskNode::predict_fourth_buoy_pair( + const geometry_msgs::msg::Point &buoy_red, + const geometry_msgs::msg::Point &buoy_green) { + Eigen::Vector2d direction_vector; + direction_vector << this->get_parameter("gps_end_x").as_double() - odom_start_point_.x, + this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; + direction_vector.normalize(); + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_red.x + direction_vector(0) * 5; + predicted_positions(1, 0) = buoy_red.y + direction_vector(1) * 5; + predicted_positions(0, 1) = buoy_green.x + direction_vector(0) * 5; + predicted_positions(1, 1) = buoy_green.y + direction_vector(1) * 5; + + return predicted_positions; } } // namespace collision_avoidance_task \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml index 86b8c703..64ca40c3 100644 --- a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml +++ b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml @@ -1,7 +1,7 @@ maneuvering_task_node: ros__parameters: - map_origin_lat: 63.4411585867919 - map_origin_lon: 10.419400373255625 + map_origin_lat: -33.72213988382845 + map_origin_lon: 150.67413500672993 map_origin_set: true gps_start_lat: 63.44097054434422 gps_start_lon: 10.419997767413607 @@ -9,9 +9,9 @@ maneuvering_task_node: gps_end_lon: 10.41857835889424 gps_start_x: 0.0 gps_start_y: 0.0 - gps_end_x: 0.0 - gps_end_y: 0.0 - gps_frame_coords_set: false + gps_end_x: 141.45540473589617 + gps_end_y: -0.22386536008730218 + gps_frame_coords_set: true map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" landmark_topic: "landmarks_out" @@ -21,5 +21,5 @@ maneuvering_task_node: distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame. # If the distance is greater than five we drive distance-3m towards end before trying to find the first buoy pair initial_offset: 1.0 # The vertical offset from the second and third buoy pair in meters - nr_of_pair_in_formation: 11 # Number of buoy pairs in formation, excluding the start and end gate at gps points (NB! pdf is not accurate) + nr_of_pair_in_formation: 20 # Number of buoy pairs in formation, excluding the start and end gate at gps points (NB! pdf is not accurate) \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index a131bd28..53f844e2 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -57,9 +57,9 @@ void ManeuveringTaskNode::main_task() { auto odom = get_odom(); geometry_msgs::msg::Point waypoint_toward_start; waypoint_toward_start.x = - odom->pose.pose.position.x + direction_vector_to_end(0) * distance - 3; + odom->pose.pose.position.x + direction_vector_to_end(0) * (distance - 3); waypoint_toward_start.y = - odom->pose.pose.position.y + direction_vector_to_end(1) * distance - 3; + odom->pose.pose.position.y + direction_vector_to_end(1) * (distance - 3); waypoint_toward_start.z = 0.0; send_waypoint(waypoint_toward_start); reach_waypoint(2.0); diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index 3f592be0..cdd12ad0 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -110,6 +110,22 @@ void NjordTaskBaseNode::initialize_subscribers() { void NjordTaskBaseNode::set_gps_odom_points() { if (this->get_parameter("gps_frame_coords_set").as_bool()) { RCLCPP_INFO(this->get_logger(), "Using predefined GPS frame coordinates"); + geometry_msgs::msg::PoseArray gps_points_odom_frame; + gps_points_odom_frame.header.frame_id = "odom"; + geometry_msgs::msg::Pose gps_start_odom_frame; + gps_start_odom_frame.position.x = + this->get_parameter("gps_start_x").as_double(); + gps_start_odom_frame.position.y = + this->get_parameter("gps_start_y").as_double(); + geometry_msgs::msg::Pose gps_end_odom_frame; + gps_end_odom_frame.position.x = this->get_parameter("gps_end_x").as_double(); + gps_end_odom_frame.position.y = this->get_parameter("gps_end_y").as_double(); + gps_points_odom_frame.poses.push_back(gps_start_odom_frame); + gps_points_odom_frame.poses.push_back(gps_end_odom_frame); + gps_map_coord_visualization_pub_->publish(gps_points_odom_frame); + RCLCPP_INFO(this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", + gps_start_odom_frame.position.x, gps_start_odom_frame.position.y, + gps_end_odom_frame.position.x, gps_end_odom_frame.position.y); return; } auto [gps_start_x, gps_start_y] = @@ -448,11 +464,17 @@ void NjordTaskBaseNode::send_waypoint( RCLCPP_INFO(this->get_logger(), "Waypoint(odom frame) sent: %f, %f", waypoint.x, waypoint.y); // Check if the service was successful + double dx = waypoint.x - previous_waypoint_odom_frame_.x; + double dy = waypoint.y - previous_waypoint_odom_frame_.y; + double desired_heading = std::atan2(dy, dx); + tf2::Quaternion q; + q.setRPY(0.0, 0.0, desired_heading); geometry_msgs::msg::PoseStamped waypoint_vis; waypoint_vis.header.frame_id = "odom"; waypoint_vis.pose.position.x = waypoint.x; waypoint_vis.pose.position.y = waypoint.y; + waypoint_vis.pose.orientation = tf2::toMsg(q); waypoint_visualization_pub_->publish(waypoint_vis); auto status = result_future.wait_for(std::chrono::seconds(5)); while (status == std::future_status::timeout) { From 5fb44bc4f2dd42e032357cce90552c47eb5d10a6 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Thu, 8 Aug 2024 06:59:33 +0000 Subject: [PATCH 49/67] Committing clang-format changes --- .../collision_avoidance_task_ros.hpp | 36 +-- .../src/collision_avoidance_task_ros.cpp | 295 ++++++++++-------- .../src/maneuvering_task_ros.cpp | 8 +- .../src/njord_task_base_ros.cpp | 13 +- 4 files changed, 203 insertions(+), 149 deletions(-) diff --git a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp index 16e11993..c6309cab 100644 --- a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp +++ b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp @@ -1,14 +1,14 @@ #ifndef COLLISION_AVOIDANCE_TASK_ROS_HPP #define COLLISION_AVOIDANCE_TASK_ROS_HPP +#include +#include #include #include #include #include #include -#include // Required for tf2::doTransform -#include -#include +#include // Required for tf2::doTransform namespace collision_avoidance_task { @@ -30,7 +30,7 @@ class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { /** * @brief Predict the positions of the first two buoys - * + * * @return Eigen::Array predicted_positions */ Eigen::Array predict_first_buoy_pair(); @@ -39,33 +39,33 @@ class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); - void track_vessel(const Eigen::Vector2d& direction_vector_downwards, - const Eigen::Vector2d& direction_vector_forwards, - const geometry_msgs::msg::Point& waypoint_second_buoy_pair); + void track_vessel(const Eigen::Vector2d &direction_vector_downwards, + const Eigen::Vector2d &direction_vector_forwards, + const geometry_msgs::msg::Point &waypoint_second_buoy_pair); nav_msgs::msg::Odometry get_vessel_odom(); - LandmarkOdomID filter_landmarks( - const vortex_msgs::msg::LandmarkArray &landmarks, - const Eigen::Vector2d& direction_vector_downwards, - const Eigen::Vector2d& direction_vector_forwards, - const geometry_msgs::msg::Point& waypoint_second_buoy_pair); + LandmarkOdomID + filter_landmarks(const vortex_msgs::msg::LandmarkArray &landmarks, + const Eigen::Vector2d &direction_vector_downwards, + const Eigen::Vector2d &direction_vector_forwards, + const geometry_msgs::msg::Point &waypoint_second_buoy_pair); void vessel_collision_heading(); - double calculate_angle(const geometry_msgs::msg::Vector3& twist1, const geometry_msgs::msg::Vector3& twist2); + double calculate_angle(const geometry_msgs::msg::Vector3 &twist1, + const geometry_msgs::msg::Vector3 &twist2); - bool has_vessel_passed_freya(const Eigen::Vector2d& direction_vector_downwards); + bool + has_vessel_passed_freya(const Eigen::Vector2d &direction_vector_downwards); Eigen::Array predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); + const geometry_msgs::msg::Point &buoy_1); Eigen::Array predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green); - - + const geometry_msgs::msg::Point &buoy_green); private: mutable std::mutex vessel_odom_mutex_; diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp index 9d3f7ca8..9c79855c 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -9,7 +9,7 @@ CollisionAvoidanceTaskNode::CollisionAvoidanceTaskNode( declare_parameter("distance_to_first_buoy_pair", 2.0); declare_parameter("distance_between_buoy_pairs", 5.0); declare_parameter("vessel_assignment_confidence", 5); - + std::thread(&CollisionAvoidanceTaskNode::main_task, this).detach(); } @@ -17,7 +17,8 @@ void CollisionAvoidanceTaskNode::main_task() { navigation_ready(); RCLCPP_INFO(this->get_logger(), "Collision avoidance task started"); odom_start_point_ = get_odom()->pose.pose.position; - Eigen::Array predicted_first_buoy_pair = predict_first_buoy_pair(); + Eigen::Array predicted_first_buoy_pair = + predict_first_buoy_pair(); sensor_msgs::msg::PointCloud2 buoy_vis_msg; pcl::PointCloud buoy_vis; @@ -42,7 +43,7 @@ void CollisionAvoidanceTaskNode::main_task() { buoy_vis_msg.header.stamp = this->now(); buoy_visualization_pub_->publish(buoy_vis_msg); - // First first buoy pair is far away, should be closer before trying to + // First first buoy pair is far away, should be closer before trying to // measure it. double gps_end_x = this->get_parameter("gps_end_x").as_double(); double gps_end_y = this->get_parameter("gps_end_y").as_double(); @@ -56,10 +57,10 @@ void CollisionAvoidanceTaskNode::main_task() { if (distance > 5.0) { auto odom = get_odom(); geometry_msgs::msg::Point waypoint_toward_start; - waypoint_toward_start.x = - odom->pose.pose.position.x + direction_vector_to_end(0) * (distance - 3); - waypoint_toward_start.y = - odom->pose.pose.position.y + direction_vector_to_end(1) * (distance - 3); + waypoint_toward_start.x = odom->pose.pose.position.x + + direction_vector_to_end(0) * (distance - 3); + waypoint_toward_start.y = odom->pose.pose.position.y + + direction_vector_to_end(1) * (distance - 3); waypoint_toward_start.z = 0.0; send_waypoint(waypoint_toward_start); set_desired_heading(odom_start_point_, waypoint_toward_start); @@ -144,7 +145,7 @@ void CollisionAvoidanceTaskNode::main_task() { RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); } - buoy_vis = pcl::PointCloud(); + buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); buoy_red_0 = pcl::PointXYZRGB(); buoy_green_1 = pcl::PointXYZRGB(); @@ -180,75 +181,106 @@ void CollisionAvoidanceTaskNode::main_task() { send_waypoint(waypoint_second_pair); set_desired_heading(odom_start_point_, waypoint_second_pair); reach_waypoint(3.0); - + Eigen::Vector2d direction_vector_forwards; Eigen::Vector2d direction_vector_downwards; direction_vector_forwards << waypoint_second_pair.x - waypoint_first_pair.x, waypoint_second_pair.y - waypoint_first_pair.y; direction_vector_forwards.normalize(); - direction_vector_downwards << (measured_first_buoy_pair[1].pose_odom_frame.position.x + measured_second_buoy_pair[1].pose_odom_frame.position.x)/2 - - (measured_first_buoy_pair[0].pose_odom_frame.position.x + measured_second_buoy_pair[0].pose_odom_frame.position.x)/2, - (measured_first_buoy_pair[1].pose_odom_frame.position.y + measured_second_buoy_pair[1].pose_odom_frame.position.y)/2 - - (measured_first_buoy_pair[0].pose_odom_frame.position.y + measured_second_buoy_pair[0].pose_odom_frame.position.y)/2; + direction_vector_downwards + << (measured_first_buoy_pair[1].pose_odom_frame.position.x + + measured_second_buoy_pair[1].pose_odom_frame.position.x) / + 2 - + (measured_first_buoy_pair[0].pose_odom_frame.position.x + + measured_second_buoy_pair[0].pose_odom_frame.position.x) / + 2, + (measured_first_buoy_pair[1].pose_odom_frame.position.y + + measured_second_buoy_pair[1].pose_odom_frame.position.y) / + 2 - + (measured_first_buoy_pair[0].pose_odom_frame.position.y + + measured_second_buoy_pair[0].pose_odom_frame.position.y) / + 2; direction_vector_downwards.normalize(); geometry_msgs::msg::Point waypoint_midpoint; - waypoint_midpoint.x = waypoint_second_pair.x + direction_vector_forwards(0) * 10; - waypoint_midpoint.y = waypoint_second_pair.y + direction_vector_forwards(1) * 10; + waypoint_midpoint.x = + waypoint_second_pair.x + direction_vector_forwards(0) * 10; + waypoint_midpoint.y = + waypoint_second_pair.y + direction_vector_forwards(1) * 10; waypoint_midpoint.z = 0.0; send_waypoint(waypoint_midpoint); set_desired_heading(waypoint_second_pair, waypoint_midpoint); - std::thread(std::bind(&CollisionAvoidanceTaskNode::track_vessel, this, direction_vector_forwards, direction_vector_downwards, waypoint_second_pair)).detach(); + std::thread(std::bind(&CollisionAvoidanceTaskNode::track_vessel, this, + direction_vector_forwards, direction_vector_downwards, + waypoint_second_pair)) + .detach(); RCLCPP_INFO(this->get_logger(), "Tracking vessel"); reach_waypoint(9.0); - // Run until the angle between the twist vectors are between 60 and 120 degrees + // Run until the angle between the twist vectors are between 60 and 120 + // degrees vessel_collision_heading(); auto vessel_odom = get_vessel_odom(); auto freya_odom = get_odom(); Eigen::Vector2d direction_vector_freya_to_vessel; - direction_vector_freya_to_vessel << vessel_odom.pose.pose.position.x - freya_odom->pose.pose.position.x, + direction_vector_freya_to_vessel + << vessel_odom.pose.pose.position.x - freya_odom->pose.pose.position.x, vessel_odom.pose.pose.position.y - freya_odom->pose.pose.position.y; // Project the vector do find "height" difference of vessels - double downwards_diff = direction_vector_freya_to_vessel.dot(direction_vector_downwards); + double downwards_diff = + direction_vector_freya_to_vessel.dot(direction_vector_downwards); geometry_msgs::msg::Point first_avoidance_waypoint; if (std::abs(downwards_diff) > 5) { - first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3; - first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3; + first_avoidance_waypoint.x = + vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3; + first_avoidance_waypoint.y = + vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3; first_avoidance_waypoint.z = 0.0; send_waypoint(first_avoidance_waypoint); - set_desired_heading(freya_odom->pose.pose.position, first_avoidance_waypoint); + set_desired_heading(freya_odom->pose.pose.position, + first_avoidance_waypoint); } else { - first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3 + direction_vector_downwards(0) * 2; - first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3 + direction_vector_downwards(1) * 2; + first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - + direction_vector_forwards(0) * 3 + + direction_vector_downwards(0) * 2; + first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - + direction_vector_forwards(1) * 3 + + direction_vector_downwards(1) * 2; first_avoidance_waypoint.z = 0.0; send_waypoint(first_avoidance_waypoint); - set_desired_heading(freya_odom->pose.pose.position, first_avoidance_waypoint); + set_desired_heading(freya_odom->pose.pose.position, + first_avoidance_waypoint); } - while(!has_vessel_passed_freya(direction_vector_downwards)){ + while (!has_vessel_passed_freya(direction_vector_downwards)) { RCLCPP_INFO(this->get_logger(), "Vessel has not passed Freya yet"); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } freya_odom = get_odom(); geometry_msgs::msg::Point second_avoidance_waypoint; - second_avoidance_waypoint.x = freya_odom->pose.pose.position.x + direction_vector_forwards(0) * 5; - second_avoidance_waypoint.y = freya_odom->pose.pose.position.y + direction_vector_forwards(1) * 5; + second_avoidance_waypoint.x = + freya_odom->pose.pose.position.x + direction_vector_forwards(0) * 5; + second_avoidance_waypoint.y = + freya_odom->pose.pose.position.y + direction_vector_forwards(1) * 5; second_avoidance_waypoint.z = 0.0; // reach_waypoint(1.0); geometry_msgs::msg::Point back_on_track_waypoint; - back_on_track_waypoint.x = waypoint_second_pair.x + direction_vector_forwards(0) * 15 + direction_vector_downwards(0) * 0; - back_on_track_waypoint.y = waypoint_second_pair.y + direction_vector_forwards(1) * 15 + direction_vector_downwards(1) * 0; + back_on_track_waypoint.x = waypoint_second_pair.x + + direction_vector_forwards(0) * 15 + + direction_vector_downwards(0) * 0; + back_on_track_waypoint.y = waypoint_second_pair.y + + direction_vector_forwards(1) * 15 + + direction_vector_downwards(1) * 0; back_on_track_waypoint.z = 0.0; - auto request = std::make_shared(); + auto request = std::make_shared(); request->waypoint.push_back(second_avoidance_waypoint); request->waypoint.push_back(back_on_track_waypoint); auto result_future = waypoint_client_->async_send_request(request); - + // Check if the service was successful auto odom = get_odom(); double dx = back_on_track_waypoint.x - odom->pose.pose.position.x; @@ -281,7 +313,7 @@ void CollisionAvoidanceTaskNode::main_task() { predict_third_buoy_pair( measured_first_buoy_pair[0].pose_odom_frame.position, measured_first_buoy_pair[1].pose_odom_frame.position); - + buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); buoy_red_0 = pcl::PointXYZRGB(); @@ -342,7 +374,7 @@ void CollisionAvoidanceTaskNode::main_task() { waypoint_third_pair.y = (measured_third_buoy_pair[0].pose_odom_frame.position.y + measured_third_buoy_pair[1].pose_odom_frame.position.y) / - 2; + 2; waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); set_desired_heading(back_on_track_waypoint, waypoint_third_pair); @@ -425,10 +457,10 @@ void CollisionAvoidanceTaskNode::main_task() { waypoint_end.z = 0.0; send_waypoint(waypoint_end); set_desired_heading(waypoint_fourth_pair, waypoint_end); - } -Eigen::Array CollisionAvoidanceTaskNode::predict_first_buoy_pair() { +Eigen::Array +CollisionAvoidanceTaskNode::predict_first_buoy_pair() { // Predict the positions of the first two buoys geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; @@ -482,59 +514,67 @@ Eigen::Array CollisionAvoidanceTaskNode::predict_second_buoy_pair( this->get_parameter("distance_between_buoy_pairs").as_double(); Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * distance_between_buoy_pairs; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * distance_between_buoy_pairs; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * distance_between_buoy_pairs; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * distance_between_buoy_pairs; + predicted_positions(0, 0) = + buoy_0.x + direction_vector(0) * distance_between_buoy_pairs; + predicted_positions(1, 0) = + buoy_0.y + direction_vector(1) * distance_between_buoy_pairs; + predicted_positions(0, 1) = + buoy_1.x + direction_vector(0) * distance_between_buoy_pairs; + predicted_positions(1, 1) = + buoy_1.y + direction_vector(1) * distance_between_buoy_pairs; return predicted_positions; } -void CollisionAvoidanceTaskNode::track_vessel(const Eigen::Vector2d& direction_vector_downwards, - const Eigen::Vector2d& direction_vector_forwards, - const geometry_msgs::msg::Point& waypoint_second_buoy_pair) { +void CollisionAvoidanceTaskNode::track_vessel( + const Eigen::Vector2d &direction_vector_downwards, + const Eigen::Vector2d &direction_vector_forwards, + const geometry_msgs::msg::Point &waypoint_second_buoy_pair) { std::shared_ptr landmark_msg; int32_t prev_vessel_id = -1; - int vessel_assignment_confidence = this->get_parameter("vessel_assignment_confidence").as_int(); + int vessel_assignment_confidence = + this->get_parameter("vessel_assignment_confidence").as_int(); int current_confidence = 0; - while (true){ - landmark_msg = get_landmarks_odom_frame(); - LandmarkOdomID vessel = filter_landmarks(*landmark_msg, direction_vector_downwards, direction_vector_forwards, waypoint_second_buoy_pair); + while (true) { + landmark_msg = get_landmarks_odom_frame(); + LandmarkOdomID vessel = + filter_landmarks(*landmark_msg, direction_vector_downwards, + direction_vector_forwards, waypoint_second_buoy_pair); - if (vessel.id == -1){ - current_confidence = 0; - continue; - } - if (current_confidence == 0){ - prev_vessel_id = vessel.id; - current_confidence++; - continue; - } - if (vessel.id != prev_vessel_id){ - current_confidence = 0; - continue; - } - if (vessel.id == prev_vessel_id){ - if(current_confidence < vessel_assignment_confidence){ - current_confidence++; - continue; - } + if (vessel.id == -1) { + current_confidence = 0; + continue; + } + if (current_confidence == 0) { + prev_vessel_id = vessel.id; + current_confidence++; + continue; + } + if (vessel.id != prev_vessel_id) { + current_confidence = 0; + continue; + } + if (vessel.id == prev_vessel_id) { + if (current_confidence < vessel_assignment_confidence) { current_confidence++; - std::unique_lock lock(vessel_odom_mutex_); - vessel_odom_ = vessel.odom; - new_vessel_odom_msg_ = true; - lock.unlock(); - vessel_odom_cv_.notify_one(); continue; } + current_confidence++; + std::unique_lock lock(vessel_odom_mutex_); + vessel_odom_ = vessel.odom; + new_vessel_odom_msg_ = true; + lock.unlock(); + vessel_odom_cv_.notify_one(); + continue; } + } } LandmarkOdomID CollisionAvoidanceTaskNode::filter_landmarks( const vortex_msgs::msg::LandmarkArray &landmarks, - const Eigen::Vector2d& direction_vector_downwards, - const Eigen::Vector2d& direction_vector_forwards, - const geometry_msgs::msg::Point& waypoint_second_buoy_pair) { + const Eigen::Vector2d &direction_vector_downwards, + const Eigen::Vector2d &direction_vector_forwards, + const geometry_msgs::msg::Point &waypoint_second_buoy_pair) { double max_velocity = std::numeric_limits::min(); LandmarkOdomID filtered_landmark; filtered_landmark.id = -1; @@ -543,18 +583,21 @@ LandmarkOdomID CollisionAvoidanceTaskNode::filter_landmarks( landmark.odom.twist.twist.linear.y); geometry_msgs::msg::Point landmark_pose = landmark.odom.pose.pose.position; Eigen::Vector2d vector_waypoint_to_landmark; - vector_waypoint_to_landmark << landmark_pose.x - waypoint_second_buoy_pair.x, + vector_waypoint_to_landmark + << landmark_pose.x - waypoint_second_buoy_pair.x, landmark_pose.y - waypoint_second_buoy_pair.y; - double projection_down = vector_waypoint_to_landmark.dot(direction_vector_downwards); - double projection_forwards = vector_waypoint_to_landmark.dot(direction_vector_forwards); + double projection_down = + vector_waypoint_to_landmark.dot(direction_vector_downwards); + double projection_forwards = + vector_waypoint_to_landmark.dot(direction_vector_forwards); - if (projection_down < -1 || projection_down > 20){ + if (projection_down < -1 || projection_down > 20) { continue; } - if (projection_forwards < -1 || projection_forwards > 20){ + if (projection_forwards < -1 || projection_forwards > 20) { continue; } - if (velocity > max_velocity && velocity > 0.1){ + if (velocity > max_velocity && velocity > 0.1) { max_velocity = velocity; filtered_landmark.odom = landmark.odom; filtered_landmark.id = landmark.id; @@ -579,75 +622,82 @@ void CollisionAvoidanceTaskNode::vessel_collision_heading() { geometry_msgs::msg::TransformStamped transform; while (!transform_success) { try { - transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero); + transform = tf_buffer_->lookupTransform("odom", "base_link", + tf2::TimePointZero); transform_success = true; } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } } geometry_msgs::msg::Vector3 freya_odom_twist; - tf2::doTransform(freya_odom->twist.twist.linear, freya_odom_twist, transform); + tf2::doTransform(freya_odom->twist.twist.linear, freya_odom_twist, + transform); auto vessel_odom = get_vessel_odom(); - double collision_angle = calculate_angle(freya_odom_twist, vessel_odom.twist.twist.linear); + double collision_angle = + calculate_angle(freya_odom_twist, vessel_odom.twist.twist.linear); if (collision_angle > 60 || collision_angle < 120) { - return; + return; } continue; } - } // Function to calculate the angle between two vectors -double CollisionAvoidanceTaskNode::calculate_angle(const geometry_msgs::msg::Vector3& twist1, const geometry_msgs::msg::Vector3& twist2) { - // Extract linear velocities - double Ax = twist1.x; - double Ay = twist1.y; - double Bx = twist2.x; - double By = twist2.y; +double CollisionAvoidanceTaskNode::calculate_angle( + const geometry_msgs::msg::Vector3 &twist1, + const geometry_msgs::msg::Vector3 &twist2) { + // Extract linear velocities + double Ax = twist1.x; + double Ay = twist1.y; + double Bx = twist2.x; + double By = twist2.y; - // Calculate dot product - double dot_product = Ax * Bx + Ay * By; + // Calculate dot product + double dot_product = Ax * Bx + Ay * By; - // Calculate magnitudes - double magnitude_A = std::sqrt(Ax * Ax + Ay * Ay); - double magnitude_B = std::sqrt(Bx * Bx + By * By); + // Calculate magnitudes + double magnitude_A = std::sqrt(Ax * Ax + Ay * Ay); + double magnitude_B = std::sqrt(Bx * Bx + By * By); - // Calculate angle in radians - double angle_radians = std::acos(dot_product / (magnitude_A * magnitude_B)); + // Calculate angle in radians + double angle_radians = std::acos(dot_product / (magnitude_A * magnitude_B)); - // Convert to degrees if needed - double angle_degrees = angle_radians * (180.0 / M_PI); + // Convert to degrees if needed + double angle_degrees = angle_radians * (180.0 / M_PI); - return angle_degrees; + return angle_degrees; } -bool CollisionAvoidanceTaskNode::has_vessel_passed_freya(const Eigen::Vector2d& direction_vector_downwards) { - // Calculate the relative vector from freya to the vessel - auto freya_pose = get_odom(); - auto vessel_pose = get_vessel_odom(); - Eigen::Vector2d direction_vector_freya_to_vessel; - direction_vector_freya_to_vessel << vessel_pose.pose.pose.position.x - freya_pose->pose.pose.position.x, - vessel_pose.pose.pose.position.y - freya_pose->pose.pose.position.y; - - // Project the relative vector onto the direction_vector_downwards - double projection = direction_vector_freya_to_vessel.dot(direction_vector_downwards); - - // Check the sign of the projection - if (projection < 0) { - // The vessel has passed freya - return true; - } else { - // The vessel has not passed freya yet - return false; - } +bool CollisionAvoidanceTaskNode::has_vessel_passed_freya( + const Eigen::Vector2d &direction_vector_downwards) { + // Calculate the relative vector from freya to the vessel + auto freya_pose = get_odom(); + auto vessel_pose = get_vessel_odom(); + Eigen::Vector2d direction_vector_freya_to_vessel; + direction_vector_freya_to_vessel + << vessel_pose.pose.pose.position.x - freya_pose->pose.pose.position.x, + vessel_pose.pose.pose.position.y - freya_pose->pose.pose.position.y; + + // Project the relative vector onto the direction_vector_downwards + double projection = + direction_vector_freya_to_vessel.dot(direction_vector_downwards); + + // Check the sign of the projection + if (projection < 0) { + // The vessel has passed freya + return true; + } else { + // The vessel has not passed freya yet + return false; + } } Eigen::Array CollisionAvoidanceTaskNode::predict_third_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; - direction_vector << this->get_parameter("gps_end_x").as_double() - odom_start_point_.x, + direction_vector << this->get_parameter("gps_end_x").as_double() - + odom_start_point_.x, this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; direction_vector.normalize(); Eigen::Array predicted_positions; @@ -663,7 +713,8 @@ Eigen::Array CollisionAvoidanceTaskNode::predict_fourth_buoy_pair( const geometry_msgs::msg::Point &buoy_red, const geometry_msgs::msg::Point &buoy_green) { Eigen::Vector2d direction_vector; - direction_vector << this->get_parameter("gps_end_x").as_double() - odom_start_point_.x, + direction_vector << this->get_parameter("gps_end_x").as_double() - + odom_start_point_.x, this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; direction_vector.normalize(); Eigen::Array predicted_positions; diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index 53f844e2..a8137e95 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -56,10 +56,10 @@ void ManeuveringTaskNode::main_task() { if (distance > 5.0) { auto odom = get_odom(); geometry_msgs::msg::Point waypoint_toward_start; - waypoint_toward_start.x = - odom->pose.pose.position.x + direction_vector_to_end(0) * (distance - 3); - waypoint_toward_start.y = - odom->pose.pose.position.y + direction_vector_to_end(1) * (distance - 3); + waypoint_toward_start.x = odom->pose.pose.position.x + + direction_vector_to_end(0) * (distance - 3); + waypoint_toward_start.y = odom->pose.pose.position.y + + direction_vector_to_end(1) * (distance - 3); waypoint_toward_start.z = 0.0; send_waypoint(waypoint_toward_start); reach_waypoint(2.0); diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index cdd12ad0..8a547d6b 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -118,14 +118,17 @@ void NjordTaskBaseNode::set_gps_odom_points() { gps_start_odom_frame.position.y = this->get_parameter("gps_start_y").as_double(); geometry_msgs::msg::Pose gps_end_odom_frame; - gps_end_odom_frame.position.x = this->get_parameter("gps_end_x").as_double(); - gps_end_odom_frame.position.y = this->get_parameter("gps_end_y").as_double(); + gps_end_odom_frame.position.x = + this->get_parameter("gps_end_x").as_double(); + gps_end_odom_frame.position.y = + this->get_parameter("gps_end_y").as_double(); gps_points_odom_frame.poses.push_back(gps_start_odom_frame); gps_points_odom_frame.poses.push_back(gps_end_odom_frame); gps_map_coord_visualization_pub_->publish(gps_points_odom_frame); - RCLCPP_INFO(this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", - gps_start_odom_frame.position.x, gps_start_odom_frame.position.y, - gps_end_odom_frame.position.x, gps_end_odom_frame.position.y); + RCLCPP_INFO( + this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", + gps_start_odom_frame.position.x, gps_start_odom_frame.position.y, + gps_end_odom_frame.position.x, gps_end_odom_frame.position.y); return; } auto [gps_start_x, gps_start_y] = From 0310cd844b090914d8d6331a782dd146023345c3 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Wed, 7 Aug 2024 19:07:58 +0200 Subject: [PATCH 50/67] thrust force and mapping --- .../config/ThrustMe_P1000_force_mapping.csv | 26 +-- .../ThrustMe_P1000_force_mapping_original.csv | 190 ++++++++++++++++++ 2 files changed, 203 insertions(+), 13 deletions(-) create mode 100644 motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping_original.csv diff --git a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv b/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv index bac8cf21..39c47657 100644 --- a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv +++ b/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv @@ -82,20 +82,20 @@ -2964.98 1459.55 -2782.91 1464.04 -2522.36 1468.54 --2112.45 1473.03 --1955.09 1477.53 --1551.28 1482.02 --1327.8 1486.52 --913.3 1491.01 --506.29 1495.51 +-0.06 1473.03 +-0.05 1477.53 +-0.04 1482.02 +-0.03 1486.52 +-0.02 1491.01 +-0.01 1495.51 0 1500 -614.46 1504.04 -967.25 1508.08 -1501.92 1512.12 -1965.98 1516.16 -2315.18 1520.2 -2789.34 1524.24 -3218.91 1528.28 +0.01 1504.04 +0.02 1508.08 +0.03 1512.12 +0.04 1516.16 +0.05 1520.2 +0.06 1524.24 +0.07 1528.28 3442.74 1532.32 3763.95 1536.36 4118.76 1540.4 diff --git a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping_original.csv b/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping_original.csv new file mode 100644 index 00000000..bac8cf21 --- /dev/null +++ b/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping_original.csv @@ -0,0 +1,190 @@ +"Force(g)","PWM(micros)" +-12989.13 1100 +-12792.53 1104.49 +-12779.61 1108.99 +-12635.04 1113.48 +-12547.48 1117.98 +-12554.38 1122.47 +-12528.53 1126.97 +-12484.77 1131.46 +-12327.03 1135.96 +-12337.55 1140.45 +-12202.25 1144.94 +-12147.19 1149.44 +-12097.76 1153.93 +-11947.61 1158.43 +-11948.59 1162.92 +-11878.31 1167.42 +-11685.45 1171.91 +-11630.87 1176.4 +-11660.74 1180.9 +-11514.6 1185.39 +-11370.6 1189.89 +-11293.55 1194.38 +-11221.2 1198.88 +-11213.27 1203.37 +-11064.32 1207.87 +-11049.23 1212.36 +-10945.55 1216.85 +-10837.99 1221.35 +-10678.96 1225.84 +-10754.31 1230.34 +-10588.09 1234.83 +-10503.28 1239.33 +-10344.9 1243.82 +-10226.64 1248.31 +-10190.24 1252.81 +-10081.85 1257.3 +-9959.01 1261.8 +-9948.11 1266.29 +-9759.36 1270.79 +-9753.3 1275.28 +-9619.75 1279.78 +-9525.81 1284.27 +-9362.72 1288.76 +-9333.51 1293.26 +-9187.35 1297.75 +-9078.53 1302.25 +-8851.82 1306.74 +-8732.14 1311.24 +-8632.45 1315.73 +-8490.62 1320.22 +-8411.14 1324.72 +-8312.52 1329.21 +-8047.98 1333.71 +-7948.19 1338.2 +-7934.35 1342.7 +-7780.13 1347.19 +-7709.89 1351.69 +-7375.4 1356.18 +-7364.05 1360.67 +-7233.1 1365.17 +-6972.58 1369.66 +-6845.09 1374.16 +-6759.8 1378.65 +-6633.6 1383.15 +-6337.56 1387.64 +-6224.88 1392.13 +-6132.13 1396.63 +-5939.88 1401.12 +-5682.91 1405.62 +-5527.56 1410.11 +-5258.96 1414.61 +-5074.12 1419.1 +-4823.63 1423.6 +-4685.91 1428.09 +-4500.56 1432.58 +-4308.91 1437.08 +-4012.16 1441.57 +-3866.78 1446.07 +-3608.86 1450.56 +-3235.68 1455.06 +-2964.98 1459.55 +-2782.91 1464.04 +-2522.36 1468.54 +-2112.45 1473.03 +-1955.09 1477.53 +-1551.28 1482.02 +-1327.8 1486.52 +-913.3 1491.01 +-506.29 1495.51 +0 1500 +614.46 1504.04 +967.25 1508.08 +1501.92 1512.12 +1965.98 1516.16 +2315.18 1520.2 +2789.34 1524.24 +3218.91 1528.28 +3442.74 1532.32 +3763.95 1536.36 +4118.76 1540.4 +4473.32 1544.44 +4733.06 1548.48 +5052.11 1552.53 +5306.67 1556.57 +5514.31 1560.61 +5789.63 1564.65 +6061.47 1568.69 +6337.72 1572.73 +6516.65 1576.77 +6718.8 1580.81 +6901.18 1584.85 +7112.16 1588.89 +7352 1592.93 +7634.54 1596.97 +7720.88 1601.01 +7973.84 1605.05 +8136.83 1609.09 +8386.24 1613.13 +8569.13 1617.17 +8745.66 1621.21 +8921.09 1625.25 +9081.5 1629.29 +9306.64 1633.33 +9397.61 1637.37 +9521.35 1641.41 +9746.14 1645.45 +9890.84 1649.49 +9952.96 1653.54 +10218.41 1657.58 +10275.6 1661.62 +10446.95 1665.66 +10584.48 1669.7 +10754.31 1673.74 +10864.61 1677.78 +10959.78 1681.82 +11109.23 1685.86 +11210.52 1689.9 +11367.21 1693.94 +11528.98 1697.98 +11604.96 1702.02 +11700.44 1706.06 +11756.9 1710.1 +11942.16 1714.14 +12045.06 1718.18 +12179.5 1722.22 +12383.12 1726.26 +12317.95 1730.3 +12531.05 1734.34 +12581.61 1738.38 +12716.21 1742.42 +12825.03 1746.46 +12832.83 1750.51 +13053.31 1754.55 +13181.88 1758.59 +13207.91 1762.63 +13247.19 1766.67 +13414.53 1770.71 +13466.28 1774.75 +13573.65 1778.79 +13696.17 1782.83 +13803.27 1786.87 +13847.3 1790.91 +14029.66 1794.95 +14067.08 1798.99 +14156.74 1803.03 +14283.08 1807.07 +14300.44 1811.11 +14443.34 1815.15 +14541.8 1819.19 +14560.88 1823.23 +14584.52 1827.27 +14665.63 1831.31 +14718.97 1835.35 +14901.01 1839.39 +14973.02 1843.43 +15054.56 1847.47 +15149.7 1851.52 +15239.98 1855.56 +15272.75 1859.6 +15281.37 1863.64 +15384.03 1867.68 +15419.57 1871.72 +15593.75 1875.76 +15602.55 1879.8 +15724.38 1883.84 +15696.74 1887.88 +15764.41 1891.92 +15910.14 1895.96 +15958.35 1900 \ No newline at end of file From 266ae94d3a2df76b6aaf5dfb3c167377e588cf20 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Fri, 9 Aug 2024 16:30:23 +0200 Subject: [PATCH 51/67] fill landmark polygons --- .../config/sitaw/map_manager_params.yaml | 8 +- .../include/landmark_server/grid_manager.hpp | 7 ++ mission/landmark_server/src/grid_manager.cpp | 34 +++++++++ .../params/map_manager_params.yaml | 12 +-- mission/map_manager/src/map_manager_ros.cpp | 2 +- .../docking_task/docking_task_1.png | Bin 0 -> 199410 bytes .../docking_task/docking_task_2.png | Bin 0 -> 145381 bytes .../include/docking_task/docking_task_ros.hpp | 16 ++++ .../params/docking_task_params.yaml | 9 ++- .../docking_task/src/docking_task_ros.cpp | 72 +++++++++++++++++- 10 files changed, 148 insertions(+), 12 deletions(-) create mode 100644 mission/njord_tasks/docking_task/docking_task_1.png create mode 100644 mission/njord_tasks/docking_task/docking_task_2.png diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml index fb8c56b2..801738ce 100644 --- a/asv_setup/config/sitaw/map_manager_params.yaml +++ b/asv_setup/config/sitaw/map_manager_params.yaml @@ -16,6 +16,10 @@ map_manager_node: # map_origin_lon: 150.6740063854522 # use_predef_map_origin: true # Map origin for sim_maneuvering - map_origin_lat: -33.72213985487166 - map_origin_lon: 150.67413507552098 + # map_origin_lat: -33.72213985487166 + # map_origin_lon: 150.67413507552098 + # use_predef_map_origin: true + #colav + map_origin_lat: -33.72213988382845 + map_origin_lon: 150.67413500672993 use_predef_map_origin: true \ No newline at end of file diff --git a/mission/landmark_server/include/landmark_server/grid_manager.hpp b/mission/landmark_server/include/landmark_server/grid_manager.hpp index 31ab72c7..ca336574 100644 --- a/mission/landmark_server/include/landmark_server/grid_manager.hpp +++ b/mission/landmark_server/include/landmark_server/grid_manager.hpp @@ -26,6 +26,13 @@ class GridManager { void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value); + void fill_polygon(int8_t *grid, + const Eigen::Array &polygon, + int value); + + bool point_in_polygon(int x, int y, const Eigen::Array &polygon); + + private: float resolution_; uint32_t height_; diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp index a46d3688..9eaf3361 100644 --- a/mission/landmark_server/src/grid_manager.cpp +++ b/mission/landmark_server/src/grid_manager.cpp @@ -25,6 +25,40 @@ void GridManager::handle_polygon( draw_line(x0, y0, x1, y1, grid, value); } + if ( vertices.cols() > 1){ + fill_polygon(grid, vertices, value); + } +} + +void GridManager::fill_polygon(int8_t *grid, + const Eigen::Array &polygon, + int value) { + double max_x = polygon.row(0).maxCoeff(); + double min_x = polygon.row(0).minCoeff(); + double max_y = polygon.row(1).maxCoeff(); + double min_y = polygon.row(1).minCoeff(); + + for (int x = min_x; x < max_x; x++) { + for (int y = min_y; y < max_y; y++) { + if (x >= 0 && x < static_cast(width_) && y >= 0 && y < static_cast(height_)) { + if (point_in_polygon(x, y, polygon)) { + grid[y * width_ + x] += value; + } + } + } + } +} + +bool GridManager::point_in_polygon(int x, int y, const Eigen::Array &polygon) { + int i, j; + bool c = false; + for (i = 0, j = polygon.cols() - 1; i < polygon.cols(); j = i++) { + if (((polygon(1, i) > y) != (polygon(1, j) > y)) && + (x < (polygon(0, j) - polygon(0, i)) * (y - polygon(1, i)) / (polygon(1, j) - polygon(1, i)) + polygon(0, i)) + ) + c = !c; + } + return c; } void GridManager::draw_line(int x0, int y0, int x1, int y1, int8_t *grid, diff --git a/mission/map_manager/params/map_manager_params.yaml b/mission/map_manager/params/map_manager_params.yaml index 4b7b047b..992cda10 100644 --- a/mission/map_manager/params/map_manager_params.yaml +++ b/mission/map_manager/params/map_manager_params.yaml @@ -7,9 +7,9 @@ map_manager_node: map_height: 1000 frame_id: "world" # Map origin for office rosbag - # map_origin_lat: 63.414660884931976 - # map_origin_lon: 10.398554661537544 - # use_predef_map_origin: true - map_origin_lat: 0.0 - map_origin_lon: 0.0 - use_predef_map_origin: false + map_origin_lat: 63.414660884931976 + map_origin_lon: 10.398554661537544 + use_predef_map_origin: true + # map_origin_lat: 0.0 + # map_origin_lon: 0.0 + # use_predef_map_origin: false diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index 0126688e..37491c00 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -213,7 +213,7 @@ nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() { grid.info.resolution = get_parameter("map_resolution").as_double(); grid.info.width = get_parameter("map_width").as_int(); grid.info.height = get_parameter("map_height").as_int(); - // grid.info.origin = calculate_map_origin(); + grid.info.origin = calculate_map_origin(); grid.info.map_load_time = this->now(); // 0 represents unoccupied, 1 represents definitely occupied, and // -1 represents unknown. diff --git a/mission/njord_tasks/docking_task/docking_task_1.png b/mission/njord_tasks/docking_task/docking_task_1.png new file mode 100644 index 0000000000000000000000000000000000000000..c569b6261d257f20ffeccc5d318d26b68403ccb0 GIT binary patch literal 199410 zcmb??bzD?Y+pQvk0s;yG(xHHWq;yG1NJ|St35awzh;%DM4@ilK#Lyuz^e{*XNW%z2 z58VuPkAm;_-S_@`{f)!KnSIWF_OqW@Yn=#nRrx!3lz7*!UAv>GAgg)p+Ratqf5|Nz z;1ep|dq06c*e=qFTDO3IzPDb71MeTW%IUejbg*>wFm<-LW@YbSXYs_v+}Xmy-sO#h z>*fuZ1aJ`NpMzwaElgdl9qj3~tnDnWX}xiy=N6)uaq|~ycXemD#9&DudYHb zBll7(>Dyq^wQKa(6lJBfJk!^vb(0^;doLctQhnZ}el$oVBfgpS^Sw%WU0Vhu(Ml5K zA13Zo=E7qYI$sKj30=ifD(q1zmXWXJoy0#3k50A9^7-d+v)-v@=)D7^Rnzs8w+ z5}yV7&!~TX?HTp|X_%=R@oSgoYOj~=xQM<8sXBNPdpVDAW1ND27n`0nb8zi)Zbxz- zBd@LxB=sn70an?)bL1MJzr^wP`}Rr3`}>zO3@72ZH;Dgt_-)kx{Ae~=N0IA#(s zCH`}cs~OHP{`RD-|PMY%r)qQ>(J=$0pWB}TEmxvGW2CN`H);)z750GY0aXw z1}+BmpWTJ&W&7{+w;=>&ar{@89{s&Kf6(WgD^ohX4<5U`&eN3te7IflC~^1ir*rww z*#4h|`Ohc$=_Rssd)@(j_g9zp<0Tc@=Kro?d;iD(T%3P=QPsVvzgHTbdGl&cp7Mu_shEN~*Pj*HHh;lZQCxL& z#yNe!5IEXGc<=Y7{&nA<&7+^D?BaXguE+N+BFJq%57uqEt!}s(Hno8`%>W`MxZNVZGJ;GJ8l#`=R$ATn(Mj zws>B%K{U@B_@j(o9{zw3w1{V_gIHTzZP?SC`z|LrT?SS{aZ}n9xq?m?Ib!_{JHHmA8xD;wvQdxEr$l_Y_0W92QTo;x`SDVugbk~(=nstB>vnI#cqQeIGAZpQJenp zNyAyklShs2E}qIH&p($7E0GK-bAc>YI^`2=_z-=dA0PhCI@ojlD%>1>B#`k;tRyH4 zbw_vHomvIQy!K0@NtK3Dz;wiGLsfYCoF*SF>p>^LZSTo!R5twAHqrXjz{g8a(~o ziV85o5yp%yC}4Ix4CIT%j?&@A=O1INkb`0qG{>JPaZU=9lXU8yTjzu#H*-2iC;aSw z?HL4t2ZN=aL5Ivd)^Bn(okdr{J;n{a6#N44VWW@mf@ZNPM&j9ZZehiB&l3cW`Uozj zU9I5FtIh8JaD+B&F9?J*{XJw&J18=Bc0Rl4mG}c`E;-?*XyNSo$}NXps4TNZawd^& zTW}hiriedSYU-k4^4pO8THF(|S4GNmhxI9rTL*0rnn4J!=g3?VZ)Dd|CB*1JP*>k) z=MGv8w(!Ow!$03^(PL=b^GT3@O$g5{x{+}8C<0$56I7$J>zJHTr9F~4g&G$Ou-rYq z5$is>;zrrVbthlE=n?PTI?emg(H7ae4mO&ym%%FPDg(~R^S<$MIaq?5ZXJtKTttcJ zcZ%t)@y+}^M%Lug6`)W=OT=qqiw>ewVty)|BA+(t`q<`b2&mx?K;#)5%r?>2Gxn4` z_DX+6(iB}66PuTdJN#0DK9X@alyZQd(?B(@*&Cp0zOOa-cCY3-jCpT=$Zd3!M#@-+ zm4PeB!YBFa3ZBM7o?D+8^v&1u4{rysoLNGOf5|ua?(wT#U|6a_VmV&5FKVWn4}F-> z5)b=JZz_vPz*Vxr+cC!Ex6zkbqF(+o!-v;M1rA2KGRs>y$0Ckj=$Wg^hr7G_#6(DA zLDhcy1P6viw3Zp_)N>ZN#%hTKLHe?)NNR?KgR02jDj)k6Sw}4u@+!#bU}qb7cE%pi zo$WT`qT-%Is`oTFP3Gow$Gc*3?H%L(y7tjwGsYmK!;r|7xz(9)ArZ`Se>S;j>LR@@!!3tZdB!rF zypBfn)X6?>P~`SXKn_&=a*2#d|GAiKn>{aUr8}z*bFJ%%xn?Rsr>x?&jNl$bHCfaM z<@)!dM$Y{+JIa&Er-aqpttLg3l8JphJoT~`Z(}vx0CV_I+Y%lYmg+_G3&p+=0b9*Z zoXkfq93AHKTDX0qJYkd)56UgrWlJ?Sn)Tgz67-Z{&D+Ub^mJ7>z9%UBz~Lq(wHg`EYDfZ{g?CMFwEK(ke`F?(`jz z-nM!D>b48cP?RPdRW>#+(DZ5h_GQsy{^7E2n|q>R@Um#6_VgV3wXerirQJyt8s98N zNK=)okoVFxP2N__=luxbv)| zwapzRn<>|%)p@LAvxOcf!uQjjJ#9Z-KwA4UaTfJu6I}|sV%PCw8vn6Vo z-wte?-q%>wd(;@o!d@AB!?IYxpn_VBmcS!fs~yrJ#2CV35<}x^$3U2E{HyyS+MmtJ z*7KXs`4}!)af{RaHcsBRpWhqa>BQ9_H7=Yj>3=YZ{>;B8>IRa)#3$kgWS7C@ejk=~ zK5RUusYK<_uz-%n4ojKEXqob%Jw!IVB$7`je(E9fCkuvz#uiO{VgSPhj?&qucE2uA z?&ssFXrDi|I0Ypqo&?Ok@yK9Rn!ytCpw~3|TP`uo7~Q+9eox0jv)!o0Eb(35_^GyA zw@^eu-rIs^zi8V266hl9egltb({!VLQg5H;Tc8B&EJTO}Ta|UgCk8y!rz2K=TRU@k zkwU#28gEd~L>0rswCC`mV{V;DXv~v@i02HzQ@FkGO-dVsMmS@^txkMG_5%90!x1uW zKEAQ!Hb!kuSuv0BWZtvVRIiKU#a@=?o=@y zx>nwkAn?UZ6Kc$x+Bp}8^5Ns;QBw|pySbtqt7#Fh#r%R)?&yrx$b}Rdu_Yo8R=(A-Zo@G`+1lKOc3ZBd+N-xq z(EMB>ypzX9Qt^Vr&?jysq)>~QTxeqYwa@-ta|cI-7nfbjHp#=QOi#6AWLjDE>~#c( z>{4*~h(S@r4cB!cY`{dV54o$xK7QLc>JMebyZ!z{bHx!tjnp7PhbhLvL=PlsD;jW$ z^G@@(5T2`;Ds*#5L?p|t^2M$rL>2YHzXn)-9*uBKO1KIbLDofHNXqi%t16fJH`RD{ zGABcpg12FtZ1>~h=yWq|c{jpIS0v&b#&s0T$$a*!)7rB$61;FBT=47(^-=I?!x?boqY z^HKMc?EPg+^s=XgvYKi_kejjEHAKwpog;+59l8);U9p5n@<1AniNcJdUhu*z*xV{E zY_^jnK;G>wA&kx>=ayh%OG=AujEH@&OT5yEmFEzY?Qe3pwSD7Xya z7b*xxD3ci)EaISen}O=g~x;OjcPEK!-WoT@GNA6s2= zwZfF)wl3%pWEaFE;P&`F=P$z3K|Jdn`tMfg1 z$vciU;l+0`K(1nmh+xb%j$Y{nyrzPy=oOm^wWoWw;>4F=#G9t_qp;0|Idb4#%&oHz zoh^iDHM(-3hXMJXJb-Y~Y9c4IMy+FL=cd{UA98c)=)R#{8?fVVdA7V?(EH>gJ2jV~ z4~Fs?Y&5&6rlWHgSL_T;r|-9Sf08}!4eiFgn^M%?nGy>xlOH7_%4+JQ3;o~mlOfSg z#|JcQFI`v@DW5j0ksWELXY4fP7>MdOkj)8*Gvh5+eqXa|`hDEk@9BN0RwRBFikY!= zw%>mJyo?v2e9X46O4{KZVk;3h*hgjbOh*(Ci5?OWGx6Df4C$vUt=YzcA!(|E5wG!k zIE<}UI%zDqrs?r~so{?zb6{l_J>&h;N3t3XozFzT=4NjL**7n6;7)bt+eo1Q!3l9r zbrp!x{Ov>ANf8|p@56%Mf%m6M3eObyS2wN>N5BXQmv9#j$(L_KaTE6#iZKi}Y8$%? z0T%&!br&~#+l;)6MVdw>H?H>;de{_q$TCbX-hIVIJGfc-W|Rmp%+^oIbLeZAj-)u)BYxzNYAmyu*Q(WcDVPV4tiPca zlqus4gme=d1XV@u8bC7H_2=D>n5C75Y&^#OkJIx5&fbS*Ua)h~A@e4`5kVpD)IgcT z_hR&DKJ#47-hYRv$EfLf^#`~?49!Z_xd;Kd%`ZIob(pTzI%2e;_JW+ST4%*04MBNh zJ6JAT6E%?mLv_MoR%YI5o#z60RJz5+zp*k%;4J2W74BJDVrQfwSz2k*UXdMWVdf>y zSs}tagBazecO48{cS;I11Xe7ddn@LjlE){bxXiBLw>Xun-XgOBJZnDKxqzj=IVM<~ zH)&tGy~fbL;orIIAZd>7kjde0?qpBzeDdmVt(NM0T`*C~2G{0}1_jXH?TM(I`p2>2 z%?SuM$y0a&@JtDsx84^l?e*TbAR3}%p7St$ggE4SBPgsf=Kh}&ko-^r&KcBVlE1;ysh6H_^Gkulunx~IDTxe=$w#DiqM&9_GYJXYY za8*9?12jRu$sb}{5=z%0K1?G&yZ;?1iGh`*1-#z5w&M8*tsNOLjSs!aUwb*rJW+%% z@pV~s@)az~+wjiMdbQn0K9wGyUN6pVeE79mRzm@00s&wKqDuF#!JjaG)CTz|iSzbEH2e(QSO1XdPVjSNgMx&%;>DNUKC3OSQ!(bU?`5)`gonkywj0Zp zay%}Zsa=MvvW+W+vE;R&Y49izoF=TS)*qa_Vm|^A65OP=YZ8`G#iHED#a&;Eok$r} zt1U`=(e=uT3+$9>eh7}W#%-m()TYS_5@%93+BOZ^jLVxO6^LXBH{ z%K_d3La``fz+lKm@K(^Zbs^cn{-1p!Cy+wdS9jjiho5w2{*qo0Q^Y4P<}d+@1|T1> zAHwDkqXA~?+eScGcwje|i6Bc!F`C=)$#g!;g_$3Z$U8l1gwS%nJuzTkk!Q`@@}{ZQ z%`87Sq6M%!z^6MM?y<&0oTI} z_qDDD@Xfst52i~HQ!Ntv5}}tATnheXrNhfUfX7HOfPMvlx}hy{sWQmeT}_*xXlJsP9-Mt?}Pin>yc@EcM)FUYOwcaGhEm$J{a% z<{hzeENmz!2jq@*grIAh8*d#}s^ML@8;^!8Y_tRI)-{j!X5k_EifBpkO&bEdC zoE^r`;N(M-3A7XLXV+kxcjXj+B!A|Q&C3&_;b?oddFggU z|2D9w1MvqCH+Gf#XQQqaN4J6J%;z44PQ|W3rhA`H;%RPw z1FD0&ow99ZlVw9Xtm`?Id=0cg9OD#GF&xZTg4(9%6Y}#v_&BCdQ<@LE=I1_0WFX?X zRDc$XDdS02Wv5;g-&(GOdo7QF&6F0U<8r0%st@FJKj4&{Hei2~dfmZHIbrx4d`<~- z(O#@}(Og9&Vd`vuUEXR=$-6LovJCcVb*sXA=ukvOK$o_BXN%n(BoRUW;m&F8l31u& zUT2JGK!36E8Pm&M{MS1Va=1Zp^y80N`TdD-U?P5ArEM&;Cow15{>zY?SExzCztW!1 z9YnKtlvAFSz?^}Y{lZJi4h{~K|Gj_fj8kf?&FLkn693>hmO;IqIS=z~^!58vpXq_- zJ3{~ji;8I74kXt74dH%>mn-yH5b?e`$WvEXsDMC~P25n+^(o=O2@s1$$3T9O?u^}4 z)|Gv^T9kPiwa5nT+5!#|zTJdx{9nkqOk-LUEu5yGss3gH_q1@3x5x2bqAOI3akpQ~ZU{93?0IoowO0ukT$S^fLFT&Bw0pQ?t6v~p>+ z^-(PkTdH_)in52kRlv$xJ1cyi9@K1kKT#(Abn7$9z&4NKV5sIp<$&TxcAYMAb|L$> z9PKr%2K#=re#cN($`k4h-`1%-h*o}5mo78v?XfG)f{@T-xe2E-FO{Z*PFa)aPJ<3Kq-X6^=A!DPE$EV!G$>Xs=hCe!! z3s5NT<-zA!3%%Quc-_qF2e^;+;Khf=FLb!>Up0vTe5k@*$0vSsPDrFgK$mdrnz{6Q zV5`C6kcZnChCuR~irc<)$W0O)S`CVU0=n{=y4J;AAD}3??Mr0SQY@h(%+mPK{WM(XvKBaQ)`O0HU{ZlJNM9^8~{Fl+a@gX_!|AQ^|LtfQf%lcCNKg zi%7i*biY(30ehlf$e|y&REUpos5K|8MfmVyQrCTU+3Xga6HA4ES^(MY*S*`atsZpFV;0u0?@n#LwYtoZ!4xc4 zwMPH2QwOF4%hcwH0lU_qpg10r_qsq#Q1QGKMCa@ss6q}l$*|?^dBzEXksOw9dTf2C z0T!y!JxFzzyQDH~`+z5xNt5li+kK7C3VBgI95bJr>VNDyT(r&}PzH|1`HZl0Gv^?! zor!T=({PK#n_h@Om|n5I{5s64^_KJpf8?**k-sPt5&F!QNq;Lg7DX#(V8ew9716=W zqT}LU!q%%-0ThR_n5F>~){G{RlN$VrocnGKP8C=v^H^ywUc!0Q zDCOLzy`XA^pbILo=S*R=pkmJ%rsJ~cl0f=PeyNt?KN+yep{05R&m7Aas#f#svs3Ah zpBrSjuegLKCbOJMfhBzgn~s!u3sd`jxP=k-g+?Ja@8L*{JdElgbQhx>ECffj5&BNE@2CXN%zLJ;Kco#unN_rPVJFO!=)2Inl(RF4ahGo*(Po=WO zq_TOXyquFJdhS(?TYMo)Dp2kH$Tg~9wZobhx5V?DOo1jjVVq_1CMdJ>T*uiD*UmO} z@{qut>@d*7$&frmqH0u7%7&XcYiO+$R@eOl9$Z{OQu6V_DyYShf zTT`vi5K0?Tj~Y?Z1M4oe6$nhl-|l|gz)KeWCJt1u`1>#dD~^=_zz}(G%yW)d@OYRh ztLq{r^Os+9Ft5Z&P@4?yoqF1mMe&p6@sIm{8V4HCBcco3kZw{U$qNVDM$cz8+AMe{ z`F#k37o^yG6L$b!+ue_DR0Z{A@H4OeU@CQDf$$X3 z)0a$@pMcVbsyPRT?~eNxV!MjaH*Vx7tq&DDu)CBCMA~`=Ei_Zv1h^?&Fc{;hV;lT8 zcVY{-4QQ%IKUH9sUyWo={|Ro*=sR)xtfRG<+0ELs6*|3I_tb8!2j-f3_!B?dYPhwW zB}xhh3~~p|ub=<^{-)T`C<+ow{!l=}*C)DaOkv{pC2r`X@el6CiV1BuU@y9}EuNX% z_yp)UYtmH zw@xJl@L=Z82*`Qd43o{=;`1OL10_)rSO91YK!L85Vc0+#-U@&Jj+G$3!xqJM(!3eB zG|sjW98h`|50vcF?lglsStiJT|EYc)R6q4ifO z(PMta8sAM?x?EdK;9mP|1O56>2VTCtcO8KKoJ*^oLw`}=cTc;9O4mJSPR>W#1HD}6 zW>Cbl$B?}*x^04Y{xG6dRI&)jJvcm){O*QZ8uf;)I;$=!w&sbhnJ)$@@T@5(5gQYNoSB?SJoo2gDRHIV4N8yWTVhH9CONGZya_ zw~71jXP$HM)&V_>zjmxxJRea>raQm_+{DY_#wLl;9;sQy6+1@LU6*t7{%GhF55OGm zhRUzuY`rjtkTb+OibslqA`LeTV9Fzb!Zl~|TioW$6;G@@udb2qmuX@3p9l4$%gnhR z6SR82Ys=sV$mkU}$hH^rWCzJT8p7%kuf2uL@yRjzQ~Vsi)HG7JcX?@4BxvuLL*?TZm@NSq8sSn)gcg+;}V{HJ*1NGaQP|Pl~TSEBHvam2!aNU+MxDD z_I%c7A=dzRw&4$de+@dykl0u5iel;};hTT)sIsr!$f${lB-W>2bpD6-TbCa~s*%9P z%>Y~& z6tMQ4t$Fisj?Ehyrh_>er?9r-^R@!uw=hvE&uLXLQ!uxN?C<@rn|9l$lUzXXdcN#M z+RaZ^67bIbX;tcD>YC`=;BAnj^juig53hac`YX^B{ZC2~Ayk#MR7dq`%<}Vlf8PLj z@tjXFGoT%?I2#w-n{cWp%>&D+$1oTdIUkR(a3Ig`1E`IXoqdF|gc>u1A<^0K<>N|- z^a`m#{`QHH=E_Uf`*OmZhfacvrh(sD|9*w zkTQ%R%^Y@x>(TvV{>KTbC=)$mXaX~yj7W?O0K@?Gud>D4B;^f}5{OEy@Qi4H(C=V( z>hfboARTRQOP+1Mc9zqj^+fIp>b7!&YYC{4Ofx=Al)v%gFH zf!~i=#djg-pkya)mJBeIUH)h48M`TiV(JzKBi$EgoEa^11EqVwkl0Gra-=g{D?fGx}{1A){YN|kxagg?5Wo|@LC zScfpn&AP9b$9*?gDj1>!*PhW3qW%8qV^CcANGJZlJuc}e{wx9m88$Ezm`k&yPA~VC zugo$oHi!s{1e7uZVq=!x=zEC;*6CW2(^X4~#wQX=>_stK;((Y5kWk!7!=3EmcybTw zW22@$Tu7qtE&|CHsP6$F{MzF}B;bdN7QC!^%T=qMjRHwRtTmm3HowL_-Qx_iyVmlk z=J6iwbO1Ummv0L_M6fu-5l+-e==H^UH?=;n3N*7&;E`B;?@@G3|^RVFKzv#cRALl<=PKKj@2bAWl4e`xfbZ2@qRgzGol)^l` z^~1p7ts8VU$_=76Kf4(+5I|-7wfBi;MrxZZ9?AG@&exImBM;X>n9A4#Ad>vFrn&3c zDD;)i`p|h%h&zQfXVShCX-5kH(txKS(KjNMOP&Ecb9@0p+D*cG$G|!v*GU^sj&MCe z(TcP{IjysWAx!j@tysOJdkF3lSqOs$sR>X1$h02rORQ&Oou6mMnYsF&{Q0mZB4=a)}j}R8PhT3KaqrQ|f=KzU7FXJJEE4wP{ zfmkMj7z-*7qdnZ+Jj9qhSy4_?Zh9sl83^gie*SH3wgJ9a^cjTZdFhu!kFNR;Q8vOB zunu?9P++0>_9i%>X-p&j3|?$mR0SW0#S~`Sm8+aO4L&yRc;Zmx_AC%3Et0$JCc0)P zVQ#a})0-=P>Q8PCw1e04I~`C>!kxsS67r(xsf#sO`SINK>5;pC7`jopdQJeC)1A5C zuai#Ibi-P2zh26-P2@fRrZE&GES`2HE6xDRKWC4DD51XaEw^w4(KN(osS>#CJqbPP zy`J|p8_NArMkLwF+bMMtc5GFoy(en-y;vm0Q!Ul zaz{_j`;KA(*yDSm%AkAnI(J0o_Mr&NXxic z`t1p4=bgnWojm+HY%p6(XwMb{R=Pv}QCkTBK{nm2e-R0d8Ri;7m>|%(FXbkv5#bSv zKh(zCvWkNQ@h z{!Br_Q{ft;B_da5@F?TQ4Z@L5^E(Yd7=xdW-~bJjHSHSl(b>G~`c^Xlh!}qZWC1_z zF;m1R^VD4?raA*{^!heo(s`Xb#iBqE^+BrvOw-$h-94q^y#nLK6;- zGIj1IyT`5B>uFGL>gEGv8_?~g+BO9Pu+zz171Gd3DqLHr#Oy5jg(?LlfUZhgIMe3Q zL`U56i_`H-o{nJwu#$M(fMZ>OOv%qfVj_e~@}UkIda-a5rYOxcR}mEvaElXl5kEFa z^7iPThI$N^C5`cD?%2jEjpDc)^Uuj3=C13_bTk~tqO70nMF*j@%~DcOOy8*bEmmUL3KCeI-vCcx~hm#G{BIjrom*HjoY0J zbGWmH#LNOt9=&2G0!+aP1h#FnGDQdtAesTNcVSr1{3$P>2Z;rD%96n&Y97!mZ?Un* zLh^{1>Efl`%sG^VU++G8TajG5)8er0?I%*4TYIKV(e8(->I6z|}z5>TD94~As`H%i1pJ*NVI zmdv`WFCj*+EssSsh`DarfH%{+9-^sPv7l`;oh|~#VkQnK;h4taGP0)vkqTDj2{uE9 z$G=DQ%)?~7_Ul+SQ&?jtIX?L;-WLtHbJ+|t%-sC9OLk)m`*8y#?XUBub4D1 ziKIlIB*yucFz9md0(#>=d9cw_VcMRzs9hJ}cE&yZGjq>WfU;GpTAd-I&#deSls>iOQ(<5e9Z|UDlR-A^E?zK5Sn6IX#5o^%AJ~)aRYU=8Q;9YS zZ^5h!_dn^woE3{J@}%Td z)Tmt5{IkSOv(xydm4q;7KP@Qq_?=q`59%sGpmP0O>Vo`Gf$ArgFFC6^AB z#y`r1GR^%EOnFEtSy>gytraX09H>|=PMfca84B}{!<;kkH>zMk>D1;OLyZ4hV&;}}>+GwTosW zq3%Zrde6#SSdG>Gyo61*R_XJPePpSihVHq2qiS8+fz8eqpWlb-tUz+`s(ew{)PKIn zX3wU=mUx~1mKZ?Ar-U13w+^>j42~P#n;G5qnf`1mA(6#zS?jnyrdI^0>bZSZL>ld* z0P_^_3?J4k1K@y$0x7nuU#5M1%g+s*iJw9fG&tWoWo0$)t%3IvMuG4qU!rHZv7=HF zaC!*1mY}jS*N48vTW7b-RdHL|aHLVUk4=JJCXWK!qnDSbqFMS-AbIBG{*buYPrbKQGN_pGC_zhHAeb~0j2h?=I-`L>_rxG}cUY%M2A1%AUt{e?1fvekZz zE;fh`;Nu^J)|*n?3!N<;^y4NZ(XT{lrlpu1FP`X;XcYAB9-#xwefD{#7a24tXt}uY zoYck0$|uWqbqr9Sc@tQy^uIfW0XX zXTPVY?W4~$Di+R>kj&FS?(jEj4nqwM3VIg$65lwV?EDHZyoKAqP_o$pQ)wNPY~TuDS5IjX zhVadB%4ZPRf?*E^x*tKfc!UEYZ0G=)i0w#PMsokwcROFGU{P9)`+&$J!lh{~K)tI_ zW?;UuC_i<&N9S1i0*EtP-fz|i(AC!fW*4~M1ON}Fv@o6oh_o6d(b~t!0Q#GZj3Q^& z2Bg!<&XqM%`A!M|%jNe+m2x}k^LL}WxVym3Jc4p;fJl<-BbOR?qbG1*m0xS#E4m8D z^&3hgd~FhIZC)2e zK@!b>)L3im*Ko_ONA_+^koc-keGdmHq#L3bZ(OIq2`_dZ&Z=vSq2j13-~f1=Un{k% zl3PC4J`V~5X2M&bRCT7i&jxBZXm5!Q)cVrgGsmAcdiZtI_8pJs*^x>VpjrdOy-5zf z?NbY{=JE1AZg=8iMXwJMde7It-jM^W3lJ4?ERIEwbc@tKr9aaB_WR)z_e+$|Gx;Bk z{L-XF7H|DEpa=ydIX9q_W@eU76rN#$YJg}FP>qvzgIP0LWD(9^;dbBqZzZEX&C7oH zkzD7Bmgyz|_`8zws-57BWHq4S8ngx^7PKR>ta$^f)Co!7z{-6_f9~DsF|I>Q{XzqM zY;T)*r791P&ut_c3kV*pTFd}d4>8P6$8O_Z4xrUFc$J{4k(M&=j9~)2#PDMIaINGo zxs^NZ2B=xRGspr*)xH(PwY?I|hzXjQcv?07=u5IaZ%wjZz?E$HjXtpa0@hprBb4MlD5|5By2nUh%0m2w7TxbBa2zHu1BIx+%I zjlcF9e`%@-M52Fx=VS$t64w$y13`~>151(-j{pT`gXZRA#*uVJJ#KrO4{L7ZbtSTK zfG&s^-e8JF0_keD9vGsI3+X-}5+5`SbAOMM63t=N`poxek#idBDRVSHF*}wvM)KVK zg%DnZHo>dn&I6ANYY=2M=4TkSSR}U)*HGwI)T|zzfu_iq3%>L-OZnNkM~pAL z(z04aBr5@299c;tkO4Z;Rkv>tk=7Q}b~lO$tCi?UeGn*oY73rr>g_q+{Z&Iv&o0-bXSv;ZLRzSO>L11$K1m~N zAL&~C%*{K{Hk(vUFTH}wMoQEgFA;H(*?8z3PmS0Vliau_{e>)1TmO4u;jcYcqAW)gSz>O;?qnWkyf#B^KgM=qaEqk8@LAB#PeDJyR^zO%7cg%jMKJ^XzQ>(<8y zkC~XzhmQ+;qObkNBdk3c4LEnv*`UnHYU?>Q)BS4v9Wl5nqa%5gp4xGr8Xm&}#;INA zVROfy*W}PnHP&C47J^d}<(OQ>%j?&Fe z`S9n>dwN5!qKnyjfUB_vPQOO$p zse>T3NPXnOmjZ+Bj}LPJ&R&PWmiMU*VJp#XbrMUU)oB_bkBh*fNh=_75{J6AqEuMerD_8!Xo@=A)-bq?dI zkpJr%fqk}YwqBlkT8q<*V+7>7zRq@c6d<`jCCgBaVSQu8S;m<|y*}%0(I9<)-Mc)q z=&v2$JLcWZ1`%UR5o4Q5b9ZJ{N{h0Q<*D=~#%PZCy`W&TQKU;vw@D`H$OT%Hi83r9 zCo$&ttmU)TcIqN-*0gY&aA}`L^hD(WLGFsEmx@9k8Kc9E zkQ6?kG?0xSjob*iA8BXPT3CX-rcQOVl&FzLErhfFxFn*>&J04W){2*7?Exr8y zUSI`O2v3q1nED}W?MSWOM~`-|zwYSpJq&Mu%m?LJEmjVheE!jh$4nZGTuYff*1=04 z$yYnnDW9j6O1b^=@06s7OW5Wx&;P!jjRa3F^O4Et4rIM=S(@zC*7jbvp1UDKkvc}_ zdC!rtcp}qu%#8~fGilEj$rBvNKy68KFKRhxFonER#a8@IXTI8=+7PKRiKXF;IWGvI zvf~%vHQ!Cn-W-OL(>%X38DkjpIwNBW%FQiXfM(bk{bEsA@;Swp@z$N~I4PFBO|Jp= zBCDH+UGKKRD-rhe%x7<1k*A2@xQvI(DzchfJEnw6qxui(qZom;03Ndj^wN!&zK;E2 zU;mSRuo6t+8*lgVxU0hb6PitfFqW0BdlpUobeggcqIS>G@)diVk^4acEmgDg5_ig+ z=@Jg4wm)0*D;_W2TzGwt>9!52@ecE-qhFePd`t0N-n(eKK=*EXvMOx zW`=dzV9Wz8IwI^$xE40zn+OH-)+V%8TOs8@L>wDitt=^d1`R ztv$&+Y!vJ4b=v2b+5lf!Cv}esoWc-bEJn^e?C~TeJV2hwqan}d>bC;2TRlt*JP*45 zorV*V+kyFk8su25(m7+^W4=ff_U6CZ=iIzT_=_mRSOQehsPU5$8QK}#d5NoFy7UZ6 z4q?EJVW@se7a+OU8jLC8j~>B;zkDN~op3fI`z^rPtXcQcKL7K~jfdwf?FY4lR-Q3O z0(~a|fySFo`z+Tovi-8VeiX;9t3Z?tL|Oxy1X-gRp~i4MuD^5UrY-5Fm4>^0%!9FB zM(wa7ejizdM3n6J&V7R2x8mlgXqJsy9*)* z#HPZ!+Ha0*63OF$E-wisW#R8$cX~el+x4MBfX3DS5BA5w-)B1;y!M7w+e%OGd+%It zJ;(HSHS2HBNGYQqWLlin;qMFZ>68-5WG>D!yaIGV2XLsK&%q5*Dya1cky-Z4Glx!# z8;}34d2pHON!i(1McJ+WHjyrKg}BRgqQ6C=#9PA6>>iTKs++H0>jZg4B*9E@BS~lT zC5PIq5NK`iY^MW*e|J?Qi$*TrsS8t>FLrhS+wFC@h^<;e4q3xX9A3LVx^#&J;!(kW zRyOpY9?etd*xl$La-9Io*}K{n$J)aQD0WEne|YC<*>jCV%}e_Y*Q|y$_-x) z+~Q?DVpAmU(>gV%LhFBEL#AW)STgY&ms(*Hca--HKqFjHt+wlk{9fN;Ff{q-1{>3$ z6{i;7BZJFG_ixBs?;Y=H!L%C@%e@7nkfV9-<$YNTGhFqeARPW|Z(>kmN6|HhTA8w+j+kbaGkGjDuHMN5=ufU5;mri|hI&0{gx$^K* z?qz-PzeeT6&bua;t(Ps*2yjqoW_j;X--8)sHGGx27wVepgt8@=@rv20N7=dgMPFEa z3+5?{^S-{D8!wUv%CEzvuLWr)*V~l*+t~jW$)t!prQw+* zsOr4c2>JR6Iwr!3buB+C*5JQv@9D72Pe@lJ^@?eygvV<}w8yj7FPiB0A27!_{FF*~;v_U<^pM=>IwB)BhpvEtukrnr-1ESa1j)EI@E~cXtWFT|*!^ z0|a-6BsgT4;O@>K!Gjar-ED9iLk^xc_Va5Y1yFHU6(h{vRLKQYQ5O zd;F5bH2>RLyNLfgZTug{+86oyKT-7my)}oN%>Q0JC-wh-OB6mq1l)^=661?l#dc2V zv9?k7q4me%j{mU*8zsp~%NFK_}&S?9vd_P5Y5ij`m{j~|daN!`xLLix`U)7;q# zAwMH$ALZoaNyx27-`WM;DBeGEiTztB9VzZXrHKCOmN9xz?bx2W4!uoN*qzn3rFMmV z`{-ca{Xr_+4mak#WSWs0{9$ktNJBW-&e_+@ncaN)^_S0fs+@;^cR<&-jK{1tMcFWw zidE9?Tu#q6XC26PBQ6br3miHm6M7$I#?`AR*+iUI%km$irI#$;ir6=(A?;NZwP(uT z$S#&-mQ~Xo`>waRNQpKnp2vGPbT8{dg7VL4!icD+)G=)9ahe!RZ&E|_xL$j&q~3GP zk9}c>jk$?9NZhq=nKs4$d^$`13fQ-E^ZS{7Q9_tOHjh9>bV5+nJG7(B`zHSkPMtMf zgPTxG4yu2sg`ZK0u@1Sv`YiW{OF6+^E>6y$OVh=;JMH{$bX3N;pcd!V#^hz-?Yq1H z%O@t;kBv<=2h04o^@p3>|F7E1+45gtDF;Sz-`4E{Tj(GRv;t;koDnCM4zP)kva6bH;I;xL{D`uMhOE4MHXRqn{Q!Jz~6inc@wl>{0UT znpE(BvstkRu2JB6yKThXJNydd*`bxM#0WEg+C5tnyxaZ%7%f^|JRMh$FLt%E-vP#w z=6(*dKW6B9_iOnx;D{}3ACZoCs48^xgld0x4T=JQTA*Ul!#dIa&?3f59aokLESDLw z{H0-4v~am-y;-(|JBDScRo;8BR_CUgzz?4!SS?~Orw=IW&7DV%#fv;qN{0>;&Z7O5fL8M|V)x9r=0 zsb`A~%P*QsFt;H1zWVl)2uA9ck#F1;*xjxmQMWR#n`^(c9||g2@dBXoX0;oThRGD% zg>?PvveLSgLJK>WH;^egyAWIY{b5G(hjz{^?gGgVjT7WCO72U7RQ2%00m710n8keb z7HDp6@*DH(?m)G|&h<{LOsAD901ah2WlcCvF(#J+wNQZ;9^qOZ$VmvqIUITZRsB1Q zL}eZRgZD#}z)YXqj>pQh zDwj<>=A-0IEKA-h^}qW}B~t{^h~=cmIfwuke)YZHF5msQRQxRNscIlRP04QT?JaHoo`s-NQAbu9m>cBWjeXuzE z<~axsd5Ye@lSS+03awtf&$8+2X~1S8+#0xTXtbO?@s+mvEu(patdMOf{)py-F-fkg z?DPKLh9LnPCZjzozorPqQ*0a|{@S;8_ppv~qj|zVO z=Wl2qkIO(nf9r=z-Z_7GFp;|d*afTuD3;|twpli)8)N_LI2GCT`vkF68^w z_gB{qGhWxrx+3rs=9Py_@4U>8ZpzFI+rXvWF|$p)N)>79HsRHWq$v61hXwR5cmI{FTgrvrqe?T{BdvL@2@D#P zTw$g}pi*DM8MH%R$^>1Jep2zjYu5Tj78Zpei8C%zrnzWbI4#hV_9-xV&HLsv^6S6F z+r|%tFY|d9T#f?Dxv~7|Ovx27-qe~9B+m%MM}Ul&x2|8yZyKySXXRC-0NdE)`ZxYq z1{@EN;79NhAluHq<sFu5){3zO7z!}>Ow27-O zCB*l~9D_ne_{Y5OCMmt)m+D6q9dyyutrqj~`E@Z9LXF?cqD*gr+G^pKS0TnTS)%Ki z&oG!?z#YwYv3`n^cwPfkv2a}#ix%E<{&-8^AyDIRT<0$A8Abb(>fG<3UbJEM z|EBSA{5;qd+HaSuE;>_1z;c0Unb860{! ze_EBMGd|${GxbzcXfw*#u9v5)lv{h{^*YhNKM@r*|Km;DPhpz%+n**s9eft1nGm$_ zD-X2njKwS`D&C4FSWI9s147C56KF__wqmb^oA2y~&ZBUb5G{#m(yNy%{)3FYZVxVy zFmRj0<3MEQrbU|gE=?WXG4!Sv=Mj~~v)9gTufkjjFzPZRs=1cAD-=4k%&1>=NUy

EaSC26fiM(5hP%wDV-g@S1OR!_#2D)#)HuzP}GkQ-=6<3dE-$a7E(}?Y&%jW0Ea{ zbIWtc``IT`#$2IUXyo$YRSdsVHhILtC>vaOCR!Xr2!H6K4PlC2i%05yv#e2N=#Og! zAQTdFQ%{+?eD%Y8#eypJEEP%!jX#&8oU%4Dose!Z<5S5 zk`rdr{G>sglpEy>*)fl66b~4i?RXS6dA#L~@G*4H!0cm0+UjkSPSV*j&5>m|PjYNCNf#<0gO`xG|bVtl;1b#n1-sbosG=z8I7PnU;7 zGSB=>et%4jl3m>)NEfHb3R9AL$~I*z&)lRy7IZDh?dQ~Pe(sjiLs?=>Zb83L2a0d8 zP#L>&>AE`WuPqIz)3NhU&Q%xhcQFrhF~}B67${3|{sBMSJ3&7-Xw8FnDA0k8#%fE! z8#hKRk`6H1{WqM64f7w|l$V;G)A9{((o7Aq_0C^QSO3F70+@p4f`oH4cpLn_4J+;I z@X9CjO8?t%i|Vea#{|ZtQsCqrTE8k0-{O#H@0fnpcY2ZUVw4b;>3O?t`pC{;t@pUb z2^T&jJNe>LRjU~2WW2%?6E2L=9tP1D6X5x%mu4yBmQ`VLyY63MuB!7eaW zYBRaaqa%tMn9#*~@AKCHH*Rrp-nC65^@unjBC=q28W|XK2J;wj_H<9j1j;xs_dS zpL%Z&w+5?)9DA~RKTbA2JdB8+9z=+bJ5Hg7jNbD1lTI^S_`0Y4-4uo0hSSo3lbW!K zvRt{FVNhlUZ6<^g3j2HXiGBInI7(vJYl1rVhq4#t$isqQzp*fF2`IHke_lmtOGVt# z8ruOI(1>Xuya@T2Nhpp^_|U%9?>@9q{aI$yEJ7ag z9G)IV@Sv8s9LLX49@)jQ0%Z~2rnsr2M)-u*`{rVZpHa8OtM?T;bE7llS)y7*_PV@s zL#-_8w~O*H+X~>V)$zldDwKwPg++_Bgn<$H%B|(o0}iu(e93LY%gv-E40=?Zq((mTEQ=HQUk)DB!)rZ8QG^POF% zd4Kp%sp??eGA%H;n>i^`Dg<{3AQwXM%q(UvzhB|PB%saxL-ZY+caZiRAOHj9$nvHS z_P{)fGr*vMjik#mi_?|OHxK+W8)xiNPNtO17@hxy`{Lrv%J=*4gvc(2dSuOXNFUJN z5)Hhx&9%UZ*sflxvkPQS=Pp^4sll_@BLF^ebd0$o~DKyo| zGWJz2hUQP0&gQo`e(_r3<=v;kfX=BS>f^qbhEArZ8}QZY$7@0G4i$RiiOa=PKg+6j zC@KOFXwNq({guCXChi8@+bSh9@t^qqzU)#Zt!yCSlv|+r2f|$e5dP|KymYlzK9^{M zA%l4rPChRXI@VYCM()MHI(U_O7LN05>&l3TmcO{HQM!fZe{u7vsUCe42Gc<^o#y7f zfZ>in(|-?}p!Pu@PW)_Ue3zB~&K{<=(jBSkg46inY=rSZa8YT@{@q6^?dD?xC;`j~_u^ZR933|=&*B#Rv z)0ZrymHrk1K5-$-m^Or|`pupY;5G%jc)>3qb6$_!q?M2w=v@lnM(y|xK(12#a_?=D zuC@LVA)`b95yBGkk;W3ER=M$ZWA)K#>G^UDRfrsajX?S6OZnL*ldx*S>2Gyr2&|%&7d})*iE|j=vCydQ!_T9kMXeV={3G`Ksa@ zrBS)@geZX@R!8aq;y0;=o@MlA4heh>S2#!kLQfQ+`0@&56)b8S5qgh0;E;g!Y5 zUtHjS*>AVlpLk0X9PyA}F4V)!Fc`iVPd8V@>xTfTT8G)ydcV8D49*r1<+GN{8A~QF%9i|oM|3>U0?6{muUrj-+S_Gu7q6-q z&TuuT?3!m}4vst&G$HM=xOo(MQE#~Zk4rLQ1YGI_2E}7ap-mI~*3EGR{*B|cGns0& zmc?fBGMTN@MfueAVbb{}qEy!D%bClCTVJ)IP$Cie1h0tXwY9 zh|RC>8*o!sXb1B4mAFyuT9tmL&PEMw)KNdaQ2lB~TnO}@gR)(ZqJOw;;ji$r#cIM1 zJ$73Us-0bo`rOg4tytqM~YElC0+yp*5ZTu~MTV$=MjYoTtLj?6XZ!b3TEH zdTw2P=!-AH=1PT6vMtDH;jSJThBx9CPL^?=V;#Uruxb+jh`m=m39j8biIjMQO>JBo z`xH_bdG_Zp5o=FjqmQ8!b%lTX0p)rr@u!pE)}(4ah-AIEUEtF2;k& zcEDE>h*K`ktXROVwhc(bh*XZUcup~WGBY1ep{wUWELJF7t|F~ttJJDsacv^1sWr)U znd`w8s#aKCKQ;TuhAp@-GV<4=D5`rh&@5qR$d8z0*L7Wxk)hP(7HJOwDx-3-$k0KE z`X>qs0L7hO&qHo^Iv&Y^^F9NFzm)qt>pIspEC2 zF~}YoJVx-4={#VGN$SMGtDK17Z{ygUB$RLvZsUo;X#YsE$MdA9>H@{TgUP|oep2FY zqWP87=Yp;y&AsNO!NQ0pi{0-rt^|lbmHpw&(%{+FlG)=N@U*Dgie zA-?C-8ybf*xx*&qP{3OE>fRzs(r4W|Gpf%8B>Z9~ zlw}&SOCB?9C9%St@~^?}R_L!4xO@I2#60gZ7Q{gbnNPWSm*WE@PI8-U z?y@dsSsaD$uK7_q+*(+1Tq4FRWRrm5V+l}y#1*iKHF+^4;5cxt{-Y;`7mzHw&KSm( z{;{VznIT}-nJjVh&FO^D_Jmc(8t7AoZZDU;V-O&#@tKI``&Wg)8Ufhqb+yTlKlowE z`%)0dP8oFzosAP1xQaEvO*A9d*ji)!(GQ*y09!zLgnO&{uFEAFU&)Q7az%Q%IRX^{ z$auBtdrBz-#;zC8XH1_IibZ-mM+yC*aTTBHKu1jkT4y4ue);3^mSfz}3^n0)Q4LP) zn%4n&98@yqv;F=PV;|Zo*e3t1*gmKJgB}9^ZgV0GXHF7Ig{GLw{1(5x5*6T9; zSt!CK0cBBQ_9|`wB&0%TWEuG?WHTkLqdL0tmvQQ1f|#xCBf&`b<7U{srE7K|16Qc| zx9DKAM;c@x2Q$fSZ|q%}dHcd`s1%5VfEc;igrveX#4*?F@mBuEY)nGD)S2NBQl(#6 z4So7?-^pga_Ah((J$#wGfA6NR|(s5tMFEP?J-#B*29CxJ`|bM7T^s4EZPMBOSJT~ z#=WC^vgu{r#le<%RIfjaZr1_?3q(r{1I8o-I*d>JpQmm-1(p_p5l{$yRcH6M2G4im znk6dPNq`KjB|euuU$ows5t=>5-|=V&V3erf7qe~*y0A4u5uq;#-#rX-HHuHceUPVQ z+-6`#wd|%@LaO@H?~@(UR~_9)8)%fi*OhurnsMbv#!M|;T_Z?@PRllr({IYZ#_4R1 z8!R>+;44uKdi+qcWLwEoCm})%f1SS>XXdd4+FGDmTv_m5^P=|6Q#7}81C|{+Bnxnk zJ$d)R%f}|PDG0bSF7mY18QgR=DF?Wp$`r7=@88dq7&9hV^jeKhZOiIEE*|AYQ;JwT)o%nLNqV`oB$23|pK|Y{I0Dw?Oon24bdfo=Tk&hL)@r9HuP4Qn zSejUF>YC5{=D{c9>TUiY{^W%+5FF&aKNmm)`mA%#DUXDC(vu?DoO*lmC+c(fHGiy{ zlYr}92^?UORD#f??RnEgm($wiZRhe%^_65q z1KUuhRAn_ZIJqcOPeo9f6XV>HXGEY(fq+SBfm$9J=Tos)^M?dqMBiCx_)Nz+RB5a> zt~-29H|FIQ?HE4jR&RP9fK=Dr2M$h^jcf{Jc>@QDO1oSn{Nw;IXTaTe$&@>Gu^CC} zgw<&(;og2(j6j!OMDBaM2GW(kI>h5;l)zUNv^osc9Aut;eu-gH9UINMG&!&`$J7QL zpKx>_bXT*n5TF=U#N|2=p?&38PwYabA)v6(m{0 z@AV#1t1s-XvRcD^hE;47%nC8NQKyIO{vG@$26hbqIIHUqE5CNs)gn&Fi{xqw1-=W- zjMjXscDx@36=7>FvOB3xT%!C#gKco*S^h$tgNyVkJY-M~m6V+`dbHdZNWJZtOB*!+ z6N9yuo=)8;0`5|mS_2XWFERZ4)r9g)IODTt-|=S}{u^0sKb8fQqD*||0fKs!^938^ zq1V(6w#ly9VqeV3!QS6XZfeipi9}_F<=u{mi`lWE^)0Loy z4<7ooE-gfGR%f7QdrME`_rzNxbwF6_8(V!qSMPvFgKE`0Y=TT5wpUM2YfZU%g+mM( z-h_dqI6Ss_6zVB{+)bS;2F*0Y-+}{E#UHW)ZWo^u%NyC#3{AG5=uOXiO&=5fNn0F| z5YOa4>8TnuOph|`7Gvad`o-$QSlN9&LWyn)(E%o^%V0(qW;~;t; z2dd^#eC+sN;QZ{s_S$oWlNb6&KHz%R_qNycU%d@5OR2W8dgQ(oa=U*RfQX;a@xbgT zexDSUY>3bNp<4Op@bjM*E*?55bLToq+&AtSWDb=V*ZUB!o8jEaPk?778SY2QhTkT*dfesZnmgMuq#?`!;MSF z+d|EnxkYBl^>h%yC#>r@^{=kWNcx{3_(SqbO^MSUj*!V_>Ma3Ty|j6(q&-pPnr4X( zbQY`|(9<(SPZI+A%`EjLAVQ6KV>za<$pkn5zdXiDOaF*ZLec2xO=1dFwBaqy9|6~?`CWZI*Nm*>KTb@b2aQAahq(&a?^!`5DO1~QG5s(C*_p0Z=BdnK{lo!Hys#+ynx(c8 zW9Mp%wrJMj*{sK?@<#Rf)s=T>(gnLI&G5geBDa^bO<_GA;IayKEdziP^L{8@<>>H5@7U}wU3sG`klVjRKV=D_&F9V1&C@H^0%vuy;%~IS zNHpvOB2}LZ)0#XmMZHxjY91)P66z>eE-@U+np-dyep|U#b9D{8Nu14NpiXTcfS)`m z13E+0pJTVDp)#6(b`}w4Odf_Oot{zidEAs6(+mM3c02!h zD~YB<*wiD$^9zBYUM9t?fvrLV=e3;@-<8q_K8b14_t>Tdaw}jmjhW+wT$kbP)%qct z02nmP&(BTeD3}qNZX|9y5HO}~wPSgnQY96V(;zILZPa;%_Nl)Yh)@~EWUP@pG9M@@fRfZ5QKNrOl#_}|Da zKgq8X;8zdo`ZyKD!O$h4S;BVO$?h**%>#HFue+@qhc>^_>w9IB=SyCopCDe| zK029+*^~0(tnxV{0`eqStIYgK9yhF@@FF!f>5M+3Yd5hsstg3ENDKiJ&RTh8ahKE}`(^fjHOdu0L83ZGh}Dgi%i7;e&v-UI zc@pv7U0%IWv?Cq&2nMaQ5*Y*;JFarx(t$Dq6_&4-%}$YetB{%2pI_)-<9}BJCVS`q zkom<^ZNK86VEerXx<8^)W%ZbDiz$bXNHv83?;w<*HyPY#lvR|T4%>f)4>tEE9F3Xk zw>8DhoTG|~^0;5Wc?X{&-{#!eIPyTA+S2pl3o)S^NG}k^OjD`o+%G#w)Jj7{ol6^r z%QEty<<=B;+#gD)NptNdc`H-;^!_#adrqt!F!s1EY*I8kSb=j9O-t4<&G=;*ugAhK zTf380;8Xbqs`UnZl1Ps~D_g3~xO3;Lo_XF z|IN57C4H(6_I7baK3kEMw!hi{=p|Th$*$|0M*68mWugF{camMadA@wWbX`5GV^2^@D~gis1I2DCt^8V zydhofo5q#H4{^c_`KTd$!1Qn}>*%&}Pnn?Q{t}V$B9*afuS1GPO20l}Zgw~zLH*JO zQzSu~FsB4eg7%wfPeh)Afwy)Py$x;7H*7Ky1pR%Te}K^_ev2{2td*W_Yj=eJw0_`0 zCm25#09BmQT>_7QDfPm;wn+0Hc9|Cn{}r-V5CDeoT3$b8yInB%`WxQg5of9gmiJEu zah#y}Z0g5ld;QGXY>n_Tc$TDHE%Qs{d>@H+ZJTPz0EuGd;H`{{93S?J{SPE@vdl&D zRXVz@W!;B_Q?EjhnXrbT(Tg9wL`t=?O+w>iY)>z~MXvONE5i*!Zw|I{WAf;?t=2I) z-9=M!Sm7cMapiQjU_4P)Wqd56d_L39Wi4}g=#~hsC*FVH`?oV`ZZqa7J znhT>&c3xIBxkdno(JGb9QRSBdv^R;t zhdS0f3j@^f!;%Vt;k+)3j(ntz1WKKkORp0A?}z{^2<-2;w|`sj_W;7flNA6|DX!kB zhgXws=qh8ZS_~gG2cydOeK1=kfSF;{U4v~2ce>A$&q1q>n!urcb_}xcPGP9p=4@;o ztbB@i{^w{MvlToXgU+Y8BwrBsI;h_>Pm}DV>TVJsJo>3}M*h3+Y(On~C3nH@GirZ# z)E@ZJSF8v0oF4S>muj~+%gTFqeB5V3^ypU8g^5j0;!pUzu~fRaP3*B5rTdyJxxWZU zt0M^(!r^&AnfWNv>Te6w5(<1WXA4i+*?GbKD{9=5ces^M`LZOxbj#qEF-Wd?Id=?> zg|50+2In`XnF&mme!c`Hu0OZ7z&|?Zn?2r~!4w-?g;X<8mS;N&O zqJ;1Gj?4JuS>TH4&rg)w&Riwc3-K2pW?FR#CGJRWJmUV%97f}{tntBpZ{gp!{L$*@ z^N(zOfeMxL-K&P6dBxATlEB9uRVW+CT#DWsUVly2IQ($_#fHeqVD;J{pSh(ACy9xJ z7y2peCRz9eKAJ39|45i_iCVQQOd4>~l(!28Ew=sKXaMmA=u6xe%q$h4Q{gYcAoN&* zgJr4jQq8f4-unkr9Ph885T`>2C+YPTB%7U7;8_KAw^zBk zesk?B(&Cf#1f^`Yu}cAYtH9&L+kf>Qn_F+uuXOc!1O(d<=E|)12UvF288kPJ5VmuN z+Ez1+^p)>4gAYV~W8Xo+8tg&ZC|5&5|*Bb-gUP z)3*3nrn&f#%?S?Q^P)z&1lPwsL5kKg<{ONX+X$`epsn|K*6AFZ$yRn6gY5y@be_n` zYCVkQ6Bm>2m&rYvW4Ui{J!^8vb-q}ZdQZnb#<^%n zaKUskwYZ{|8$)~5YVEq+Y{lIu%etAO9F#s~sj)i7p7lUf)m9Rr8T!^kDM2h)OK+@%fEy?FQney;E+4=)d5Lc=Yw0nGu zugi}Le%)YDH4_-ecXaQKF!-C;fa=KIEEiQ-wp&|L(;bC2e>O*-n9?}rFb`R#Z#)Vg zJg(M5d{h#Z(}2g$l=Y`2(qHl5gxclMA~D4)kNDeBBArFmm2XPvwVVKJRHWS-Pl|NQ zny&2(HAM$WEDeXDm*EQHZ zl61X`9io~QZThyecYlZ*>`XE@%_c`dyWVEzAoWSA(cr_fm5}TkBI~zCcCz2Ht8RMQzbUC2| zAHfhoni7VARw|?MjT9GxU%iHe)i1m*|Ng2KjVa&PaoGo5BV5A-*+)U~!nENPIxFy} zBcc}xK3FJ`$rv?ZZ%03QFa@aTFpz^;Ry?VdlhmU>amg6TDN_7^0Zj?21a8YF0jlDp?)UWa@y|Y+p z@fQ{0Y5$~?D!UP7c=-dqa?`+0+07TDo9Bb+#!{*oI;qsCcs?49>SzjDWmXhi!=YXF z;hWj}h0Y@3_<>{vIgv3F&a#fo$outg7RLqR><7gT_rs@Oxjwg%KZ-PEY*ueBY?Y?0 z3|U{EP2U-Ler)F2*WJ3W(xo#Fc7AE(9dnOPv{xzDv2UcvV*R0FYtAd}o1aHj-`kM4 zAr}g6u)rC#ModbO!D-7&dsb=T9G_=bQQc3Za}M27=qL9ahKM6~D#5Rb)P+Nj3ycNN zHgG_T4Ol8Z;}~loL}mlcY0w*677A{Z={9ugGe#kQIXnCoi!a3bv)acv20g^Ew6KhI*30}|4&;2Kdfk=$-d`+XF~T89On)U<6bl|Xt+!8H-_LNpwdv?<~u}e^{egm;?{c5a^M_D2% zJ+LAfaao5fxz_WG?po?weAQ;ILDgOs#($s@j+C@9|c<_;OhGcl^~1LfXOaTr3u3 z4N&KZUgpo0bIm4&*r0AKk>v=8{b`Hc+|BXL2eu>6U$_$Uyh-2+S}ajmt)o}&%4Xtp zSkIvU)`h$=qeqVRw{Stnq1FeVNdrTEgkCvvzRapgzEI=>(OwG@okFvM7jN;f&3bW1 z1&B?I-^8psF3w??mF5@0!#RR~Sn?ldG0mZ-xfeevoMBb)lT3DI7r*UyW;$FRwDK=l z1>ZF%E{M->*;_XlFe5Sfi%pC~M;MgF+fYqDw~K?VM_eA4V;t4*b*--Q*Q7SZ#ntF{ zqIz@lJw=4r;_-G-z@Ae#%N#9ZyG1&Cx#OfWXI)=AM!&6+%EoCUcdr_DZ<45h9x_== zC)@hByx%5YRQHIK+eWOjxv8gSB^vHw`>V;#B8}?!+m=^?hEagw)Z`FgT6E(Wd*v(5M_x@ zRJ(m~(JHz$XOQ}v^W{7awHz_r6fb8+D|`xF+*-F2+Y-jH2o_nr3_|!LK&z4Cdi>h_ zev4U**5X9k8)))PT0L?8IO}qgtp{_&I5+5i<6)N9E97pv%(upD8vNfR5;s~Ip6K_lE3$x8UuVHMR~rp%!uge(MR4Q546zf>x7lN9$vOeybm zaIAII>ZF;G+bft&_|1nJ#rTZ-SH@oS`hS```mJVIZ4@lnzxLV36l(G4y)tA@C_5j& zeRd~u(5i{z?WdE0u&i#3-$w0s4zPD9(+Yc@1ua_zHpXVlF* z;_OF3G1i9a{V(kaSr($+;A%V3<@(F~lMPPAj-mIE&7_}yYYYTPuN!)N@O!4IarcGG z>EagH=%N`?O7WElS}jn^!Nb4cd4y4R!@ED8T-uzVf3ZqBatrM)UH;h7BgNy{(j9t3 zKa1t4Q$FXZo@4lnn;P9gY>fQBpJC9IC%72?yxvlOXAxhBmB3H{*(j;iGfZ%vLH*#C zg9v2pmpA%aE^JxEYM|b49qeK!d1Gd8*k6U-O2z;r+4>4$xm=vwmsC(G7G0g=&D2Lf zO0=$h5~!@7XCHtm>Ko zHEy#?uC{$_{X`5HF!}EEufYd~WI)5?5N(tw<_g;+9A7wYkHPcU4)sQ08?n6|XM29V zabeA={ho5$;A=+Q%P^r&QCp{-=8YQRXtX4{g7UqmEY`18$ya#H7f~p+Qu({T ziYvaT$yYk$FCx~ysq-H7E7YtcD0(JF~_gqEA}EKk!B7yFzfAF z=F>)>S@kX;%o4RjV2&U;{YCqH5nQho0%`2j#Ju$nWr_9HQ#C0YdqNhkgDbY$|ArDe z+^4Hr)BM78!%SmjNEy<&!l}St<~0E0Tnk0-dUUJvLgoMXzd(~7(B(l89%wMA_X$ce zI)tudaTcXu)jp74laWoo$$Dh`6LlH=gbYi&Wg9Zi0vC?GXM1Me1Qj*b6SBpaq~d;$ z0?^5Ij~kG!$^PIkvR z*)POstGdQ8yGY+fT-r>UJN}j|Vv1Cb7#V1B78*hJF9e?3q!++XZ$qNp&OFZ8aWL84{3M_Sp;sP2VQ1aV+GmyIEe@Vg*6D<&ezMyIzebzwd z|K*}*d3*=)f?p=IKAiz2-76Dz{4PeR2S=}%J;k>|#-4xtYv+Nwhgb~IVpIuFVQVfX z48rV$LQ|WK=oi1YsjPa+M_=hnX2bM4KCKMBi@7(quwd#bbjdg|^jE;ACt~@!5ik3C z@%20mOZYSgdF*o)sRXG6NnH)$jNDZOv82OD!6yY~z*)Sr5+B^%IU zWH)5L-MpM5*gqabm1fp(s9!4-60v`K&{y&EwKnj45fGvw>l{<3maJw}n28%xchHgB z_hVK%i#k87r2XSPtE}YPgbD?S_^!Cuos?VB!+Y{#PMM2EF-XP^Sy zasK)EkC;<$6Zd?d!4k|JfANd7Xkaq~n_)5m1v{*b*n8UMN+xk>#m zMG=7)QW!o#;K=!Xh7<$_0c7=lwmw%py}SE~D@759SIiBB(dWxCOWdC*L$ms?d9Lyg zKeGw1r>gN3ofKZ$K0ItMY9@O>jp{hnUYXKDlBN2ook{4>FpYn7m|4I*#k!4$LBiQ$BaoN(3WhZ1i(59S{h zs64T098r?l{eU>GIwP?`SAE#$8K@hS>KZn{2@^HxfkrHPJb}9L{Hwt?eo(Ns5N6mzI$dY zE^TA(Q58#a^T|?O>gPtbJVMOg4BTaWHP(57)}+P>DlXJ8$eoCDk(&)eH2*;yBkSiV zQSv)25L7!d+DDoNJdZ@=c@pMzwS|4;)KQC_62(Dh;bb3Y(Yk*r@C zDQ`*HXkNeRx6_A8@~WqEdH?%+O7E-gS%>!fsQV`@y7Z_?Ea;)rCw|hpx4b|XKJ$)K zSx$1qM2qc<71_h#cWs%>Ne5Z0^@Hk$dllk4+Vp&FZ?B>hZSj&1NN<0#Bq{r*7oVVa zEQ($8fc#R6mSCNo6p&}09iv?0!aVT_8q(muk+Sy!P49vWut`}D@$Nj(KW9^EEVsSA zXGP;RKPS=(vo?r=M>o9ef>eZP%EqH0KI(Jr{mU&sO ztt0heJbKNzpJP$0?4IM8mKwGXm7iBDBJoJpxOUN)S4N)A#?yDS?&2%h#UZS)wt(Y(nab4}IxSPk=(+ZQ&4g{7!x4V-7it=&ZqU?vuXq zSMr!!A;LKWIZbr4N(2=0G&TuETqyBjTc)BP$vU}k9n(#G)tJF93VP17xDV{gQt?Or z-Q%#45rKiV+yTqqkH*WB+(%hdpx+U+*Fos*EWdT1q7ZKt0^fBaHNEE~t`=SXP*TcN zA2}TP=bJ8+ON@-0NH)HSTu~FzyR4J5mqVnDbFDY1eLWk(eEn^5 zvjrvRrMDmQUGm3?45fz?fCk1&xz<~_xH7y4suJFcH58y^Ou z?jl}cYy26|=jIzTRU^YKe33NLvn9WK%Sq&xICC1m<(7;fIWI)4>^k*RctKa4F@)8A zh4Kp+vhec`uC6fJ2Cuu(P5kvwdi()ExLR=oDj^s{z zy0IXsg65m{!4(;o+BjF*R5R3N6_xM}VupD~|I;mXY0TnSv?_CMM z3iYEGmk7O8;(+9mSt$OHF8-YQ97lxRK^sT&)-9%N^$Gvj?%Gr|*VFe;cja@?=sKG} z&ab4Mg=3#kH|6(=Hu|nLl59D$-KV!{2;WPP$NLviTY3DA>do?-nod%A^p8T?HLzQk34* z4nO6k#u|vOSEK*-?yaSLr|Lo&lLyuq)AQZbVFjLR}E;~s1EsetuC8}_J zUM;6r=}hv?h}t@V8AKzq@6f69e}5QPUjK+q;kmXiQH@7R4bMt{1goc%Xk)*_%X{@@ z{-+Bv7!h<|_2&0ary`&H*kTG{2uaFnyf*)jeA$E`M;3beth#lMhNod;$mndzATVAf z!JJu(2PGUX-Rt^V)zrwDxPV-q!c>o7H6bCLNqHt7)es{!o)q1hPnn>sa?ozGn}iVz zfhi8=XrSg=)@D3GtrVJa=t$fxyH{{bFbFCwh~HU)ihfHrGKS~MR1`n7A&xDC?~?D)=ErGS8=(dI3WVmq<{Kp@@JhNf6kAWV&8@o7 zI17QBFrEkgbi`^!bk^aR{bI0vW*vQInv_3|o}bDfmh$AW4`G=GyIyT&690~m2;Vh! z*#+S!Ka~cp8z49~m~pZM^NbN|Iq9mEnR2fuOj!IaudJgV<}a7@PBm<7X8gnxrundK zC9=cHCHJuzqc`J=@34tx$H&ya$kxU0B#2V%5&|B&-OmTw8>tdY#_MZH4%c<$(i&{G zVEbzu`;YxUFCVrhVlN$ex!$SX4Tf#Gs=63d^im91nQ=ww^4GHv?GfwKot?c$Hs!0A zp<5*=s)m)&>JTS>r)8uY;?RD%5OuE!7GtwE8GWzem116VG>BZwNurkv+RBk*+;c| zlbBD^+*y!^g(hOQ5G9jHn&x(FiD8%V?L8`;YBRBnqq=J5dFI<19@;pIeOqrqB)1Dc z)i<*4@mSuEm-Ju3(!o_(si;=$er&U)-01`I)Ua9kwq*>t5DH;-ii?LrC$Fb9`vthL z!^&fDCd9Azm^~n77p3*vjstX`oaGn5$U_rJ zqkk#J0v@>4+n_`XJ|5m&J&Ura{Nug};=rrl`Bbdek@0ODItz;m|9k^>qBEwnLTfojHGq zE`lDKSvl1&wx;Y~ObMgEI~y5GCRP-Y5$t8WXTnlN_X=kGG|ygZARXfU0(Y4pLJ_(R zieA?teI3Hyb~VT(w6x!-DSJf`;|h|_B~IO@FJzA{h?k~O?l6*W>P!s@#_OAWeevwa z)`CBgB`@vb?S>;w&l)&3wUT=*pdQ=vevWdG8=Z;)GJopxe`q?#;JDjxjmNgtxUp?C zPGdXS*hV+lB#j!|Y3#C*YxG;L*nKpq8uHBJrj~jEgH>Ti6(vV=UJJzqs{E2qpWt;W`|OO_#-AA zIJsI;huBGwMXjD#N9qi(7K0P5a&W&1ntZOV2(lly=#oiNpj+|if=Y668KAg{r7tBI z#5ZU*~=?n$G&dAMmfX_wLm~5WImZ{s>UfHS!h=tp9I0b76$|ZFkD*4bsw;rm{wP*G@ zk{>A>kxveYmPT2S&6-r_Ht-i`ca|$=+qODA{&jC=M$>FLPO^wp$+mgMRe2F=j~K0a zXY6z#%lue`IZUo(N@mQ5u=YH0xl_~DL&sv%>QadPkLnjq0(}LVCCLODSe2aSe?JUU zb6AsWGV~7uUK;ueMK09<^L!>z^Z}Ri_nXJXGd7y}(idY)R}OXarVy!iycXc$j3};X zhm~0`R*T7omXHyS+4-NJ53iKBZYiDlL<1%O`^?|mMfoC9tb2vSB&Eu*rBkyV6&K@8 z^q!@O$jPY_|8fkxj%i$?MwnFm11&kJ+(Yj)YH%gk!fGEWEaMS-8Q?HS(BzJ-EIL|es+ zw-d&Xb^kZzwfFn#vgU7X=PX{`Z^E(N&lzMNPsY>PGF)dfmn0^2s7WjaYJA0Ahc@}* zIqQ`rBFk*q4wi?RiP$aB(*7#;6PG(V>#>99dD6>kiD%#j2 zP6rMP!cSe$$}doL1-(gz8+*5<kTODVKNHF(T~&Li@*U8~E%86v22H%6_k(5&!$<$Iip=sx33DdLQHA$>FW*jd%xn}(h@}e zGUS>9UCz=zoc!euc%JE@~8xXD3r51+}QT3TxJ(-({@M^0tUG7yo>PKx`Q$0lrq=&jX#)9G?XzE zY1H)B(-^26`2fj`!iAEv!2oaF#3Fv;Mlr~#Kc9-(EvK9|3<~pO5cd|px7`D3ndZkK z8>mnYMjXkcZE4{J&b?@;zB57Ju6g(5X_`_?%`&MkJ3sp{MGW4Lzf1^b|;{8=l7(BJTLx|{|&Im+}uVw17VAw`=9#V zVkyXt+?Z(f!@2KOJK)7t#-m-x?B;q)Y=kTPR#-V+NwXOGJ|s|L!C$S}CgY%IZ!b({ zIJn%spCoN;RplXhb_xqJaf)o9?9O#x#B$iFi&of-Bk`fzf^C(MgZU~G`g5=!d*DQ| z#5f?LmnBgo1-i>37i3QmyT?A0s~%7kqiD>Ipy=~Rf2_pOoP-FS`nz?k7%J7#2tHL| zwedUHe@yfBsE*zK#+^--#?DCoSO>-ITmE25F}m70&na4^JUBH z{$@=JPwe4y10GfAJ7jFv2Yk$9GymGU>EphcfK1qcYEpZjONw_9E{z<4y8y}g`OU#U zb#O-D2zt>58`?2yflVtKYrZA8Lk#xaz-2ymQOAUe4c)wW^m#)U2}wtP%7>3-@}1PR z-=|d$i@M%fW|CLb1HItED>8-NQg+`R6jEwiT%$ql6FV!YNrx^S%@cFoZ}I)Vnd_MH z8e2wtFL=NPx!ld7uMr{1b^%>%5FUhDfvb^Lcs+kihU67>1EP}2yJeI}3#q~G#XjTL zA87rc#T3=ia^a9eyO-Zop4q~d;M-1hK)(L*g`Snvx4*_;p}vuvC1!R;*Z)QJp!Sh5 zS%p620cUlJ%(UQijIQpPWGye=q zL~hWdZ;nOisAnt?CT`+gKh>Y$^9*m91q8^h3|*4d6O_1a?KtaqUEji2@=I4Vz5GTk z9Rj(d@TMnv+_PIXqO24(NUlQVA5>q(CcFKwhmEXHT7BjQ*4o|dejveGHZctVcCm3& zIJAzno}Rkp054zdLl=s9FuHwNhK_k?3NN7O2ZfyW(6$N0gxU+gWE;K4Q4`HNk=1>^@K1rBc)<>}HrY9_(2AjC^VZ|cvsuQ4jWx0u z3w*qmE-dsLbG4vmxGQfu7nrk5To0r@TAc=fwF&LdWQsL8lUf`N&rrrLeHU*&?g%9& zg%>N~CFNWz^K}U9hC-~}6#$e^JixzH2NjTKX{btA8fwFQ(9K6WYezQYL^g9u66rs& z)y<<~of#-AA9`7B&rHM=}Bvuv4ev0!T zzb0cL^XW?r8HxYWOi`b|Yj2eBtJUEJ4bFpBvAto9aBko2xDN5lcr&a;TM|G*3j6N; zr(FYnd{Mn$Sk8(=ReC=lR#0t3HOZAYfnf>52$aTzxF$h?d_>L-j~Uvr{^SUDh(d5*7N(kfi3)|QKsO4RU@6b7Wk2K78a^jA|q&s1Sw#M>t@wJ*X8efd27saBM{CMWqJdy7(gr)|Eh+p z>lGGHm{MhVv3H!7+J@%%B1;=c-49~;ae3jw+NfRR2~_#Dam}`lFJ>Wm=k{`%&(ui5 zbz7+{sstn@?03U5#^MStZH2DNDa&)@RLapsqFvF*1f zO1dYW63;po*eLK){8n2M>MVowS5}Y2CR5Et)(+L)Z(zE_)=oc=^(T1!z#3y|PGQo* zRFbCx;cU+69UN2VFY;5yJ+jc6szYKzR<^D{IR`RUsE^qHi{L!AY`;e zEr&8$6hp$wVZDhV@2+Oknoo{SLn2(B`s^HV%t$DMUlE#`~a_L!>Kli&n{5k7ZLJUN{nn9y;(bA5$QGEi`!O?II( ziQ$0jiVQVaPCxr@ z!#>VZ0f*Z93!@@-=)1`9t02jx1GWsb(Z?O?`TLjb&nqk2=#(04Rl0Mz&qZm(wx1-<^l7kwv|X|MkAbT5h(>;ZXjbIo3vs`R(Hp$aP%t}l%_r%fm_ zDkkWp)(3e$HjAY-E@~Ss1hQX8eIq3B#dTR`gwLva7MnHO9zm!iN#|aBC%;&>MV8mi z9z0Jm_SJ!5|MyCJF@%=cZ5U%JeKT<)nTTRi0x%bbroAyI;1eHK)x%NjPt%Lpt+Tj+eyZ3(H{>W&f!bXZAUzg5`Muzr!+<|o<>g6E&pu)IV@N9!&MMO zrLbf{>w6~sy0C6JO9xVbk*sAl!PWgQ_=j1Zo8WknZ9-W2T_9-}>prs}-H5nL1aOOM zZDHxgTfRV;Z0b>!#}K$t!Xj{MbDS){F!cw2=AuJ71eX-E_sfg;qum1Pcbt73CTUL0$aF&^QS>Fs+dhf3FA<5 zIiQT|`>OZt8~7aO`aOlFqIVs%gMaf;#L<m+ldfc!spO9xxg#hb6|<2@8AK5n z2A{9`#}j%wDKp2wkYsKxo~k)(o*%a-u${ZmVAY=M08iJbTfFn1DbO$4yMCzm0_}GJ zv=R7rL$LFG(Eg0M&wNDegAl#@LbzKQ$qprXj|J)dXojewH^+9^FR$pE{fgJjbFgP2 zHiL#_hLsNr3;LZZ^!yPEek9PQIb48_6eUbll$6m_KfR#?z>lx>((vQCq|iRys+`!i zO#(GI=tpU-NBnLt5sNI z5jyFa%Z2~}*;L`j8O;{I55nLr@myiXQF8R{Dblg?CHI+!N{Z74;X_-))~p3cL#(jm zDmyi&8)%GXHgF|w)v~r<6`K864)Z&_luk|QrmdtszhC;*Ubv3Xd6}MADF^y9tu zZdgW@6kyo|N4T83X7-##7Ar6^KOVn86bwp%z$Q6Lu0j}zVQ&vhdDVjQadatmV9Sx%03Wj!-ZYFgollwl6$g+_0 z@JA#^tOTpT@TnRN&Z03i*@vKMsQZM!Y2sEtqqI`>5e3Qh>@ur(N$0Bak*Dl&Fc0bu zpFO)PT@HegBD-Iv>5nryDzfqVzb*IVGb9Uieq&(a`c4q5HajfT4Kv^VdbW~BS<)iO z0oO1KHSYt0oX;`4$e{V@^<=%t+8R(_nX%YU=&oO8_Ul}K7bpOK5((T-cttFdTHJ#R z_=8#0z^!p)Afv>^)Z)=VyX(4iS^N0%RSxI+=+vK*=)u{iAKVQ(WV(beomwOyr1=g$ zf#!VVir7DMrFERPfOBzJ#j zgN7vU**P75dzs5_*28*QM>N zdAxeVm_k>TS9h9l+x@71-fQkYeEF?^Cn)248~aJ7-1W-Y&Pp0DDS@X|?eh6&)8$Y* z2drI*ejF=$WmVo5OlC4b=a$05Ry`M6VOzXhT{_+&9mpy-sb3jdmm~1FfLr z;%@fClgUvx8wIyh;SJ!lu&sUTTn`(`4Y;AHLW{tPDmuiw1jl4>UwD>(rNGBsY{`1Y znt3|ScmUn5FM+6jHg&_rT-E!g_}G^c)LNZP(`33cPODOM zu+~8$K&PCFU9PaGl|);BWI0#%3eDB&m0*gaBLC|-gpju~Gpn{Ufo%+%_ z@xnJmhSfW9Ld|7_bj+H$G}Q_4d;&f;d=sNLqR=T_wx?u{G+$6zt!1g4$CNrb`C z$NcP1bpN{}l|^0B4?dG=2Tl_AW3QNSIN@1pEUF+P)~#c9GxLL4`KXLeNhr?Cq0k@= z%xFlfgs_E&!t5`%^~@{n@p9)!T?|v`bkkijI!@rawwS#;_%h5~cSj{;cF0?!1eaX_ zCg6IyD@=>DfLYka%%4cb7+|7jc?c{-Sy4^@vp10X;11chH0m6%wg)<)u|__RuDowl zT&!bj7MduCUH<7Wc)*&(c2IO~Ev(xWhkuhC1 z;F`E0GJg5FAaI#V>wGko)@I?MKhzApcLVYpGF3*3sAw2^{24Cx&L{C~PXB-^i^4NC z1xC~H?GEGQPE)he?G61jXzOkl+^(}0wqVQI7+Tr{tRl?vhYU+K9-8yh$_M$?>)K3I z+<E1{xSOo4p&PrAxkC{}?n7$wQU4X6^*FOH0xW_&$Z!VkpzjA}`6v&zy?`W{Q zcE%Lwk+&|8j3DL_9eDr5N_`!dCrn{dx3}i@Z8^c8r?v`$qn|CL6H>@A^78Hw^Nz!s z;Yc#U*%sxIi^=w};dL zW_rfpLJiDTG}ERnKo)*1F-?^R4%3Y3Z0GoeX&qNhN`Cuem;M1T?f-bj1VMDZf?Xyj($ZVsV zWBzL_GYBF3aZuDxw1A-dS?+jdBCwK&8D?6X=Dc|=HMhw194EgGOQ z^g-x5j0#nSv>EhBOz-DmN@BLA-mdNyt(%CWL$2sKeF9y+kNZHT_S8`$`ks6KExQ6c z;_r_8^1ZZAJc*{fFTL9sE8$VhR(mX$oeg+q_0^FkkDS0qHQmiXST81wAW8&crYfg8 z&s`R~%w}*gExW_zrI<(akhGJO?n++nhHFCDigpH2Fiu(6#f{pb&J_{!vFM(O5!P3% zzVUrdpjXp&M7tfR$#uoE1Pr7J$s~)X&zk3y=)<1eQ}=({fUzP*#SQ&)xtko|eQ4DG z?nO#INr*Bk_Wt}+L7Bd~wymcqJ?E_5IpW!_wT+orYcFQ3$QaD8bj3ZMBmQG|U{!yf zgeuKuVL$nfj!#{peo!N%ta%4m^>RC?P)q*s^VZWC3Zd4GY8(kTd@i(yP(f-iCaG$% zO>aqcuSlpJTp_fxIeSOOm$kC_%n361=Tj~pCio{UURZ4gz?!lFPt%%N+noG?cpFGW z$XSxKt-zODFG1i!Mi?xw1bPt{cT877dpxCbnn)inq`YKJW3t#PhHxJ#h~#vUw4%ZO z3@m~zsf$85gvRLkLOX^3dkM9StOlIN|0g^BjaKf;WyhZtUO5~w?%KU>_TTHx`3&3* zaxfBo58S`EYd9e0I><1$X(fjnt{-ROBtsFeoZLB~LH?8Z1o~3s+vh!|2jTao7aj7? zj;X?^AN`biCQjNe{$=eaNf2+`Yn#h*G{<4J3O3_cPy!WB7-q*AJ*g*b)CrXc&wE0j=I z9xyU>ID_1EsiA0?U#fAEPXafM^IiheBKo>**B15)7O=2E7%{baYxQ+5*m&n`A?uL7 z{&aA0ixN^(V#OALar6?!xI=X??`oiIwQ-w%X$Y^AP*^gq2uC=5#5g?7B8OB=6N!x? zC&OwcYk)tG@Z*ck@925+_yfPcoAJScj#UJ@bee(}0n+{Gq`6 zzXTP!{pRKh9#8r_LDl9$7bT&o5-l6paiDtGJS}Qq#h>d(w@ezVdwb@ps}Zno4ERR$ zvR~^EFW^?j(to^-n58@-X>a6|1s0|uZnjg<8)^)N9nvRI7j1sIx;pi@yxhlblX)FH zxXfa#n@c~rXoE&ZbwLV?H~oXI3?Hh^E;*VjBnV1gU(;SI{=2Xi;>DRwu+S9BhTro$ z&y*emUs^BL$F)UX_;V}?if+y`o4BxME~w;7e`42;2NN7uIBed)FP@bgCjGS}maozf zADuc-IV`ZDq1C0&voXbbFfgU{5ZMdh7VP!@=EtM&%c|(Kp7l07qz&a%4D@udiKbDV zfRc~M)FO*`1Dt5Ec=K3hr32|(X0#I|opg+D>|Z$r?^d+YE}g92-f3sH06I)R+9+V2 zA;H%dbsHDLeV@VOkmZ!K_wSP_4GuG45CQo3UF7q$d$)e`9T%CA7zLhC?qtPdR%<1y z(0cf%mZy)3bj|R0HeKo0-0#|{2J=Y}#HkBOd&S91!X;eKM&=1l$F~t`d-r;Xu0Tk_ zTZrud4a0!a!PipGeEMhTXAV(iKw)%sz*_j~gd+1m`hdo!daEHN@T;NsfrmqO^ZSTJ zCX!>cX&;VR$&z5ETx^zo+L!0C!2A6#&o(v+%GGa*+X9yE>%Lti^2$qzOf;byniUp@ zN_O&aiGkkIT|&GnqM%Kb(ryz0b@=fSwSVG+WcZ4gK?#3!sIpUfN4~e!)+r>{kB2*w z5QPH@g%vZ*3&T+*6#;2<>N-k`P>25zZ$=zdUZW9gN|l|MF!AQ zf5Bmb^##r#NAR|*RFbY`%5c$=9<%im&E>%|WPzmR=t||v8OjRY^IE--!)L6cfjSD? zdptmAGW_!OyNa0cq4jd${Z~SdFz0WTc*I%i7s<`eps}xUtF`y&K1rj+1OpD=Groy( zJ`q*@!|^~RRohbp23e+&WWisXPoSPK*u|NvwjR8{(w*X@xU!iQ~#aFD| z-0fZiMb$^h)Uetr(;@Z^STIJZrg; zoNbG)e5J4k-B*Rtyat=S9<4XUSuU;_@sfjnr3X4vevIebqX=R)a6+<=?RxF3%s~>s zY0yAisv-$W%pi|JR&eDO7mn5=$|*3c+Q$R6=pokWu+8pvtF~3OcZA(JoORszrT=n~ zPcN~jSg>(L?9SI%7;Z5w!d9^M?4C&1*sGWS11tOO6Vb2TmLOmtd4*h$x6HXVjrO$m zAc<(%a0{yHb@5Kh#4R~O-X{MLI^MERqm+(aoMgXgg7)EY^jEgOP2Ww27cay>p*AkG zIJ;xk^vV-q8MJNV3e)kNK;`_1`?A9@FcqM)-Z2RNQB9s#6kmiTuriU@eC1t>(2LaT zs1}uZ78Ka@4nw}Q+J+P2=NzpC1KDfbGWPnoBy6180Ldg2mhg-OaPM} zkQ2sXp!O9kZ`j8K;ZN3w=9pm(a+7NoBwrzCYNm2Gzbax0R^vmnD4M>k^SnD|a>;>F zipq^bEvN8qsmkd*T4LTxra0qa^i#TX-9@e(*YgW2rV18}c_CypUY(7WiP1*E;Cq^N zDnzbTJ2LEH7bVwbDG@*ykGYL}O7X&tQ*4|Smh9NOgBm|Od6hfIjATsTF+>ou_7(6l zbx_?hkTR(3sohJSo7-c3J#jVI>>9Rgi_0IlXQ|0!=7&DD}u+ zEV&O_6eZUkMrK8wdSsEl`A>9 zUp|Ts9!tQ?R~Q*hf1<8!tuKy?gX?k<{jLb#5rd({g5;P3G@ueyps1zcn$9~Hz57ay zosfDm&21T=i6c_Ro{2-#>j@bu;cLd<753L_(k zi)Bz2IqAD-o&oH#hL}S2H#R2*S?~fTijJvGv4N6 zt`9KpHZhz-As1Cz7Ju-iKl}FD`RhMjOwJqn`l^iC`M9CJLg)BS#*H4@la3Ksq3@#3 z3Pa<+t}Xl#&;>$=4UdZR$vFS~iB@YJC0*xAb(?M4*%_Z!GWp}AgOH2+tv_{+m-f{Q zCuA=wpL6IPL4!BH+Wgq$Dv7OWwRT&Itn0L=fV3eR^Ii8I1fcIuqG?d$k#?uy`Q^6R z6PcHX*>f+;j!ytpIsI@qhJa;$*xC2bIDwklB~&eR9`e5YMZu_{nf*c@*xK^-rWylX zf#$T7n!V3_)ZCEjOXC|S#c})t!Tt9?7*tDaQcAb?+a55`5V58pcYeB%Lvct*G=4M)t4kwkTl4kcM6^LGCl$!p~~5JxpY`7-XAQjOY1w(keD(P9(o%EL}a>i`qPK2`r|Y6kfs{eyjz(O9lvak1rKn z{jrI2smJU24d=U%)B@XH+rnd8l29es9G?NT_Y3uG_{tW7)u-?8rZqwUx86c{|KLvs z$2Wd`KuRk%g_JVGCvqYRAMJg9eaydWt|<1MNjb(k1>gNdMM&H#$@wK?RisyFz+Cr{ z+&KIp!(dca%wIdjWHrW9iPIq}iL^CVxM?JliEMWlqcoXQ9~!Gv5?YeyDytv}(c@pe z?yTYr20>=ij2cy`!mxP`a|l$@bu;4?6z-z&Ezp9{I(L9p7FueC{lD<}Z!AOEnV);N zva&dtL*^0#nsS6!Y{gea_E7|h^$J5dg8Vv$jLTe;@j09fc6MU?FYUXl;Y{P zt(zxu(F0edudWf*7yA9$BPt?^UbIzH8Y}BWyk9aWS_4L+>f96nzcPj{M9eN8yeyK8 z>59A@&a&WGj0nTGpr#D`Pz^)|Z7$gjce9wM=6<3YCuj z$PtkR!i#kp&Zh=yn~Ceficgq1x`16)yq5jnv%q`~-x9F*VgY->al>?XfhPOX?dl-l zcPi@==q2vq1MPl6XpA^>oRt}K9x7j4vU@ep?Fnw`QFo&y{4#8uxvI9NLrKTZneFKI zVBgF$d%gHJv)gQ+=qV6t?$G&={tT;V^#K2PXK_8SjfetU*!tdbnF1=;#y8OFl zTy<%eA<$aYJVVK7?8y7IrtNJ6o?caOdc5MTFT!_j$e*WrX%2E939kO8FeGR%(*h~3 zpyDrAcI!tPJ;lfJ>KnCeSt}T|NnDzbTB@!}(oX@<% zLS?@E;8tl+hZJroKA94LKi0fRV!40EQ!F)`AQt1bo%@~$D~=JZ6O2*3%M zxw<9^LLEdwuA`7Xz)%~N(-VPQh$uNH**N-GiCmgB?wXBh*eC;M_Ue0~E*{)yLuH1q zaw~t#v!4yHX#|8)!ykm8oUUL5oZiA87+{9t&2J&Pvcqf8 zC+XjPCqZ`Owkq+)($-3u78OgBe5P?l&TYIeYMjkiXmyFrp87n>E0cOix9q1%^oB37 zNrz+Q;)ULNJE=^G_nMZO607XI3Mf8)PuH?*u^wltxE9gHXr?MfJgB%8-ljbKe+$>w zJ6ViFDErxDal>i}#t($qySRj8-KZ}2fRbu-keOdo1*Le&{2v@R5QlYN2UiB^%0=GX z#%}f+r0-RV$QX4QB?&u@BXd3rU_u|zl;z%AJ%ZsI8ca8W_>EN`%@Gb5qn4e2M7ko; z+2RxGEpMfd{!XaehUa$qk_H_dx-+n+nS8h#-#h^&;(t=$_7qk-B-novbj^`NvF=!B}l0Khdpw_c=T)X3GF zfL<8gx!J5Msm{%wwMhL#_Q){pC39LuL9uP6%_OVy`$je2lHcIRgBzH50^Y2XUHeIO z-_oz#E`w?Z3!FX}os5JB%?a(mrkowGu(xC1O-y4pIxG+OIl-IiP{VG%CW)J#l ztA1jwS!GPRUS7f~I)9uMgGd3a7isS-)h~T(zSCOvs4(u)^l9@@&N%jd>H_{);l?kk+<-}5`%WUNiWeZ6 zW2kdhPo|`=<>6K~m?s9ot{{+ONti+KdQZ>SYy=duqT>w3*_a$EWFa|+QKE3*3ApqY z`p;sDHQPZbi|I;04$vc?y&#?F)e`kY3m8=Qo08LNRxJh>z0w3J@$mQjt~IO8?A0P8 z(yP_uofmc~*kN>Ob=!DwV=-o-`Zqj0Cy;rX?sdLW#HMC#ABq@PN5+jTVA6FB(Q@^-Io(mW#xwLJ*t;pM7= z>h|AhUe+%a?3L-pf|6iDf5rwg3XU`lQwi!M?*^d`O;_!kXDLmzIG z{*fCK$n36FP%A2Zt|91aY}+RP>1b@=VvWou@YtHIH2`Nc99qFxC7ghA!P;Yrnqb|`hVg9!z1RwJy$5DQAM>l(DLEerHe3u9oFXt<`CqGuG9V5pooq?1M z@K-o)FGg=BidaQ;vCEGSC{(f~T98=!j@$Ux1WPvhJbtzYYL+pmiQFlR5RK}GyFCKO zwT;o^P~h{-%X2H+k0~T~#r7#wWrwo6boK@+z^te}SY%uJLPTkyti)H43{`}(fDJ+K zYP#A*DCDpBG~K7p)KRJ;dfX+R@k|4m_kS44m~UOmqQKAy?qqhl=X!( zEu6Wy-rOsm%9uZ<+6m29mJ$^ia&oe2=hfLt2LCM`-tttkm`ku{7wOXD{iS?sX^L&OI*o(>T zxLME^2hPd-OslgjXgb#`8-G|`$ zrgOhPAZh-Zeov$l@VOv{XBE1xbKQcRr}aF7=MZ6<^%Oz&nsLC;R-$HIBpY2*;{|tB zTJU2j z#Zn7-GqGu*NQ9J=Cx;E|)X5^(l%D&$YWY(5UhvpYtCo#mqAFP`=*q%OMuE^+f4*!Q zB!1O8-hj$3Qy*(S*+^{KbXPb#}c-qxq zNppbU>J4HFd3vS(^l&cv8#wjJr}6#J8J-;0yBPb65)AGeS>7PyQI#f-WPPv(-4$h8 zdb`qyQUSxn{5$}qOeaZ2aFs$tl|aX}&!3`iVV~i4&7!-@;OpGv@T&_K1j^W=V@*rb z$b|C$0g(SekZ+@KM?|A`bqy|I6F&cuV&LQ|*{<1Uvf5PP7g@0V#wU`f{fvQ*<>|FC zm<6LZME+PLQe5DZ3a*A@Lu?hoE$j3kz)+TCG*V5i#;*HPC3qUtL}U^xPuYa<>vflm&Rsh!j*wZ`J?+3ai=?L{~y0X*$*;ZF;S; z?I|`Nq(8)j8F29xIihHqbttPcA!klTr+cJJw*sJI!C)OeNr*pr! zbN2UlQL3|%5kR4QrwEBZ4A#yu&jJcE#UsA>s{t8+_)HuwZZ5NtS(W2vw9-tnm-K+= zu8ZCv9^b{lL%*+%QCuPZj+H7adjZvv#I=|V7xUYRp?QhDV}GZWaE7IKP)lcD1MZTk zgMu`mvZ_2$0)GnLn%;3$MQItn-4FyTl`HgJic`=t zxb=2^@-6uM%*zARqcxwJGI2_SE(fm!Ad?A&BI3H!YeTi{_ReTcJ2KU4Fl5(KBf7n; z%F?H!=C%(0WWbl?SPO-0b6q39V$o(LVH2i*-}7%{7jk0EZq6fKckJmt%1YI~xMvDu+)=7@-(BL8#LmX*5npN@f{)$23O zFnl{$gv`|fuSz94E^)a?F6G2=Fp zslt8>2&r3s{U#9kXR-BKPN8#|y!4;$a*Mc*T>_zt`D&K2<@%ASKJP{5nmRbIxHls6&;C<>Xosx@Q7y46F z;9)cWhqTX9Flv^Yh{OC+(O3liq0K2hXD9&532tZTNSwF{E}}n)M9h5{TjQc_R7WDy zbOYaup4S$v(?~N^MWdPZlh;d=WCAiy0TSg&HIe5(P&6=4`xir8tP|MxQ@5w<|!u7KfS;XP$L43Br?u2HxkaMw!`PmVOd zKV<{Y`MLzTcS7EB+bCB)nG`qF%Zhy5K08S&JooDcs1q{USRT>JE9Kss*pZ(`u0?B< zWk*4whrhC2A{T(z<6Pob5pR4!BWAn(7#8q}3w6K#Rv_iTVm_%@EUaam_c_wHlvTy=pA%V+^lP0xV>;_95*0Q$0T7FTcbr=(TH$@ zoFZ54EcwFWyzlE)j0n>R{_A(#Kbg(4JCND<)7F(+KnmSraIwy~=)~UL(O0P~Dq};h ztz&1(fgV!a*z!%My57?dJD{djOvP>o94owlE_g-m*CEa9ad|hnHK*Jyt88GHj7Yww z14=28EiAq6f;*}O=7$KIB+qXEl-B_cVCyMHC3hilOyuQ$wfUD8WFLnteJxzMmkqN> zlpphATX^0o?^!d&x%?M?a?&E#Avefbd`Kx_3u02_Wy`unrQeeVWUK4WwG(6p+)d-UfE!S{w0z3a<7u= z(mREK9i#^QMhgfOOoRG~BIYz-kh#;Bflb(jjfZeWSqQ=683gryr z>?Sg&zTZd-j1{5L|1H0k*FM?_qJcS?c4YL`i0as2xlK zR36*uJ4t_gJ&8jjhOQY0-_B$cMzdAX$s3J8BNj*K`upO=nPxvQ{9`nC6 z8jnZI>SCgMuX9n++jb%>O-}g|+iwXdR$>Zq*y*ZaL-o1Ck?S1IqytcEvc>EWj3{Z3 zQSxbvNM>VJh8%&3Vtx>dK08_!TFnxHUy+xfaer5tq7O_(u)(!!!w<6j{&0lk;fN>%2I{v1Ak z(r;uNE0yHyz0dD>L<%nar+)9(y}a&-1pV%6Q)wcLiaPnctx^_!c=-IWzYE$lHu9O+ zlSqo~B+EAZR9q!iBKQ1ny2uT98RbnIqk9>`{nITcNkONZ)8}MiKD)R2+ zW3Vm44f+EXAfqkC+#JOr49LflI)WOZVDi9oKVWoU3c}gvU9xVabno81OUh^8FYev7 zbbsk%B9zS=UClbxuR2N1*}R1sn!%Duk@cW+eA(}Z-zq5e(OOv_AF+Pr* zroh7GTf!sOb@i$5-EjERHy-xE?s=~LrHB<#tRa7TX!_h9_EK~gj(=nN4I%wg4W%Ilux z-KX-9UJ(u~08{T*4bY;Eu!T;m-VY4{x4I*A>Xrrtg(AeH_6u7^(2fg$N$hF*#$Tbe z2+8bF5Y;fw%puWYD~*}IV$bDcnFEvM5et6MGGtdjZ~Y@-^LuG=EeYNDoi>sqGv;h5 z%qLZg!@Kv%Aa#-Wp|}YLxs_A-NPG$C=_d-T zs9C%&vL0%RmC$%?=nIw1X2z8_qoyO<1HJLbp9!Q-IZ=zyUD&wfpqYyQQ7D{?TH#Yh zIzbou&9U#F?ISYkeOXdk1NAUb%cU1lh}aI~!KRu0>^CCy^mW0ei=?w4(I8mB{?sFW zt8phyrl1HY?GZ$gF92Dg2-!x||ok+?R_|%a+ z0p36^f3F(vzj%9^+ahM`oE{gPZgLgwG2|)GD9WyVU;6C&&vQ)qRQO~rP4ri0Dm~&B zud!F4`7D!5!JJ_w3R(T%gf3xOxm%6LA+BHil0nZD;<3elgEItGH9QAY?@tBkorjKY zBIX(LaaFLtLz?S&DqlviFRN?&u0^5iJ9lRdvTm1Yvy%)}zsTV@)u(Y){2H$CXhSVr z#9`?D1Q~^w58?~9ubc>j_F^mLUJh=39i!E}4sf<92M1`gvF>7yf6lPSRN!#!9M8+# zez`%nRHjS6Ysz(wkVQk|2dhhq?7*iM{usY2{))a{#U+h*p{MI^v^Ng>E{WvCOn(gt+5Alww9E-K zj)(g4i8gm$P2pc+=xa$FM+_BOrfL0m$*|q6YE-T&^{~HLSjmvRo#%{oall0kuq?60 zh4#COE9ybWeJtm{I}GrQldl4I&5w_u`ut)41VcGvvL)SucEyd8Z#o(JpKJ`EpEkB& zlb(xKwu6(fE>ToT(bEo@Y6G@{&jX+;c(CwKpYw%f)&pj>=li<5&)JrP(4)R~3Tx#w z-u4Ff8d};55IekWcfBMXE*@yKVOeCD$D%@q{JpG zDxJNaPr(mTo46|33jL6II9;o#($Z>SWdW^;J$!_%KGVINBNyNV+V-J&Hir-hh_Wyd zFgsp^5M{%f2@w&6@VgOWD7n@Gi4W1;;JWDizlZ9=!bQ9~kLJ>}hZeWR6vpUQzBC6j zYS!h`YiG0)mM$+}P?NZWFax{pdqmLqoxi8WrFWsItO_*O?2Hrl3gPw&g|@ zjYt8Ud8JOKqF$t|&fWbi>c4VdV4Cn_)LFVP@W>Yffb9xmWX*p5!`|K%0Yb>2 zYZ(_Mo_X*2wws2h7cx$ZfD02EET^MG#UQ~b z)x}vDqM`K}H&o(%aC`W|_ncBd3m;OF80c|AYfB(!2msjRSeAYA_6)=c3>ytPum5&_ z7w$<<&ucUQ`G|@?7=!)GYwygg2p+kC4CDp|!K_HJDy`1F!=(iFf7HrQJ1MJ4e=yYN zFE71UTNrmNG!?Vko~mZgmd`y7>(%40nU|!Oc+P%!T39T?VQ4X&Fa>n)We8_S;HVgk z%k3#pJNGVhNFyqYWC_CkgfB&20k)j$7o>6n?e-tg(s3F4sO4JkYd7)%PzT%gB7#J7 z+6dGbs(os_3~6{s568$PGmMX$saU*p%?${MrG7NmzEB~tTny` zWet+j{LUmi-cmuUlDReC@1Cr0Fh4(%niDb_D8@yV>q5nl=}tg0n=pfqm6pM@XVFJ6 zt$Iqu*;o&;H&>6K-d)#@ujsL)Q~=^IH2qy`!q@z#)}O!m{8_ z45>eWkbAQSQUSV%OC~U>&hjWQzb@fs=sv9Cvks=ukxAkmi8o1zFz_)o3Zv%Wq)?sW z6>MK!=!mtKM>T65+2yyb>DkP*^Hf%Fb^MxD-u&TLGUQrSaD`OLJ}A-evCqJ#o3>gB zo3xXoJn`AeCXtl;wjbE5;oq`hrDqVmy%B?iL;4D6g_*(zwFQ&qyV%PR8Xtwo2Q_w_ z$^`HS@{?Wo)G=uS?_#G5EBlSi_-L$fh|>mj4djk9SB#Ugpx;?&aI-2yo@up52YFSS zq|6uEeR*P2ylQ2R8IHw6HycC4VEsKD&L2Md2JiQv%oDJMK}ie_edUY1y$MZ75dIJz zR(n3fdJL?vI6q-SU3~Y*-zG#U20-1}%O{Wa_lUsVvzFmxnT14Np1GD{QGBA+^m;|?OVJQjt`&lEY44Y(T?c9D{>*KPfBAU}?M3Jb zG$vHAtS#4P6ySe$$Fnmu>?gh0ht#c4dWePq)U+#~=xL}_PLk89RH;tx1wp!`Cbw}Y z$|r%*@xGQZT4)UIZiqJ#7)5l+-`FbFUgsefmt)4y=FMnDvQ9+NZ11#oevBV|LpPfN z_I8r^c%eM_Ae77hVy|nZGcv}ewb#T-mDl+Psm`gUTk z)n`q92xqA@qs8d8`p22d@oU)!pytdn_PL=C4))~4BcxOyP#SL#;(;3YbLu}^9% znlbi-b;f%gy}Fe1bJD!=G+uJi>u&wk{y+L+g>L`_kMpBIoc-mzL!N1m9<3vDjHN*yxUq<6Uw!8V}Lk;m&MRXI3)EB z{z+IAEDRCV6lCrLgS4pOuSR|Tt)ik=`@X6}-QnVW6UWu&pl3b-{l`b*(#TcSmn>NN$%q(67abpRI)9hIiJ_O!Q$83=4hs@y{$)ELJc zV7i=Z(5v#A+-5mL!+L_|JLT9P)GiOl^Mp zci2+X}&FJWCWW5}_ky!}8>p81tSXN_|vWsPKaGyF)|X(YNvztxqx zEAQayc$xXK9G|-O-I=4<4&l?R9IwIP0>KM!lr!E$6AmMysnGy57qk)gFedaB5r%(= zzfJnBR{P?>684)V{fley*A$_OeB*cSZC20`AAP#dgjJ zP=lD>2DCT88ZDA(&;a*F&fo!?0!NUb$k(*;uZ>lAP)Y8BdD>QeNA||&hkGzE5!IUh zO==SBBI4hSP6@SIT-5p(I$EB$XLtS@@abk3`^1ndF_KwE{c=w4REnR%TESF&u7ow~ z-3aY6H)V)nsG|C2?$&({Ip-yeNggOna@@$*&|*Gf=)BswFSM)i!PG$zU;$Z>?U$=e zpEdDIY%bFuX}1nM^3IktD|12w;nov+>!A5IDOy+RL=DwRO`R$^bqEI9#;i>5&J}#L z@aZn{62g8hqa4`)HK!fh-tQ1B=oM+{=kwe{`2&v)9+F+l;x3{)6A+#?2Zo(z^~MIcdAAE&~5&|Zr@g+^RkhuBR`4F zz!%*YZp)>e(rrwdsP-qd=%}qx@0g`}B^1&S=Rs{VIXY!e73t`*YWM~`kbAG+y`^xt zH`KrjX-B%JwP?>54)&422Ne!9%^yQAaSf6eWV(Vw-$ig|iZLw_-IGXrrU7n}J6(|O`&mrh5%U0dYx}Ls`(4jt?t_Q&PkvM#EE8_33@+Pn~7#yCy=C zXJVLYU?}v!Azy_%({Ym-LRqS35_j+R=LUwP$ycBrL6vb;B-U6SQ1}y?AS0pQx^==*NM>H#KsM;VHbI!t!p^eOjE%A zFD&~B6_ib+T2C%iSp0OFtR;H(c>dG;ms%E765_$bA4U@=nv( z%+6!1np0NsuA2I2@2_qq0o&YdV%0imikSW*uodFZnRcSZ0I-GANBlPe9K#(6xOaMx zJ}NOqNFxo_d^jQxMU1X&vFAdzKM2S^atE0DV}kO-cX39F@!6MeEVE~@CV|;{ViV}! zUJZnnt)ACdH<1HP;MJ>djb?R80+`n7&Fn0XVTyq4kt+%F4_xV=kIuc4XPphqR^!vb zYW!nJ_u@9W@K0kL2d7x}r$-E+Qf?2HIz!cBS0@BpoL<=T*cb+a7uTv6S39Vw#lmvU zC(k#|OGhdfXY!ihasbtl5!oV<8_JL2I@Hq015hVcK-i`H0I@q95jErhsxI&3Ew{v{--$GX^rkqr8NH{p2&UoS@=Trkuwq# zQb7uqg9$a$8d8pE)BshKD*ik=Cj<}UG>6v=3s1c!O%+EDrIWiynDgf4OkBBr$#h?x z2;ZR7od=Hn;JwP)`Ie(Wl(Ncj^g1MPlis1dLW1Y{hKB@tgf6I4F!ehW#4__sa0TMU_`1j3bX;BZhHUBb(zTqWsm@qHKzL7g_4`CjnY`Un6sKXz#cG5h zV=+3Qb?7yBnN;m-LOUwrlclAsM3T14Z_bFj)QCzqV{9t~b(0X9CaGd+U-6Bk{Drch z(z+JTPGRonqAn~7&s3Y15nq1!OCN>A_16B+N|^jxuMI7ex|Z)@n^HS8EioE4eq{54 zOO27`NDn^DXUs4>P}*YwLe!(=(AQs07qS^YHTMFt(TY0&6(&agHvk*1pUnT!0#yF% zmwxx6AQ0x)n~S5DuY)qduebJW6Iw9 z1m_jBsQ5`f2cgKPrwSxVhE1iBKHU{Z=#=4z8Z+f_G>&#^Z%*_H(b9RW<^v;y^FEL< ztZzEu1?A>PR?ssag~W>;!n*(llG)akCy&=?pue2F-RRO5Q~r5s%x1~G`%SelvuR?> zxSj%C&Wl}8;tqRRU|?;4*y3E&5PkeMF5a=AC!5wL_w0-uXbn0rD?4Cydxr6oUNp_| z%HbUA@>z7tYQX+7{KbcZDe6KRifpNH_tq=-uihpd$3+dvaWgJ+A^uXGnTk`wrA`Q) z@tXJ@%KzQNUpW7v-}SKU$)+SHmG74@U0uH* z`Q;NSFaAQybVFWHp$tY;y41YLV|N>zCVp$C@)y!_I?~D%`%Pjy+lGLr$%>=}86bML zv!dK<)NFk_NG9AQKV#Mgjr2p13iwrP-r47ZhJlpXtd)XUS;;Ux@6LinHHs6%lvuM0d zphP7MNi#GV_%w&-VzsvcHVXP%5YG0w@AI@*S5#d}2v&?L-x>`_SjJdhO^z5`$^}ew z;4@gOC$Mjnk1`e?LCAqwLru_10ub82-VWMHb2T<{uYN5$za4hNUord}sQfQJ4^>K# z(QfAT^DnCZc3hK7dqpPM$K`$Ojl^oOSg&ezD_326o^;Z(F%exT4lP!j0OIjUzz#`M zD)DX>;yqjXFWtutO6AG)p*e#qAS-(>#C=sef3vA<-X`e;3=qcvY7cCSFzBz=>mqe( z3tQ@KA2f4Ax{SW9U!f+LVGO!q+vsMj%)%~I?#(Z|RLn4KYF5eTexc1DvhqF{HzusY zh^L|1;W>){DgL02{%=Axtf@cLqUP^1=5(F2jUO0vGw{`rMvZ?}hakkJ#d-MI7LnN6pknw+t z%=7=+l2H$7^2}M8=nfykM9uT*5;oRRv2=#vH*ZfHa;kCwENl6gQR?pc{1MTCHIlQ5 ziOO8KWh%m@A*fq|mTl>amn*7DlcdXg!ud4DAdmUtU)imr>+-LZbG|9Gtgy3;v;vLs zZw#VLIq5nDR+g{w0i3xvYlJfOV0z6dw|jwFTlKFXA0`;dOC!618b5Q>qTf37?xp5U zi6*jPF|D8!V9V-B75@K|g$7C5jCK6)p%Z7)6kE9m@4Z}e@dR=1rrU5?9Jj{fc zutS)XYM-~YF)ILKz~B3XnAXAx)moB%?QIo!D_npm%#v0Ps1>ovKyLTW13o9DY&o|b zwfY`gIThR%Dmh3F)9UD7HC_>zKt!XGpSvns_G-ez=AoIdmSDS@?-TwRMbt)ncLrB; z;2T0`uC?<1_6dyHvKsQR7YiW0EM%^M&&tfOwu18asPXqe-ewFxvgDlk9{J+9jkK;= z!{n*n)YZWeY2O=Qq}iK7VBbED=k`VV#i58GCDP*6F1$YHGVjg1d~qTLSK!%RE&o@bRTo zB)wTMUC8PZNQWInw#dd?h)3QC5S%}*7>fU@Aq(w6ng=gI(w}(KA&Cbn{qg#?dzU625|sl znf^+{mL{9!NfmpejS<2PxH~vi%tsYID#Fd6zeY`~<+qw8z^r%qfwI?3TbE~8Rf5q& zW&;eoX2gb9hx3yD_;Nm!QCZuj?p%(6V^mzd`XeB}`a&WC$c)E3v54SD=PwkYmS)XG zT&1=$KRh}}qi6i;5lY1P^JhW>_kau;{yA)%qAUn=e7cIfIdA)2ZVVWQLEy2JrB#Vv+{=#aMV z$?zhc*j~yjuSZ~n2yz>@Vo3XlWS?$;izloYrYvE~thu$=55A~y9z@ucfO21)>Np^s zT9~}UI-w> z{c-zoz3;Ll8t)-OqaO3&f7+)Fk zS5wiji8}tHnG{VvW+Q`7AIM*}I1@fQ7M#-vxA}IhhGa$_VlW@ zP~o8L&r!1Zg)gVRIS&4@p!t1}_NqYZ?^pXm(lilDR75#*L|_x0KJ(WG^9Q*Ub-G=6 zkcp0mO2Ja1+v_Co1`-Jx@lR5R@dlmg`p<#fY{mzh4*6p13iI`C zD~}Rn!(8M#0b9hngL~2dH4{F(46;FNV`+0iNq*YbQ@~(V@Dg+^z)ed>DO8{8EQtxj zsUYjUHx5`vG@(3ec8iyj@$c;rTR++e&05?SAX}$ zsl6ZpU0#5&&2*bva)_6C+RaXLP}Y<|pnzFH_Wj1-(UgujQ`K;I;!G*==kjUyrEFh% z7GNPPro~idC;wL;D^Ij8qNSN=9#IGEhrNQgnw}q}7D`4bFO1j<71U$zOCZ-%Bfhz16{Jz`F9&?bVuMP1@?AEijG0FljrKAdvjauE!! z$7h8}Ke*~1r4i`!yM7M8=BApeCt^A=%?!pP4%9-SUC_$=D8%@cp=l4VCL<1s*!Dko zZz2-^P;LDn7&aI~?_t=`;+LL3som~#$>W?chmu zWDQR3&+<~p_s!(q74#Z=$)dn^W)aYq5eJw$bl6oIT<7BJM*u|FvX^=Z^zxowvx$TL z!+VEODBC}eQUvru+9B$m>bKaFYjePKNJ?=D&E z&-f`GNEgq%-h?B@T0Ebeu?%dWm43*cz@!Xog$OV)iQb|`xCB^9T<6EQ1<;=*GB%i# z$Z-3MNUuwS6PGJUAD!ka!?c*X2}1s%FfivK?M-6ehSL2mWHE7t*%pW>)=*NOUhyYk zUP?NUlHLB({a&zGOyIA(i=_v;cv;xA}XU8`-7RRN@zlcVR;bTbN?J5QJ zjP=9an1@x#p+YAdub#Ji{EnlQh6}?3(+Uqqj`A1&lc~34kG4e6d~GAX=jGE&vGC^Q z2Gct?F`Jn)hlAaTy9m4LK@V;te0p*%I5@)0pj#E6;)%l6oh^-y@3_>u&QF@h>4$d& zXT4^YzedAY7U(32k#vv@%g?*uU#QEb;Sry`nKh| zKffHV&;QR-_%fOIAC)>(eHE{b|f4TERxga}#vn%uR%)(S!hq2Pm8-86< znk{B>c2ABUhmCHgNGx3a*|T|d;hvf*m*rif-S}W+vuBShX5Fl=OLc`J!Ap1Oo|sN; z+Fm*pxSKnKq2qUI_N&GrR>t>arT(cC7WCE-qpgj-AuYeZCw2DQbspykPna_q;1}cq z=5PSSQ&@N4*g!X*-V$|bUZ`aJkgW^1)P`hUrz{@Rl%`yDKIl;PX(M}cnuY3kuUc!$ z&5ofVox8fhk#6batVSdDdljEBi`~tmi(>&7q}oJ|tN6W{om@lfFeWH%AUz`aHq&kO z%HmbaPQv!jb8^Senl2h6NKj6deGUiVLrv<_)%*Oq2Gh$#<{mU^Mp?12PX7Yn+T9eA zXa)3|cWgpArxPokYyD($!bo==XvJVCRG4-7n9-(M{XGPrZOY+C=RFc=H&);y`8r=8 zTVc1(yt;Y<6LbY(I}`Hh3}34JsU-(W`dEIJEUtH998MaGYe3)v-+fDM_gq@^)&q(R zVh$d%hCmZEgSDWFPZ=uM&2M>EAAjO#f=Ctpa{;KM&^+wktXe`u`UOd{W^zwLV*)1P zxd^|wG#e~h@|tpHb(rwN!y*5jNj?WcQ3e9*xa+D@M%EDGny@B{!P`MAeI+RP;4)V9 zj}0G@(ZA#LqS~aF#Q|fyiJM55HV6j5(+X}59nnWb zqq8^JPh0O`bz}7yOdn3WQ(ihlw!_K$Tt2Ud1|#WgALu-6hXwxai6~Kg+gRb$=>eP4 zx|q?N@^c3_;m_v%YC$UJb;d81rLvCr)J)HV@QQLt#bjUu9S}PUVfrHd{=|`io^9zp zU8935)K49h%C<3)zPYFcT`~Gvzdi9v_+ZI%vns!FCLRv6s^|bKt(j~6)>k-?3ViZx zCrNIV;(98fO{d`<(fzt?=E*3`xMNEef9S%8cafO(ieYeXajEqsRfB-$Foa}X7S^nR zB|LH5>TW#E+|V81ifPl-maXl=8diw5Ge&Vx!Kvn_`&kGQKCefhiP`4Cx%~|$$sy*? zKVa&ux%J5nOB-lFqJuxQXHnzQIkXhb0|Wj{Z%Jz>K49yMY8h&;UVq%65}BE&`N^+~ zq#x1uOeo6m;W3K4cnPADs=2B$Cr8P%+{6`tfhX z(3}JkB!p|k$mo4_-0Egef0j_Ah=~U)^s@O+EE7HdOdwELqq>%{0S6~txmk!|#{x>2 zbZ{kV0z$)0y=QMjQ60#RCZ9Z`IoDQ7T zN!9fC{E2B1wY$r$(!}`Whg99^{Vl{o+5M#M50vF($Lr`}s;L3jkA~!VOcf$nY&d=w z^{!_-{a7WIn{_GZzgtzgF$JgfbAgq7pNNDUwkA(H@;$B3&lUs`YCN3Rz};TrE!u2< z%N;*sVclH$)y4+qWR*XhWB9X&KC7?JGTA!R%q@v1K~4`7h88wI+ztw{Kq1;zg}$>l z<}W9yDl~r|?03ej&b#~Coa3Qye1z*nkU%4dR`T+M9Ykqs6zW#a(iQ>q$>DQ3P&I)_GaI4AD6kO8aZmr4YV)o{)Ux7|tlBB7J=F;+!vRpe`$Rx!!UaD5| z7D8-K@o;Wi+htXiaWcp~Xi`449S)5$pu85?nv4#QU0>?e z3jAnNAJ&GQRVCmTzb_7hlTSs(i+`ObZphiE3z0|y{xdA^K$0%nO75dHC=ii@sGf$w zJs$o1{UVEK50dE0(aSn+J+B+l(@bzYY7b8eJ_hg{3X;~)zHAJ^hu?sWK1zD0ME_<2 zL;UZWEEGc9-k&A>gYJ=s^P9uPRy)X)1XSZH9Yz?c?XQGmU#&4i8nB!W2KyX_tk(zc zhNJ6Y>Lrr?*xSyK!e@A8QnDsqS7~dNq1NroT`V=Xxji^BzqE(bMCfW%w93+Gk_TP1 zNEc#XmaKn2KX>A=6|r{iJag;2lyF|^X@cAB5l@6|sqkG=D_J^4797@k-kxRTQY%#4d;avHlB)cgRJ2c|c0$(y5>S1L`X-GuDMpObI zOzx_f%JkQYrGlwT=TO#QwsW3T?{~9AJfFvkm}xUVi1)7}ZhfMdqEj4ZLcd`WP10cw zN$zja_@K0e6>4iC?oCPoQ#e!ky zQPYIv64mRSpW?;B^TV=Oxro}TRa6E>(Ul$Q4MJ(rz*xfi8YLKZ>w+h94CO7q%j*XP zyWU-Su3}$RBnD-jl@?#<)$R?XG%?46Az$zx$bb?!E&9a~i$4x+P%ApQuafHW&20V8 z(`4oJUy1VvU&;WDvX&K7skZ(N&W2ufDCzXZsvQ`lPMBQ!t7q-2A~U!3HNT>g^nuWij>eeNO=0vwdNn7kEVX zfJ;ZHrJQ0Nm6{A2!`dKDp!Ul?oX>T{5M>CUsWjcubw$(r6sD^ctK@bo z5NEb)oaozqZBi?boqi0pQJ-OeT2Dh3?1fPm=sIP?TRe5?g8Dz|@ zu^^cwYczjPBNuMzH;1w8u-GE502|mHg$dKCLO*XqyxC#@@#Wvt7V`@+{wj`#sk8Vn z1E?OT-I1x8B}Ay-{ICgck(s<2RmB&HP>Wd5g^Od76XR%3Ko zdaySi*I-~=>2JJ`J)L-3FAfFJ#{uD==c{X4plfqO4rI-Qdx>|tHDlXmV?VQ*7Pg;r zSj(MOXTzFYDDMnl7hSJ#8ruF<%AG%TcqQk z%%|p_-&=Av7poHQ7rm`*59@|yQ{~BZ@~D1Pt$hR-Wg*>J*5=MxG%3^Ax)(;Hjp=of zu8vaAVH#>5hT9gG*^p+ZyuA$rx$a;*HvQKEQ_olHlGR##Zsn{P5RI@~V?ob==|V-< z5`Fh)%6LiGh7A}?$34(*uw$Q^5hEy8d&PK$NhE`&R#J7oqki58 zsb_jo6(Ne@zy^vXV$bUu&8iDo6S4gH&Z{xr^WcI^*u)&?9o zzsk+ek)-^6p8|#It71FpjS1nE=JWpkh&#S#UhmQ#mog_^C9?a_;{NZ=4hESouv{aF z!Yt)dKv2q34fiP_H{+RZM#&c4FEYqk3r$ zFNY;gFAGK)))4Rao-e~aX1yDQ90OaNdry#3o{li@fVvsWYi-hBKH^(%&h3RDWvfcV z@bx{|PS!5=Fg#;rHa$7nTylp=3ogz9;s+u!o+Y?_ojt~{K8|U3mGIi`6glc?n>a7} zP|A79dxSjl9Jo&*)t!#AvvqRj-pUKhR2dSD6^ep+xFN$_)kx)nBhFcE@OPIuJ)jVb z7@UXqHK+y>VG7U$}#S{+W1$0KEf5q@^yEwfT*~leC<kG8QXQY4iLu%-GnQ2b>7!a3*WIUimt)t>vl6^UaD{Nr@4r zQSD#^@i5}r1+@fgbvlA*%gB)@Dj&Jk9qBB(8S&neE%RxAidz~Rhmqb=Yf#s=nqEoU zMF)OXUVlU_%}TVTCS6TWzowI*V_B&P1t`G5eQSV)-9(H?HJH(a1BW&WM$;dhPOj}f zojrARcr}NV1`(Js2>$)q@(U^EaQ&~vL|-tW=YO;SE&7o5o1MbPGv#+IzZECdbXdKW zu=;-_YXcJa!Yd>Y@!LLG~$dZiFTj1<}wryu}LP zQvOA1^1Tgy))2zvsgX)b3|fvg+j+KXiv`GHD2niHjJ_1fm(B(O_t5Jzx?- z?8uA^OMhrD4etpEdH+GeboiT-E*Egi!1K2KFeiOZG(F45O`{wjavsOFK{|kN&8gQVbqA0%gPWn7IGTDc~y`Uyut{=kLGV7zAb z!XjhQCoP)hk)n8GMHCRbd_PnQ)>3bp*Rsv?;d*toJb9FiPJiB&fo2|=z)R?Bt+kQ$ z#f!}2JkQ{78tLQ;6;^T>8FD1WK3&=YiyuM%?qGt% zdE10bt4;x8-Cf@Br2qFxqz&?j@zs>f8=^uOu^TuBAr15OzB<3d9_7u3aQpLp1+RbT zGlW@d7^Ve^u2KL~UpvKY&)BzI##OiuN~_kW3}(`6+0Hb7H!=AOV)V-GMDWTmx);Sy zc}Y@UHBQTrma!!-ntMF00BOPrTWm5b9I-7D#(civ!qBj~4km_G3#p#H{ds}Z0}{)X zF;Pbo(;>o@_KxY1Rh}|2Gml25>M#<(-lCuU2RV>`y#)*qL~lKK?H5GRL zS#+}2O5f3Mk;olcn&Q&Dd^3r=+fGef9{aNx45xnktqac!xp2mi&0+tV`h(Wu9X%%) z$iZTru$R@^r}-8Ra;I&@zmeNtb_HkU1FuYt#wyHy`v`}8&kbL>%^G+9UX zIvH3pzE^0@VdldXYB4{N43yGyg|2QkKM042o<$ks=v2bC*VWj|^%w0r3tyZ;^0eRc zmb(haed#)L|H_PyuU#qy^apII92-sie_~9Qi*hWN63GlDX+de^Y;6GcS0)d<><536 zpVT@x+YUMD0~fyMq1m$}wARATzX%Yomkp2ANfPk2=jlMt`z_2R{?62yW16x)Z*gcZ zK%$Rk*_r_&SSaMVa6}0^W}__jF@dZHpe=eyWQZ;J;@wVzD@9U%wOmGlNNPD5GBr%; zz}TRQ@EWc8^x9%q)2Exe6@*wHP%nzEZ7NTRRalrBr5Na6y!t1_Uo%i1HW?nH#^UqO z?%&I0&M)5sg|?JHaWUWyn&bKd)_d6kt_y}R-#5G+D$xpbiL0&{O3Jn9zsqUoJ&w$V zJrW&iC-fWN^-tS*-dbG#`+O1*YnNPS_>U^zQ8sL5B+;o5x)sDi5OkMSD7}bh)xiHAjU! z1y+$c4S9W9L|;&}ce|9WkW&1&4aJ1+-i!|=I!SGsJix^u`#@Gzzj5+cWBp=^p(iS_ z_>9k6=f230oM`z(i4$oZ&4>dXd;PCj^1GLoDgW_$OBbrP;ZAdFft>j-c+f42OyFnn z`8IIDT}ARjjPk{Ij0_4LD^@%m4cMuqod-5{#lIu63(@Ct(N5PpR>-4y7adg*8DskD ztIrVr((^<4hO6vQEu5)LRzK8(s#hKH##DL6t76i7YO`M~&7cBJW4^L-I7~Gt#cYg<$Cs4Zx%SSYzk2>}d za#n$j`YDl%4#Sn^3`~5!&?uV(!fToa*ohN;Z}|9fIxYQvf!~`o_}VbtluFra?3p@- z>Ye&8@Er^4h*&ly<$&PP#)4Zdz3mGU=^E>QrQu&*wp4f|dIHcUuGwTWD0T(V?o6-v zJ_i=vvQ^*^1$33^v;|MX5p_{xy%G_hjZ*mvB?1lTSin3lxcNY$)p0q zuV6g6TDrD66A>w8Y?Jwj;LoU;*1P$ zaJxn#;th`wGpI)Euq9ar(57dj+5MwdX@+hg0>dl$&9&Pj5=(xxV=rwJ=AQi{=UqO# zF_Ylfi!P#e81u8WQf_Iy795*lS7lzGho4jAuY`k$=tptm$tky8TI~Yr{xfWnpoo1w z;un}l>FiNxN8EFqnv`Op9_@hxDdA1T37d&Xjm=$W4dZ@kewaP!gOu z4HddSj1MSa=aYg0UpS(ROC0qLwRD@QI+;Amxf05H+{>u!GcO|V-RIYw)z^oILWJmL-lkOT$OY#j|gL*`$hu4|AH6fbA%Ku3!v<|F18Z-JG@ES#8fp8IHcNI_C(%x6ibE2?x9u-$D zIx=0mvnAmx(fG;i-^K{#ICDk`8ct83W%t*T$6Vyrs{l1$- z5zZ$T@4wCA!5Whw>gI{Z;g;puY{2Jwie)mj(mx z3&Nqa&G)i0s()d=LSu{F{2!laoc6)v_)flVUJP8-=mU(^Cx?`-(dVx=mpOBgDJgoS z^Q?K8oVtf!yQw(cw4l;P9Og-&|NLZyFb_NlKeP~sqx>6pKtb6wr`4P-#8#|=scD@q zNMTj}wo~?m=qBvV=VP%GQ+u)|>p(S|kX+g1oXGgEIei{x$qq-k_ucb+8R7V`UxwOc zTr?vz5v0qUS2`#c?&b-D_WpGS_1o8641dEm(Nvqfp4+M?B0tIiuV-qj-Y#RmBxG(U zXjwD*rx7;d7x*|Ve8uljV1Wkij@_Nn)iZ!lVb0Q`5^(kkL+Cy*zA5Q5H%2&VplIy|7-NI`U+wlm724H&an@+bdq+0+I36iEyVZCD0`=ZRk5Myb{QX<>Sowu0C#B&--D4atuNd+h-wx%TrfU@X3{LCMn>2sqX~fg! zQU1$s2edwoNmvJ0H}#+0}2Mbzmal$enKye>GY}uxNHn ziKg0|8WBjMnMvDAmB24&y|Ua;Vx+rROugss=FzTQuqbrDTW{sWnvTAH77Oc?DlqWR?T^c5(_wh+r`!cOr%)f#1ntU)vb zoKz<@hgGjMIkJR7hb9|$A}Gv?Yuj2*s1`8le-UsvYsuj04*CA^A)_ld)AOcU|E%6| z3g_&lK#t~ySKimT*avvscFobNIALSQyByG{0B{+uF3G!P$L_~M%4V3kkU7dxY<2;2 z$s)8n$9=HTfhAl*JgXUdd@QC#?Vd>MfP$mSrxWkc)>ZC>T~v!b3Eug5Rufrh2?U?t z(lbt`_iBC3x#?=D9%1|}?`s}CLk8fh;yrd>hnr%@QoPfHG?=0!_r=~*ui;>!1pE}$ z#CEJyy~irC(@+Y#AGauF%jf-LA<%h46K+x|F`jooGb6RX{;wnJ!yCp$l@la1uuv-M~{Rk6oT`OjvsJ7KD?S4II*R{_%~ETNC$_!FTdsZuW$= zbwz|r%{b@zhacgbucko#&&SB(yg4^>Xx~g$1f-qr8=W1S6&c+bN@$drF$G#`5`Ss^ za=*e2%j5V~N0e>VmfG|tjltIP;FPmm)#h)cKmp+jgSsU@7b%1G@3 z=+L~jai^cLB@?pe%i?c?`xk29pZQ8(r>e;+E)u;0(@V5I{EC$l+lLhei)(n4%SeImIyCPY1yWE>s6?RuA+;7g^G^^Ddxz>D|AW}o8K`*kS( zU=s5LVq58|BeOj2BOZ&tusmHA#xtPr+|4iNE1$XeEQ}w)cVv;`d-C}WpCyL9x>q*c zyJ?S(Fr>CWyY3A>HdwQ!s zvsJ65p1S3jLG-Nf0OiQM9mRIN*vM;EB;`(!S26wbh9*Cp&;+3%8y+JyQUAg9;~Uv` zgecnJ2JW$bVcZ=kpy{t97{hsH7kL(1G*O3K@ToKTf zul-|L+f6=vO5IWg&FwMktgz{$HDr_QmBP)oNzDi^vn!DYPT_x+w`9uh}4RF>I8>MY##De)&Ib$);t!Tnfo$uPG#9_pe;nW zma71~d|Z|>(;~mRO0ji5rqxt)Er0yQc2vYs8v>x4OW1|gC)yV-{qDhuy605A)KnaR+#+hb$IXgUC&lu*-rR2buJ9UGgW?) z{^bWry&mE6A(HdCkNT3kI;4!!Dlx7%e*$Ml=bMx2azka8S|>%K*jeK;9NKkvus!kH zEiQ$!VHPMU?D6{qpjvBZWHS}n9k2%}3&yBUUzc>{4NDUC>%=)bN5YX69>VnpR&iFe zK_m1LHXk5iB9~V`_!zWDKLMDuL7vDwm@j3gPWpPhVF|t3i2UJs$J;|-zOO;HHHzb2 z58<4-1of)|yH{QhbtdOftKG*^=P5j944ob>{CwVXq^@5xi7NztMO=$5_|B7}x@dLP z?c3H^2cLLf(bDjiBDPkQRF zyzIpmA$_13SZ&w#b__pfpKan|gsU>9!^4zR#wh~lTcjG}Zl*>a-n8kWCB@#z}6nnLtoQz3DVLdYWf`MJhYB8MxY=U6XD ztgNDk+)ml)RJFgG6IF41F5zf!q&1QU$}DtiM|6*3X6Lm_mp`wGZ{~w#$9+VgmfK9f zoY7ROUN=tzw9_56{p+2A$^*3`sliDdu_ld{?OyrJ_Xue5(+|`zc#^kSyGB{&>kcsq z1Lk*=MumlA@+0(qv065)tLnoZvRXXA-n67{Kc1~Tgva--?{Qkj2)}f(=QI_Ib&p?^ z4Rb=yxfUWmJYO<+G`*c=R3%hc_QtC+>z0~(vU)V*YF#?H{`DG76Vu{X0$^45`lo%2 zv>WaTca!UWgJUWu>aO0rDRQ;@Doug*H4v8wrZ=WRtfso@b6DPBu-aqQLDO8d^v9+k z`vt`Zt?d_66Q!PtVDQC5&BQ$I@6$27wy&{;`VXsq&NKuCzk_QL?rl&aIzS>&K=+6*Zbkh(vf=0km`2G-CsQZpj{wWX<%hom$cXdu-vRK$(0j6raS17 zEaG+3_}MRJL=8%I$}kIX*vXQ@c4x|u4Z8Ms%gsh}Ha7=a!)crK4M|$Hc}5Zf6e3YZ zou=S(@r`P$V4$jTDOJjRrb=d~tmCaWk|j)IQ;`AsE}!|&b1FecB&Pva?_8|U4WF?f zMP$%ay-YTs(1wobr*9DtcJ@tj;JnLfvAd;8ZmfInE7faRy(yH(K*Qrc+Ld)qKJ2bE z&o-EOp&p6_OilBTW5#gA&+en8lN!Fjq%!xhH`<#%b3La{@!yIbr8)YNU0=o3I^l94 zt;9Fqc-J%kE3?5dzVOa!9rn?Lm3^Lq&iS!C%BOSi9(9Czzl*K&-MbTVAGsrEwPGI3 z9J_34rF}YSqQr0+=|%kfx&Ls5n0Hga6WPnR@uqxRqsavJ@%3i@-zFw;?;yih`DpE^4T9Yy`OX^?(|t62UgY-m zd-A38?yFd8R9R_k-1pg_@xE=S*O+E%Gw0eA3ojCH7XADpx3<}4-QIQ=t0Xih9caZ# z2qgb8cWM0#2t??5}{&2$_wXOHgt3<1zkG&JJDY_Y_p$m;=ktxU{OI!%@3W<| zh7!DGu!Vl5X>Gj&?lP63gPRmG(>R;yipm|sWXqBVLd-OzKqYA6DEm2 z`|E1q9}KCl3exRIw|p%QY6t?!(vb%C{|W1;zU_bBx9j8iklOoO)lL(g^ZZ%VLlNBa z*NoH@>Db(aE$uM863G9IyTP{ra0wDl{_Sn+kf{CY>ofPyg429`$!4gjG2gSy^^Nis zaMr`~{>)&}*>c8|?ec0nt@3Ja-8)6Gay}mwh-T!3iB#`%aMDF-2?Z)XdhMXDPSitm zl{#RrbC*@)m1ULVRx^Bi#M*cAj=JVSHWD}C#Xy7nS?sfO!d0K-$lFLyY8QN#*9Sdd zvd;%uVcKR)_mo*Ll7lw%9G_)6zO2=K$Y1f)IR6JMuq@@BjqY>L7zV?AbrKuf!b$mD z&v3v4wuFdg`!+cW-?YrbgRTw(9m!M#p0^>}uh~*~Y+kMvZkLsU%&$6~j$p7{Nob|W z%2_u@ay`cOgC7D@V;FP;Ez3JGE;q%+Sl1A@AHB6d8|{lXY_=TLdf@b_P00L z-6AUQbvWF>Mz?28cex7gCqbA7)tVI{Os9*9AuQTvz8HJe1ZseL8A!3GdHnoWx$52QN$Nzk7D%zg0C? z^ja>(AEcQy2eCST;1}r-zl&|2+nJ6j+WAF!qb=ahC*?J(#)CS&NO`MN5*SII^>=rQe|@Ih9`#KnuoHFZS8>HtiwiH zI*8-v*6AJL=~3;0VV(MT8xnD?S`Y&oZzd0%m2OJzH zBTk{jKp~7j(h71`8@Sb&zRjNZhGH{TJ~s9&UkT*w!AlZ%K3R!Mt^_9b_nVPDUfF;l z_+*@LnaTY9aj=mfl$NTh>hjR(Vrp;Tz~CK+4VtPR+x*DuR776InYC zERW6Ec9#(N0Soq8v_3O(N~(5ZP7Z&%-<7Z5TvE-;CJis(9Jt8ArcaLo1xW1ES+oPh z8XWbGT#_+O4=qit8RAG!Zx)Ys-ajDAv^I+26u~(BC>sj66>(!DdOd<}=K=1pUANI$rlq#FMB0LNJbu_7t-bcnSUr*_rrz6YLv>U|Uswp}U zom#a+_?;0w!G6ZrGvm1Ixg`)a;VA;rR&*vql$BwTk<3wmC-=jq&<+Oxgy_%k)KLp`lCu59GEwEN24PNUoL`mn?9KSb@H^; zJ3k=MxLFCr9bFzm^`{eRS(95EYJ{Q0&0KI@dYSi%$-Rv&PyP2Zi!EK@5JC(8K0P(s ze_rPA(?k90rSLyp{rT%1_D^^Je$32%{pkOE$no@V6Ad6vDz)OqRDmhE)a{+YQR@osvb$B{{fzjSsE zeEIeCKMHf`OaxfkYhO}^VkukM9t9{p;F%{A|o@sYm&Zzjy!}sYk2TaPiqG7`n%%dq_1TOO?O|Or+Ck;!r;U_ zBJ{UWvQQr@`R*OLk4+eyoi78S6@kC4$cwKUh68g9zf85w*MtQ(Aakd`*VKHdyTb2^ zE^43^Q4SGxV74j}42xv0!z_QP3h{3o9opTh1fcn=CGwlzhOaK(i5(}@NLym2)V;r} z`;5?g^dIFUJfHN0D60{b&W}okOEzOARjJ9;gIH(IVk#7I2}q^D_(s%nh9_Q-SX(ul;;$kg!)BAdpg+h^qQ{S1 z11~P|tDXM%Gz(D1@oJEIs8pSLm}t0&i?zFi&()f|;6o1Yw|})U^O60D1e45md0qo; z$8V7h@_;u7T8hDV^I$w4Hy3!kz_zyS_Nqjhfk&mjVdp*0Q=V<2ND_lM6Yvc%>>o-E zT9so8e|UrD6Ye<>nbOeTeCI|-TcZ8VP>=YG38hsm{_r-TwT79~3&wBC@yFvqKjg4C z-xH-gk`iit;DM`_>yy3=>lw_VRfOFg%_kO$A40r=sKs~CN-VG{q%9SW(c`onHhA+-BI|lZojcwk++~rIG?_m* zp8a2vQ!ui$u$kk z__rmU+eoG5S@5Oo_BIIG3q}*_V~wT@y3rf(ZT4j&4^%+A_xwKgSgh`{AvDkKOV}BA zS5B;q$HoJ&Il=7&eeBFaR#dGvU%Vy0#JpcAyUlR>3wq4HvDS*-92+>=1c!r9=zv*L zM8+ns$0uC#G=^#vcT4p8eWmgyTC@nJ>#tH4q_a_WI}wLsduKF(tbrY4_c|%1$vi#V zq$lKKWxi!FzW-_w>?5*>D?68OynI@ICA!bmYX0Ij0C`qjjePo@Z4)44Xl>cGiEd3 z-~5J{uRDOBP*DS3{a+n+IAL<7-+uflbO~lWpPb&Kq8X)@Axf&`9}27j9Ue7|D|6 zS&q*(B|%z?clSmP_P&w#Q3}bF3q0Bga3hKSgx+8{0g-L`o^+8n8W@cGPlvE;u`s1r z@|0H)7AIO!h6KS6DZY0+)$p-r>3i<~yu|Wdy_h|L^l~d{KnsOpugaMMAHk|!QJM+y zEPtYKcr8H8F_LVQ6TYIRa+$NFth9nF1Iafk3h-?rZR9gsFYc7Bh|>%Fc9ASD-cVcK z+&jIz9j~SLXqvL|)L2&o9|27a9`ipr_C~}@3lP>ONe!;K)$@v&f@4$GG~&#ph3$tl zkq7qFZd2UDBGke4X2|{8%%GOS(gsn~%7Neka0SCVv7DzVd!v0vIZ7>#_}|qpcj#EmG#34#i>xxl{wP_PZ&PW}93TI-eh0IPyi7)00J2PKl&MtJkY|(jBWFt46pzvt zDPgUt*{J1>xb=$ozLE5JdC4SwXTjgU=7(zw^IE0GAT6bNcrRTIFkon?mX&a7-BJi2 zEb3>bU>k)1b_$ovDZ0!pEzRMhEF4LDrZ?Qof|n+iElMY*gX6iWh)CdFQ+(9i83Wx6PRi4C5fVu3qMToS zX%Mf|hd8Tt40?H38gH6PI z>1{>db%m2KFoj^EYuYv$ja*if^_IkN5VDB<4|{9DU;&LtDF^;H4NgTX;S zreFJXY0D~|vU!mea~Ux(;y>$&y2o^IRmMF4ZtlBnOzfN}y5`pez}cd31(tjJpjL&- zMAQikYY9OBPe|v@X7{2u_jJ*$Z1_vdHQ%Xub}eRfIaJ%VDBP!CNj86AJ#`9Z6DZo) zBx$8`LoAwHMVpHDOVH5*==13LhIzkjg$%fspN=hyZ!MnmuY7AKuZC`K%Qyb9w%zs* zov#i)?}(o4Z)biJSZ{b`+af+=#xki?Cxy-NsGIRU(IZpr>Ecv`85nPQkZ^g)Yo;Jx zkciSY97c{mP+@(m=J(3jGYWBb9%Oh}%C`RrTbqm&VLUcAG0ivdoUgW->h7PtTn>0> zIiYjz3k9`UVuOndl|)ye5hq{M6)VGaTam{&kQx4f?T_P`gzoQ_o>I)2Xq@b;S2ic2 zb`{mcQMVlUHaC+HKyV3?Hv?cFR8ps!{GBXsDXX0h)LgVOy85<%>);VKX`4@k z>l^j#wv?%Vi3J~g1?XKfudD&YBb6@2<)kY*2hGccm@XHh79!}5%kv`S z!-}&bAM%^Qat=#dYf~4h=E+MMm|LMugvnJTxH9j+%c1kqtk!3+$qsvpf^8jDzKZ~n zq}dqBYvW>TzK!5bt({=s&}PfPeu7gNAxFkb2`DutcmalV_|~mAz9uxC3nLIfqez8{ zY|#tBN|R7avk2F1klA@>MWg@uZI9WLko>xXE~gpNIsgP-c41a?ez%uX#447(?^QTX z5$l1nLbtP23v``($Bp3)rH1zFkL>Eho|5BXSkf1^t#g~W(}0)%#68}Ouj63DcB+o& ztb>k+a??ML9CQVFkm1$QW5cAXSW zjLAN7)-_hLIv7luF_(KB5$WWPeUuM^ zT&ZF)PXFcVIW#@_v$>RBq!EZI^aiNcrNGcew2Cosc1CcTneJ9QNaF}J0ynW(2v#1{f$H)&(lGoFIt86K^CU_7q}$(JaAQEUIQw=CKAS{ zi+=7?T#cet*o?lH7Z?BUaY3b3n zS@JSo-kQ3;bApZIt(h?!$ij$d5UsE&P0FCi;?*#h4+-Iqx?AU-sr~fmOld2y&OA$K zrKN-3>BE~UiaBG^3#U%ZF`%(}*{JCCaLVM8GJBA-g*|Rky`?n_>Ih?B0Lm>W(dtgP*uK6NeHc7#4;v@(w>4ju<-Z!g#fwk^vQ}-NLYO zUczz?<=4^}z~3)S-KnU%1mEX|S0$e!Yn82Wkb=L#MG*xT2_R=~zkQ5GsYPsr^xdOu z(2rvKw}X1O=iNpy-HaD(Da@W(2hhfAM>k3q_6($A2IHNsju?+aCh|`FNe70FlFU>kzAsRB1sq6vM`3?;hweLMrJ|;s zQjC?qz5jyD&bcGEM#bL|qTHY&#(1YoQrn6Fy%3?Ga+n&UbLZ~#S09GiV+yRNc|lE3 z4q=MSFD>LfCSL>aEDiMsEhoRNoP@F$wz}`p8jOE*se{f!DlUWcatOs7-2`2hr2&~G@A~vFO@Z$v9a$$Bj!y{2Fkirv7Y^dSKmx)i zmrgpDRSThpfL$-+C@np+y$I67`? zBxwP&K+6_VX$PQ3>t{4BWpSTu<7X_b^4&Wm%J#ozFrH(B=gxmk(ydtjOJ?~v;@-}> z+yH@^#EKHJTbaC`=wCM!qNxQ`6)D$w{nJ|Bwg~(>H~=fY8BaQ&h;f8bpf@V1p4eH1 zWMncYEP0}cLgP1&@Kn=nU(oyoPflA_uFxzKz~^hoy7?y~Bb8ejv5H&4B@!*eZ~zB! zw4_m4r{=&&)O`sxJO+`QiMgh2?zEDI-L}o1wOdRo7g~DE6knGtQcgX#am_1+{7F%f9v8DTX0vbQma;C%p0K$w~4W?nQ!+mPYqi-H=49c zGhvTmV{NHUgNaT{V8*`&N_|Y~L44mkxYOmQv-%CHy*I0ho#Ld+-H%rlU_X64iKM5( z#U*7-BM7C*x4c} zw{3^%gr4>*u7#wxd&_dBBz zy40h#e9hkrES(k03o9~hE0qcOMI_Vn_YXSmm3byEgzOv>hvO)Wlc~CWX_1-v<4)Ew zf{YP>uO`Gy;iviBOYJ^&No?2uja7Ijp-T_+SbG>_s=C};&Q|7o)6+bmOjrt3Gy`R& zLw9dPnnDk1Gr+(;D#GCSbJlsYX236`y!0ag_(rI`D-r$_0AJ}s5F^mD$ZPZw0@;4{ z=gh=u(uXvi@aeq+9fj7Gl~X5v&v#3e*Jv{l8#*!jeyjYvMifV9Iqp>=Xs;TD0{{G6 z`>x_?B~||OMLJk2T6oUwE9iI4HH?IIea*g(5iy01u&>$l@kLHf+brS4{=I;pv6t}} z*5gLRlmsPM?N>nZ678dKxu6qpYwg7b4-^DHu$T4(*_O52^2d(`Z+Qi9oTqt-AJ2|F zRAhM0YB<>Ns2aGUnq8+xWKlxXC8|uNZ4Nu;B2CIN)KF@Ht(n>O=x~*qH}?AnpUpKL z#_oQ~e>i{heZqFL^yzYZ>&jqC&aYjMn%eqySI^6s8YE1WO6}B9b=O$lCposy&{-ly zL<*7f=-a+h-1~o!uupnCH#is7qX{)Y$A|g6%;2C1xDMOfZN0cubOlKaH^cGzmb}%k zNo$Fg4w&EO=;C}bUwsGoH|eC|NHy6>X9e~fIH{+YT8a9eOE zp9R%4M|#I#L({fWULuWdXkqI?WCFo&MyPJwvER z#`bvw!|~JzyNT#SS3zZZT~EQPT40>Xx2C5((h#V#l28BZV@I_`m$I(r&qKL7<8uqh z+4DCwNT5RNbaY?1xNl__C_SfDYsT>C7A5gpz0O%exd*H^>#$NJz$*P`$?Z~s)m`Uc zv%RwIwW^2jv34@KH3uQVjuCdh<_>(+UzgN0LX&qLUC7<22Dcnj0W&+ecY?>_#>u~S z>n8zkQmk9`ZvHHByhA=ta?Luj?n*Z!RE{Sl96+Q+r7DnZjK)Z}^nFmKOR=Z7z0r{{ zofGu)7?<(3eixF@Nf>j}MXWSnp>Fq1muR;M)L+#rl(0~`usG5B07ZR$@ZG=Gcj) zoRM7~y#xM=3y-qWeT;wb&+eoNBYB^=#_{`occ4BHMDVfP*2tBGg)JJ5dnrGs@8`k* zDHY`~Bn+uEKfb+rv6;ng)aPe&QW724ciW>FvsX#Cr=veSEuXhr@aG0;bWT8U(Cu<} zP(R|4Fl?V@&^$PH9P`N93~ujiO!5GpgtVOKHt7(tAw;X?_)``SbG$g#Ge})&)}elI zc*$9?0<+ivZu`JLX9aNQ1mg;B4xWd~dEFb!kg6nvbH8`J&THaNMp`d*b|`@J)q!cn zuy3J}K%Y;sbQUXhyLiWOVY6;fd=5?jc6J&w+gQC?*LfWIv!_8nO#K^!mWUODb|(TG zk7V%^1#cQWPtH@wVyQN)QA#L%?}H)(KG5rN!f5C;@C2y<(=QH7=acf z|G>QhrKLKQJbdvU7e6R}H-V!!!DlSeo`G$vS;D;zN{)9;v`WlQ$EVE*t70%NOf_LD zex4R&v75brZkx<{~;_@-COrwCs?U^VoY_6 zFW8ohr*f2${HsX_mVXI(McM9|dWHn8X_y)qXY#E#t%gn(e{t`t$;!{rn<7%(skt)N zkD8{eTT9ZH_y6v9h6nD(gzIX%0%8wRELM|R1EOo_DiV0s8yz2xg^H|BXajO6U(<%M z`?p?v4hBUZubNL~G+@nQfIjaIgrNwnx!{btSagvjT~FdQ^u>v{Zw2C%7h(85a&X){ z*3=x+HrQ8bO?Bc&oJHFG|i?23HEQu3bkNZ zEO02DfSn}I5i>LHn2kmOy_evt0XdXk>Bl&Bf_y`jJtGFk73z2AzH3V#;JWjwYf)zN z7x{*OeN#`bouL= z&o?Vae;|2J^t*IFVczj&%{ckO-Q5{mXW!WNT=;f8vEtgy2UCSXsVii{LY-+QLpEh+ z>h;Jtx8l92yGe%%oz3>#IESA>BYE|~AG9j)A$@ZJkm!oQ=&mGR!Var>7Pqmoxz9@2 z+gZ2G3M+n@4t@+epx7q$PrbAvUP&*Jq?5s3^g&O~urmJZ+>E#T^1fY5Ny_o0kZye| zkylfvL8TVB-^*6jpxBw=mh`-H}! zJd_DX`J{XycAmMhlu#stSI!==@z)ZFjg$O5;wf9Z?Ah^dDDhxZoH=X8 zzAP0af?CVf(MYzr9WzND^A3scd~-19J8a5HmYhX>G1e6`{)F|-XTw+8bz!MO>N7Qo zmuyNOEawi*rb%jmaV4HO$qMMHt;OXa)@U|>)z(IPXbxcZ(Mjeo^(N-+$zt7>!nJqo z#SEF7m6gyZW}eoei$|7#hbbi@g!k$z$&_tqpZeeb_HeV_;HDGrIN0uncP-DimXU7qqH=~9=F(~F`c16+L^EO0}F?Iw1a+kVcsjxQ_B zv-};9rd828z@&7_lnp*ItxZn8vBapzZ&Df7N%loL?`C8FLZBil!Y59E;ZvOY;}~+cVD48(v2(i$YyixE z>kb%utw>~2Hk6BU`xJIJX?x|%@53wWZ(my0Ces_*Nk%R*DpS3vtDKaav-wqL0@Hv^ zenpwxe9}i*T`)YbFSchnlett=E?e5$8z~MNJY3`+)S5*>YdR}4n1a72~#~06` zgHj1`Nhr|g3r4PHv&Gvxpv@?r{QeCNXkSC<3sAHvn`n8drRValz!(B3-VC;Opb@d> z4nnCHm}r3GdG;sinhwvVp=WeTtHWc`My&xKG49Cb{i3jfeThsLy&Ul;DzG7X%JCm_ z_hhx;0M^eEo_!ngR&XJv2`VpKx@ zd?pj+vu5-Xw6p26z;g{6o%Dd}yuxKeNgTx1A(YedaB`qToeo**b9Aost*Vi%4UM;M z0-*ISv1RO5lR|u(!YN5dV~R4&UlR$q@;CR&5_G_JS?g+&G`OSB6WTTPflj1;urOyO zD+sR9+mCUD$goCj_du|=eiSivc^Zn1B?n2z;yx2;o%Ron(?5OBmiQ8YLnhzfjgy7> z)88J<2%P)$J@7l)e zpUQTEqHx-mvbJ;Z0G(B_Bj(Myzg&P}2~Lgs8V?lb`fm?9PNWEzvx;}2Ar}Xlr>CpG zHhm{RJNqKD%`LC)i-N9peir=RU*7K3cLlbx_MnjNPij}zK5!D+jkeD4pRf7K_=`E1 zN`e#u*C8L4Y4tDV&57^_G~Z51iWg=pN?6hmN%A$xe&6eJ1|?N}?ON@(AV!2cre!^m z@tJ`UuiuUgww;^6qvY{6yutMGSWWloM%8fwSz2YUkv}GQ6<3PDufi)5EOByi99Rqu?p4n-EU01q`jDS zhbxE|<>6oCC!#=mgRvlg1m7uhJd{d!r}12h7ZFYv-qW5nJ(N_`N$ zva>@}XlUcOojU;xWP9ru0^{2x-&C`QiJm6i`zY;3@%vk1%bEs$yTJ&y_e57?H`+kU zA;lRooI(T<#h&qV}IB`zO!nHYh) z-FMEP2R8ZZKS)4##^lb(8^*5J&6Ledn>F0@<>CC3AS4 zQMzQJ*5>M9r;|xS7CxGl>B$Be?B^Tz+rgJbXz4M{vxdIx!?}vr!{-FSc;2*$eeG}GE1`@1HVWS&;rRLDghGU!h>J}uB6Fj${6SA>K64-Dt5e9bf{yQqYV)0Ljo7~00_B)(R9DPJ_( z`4KgHVy<*rct>+rLuP+^vHAN|$17MVDdGwjOB6F$ z=>B5f(86B;PNryjxi_3r>y7f-G$MHY=FGdEJjSWdY4T|a+ z=iq~18H3}O9pWAAoWk}76;3Kyyy8jrC;8>^0}X(q=0D*@E4|^cj!9VW2v&q0wxz=c zeevjTFpvPhi(kF)7@~KK)iL%S~oWYk;isI(lvqbifNe%xW;2CSD z5tYz=`TdqbjTZ`^;P3v!xRm4%A|7eWnNQiYvF)wk@e$`0~5E1O>8W`=nd& zh7xos@OaqyN`VA{*?UHLdE2lROW3izn1r;vc;rn``E}=L=3uP{4RAOU_4baJ5buzM zVQ^9_e+88$jSiKh%=tyqB7#-LXMqe%ClWC!|DvF%i)WWZl$?$QpkM9N zc0I-{t1P6X*rP%I%>!TNiHuT9XbgO062*0cgEkpCY@*mXwu}3m(sC3uE+n6W&C@|- zAF8{5$kAO|y`cldm8ikC*egm&(F)`0Kg$?h>Bx;8M2x~igud=rO*d#}UH8#6->BV20BT|rGIES{{%GDe>y=Uw_eBz=sC%dZ8 zDqLE+k6==Aa9_Gb$ii}zPc$BGY!dM9RKsCjfX@5 zZd;yrol8D(#M7HVPnL$h1q-7<{fdq+ee-+v|FTd%@7ufOcN96!bk+*+b==$@-N9OcTDOJ18YtvKjReO9^sI-EY?A@J1;zP}pZZmY;6SID6i*W9W9 zb?1jdADDu0cCJRPj1`Rw2#pjd3pWK+T81By>bLBeJmTgr=(H{T4rbOKYAZ6Mb+;^- z4nAkvW$%-hiXL+`UTc=v_s>U%0$3;v;ZdG4%fi_SCZ64k`yQ{%QX? zBq+X5Y*>PwC6<6S6EX_|T30Hi?#FO+G^*d7xbZFZKH0&Zg*7aanTC;5MfJb6;XvkT zr0RzsYKYlLtckDr(H}k{`@xcEcP-%keXe5ZLdkRqlju<xGMho>K`ZZoSyvbP)_b`J?pR8$*(mvT~5E@ART&j0(UJvy04L| z-H`z$Of{xJC^|eu0`Ko9Fmrofal;}rU;iC^wuTxv%*W1H^AV%imj8NP!@8SNF6=1e z_?V|tnR1tffM@RtC&a1GGo$1J_t_*v0=`#UY`^)##ijUKHp$SUHEIBdm zPI%}Q+=~nkP{xZtKhYMB5et>^K5_SNXe3@sv42S+V_G|Kg}9q4e5kP+Mc?CCvSC0i zd+W#7r>ZY#{}m|?WG*yak@LBFOt=EcEXC}x)a)@WY)~rHB4X&x<8-bi1c=C$Y&=~) zgTTb4hZqiJn6So9-k15Ir2W4M`^yE##O#N_A@&b@ogTMNf0;&ot$}GcIz|T<3^<9j zHIWf`M^jKuR-NQH!djdn$6vVU!{B$7!Uzn*$`VIdWY5S~O(3z5;IMY8&H%63}N&7u8hCqKH31#gj31N^ojhH-!f1k3sZ3#DTl;1Ht6?B2kq|(=mKofB5pN|BaW-ONdiTPd!u-=b;$j=FeM^ zr=!kEMm6Cw>O!7=yKi&@U__ZMp+Qf6P6jkj9HL6GCnJUJ*B)$~ZuDN!d1)}tpZDig zC;#1(5SYpi^@*&Q?89sV>Fw9u1{YxPX0i*e@_11yHG&Cb0O?=;ISL@}`gdM4y-^^j zzn!tV7^BFV-`bAb$mC?+#H6*%+9!JkYkECT31cG@YwJ^O*3kyqk5Re7p9v6K`ZeXS zx+?+5+zIaIG$f&yc6&y)1sfYnizRimCE5R@`#1kL;a!*n6eORa*Xd{OM8U-5mkHGO zIh6J9&BXkh-6o5`Z-seCm9ue7+Sjj&-9&W~>`^X8G*+bp1>`4Q_CdlGancSyuEf3j z{g__;G`F^^by5?k|C=y|o*K=+oYRM^FeLvvgt{|0KDgm|1q@<1jBU>Ar4mu~iY3@z zTd%a$#ajbi_8SA&7u7i_f4{pQBf>)z^?0@kV@iS(G8VIuS&J=Mg`2O(wp)n+tQGxU zFfsp;8ppJM;t1+bG_HOjI5sUr6 z?Gj6C%}n$Aenki`G%QU6n)RLc9)lV7GM`HI&;cE(Y4SL%Qe2iK2;bszYkM(9kp zEZgvxX-33Wx7C#nDqF!N$hGdfR{|9iwg}6kB_1AX!TJ*<(!JWJe{+#|wB`>@GcWI7 z8@~Lr*dRA!-*A@&C7bD!_l><%IVIRL!orbYIx>`73Ba5Pk0)TnNodH#>`PsvHmvTM zSp|(c_4t&$n>6GlDuHy%npwA?OD0743BdL08H~a?@z<{Jo56*BKn8P;@ILe(X+Vno zwJrtk9cScFp$9^q0C9rdJEDI(O)#TmK$hVJs$H=TldMLIf+3UKs!CN@)8jG}SCs2D zYZ>Xs!k)L^Q_<+Gl7{q6&jb)6XBzcR=t`WZ6kA9=&O4^S=3xMcfv?X z3^sXro+(#owT^t5a~I#hzA=s>zq%;%9p&`OQ}Cs0UF0VDSD8VTv+9+BnNz~ffrsoP zyQe4dk_#w?`^FQ3d@{uD!mJc0Yit-qN4O7f}sTvrRYl+=_S$<5C~FC zfB>Nf2oNBI0HORBzfZq^#u>($%$$4A*?soe-Dl60doo0pnXlsUA+Rv4-#v-MaH8d> zoMQqZ(g%3AbNC;AiaLC}-AW%NE4(TG5u51L24&RaRtuo}_}X5d3m-Mbdx-6u3cxcz z{fPmaW_8VI~{e*^D7N+jUw413fa6d<8t993ao=^q|; z_(^`7Cu(o@3HS7n`?mD+aHsVCv;vyRif2AvXUCHsmJ8;87~=|SW3mTKc6v=IW%6sR z9RGP+ll~`dWyCHDpU#jJFyGzQ~2;03M}PdFUr1{!K^pPTO533_+xq#(q{WtkSWW)MR)g$W`L=g-k4yu z3wu#^dZ^J`M>Bgyje18Lh|w?>`Qw?d z|6^K|=n7ly`*5eUUc0ou^#k!T4{I~1<$C_OMQ&}476bsCmMfLFV`?7v5_%twV3kPg zaP#mkV`SGVHZkEiWF4oV0Md|P--;RE)VYbg9*q&<_$qewCPLC%v;=?E;M~rb0?6{r z;-`v+fQzK4Qa&&~(S@V<+^{iHLpJ8z<^*^#8r0nUv?7~Ik+L!qQ z1?^)yPbIj^xk}7j4ymZTJNlVL!17zEXd%%WktnPk9;O~BS$lP>H6)5tkLp zlKhm>_q);uGh*a^a2^R0*a&*kexxSMBI4ibd&T3I0PFsA9s1Jb;rw9~M>_wp)%d7k z&AO4W(9KY%``G~J@<@&t8)E|olB2x5{EYk1XO;U0RWC(eakFbSlbWzn&jfz) zo1f)YROkTi)o6HjP~>WZ_n{&ovgWa1_K~L&#elVctOXExxaEFpDu`2R#a!_9KA9v`yo@u$bo{-+XByc;~M z5!16BUtjlqI0P(Ps9kGWfPlWh$fL3{m=)SAGry z$-v%=6)g9jO@xo!3H`Gxd@RDYw?rN%?X@$M$4gFg5CcRwu?6z>{;m%8$7PYk3aaY z+pK3Q0K7-eVEx}GpHeNvp;tNai?<%cU1GtwKWU(E0S&1$op#?p;PZ&%Ir!mx;__P- z#+KOC(^o#$Jbt}9d!!VgxO2z5l`&6$W{cJP>quprwqgdUN1fvU&zuT8o|3=)y%Osf z_9E0j;)&cPJx<^N&`ik%V(Z*==TR>V*ZbdmuOR9VRdN(b_~K$QkZ=<>Am=34UDzA6 z%nzu7b~5>Iw;mk_f4u(jF%*D}unOF>6Mea3tV|8mC$756KTaj^A94W#OFjR)$s-ii z2MJ7MipsSUQ5W=zH+B=OB zjn5-Os{z>&#)(Ivtc}J*!ySv80|54BX{6+T3!*p1)C5Q}{XT6^mFxh%c*>uKJ$vG^ zXYs#|_ECAVWS}by5MwjGxA|9hGX0!j#=tLs0>YliDdh*nhQ&4_|NS~0f;4Rhm>Nw- zPw)uIdYr7zb3TM}2jB>YDcb4Qw@M{vbzqNBk`}9n!?XVvbmNN`-tByKGC<@384uH* zKv-zX>4$e2 z;I7^C%i7+9bK73IQt@A*{}EQX)2}XRvPFPA<*vHw%OtHlkr|*5zIpB>oK}Lqt&n@o z>$-&Yixa0`oH+XN=!S6`PgADO1yaDRCy#MB>$sBH?U#Lzcz7l!Cv_@L!+XiS8Rg@X zA+2*uZ1pPip|Oy-F>}#DK1fv8tLCrzSG(lO^{+~Iw{e)wt$U{y7<(j4TA-UMxhZ_} zKYk8_jtwYDB^)0pa}l^R2fLPkci`JMcO%qTpL^9eSL*lj!#QLa&*%Rg)muN-QOg1s z*M1^`@O;9%S$S6@$-707-yLONp(3fXGsZLKtUZuCpEh8)fz(qB(#_#oY}JZ=iR8r9 zDM$KsIs4y6seK}>%-bVMtSD{)cl4&k_|1jI9u~&oiTy|ZcLw}6GWZE&U_ym=U1$t6LHh|A*D=_@c*rz>t}GLXPn5;J??|g!*7GqW zOpP!>#Bsb@_`Xdw1L8D+O(jq!k8}V27JDMdaUr-j4N0|^D)MDx|7LB38kVMDQ^Rl0 zd0scC+1uL)hfhXtF22hr{`BKR(lrypTJ5Y3sl|oNfBtt6ySKdZt{~*!5Z+VylhLj< zPXxLynL4tJD}LKUgx25WU)@9GFMBjK55C&0{G5vH6n&?4y<8EJqs|26FpC_frcmFv zK%$3ig(W~Nf(NG3naFNqZrAee#=Zm{7e)MAUVxjb^DSX%8gm=&CUp;d|Ee$6^-!5? z;*fQFGljk;&xBFF!(=hw7>fbXGj<9N4|jX?|90<2JMGqwM38P`=%}rFhS)r2R1xoe(rC$> z-~>-LNB-22ZNaOe$(AMjG@dfmRB>o#!E%toS_9Xn$(6jwMoKE?rBp9V3SH02aKK?v zbM)(0+4$h2LmDEfaQ^p9JirP~z$9&nJsG3eR-x9}5P)QTww+5vo@-iO*kRpszJnU> z_j3v;@?GJO_pUQAi&=OHLbQq(OUvK?XOi4=@y^eZZVNHn`lYy|KU%a5qmT}@vp$hc zwQ=QD&SVjVa-XRP-sU6|WtIZ|Qi&n|kf^n_Cb4s%nBp}09LMt2L+(HIAm@;t0?Zhk z#F{s)CggDXHOyIn4m@~m-B#+}tVnF@d3$qR9B`|(pBmr*c&OnOsFnB!{Bp^ggXBp2 z%dgW7P-3}UIp{GZ%G&sJNSOEj{Z-QK+VLY*<1UMvkRzcX#ldRNN!(}<(qGP9FPiYx zma1=Y%+}ruQ zYU8-K77aQ+z5n&r#1=P2wdo7<8IjBbVB4-pUio{3e2|=4pN|DxXYB0-PKEECXLK=d znqJVK);xU_!rJ8S%EB(Fukg^nl*l| ztOw(}_Pg)`efU3Og|jfWVr}yjm@_XEx==5C@8p1oj+t`@578&WZKs6T@3W^}v0#b@ z6^Ylb;D#+?B~Q|{aly;+8b0lUev7Nw(=Ud<_SG3gyzGyn;I;OWeCtxQP$K^PA@OZ> zn$kvB0X*&Hq*|3+c}j?{A`CXyT8@N<8ATOs@f||xzhC}9DYByy?I!cqD+YV$eAO9dTrOKAXH z+1m{pI&nN-l*0X&acJ5$cmJBwh3=elSzf|u%~X*&S3BO4RJ59XRtIe(KOf_5!eS?xqLFup=@FC(1!F#&`%7^=`c@{qFwq zoB5(|=zM3T$VB$BImmd!mj{ZqTni~M;n#1F=L`m9;1#WPtEUeK_UX zVzoY#DH--mkf;UkTKjCxKqn)Eh(11J;Z4NyVXOBagX2)@u`3cYgXHJeKVUBzvW@IdvNLwXwF zTI426^3(JxG-D|hEsM6dHI=&*p10B}G!SrmfHuTiZNfAZ8Vx^o7>jd9PiB+oyFtiLK1DTTvTN zd2>8S`R!&~k0IQ#dysJLoeKh^XVuC zm|;hZ=9@##ak#swIoM|gX`o|Nx#TN*zjQUKqOG=BskYlYYoqPEv(76ai;A03DQq5?~XY`(o+mdMma$m9KP8Kc)Q z;qW)eD_|!N#e~&L)4-4L-vxLg*Iy5^1G{+X=K4l1DM>Bssqd=BzjkU?PN2o-yF>7S zwkivICcgcGpcr2#DRb(xr~G!uI7ytJ4Z={v|+_qCHm>l0EGQU)6| z656$S$U6Opv&M1=S$zOQ5Ii-i?i^8a@Pq|j`!)5a@3%c*Px?14(>-nLNG8|B^uRW) zSDOcYSbD@hM8cJ$qgPl z8liRIjM?0kAeA;^hsW^4NTO)knn8?j30rYUK+gV_z{Iz=&tseGzJ8b>Af8v`5Z%(k zn|%5uzW~Z9TWL9dg9^4m@t6zPpd#5KPvyaTM}A`y-`}0RpO&oK4Eu821OxE8(TZ+P zQM7j7aJ73}ovUs{i1?sJljYtH`KaBGOXfz$-7dL!O<5pzM`lsovX{l$xH`c0Qd=Vn zYV{k2w+j3m+9>OC7o2$Sg?Q%Ha(K@$xL&2yGnDS@=T9lG_fF_p ziLF{*tGvH|jrj~afkNAtF{{cObDJ-a1#E=+&}%_EJ*ulkHEvcRL99!k^SFBT46RXs z7=P$J-go|b&;6*zGh)<-bqjRUScYD{>m~FH>F|8D1!0PUoPmEw{(%@vKG%Ts&zAC! z?qz6);#Ng*gP=~ixV9QQmH=Q2aQZv=p-dEdZpm;VmlD)KH%iqzcv4geL{C4yx*mO$ znq$^&1We5Opw?#}bN{(0lb7xc@|ov8X!|#2s~Z2o8fKG-j_@~5=^(l(3xgSWi7q)H zZD0UqTQ;0oKD;!qV42PfcU-!s-y?3lkkC_FN>6Dn%)1V~u5Fc3b+dhos^SEnivnPp zH@)iS*mAx1POCaCfH=|6Y8E7}Me-DQ{IeG<81dtnJN9*t7HUk|=(xQ=rzqlb4f_7^ z1F|@OytLw1%3;YSOYu)64u0u{ZMz@cLsgQBJ5E(Sr~NrmlGPgJR59EKt@5#k%jD83 zdGM8T-q5cd=<4+z24f)3>U`!TabJB&os2z+=q7a<%^Vs-g?mN&5PUWn!@~b;CF6?Y6n=7+`E<*Iji^h>dhw^X;s7tgP4j+!ofg8 z6JRS{=wwiXCH!dc&Ut$8<}SwO?AE)#XBP8LcKdBFyAII8y~<#!eP7~jGI$m%eqbO zeX8E{*lnOIkvwrp#gB}Em+v6`@CcFUE_~#6AohnjZnLUzee)JBxXYp^IvAn(g9m8G zDguch9y-sExmCrrkiGl?N4QuwZM~rBkL9Ni502McAmI=3gil{d*)ud~P(vjm-xbJQ zy1qFsoXW3qN)tgiNnlEC&isT{i2>OkYp$?E(pA3mEWjzekU{5R!?wi)c1P8B=eD$r zDlALQMD}$5x{#(}RPs5`Gi^?nU#D^DDK=7BvEZGa6{Vueo2?eh_qcaSuqzDBn_YYjL@!Y$W8ImJXIUEnNe&L|^_YlNMf{-+sDFWg zfvvoYD(jZ*QawBOhU+*QlwS2|Sy_SL4McQn)F)TQyHmEOI+{+8IcNI`3er>^S%5%n zV#Fe}l25cM?>33zh@O;sqA2)0%}r+ezS~yOa6N#Xk`>)+73)B;>I zQGb`U2fG-@igk6X#&r*LeAUnEVA-n5BUf(_`X3NC`GPGSps}?fON2s)#`N#A=N*@2vy;)9eou_4Bio4*&3i*Hb z(+O_n4dUcJXIW7$Bmtu9+!!)(V&~U4?sG-HHRu69p&K3Y ztY`}_-PdJ#-g|T4{zlwk)e11#nW`e%H@^W%*#SAJ zc!tGYwzsTUc{z8$Kwazie;PoG#rbKaxxQuY+pcOQK(^`YwDp|-J0$k<>9ePvME1nRCs(eHjUT`l$sue?S7N34p89 zglnq#T%zlcY(h|rRoz0SH;~F!`wjT3VXMu^0+IEwv*DfUe3&aCur3VU+V}+v&Z5X$ zp7*p@blXLgZ7$6=mL7flYM*yjNXFFN&9lDoQT>9|AP{~E~pQmWPS zMP!{YpSIz))P$TqXBR+dN>d|Va&UiOx$DlzDk1*#hS9C^BbDGY4SOO+`dha zbu(CgtEh>cRmyPvz7y^Tzc)a4nrwy5v!I8xUt#Q+eVa1Pyd7kon|eX6Y5U>=%o zff>Fv2`x)KIIReWH)yTws23&F*J>Vo{YzFAFOl@46U(}VGD6|)H7J1PzUJ`2O@?R> z(By#ZNKqU&qFtf}P8s-R`lZauXgne8$@1ihCYJZUN~Ry74Ny;mFyAcDm2v@!Qx?FE z&72)hZwY@<0tz9=lC`#$Pj0^Bs^;#I1>g1xX~ zc6^)igb(i~3*#`Yoi01|9jHS|Zf_Xir$^XG!JSq{QAkcvCojXdHglZ#0s3Rf3Pro3 zb_-L7=S3=93n2q<>)tI8fx_Q30tO^m{PC8CBGpLSFTQL5-4%IMB4c^LhI>zu7j@P; zEk@P+FO1S9Mdv2FEthEc4GE&y)rNn)DBXM%cIQtJ+hv+bj`XxQ7tZ&zZPzQXWt10jn$^ErcOBDF6RFPPJBrzxkxItdLX-DPY9dW1e;9U?D72&<|WQ+01 zmUK^bg}OEO@q5hvcJcqYAhy;yUs8d4)>sCwnmostg?VQIc|b!?e9sCpv_5RPWNu3ZN&jYj z2M)HVyS(DfT#v{yB^fQvx3S1;^tX&i{c5AX+) z8ao*p(@C~_4Z%j8Q5+W}H1^M%W(Ri!_`ad5fn98ZsQf^ZbelG|evWn(pFUt(j8blt z%--!E#AvK~x+?`Qcao_#OTE5-2zFL2dAJDjKnZ>Kbl%g8+Sf&r&5$%DQaSdWF;F&W zE0bE0U@zpNQB85(bGuY@|&yYPJKYw@RsYx0gRvdr#LS zmZNc3f`L+ZxmtbKFa*k>edGf{bD#?3xLT6us`zaT@rAE!dS6P@FHLyP(=JW8f3we; zCizU{Fr*YDdZW!ea2$y5`%|y7_x6bEq=QNLvXDu_)b-q|>!}XcB=p4lC7nPSo#Iasa|bd_Bpt+p1vfxf4PvMUVEY2L@0P-IFo(1PuU*G$Dte)# z(QkmwxcUA56=*+K4NwhDP~YM~ZAA!rto5C0*q$X$6%u!jhtg47P$l@XFW{9&HTIg} zbL-Q#p1?15H%`GjH+R`=890hJWw|tYIn$4G@TIb-P7BbKpm&0&GFZ`O4(VqAt7bbQ zw}+?#N(rPRET$K_2hLN0hrY4qKZdBP52NxsXSz~f{Y7N&cU0*N_3$?}$(GS)k8fE# zS*dpMA@I8s9g7{)To5qltlM+0V%C$Q2oPw%&YdUsS$CkrGl9rvSa;U10|Ap2sN7Ft zU1fpfU}5vB6xZ+kn9(58-tq_D~+wO1N3EpVd zNHm_<9nWZ3TVXUX_N!~DJ1TMpVdV&<&61hQUuXLwe^>r^-pyUPt7;Omq9-m*^BE0n z{!Zk7ezOj-5PqX+r4?6~;}fIEI$kXY!PHcmL8OKXMlnW~Uhq*y=#ZY8()=dwj-W)4 zWbJyF+*J;7)XU-8wGZ-+zoyAIMzL&)p_)G zdFZgL1uBG;cq%d($9Ge|V^F&;p&NKOx?Kb{MUxDP%U?c(hy!4I@fruxqCzW;djf(X ztKR6nlW4QhoiNngg@aY?(7hP=H3`o8dU^n<5qb{Mkh{z(pl3DatcQoL zxkpf&xbg)bUxQ1*=5P$bc^_0FXAohx>Qpu3v)0}YzCn0X^@+C7f5IevGw1xtMGs3v z6qn<$u3;`$S9wp=trnie7M)Ipf79H)q@-1`$xV_U zb<6DdqEpwPqvu_u!{XhpU7 z^V8vbdJ-Dj@{BH{)B?3_kwn{rT|p2%MQq^w-X_YSf?z)t7I#$g++X{Os!R%fbYodG zPg&DXojXeVQr_!oHuA-BSOe9J)CnFeNKuf3@W)A-FVzEeQ|!u5Xs;{7APVcb(l<5{ zVU@v0Z;sR2bLGJHFtR@qcF($WBrHv9m|yGwmv?hc|2ouoT4Pmt=Bg{A4H?2p9Rs;oP*)QsAFoq_@c;*oD`jNX|d_o1bgekf&i zM}?A@bGz$3Zp#x?v%7UQbld3Q`(8q*4mK(X3V)-B+Eve}kVRJHrrsm}dVXI7lXCq# zfWHT=?7BI7i|_E`fhpBZ>%J?aKrLqlnE>DR4qY2Sk@oMPo{ZXh(Bu4v$3nssn>OTE z^p9r;ZygofZ6&T1!gr*ttEYD>@!KQS`#SCtQVkt~q(6izSeIRL1$WLL>*Qf!W`;1GfhJYtVu+BvvNJt|^^bO3klxFvpkrkt#c| zj|AItFz}1B~3_n60#H_cmjU9XbfU{<|#~Hae{@CR{Vr32L1wo!FxBl8Q+Uleq zBn$5U5~NN_?w_StHf#`-AOr***0KzT<<#ktUBQpEdCXf0}PhJ=h?Q>4k`G-PYp-1RCsY#g3; z82Jy^elb@fUmEglZc#S+kys-pHqarHAV#$c^_Mw$!5P%k*marr_3mShfsn%9137bM6firy5NC!poX< z>d&UB%HoY!@bNpKmdnqC@=heDBJ$l`>$3960D!skx?w>Oh$0~!1})3?`%r5FR1p274MK+W+h&-t2%J z&8;rk)+yRvd@L6YaKDOnf3;=j*3DgkdTPCo zbr@7kRADP0XjHfYMJ>lPZ4-?y!2|30?fgGsfHeE_fuQc7t{F3Jb{$Im<`n4$hzPhz zYSQ8T51@txB_-BRt8SwPQKgx#idO>E^~M8*9a(abSv_xj(4+<0IxhY-0PP(IV$km|Zm&IlS z@`Bx#yU3jZB?p$o5oyP4D*%>ynk(gXum93Xx(^k-?WG@)dId;mO?y52>Vx5*8;{}a z#H>Qsr8TyD;b`{#DWk1%;43oWUCiU5lm7 zIB#~!<=NKrAwmd=Y@cotjrqQ4cfrHDc|k_sp2IAz@K2)`_T?lE)N=3ko|kcvWbGmj zkx?$fzn+k4lHqkDQCa$k74Vs$g)&s=*WJ~m!GSBjhO=!-V$3GuO$8h;tfDszqDJq5 zttAH&j(5o=Ivjac4evoJ)^Z>D(XXS{hm9EGIYI&gxQLi+t<5>xy7B3pyV>kL+aio0 zF}&c40(t^@(^O2(dKpcnwV}k2ZDnt9u%kEnfoh*sy@z`i)8+-#oQuYe0GvTONQ7?@ z6bm1~{%8SEc5qmBMSUrCAJ_!w!6a&SjE-v041#hZ^bHFL;GU@72zLSe34x1NsD1hd ztad-x>?3O1GsMe&EQV-Q`o~8q9vSiClYaPsge^~{;T#%iS;@F6uaaIbYZVNe|3$?{ zHcxS#K<)U2R6&aFXpHAdbDX-zKaiT@fNXE!Hytv)`mI@>@J^Yy+~j9gM%n}PBTnDR za)>K?s+`kWhCGm?(^51zISuX_TzySfAy|24^G@uS?X}BxFY?KviSkYlu9n7Jz;N%7 zp@jl`6CRaXc=|>Lt0U@iH|I>OGy-`=G`tm}F}7t1m_uP(~^)Ey>(C5Vs9Hu=Lpj+Z#eE7?n2}&nrSDmE=G}fuC;A7AO1{vxm!S zV`SnOWJ;H6c1Fr&^{GZR(w`b246!~!^L2c?bk9FPvpAeXr-v9G4F z+jWp3xZ5sB1@326Z(0bL)}wH0PXMIwl1qzKlZhF5-v2k64kO3f{Tn@b91O)g4ZirGNXU+%uY$B-Fu&p3X61--q38KzMW7%$@Xd^5NW9HclA-$zD`V6> zo#k0`MNAfQfj>FPBgx%-XkxCDes5vFO8apPZK!e@ixktkg>Y5AwF&g_Yl(KFov}wA z0s~HorUvLhvl5Bzi2{uY0AY=Gx1jsU@clJ9PpO1Q=v0Bm)=l;OU%gxP8VtG;@twnA zDPE;%g9fEa9*iAy1QB;b6!zJ$=2L;Q6M+VHKXM;VyBS~MFL9d&jitzXnxCfXBS|t_ zYAIU#${(u4YI=Q@c7s9csKD}f3kiR1Dx58IgLch34Wpdwd-PltcLgC-o8bPG05o9C zNF~-c_RFwkNZH(nOu%=`o8MZsC6?pfStU#mLyKPtbmzRe8d&(#?EBj+VV(#HMzkr0AItbNvP<5)8i zIZX-ETn%6F)UncW-d0c;kiZxE>X)bK(t+Qg(vi#?N=T{(8( zxgU_w=-IK5=;I%WS&qdTR?GNrYW_Cxj6-r>4{hoKVmP)sB5ON;8PG%~)k8z0K-@rM zm6Paa+x;>K{ePQ>i=`{|icNROu!s$4rjoR=K?8nycZsyEs#Zom*D&Nxsx(GsuhJpp z46KxYZEZCmLk}OI+q7H42C=TEm=sw<01Vk3$+?Y{?`;HxR!#2YRYKs&?{y)h=9hH& z+xcrV$nuzC{#~J_f@S5pP!q#NfNr=-nR&6uOSmi}WaUJGcf#PRD_ZCh&ju> zi)9u7^OOL-y9#JR6lXXB$Tv~#b4x47p%k6J-9y=j|BOvq6m`ih>0tt*Q%9U^qA@*} zpSR<#tcUS6&>rdHgT-@QK7&iP0}4};BLZPtGDczpZ8EI-19$&9>ZrTcxLhq4V#L|? zcIbZTk?Xb8i%eBu+lukx!sB+3sL>&9(TJ?XrV7+%0gq(%j-njDegah?_p;nJhIkp$g>GBmJYB>$# z;QfV)z68RLCl0Z%VGg|EU)dRY=t7`{GneeU?*nUVWEa;gMM3bj)=5~R*nFknr#*vm zMpMa@<(Jc;(-IKzamAm|Gnf0Jr+|{E0H$@nk;{Yx^uE;N__AN--^R|SdNxh&Rw@o< zEeDVrC6oHS0z9IAsXU*mIT}OdclVE{@}q|WU#5Q%_2bd5{kS@ANhzzZ5Gr`#&yV%r z$`{E$hw}EL3)miD1>a4*s$|9b%Z-^gcJ!91)wcpzsUWQn`d*8uLQ##o5r5SpJU?o= zJ6l^+jr6JCRgJ{#(dKYasxyRCTgmO%Bh5W`&z5wSPGZ@XD)W7k6|SsW=|h(sFsrA_ zidN?FX*fAVnpsYH$v9~i9WRoX&)8IQtDKq0AeT09FBU{4a}LzFz+ug^7{PN|K%HNK zAnUibV*l3NB7zVa&it#CM#MYTtN@6yRnm%&4Zp^ts>*~S2= z&i88mvHaGdI*zv=^0ymT|033H{o;2iImJ;R$T!`8Dy6JXRR<=}nz))mcmkfqo}5}H z%m!A#<~(~QoKoG2wCy{2$YOwAR$6Y1$&t0kev1t2jPYk~ZxrvYdNHx(VPVJ=Nr&Jl7oqMN| zEp;4!xc~z~V%#zay143>8|SO|eZAHzg}AhoCz>f32m*q{+Dt%z%eZX22?Cau?Y17uV2NEv(c`q30+O@(7T5dJhPm3h4jA-YjqUA=i6SxQ`ec)U!wuUMBO1bj%n z=(~-~vLI?U<%=fjZ;qK}sZy(#+ZC2M1}P6&uCb-S=5b1KwtIsG`68L_QTioovWHU) zbp2MKt!^9-a`b3A>ewOr{%*!M^u(aVPLk(W;S{z|itc78-hEzyuWmU$)czuxmB#L(zvk4d(!`mn2aTp*YW&nj#dd^*B5!pS)eD6T$9 zyVLSD85s!aI#VH?7o`3^Al-^iuUWNYD?H5^Yv~oe!w#-2HNsfp3asa1%F$ zb}$w#_IH;Zf2||ZrQsRUMI5Y%N*md{6`M0RqtS2|+U@yZqFmcW&|g)3 zpyIIjtYh?!r<%=IzW0OdzPSy3bBc@n$DOyWMpZi-8qE$|)i#o;i1__v2Oh4BbE3D4 z%4ZZ1CELjzqOXpURMB;iQVz$@?fVZU6RMRg4FmB9o?B;PE6q%mN!9}nEOPF5usoK` z+je)sNmIt5q3J9c5o#WB7nH#zip(b^(XMrX4^U_BovKHoXFL8+rf|W&SQH-ZJyHNZ zkL5AeJ=D!;{&N(<0v18iBbLp{WjI0J8E=V``?s!SR3o^f)%QbaAgYCYQzH7m2OP--MM_wy`NMhGMDEI+uY z94vIMU^lI#@iCUi;Tk$1;G(8Nt@C(6orzQG^MyH-(5HhkRJF^X#^SuXE9?H;5sSkb zN>2qj60Xe7^70^-v8L0Q(EC@N!xI6p0<<4!u+S2NP*sRU?iQ+}?g`C?HZC}OL16egg zXcwrrEDx>T^{?A~?Bq#;E{K=O+XMj?Z4K6mDL&7sVAB=oS30f-9P&16@X7c!Jb4NtX= zSCwY3l|=yu8JzQXEiC^!AzNxCG_J7R;JD_YyEvF#5(7F!I=EZgNb2$fC3%f;&~cn* zRb`Tf5IV3OH(u0$Zr$S z(lf=D1G;W9{{|X_fC=Mj_#DOaRsVtTkIMr>Lk2gAFSc@qgZyFmjZ@m0u2+?8&vqK6 zxs&iZVHrUm1%3~U!Ty@g74!PZ;+yR|4e^hAX43DVxUt3cS@h6um}K+PwzFn^Cw_4+ zpDn!-EfCGX38(%G5AY`h*zwpaJ#0N+k%bagzJwZra-q{k3g@y zIqqN}I>Sej!d+BZ1sl9jjL0cRTOg@s5CCp`lq~NGR{tfRC8{ zvEv3N;hoR@REm7t$~YXa72GO`If*IRiFm*I%jx$t75g7BusIU(^_#RAX|1W?g>yCETjK?wr*4KX+819(!4cg9mK=v zxi2h1aXiN2tCGMd)f>QS&7{!h5O02-aEKflai@fM&(9dShDkF~_>fSK@kEF3-T7R* zCO4SR`ATa#IsqYR=FwlJ8gu(#?9NDK=sS6hf?G%zhl}^heFnmsaTrSyL?Jk=kZ0Y~ zT-Q^hVEQJ=gn2jFa1V4(RG@yL6VTPjv2{=1H52X#FT3S%Iv3CfqzRFc?88>uDvk~#DtD6gFh9n3*D}mT2CQP0mn$ua(&Ke zDJdIk8Mc&MjNTo5%jIb9U!go1^NeC7D%q~B#Y!qgoDMm#GH zRqOxPhqZa=+fi7JK+29QrYbl1nhOKkXZtJ%Y+UGHv82ho?B*{$SKr#MI!06MqaV}kelph$<52n8F!*=r#59?$UrI4hRd)iQsCK%0lLK** zB<5b7LtJ@UFMv;u4ZP03j8&`*8n??-(`FTIg@fqQcVJdKph+-quP za1#QMH#2w8AgSuOAH|Lk{8BIxp%ns@7@X#pWD;IWRVWOWLQekP`o9AMHa=3wRL42UQ!>+y!V#X<|)9YKj_EYWE>)&gY zhuc)3`c#=>=saV|KzMqEUBjdqivs z`tU~!U3~(X_qj0jUa5R@M2+TsHc+gBF(x#;e=bWWaQ+_}Q_}37)(f1n^F+q1`(4S# z!-cY}j6Yxay`Wa)9U%|;PDQk*99_>fPWb$C$ZhU_v`$M8-d+Ide45U-eiHvc+R`S1 zgPHz?*~az#^Q5XcU1@nR%zUcxYM0}ac+u#Gp9PaHI7PRBoru5I*dL*c0&8osHz}c^ zR>m!dtF%oU{zLnp)lPeIpS*NJD^lAsvtp-kd>}SFP$HLq$sDL-b80RAAe!tf+eMi>l4`o z9m*>f6HRzZPIp$%`*JfK__-yr;`+mj4t(+NNqDZ!H;1n+aN+-Z?b+mfs@YTa6N0P2 zl~ZAL4(gd7lF=%6OnQuCkkeaPzt=pWUY*KjOo)or4-R zf`zVep20}`-im}QuEpOE^_pW!%n(tyY`^z$6$udml}-_*VL`eDUO+%V zLb^fHrMr=oR7vR$rMp8)1eRW6VTl!xT$WmDm*1n`d0#&>|Njgy!|crNv*$kNI(41< zpcvYwB%zJoNDXB~c~RMhl~0I}uZNj~-U1Y7T(AOtxkd*O%LJqB<{V;shb)Q%Qu_7i zbyA2e@~$6B*xoRLz0!62avp0hRRre9Y_RgVKlbcb=j);BKS%KJ`QIb3E)_U?T|$)> zOhOD3Dl6zHN>?AMYM+~Kn~?yoz#5fB(aK+oZb@=Aod-=mYjsO$ ztf)~91-FG0j@t!;ub{L2JLT6VGoc25^eXI(?nR!-Z$$>aBP6);puC~SN>pFt!@3XG zOZw$$3LcB(p&T*^QqBwen%0#H&}J z4i#eIw&z;X*hc*pnm3WsfA7xFqGzt7oTsXwK}=0FFdfS7Ay`sreKvqFpk=lJrqHpr zUnff_7m+5>T{*G=1UEOq?OBsn`tdsHBR{S5+taudGl;ZDwh~ZR=mKp3$b@*Puyj$}rg)cnw{4I@w|Jt|b-! z)d2Y(^Qp(V&(<=g$2-xL_c2KMW#{+)KNjb0^grJr`9=s&m!#)(#`D7QwwOorl z`YrJ&{ulq9F{(72r@+KAmIb>e^B#^MBU$aSlKbiL^BB| zdi8@VZy^=Y_u72M&bCUuuF3lAu

QM~FK6B>R8o3ZN7RAsNR^!fOtQ%}@4QtRQ?M zVIdbD>#4{jRYuug_$7f14VzJ`ZJ` zS1)1+u>SB{xBB@0$CBT+&>EF>zdx+k_r}=6WJ8CfG?-#irgS%cyKrf<^AjJ@H6wH9G|DJ}u(E74C zShuM@oR9RnG`Z}mW%4+@u)Oq;o<|#N1fAF1TIJ8?e)LDy=O6x)btW}QQ&hWA{r+7T z)A6Ot1-$(Ircw$%f~N_x92D=r=qW8Aysy2O>d2@Iy|3N17kIGCPSPOZsPo(M*0?_Y z_eTCQ$Mjn&^5>+t!h%U|A|-qLWDigdVGoxM1{<5(QBrqJ*uy7jDg=~w9?D-N%3oF< zWgML7@uS+y*Y%3cSXysDYQJ*%`BnGWS4Kj*kFNV0o$<80X8x+qu zW#3N#uSxlATJ)IUCQVXV^6IOlP-6P6s0Y&8u85ik*FygsdM|7EACLJP3ceMsP-*Ka zeV=M6zZGyLmskHSjVSRAS%0vrhy^d5 zR=i?TFWIEEE@4 zT5H$csT*RN3Y9EYj2tOZ6gVM*0dTLm(YDRM+V;$g;HEZLtJl(=yr@}P^hWPFOK+TN z1foK_9Auf=!DkCr8(0f#Rz(6nVtOWX{^y5+Q1KDg$mt`|N-$GcGaKOf9p74HP%u8X zp=KE#s`CeLb{~h|R-@x;q7^q#oPQ9_N2VsXnDX z)i)V?EXCS{FXLsJI*Hv5AyQ(%imaEZqQN#Rk^JeKg+P&_Csh|| z4E<#fZT`V0e(K+$bfu8|?sp8lp znGbuSBtV)G9sBqb_F#+1&LNA<4o%Wqm0-IanjJ0!+FK!9AiH!L8^d=he3Fx5|7YK^ z|8ufaHl2RJKE1}W3!;npiJMcQn6u5}D0`v2%8mLHy4mU5jIRbhkUtHT;VMn-Ws(=| z{wb=QQGa8_xv{75UZbOJdUSF2;24iLRia#RC{yTa+JtxF#DEau51tbVa|H?% z6J&F7UhMnU#3S_}VJ1gKx?1S_bN%0f z%VhtHhpaES6e7uJr~14z?0)1*$Kqa`|77!=8Y>Z)$!sjE0f#o-Q)Yi=`Mox?$99o& z2EUijM?ZhWHLtT@$c}37EfBc;BblSA>vHfFOlt(QfKfF~>5!|wo)97j8zM6M{Zy#H z&#Av_Q@8zkO{bQT`gGm4uX-W3Vng7B4oP1z^qf7q=Z{B8vU)T7*=cetWQHI!*nQ!T zB98o3#7qriM?VoLCgAFVN0{MOPbf)tW7%hQOBbvyQYi7ajo?ooQA0$_8(i4F@`Z7Q zu2h@gxbVKsrX-4x8^|zxyPs_%0MDl)8fRf9W%hVS;=*Sp^+rlEUt-ZlNYqM7H2;fr zy=u5u%8De|7>)BbqqtbrMQ~|0Bq=24v2#5OCt>tz)T(zK=!&zV1{^b)Zoz9+-!eJ? zB`!d*^qU0_JxiKgx@}xG`scxG9{q)$2Voxwe-Xn3w2!D0djsP47uOCT+{KZ8fL&xZ z{3?&4KKRVkoGAog3;HAPLf_kqb~df6k+hxxymd3+ny}>t##?JQgPvd4ilecXyb(RThrW!#`PO(O_f8uaDDVYrQ#gs zKLHVzbKmL951I{e0;?R{+@$$3fBRiRuWeR;19xQuk?=zEqYvwBMMH=0dYp;+RvSZ6 zEIp!0(6oFrUMjwYs0j&u6SLOUx+N@q+n=Y+l>hs*gVU}(i+%}UOOq>q*S=$C>HZ}; zOJnCWz#0_ zO#*m-75lhB;v3NcE4GJiClz3q#(w=6biBP(uU`VX1_sxUtV^kGQ5B{8xXxE+axcO* z@#Ff_O7IR}jn@pXPsShzOOVR<%B)QYyFy2Rvv|*`|Ppyq0bNojOdQCK&m4J5ZPHjYVXj z$tj;i&tMc?lCu6OUZNQGwO5uV=J9|9yXDsI?V>gLKmI@VZy+I2Y{AgpE2<)y!!Idj z=2}gOa3r=^i~}3SP5{9pfI31=V>K-S@fISFZ>S0;%%j$0xyB|MdKMxY!D(YGx4ZR+ z{e_su-0Jx_FG-ofGz+cymDayuq>=z`SxcArCMop3kLYuDz{*;)wMJQ9#uM9R5IPus zIVY;PXFz*s@jNBfN}g-nYeHPV6T4acz{5@XrqrXF|BJoa{>EMj$|91HSjX0QRApjk z`dAtRy>oq#IK>v)i9d&KQb}0SdY@(3 zrBU7?RFfuLwnVxOjL&$$UI4dEj6UA-s#^1si5^11S1Wq`NS>!q{*=Gd$8~POYt~$& znzHG=$re#CCsRel6xLbSRrXb{G-r%0k~S%n0%^276l5@X)zQhqP`=MfzmH-@QU(J- z;LRP)k)I;W*<}Bk*xKE4Kg-MaZ84V|OQB1QF4ye|p(e26;&jzgmdM}E^_&a_y+O7f zJmDRKf)XPtjPZaBqu}0Q{y%ncXvdEym1N3x^?Yeu%r{yu7t_d8>I6y!CGp>pJm=cR zf7=yB`VN_+%ct1+2!s!)R#@=RA=b}!n{d{^2xbP0XieAn?SGQ2c^`oYlP0!MCtD~X zhwQ-}M<4$(C4Xh*yl)t8VeYYee_{Zoe;*pCPCPHVmMW7M?kED&rrWCt<_ySG&d}Ky zzKZ4=qmCD*cG|11=@|BQh+OpsZXJ0t9HS@j+FqkZEmXf@XejzYBBhnSOv<{IkDB$f zB&GYJD$C(V#IVMCRaLFgYH+9&sr4l098_?i)De$q?V1UxdGDu94@X z`$+i|m@Rg5abSwme@$2<$TG$rH(*5_x(cZ6h?&S^5aCN}i|l}3UJSs*nPf>iJ)iiZ zjZ_AZ_up{m*xljDe$-b=C+}O%#X77K?fP6JMFiFd%=Z_4)*Z3Ys&ysB|6xttiu{uL zVED6hs^dj-Zr$0%LLI$kDrS2vfmC*J7?Zl7o;k{#*xygh@~^+^VM2AN=XEuTo=fvMDMWXrf;Z#tzXYoDaEL&k7#i)kI%#f3c(# zg%U79ceW26Xn`2K{yhjFOM5}-XPJ?1FI1GS24n~6U%rW|q-+A2bm#;N;OhZY=r^N^ zIbJBV@L5O$rk4)Dw9F0*cu1~=KFGNQ2+)SmiL_@P2oT}Paky`VKqZ(Rcoz#bn9yyc zg_Rlw2yir(7Eu3%K+)LnfOm2Z;n4K+pHan?2R)NQ79F>zI_RxoC}Rn5Lnp8bc3f$x9(&XceY5#K$} zkL#%Xrlxbr3}~Y*o{Q3H5Gj&{-qg<)+e+o%nreLKCF0=H7)NPi*&e3}WG;vA+Svek z11`R9`suw^zUIcK!*^qcCJ*LgbICpr@#*h=(=8LJs#kD$7_(P8J%c2GmtdmiFX5s_ zuyH84s_2?9CU#HM*RTQQFkDAO-qxofu#gn|3L4>Mudrhy;gK*6v0{YOE_ zfd6?CCVO3e*Lmzvo+W_e6I!$9aadw1mMJx6dn>=S-KSy->;Cz;>^l;p`Am&tgfokW zgZPu`tOGgat(Ph0lNP}cBIQ9fKP0zM|KVJ5&-`%*4>wEH;2=i+CEak_9rSDlDD3QU z^DN2?^D@>0MCq!1-{zzHe|Zrzg{&Sew#Mi!flAW}WEC9Kzzy$@_pA{7JY>P(pK^kG zfp8JV2$4TWd&~rZ&s`;+NEBX2k^(e{%>D%mSYvZzKa2ut@XqVTS&^edvQV`|us|Ww z=3K~F)8|nk&$V_b^4T#sbnKZsFKeK}8k6CEv1wp@maAJs{an4ZWsDzQ1B;d-U|)s| z1_4(h55H;7c0A7hoKpD-*VG02KUKfx%Cr?6zP6=-TP;mBZ4#bslt14nnGk=Ek@*oT z%+}!l*hBY&IwNRiH#s`%NFhVYMk=tKZ3C2WNfh87uaZiY^RR76GShppXnV-jr4R{R zsM^uoXI^8+mkYMdNC(P~V3ic{cPO^^T|QP)N_YWANKpn*p4y_LXZb26_B&*8EL|uzdA`hL;kDn1r1VyN4}?7h&T}U!n|f3$YE{i@P|^K4*dL8+1;sK zN^rxzZ2HR!J^||7?{)9EVo;)=-}jn%=z5#N)n=&~HOW*7V)B)OG%!n0mc)|02C?Dq zhnZaJ_eB=WDh$cCVH_0OQ{9G$qkd|fwFzChfk$SAS;f<{U_NT)<)?Ux;of&x zV_apUK+V(Sj4H<{4@H^impC@`H!pBaXF9G<220)XGCuLw_RTZ@3&UX|y%&!=&>~JQ zkhQllbuQH$!F+tFoovcLkd;lzcWaP~vQ`&=&tv|O$@^Wxx<`e;kJ9E2E@pn}V$Mm% zv1aum^tjEO`)tXf|Ms})KU>}D0$J|ji4ijtF3U%06$uWPl@}sLLV?%twz&on_)`MB zhCFS3`31V|vp}s~Mg~7L^IXVfn*;q1>@F zV5!R6cK2{6Tn-N?ZPtmLC4zL0O<|Ib^8;ECD!`@VkR-%baTzdSm%EEn4w%)*c_Zp$ z@`>`Pk4axyA zC6WhR0bv*4F^GIK)uDB5;@8-eo{xRsa_O41ybNX19K+~fq2R-MkI{+*IAT^B)8&KEz3!gz z!X|I6(i0;WbIWYlEYrrU{ns<9TpW*gCnJ!S)C$@7vi>(cs$^~^#`8CRX*r+pyV^ir zM+gUD5{v>iP16|mYl1FmW{wFe?w$BUQaiC6;su;bcf>Kbob{-mbM_TH{ny4iqoC8` zZRQRRcWc&SMX2#QY2I79l!GPCcOut5I7QWmK(@}8|L_HASv26DDk9QCM zWQn?o!RH_@p1}jxdQY$@tg;0>vv}0MZA*_Vn6%_fb_9{n*Ey`e%Db>FAT{%J|9;}N z3~VX7dgyeu&DeZ?T17Q``SzD6RL!|*r8kKEM!#8o$YgX}mK$4z{)nas>Jb1H^+kwS zl92@BmJ5h@I|dfCHfnl}BfN|iEZ8Qt6dcsSpQ8;y2MOQEr6bm$8Gvn$8xz7Fs#x?g z=>8iCt;HzF_~*1OveSd4&J-IB^B=1DL*u}E(mP*}>`qMyy`mm>RdP-yL~rIq^x)~# ztQ&Y6@&hB#iK-KlW(^)SF8~+t~mF?0n<^}PAkc%cl-ReFc?9WIaC|GJd z=g&Bp{3>l`x%t=tl&uutiAj&7koB)&BeyYSdt;l?-BaQ4=R~Ei)=G^0Hqo(P#QhzF z8EmZM5ITN+en?D2P=)=C1z(76*NGFA<9_y)*Z4zO_J-1_2rNr>naur)Mt;?;1}@(y8BgcJlBPuTMRZ1~ z=X|cxRP1c6q6s_8DNc9UTblJbn!QdFGeV#JP#DdTc;oGf!@}L( zvdr7&n~~u3EwXEPoKV3q!^juw5cZ+J4k(mGs0SESEwKY7(x)p~Z#FAyW;Ax)wz0uJ#7aMV2GJfae2lfM++<^0GG?+=6~>_U zPVeFwbE*55ud7|(D6Jt)jZLnFdB4~{JUWGFj+HE) z9RU00kA-xX&vh$1k3dN$49jAh=ZPgmL8q~ZbJYZogEbbts`K+KYDN`dd^p>d`fG-h0S1^knSlqND>1&wz}}6)O{#E zYxyyIif~P32Hm;RIX$!-;9eE5W#OIarUyj){qqz^aP-d!+r*P)FD~fL%(rOoU-8$D zJLh45@9SU9N@%$_?czN@Gmbc-^|#YTv8umm_Y^&tY}}muUU2KDvdb&E+~Q85&5e8g zmtEYpJHVC1iJ{_ijnP>r#Oxe*Q1U6z=nVS-0GG{OH(6$v{j#=3(Nqwrjed>M9HYtK z@6|v*Sfs(FLrFtyFP~f7Dzym_9QEs>p3nwndyL%bgJ@w;c9H{)phS|%4wHcOU|*eo zLxL#Qo^-U;%>$;+Ho@>>(J#$zdvb8>-q`ufGDh0}V8E%gI5Jk)%enE5uXBvk)q%A0 zR!geI;(KTSS95#brGG`HZ`XAAY)Mz);^P7`{gjXExGbC(-%fsPU3&Qi!NI)l#(O3H zl2RPE!wGkm$3c$~y%ybXe@UH|-=&o9EUK(~MzLl176!EKfdcp2yG*(D@pm8k+~*&! zli*gf_0f@fJ2(|Rd7b=js0R$cI+4y*9k99<5;y-Q>}Ajyq;Z6>p#vdrH$Efafv_~c z!V7fouX)Wzke-87J^O`Oc5@dn53EmcOW9gAVf1>!Um5N#L6@K?4%ZLNpSns4`?{X+ z+Mqg{5ikX(YgPkVmPo%{-c?!{_i~W((dL-92^{tIO^DgeA`$6eM#;T-u1B}~L8m0m zr_ic77z|;Hq=1WVC|>?@#2Yp?(N@b!by@SYIWlXwK(-V)9slE><(>3zovqK(N%f0; zlv8hg1S!|3|Bt;L^;B}Z-DzA|1> zw`I2tikLV`W;iJ`Dth^aF87bwf-ksyid278nEH`u`XE9&aY=r+8YOxzljmrVOPZ@# zsm4u7$e&`%SlL%k5s}n_WoMThE-WM zX##&!RfoFWE!u&@wZ_d$b?QK8(SyUvO--*%B`-#5D)4RDS9H)DZ67f&@N6pktRRP_wu5+6}RN{3^0`?@e z9$&aw?^~TAut^=su(X1i4T@uRb_aA5yCMxVo4(f)RcIwC9mMcHeE%+x?Skce9dglj zFHA)j6pfA<@x~2v47J}vc#u#rl50OIJwg7otn`ol@_y+6wPrvop39$L@Z*QEap;R0 z4&q*Hix@&Oi(p-45MR?y8g~PIH&fu>7tOv+d*CUG-YCUP^FT; z;t!MB)d*HHd;P}FWl?)a2McfdO(kf@=H?-?U8K$JjD-E+H?Mwebz4)LUIqsG4MYmV z&8sx(JtQFBo0IhF^9kw#AjFZI0UmbksQ;V7;;be4`TnjjH0Ls2eT=?j#<9G<+nrMSzmT`7{4*x#_L8LRI%DSBv{$y$kaUm z^{J1D8Izzys87;1SH9xF94DyJRel*C&e||$XZC0s%tTMG?z`3>E^U#!FqhH7OCV^{ zs~;IxQs!75r#VaEj$XqU6-p;yaacVR?Qe11a0^*utQhcmLfb7J;6}LoVyr&yDBiu| zsn+{(LbBKqm_NK}8&-6k%2I9UYXXP1d+MVW*1LzrS;dVKq@T~C2%YC{cTbu~TS4*4 z@6EX{pV-84B{F{`RgX`+o+M!6kn%$@CNHA1mjh zdfif`Hw_kc@L%pchv6+QAVv@#FW)bF{gT!ZGr8zNDW8NQ8Z=Qm9DT6GSw0;JqW=Q9 zI|WGtKSd#8j>ed@^NE{XXG1~jOUszpdH6jOI!aq{N9Gy6~p!s2{ETzHij(9f>9`I@W~x`_@bO2G)R#fjpQ$g{1eE;n-x*Y( zN~W_rMY=Qt@8p>pRfFSzyV2^xRy35&V*xG@%NXL(ynwXKGY{!D{CUw9Lyk^AjiUwf z85?=0&6vj>+mD}8Y#G%=6+b_hx50RBmkSfZ=ez`7>xqV1EHP9O^GM-vSII_=S+%_E z`mPfR$k$H;wLpFrVrfSpEA;0bh{>7o^Vl8Ef z91w&bcKtj!^?+YvKxIzty?1G&k*1ngE75Ch>OJRqHeEW}6*9PrOZDy2Edzs2di6qY zO`E@&nM34W|H}oa@Ar3oagbe9?0_0CYS%B~UHI?@*N5*Br?PvNxiLMn)dqj4{O#DgZT_^c3*E^x-3Ze7y-$@5OO#gcbH z9H)__7hG7b;h~sM`E;6OPxW=nXAKsvr9cjWqh9>H)y8Zl^RIvFaOm){{>0Kt^9F0? z08vTu16}bQ1lZ_R`@NU2=R4~A+1vZeriCv+eTbdot~MMT@uIb4V*rr1L>@K^++Ng_ zSey*dp+LubK{-Y2gL4l|P{h1j@%#zq9Sgt$z(JY)KAv+k_t*lrEsx}J&@Wgc9(Y`> zi0=_i(Y<4pi#3h^+9(Na-T65J&kc3y7Tc8tGY%TBpU(UY9@?XA=he9yq$HB|#p`ro zat=}HlsfKX0li| zASDs#OFRFgHCnpyaG^E&BOkO_%`3;^gTRlz?^Lz1E2(0085A%9peUw~um9y$VJWjF zOQ6>e|1RMGolX=$I}j8dbs(ZQ|(cW<<7adf)Lph7thD93P3;P+fk!*?2~ zkzb&1Eo#`Br}a(0B~*Ghx;|oyjUN0g?jh2l-9dH)4`8&B0XS}SV8PybmX1@ols6;s z#dEDpg%%xd+(%nXKrm+|pG|?pS@@Y0$_GS8rM-y>Q$feCy1GC#|EBt=Cwki~b>Ptn zxGK;oP8+i!lIxEtTECi!kT=gG(82KQ0cnoG2RQV z9|E{H&R}{*o({{zJWCS87vUEGg@HO&LEt3K!t)=kF@SmSyN9EYvMx}-2iDRuh(QxS zbO(rr-kVwx0nz1mq$pGxw(F3WY1B&|10X}`iPty9xJmoowd!y0Bli&~sY0gudtl>< zVqBbegH)kETz~XLxr*dd#eN?)=bGH%+L#_5ZyQrL6-6G#p4jh_8?#Ebw=XJn0c!gw z%lN)iQ*D%+vhi0?B|8<3w<9zBnwP=R`KE~W2$z6L@8e6{!lIHAiKk)S{(W@~t-n$n z86ayr7#C~&K;&drxR`~(oCMoDgOW3Zoq{L5XD79fu&0_&0&YxWJlDCO zI(vm*^aO`^)q~{(#q+@&?J;u3mVEI0?U7_k1v;%a6B_Vt*@i#E%X$N>SWOwc@&ngu zSy)rR2nc0$!AiKkiR*lF(jZ=+_@P?8YF(5-z@|a^mwccXmBYZ7L#H`rJy(2_OyA^H z67!GMGz*c~`_d|xkH@<3t8&yyrB&)k$yoU9Lab^;YK^M=1L@jJ(LiXt z5Mh=F&LrZTZ+P&tlWEl5kRYtlN^+7?*Q8RA|E2LN)+Fmh*~Od+5Z4@+g?7evJipNK zk#^GdMt)%&96{=KC5QoLnlw3hqQtox%YgC`>}t2`I>-ND!>QVX+y;_s`-M8vPq@rl z(Jn$&Rq#?8AcIocjNa7k1TF?dE?*;m$}2V2#SGo+2~Q6HaJ(dOfRz8sw^C1Eajvk; z?5)e-@R)SybmMalOcUFpU%9k;NKljLAwn2z1EuMHRz>^8ZYB55v?B^nJGwC8!|&$J zQ6?|dGg}2B1IqBUK$U>S1%pkMQwm>Vp9f@%9KL|@>VZ%3+?V%Yn(gyUd)I&XbAVbyT94zMs$CB3-kY<;;e#J)Bhfli zeXBJ&2fFn}W5@_dJW;Ih8Y}+ES|gHuzDt8%mygz3&I5JG61p_asjKLuTiU!gfSURYd@c~CIia4Kp59vSGe`e4ra`i zMW@F|@gJ_;)?;_S*%GlSL>_eBz80ywLQ5&J85v^K|G2~8h^1+V0S4HP-w(bYWVcn# zZ*{fpweXp`e^u_mPHY`nssaVZ=w}pl!>a&$>JG%*IL0f!vCM8;)I)OcXAN^YsWT4+ zLwi#U#P1eO_=Fh6PF#;|Vk@2caJg!zSwK^Vqhao)ML;`S(WTmTV`Z7OmUSflOOjKd zEXkKaXTC#NbgzZs10_y$Pm~2I{?La0$dn|vw64D=Qy7^C4#e+EFT0>{QV?I{!!G405@8Hf_>)kgiW7!c)H^~$oQgo9dM#gJsno?H6040L6z?SKuop|R`*vaMcB9d+qo7PwayrtXC^pa=Y`5|oJUE4N(%OFNeMLKOgK--o_0`t_9$Ak$KnM=gU zs{1}t`QPeXrUqIo)+l>o-eb^-@?aOw|N6nzFS|2{yaj7OV{xLC{ywjvJG8evF1cx- z-;lAfrVfkkyb0}!p~x|gz8W9si%R>=Jlk&DuBg+4Y3kO^(dT4LKiShTm=f~fa$KNr z(rPk0SSq&EasH8{%b{?eZkBS(cy|%W)43zI%O6Rpfd?P#4?TDPw)-o1sD}X{p#cmL zfNQxTDR1{Z`b@HaIG78>%c25%kn>ffdOpgh24~9O|^YyS|4ZJbR5HU8m*mS$sZ6t#j!52mjI$3|mf;c+2FmJlT>15Jd z;HkcF*pN)DtqV6|_0VO&#fJz!V8y+(bj`p2&6UYFsprTLC33Wp2M}xf+ag2kUSZap z%_fGfOKl3d81xJ3GDowH+7AweMZLD3!G*$BfZOQ%q+h=PmPfr|R<;8oC9S-`U`?9k zAXVbhzm!#xP&I|vQujL;+WK{iOIKD*9qmEtST9Kqu|MKgCSz@5qExATH7I&`EpsPeBvL3npY9cr;?Ef(E!|h6HLATMjHmM6 zPTX*VmnjnfN7wiS*AKS2P(qdASI<0Hni1?q?7-a^cV8UXCVc3;kVL)TF*r}LxuEw! zbYau>npn}V2_OQ!8Y>prT+5eDptZeG4`I{D?ZD43dB{ybN$WM z^`3!fy$~5dAa;)BJAXf$t9a^Lul)kpr&O_Y6gz+mnTa82Eh>tYqKG{id;?alb;Ys# zf!%FNj!X=ylN9(bg>(5-`%SmfU`pw_cUmdPoQjHuAUhebxQ&i=lm*7s22&sBW$WL* zz5wwhuTv|Ezc@D>nbf9A1bS+C1uh)zO~5d$T0hEO3ik!>Yzo(~3tU5&Ki!_8K<;hE zAp#SSvkxcV=}<8Sdc|2gb=!!tBPNfroZ@zJ*sOLnIqq;Yd5pBNmUsON*=<39sSRQRGWJnd{#ONy3h0S$J1qT zUFlOc#{;)LXV?1`$woRXx^&w8O3$5d`NheQZs57hVEjlX>2_JRgoKZrf@4waqqf}-gS zmV(%aC9<6Z4rL0XPR-ouLkt{Vrf3mG)QId$a@^Lw0VbMx`>uhzZa%EIqHI&^WeQH= zV{B}3nKQ&9R(~G5Ft{gHIaPGGb}#0rQ0-#+<6Z!;^iyl_X4BEu{i)snVzeW`$IDfg zzu+1Mn8^c%u(xk>EZr{Y3gW;&dPoyDZGlaZ(j*SoQg{bExk=_g{hr_Sg2v$5Mrh1< zjZ*sD;S_#Jf4x`0-n1DXEOX!6vsc49WJTM6l!$XI?-E&!%r?yn`m`q;owGD@Hn
9!&-X^mt3stYkH7r&(4>zlz%=9si@^RO2-f<%S~@%VXn{UARz*m30iQmIe$rCbmN|1@|unCO9_aP zhyN=Q4VbPs)?s-gL;A`TsK!^-24LBf;&CR%-mN2=&?_14i$>T%CY7A!naQpN$UqZ1 z{J$0)!$Y_qz?BEWQnW330ZXHbw>8nSA7P+((N19kJ*wwEer3Q9iP;&5Z-0HD#El4e$3SV41#1zSx`&uAvjC3EUGlF0!@z+X34MJrw&YBkH0nAXi4qt*X_c{nOt3bHS*o(#JCmu zVJm_E$)o80X?7@+%Emwh$x&u$f-8K7c|-e168=siMvBW=GB z?kv=Qko}R?w6()fpn8o$#c0?!=q-=Zyhv!e5=7F2Y4PXn6Ib8H4BgEHe^w<582nga_{)Y>1RBMfCZ7>TgtKVlU#LCz>Gk1gqK7cLv_7j|uXgFGgs(UVPQ zkc4Yt;1tjzUgvf%vy03drR1HK_U0e=`!YeI_eBuk8v^UzeaVzvBVV_x<`z4?g93pT z@b%^I4_I4vic=c=Y?*c9fK^`hV#N%WC1RP!@WCPTJ`pxuryR={Q`dcWgN}_oK_=g< zTx>mr-<)t!)JyM3WyQWu5~)#?`7a8xTcUjmjAa1C12^wY%ePt@H2qr4qq<(Jm#H|* z0UXYOeG4xy)Gxme_g{pBa3bbrJmNRwDd#83C^kN2Az2abwRl%c2~==j#14 z)wf#B<3T-0cfK&NoN$D4W5lt$8P2ayN8|v*KEUr@5V0a5D07(iOF)Vt^@(!!@J57Y zB*=GZYulgi5Fxo{-aaVsERVCfMfUXnNX|lyUI6j3@G-^=*wPk&zZrwiCNE*DYE487 z+w|Tqav}{bz!Pi%Rj^4eIQ zC277*Ify&T7AL>DvR=X5nI?XR2Clwn@YwCvEU9mKKc<*&{((|HwnM2z%N9ItQ1E($ z@Bm^74hL9Tjkg&Q`1O4324%&L_UXm>VEbB$NYZir=j={50U3Yto@>q{DXMH6x=a{OjGh=I=)!;8I^e-*>pI zCoeCzos%~|klMU%n%j7Lg9sSoSf}j#)^&c=W#O=SOZsxi6a+v$x+B6nlMkr8y?>EtqiD@@$VB{w95*Wb(@-`t=v9J04ek69?)f4(g zyu9h{T-31^lo%+)N5~7(uXdpk+cq>=Z8)SRM%(&0c`}-7cz#hz9i_SJEx9XhqGf|Q z@y=17*3XGPe!cB%yj56b$%^?c+^WFM$o8{1q|5oaJZlG{eoW;AI+-8v$cPZY3@ zlz9FoZ=7b-YQ~j45~68?(X{R`C>yMD6gT$CPsAF`DoK9>OHX-Cb{!~IxuWJlpvQ1o zJ(`*PeW6*l`kQ4|K;t(s&*ct=|G68;P!i zPT_4zpAavPu3G$}rix5@rjdkFbgfnW*;`-|Ul5O_Cv4o~%=b4#&Z}J3eYvu$$fUnf0)|wk}2O zGNONce~BZep5)Dfr}pYQUM)-I&e|oOJ~;GWnHCQF)lI7VrEdn@98 zLMH)?>#fj9`|pIJG;tu}F3Pck(rm7y0**Ck1|V_=_Ee;WEQ4R82u|KT0+yT;ygPU>HT#|JpBXW?5}psR%#J;e8;${J zR>wx&v)(WAi4W~Vb)q9&{waDf#@r{Iz_(VYYf`E3ATzlwoDOBO1H`uTb>$a{2gI2R*jOr|rO=G&jEC|J zm?sU;CxGmm4bW;Ays|srPNW5Bp8pfUnB)2+ybg}~(i=R`M`WWsxZ}B6-lZk7^A+n6 zVAJ7w`wMADM0h>o*_PjZCCiUfsnS$dlQD@Vq$Upj;&K|amMi7b0b7zcJey`2GxbgD z`PbPCu84c15=|1swhSY)jxnc@I5b;|Te91ygDcWJ6Vlr7H!9NU>&_Mul*xAd781OIZ_3sUU0FG|fM+LF2SqWbxtm@Ach%`Z&5>M1ekSozIBk z)o&QWCvk-?OPUo(ZkIdVCU&?SfUYt2>*yYBrHg)3y1pqLV9NqbfN>Ihi&<|*>}w

oas;#_!8>*m>#TV@{C zbj={gwQw_%dNN5#_~Nr+u44KdTCWxKAAK0!{sV!in{O+NlXXg2umU$$RNLI!od< znIj2Nx_)-o_T3{1_FiUhh0K%GWgOf#aR;N<)xHiQb?_6#gT5d4?Y+Vhv%e`Z#))lJ z`S&fB9d$atMQK$I-Vmj{1H$_&xTn_tG`pL*IVsJAnERrhFkWq#PNk*c;W{ka__x+T z99eG?eHLt&%+kDbg^TRXJc-OaF8!uN66Dp38j?J-p87b$D(bP$%;SH+2|r(V(igd; z&?tIsHA!LM_YbUE}7B zxh><}KwTEcw^rUIvR&S#VGXqrAtD?sQ&6M3XfzW@#1gP##KQrLww_El#px{H`!U>V z@@?voY{&nh;iLBYSaBm`(-%_)8fC)z5`NT42s1C7U~%2975htu3Sft(fc-6+kg;-U zhxku6gNMMvNwnWqX|z zf$UptrUd>q`z(>IPp7RZ}hcZtx7zSn!13|iZ|J7QlkF)+9ar~}>Xcl$d^3(VZneH(Vl z*_$FZHyH~oV^R5Sd~9HU!Eo79w9nan8M6Bd_hqQgqcSwd8Os-OpoXV;ltayrPS|oT ztM6XFu)$1M*L99s59TLrX#YRvzB($(EqdEPQc93+knS!CDe0DwPATb*QMwU9atP^0 z1SE%+kglOShVF(LzSn#2?|OgVTHpWQEZ4Bm@qN$iv(Mhoe)e`WP{u|ge z_ts10CKsxM6Pz4pt^T!p7`aET#XfB^J|baJ*0-KBQE4mM%Cc?=5;QkgMLZ4xL{CTC+Pem^i@@B zHCp1(&6khWMt1MKt^Ex)7wgx+XGTCJ{qoAf65!^JJB?kddh;BW2vW})3`SZ8$Xt+< z-g~rt350N^P<4e0@efCwB#I7y^FEOAvD*%Nl$d&{0DvLMc8-)oeoC z^>eC^!@U2a1z7%6Ek@=JJ&d-7E(L`!m$@%*eq6EQpl~0%Ux%?~3N6ZWpmwD9RHdFL zFFp>nGfzxC|EA_HBZB9|QckgwCoaC8mpuBRqd(}+_j~3H{vd?7DypW4MoTf$T@4!3 zQ%%)7%s3w+{%+uE$xK@z*o*P8=h%HA=~0vK(SFjY$_)!8<*&Tq>ah$C!a5F? zxbNa$I+`@Z^xK5ww#>lQy09jfP*~0l$X{}Jbnx)d_c)-YcP|JEGu>rren;Ui^7Ka~ ztt!gU%}>|ltMJx*A)$U4=$>+>af4@9DtWA*8e?buTrY{ebvUdQ?g_r{8%{Mc=me)6)aYsNg| zth1jaW*Cq+^b!-#?!6mhHN_K5;G)DHW0&{> z9?x4_v0Iy9|74v_HF~(8c!H7i%~a6GY2h?tV<_^QwV7iYT~w*WpO)qBjOCn?66R)U zJjtNIM;CQa$lCePrQ1c_=q%@`{`$nmWy9Tyu}h27So(;>Ohp6$Sc0i#GtNoyh)5E6 zz_E9znUL}hJbOs2A*+DGs`jeF&>0qC~)2XCa z2vbHG|7A`Sg)5W=>cpr8^6m4$i?yZ`&KdapCe*fksLwTdx$-_~bAmPN z9)ADxW?{GMi?Cwy;+l0U{Blvg^;aiZGl zGh#)pBwg!(aJ5raFSa9~%n7ax#dQU`jaQkTV_7ZB=ChHb!YZn75pJ|}wk93{ES7;M`sbN4tL zZ+}wuN7j)=A|D3ta`W1A<$NRS<2@NMz$UR5i|nQb`CNRRXRGo`Cf||}j2Ryj5TieD zXzjcNBi+?|R2Wb@;)d0G4xMn%jypZpZ^LqzhPrCgDr1fX?lUhC6s88bLVTTFdyn*4 z?BDx0af-!N+26?NiA#DJ{l~6omR69*iEp%a*>bA+9$a|xJK+!A1o(^|Srd zmpkr5_k-L&VDJwW+^dTDAdiw@m-2_` zBuC@|XnRysY)f7eCjja0_^sOnCOjZhF#*gkvp6N;^K5wiDeq5Bf7m5<9C`43F+I0jSqfun{?tN%*X$GRDmHKBH5W zcVx2P+*@!*74v*(L+As2$%J#uuSlJljE)OCW3pPYo;s8k!{f^83dXdITj7^IwR#$~ zQeGs|MTCD>6v0!`xf$o~WZ#+~4;c@P;DNX4{qy#PDO4YB=z!rOR#8Ch9lG?8@G}4< ziiMb1`j5)trNoo@a^#5z+>Q}m;p}h?`Ro z`UTmVXY73(@BZf(4mJGJU(i5z%$PD|OP$zD9ot4VJTHcPdf*8^c=Rg!B|Qo4cxoaz zO+z@#vXvQ?BaaOJQ~KD%wrK2GLt#M!H`^gE#P0e*Qg$u`<@j~2eL~YMssh+xi+ zrL59G%C0lQ^^lXxH&CHA6zfb)w429A3i#3wrBr+{>DA7J(L{Dj?aSG!Ykkr=BZa&X zFX>?_&wf6y!MZtT=@#0#4G)aprqt4bNjk*2Vp+4@&b^(7Ts&F0pg4!};_*ZcfCXh< zpku&ViHL)(qyD70G?!_;rS-0vent~VJ52LO&}2t7M`afvLOB5??l-RswzLG-MR~`l zhF-oRD%9aXIj(N8U-n=r7RoQC4jt_W^rfBw%QwlOHY95Gd8!J&@Kw~@j_-EMYU&C= z#fUF;Y%}!r$XUP}YH1Cp=ok0v%&;?7E9NWavnsAFK+8cbzq6yzRr3fBBVX&R66ztw z{sk+m-P^Z!TH76-S!f#$KV+T!BkiLp^$c40A`?%O!|RsH$jPDHFa=RsBxpPktGE~Fn1%LA$h zag8B01}Wfl53m`U*sjpUyurw44%wm~(viG!W|Ex{408!l%kr2uYU(^WudF}b5ZBB~ z**wd~PI)UFS30E@tm~;;1X|_uYKsnCa|Y6TdQ~f=eb7~cq2P?NJF@?dtduH z%r>RlSF$Bo6U$dV)~$I+z8uv)JGxiW*>f`OM9znZBW(~5?;=iLup<~(!47m6ifc;tOg55HW1XZ-8*y&?Zpd61Uezydw%XESba&y-a~Gt~ z0-j`ijvK-yNrZ<40Aka?wmQ|5pRc`C{=`Yg2bP;9tv@E-m2*jcsL*!Cvfl)rgq?Wu zb2?!C{5g`OVB|*$%` zH2XGK9v)jBvlcdi)dcC5Zgak5d+%|jWAom&TgeQ9tIX@p@XiP|@77}ZM=5c$`8;~C zCnQ-MZF@BoVJ3v>mW=s8g2=B^^&0x`2-mQDVQm|h?YfZ|T9fxWI7ll!t5w{Yh1!@BLKkAgWr$@vv)&A{bG-3QjOA?lD+~^e}H}LD$m!njK{>-9r9V<($>p zqQimFZPLOZQwL+W6_gY+^Lv8BT|MQYT-){imp8hw-tp*35%H15Ihx^TWwl{${*qqM z{|tx9pL$PiBIam$J~V= z7#Fd_Gw+^rlfl$sCsidh=lgd4ZjEXyT;Mj)Y8PWZ_=z$nSOQDBDhW8?qbmJ2OVMghox8Ww9 zSslQu?B(~8;u66q)$u93t`e;RRLaL71iuIy$P`tD@F^2@>!! z^MtisD9TmnnIEmF>c5E?pc5#HPNqgrI=+S8W%nZr6OiV=KD-HodDdvEGx5>5iC5@? z3<$0|c{(V?ey*(c%BgCKiQP4|qN+O>V|V|s%!gVW?*XjpiDtnjK}0Fv32Tqu?U;-b zT1R>)i?Bj1RN8r9ag#)yEP>w8ga1Gis206f$cGHCiDS@=mH7MN$G%!I5!O+TN{JU4 zXnJX`yG`O%esGqHA8$}XO5?}F{!2t6t>|f5WV5)RJ@Mae_Qa<>=mDS8-ghRHK9YO4 zdoYqA7lrSouLF0k6l+{F0gi$Mi@dQQ2SLmj#lC{75%em(I)OcueO!#oWZESItYE9{ zj^~@n1jSHNi{hWNh=JQ!-5FA(6>sj@*jwfb;CZm!jLh1dP}?YY1|L-CJy3)@*LLsq zxq=(d*b{gXg1@A&y6V2Y@E6arzgo%W&2QI}OB4S5K3(56@ZuoJF&VrQ*A2vGFN!~_ zE$|2|sIQ_HWLB`DjBT5qG`lWgLNTzf*N5j{Y|cpOMHdPqmRX4I=Y1TxMJfBlol;S= z#f7R@bQ*O()G`}!=#mN0PnRZHMs><*XT8-=?bv2Ty}%!jPI4b;HyT7K7b!~^lxsSw zjDf*lq3=EyRN(ArS8>F+2cr#V$dM0Fc`sMehy8GjzZRQ)QPB3$J(lW&t#bSe2j*I2 zQ~cZ-$@EVO_v!~E<}T|d{jTTmI=X}cuW&o-SR23R*1h&*REZULlm3f=v^rne+J2y z?^Q!9E>VMQ4TDwQv91@_C6+@g-voE>xXbrBru6&U)IrVP3nSwnSjM+^9%`J2+|*3v zLRzJ)LCHW{L%uSs8aVtgKJC#4Ok@v67(hm`Ab@UIJut{EHUD1IkuNFt0P?V{hB4>v#y7H4hyony_Zqn* zX^B`U3G67xI}=F2-bUdtZ!tQ$Ca%&O_0$Gi;K`w{eBQvdcY8W@VKOJ8gT1bxYHy|q z@q9CnfqqETqM3sV6ck6yKU~Gm5gAY-Rjo$bs#pgf#icKCdM^y?{T^&os61wu;MTOL zT&a9tP$Ov*Gj_ARSb1bd^lZ1;$O<(##nwD9V&+L8Cw+;Q`Y#i ztfsI4x7;B9i&NF3PDyqa4DuGwOc*8qp%E);SMGUaooZYWmrKmr)Ec{?gw9Z-;d3-2 z7I*|B(@l@JMDxyEDU;cK;6M||NfUlIi1d|kKQ04yOfA+C7qs*PJ)hRL+{P+-$a!-= z>ub=5zJ>(pNo0K#!9q0!o~^KWhbwcc5I)V;D9p~Vi6>XnSEI%0O-zk1=aT}i_ zv-bDvD>7~&%g9E_Dup5tx#8y_L6rF;^i+{%z&f9_O0b2<0|b8bsvOR4NHRu9ZML#B zgs=0i7=sa0!LEQ%RkN5+O_&im@d}q6>Oi9hu>;kEw zR?7}Eu&85w9xbO{AB^|aZCgCc-`p=VNSI(2pl-&&{xItS$~`qZ6&|6ctq`BxJaE~O zf!h6TVS^jPKa9cewD_Gjf81t#an2NE7nCum{R4XF{H;m(^V1xiKq|VY5Q(xH3OZ!` zD$b$VSG*kGr*A`yh!fVZm(=A*X5(ICuyaFDR{g4)ze~W@vMH2DIF_QwnTMu^f7a!u zo*J36k)3s$1b%2kN}*~a3VM^a7ChBbq$x*vZCl;X)e zk7hD+l4kXLp@v>oy>9a=$wS`f_CePd0!b|<;kJR13XYR7sykiA{K)ZN*{|zs^>80f zQ7a2GI)sO^nmi*1lXxnvT(uC=_6p>Ybk{A)Lqd^gZmGjpy(XjBgc zup%@(jEQvM#xm_n94fcGfw|9?P%e=Hkxfzdb#_U5KBF;f9i@yWyYDw)xm7YEv)C#hC=dqV02Sq`evp2(O!;We8^>olb|JZ9 zj0QJ7m32ohD?B@VrUox6-q>t0wTZZAuv|Jgk79K62WkXqrqyEQKU%ews9lt>X_GwI zEyT{CG~%_C3#5;<)3Vi8TLv@l^j?|OV~gv&4uu?D{WlXnRoBNudgyG>KAFDmEt~e@ zLkAKyL?xY9`I~n@v)@+eef=nkT(WGc_>yO)T@1gZIH|F?gdfG|V%XV0{B-J9m_N!# zW~H4*<6v#O818Os^yH3fDe{Yo%2CSJH~6n0%sIh8&*Et7SU}89*Bt06Mx(N5uL6w* z{TH!z!RlJlc$mYb__nfk*r*)TAyio+YS+D;Iq_&6uIr`_hh9xW0j*0d`n~&3&POXG ztQ?;^cXv^IVxo_k&1$ez#qWf-$?!Ar|7}D`;$%{nMC&=@4ne>1GXO{Z@k^^KRk}!g zKI{E%pJ%c=ZP0x{S*RP0ey(pF19Xws5oltrxJt`nngNF+Z^IuB-NI! zcs!59NbYsq;Mq(78hy7d8@Dy(>APCOHjn+5I1);FImlt`@fhkG%?T{Cs)pB0b7PPtD8}R{XYVv7b?d zCiAk#)sLxTzcZ8`NqOM#iesMA^}q7w-}vieg1qjtFFM7Kv2-Ix`8VFu7l`sJx3_Nhf+*K1jSaT?he!C^{f z#pEaUroHU}N`k8Kq=(7*q4V3=gxZ<&U%l29DC-g#yvoBRE9uvU_EE$+M2%xz##qWZ zKfc)7`~*FgH)It*=44s=YbQdebwX88r2B;ikDBE-xGqQutp}z$BQkwrCzN z0KG(X@F~qpQCHGp4NKN&4yw1^UBGjY;s|$o3j8JRd|S;;Ub}&QJZh#wt$YlW&;9ZV zPtzGJYzXbDb4FYxaI{A0S!dbcN5o<{xQfQ&Mj;MD6rC5$Wz|RAG(b%zimXr^Kz_1< z!BQ?uy79db%5<+2qqFnuZxU}2RbfS+xHBAGY~27<(0__Su79z{0mFUeAe_Z|q4Cw6f_mZMuHb z3J+1+HW>Sfq(U?K&){^CSFev{6tiGsl-H$2B-cF4#NDR=NwJyzfpvRKXxhqmjiR*9 z@tgA@mPq>LYhi~PE`a-`-&EoBU44pCI6R(~$q?P#xi}NXZf0-u;Na{2UJ#PnwwIqF zlW+y)^~5V?;~qy1ER9O>F0?(@>7uyL_&9ZqBiFSY?4NpE&mG z{%XK>q=%5|iWpf@BQFRQ;0hh{x|RA z3tpBDwtqx>)KUK{!qxF~u@XTz1i9teRLT5G60(j}_R>g{5;X5UW#A0K8Y<2q=>7Cq z9}_Z!857DOeKX3hXy%srq9+48^E^BhvFR?i;8~B$kWcEfaD{L?a|?oH9R^Uth6V6| zIJSbz1bI!Mc);y4MO%wPS0Ocg-fIy0LM|6oTe`^1%#d0``>A^kdem=P>i3@x#n;p> zkYaL?*}Oy_310@O^{gi)PTa9> zpr`(2LAaQ4*WRVaB64oa(Zn3vgfA&0XWLX4J8GfqeNM-d4*IFbq%E0opZyL;!;wbVW*UrTWQ3gw^;yltb)#c+Y+j z2A#^nm3dB}Vq72u34W!CUOdE=C}b?S;Sk&TwU!-yJg_#2Ro)p&c*6g&9ZAe%Ku9JN zW+5M1jb$F4)qQ0-Js)-VI7|R;srYrI2R8B=c^?DgQd|cEdgJhhfRPd`j1_gh5gyj= z=gzhC^%rwRN|^b(;Nab;a|NcvUgu&)X)W9a8Ag(Rh<$Cm~P*_m@j?4^_*=V z;_w5Sq}Svc6SM$)lonO0=8dihv)8O>q3gH)i&?|ZMq!*4W`PgN$pN|x0p$l8za@M- zNU0M0&t&+JZnEV*M z{Ba_;KQ+jt3gm=4`H?n&)A(K()4$x3x4Q^`lRQcEba%N-rr70W#&Qln8fJV?RV~}V^es`hzBs}&Z`|w1J=n6M+wF*J7Y~s z!^4`6&}?EJSF?`|`PKFniKe}4E4fU2_e+hP4maz76(u>lt5L2B@b%n| z{%6VH<|C{G9t`O(IO$C-uf@IW#zJFU=Px{mnl}3dY080h=(Eo+U#C2~o0CWnaQ3Cz zISSGIp`xxb4_DU2?eZl^V>C=MXjEOtjIgcQgi%N68G|p|{Sx26&wIzfMz?LAR4)u` z*rB-^WnuifZ|nEu-mI<6-S?)UsdhP=weAHuJ^~iutE^Qzo{R7oxiDe-^}71ZJDw>Y z(DIuTq4XJYjYeH=e5{y`Y-dJp`_bLXt!W3JYxam;6SpT}=E8zf+N$^sUVq1z-|t60 z%TbLaISOBjf8rVV-J91y!Gf0*;~0usyY5Oj50K3t{bIXp-u)jfz?~CbFp_#O)>HjB z9i(%C!*G@9IV0H6q08@!rd6LH#^`N;{?l!cuY(NFF6{2TpJ!0y0m>q~{;8RI z&xFBS&#k?ElX6SXQ!)CdqqAzo6X7i9v>H0nRQ_bhwLauFb;`bu36OI4A8!r;R|QO& zT~nZ8HwI3#@;OF|i6G8Vz%$Q5L7yMeJK@cp5;F1hO@Qw!&x*`ty6u`n+ zYF{~d*3W?^S+uMs{ccsv^8U zfxp2qGLFS3C7+)E>i*i_EKuR4Re%kJT1vJIj+W$;ya?K1ZjT=x^&l9s2zIA-RI z3Wa!7?R+x(u3zZ$VCRm$8Yjp}JH&s-eUXeUX5>+dSdO}^1#1l@cS zXps#91%C>a+4&G35Upnx^j|18rEo3}%pLN8ZfH!2syr@*Z{L>Mm(802AxraA(GB~; z`Ax+@7CuMOZBc63lqGO{(Z;UvDLkuLt+lrCRnl3wojbVt_h6Q0&dhw$p6Qe#Nxml1 z9u;Op@42}mv*z*1plBk!)N645GPmU4UdsU*J~?-sZUm(( zsCGW&uWPx*TY~p9lo{No!55TISYJe!3Z{?G%&(a^rt``JeGOZP43R7#RGtl%AsaTm zk#Xr)$yj*`Z`TUKeN+KnS4u1oJ(E@wd+b z@;g|l+8?@?GfsaLd2-VomAVvZGC`%ZC#!USocPWUwpemqM;4^fp)b*lJ$Kor2AW8} zMX{OGJt8w3B85P2H2Guo;?fx1rA|*{ee92<^2z5dR|}w(reVa#^4caqI97kPSXmE5 zZW|S)acpwu$JXw|AWed&?t)7Ann23fIcf5{5{F|JaHuZ|V#lECnngKfCV&n39tBnm>U_0=;m!a;i#L=+@dI)0H zm3N6DJTUntLm>Z5wcaEG{peqS$r}96hXJNFB_*?9_T9O4Dc`iHDabP+>qS^FU907B)a3rXDX z*ta48u>@eUj7gEH6hq<8fj*hYmi!j4ibQ7gnaw;vne8c22MOwYrbEJ*|GMec!X}xu zAK(ltYQtBNO|?2_qYv7V7Da?yD6E>SetErvpQ1vwDBVn3a><# zOhlUcWdqV*Hpxmz6wx@fTrL(%a=>24{*;{SOGN- z#;v#uT~$e3%E%5}ohRRY041mcT}9q{w#X3=-lZxwpZ2$IuHI6Pznkp)91I_5Te#Ti;C6M&9+ZxdS6Tf4GF$WX>%Ynv5QR3S%hqM7?OiOBBd7TF%)1VYXT!%bB~ z3#*l<3qghfSEq-%5co>~fOjZ-xz3Mb&4hFmM;(r)83C$(udKvFOh@G3Rgya6H~xI{ zI-zf*WYcVoKa>smMt_-Le?dJ?jAIlqqF)5bk1~=mIW+aPEYMXMx>yq{zrg!D>;dn= zI_WuXqzX&`t8ZUk;j~r?+Pnp(XL3$l%|Yp#+p*yyVy05)J|XVi-~gTsm(5Nj5iZ}l zRj5>;&vwGN7_3WJCR4=Th-4@7>9c(GcLumUxruQ^>P0P5jp8*ex&=p9SHqK^-J13e z?W-X_2HF?-HLDo_m+g;mO$JsNg}EQ~3RR|swQCTt7=b?FjL}jSOair)lgI+yqPwj!pQ&&OMr z=S6fB|ZpIepNkTerl3Ap^b?D}?D-XW)DyG`9 z&}`GHT{ymbqL(@5a5%cK^kUnMQ`p4JBYyiDZ-PX-#)S?777e0`wyB{{O*1yzMUXdU zJoc!AZjq6F$|t5rRaZ!W#ZndV_@0Pro-%MshW5MqWPoq$+*+l2Diw*J9v)7m;eow( z4a=0H{5wBj&CJa9Y@od@^=!B}hs|GI@XX%MNl%h!Vicx;#bt2bR0&1zGm52kThT2% z{w-9|Z5e{@aNo|6PLQwxPdK!R%kynRTnGceq8`8^sd|FSL^|6Z7xD?gtcpOA{8m>h z>+2X+uOUY|I2KYD&N6QC#${(o`gbL0UZu;;x;%B<7u#UHf%5B^#PX7ye4^ll9_01wfG3DnNwfwTc1Fgp4&Pu4amM*j0(>YA_{; zpSrf2j$W-xTV{6myF19dsII^u>LEI6PKrW#ApfHD%;J){gJo*Q`E5;|fSz$f`~Km5 z7f|ycIS=ebR1ai{4NuIYDXCEpNBKL}>%6`u@7xr!sZ{iJOb=R6Wj{dKynz8Uax_Y+>r&2<|q0#(ksny!OxWKxzoy@q49(z;gbjT-@_K{jsC4!(kF z>!))1X20K=f%a(@f-mhs`6K+a+!lhTUDu;~qEq76P4Ss`WWC2P??__Wv$+6S!auYU zKs3-KV%=29MU}baKZgo`XSMX-z%qu9-}hsK?#Sy}Z}Ie!I6*r!r-d9jHCDd+Vj`q0 zKl!SFB8&@=(EXasG*x7?8x(dDv&N{RC>jksHCNnoie|6< z-DAvad_$EaH+f$}AdzA|#QUm@s&FoINE46hz)K%FBKcvE2WaVL77D`ZzG{RVtp3`N zOk*5FI+VRl?(NFe^IU2g>=;(U7J6wM}X+7U`jQRHRaAa|bx=bro z#5)s1{5mE7G&wcxt;8--dd$9g1=;r35&E{22l+sweQ3VVwfVf-_~^j+4qOjiKftbQ zxZ$`TfMwiE0Iqwp{uF?Q{V1*1ifIYeWtH|$=ck|oNYh~ZBA_Ddx3A<#pwEyb8=tmj zYexLGcu)2h>M=f=$+jY*@>q86HC0|I&rnJZUqlp|y)!ySc!bs$iI8{sQ9}VXZvQ%W zuujUW5ANQwFq62LFLU!(JDTtPPKle+FVR@EPegO8Tl)cMG7Xf-yxTy7)vj}5ULGi% z05b2o%*mmRBB}fKQ4IqX!CkwjS1N(+HI|PwBA8)=684_Z&xK3?N3!kydK}4V z>s57Mb#&ew{q~Ot=7-%W?Kv5w+c)YVXn24M&D6{)9+1ufI%-4PLHX!ZD{F_S`dMH{ zqo$n_Cm-l@K87O?U#m2v_UFMS7LW-R%C!ML((Tgoo42s=o>X*(BmVEKNVL%)g{Lu@ zFfqs*Z<`%4xelOfU>ne|d91igd+YU>I50sz9P73+)$w=u^K z?XXw(g=O@FB$hBnjh5unKgMqysHXf?Lr6g`pazS&Mb}wxV{vE>S`@NRGCj5bhMRFZ zu74OgT#cU&12)Sk>Ee56`>O}k7=3(Y6Q+SpsHUKqkH)9(YZp*Ey57~m66_p<2de;f zhzvk?$bXl5x($+PH4&xm_6NFvk|m~3=wzTrklg448yOr6FIbC&ZFLrD2zfnKb{7Xv z8W3}Ie5dOpsJQ4=UbVX_Kzf`1N$v0h3PYc-r`6<)X)>hS7N6=A6J$1NQ z2jhWf2OR`UUK}08*d6-O{ZkK@Dwa(PWE4fai6Wx%gxiqF@WAYt?eL_uo;ZmzR=2w^2zTUoZW;tp z6i~EmF_~-WXOY(R6ksOFz_p=PjO8QU_~d-CyxY#-s$~HABau!KKZBpOGLFfkkWrK9 z4*lNhm3(pvTCS6#Q}R~}Ll03zcBe&YOP~>Ud}#@6^wtbc?6mtCfX^#xG4mx=`v0y! z8$5TJ2e*aTn=e_c2;c@ip6A>ieV8vbx-Xu}64eKYV%O&c{lbztpXPEl!km<3T^byD z)+Ko_`@8i=7W4|QD5W6NlF2XmF#v|==mwkC*JT`ANO#-Wg5LWD?SNR5$U>|%IKG>( zf5$PhViuGsDv`rp`7c-wmo$iw+6Hu0A$rm8RE9oT_gzgq{z`GZbe*Bc4SHHdVr(VzT|DQCg?lc*O3VbQ8aC`r5!@8BFM(tB`S> zj{)Rb)6=JTNL{s1lk;_9_^}hZHMu@1b1hJgBE>-Q-`W8r!TYLOpX7`fK7gPz4P@pY z2Dlk3JBL*GzWaApQ3-%ZZTT&%D~Pf@VmKb+)z7(chb3>4%s4k$oD?8QH57K&vNzr4Ey8`$== ze5OLvppj81FVFw6@xCrOCFR@Uc@&tCE4D$Ppl9WU-@--hI#fX(|3w9r#NLza9|l9I z;@?Zu9_SP|gs-1&Bg=n9*VcOy{>$&AZ;314Y%gYWJSzP6s~2P46w~4zOo@l;sH2fN$TLcGIJ{q*xt0r1 zUhCg5M8%3vEkBB1i|WS`$WAE)xjhV7?FsnyE|7A*vQ;He=%(>5o%PX?0&StapgEb$dW{=Me%X~&G{#`D25Ajyy z%`|Bb54qpzNJ#(nGD3$38Q4AT4kaQbDE|?khmee_^Fe{13sHK}Wb3Nc=zg0>>Ab~a zh*KMQif!*SZBN0fq+h0_KOl}R8|ZB;TG<_S=eFou#)YK*+TSG-IC`L}4$@v*DGoqE zet_seWA6vZO{p}u2E@7b{LO)@&jNpK6%)Y_*cx%Rt3`r2W)xMMuG4VZ>ACm5Vk{-) zs}^w~p#169-pl3_7sZw0dSi9e8Pq;a!|KUiSX1eE z&NrSAmv3S@7wZn2Y<3YdPwI1}w}{6r`ih^p&Sj~KpPX{nV`J-Xd3fFWKe}8MI_caO zzpFw@B!(#9Pejf4WD_68^x)vkq4SIqv!~t%Rhh&#rmlP3ydXW`N2%y@k|Dvme-y){ zS&OHH%%B&}?!@d9R(+hsNt&5!(c_d2Y>oNr)$E7dGXYl@Y^yRm^2Z8=uP9^tzdMw} zZjTdPTJfjyM0~9y#0ZF`=t;k2C1J+qwjZMewLYEbE)cwhR&KdB*)Ko_H#6)E5KH%M zgDhFx{6aylLcop_w>cCdyOfs=1I4F@RrP)mF7DCglj-w!7d#byHPhs{rvXmFR>loCzLCoIo+<;<^wiXU-WHSO?PSuOTc2yA(O#&_Xz9Z`=SNI#Y z5Xx1;hXJ?QeFLk+Lq<2BHlkYd`1xmy6-FriW8gDqiw*H1@h^vj!2L`CKj5!9jhp4z zhn@A_41A{qn;pqW9G;ks&ao=q7@IT7Zrp`m8XBRa@5oT(fG7wQr6*Os<9rEcG>7Dy z^%Sq`yxa8b^A99tWyR3N)8NPK4tlcL;R=piG~;UQwfD*_$r#&kqxtJ~GDeEY9ETF! z6Bk8!c8wx-@eT*n`b2zTl)rmJ(_~RIbLZ_Q3tn9`nezuuZ;m1QC(7s9P8eE$Mtq2sRXlWVXsslT? zAzo=z(JA1U^5t9+m}Y*Um6h3Pq{5;5KvBi5nzS;b`=q=%Q^#BOa)`s0z=7f<2@nZb zAqb45CBLj|%}Vu>iF(I}i$PTn)eXM#^Zz_sB?k8ws}A5K+e)Ce=qi5Ir!@Cj;mucw zsVaW*tClM|;%Las;dKMl#1_HpoyjHCD(&D$LfS2ey@*|~W{1^TDlG}=m6{(f6VNW` zkcK7cIb0>^84icS%+((k!^<7a+#GPOh#*$-~G65Fk- z_+D47GPe5+>EYxJ60pZm&!DAuZ%nUXGZ{6P=#X_|hpyi-;B~$jfZfS$%@bI`LV$$e zNtYljLP5KLK*f0lzBIPDWcgPKP>Gwovo2IWfR|hX1kD01CN6%y{_g!<`^u$)+B&JU zi<_t=exmd=7THCxXy;xZ&qPn*H%Wt<)3^*I7c6@m&GZzjfNN1lxBF*@ww(Ho`LW%J9&1kMs=rDS9pJ0e`b{cn zZbe!>#c`@!V$~7`#pi@|Pe@-SUA#No{~b&ZZXB>Ker&nV5xoV@&P@lK+amC5BbZiQ zi0vL5sI;bv-*mL;16yX^?|y(Uuz3i)c`)2H@Ry)x2h$$s)5Wt3+W&%v|PK+BUD zfQHif+_mFELC)`i-%XL7&{v_!~>?o&fc$P?c(zdPedP^ zhvz&q)dIwax}-r|oj>&<{e7nt?5e$|Nal}nG{{L4LotZ)UAOVRmxWvkL1lGG@q%AO z6BARJZ4FneM%@-Vd}jq7(;LkdY$}bDgqp+5AAUP$@<7Coyfci1U1iUiopu<{0Q!(B zoOzn6JIzdK*yQ%4Cgu{$+}vJ`2GuMY^XSY`b=E7{R4E*jCa}oQ?RMi!PPWmy91-ts zyUBLlo}9{iEQBS=o%=y{3LDfoY<_Er|L>Xhe!^G;s`WNzC9;1UR2$mKb=n#7@CfUK z%ukF+WFpJbqz#$E&L1SHqZwV@huzv-kBvlaB0BWEj?%G%kY}G7HS+dt{}I>iB_jdl z(9@ykQC$Qp!&KimHoGXVz4?QLu79qPP4n z90iGs4i(Gamy9uy#P^s)Z=>u7-Di7|!CUJNt3Nf1Tnt~n9WnZQrLS9_@5%KsX5R~O z!P~kngRGOCS&5EmqiWOQ;{wcIa)`x(LBxZ`H>XDnCZi@x*h_G#JW5s^gLxyQ?SMS{ z?Klg53S*?R=+LvUIQMl}0|=D-oPjuUpDXD5eecEdN=nV0+{6UbGTVRH0H~&WqAR%%GJY#t!7CDTiZy9Xb#PBZysFa~pSQ7+Uj5PqW*z9v{vrCd zvHS0w(iwyZ3^-#l{oAK$I7-FY zFV!SCDrs(;1^&TGzi+s%T>v;xgP?(=!N2!6i@r`BFxElPa9}+-|L6bz{xKYH1zeag zt(N|OE?Ow=(S3K!k5C0Q!xCCWw`?B`E+&Vm=l(O1f89Vx<3GO!@Ee_NUPraSPyCA1 zIsRPxgbVOaeotDj zYuZ( z{r_-VQOSaPYQXGqivJowlp1{-FmeF;zau|EV$1_3Y1ess>vtd-FyIf|%!vJ8tF1FJ26~QX zy)#G7I$r+L2bh-Z>%X&+`5||^>C%K%*N*eYRrTevOWQ{sec)c)d4E2_8CSb}Y@rwH zO3bdx)%`5^c^M{G_wsR?7vTDV|C-juSVoY4W4V6^qm|hv z(uV%yxvYq_=EufLH04Jjq+ld9=xBLP!uIMx&gsM|9O zkqI;`Gn?uvfTMvnY=eMotd|Bf$R2R=^T`1uRS}e>j1V_oCmslEVI+HG1j$Zfw${#0 z#W1|3NW`gFdC%XgP3Yk2c-TKqPS$EHEdIr|$Mj5nKTzzfje`)r6~>6*g+Y;Zo5+V_ zMq%ZXq;Nffk=dnis<2A zo&4>BxVZ7w8_u!DcR^`q$+yZkm13sWYgp{VH-yaTv~kI9bN1znWMXgZ9K!@a=Tie< zgKk|-q;uQ94EP&x@=Q`TGX521P6C-{k4@OFZuz!bNqDzU0|b|_5m6l7L{gdgZdhr+ zb-D&Eql7d_w@A5kcP}XoQcEn7 zBF)k)pmaBq3nC>g9m0a7#L`PF_1@_7d*APm`0hhJJXXB-ikWlHnQLaQ!mzMS{InUb zmJhF*+#sjtIW#7J@n}V_xgKOf*+J$uom(GcKf0b0$9Q9fL|eJB!H;c6&CYj}xVl_4 zeYk3_R&uN-?btbiSEd55{0DMza^!cH__@5Y;@*qcJI(#ZtPVH0=7O@QSXsGoH)8s0 z64;c0+Hj!@!#eI;`4Tm)hCy*ecTb!qxB4uB7u_h~*$dIa#{JLO^4F6EU&a1T^0`VT zXZM&_%wyU}To52E%N#etLUXmT+vtA=_7K=voI~(zh;K!NgJS)5Zv) z78M_8vO*h%)k+24r5m)r7|pk}i*P-Y>@gF@CL)N-k^qEyIKkt|UpQXr0xMQIm!3QW zaXuT$Z952qE@M_>8hs8~hE`?VkYs^e^pWz5bV^Mt-jh&Dg(uj$6sDLQ2gA&G>t}jR zJ(HVJX5UQg6ixpOmvT;h8W9oBCnShV2CP|JnAcBS4376WDh%KA!c_@n|JMrtINbjn zqAxc!_v-6H?(XBXqbz*uQ9zR~oLiz!g4M$JH)OEVe%Un~izB6;+)XHP&sGoNVgahP z0Pp4?_g9wQf~60Nv73zT=gh+DQpMG$RLL6ZAkVNKm99D8hHLJ`=o$dox&Yi+;hyIV zl4zB%@N%{sjrFY#8&7L)FNA+4gP0`K31dnt+Xs!g;4;{MSKHX zRv`1#*gDJDhh|Tg;z=S)q{=Xl5cHF%f*xZ3vCprq6}`ZIji$0-R2@Z74l7I415qw< zF_j|2t{BC}kvG-PpC44;5Y;C+A_YCq?o6gUFQu7R3-+l#HO~ZJ?_Z2$^h!=k%$ko3 z<^Q!mqO5W;5_mBRaEesW!q(&TBk1XR+d4*vrl!WF;Qq!r&&Gb{>?!d&^6Dc1Gh|t5 z;Po8`{@sgjJ8LXnT$6;s1TZ&+i=~`vq5#^5jp%t&;D{+U-yyOt7nEF9m+tlLw=HV- zV6EaREfv<^RRz1XZn&Rv2`LMcW21Qb@x+9?4MoSW6%48apx3~ zRVi1y&Q3)CDa$%=LE0^i;3K5+*B^mgm8#%e zgd=x}e=v8Ft3zbS7-=DI_lAeTH0{c9BG9(MunVZ$%Z#f#4mA^d?NDkn8KX3;#-eks z!%Jgi`t=&uW;+T%5t|$KldvnWzJV{n=5ZpL$#!xynn2(G)+pKyZ3_HYxP%?+ z(QHaWHjV&Vck5TT4`7yNY)~lI4IOBgJ=98!`r^m_xS451C17FyEv#uJbo~Rbrv(So zpldu^PO|0u_yldvFR*-iZ`ma4(*ZL#Ik?Dee4r;kS1z$kXI_dcbH3)M@a)QK0)o4@ zx)RgFR0`)of5FCk@{@k}(_`T4;{|e^+(>zAr5)ee7JI{|ZPl+Y8 z=T&gXScRFxx5>@!xLE&{@I9}{D$OnMcz1WzWMv!T7P98xQir#G^2cF-qkE@ zjLH)V>&XuTx7UO!xP9l%fv+UMnQ(7s1G5R~R3jMGVSU<7?hgDSUj~ocJ!M(_NLzW@ zE^ZHn4+T9wKUFzIJ^5=Mbb?Q0SP?d(2JwysHlrq2krEd`W+m!?X&YGpGAUAVUR&Z6 zy55m9#7sB|xg6hiu?*a4mxKln`DG4}WDOkR?z(0RT0wr(?_R7q0Uj$jvVHa0d2{-T z?WEJ>2>`(lF4Y9e5E&h*=u0cVYtzkV2rz)PDoFrX5#+FAjFZMd5MB`~{uBIZahs}N z??&wFW_D|BPYC1LUi7-D@BTQX%vuWu$>t>!u4mF#%IMPQe&?damFmlTh1O4$&1paF z6eL=H%-CHrUh-7lC6()b`%wY=JDPt{kIY+O%$T^J@+b;8VHZMIaZz$@W*_Ipo~R)KDB`&_zg*b1h10>2U7Gj-``Jhn}OtvwUp7_qO{TbQ8(s=ihq z0+U+HXVObaU+bI`1xE>+sj|r#uxa;Kg~WYgC;2Ctge@m2{;*b{O|0Rrz{d62^^U+s z#69E3-`>Lf&j#^7FuJE{efFL+2jH(8=f`*?YvTM~;N{TG>rd82#MTC`D zKtE;><0w2@%K04v#zkEkarm814P=&r&sDZ4FPC3Z350+A5g{dsxE~~n(53yFLBCxq zTusiFWmx%^$~F??*DL))kBz-BC5-X+9UzUE6Dn*!TCvdd zsXwguWQ6gunPwl;ECXyDw?dMjAznL}9!ULZZU-zpV}ZI;Wu-550Knw1fGmW-`7*va zo0_Yg!QZ+WK%}FatUKSDIk6-lI_Jhz<$N|aeh?(vB*Cwculw!C6WzCNDkaI4O*Q?A zA|-Qk(~CPpcsK|Sq<%3!Z8Zr@Dsv@wKEbg<4yi;7;?fIs5n}A8197KzfbV~gE!2F@ zk^E7CTaEd7^oz>abGQc(l`a_caEqB=6E9HVIKUh*TNN;cFW*m=4aXDE;DP|Bf%i*3h-3NqsG z-yiHE62795by!va6RQ*(khzg}_FwmLX zH|;kplD5CF3C?wp@J2!Lt?8Rcx7g6rfw=X|Inu`j>#ux=0jcGP;EL0j=X`Si~8w^#Z7?mle3S3(2&c(+^8Lp)%&skX&`N9Bh#63IN)U(-AviJW!iAt zK9DbTe6`kx+0(=0>|%|*ysV<8u3jZAe|(9M4m;Uv)FUPcapxdc!RA$W->pYPPuG|> z2cmRR3lj@F3BK^9%*8Q2n|J)aV;qJ?-Pzfx)m^ZyYDNoVUO8Jsi&OvBn>`fV$olinz$5S{2GirX%E!9le5sj9FwzK z@z*1M)cvNSW_T0r1l$m8r++TeAWDP$gUC^*Ss&=~hh6DFx$Q@-i+Y-~nzA;Z;kI+9 zm|ssx?R;9UGX==9093)x92<Z2uYb)JwLD$;BGg-9%L6xo(R}quMpEvai zg9YQoNvZq75o3Hw>pen3Jl%7e8R3u+S4x6=B%&6!vIXgtzwE1;Ml385y=8SqJN9+c zHjM9U5=!UW1?3Fc3GO9D!j0>DX4go_q{{R=W&<{gxLH<55B(2U9&mkK;BFf3eeqW# ztXC^vt0zdoJuUR!WF9+vho0a?GH_@W;-w#TDPlsvN|^Ik7Yz@1^Y3qoWnNuZdHgJG z!%6{+j82<{r&}T$>Nnzr3Q={gTxOo&x+*Q;R&qYShd%i9(&IIb!;yIv{ad%59m%i~ zPwmDgk7U&4ofbFVB|?4y`m%(T&r-8 z%h{t#l3F+FWRZB@oL>qv^G+_FHN6w*6u!qOhgL)tFmpzn^IPcs2P|losCUn-KizW> z@boy_!91>Jd+{lWS%&*-8!8QYXfIkMd%oxj+!%`8bqgoJZILR*o|d`u#>I&%=+fAB zl646w02hr{Z%d%E>nk0eKOulI&iCEPOA^8hx#n~!vJFXTR<`EZ!>`*JUZ zg)>g=v~k!>v6=VRp>t9auU)i^ z|32n{@~{feWFbM18nmGH`gg>7Ks)F|_S4ZV2Hq0`wxKAVk(b=YoCIl4wfTi}=kbhi zT{M{@**{d#gT8mKk+_h(5))Ep;7}_`1bEki|48H^X-HKNA2y|4e4I6vC^bbwC4VPb z#BZkZCWagT?0_heT0sy;t*Gy4$yT}bilmvs^W{`GR*Jsv(s!S>wQU2Mqx7R13tKfs za;5pO7gSk`9*^;U4}8{xuHEj|6)w98It)%wcqQ`)dB%@9wG0Ez5#$reyYG4lV7%n& zwE)X-6WMB6!4?7MOoV4AjwliC*gLp@{etQcezUgn?4RztzQupl3(aN4!aJ zxPl{NF`0DO`M48z8U1R)HJpnrjk0UoOTf2SGD!G-;PcX+Co%-fQNk5j;VBtui$~#l zR!m#I7ZVQR*N#DJ%3-L$C>jn0aK<{Pq{p~HqA&z6~Q66>jrr~{U3DT1j3W?rv^_m+nI3q3e88ory!AO~!(Fd1ti}6XWDHV&>GBbiZd#ef14`7TSiJJ< zI_($H%U8NY*i%urW7h##4KwEaGPV($-__Ddi(914!j_dn+dpJl_WSe1YFMJ!s3S$` z7lNq=fdQQb?}d%{`Rfa{`V|$&nUfV>l=<;V`A6fCX5EYIX^d31%R<#X<=1hlu5yhM zRe6?WYzp~J3>^QcWu<&ic>*qzeRty-|KM3gv}1E8LZr#M5{e(kE90~l6&ryRyQ{* z#OIF}Yb7NmrKDyoqsQFFS+ox3)R^a&N@%{$EP$AG%=^#}^Im>moUz?epLxkGAR<$z z{zdcCgF8MASej6rqgFTC)glUqb=+2_Zq?eAj29c|=?1?H&g)Chxzja#`vrtd664J| zwBHpK@-m_gym7%VG~xH(stae3u(GoHo~`O!>~-MJHM)gaxy7*c7fu$#bw%4} z+}6DgM;YEFV(%L2(koDH8Q{F<7d{d;vL_=*eWE4{6yCr`u zxk1>Y4s7YXhm+L);=|PSpzHUK``Nmm1QVz+-=-aIZyp&P_~U_eqAb2a;TQmgndd7$ zY@dT|vt_xEpx9cj|z}185kIP#m_HWr{ATkP%yT3z6)K1sfe$6ww>dY8#cbFpJEuMGJ68_ z7?C7%K3E~7@!h-C=Cw|R^jc=mF8p4X$c_go*><1JMRFOCGvP{&vH>=3+Lr&B(|FMY zOM(K!+#fsb^s6Et-5S4I5KVBt%_!3YAf@u`0(yyG?tko~T><66-9VfGNz)mV)Ig*c zR3CBz)7|RLLO&w9F>N}tahC@891`~V=U1FD6r&Qg3h##nDWTVT$M{@L5<7-f_jmT! zTdqCCtAs>ve8nhWWo#dN@XL7$yRC09GB=m;pQHGZBDd`xw6y05ftsaW{19s98rn#G zXloadZs2)(QKQm$fO_fy_+7t#Edw__$mf0EafIbgbu?1$QhI0BG&Xh>Wcl~)La%py z_g6``clh-T4Nc9>1J#S93+Bx%EkkzO+T=5>b?qAVW`&dol$_1HlcMJNP3Gqc#vGRq zu|HJlf_tmsmzN|y=QsoTV_jS1wZ_Dw63|N$#H|)LfT*7Ua-t(~E(X-eMNT5|^hP^= zI$T%5Npu%0+X)M7z7}KW|PD;z(10nHNzy ziPB_IWs1l#fo7aMZ9IwCLZ|hAPS}qi5V)9U z(xhZWiB5%ikj>MCqesKrA{@TW$!TG7D3(krGU&R6wE zYH`?&9+PKB{07#ze?FuXTu;IwZ|!c)>$&|}ZQ6L$97UqKLNmo%c#q;I+qM1n`N0w?H4HT*Np;@ow{ zH*b~r{P&~O8N3W=AvM$eMsQnlY#uXO{>Pj}$k~Q)0Ta{P1!#;(i+4B?lXCoeG{scK zVJ}mbDobK$jFBPo!MtN~-Gr9*?%bDoM~w=BhW8tUPHmU`j~+c5f5#U{Yib2a!xujd z2nzCRy*xJ1FXNx8STNmRVMao%KWH!&q*8JcA?I_Zwg@~3-+?V}Q^c-E8^@2Lj^D!W z8n8${Ia-&c=14pG47g@pbiRK&a%%t+O)lYHaA=qO=+KyL!rXBwX6zh1&9pkue&~$=uzlMVd2qJ8|?6Y=u+sp$%+tIHSCIGxnqQ z{GhP}2x4eug&qcjNvrYBiQBQ~H2{*$82!R^eR_W0($hLjRDn6fax*dIolx$wsP_(6 z03Wsjw=shf&nRz4mVF?DKw1*5CCK}4^y1HX|dVOD_V-Xn~Dq1;`5ORnKSVfT*A=y|y6$*rd5+8j?HXzzG;IpQkT@`OLl@rrZ_POM4qWStTaNXL4GOwz+;*2IU2@*%YM zLr?g-pM=t>5fh}4G@t!17a+8&Eigkl%%eG~pXd8GRrat11-cC7DlCpm2QDmRoHesv z658OB_&4{vF$vZ#MTj#8)CN#31)wRnS}GuF@W!KC>kF5UtvOnUw|i?YW04CyO>h4O zlYl>r*#X}_#98#4DDq{~g2cJx)O=9KZ}^M5Gd+=7q=>w@=@u>5D# zp;4#px2rq!_Ik41EGVP7^U1u;QE@Z-0Q0yXh&7gFBrbUCiLMkgUidOrQ>>2)_qQI? zvi3z6pzX!eD9p&ZD6%E~dyc<9_4?MOm=Xq_I|W886ed(UpPBi0jY zz`M~?u+G(W_GL*lX5>T>810UV(#=78oK6q`3TL=@Ru_vgGFx-rVV+@bTV)&d-kj!JiBry%~J!t0@CnBQyOa8E&fcMc(DH&}i19MpUU`0o9fd3y5Dqf1!?4PO2;q!qU=+XvwI zqQ{hb7>IBmPzrcaf}?{^@WEva(ctWt-)FD34|o}r#$H5u-o&e5`OFC87}5TZ*&kgS zrJvlT$BXA>QgYiP_)^s=`Pzof0pB(gb|iP&05~CG6-r-rFE6;2YRzCs^1yG48CwDy zPJ3rYLwgQ>C_;8WWvOUrZj8kKFMlCUCmmQ?Z}hLD zOS+wap4*eUw|_Z}{W3oW>I&i9N9u9{LbzKI*q0qW@wocOW@PFWQ?|V1!De2=OVxMu zGRNN4+t%sRBktOK=PTSHQQxXJ(){L??bFS_3)V)Ju54#mx=PV`s~^|I(kM%tLP~c8h@oU`t9xQ z%92qVbg-jiQiJAmhxOW7W|T5^RTp+Bugb=e`qy9V#ockjo~QZj_q1-!C|y7_=kO|EOBoZctH*CPF-(BA1I(bv%Nk&*SA zQ-?EdY6Mh(a^X}#+5O2lCkMwXA)(&(c5wm6xy6Ikel~V?f*X8sJ=qa9@jGMC zDfg9Yp3gUSa$Xy-B~gLXyH$?akJzLCno{_s`|?ME0b$Zn6p; z+i{kzAPTNY(~s;hu$_`o&xGTKgL-d2u($kJv2>x3%G-V)|q(Iu!WqM}#6kSBazRH^OLMltF@+ zYO(D;CtGg2Lnx03SwdpSl1=C{AIVu!#>1m+aLE^O`1v&Wy|)bIb3^ z-Mf-pa4j0${<#ZL=zcO3RHqGuN&zx|&_!yp2mA(1C{hH1pjJ>&xL6ZtTg!>LN(kv9 z+V*_adSw<>Q8B;zd7Vq`0S)=Vl_F$aOii_RV_9$;2Q8C>bQVm9?r{*KAXJrN(Jz!B zk0wbDS7659F@2B4ucxkiCx+#KWBIfrBvB{>KnH-sWhLb49;eV@w@>G)@VrNJ3P^%d z&f@(A8(;t=+Jk~1U^YoI!n<_*_p2t~J*ND->p&)gx9urd5)EYPpLA7Mo0*2+RiV6F5_>fY67-0a12~VH+TsQ@zOV1$Ac7I;1291im`2Nf_dWl^^o70It#y!Ve^erS z>)CG(0qt}Pae9s(!8?upy%~4zxDcy_gXPU$+n|1X(JbThEXIL_&Ri6s1jf5!89`z| zJ2zKUNzB>Rp-=kJH?AQ3z$ai_Q2pRgHtkl`S}LYvMp1c*dlLq|Hp6!um;SL{Mk5ij5$Q25%+Y&CdvBLen=N}>QdIg z@^e9bAF8Bvm-HVjc$A`e&(_xXvrG_DIr>W%srNR~?fZTXIXSg*ONv~n90ld;L z#MqIEU~-pE|6j2y&obD$?$ITxf9wYzpRmnk)GGJeWl3g~S%TcqeBfbHK9Ww^Ls+Wfqca0Z@UCQ%RfGSM=SEL@-La^7upwRONQ zeum+<6EPwrRgZce`112QMyG~u2 zOc(xP+`AO5`8Dv+B7q53RF4HlKO0g6s|>vd)f5$(sb?7%fUy#N}Vw=dyf<&!zV zj)g5iPN%wQk6D37)AiqAFP$~z4s?CXGWUK^Q{TA5qqA%QK z6ePGp)rm(7lZtfYgFjHUzHwW5O5KW2T@ic5v@f_0lz*_H;;c8Eqk`IdlV6n3d8uSt zcl^3xAeqI#^@XT+4fnaHb)L=LGcsbn*{STCoqBMmJ4U(X76$Ajdlh+dck<`UWE)NS zU^T3-WY+OQa;zZc86dc{@b#o}M)|a7Sf|0SzPGHox#b-5+$qCgrE^`KmuUB+3PxD z9_=#KiiE1k#TFXsg2yC@=&QYHr5!#&7nio~=APJfQ+q`}`%hIxLd9UB&eeyY`k?;9 z7fj-zZhr=UEJyn;{GHq<@Hw~KLy9i-nqd#0n=M)i2hyZC!_N6PV_3U8b?;25OXnD7 zllOE=zD*)swv0DlWjBAsj~8D2bd-P2mii}ZC*CT0O)@6qlB1s~KP97(N4rFoS*BAh zpe|o`Wdc8KGTKcPh|Q z@E88kJ}L}zu=T?4(zJNrP@R8#l2&K3UsHIa5N@Zfbs*_##e#?#u1~ic-?ja@TllMY zWq_Tj>8W*r7w0jGeUg07cn#hZlv&<#94db5bgkiCU$Jlnh30IWN^1$l;Jnqpp3ZPI z@tJbm#H3410x=H59F$*Tmtkvhts#@?{#%J{0cd#>S2lb=*;FBAZ9TuApLFQoBK_tSQfXSXjpI`l;q1cWGr$kgUC~XzUm6kUI<@ z4wLXmYAmcp1{u|uM-E~QF&ZcNov9e_;(31^oXc|R=mPQ*-BeEw)(Y@oqDX;b5(~}h z=8?MAq57fTmyYW3m6Aqrgr^E(rPbBiH5~RxQaj0UFA7Kg`*!0O!9#aXQWRYp~Wp@Q^+WRy1KT4 zOC98djgQ=_A$O^X_n_AaE*4g6#^ixC8cLMfzrq&BXKg5NjSKWZA_umdlOzygNI6Im zUn$CWx*xM|{>nXG`qyPSu;ueZ>Quvu?dD?976rXQ{lA1 z_ku+hmUL&BBrOI3PD{(YCoKbTw*G{*M&S%YTP9e)MlVRFke5|quvp!aXf&NY(_s^i z@yM-7fD-_A>#WbEX}=e}q=jl-pUeG3a~A^nLWvZp=3gZ()$ zHDmF+Xs;sPG0831h?1q!7SAWDOy&0P&j%dIMS_ymlWj_-Ms_|Is0oby<{c?5CVrz@ zY1v?M_8FvRldnD`l(&}Pp3#&MJzyP=U#%NZu^42i$DB&)s--nGstY(=fiU@dN+pfO zqa{Z!1O_HFxX$j81|hWTiU{5btBM?ynhHfwbUeL()|&b2HgQAFI;PT;;%n`+8iGJg z5@V}WSrHM4AGcmx!NS#Maubh`npWaNqyl4xDCDyQ^vE+U8efY<1K}qMDD;IQpwL>D zu_uGU2A6%)M8#=XSl{yM#e-ZoxnyC3726tcKB3`g9N^Q4$j{WIDm-zme*$flMh)Qk zC!Zqfjt|+ee_xj474Rf37d_myB+11bnbBy5l}eHXz9O|KvIvK15JtQkuxK~8F-edo zBn`sk%QM{7YA<-L1cGKYf3?Mn2liVjqi#w#`G&C%CE6(h*&vZ<>Tc>MZ znB2?LlbiBYEvP5(0sHyg&qE)Gmfg(4P~7S`lG~@GEpvbUCTYK2_H~g z^(~)TID0*h^6LuC{YGI6SR3gD2a^wUfItQ$6G)12z#$*ST4nUo3#XnN#yABKrZ(vn z&r7O=!HQai#Ygrp4>sJcZ*y89TD@uP3OmMZP;&Sk@vX7y@9Zq>q4+ve{it1usP~ zQo;s6Yfl52Rm^oRJ=zK!nUd3=9t<9x+qQd(=o%%2@jvpq=e@g%NIvtrRx$Oce_bnTGsQBdgY_4bh)Z^^x z#o$9P73QzrmlnU=d?A{@Kk`NVY-*t}C5iz6ImL>$!w<5*&L4)5WS~*&0Gd}u2i(B6 zD`3p;)f&FG%R9ER%=Uj~&opE^-di`nYtuY6F;e=o*S%L za(GPEGi|9*1LHjR6rWl-Ri%5|WuF}+oI5P+$NbitT4d)C85*BUvZ<)AzMfFm^ggoct-TKMnTTx2P_y{ z;nnaGHp;s2c+Tg(`fAW!UW^JUqWP(*ZfQ@S{>08bas?aj7eOVpM8>*0js<<@@b zSyyrsmUZL&_uP6;@u3tx7*vISk;qt8adj-dBR66o55a>iBKKvAy$IgIbmIL-4vaq9WAeThBTVU2Bb0Oa|

QhKd$?>*~3G?sTipH>LZpDhA|ZtX2mCCZAUaBZi|UU}&Vm5JTUAqJhuUiXl8f zLb!z~^=NNsojmK6UY6%Mx<~oaMG6#O;{F^UCpQlV9bk&ymINJrEo`l7detpdu&C6| z5D~sM$1scXbQfhkTFoJ%8j^q7`ibt{m_u7;ekS23D69y@sCh#7Bef{GV0QbYwq9ub zxUhm(X{(QdG^?J$)m;@jnRh9AAJqnpfQ~_`jg#Jit>u7a(6|SaHSodg>9mbsz^eMJ zs*$<-`ya%wrWS0(saR8Z(+7`cn`vxn>ne&9R+3LtNZJ36^4sbgfF|TU5CUr{A|ma? z+>yaYR{qbX({;3_`sjyjbv-9`A47ywlZD~ReC#E{u~2z0@=X848pky$)g)nuqvlyE zzWJlZ_)g}OUiQr9x;N+O*w!n7QxBCp3jq^24uQ#RF7k)v{3qobQS`~Z?_Ir2)VE7x zU}#A|+B!C*t@?X0r9@2oBJW@h%zkQ zw0+|}^>F*wLQrm3c8%9islUTtlT{@9O9X;KoG^fb2E{ISbf7siOjR}Fr>8Xn00c6% zT}Q3eR4L}0^G}sHT>~7A^&m6ohE@X4U*t>haQK)?Jf0?)%znKCA84M4p?m2fc_})P zXr=d1 zkbclIX)g=0Tt8)L$ z2uH}t^hq@?s;tqtq^6Va;eO?4%hZU8zkr7mQ$YeudM5`*uNtT zuD*)O>cqn=2MOw3FYc_TQParBuhkS;!3-eI>yI)h#m9a5weBYlWSr4Td=t3+cPIge zuL1eDt(vw*+_$fpeuvu}6nL*UaKJm&1bb-+ZlV0DC@Zh&>&R4Y>5-}Mn3`Ibh=g60 ziiq9N*?s_DaK4Diru!?SU(Yr$L!sF7is2DB_~U7x>jCu>>W#+;aTIJ6wFq&RJvgah z+*zED74?gDvgrn8xq{|=15tYc(vy4p-}nd!#+TpN zH7$ZGk&1(1GE=_JB&4u9;Zl4TH9bzwb$FiraWZ6R2q(IH5UncafKUA|#MF}iSp;Y`U&U}jpGwho7*L<4xC!5Lb9ed*ywVo2= zppq*b1h?4m{w^q%;r+Yl2Fd~RypO5h&9jkHL}+wZ&l8F9EKiP?+<>8VcNm{)DdkNa z)Y4950bMcA*cMLtXxVf%(_JyRCu&xd&`NWi&wHZaAjb8n8yMCys4FE^lDAjJ82SjU4rIcgToazZ809E zZMYlhy>Qr6EPBcU|c?5()_T!KaYyRR>yW$ta;R5>qebCd93{HG0jj z(gh?|UX3#axLG_DFAs~gC=#BJjvrJ?1!+B9VJ#}w^c(|79q=r^dJcp~`qg|K$QCq4}-fk5!1LNvmIH+c0izb%m9D9_`tIFux3L z2o3zMSHV>A;_o=VS>B+B6G8mfJLfVghzKKeM2N*Q!{mZs>Lcz39aRCP+F!fx3^g&E z7R7pcwy!ufA#H^pp}z4*Vq)Sqv2MyiykkSMCOY+Uv%e^BA|>z=pF;<3zFouT)*!?w z3c`ulJX+9yaWuFr)7i|K|3sxE7T-)wqt;6aa3p}o+4e>Oz1b3+WTqIql+T_>%;RmP z7pFgg6T>f_m*N9^siVH67$iDz^*Lus0Q+I$6sm8l;i!70mPtdxHCCQxd>CHt`1ojZ zC$!ui%}2f>Psmg;ZQvAx{@E-efZ$LPpNe;K2^*&}CeJRg+p!8H(cYN{mNx_Z?$H-M>$*2#N(mD%EMaPC=-bs9f zy8uY+(D+PV@5c0Q_N}Q3cnH+B38`mVy4NWixh)eipc$?Z6H2?AwmwsvqfV{N+;NZp zE-qHfHejc0+1@~Ev?aXY3HW9AEHg^ZH>6@qww&2IWvrxD69o$-Gv5^~jMPQ70_MtF zt7y;Izl;iLv6)+R%#Yplm2Y&YutJj4BzSM18wM1PHE(1LFhulXylCk-_)8{dum(>6 zxx0B5S7lXW6!IQy<&kCAH*YR$`c>cm^K>cL-_Vf%EwZRu+DcMcGKg&=Dyp^k$$MT- zzM`8D)004!dwEv!FkNpqu&2h<2|w>2E7u{CNA2CS=~mSe%ESG2K!E{iywSDszpue+ zxd0}$!A1htnrB!jZ{u!DaTS;fKH}iu-C9aO5MO5&DF3>Qwo04|>NRxRMLUa5l)hY* z0%VmU!|*&^3IIRASJuhQi-`8NcFbF|vgDr8R`F}8ZQ4jzrTO^Q+~V!)HuGDjmqs6=bVy(fx&H_R7RPe{ERE3*Yf!V#V6dVGn;qHd$VS=_ z=`TecRnkNBZu$NkMpS7JcLB5n$rPE6ItomoGaehwP`~@xpTHRYhO0MToAXki2I1zK zG(L7*2Et%|-X;+HH-6`RK(f^z+R=c?b~de2pA>8wNczo3*614Y2TL2P@2PJF@;9ZH zpOtsleBqCmo+zy-bIjiYVuX)<H){`|$MIUzL&gpjv|G54SP68q#K2s-ai9iN`R?x>`gR&uW#_tw%h<-Blzed(^^!sBh-&T06 zK879rPXK`6qk<(tpMb^@D-k+=0CeDfPuWO_$gs4*7PBe!k?8ngym~5&R(q~#6?p+# zH{U8vJWgRV{494~CW}qV*Y0GrIZ?*q*D%$8FYE%`6om>gHJXCo%e;OU`ZijNYz3Q2 z;rgh3*H`zP9=2NyN+Y_?JtiNn!z{TL@HTWd9S7=~HtOJ&|A~w<9RQW;4jhKDXj+Wth(^wP>rV#-^@p(u(O)P1f%)Od_XWCDda^H%oNf^n%f-Zi+Ym+!CzC z>=%VNlbsLY!o{A-O1`^NRMR*GpQX_Qh%`d@XW>#O?9HukNGiL?AA^VEdl)>K2z&jGlo>^4)7^YD5%Hc_k%EB1wl^ne2>QXFZqwRgQtX}WBoRP3ym3MqGihF}^p=Kc zGri#Tf20u0kmN4=ndJn>tPnr;wk^bpHWPKz|hQ zBiR!_FKus~7nwNBuaz1|*f}p4#^Cw<`@+Q~+CrSx5M3pw>L*9K7XI>okeP-5ElmJF zuoDKlcX!J4)2;W79yz7_U+AUwM5_EZVyu9EAV7OcUG{#E=kh;MU3_-4s1LaRHwY_wDc(f&VEc7S<3DjQro{>i}K&@9+Qr>u(lfNDcD;_RRmK6(i^&UFij4#FzS% zj$7^jEx&m-wb$+FLV$HG%6x5Yn>B5FiltqqqM8M?V}n5Y(9Q&EA_muG+t%`F3WOO)aQ}`$q@A(DnUO;2$`XCLVmV{C0HIJ5pe~6X9oyMcT3at7EL* zZU3G9IY(;&Rl;v;ED2G@zgxTbhE@|RV3+Zu19QOPcWE)_;x*EmGC#v>Q6hQZWnOWe zm;JlOb^ZP<0sWyU&Z4L&yD_al z78a2~yF%aO^)&U6;q}t&@B@igUU&csGYQ-EQ!H7DP@5(WXqP{0`|4p(qs>HZ@E-U! zQGEBxCxN9%>%>%xL27%yGDzF|s$u=zVgD0HA1p7mQqR~aYsv|ymF44Mi?1z?eo+%A zClPIMf2dbg={2>1a%YyuD$f-q)Hd_#tYGnksr3<{O%yiKS=bSzcH+%nrxjzF8&*ev zrZ+49hrKrsYV!R1MzvO{E5Fu-S~j&TB1$a#o}yMwAfO;1AVe*TVV8YNQl&~Q0-At8 z0z}IuyAl@Jf)oW4NGOCw0wf@t2}^(^gzWE)+Goz3Gv|+Y<~`5MnRytQ!OWfezOL{6 zT|eK?bzOj7|2FUYvYk_O{gBXIMe^*%Y5-h$H{;lQ6-mVwgGcobZkf2m*j0;?+U_JV z4zIm>HGDQY2&F;WGvY=4SZ=}g$Jr>EsU#0_;=dw_LvIQl?! zxhS(Tkl@)1=>n#R|)+yJe-eOngU;Wp%%M=bpr!j~r-841 zd-jhvWs7fQL$xgTEa5>+!QGxiQ&}$ycTE;YilMiMcdfKR58n7|z{wa`Jbj}iK8M`X zbLOu>=h^DQ@n%X@%{g_8o|&0A1?rB2V|vtz;8ebv`tuWWyO=%WVvp~8P811L+Uc6N zf`mA+su#sdy-CY;CBWu~pd*u`fLH2!eZWc6V84rVCPzcP9IZ${A8YHzWrobAC}%l3 z&QPGx@3Lff{JpGx;IjT%1)8_5Xcywtle-lXZn^lDb(;C+d(g=7$jfvp^7VNhyYaJ3 z5^F5w7zvgqF3VR)1LUi;MA69#p?j4(+#WoBOl#8>0Mx-WZ=HoNU&FVMB5s+i^-{nRc+8?;*ek*++53UOExW&ba zk9#?9n{OEb=KEUsw4f+<=-Mdxu~--yw15Q%P4{G&h&e8TWtLma%FpDelRO`&QC8cX zIeg0x-1ff;-I>nvuCwaiiQd>M@2m-ume-*5SFclR*iXac<8u6nwL_G%%|D%-$O%39 zQ^{<1!WHK5OY~ESxpT3z;{(fKcUDMXVQWeLB5bKNj1wp>F&%fEHO+MC@?ZTb$Eh2@ z5F;n#GeXORjruuW?tpPNd8Weo=*IUHJlR4w($V=^|B(%#vkSoN=w~+ppwVZpfPYKv zpAI5mppNxkj^^+4T_8L|LKy@k)xrBkYHIs^Fl>Y z8%ye0*e5SaiYSUQeS9#|!`0LbgSlecQ$NZ^NGSS@g!$5D;WYdstxt|muV!UBxsb>8 zu!1csH*3WY1^(AZ^>$SFWNEm<+4NEHNBRd%0b;c;;S%cEF^8G6M~y&wZ%7G|5tPgp zQZw(AcMr2aQG_0%F3vaO+A2t6kKO-RK%YJ=JChiUAw{oPK#T+_c*c8Xwnb^}@k~GU ztY4mCI}>paGp_WOKzZq=G1bqhv@E#0DGwfR}bL z9t-1Y@$S{GUg@+VAh?B!Dw(pvr$tlNwxbc8rT^#m|5UWONh8jV|5V2Nc*hcZR-0G$ zXDjI5Q*1@eEhg)F$z~$JDu6Jx+z)adIvI=CeqBOuC^IAzEW`pFRk-ZlI5XpQV}Kzq zhp2mu!__q@)KZ*q$1a`xMQ`^})!O!5liI`|ey7RKWt6uxyCxNZ`xI^jn&L7%OY!Vi z#m|@-U+BWyU-h@g!`q3%2YD02gPN39_k+8NpY-@SmIY8QGs+ma_o|wbB zG7cmapX`Z_h$=|U)Hu3E|{S!Cl+e-AoRHK@x+j&iGKj%#~^#J?=YWE zSl{+7ZNn(s@@~TQG(C;O%8qMgnQ6K|Ovuhi6l)pHtL><=Dzu_%4Z96uM;d$3_)XQ& z-+0^Fgze#2cj2%K!{CI6hn}h?yQf{Md7|aV?Zo%n7(TFTQBQ#QGZ}Ro{Zsm@L~Qvv zsID$U`T-0qqz8ZGuw?BFDg6Sp`+{=;rSt(#=2|aw$EP|RIw1%}mgG}<@Ot@ag4sR= z=-|YS%Ah(I|F1^j+gI))8tha5dVC~*LrEMZZx_suvN%q>_L0uDNDBzQ;gse)SEc7o z!acD~tIKI@_Q;?nCxh2MWUh9Cn4XsxLo^$V^eR7Ny$$}SonmzCmvSecuTy^+pc*qq zM_Sr9xXmJYpKDfe-foJOqc9X%&XOk_=N6A{oKqch7s%lXSr$O8Z(|Cd!Vu1)o2wCV-Di~SRQROsim3!t11oGE) z$FfUGDs%_d8_-N z?RjbHu5ATpFcch~ShH(#dw`DUQlN%T!c|nY(|mh>DGxme$d3B=OR?z{f6=k$?th>o zQvb1eeBer%^SAy9Gs>CAhXR%fL&V3;Q-}T6c*>lEK;pVfOvw)rVb#r1WMRS=0&lQOw?W>RHRPhNb zT~;TW)r+GqX`cLJN(+!prPHhl7I*Y_^CMiXiq-*-A_ z^2e5@|F|W%*ff@oCq&F~ge$?j;W|ezmItMuSphl*>=}Ks`!m~D>geuxc-aYkSJzgO zFsNxS2p}LhUCaLdRi?u(15z2eOP9k^KntA|&*fDaSqEJ18M}txe)*qxO%S#04fSox z@W|aW7@+SK5m5x_K)pz$Y0}JlIWYR*uF3w(fJ|OvPwlYJBa6;V6mHu~YA)^%KszgF z0Z@v^9=5mlT=<;49PV@AL&#nCn5gG+cX;4*4>)LJXdXI;9VP`H$=I^;w-E%G`HSY_ z#M$mpiWl7u3mFf%rEpooZIRula5EXiydMqVD{7I;s@X>mtN+S28?a2 z?X&~%pp93i9c%747h;QVKGM7!_91^`VQ@t0b^O$-4G^^gU*c`$qgG^5Q**|0g<>e1 zGcI>vCnsX=CKcH4y8Z2FK<<%Uw;5%l<+}8jJ7U2@lfVSJe=W29<;3@6aljZjrMYrN`ei2 z+?ZD-oI8kwEui0DeB(CFYAf={`FKNAypIBM-GInMN7_*>@4W{+qgv#QV}EDo`R&?e zxi7_XB{u5P#K0@Dr)}@dAj8-*Z#m3hDRFv2p4!WC-BC0fbkL1-VkqP`cq6jY@9ZVz zO0PWVwdax2OB2a8S?ac#_`Hj|b{Uj4j`fLC8%*jCXu0%#IO`MpBip?TcS__x`$Xm+TKq5@)s6QN^j2c?SLbT-$hXd6cD_Wj;XX zT0@jfUR(S-@$U)3kn1j>v`A&w$=aTp&)gtgv9xtFwZ*v3#>W50;~PV`&`<&$T|76+ zKUaOiOH%sv{w+Rg3qgKws>Mgh72n(ghC~lPsnTkgdrej!=dEWQ?P)N#Z5a1z{h9OO zt`mQsv%QiUWHJVUhWq$C#V;K?^-T`}zY+N+D4nakU-0_Z5B=f0eSoAUD>7Hu16z%q z*r5dCWmTS-Ek9w<`xX=lh%w2fqbKX$HIFwqJU0qXMdM_XXCNN${*G;hZ`rtW??`-n z*0W0ociEWQfe?b%(`FXm9G*F=t$E@r#_l=DenBb0sG}n&Ns$AjSM;c{T$j_O*IB{a z#%;@%Us{DW$Z3!VshVDCx15qZ7ry}>_wu6_-p+w zdD-;Fvu+C3Oy)b_Tpv5v2@CF^@#y_mHeMD%wkNs2uHHu8*@xA8kUbn@mgBf{%dpvB z!DevZ4o=KH-Sg`Mlsg0$_yBSL(Mbd_8Xy!Ds$KgFsAfi&dUV-l`xe}dE7AwIw)I(+ zTdKQy=n!6slk{Gy15nFrV&sEzN4RUF=E!An^~vQI3E9tkTTbw*TkYL=yVl*ewm!v0 z*W^1qDCS&+Hwm;XTz91k#TAXWEQ1){yc!xyNTr)yS#@d#L00!>RDcXcTCSt2<=+QC{}_G6 z53=~Z--*rS(&=E~S3H57b9}i*d@_5GKZ5Q&-4PuBc!uskoNDmz49W5!<^5Ep>y%rf zv~sgt1#MFHYt#Kudzr62t(N}d#(xfQ+(a&YN6JUI->d<L1foLz0^c>pq06?4vhK9Tl{}OBCV0&x+*g5AT?&)b)T75WNmqQ zXRMH4#s2`y7*xxxS}V|L88KnDmHai9?PPue=urL^cI09;6P_QujA%{tCN6nrB|p=` zW@QzH)wZ9={m=}tbIuhSGuxi6GWWXXFMs%H>*OzoI0{_Rcml(rCP=@jI$iahM{Uj@ z4fQ<;2aLxxRKqus@!JK#XYtE_nqn$AEkMyY>opQkE1%`nVN@qm&R(zFCVk1h+$i172e5T{&(3{=%EZs-+INb+%!4Lg? z(8AeAXOPF3v1RXp9PIVIAqV6R$Xs&LK!2R%PYv((GVBO*Fj!H3H*Y*xL--4ppS$ip zZ5#fm)&0^xt!DoK<<3BVTH_VL{i^>Y*J-@&IU2idsVM9gDADU|W5u|o;0F!Ta!rQ9 zs^y5*&J8BAaU zu?FkT9PFS9qoS5d%3DDyS6BsmtpQCcsPqv26R-E`3XgedTDD**?ROfoX$8d^UN+39 zoTzzgTW*zT+jBQt^eW7&xXIr=COV3zZj-PSlD`Od&x!_kPses{0g*~*rZ3Y?2Pzm9 z%)xqo2uyaTYv?Rc+Mh${7d0lFAhq2~<@VWUm0!_&HyDWrCRwUFDq%QOm;Ks#HNW%x z>g@XaJ6Q4jWL-|m$PeHx03355_5FBm3Erw%D2G#Aee#o)pryK$bWL)(vu)pj0j+BF zu!d`EV-9m|S9<;Y^xWgAjq6eTLggq0NDftI%3r1nYkEq(o1nLi11~?`geuQb`LmSF z5)LU2n`usMh>hmzDt;kHl}IaED4Oc;sPhs2Yw;RFcWM>w{?$jW#p6i^`cSg{+bkP( z>0(Wk0<4Ht2WkW?RegIl#no;X>o1AM7SzvFySJ3P4rK|UE)q7ZEnksUetJYx#tKr? zIN@GODl%+Jpnv~_+kN)ZvorM5ZjZ`--L|A{COALe{;^&<&uOWO!_Vz0t=}T7TQc>r z_HT$wI}}i9voxXx(kn0meQ+y?SA4~-_N`U% z`nPcqgUdtQK<43(8$*rJ+yHcygsUh{uW2ob4|`s;E5dd#?di;B?pf~vT-?Hs4ovelo_MeaVrN$5a>{xHqqOzagUxp3`&XhS9AJA%luVcVH z`uo!>>Vol-bqu~aS8=9h4ovr1zaL?Cq^2#(%`%-PAy0m_S%3e!8shLFdFH!=$UGLWshp0G>-X~3lL#G7^P@VXKuXIXq7}x z$R=x|HyBnYcHn!+T8P_L`(6Pr{^?TPgWeL0pS~u$+A;lbp`i;?UR?VKoDUM_E}!KZ zi)WOf?WBsCnRB$dk2m-2TEiq2RB)7y+r;JjP<*2a#VROj9whh7@QJtgi+#_418vTg zwHIP!gZm}_vr7S7@=5nVb)>067kOEls&R!7w3u<__uQiH!jXdejN2p8HnNw@MR}G_ zj{;jAzzv*i@S}YA8fA`rpAHdccZwH$r&ftSy4C!AE@M6esD12Z_GUR)r~OpZ*yt6v z3$Bg21Pj;&{12hhmS$nQvnOpf$(0411?Re+f?M+GU)2xetmG3oQ+V#eCf!oT5F^^_m(N6 zf0bEb*%aZe>s}q$&{SAU{B}Oa{BNMA@f+9f`2{KF#PL^GuxTM#VX~{(Dx1A1KEdRb z?15OBTW%CznyW2CiypU>x6PC^3ghLjnr@p4u$>J-v+*`Kq2jh6< zC50hWB^!~ki}R2$n#tOv#u_|))y~-Su&8{kdM;TV7E`k@Bi84rfiKyjilS_@Jndvj zFN#;^NF{}*4$2CWzEepzK{Hw3Zb@qYt|!YA2#<(Ii$No-!B@>SW#GYa!t-6ldD@g~ zPEBcpkz+sTxDVFf`7RRkM$6{6d7#9OJ62)!^YrD&`Ruec*MbFK#h+bP_*#yuhwEFr zcmU>ljJeVgeejx&%C$TSn)BTUV1Td9_6yy!v4YjelK7gMzY@>R31=v0)?BTLbcdsm zgV(AbMQLRlth*o1Q(CF@0&hG1`%`gM^W8kaXTY5Ij$gDrP|o3~2Iye1M2C`Vj!jxF zLk}50L6j#gHZk&!+HG}$Cauf4>$BeQXQ4!oYkeQ0y$^OG-A#5g-k6==9Doop>+nxP zEWNZW38(kU9~^!74c;RNZDIfo668M`+V<*?55}!zJ>dSdgaEkNMbf^Fvu&=%Hxst> z9R|90=canwbF;5~n`ym7bJ1J-3;42uQf^i) zYuDt>m))j80j*(;LoZNmKy)F60eaQw*98g$u5Nv$F$NF$N_YLn)=6?sDKM5#V)M9@ zWQ0J+T<^ml0WOlR(3oJP=hb!%*h}#4$yx++-adHSR_UY9ExJkPBjI2D7SL_F|Ftr} zCD_%2Yy%-l)GJ4Hm@H!VC-J{xtIK#_IIC~$JZbH{<&Lx_1Lq|?cx6;mrSbum5Fht z)Dm{IAN}{fw_Ysjep1!fBZb4wi>0r_I?jFWPM>`IrQ2<<^;vsgf4B2V0WQfVu&M{OOM2+@41SyMx2Aygsa|(@(k~Y~J_vxlK`+n3zvJwG zDUUz;`R3M2P>GkQU%GArGrBf_ovZjF zX8F$r^+xj47Z}t;6GjD-i>1>NbKDHa8j5V5*+!O$s^OD;x zAfVW{H^pzy$>O4Iyyk0WK8Azced4VG7E*~RqC9T_@qw(!W3dHnyE7y{DL!|p)rr8b zcj6=kpj^^xY5*yg#F*6EHa$)O>HhnI()S4?xpOH^g8r7#_;9mg1Q9s}?W$NPLlPK5 zb`bSeE3Tfp^w52T-jJNxdrWroD(a z_!Cj`p<)DM&xPsWg0@FhyR!olKL#j^-~4o_xw>a299+*G*mEDD)LFN5+S`mQU+&~j z7N|#{nqG+;pI5k!@Vto>tN18xsK&U=7|@Bs zb;*^4QJP4lI)FF+wz4v~Nd4g(am^R!V6I(IPy{M28pg4saOA1WIu}yP*nqm|RN%c1 z@9Ac@7^FyA%E~-g)P<~TPV{Yl(7|*(uV2w5{jdKhVi zrRp_55R2XXQobZM;#E6VsWGgGD+V6kLz9ig#|iz{HJH@S#Kzi4Ma(h_SXT(K`rcRa z_iQD>LZ~Ex3tIPo_EV0z)B8!LkbK9;s8`vYZTV`;Tz_H5#2~3>Ik`pjF&_=3@7uh; z7VzMiHG$dMIzbWblfY}SQ2Z^Ztj>pwoYEP?P7^7#3_75@?92ONG<>-o6MQFLbT#$mQUt>xV_)Qp zMy!`637-;cn?F&yVC3W76X$0`K9?2Cz zE`>Xz0v2ZgnjV8%S4Lw)6Eb=3K9sr9p2O)~2%HWQuIaQeANR(#p6kM}8y+!@WWROi zbqP_A+!Zqt2;D#WS8Yw%n(k;5S~oF6!*}8D-SM+yiW6Z-1v~&5lxZ|Syw=~M$V(A& z=a$oiMi!RuraDkM*yOrSQy9X<-dPgZEI<4u9=Qa5%SoBO+yL^XEwE%a3|hmrCwu|F zMQoA|G}bmW<%cFbB4!)-q33;=5;h=#^|Vnf>5}Rw3z^yRB7eJFJi32a$E1E882I#% zAUK%N(`4@+DZi&e7m{UOW<*jvuww;2*CD;OmXvD*c;3ZX6(XvH%%tT97uXO0yUHxd zb((jg$9e|_ItP-%A+~2e1>DyNVvaT3p*_Q`3vhAwg5g(tqG}1D1Eral8B@7?&Ufxf zK@OXySTI;H`kpl&dLsk)lUgWgBuZ97hs{O?jP=E4;sMD>k$%WiyuqWr&P)9|#UggdvNQ{NYEPGfYP zXLJNY50|Pp9InwpqH}h;!zt^DTz_-5V!T5{U3)gU1K`$DTjFJlTh}($N{sbj_@bg= z49b%pOC-ySw2E^lnunvj3Dp(s7Rxa^aE46J!l#TkW9KjgPW|HRDTAJLV1qlnLx#?1 zBLB276qTdORT!5c4d7_~d4u*GYUDhip*AEZxP)J?aQF`HKt=oLgLFxpU=Ql*_F{yF zlaP>?3xXghm1Ab{H{(b}MVXc@u-+l`!7q~4bA>{xOt6K8<1NufT+# z=qzB6aryzYe0w^-Nk;A#@nx|?KweQ__!CROU>-M%s9`=F*9{N`(!`b@I95<+w0D_4s#?yYnEJP%sb0R z5=h(t{fI#H2gciKsV$OKyiOEYIT>%n&u}Fu)5q<9h)m9N+P@@=lLyL8*bBk6$XA9m zPS<(B9E(GgHr;L%y>L%=MU!Z%h8}wXm~0|2S-5{fR8H!M4>dJRj-Yvya6EKr3uk;vo9XuBqTnt zRW`!__7P~YEv^HiQSXpDt5>L4Qanyu+8sZytI$?nPQsx@V`xH&x|&m~kdCNsmAa8+ zDhhia$ufLKx_iX%`}#6jaG>01Ynu*??rZ)-Kwa&V{y6RAspG}Q#MoVPVF&6uZ{MFD zdYGPWs8&^k7<=n1F)k(pq4t(>3-!8rLATiVWmUz1LfDF{j_@w#(yz@X<5#@rWK7NR|Rc_sxy&wuoH5Ny}?D_KJ7e!EVtl`$bRREWrX`Xpv5RQ~t5%-gyU`o_|Pk(P~aar*p zh%E_SZ^fA=cZ9FEiZLCxPR+yPECBJ~HG>*s#AEnj-xM~2yW^7Du`lHw-HFY;;2`%jHHGBJmteh1w@~cP^M#{_|HQMTw?&;_)X(uj4XFqD z6GD-tDKbOqv`;MskZyI`_6?tuY&O^ctVz>o|L0OH>6F8OBIXO4@n7}ei*uicz2*PC zWc;5?K1uTbQA+=hT>kgvviFJX(?Yb#g)7kb4-(G>Ou^3A?!W%^1u1#AiC?@n8@&0~ zpqAcBH)TO{r4=B7Z|bqx710`gOWs$Xh}@)S4w$ddmHsv8)QfOToXyvcW}A^>Qgx0k z>JOMLThkudX3cW!O8AWq+brfccIZJUWQf=qgBjfaGVr!+bqV_I!X5Cg`jb8iTks^P z8veEX!`p8+wtc%ac_VF>tottWimYBA z6?;R8ATPTstorTFk*T^*c|YkW6vNP>Hx!xjdYW-D1EmgcmY=!B#mG8BM~BaSCCTAM&0-r|5#Kv-%)ho7L*|&%v$CQJ z>-n96LH~`o?ddUFlg{Slf}|if{FKioDT#tk1HwSmnQ&`!Mee4Fr}1`NoAmN*g2iPS2%`wyyfPaVL7O z2k0(!*g?%;U^V|{(&Nsm$Wd^tg_TOp{M7E6+3O3}#Qr2~lM@uI3Em#JZ%9;KGn$eL z4J@-*jExKv`z*u2?_|rGNDQ|LU{nBq`Cr*V+-X-*b?sSbRn9u#I|Z;A@kbgJ~|c`tGuD74nN7NG!3X7^`Wd!7GL|G z^h9MxSU^h~F3aC2q|08Of4e#7#lPdzwT+Fa$>hTfnTa_(t~`>apriZlwM&#orIYy& z_r=g1_qmjK55yAh^}rw@N^GUV;Pftw$SG(83P?VARhlz&uw=oyb=|XMI*U)eZt4n; zJjO`<_BA_V?f0i9CaKxvl1x6^h4?gk^0moM_vslc1dxG3%{k6TtI=B^^a;)fc@XTl<?{&`!>^9!ui*Nc{Cnf$_}IR3PBZ3R3pE7Y8g5u(;P zGA#(!_cS;yYEdd9N0DzRPQzg0sKX`|h@Kl5!4hBKq zRF+A_y^1q~9ywr??z^%Pk^P0U#Xkpi@4utRlk~My z>g*%KA7ufCDhYt;Obf=*t4nI3`F&9#r)1XwudwfR8Q*+3TMMaU!4XC_N{(34B^p{z`930Js8&6zp(<)x3+&WP0j z8I%$KVQRrf0sx;Pz?($?z>R?ySfP1%5ex<2a>0wv-5Mz#+`sz=4mQI|VITumK2&+g z_=S1#!qQO`x75Ni)>c^4mKkc@`;_oY1?OwxYrR!|sJK@pyAGlwLuo{5&UMX{#)pZq z;~Z4Jj&_jWkJ{R@U@}#TviR072!oP)6k>E>=MR^1m3icu0FMyb+=rE_%EoM?llln5 zIO+E{7b6QZi|VwgNsUQX7k4BVscXULMVUI<+Qr6*F*`=bkz_l_7|ozUCOuV}vNDpDydmPz3zGO@3;!gnY znA{{U-or439qv8!@sAgMS)}zctU(zvkX6d{--?PbCr4RASh2)tGbDjMa~#_#Glpe1 zgapj{IRUVIrlI1>y=XxaY!ia`OI?Z@u>RbaL?c#*t2-nwPyjzq6D7TCc%8J_e3WPI#0^ z&xmN390#HmBwhETKC%gBM*4`TP@h*jQw&9yko?YrV%7YGLbU6}#cYCa$HdRHDHuocY#N06np=eoy6pJR+;JM?lZ(Pg}P(Vr6Y@4(a;_gx)3e z!bO{ALW(dnpspR$pblV^#!E-0vjlt2PuC@PU{8-?WcN9eBpPo)>B(6=yFbWH&?d+Pi#%b_@9ZXZ= z2U#@7SSw_{n$tMQZN}QdO!0ozM6jB^rW4Z4@N-tKg=_jZw&0WHa_McAjK6ul==mGA zB0=BU%5s{+K4q*{a{qB*80%drw;c=MkVg4x0O)0ByYV)I9!=kK9ze1R&0tBHq33oynZ6^J%F-YbcmI{-kRgCw4B|Kd?k%B0}s%5lgzul0y zC-sUqDs=Ra*;D8OS)s_`y~24R~CxMlnADo47(zq2-2eV#l3p_9&l zWi0@b(B9XJcz0Vmt511*OC$^a56zp;$rB2cQ*Z2%P^qH>j#9NtTu* ztdR>_#rfdAxab^zYX>uB0U>`yDEzI)aWhAX+n41@chp6cVlx?vW#u^Qo1QMs2wbRG zU~TYP^C}FMjcB%n8KNUsH-3NVBsFfA(S-y;c+{$+Uwje_-ZIEQ(%RyDq9(KeFtrwd z>DFU4{4H&u2pllT^z~ReOsA~gX#`daz)^IzR&l1tYtlq@Y~ILX+=&JASNPjTf4I64 z^4SAqVRU#agAHXN_)LEk^(J4|xO>O!8<3+T{z+|)TRuP@$U8*HyV%v}^qeLd zh4T>@W4%=RJB^yEvQ$Z*?z|=;GST)1+%b>)qc-etkzUB3Svq~i&d_1gM&7^}`pFap zy4n2Vw(Qw4y|`rkj;f4$oM;B7&}3E~6Xw{3Fu*B*d~BG%u!P<;pVlXvI4*^eo9X|E zGN`(wA)T+jZD*a+SC#}>_i`E=Gkx#3|?o-7;V zwO$>2Pe~0+vI?LP`ev?!O-wpP>0;+VofIXD_sGopw53-5FkzOlz@ps(Fl!8nOKc`( z@j%!5fKJ2h=R&jJYLYiIkZ?+t$Y=Z)%M&I*ili3o{Z77qFRJr0s`O_#ZZuhkol>Y4 zTF}>#8=1TBLPp_mtM3CKMS5Yb*?n?gM6?TI3hSbp}Yu@il zV~4YGCmWc$5WL-es;Ao+@vxZ}g0k{v;aRLPLAPQ-wP~H<)ntg&1CTrfrXPxR0&lo# zFxzqy8*Kp)Su-FyQh$UcCAJEZ+j}X2kg+FGNSI#dp8Ki@rD*_+T#A%t6h^*UY5AO& zD5VY=ndl)08mAY-yx%eJsYJvr-z+gRa~UX(k_AHn=BF&_^2y^otTv7H)pHn@%{d7m;=9Y`R<&d^iOVw^=~%;i4psz5MI zJEzR?V=55<;8a;z-~Eil#e{nLWBIzrY^2pVzfaT(QZvTmdCk&siI>XSM~eL@k}Xj4 zA>nYQv*DOb(|Yx}-70fW49CJ)nMDg2k`ws*nU&sA>sNAr4bsN>5`EQ-d8eM6us3AT zZ+^M$9k1ff*3QI*NuPW4tgj@{_QJ^ti3Xj9jxd6)w=)hvy?z3rYhTiQMxi)kgyF?g z+LBSHY#foAbqxXUX_`wIrid~h4h&gZ@gFGZ6(_}n*_J|Cx^xJku+tz#$$Ou$MQwRR zO?@~DYf!)790X$tfp*v2$H~(dl#u(BF@-W*VW;gxozoK7>tj$=30qW^yJ;2XL$>qh z9w43V@nlZIy1_NF%#4Ix?Q9ukPS{6oASjEyF-BBkVFDW{_jKt76LBUfDxkS3{jm8C zfmv&77)e(K0!XKjRUpP92=Hd5YmoM?!K)4eKh_g<`Tqtj$VlLigeFH@$n#wL?rUEvb=ImTTH&#l~qa&Py_R+j)6JS~DxR}>YOJ1m%xjyXx zJzTWpzrT=Js1I1ymARJSW@i_}cb>%jo@964@CFFW7mZG#dkf``_YeE$T;AcAXSy_R z?MyYsE{?Qm!Q6Vz8`Rg#4fVrk#2=MB!upE!GD=r9o#?6jKJ2uUjWP9h6aksl9mYCI zB81rE2()4?WWmPZ?>iGn2A?Mc96Gnu`mMu=>Dme-2qBCmCM*E*xWa zvu?n7`*imY{Lahbda7Dg*ldR?i(JK(;m6}8l2LG?-b8p_49*xC{hh~T(@$0*!FbaU znsIgnlHcaQxeVkMhyj1+So%mIpFJ?NOtu0!evQLi_UKC>=BF;{$NTZ;<8SjPfRYb$ zn#WRPx)uuW9qMRYTifW14B_Wr(`YRinsb2eAdb8dM-Akom*4fwMwY1&g0A0Y zO!b+K{o#_I@*8XS=kkMi@tfk|!GR*x^vLk#o6CDI9czN~6heeeH(JyYx7T1LCP!ZSBrFYG4CSzsWYv2(*usA2iLK8=ow$ zG{GZ`a4g1O_^=XCIvX&S zhM25EWaN?8Cudv7C*CD57cQ>H8nK1+VtcG#P`v1 zq%M0aN#m2sMh3sMY;O{u7Y_|X&1xCfdPmn2)+O$dmo)tcxTQ|n7=I0p z8Icj}S7niX!vjXXna4m!41VxsOi0)44i_Ozi31Pm^UmTZ zUIr*D(-=WH4Fkpntuhh3>3jk*-`AJz3@zWkE1dF)SJ2n;_FaH~;)1;%kVGI_o-p#> zBNT&6_|TX$j8O=mBYpF73(Gq70NmN|cnI_`)%J1J2l4dSa8ZrmLE4OeO?z&jO}y}zH+Iu9-s(4c?*7a!lvg_LZ3v>G|af9%~t6NcI+x1 zHT>fjG$#Ky@F4*k4!Ri+KdI^DdQ_=b!B1cU4)ZX8ErrgutoPYuZE z#G((|R;X_2@VSD7kfh<+)M5)Km?x;@rHZZ=(54YZto2o$WRA=4g3pYFZ=AjL84TS( zn!kiM`L4f|&pZHaAwf}sKSi+mZ`O}lm)?1>Sz3ivzLih(wM$1?+mh({8#DdxSA%$! z58|=>zS00iybeJa+o3uxJqj!e0+7zxnK5QbGAzw489QkBi^D)s`*t zi#uyxv#*Z$jRa{u$7Ph{8sk4T9Bjne(Ua2t!Q1xRGqY+p0Ub~*;W`q6LQ zyj~uf!RtQ%N#D1`(N3P(IQEe6>R7bK1cAgE{g&YOFKMXBnQ}X?rrtL3X4`N)3e+u3 zT)Ukfww`66fB`MLr7ygvzB|7-fV7=pRKis+7yColHNi%THOW>tOp*Jeb+ncI(!V5& z>S3HSrUZs7fgsj$nLdY921l19w(RDjb>41X+)X)qr?@*CEPC25`n~nNV(ecgPz#2S z%qCCSH7_4W<`)gi$u|6GxhoVmP=vUEPzpjYrd*WWcp?{pNj(Y~6u!av_j!S)5V$k2 zlKbP4&1xSYOpT5u=CEg0$U8T10OfCNkKqqhzQ~0nnm0PZ%7LdU!iue$d zQ6pJ=gN)}SL@OG<5H<5WwSjg2S}78%rd4JikFCU=nQHR_U=e$Irgb{U1j!08R|`P? zst*v3Y${X(h|MnrvlrsK7~+9m^kuMO2XP^FRl@p0E)D+^@clsjn+W167P-70O-0Bi zflEnZg~BwW^JZo!amOw)M!4iIIBR z2U1sW`|lSM7XqH4h5_Q{FkSI43zZ&hYm5dsUsh(eKuyb{(vqzJ$c^Tcbac8tjV}wv zk=DHd_M6ApCV9>##42w6mGlfu%%JXbom0|>z+m8f8sQ8!a}*NXbF8pb3~4fEPB9EI&&Nhd_XJF$aeMT-N>$l(^c->U;mwYW{VG z7wG`0&(|{7hER=HC#vr;R8w}rnX(JU@AKl@K^p&R>|^XtiNlQxCm%WiY#()P$Z#WV zZa^@?<1{Lwt4V8{w5+}#k@%93fjU2p5%&yD9`Pk=T_&e>j*CA|s4W7Jyq`^Hzxh)5 zBY<`Q6sB1Kb@6^-R%KYoyfjPy@Wbf`vnsVOqYVI3 z1S2L&Z2_#4G?l_&8u{_@;0rzz* zcLHR_NKx?!Z&+D=LO)0q&`r=M-5gG7r*teQ13XZb>b5*H^osi?&ux|>7%>`GE9PQI zipAA<%;#=;I5exIC@fQ<^hhZ|_Kl%BAF=wI~#H#08(=q`GjA$QSWHa#XNWUL~t4q%`OrVXG{_4EPb zkU%)W{N=*x??_f@U5ML=Sp-{Lu_^G!*_(_-TySVUISno|-efC=&EKH;G~>)j!mzbr zq2icoKsTVAZU7SN#QVsc7;js(+ay-Uy~qY^3}=T0D3mcnJN?>!`=XN+YoE*5%mLri zKln*L%(lwgH0PwdvyJi{YMWo?+k@Kj0nYUx3gd)|wn$Xw&dCT0b72?~qYBWPmM2;t z+k^Yw@-KmjrbY;u0$=zCj$xhTkV9)RiQI_dzV=`{s0mOxF*i*KH*nI}7#e^vLk zcCF;X1czOqk+zWbAkuL$^iA-aE>xH72i2cIw9%ZL-1Je6k@a`=S0ZLpPyX*k(p`ff&43ajQqtWF4WkDT6_k>Yj-iHviprYSL7EDyaHMU`+K=06|jkPH1*R2zsK6$L9@e&-sU zP-9TpMDE?S7v;*LD`Iuh<1}-$p|{thWHgW*Mg9p)re4JPT|CKcWzpw@@d<44mMrTd z%f6Qi8ZHF$ea1(x6o@_WDfB7%4sPW%>;cq=xD=100FKare_P6-9-z{G6K>SKFvlz- z(fSi-*LpAPd%NMB#r`+Vn-dai-ooF4GJ=!Oz4+YYj-c_68e@HwHgQ!#x1(90P%HVc zcAH(w?6dlMwzRN=&Hj$VXsgEp7jGU+_E{LK#b)R#Nu_`G+P}tf{%by&akI*6pWkrK zFQV72sU6*MO-OvEIV3&RJmlNfrjsaGxu8)SDwu;bXZ?O!k7ZW$`<_#VY9aqje6|Wc zY454!j@@d#6Bh6Epb-VKEpaoqt@e183~_LjbZrLRztk^l zp@YiKI`hK^Ij%SaO~>oJI#I+`Cu^Q7h-AK8IGYgs&rb3JFlxz=b z3^Of>UaM1f`pX|n);_lce}boZtQPAWqvH=hfjgW{LlTna0+Qa}FkCB-dLvZez?_~? zhjjcF%NRPo^8d?`>(4) z+s5;Hm(rs~6_BEbgXJK9VavWZ{@PJ=tQ{w9I0|A`c5DAYDM(;ycilp86rE7-lndYG z6Xjc(?UF33qo8;H+3ADX+{P`JdZzBkw6W3~P598mu1GR&F_Tu@rTDgBxTlJC*0Wh_ z8%Ss`a414u$f;gg)A5ySxe&!S`%gq=-Gj2yjgbsd^}wRxFbX51Y3ub7C(7ANVB|o? z_Q5rvy8sfv8ow()EztZBYD4u#6mnMp=bQok#0$J`?nT#1coOgwhwbrcpkH5Ifz>ta zA4`bCs2*awA&qY~)UPxb-7%W8Svs?PVILcqOim}j%noiFOa1`m-Ouh&#yTx^Zl6DO_x$m(SG`r9Mr1)KcHoZtt3KR zFj}Z!jmA5WH%gozh}jQsxptXB^+3@beL>fSEtdCNzy&HJxm1JghHe-X%=j@0A?%@;g zn>h`h9n3PjP)>$W zG*p=ax@S^Q>xGj-vb#cmVr9`j!E*n(+w)2{#}tsG?HO_gzgX*w^DsY5o5ut=k3gI2 zH)!r&z}!e=-+#iw?g{_J1`da}H_`Vl?s?Ak-%H~XyZ+C+9TfGV@9$iI|BIUae^XQU zY+4Ea4Ya;@6#bU4+&)~x9h0cK#e-6gU6JY9`RL=jC2>!V`;TJ;Fy|5dbG)OVaQ<^M4s&_`lwG~o z_79U7e2XL<_|im2Kcz{s&|)ngGc~REkFpm&dsFV{Cb^1{%4uAN!6x3(BWKw1Jo%f_ zVZGa6;RH``6O2MDW;j!Gq{`VtV|eBi8KscS(&wx;T9mCW28I(8?hUct zcqIu{xRYg3(F!KVY@7BfjUX?0likejqg2|Lub+u6jfF|ha)IcPbUIyJdX=8z*RaYD z$2J8SFon1)=&hLiwb;ru$|}|B^?N8j1gJ!3v~MJe2b`R*WZQoWDVnDtg4VzN##=Gk zb#3^)skDUx^Xo^_Y2Lf{P~>fY5NIG-wLeGa7W}g1*wCjaqlV!08i6^x<&{;8kEZa90jc_Yrp!PkOUgeE$z(Ma5^!Jrl&p2OmTxsFI5Whpy+DsYeDz(9uZSq z2P6nxp>D()&O9iuG&HxH*$@U>!S!!-uZ79;dZc4dFEdQ`Ixo9b#-+30kb31F#ONje z4S&?fnTm~T;r(L@(z)6x7zH@cX2juE$HL*{A90=C$DdzO=hg`$T*@@Q9;f5Qd>GIsv{5khgeK!=+ZMBf{GFU?s-uYEq>@DBk z;*ALjNyn`x`PWHFSs!^yV;1S1#;y)5mwOd8bo$`0IUOqpcs#lbwLg8&AxHWra(G`$ zcO989x|`YetRXP|eKfQ!9C}71E`NIOEEPDhYm=gvJG+dyzX?Tmh1*tCIl|&lqJ$J5 z9_SX7xutV1v32^BYo>9JWUNQpKYffzjyaIZMcY??a0?skYz-24SbSg677hKst57aZ@O z54YN=57J%eF7&p7GbqwP`EZU25KwHo^^~R-x;q#pe-h&yq?QcU0sZLmEqvl_mXjop+ zve8LhFVs~AD-nG7#;gjNuHXJnb@`Ro@j7Pl+RyI+>N-h(_+h%xCACXv+BXKu9^PGO z8pDR2{qv+vb9~g-q|Do$5ir?Y?x%Of$S35KUeZp9+Zzox`$leNp8Hm|1?rzs=?<^j zuU|G0;Cfo^a{A(#0BSOE1&oXO!pG!Mfaocxg031O&j$>b-Rx7Y=YCd^sbS7~g)|Ru zzg->ssLjkPez4a(_Eu5qb!Dgm*M~ROglt<>_v9X2bSHJV&jy%}t7N97182ulxw{}h zXL=UnEPU-Ym`}~<8n{i#LRaR_<&@D6r2ZJw9_n? z-~aZ~tKGQdJNTN%dp699P%B5U?_V3cH??9fG--FXe>izGTfR1rd*>|7NyXH48TR^d z!hYJ>t%tx#0k*-3vAS%D_h#GV#O3i+Rqr1h023yyv8QaU!etYB)Y_&nt~=ir=Oo^~ zer?1vS?sp2e0-xUsWo3CVu_XOw%Y!Q=sr+jY92XF|IxyAh4j^zm#ku>js2u$2HRdP z2Mj76q1wB0(%GuSE~6|n4>fQfepQ;k{GDK-x?%qEROr_OUzu|SK(?6WPVWQXHwM>l z>Gn;X7CC6E%%`=87B!bTtn6>GThkq5-Yghq3gR}{aWZw>oOcu*?N3`(<=%EKZ)d+( zP$D+JQ>`)ol#NhLUu#7E5WQ|90Q%*FrGxNr-+!qfLXN&e`qsPYh0m3JWlJ5)Q(4OI zl*!^mGfuAV#kl0gF*_}!A3iEzF8mn8{A`pZ<-F)$qGhK>G89((z+B4?vo&kAGe(1P z4N*^Hid9sy=TX$FxpXGs`kC}pT_NL+F1W%gb?&{u+C2W<)>l(S9(5yoatc|=bu)ny zHWxR`)Fs+9|N(2 zd$lK@M(*@kt5IC$zVTQh%u_{NiY=8nHbGBq(=V-kY2}z5^G~3@T7!LRt%ZZqb3SXv zDe@wRLc4ngQci8S6Y(Fe$Xa+y@gt>l+ffIVoD6dY?UmP;vgMTRL&hx0eMb__s+Xp3 zNl|oP=YE_ogtX1%0jrM^3M;-LKRbEx=G^&s02J-L=Y++CIPMN-)@5A#3v!oTisi*g z0ni0AsN@NPJaEP~*Ywz8RG>vup5;S!|D>ll4C2$xP6GhU_b&PF`d((ePvrc z5*URJ|L0_`iu%>MBUcQY8lmnLA)jY-E*=ZZbw_iVXTUnNfJj1X{|Gbamjg{V;pg*g z9bF#TbatS18MRHt&jmEf#N=FMVal%1Ac#Fp7C4f^Cj>{de;L2KRY$sP)%7;}ZrDbP zg85*C4-Mu`=DO8~?jNuTr1c-tW7owtszr!S)`J9m3^jn`N!q@~VPSGtk$F4eB8 zvM|@AfX|APEV>XJMWy)nPWfzL9f{62Vxx!9>VbphiLS`B++AkwtCop+-iVtMwLr>D zVfC6Q1S4AWBv9oR&ifZh`cMJ!Ro1YRaV<&tjiaB_ZXs=K81t}QAixqLEW*tgVoRHgEUpX4}BB6Ul%C?wcDew@`;k&Ecwwr4yZY1k% zP3<$Hj}2jb1!fFFGQJy4nf7%%#XLj)Sg;X&XNf+-1?Q1KNDh~qF}1zc$9Lu8tkPC zo7arL`}&^{6T1g8gSJiIn~&~A^z&1RcuEtp4F$EK8D1$gxjmVrl`gTRF&g9_zJA4$ zSoB(r-AC1Q?8e*PkW0U_f7rZv;~VD_RlP6HqCbD;P0*T$q$65|TXh^$Ln=rYSTZ)< z9i3Qfb3H4gVlIPyzZ~r`FgEQ`$M=KbAw-HCqB9bzGe*AxO7!}12yK0voU{Rfssr7m zx1+GmFifNzUT5p_A=1$Jfm8PRd@_>2MA@Sd2RXD|rk{0zd^)4FiSz3E z6b7F$T4#~_Zy{qK#k=x_<)pdA`QAFlyz%Rf?{>UShxJ$(5Go(BR)0mIK4DoI?bKNB z6|Vby9B_ZX+nzM(zaKFZh-I?Ph3NK8Kj8#Vk4pHAY!{oR?t7#!V@O%YQOXbV=FLwA z-Y#YL3d(f0|2o+i^Zfy?OCHmsr$_785{a^y*i(Nf*Blk)NYTHeIAH4&9(jBync3R@ zJmjq`My>lko=>`aB0732?cM73d3OHdNyK9Cl!VuOo?GVy=Z+BW; zX05nAKzjM@STt10Zk0Mrtw$t9yTD~UlLS5eL;`@gw@B{go;SI?<&`G`nI8K@(DvDh zxjb~~t!XxHl)l=wmRWFf`{~LyeeH=nvM%Yi_o8%CBgj&v-3$Rvx7`qz_>|8bj9bc1 zo|oxkKyDhTkct_k=nCh;Eal98^$*PN_#8qPbXQMqR_#BM9^DVluGpCL7%lRGG))PaZGXkR|29#v!{HrN3E6?h$r6B+4$rtZEt9w>X&b!guA)4nahTnJPC!KxHts>L% zC+raZAKH@UtXDT#JoY0%Hmfzhl2bFBHk)kk7AkTzVV%27xywd;_!LDp3e=DlFZ3-4 zbta**E1T>7q1?80MSK71Nk5Y<r&In%DPR(aMumm2~5y!KAMCHB7Dz=W7l9tc6;vfYrap~@(B_@msd%GlhlwV}O! z(qf$PaUOSUls04K?ZJvza|^KyyN@nIk?fw=9+BLBwWzFy?elj(_d3O%4vuncnY8vq zobQsGoNrwDKz3k{e%E9yI5)pMRlwqLIF-b>F=s3Ie6?n~wR0t$*IIdwxz&|^yw3dB zd(as-jk$m7*n6<_EVIE_wcT^R5GaCWwC=?pRLdaRhsdPnw884yo$EWi7lnxyuONtI zC>Q1Sx>>Uw?B4;~gnaj|?)I+l@fOVLzF~PK+o5xoKpPT+Nh7vq<+{O+@c1q`ZJX== zR=uhdHkqpx0r0M5B5d>Lj=R%242E6Z#EeIS*QZAd7Fg*qO#&^W!+uYVb`a1I@6jG)Aq)7+>TPhp-@dUjN%RDBa+KZm+43eNME@-vL$ct+Kq*tWU6jP zryu}R`ZA2?j|gl8Ml_5vSco>#w_a-d60kl_5JAJ8>y#NrS+?eL(>^$$jbwzmk3xo! ze~Uk{K9`=UvBp$Jajw9I*yVD5($Euw*-|A?dV(Q9@yBQbw*`7~KQ4lQC|t00OVXJX zc6_O~RvEhY1*xR(trD(K(E4pW&k!(OyI^P%JT2CSPRJIhWebH;Outfxs3S#VrVbS~ z-g+gOTUA$phd1l?QK{|up)LWzTm%1>gkh^4mbIrr^xLume)_KN2vfwf81eK+&!Jx? z?yC>J-09MZ_P6Zv;`w47^ku8miE(pV*Wg}}!(jg1u*LbDNb<-lVx2_Jx;fM47S;Y4 z3B&a1X3@#Zofgd)(;pWf=<^WpenJ@h=p}{Z^*vMSCnJ%Zax42+1BVBT72E`WedBFY zWqI$vgD7|CViBNqQY$a`{wFA9FrbFb9TmU!eQKQeWzDJAsj5i> znDI4-gt<=4Vk;eoku5haI(>kSL2V?y;>r1`bT_bB>C&D~69lDB_wSJ0h*$5Y{Wy%v zkEjrNxg)P&1ZY`#hb8EI&GL5U|3A%e{O@H1sG{qv6pGIddSf_HT0;M0tn2UaKpbY{S@nXR#yom={=h?L z{rH}`bmV_d2K4uZ?~GK~NK7W3odOOK`k%`IV`CS-6hQLriaE$ZBxKi*0UHOSiN3f8 zL$ve49e@hv&i`)ImRM*jif(mNn&WKdK(*UkxVxOq>n*^+uLF<&b~{v&I_hgo%b7Hx z>VjGvM)JZTQAPfeG31+f(^Qu*s3UUqI5X$Jzr4*`+}&T0bS7U|5^i0_zm)$H9DZ`V z8cB^uiMNmjsgFcnpeKi1g?Jcwr_?E%rJF9IoF`N5FmHd8{hP--Xw!zXeVf1W0nmcK zZXZyhTy<8Dm5&~0r%;k~9pt(Vi7$k|MmX5(=BSfG^prHhuCZC*lNH{)qGdNqjJJ73 zdxf?3Q;6ej@c!ECv(R$YIN0h1&r=U5@kZ^7GvPC?wQ*$@o8tx|&7%PnM5hQ12)$z3qrQjM zAe56r$~9Z(tt3q>TwK26&~IwSSb2G`1kCdLXVf7#qG>)SaG;JY|CB&Ryj}=31}}8m z)eG2u0iFJ!xGeMq=2+hd?ZVCm2Kk(jO&n}_*adZc_0}*L6Wx5_&lVG*0J4q(bmhF( z9#sNYhsgA{l--FK`xu&g-J&(!(411L*)?fwA;v3}Aq`a5z8pDSQvLx@s%4ZYB+KD$ zm?{)H@HvSiUHQV4ns+WoX5q7AG87EPw0h`#56F*B56Cj-8+vJcx7|;6UV0emK8466 z=Bs}vH2PoxrBS=1@lx6xtJaT3l^m7i+8W>@fZ`0kr z2^ODFy@`{vw7(MQ<)=Qfe_@lb)i3elT{ygCRP^OcGnkAV@ z6s+0CTt&wuCyJEKHuTSi2D@%=?Tv)*HyxujO{81f?Ip9i*da`6*S}qo}BoL@>{pZb2Ty?LltA>Shf zLVF$0kR*vmn9ML%3ZX?9y;0P3LRV7|*?gL0(VQC_Odg8^m&|8=EKj9haoyCW)p0_1 z-$H)uvdV0-H$Jg3sHh4gzg>a}{7%tbsU}W;oA@;*4kmIjDFjB-6`FnEQ6c}3_$}=f zWOtOn_RK=Alf=<%f9lNKJl7ywbBXHQBlvDqXyc)MzEyxxJMXj-elIY|{}8+16yhJb zpPK7)5EL7~8Y>ARh#4ay{PA}#fG$v)h9ew0(Tv&+RgHrM=5s4aEJ0u0puX|splJ&` zX!N9a`d9mK{AbNNJ$K1ePe0L>_`w#SM+0Wb(sQyVHe+o3r%N)Sc)*& zN{7~$Tte|ei}uw004jc`rNsm`1v#YF43il0XDlv98lRp{WbO~P9;G??`a0<5fJ`iG zYE-#yFlyUvuzWZ^or3JiS`BRvuOCdgL>!Bx1)z!Sb~WQLd>1oPP~q`@_gl3EA85MH zPA^W)GP!PVi)3iitw<@8jK;_9xhp=^mXzo_>NcJ}?Qay=M}mUiPn0_Ycw3p=-#IO+ zRiI~EMW?E&`q2i+cC3w5$|rKCH+3nm`e(#1$7;ul&>uXb^QOa)yklE^A3jzNJ6OJfwSTi@$0zDx`HV{07Yj0p%w#_MvAVZ#TPs zEjFwLCS}H|icf%+p{MHxW!oDfQfm~7u!(-w8;tLx>l>?jH)VY3fJr|tCAv$d=uz1q z3PoUkXxfD*3#7%5`x8HwAVpSM9`3wZKiu+I{YjAWn$7mHk{I_V#t%lk^m8qh1DWUD z_d+lSRA#M}q}5()siTh*?-9jcah+@G?qq@XjTIXnql?Rb%+BV)c>7 z7@3%!0L7I=Ci60rhHrt}&7Hsho1eCuyQ4<#r3^6+mTmixm?7%h@qRsA<};S!#YaD@ zf)5VqRdO$-Yd6+An@-F$)^!fpcO5i#PL$R4qzf5Zv%=3mXD0AW4kJy<$L8Tr5Sfd!ohgVX79!f z!~QY$3fTeYM`T>iG;1{7t1k-+w*mu+7@5FDDq==aVYEru32ya~y1z{0#?sfGA`!hRBfdyVv6Z9Aw`QZh}DnMEjlVC@j`{bYdN zZ-?))dR&IwUo_3q{U=|&ujgH8_pL<~p_(--#cdcoKShgWjwYML>_$U0@` z`y*O*Fj9~?x&g9I%>q3Yl5m~*d3wCxbh6cWiYWjsbAgVpw=Kl1Pn5^&T1uzxqQ;7Y z0=Rbq_O%dOL4#*uQR($#K15HwMP)NL?K<47N6%z9q>8!N=WG&m9PZ8EaR>H-= zWmKoC)(1{+nX6JmY0tGLV$9+&CHp3Vzri-nOo98Jt|QQU4hEi*>7M$DVFX41@yg+M z(lxCdA#Dnj_E!RT&rMGfvt~qd~IWP&p1L<_K`E&f?@oqGh+ac3Bv|&*RGX9 zB}bn8c)B9>M^lem;XGvI(UOdH`#Ut{Eo8)=O~lpSw#Xg|l=s7vt{rTTD?+wKD{ z{+;emY9_Z${eJJ`N@k|x=!BSofnLAez^YJ;Hy?n3C7&|ZJk6!M5Ev~>nZVtRD}OV zufSfHeO~Y9dDb|4I4~KW;n0Zstq_2L1_r=<+Ij{rpx=!CIeB{ z*1!}AyLQ>y2zI^SqcAgEAy)Z!`s8K#=CB5(r_s5;Dq!ByH|^HiKilZr5OO9BI++}_ zPkC)<9-Q#?#h1A`W|oj6R+gZnOMt|3@{b=Ij&-l!jBF#$W(rl3DQ=m6um65WTmT{OOS#^$j{nP`^40S=}oSH$5Q?ESc0jS^6Ay%j{_fx(g)=gd~HiFJ?NU&?cV+j2dXOCbCr=*RT*VqNRmgNbr;J5l;e?(Xgi zH#eICvPjRZ4!RyUGYuihO3AZb1_WB(w>!NPy8UJp;z%wj8W~EK&6IsdyJdN&Yc+<$ z;rMSXDdYRLh8e0UC^w4o&?zzv5__#cJ+*A4L$rUrP*2(w4wBSg@fHaV3`{-8nV<90 z2$)#|*XHNAiJ;@RMpg)aXI$qhWMY&(u^xMNzB$|L#W)FB z-JNfcFvot69CUr+Y0R!!Yb&(ae!s~*j0RuM)#UO0dze*8XPB((WMZRnscn_I_&mo; zcu$iZwL}#&L8@bKEy3k^a&~q7OIW#R#Pkbqs#P3+| zL&`qF45mv14`$<+IE{vEh0jm1mkHdBDE@3^>OOXS>>ex$>$eX09K$u-um3&EK(I7! z-Fpn>T`}T0$@3ma+J*{#x|o~mP1Jn&@BtIba;m^exVgQ(4xDWj-~9-%fNVT~;`-4n zpz(QNg->?u|Ls!gv;1nyG6N$bDYMZ4z(wS7O4E@4r`HF=LYCu>KOu=+L|3nmz3+uXuB zj-i%!H3a6<(EZ(t`zXKFCRRd-X+gVXb_6*KdNCR_US&6^pO}X~9XvOIVhy2r!}c)R z+&(StdvZ{`NkKt@1~+QF)EM_OlpI)#0^MxMm|~eDd{!#m><57-mD6}iP$}yZ=i5x6 zk9VE?1>2?Jn~0YCc1Pb|mOFV85Y+emIroO3o>N>8qa@ddxMap}=sCz;_X&*{s*KW5 z5AdoSU9>C5F10%`Z4HXL!o8@3x5@xhS^F(rC=t20yL;Al@OB`=cROQiuDO`vpA5Oc z{NF}|+o&4E{A#~JrJPL;I(>Y&HH-90yaQ$u}xtV7hz(5Z5Zy7T{&@z_OpG1x+ zDu>(i6Z81Zc{EMyQ8=?)04!P_rv^RkgqHlii0`%z9>{*jb#KD6VWfD1D*&^UkkKOf zWKVJ4AAc|g$rxdwU+&i` z9@8)f1s4gh$RXRf%CW!tHO8Z;dJ2&~dEvg#^R(GT#te$!^#25Gm%m@GAb5Fr<>8<6 zT_#>S1*VwEWc$TXnxGTYGtx7-0veNE_ATm%n<=%F?2sr$-4bzN-olBc8As zDS_oOT%+$cy8IkhE|xinm4NO`-~jFh9Q%#e{|&R0R{9eVX*szu0D7@B&|Ml_w@ygT zHuU@?MXSSK%DJccxbm)gKyVYF2-y)3^x(0%oPX|Oo|Bsyk9gjP#ysuD1nYb_k0`SN z{a9Wf>|>ls2S%3QXY<29%i+EN3&jTAY= zBUS#g!-()bqb?+7@k!|uQ|_A&o7*AUI?8ecKP!O)seWU^&lExRZeL!DJ{X)hMLdDi zphz)4-k^9QG93M^5PqZGAWx#4MD7Up2y`|U3{KIFf0B!;@76S*P0-aPepofB1v*_I zuK~o(JYC{i&yvrBbzA2E$c!3Dmq7%d)E*oh=ugxjEaJ>`Z$2z8L#8h31@G<@h2R0z zcj_=ZeacDMReSf4ce{E|yW7+t$`P)M%pWd&rPvP!R4}eEw)@Xak(hQQSeF zN=T3>iILg<{Dt{pOQ4QUhV1Zes#V@cz_h)+yYg>G1$F{+ zYw?3D=yN>!#u?I12QyZsm(XtlK^BTkRIZQusK*uYYrbFCSVZVH*JzDX4$L0!TnEk= z;%E5h&)G<2S*(OfWu8BoiWNiXM;ofqanu!hMky)HpQey^&Tz#O zaq|;%)vZ|1ftz1-ZrVuS+~q&oAb}GiH#@b`8Q^q^TY`s?J)-n53Z* z;1N_Z*djlEemoZ3ZQ2b!U!K!AK@JCeDqe2Yr_AfA?6ElA!3Bf0lSTr*HLu5Mf1m-Z zvPK@d^9WdJ?R7RQ04+8@TPgZ5ty|#=k>0vBV^IghJj zR@drWRMfic0 zE*-cX_Jj+Ciqw118jx#qm|(^zfJIy9f&6jLecd)3pyE&UsNlVd%D1Q{;XNs%xf1MFP@a5j43y?~%Ha0)TZP%TpbyJ2q=u=D6M`MCyM^ce_)MrMz zp6S3LnM$LDQE=N}!4SmIs*W}m+Z%yfJsHHz8jGB*&5@oBr(cuU9gpz0Un1`X7+wWJ0ai_LdEdb5N{nf0-bl7vJlC+Mey7EmD<;RQ@=sxetzON^Xi-tIK``ea~iXk`VmeI}J zq7N&JjH5~6|Cr1H!j}FPc4E+M{p*APcDg)cy|=R*b2Ryu+VrtlMj@P`Jzw|TNRF|? zWR1ogu>7>;bv}<6!&pi*!~4{k(99XCfmtpBf+UuUU}YHBx?J4n{03E1vt$5Q9Ksh4 z;{l7~Tee6p8bmBfo0V3RJTxHYQ4P-@GjWg_{^ebidA7Qolb4XE;klS$Mo zG%L&W&~Z;)3gT@>P{OF{gg6?wT?BnnxT3-Rt4PJt#A5BvwH%P6asG+k@=%=4b<*5f*a4MBJkOBItibmBD2?uIL7-!~Xf z=y_85O!;2?(>aIXS}B-LQEOjNySgr4&Pu$>2{Kj%l2`f0Q{-5N69tya;)4B8{PtKc zI)0E5zm5fJ5OI8k+&4DgJI*q`$Kb59?cB5>aVE@dIP2~_o77RxiK2VTH+XpT3=DW| zYxo*;CyF#Pl*G3@mCBDkVTn@-g2Us-=|T^ClQM(gIw>KTjUcHiy7Td#&ffvPst- zpYLJyC(gH5cTLZ!iF@v!G+LwUX&AS9uOe>g`076{ija7mO>G$)AsDVfv=~d4&kk0t zu@uZ(BJt33iQg9J;&U#eB6z78mG$qw^e>BDiQ%&oZ7auXTcY#~{^Sp-wOBuq=)~2%o73Pv9PlmL1}Lq-aP7(|8&4$xwBOwfTd5!ixV1ne<(x5}b!EZ23Ct4+y)#iF^Xq z5WVUwBU6<7%$oh^8WyE1Ky%}PZX6$meIMAfPOLOzqTw~xN=H}NaGKM9n!7q(aiSZ? zoSIeo>&Ul#_3T-&smkn3CTaz55}*Gx%7g3Y{B_iX+oLp9(C_bFZ#_}{9a;#!7+AF3 zHoDeD!&B=# zd_Q?goRv6uTe-n=j~_t{dKBj;s_<{V;I2tP$J0*<2u#fWvK5Qff}T3A!TnzoFO&+I z#;Jr^lT{3gQjxK)TAlDn#r3Pz^MY|1yz8Z)ahfcPBBy~77rR!cz5bkM^N3N_7dqWk zl%nMfOkggAP^P0iQ!z0s5#C?KMO)H&A7NW7x1pm?PEbGkH;Dgow)+)ywjuoyBw;bn zVsHy02nZ=XQc^TZDKs&5B$3pKIk9O)Y516hoUG2_WH|4sE8nUCF|Edg2#*xD3R*^4CMzPpz$3!_M+` z9U2BWGq2$FcoQe9 zGuCQVi9c-ac(|1>F{Ye-Tt7TKr%zP%7M}AXAlf1@CB(})VFkKAafRR~+u!^reChNw z^)jaPUa@@GL1L(XWF-a6y`?QgKM^64xi%1#lh3n%*|1?reVYd%o+pg-pvfk}16`;J`#()iuK8oE8#+#yy4p$PHu*uW7#wDl-nmoxksRb1Hi zq;$Kl6CpxZ^wv1`+cxo6m-U2AXQ{J>0D5zf=|s3Ivle9i1w5ahQt<~mXCCKU?~~rl z@(v(^+Y!HV1pf63f>TxuM69O)=ku&w1i8kVEKA6JwL9*gtL;Bio`$>XbV?i52KXa} z)KfoNZx>EYHem#3S|D_82{TlRL-l-sHK982k4~y8;FVpg*W12LvzE-0uKJp39>~l; z0`d*vi+$N%I=vt+n>cqp7wR?LX4M%iFBm;}EE|^g*|1{hy?U;3RfR1$7jfV7vmhaM zo47q<0=Kf>8;>{jq!L=|aL#EY3Y1O*i_wjdcpPyktd(=O*i_1wY}+O0Nw^?TMXi?z zOrHLA!aLIzXU8eYr6Gr-HdI^+nla;S&SO)bI-9I~WV69C?B7v*?`dlS5@Sasof{$HD(rRKC=FHV9laj>iTk zZ?2Q1SxC$A5E^(@NZ8_D|F<8r zSuzv7ep6aVUcyg}eDx|e*~Unff=vybXq_5+pdtYq2$pNWms;*(D-Dwxi|>a5_&+<9 ziME#?b~Y(~W8nq%$=_Pp5S}a@IO}h?L{NYF*SO01#w~)$noMmS?gT{OPh3fuKgt%Z z!`KKnB@(ES6&aa)K<;DSDmB&(PiS9gvTF;NMi==WODnU44CxytxUvjD4&BR8_4-c; zu}tEP5Lx3JHg)qaXhVGbL$SPHOrRO_y62ek=;owaFF&fgThVPw zySPlxb9>!;wBP_(2jnHz!kx_B;FMqjp@RrAp$_o+5(gbA@15Fdm(Li8S(ms27!+~X zTeTlHr&9X?_D`@K>OM^K$9ecWeCnH#`*S*QqTWu|yBP(O_w;V%u7H8Sq9Rei&J^XC z!0OD2_T^mn?c9i1E)z=b0OuK>=*`L}JIK4@9B`-f3mZBks&E;~yT3z`+X|i_XKH!2dH*Dktl1W$KIZktwxGnsnZqovn;`p+7|E>C6`79Q+ zeIJKTnYOMdvL5a&%Cz3baDwoXG}NOy!;*^0R&3|a!yARB1MVz5h_t>$;5YcUfC(;b z!<{N983up8JHJ(A&QkVMJ>xZn9Edq7+!aj!U`l&Xhx)mXdZa<|I8s{nuqnx>f7U=h zkIc?$b`prZK{?0uPj2N_Q;ked#)K54;OGGqFjQM^z@D^opn)yh=+LbpW$CBeL z2WC0Rz}>I%D@zS2+~_vsf7*SSU{3SmJ9ilr-6Xh^VR!X1;7JWZ&$gzB_N5A=g?;@s zcoavgNf1J1*snn);`eH}QIN(s-K}rsk%~a_y2U&IAY1-q;Zq|@4Ojn%F2p24X`f)x z>u+(`xJhiqrxGH9pTWu(S)y2NbJ$pOrh45BZ>G|Y$*Xvp?rX;l%a#1t8p&>$eL5#3 zJMiRAH%!8%7@)ug8t;UewCQK%lGodEmW2&#$?QaCG5p4vUiddal8e#8Xr=({j;twEE(u@;;o$2T#IVP2w zl_fe_Bbq@IL=j$010D1d2IsH8P}Q^?oZ%c8NClOjlrOr*8Vh^-BP1KU?GDlwmB z-k0_3PF9Z@u)xw@9WFHGW#8_k>Pt|3tf6lE5b}{MRRdZrp|?B|x&#kJ&6QpO8sVr< zC0u0wypdrvq9qkS6Bf)e6!E!P&BwQLZo^scY)5PXphtTb(Vk#+sR6@EJZRC}=bMkF zGI214E_z}UBss6D=T;8&tLNS|MCsnrsyi&0N{!P0{u!X7mx4BuIx|X}D!gu3Z_aT1 zMiGI(;%4N}TRit2BfsP?s7J!Nf47}+YPG#m_c7TedEm2YQ|0QLX#K8@?MJC}Dl2ThteK8;-NxU)R5#Jtl=%LP8$i7q(d%c-d&w15ytm_d zq&?&0-4r{YJKj>OO0GSunR4`k{A|`u+xTAQ%D$1l{Dq-G8(N?+%-Jk`O`H8mXTpB1juijcC2k~+^@QwZ^|O%&AfBJ@+$rnxVL|6wmh&0 z)CVb*81`IMTXWvj%rb@luT!LEMZC-X_Aduj!`JOBjO2UuyZ8;;S4YJg*Gy_dZp>PC zUuT-ER?#{C%fcs^8P*9wW3HfYw#xOYMRx7)6~pgpoNP-Eij&(tbC&*;D_q>mvmD#!-3z>o*2WP$(q-1%$Tydf}6|H>+h63 zE9bA1mz!DSF<;035&!+yhrjwh|GQiLW%;YT7fXRX>qKbmK9G66`RnG%uV#CGQ*@br zCPq>0@0IG=&A;rH+z+|G;`|5adHwveTQ@24Db~I@e>41UM*(m`!xmcLH{>s0zfsbu zjZw_C#h_w()U#*H^6-8ifdj(4~|I^ac zri(aDsNQdLE%;yfzDk{^yXO4+9`kq09<$)DuQ$}~b-30hzH`QGVB4P`7OH>Z+BV!1 zQBn1DoaB8_^?dJ_${BX{a&_su&C-kKyj%hvq)>)=Dq4Ek-P+~n+$=MupFZ>Zl=Nhi z8r_p+=QrL3PT1T_du@2toT1@$12ou+9LMI?mlZ`Sb0`vhsNLkI&WbwNJ2Re|vcQ{W7_^Gnbu@pC1{p`)VGrmJEfK zzYPC;w>kfs%O$gUkHs~gdD5@S>z?bXTYcWsxlEejKq<6E$Z*c`wqyObj}Kzf|3+sO z?7JAf`^EWve3OB>V|72wfxrb;a=;^&TVed&d$-M!eh6#y15aRn4GSqC8`j>Uo!fCI8)796^(|4RzU0)rjfASX71IPiuakd0Hj0yMOM zYV-u46$Fqi-T`$yg|4H8@vXb#znB8u2nGeY(DoXTM{?Ma>bhV5*Lt5**b_csGRSUE LS3j3^P6L*gZ-JD%~A|QUa3FB_YzGNP{C1BOyaeBMkz|(5WCH-5^MpbPUos!T^Fp zNH?5~zP|7G{m!}0ALs8g*X1w_^E}Vq_p|qXuY0XEhHGmo6XMa~VPIeosytBC#lXN> z1OG6t;evPWN|0RxKdv~-t30{}9^TiUg@NB`U6c%59=@=0afdirVpu!8u(#xOws5kv zba1wL;j)R{ECW7t=kh}ePL>cC=nID%kD&IJ7>{gRZ}5xVxaVqpLy%uk7`&Dcx+}pi zazk6=hJw<=M|3U`^%xj8FjN%pJ@QCfoBIK$*RTI^w5ft0BmYuCf0xzLh<9&&W3{oN ze7S2-agDiwZ+}jGt*gIrPS?FJmO^DDt~Ou3_2%0L&ihL2+ME@tJOx}4ywc1#k9cmp zeJO~kq}{x;=sb3DrlDXPMIwKVCFtKL-}~cc^tZ~X|M#0_3F}sDW&g*URtXRz*#nyY zjs>s6%%xF3nEd}e-S0o%`ah2RcAabz|9@OFh-4Gv`daY+JSfcF|H}UV8bgPq9G2Jr zI%t9MMnL2LJm}gM(VG^t|2dl=kE;)!@cpk5e#ZX44F~i8AI{$p{l7-1!#I01^iRO~ zDt$LFe@8m3R+-SbEt8vX%tCf|NI1kjnC^te>{QAZXJXX^sU+=oEW6{XjY3JGb|m&p zcKJ@6DSqGA7YYB|;AS#at~e}9M^+x+hvEyL_VezccYkp9cq4AUto;btE{rAdIHVc! z-upPQnz(T2m2akFC-WKi1xkX`)#-*2x_lz~;$%K}2(3s{s@b!Bmn0_Uf_e{vcezx; zMAiSSnJ{w+9Es@gwlGcxk}FIwhOAMWt#zDzy7scNHG+htCrMNxkgRIYz-*819BLJG z5+}j6KupLmM|9h6_2NST0{p(7C%F?UQ5+o?i*c{SqOYCF=&X>3U>bF&CCsMgMlC&q zZ$?7W)O(Sm2IMJX5n);WPlh=NykFg*^iLB#EffW}mH^ zf3fEJ9r$lj|15vsp3&a#oOoOH_7YKaQPX#PN(1E^AL&VL6y0H#9XmsfoqbsSFit0$ zX#LbFqpd^u)6?sSs?iBEn%(f5XS3%cTemiuUtp5sU^Z$Bok{KgYSi2|djxJ}BnU*8$dWb2iV7BGKzO}Wyf%n-t9`DE&jgNSm8*uD3H z8s&m&9|hHF47y;Z1PH}$j+mRjr`RF(cZ#i#rhRWk;JT#VEifLM^4|A<+;~ideX?ju zr}0s+eYE;OM|IvxNX!A3v2mQM(Xu^*_u_$pEY%?~HhMVERQ8GAj`$2WF&H;OPWJj* z-@_e~`Xka1$U+@aFYM5I2pz8~|C19BIzc?;?U@<4t_LC+*d&@#Mnk;DVMTQ>&~nBg zIyk2&^Hk#X1qUgvOw&XzLMjVZrqx)l&{(gOUE|JLHUWvl-6zt#LkJGx)Eao0Z2I)6 zRY3Tv`1TT>1A7cq!awLwc3QCfCwH7aWh1PP$AnDg=^?GH_gG?@PcOtKjTpy$GLLIW zeWHI13B0Zf6cvi>LpqEYGu;`k9M0Xj zYFsFFJaf@^d|)16U(f_M_!I=Ck}4gu4>E+4GC2u`b%MqvlAj9@5Fk3L7x2L19y-QPf zNAy!rDLp&PmbY)jux_9EKT6(5Nv!wGa=|Kem)5iB^+ZhD+fWmc=ipww?-z@Ewn zG2GDdmHC~V7BVCMW+z7@p#(&3q+7;g@lx}{qk``l`k&7i&hPUTLVUgM`+9HpKxQ%^ zw&~29uE>)D+9-W{_v7{J*&%$r519GZ_}0^9JT(}be^jB)w2ia3>@T*^2dMnrD^H78 zg<}l7b}6}$KKD>ht~MiYMrTonO8i`k5gZNmv~?CSQmEClULQ#;k~*ZB? zh!%c(9*#hG54?vm+Su<@j=wZJiJyj~TW7i+B{rZR(#EgO(|3=yEBqk6UbYw}^b&j; zI?ZYM%kw<>p~k@0vmRK?e&xdz;VRj^J-EGv*$aXfiMl-pxA1U18t+ukX^3Qa_wDy$ zOAHw48v5<0A~M@$iM`OjmbKXCUQiIM97r(O6<2J}Az!*}3e{xFzH@z~5tCeW@{}dDj=;Y*GYLP4Sw^DEgz@*-=E=}Z|%sYPpq@W z$rSE!I^(849*HrnLdBq8H-h5ReS6=$!RfqvU&M3n3|kdKGfeEsuwGKxxcz4R2eH_s zS?fW4={4lt{Bml_73ZA2Qa@ijeu7`erXcwldhN0eZTbw3S6=m4U$)U#T?Q- z`-O)mE2%!LoOB4`WB_SaxS&Cp$Q{bCMz%N25sk3_YUp=LnG4lTZhXKldk^}YP81$$ zpl0(2===k34wVpe#gw^AZ}>C>D_!E38ZrgN-7(ww&8IYTKo+%LB8_TibOB+pz#wSv zfK|rjEgRN3r9QP}|CsyuXWXz(w(B-XeL~<^Zzx&2Q`nGQq!2-6w}OLMbOS;*GsUqf znQUz&M_jp^)@QbM`R*$7`Q8}oDYgE^*X%s=Pemn6$b~#kJ=t%yswYfJFmir+*P&U7e4= zvaTN_GId_FT;aQsa22c*heDOc`o7k2yawACv1w%1gagT;4+a)8TW8LyRIGL5r-kZ6 z=Vy2?XHCwp8Yy1?jRnlf|E)k(xU5n3mihfWZ+obcKRL#?9E{4HRt<~1)a(|JEQ|@u z3Tv8R!3)&ECr4EDEcf-BHuWf99Q&1*dDA1Ry0A{*Ck!&y4Tg9*cvJG%XL{e=Jd?LP z4X1n#S3QT2NGE(Hs?;d{+QW3_NmEr={_8;bkqL1jvfDvkn{Um^Bc&aMMv?n`{U)wu zP-6Z7*ur5kB4V{BWv$p}==_-ccy2Jf>>EE}??dO=^F=?% z(O@2rS);~FPa+P~Qv-I1@hgFtXC>XO`?rUfPvfgY%`3LfM1{{9M+{J6Ae3d5$6?!J zJcvkYPQBcb)mz=ECk%gTxa`fpA)K$mZLj*WsQS!FiQS57*eIqtR@{>GVR1N$ruc4f z_(hA8NgO1)Mt}f&2F8_;m)z_f*;nfu2{oh%5h%!&9Vo-g)DaW+hJ1E2Bkd;Dp&(Pn zlpq?01&w!yR|=}fK_E$P+YdIWv^iqWCf;dw9m9%7U~&{}F&{$x6m``y@6g!N(1Zj- zB~(_5G{2QQHqGw6-WRusfe*bV^iXF|?wH3Y&L$^kZ};Q2d`c%00!?okUZ+nC+HY+^ z<=6LZOh0sc%nYlm>}o;hVAds9L2pxx7$IyK^|LaIa%I2$C>Ql|_a7I4d`H;d&0n|I zE_EP3{I=qXaQm_x+0SDJBFbTMu|llQYjR7UN2rNHfkA;`b&-e+A>S=9&@Sz{j;llR zdi>R|#yiSh^ke%Xzr50`zK$6i>FNK*%%2f|H)%Z0<7b)fpLL z9OsX(9-76Z$A?x$@2}SU?C`xJM?io#pm~hZ zmTzu?bx;HIq@*nE5^YYcH{5YOi@gS`taU#-QH|aa?`}jZ$Tw>t(F-}~Cih_az9QmihlK{pG`hjN z=9#Wa7P_RmW^sb0ViN_SGl&L7yM4Bke$ru?GSwvhOb&}cV{@I+=!$zV|N!u#%2X5O`4Q;pFJv**oXSQsT<6^roSWFRxKm*1X zBPes1`GlMy$>GU36`g`QS0sEtX}pz(NYeH`+)*DieJ6ZZt42E)iGICtBP{#Pz2;OY z(O=?Z00l>Y z*S6@G&P+WZG=47x6C0TatJ89|4PLh<&UcW>Gn;#lkb3&v_4;U;@1*JjTHFxmd>yUe z(bM`QhYFI_PI_jYWaG90D*+99#%7n5iC<36X}5|dXz-5bsEmi0%gzt!A2Xk)Xa5wD z&>UwThP_T5(8-#osaXh!b zA}kop8mW)8IPc|U&!vVwaxY^zFzFtC;nClIwkn33Q#QwwEdO%XXWZyvTAHQPJlRsR z(we%}X%xr7-N(=uoWbjL&+siPf}JY-*C#_@aaBGMZEwWzMiSUGV87F8&_57!!IIBS zUG`+JoKoL>FLz|WU1mRp|JPQ%#`6YxJE>3I03KZ8*Xa$3bSqxNHIZwv{I0KcadV*% zaUxB2W$5z+PuEnpGjI!sPUDRqSy6piR#O>*QyXUxbEC5{N@tex>j#z*SM8)5hxRM4 zGT4=lN)(K4)*vF#D`K}aPZqf?#9-fbIkiAx{^05ohTNcB(Ti!(I0eO z^&57COmWhE-}7QvD=9yoqn2to5C^a{$8d&TGP<>6SL({c5{-onp7k%Q!<808w z?u%qp?C~UfgI;X7yo+ChXXd@s-63$_${d$KP9CupjeWkq<561QHK_O9qk9L*Y0Y8l%bMvJ65{tq8}B`l?gl zg017Q69z)qKIfXSc^)moZ#z@hZH;8sosTdl zCna|~F;H+!@E?{-f7klVXF4PY_=(q0CdP{6Uuxq_J}P;JY6a&blQjxWQk}aCf{8Eu z+WJQYqkiu9HCo0>2OdMcP&R}ebP9&gw%L|TXWIs-Ru7CUGHbhfo^h$GKjhReOPAW3 zbo8^qx&)$$h0RztyiAGQ#tqIkHl@y% zhl*s+Bf45QY!q@R64D{T=$FIJ_Y&YWE^(&Tz zjKF(3itZ#8Br~_nvw^(bhIO6%&1JlPle*r~YRP`{`upXA3VE7UrLN1ATNt{^Sx&_s&}@TS_mS1$nsiBM7k z+Zp=z`n~d(%Ji5a_|>3d(0qPq+jOz*SGKiCUtfG|1BV!7aO)+dG$_7&9&PgeHN@!f zj48c~rMl5|dJlBg$`tHN=zz@;u}Mq!b+2(|-*B{*V`b)>o+V{SqizHz!HO3nk-Lb; z@w&MIh=s&nyKvD&_IJrtJ^^v}fbe8CfR~}am=U(30Pg6PIv|SH=MN+>6kGir3I_qs z9@@B|L^zEyEypi@Hv1tFHAnW2f|J~P2xa3Jy4QVhxKVl$Rjq730v)@La~J|$s58u^ zy5oFsaB)>s<`^44vGgr>f_6x7w6L|C^U+lk819pV6{0Qz*>l+ic!CAR%n0T?h|Dbl7X4h-IQc1Zb(3-fd|{1&E?G)BLYjPT2CpE$ zGNoAkDf|!TyyfthB>t>t zWP9951Zb1BBB^J7JFEE>W;s61m7QeqQ*6133CYbafxT;FpW3d3KtX?PRPD1bN_|Z|2z4JFMwDkpMbWqtcU6+o}nZ~E!p%jb8bJ;_Lv=lwOa@7ue>&-U`h+$OGjzo1Bx zNapG6B@l|#H>h_DUCpsneYnGceKJP~a%B108m97z@u3;Vs=Mq80?JfTdmgksCY9)L z7Se0?rb=31%$9nR8%KiZ`mqn))rKPJ^APKU^WJ5+*O=RQcos&md@}BoeDZ2%pUKP3 zT9b?K=5-z@?1*?So*B02-9A(6Z~h-BH5K`>3xo;sO%W+cwq~y}XzXh)N!7Ee3w_RwLe1~G~IWrCU zIpJG+_eVWH2)YL}_#KKSY@Q}e;r9XTB*06bLs3q&HO>3{1Wfca;>E?;1!Q=W?C?xy zhO{~J(Jw9g(ox&m@HR113}O2@G7r!gi-QhurhxpA3Fb0uA*EmBxYVdsw|~A_cYMwP zAbN+BAWiOdIWd~y&AmxeANdmg*6DV>DWtb5#kfugE5X$&`m5k9t5xX3rqI*@2bKaoHxqaJ zVN${ldB~oM(o6nppciA>8Bm@nzNm2u9k_Q8ePdyF(I``J!`arr*3N zIF&m?0P;r+@s0YcXS|4yZq;UwPR$k;?>mj3q2Lzk&z(v`5xlkbR)0;@8$ibS)+&%(nXF0QjaTt%bvJgE z7@!OE-Bk30VTh($dv4*WO>8>NcQM|sjO^HgNzxX>cN~S3j?F}&O7OlBD8o}Dk?2%5 zIG=7xT4%-Fg!yp%2WDwGnk$EmOaPey7Kj?~cZ6Sye<%jTL|eq^)=f!~3#pmZZL#Rf z9#Y@=b{e+5%cIGD01w`~Nq1_N_;Va@W{A1G`RMiXJQL(Q2RVPPQn}zjFg>&5>hg=E zv<5lQ&hU3f_9EX_9d{GlOxGHg0MK?=Y|soX=OL^yi4PTS}2@MW$PJ*Cn}dwDF?q z`AN{~q52Hi#DFMzyCm6++Ac=E;npO`GaJrFoJ{0(f3@DvdZ|q2;OhP30YjHCaUm%E zd{QUNLu(~cS*C?#ulGuZsCrH6&Smt!og}Sxsx7Ko614lyd$Ik-yS@YeuGB;QvcnRc&?ZziULNcZk2n^O)z z=7w0vZkV6}z$I>uLgIp-$a0}<-7obxQ{DVWVCtZ+6pmhTmt1=xWqCHDzg8^UimnMm zT?=mOS>}42YfyhKGa++GT{3%~)Z5SMz@Ev&Unt(laU#oU>J`sV^WAkCfa=PlMPoO8 z<`LTo_`W<|ntF84ks4@qIv3(o)dx&l+=P1S1}U{;UVPAex$(o<+w?AIh+ozXEyGWw znlHrBNI#$1cJlyB@$3c>a3Nmm7ru(n=TtMWk+(s6fBLbczxgyuLhB^)!>h)y)IKuw zHO5dI#`lk=W9BN#(;055#j;=6q;D95X7JQ3wQHWG_?X2kPlW=vXOQxZ12Z640ogLo zNvUU}j_4RzhE?Ja2%T}4Go25+7`u)$`(5wofORZv`|DaptCzfnOeN(8zi+8fS4VWZ z-Cm3kU*MF7u)S}B0Is$Dfv?u|s!g2C&3pRoO74^}h6UG@)mNV-ABkq_%y~vn*pmkL zuvUgZbPM_&_X3g5XW6n9-Ukn@5)xp7FGq=ZR)4(RIDak4>}!YNK*PV{p6?)Sfu52E zL41frJ{vx`KeLEO4)z!efVAqCRz#wyn6F(3+KuT3S00u|cb`6#*c9m6d_bdVU?U8p z7%5oa*-e$*Sv&s$lC?XL`(tJNjVw5onh}b5(#Os3Cq;6kk%H(Ly_WG?qOVaHh z{y405ke?uKDJu>im-A^=ba=Ku|1X+Dra7y~CnO}eED#!!P!=5;G9?z!Kh(N*4F0@H zf*Bnj5sN6GDZZlTFxShilL5_IlgafK?1eOZ}_V?e^xBvKx7}Qla z*)&T#rLAWXR$(~%I&xj2ryW+#NG*hZ7ngix7g~Da;o;u$NuNp&P#^7cca8QGSXiF^ z^#a@uo#7rUoB`!3D1eyGuA9x6Q^05>4tjaP#wqewR zGHs07iiW>xb*!~kePtJHX$>UHC(qBxI(rNSk66zfQu#HHqe1~7-3fF24)AH^_V?07 zHOlQA6{j37JaYIY^P;K#HZv*9AURh`9LSLYsRK__?t3a@v?mAZo74E5hME-)9-~SH z)X6V-pZY6-D64$mRdZm6>Kq_Nw`l8~5#E&rnVR0ClPx z89}o(FpfCcogN@Zr5e@s2iv-TazWo*zBAX-L-F8;g4o-^n9%X(YZ9;1`+gl{vLgJ_ zaRSVXie513z1$wieCfOC{Fv!{|HLDH245udnE>mC=2xm4Z zdYjd>01DAW-$r%2`B{+^Sn%_NdVgvgYg0-n`PPaS7N0cBnJHr9by)}t$b59{o^n^c z53I;9ixJNv$rsgBo|toL#ey&b7}2rI3VN%!JgYB>96gJw25z`Hui^K0iB02$rS?yx zUj^C^r{$kA>-M%(Fr=v`*u7U|*O%i$FTRY?c>VcTM~ukY!RCP%!v^4#_mk$TH$Sf1 z3$7fDemiGB(hbfjbgu*9vq~({E>h1zxp3Zw=2>v^k5b9;iB2;&7_5ps9}~RxfEv>` zHAG0AIX z56dH9ZxRh;E8KQwtT@8TU6Sh$S3MC`%CW_M_kNKA@2n=f}#FUI+_s7Jf7adxHfIv5XjRcyqoFpFddSM zs`qCeLl^VU?&%sbBMR8!_l7on27BH8P>xZ zCm$j?FI+7C7=JE4|69KY%A%(aJcWr40G{sO@$!QJLU_$y3a~*OzofJHuh8(y*(N>S zE>mZaOuVCt&Z=hr1@HAjqM7)lyh7#|s_}v$(-ggMlRCGnN~5?OseA~jS&(DM4}IDK zv?zkJgKY!L{GRH3WdSKmNc_1MJzzmyQk1Wp`v)({z;~zt!)2lW{cHS2cE*7P<%(yK zxDO4{N=*Dk@2YZ&pmQtjBX4e1l}>8uAu{Y2UEW1{%+Fi#l!~D zQK>y}K=V>_@Y{!)HK_3aifxf$ghTwHwMp#T3*TrUoDt zy7Agw%F-Efp%;tz2=z~?b-?5igB9g3IC zNm?;bG590X4c_|4yyU@szqMf8h3;+!^5pf8gNt5}q4L*DMmF89;rc9a z(lvHhtyL|lkpcwRv)r=?KovPib>Z1>tXxC08#RdBCstik@|aI?jIhhQD&Wr@``oxR z`<+0=TPu-WcC?NM^9Jo`^F%Jv7UgByRuyv^`4tT_3P6fY<{<461$v+@tV#`?JxaV_ z6g8H6TjR>II&<+8AGcRKDVLDI73eS||vKX^I(+J+j z)5g65$ppy!5B=-^bJ_XS%(^NaG7aP*^}gIWv1jgQT)EJ!*N_L&Yk{Tw1WKiIi%)EW z!S=&F`?l4ptAb9yD|jR|;2Ce%6a<}GjD)KXn6sJ}y;Hvlm5h@>55C%pINJGKf{+w_ zWOT@^EHGdcpNBk6wmy)xUTQUu;YN3Rj#G0BU55PRV)fz~Si$`~rds|B8SewkPuvTa|qcD9&f!4?b?bs05759W5wFy+Y3F{rygL zUdbTx<0F`${F&v68=c-|I3zfg33S1nz1OTvft#(|JkxdH3CR6~b$<6%_TX)bWD};F zgh=yFeszGujX57?WL$lxH`9$4z8;7PqaY;$tQ0no&RYhSlg$je1}M4WxF;Q`a>U)~ zGn`H8T*?hIH4E?WI3|KyF*iPfkKf+aq4VHUcU@wMYL~8x)_N4j@u^Op3%ePavLTozxUCbQUUL-+E-D(+BvC3tgnw{71&Zj?E5pe~)! zWJ(ul)P$RzgpH5LD0`(c0BQ?p&l3)E3^CC_;{w?NC_UxPTS1@?c^FAuE~M$yuYaY# zCmn|zblE*oF&7+hx>C&tv?pjSAUhGT@<~T`kDOCE)U+(lf}Fm*y&mIOY53Gf>fRPR zr<^j&nc3A+pd7zSJVsv)&}#7Y-*g4!L|a8V%2t*^)d*Tjp{S!MwvxviCDC14xG1zE zp#ca2r@G3Ns(Vh(&vukbW{=_kQShx|XkUwDEk0j$&oa z5WMsv@W+9S#ZtqwfT={!ajn5Jm$N&}r+F9s+v!?KhM$8{d)~*fN&n#%J3jqe1b&9JP&`rn4krcx3ixAvxNfcnzc3s` z6T=?)l!Rw5=CO8F**SIM9gR{vAqyc5^vw*|ocEGGZWJ-g(>%#nwSNHz8h+g;DIid} z&{gaBop2KTlF%?xJ{dcxwP9Qr5D*G{-}|GW(5*uJk|Nw%4xK$5*c8x!i!r=&Rj(r> zIrOcdaRBKCD|%lP_#Ey!+);));ffl!)R+>qduHD&kG5!gB1d!B0XrS-W>6;e9c$k_ zun{HFYVu$$^ZObQyWSq4H6u$SZ7)e%(Rm%T?O>-?MJr1(;BNfvSZ98|YtJxe`aZ^7 z(v?4ZP=gL2rUJbwZ>N~ci0(ni#+x4*%2u|FRyRN`&p3EYn!;0io~DRwB}=1`yYkro zNMnD^sIXl9uhyBO4v(@EZevdmD1aUM>fVJ|kyd4&JT$g&^CANPJaXz1WR}$3P(g;c z3hj`$L1UyHc&`meiCA$nx?9lcD?S$s;`v(zp6ONWBNFV<7FlAf(eewN7>wDY!4CGK zfr7DA(h2tPfu(Q`_OPs%ZEAfcTe2VOt5u=TGvcrMS)Yw0^6@^PC&G213k8KFYgn0! z(5SH@)CW*qIjPJ}>w!qv{4^m*DiQL+5kM*e7+NgiK8tJV+_RlkX`UI#^K-!|@9^-| zm$OB|(a|U6&9t(D&wS@<8=rdRn4kL=RBc|s)V%Hr{&wt2`hHn}^*m2Cv zGrvgwahZo3nw{$^6CRD{A@zaiuQ}i}e{3pqi1M>K+TZ}}AW-Vy8GG5Nw3k$TnKf|> zzZ3Fl!QZuMw%VDoC;R-9Iu7U~y%@bq6)1w!R;+;@YAE9uK83HJoRo3Xn3EKjq?;k} ze&L5$FOn;})4bDr3ZCl{5HYdt8?kRsd}G|CSG54-1g;;)VP)HNp65mx&#boafI5W` zgkQ&w=3oL|(^v3;QTu8erpHsJKBuE23u3IlPI?v<8Je{p`Rr4=%n|_J7!;rn2FYzb z&%#Tf$LU000e>XPI~~4>%P1L{seM8C_e0`%Y#IX&>El=J>D32!xkmKG1iv5rLdNq4LI7t6bhGh>_fI%+< zd6D*;2QC2pkdEu;)ShyWJtF4w)#eXAJMc1^^eT}N323)PRvC|^>1kCEwCPmeHisg_ z5|Z@T6Vr^KMr-p^2?%tBfI2=St$|enSbU&ASzZZ5l84;W9oggvI4fiQ?D}YFs&Pm= zMCGHPLJ5EPQvK?o){)#GO}pAm&JX*fo}&byiZ^(ZcMs&+V!hT_T`4CP`NF_j7hRv9U_E&Ypy} zr@I15{(Ch&(B)q~&*)rg09rXPd03=c9UC&$j?n6jhq3^RM~3eXGpGzNr*W;<>#Q4b zXWY272AsBzPd;DdU9??hVYAO+sRQyxhJN1Hcf$*%eLMH~I}>l1|0Wtg{6*5%5;_FF z8``kC`9A6Tc)>Eet9@>oLB$I5EqTCWD>Zx5_zQ$aWJP$5eU*?|3GiE2*Gh^3C(Q%| zgWYjPy!wue$c{bY7Rn~R?SJ2)Wv9#^D#jhv-hxLU1j z?&coo(1GH4r75whFZ`}qZRdHH5Ma@E5*=cD;*-*hS>b#cn=P^~d%{TxfLG`NUXTvR zgM+r}awYH3fU`o)JJ2Bu8eE@J1=N!mz~=gtIzMI6r>Hw(t)wm$YN%3w|A+<7_te@6 z`b*}m$<6rv@fa*4Wj&Fmi-b$eZtA68st0t?+d^s{=|n!IeGA*XfMn^h7MFQeym~M# zOte4gz0C_+ny%?=KJiII4Jt||Nj`+SOpM+8pwxkESM|yogTyq(ISPZ92h} zH!Uy|R@f7u#Mb`6lxEF;TW)Q#X<}?uU~!`C1sqgTf=||%0ruSE^u2ELJbP`sxQg;Q z6W#Yrw#R0vkL_d9zbZ?e9rG3WT@dd5q?&KhG8+8+T)3U+c5VQ~sH42gR&-VPc@JxW zKN4NmycO4Ne(HYe8v`&-+isg>nw<>PHOm52(1C2}noQ~MTlyE5EX8UK+M-)!g9bQT zb3PhIz=eS5khYH@ai>Z)Cse#UVB=T)t!4#w{e`ybhAT~4_U^#U@&Q(+FlLKQ0u118 z>4EE%8FMEDh$=@xL%~mex5_4<7w-JLiQ9`($tK~+!Q6Zrx@n1!Tkfi$wOT8Z>8xS| zB^Ma0@7u@5=`Kc)o7C@->H|f5^@t7lWuH84FmwvO4$Q&)l=DyIKsZ5I-I)i4=bGP|Q?C6XmctrMi+Un{9svW|cbfG8S7hjuqPS352WRGLp=k}hF z&3BSWH-62YfU>_o-|vNaWh} z{k=&->0jc9=5oD1xA`7E%%hyX6C4gzg*yY&g`KDZ#g%L-akIQmVCHyTb=(B2OAIPZ z&|py7o!0pIRRd6rmB+vEpEsZZ{LZxmhIymRreAB}&blfwhd?*X>7fOif++igswu~N zAAq>J;4iLYX9i4U*OXm4r>Yo$56RU?xL{_j#AoRE0O?p)IryYVV%EvTsZ$U%sL3hK ziclx2J~zbkBkIy2?mHI8%8Y@Q9H@+DmvpsS-@plb(Cp$ig+|9dRw!p4k(jmyEp%Nn zL;NQ;!B1FI-lTnQv1eytat5~i1o)!1sAatT-nBxc3$CR<^d8y<$lnj=OlYI*(@?;m zJ}Bfhf4h?U4yYw0${+OBJjZ(3BXD=8U84((*NUXA+}8ylB(JGhH>y*2>v?DGGA?mR za;K#_LNQt8323K4x-l&TL?II>Y4eWmsGQ#8_hNW}_1X(G^-QPAwai;j)odus3*EOF z*d5ga_%C?-*3r}Xx`{tU$CNfvA!7B%NB_!SVwuyjkHw*2E8WrwY}dN7m!aR;Tnb87Qh2K0R=?jibxEN z$J{g0xm{07e71y=xutEdX3LFHSFl119@9 zeRux{&wo=0TWGew6hcopj-c!7@jS%>BW77YD;(bx!92rUh`7qq(JT&_yb9J()#QOheFzth0xbAb|XihvXEp^yYBl6N^ji>-#?MsuD?S%4I6s*2cvv4Xje%{EI zpdI+T1^u>eZ(f`uzff}kN|Yxx_JBn(_C_8lGigri=hq8DZ;;rT__2z-tR{ zLiYHk4vpZidXKsBa9gB3Q~CDy7Z_k0Iy$pOki#b(Hk1!knKfS#a4%oLJa>mCA3{4l zCAfqM*1PGCiE9z7irUav`ImU2sohoUx30w8q`8w|T%?_r#I9fP-D*eW!jmw6>n1VB z`=^8I(?g^kna9U!dI#-4zS6=?|FlRG5B~YS9*n)u=?f(Xr^W!pfC}6i3=VjV6F7kZpA8=X*|6JI^!n*}{(^yR6zj_v=+vYpU}LPoS5X{% zPdR!K_<%^MNqz5I7XO%wo1nd=iZLRdHqEZW#-5BHJ<<^XWiK_a+mcB@JvCxKata2{ zF3>07`o!=9hohq~$g12(l70Vcb= z0N_Pt5LGDA23&Y3ax?u}OC-Fq?h|Y7dR82)*HyU8F7c^lLZqn7v*Ard3G+LGKu-1X zD>Tsp5%PeW2dr-WfCNb896S3BC9j9j1cXC3;aVku6HvxlOx=jlMTTU%3O5XzyvHb8 z!rWwUEpBHu0NRxRRfi}7E+?ARIU3-s08)(_Py)LLo^T2!Uwd%!ojxQS-lLP2X7$vH z%MfwH_kHK6;ro-qG^#JQoEn1w$d&U@=kz;osQ(eQWB+Zj5=7M9opFHfZNa|+tUiEW z)P4erdJY}{VcWZtNT}(%x~~)D zxz~kCg;su^Z(fXj2Fk5V?M-@%A< z-^i*dd-c?kNj~D1v5XlmD*->>ldVDQ*|Ssq{ISG^5R7X9!PO^8=2QOqCGti@Ra774 zstrX|F-!Ejw!19y<=!LL-!*ws7k9ConsL}+@S7fvQR$PeU=%NtdLx^6Hym*lleZEA z!4>ei56ZTt+&T4r2obXOp=0=)y}O@v81KKa$M+XpiYvHVH0Sr~#X0ICqIZ_{+u1g? zFtPi*7!%dR25P?c!vDnlln$R8J&#e_ChQwTcq0LorEA^+;0Z*W2@4rI-ugxH$eTll z^&pjL-%8j`?r?eZSYa%2LWfiD#s|W?RWr9(!J}S-ri=sp%)->^FEc2aAIvV*$$zAg zc-NPh1S0`->d0X*o_X71mABzU}3SRuR#MnPr)97 zvlmauGxsv=Q?MQgl30eQ@FCq@gUOo6=RVg94`c(ECIJVe;V5eo9sfy*r&v~wi@}ll zwt4jRX=T&KB0H&9$j$dn713@x7{0P=_a9)Z;yB`4_Vbps^2XiBam{s@AL0{rEE;`N z6X|4++2_uzfW{a&$+X1U|9WJ}Q#7aW-Xu6~e>fz>Mp{A>v$|7H>TuhOLRIh4&qy1_ zjdqynn7)YP=FVn$-8n{1uhfS)n7)b;zT5+`lF;b|JSOiwW^HprWfGC$4>MVU*c-X^ zqdi0&+zZSKiLlwPZ{=KlX(Me9v^j$bg(9{*FYnBZ=IQ1I)31~_Vc-$6ePZe|tqi`^ zq}_X^2uD5fj{LM^8rJ8hV>#U(_f8I-A+j5{uMKay52Hm!&{e}|m*LI*?*}Kts=;&f zlaBazX5}#*-ec8jzgnl#U~FLA^|IfsC(M5rlMnacx@DK=FaHsDhKn6@>r|}t|YpDarbw z&3AM&Y1pUO?YaZ?^VxSemcC$mgoDVgxIJmfxD4@qGAxpW>mglR%bX;y9cRa$RJ5uI z0Se;65fYmgP^O+k@rqOK&aDq@{jyvOS1gNlT`BzEjJ&HG`BJu=sjH*Ab3I;t;^v-~ ztsGA72}4es%wF)v&??JGSs&pWcDl7TWn+*XMw}uIzw@3dLE5Q#7nFcCO!bBdhA#Wk zhuF`k4eKmRt5=9@>P|xqvNElUD95?HMC{SM$K*U$Z&?N_S!d)IsqNqW>jem-yPboVgSF{qcRADRmNvS%QshP2DG*A>9; z^oFWCzT@+=S-BxyY?3ch(8uK()2jzDgdUY3;7~1OI3cUTx1m$U?~C?4r{a%~@?5xu zs!~4gF$YgIzSQ^B1xU{B6XLH40 z^CyX=Bk_rMeUiYlBQD%{T}weAHm-LeSPvL=6_ze=={m1mzfnjQZU!re=jK; zthK)1FT(EN{Xd`L_jweB{4u@$9D4bjMcd*p{rkedpGFA&S)hL|Z-dMKMC;FCm(Rl3 z|Hq~Ogyf&Y2QdDR$^A3(|1X!F)HlWaAA_tU{nT9jQZH$udOXo2a|aUgzlL1*ir^%_ z6XjWL_um~DDWVk}^p;*2?f(ScRWemf=*JZ_OYcjr( zBUf(HIlC=E^>`DqI;ap=;ELg2cOUh^%)Z6U<{Cs0-B66{Ci}Uou=$LM*-WN$W1JrJ zX!AGmR17ZYc#Z!%TfLl7DDA%dJM9vP?@LBo;^v^obZTgD_JH*|Ata^dA|CcW9Tt{6 zR-+lYWNPWwHs3{r?AeZAXK13ILalv$N4ShFztR6#HsV-iJP5YI{RZ=Q@mLr|-~0%= zcQA5Fm4?qFgw`~!HBC4`tat8yYd+=v)_lla^G(VEx%XdeglREta?0yGnX7{-`}s=W zKPn`4`7BeosHb6Y4HA;}RW9#q$HublnnTmQT0~HjNaXL`|7ZPLh2R20!A-WQ6@0?Q zNW&($^}Vk3SUk=e;?ei)8R+{N=slcPJ^j#sb{qIn^3m?|RCo0XK8K4XrWc_czR(SS z%b8zzN_A$Wx<(VZ)q($g*2-T_>PkVC9VjfH<*z5$D?C7mp159ADJwlKHczC z0szJWwl3rLty#VS7UbJ(Yb5*M3Boj4ukiG}TKC8wPb~NNf%%i0-VtBv8q6nljZYeU zpZZJFJDi9mI$)HGbZ7_+Oy_lJuxHqO0Y+}>w)fv@R#HeHx?(~8`aJI?nqOC6HD4H+ z@QaD!K!WeL2$?owkbE3@m@QVFQU2ik>$6GaDwT@bIWlmq71wDShP4*%g!1Mc%=GcC z_{aYvG=E_@DUL#IXI;T zUA=S4j-OdKeDXoHD#}la_&;7T0Vd(pgHfP zccfU~-SDoa-5(yNBav^bVNmHnOwvLThAdDnVHf6pErmz;Kuvhyd{7s`3^YN;VQdf-%SIG4Z@BO#JQv|U1VIGik zve8#@A|I+SuGtj7uNM0lmMnyg>7|hR$m;Z^FI?14k^6mrX^IF z(F#CciqF5PF^hZ=dk(Rl>RSbA^%UrGsYFKBmo$H7+1=i=Qotl7jf(lVWJmBm0y?LW6uZl0PilDji##7{x&(bbP`M-$z>ZmHapkKPXJC$ybZV*tB66r>|mF@;XLMiDksY7=Q zNOwthcXNolz4w0a_lIk-lm*9U&z_lI%sl>AlAnD^T*5-t&_G)XPzv}qz8LK6TDffX z*Nwt40}ph16)3cdxH_zt&HBzd+tpD)J|JO%sxk576wM5P5cLCeruY&_I$d6kMNwEv z@ygC6PNcfqYP0abgB?az9Tg) z4>SKAvofr7`Dj}p76ii3ndg5$?}5t%PEwqbble zWtWbE{rXIoqA9#I^L%#$Mt2|Z5tCO)OTk&kasSr>G2(Ig^g)-X$mF4Oo>26R$5WA( zhr#xB_hY$tbrY|hN}J>a@@haMsy`gjNRK4A%$ zAq~_Dr^qAk){%_`I7ly&0fB5SN#dE+_wMYur$piz{cjZgRH8;@(`VwXzi^&Q{B>R`(Fn$nTr+L2*l2hUasOCS{3V`OdrupCDFN(cS zF}@hram@B+*XJ+)(EGtHj4~Mg0f84Bix-ZZ^6GKPCZ8r)*32HZhqL`(OlSH%OPF3g z(Z{p=JAEe21AynjSkt%_)tcoAeKfYXv9A-VTBy%FI&7Isn)Zr=8>Ie_@uOdq#Z*+x5WpeS(EET`Z58HmSE zR4*z2)lq%*ZQ{zaTN?!{W2Z;j>$>H&VP!wr_64QD$71ny{fh9A>1GHea`Z+P_?^v} zK4JF*EVlAe3SU2$VO3}a)Ufd?l&Oue_($)@A4nUZWUbhkIvj}c8$Nzpim9Lt?wIJM zx6Fq;UJHRIK#St7zLN;1U}VbL&}b2&vE3kd=MI943(~7Ms=ub3Adk2psr0svw(~yT zy&F^v<^~6t0s{`v+3t0+#E=dxU6auvV~-8t7nb@a(C_u<~x7 z(OHDu`ziGqp+dcLrkVKPkFKBhS3}FCn|cddr`x;!7hZik5yyKC&?!3bN87k;gCnl_ z_}B29d6Knl!Hg@9GtN@ZOC7^fm%e+IV&s# zObF|iL9p+A>qAK{O^RE(vt8;SC>P6C0kD$ki+BcBMdQ zBNeQ7X^8m7*hjy84^#ILZU95>l^U+pZtHp8Q;l02AE0vgd{xfK8)LBlD zp!!kY-_3?kfvy)QTLYqCzfGicV+6QAQQcr8Q4oAH9G&<3?kbA-Zlk6n!Ck06@WUF5 z*w0DYwv~2257i?sN~0Q7Qp}btSg%_;koO^r0RPk4Bs9tW@e*5babC7qIA#kFW?D;< zAgU9$Xs9b+!Tkd z=T`tj*5kupJLgZ35)f|{7!c=kzH-p753jd2SN^e#)}ZpQ-LrhFmXzFTPu@%os?im% zZL3GL(gDd$`vVpDgGn@dlDlBtQFYkBZGeLN6o-T3q$<4so5~+ty{OaoWw;g{^3SrD zN2g!tN|R8YHKhFQo4;||T8I+A(1dO>A;r^`&MVz4dE@BfobGMY=sD9(a1WO3XnRHa&-UYjH&a#`{ zHBBcKY!sBi$qKHJ`*KE2+d9Bn>uOPX!`M{tRl6CH#Wel9Q3WMKm^ivi_rZMj3=mqD zjayNgLYL6JJ)iOVpqXQK0^=ujW5UaQBVn3AWdJ^aZ_>qRR`WPKEt=z|&R)EdGKgru zR>DF<35bB%zw0m3q6zG~gBwtT#l<<@4C~g_@zyMWYWo(d1cua|9^&A4eKk^TM_d6S|t@N_(OG*}d79;r?zPR1M>QFXqKF(afj7@C(|rASdUr)xJ}D!vjJ4f_}M zB%9WRh|c+Z#U!eB6G6oKUUc4i>Ix{#8y;<6Cj#_fVU4x79k?4>mk@-9a){|_{Y?Jo zMNDxD7~#quaSD84Rca(HIdHy9Zh_@Z_v_k28k)#*a_iXb&-~ghxV*_b$UhJ_=u1XHwQ%&eLi!WoA*W>{9$=`vica=zGmrw4qBJ< z*58It?a%*8ggErd@Atq`A5;7wgi=6p5%0~qN179lt*q8XQ@@txu3NKZeRTFmn1w5& z1^#{3lA;PWDMX)|=uD%lryZtgA_(-1J=(4QUxiXhHvE0wOFy+=V*Z1!RK_>s`y;X2 zWbVY$U2uTA**<|E!(otw{>2mqR?Az^3gaOiGLI-}yp@8A*Jvn6|J`^4*XznsLUYx| zMaEFq;WwXeO@(ppi{Fk&L ziinVkD-5hLI%`YVyjbG`C10;*X5ucoC*$6Byx7D)<`>KUm>yX+g$q47hR79u0^S+1J1b=1mh$tYZtF?E>Pb+dMA0>u`8WM8aE3isOGxdrhkR>081n zqC`lGFL6Sn$6Os|y^2)U3zjdDIMO+Qxp8U;n>mApYfln!z>o;Qn4-6TG2`U8{E&>b zCjWR6L&lTD$ObA72K;rsUX!vdsBZPs{wpUbt3Gqc#<%_7&B9L$mRv;%242lYBh#u0 zAsU-QRzSzz5LQ;td3s*)9IAX`nB(IO)^WrQj=eiy9c3W@lb8tBj&#X$afbec{JIN{ z=2aq0_DFzBGBVd&N%W7$lh!%IClCn5S!=_;aw9vgsDo!N zvmrmA2*bha;_+$LNkpXGl+=_IFnNb%(JN|TpL&Jn!8=>Ve_^!%cnR2K`>Y1@s3Lm! z>wcp^og|`CDG7CzOxHF_G93Y2qdDb`xlGp!MXi}8(g15Hbf<_kXv?@DKOJ)VAH(ci+knAQf4Vxr;YH-6LVMx82W=hNsu_ z;O7aWz&!`=tbJ$?C04mjD=Tb`tL!7}tLdwfOCh<4r^g%|XwYz_{ET}?u^MxzH)r?%VT zh4rWAJ^ezO!))f17x?Ao&F!Y#53A9k*K|3|DA@TEpMh8mxnBLEeh>)+&EgxEMg1W_ zVW>z^10u$LuoGYg??>!s(#+>=>Pkpe#Kxnt`^{jC``M0P_x3?y(FkX*R)$QmnpE~~ zFy?&$`bWZB6<=x=MH*hyGHpju>@Sh54bMFeV|K4j@Av+-ITCB`e#k%-cF#u(^u-Pn zNwuV;&RIDzxq7l_xaqk8ECqYSR-^m%`B4jl7BvluH$q$HTZ72+{r4RPd%Mh+FK99m z4ifH&6N=Rrr%U(vk2i;W0nf#Ubm&mKoBid>cf4D`4E0%39c8cB(VzOj0H0^hq^NZJ zEF#QYd6-Ef5-3&ni^HbhkG8oVd?nhnqmNfUYPnwQ!YnxWb@t7UIyPM3rf-j%jqYDz zF00L09b6QYo}}_nJBuvE_g24gYEO@*RZb%qd_{5M2UpWOvBda~vehe-(xFNcz#T`7 zm6jHyvYnMN^QHCG&%>S|1FMEpIWu#BD`C=lbtH(h@@dzG%|SF!({W7nZ_o@{z>2^# z9Iw5OcQ|puL5p;8;N6ys~H%IbVM4*Ia=k}D!;m_C}Hr_{+>pdsc$DiAg;u98#w^5IicfBm}CLXjG-4q ze!_rWkbjM>`oTN*)2o$St#mLk6$*N4SiWnw)3qK$ruyqQezAZs7eHyz78 z`@k#?JRxu0;Avz9=WL0O%;5n?<($%|BK7>4+L(IdO+cl%#DV=ge>MrjyN;3f$^ZX3 z&j3g?a#ORFlnVq3{_FZ>Q=2EDx)t%hhY6OJK=}Gqsn=rIIJo2Lo0(c-M4Oe?hG(hN*(###K(~di9_5P0k{>mgj6!0#&Jnf&z^l4Wc5P@8>(O>a| z8o}u}VeA77g z3rJ{=sciA3&fllVGs70Ex5N}Q0KNfiAoi;y>&h1Mgn?t6v&SVPVk-%6>=6O`bzZ5r zW6!eYw6$Q(vch(S)}R-$m;SSRWCe<|EdYU_k7<_3NZX)^)f+@~C6|$3Npb)cV0)@S z+4=p*=^GW2AaeRhRf9$o+);SLIc|NJJ8K6Rrp7H1_uJtW)2(Cr3p-}oynoL>;qJA< zU_>v{0H!!=S10cmpXh%AYC0IMu!Q>jqhq?(=lX?9osP~`)y^{ua>cHX%ep9<4Q z$o(p>`aU{}H+9H4#oK8Q6xys>|ARzG%@d4$cKH1O8AZK+Pi-?30V~b~!h~JDKhP>f z@3&9Jj%q+QQAsxpJxEyhuLrWt5HJKqGR3oKwS2*bW{PLIc*CsR0L%(Rn{6UZV9!7S zh7R`-pic1nOxRFjD^ZO`!U5zAXcrLP=p_e^t#qJ1tTs?(y<}Se#0i|J3KKyjaOPjI z9so53=EAqO?c1v|6}AMLyx;48W#YRS5o%0Y{snEf|06rGevQH)xIR?&Ype)Q^p=z` zTpJ!5+t#f&R0L`zh&8}>8Cbb8K-y4Ven|V2+O{+^9$gobfw;m7nwz9hy#Pa4s zla_VXmWz?tOc>B=g`!A7jE_0lk)w41;jROpxbcqxR4a?fk7~y03e+s>0NxA*Nf>bX zGz_J)_53Hq^PG(qsBA4DtmJ*{fPo4fcn}3wJ^17%8y-;sb)nL@8L`1Yt1Z?Tvg|GJ zxap6Ti31+n1*=Wb%mGRuRUTy3oeKNNMkio$GLT?{P3 zp>6Rb&dk7o>tLqk+KF(OYz&l1e3On07#Ov;S{kqhmHw8cqO^(~PnI|M05sQaNHd3k z6a~CTY^BR#@%@vrZ(*fyqHFOD2wubb47#c$mzW-{C4oLw+6Hsb6|92g%^HOqv~!`; zwbcntz6JwL@A%-6lLHRe7D91mw9zQo6U=i0VF1Lemxi0Dqs=G8o+8#@FE%xYv~XKo zHX)gPxUee!Ua^3o*t=+?cmDDIr31N>m#Sz5=;IjK$)gXlwbEsZg~Oz|_YFyAPxKUI zo!K$l|J2wKmmk3Kcx%G5jD`25>cIvMtwH`hgN*toX|FXp3JI zQ*sZ|gB9eH(aS(-x>gXyEyaf^gJe4V;4?ot+8&L2qHL{j7rg^wHzHpIyRzzxbbka0Y)vU`HsfRnYr}!XKFrM ze3S>pn_C%sLPZ+=;zileG-CPprbZ(}J+|c|WIB;k&^1xy?!nnH6)9_R#-C5`dB(O> z>=CzD+Syda%1@pw^jscCMIKp;{iaxhVLr_j6?Hw{HrXRPi-5;o$uvhrU4=m7{Dahp zL%6G>WkMwY44L)`jd=8+C7y_f*4Uyjs~XP#W40d+EOx)@vxd-MIA^%hL^Tb}CYvbc zlZQ0}k%$6JfuPX-%f17klx66WeChutj{-}J&~!u;eO>>rQ$Wt-2ebMAD)8b28K@tL zN54B4UKEi_B5qsoRP?{zFzOKZ%1M3blDF`cjlwMS_3f~l9M;dW!k@zge00hzZzjD3 zbn_Q1ekpcl4VN2Xp+MEpwchKhFSv+@E~~&mpY2f-11Paadt{IrbPT1k)loBTzn|g1 z#3%jiz)6!8ALCmQ2>>P7h-I0txNX-MDqRUT=(OIKUdZ5iFJGcJzi}M4V`rV}D#Vyj zk)4aQ@Zo^FUxe63UBUKu_ro@?YVIY;PTF_;0p-&cC9PU%6oh~n-+C}79Rjp59Jrp( zm()LkWl)6?uf6h@ZeNfqaSHDM3_fLpJ+479#-LN2L)rxr6YoKn zD5YlkG+?H1u-opvK(`~?s1a5}nRcKq?Yw9221&6mm^vCnr4|d>X@4WjIgt3|4KRF^43dOF~R(0s?_S8s4`^-*X6qotLIo_x#`XxR&nnwX=> z@_S;4bS2*Mr_tPt_wcPs4u(f>C!a42E3b})^303@ zIdOaJ_pk!-JgIJYek{t^llnYtvrT8`ULbj{jbEAsVMFhOiu~VJ_hOP7*!Wh7-rXKT zpM#->V7Y1+nqGxv)m|+cxrpM1Y+6*hvV8wfCF@p&nS0-BvB;rG z1Ry7^0u66tBQ>+~c@LiDBJ%eB1mH^SHicJO=|nJ8Wa-L@=|sk+*|K1J%Gxp!zoB?f zHT4szRC|~gaHWD)2=o)9D!*pBAMJkFncqeQpvX^Z{N?Mqb<)%o5HPAx2fLfH18MBj zNQSyrd@|4Y43iGuT^+1ZA%LR7np5ZQ zPRVpDGJy~RTofk2o2$N)wCWym+FxcH60c$28mjCDd;imz?0GCcZ5z&_?K(0FgM3fyY^GPsJ6g-`!9BI02qRuh{XNXY+x@n_ z@&3~7HjdOuR+HAN)k%lps;BYr(Qg8pMLQeQ;L%rBG7z7cIH<#!dmT54cPcl1@QZhW z^8lJ#SMjER`eo9utQau(PZs6{YEnlP<&@@0wwiA6Q4>Utp#evq9We3}AgV;R3!2rF z!_*3F2+EL}(Z*G;4b$d@I>&h%Q-d_f63sTY44)~(nyG;cwl?G=@OvQLh(EsyCIE~y zjkT7TVv#h4hZrFw%s&?J9UH5Y$iUV~acJ9h76JMsAnb*EW%>aOjD*&M7Rc2sy0602 zKR1lb6NY>^8fSsCa&J!G*LXLWYTh9(mRSsQEz8T>Z83RyjT{^^D;Gq!&4GZ#A*-hR z5u+t8o|==FHZ;j=;zoW-bH`BFI6=NquIti_(09tV4`3MX`D1ygB^7m+o^Hp|Xj^x# z!!vCD+u?5^P9_}&7?1Dhr&Y3HnGy2h0e4neeMc5>vT;;RJ$B*>Y4F4b|0%*_zbi3o z^pfqp1Xbcz#!dXfq;LM=&=1pZX;pkFPKSIS;(h4avoo*nA~|;A&uh_Ep}uovlc(h0 zRL3W*G_BNf$!Mx0m0n;L1_VaT+VF={CK5Pc7FS@q4bWO2((i;*YX5PBN-XyQtObh+ zG@!5=TecOSZ^(vENdJ7dY?{N9iUWtW<89Vtzk?vO47QR~s#lhV#L{#q-oW8w+vXV& zl>Qzhsewhjf0D#mg>NM27z@4?BOSsTC)Q9^W!RgyclUmhE#G`LaOcpA9(sGql)= zP+;)+c<_$|*DL9AU4o>Rp~79uP`!wSi%sN7 zMktmVU0=_ws{-FWzOwz*k`PJiBH#kI+U4_ET&jjpVSJ`D{+GW9=I)uIs@76id;69e z@YiZ+T10@3f-egImIOij&QBQZ4n{|v#0?jxxC6BwOokyoEsU_xhHMha#U1@8*mAwx zavaH3sAat{j`aFg<})GK_096dg>9)Z25=HcHaD3sTry{(Vk94A1I(Ho7Xi7B82sA` z9(Cf17<1ba1Dzn_#>jo1eC)13pzPar$>?qRj5Fsx+I5a{)$GFQ@HPWyuIj?P`-SK) z?3>g?C%?0T=61uGb83O0r_1{^m#6ppJs!;{>E6v^D^AYmdz<)?L%Py%zU?88NA>O? zv7Z7HM7BzCb`vBJV5Sf?Txs_|28{T_NFXbG-ir3y7gwHgMfb-s_DWl2%#=Dox zCdJIIFc>=YDd`~l?A}TFm0C(2vQ>YB6HSIWpV%Nz6v3X?NC3vo8C-HM=12exj%8LM zUlZ&YCc?vMC(=8Z=&4h-j%ozkWX>Hp2c1v?pC#q*d(%SJQBlX9mQ|zaH;->51e#zj zlu^J57o6A*qE-8|&y^$Bs|4#ZbvcP)>}7BKuF5s?xL18m3p%r{s<85*bB%_jka*S zG}7Kq_-sdI0J?@MT6S`S;=lnt%G!H8mJ4*0hobLRx0u7lVfRx{M>jt1!~879ZX)J} ztXb;C|IqPOLh~E?xta|pg}+43qIDxK5~gET!7#w{trI2Vtc!Wu$^od5+BUcftr30s z<;0`4y5W=e^q<=e>#u$)X{^>M%xYc+n_c&yr)ZcrSu zF{P}pnw*yLCAYXQ>@f?aRE~4SHO6bn3)K*BDl-L+6`$}bVH4( z2L4}fK(`NF`eQhx-}PNZK4@d_7D?h`W8R88?fV*@D_`+K9?w_zLcgwcV09uI$f?WX zqQCsGH~QhAK0G&!clN%sn|WEwX$f-osW^dSw9M`|eXayXU!jj^EEeU=2#d#WleBO zm#BXDbzOf@ExlM#yBh0{{I@yd!k) zb*O5}>9U>Cv{^ZN$XyHH8xFMyE{_Q$Sw5Z*w?btfG)os}t?K^ucT8x14;F25pVR)T z=l1VU$#&6KdoW`#bY0SC+8O=P;NZY!L?*5MgS4^X(AW1oHT;RBWmL4v#rl+y9MV5y zA-nq0Tiz;b9A2F7)iS?-j9nJ!tJ(4MR+!*llb_~=vcFHFpO@dU*T^_}-8eA$5X*7K zStEW{m(k`=G50N(L(Z9n)iuq(RcRpdS*gA>pSBiT4SG&T zQF)(rXh~m)JxJDM7CTbM*myES5rgstl5w`>npf;hcDmoSJndo6L2r@3Y?9+iONSy6&C*B zt!BXtTf#YJ;|2(JmA1@ANi*jlb6{X}?ks;=|3EKVo_U&Vd(cEBSef>yBlRh;NZO(T zHdKHh?FDLV-k{Qrd<+Cux??D50#1y2?!>x97|DQ-&lM^u!et0SSap5%7bY=+!QIda zmbK-l(x_L{Lv{ZaF$O|txvy6v#^eD!SE_L=RSeBz@$E?fMz?C_knhvw-z|q6xg*E% z;r&lnB!x)bg~iGZmLtB$Y$FS9r?*?OgRCD065Nn^pgs_+(PPr~CU~#0yI=UzMn;b@ zB9FTl!i1b;`i#|@FQ5xWSMe*@8&6PWop#Ntf$9JbTzTR1KHR1^!7zbYF}-V4*Yzu= zJKd~v(cnEFuOJh(f6`-k3PT>K;>2__ykGJkyZqf+yqAa1l5bZQ*XQ1>FLC}m;?=98 zWV-zuVsefoKo;vP;nT#TQD9M=qZCX^vKV1=2=Qg;*gwfMNF!##bQMA zBqnO)_ZNXt+dhuGwrw4ONX&p;IYYr2KMCQW5MNO#FLM)K?TS*sRKhDLO~@i|aF|M& zw?sgfqG}7%OX9ekGohAJKl0_EMU{W6=`r#L>&_Hh1ZwxBt!&cvhB>unss|2N^V*DJE{ ze(obhk-Q-mb`sY04(G#h{RGMb@hOh1Q*nM(PlptvLdISGHxX!pGH3sp&?;#-_h0@l3c(6SQ$JD2QYGy&s$xJxfeFmqT# zCcj&gg7QG~q$q6ZvwgdVXfHQuJM~S8c#nsX)2BtwWLI_Y;_!kuhxGH110!tLTF)k# zqncs=qj1g7&YSM6)aZd3MqA!E3Y*;Orf3cGeNl z!D1Ceb&sMf$AS-fL&I}*OnM*&$@|jTb+NSc!evrNihkfUef!n-P>sE#_4p=?`Ojhc zA7z(jldHbZr;Un+e$>Lu|8abV6nGFvm{1PU?tLp`c0WN-*n6)G14-Yu^4rQ8GeFR&uBnQX3Tf$Y($jSYQH3>iWm0Ec-Ylc zQAE_fcL@i3YBQb(iT_{|5fifv4#9`|En;%-cW+oV1wfcySYiXi&$mbA6X3Imi_x21 zAZ>aDgw>QcL1hR=5u_>INq&4J^Q&iq-eOlKpQR~HttUq?Mjm`TJs3F!zU#rjTSCdG zbvv3FAPdMTZ#$CKYSwK`qoT17UH$cme_klw2zuNv=2mXn7ufF|^W0sFKU_E=1rzNe zb{H$?&LcO$8oe`GMjXIeMTKQ`IdgF$9p4Uo^UaAneaiIPr&mXJVk>9j zVk>l>dAGlNV%z&0N+~VW-RXTEQn%7?Kkp|>IOJtIf9>Xx`Sru+DDUVp?~2Q_`|CB@ z_jkC%D_qe!G2MMRLQ7*(iv@IkPd~TpVhY~SwCp7k<(c&arO2BWqM+h?3(HS24~&UC zzKK7pYrfnmY%J@Q%Gy2IeQ?a+piqT__4eGKsC8PR~%V3V6p>&K1f}ro3!V37&H_u5VFiteUzS#N@Tr7k8*F}uH)(4 zDA|zaBT>ut!zSD|=hg_uYYKl|dKa=tgUXP((GWjE5i?_mbHusl|5%h7@PH9g(UUx3 zjQ)L|A;;-UPTOOz{!Ea3z$Jf4JkJ5=AAb9&YM=0_+%?3lnVf5#ohLRPmp%ulozZ?6iKC&PN zg6BB$#H<2L6WZJ!j0gur1MXM!pLA9W&$T6Cx|X{O!m;asR*us)GhbJDUjEJqWH#dW zn({AoX&TuYEprRGy#++yr86|9w1j6pdL)bBl?~(LK0?M{a^-&Q`r&VZD#-KYy^yK7 zU9v&QrwX;d59l?hEZT6fW3hD5EIX4S^7`WGFH`rzR$|-mxOb0Ah2p$%uc^KDMN z7jrIBG3X5iL^;VT5vgb@5Wm`{oRUm;!z1 z_%#x`-Qh|DLluz_SeA5D2@jXt;P5=#>P&>QYyQ2bBypYJgVHZB>66)@hk5b?u3uRc zgWTd93JKM<>Y`)5STu7Ea_qb?^2-RVDpCyCW#kE4_<+4e2}$rAc-h-&8rIk z3SY_+iQ7ox)naEK_g%2tusEkFVQ_L9%i~wn{B40We^ke7UMcPG=p^P96>&EGo6hOY<6T-v2(`&^ZS)p#!T#1o~m0y6zi8 zk<#zvF5fli%fs#pa-?E)n4K8cXfW7lrBAp!;c=g$=fGGmO5BjfT-~i&6$I>x?!*`~ zzD{R-W-|-j`ZV}npleUP!~6WrCK{F-rO&=N4~q(ED@RCprd$M(lAbvqrXnOHSn9(h zV*@7b3m$YPmXmL=UbO5A3!$+ZRbuj(j%Pl*o3LKEV(Bkm=EWOF>I$xS-cDW>_ORw4 zeWEREu)QcE#zFoyiJ69vL+6pBs6`YUo6O-7YDWbBCD!*BnX0#>Bw8|?1?|R$=ag|2 zi4FBvC6i)ym2NhRLH?ah$jD>^6@hUQe;3c7(4HKh^^@YrkSZ1(kdcM`Wb*uxM`~=Ug^1+hI)Y zzJTB>gk%u5L-+R*5dDV#=R%{C6{a)X>Id@MIXLke{|(3V zX}u^{n9@XqM6~YaqMQJ`yrVzJR-TP27G>JM_(0mQrE3s9MCZf%wt!WSu@B)5C8eF@r`57bxh9ARpC}+1+svE> z4#fz+z9f-8#xsmDn~`WKGZutMFt)-IN=v>)cO}t^8BFOi%~I2et;0t?>^myBzWn@Q zsW`-JhrHz+DU+Le0)-JVucq7IXNn8Y=jm{WhsZd+8|m9(jq;*WXxb+pSg%2Z!+b@! zYhgVsJjP*}{RCQAFXmkTRz&6>os%Qgqn2geM|08jTZelB?Vmyw70?vkWdYOOFtLP; zWR6FJgb70rg!pUu)Da47c+pD&nNgK)18RyMXmGkf(|3~d@rJ|Q*3%SE_qv5)x9>g{ zsprFnJ7WEY2BE9-1z#o}m@#i*9j7MUCviYbrT8{jVLzr6%fMt6BVNO=ejwIz49J0Z z=T~fpfLB+cN8v4nA&^;KzlYp-eAxK7fWbhq2Rnz&R|nFarqTGMIhXPGCCvib{rV@i z717Dd8YO$hfX)7dp;xx|pQ0g&yi%qzGuwP ztvEl6%!xf8*PR>c40=n?ABs25Dx(Na!s!gI{D$j}&!!wG34D1W`7p(5LpSXBFyTB` zLp|@!W0lI{CPL-NCORW_tEGcsBe}E!E*s@7uov-<7#Nsr<_nhZR`*_PaAL+&nCXUg z7BH18B6y)EZ)HBRmrGu!tNVWl)lOt^q+d7t{s}WPdxVZmQYBQX^+hfr%gv{iSjLx1 zO#CD3@paWOVhR7_1;9tX`f;6UEVZu(Yjx-EQ=_kC9IL3Xsl2&CkiIA!Y9pruVPhQ1 zN$huZT<&(X)~#bhmLBND*iih~n`RWBZKn;fL+<*KT*Xji)P!_qBmGuuoP<~==-T2Z zMwevOmgHNJWsZ@*PO5d4FNenJK2;54wyKJl!INL_F~N(*WmDE-?giuvBo-O-iTh?4 zKbrCEd@)0>#-e?3j;XtYOPLz>N-`uQs3 z!<<27;J?KeXI&>)6lF+}#NpJGN3JPgkH|mq?dE=L3_J|`%5al>qzbJ-J7SuxVZ%db z7jHz08~>xay=LDM^qP<5k5(v%_}uAuZijXQD`T(sAmuiuZhn2>eKNT$czxb8-# z`QHEH_1pO9yrBH7pF@f;R@HqM35s`<|MIf*?1g zxrO!VifvpahlU-!#v=Cq+HCWeG;|29|L84==!-Q{ZXG0)OOEg)hHnnh z8B-N&Zg-ZSVF-AnCkWAZcto*WG6e6)?%5`U&nHt3&(G?m)Lp)8B+~Xbe(Uke+;;`1 ziNL$cwr1ibE%^svn}Alx)TP(4u`Trf4fS@c_DUT@=`PK*#JqP!F;so2+v8#$aq1(J zBe39_0vScwid5Visw{V^ytwbzbS~<@@)3!g)O+|DH|fyu81lL3joV=IGWxfZRC??g zrSfieoa^3CJ@KEHXRQ5;)wX7}^<5_2mt48`Dp=@`LJBx>uQiX4$Z|i-9CJh^wDz>l z9gLn=83jsNK`6EL$8;8Rz$-b}q3c-Hb^1$u8n_G|w6>6W%sY5bk<*blr#Kzzac&7@l!1xic7h@(IYi-JMXJBEf)2+QU*=E|5TCvk-P+}FFgkIYRG zMX(?H4M?rozkBDicnxCR$Ek4lZ5%{B2dF166unR4X7j-*3k$uBd zI6K~n5)n_o9lndJ#s9e+;v~cPJHb{Vx_R={{QMP~`TUlRAU<@6TAVSsaADyS)<@wF zFy?4SJZ65C>3T7cOdQt6Ca#r}*JxM5e-Md23pZX#n>VW9m^sKVkiVj(A53Ja50D=J zH61T}S$3VVlQvMAgHE`Uu+UPTHksdKsm$|u7Zsr^d>bsvuDw2l+pQM~;q)^&JV|sI z`(jc}<86U<+7zhYdSTKYH|Xtkbj-iNt+KyT9q~1Fd1plGtE`6E#0|ea<^U&$UW@?a zVWg&m^VbTdKJ|sBrE9%!Jzv~JVzk?&aF^Fwd#;Yt%XMLRA6dS)PYPwOIQvzf80v(a zY>Af+*`cU!w-NSfJCjD5Ihjfjm(ye#FL#Rrm~iv2wo&+fXb%{_nh$Om{cMR*_82Eoge-B4N9J;h(Rk^S*+l6cX`LD#D-Aqy^CPoK5Km?(q&tSV;cvmq1YqDWM$mjG#C*fw+NfP+Cm+!ERkWcC z89n(g-|#gZHt=m5J{)I4D2U5mFxjNw&^)3$pI_B-l$2q%AErVz5qjB9OL zyjv~1k?_c9@NwCFL7OX0x4)>6E#a1_Jxpd+5E$gpIEg;`0e>;RF}24mQPi4c`HM|M zz}TZ)!KqZwy;p;bopkxiT1!Yz||J9d7oM2N+b+JX;bPS?W97J0nU zxot6b9vK%=;*pw7jirl5QN3_OEmGoEixt8z>5!X10Xg*nSVAqRjjnMo=M^(Tp4c*U1#r%Ag&!T%BdSQQGQ7h|aAF$c3 zccR9Ind+_X3Dewg*TQyrvCK)$rAaJ@nih?@Aam=Bk{V!Q`R7k=GLpKlXNpHt6ozxu z+!qQY$Zntws~)T7OGD3fZ_b z{^9o{lsvOsIu`A#7Y~2%8xNz*_(>xaeA??#c&*Y_^xo<&m+S}+c)VPa9<{Jk`Z=r) z9M2)NT81%`vLXCw9a^~7t z2Cc7Kr#oHkYBYXM4cV4$<6Zx9KeMMAn@-cLhg9ceCZ(O&BcEjH>islz2v8h(bKLLq zhn?U-=i7cNwZ&6nJualR|BR(&IHVEN?t-0QqfDhX(k*;{-8YL#B9cWBEpcnYmb1F% zIyQK5FidegR|au#FOV_#ss zi9^XJsQUm<6W#mb4MJ}gJ9+)gk3TfAziEvHUwozS$YMwCvol&Kzuhho!sa}|-6EgT zB~BbFx{X)W?y8#T(B0;XI5DL9)6hsMxrec1Yi(|@f~2j*B!+oO-#wx*bp;Jr7cG#j z;tCrwlP)jn|DX~P`?Xaa1!_htOciqDp-ry8_9(fp{ngO*h4r$gj9@fDx3}1a=vHk2 z$!?(|9y1o071YE-6`2_xFnU|T(aZI&jEu6q3`1+`?LQlWw#;cF8YdTGbxYNbWaeR^jNQSD#UGt&-i<+_Cx8Ndlpa3E6tZ zpYedPQ0cTt_Rfa9!9Yfj(;PoZv*uxd$*mwH;gSVE};8%Dm&uaTlXnCJ2WKiXLL2FIe{`?rF z{^0s`>88oaGq~>dwVU}gdqz|5(Di-dedMtF$y1F(jk#y^4Yj)KnQrrC{9O)L9Kn<7 zpWMA>=lfOwF1`K^&M?Vqy-~=zGLYfMn>00A;i228JRkZAEzp#$Tr@i0aex|tjg2_T zwcI;o+8T}ix zb3=mgjWwQFKAdsl&*mR$QmWo)hu=#?!owkrZFy&YMe!!f#ThdjSP>4rVM6y)p@*N` z*aH-^e8XWSH98cZR;ha2($K4%CY)^}i|ykxqYq_YOwJm?gX_1%XvFq7lMauV%6#|t>{ zb$dV>vp9l&*L-QMmDk7HBQC!L5fw8Q+fF;bBBb)n1d>)fIqH8Q`S13^{a^iNZQE5C z+n45rtrz=HgZrfVK$}Si$peA`_6j1Y2JAY8bYp8R(^Ws{VEd^Y?F z@g41i_{1#MNbH1>JHWnt?cSt-^Txce-kQhz_WpF;6SurIkOx`bMhaa8#FU#tG=s-O zMhX$Lf%HIV=2I88)`+~Hrrjlwj}kb^FL^)Gzt)q?<>Bfy?S~WC%p?* zu$QmGjy6~hk;>oD>NBqqHf5Y~uKa!XAAN$WYB1an3cq9?wlg|4DQehbN{2s7L`+;@ z3VdYN*mFt!SuIvB@b=a`Ya7+GNN*aOBo}zaN~>* zS|3|D8I1;C{p9jALpx&^3EWHB`XTAZ;oH(?tTIlmmRsnivkq^RUWtTT zG9nl88Zc&n&kj5_;hovXip9L6-(~a07y?LPELCD=`wS<@-8~K_7}k{=v;yjnNsf88 zvJUbOwy>K0r`~{P!_>9oVfE&{1Gwo#HBTrQ4nu%)Pq}-6qfso=qdZ#aq))a;MlDZG zSs@r1J(tn|`4=&QcoyOL*Z6M_ZhDFH(Zhh7>rf^8zW-u?Soz>eAR`wLJ%`Ks;eE`n z^}-%zp-yTS3Z^{CEaVna=phKtx^l=(IEa%L^wqT;S12VLg_|dJum3O#1XIaXHJXM~ zyWf%7uy{bA>KPr4;suQ!@^8uN=fYL=VF4e-+ad06i@v5n2|$A}QacwA(#GtZ6&8E_ z+4m!A(Y)@ppUA>ltyVYnss=KBE)9K1HK6FHZB^6ak_!Q9NsDc?Rwuut3p{Jka=T*2 zK?;4DA~~hBRF!dSTM^Y^&Tb>J3Prv<-*N5DOErE6@KnF5CxHDdTy;kc?0YsM?fzyH zEn6lE1i+0eht=Uj|? z*163Ndj^goGk1;&Z%ZR(`91hBa`T0(94~e%2WBjXCV_8lPimblncTC)=%EW95@L@Z zVdkD6AvBs;p_XV?FvC8l`ISOG*l$!vkX;0N@*CGziZa7h=*5KG$VQb3N4vmr^m8eb zAbq&?smuIow~f@j6n}q_%%du8%FQuIAM_CR9ck$odtcB3t#qS|gC(Vnd$IA%2Ajwg zkpCE-t8;*gn)^2!=#7%;JmR;eYFvk=?^uD1hjR;Iwix4lzwWMmzvjhX?60dxC>k;m z=^cDyf5s?6HwTUYM|qfK5!$kzW+za>@X+my~A7g6j19zCk%4l5a{gn@@ZhKb;P99+4yxw~szFsSa^4yD1Y(+xhR_Dk^K4j75&gyf>j9|w*Nfq^%^UvqSgW{bN5}>%E|3= z{=%H0g5@}OT1Uh5Y7$yYd-;A0gYLE9vdY_T>?pQ?K(}M_p?2(bSFajbSJ8JFt_Qhu zZE-L(PP!C$G{J~T!#Igdw^2HXf*1l$UTs1ADB>jj=izZ&4ng$_1?!qc^O}Re(F=#) zyB-ax=TmxZfoC2f{t)2%EOk{YZTdo~+(iLop)32IQeU|(WDCZ164`HZ0A)*X@w4G3 z%isdx6OiTqy9uIZ23tK{ykK3xy7{O{O)M!S_R2bm?8?37A%^~7K0Y?LC_BSu!iPJ> zzcGB_YSDmoRIxGWU^RzpGee%|SttK~8V3BfRTnp)pla&1%NrS>jyDwt{k({%;2a55 zOY0#<*I!m7{VdpzrtOzfV1Fy(;H&7s`i@KUWqaw+=^aBG{1TpxMDoP|KEzr5(GEzRv&ZvAXC7w{}dfb;6WzN8^bzH)W?DNUHuJetJTS59J%E@+W zQcit0#6*4E`cHYq)DlaRVh|cyENR74zzM%}dFH)}AwhkCQ*=7@+jNjnwc56i5uMfu6X1Zz?>l zlhMRA1p-8|8pwc=79wMlnUTbVJxXf?M~T9thFX}B9KM9cnJ9a?@s%X!%eRwsM4E>4=9H1E<~J|Fcn(J z;0A-L!2?&vY`oH-4b9XO=jH7x|G6-NNn(zyppbfrgSs7P65i|=0rF1ki0Vs0vT9&R zt@F5mJsVWrv5(qaFlPYeHKwGv=h=>b+=j!TRr#+ClqUdtWVHzebN<)s>Eotz{zwS2 z=Nn5gr^6o}tegi16)jkcY(s@HKu_(iuWk6sbj(V43*XlzgI@#`st1wYF!Go%)_~*7 zdF3|1(5whH5OsI@gYQy*9cV?`3h*1}d^yg2^Ca}TqSMMQ`hy3S|H@hUGDfe4Zn#|E z@fIhp|DMwrs*mjtO~vm2EJooqpr~FMj}QJmmPXZg;7^rq7|^pDnLR=T`!U z;_d$+u~)dxI@K z{V0Bex_Tn|H^~LwIJEYK@trE#INow|R(V4SeK7N!;IQqa(YB11L^N1nh}iu@FnCyN zP7(eT>?oC~k`ldYzCCwLjlSX{F_T6f1%IoLAN;wWkW#j`ljp36_jAEViv$^>X_=T` zNgD7JG)|xTZ@PAlKDa=Mat4>423GpX^lD-rv(dYq3JME@F)>~(NYw}^?@6Y`kP3S^ zYp|%bkSfgd?#{n3K1-E7aHN%2K!$*O@NVfFO`H_*MjA+Jyh`&63On88w zR%bByFL2?aOwz3FY49mcT|U)_C$0pYyvz{0-{F@V2DXdvB{PL|K{|X+eU9eqm$Dsu z1Zj?nMdQIpHBE3w1v-ZCBd!j=EyjXPsZ5!Tu>fAr9m)%-`qxAk{tFanQu6qFhtHu# z8cx4VY53^uj%HU6Qn=n~AV+VQq3w|Al;ATDw_6ZZbo`?%4Se&)z|}yPyf(+scl?k6 zTH0*GBQ9F~NEe5g@p0BO4zPPRE-Wq^W=e12FLo`f88#9)FNE}Nw*-`6fn5XSB6{dU zL)lcytA8NhN_^bk4^?9U69pR9`ErJJkBoF?T{qUC+QweBM9-+5sDlA1TCX1?P~eoa zDJ_k{X-lJ-&qeGhXC^jgfEB`0&c?aGz9Zx&_T24GEJmp@k<(lcy%oo=DgRQfl2Sia);&?4JuiWgA@ z*@wXF^CB4ALlh>&=-b7--)pHyvLTm;UNVI|sQ)UepJ1FLDm0wSb6sO&a(p4mDBZ_N zPuGox#)b1OD&+Iq*CKXlD`X7~O~3)Fp{REclSS>yi9za_tz5e$r>`bz^!}&Na1uhZ z>QLMvCe(MA4iFCguO!ns-b1%T?!a@{Z8we2VKP^CA$5OF75hH7`Mce9VfWDyO`_HE zrIW^*`ha?rYRvl!a2p!8siB1a;$crjFebCbPkkksK|%=iM+uJ1>8nFngoG+=7l$D} zpyEGVI(}dj-_gEEJ;70qyVLXvIbD~OX85)%RpEprZ2oBP3JUTbk59(Bi4UHNB9SER z`|fo*OJ2OJnOES6V9^(ulqN$N=H8Iq`1oVwzO9^5`vJ(imcspv>2g-npR6R8Lq zOYNA|{)QVOdnyTaioM51^mynOaKKIiiEUsYK-X6D;LzQ9D{8vV;+*{6d70aDWd_S6 zKwO~Qu!V+`=NanBje}SFoH9qwhF!chC!DgDm6)|dQwQ9X$u=)2?3p+~+!3P4$4#wH zldKj}_+K9S8MfU-tSpiz$8-U2Q~=th%DI!m@@CkqswrzjxuqS2Djt~}b)^j2=uj%l zkBw|gmxNmJl2eT!j{#uBHmW)_dp=wgXaNU&UBNjW7h#pQxaIfS_~*zQl&5fwUfID9 zH~f~ZkQyiV6F4Fa1oO(;3awf#ltc=2--5j7-!5v~yTYou|0@;SZKda*=)80u3SpmL zdT*o>B$6fLEmZgR+mm5Vy*4G$w~;-aQWhUy7T-q#8GU8VoTItmxR!h-51QY{T;DI= zdmpD6R^{JC>SqlcoU_%;D8NfHSCo6Wz=r7LSVfS?|I-3ouY3YVmcnI=JbGbIi(peT zoK+WxHJNYd6|wY^r9nFbkg;D5z%R&rFUPKKx0ESo#)az*ES5rs zIxdJ+!b{yqSl!B{n8`vkk%^tMUBG?mWPqeB_H?x0ullLS$36vbP>&3GsyT|J7gmUC zd9+hK9p4M;3}`aZ__=L>xROGRJSd+QUxs`#_@pOD9U0Z0#=EczPCYrW(`UJH+mwNPtHady))2 z{mJV$)nN+n^2cN)^VNJHBiz+@*F8=UeH|*0{BcGQWB+h0Y6rRT2CesFVus z1d}=C=^ZBI_AVJhbO8lDHG2U@H2A7AlC!I-uUGU;1Ql}32u4e|w0cNe)GP=xqmyAu z1i#b2yK-|?AsBsv@i*)ZX?NDB>1`svWRVsaV`3QaC}{2J90ng=F}~$Me1Q?C<7X@z z!^F|=xz#HC*lO@SXq61=|8lb`w@ zHC~RQM-SUhMSVuu&7~z14tBPV4O{8FrbbSFuZdUv%;l=bqC8WZAbGCdfb9Dh$JQfy zKfoO>CRg(Y`eFtktV2DPv9UkBQ}C$(W+r3`a97`L#6zos2%cSly4Vz;!i}7&5>j1{mwK zH(%rHg=^!vc$Y}R5f!GMY~3Bw_E-a7;S{M5{eyh4dTHd zrxA>{R}`1fOFm^CB5|>#D{dS`jlw-J&i3z~+8=}d?F<#}hT&*)Xi zp3(Ho-5Eqs<&4b+Z7{}1RIl1Xw~ldaN?AoW(ND&@gzv`P6zJ+y3*2VAhA;I{D*;sM zYUt~CwfeMlkB$bW=a7N}+CssZ`5tn=FR!+MsG}dx+pt!)YEpp9XClT%2AXEd<`6+N z)FGhnijtiDJYIKoOyA)FZ?oEmrRj%i64S(%a`s~xNy`F%7#Lm+EY1ka@6T)9%rM4X z6jLU4j=h1LS=`w%`O8%f`}0HpM@5tb$}+w+y}^Yv9ex_iw>gC+X>=)H?nqFX;hnp! zpdxiOhFB+62rXlqKNtHzSzMR$KoV->(w(yA@)XnT`$Xg0T=A6 z6KHo*bRc+;c6hM3>8=Aw_!&k^8JZ$gA4?rf_zp3dr~ViFxJrhxZ$i>F9MuKsH#xef zML`Xlc#%S$jY#x(4J;JFF7v3>vE$XM0#VSg_zw{iA3N}I0CMC|aK^BLBN4F}H#e$4 zw3{_z1lt2T#;AWoW7_4{fnUjWGHI&p42wH;0CgEBK%8L=~g>DbeMWfr~qwBE}kBI9K;Ge%Jk_W|hR2&GU-eOYIy^}xdUk4I{&MB|4Tt|U^D^EzKstq2WK0a>pc!uz!-qkoLgUa6*vDZ zuc4vU|LZbT7)g(R|Hlc9Klq2iJ_G!?RYU4RjdiR&{uDnqX|$2$1V@$nu(K5#7Uq8x z_?z)LGdCX@>?pt#yn4FFB@0Az+>xN~BBst~Vv1n( zM(^Il2wwUlz}$WAW``^;JRB&8h_4Upsi;P1I*40%X-!VF*vK}P%(K*#bn#T^O`~f? zQ_OtT@55`dX(NF(`3J$M{|H;}ZhP>D5bk5ExZULW-+*zN5rTpqW8+r>Ji-?H z%|HzGV==Nt&rud@uBq7IJw&AsthEbjexnv9{nP>K!lHP$_GOHi|lKz+c&s;rSKM95<52Qf_2KFs3F33@fICV_;A>P|JklwLA}m`q{rXqWJHkuh6W&Ep_A2%Niz3e6e=lHnzKVlsI3fW7***bJu?F2( zU`he1km1oJe~gR1y~kz5#KoILeSKf);vjZ^?*Xzk|10a^a64Bc4`4B@q1P^6zfKZP z3ZOM{9tMoCp>L_&dRSV?aQZnniS8_qd+>-C`vY9++MGD`lH<-_i{( z&{thFbk=`ZoC)Z4`p>{A*IB147;+z3Q&OqufvPFtJXk$v_UR80g)SU)>exXoJFXi| zdBr7(1_wrF=38t~tDzLq=zUuKbx_)}XtP~Nj>(jJ+YMM8!Loy~aQ$BlNyJednRnia zLhN_??@)5?wv&-zwk!G)X(WrsM#u{sMe_m4RtP*Y-&KvBR^kK!BK>O3Iqjq&m(-7l z%<4Xupb;wzoFCq7N?cWE5wR)vVd zui*00yOZ`wsKsL-LqEb{q31DY4Y8yU#vs~g*X9>(>V65#$hilz=@iqqSYx;L+U|m4 z$7Af4ZYh{>Am_X%eNC9{rC-bv@XT0Fz5Es{|5vE~Lm&TpA@x8G>-i}T*SdYE?^<_( z&};M+m>#j80IHe!*cHb~-ojN)ayDq==wmzNO$B zL%;7+EYRq#y}zV--HrPK#ve0UIy+a8_n501`Fpq6{xeVt;iKew(sEo zW7z&pYUufn0Dp1%6)9EZ=pEr)OmdB@2~yIu6amPYoz&6s3=Y&pVVvew2jlNQ z3sa5*RNnLK03Vt|ouE^s5u%7-_+Vfj7EK;jpG5ty)0*Jm5>jwk9d+J?Yf+%o5KyWqCLV{U5zlC!k!~3 znL&9%68aGoUb77f$&Syrz{#k}-qehZT5ZgsIN?byyex;g&Ua&UdRXVfZJJ znwLEv3Hmp1dfh}W_Wamnv9pL0i)mcXy@vqYq#yX180#EvX&fptSsWX>$zWTN23X!% zEuFzzTyvX`aFrnZX1UJB@1BoHjbeD!8{f5?P#UF-+6fy?GlB6%Yjy6J-deolb^_#Y zPqPt2S%W@jt+>2-1q_plAB>C+&Uu)2B&}%7xv%5i6lhX(0*mH~)H>~G)}B$Bby3Uw z0XhLD#K(FGQPie$7ygk9*3@7n!M_g)hJ}Um2EnD#4G@;^=yo4RL}+n6bR<$|oqD?J zXdn%gYz(7NX&#LwN>yEoR;cJbI%c_;){{vMy*v{&ApH{;&yo_F*<}XAH$WClx;+S} z6L?p|v9P-Y4QiSGY&f9hX{wNuj5g>^m`5*hl2idJ=BgA@s;UtrATrY|jjPOLm-dIJ)Wd_R)o>@a^@@h(;!^TGdxgzla-5? z62U8J0f>s})}OKMvE2vt`Y-%|lUd=07UoR-h9S_kx#oIBgO`5n)>mul_=cS=089ly zX5VBatsw z-0YYs3mO0^ZGk_lp^@L%5@6!_gKfppoW7!_?J5W+H+KuztN^X|-dg?wCG9H!xP+Ex zPbb7Td;v3izTGVvQ)`tqP}JdHdiUm=gA7egG75?nyX1x|cIF?n9+~<-q&(z84rslc z-j~V>Qs~Iowg(8P>w<7?Jip0j=jnY7JSN_vMmquU>vl$WBnh!y@NC@FOYRB&p# z^y}bAi4oH%KoS-nIUS?81ASFEy05kmglZUDg0 zd}V^e@qUte^*Le{j`;md0g9w*Vp3(KjDi9Pd$hX-P>fzjF*aB_HX2nlxF!kim^c=i zb#rf*s!+`%Y-MQNIk>ePJa~9Hx9E@%ArO+XM1&73Y}0-(lQbR=?m3TAF1>V{%Ln|T zud|t&Wyb4pMoHw1?3CkzAF5d9Y&4GyX(UZw{1*pc{B?S>i?^sEgf}mB)zQ3igU}@8 zBD&J35OU`VihkuGN;er6K;K){?NEPIv(}`a7DhdMyM5&Pjfj02Z9oTQhTu9p<$|8= zD3<{L#U~c)kltVfN_D71(dy5$I-$feVef)OLIGAJVcdrU6()p0BjI6XC!HGSd8`f0 zM>taUOvaM`SG>2WBCYJrLnN!3`@R>nu~ypid({Pb;{Z$qaK8_5_E?CR9K$t(Sp^1+ zqy;wK@rsS-|1JvFyNL5K{w zk>JP6ozxco5}tjF3N;@uuC4h4K*Q$(Z#aPH)!YTsVNGj6)9f2>1kZwlC7^$#l0`h5 zWU=k+-8qJ?Qin0gvjNZ?0^t2{<4v{Jp+$|)fTjcvzEU>>e!PPJ0Y7o*7dqWNA%LSz zr6`k@G1BIZ#|@s^38d}u8LbD*<5q~)U_|UNiJ9cAB*Ondv9(O<^C7-qXDu>_Fr5A( zE)UWbP?LfE2yarQ0c8`5zVN*^vCo0lSU(NM+DtP1nSQj#ej{PtelxM_Of+8@oyezLZ|5|ES7Z>tP}}Y?{(idkV@8Ue4TR9i98v7ZJ6V zEHSh!yvz#O{aQ;3I*WW2REm)PuP)*&NS;V7#dA>LVE>z;afiLaU6rcMX^oMm_3sCR z4Klc?KGy888Vs#3 zeJ=J6E%?&fb#G?gN100`+2gRkD` zD{n0PRFuvx+X-Hp zUb0}m>*fp^NA-Q;rgb&yAqT=(?E;)v-zJCthGwHVha1_VM^FT^ZWl9i4)kd6&koJa zODf(t-SR+8s;dP>-t7={cqArAKl=fm#sKW1(Znh_=JCZABG*4U2Xw6E!8`Pb=jrNGgwr zw$ESa)?iQQ7otPgUB_hw=irbD{JxHAY=UtO9unWxRy>TWPM|s|U|7R?$mpNm_T?le zKebQpt1hA-?X6;5BW+$&{N19}wJivB}>#j7S;T$ZuXMsxHx)h+Br9 zDxyq0GTT3lm+*UvkzwL11OyK`l#7^Wu=&1NrDLom(BBr!pFBMM2J1vrFi31|Z20m?zPB(UfIlo>!EgAMi|X=1Q27QBS;(pDAVmx#f6#N$eirv1s$N|n4m zQLmLCesmWlDVjB{Pt|%b42SOZ9ThX~NkdKSF73kg>!S%4EJr$;?{y8qx>kzS&A_-U zgbL@NUW-6^kruL?5Y`zZT+kV&r2!J)C~~Bc-et$PoLC5k2) z*dVy>l)l!~C<`C9WAtZAg1I^^EXhgzx&N0bm@6(;mtLR;Im%#E*=;{Xo3f^AU_zz7 zIoi;@8}%6v972q##K&RykqyaT!T?0MK;7ELN<}4gd_u!$BD*aPkcGoInv9ru>XpfH z@;YCnV+{D?2f_nIR5o`V?aPSaR28eHdMwCY7!hU9@q4pz-BRX!H{1o*X)JtJ$6$Ne zl*3&`EXCPEkcQ zuvnsL)0Gn`rX8m+u*axl>4zGhyw)g?TBnz$e)KomxU}bX=fn$ZtQE-(e$Of;fR_$} z>6LhyvxRopVNAo#4oKRq?tS9S0#dO)0S4%u=?!oRQav9qD{5g-jSn|H2_hft* z1K?n`2ubCu`>3fM#sMQvnFfj?<))7iN5)L?GG&~esMznaO_STFwNfpYjxwX9*vB?V z$p(nXiqOUxW5eg)NUPf-T6;(+qWhMBG~z4u^9(vwO0puJWM2lVqQl}8;-Jxk!)ndK z2+nY3_N*wRW)+vDdP9D>T>!{r{o%+Z9}!43?-)Jfq1DMtK_eclBq!3ptrER$la^9I zS|OfDUW)2`zj3shgicNe5V5M%TVE}Hm0^^}Y}djFk_vtX(|h~=rYRq_+spF7{>q$9 ziPF4f`%SAV!>iJ~iRkGMoAic`+J@uQ8EI=dijXr(U%cJYDV<*Q4^PmTHi@(J$8@1- zZKOXVZ&tVq^~vAlEe*qWk2(hbB#nhRtH{!&SiAkj5ewI7__|%Ba8wu`k2*z7g7JzT zBQ%KY{n&Cqi(5;6Fo*T0@o?IJNbFTFnbpwcLv~f?kw^A=v|A3{%jXy`w*_Tjr}+RV zejkcYoX7h0Tqx-b(B*Y%`!yYVCRGO65hK%O<-q=ev}y+KnsCU@1rfD|#y{v#NSdui zX{_0LT>eljZugfK+(+wZ=3--u+H(aEd4ymI4=!L&qKT?h*pK>_=X3At@=GUb@Ro4;|Nga5Xorq8~=|caI(YJg_DWOfrKEL}+9xPDq6qr_Kg8;Vj>O*!X=Z(x`o1D|_VdAH~ zoc(HVVz%nXAv|xr6feET+cVaWDJ*QD!SxIj1OzRi z6(02YDy~i1uMr&=i7Pl7Qo>79W=$b%Sr{D>o{!b%Z?{Cyd+_0orOt;O>oxyc#eZyr zQ|fp?wZ>YScwZ86L05S7V9X0iYV^Zq!vLWnN@Vr`wLdZY&PQF!llVoNowQl`LB^G) z9|qIcl9_WX%c-@;}=}#izds$p}gY^K%QhNr!%`?V1tD^vQ_&Psd^Mj7i zEhLn%f-Aw5v^$^)(=)TZs-lN0%6L#(Cfqm+hm7>o`oJ&YQshA;5wllz^_6nNqVW#z5NO!_S>MO^#K~x%nkZ6KV);l;@H{+l>(Lvm`|Aoc9JW9y zS4Z~=$ zO7hwvxxwH>hc-6}E=j1vY+uA-PAaSJ{!J%P9oZP2=}vQqMWw&tquF{R53~_gV=4XEy-Bb`Vfn%l?zL<7{HW!^BAi9POK@Y{FU3h-F6?ibh9tM6`WtZIq~&58^yy&!iXCyIrZllVUgDdI74xhf#8L{-DXhB zNm$*RVsV$PdFw_h*S3xerXS5*fh)e-Uz3d|KuRk+ubf9oAAf5+8YFK-K}QuuGRF55 z_!*Nm6G~ckV7NzIl6X>YcHn!KUvAj?Y;E-tPG1UR*Fi~q+*W!$>}(2+7%gk_*7vIT zBJ#4+KwQ46{@09B=XIgz*QW&=GaKXiV3?fG2x4U@ZQr{d%(c+U3$y(FZ?K!Zgzf&l z*Wc&pI+AO89Q@P`mO!MJ`t`fbBZGB4w0)ey%oK9@DfQ*kvsLPv{Ifv|@5DBVtB2{* ze7;J7A8o0~m;!?}Scg>|HC^HZEb+G%2zNYAUw?3Vrucwobte`FWz5l9d@7SQW~Y-= zov*wuTwl7Tnx%GKw;vJJWy!9>FBLPgAfYsy_^va}>MvsKaW4Gxr>Co*8v90hGH1?; zNVNiq=dZj~dh#7XULSW+%<__JqkH;bBXwZRLSp;Fpua3CjpSc$+?y1SFHiQ&9Q_G< z<>t%cy3njPd^MPd3+mY7h}sGvl6Hmen~#!Hys#(0XgD9xs@Qq^3GUw~*5Q;-qp7vG zEKlMZW^zUUIh=2)PqY~+cYaFp}oft}0lPQ;wnOEQkbFR`sKMirYn;?AQBjXZ{O@Qk0I=#cvPhAepS)5+PPRct$6yM6TOC z^Nq5A;!m{n2X4MdN9bz^YeIt!HD3k)B&gNS>-EmoPyYJrxt^2KisMmVfzO&d*vgAR zD7)P|cjcDX_i9kKJYhM!%JsA``)Y-UScVK}4=4&w*Sn8I`XsT$@~d2)U`MX(>CnTrM zL__yn{_^5|Si&tCJR$L>My^k@7xz6_@o#f5lR2q&Hfoqf zO!W9f)v&9d=*?;M&bt5Pze?iFtX$-|gluIQ=~=GF5yfoZ!}NI*iSSxJk1cvH5vEq2 zCew_cFcqj$>`r+JN#}nmgq_DQ&eu*=ZHeX7)f<{XZuj)@on=p z@5*M0rIq`8rq}Cj@pMG+y?KjGvs987o*o=_9?rTLS+sj(<`_y@gS2x44TurGwXmbu zhVFEq9g$O=K}-Z5H=%EtP(<byXuZqb`7g&8vXeb)el%%qi{^j)4n8#FJG8+^tS zUYZP2BsFFxpV1(73T{pn2;&sfM*A5Qv)-_(wFPM0=p6mS zKA@1n^kYmo&4M1Yl>QC>l)U?gBf=Az6??X_XbZx;F6_OsE$okpYqB;JEBcdf*a zi^czTU@`r2`Szq1TKVagm&Tu4^)w6B)BlemKT1 zcB?(y$CNPi3S-FhFV%aN@TxDm)#?VWIWMx2FH#59;Z6JCHtqZFweCfP0d|N1_K?Xi zw=^#EWEAXOwJf%E>YdrY)+vYx7&eqlNJDz-;KQ4BVfXLujJ}H3jkvm{y0&S=#+B7b z0u0)UGkSdAwVxoDZ6y1~py)YfDdI@>)vitHlJm-}hwrWv&%zKmE(ZKicsGH-lArXn z&(juvg8j?*573(7qn0)bd$`A1FELA^V zCD^{7c&shH2CGam(-GXvJO&KNdg=}_lDu{?&>Cp<-L&owlWog?o6zv>@Y{s~=V%fH zg%_$4D-WN0Am}4COb$>9)5BF7z8=3JK#KCmeN=jHbOOnd@tupkWwC!w9kpA0xi`xw z@~*M>;tP~tm`-QF8Bp|08Q&jkS-h#>Uh+^%z_P-^IGw;pj%A;^zC|aTxb^7#F!sa) zs$6E6TveX_N4MgukI?uoK=?aZ5v}7ewa?w4DYeQJA6EU_16QC$*)uz`U1VR+^`aZt z<);^sy3n~(XT}je$Kt;gQN)y^x2h-G!g^%#Yj%=@%dMrWcJJ-ya4i^u_Ju=}hn9Q) zbK-?SBXWCA)N>QsCA%V7l+*mC@gl*0Jo-srL^Zc`=K0UmK*%K>w*Q4%$WYyrCj*Clf4SA>fiycCNzvHOt~dSg5X)ABlm|qf;#-pG(|E4(K+y%^xW=a@8^Ti zn7}26N~cp}%kH`q%LQFb*l?^)GR7FTN!BxIQ{@T<<8J8K^*B|&>K|HMF^)^)?EO!V zI;}b$5^IVJF7cKBg`=KLL}WJX^~yNRgUl0Zbuw`Iy*r%+Qt7u&qZt{l-10B~1Q9JDoT zu>Zx@2{xwebe_pf)5&;fZVzTK#z526ZaL62D0lAJGUrxD+Kq5+6L0_AOucZpeNpay zz1H}bHmohzWpBsZ<_{|)V%4wf$|6`~RCh{a-4H)!o7oA5=V*RkWsxegSkp>;0G!<_ z-;BgV`+ww=%h*mn_Uu3cEp9W!Q|F>N4(axfzanY{%{><*fu~Jj{iq*6$Xn(xu#2?a zw!a5HyFPwm@#iX?UF>L-F0tecp0xd_*VYpf-T*h=3fFDMH@<8q`B2_#a@3ZBotNlo z=6yHb%=?<_<1so@f{>9n`}BA@Aku^)RoUG3i9JrGc$q?zXmcX+)+wH1C+INSecF0U zK411+?P89j%h%JyjVxOREtCuWM*4%kgP6(sdeVOO42$WSoAJ-8bKI2FyE8t`oWr_L z>-VjGEbS2VH{&<-rHg8K6kNI2mtUI}-#fpDWswEQ66h7X3X3p!msb(67baOr{ zwF-l`pEs}^!)Y`4E*@$k$!BafmShF7*^3WY*ZK5jW+UNQF^S9RBXK6f2 zx;HPPppmu`26B!Pk=1$&zpSJpt+({P;fUnJ?g{kd7+>hK8sCH!q|g#jf~{)`<}GTx zMU;wp8_yyQJxZUo$%SKM{s{{)jlbYwTH=CwJue>r?pc722kGuT;(N2@;zpYlOYocD zdev`rU*8BC9>@r)EN|)S)NzWc)$YJ|q+tQdBtQR-lB0G%;nRo{Y-T?PZ^c~@k1{Ra zzI#%GA5o1`b)P{TZZud^L{Jxz;C0$w3>XW^?(`ImD8PdgD2W}7&z3$KpZK#@!BIRR|JC~?R;9^MY65?Dg3mJA zz?V(0s}D}p7`Ma@eHk|TwvwR`=2r#Y7J<|)kS>HvlGhN7Kl1^gTBq z&)mq#uRiU44$jqDaPyq5Yk=Pa5A9JJe~vi)E;9JuUQ=$9__#u)VooHU@ZF0I9Bif_ zzbwS-2yU9_-7kza*^tZ@r{{_2(mBlI#xyknyA5Y@ayL;(oZLR%gE1#Ck|egToywoS zF{zq!Hv)%8x>5DH7ax}A^fDiWf$3D~Cl61nLNfB+Z-VbM#Rk{CdS-K>8eH!cwmQ*M z__-pKAZM#g4<}mj^{14M*E8?tM7Bq7pA=k9Q|N?NV|`$EbNzi|kF{-NVoAxnBNmY1 z2EDGCS~t5cat(S?>vD9q&-Y0AAqa+Bv-Xg_>8lGju4edGf}okC98-{92sft>z9yFm zx+j-G{#6R#qd3Cf=1#YA?R@Fta-fg3xHV6w7R{-wmSguxLlxPzYa33bmXfsQ^)R6~ zM`C@TXZ9Y&7`>xlP*}ufw5b;T-pJ)WiDfn~DBBKW{$=5bro};Z8TBSibbiZMDRPY< z2xYw8Nkx=LHHop)X9`!7=*Em^rcCzW744LA<6bTDd$43^99nk#Q8SwCP_u0vt#2~u z{3g#Gaq{rJBm@y#?s6#(mg7<10&Hf$wbI6-&~5385L9nUH=;_Vn^_cR!luMIT_a7} zZP9m6aO?Bl6R8TS769pUvo%*GS{T(jN+{$u!-XO}ezT2ygt#-&k{xvQh+GQ3_=qoh z6p-I7y;H2FWeg7-rjdwks_9Qflm4|~LF6q+r8qXvD%w*Z6l>pdn(#IrXSZ8Mb$)CC z*3Z3gJM)S%Q+B$5B$o%Rkow}vU#|YB{NP^v*hz*jy-4z(uJEGa8?Iz) zVTvnB`@-vM+3Ql|VHHV^+X}R604Tb8R>EAH|1N#r<04`pXFRy?4n|(l0M@T8*?RWZ zo^5Rw%o-Q7oxZC{&z!_BwY0mRxSq3_AhnBrkK=?ql^Zr>YOSA^>Z^I{j_0EkQdY*? z`#Pd93^~Gi$*dZnpCEf&>rJ9(mQS?#1Hg7d{+XM0Uk@E*q{lJJJnB4VV!XoCA%4BM zf$JJt=#6G#*5U?YUQq&Ib7S*Fz>&B1HItR2`#EqE6`gk)Osf#%V}u^GI_K99--Qe zjOkuTe>+P&Q^Ko8s3V?ftS!&%(YkfPy}(=AaH=UHxAR>c4~cd<&uM==g5&wl889eC zKAnvJXQaxe=$Lhno47}zGpZ=AWeVf9i!Q@Gu4|1K?_L0=h40QyCXSCex|qv}kJN zszIhjlk9|s$(OdD{psc<(?j49G7%9++vL|coGs2Eesrhk^Tx?beAVc0B1VyFx5BrC z^t_*BcKMVcsoRfcF6G^Mj}iz;rr#m$1ne87Y|2izFkvB;^cbxr-X_47JW@$J zMyGA|@i$0rFUQo+A z%n5icXrggadV)RKGd6RHtMK%okv3Oj^lXI@MHYKMQ2l^F-Sc;*Q zWV5jz1MIB>r**Xi{7#;mYV(aaW#?iRsNP9PiI`l%pph_KXm?LY$T`$2F07~`{M)^@ zNoJpUltP0#jG5yiL}T4Pj4jOxhfh+w2ZjwCq`f1W-cO*k3F|tSU-~}OzIDttrHnh6 zY9H10`V^PHcIM9d&zxbY;e6{`J!ez2&0jn{QshjR#v`#pl{jJW6~yTc#%ojbD=u80 zfGr-*mraWI8YMxdNBD@Y`5@{(EWWwYOnmsQ*5IA{Z)F8saQhg}Sl(xvych}#&ovFj zi5BusIxTawMz*Y~mFf>lUo-^M**B+Ep(t;dkA^@AMvX_&u{h9vqfT>B9vg8vx~kjf z7&k99Rw5EUI?fC7JuwP}<<0?In19yJV{p06p2eT|#ul&52_SJbz8u7+kWC1hr^*6- z-jf6dJZ%#R)9jM*ra(`CKD<2a5N%LyOLp8jk;~uplcpShxy#@7a4l>|75ZBB^;n;dBGW-84oa;hdfbY0 z>oG83zaGtzvb`ZRw@x8@6&$tWzCdzK|_%X^7x&kTyjK_gG@ zS2rU^IRkQ>towYJELnLGWxQPxdzre{CmhA4neX{HG9IX-pxVHsU%yn$C$tex-^z-l z!sQLF-S-+{m+(iB|7n61k9(kS$aoZJTnRiHYBKvy?wzz!eJ~V{lFmuuMWDbOJnQZb zmo%c`BlL)Zd^D4fH>dg5Hb=rY^L}d_w@rAKae9?_g=$l3Dw6xdz}ksL^O)Uy(2>aR zS8IPp7Ef*~8f6av3gKR*_GR7R(|er22K;U-I$hI(kUQo zIMZt3PL`mog1)4>FBz-SE#5R$zw8O-hpDW2=ETwdWX=TN6zghUDbj6t4rjpNyIok_ zZjPrym9&Wz{ONn`zEZQ`)#Yc-w|68?$b*SBk zvjQMC8x~T&WsO#RAvu=6XKBhRl;kAMp|F0vN>&ohZz>8JIy)CX!h037NRKD5cV&fhj+Tz)_gq~)<`Lk~_Mz1g2d$$GO4Usz9xOd&Oh^+O zY-r!nVq^#J=+h@K4CR_Ux&z;Vk-EwMX;=fHBzt&Yo0nmcN?<4s=e;tx9`#)pKq}?? zC6bB5s@I)LmyBUt7FFXxX=ZGUxTj33*Gi}H{7nk!Ekv)KZY`!t`vFDY*Km)ot0a|; zEn1s$U2$)?V9munGc@{>#-*@CQmE=9GpY3GMq$3miz4m1K0hA@jB?pbp|Hvrob2+l zQh29cQpwm=Lde_9tv)(4C&lV{bHVL`jNFM%i{Y$%smWQ$-bgsvd05aTwQnCi-$Fnr zKt*%!x~GnN&gZc;{53mPq;~pbehbyMUSm-YEw~G1%Em8yQ0G|1c%(VECy|$1`o92<@%;ykvC0S-99d2IoT!JHHy^rS!f@O&>I~ zO(fVPtN&E<-snyj&z$QfS%L|2enmyEX$7ni7Yc9J4c`>X7a6It?@Ci- z3ZM=aCXuG;JI=S|a#!G{L|$T+?V4p|#X@{?w+nb2-r8OnPqUw8-V-+SQQw?0EO5~u zXZ{J5T#cU<0SZQrqB!GQvFM@%kX6b`##j+`*pc11lB=H!l#ZI?H{T&P|>U!VbO1pbao!OAGa z8o%d!7bxAJ2K7&`UF6nMml(4g&3n>J5S`*Z4vMYp}R5?8O(v4E*~3f!9c zy|g(ql=ne9p7#>jyfuh-rsk4zbtlqkYe6oh)yk1~iAvNF?OA=Pddn}5T!ZYotqFJd zKiNdMAGIz1TG%l{Un!>y&YB|X8*T$uRiE!z<{F$?Z7%R^WR=G`i3n~wmJF)5zFhcW z=HmLrS2gj+_{tte(>FK=R`h0ocKQo#k|Uj_&qtf`PS&)kXIC#)*2CFJp~8GD>9;6G zQFMtWyXL0f*lBI6W%*O_PI|~3jCl*D3-7-5BR$QrnL}J ziXyd{nhm}z0dskLKx?!o*-&lXWCMp+RR0KNQcgg8MSr9%-@FM4=3~2Yd1N9bIk!Dj z#lCn#1kT)etSm3;%QpuU4&i$PrnGGno5;X}Ud`)5w&H8QekTZ`jju~8D<1Bg5=Vs; zV~{vG(ZcIwnH@BKYMru9JsIqeoLgNm6-Ac2re8JZFFnI5Y!bWLk=0!3PlePAO~3ul z)z^55{autqLKGYi)CDIDFl(T*Y-GXpd{+mJg;J?#~vxCuMp( zNFWwlCT%Kki zSKFjaFS&`%a#rW#GLHS%u0IloV*BTuOS_s%<&L=RRbLJHqP7=(7+OS6_xy`3l&H)rzh%Fv8^KNKw< z8|sWUt&h-VWtdb|4azn76xGkAPRN%|IT)BMS7T9#Xf*n88Hw(8jc#&8hew)R(dyR$ z(Z)+a+mXDdtnczg(DNi{Ob+IsU(UxmcGvF(*Rr{>djyJ+oZ!!509f3ij$@`W&ezL! z%=uBT-v0sgatj>>?(l>-I&MBjkl%yIJ)=fO#PA)P(lpU93go`yTSi+S-SgoV1o13X zmWf}0gJ>tQ2O=)ZJijA{Y1`5Jg8#yWM>*HIHrvQPvXAoqj5Ccvf!UeF0>^I8pG{t^ zAEN|{g^fp<1o{YK3%}XrdJ?_*Oo94S4#CV6z@Tp%eEk(IwA)XY=(r!Cj6!PwE|^%v zghXN#5a6_BIB9XC_0P1F>BEY31-}8^opLQ-Y)eSdaqh;~Nhn#w<)NSih)L%37e4j% z#T&P~IaE1Jes;TE251tMK@Rl|&j9Au=|qEf?svcU*ipHLH|E|ypQHH~WNy12{poTL zEnpk3GbbM8v>G3pU{u)dRKN=G^y9>Pl^-tauBASAkoq!?)|@i+UpXi%bT}dybvgF=1A3Wwlh1MQ)vw+o z9=xH5x}V5viXtQ)0~rQ;6*xQ~WE@>atHvo4*@&dFxcNqD9Egb5!D5%Cthi56H_2UJ!UNH)M%DVih@5+nY%eTG@dOj`fyF{ok z-g=JqC={*K?l$vtp(K{RX(-p}{aQ3HzU%&}G16qnQ!#!g7Z8U-OAJ)=JC&&&`1$Z_ zeIl%O<7J(IJJQqLZWcr^`n||-+R1LRyi59_(M;jn<$#}KEr zLd&N^T+);V4LVC7?i|Z0TTw}a zM0mRg+2ak{0Z>{vi^P3({?~@PgZAty!G$Y+CSHVGxf2q^&2M5KI^a;%La}NSWPw&D7+hQ*YE*9g z{RDT)^qXrF_wl^{c&p(wg#lPxubTOs`90ZdVzKP&Ao_rgFB@5spJpM8IUZ5Fex!wv zFL8FO$XjJ%>1`>*a(DQ-ieUo0-)Ok8nQaQeubRGIwvnFjb)6THdKGIijD?f$_N#;i zox1;6P-mff!zt^^#<>?PG=aB|2*}NGOCz;kgql(QM4RjM6?=Qt`D~C=ntd%^;^5dRk^;j*kDrUK zQ3rso)|D_$W?9JS2{>9O8`4hCneIW?APyy+d|X!3Tj`R}!p)X=N`9zRJ+dyc+T-sF z(u=@fw<~X|$xEU@&Hds?rz_5I6lcce)J^`*9o{^R#w$NTO4pAL@@s2>MIO)f8+l@A zs0rE@AiMW?Djr6?IvieL2mS<=w~mqy*)8+36T5tMuR{NNiq|B126@VZA#w8VLr(_1 z^PT#5VPcee(lKf^Ly{`Oq*SitvEHBn<;btRF_XXpaxU-Ukjt~NWY%~bZZGr6i$Y$B$+urg^aLJpK>?H~$sOkt4}49GJg8n3%+HRvJ3WyZ zg~Hx#`kG+*EjlhTK72P|Ku;~x{bYciQ#@T&{Se8nF&gKBu4PbV?=bhKG|x1rgSk-p zLVm;Gs4b_jdc=J?KEzN5O2cp$qCWj`^(hf>D;pTHGkA!r%39F<+eb?^ubS*pl#-sc zj?ZLJ5%J|m_p1g!p;eTOYUpCM}}Y1s()HJ_D0KzAm0;2at^M2 z^f~Unq`zls?Vw-H5m>@mYEoQdB)IGTY15WrPqeeP;2!W~{-w3lq`*T0Z+I`aEHP3*m z&n5F}GKFebAkg{k^=423%a?!r&;Q&@UYT-& zXZ4QXQxu={Gf>o-G9(H`o3@6_42e&W)Z~H0#Bq=7BBF62{z!Zjw0*1>d7q%?q{O-f z$Aad#h;-8v1qJei!ga#s6tQtJJuElckB<1civjaN+kkIS$wON! z2^f^|jeiHl%R~5X=wTK2LyvD>`d6A`B6Vv1$+f>CC%nRzo#xt{A=6&8fWkr5N5%lv z%OYFt;porpo%Dx7x0^&2ddIxa%hcZte?_tB324TeHE~#1=;GV)Ay7@rD&Zr#9b9^r z-`C8T;OQ+(A=a?q&K9YOL6i`|AZRTT#iGp>YlYJAF^><=MX~x`jVL54$@WN)wbj;o z7?4%VV^yo=l-%C)htBk=roHO<)@d^T_{Bxj+O*$Age?)Z{xzCs{!)G5|&9+Rtv@Tx5wu1-{?Png;&5NJUyr`WH~w^uA6F8B^hyTIbgLw-})ghEel z6MxJ45M$N#RrM}c;N-+v;B2s_Q>jJMbc0Ph>@o5%tcl`?;q?OIyJ-1R#{e3icUzjh zFbvrpKp!{@m}T!)KI@=ruzpZ1Xf$n5b7H{@dhWN=wR8QE{A1~won3ie=iahe!0#_k zS^-B9t~Hg{)kmE-VPj^q(Z~0d7N^(1!+I=*Y5{+?(2nvsoZELrww|l4+RHq9E*l|K zN+jj>h(+0_(z=+eckQ2B37e$)zXgr8gmD*#tDuvvpLG<>sUOyZ_D5&xPXKOgQ^<*BR!IF%lzbTg3wIj(wKHd+6T1RGCq>^V--8t7ykm~ zckB}8?=V9KW@WTkMJbQ2Gd+goWU5(;h-D?pfuwG$QxmWX(Z?>ZS!*JZqN8>_dB2xy zo;zE()6+Jwke{Jic3>eAP#-|ZTi<`asNMT|AN=BWt*TEu{?r)FPzS%p3<)-vNsZ+3 zTKS-8Qk=E9(Hdk4S3k2o5GDOdji8g>*5B$Wka*;%nm~-bEcFBZ*tIT zia?bab|#kUvr2(d$Q`3Bu{eOEQs^kkXTG&2b*+ki8I(ngr?X%b?TO`%qsg4ks9dx? z8u`WdTkf+-65_0BaY+ku{zDOs((`?u6(WUR|4H)NR^8a`vN>rstZ3pkWs_1%mLQ%QHW`!1@TcjJ!NiUBr3HGByv6q8IBrytpr zi+PjW_%a3x zY2jubK67TPp{QBcPg7esVw`#=#_Z@JXoO`6k)Ec{7bd;v+XIyv%+dg=NA5{PhXiUj z7^9m)-o4QUgv_0tA5XmS*vK^IM(&i7nvIF*?m3fM5yGlg~F`63g!^0x}EpTHe7;r@UW)G*5z-_X0+uK z{2D^GZi123BlT%=UvT~uy1uw((A7vR`(^W@{O0A<7$0b|oBHN)=~MH<0aU>^vkZ7; zSM%)pGHz&l4G>!aXjr@Ea`n|-DPV!sl5CnDY^!8;%k+@;&1;{vDmRYJ+qPwveGYi# zg!+;u)-Lur;O*TJb{b{D^(>EsuXA*jVU!zxNFf;kb2^!zs4tpkx8e`?LiI9P`KZpG zNM{$im+DWd1IpmNGZpXshoj%84FM9}HbnzJ9j!v>WWsGGUGC=8&jf81sD?oJd8t7X z)6VP5PmfQV)8>Ih!s`T;W8^?MRC9jU<>=tNmAL==coPHnhH*{Cw1=x-vV@zyW{OYO z{c%V})n5Nu4_vO{Z7*;#A9~u#(2BS|ZT%2yub$9MC9Kny3hv6c`(Sy!Ed+(W<^_M5 zQ?TOkMmEhEpm{)q!Z|~2LpzNaQ>5OVGD$vCUu4|j=?gXL+|9q0d6XXwUDV;PQaYSD zu*^uKS3b;Zs(E0-YDoug>MkINFC4CekmoSq#O^`gOGF`STzb+B{nc8-Y~QI22MMz@ zX8ul}|AH7>k>x}NQJ0?g=cqvvs~V+0TOT%q;<6Rw$yC@USK39mms(Ghcu-z>lk0GQ z-X&B4B3jQLQ2btgizA^Pbko1saR_4^j#IR3I(0>`hw)Twau8qt1=? zx0@E061tiy#2Pz5gJZU=vlZ6J8NvnA_xI5tK6>qD&vg-2N{MLEff%5gme=K$afn{D z(8N*(^)DpPwYxX;+$!$R%bGrrV-!GuY)G7)^S9t)v6Es3A?1CtgvncRw@@ZB?XI#7pwUE>=4&IWt@zNlV^Rlwj7i6dn@1=0&YwoI5;< zmB?@lc;bTrIy4rvWsL`?Wo9rjN!%V^y*HI~vi$b2)-=|LWlbxWjD1*74^e90{)w79 z5(&ZeJrX?epshJ@E$zCq_&f}~Ph4xRtDdX<{`uFh4t8Dx%(t(>lYv7{qoppkRxiD_y<;8JqulLEAi22;^uxU5%#1f!(1(6YWS~x zfNRj(rh!XP$U5#>HpP=;pW$C}KHzQswO^5-M)vhBkhe8I^Gvcmra@Gao5buQ;dRuK zp`k8*4BJ$m-}`^H>`CdM-)3ybo&($#%v{#On17un3 zDhPdF@H)=`(<55qaGq?TpSlM=U zYkd7<2n|7v9kj7>&}jW$Oa43G^zmBp2hsBKBC+wzT?M|SK|GKTQ{L7anl@3UI9rUQ z&qMqdyBnn$XFs*7<7!$Spj?)ko5>c^|IRo1`7NFyOJ=f)gAE)%CL@!lNb%?m3h>n&eR#llKk zYcH9rQy5n&&Ymf9rqu-G-4}cup;J?}6s-?4#=b1@`(V;nMRL*b0%RoKJoPjD^JzdC z55AIx*ITrg5k|;Xno?psn`YT(gLM=RC5Mc2c=zr_@+XW@+$1`djG`p-3k6r<^4(bb z?@O_z6$aejq$T`_vc*tQYZ7G^H`*wmQd0QC32mzhB;KQdKO6)vR0793V9q(@89i_> z6uSF|lPzlJBJR%(5zY}bAGVTF+-d76a7|;k9huEkH{X=dLk*{_+*w(>YwvkYT@9~- z+FfQ}y)_wUBX*9#$n@5MVPUK;Q_Sj!ARV=PUeMicZUc2ktY|$^eQ2yUYiXH$tH&FFHhx6 zJ7-qn`Il`e;2gXYf*iY9$ekZ!OdJ$$?n_ux`hP(rpbzbykPgLAtfJxF$2Ld`=DK?m z;!A^(Ry$5o3`8cd|8bfjL!H1~fADcS?`Hb+F?8`a7Eu8QbDS-*kKp6wHlNY@_liM| zaZ}F^)nh0Q!Zy2i<|lzLK4kONHXNWpl9TM@pL^B##;|t%_60l>Ytj5UmY_owS=PUQWYy7{AZ&UdRF&$-$> zhttM3oyqdVWXjO5-_(sWkER>;Su{#Emih6VXLJ%Ln2!X^NU?8kn`ez8S?=-RlCFf&QoewX*_OkHlMnsB& zer`ov?Tr7ZS%2qZJ1{1?b70hXP+Mw}XeBsnrM{#iMN@x$)_#qsZO$}C5wed}oud*=lUI{b68J*%Bjw;oHFTNLaJ zdf-maA^UJ+H4IgQkI%!YY7+E$>x>xO?1utLl$`ORSK5NZ17r~7;m*B*0sxaQw3MLo z9tsN35D9x#;fGVhcIBKu9vzfL`%q)^4ig2y-iC7R%6s2kLE&1n%_0AxW8%Osed-KU zH0(>7OgD>k_g{HJT>iaBM4kJmTOpWOq;b>Z8jR}h_b?y#PapY$Z|?Ha)d@H*X=9m7 zAkmVDed+1KpllDa+w8@w&y!2f379~nQ+*+c!7#4J*`9kDs%lpj5@N;OuhyOKUTo18 zLV2VgFASY6J4Sbb=U;!vKbws8*j@jkc8S{8owFSfX2mcl;GSIH(%zqH%8-KhlHtr3 zu{Wdk?Pr@St}63Zd?(K9P|mQI+S+xD3!9?ke{yc{0(KPx&cnK@ery=XoGQ>}2vLY` zA(TXI5P>WHT|BVuZ#Ygt*gQus0a4;Y{K-Wu-XGT3enuUw+{7&Yo*2h)NaM=3dpBQD z+iJ21l}qFFd^;U6>TMu2`7=yvEv_&xb^VTSb(g4`T_7&&1qJU^$~ z5`8c&17aeD?h`&+ghMSQ^B*G)HLJ5xui#7iogm`sp3gWm_1iO?0>FLTvd~ zPQQW*z%$r_44+>(o!(Eh!_{s{m(&hqxL@2`Z^VH{o0W~?=??2nbdQTb$H-d_ zt%sNOs|QWWgc2A_Om+VhK$qTT!_>&w@P-ao9&S{VJ%5Zp{_(gd*!WiF+e8#>(u*wr z^n?_AnT7b7OhQS;6lrS#vQrJbHaE-9nc*(rFxjCtnh`k z$oLH{``5Y4hxhiXu{pX!=a9pl3*&2 zc>m)^uz+<@b3guQkc&vOAN8BMldopJ5*4vMbQE=`PHMDY{UvZ(^rbJmE521k;22&z z%`&7^cZKbUQw;$kW`Z(qmWbSo8r_6n^TZkn?)m8CsjQP6q>O!vQ5Dzz8LF(1Ot7%f zKIG;S-oU|88CN(}-$>L#!JjFS%e&D=R8pLWJ=snGqmf;rY}{#o95S4&q0>?_G_Ae8 zug?=my+t?)IJ3J25xECsyL)821!XrR9XOY!6F%HI{$s!Os7MIFGmx`FL7@L!eEvfa z*v5Y(@S{Q!UsFg`1jE!Sw>^0iBgl5jxwm;a?*cwojCvgRl)d@AUCLFb(L61EDv1-Y+Xad#Vw z3pt*TGA9fDVf9E)3LnL(G34i=QzAbi2g{*A+HrcKyX($`7IsB}uOshke)xWj3R?L7 z1RIoJW}g*n@4l_zqAg6)Reg&v6{unZ^CNxj4}fie@pARZVbAaEK%q>}^I+f z$!iy}wjIXWh|Vy-Kk53%;-ngbTRJn~U(MV}4+yZFoSEXSL=CBNw8DK1^wH{csKTun6~T~EbZ*i3CwQ(&QCFE$X7Tv3u$w?H_nm$BO8mT zFx7Ouv2nYkKX1w)29=mg+F!o6oldP)J=!OqxDfTtD#t@CRGWo=&fX1Rd4_6V zgt~CM#qv)tz}i?s2hze~3mAjCj3Z*cxmw;uc&<~{xLTi~cTb1NZVcr`+X9# z%M(;Thp$Lvspc7C;`)i1heQHdWge(cvNwSF7f3JWhqxWYo6Z43C>3M|8uMks`z&Fx z+D_jd8zRSl=Sp;PoJySprCpDdSC18V;byF}Ro?3g9+>B3NL?*17AjLIfaKJ8rU)OZ zVjqow4ELJMOvy!$pHxHZ!(XvG8@LsRou*9=ET8*g;gzLF;wLnx)8b|z>Yrf zJG`{oRB9uKnnr<9yfh)0pfNMV4o`%1)4!k@t>#@_s`y;s`e2it^a3R3?~yBH;4<_U z>8^UQ=^^g4jrAT00l>%FwvHQ*iU!99UJ3gCiL&9x>#n^kxF^3&XSX_;Qngpl3zKeg zsk|-K(fg0IyF-gd)MNfNUSGxgM3zO9G$it--Z5QUXgPfn4Rf!UqiW|<$O=djdFQCv zk`+~%EOp+ z2JzAa_UI-2*fL);hYJz%gl6sKgR+kxl5*VXgX-d$7F2$(yn;oTO5H1x@_REAL5I7> zr)$HH9$m1h(wK=$DRnEUsjX^E^doW;2^+q7$bIDYw+R6X$B-HMc8|N;;|F`F#fAQQ z)LVt>l7|KC77=UMFFWR7iuk%zFsK%J0HzhuYfky9$>3@f8~LvPV_G?~tp0?vjWEhK zs<7Em6Xe4~d{q5n4>r8M_YpRopQ_>5Ny|m;cl*MSvN+*2F-q^CT*UmL;z?tDKacP7 zeD`kkJrSBILZ$##p~?2nGP+%Hbw_bwQl`Ujn#rcGtt&^0)O@V{oOerh#bkp zR9*KIe+U6ep*%c5Q&L`5t)J$>89os~HF1jA?!w@w|V-Odh#dP6kfsT^6`*8 zZ=F+12BkHX(z&%P#^L>E(7EXUi}cy8k^Q)+7BD5KAFgd_ZSBvd;OTq!wJSkfp6Rpq zzCkE+OZQOGybOH!hrn|a!}qX`os*1tk2EMXyZhYc^$sfAoQ%tWrpP{K4VuysAg4$G z&ukZ;FVREYif6>mdeIi~+n=bi#VN?)pWSH^f~>qtffYph+p5|~8M0wT3GV%(u}g?` zppdXQyuui#tG?;&%<15e22Mb3klxbGtA9cgqevv0#a&73Xeb4&2Ws&F+}@rd6ju?$ zvty_9xI<=YYu-IZL$JY(&gDa|+IfZ{C{y4XkKf|84zkN`ewe4L+;Z%i?YeZ{l`mKO zJQx=Le|F%9thb%hTib*n|8t(d!6n%98?fF`*S!TF%i(zVyKms{wXPJ%G_{U1UO7~3+pHCw{Pp41bR}KuU+e!O?0L)@l;A(>{}JLd z*kAt7bl3d-j>((4^UaSsJMd-yp8tEPyD)eEzsf@Y*awoKyibrt=zpH@?`IC7BkM)~ zwdQZWw>|lX|9?H!?1@_4zZ%c}v+@%z5|#X~HUHuV z3R3C+v*uqS|Nlm0vQO93|E37=#^iccVj1@@X85xF4+Dn&$jU0{HC@|ud6X>u#lcg; z|KX(jCNat`XIw$AK0y*zEOP7Y|C{p^qg8w_cEm6{G;hxDW%#AUS!qCvogZ_Y ze59>A>l0!anbTi`kUwzGK!SY#Vd$sUs2IDpyuknqv;w0ayJAZB9g;UYt;lPF5bWLz ztAM7KpE=`*Y|F=4AhMBKxe+gxv}~^m@xBG{mv~O8|3}_!ZV@AQfw_%{BVw7rOL9xZ z&V1mdW{=(VWKh($R|y2O%EcTd)Tve4uCpGUBs4|eDew?t_1A;=;{QkVodOnY{arc6 zz_!+5!u7yG8RRjrTg$-50B?rC;UnL?#Yt)XKeEivqXQl;_z+V2R?3<9pE;u9bdTJ* zm)HK{=wkDL@6-Pk5X}}_;}YAuIm6$Vk#=)HI63eEVtIKigOI|M2^{nSQGKG&lNAW~ z(df#FX0*K0hhpd&G~jECD|cWe*T_de`d1bHQ#^8f`@b}z6Gh%ms!8!;y7N!g$Q^q0 zeaFKY)bODn_?YNr2loQqWM#4CRbi8#mHzyMxL(v;LG-)2^-lJAJZ>TBZVf64B2#Sp zwV6%S6wufpg16mC)VTC=?SF01eGCgQZcj7TX5CZ*SMd z%g(GIkZFtv{L1inX8L%){;1S_&j1`h=|62_f;^0lCiirsBkrEB7#p4k0XXysS{TM} zI{-2l{S3^spzVICV5sY(&(x9Xl-!QsvE`W3IoEyZPfiX1N}QfIeSY_H-t^T8$Nb%Pwko7MOY2@$+eJka0&`+WhBUOXpPr;0!%mJ&0=mIT_#$&-{CEPuJ|b zx%FkwszKS}Q%lW_cpfFzC*ZLZG2rzPd+<%a460k;8B)pr^z3@?NF8sZRlK(h`pYES zIR5hmLtM;^-Jc8BW{>m*h)4LPJ;(H->Q9XkP(#%WXwLy)4&dp0-iTF$7orYrQp!}j zFSYzCDl|=>eciPFhV>2Wg~X-_5J~xx@^1G8*$4%q3lp&li5sWhjs1^Kb-oNMyKchL zF0z<{rOfhwqBO@;kYzR+p+E`{lxS$o#H={ru2|^M?LV6f zQmJbrJjmZ?=>Eo~xO{eb`=#NDVb?Qk?yA{f3YwumiV8|~gU1NpBq+>o$?yq!z;xKUI{n4fA&%Rp`Bt|v_dh#yLx)+i zs&yiNkQOS9TG<_>kVG^HwvjIV;qj}XE^Kb79XSQ`k58*r&`;yG(|sO|Nqcg-tz7o& z03RF1tEM1=^j^FIOf1dZA4?ipf*($N9uj*HJ-3goh=g6joH4&? z-N1Dh)w>K>s;v}7_rirC4(4(q;KBczo&G_N$Bh7%dvOW8?QuVbu_M!9dECD|_PNbN z^Yi2w4v^@Ygq6SNfQ@h==de$QE1wFTWV394@u z3p?1amp7L9|Jhjf+~)F};}^f|CNeZ2cNPU==21dH_lWh*V~Gfcj^Up5vF8guKuzI8 z^G7**86y1c0%m4WL+w@tgTceUkg**lZergbA%-4&2-Yn?o;b|+C(h9pBnVRG5%##|<cBUGpbKSZtDnzOSISi<1BHGFC zRidhPQ&hAENS=5SY{pQ!2J-btq5@2l_H#z>O1~xd=2Q2V1mV>!BwStmc(8j{i14IW znu@vY9=v0?6D3>>PTIW^cln;f9IY zYy^)z@D(TM^Xk{p&uH?BbSjfdB(`@Z{c`w7o(8BSv#PvKy4jfiMJPV!KNEVeZSf(5 z4}NugBPF93Zb^s#@YVJ#Ud;E8+~}PYO!$z&3qH1-0B{TD)QRYQBcr9VF)dLyd9AMG z&m_h9893LolxWIGs6`eee(d%A*vn>Z-F=>)y9ANmxHg}5dU?zsvoqvl+2DDNk(2OB zKA9-_c=5iUlsqomXy9cX1KPEqN2wot7uX+r=_()=@TJPcx~^!lx1aKSHW&kjq&1nK z&I8uTDkWu1LRE`a;>d5F*i`XZx}TO^)k-q*CudVd=o<_N8#mT{KdkU4l8Z7Gm^byW zk3IlWijg@$`yKvujb%|+i<vPdFmE^kSM5@OV;!rD*k2?Ykb0YZeHZPwY{d%Ry738WDRtx7Dp~q z3^P^h0q~j<_=X2WQoaNC^3cuw#YYG9xd6@0S1Nbzk|!K@>{74KR2YD#-YiANk@Y*Q zwxifnRpLexwr*a5ae}?w(K(6ftk}lQ^34{d(!v_tILSh6r`HQBM#V_E`gnx1j#6c% zKd+oqX~bb-RbXzuGrAT03F8Xb_%Tqo-68Rr(AJ6%Dc1d1R)@C+ZkVdemsqOLQiJ zAUZ*GqD2tBMH$gMA$sp*jBW;F#@Ukhee?TzKArR7zt*gkvUYi%`@XO1u6u8<0V4f; zg?JxPmS#=aDdQ>Lo%(G(qR`%r|%NMJX^tWYw{&0~*Z1GFofLzAp{~ zVwxsr`LY7)K;X~MV9&msR9ZqtepO4yziR$I@Z;%mW16b!FlpP_2gLb(q?57P!R`&f zghor+b}Fh=8N?nt*hdQXhwB^Tl}P11%VNd?+45SkNL2M?l71!>?CXF1y}5}^Rvu?f z7nMaRr@>fgkx;7F5__|ZEaTnl@=y&=|8QSok<$y~m;lmf{8vEpTeHqztPoISR~X1t}+l?4Zt;Ci5O@#1|&8bB)K98H^}V;WIT{5 z+W&J$>s6G?d%bt)7*834==ok00Apd|y0X!|+HDWv9$%CpLO;Jv&+h%Sg*D*m9jvr2*t>LOZXUZA)0Y#Rb zL<8aY<0_2wUTe1}wn<%@J9`Yj?;1(BiFd}tF&9|L(~o!CH|&_1Lppi(pe25jYre7f zd4R^kp7xsmdq2-rNdO)?`9ebdy}FC*BR`%#`|>(o(7$Kz5~F{_gc}bkuc#fEhb7m0 zu2M7)u{95EeAf^Sj(b7<*`WAoF$2${6AlQ>*lTL<7vCvS2OPzp_$tH4A|2S2sLQSc zo3%9{D~|WBo-THA)2FRkqUS9nJFEbtB-Alrs?%chR7=>x4l}6s(3phs7N}k-e^GJE)Wd={&Z41&W;fRk zocCb^azGnf*pvaooW*vIP}P!g)f2sYbZc*u>}7GNy@ z<{T3>o{tt;5V`OCpoSqsBwh5h+?=YKY)Jb)5uU?YDYhb)G%Y8y{I$-BuLc)VxsD2( zuVeWCxD*XganiN(fdc_r36%J)(CAns8R-r(gH_7L<+aFc3D=yp|Rh^-pKMo;h2m7tiWr6}rtur_1KPQTAc&&btwinY22gV}MJ1oG%o5>%&rO*EV6 zEloi@vKqs@u|l(7SyA`42Ho=h8B!FvP$q^P;t_K_q#X<|dc1U8Vp5-1$hvc?BBs)k zJqiCYJ!fif{=P(;^^xg-nivIp?n1h$)hZIa=S<^X(OreYIGS6gA=bIL=SuR{_8u@?QshFQwQuaxzj6aXIWZB z&lvHzMY{3TP&DMG6}L_Yi?CJ4Gb=gZ)NX@;)s zA5;OKaDSif$~0$5)v1-t!sZLF$Ycbu5F@MVnS9P(Klc!M=-3 z<|IL=N0@s7F5Ou?UlpY;;}bu-;0y|Nhcp zq#82P#n7mKd2@5&j5RbSD}Q>%aU$&jm?xG1~!(wz*4%clV>_afVu4=}G!+VA}%T(fRnTck~@qELxP7Cn9F?4zX#5^*$2 zvCU_VBPx3yXA9ZF(1HES;-PxFZ^D(|aP=a@bRIj_)c zFQ6mlaA{P)^tV?B$7cpMhfiR-{IT9MYcixQt(}SV-oIC#Yby=ChMOeuK@COpQ(Gy) z)ecQSGvAuedRkZ<7vqHAVxZoGKX|UYaLJPn@wBdznkq>KMG5{vYy5Hg&JB3KN?!q* z3kz*b`6eprwa(Wz$_EjxrUjDgswkbBxwaKHBjdNjyRGbXb*U$cA0;oPHWx0#+?=Fs ztk4Ma_;^qh^}ko6l`OGc1WdZF=l&h+9h;fDYkI%%0^ImrJwDk_ni#)>)Kj0Wc>ol~ z`%(XLQ51<$Y&zd9`zRTZ^s2eR!peW@h_*H>6a>y~{q3dz;$uQDQk1ddu+wX6#84TSY* zcrPo0MP84&}(ncmgUS0)MUUw!E4e?|yNhdz$vH6(LC?14wM>Zq9W;n7)(~_95*eq2AEJ^KJ-zJP7^qk5>SXDowqbu}-8e-KGwffiAL>PwAOhoDum| ze3Pl2Dlo+Lu}#3N{g@v-U$wT0)X;ho7SwPO4{GN4_w=4H!`G)~V;Jj~`XMq)u2r2; zJknN{+j!Rli*hB>PAv6IiiARm2X>8(J&-$o;CO|&-*yK?Je-Cp{yIgYUj`}OG%bc= z!D#G`@2kni^ZTB^9%{b4S=MxAl1E_B3pM^)Y zoG#sMHfsiL7k2#fU(HmXa2!EkX@`H$Uwcw~6rY!@d1xVh?JCE)`n~)IK|tW6^zWMD zLAJPm82jo7LF|8Y0{nye+CSF<&W*zQ$FzZC6yE$Jec%|De+?91ft&vV6u3OW|6Tt7 zH^PZMb^i75EWnkH|4+?-q;MWi5L8B)@IS`P2n@!&`6D3!T2{sD^kk=J*|Sb!TOQKK z-jTxpVPI5%f*|LjB_ktw*_#$~U{CLfLPgx12>S0@+eH!fAty|@ve?0;u>t$Z1Qi!S zg_K{Mq`66?X}rw-0xt6lIb+vbW*(0BQr-(JUD((lutL&vaC~IxA$sh_ME?jz{x52M zUdg_R#eM&%aMq!!p+&*hA>6o3MM?Wr6kBO8@TLqla4I>Tga-pD8bd$hx3nNJ{QpsY&fT@_}{VZJ`B~AU8)ik(p2{>=h)59L9%r*GLl}V#? z9sQ83iLas>1e}~V2FYyI=|45Mv|U8))gO0S&ov+(CK?1OeLr1eN-dQOf;)&TBZXHY zK2a?cTwt4nhTX76xtPH5aITg&pTgF>ABRvg?tv5J_P5|z)a zs%`4oIH#T;6lMZ{7U{lcr_MW(`bpdFgWHj!O;ht!%-88?+c`lZyoG1sp{W2?a zea;Zlxh=A=e%df;qpq&CCTad-{agfM<>nuy?i&S2y;?+P+f(A%uh+6lf&7YO;Os4= zZP`P&&6zr*i?MEZ;faru7$RF8v)9XxV&>MinI#66h|_zdSzaM5>?ZlM(`$#i(DmJu zJ)1J_IIBAaWDM5319s^FM{=Zel3<&yPv&B={{BR`35ZDu@^_1p-1bU!V5UJ<<)^&QNC|oz3Z}@&fVQg zK}g!G+}@2DB3M6CzXd|`rc+{4uiif+5ryP<{}x7hIe<3Q)iX4HqsI$brGPykenz7< zsvOdr!t(Q%#ZE5_BKX%j*+rDTRVgP<{3IFo36gVTCy>b%13Q#}Q^&VnyNSbZ=uqC( zPlXP4%}|W{By6maoKzg}cWA`>_j*TO_l6E{0D>sOg;38lts3bNUaE5t- zIShqtrb%B&0L78dF*Ty-bNqDwBe~Rsr@?vYK@2RG{ZczKBTJh~H98ir5LAW6zM)9W zIfPft?Zdh+@(dA^<$5_JbdHt+Vyx&a-vrM9G))01gN1`j_#Woso(j^jveGLVfYn6O z-7Vxd7Cs{Dh`&}|^j;<`j*2CX#tx9e#xE00&pJ?Tz?2pP1+o;0Bk@8LNOErB&{GDQ z#gs0|Oc6yT{%IF`0SO1}|+f+%;XI`uVaJe(}B2V$qr6-2)W@ zCF{3+(4lrWAMgd(`7nFo<8P(mOx-=;Ve7-jptBG<*2THfA+H169P>7 zE?$HS4tkw5$A}__%78M4q3XauyrqzNg`1y0mTbyoC|&W7cR&{PS;X*X>O1ORQ4TC5Obh6ob`8O_%v(lw=1$2VO3s8HcBa!F1P;yr$`Ixt#ba59m9nX zko8`-Ui68PbL$2_h{CB8QvHb>?B-E~e~6eS<(ZyOUA%=MbAoO*ME{#V0RbeL07psp zr)+?T+vd}Zn)k&PF$aqcueY(A)zf-Y#G=y!5?Kn3^Q9&}^`MTw*#*bOvptuMcZm6U zZ%X%V%xsYTN~U4gV`T=<3;)eh`r_707Jdgv(f*tGn(X(~ncwAZ88Yu-g^CW{@aHdA96p~D6nsP$3)fK*YE5R? zKUukliSCH|Cs0AZ?4SxgT0%9e=}QM2Ib)*N0!dIY2~duI z+3|@HtX*l4k+8X#GeK224LF2i@_3T>^D(!u0V{gBZe9siQmVy9>2)0h27|!;Ndc+x zpx%9wH>KrLYVqISR&z3e34_S674q$!{>vrKmzl{~^lQ4-2DHQrjUUP#f$niL8{WQN z5k`*91okk3(v>(ll{A5ZKPcA>ogQ-mWhSt%Lr6-)m>@f8T;6kY5|Aw*-PDFLrxANK zm$i-u)dge;RzlfL0VP7fQ$S_fMH4CFe(6Bf8>pJcQAho$(OdA_S}EnRT$&? zs|Hy#ky^sTLmrQ1M&3boH!>$bAGAM-sXPG>A~x45)69*ncGGbSAAnXbRJAT>Y9o9{1f&+F%0Yar!r2aq8pnNat)xJa!Upecen+kLRgTz6&cYm z5P}Hi#6QF0FmEVY_R1hvssyeIZwT0ry%rB*DcuR{VCi6v9uTGdn;oJ}T}~fjuaUQB z!s;gD=3QLo&frU>WR99^jr0yEv$|ThrIVnnI+e?qlltMbzhd4F-=Wio?wvDK&j)=C zc)4{M9D|}|jcE!02NhtSiN7)5|7EfOSpL6E^IJlBPyZxc@yFGFN|;yw6vVD%KoKhW z#C2T#@#g?(ec|7&hAVjgzt&qUdP+lP^oUXxi?QppwVz>f0d-Z227W%SK=4LN&dCwR z$6lzB$4E{90=>kI|4vCY`_B>5>4DA16h6-cq=IE`5+6?GQqH=sP@|`=W9xs`zOnjD zE`#eCp#+=Vbp2IB6!>KVC*(a=k&5q5mr8Sg|6DlY`OFy+yk50YFNabB7dY*Q5}Qph zA+_yUb82LQ7CX~Dd$m8S8AV!}@-%nUT20_*GZs>c==VCg)ggtB(0h72i>42u8WLaQ zy6FRDSf70kWn10mdb$m^dQj$^cPL)!9)16HeC&-h1pZ*(KMSb_obmB!#M)YQgyr2M znz?QN!SUVAIkkgkF+gq5r}xg9yG2^ag*fHxW!L(ywb35ryK6u)6>->gzS~cjuMyx1GnaJ8@7|fL zPd2ftfJCkT!cQ+l3xPc{eMXsY2!;K)ZhO@$0wTa}Aa!jnkrk_JxizW3WwNS()G+!L z&^NxzP;X*X_|#rAM}bu_Y^5rK^I!dW_@T%l!KO!G-Hx@wU-xWdfN)+Lrd^bbl5I zj#>ExEh{LA!jUG}NspdH=l@J`k-Ki?4g7rjx=t{gaLix$byQh4ssT@?{ylZAMy- zzR57M;t&b1P^g?aBpuR}+&NmSu+zJ0djmg7HXPq=_R`1+xv9m-y>4#1vqyHFl0Ncj zfD&C4ivv)#73pD5qk%8S7NDI4)xBq_h|jNV!ip8z?&IP#^>3tfY=egw)mR8>Je1%0 z!KpLNrPI58lXFUp-@0E%4{Ac|m_Z))2}wxhad&8NOh%pDnN{#} zFdh?VCvtgJ2T!_*i-d<^!D@5e1*Ss?{u~>Er?1T(D>YBa{6|RW;Aud{+ThQlN(DA~ zW6xxRg57l=NI9Zi_Mc_Ig#*NtLb;xOX0;F;<$TqP3;z6QXZO!N<2|k{QTv`y*I2G< zUbjs!_Gb&@c9Z;y6}>+mcBNc? z!_rf(XSQqts`>06!5X8=R)l%S+c$fsYo>Mr4LK>x;A1SYBg$5yA2Ln(CDjEoSo5vg zuX@&3yH*ACqH;f{P4DB1rt8~;>0!~-OqSpe>h=j^_(bMm#i5hxGDWXUYq zcF;Eq@QKbUw)f68$-~EXQUGO=dm@0ZN&%@Amnv^U=Gx2k7ty*4R{W9%+*U9M8!LEJ zE?Ck8Sm0WHh{{X&kzr(g(-=4}7)C`kc?di>0OaCWxN z_aN3kT2mNi!Nu_&`wd8~zrS_^S7jIo6^|(*_>wMf5Obc4e2Nz;V!sv1K1Qpo;SBW* zNn9a?n^imvArOWD7~ED_82maSijzZMM!uu!rVUu7^ZXe zp@4X&cm_aDF*3^3z~|tfs&##4SNoC!W))OY3Z0E6^SZhb{`;H@*J*uVS2#D29JMo8 zD217wh0QP-0&d!kNl|Cr`G5h^p zdv)riwox`->plr12CIOS?ZL#^m8%di^0+3DWWav4aZp%ev6pe`+ z*ZfkHoMeDMS1g`Qfq#p!{@SlnHOCGaJM+|7-1y@c@0%30p2#ZUu>|KAwgc=?pTvn zcU9`~@Xdf;EKAoP3^M39c&AwO;$5QkhD_)}>a197-;9;^IPI-njGYMU?!8!VE4*ww zp8-a_+D_L5y`dWM1$s4;V}AJfH#cYkZZ z7i`YB5D$i39#=SmZ89OaNKWyilxup;I?8>P=C(J+0{RFY{Li2SQQz1AVR=h!Mq+pET+=>~BkteH521HMaIY zlWN}Gh(Qyb+%o?&EJv6KM(JoS!mIUP7_V&P6Z@eGV;T-OWAvLbLi3#x9kH&^t^F5v z*kQq%+&^{fqRG0q>|@5fr*h%w-lrpgZgTI)1Lb;F6MdjW-{EE8+%&q5-Zq~qk^T`` zeV@b-S0ArCRTKQ?@tBmp-rB}s8BiaU&lQq~PUcVAUWS$J-1A-w(D$~ic0B-UI4bAY z{GKL(w)4v^n5O-8Ltc6Xos7$KLr#D?(6U7f27dYSq|3x+U4{0QEWMi#jc5G@>T+Li#c@j2S8Nxx?yp_N)JL-)lQw+_J8o5o-vjx+Y1S@eI(QDl_Ui|!2c`2eX=*gZI!Yj(NneVCLOL0ex|v`JL$UuM2@yA@}A zjq)xF-(91r`u=t43#W-mT_#-h=5tbu)u+LN_vkO+K0A`qK94wn#jcs52*Gb&5wUkI zUy9#6wb>jvzs*5ksg<4Dx-;c5hS8+MocUxo62Q<(T02m(CM>y&wrKP>ET%vZCp7fs zrT|Yn3WOUkNUqID7W>5fCSFdnudT+P9$Fs0_u~+k6WWUvrL`U0V`8eZHuNBY=fv5oGom9(oW&Zp?dsWqjDWm`fk|ZMq$f{l1M#) zO5rgKB$@fMTxSmMp7swTfA3m=UdC&B{kh5c8{VZKow!&D(zvx%YIyrZ99aGPPZ|WV z1)T8l8{goB@q~yOYT5dF!cPME*PZ` z3TdZz!S)0Wz9@b0l4;b$aLIXM11@J-g=uGr)4iGreJJ=wPUy2*GQ?zuBo(2OtiM$a z?qve&n;6qpHz63dqK>!MfEY=UkE)?%H3psnYfL-Yhgqy6Zw*%JUp8CAZ=@oob;hJP zd5mcV;XI{HUTUa8ggDa4#NzbBs6Ukgv6{3aCDxu*7%-9r zPd>N;pwt^yGm3hoDSLIA;u9e@rhsuD0D`z|A2q&|)wcX|=qkLFwlu z>gqaDqJcjiOTkwOvIUDRzfOOL#Z&$pV44zk<$@RkG0wT&%tTi;Z zUH=i+Jhb!I%A<^^=c7}zjuXR)0x(ZuSSb7L-D&@+S3g5>zApN&9vVKpH&C6$*-t3& z`m_)>@*=oq!BpX51Fv}y;jD16t6Oeip)vg!r6|Eu78B*R_S5^YsrxwYmir8hSMP~1 z$t6=8LO)4jmnLwVg*_!jLf*putzhpv;?r+Vh_ye?0$wiZ&PLBWB?Lrtm=4h<5uD3z zT0bvbdvM!e^1#=B<>b*$I*@$-1y{bYmbY07uQc9H~J5zz0^BFdXi9G%T zs93vH=(7$iIF^)VEBBMs1Yqk`w5e$A5D66;0jp<8OBM2WwECY%l)!sQY(TYJ-I zndP5(?pDO(XH=wNi=^FTjuYKzU{KBL^Sm0WG8#I?Sr%YA7JwWB>JvN-R7_xjUlyyX z^x^|?37cD`JY$C6246uEi;Lq{2bF{#L|~6=20&iP>d)+LmRA!d>nu%6xU-2HKW4ccI<;JP){c!~`eye&k1EkFAfit_T*V3c5@=er!W{~u% zX^Uj;zH0!*ANx#XzAf)<;9WOXFll=$B3&6fs>Gy!?g7lI_1^~soOm~ML;VKL0?e*|4x5{Mo^8R<@{88c zbI1`nEnUzneh!*PMWt5gY@_{oii=H$Q(&r_2k!EIJC_z1t}JW08koggujG&!&|;90JB3fC0pOt-xD@t;5#Kd+PF1 zr}Tgo++B4pKtD)`gWhodOsY?C`%>E1^PH|Tu@Hr6^i&liOAU#o3jS`2mkht340A{7 z7f*aEJnuw@_Kz1jA-9e3XFnA8;XUFd1mUJW6|;^+wS*~gvUaQy;y`1P?UleOoZgl% zJ8o%Qcwoc)dD)nvnd3#?xp1ji}h#Ez8xu~4A~*2 zNWB&*uQ{@G@nIbicQVk1YrterW;!Wy92J!~{Ip0_;Pr*zY8CrFk^D>d-Bz28 z`>JQ@$EGrM&vr(HzTGPA!0&CFbAad=##;@=l6qoN(!L23&(N0^Za+4zlDYv?Ogo{N zh)8DoSD8+6g@dX>UD+SH4jy$`Z+P!{LO{tp@g0PXWPUAE_M>EehqY~Z8~vKE-<_Nn ziol}8&-dEPblYDKXih_Jlw!5aYx%X{U2euL4>86R4FJtf=1mAnQT@ef?BUeyzR_*V zeb?Juz`mQt6_D2>13|)5j+qB3-tRd58njnYPz*a#Wba^GS;>MN2hg};Dq=W8|FSjD z%zu`-Xt?|o11wWJ+Y=D; zB%18Up+0&_`hyam>KnY#}d!l%on` zJW+CK{=%ivgt^Fi@cco$75c3ptTW~>MiS|s8PuxfXq#=UG4Hd#Ul$i45lq6txRv&* zOJm>ed#UEzPkV~dYJWH5WnPD0ZUij!5=4lQkkw(B0IRlpi;?!O26Q7ckr_fK}zo+Nv z>?aD%&`5`fJgMfz66i70ttw(s<3Lg2>D5bZ=_;==B~zZ?CK)U$!x;tl`T$Fqstvs+ zUW%vEx^ejM>z<>QNyHo6+yKDRr%I6Ev=i8V=exm-7R5T=?zt7#yFC5_y+;hZRr{ps zL+zu&Y;jtmSF024v$4q%f)lIXop}I&8y+nPpN(7}eSkouBE*}Me^{YwO7tPcCTv9- zfbVgJuG^MeN6g}%KlzLt=OUw`JBIGtdP#qs=?QFl3taS1wM zutVx+Tx)yT@lr&sL%#|msy-x_!xv6VLJVMUg@dKa z{#_lFQv9+iYSq`|Dfi?lrtFeI#n4~s1ppvJ&Edjamzl@kT6YTDt^5H()TlbeNHcuY z-}4H=F+)2;U+Hm+UU)KP%KwYyj$5ky_A`wiV7I1fiK%8v>2_(Rj2is2ZDX*X>#RKt z5)en7+`FKfiJP&TfV9}UG-l<#5bZkX?B_B*|M-+r%5th({tN`ZZKXp}cP{pJMbIq*@CU~sWcRWpqhae<4`_mp#xQsPkRjO98sSO(j<}WW zz-CP9`^RAIhOSbc%Y^(=-wR9Dm$}xbL1pDaehrSNK87yEmN&->#L#IFtAj&!F5`=^ za0_;4SNVANR8r-lW^_@znvd=OlF&u8q&lxp(9F;*u65*K+Dz57$PImMmtSjfYpGN5FHC+%EE~>Pj%N8l5ztm;jm24w6 zw&Lgz_zxOhY!*GK2+|Pc#ObGdZ?r!=urN~mp+|Z)T*D@T4r8&NXDh=H zJfHO!$I{2i+US&GR1T_tn1xJ(y ze}z|g=L6{G&k(@vl2}iq{m>FoG}1l|w)R&K^Z&aPJJH#NmjZC&e4;A$AuP==KN~U! zt2NRPQuwKe+TPTY{rTvXqt~Z(>u4+V+_s7lraem(&>oxB=7Fo23|Q~Cm>9OECCJ-v zB6HPz<;AyJp55ErjT+`-TV4wx({DPx2{3oZno`vQRYKyuN_6h>q2#c`g!E+t;eSqT zCFGukrm}??{rWj5Y4zHOEqqr9?j$I3&oD+aPws_WZWx8v$XlL?r3kgOS^t=!t$}l- z)Jp^a#RggfLlDKpZ?@Qc`Az^%=PDmsk#Z*3>gj4T+x0PcY!97u=E=>==eq*9f#=fR z2W$!dV2+?-({`}@$E@0f($gfl79)WACEk2`rj~{nTz_+EU^lvhvdI_K*K%iR_mp@C zE&0Rr%ctA@M&}HdeMQ6!yi&fvdnR$JgHNtl)Qhvd05yUM=pY7CG%!i6p{c#cwKC05 zQj%m;uM(<0e0e?BY=!<2gS^=ETyj#BB4c_7#2Hs-0^YXznBN&-bYB6u)`)|Mo@_;p zUj%IM;V;-t8>MTiby!_kieN>Zo1nGk>P^)Yn{EWin z;tkaR6jJ)L@&?bt0UT&p0fNmOdU=)<4-X<=iDgea*=U7p*mW>W+l!bgDPD>hbEBj( z0^n|`zzfMeoe!pe@IVfkEOXhS|8uM>`26^3h#ErsS-C4?Z^WBOcKQL|&90HJ;qU*$ ztCH+Ieys!H0{U))%;Btm3|vKXTk-bkvAEjXfEW&v@)t4MuW3bey|#Y{QOR&}WhfNn znxR!Bg|55M-aPle2uKd7i<-FG*+*sE5?0Gqty8(j5)Jy-FRds}In6wP$MPhmNLBP$ z9zi)i*ij{vpPWj4P2QZ^J~G0rX{0yp1J1VFpg5rwBY}oV@x?iK)#x9kj~vR?Ud<+z3ggv=vbSpk#dbom(|9YmE>Kvq3Eai25iBn z^0cI|d|RFlJB`P;No^d7iqaX8dazUWaeSs2KS;91RT~lEc{5eDei-;ZB(s&wWYNi>|yo9wuf90|Md8qb|*1+N<~LH zQm*VA<45JX*+MoMQat&k&7Dq(+B{J5uU@eKeA8*~4eJ@hT6HqhDls6gCuTBE(kAb| zxty(Wn+kT0mAvtx;3$@~j_d7IvTGI>FPFI&M!{s8+L*4`dw_{CAY3iotfX@rIqlH?wbM`NbvBENxdL3l;vkK zh7bXF#?E({qwV}fb!IN~;`p`n;AI?{yVuU*(_J1zJz)~OWAVo0Mu)e?>}a3qwXnYm z08GJHEdTcX!P$L$I{HQEU^C5Rz01i{`h|wajTPZwk}j$hj=V(&0G#+1B=ZPTECY!T zUjWk9NQ>j?O(A!}6ENaYLY^2 zq2l*y|~ks`!>Nr~PEU=2xw+8cSZ>Go#a=s-&gLAvt>!*$?GuPkNK@iyAW- zlFyQ4m5>krXOve8xtGuQ9{Vk&%>;v0d@u*$){f(?kZTH$bF}eIcb`pE*C|-fkXcU> zGc9m!&v}&!C0SZD(tp)$JxE8tL-U-d3fd5-r-#CQ^lF2&Coo{;Y25J5t5o73fu@B} z(z_`64*{$=9F?pFxmz{2F}UkNVuwngXVXw1$t1Iul3v|sj+Vol7fyTNSs$J=`6`>l zrr+PZp!ug^h}Rg0``w8Lzzd{#@!~;D;8A#_chNgE8H9d&L*4AdcMz7<-jlbZI^v5` zD8;bCxPRj*fO4Q5`L|yV`anj^j+QBqjz)l)XIT!OTSkIgKcZVrQiAkRJ-pl1wtIU* z@W)zQlaIewu%sbm!zPVAq=sGGp0Qh{h_{64atK^`N?BVc8Z%Xo%3W&++*)@Z^zKQ1 z1n|km=dTjFKT4i(H<*W*=SQ#C+ukq%hBm?Q}Q!+>7CQA3}l{d{Li zLKm_J&x0C)Jn5H-9$;-A7Q_h;hdu_tc|YPm^YG(yZM=t>-4TI{=YniY~{k)mgtBWVvA!e}X?+2ddU? zmp6;HRSn-dn$srhk4~QJ>>647I(YN!SK8Bb_s8)h@pX+ji4a~~!!oe5pX_vT1L}sL z@a>wkU<{+fj7VK&K{J!d*K5u@$NFDJt0`B6_RVw9&TWZl4|v5q-@5{|ba=%c$~Q_) z)FS3hTRH4!^Nq4p%dn`jWNjm~5uW)aakmI%X|Jr*>H6!nEMyrYx*`gdyJxcC{X1<5 zCYNjH6IBjc+%88fkzP?)*SJQN71~PpTbt>$u<=(JOTYI;Ap#(mfnOPE!LzW~4!v5r zuX_37QiB~k*l~MRt^k357sr#M4I%M)D6F%Koi%DFf>ll`2q%lFPFkg83Xn8rtQN5q zb)4qze3jwX5XncVY->L&!Ar48nJCHQ06Zdg-Do)XS#f-3*zw3rz^^+>vTp!xX$pDg z^el9db`-h?wQicCNu6$Fc)iw4^e5lK&FiMtx;;YUjz~K(c`=`*pOx3mI8({_0eIKC z#P_v>G1WmHU?I~!$5=JZnd)6y)ZGW4e-`SAqNJ$g;yY|+bE+EEmXKlCSwp+!wqzl$ zbi4x@QprQrlic?W)3w`tm;%&{S59n3A2@PJcrvbv^gf{XPjviTPD^PRsC`zgnZ=k! zb6^A{T?aZ|GmYQBd%Y;y3HqTQ%d+P^%(X&>uPk-ur7=_qX-IMZV-R5`P-`KDv6K@$ zh-ekYxk?JCCHJ`{WBAG*{+nLz% za9tdD{7RvG0SCy3Le)^xV@`Y7nl}CU&q|{N;aLIpm5@hOmp!s!S^fRcmPjT#E6Ay^ z@xhFbl_tS4KeR! zoFSvLkG&S`_l{9Jj*k@V?3`W>#r`g8snSbFtxxIIc4kj@QOVUc$Za9VQV@-XPjbj) zavtI33BVt;ryvXpb5dzDfjYVm#twOBolHTS?BEc{s8$}ap5+IOW0kMT!C{eh_FLmJcb5vWJ%?f|rjPGCPSUd8{426kLCCS<*vZ6wHyb(UGzhUhW z>|O@k<%o$rcSe@4d&M!^l<~Q@ZLTqyY8MQfvYG8|9_}@3#cf& z?|WPj0Rag?LP`_~0RgEYM+8NX5Rh($lx`SOx=~tMQt9rF8R?Rt8OZ?#7`o&4@V=k- z`(OWOvD8_v#hT}yd(YWt?|tu$SQTLdxt`Na7>(<19Qv##zYP_NkH_sP%e-Bz9T~2; z8o&69F?d+OB-&O_O;=1!w+(q$2DkNn%}o@xj1$Mq_8QJB5?;}TFBD$!fURWo1-+=X{eQ7KV9Trge)RWbR?Npf)l;a{ zVYy1l!(G%0k!q5wg;CDTK(!#RzVT^DEOs~a|6wt&*rw}v;%@718=NB!`TeMn4(x+qVr+3Q;#?IgaC1}}^ht~^ui`yUjzW2i-(_n97k^nR&}51;|dimo`!-r1?Oy{@{N zwmOlm(ie-keNWSDkjBJ>INkfLOCrR#SgXSd%%@Anj4uWS} zY@PR?ZFT25F;t~He6XUMWkNNq?0fX(J&aOMsL)ObgC=}#kgiCEyI4CTw7-@APdgFn z=b?zm_RAWdwn8AUH$Nka4LYdf68$+--!#a1ZyHv$3JL>VB$k%Nb#Yhl)Eb+>HG7P?w>izZ-{m%=q^oW|ipQHPKVT(>_ znId_EaC7shpEtC0iaT;mjm9j?FfreM!8`TNh$aFg7YdPj$ePR+3<~Cf1@byz_o-Ou zK&Ch9Q`w?74)L6E@49MpG1%Cuyrle2SLS5>+5Zf}_s43&Sw1@PgTnu6cHGur$>iTU z(PA!uYc9&%-yL!hi7IJnIOwZ%5R0kU@OXK-6O-0(c~Ptd4p?0Yg#?U&iFqr+0@aX(tBamhC%KVK7}>RK4+7a0RK;N%Z=EH z1@V%?$mYKczbon)J7*!Ot#z*-H>{g{HsqS(g6(QtPX4h%mdZ6e&Y4c}A>{deb&4PU z_zy!9SZevhY<_S?DH}S^)C)};*UsB!>bJoSdD^L^NQ3`aJt8=xOOy-n@o4+E&MyfR zjKwfVhvG_zqKi1^FJos7^i~(Q(ObnBA#{SND%&J=s_+oN&*vIlHDOeTQ;r<0dBU*r zG9sHZRASY-luV;MXMyI3Et?1XUPl(;Yd5!4!;xol0BJ}Np5RbjE8Oayv9EdFve7X( zyiZFG)RKQl5WBBk<{2TrN<_}M68)(iJ^S%gwpB=~?%%dFm?n^CD%!E+ihCQcBqeg?7Wi9x1h`fNeRjQg; zJM8tyXMwS;IDw$Pe9Y!4|BItIJ3w>u?^@cHRY6Z!kop&HiLGa`L$));O6G`JkbZQ< zcaPg*285EA(43pz0zvEKzHsUn5Y59%xW=Z_xj*x6Q& zT9476TJQ@JBuM{cdq;xps2m8PZBA1dtzv_wR9bzj6DZ9afxV_6$0hRy=Zl6~oIHa{ z`ZLnVQ^cNzgJegd?}^Oc-+}QuP{c9gPP$?PHsl25VDG zO`3|21Khjvuih~N%(zcmSJo`^x*O2mPp*t#BwSfA>9gm(wsNu8Bdod+zRT?3dY~%E zd_jKyxQ!%1pC7ETwkAmqtcuXHSWyl9#EoTnwc`$*G(Ub}k`JMGCdr zSVrsQj|1|Y*f@FqUNGs1Hfbil&HL0{YXkdix-V>Zx@E9|*-_Z{qpK>4MxFuqXCtD2Mccgg?n*zU#<6UF<6t;p$%17tK2Gn~LUc z;W4Ub zomooMr`--X!Onk@QpHKiLQRFi=rZZ%vs?W2AopXSa<_WXVAr-yVr`wKT}?al3KyZM zdUuQ8f-T`>;J_#A1#$glHxx?P_twWE0DE!IF(Uw_U0)lNrv`l|tiz0O(hYB*JCgeb zQFWnI8rZ!)lQ_|+JKJnBZEd|uw%f6(7~di5c|P|BtL!h3gBb(2wgf%O&++3sTsU>% zl2-T8z64!-n_ULxuV$W5Gnvo*la;!aSgaC76u->;q2nt6jik=>~Wy4&ifTTTwdXYmq#&9XfEt=eHUg< z_4`r3ML&$1y%~S6@8{vEU?jFk)9jY5IG~W+6&bYYY=^r#eS!hX=^!t!XRNU)k&Tuo zyt~|Y4ggW9hc8;JS!9U8l=DAy%j2xSldPVd^^Vehr2;d6y>OR8Y`PX=Z0B}@Udq*-wgP)SQCe@vu6O2O zI&KE)m41=R@6}x~s@YnTkW-nD(=}eQbW0@A)2o-j27P8(dne&$U0%J{{d*a{=Kr=j z%~{TeK&zdnSqdoov8q(Q9~vZjvu3kD{c>8GKl!c&l5#PA%&oyJ>l4P~MUBXl;dMn+ zYsh`;zTe}Re!jxX!_%@$%maF_2 zVx|T3(076y6TB)SleoZE4t(W4lQ zuZ*X8I*8zV23X3}IB|0YAhR9f1(_>;v^4e1qcZ{0+Q4IX9%&&|Yp+XUGoAG$`f2u7 za#KY^->(y&{$GS`J%g*6;;Q=FNjt~M*7?`#7MELr>kAke@YTW6;4PP3R*@rks+=}L zLo~JFJY`*DaSZ*+46=~_N_xTcg6JoeK$2qg!K_nNvncqTc$uZOgjwGB5AgbOJqZT99f|fPf(#nC$Qxp+zH;+W_%`SE>F*`i0Y8`(X z>AD+$)vCT>EllAw%t;BvBv(0z`c~Csfk-*yXD6%W3&ONSXd`*SE3|#NR#c3 z6VET+b<Cui5r}u$IV%tr) zDPPz2a-o`v{w&lUAd%3*TLf z4IS-SD_}8R`rrv^7Y+&_hh97ZO(&0lQy02jw0uz)j`Ie+$u_X_JdtC2%~U)!bG zH;w|T{g)^v2Rf_s{P^MqM_2Xrm|e$YTa!H&?itW#7^L;)?s6@Zi(GM#04|{`CX-P*(}1Frk0vSFM~-YV=_rvAXj4Ze9%)6w8ZV zS_}5iFk40zlvo?d=2W7Nvso!Ju0U@>8DYtXg*Wqq0y|z;Gyokjim9Pa{SI`;H{gNv zhW8DElICt9q6RkxfKt$LylkqD0fd?rVYwO+)g5-a;UG|x>b(}*xDdC}gTDx{g|!6s zZ`fpHMXRIG)vE-=q?+L8TBObBoId1WQ4cyuzd7zS!1+%G3EKO9kEp!DG31tU2NZ+K zt5ZPv_}=rO&D{8y+4hH@i?s5~E1J9K^McCMS=F7D^mLQYh@~1!Y?gSsj~!PCo&$+I zaho&{3)pT^H~)nAF6V4lKNWkTRlWZ8FIGGgog*kfN{3EP@qDDW{Ynm@#=!zi6u?ga zPdhX2hk!70fBnp~FwJDP{h*wd4MZ>{<*X>U7S5 zg%#Kf8^}X6JvH^0@#YlI_vQk5c8)I7LTW1MykcPre~Or#1pYH?!d9h(k|s56c|MN( z5SOedm0o1_?i+BoD;=kdr1845=a%5Cvxr__-c#XWTCFHOu?Q7FhIgPCVRQx#wq9C@ zvYI>FSU{cMxUw)mvRW!H^jYENRKbDSv zwvJ^c`++n^k3Iy%k}2I5_-Td5XVq~Ej+!lZ4bupCO3U;K;6Gz_h}^E{uVcX6JDuh; zsyD3R#qD=xpJc`AI^B|8LpJ>W&cVnhC?3O$%bkfD9TVDc0$_@jIe8oMS#+1Pz|JUS z=NtH>$p+B<$1QjKR5WX&(5v{kcT-Yh@UJ8kOJ93f`~O;It(d1vpU2&XQ4VgyaRRUs zmsLs?cm&+SXSz9q93R04ul#Q&yN4wKwOAjLW3EUJ30+Rk02>y}=TC1N$KvQ%hCYxb zeMoZtNsd^9vOri=BYRwP`Fil;^2GN_sLpmi8%VgeZot|9f2>@9f1*WM;haOAZ?&dS(itW=IGccs&m0O|iHN4yfg{hIbw9Pw5l)%W@aQEff92!tFVEEQJqhv?4t|@!S++i6|ZP`I0g%yq-p@ zN~Hw?3c5%Xm&hQOd@PSdQ~TVK2Mdbwr($FdSBSCh>eMecOB=g#K%528C%+)$xTdeP zz1i1NDWhYh&eeTWv$bL6lHtFr5rICL=~0gul2^jM&fb}++}(ehzcy^Ynt5Z;IZ~iY zUmzkO>*9zlWd4mAcebLuI!NVl(7PWaaz7H*+Fe;wwd^Xtfq@{owKn$61n4l!HcwWb zcEM~>YsI5z!19IZo zn4QXV`=F8%uJbFbjHGAb#*OnYTQTIpz&U$#z(#wp&%k_>8?X*djEugxA-$7Gz^8uN zC9t6(Vx;#8JsOQJE-Q07SQGm6Lg1yYdrS}nGN7d4UOjDGMb5kH9WKid#S#v|2Ir0g z8?=tuAAR`SvWZ+Tn*}^-Mpf5~;yVlVu=Fx8WcB!{#jJ(N27wBH1Sma#+d?W&;!9G0 zPWQGRS1aX4DF^bkfL{_cKyKIa5OMmo~hARW`C? zaP?>O@W<4&=LW?i3-j4kO>eEt<3>)HrVX4aiK3Itz04RO(W-@rb(a)r2Y?0<6TX@E zQ=yDb&3PsGwp6o|I$+uT^_#keuAQ|PMx?-eqZ~|kiR1-@G{6lg+4^ZP+;Q`_LKH9Q zVUu+kR}~b4+mV$dcm~h^C`X1Xab+7}TLh@UvJc-MzmBb9aZNPa{iOX887vyuEBHpu~4Z`Utc6$utu}?kvT~m#O#kEFB3&wQa0ee-UVLx+lTZylye0v+QC&3zl5)S)$$9X8 ziMkEhgK-O8#_zA}ee`05;8FEzZq8@pJ5k%v`m?9{KO#+qiJ&&nZCqv2+z5Tld{d{Z z7*MO@1wk1f{?=i|Q9jCa7u-12>aPU4Ai7~DS>vTA#%@mO38Ds7NA6(&(n+rg5o^5> zpQ!|n`w{}&m&q01mqGe3_1F+}nsbm9GNQ1s`Wr-}?!@KJpibt3C`wJ0ugykNXhXlm$z_0jj$+*i!F`C9S)o;6(3l5k+S z?swhk*{#lF#qE7pE*4Nq#z?R)*%XHbtrUzYBuORdl!vC}KRL<&371b4h$AQ0lv93@ zB)b&&E=Mw!*(>g5EzJ94i9iO5mshwW??X~KXG9G6Iyr)h+i`1(S1#`%a8YVYi$a6$ zJFAE<7(Qwuzd)6R)u~Sj8+)&i;?{ZFS0FtX0HPYUGV)NL0ytP!{+Mnhe%DAw0Bz^% zB4C9m+bXlk5@;~~4io`G)BZN&1LeY4n1<^8*v?HDq+GSphEGBmiBlT$PXQ0YHvZEwM2ZKRs^|OeINjcSqp@t7#_cfk=}M(9KkS6CSTWGuDo!4L;gv zaj?&7=}0!g0LUXb!@%Tv>ycy7Q#422w3lGSk}6^d`&B{YY0ww6RJ*1hGQD46&jC)1pjNn$y~{>8)c5p?pg(@){Wkqk}k z{^tr5Plqmi=1l?+n$mUY$OcAhNEIsIUi|>wS=mKPfu$B z{X~OqG$?w{JJYuAL(@Erk%)~Vf@1$39sGG_Kd-nlebnM z>wV_39$2VadMfv$CdhPJ zGcjt+l7>e%wqT)*Ne>Uf=KhUR2O62q^*S{d%kyDSpyX}5&hEyty+QN)0_6qBPRgWT zneT@?J4&8I==%68$?qnve&n2_$<#O8IrK~exzPxrvYfl84s{=Q1EdZ*ZXiPpekqy+ z5E3O1$l;2&q`(gBMMstl9q#n79q2`cP#3#oCC!D}Z3UW)=QZO)n1z3erdbESJR@gh&is8zK`{mrh@Pttc0t@ zY68OZzpOK9n~D}`Sxo}hSc(ajPi%DIm#f_cW@eTvF%}?sR8~ICUiy7sz3V7HBw|bg z0BW(-FMrhl#lsB9?QBFp6$VCZtt*<2@QL{A5Sj)J{ihYGD-mkIY))LJ+mk-tp)29i zGOQRp?BvR;pC&o|M$uL0RcTpoJXe8MH-MWD(IYTD z$wov2LR0HK_>u=!URBj)&~_O++;*BaF`;HXyViL|sN3lL{c}yQ=`01IaQHnE84jM_ zh1Afz!v#|t;DqEf*~tysbsdHo%vGttX~0W#KVbR*q*}W9VmG?NIN-xT0@7)-(~dgx zJKW*mU+?fO7j>#OKGV6kU$4(#qb05Uo+7AQ8J|VAObkhzPELC6joqe1Cqpr(+ZbAG zT0mIV`Ch$L|MWcBI(>$7N+@|M>m$fEdT+x6Pz}ead}Zi@tjkL5F)%8zM$fpK1W}@M z`oiV$6@s@cYl?=RFS~~|d!(?mhKzZVyGdEYyL02ngIQm;1139W#rF@TjDvqEK^>Pu z*-*XkSh2q~{}2%0jjDXg^uW!z{keq0u<7R-&{G&c5#|vn5mv$F?3%5`NEQgT>KGmU z>hlwcsj8{z8MJF%oT=fi_r4(3e+lm;XLZN4oVcR~Xn^D_$4&?Dba6gE^>mt}&@Oo3 zS=lZT9d|g;K+`I^j|e{QLb}o6ou(6~76He)Iz)+j@}ZM>Wz&4N-&b9CH(`KnDrY3X z2(lU!8!oX@CI8Fz1HGM=+Kp*xFuoB)>P=$+?9AzBg#sO#vJ&JZ-uSsRQ7o?J)Tagx(>hhW$ z=sV{Tf}&^+fI!IRF*Ns1*qFE39Ov8ki8LpUA7){WjAvI6oCt0qdS^=bn?;L--_h&F zn2I2DGW1rBeMcO!VP-;TQ<*3}eF^0t+zPP+KeJQ7^S@-g@T#q?Jza|6u7@7I$n-uI z4(RZUz_h^ad@pof8nh0~&C#8CWk`~m+P&;@YTXdcQs2OSnFwqxj@^oMINqx!6Ss@) z0wd%2&>a|Oz#Y(k>*3j(E)Z)za+xU}Hall8sn`k_JRe7a4ZU2k;DeBdHEyf_%49%4 zw^tPRmM(Eyr8rW#Flu=$F5f4qw~bC)N%? zPrIMJL2T8%UkdHW>NgyD9`@|d`x0->daos!8>|qtovX^e>pz)rkS7;Oh#p+fLL3HK z&y*sqN{SAn*m%R)6hpJ!R|c5~hYZ>mKQ;iftfggeFjc4l7&jr?xhF)}(St+#iy@5r zqcV&Y<>jyQRIsvy@bNoCpM=Xkto-i0f!9Zx8ps@upZyg?_+#*WJ6Me7DnH~z>V;?y zqYTsa6F|wn}k4(^+oYvA&RI;`IGjI+ly-lxMHRm%8SUvu?sS%L+s&IfiWvTi< zUHcyI9LUC^6;Ccs(hgT=6=FJw#I|FfVD^J|-o0-e<7gT2^^$UMX<*`#{hC$J^C7vz zkJ@e~)$zMWw7!Yi_Y4NZ*M6^4Q2}huN!x|E*;Sjp1`_6`0SO#Jpqbe<3*S^$10PtI zC?h#eV(ufS>bjnJr3KU|Qo_v3>wQ|SW`kqg1cy*qZ2agbmJSK#m6*sKh=t2}v3`SO z(QH|fKRWXaxYx%(0d)~;EV5{iVn31a&fMF>f&=DMMq9pg242nCcIqjgOTTpZ31|k{ zAu5xc*DV{trpPP!v+LBOzx}d+zagaP8N-d2Be-d9&?=IO(P+`<@mLdxVk#od%I7#_ zmOt(YVO-oXm5&}UwtlUZ7|C)k$5L`5CCf)*`}&nHBDEFjx4mrx5Lw{e3*=1(@@mH| z5)SdXKFB;6zPTK}>~Fgyv}-+MnYVR27VEEEapzxk_hQHK`_Bu&|J;os@;N@Xydhij zGj6)PliaxYW?kZ~s9%W*zYrrwRD#uDmr9>GdBjavjL!+O{$%5VZbza>>U6dDJaa$< zeM4>qFn({fyhXb#J$DB?7hRwJ`+##~z z0khSf-q9y@XO}tPy_Fr5JPb}GSn70yh|z4@!!`P2e9|i=aXRn2+&e$q{#ekiwFi8a zbQ6#}QAp`8aKQ9?^FB{mS_dTHYwiR~q%lkePptLgev=a8?#hn&mRM`Q;qv1*p_i9d&4V-lJ_J3Iamb_Zh_Q7`Fr?14~!-8(H}RpXx{ z5aP$&-+YBUy+gXlL`6lt4o77!uTR=gS0_l+?)ulkK?R?^euMoYgPWApI@{3_R`h`w zD?aCVwh6}Y_d-W#GY;f#+LyUU@+{AXKGCku3CP^xR|8M{Vs~!%Y2=;rE1x5RJ#NoI z9Ju5cT6uk?W$w+Vm08;a4BEkFjBn2@=8{VPVgp>|di1E$l~H3zX!n1CFi$@*jO@Wp z+%b&OSh&2mybO5<$D`Y}IXK-ZpxY6DX65!hGTH-*AwF~NoSD#QI6LN^YCfX8s!{0V zF%uIryBO7D04|DwQFpSLy%H(i4E*)k&i@H>BzbJL0x3LDIkHAlfS)>WVsy(&L$mGr zvw)s&c^9Iq&8T>BI)Hnxeb+E)zJLU)e!q{ldHNZ%&6XD z2_-jTb~zr&*E@2=tmMS`&&q@Ki_~m1t*d^@9aLR;xttIV{1H_u4jMg8W_7c3LUk2Z zT`v?c{)sy}7=qFU6CsLv&+Afq&x^`PVI^9cU}Tq*woHb<7a7^N{XsnmmJd`F&?|3Ir5z})QXfrntH_G&xG9%Z^Y}J3X+ED(w z^0|)abyND%^4p_|M;VNEFH0h+Q!^)u%Qj4-3WWXL?RNNWLT3+p>D~xl+1Md#!jtMu zAO?1f&pttfc%L=u4Dum=$``1*o*dtnarcvX@cvE{#_{6VCYx<(Vp2bclQ2WfTTVH? zz7prI5p`u{WhYR!rso+C{`N8`)y1&s))a`GL|aO%T8J^>bG4{Pu;JrxdD+>khN&J^ zsHHr^e;Is+=D`g&?z8vmuDUR=M|FW6FOa_QDfB&+ z=seYAo56Gg{R>BNn*15C!jcje&z^BaflZCh^Pg%k9=?@c!5gs-$Oe^sHYYlLRWL!j zyRtw}?3Ss#Lf*;qW87f5Z&1i%+q`XYv!rgF2v`T9ok=F!FH`FlSR%HywNcVkyj* zS#;t~x6m}5N6D7HY|n4x2FiU8d1{BBPejLN?l)~zz2J9Mkjao_jGWt*pB1mpDT>vAU;ewWd+xlCF&dmv(%N3!UggWF|ru#AqGr)q5 z!lF%H4Y8xFRd(lo`qc4P%es3{&)t2qjyK+?*GrE@C(cki&!0Zsr{DjJb~y&h<(u;& z+U}b}T0y&(p2h2z%Zt3G63_9Uj=3he+PyY6O{kuDjt^<()1>9mZ0j5LIcVdfA^Gw` z6H{8Oksvfg`i_P}!}eP(p04*1oEid5f?g5vPM(gW)(n%Fjnd+8CNxEHwzc2$vl<=;wk=;YS;N%GcS<7j^gT zg0{t(BJdAR!ffW8I%rO$b|G?5GLB&^OHw*4VnFJbPH8UH-$2TlSEWewbX0KI^k`H5bLE zc{6YCbe%feZ~5#+MMb$8=QkzcDQ`Kg$ITFl=YUzw$?Trn zk&>Q>x}>zU^vjk;{ZU*TBY8S=X3LN#k-qD!EWpZDc{BqlteIV^R#U35s}xKJ&i^W9 zD;IncsoKfbje{HT3y-AewU~{IY~_%#Txli@?E= zeIIM)jB8b&{UlSKJ>ntc3I3aGP{vWe&|Cb1$)Q2FK)6IMC$#h%VKF#~6+Q4-M^cX`D`3XWx?xBPk6p}SRa75a} z{w~D_G_Q1V*HKqP^nFd!qY{WMAAZZfKUu1`iUQxzAhdzRSW>rOY~zvuie~SRov2Y; zfTFpgL*UIvh*UL$G+s?BUf7W6am%ytry}W;mgen<>%a@qXAIW#PN3u5`Oqg}$sc2z zK6`Nx-L2$jQFw|?__ux5!^}=BLeCX;5p?I=t1zRrj0H^+@u{L@En4$mQo;Lp=)Je7yMooOkv0>i2ks;Cb zWAnPl@9CU&{gt%uR}FT=Hv5AM5Ggub6QlQ{1YsqspkemDhac2<$@ADfBvu!WLvQMv zx?--618pgtuKN7gqNk>&00(H6;JVQ1L@XQB1^A+FEZoqV8`#P<%9|p~iC{ zV2@76M){Dyo_>;sw5$E|*O=|vF|)4CmiIJCoAY`O^~^CA+w+@K z*0iX$4IU+xq08XI3Tg;`$@7Re=az4{!@g4p$7P!=5g3z?Y7MKp0y}A)yOCw&fuijAM13(`QYRk1MLt$8|I@(bJyCp-=G5ZZo-wMDmn9z7^nTL;>{7hVH0h8 zXa2PfpN_6{b_21;dxln>3nSmEpLO!*^Q*aeFVf+@&;F|ZaD3?D7&8oE>cD^`njSt; zqIKFSvdCQfssIE!B#l@L|JRGn-wotYeVMip~>$X^ZJa6bg}Q8F^JhTZUuK=p#* z_4PX%YoGf6K;Iq!rR}bcNUrZ6?b|YZbCD$K1$9fB z|MBeot4@BHOt8auzL#=o2i-=mIl?w-AI@0HKg83tQnXeVhh3Nr()>N^>KqC|os0}? zpDB!Ink%bC-3HqJvKhMfb`nT9UYJ(Q_Hb|34cg}#e0(O3e9KMs{q}7PB{n1s0$euE z&sFY9OTI44C|BzAYo|^-i67!+|Il2~EgO20vAh?E=c!s>)x*Z!OTR@IUEjI@$(wzu z@HCbUU`es9cwAe19f`xo;;5dFTMq46basq>Z6a}Vzlp>1+65Ze#;TsG0Cve4miOek z7HszN{m=?6%zb}9#l$fya}!x<3Y>}H^=c~U5B?VSV4i5p6Wn40->9n+nJuw~;dKk& z+Gl3p9$Kh{j~B?r2Owj}uYM#&hx_17BDDW3BmnX$lI|IW(=(i@$o z?xN?nGN~D}rFl>Mk{@xf!?8e4g0SM}Zlx)e+#i~clG{o1nZ9o<7N|1NDS|G&Qt?Ek z7V59QG9tB?kE}R!4KP&_>pbh#7J2s_EGl4s#pDnCb4CMO0+@7W@#cIhyV5;}>%DzL zW=~u_8b!_Gb+Xly>eK&9_5Xe(zxg7KMW49KPoEvu~Y7_tmd;ne8Yjzmbf0jF?=Nfd(^Y<>vq?OVtfYxt1 zc9~`Wy+6v(4|AK}q?uT%DG+>|j*c2C^&X6c0>M1^7SG876dz56NbV+-Vb18`s2i!A z6Tbt1;5wqME1Y2>(JF$&eXqI3Lvr$DJyCM`ab&pE*19N!64J=#;Du#Tl#`m;XhhRK zz@uGOCC}SlV9CJ3l5ROK@)k0jHy_5A zniz|HsgSNiXDRmaSAAD442e&al%`>(eEJtK?;Eu`NwwRx{@ z##`e0z|KSD7t3}!M=$(YEhBOj>~rJm(A*Zq{lUvpvjv)7J>Mg|JH*MUdEsB$gep~& zzHOy81THR>ymhip2!Ib-7OAXYy4!dA&Rdm1Jw7lq$!<-d3AG%M(6Qu*cl(5#X$q)>f7<}7H ztXyQ>V#v?f(7UJ59|$c!J0c^)4HjMw%TDbSd$E*q3|$<{J$?KOtTZ=y0wr5raW(@z zJE`&?O!ky$Y-?heI##RD$G`Taxtv9^(2z1pa5`6P<5cDjFEzGR?^0-jek$+-3a9LZ zsv--n&6$dQr=NcsW&~d>>AB#%p*F2j^o(ReAD*oYx2;Z_$ZM^*quBNV=5yy@5`29E z{V}sC)ZFM1P(0sr#_|<>b+@*0=k{x(n@_L&iARbWr7 zp)`IGL}K&7sKqXWidp%+&d`#9FJms1HBTo&Kt zZ+e*5-m%g926<8*%t@?eWlkrbZyf`hSgPWF5ol09=Se~=CpP5gz@QU9)%;|@$y#e^ z!0YDAiBGG^MrZrPrA~}n#uI||T;2dhwUF`Nhn+mh+^?+GM41YG3k&HWFYVv^5@0$K zT2rdq^nqNwDSH7Jh@C~U2b`MS;ys3T#zVX=3vN$O|$+jt;$i#s++6yUnZxcU7(x=zbHXgZj1vs1m+{N<&CqU*#kWAXXJ5Uc>jhzZ+N#Y9pWCdAB{^i}7tUvRDB z^X~?39qDP+ZBRl3jk(BZk~>pYEZZ#*FX#60UTVMM@q(GRDnl&WB~gFWfc5v!;|RMQ zM!ZPg1I5$n?BWBSrt?ZtR~^-;zqC>mIER(VHL`u$9HyW_VQQsh3PQbOgCa%fpfSi! zAGBwyh||@|P*X=e1~M+0G%GA{te~qUsV$dM_F-k|=t=%J3sy}PqI0tD2>{FhUE82l7($SZH z@>TWLDQkd+sj2Rn%1&7NZkx5>m$O=(oE<9N^i}nypXGXgyM^kEZ6{#ri60Wwh|<-TD* zQiApt2S29jM8$_yDS5h<1wfS2e?V5kry<_t*UxQt_JD3NDlc8ynn=X@aF*HObna7& z7XJ32G51X;c)0DlzW|sc(hjim8l8zDA`*a}WChOkGFK>QaiZ`TS{M(K^}M^Fqh6-V zYID*q5O*vGumt};L)f=vSt`v5SYE#|F+8x5%~C^~_tZUs4*oSpHL5ZmEw?Kjcz%Dstp#+2l$kb^zT(e! z^+7QLN7XHGZ5hfSAT^Eq7j*H4czkhc-R!*ye?2#>{dqHU(?HA=qac2NhlvOqH$C!5 zKh=lhS;TT7fjMO-_oq|MfQsReBbZm5sV#TNbgp-Nh0hgY3#>P@anS48W-3C%tuTX; z@chl{d#wT74b7+d41L%;qu0rkwecIsL;pPA7!z-?uMN|Pewut4hr`;J`cyqeyhO>w za*07L%RHh3|3!XQZiA;4Hg7J^tUq5pjaKFM3E zc!i1_QAlpSaMnsXqeJyKstnr;(;o1EQ;VW9mPHq10_ZV0mCW4zYgkHG-v>X5ADLVc z7N9L(=x{nvkJ#l|6N?m+%rMJ-5-&3==zODT7o`Pivkz?VTz=6A4#LxTLt$>sDL-Sr z?K5u)&gXP$=ICXG;3Jbu%klM zf^pO{V$O7saaB6+5Mgw_YJN}-aDa_K?`c2rl+2erq?{(EMiX7LUZav-u5XP<@vyMl z8awHa80x&F6;`Oiz-B^==_}NgW@rt)yk3(A(`{ouQVKi7jv6#+t1U`9xJU=&qyKN( z65USQj}u0iqOy-lQ^N)5mOV9)9hYXr3GOm#v85|65Q{cF@|V+ARDrxBe3ajOlTK~- zW-wD@To1F?t=#^ut!qLu>2^JLZk}->j+5_L`9o(Uekw*>9TBP?7s5O$)1RxY9(As?>9Kq!s`Z4nVW#E6X^x&I|O(HdoYSBT6Cf^oq6V^ueG?6#=>}qYNEh{GfM698tP}&8cPn+PwUo1bvm`Dz1jU zM_dQYWr;Ze8WOYrf0L)`cJg?bn^hb)w=K9AAAvBu3;uP64-i_-H6INa7B^VPCL3wu zDoyNOt=6;c4Tk3{d|cp^Kks~Neq~MehlhZ=`c>(nsCBV3jk8Xm@<)xMWRYCE^G)-R$QiY6bAPJR92GkOoqo}UvbjJ+teY-Fd*{_2QvQCBwJgjp+uA8k}zVq5me{Qnm@ zs-Cco!_SSa1Zz=NFU&8kI~TMg;BmoN0A?~$t`tb)VW-crRNQ;uKew5PYxN}Jnc%sN zARayLc06ZTxnQ!!BB%ON+ABS9Edp-6iM_6&x`&nMCf{^KYgsz$%W^MlD-*gKk zTvs6BHsD8gxKqBl$cMMy-?jl-a(f_#ZV8)0Hc%riLN4NUsvqIkah0Xt;n-10irGjH}kpMAFKrSE&rIn|?|2 zFaG_{3vdk)6MBw8*F4Y_aA-R$Dzz!_c-cTuazhz3V)jc(DWZ_&Si)435a^40|SuOzZxpxL4 z(!}J->-H|VcjmGQr%}rHS8kTu`7~Slv}ST?Zhm1Vo}zQEZ=K0Q4!vIjdOjBo?~e)- zlZ(v{Qg&OXE6TkJqltJ9h`oguP03H=+SP&+9QuonfH#hjQ_A{1@1yZ@0;L^-$IVLgaoIW^`xRtCK-zei#}< zQkbyB-C8&lR{VopW^UcZo*{VLUsgF*RHlKa2ZPS=K#9vzlp}1MTt*;}ksruj z{I2rNAt5!5E6%JstXc^gT2A>1ZjHqO{Wz}=DQW+e1Ysor^6OIIM%uA#E6gjA44pw2 zwL%;Kum{|mUDUWGp21I`Wz|v5InAi>`vZKzL~f%{@Z$3~tr!8)X+Oo3a^ZDHm%RS0 z^jH`f0@uaRKx6R_{W)TWoaqYSWE^G4kNv_5JP%4mmHvKewkGzeQPf9cJD=*GJnBrJn4VUa)Lx~-jO=QM#k@5h)vJS`Te zvOUiWGDH7A_TD?HiS!KvMO|f81lxiLf(1dOgkA&^yHWxRtRP*GBE8oHqKk?Of)XUs zRhkG05{i@%5a|SjAQ(z0p%_9EAS6ISk~_God+)jb+;jiB_nco&ILt|AzWL_;p0_>k zo0;xX&F(`?V?1-u!cTRaS=ovi`v>u49CjYL7&G-xl>thEJk@T>@e2 z626FThf#JnwBOWriWpk2t8V+cx8DlMI`Zi6XBYP12Lg3)f%zmGb2XOk>#C576pFXO z`!XikeYg3Mp0oZXADDU3>JHMGL0+=UsMx5lQ~yK^K+NEl55x?giZ2FwEA~bSU8oW$ zEo+?CZ=_{t?@B1O4c)F^v|Bt+b+}IH$s5;mox6>%={>$lsU{o=xk;|Np8WkX7kAMk znj6~qZSk=u?DoP`ROlAX`Qz0wTL6sxM*YTveeqv)s%am>nJ$&kV~J~b+hHOF=iJOY z+~r#I+9S@n9T61zbQk9bq!?EfcJJOEugxGvdGGl9+$$%21L>h*rq06qpU^pA4oKQr z(}5+`uq~nWrb>j66lKTD8&kinAG(KONR`O#F)%n}Uafu=2)T-1Nk1s5v^ZyT@$kbx zhbR7eq0!_68LxMnh`luy9v~iRW5{>@iqoU_uQq;npJ(?*7_W0L^ z9C&VgeTxTkPbi}JE_iaH(#Ma+76VxaxGC!R+d@iF-;RiY2t@$vk6ZTBi?d;NkVM@B zgMCie4&sUoci9hQ)|_4fn1nbq{$nxEeFvg_k5!`XAhGLAm^^tJ81un*nY zXG?0Jx12y%?VLQKj}2MI@uAP0ASnPudrAspoIiofnSXJox1$+Nj!5QDT%PRSQsNNk z8JfYDkaq3(Mhhqw=JbGiL&|9)JHLEFuSVrVAKZdhyO43Kda|U*J9yFOHG$3AyBBx1 z^4D?}ZTXd^bd@m}gx^?KK+7&uUIxG2v4O6b% znYW~gW){m$D78f-EPs1tlUMy&V(ERObb&Ctm@_gLgKl+?eQ-20@|f`6LIM|+llBvr z9r=ZT4kfFGieXd>q|p8aGy4XEnM7+P)-Is_`m_ZW*REFYF1j*WXVtC@IZcQ#IlU#_`{`dA!PbHc3}v8k{BKFi@gZ@`;VBKqGA_F3(=G#(-;m+I)-RCF%ZE`^HTtGonJtFBC+2Zh5=< z{0Db%i)ti;-g_Uu&Nh-eU-vGX4NWU%E&4ns$d>r;3H^CtmuEt@4p6m>V>a?eOhzPt z(5b4L2IR#ejfrCvEw+s`*Chb5cLArrDfr;OHF3HIsF0iXUW%9;Rjlm&j&OQp<6r^p z1LVWo#h_9pPl`g zA(+0qe71I*mbU%JZt}!0A$D|c?O|M{ZH(FxE0+1>GQ1J>j?#Q)Oye#K2 zj{*(ch9P=9){%Z+;hq0Z3rPD=jNsOR+L zbHGP-fBs@5POdlrwU?jp1RRJskaYf9mdi6jK7n_ z=lk#2UI73*|Ghi_1pasEb9KP_{deg9PWzvrf_B}9tRC9`Q&!L8TaKVZh#W%r1Sj}_ zuD^!!|7Ah$eEM&>8r6yXTv>8PXEb!7yEPvG%CFb|UEW5&P;p?RUDXMJv{cfZ*u;pO z3~Jo6GN03LjAEJ}p-c}mABW_x&4AF=!=zDV-<1I)8^LQSL)mExW(q;GK7ZTtJ~LJ%zl8@R*))t`WBc<#}(JP zua9C>3s)FrfOn*1cw0DKRxViRb3WA55+8Qku?>r8ySrNmpL;u-$j=n4flPOs@jmf- z4<)F6-5vX3*KMU+OYirPG2ve?$Jl%kFA3y6%>kOR_@7Iq`G4zlFA2}?3!2)BJ1a$r ze?^$-96Ka^>+F$mLsqM~&y^$L&8*tD>Jc zAQoYy@24f;@%!(O%4OdQj`YlK-tl?&h4?}@H#a)0Mrs-85C#g-aaA{}o`y@!@9BCY z&@>_-@!L&_$>onH3$ZaTD#vfTF?P8Kl5fem5%O0q!q z#ea-#*{NPt^_+q=VY|ckkq4Ihw@tZT|MPH!cLym&-y2!OIUIPm=4*6dPXS<1d(o|@ z9vbDSFUOg1pJl2F$DKH?CZ9$=fIN`?du(&Y>~yBFY9PHR>7&|xXOraWJ8QdUkj{T} z=v@P6ser+g5fP8VPkR=dWxD23flgWr(Vak>h)un*WZ_K-Ma62lpbuM1Q}Qm0`>pf- zY5AT5a5-Ew?^>gm>Di)tTv% z+S#@BH(UBLQ(wL=tlUv}pLF{TX-9I4K{EGPGS_!{>cE9;($f=tY6aulNH6xuVs6N0 zfv*g;Jz&~iJ377pd2pA2@HJW?aCL!^O<$-%Gu1~nqGb~T8b2RQeyMe=-e^?#4q;mn z^+1N!y~jC}dhGWGpwrEB&5H1mck~L_nL47hUE+J(6<8aEJ`_2TH73mF>S;q;)FDbIZ!4ydy-3TyEL1%PFR(O%Urr!XWZFvvR*7 zZ=W%?KXQIQy!Cd(n^=Q7X>5}C(?I9@%+q#2@2S4wy#KX@rwj0tQ=k9XGMqtSimP~u z`Nv5jWXO6ZkvqCdi#B4dZtLeMPc0p>6J#kvJ77E7B!+eBc;{ec+>{pYGFAI=8M8|X z#j;9F@Sm4^-_M@M6gjD$Cl^ZADuq3%*H4S-30vODnZ2FJ1s{^tq5r=7x=DQEF_qun zqYq%3udd5_Pi0h6ZEqZkO^a6cvrl=SK`>M9iCu|9{grxb7gIaN$j0QuJvV)Q zSK!7@H2CVZupIF5oP>`owpj#so}Xq1;qAnHRrvVnz;6cvhh%+2uMe;`KU`7P*^0WJ zZ-X2Dc{mT&cLdHmp^&qQ2%$w^t>7LJd$|9%2=95ih3QJ;f(lOVe#`BKeK(Jl{8_0d zS?Y9qKz#Dk@BTiRrEhEN>>rpIUEXhcHBS%_%7il)iBhf9Swx{gw>tz8eRemwE5xp6~1QTx56tdjkHoRr<=86R8TCgseT%}3l7)g*>l zWD5U99A4y`a^uW5stT)pCM80D+rvjM2gAM-_JwtE7O$D5)$OKCNmRz(aLK^cQ9*TH zpTH+6!X0(73pea8?QdENNIK*9fiVO8bagr_7hTcJemeY-*Jfse3EE*D)sSl|-P9*- zIit2k9ukf$4eg)V*}1$eOK#0(V|9W&zM;^(9w9Z9QT97!TSD_~Rng1`0UCLX*+P>j zg~dj;dYeLq$vJ~3Iz{yq<=&aQKa8w;MuJZ-?D|^xx6SpW62z*q0-gVy6^lKPm{`7X z|6OJyZ~7kj{vC~zW22{|R$eaB<|}CkQyuWh_0BQ^mUZ?@eUf|zNguEWqLyaqkrNsn zNB2Nn%HSQbEX_zL4YrpZ#p*+MG#rXS;NLknp-II+v4L6rZ{ zZW(b>L;H7=^UUTkr?=RLAa;Wj*RgnFz~xgwP|X8X(c31WocuGNH12&0U-!AvtF;A& zgfcN*RegHM3<^TO#`-qMZTBZAmm#zy<+`q8h{lGZrUlG3_h{jdF~UgykH z`rzZl=!aQ}@^-%PBgt~|%0>ZN+iz>Sflhj)s!Aml=FN2YGS2W7oJ=Q3v{jJ4lArqW zH65evE*CFucRF^1me4XK-;ley^Feo7-HA3>LwH_+@hz8;<~`3lmHTs=&jx6NqMGAT z5{w|5`yD&C(xHQ@Z9QO10{PL<(4`SG+5tLsf5O%ErkK!uy=0LmB8m}l;=+%^YVJT9 zjX7=Jl4b8ndYFYN<-Yo@=;nEFJIqPs_IZtSt^xG25`KGqNdsSSBpfVzWZ$y+{hETt z@r!Q<8u}R3=SY%=cXkdP8Uja%kqq#(vu-2KV|MwEEE5<}h%+ZWlAHfbRu60U5nKDH z9*F|U(kraNua*v7oMu~z)~o1B?Z36nUw_-S1FF6A`ikJ!pPv(??(P2Q7jxmTD`dua z)6u}`x%a{!%cibOMf;CHd)w$uA-d)+ z@s4x&)=K8B2$MSOm7vO4r9$-1?jJf^!uWa;eNAwc4%}WoOXLn@{qKnI`aP7`TcY1U zsto^k^3j&i5s@TM;EOk3U-L)IUY7VglM|23RGxrdo>5f&^W*MovzFB>ponLDL9-`) zY3x%?&W}IHM!etNHb`BSays^y_WHQTch)y~`U+Sk$&U4s$`^YuRP6BW_F`=5nSc5|`OCUlqRDjRNu#p#q)WWw&n*f zp6C#_KKL|FhNI`&C-sb`-#UknberzpW!&}`m(_Ob$=~1a6$68;`it;8Qi%nhy(H!rWTyAav90lzdJ?X@Fw-#b6cE%fR#8n#)^eZE#N!|8j( z3jdIMyNPs(6OJuo>@rb3O_-1!A3d3#@Bvr;h^X+SL!7j%BUvjQ{Fk4qVz{c#$xmWd z-;Q7#UU>x*BNiZkZ`*mgJqC4YE#>*>iSTO?U36+_*3S6I&*Wk4i8GaAK$aLp{)@wP z*q`iiY^B69js>qTeNz;+JwoD78SyIvpGMDLd$$!BX9l88I{&zjJh8ldqIvm%yZ-xw zDej+m^hkT50>$0!?Erv%vb}-4qgN$~k@(3>y#ACd??|kiOjGv3*I8`Uk<5m%$p9dh z_yBQTNb2Ao3Gdv_=^Fk@cuTwbTjlqXjt4A0I@|-AZ10i&a)KlUquIb-zboG%Q*WpR zOB44rNlr5nW8>?-ymUP!(VL(#j&onrIDb%Y&s$8;)7B*AlW48OLw*O%fhQ(DN=OJ0 zS$lMs(D`T>!teEjG20R$EL8!_UnVeZE z8{tSfN}gSuGKrYJ)Y@4T>EQn|m%3M12cg-nH?QtAdb8IR`@PZ0jAM&Rl81^St4wP8 zW+%s9NcuTkAC}0|IWH`*Ze|Ql$S3#4dU~ z8U1x~2I-1}aZRIS-{%6EaF&``w?@1luUx*Rgq>Z+%=#37sDa zy{}9L^0>p%D^13iZi#@Z@O$pc(asv`lEp`uFx?T|96kExS?aO|H*Re}4}86TO~vh; zaTF%c(;`Ek_qP?LVmp>%F~=C>9cSREi%zgpPQ1;D+3p-(D67w*r(wd(2pM#cwnL!2 z&Isx?+85%h5T_>=9Ks7Ku$v#KM?l<&r*R@>aZW<*x(xwJabjg|;G-Y~sCTHWZ28}* z(13?-nK+iq(CA*zIj-VB{@O=0I|zrYVB;6qD?Mv88L|bsLy@c2YZVL@8!0B->Gk;L zlc)dT%3ZnLi&G^<-HVviWLu*G?~KR*Dz38qnz()ga-=#9&UR+6(F=M!=4;woVxsGObk$j2_&(g9v&Q*Mf)thX*W{Dg>8s|GqetaBd&Io_JhPPt0Os6CDz3E(~($tDWGTlaS^G?{XS5p*LeR@VO})`i2%T z4DkJ2hFw}V_8CUPf4;XK@j}m^s~_$JlLOXZpsFRsHv6s0(o-RUTlrtzq+3Q(B>nwn zi4;iZBn>5wSdZBij=h=fVB_sI${A|ng(T+s>2$4*Jy>)=oLY0_8`7Mj4bb=|savgs zwq|pTGh~!uHB39Gs6I=95gu$WJ!87&nMg6;C0+>Syy$e7o&X-`TCZqS4UJ?~=A#Ca-!<)v5WZ)S>84o(&Dgk;92<!4I(ui$-{$)ABsrJv;-sV?8@hcE1L1kGU)MO z0*otHyMf{x`*9b`D#FRO?zziBiMhR!*P0e=twum{+2dZId1f=#7}SDP;nRa)%QlG% zF>FsW*+k!{EMg>dz%UJJ^}!gd8?qpnVU=@$3osvs~5=0E>*3X5YlgDOVrv(3>Pq(+s= z2gl3f)G36AMz5zbe)a;u49TW-)ECJcw_d)lL6X^52b8kYL$sivDssLzr)c)^lXo7C zFmk(~z|xzDmEh5e&K94BY}oKb*RrdDU_GjWIRztZuIH$4Oy>&G5NX8z7^9l&z9S(R zXTa#_j$tw}3T5*=ZwRi1;y*X_LPD1ammC7c>-C)5TT(r{`;sO!N?cPycLHI+bx$5A zQfj|(pnf<6N-(}J=;OznD_I5IC!y)98G*9YFs;$z+ApwEVp0BcE_|v_m9?58#i>T_ zZXsYWvYM`i%LTkqSx`l(fQe?_7XQN1O!0Hx{^PrQL&P~LTb_pYSR95 z-^cPL9qW=iQPLg}^zK#RwNz?gx@>&24xUMs%&yPm8f&jC0Umb@^OE4I6ki=4Bs+)n zxV49R?`yT>hxApW3_}9$Rf&gvHG0Tr9!`&<;UKsR;o4~08Kz9Rw1&5hcQ~5h*jY;~ zCU}*(V=&JypjnxJYu!G+1m6zRiES)h;hNiJDC?G%M{PP!r?mLX1*tql*lO zg@Zm~GRUsUabn7a;UUk6{pKSC4V+7=t&u3kd9KBIjubeTYbct_RAlE44rEt0>yB4^ zk%|b+v>K5+jafw#Y3^Ty(02Z=Bgfo_j=7n86p;t=pmuDsb}k^yhi*z-HKQ`29JVmc zBxG{*_r-f<);YeJq}ZJTO*S8dsNP<9SE2++?&)l<0@|L&Lro+w;fK! zDKbTJ$Nb?P0s=ZnFN-=4$llZtC^ChYJDM*`PF|T2PXk}v4tngNKr*wNOIcafC3lDB zID(3jhIXuLZHG^u3C1!hL>obm^0rHu5wT1^+>Se0YEF zzZ!rCFymTa_^ivCdw zbm*SJ)^1I?0S$R6ZuxM}1+{V@bwHJsI3fp?4a1L)AW&R5{t2@`_b73`a!f)SXip-jbab&llh%DF!wpph}3s zKT6P)cB4z(*T+ar58MOr-#D&a&TMm5Mw%4O054HkFqB?E&4);OFve!>Bbjo9Z{Qra z&`pLd36A{|ZDRwBR9j}BRRV`QAF&weQ52B}ARH=g~BeEclQ#5|kJ426v z2{AzM==?SN#%0s=0;rKiy|O%Xq#D)P7N*tb5ass*CNHNHQm2g1ZCQkTM7F|Hc>yWa z!)Ef(IXp>HzaitjovVstcZ9Dx#cos{ip-6Tm83yBrdnX+x-0Bl!#3p85?@*#MAE=3 ztEKzGKWKS|Kf9jZ>XS;!jqUNk3tdI}WMMPAwTZo7wPWgp7A#Y5EMa1zFfk)B5Z_S_ zJ4~mlz^(@!&Nl@|(zSKJ)uZ}H*cE57vh8u4$bfnGIfl(u@+q|v6ElbQ1nx&L(@gDC&yXsFNV=_cF&lVztckx$4^D*GN9}`j#p-FR z>w%3%Df6GIv{z_OyuL`_W(!>}hKZ&KhW!E(x*X}edvV*=!X>+%hEvsYKnjaV!Ym7K zR9rZ=qY%sSrA;)Z#R{$gYnIEz`Q%_TjfW>XSQ`rBW&ZiY(lMbKrwvoN6p=OQnQk*g z4)9wFj!?U|c&V>zWp&y}Xc}lvcMO{|nuB=82?zJVqcW?WW=S4fbPIz&^ZU@z-`uh| zUXLmHl2hVQS)EPYi$Yg_P$)qd{{GLTDta2chuy##Zc!o6xPsw9OG|!R8sORxFDU+ewTyZ(Y5(t$qsoRBYB_yshzL+i)l+*IWu9;j zp^GN9z&DoZ!0$OHfQmynwnvW6mlbo2JXO}nDfXc`_PBui#Bw&3`KwO+ONqi#Gjr{9 zH_E5kv50vTSvjOz{kO*Vr$siIBWj%0P55X&Y28{dX8vKv&C}$u_VX4w=)ZX zXE(xB>6@+CzTQ@AwVY#HLS+Ea_??RE4WD!nBoYE#6|ZlMJtMNQxyDa%^na#M#?#2q zL#YwQOtHt5VOg&Vwr+E_LRw7+g7KTnq+2vYk;Oy`sH@-(1;u?gZ*om|&e zsubt&LUP`O+6aT(&Z}P9eM1|2b;s&agk^IumkXI=c}UL?Q917X=B$JvS$Uj_jlKs% z)36k{&NA|A;B-qa z8gx)Iq}`(6FEN}NG=I{x8f1UnQmf)jq^VtIdR~FXY2Pwb!LYG~MSc4FVzQ`a5wmC# zGn7Y3es7m&r`Qrn-t0F1#~x)y=F2#2VR?Q%03s;jUr2EmPn7c<9>P=B2Bd}Y8k)JC zwz3kV;$~U{8oq$_1r}@r&N{QsXK`Q5UjERX0It&G&bn@ppD-&`u;4}+GlwoRdv^JD zgz7&kd{@0VB`63};sOCjyv&fspw@US$lUkI3%w4Jnkc7eg<&Gw^C8LENS-{0mIODT zV&m2-UiPUSd5V`70^XMCEWO!Tx$!`$@iX~`Mv-S9Ar>MF;NCa5!SU?K*Z-*GUG;bR z3~y+u1E7a93=tlTHnw@D2Fy7lLXSxsydn(3CR-T!MYAt}x#Rx9Yjf@L`JE$kwLXjX z9j>?Kd`A&6(=jeKrzi5m#z`!#BvJ5(&BsP;ITXZ*P0?PwFP8eRUyMz4jxIF>xGF=> z5g{P$PLh>31nx0)vIfk(X9;k)Eul4e5pPsgOBhX`lKLXwdQt?XACx zXqpk58|byRX)t69AfWC__`$C5q9Vgi!@Cm(w#3lxKl#O@H;|JI-7|Y{RV_y?6OQ*0}nkG zu?DJuk!{<&CFG%uw*rm&3-Z`SwFy=f;2D5;pd+9m8*pj`zQIT9w0$$C?j$ddT-}vx z2)xq^qE}5as&Th8dhhreU}6xp4OD{eqf zae%gQCN>M<8=dsvO1=tg&0o$iDmJFqq4D|NVPelHgfAULErs;W)yB*FGTxTr(Dk0_ zva$U#qw{61syS|y%~`-@7Enb}VIWOz^BKE(R8Ro4(fm-1pHe?&@!1qnv z@vSv7YNKS-vH^7QUDuap z-!Y0^mk!$O1zFYuq1hSA=)7h6oP^_-+(vywLU=sF?Wc^Gc^mN2)q(fEfQx?~^|~A0 zm?@-XI8uc*O<{vEX-`@ z^pCBgknXFRx}LuL&0K-)Y!vd}SlxTD@bC3&_{F0Kcct4&flI4Wq$%XWA+br%vp*u! z$)q=%-^>Ien*XsnlM*QwQa5l6kahM)0kcCtQlTd42L+&}wcNNZwdiB|Xp(cYSDh@~ z*Z@sg=>2>7T`{~!afS9=bojZ^*e!Lu>;G3bIn>Y#S~RW}Ri-KL7Re{CToZ{3Q#dLu z0IExt6Re3^0Ts#1?Q`tP{Xh$Gm{MQj{y6`-ZIxo|`fR90UAh{@LNq=F*%Z@XUF3@# zNpWLFTT8FWzVF04W0v+r9MGpHys9fHFVl4JsvB{89+{jj3r!y=$TO6OT2O`)!g@8> z?u7`021usE5A&5>EGu7Ste%sfb*#_;e4YONI3_lBtF(CqHIkwTL|AdTJ?$jLtE3)5 zWC-ShmuIqt#XBR*$=1ItQbSj(x=*5e>SB8qoD=I?dKLobbWNcsEzGLgb*#ACpKU;; zBFgQh7A8gGDpqC!j9Wxo_D2otWc)C1@Ee9+NDZ{jLlL@1&|19D*yeHMXpURJnpG4B z<_VO#3OeEXogug7uFWgE@jods-xu}yYYHRCU-kYG;kBqYR@ft+w-AlKA4R**up zaZ}jG9JL%dXhv&53ZX45F-+N=-sLUc7^NUm)|kp=pwt~EjC`XEJ$r3UcQ!0}iQ3G3 z9PMst6{}STs0AD%W0WDX5)h~!H_kPo^ylv3Tu5^r@N+B1q%#0Uxse~$%k*>2)y;8C z%Y*i*tjBsHTL>q%EUga7RN=UHY0+tNdNQ%!S*vkfMh{4E=J_ms?;IVG(Jz+cv9#ov zPS9}s5TQ@@!2`TKe&0jF2H&Ujdj5>gYPDbzU3*)r5_{KznCIsttN_~k0c;v z*2*x?Nw{&D*&Jujo}*MdEG=HP6eFA>5+tn@EpXS-Vk33M7r4h7B7@=Z_4 z3nN!#-mkXybMp_>!~22qheFZ{;JN9Kau+%N4)oPJa4;5oEc2L$?|ne2 zCw0AS9Ha0yZEJoW^CEl$6pw=Fpg}h06CfNF3h$f$y5idTx^!uN`=%e;Dad0vFj<*Y zkve<)TV9_2La-BZbs#@4uUliS&TOR*?m_az?){M|Hi=uC#iXPQNr5|PbpDBYI@h<&37*cn1*wuZ9CdO;j8>;7 zuX5z$BOf}!Q<+tWUY2xB2D#VIo#gCJTF!N32^1HkxDFkgAuhJS!CYT(I#<>yO!_og zO_5Ydp6!&W%}j7(7EOJ~O94vuX>&Rvb)c2bRc&$YQNp4_cb;yJmTzNYF30#y=C0-L zwJJ}ebXm`lJdIiN0IYqXAzs6CG`U)-+v?(JL#4ib6ekwYJ?*-KWH<0pjp|zUq4@4wI=#0vIs-HeM-P1q)@sN*wZ%J5+<+#x z>T_E#LQ-Q;J~0bu0FZ~p)@FadM>YKM+tQ~Z zIlennhciAd84`~F8ksI9i&8Qx&GGM|SXvAeKvBwZ%nPhKJ>5l`+Yq(TTMJ8m4Qfp8 z@o3-T1ZgqLHo%`DKoS+CXyHk*i{o51Z3rCHE(lBOE2s@-kc6%xfZpX;6Rm1sC4P{Q z^_f|Ft$(D7T%Ix8y&3<**?HJ28IncG(#HxA<)|?F@!30dLRUxT;&!s+MxhRQSyFk3 zhMX|4oz!o4?9%p@1HUt?>nM?$FQ?6al#;CikNY<&4(mY|uozU7A?_pq3(xVsN}5M@ z?qCSw^YAjsmQ`$4a7`q5E02vstb;bjC-~>q0N)^gdfest#-h$Ojz~o~Kg?@*7P-a) zZ}Hpor^9#_$FP*}4fE0)=vOG?FO~r;QIBh|dZ;<{e7CXL7O`>OqCUm%Ip7Ix6jPg{ zR$73cIL0Dv@gecRrxp}XyW2ZiOZHiI^zHZq-tq0vA^j2B%Uad4rqpj$t_ zXTa_G`^H6ee+c+xztUWql=STPNoV0T8fXWHR_Vbtue*Y>tp`Y4lO$n)m;|``A!Q(s z#OHE>bttNWxr~AsFu@->s9rXhZeiqwFQ70j1u=OOvgk;Q`n*brPfL|@OWHu5mKEw zq(%FP)58<&$S7YBkZl*HXrT##!cxTUaS*uI1Iup@iI41cidND?y?3LG%?h<6fe+d! zcC|}g6y5NRtL#`qujrL%u@T0~!L9fqGw`&b??_J^sYlk^rld4qLKxp$*!e~_pSNVh zDZs%SQV9{!kt&n3U2pHAOpwWrZ1ERWLfta6dHFcc`z)@@^z2lptnWj&WOMZM3+QY| z@%+y2bN3bu113&mh$;XBYH0)zFIE6&XPGYBeoe3zL1Dl*iD$z7V=@713E)nGw+MpS zVPfydTo}OEv184LYI@yn&%5ku30(a&Rq`Xw$rN|Wnhqc&*5*}nCCmJ0BPjOncB5oj zZ%+}HrD*qK53IjQMTvYV8Q^36v-gO29#-f!eUGO zsqPKc+2h72{MRe#1v3Hk3yqtrBuQ!7{2GE!Nh#I2k+YtWS-#y0Qd8p~h5LGfWQc ziKwxBP&XTbCc9;pvMjN>j7PM8EIxi$btr+(u{kk z&z^@NKC%Szc*g_cR%GUyjMMN$9m)YCkD@H3nEr?`Z-+WbkMtBRJ*qP)Of;}vBo zx{VQZ7!IC*8_HW9zmA?a0gr~&(OuL5EZcI(^m;vA$k&<6AAvSq~ z=_#_^=e`xTSEQ$>D?8K;rX+N^SC3i;0Ub@ss}o&$1XDXU=*CSxRB9(xZn$dZ@ska_ zOk=?i?Qs)?T$emzuzX(U(mw{c*BVBU>$cKF_g|$sAhdD)=4~|)V}DiLdhSHw+9a{! zT%)h$C`;j9G})`mCsk3DY@y~b(TfR3ya3p?n%ZzKPSS^3;4LUWPWH=zH=N(KzMPeK z8M+IIEOduqnq~WVYVjt)8U`H~#*IrEDJhj?kX&B$n-z}6yBv>H8-w1rJ~J+yC`Sta zY?(0c9EweM|K_uKy^Pjw)i}2%_toPaS$aam;|EW9_gdujVdkD=i9(RnnN=U>5E=7+ zhJ)*hTsPX~mJ(>wZ51qEkCCVy<9j+D+YD7^O>_D?BTeP z(cJyRqcz=@t0J4HKxinJy6NjH?rI-~UuBcso&}=Q%+YDgjr^@(CXLeN_M!~#p>apqBX0n*|qZOlg?5yG(21fIdu!`{!vU2=bq3y?O&J<$H~q< zj_GkIL$y$YH(Zvw$FLy=-u|0vBv_4FRDd?71p~Bo0c|f}NjE_GTE^pb$8>Xmebe6i zyQ$jST~(<9%asV;G+-9(TIk+i>(oYAaKOEC=yRvr`i0b>hsV2|!$El3an1Njao2Ap zZUc+17z5PPxr;;TSwb04R|kx}bkm-t`iG6E;q+Pwr?-;MN%?$P&2A$Okk=6IBSKulT8*2w5o z!(gx;Qw2|Rb`OQ%X)2lyQTaKJ;vQd0+<^DZp@Hnrewu~tN_}0oBCH(cHUodK9}!)i z4?J}@Y`l^SP~?c@jeut@I2;gu4Azyz`iu+~K!;hD8DYB1E~vbB$ln}8x_PUeLAHeI zRdple1w9j7SY2J_-TIW|WGyH;-w?QA)$BKGX$a3J;Sq>KiG_fFWTbu8W`!@5@2T8ChLEA`DZf(BF;=Ha&WW} zZp;ojI<~jyE~*pXLaIp$ND}r+UHMFUn$T49gD_gkg4^8`Sz0MJ+uYh zFP25tNq;*3XrB;JIR9f8$g?LFlO<}rZ3v)={732c*3N&q^Z#uC^8CLDfe$>@X|=Xg z(csHwa4$woq4~z)7b8KXJteKP72RLMu9+m|=ANp9^i1dLD0l0!(^GQ_rP|QZ^r*@CRYjwvHsnZ zQ4)y7{MZ5b0C>_j=zqKae?>9_HL^Vud1bx-@7JUQ@m=EhzY7|Fss{lDtuw?nDp1I5 zw{p}!i=J2bARv2To<7NNfmh%ucDH_g5$q{S*>PXM%!;$rL$`1tF5^)Ntf?Xhf+zQ2 z|D031^>2=XyI}v09%bTnH2p82v=&@UD;`BZn9BB-WJAfWfvC`_Tzjg~tBnSNj(-%|1h7qLIVH8_xb% z$EaVE+~X{|xFg}=H8^Y8zjlMe@61Yy>0WN$;R0lQD<>`(X~>&bo?Ar?M>1NlZsm2I zw|s*|nU^A_TsQHD*3IkQl=LPm9k+3$`5xob9n`MZ%6RGVcnu9aMF{zrQb*ONf%f9Q}2Y z&x)0WDQ}CDJHOxeI`?{<`XP{f)SthOYFHi;oR8}}c-;MY!5yREKxu!`ly2 zwIZJ%K%Y<0RjqdVm;=tg?2x)R&QE}FDSX^APuN(2GIJ%qA~Ptcyqh+j@t0fY%E(S- z$7qx`WK+VWe;QbYifGPlDI(^I<}tipnAtyeDR@`vwQiob%@&$xIKwS2Ieg8O zq%jEX*x64OT>uao*SVe&R?|nrm_x%ZpIFVueXL(S_RBm+VY1?wt9L%dXw0n6JgYlf8&2snKo`GxkxNbxB3g8Op)HqY`D`W za(~}5DU7g6{m5lY>lD%f68`o<&uTvp(m`*rvS$`9YcyygD@-RM0QD|&d%)BR@Qd?? zi;u)b=;PB7h&bR04I(t+E$Z|iZH00g&`ZBhJy3{TUHL!B#z6Ie^qs2u5+8dPU&bD& zX1?~#*=eS9zs*sVV@cm8YFo0B5Z$aogmch2S2cZS&(K)jiVrBdCuo51;^DO^uuA+w zWdED5jA7#I`br~9 zl>W!ySU}G>HDEvjN+aW^i3A=@OWEPJr`uD-z8q|KHX$nWr5K65pUmo5(X#13>0i~klAtK- za-W-ZOp3@!FheJhH}8%tlU1P(%U6nLF8vOSoGONXl|89n6(^v&pJekAB8{8(QH}Zi z-XuH2I{D}L$roQ0DHM$C$ak=l1!W=XG7@N&Y_FR+E;L^%0F7=*;9pWmIi62MOyLt}eadPK8Zv zb5Jb!@Mx}&{-}Cq0a+}_yKYJ7Xg&qo#d3zTQ8bGwgPAgeKk&D~ZR+4eR zq2i?>vFD*p=&+UbiRt|fRyA$RqK6&quuM`~_9%JAmwra#5cljC@n8V%yJBLOx@qGQ zS=fT*Y5ecgWBV=qCgw-&LR%00@vVVYhnV`_Re|jmBbFzz+Wf`=h=BpeNn(8c(Omtr z!;DF-xT`~CdIr@S=Yz`mO48-F$GG+n*rc}yAN16I&xlBA-Df!OLpBdp`U`f&y~5n@zpV{D?Leh*+(e$$a}!deTQOy}NKf-MRC5PV0z zrGbpSwKT!!voy8!g9TJ+7)WqZgJlls$35Hw0v)!l$e*`afr|e3MV~$D5;#I1=ZsD_ zGGc$b&Ex#>DMa8g%gXAQ_5Sh7dpgo60PzvOa}B%3mJ2OST4N)ttyY61_gp8~C2-3`u`k`n3MoYZGih|h5<*dH}S&^mDt?21F91ohDmO=&YFH@`^~U${kGnmq9| zR>cLakw0o>1&;vS90(=z3{HD-0PaWwNM;YK+GsH`17E%o2HYG6{jib*v44A*KWbw)+sIqv zVq$IQ^EQ1@8XB@agX_{=rInAk=<=lvdYp3{w7C)QmDAe4L<+chW(+y&C*LU=@L||- z`vdR+ok85^vXX}W_PK*pLhM_ zNixu>s}^JPsiJLvsTP=xbT`LBmK=y7=mzf9HJU=Nq-&)H$$?YBhmMVP^(8H_yBcv94l`h!Q1V5gz$8-pfp!sui6Powl#M6D_4WmXGzu0CntBU~jTr>~7@Oax~XIpw_ zy94m@94u=5d1;W1oL7LfK`N#VpSi8=)O0Z4<6OCo6L|0!e58RJqi^2|-RTfET;09L zfB>j&muN(jDv-{k?{Z|P}h9qmJd^K}Dxpo8BoP7G4&F;)!)(E`suNy_vKmWUm z|M~F$_jaZKCz=1>Mj${v=K>fk9&}=IJ!SWaVKm!fxy2t_mTuM;P?~&1UdnY8;}4468LXmF{OcV*ga556 zEIxy8EA^Gad(;z^ebLF%fvIl4O#2yx{*)r>o}49zCXY28grK~8Eb7H|m3$9Edi+2a znU#4Km+$>~6G%__;d732T0g6&0*GtFWQig@JKLF}^&16dq@1ek=(27@fA1Ak&$C_L z`R$a<;`Tp2>-(A7dWwtPT>ENbv_3L8k8Ox9^Smru^P8ATB_Ela7q#95Z+}8DzglGEeysoT=-?Pfm(r1oA?iw*r}W>gt66WO|Qz|$ER z{4qV-QQ-}Sb$Tw!?b)LQh0KAdke6Q9dSCN+Oy5}j^oaT5*(+MUw?_iEOJ&&5d4Cj> z$Pf-OjoWtY~g9onM&JyZ+06L~Y1_=OCHpOyj5&bBEsaB3VK`2#7; zYMn}~-9^1(?2GKeNd+z+?z@F8Azs6`d#|%+QqIk{u{|Zy0F6`B1><3H&VjOid!U0h zq0js_7Tc`7w0wEak((hZmnMBtL9l@K2|w`se!pin8syn`dw{wV5^YLjgTT?TXUshx zU*f3bte-PIAgfqTViKpy?T?p?-R>YW|E?dt>}c)c&J~F2C`7pn1tMdQgi{2)t@}-* zQ>{-dkCpU$`-N4+o%1l~PX~vA)&zli>w}i{#qoNz0N001k*P2@ApO>c7OAe36*BD9zWT99;$9t8p|83T#qOLhEb`A zpexOVjE(c!33YKS|07D5vlx}KWI?=7tgjM1fJ%J?oBvpO|6I&T%huZ$t9Q$7OzNp& zYKh*YMRB!CY_>>Y;@cy&txvyV(3uRvf86G|fjfnooGH>{&rAPsztZuD2-PR;BX-Vb+2~bh74l{ zGOw5DjsIHPsq~j5=&i-nPm8Q#p7f`yFjHuS19xrGvz0B{Ma+LN_4oEt@zAEJaZ=hg zGrw^3x>m?0eZr{|HNMM<@!7Gg->ZT}l!J)^=TS#-zjoqdtfFkFZ5*6Zc|wM#O5|m1 zxeaR{Tp9_SX~dHzdPOYu3tPEde|+D#xS03L&%OT2yTK%EG?$z3&mSNHr-x<&OSH$I z##6q2L{W3Dgfl?-t`zZ0{B47~kL|?ZVCUU^M?7PAueoTgvUpJ+CA?&Vo9s~0Chala z-gMUH{O{sTKQbo1u<(S=xq=e{Y_g(zt({weuI~_5B)b#uj@2ymTPXWZAgEkQLo<{|NFC^3P67t6shdnN{;CKEG}ul~Tl2;vsYWrG@6 zP{V4ZEqILeow^N6dr1!!uE&&)>-;YdRoFZw;eq(#+C+}0`y*TBet&4)PKa#0ngAH| z@o+3{d`7Uij?ObtSe0tn1vD)~;0m(~i@6crD+#r@N~3Y}L&~kL3G1M}Gp~Z9G_{{a z?@HLN0lJ;}Sf`kv&=X+RwT5NED?A1Z{&9Xg4jHlQedB1w^@f{Uh&$#6-M`v^QF36u zbKh^L<=&;-wFOsc*aRYnHeTuL5$iSKb)}l~_qg1cXRanU_w95ZX1Q34nL&l^cO~qZ zU4KYD{lWC!b`F7a+wJwA6Ka2EdM{D_F!Y!VYyZsc31&%rY z#S=RaU*h*{?GeSpvdG=#QTAz50`4<78?_(mF*#WexSuQdpi8{h|@2(G$y}?Ae*VS>PO6#SdrFXs6Ud$5* zdRMAF>DTgg-20_0f2`D<=W(l>2t;ya4xd@rcv;WjpCDHZqDqPVjfE)@o@zqO(UISm zBFs+D0%*KeBXn^fEuKpP?G2eC@3!Os_K}ub-<4FLr%(QZ`VLTa7+=IJQa%1%Wh|?O zhkGI8XRmGSI4KY4*gZ;62O?l1&G&BXl)v-~4pG%4R*Z+T9Bv^e&NNB8>g{Sj((F(0 zJ3~!=jwUVlLhj1}|DftwSLQ>xMCklcmz^y?Dlt3FwtP{b z_;Xo}%!K__^PloON!{5V7>BF!xGr|BJf+;*Su`IXi_Ax+6-OsyhBq&6Zyr9noi+Em zBbeGWsC6Fs78eO!gVbwe&Y!?>H=|Pc`|P_Px9Pb8n)vc;$ONpf=YF>fe;G4dl>&h( zC{tUg=RA1ow9M^7+;_QN0L{#GC`tb?3I(crd%tUhrO7{Mit0X>wmxCgiV8{QwfwfN zi1lZ`I@R~06pA-PS_?c3dC6Pm|0RHL!p2wngt>g>l^6ACD65wphNiVqoP|DX5n`9M zx5i|ph^e`}&Z~ShxF*wS{h24lWPnS#VLGxb<~hfeZF-e_hN{F${o$%_KxI$)HT4|> zP{eLfx!TqjZ;`B{fu^=FWL>hN5-6%K96sWf;H95Vp5jtP$$93okY!oZb1x=pxrSvC z8RoOW+v|JVb?fu%W_~*eVd&05e@D+BqLIiRO#-RyvV}+V;pqvw^ZuzZF{V}SGY5L{ zABxiM+T#Do8}#JpU{pQqC}lVsT^fz|x4F7osIJhrCagc{Ulwf{@N<6nMe}@^_Q>YW z+jBIpuTe<4#b#A0fr~I^uM;o44t=;gsQW8w?!AOoFr~^c2Vv#xbv~A2Xgg-F0+whB zCo-k~w#p0fq=zM=^E3TPmjq05R(7oIGL?ck;8~ipQ8a1xx8_Rq>wOY17hMEzI3|<% zEq0I#QI|aKd{&YkkDiqsFz-p=|WmUD=dxeyHzs~w3d+UjI^xVDu z;O+7qftbf4TwTa1+Olbi`~y1O67jo@N2Gl*rV%J%WiIJm=+ykUTVz20lxUSv=KKj3 zbq}ucH+AtanfQcZ#Eb5k)ILPeJrNI{FJSahP!6t1h>hdTFgCREo;7(_T;FO-xD6m; zc=crnw88Nu2(-J~X^Gz9d%VrW!nN@249_q4_hJkM;UMeA&j_j+b+#Kf?3K;G z8?;qaMWLI0y;5Yn-b;A^HH#GgKJMh4<0Iek2mROeem(vjPskrSerS7;Tx9ZVVdRjB zIOAd)d*?jM@%tm!NRF9c-l#cDRu4Lr<4QY?dFI;gHIpxmYh-~PO)P|$eM^#RsWKGM zK=C?L@S?vE<{4-gJwb!+pNlL#FdH_wtKTkG)la-`@lHau_D%ksvp9cI^)sv>m?Uvi z9~ycg=>AW`O^XSBV(7(#(VbtC2Db3}$%#GZUF5feiaY^Pew4S^0GPNg|JUpVC%I7O zEoGJ8Hh$6Vf{u)~+?A76&wS^&BWnUD66Z*A@7_h~UNfm_Z-^v@qge>aK|Je%t9!$6Dt{pY61t4)EzyY=Dy8IM^Sc}Hxh z?|pZF<-_)EID2h)R@gl5{p^c^c(lM^>61ye4@%pmJt+UQY0n?W#n{L{=(`j1P;}CY zMe8=|U9Qp1xLSD4o(n+qzbyv$4wRyI&uPW=&G2P+hrT$=NpIDMvlszyQ9Aj^OJ_BL#ZVz_-GF9$!bWIVk z1q0M`Bz*UuQ=e5Mje0-JSMo(Ij^SH8p?5NN2Ypf}@9xS*+T`Z7xvOI@ME$>Lz9paHTXO4CcP1e2xJ0paUX4pPXXDOslXz; z8_15uw_x6-FZ`eL!|X*mfz3@z=Q(kdh`Cq7YPPnoX3AtyS&j(Y5q^cQw%ffzUc9I? zrwJPL6byZ#J;>hF1XrhIcK83(n7!16q3f#V9{$@vS z=wp89mq9Z-tDuG@()0O@Kq2)Q-|NtDlFG?Gen_4fujz&SI0DZOqT|S4zEu202?jJ2 z-2@HWJ}Sr`K2y5NjF^R}V+GvYH`Nq)7*%Uc5({o+dVpRFC9gf9f7=FREW@+VCTvCR+6|{8To>vBX^W zS6KXPW{m#uk6$wcD7D4Y>3*y8j!};l{Lot`dFQSqFR&=OJxL&YlOuo^zbNQY3@j{uRBX3e-gxC`^Z^|l4LGvq zcuXyBwHzIF9qJcfJgswPx+o*%u0l?Hjst$_M=dIa|5u_EBB?Cen5zU8Jr-hemk z=6F)?|41TVRDnS$qU-($^Iq$b@whMcAY`P=)cjpWr*HwCC zLDrTUIK$()w-c*Jt0Zvm&o?hR94sN;sVgY^4#Njgw7!xe_0By(k?Dkt!(YP&hc?na z;0Ff9Sf!&hm*@YMf}%6^Ong`L{`s zPR>d#yPxI~%BBR3_+zPT=$CJHqcf2jM7c?le@_8qt}Fa>`~k`;-OFMy(T~n#Nxu5$ zn9U*`t+#vo#A`pL{$D3FI+JwA!xhQ4I5@A&^zR>+J#FM2L>*6WV}B&WMf{zA!syoHfzMTr-}BEC??NbiZ4Nj9pTl(^Y_F@aWwLA znzT-ag)hBo;|U}7efD%^;V-3nfE^515FGVpFFG^iq~xPCnO{oSQF~dEcTGK;>5_(o z)J^yMOzt9*v$j%m9+FmCdOB)y_B00Lz1%$Ed8{h0-#nzhwaFIdnSp|!PsZ|_Ob)dPVLPt z-mn&>-xl^)r^d=phD38P7iwEu$416yf$v&dTP+RrZU2~eclpNB@Qi=NL&|R)@0T7y z#0MbqcXMH%Q&E8IHL@L@3rYsoH7zBC zTDC_yl=^R<^8R1BN!d@ z&qSS9tat_gr-~BQ984-C;FiHE`+vvP>h5thl;z#Ou2EGP__cMfvijgD((jOrDYDg) zH7e(!aw$Yym1irD!-ae6NAH?~p^(QpryuTff;iYMn9aaH`O)aV*?5fUcE{t@k?3&OFRx zY`1IM@MxT9h#Dubj#5PqOW$yF^_EQ#mK^6VbWSp@yk+@9lKq`V=-B=aWfMk-i(Lz$ z4~1rrIx{W<0yrT^(RC2k&WXIdIZG+C!=JA-yqI058~$NtX1SLdS7xDL${DZotu01Z zB2Al(zrdsV%g|I|#6Lx**FKetJ+;W{+0sHf*WQYFF~F9MiAGGtqk~ulCop2Pdxf>|oLP_RvDpyXof?yHDO8|bmsc|)8OQAwwLYW zQep;18*qnbz}7rrD$35_bIk0Fnw&%;O^XFy$B~jEz#kq`9{`Jh5@yr_JXUWr48<+L zMuVwsrtz8cVmko+#yrtw*Q?YWHa0QOnq-aM#}tOGFvY0mT+LGg3I+ymGCABl{2SFZ zU8fqj0kY*E;98hq8oeWTk`!mU@Fo*(ob@z$Cd|D3b0L1zkIKi@xG_RJ(lSm$Q zs>nsHtvQ9H`hfYf$BPW>8yFbBo1H~SY!>tCZO-JvT-GkH?i@wv{OjVIXXQ;lRS16= zEA=!BfgW$(BTfdQ8{YaUUeXA)wCd`2NAY%C)6-U0ytC#E{&1H9kLFMDI`n6# zXHx>7JE0(WUA2yI!t_;UCT8GBTJV&mD)U+DkDPQkZ&{vLV?+M^q&@yEDloczlCXb6 zPoPM~KK@PU3Ro&DkGq7o-x;QVgSXGVoHcmcc2eXr1R|91MnXcuOfrSW;)C9~fTM1C zL;03P+e>Fa)v$J3DGtvE!MxES4^0MFSeW{ky?)euQm-G)8sz_8()sp2fS;%5v)~ps z@CRAnyd}-%e?DIgf~)6T)p{vqIRI(iScv(O_(}Bp{=w+-5fVyxRqu05k>t=3b%TTqv;vrm!5&GnN;>b#?~ z=?q~HJQD=tmw9el++llUS|=}eIZ_Ur*3J|WOAm7+#L zrFGwcHszy0ymFg9kMiHTAnrW&b7v`X)JuS?@Z~UD4ZU^9%QDjuiuFG?3 zAmY9S@n|0Wd&RZmb3s&~pIyt?YWLN%23`qmWpgd7H{WS+GQH(|qxixr=hhv;TyAv@ zjp0eKW$$|5I&87bXcwl`Pjz|}rSARLjB#8J;sROI6+ccnb%m?9pHSy=(9L=)J7ybS z+u0x8E8eJNF|`5W#OCIeW1>s_EViB~db|aUs(#BTeqTz8twUH$T)eF$8A{w*f6R47 z_UV15txZLiO_yWh`Vj_9xEe^rpni6mi*jVJCIUf#Rw>C8^ylQx@ND` z&vSYM*(wNVu zj`e^&qk!$C$&JMvR7q)R)8?iSaF!(7%*7>g(F3Rx))f7ioSdf$-2dlzH)+y!yXA64 zW##bXq>W@szkRv)##3aU?IZFR>N#CQcwv?==;m(X1(tD%nFSA7(L!6p%R&)RG))-o zckeW=y={rHl7Bx>%~FU~TGT89C|~XASyt%i@POY-VCL+V){HCDHR-JHLmq33t14K8 z*ZkU24uc64x~~4>?m$y!V6Lu>RX4E^H^{uaDyn@~xf#U7s|kyIp-rqlnh)dEHgX08 zQ--9xKQ$Ex5*$DWGgkqPT)%XFjJP7^=LeiuLvXqt1PSNe6%t*>uvxORF7 zv(uBN1SCmW>p+@q0hV(^C_(j=P;{#|Nb`67L?wF1mxMiVIbN;T(xppcsI2l_rbqej z2#xN+=Ac2X9OL&YD)=`ekM|>EtlG*QuvLER%&@hwsqT~q=a?t&ff4i9gR&?Y9pGZv z!-7jAD=FR1>nlbHR)=zZGjVanD?5hV zj(#es#!QkgA&yQBBem(-qkH`Lf7QD1`4;nisEoF_sFjJf_+iq__HG`pK3jTuws(P1NwfdQ^#`TaYN^paZCfhDEvY%t)cXbw zyc$e>%x|6VXr_BFI+H*=y_nVMp4e6>ur<>g?e1kjmd@X}OS`|{)Z!9$4T3FiFX{E4 zxa$+mskemCQYLFSycXYjtz4g12NK(<>S@9vrO+?)X)RR%%lnaEC54xA>5W#w>)_H1)*q5(ITwQvgMI9$Zz`7aIxdyS}5PFG)m6I z+-<2`<{9pc++5|ZJT0^M&`5b5ov2)$N!*BIg@D@k$WB$(b0O3^akFcn#n~Il$W$J# zmJbI7TH3Mhvo7bp#}3OkEO`MUs-fi*>DqYK?YL4YnvQKuY1@b!E(( z+A==^A?zIMCG0}AL;bR!C%kxuuu;WY#!?LQt~WYqX-Z-``eQ9XaM8Kil!FWxmIhzf zLIO8dbDXeTJ~vSA-`Nt{)Z7s*?faX%YH0`gog+z^Aa)Zcqry=Tc8OL~=$ukf#j=%C zzUQd@X!(-1z#bddv9t}oB?geV?nwuy=h%C1z8^Xpf)wW(GO68R@N3!Fh--9mF|-3- zN(_38CdN(!dv;R88{1wPt}wk(Pv3y*zGhz@F1@KL zmMDBs zJ2(2J-FW%PZc)Ulg6A9hvc<||{nKJ$GABU)e3Dlh zIQvl1DUC%&qJU*1eZ7X2A$+8Z2VVJw(IYXIa>a0iV1H?@ z%NV@Onw+gh1fW~`Q{CzbtCvKUi`G)ZKQ;^bnD!+a>m}(rEz!jw(Jgs8C`tH%M zXAG4J$fjB^EHf0#%isEG@uK8znK8eI|CeZEL*6;=aP#O`mH=QkVZgtxchf$r!pXDD zIFCER&!jc3XXKSUG59v7x6UPySl*<^ zUP4AP2F#)U*Ct$p97IJBhBr_{Q{K^1ZS17?y253%NS!K;q@gYY6)6-{dF>zTcy{)i zvzQ47)x-l#tM9r;T0P&zLC;tXM=f%hZJ&%i7SKScjRrdmeM~&GPgJmu^IHjXxRx}x zJgmAd?fn5t41+Gs>^};`ML=yo>vIR+YTH?9jwLSkG2j=b1#qhog}xCb=~3YyDx3!W zw63IETjd%+a=G1&qoX^1-OmLRV649vO?k5D4g1h{2k-!CUdvdORNAhAm=J4u9pxg= z(Oj4veY&-L+LK4<$JqiXr=3G^K;d>qf^J9kT&bb_6|1Jkk?HBHggNp0K6_6#*kThE zcwlpL=_%bxd{S-Zta%!gu%yS`p~b+!;Jd$(*4^Dbz==iJ2<+qt)b%Mw)(y0xr28Pt z6jtn!*_K5Uh_Ni;^Ih$-YLGl`m4x99rp{~=S1RaCy$kmmos_*zeCpi zQ0~aq`FOEVT*J17goNHr>5+^MS})0$9X2RG;IC`fH;@>c6@H&L#gkbn_y!JynT2cI|<@mtBy57KJ6wC5xj=;_& zv14)xOxQ|WOJUCC$}>xZ9|=CJ9T*Fx@q=memsO6z>lp)K=&CQhEX3K1gta}?Lf{eM z4q*zx>02iR>5~1-ACsQzpiQ}g$bY4oFQ0*s&oD5!`wnr`Uq@-<4qV@?3^@_w(OnBy zuxB!f zy6tfd13tywi0E2NEeO3K-<%=ejad1CefvWVw#@M`va65wNS^I^$$!asD>4T|BGw4ys{<(`Oi0cU~6H!ICY^9Fu%Umz@dyD6cG7~JO&f3yNs>0J32n7D>*5T>?-l= zksmOg*{Zvdf|AGnXr@!e^DMj#v$w5Zf?NJ|+lP2~02n>y&yad*zNm5-PC zx>fg8_VDlEDjD!co6n?)Ys!-*6^1Ylo_WJ=p4d`ZxS+{yN(DjCKtRu3&rD`JZy+zX z+|gdtGuKwe0{3o}#t*TK?>8bLnE;&z#+Kdp6>ZWQ^{r10hq81%3#}k_x&?!~XnahE zwXf3XfQlw}yQ~m>w2G0qiLuFI+n@pz1YKU(3{j6vUAEM87}T+Em(5~Z_bX4*Wk52* z_l}e`Qj2lp5pRnO^(eB;A-TEsAvI5+@rLW=i+<&B0XwJ~i^YJZwu7k4z(8z!dXxd> zJk=yC2?_Y}>6(JvqxjmSj{^iP!6^#J^n}Ec-pzu31MTGlNLJ2rxo;18lRsCZ=|DNvAFm!WLD%{!!O68=Y1xHavEo9sRtC#c zMk@4{{CtvVYfL|{(jA%#v6Ck7o@>7Sv4A4vwzAM&u3R;AMUxKDK_=}jPvHKOcspHP zNrP_BFY)=@7fDOUD@mOx7WP~QEfs>PNW2eE?sI>o5?&?iY5h9Vl{ub9=y@70WUEUr z2^I0JnOIov;ISLELoP*v;qet3=!!VYA)fEF7rE#)OATUtDEA9h!kzMe;KJ?h774Y@ zE=k!Te6!eaYs#rHW3er>pHqt`r33%@z?-gH%Jic+PHk-jS%hX7CEZD^+cP@Go+ml- zlmFBB7~r_^7!@(Jkm|_%DCCM~|A1r)Hc>At)v00$!DI3OSvzpqQzDvHHud@5Q*OQ) z9~hFBO)n;iCkp@(`3fi0fKO4K+a5f>tK4IpJ>*(h@}Q)t2=HUrX-~(xGC8_Gse;&f zgd=%AdisqMKb&uw_~t&&CYR|j$U*X^FMY97v6tG=Mj*xL{JZJzkZ-%+(Hn}D=cPX(ugLEh*1)dI z`6cNPt6r@E@b+)#Mu6{pNi;D>^R)JX@>pco%x+JxH%B3Rik1v*0Noc?+G}ei74`gJ zai8rqjmCfOP>9Zosa-x~TlO<(_Y@b0>$`5_RAjPics+bYu)}?_Y9_g_%)DKi%)(j& z`cvbXNnC4BReHI_btLRq!#PRGnJo4`=t8BHj8>GvlPekYp1FH*&rF<+q(_}ShrNn7 zwgkea57@UHzOh{DA!H>rsD6HB*v*|%OfPBB1)Kl?C3LD4g-=9NU$M*Y4l206weFnJ^cVv@fknK&qE z((MU2g}doGvP-?5_0z?rVsv|1J`0c`{M&5@_+)3nmNBf$`m$^G_!?#rb`KFk*cuj| zn=Rd=f4kOYV+e_Dr*<#z&JW$qSXWw-rXoykeD%+%&F4+;*1ITuU!s50Jlg=m9W3|- z`9Uxghj6pG@xoi6$l8w1jN%NEjDPJ2U+qTeW zdC7Z@i)pu!>|R%g>>k70H9kg&d>V^rg>$qc%V<)h_?}xG57lN;0zeQU+ROgsy^DRu zR+^7X#=5`+>#u8!XQu?eg2}fotpBgYB$!>2Ci~$M{N8c~bd8MhiozVSmPEx|Y%+{Vx$i4vAH*=$v8`WfiPrb1Gze#Qy!F_1?qK%pv z%ShO*-v*&us=POMPujTKV?rnI<*d+tCH)ArxY%vbY5yy6M*7!qiHD4=F#sUH!IM~$sZORhKRP?k(@Q)vYhy;g8CLno^ zJLRw9mU40MLPP6*Bh*W-Dy}SlBWnOBsNT)htoRZR?D2C_x)+sP%X4jUA=TIURAm%%2uI^+#(#$cMRv-6*YABtJY>gW z;il|;@`l#YhL?Hi089)Wm}i$Br~+1f`%B8J#w?S{00{kWORh6q^6s-W)eGPJlsE6% zu!8Fi*i-l|YVd`>x#zu7sVgWK)y6|s^1U{VNMTsW%u+0ErwC7mdXjSeeZ!CI2+%TO zq%QEHUR=(OoSXFNzeUI>JdxrJSnM zDhr@fg-JgQ$>>z8u7PEA>JM-~&eGfRAM>bJ*c5%|kB~7*E|e zr9;Sc36!GzEs*^Sr@CZmW~zpHU?g#vrP^wc&Jm<0L+&H3rk<${+vUD(_NYB0J<^lHXvr2!aF;0NlTnUP*W-8P-CMIFe!yd59sSr}%+1F${2;Q{W5_@D!m zhv0gw5O3PQtbSA5#f>Vrw_FK{F#*wFVM9eyj;N_l>0SJmZsdZ zov@3|*7*Rka$uo1%3w-Xmw=cEL>W1{ZsXFt-`E|?r@dY2dEP4ugpq}I`h(u zCft+YrVkl2%{Co3ZfQ(CU78rPT8o|^)hv=XS!$XN8Ovla$??QGU~tNa*Tk73;qyRV zFb+0dB9z`4e_;BNzvHMzP8?2NVI;^X`{b1-Yl~9UvAh0y|K-AiTG)f5Obn2{@Ba0e zF1G5~_yCkQl}r^W>9e**Knm?M6wENhuGqQV=zRWk_4)D&E`Uwzjuv6X^&ARo{q2!l zp&zojJzdHN{Bjf}jk?N{AwHj1io$K~n?HFa<5ubIHDVQ5NDR z_%ny@zO@<61a!T8QlMpUW0mBWH(-aSk27)O9%Ag7ba&g+#Fa2r(&&sG8;wwhDR2nS=G~kJCy0`I>&+&&CqB-TS3Tu z-prd#c(;-XuQZ@r_d3O?_0@*<20(@G1^+Vp*tY4m=&0k0I7LN@~4oJ{2agirxQL&@z23_ z^G?TwoJyr0IOQ_mTim@=Wbb*TUsOoUgY|Js&^f$Z6bji(FPWqQ5xfZrNmts7^J#O` ze554tsRzK?Gw`vn-CFrFABxG)))Gs45?ADDf?H{SV`RPRvi3%_Eak5*I96iaB9PGQ z=922SGFv388PTp2^1;X8yJaFatNr%cgT>Zv{585?xJCTtCxQn&UgsCk98vM_hgpLm z^+<=6pv=st*&?dLMn43SaZ`_r%($E=ane2@nNBmmD;9Ovr^~ZSBnl=qu%iu`1dqNI z{gBdHkStIzy?GipoRJBRD`k}*sEw6m#MFl)O?Ib$8OVoBE76LfzJU(g6p&lxt0t2d@MWlGG~rwm#25DZ?uMT9opHV?%>V(O|^s( z1<0JJoQ+oUK=PGh?8KWi_^YNX9B82R3kgY8#Ho|7-jF>cyu?ZkGdEHvuN!)Vew{FJ z*KUv0iJGY>?rO^!Lp5<|<0_I$CN)MQukMmv#i;`HcS3VOg$=MgeM^LI`(4^DvVr()|2 z)mP})*ipP`H#wGPqp4E1*NWQS5M}7P`9DPpU}yBneHIH7zC8sVo$~ZAUB*2UmhT$` z?(f~wyTQyblPz_<-tQ4g?!eMr!(HVdr|cpghS|oX;y2u(1dkfi8=QMeKz;(FIx*!c zD-N{|$Ly3{`b?oJ7RlA7eWqnAYvt6Li4N(N?t7P4sG9KiC0aH>8VgtZfk&1+h25dq zTzu1W{}Bs;dj>^$$e7v%X(hXBpQ#SkjKiimV557Zkv-z0IINLi!YZ;SH0Tp?q|tzc zWbN*$%v4M%R~56FTITELxcowHsoYRpp|d;?Vs^jTt@Z+~#_j55vKt1uZ*d(UYp|QA z%aso!;4oY;Mte7RGj+;UIdY#UQy=^U=Gst_aUD42vV#NFOW#`h0Tu;ZShEyBbEHoJ zc3coqJwL*zB&I&XGsER4o*)&5V|ITUxUus&DS)P5pQ&L zh$%LE@m~mjbLu)UXDCp-!Eq$aw=?OP-sb@q)LdtAIhYTSJl+rz0F#kCuRWPKLo#+j zY-l?E@_RUc5$Y;{gBMPv2wS1=oy+xQZ1Nk`n5d_1_^l-3+mqQE zB(Ug&xBu)-OVnRxNfxAN9-Pb|5=hXsgp$kZ(1XfJlT)w`5L)g z0?WC`LgKn7bkcDSnDTe?Yw*8gK}f2C;$HzA<*@1GHSou search_line(const nav_msgs::msg::OccupancyGrid &grid, double x0, double y0, double x1, double y1); + + private: mutable std::mutex grid_mutex_; bool new_grid_msg_ = false; diff --git a/mission/njord_tasks/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml index 4b675182..61a239f3 100644 --- a/mission/njord_tasks/docking_task/params/docking_task_params.yaml +++ b/mission/njord_tasks/docking_task/params/docking_task_params.yaml @@ -23,7 +23,14 @@ docking_task_node: distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame # If the distance is greater than 6.0 we drive distance-4m towards the first buoy pair before trying to find it distance_between_buoys_in_pair: 5.0 # Distance between the two buoys in a pair - grid_topic: "grid" + grid_topic: "pcl_grid" + dock_structure_absolute_width: 4.39 # Width of the dock structure in meters 4.39 for task 4.2 and 4.52 for task 4.1 + dock_structure_absolute_width_padding: 0.5 # Padding on each side of the dock structure for line search start and end + + + + + dock_width: 4.13 # width of available docking space dock_width_tolerance: 0.5 dock_length: 2.0 # length of available docking space diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 31b523b2..f995383c 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -247,15 +247,24 @@ void DockingTaskNode::main_task() { buoy_vis_msg.header.stamp = this->now(); buoy_visualization_pub_->publish(buoy_vis_msg); + Eigen::Vector2d direction_vector_up; + direction_vector_up << (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + + buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / + 2 - odom_start_point_.x, + (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + + buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / + 2 - odom_start_point_.y; + direction_vector_up.normalize(); + geometry_msgs::msg::Point waypoint_third_pair; waypoint_third_pair.x = (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - 2; + 2 + direction_vector_up(0) * 2; waypoint_third_pair.y = (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - 2; + 2 + direction_vector_up(1) * 2; waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); set_desired_heading(odom_start_point_, waypoint_third_pair); @@ -377,4 +386,63 @@ std::shared_ptr DockingTaskNode::get_grid() { return grid_msg_; } +std::vector DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, double dx0, double dy0, + double dx1, double dy1) { + int x0 = x0 / grid.info.resolution + grid.info.width / 2; + int y0 = y0 / grid.info.resolution + grid.info.height / 2; + int x1 = x1 / grid.info.resolution + grid.info.width / 2; + int y1 = y1 / grid.info.resolution + grid.info.height / 2; + + std::vector occupied_cells; + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (steep) { + occupied_cells.reserve(std::abs(y1 - y0)); + } else { + occupied_cells.reserve(std::abs(x1 - x0)); + } + if (steep) { + std::swap(x0, y0); + std::swap(x1, y1); + } + bool swap = x0 > x1; + if (swap) { + std::swap(x0, x1); + std::swap(y0, y1); + } + int dx = x1 - x0; + int dy = std::abs(y1 - y0); + int error = 2 * dy - dx; + int ystep = (y0 < y1) ? 1 : -1; + int xbounds = steep ? grid.info.height : grid.info.width; + int ybounds = steep ? grid.info.width : grid.info.height; + int y = y0; + + for (int x = x0; x <= x1; x++) { + if (steep) { + if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { + occupied_cells.push_back(grid.data[y + x * grid.info.width] > 0); + } + } else { + if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { + occupied_cells.push_back(grid.data[y * grid.info.width + x] > 0); + } + } + if (error > 0) { + y += ystep; + error -= 2 * (dx - dy); + } else { + error += 2 * dy; + } + } + if (swap) { + std::reverse(occupied_cells.begin(), occupied_cells.end()); + } + return occupied_cells; + +} + +void DockingTaskNode::find_dock_structure_edges(){ + +} + } // namespace docking_task \ No newline at end of file From 47ba8abbf8d8e8ce70de48fd77d5e903f457cd9b Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Fri, 9 Aug 2024 14:30:56 +0000 Subject: [PATCH 52/67] Committing clang-format changes --- .../include/landmark_server/grid_manager.hpp | 8 +-- mission/landmark_server/src/grid_manager.cpp | 59 ++++++++++--------- .../include/docking_task/docking_task_ros.hpp | 4 +- .../docking_task/src/docking_task_ros.cpp | 24 ++++---- 4 files changed, 50 insertions(+), 45 deletions(-) diff --git a/mission/landmark_server/include/landmark_server/grid_manager.hpp b/mission/landmark_server/include/landmark_server/grid_manager.hpp index ca336574..0f2a78f1 100644 --- a/mission/landmark_server/include/landmark_server/grid_manager.hpp +++ b/mission/landmark_server/include/landmark_server/grid_manager.hpp @@ -27,11 +27,11 @@ class GridManager { void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value); void fill_polygon(int8_t *grid, - const Eigen::Array &polygon, - int value); - - bool point_in_polygon(int x, int y, const Eigen::Array &polygon); + const Eigen::Array &polygon, + int value); + bool point_in_polygon(int x, int y, + const Eigen::Array &polygon); private: float resolution_; diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp index 9eaf3361..cad2313a 100644 --- a/mission/landmark_server/src/grid_manager.cpp +++ b/mission/landmark_server/src/grid_manager.cpp @@ -25,40 +25,43 @@ void GridManager::handle_polygon( draw_line(x0, y0, x1, y1, grid, value); } - if ( vertices.cols() > 1){ - fill_polygon(grid, vertices, value); + if (vertices.cols() > 1) { + fill_polygon(grid, vertices, value); } } -void GridManager::fill_polygon(int8_t *grid, - const Eigen::Array &polygon, - int value) { - double max_x = polygon.row(0).maxCoeff(); - double min_x = polygon.row(0).minCoeff(); - double max_y = polygon.row(1).maxCoeff(); - double min_y = polygon.row(1).minCoeff(); - - for (int x = min_x; x < max_x; x++) { - for (int y = min_y; y < max_y; y++) { - if (x >= 0 && x < static_cast(width_) && y >= 0 && y < static_cast(height_)) { - if (point_in_polygon(x, y, polygon)) { - grid[y * width_ + x] += value; - } - } - } +void GridManager::fill_polygon( + int8_t *grid, const Eigen::Array &polygon, + int value) { + double max_x = polygon.row(0).maxCoeff(); + double min_x = polygon.row(0).minCoeff(); + double max_y = polygon.row(1).maxCoeff(); + double min_y = polygon.row(1).minCoeff(); + + for (int x = min_x; x < max_x; x++) { + for (int y = min_y; y < max_y; y++) { + if (x >= 0 && x < static_cast(width_) && y >= 0 && + y < static_cast(height_)) { + if (point_in_polygon(x, y, polygon)) { + grid[y * width_ + x] += value; + } } + } + } } -bool GridManager::point_in_polygon(int x, int y, const Eigen::Array &polygon) { - int i, j; - bool c = false; - for (i = 0, j = polygon.cols() - 1; i < polygon.cols(); j = i++) { - if (((polygon(1, i) > y) != (polygon(1, j) > y)) && - (x < (polygon(0, j) - polygon(0, i)) * (y - polygon(1, i)) / (polygon(1, j) - polygon(1, i)) + polygon(0, i)) - ) - c = !c; - } - return c; +bool GridManager::point_in_polygon( + int x, int y, const Eigen::Array &polygon) { + int i, j; + bool c = false; + for (i = 0, j = polygon.cols() - 1; i < polygon.cols(); j = i++) { + if (((polygon(1, i) > y) != (polygon(1, j) > y)) && + (x < (polygon(0, j) - polygon(0, i)) * (y - polygon(1, i)) / + (polygon(1, j) - polygon(1, i)) + + polygon(0, i))) + c = !c; + } + return c; } void GridManager::draw_line(int x0, int y0, int x1, int y1, int8_t *grid, diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp index 1ea93527..a5617ac8 100644 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -70,8 +70,8 @@ class DockingTaskNode : public NjordTaskBaseNode { * @param y1 The y-coordinate of the end point. * @return A vector of booleans representing occupied cells along the line. */ - std::vector search_line(const nav_msgs::msg::OccupancyGrid &grid, double x0, double y0, double x1, double y1); - + std::vector search_line(const nav_msgs::msg::OccupancyGrid &grid, + double x0, double y0, double x1, double y1); private: mutable std::mutex grid_mutex_; diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index f995383c..e88cc9ad 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -249,22 +249,26 @@ void DockingTaskNode::main_task() { Eigen::Vector2d direction_vector_up; direction_vector_up << (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + - buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - 2 - odom_start_point_.x, + buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / + 2 - + odom_start_point_.x, (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - 2 - odom_start_point_.y; + 2 - + odom_start_point_.y; direction_vector_up.normalize(); geometry_msgs::msg::Point waypoint_third_pair; waypoint_third_pair.x = (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - 2 + direction_vector_up(0) * 2; + 2 + + direction_vector_up(0) * 2; waypoint_third_pair.y = (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - 2 + direction_vector_up(1) * 2; + 2 + + direction_vector_up(1) * 2; waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); set_desired_heading(odom_start_point_, waypoint_third_pair); @@ -386,8 +390,9 @@ std::shared_ptr DockingTaskNode::get_grid() { return grid_msg_; } -std::vector DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, double dx0, double dy0, - double dx1, double dy1) { +std::vector +DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, + double dx0, double dy0, double dx1, double dy1) { int x0 = x0 / grid.info.resolution + grid.info.width / 2; int y0 = y0 / grid.info.resolution + grid.info.height / 2; int x1 = x1 / grid.info.resolution + grid.info.width / 2; @@ -438,11 +443,8 @@ std::vector DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGri std::reverse(occupied_cells.begin(), occupied_cells.end()); } return occupied_cells; - } -void DockingTaskNode::find_dock_structure_edges(){ - -} +void DockingTaskNode::find_dock_structure_edges() {} } // namespace docking_task \ No newline at end of file From d59023fdebb7d631722034ca599fe1948d5d74bd Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Fri, 9 Aug 2024 22:55:32 +0200 Subject: [PATCH 53/67] dock edge localization test --- .../include/docking_task/docking_task_ros.hpp | 11 +- .../params/docking_task_params.yaml | 11 +- .../docking_task/src/docking_task_ros.cpp | 641 +++++++++++------- 3 files changed, 404 insertions(+), 259 deletions(-) diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp index a5617ac8..d139c8d3 100644 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -57,10 +57,11 @@ class DockingTaskNode : public NjordTaskBaseNode { void initialize_grid_sub(); - void find_dock_structure_edges(); + std::pair find_dock_structure_edges(const geometry_msgs::msg::Point &waypoint_third_pair, + Eigen::Vector2d &direction_vector_up); /** - * @brief Iterate over line in grid map. Returns a vector of booleans + * @brief Iterate over line in grid map. Returns a vector * representing occupied cells along the line. * * @param grid The grid map. @@ -68,10 +69,10 @@ class DockingTaskNode : public NjordTaskBaseNode { * @param y0 The y-coordinate of the start point. * @param x1 The x-coordinate of the end point. * @param y1 The y-coordinate of the end point. - * @return A vector of booleans representing occupied cells along the line. + * @return A vector representing occupied cells along the line. */ - std::vector search_line(const nav_msgs::msg::OccupancyGrid &grid, - double x0, double y0, double x1, double y1); + std::vector search_line(const nav_msgs::msg::OccupancyGrid &grid, double x0, double y0, double x1, double y1); + private: mutable std::mutex grid_mutex_; diff --git a/mission/njord_tasks/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml index 61a239f3..121924a9 100644 --- a/mission/njord_tasks/docking_task/params/docking_task_params.yaml +++ b/mission/njord_tasks/docking_task/params/docking_task_params.yaml @@ -24,9 +24,12 @@ docking_task_node: # If the distance is greater than 6.0 we drive distance-4m towards the first buoy pair before trying to find it distance_between_buoys_in_pair: 5.0 # Distance between the two buoys in a pair grid_topic: "pcl_grid" - dock_structure_absolute_width: 4.39 # Width of the dock structure in meters 4.39 for task 4.2 and 4.52 for task 4.1 - dock_structure_absolute_width_padding: 0.5 # Padding on each side of the dock structure for line search start and end - + dock_structure_absolute_width: 4.39 # Width of the dock structure in meters 4.39 + dock_structure_absolute_width_tolerance: 0.5 # Padding on each side of the dock structure for line search start and end + dock_edge_width: 0.13 # width of the dock edge + line_search_distance: 2.0 # distance to iterate along line when searching for dock edge + dock_search_offset: 2.0 # offset in forward direction from third buoy pair to begin search for dock + @@ -35,9 +38,7 @@ docking_task_node: dock_width_tolerance: 0.5 dock_length: 2.0 # length of available docking space dock_length_tolerance: 0.5 - dock_edge_width: 0.13 # width of the dock edge dock_edge_width_tolerance: 0.05 - dock_search_offset: 0.2 # offset in forward direction to begin search for dock task_nr: 2 # possible values 1 and 2 corresponding to task 4.1 and 4.2 models: dynmod_stddev: 0.01 diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index e88cc9ad..000ce72d 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -8,13 +8,19 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) declare_parameter("distance_to_first_buoy_pair", 2.0); declare_parameter("distance_between_buoys_in_pair", 5.0); declare_parameter("grid_topic", "grid"); + declare_parameter("dock_structure_absolute_width", 0.0); + declare_parameter("dock_structure_absolute_width_tolerance", 0.0); + declare_parameter("dock_edge_width", 0.0); + declare_parameter("line_search_distance", 0.0); + declare_parameter("dock_search_offset", 0.0); + + + declare_parameter("dock_width", 0.0); declare_parameter("dock_width_tolerance", 0.0); declare_parameter("dock_length", 0.0); declare_parameter("dock_length_tolerance", 0.0); - declare_parameter("dock_edge_width", 0.0); declare_parameter("dock_edge_width_tolerance", 0.0); - declare_parameter("dock_search_offset", 0.0); declare_parameter("task_nr", 0.0); declare_parameter("models.dynmod_stddev", 0.0); declare_parameter("models.sen_stddev", 0.0); @@ -26,255 +32,316 @@ void DockingTaskNode::main_task() { navigation_ready(); // Starting docking task odom_start_point_ = get_odom()->pose.pose.position; - Eigen::Array predicted_first_pair = predict_first_buoy_pair(); - - sensor_msgs::msg::PointCloud2 buoy_vis_msg; - pcl::PointCloud buoy_vis; - pcl::PointXYZRGB buoy_red_0; - buoy_red_0.x = predicted_first_pair(0, 0); - buoy_red_0.y = predicted_first_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - pcl::PointXYZRGB buoy_green_1; - buoy_green_1.x = predicted_first_pair(0, 1); - buoy_green_1.y = predicted_first_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - if (distance_to_first_buoy_pair > 6.0) { - geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; - waypoint_to_approach_first_pair_base_link.x = - distance_to_first_buoy_pair - 4.0; - waypoint_to_approach_first_pair_base_link.y = 0.0; - waypoint_to_approach_first_pair_base_link.z = 0.0; - try { - auto transform = - tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); - geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; - tf2::doTransform(waypoint_to_approach_first_pair_base_link, - waypoint_to_approach_first_pair_odom, transform); - send_waypoint(waypoint_to_approach_first_pair_odom); - set_desired_heading(odom_start_point_, - waypoint_to_approach_first_pair_odom); - reach_waypoint(1.0); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - std::vector buoy_landmarks_0_to_1 = - get_buoy_landmarks(predicted_first_pair); - if (buoy_landmarks_0_to_1.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); + // Eigen::Array predicted_first_pair = predict_first_buoy_pair(); + + // sensor_msgs::msg::PointCloud2 buoy_vis_msg; + // pcl::PointCloud buoy_vis; + // pcl::PointXYZRGB buoy_red_0; + // buoy_red_0.x = predicted_first_pair(0, 0); + // buoy_red_0.y = predicted_first_pair(1, 0); + // buoy_red_0.z = 0.0; + // buoy_red_0.r = 255; + // buoy_red_0.g = 0; + // buoy_red_0.b = 0; + // buoy_vis.push_back(buoy_red_0); + // pcl::PointXYZRGB buoy_green_1; + // buoy_green_1.x = predicted_first_pair(0, 1); + // buoy_green_1.y = predicted_first_pair(1, 1); + // buoy_green_1.z = 0.0; + // buoy_green_1.r = 0; + // buoy_green_1.g = 255; + // buoy_green_1.b = 0; + // buoy_vis.push_back(buoy_green_1); + // pcl::toROSMsg(buoy_vis, buoy_vis_msg); + // buoy_vis_msg.header.frame_id = "odom"; + // buoy_vis_msg.header.stamp = this->now(); + // buoy_visualization_pub_->publish(buoy_vis_msg); + + // double distance_to_first_buoy_pair = + // this->get_parameter("distance_to_first_buoy_pair").as_double(); + // if (distance_to_first_buoy_pair > 6.0) { + // geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; + // waypoint_to_approach_first_pair_base_link.x = + // distance_to_first_buoy_pair - 4.0; + // waypoint_to_approach_first_pair_base_link.y = 0.0; + // waypoint_to_approach_first_pair_base_link.z = 0.0; + // try { + // auto transform = + // tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); + // geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; + // tf2::doTransform(waypoint_to_approach_first_pair_base_link, + // waypoint_to_approach_first_pair_odom, transform); + // send_waypoint(waypoint_to_approach_first_pair_odom); + // set_desired_heading(odom_start_point_, + // waypoint_to_approach_first_pair_odom); + // reach_waypoint(1.0); + // } catch (tf2::TransformException &ex) { + // RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + // } + // } + // std::vector buoy_landmarks_0_to_1 = + // get_buoy_landmarks(predicted_first_pair); + // if (buoy_landmarks_0_to_1.size() != 2) { + // RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); + // } + + // buoy_vis = pcl::PointCloud(); + // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + // buoy_red_0 = pcl::PointXYZRGB(); + // buoy_green_1 = pcl::PointXYZRGB(); + // buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; + // buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; + // buoy_red_0.z = 0.0; + // buoy_red_0.r = 255; + // buoy_red_0.g = 0; + // buoy_red_0.b = 0; + // buoy_vis.push_back(buoy_red_0); + // buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; + // buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; + // buoy_green_1.z = 0.0; + // buoy_green_1.r = 0; + // buoy_green_1.g = 255; + // buoy_green_1.b = 0; + // buoy_vis.push_back(buoy_green_1); + // pcl::toROSMsg(buoy_vis, buoy_vis_msg); + // buoy_vis_msg.header.frame_id = "odom"; + // buoy_vis_msg.header.stamp = this->now(); + // buoy_visualization_pub_->publish(buoy_vis_msg); + + // geometry_msgs::msg::Point waypoint_first_pair; + // waypoint_first_pair.x = + // (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + + // buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / + // 2; + // waypoint_first_pair.y = + // (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + + // buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / + // 2; + // waypoint_first_pair.z = 0.0; + // send_waypoint(waypoint_first_pair); + // set_desired_heading(odom_start_point_, waypoint_first_pair); + // reach_waypoint(1.0); + + // // Second pair of buoys + // Eigen::Array predicted_first_and_second_pair = + // predict_second_buoy_pair( + // buoy_landmarks_0_to_1[0].pose_odom_frame.position, + // buoy_landmarks_0_to_1[1].pose_odom_frame.position); + + // buoy_vis = pcl::PointCloud(); + // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + // buoy_red_0 = pcl::PointXYZRGB(); + // buoy_green_1 = pcl::PointXYZRGB(); + // buoy_red_0.x = predicted_first_and_second_pair(0, 0); + // buoy_red_0.y = predicted_first_and_second_pair(1, 0); + // buoy_red_0.z = 0.0; + // buoy_red_0.r = 255; + // buoy_red_0.g = 0; + // buoy_red_0.b = 0; + // buoy_vis.push_back(buoy_red_0); + // buoy_green_1.x = predicted_first_and_second_pair(0, 1); + // buoy_green_1.y = predicted_first_and_second_pair(1, 1); + // buoy_green_1.z = 0.0; + // buoy_green_1.r = 0; + // buoy_green_1.g = 255; + // buoy_green_1.b = 0; + // buoy_vis.push_back(buoy_green_1); + // pcl::toROSMsg(buoy_vis, buoy_vis_msg); + // buoy_vis_msg.header.frame_id = "odom"; + // buoy_vis_msg.header.stamp = this->now(); + // buoy_visualization_pub_->publish(buoy_vis_msg); + + // std::vector buoy_landmarks_2_to_3 = + // get_buoy_landmarks(predicted_first_and_second_pair); + // if (buoy_landmarks_2_to_3.size() != 2) { + // RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); + // } + + // buoy_vis = pcl::PointCloud(); + // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + // buoy_red_0 = pcl::PointXYZRGB(); + // buoy_green_1 = pcl::PointXYZRGB(); + // buoy_red_0.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; + // buoy_red_0.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; + // buoy_red_0.z = 0.0; + // buoy_red_0.r = 255; + // buoy_red_0.g = 0; + // buoy_red_0.b = 0; + // buoy_vis.push_back(buoy_red_0); + // buoy_green_1.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; + // buoy_green_1.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; + // buoy_green_1.z = 0.0; + // buoy_green_1.r = 0; + // buoy_green_1.g = 255; + // buoy_green_1.b = 0; + // buoy_vis.push_back(buoy_green_1); + // pcl::toROSMsg(buoy_vis, buoy_vis_msg); + // buoy_vis_msg.header.frame_id = "odom"; + // buoy_vis_msg.header.stamp = this->now(); + // buoy_visualization_pub_->publish(buoy_vis_msg); + + // geometry_msgs::msg::Point waypoint_second_pair; + // waypoint_second_pair.x = + // (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + + // buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / + // 2; + // waypoint_second_pair.y = + // (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + + // buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / + // 2; + // waypoint_second_pair.z = 0.0; + // send_waypoint(waypoint_second_pair); + // set_desired_heading(odom_start_point_, waypoint_second_pair); + // reach_waypoint(1.0); + + // // Third pair of buoys + // Eigen::Array predicted_third_pair = predict_third_buoy_pair( + // buoy_landmarks_0_to_1[0].pose_odom_frame.position, + // buoy_landmarks_0_to_1[1].pose_odom_frame.position, + // buoy_landmarks_2_to_3[2].pose_odom_frame.position, + // buoy_landmarks_2_to_3[3].pose_odom_frame.position); + + // buoy_vis = pcl::PointCloud(); + // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + // buoy_red_0 = pcl::PointXYZRGB(); + // buoy_green_1 = pcl::PointXYZRGB(); + // buoy_red_0.x = predicted_third_pair(0, 0); + // buoy_red_0.y = predicted_third_pair(1, 0); + // buoy_red_0.z = 0.0; + // buoy_red_0.r = 255; + // buoy_red_0.g = 0; + // buoy_red_0.b = 0; + // buoy_vis.push_back(buoy_red_0); + // buoy_green_1.x = predicted_third_pair(0, 1); + // buoy_green_1.y = predicted_third_pair(1, 1); + // buoy_green_1.z = 0.0; + // buoy_green_1.r = 0; + // buoy_green_1.g = 255; + // buoy_green_1.b = 0; + // buoy_vis.push_back(buoy_green_1); + // pcl::toROSMsg(buoy_vis, buoy_vis_msg); + // buoy_vis_msg.header.frame_id = "odom"; + // buoy_vis_msg.header.stamp = this->now(); + // buoy_visualization_pub_->publish(buoy_vis_msg); + + // std::vector buoy_landmarks_4_to_5 = + // get_buoy_landmarks(predicted_third_pair); + // if (buoy_landmarks_4_to_5.size() != 2) { + // RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); + // } + + // buoy_vis = pcl::PointCloud(); + // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + // buoy_red_0 = pcl::PointXYZRGB(); + // buoy_green_1 = pcl::PointXYZRGB(); + // buoy_red_0.x = buoy_landmarks_4_to_5[0].pose_odom_frame.position.x; + // buoy_red_0.y = buoy_landmarks_4_to_5[0].pose_odom_frame.position.y; + // buoy_red_0.z = 0.0; + // buoy_red_0.r = 255; + // buoy_red_0.g = 0; + // buoy_red_0.b = 0; + // buoy_vis.push_back(buoy_red_0); + // buoy_green_1.x = buoy_landmarks_4_to_5[1].pose_odom_frame.position.x; + // buoy_green_1.y = buoy_landmarks_4_to_5[1].pose_odom_frame.position.y; + // buoy_green_1.z = 0.0; + // buoy_green_1.r = 0; + // buoy_green_1.g = 255; + // buoy_green_1.b = 0; + // buoy_vis.push_back(buoy_green_1); + // pcl::toROSMsg(buoy_vis, buoy_vis_msg); + // buoy_vis_msg.header.frame_id = "odom"; + // buoy_vis_msg.header.stamp = this->now(); + // buoy_visualization_pub_->publish(buoy_vis_msg); + + // Eigen::Vector2d direction_vector_up; + // direction_vector_up << (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + + // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / + // 2 - odom_start_point_.x, + // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + + // buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / + // 2 - odom_start_point_.y; + // direction_vector_up.normalize(); + + // geometry_msgs::msg::Point waypoint_third_pair; + // waypoint_third_pair.x = + // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + + // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / + // 2; + // waypoint_third_pair.y = + // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + + // buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / + // 2; + // waypoint_third_pair.z = 0.0; + // send_waypoint(waypoint_third_pair); + // set_desired_heading(odom_start_point_, waypoint_third_pair); + + // geometry_msgs::msg::Point waypoint_through_third_pair; + // waypoint_through_third_pair.x = + // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + + // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / + // 2 + direction_vector_up(0) * 2; + // waypoint_through_third_pair.y = + // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + + // buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / + // 2 + direction_vector_up(1) * 2; + // waypoint_through_third_pair.z = 0.0; + // send_waypoint(waypoint_through_third_pair); + // set_desired_heading(odom_start_point_, waypoint_through_third_pair); + // reach_waypoint(1.0); + + + + geometry_msgs::msg::Point direction_vector_up_base_link; + direction_vector_up_base_link.x = 1.0; + direction_vector_up_base_link.y = 0.0; + direction_vector_up_base_link.z = 0.0; + Eigen::Vector2d direction_vector_up_test; + try { + auto transform = tf_buffer_->lookupTransform( + "odom", "base_link", tf2::TimePointZero); + geometry_msgs::msg::Point direction_vector_up_odom; + tf2::doTransform(direction_vector_up_base_link, + direction_vector_up_odom, transform); + direction_vector_up_test << direction_vector_up_odom.x, direction_vector_up_odom.y; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; - buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; - buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_first_pair; - waypoint_first_pair.x = - (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + - buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / - 2; - waypoint_first_pair.y = - (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + - buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / - 2; - waypoint_first_pair.z = 0.0; - send_waypoint(waypoint_first_pair); - set_desired_heading(odom_start_point_, waypoint_first_pair); - reach_waypoint(1.0); - - // Second pair of buoys - Eigen::Array predicted_first_and_second_pair = - predict_second_buoy_pair( - buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_first_and_second_pair(0, 0); - buoy_red_0.y = predicted_first_and_second_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_first_and_second_pair(0, 1); - buoy_green_1.y = predicted_first_and_second_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_2_to_3 = - get_buoy_landmarks(predicted_first_and_second_pair); - if (buoy_landmarks_2_to_3.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; - buoy_red_0.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; - buoy_green_1.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); + std::thread(&DockingTaskNode::initialize_grid_sub, this).detach(); + Eigen::Vector2d edge1, edge2; + // std::tie(edge1, edge2) = find_dock_structure_edges(waypoint_third_pair, direction_vector_up); - geometry_msgs::msg::Point waypoint_second_pair; - waypoint_second_pair.x = - (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + - buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / - 2; - waypoint_second_pair.y = - (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + - buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / - 2; - waypoint_second_pair.z = 0.0; - send_waypoint(waypoint_second_pair); - set_desired_heading(odom_start_point_, waypoint_second_pair); - reach_waypoint(1.0); - - // Third pair of buoys - Eigen::Array predicted_third_pair = predict_third_buoy_pair( - buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position, - buoy_landmarks_2_to_3[2].pose_odom_frame.position, - buoy_landmarks_2_to_3[3].pose_odom_frame.position); - buoy_vis = pcl::PointCloud(); + std::tie(edge1, edge2) = find_dock_structure_edges(odom_start_point_, direction_vector_up_test); + RCLCPP_INFO(this->get_logger(), "Edge 1: %f, %f", edge1(0), edge1(1)); + RCLCPP_INFO(this->get_logger(), "Edge 2: %f, %f", edge2(0), edge2(1)); + sensor_msgs::msg::PointCloud2 buoy_vis_msg; + pcl::PointCloud buoy_vis; buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_third_pair(0, 0); - buoy_red_0.y = predicted_third_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_third_pair(0, 1); - buoy_green_1.y = predicted_third_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_4_to_5 = - get_buoy_landmarks(predicted_third_pair); - if (buoy_landmarks_4_to_5.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = buoy_landmarks_4_to_5[0].pose_odom_frame.position.x; - buoy_red_0.y = buoy_landmarks_4_to_5[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = buoy_landmarks_4_to_5[1].pose_odom_frame.position.x; - buoy_green_1.y = buoy_landmarks_4_to_5[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); + pcl::PointXYZRGB edge1_point; + edge1_point.x = edge1(0); + edge1_point.y = edge1(1); + edge1_point.z = 0.0; + edge1_point.r = 255; + edge1_point.g = 0; + edge1_point.b = 0; + pcl::PointXYZRGB edge2_point; + edge2_point.x = edge2(0); + edge2_point.y = edge2(1); + edge2_point.z = 0.0; + edge2_point.r = 0; + edge2_point.g = 255; + edge2_point.b = 0; + buoy_vis.push_back(edge1_point); + buoy_vis.push_back(edge2_point); pcl::toROSMsg(buoy_vis, buoy_vis_msg); buoy_vis_msg.header.frame_id = "odom"; buoy_vis_msg.header.stamp = this->now(); buoy_visualization_pub_->publish(buoy_vis_msg); - Eigen::Vector2d direction_vector_up; - direction_vector_up << (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + - buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - 2 - - odom_start_point_.x, - (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + - buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - 2 - - odom_start_point_.y; - direction_vector_up.normalize(); - - geometry_msgs::msg::Point waypoint_third_pair; - waypoint_third_pair.x = - (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + - buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - 2 + - direction_vector_up(0) * 2; - waypoint_third_pair.y = - (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + - buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - 2 + - direction_vector_up(1) * 2; - waypoint_third_pair.z = 0.0; - send_waypoint(waypoint_third_pair); - set_desired_heading(odom_start_point_, waypoint_third_pair); - reach_waypoint(1.0); - std::thread(&DockingTaskNode::initialize_grid_sub, this).detach(); } Eigen::Array DockingTaskNode::predict_first_buoy_pair() { @@ -390,15 +457,14 @@ std::shared_ptr DockingTaskNode::get_grid() { return grid_msg_; } -std::vector -DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, - double dx0, double dy0, double dx1, double dy1) { - int x0 = x0 / grid.info.resolution + grid.info.width / 2; - int y0 = y0 / grid.info.resolution + grid.info.height / 2; - int x1 = x1 / grid.info.resolution + grid.info.width / 2; - int y1 = y1 / grid.info.resolution + grid.info.height / 2; +std::vector DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, double dx0, double dy0, + double dx1, double dy1) { + int x0 = dx0 / grid.info.resolution + grid.info.width / 2; + int y0 = dy0 / grid.info.resolution + grid.info.height / 2; + int x1 = dx1 / grid.info.resolution + grid.info.width / 2; + int y1 = dy1 / grid.info.resolution + grid.info.height / 2; - std::vector occupied_cells; + std::vector occupied_cells; bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); if (steep) { occupied_cells.reserve(std::abs(y1 - y0)); @@ -425,11 +491,11 @@ DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, for (int x = x0; x <= x1; x++) { if (steep) { if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { - occupied_cells.push_back(grid.data[y + x * grid.info.width] > 0); + occupied_cells.push_back(grid.data[y + x * grid.info.width]); } } else { if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { - occupied_cells.push_back(grid.data[y * grid.info.width + x] > 0); + occupied_cells.push_back(grid.data[y * grid.info.width + x]); } } if (error > 0) { @@ -445,6 +511,83 @@ DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, return occupied_cells; } -void DockingTaskNode::find_dock_structure_edges() {} +std::pair DockingTaskNode::find_dock_structure_edges(const geometry_msgs::msg::Point &waypoint_third_pair, + Eigen::Vector2d &direction_vector_up){ + double dock_width_structure_absolute_width = this->get_parameter("dock_structure_absolute_width").as_double(); + double dock_width_structure_absolute_width_tolerance = this->get_parameter("dock_structure_absolute_width_tolerance").as_double(); + double dock_search_offset = this->get_parameter("dock_search_offset").as_double(); + Eigen::Vector2d direction_vector_right; + // Rotate direction vector 90 degrees to the right + direction_vector_right << direction_vector_up(1), -direction_vector_up(0); + geometry_msgs::msg::Point waypoint_third_pair_map; + bool transform_success = false; + while (!transform_success) { + try { + auto transform = tf_buffer_->lookupTransform( + "map", "odom", tf2::TimePointZero); + tf2::doTransform(waypoint_third_pair, + waypoint_third_pair_map, transform); + transform_success = true; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + } + } + Eigen::Vector2d line_start, line_end; + line_start << waypoint_third_pair_map.x - (direction_vector_right(0) * (dock_width_structure_absolute_width / 2 + dock_width_structure_absolute_width_tolerance)) + + (direction_vector_up(0) * dock_search_offset), + waypoint_third_pair_map.y - (direction_vector_right(1) * (dock_width_structure_absolute_width / 2 + dock_width_structure_absolute_width_tolerance)) + + (direction_vector_up(1) * dock_search_offset); + line_end << waypoint_third_pair_map.x + (direction_vector_right(0) * (dock_width_structure_absolute_width / 2 + dock_width_structure_absolute_width_tolerance)) + + (direction_vector_up(0) * dock_search_offset), + waypoint_third_pair_map.y + (direction_vector_right(1) * (dock_width_structure_absolute_width / 2 + dock_width_structure_absolute_width_tolerance)) + + (direction_vector_up(1) * dock_search_offset); + // double line_length = std::sqrt(std::pow(line_end(0) - line_start(0), 2) + std::pow(line_end(1) - line_start(1), 2)); + double dock_edge_width = this->get_parameter("dock_edge_width").as_double(); + double dock_width = dock_width_structure_absolute_width - 2 * dock_edge_width; + nav_msgs::msg::MapMetaData grid_info = get_grid()->info; + double line_search_distance = this->get_parameter("line_search_distance").as_double(); + int search_iterations = static_cast (line_search_distance / grid_info.resolution); + int result_index = -1; + // int line_vector_size = -1; + while (true){ + std::shared_ptr grid = get_grid(); + std::vector occupied_cells = search_line(*grid, line_start(0), line_start(1), line_end(0), line_end(1)); + int max_result = std::numeric_limits::min(); + int max_index = -1; + for(int i = 0; i < search_iterations; i++){ + int left_edge_left = i; + int left_edge_right = left_edge_left + static_cast (dock_edge_width / grid->info.resolution); + int right_edge_left = left_edge_right + static_cast (dock_width / grid->info.resolution); + int right_edge_right = right_edge_left + static_cast (dock_edge_width / grid->info.resolution); + int left_edge_occupied_cells = std::accumulate(occupied_cells.begin() + left_edge_left, occupied_cells.begin() + left_edge_right, 0); + int right_edge_occupied_cells = std::accumulate(occupied_cells.begin() + right_edge_left, occupied_cells.begin() + right_edge_right, 0); + if (left_edge_occupied_cells == 0 || right_edge_occupied_cells == 0){ + continue; + } + if (left_edge_occupied_cells + right_edge_occupied_cells > max_result){ + max_result = left_edge_occupied_cells + right_edge_occupied_cells; + max_index = i; + } + } + if (max_result > 300) { + result_index = max_index; + // line_vector_size = occupied_cells.size(); + } + line_start(0) = line_start(0) + direction_vector_up(0) * grid_info.resolution; + line_start(1) = line_start(1) + direction_vector_up(1) * grid_info.resolution; + line_end(0) = line_end(0) + direction_vector_up(0) * grid_info.resolution; + line_end(1) = line_end(1) + direction_vector_up(1) * grid_info.resolution; + } + int right_edge_left_index = result_index + static_cast (dock_edge_width / grid_info.resolution) + + static_cast (dock_width / grid_info.resolution); + + Eigen::Vector2d dock_edge_left, dock_edge_right; + dock_edge_left << line_start(0) + (direction_vector_right(0) * result_index * grid_info.resolution) + dock_edge_width / 2, + line_start(1) + (direction_vector_right(1) * result_index * grid_info.resolution) + dock_edge_width / 2; + dock_edge_right << line_start(0) + (direction_vector_right(0) * right_edge_left_index * grid_info.resolution) + dock_edge_width / 2, + line_start(1) + (direction_vector_right(1) * right_edge_left_index * grid_info.resolution) + dock_edge_width / 2; + + return std::make_pair(dock_edge_left, dock_edge_right); +} } // namespace docking_task \ No newline at end of file From 8a6aab5649439ddecab0182716e4bec57cbe9d2f Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Fri, 9 Aug 2024 20:55:58 +0000 Subject: [PATCH 54/67] Committing clang-format changes --- .../include/docking_task/docking_task_ros.hpp | 9 +- .../docking_task/src/docking_task_ros.cpp | 160 +++++++++++------- 2 files changed, 107 insertions(+), 62 deletions(-) diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp index d139c8d3..8463100e 100644 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -57,8 +57,9 @@ class DockingTaskNode : public NjordTaskBaseNode { void initialize_grid_sub(); - std::pair find_dock_structure_edges(const geometry_msgs::msg::Point &waypoint_third_pair, - Eigen::Vector2d &direction_vector_up); + std::pair find_dock_structure_edges( + const geometry_msgs::msg::Point &waypoint_third_pair, + Eigen::Vector2d &direction_vector_up); /** * @brief Iterate over line in grid map. Returns a vector @@ -71,8 +72,8 @@ class DockingTaskNode : public NjordTaskBaseNode { * @param y1 The y-coordinate of the end point. * @return A vector representing occupied cells along the line. */ - std::vector search_line(const nav_msgs::msg::OccupancyGrid &grid, double x0, double y0, double x1, double y1); - + std::vector search_line(const nav_msgs::msg::OccupancyGrid &grid, + double x0, double y0, double x1, double y1); private: mutable std::mutex grid_mutex_; diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 000ce72d..b58d8a38 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -14,8 +14,6 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) declare_parameter("line_search_distance", 0.0); declare_parameter("dock_search_offset", 0.0); - - declare_parameter("dock_width", 0.0); declare_parameter("dock_width_tolerance", 0.0); declare_parameter("dock_length", 0.0); @@ -32,7 +30,8 @@ void DockingTaskNode::main_task() { navigation_ready(); // Starting docking task odom_start_point_ = get_odom()->pose.pose.position; - // Eigen::Array predicted_first_pair = predict_first_buoy_pair(); + // Eigen::Array predicted_first_pair = + // predict_first_buoy_pair(); // sensor_msgs::msg::PointCloud2 buoy_vis_msg; // pcl::PointCloud buoy_vis; @@ -67,7 +66,8 @@ void DockingTaskNode::main_task() { // waypoint_to_approach_first_pair_base_link.z = 0.0; // try { // auto transform = - // tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); + // tf_buffer_->lookupTransform("odom", "base_link", + // tf2::TimePointZero); // geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; // tf2::doTransform(waypoint_to_approach_first_pair_base_link, // waypoint_to_approach_first_pair_odom, transform); @@ -254,7 +254,8 @@ void DockingTaskNode::main_task() { // buoy_visualization_pub_->publish(buoy_vis_msg); // Eigen::Vector2d direction_vector_up; - // direction_vector_up << (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + + // direction_vector_up << (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + // + // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / // 2 - odom_start_point_.x, // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + @@ -289,31 +290,30 @@ void DockingTaskNode::main_task() { // set_desired_heading(odom_start_point_, waypoint_through_third_pair); // reach_waypoint(1.0); - - geometry_msgs::msg::Point direction_vector_up_base_link; direction_vector_up_base_link.x = 1.0; direction_vector_up_base_link.y = 0.0; direction_vector_up_base_link.z = 0.0; Eigen::Vector2d direction_vector_up_test; try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero); + auto transform = + tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); geometry_msgs::msg::Point direction_vector_up_odom; - tf2::doTransform(direction_vector_up_base_link, - direction_vector_up_odom, transform); - direction_vector_up_test << direction_vector_up_odom.x, direction_vector_up_odom.y; + tf2::doTransform(direction_vector_up_base_link, direction_vector_up_odom, + transform); + direction_vector_up_test << direction_vector_up_odom.x, + direction_vector_up_odom.y; } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } - std::thread(&DockingTaskNode::initialize_grid_sub, this).detach(); Eigen::Vector2d edge1, edge2; - // std::tie(edge1, edge2) = find_dock_structure_edges(waypoint_third_pair, direction_vector_up); + // std::tie(edge1, edge2) = find_dock_structure_edges(waypoint_third_pair, + // direction_vector_up); - - std::tie(edge1, edge2) = find_dock_structure_edges(odom_start_point_, direction_vector_up_test); + std::tie(edge1, edge2) = + find_dock_structure_edges(odom_start_point_, direction_vector_up_test); RCLCPP_INFO(this->get_logger(), "Edge 1: %f, %f", edge1(0), edge1(1)); RCLCPP_INFO(this->get_logger(), "Edge 2: %f, %f", edge2(0), edge2(1)); sensor_msgs::msg::PointCloud2 buoy_vis_msg; @@ -340,8 +340,6 @@ void DockingTaskNode::main_task() { buoy_vis_msg.header.frame_id = "odom"; buoy_vis_msg.header.stamp = this->now(); buoy_visualization_pub_->publish(buoy_vis_msg); - - } Eigen::Array DockingTaskNode::predict_first_buoy_pair() { @@ -457,8 +455,9 @@ std::shared_ptr DockingTaskNode::get_grid() { return grid_msg_; } -std::vector DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, double dx0, double dy0, - double dx1, double dy1) { +std::vector +DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, + double dx0, double dy0, double dx1, double dy1) { int x0 = dx0 / grid.info.resolution + grid.info.width / 2; int y0 = dy0 / grid.info.resolution + grid.info.height / 2; int x1 = dx1 / grid.info.resolution + grid.info.width / 2; @@ -511,11 +510,17 @@ std::vector DockingTaskNode::search_line(const nav_msgs::msg::OccupancyG return occupied_cells; } -std::pair DockingTaskNode::find_dock_structure_edges(const geometry_msgs::msg::Point &waypoint_third_pair, - Eigen::Vector2d &direction_vector_up){ - double dock_width_structure_absolute_width = this->get_parameter("dock_structure_absolute_width").as_double(); - double dock_width_structure_absolute_width_tolerance = this->get_parameter("dock_structure_absolute_width_tolerance").as_double(); - double dock_search_offset = this->get_parameter("dock_search_offset").as_double(); +std::pair +DockingTaskNode::find_dock_structure_edges( + const geometry_msgs::msg::Point &waypoint_third_pair, + Eigen::Vector2d &direction_vector_up) { + double dock_width_structure_absolute_width = + this->get_parameter("dock_structure_absolute_width").as_double(); + double dock_width_structure_absolute_width_tolerance = + this->get_parameter("dock_structure_absolute_width_tolerance") + .as_double(); + double dock_search_offset = + this->get_parameter("dock_search_offset").as_double(); Eigen::Vector2d direction_vector_right; // Rotate direction vector 90 degrees to the right direction_vector_right << direction_vector_up(1), -direction_vector_up(0); @@ -523,48 +528,73 @@ std::pair DockingTaskNode::find_dock_structure bool transform_success = false; while (!transform_success) { try { - auto transform = tf_buffer_->lookupTransform( - "map", "odom", tf2::TimePointZero); - tf2::doTransform(waypoint_third_pair, - waypoint_third_pair_map, transform); + auto transform = + tf_buffer_->lookupTransform("map", "odom", tf2::TimePointZero); + tf2::doTransform(waypoint_third_pair, waypoint_third_pair_map, transform); transform_success = true; } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } } Eigen::Vector2d line_start, line_end; - line_start << waypoint_third_pair_map.x - (direction_vector_right(0) * (dock_width_structure_absolute_width / 2 + dock_width_structure_absolute_width_tolerance)) - + (direction_vector_up(0) * dock_search_offset), - waypoint_third_pair_map.y - (direction_vector_right(1) * (dock_width_structure_absolute_width / 2 + dock_width_structure_absolute_width_tolerance)) - + (direction_vector_up(1) * dock_search_offset); - line_end << waypoint_third_pair_map.x + (direction_vector_right(0) * (dock_width_structure_absolute_width / 2 + dock_width_structure_absolute_width_tolerance)) - + (direction_vector_up(0) * dock_search_offset), - waypoint_third_pair_map.y + (direction_vector_right(1) * (dock_width_structure_absolute_width / 2 + dock_width_structure_absolute_width_tolerance)) - + (direction_vector_up(1) * dock_search_offset); - // double line_length = std::sqrt(std::pow(line_end(0) - line_start(0), 2) + std::pow(line_end(1) - line_start(1), 2)); + line_start << waypoint_third_pair_map.x - + (direction_vector_right(0) * + (dock_width_structure_absolute_width / 2 + + dock_width_structure_absolute_width_tolerance)) + + (direction_vector_up(0) * dock_search_offset), + waypoint_third_pair_map.y - + (direction_vector_right(1) * + (dock_width_structure_absolute_width / 2 + + dock_width_structure_absolute_width_tolerance)) + + (direction_vector_up(1) * dock_search_offset); + line_end << waypoint_third_pair_map.x + + (direction_vector_right(0) * + (dock_width_structure_absolute_width / 2 + + dock_width_structure_absolute_width_tolerance)) + + (direction_vector_up(0) * dock_search_offset), + waypoint_third_pair_map.y + + (direction_vector_right(1) * + (dock_width_structure_absolute_width / 2 + + dock_width_structure_absolute_width_tolerance)) + + (direction_vector_up(1) * dock_search_offset); + // double line_length = std::sqrt(std::pow(line_end(0) - line_start(0), 2) + + // std::pow(line_end(1) - line_start(1), 2)); double dock_edge_width = this->get_parameter("dock_edge_width").as_double(); double dock_width = dock_width_structure_absolute_width - 2 * dock_edge_width; nav_msgs::msg::MapMetaData grid_info = get_grid()->info; - double line_search_distance = this->get_parameter("line_search_distance").as_double(); - int search_iterations = static_cast (line_search_distance / grid_info.resolution); + double line_search_distance = + this->get_parameter("line_search_distance").as_double(); + int search_iterations = + static_cast(line_search_distance / grid_info.resolution); int result_index = -1; // int line_vector_size = -1; - while (true){ + while (true) { std::shared_ptr grid = get_grid(); - std::vector occupied_cells = search_line(*grid, line_start(0), line_start(1), line_end(0), line_end(1)); + std::vector occupied_cells = search_line( + *grid, line_start(0), line_start(1), line_end(0), line_end(1)); int max_result = std::numeric_limits::min(); int max_index = -1; - for(int i = 0; i < search_iterations; i++){ + for (int i = 0; i < search_iterations; i++) { int left_edge_left = i; - int left_edge_right = left_edge_left + static_cast (dock_edge_width / grid->info.resolution); - int right_edge_left = left_edge_right + static_cast (dock_width / grid->info.resolution); - int right_edge_right = right_edge_left + static_cast (dock_edge_width / grid->info.resolution); - int left_edge_occupied_cells = std::accumulate(occupied_cells.begin() + left_edge_left, occupied_cells.begin() + left_edge_right, 0); - int right_edge_occupied_cells = std::accumulate(occupied_cells.begin() + right_edge_left, occupied_cells.begin() + right_edge_right, 0); - if (left_edge_occupied_cells == 0 || right_edge_occupied_cells == 0){ + int left_edge_right = + left_edge_left + + static_cast(dock_edge_width / grid->info.resolution); + int right_edge_left = + left_edge_right + + static_cast(dock_width / grid->info.resolution); + int right_edge_right = + right_edge_left + + static_cast(dock_edge_width / grid->info.resolution); + int left_edge_occupied_cells = + std::accumulate(occupied_cells.begin() + left_edge_left, + occupied_cells.begin() + left_edge_right, 0); + int right_edge_occupied_cells = + std::accumulate(occupied_cells.begin() + right_edge_left, + occupied_cells.begin() + right_edge_right, 0); + if (left_edge_occupied_cells == 0 || right_edge_occupied_cells == 0) { continue; } - if (left_edge_occupied_cells + right_edge_occupied_cells > max_result){ + if (left_edge_occupied_cells + right_edge_occupied_cells > max_result) { max_result = left_edge_occupied_cells + right_edge_occupied_cells; max_index = i; } @@ -573,19 +603,33 @@ std::pair DockingTaskNode::find_dock_structure result_index = max_index; // line_vector_size = occupied_cells.size(); } - line_start(0) = line_start(0) + direction_vector_up(0) * grid_info.resolution; - line_start(1) = line_start(1) + direction_vector_up(1) * grid_info.resolution; + line_start(0) = + line_start(0) + direction_vector_up(0) * grid_info.resolution; + line_start(1) = + line_start(1) + direction_vector_up(1) * grid_info.resolution; line_end(0) = line_end(0) + direction_vector_up(0) * grid_info.resolution; line_end(1) = line_end(1) + direction_vector_up(1) * grid_info.resolution; } - int right_edge_left_index = result_index + static_cast (dock_edge_width / grid_info.resolution) - + static_cast (dock_width / grid_info.resolution); + int right_edge_left_index = + result_index + static_cast(dock_edge_width / grid_info.resolution) + + static_cast(dock_width / grid_info.resolution); Eigen::Vector2d dock_edge_left, dock_edge_right; - dock_edge_left << line_start(0) + (direction_vector_right(0) * result_index * grid_info.resolution) + dock_edge_width / 2, - line_start(1) + (direction_vector_right(1) * result_index * grid_info.resolution) + dock_edge_width / 2; - dock_edge_right << line_start(0) + (direction_vector_right(0) * right_edge_left_index * grid_info.resolution) + dock_edge_width / 2, - line_start(1) + (direction_vector_right(1) * right_edge_left_index * grid_info.resolution) + dock_edge_width / 2; + dock_edge_left << line_start(0) + + (direction_vector_right(0) * result_index * + grid_info.resolution) + + dock_edge_width / 2, + line_start(1) + + (direction_vector_right(1) * result_index * grid_info.resolution) + + dock_edge_width / 2; + dock_edge_right << line_start(0) + + (direction_vector_right(0) * right_edge_left_index * + grid_info.resolution) + + dock_edge_width / 2, + line_start(1) + + (direction_vector_right(1) * right_edge_left_index * + grid_info.resolution) + + dock_edge_width / 2; return std::make_pair(dock_edge_left, dock_edge_right); } From fffa9739ad50ecf198a6839a9eef67e987b67144 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 13:08:43 +0200 Subject: [PATCH 55/67] Update docstrings --- .../hybridpath_guidance/hybridpath.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py index a4a4db3c..1453314d 100644 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py @@ -86,6 +86,9 @@ def create_path(self, WP: list[Point]) -> None: self._calculate_subpaths() def _initialize_path(self): + """ + Initialize the path object. + """ self.path = Path() self.path.coeff.a = [] @@ -100,6 +103,12 @@ def _initialize_path(self): self.path.Order = self.order def update_waypoints(self, WP: list[Point]) -> None: + """ + Updates the waypoints for the path. + + Args: + WP (list[Point]): A list of waypoints. + """ # Convert the waypoints to a numpy array WP_arr = np.array([[wp.x, wp.y] for wp in WP]) From b423d55ebf1778abf78a92008b638169defebee1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 13:17:36 +0200 Subject: [PATCH 56/67] Cleanup --- .../hybridpath_guidance_node.py | 187 ++++++++---------- 1 file changed, 84 insertions(+), 103 deletions(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 65521a4d..49fc5e4a 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -5,7 +5,7 @@ from geometry_msgs.msg import Pose2D, Point from std_msgs.msg import Float32, Float64MultiArray, String, Bool from vortex_msgs.msg import HybridpathReference -from vortex_msgs.srv import Waypoint, DesiredVelocity +from vortex_msgs.srv import Waypoint, DesiredVelocity, DesiredHeading from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals @@ -29,15 +29,15 @@ def __init__(self): ('hybridpath_guidance.mu', 0.2) ]) + # Create services, subscribers and publishers self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) self.u_desired_server = self.create_service(DesiredVelocity, 'u_desired', self.u_desired_callback) self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) - self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1) - self.s_publisher = self.create_publisher(Float32, 's', 1) - self.w_publisher = self.create_publisher(Float32, 'w', 1) - self.wp_arr_publisher = self.create_publisher(Float64MultiArray, 'waypoints', 1) + self.heading_publisher = self.create_publisher(Float32, 'heading', 1) + self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) - self.yaw_server = self.create_service(DesiredVelocity,'yaw_reference', self.yaw_ref_callback) + self.heading_server = self.create_service(DesiredHeading,'heading_reference', self.heading_ref_callback) + self.operational_mode_subscriber = self.create_subscription(String, 'softWareOperationMode', self.operation_mode_callback, 10) self.killswitch_subscriber = self.create_subscription(Bool, 'softWareKillSwitch', self.killswitch_callback, 10) @@ -48,19 +48,20 @@ def __init__(self): self.path_generator_order = self.get_parameter('hybridpath_guidance.path_generator_order').get_parameter_value().integer_value self.dt = self.get_parameter('hybridpath_guidance.dt').get_parameter_value().double_value self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value + self.eta = np.zeros(3) self.u_desired = 1.1 # Desired velocity - self.yaw_ref = 0. # Desired heading, 100 if hybridpath heading, else can be set by service call + self.heading_ref = 0. # Desired heading, 100 if hybridpath heading, else can be set by service call + # Initialize variables self.waypoints = [] self.path = None self.s = 0. self.w = 0. self.v_s = 0. self.v_ss = 0. - self.mu = 0. self.operational_mode = 'autonomous mode' self.killswitch_active = False @@ -79,10 +80,7 @@ def __init__(self): self.initial_pos = False # Timer for guidance - timer_period = 0.1 - self.timer = self.create_timer(timer_period, self.guidance_callback) - - self.lock = threading.Lock() + self.timer = self.create_timer(self.dt, self.guidance_callback) def operation_mode_callback(self, msg: String): self.operational_mode = msg.data @@ -96,69 +94,62 @@ def u_desired_callback(self, request, response): response.success = True return response - def yaw_ref_callback(self, request, response): - self.yaw_ref = request.u_desired # xd + def heading_ref_callback(self, request, response): + self.heading_ref = request.heading - self.get_logger().info(f"Received desired heading: {self.yaw_ref}") + self.get_logger().info(f"Received desired heading: {self.heading_ref}") response.success = True return response def waypoint_callback(self, request, response): + if self.eta_received: + self.waypoints = [Point(x=self.eta[0], y=self.eta[1])] - with self.lock: - if self.eta_received: - self.waypoints = [Point(x=self.eta[0], y=self.eta[1])] - - self.get_logger().info('Received waypoints, generating path...') - - new_waypoints = request.waypoint + self.get_logger().info('Received waypoints, generating path...') - self.get_logger().info(f"Received {len(new_waypoints)} waypoints") + new_waypoints = request.waypoint - for i, point in enumerate(new_waypoints): - - wp = [point.x, point.y] + self.get_logger().info(f"Received {len(new_waypoints)} waypoints") - self.get_logger().info(f"Waypoint {i+1}: {wp}") - - self.waypoints.append(point) + for i, point in enumerate(new_waypoints): + + wp = [point.x, point.y] - self.last_waypoint = [self.waypoints[-1].x, self.waypoints[-1].y] + self.get_logger().info(f"Waypoint {i+1}: {wp}") + + self.waypoints.append(point) - self.generator.create_path(self.waypoints) - self.path = self.generator.get_path() + self.last_waypoint = [self.waypoints[-1].x, self.waypoints[-1].y] - self.waypoints_received = True - self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets - self.first_pos_flag = False + self.generator.create_path(self.waypoints) + self.path = self.generator.get_path() - self.s = 0. - self.signals.update_path(self.path) - self.w = self.signals.get_w(self.mu, self.eta) + self.waypoints_received = True + self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + self.first_pos_flag = False - wp_arr = Float64MultiArray() - wp_list = self.generator.WP.tolist() - wp_arr.data = [coordinate for wp in wp_list for coordinate in wp] - self.wp_arr_publisher.publish(wp_arr) + self.s = 0. + self.signals.update_path(self.path) + self.w = self.signals.get_w(self.mu, self.eta) response.success = True return response def eta_callback(self, msg: Odometry): - yaw_msg = Float32() + heading_msg = Float32() self.eta = self.odom_to_eta(msg) - self.yaw = float(self.eta[2]) + self.heading = float(self.eta[2]) if not self.initial_pos: self.eta_stationkeeping = self.eta - self.yaw_ref = self.yaw + self.heading_ref = self.heading self.initial_pos = True - yaw_msg.data = self.yaw - self.yaw_publisher.publish(yaw_msg) + heading_msg.data = self.heading + self.heading_publisher.publish(heading_msg) self.eta_received = True def set_stationkeeping_pose_callback(self, request, response): @@ -172,71 +163,63 @@ def set_stationkeeping_pose_callback(self, request, response): return response def guidance_callback(self): - with self.lock: - if self.killswitch_active or self.operational_mode != 'autonomous mode': - return - - if self.initial_pos: - if self.path is None and not self.waypoints_received: - if not self.stationkeeping_flag: - self.get_logger().info(f'No waypoints received, stationkeeping at {np.round(self.eta_stationkeeping, 3)}') - self.stationkeeping_flag = True - - pos = [self.eta_stationkeeping[0], self.eta_stationkeeping[1]] - pos_der = [0., 0.] - pos_dder = [0., 0.] + if self.killswitch_active or self.operational_mode != 'autonomous mode': + return + + if self.initial_pos: + if self.path is None and not self.waypoints_received: + if not self.stationkeeping_flag: + self.get_logger().info(f'No waypoints received, stationkeeping at {np.round(self.eta_stationkeeping, 3)}') + self.stationkeeping_flag = True - else: - self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) - self.signals.update_s(self.s) - self.w = self.signals.get_w(self.mu, self.eta) - self.v_s = self.signals.get_vs(self.u_desired) - self.v_ss = self.signals.get_vs_derivative(self.u_desired) + pos = [self.eta_stationkeeping[0], self.eta_stationkeeping[1]] + pos_der = [0., 0.] + pos_dder = [0., 0.] - pos = self.signals.get_position() + else: + self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) + self.signals.update_s(self.s) + self.w = self.signals.get_w(self.mu, self.eta) + self.v_s = self.signals.get_vs(self.u_desired) + self.v_ss = self.signals.get_vs_derivative(self.u_desired) - pos_der = self.signals.get_derivatives()[0] - pos_dder = self.signals.get_derivatives()[1] + pos = self.signals.get_position() - if self.yaw_ref == 100.: - psi = self.signals.get_heading() + pos_der = self.signals.get_derivatives()[0] + pos_dder = self.signals.get_derivatives()[1] - else: - psi = self.yaw_ref + if self.heading_ref == 100.: + psi = self.signals.get_heading() - psi_der = 0.#signals.get_heading_derivative() - psi_dder = 0.#signals.get_heading_second_derivative() + else: + psi = self.heading_ref - hp_msg = HybridpathReference() - hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) - hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) - hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) + psi_der = 0.#signals.get_heading_derivative() + psi_dder = 0.#signals.get_heading_second_derivative() - hp_msg.w = self.w - hp_msg.v_s = self.v_s - hp_msg.v_ss = self.v_ss + hp_msg = HybridpathReference() + hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) + hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) + hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) - self.guidance_publisher.publish(hp_msg) + hp_msg.w = self.w + hp_msg.v_s = self.v_s + hp_msg.v_ss = self.v_ss - if self.path is not None and self.s >= self.path.NumSubpaths: - self.waypoints_received = False - self.waiting_message_printed = False - self.stationkeeping_flag = False - self.path = None - self.eta_stationkeeping = np.array([self.last_waypoint[0], self.last_waypoint[1], self.yaw_ref]) + self.guidance_publisher.publish(hp_msg) - else: - if not self.waiting_message_printed: - self.get_logger().info('Waiting for eta') - self.waiting_message_printed = True + if self.path is not None and self.s >= self.path.NumSubpaths: + self.waypoints_received = False + self.waiting_message_printed = False + self.stationkeeping_flag = False + self.path = None + self.eta_stationkeeping = np.array([self.last_waypoint[0], self.last_waypoint[1], self.heading_ref]) - s_msg = Float32() - s_msg.data = self.s - self.s_publisher.publish(s_msg) + else: + if not self.waiting_message_printed: + self.get_logger().info('Waiting for eta') + self.waiting_message_printed = True - w_msg = Float32() - w_msg.data = self.w - self.w_publisher.publish(w_msg) @staticmethod def odom_to_eta(msg: Odometry) -> np.ndarray: @@ -257,11 +240,9 @@ def odom_to_eta(msg: Odometry) -> np.ndarray: ] # Convert quaternion to Euler angles - yaw = quat2euler(orientation_list)[2] - - # yaw = np.deg2rad(yaw) + heading = quat2euler(orientation_list)[2] - state = np.array([x, y, yaw]) + state = np.array([x, y, heading]) return state From 535df5ba6ac2840c612100f95ed0bb23f08f7759 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 13:22:49 +0200 Subject: [PATCH 57/67] Remove unused packages and files --- guidance/dp_guidance/CMakeLists.txt | 27 - guidance/dp_guidance/README.md | 15 - .../config/dp_guidance_config.yaml | 4 - guidance/dp_guidance/dp_guidance/__init__.py | 0 .../dp_guidance/dp_guidance/conversions.py | 59 -- .../dp_guidance/dp_guidance_node.py | 106 -- .../dp_guidance/reference_filter.py | 51 - .../dp_guidance/launch/dp_guidance.launch.py | 17 - guidance/dp_guidance/package.xml | 21 - .../noisy_odom_publisher.py | 57 -- .../hybridpath_guidance/u_desired_node.py | 48 - .../hybridpath_guidance/waypoint_node.py | 98 -- .../collision_avoidance_task/CMakeLists.txt | 77 -- .../collision_avoidance_task_ros.hpp | 79 -- .../launch/collision_avoidance_task_launch.py | 17 - .../collision_avoidance_task/package.xml | 30 - .../collision_avoidance_task_params.yaml | 23 - .../src/collision_avoidance_task_node.cpp | 15 - .../src/collision_avoidance_task_ros.cpp | 729 -------------- .../njord_tasks/docking_task/CMakeLists.txt | 77 -- .../docking_task/docking_task_1.png | Bin 199410 -> 0 bytes .../docking_task/docking_task_2.png | Bin 145381 -> 0 bytes .../include/docking_task/docking_task_ros.hpp | 88 -- .../launch/docking_task_launch.py | 17 - mission/njord_tasks/docking_task/package.xml | 30 - .../params/docking_task_params.yaml | 46 - .../docking_task/src/docking_task_node.cpp | 14 - .../docking_task/src/docking_task_ros.cpp | 637 ------------ .../maneuvering_task/CMakeLists.txt | 77 -- .../maneuvering_task/maneuvering_task_ros.hpp | 40 - .../launch/maneuvering_task_launch.py | 17 - .../maneuvering_task/maneuvering_task.pdf | Bin 120289 -> 0 bytes .../njord_tasks/maneuvering_task/package.xml | 30 - .../params/maneuvering_task_params.yaml | 25 - .../src/maneuvering_task_node.cpp | 14 - .../src/maneuvering_task_ros.cpp | 468 --------- .../navigation_task/CMakeLists.txt | 76 -- .../navigation_task/navigation_task_ros.hpp | 143 --- .../launch/navigation_task_launch.py | 17 - .../navigation_task/navigation_task.pdf | Bin 99398 -> 0 bytes .../njord_tasks/navigation_task/package.xml | 30 - .../params/navigation_task_params.yaml | 22 - .../src/navigation_task_node.cpp | 14 - .../src/navigation_task_ros.cpp | 924 ------------------ .../njord_task_base/CMakeLists.txt | 85 -- .../njord_task_base/njord_task_base_ros.hpp | 162 --- .../njord_tasks/njord_task_base/package.xml | 28 - .../src/njord_task_base_ros.cpp | 532 ---------- motion/lqr_controller/CMakeLists.txt | 27 - motion/lqr_controller/config/lqr_config.yaml | 5 - .../launch/lqr_controller.launch.py | 19 - .../lqr_controller/lqr_controller/__init__.py | 0 .../lqr_controller/conversions.py | 59 -- .../lqr_controller/lqr_controller.py | 42 - .../lqr_controller/lqr_controller_node.py | 103 -- motion/lqr_controller/package.xml | 27 - motion/pid_controller/CMakeLists.txt | 27 - motion/pid_controller/config/pid_config.yaml | 6 - .../launch/pid_controller.launch.py | 19 - motion/pid_controller/package.xml | 27 - .../pid_controller/pid_controller/__init__.py | 0 .../pid_controller/conversions.py | 59 -- .../pid_controller/pid_controller.py | 62 -- .../pid_controller/pid_controller_node.py | 109 --- 64 files changed, 5677 deletions(-) delete mode 100644 guidance/dp_guidance/CMakeLists.txt delete mode 100755 guidance/dp_guidance/README.md delete mode 100644 guidance/dp_guidance/config/dp_guidance_config.yaml delete mode 100644 guidance/dp_guidance/dp_guidance/__init__.py delete mode 100644 guidance/dp_guidance/dp_guidance/conversions.py delete mode 100755 guidance/dp_guidance/dp_guidance/dp_guidance_node.py delete mode 100644 guidance/dp_guidance/dp_guidance/reference_filter.py delete mode 100644 guidance/dp_guidance/launch/dp_guidance.launch.py delete mode 100644 guidance/dp_guidance/package.xml delete mode 100755 guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py delete mode 100644 guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py delete mode 100755 guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py delete mode 100644 mission/njord_tasks/collision_avoidance_task/CMakeLists.txt delete mode 100644 mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp delete mode 100644 mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py delete mode 100644 mission/njord_tasks/collision_avoidance_task/package.xml delete mode 100644 mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml delete mode 100644 mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp delete mode 100644 mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp delete mode 100644 mission/njord_tasks/docking_task/CMakeLists.txt delete mode 100644 mission/njord_tasks/docking_task/docking_task_1.png delete mode 100644 mission/njord_tasks/docking_task/docking_task_2.png delete mode 100644 mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp delete mode 100644 mission/njord_tasks/docking_task/launch/docking_task_launch.py delete mode 100644 mission/njord_tasks/docking_task/package.xml delete mode 100644 mission/njord_tasks/docking_task/params/docking_task_params.yaml delete mode 100644 mission/njord_tasks/docking_task/src/docking_task_node.cpp delete mode 100644 mission/njord_tasks/docking_task/src/docking_task_ros.cpp delete mode 100644 mission/njord_tasks/maneuvering_task/CMakeLists.txt delete mode 100644 mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp delete mode 100644 mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py delete mode 100644 mission/njord_tasks/maneuvering_task/maneuvering_task.pdf delete mode 100644 mission/njord_tasks/maneuvering_task/package.xml delete mode 100644 mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml delete mode 100644 mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp delete mode 100644 mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp delete mode 100644 mission/njord_tasks/navigation_task/CMakeLists.txt delete mode 100644 mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp delete mode 100644 mission/njord_tasks/navigation_task/launch/navigation_task_launch.py delete mode 100644 mission/njord_tasks/navigation_task/navigation_task.pdf delete mode 100644 mission/njord_tasks/navigation_task/package.xml delete mode 100644 mission/njord_tasks/navigation_task/params/navigation_task_params.yaml delete mode 100644 mission/njord_tasks/navigation_task/src/navigation_task_node.cpp delete mode 100644 mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp delete mode 100644 mission/njord_tasks/njord_task_base/CMakeLists.txt delete mode 100644 mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp delete mode 100644 mission/njord_tasks/njord_task_base/package.xml delete mode 100644 mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp delete mode 100644 motion/lqr_controller/CMakeLists.txt delete mode 100644 motion/lqr_controller/config/lqr_config.yaml delete mode 100644 motion/lqr_controller/launch/lqr_controller.launch.py delete mode 100644 motion/lqr_controller/lqr_controller/__init__.py delete mode 100644 motion/lqr_controller/lqr_controller/conversions.py delete mode 100644 motion/lqr_controller/lqr_controller/lqr_controller.py delete mode 100755 motion/lqr_controller/lqr_controller/lqr_controller_node.py delete mode 100644 motion/lqr_controller/package.xml delete mode 100644 motion/pid_controller/CMakeLists.txt delete mode 100644 motion/pid_controller/config/pid_config.yaml delete mode 100755 motion/pid_controller/launch/pid_controller.launch.py delete mode 100644 motion/pid_controller/package.xml delete mode 100644 motion/pid_controller/pid_controller/__init__.py delete mode 100644 motion/pid_controller/pid_controller/conversions.py delete mode 100644 motion/pid_controller/pid_controller/pid_controller.py delete mode 100755 motion/pid_controller/pid_controller/pid_controller_node.py diff --git a/guidance/dp_guidance/CMakeLists.txt b/guidance/dp_guidance/CMakeLists.txt deleted file mode 100644 index 7cc58d78..00000000 --- a/guidance/dp_guidance/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(dp_guidance) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - dp_guidance/dp_guidance_node.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() \ No newline at end of file diff --git a/guidance/dp_guidance/README.md b/guidance/dp_guidance/README.md deleted file mode 100755 index 3dbdad76..00000000 --- a/guidance/dp_guidance/README.md +++ /dev/null @@ -1,15 +0,0 @@ -# Dynamic Positioning (DP) Guidance - -This package provides the implementation of DP guidance for the Vortex ASV. - -## Usage - -To use the DP guidance launch it using: `ros2 launch dp_guidance dp_guidance.launch` - -To run with custom waypoints (replace example waypoints with actual waypoints, and add as many prefered): - -`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 0.0, y: 0.0, z: 0.0}, {x: 5.0, y: 5.0, z: 0.0}]"` - -## Configuration - -You can configure the behavior of the hybrid path guidance by modifying the parameters in the `config` directory. \ No newline at end of file diff --git a/guidance/dp_guidance/config/dp_guidance_config.yaml b/guidance/dp_guidance/config/dp_guidance_config.yaml deleted file mode 100644 index 02db4e76..00000000 --- a/guidance/dp_guidance/config/dp_guidance_config.yaml +++ /dev/null @@ -1,4 +0,0 @@ -/**: - ros__parameters: - dp_guidance: - dt: 0.1 # Time step \ No newline at end of file diff --git a/guidance/dp_guidance/dp_guidance/__init__.py b/guidance/dp_guidance/dp_guidance/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/guidance/dp_guidance/dp_guidance/conversions.py b/guidance/dp_guidance/dp_guidance/conversions.py deleted file mode 100644 index 92f14cfb..00000000 --- a/guidance/dp_guidance/dp_guidance/conversions.py +++ /dev/null @@ -1,59 +0,0 @@ -import numpy as np - -from transforms3d.euler import euler2quat, quat2euler -from nav_msgs.msg import Odometry -from geometry_msgs.msg import PoseStamped - - -def odometrymsg_to_state(msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation - orientation_list = [ - orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z - ] - - # Convert quaternion to Euler angles, extract yaw - yaw = quat2euler(orientation_list)[2] - - vx = msg.twist.twist.linear.x - vy = msg.twist.twist.linear.y - vyaw = msg.twist.twist.angular.z - - state = np.array([x, y, yaw, vx, vy, vyaw]) - return state - -def state_to_odometrymsg(state): - orientation_list_next = euler2quat(0, 0, state[2]) - - odometry_msg = Odometry() - odometry_msg.pose.pose.position.x = state[0] - odometry_msg.pose.pose.position.y = state[1] - odometry_msg.pose.pose.position.z = 0.0 - odometry_msg.pose.pose.orientation.w = orientation_list_next[0] - odometry_msg.pose.pose.orientation.x = orientation_list_next[1] - odometry_msg.pose.pose.orientation.y = orientation_list_next[2] - odometry_msg.pose.pose.orientation.z = orientation_list_next[3] - - return odometry_msg - -def state_to_posestamped(state, frame_id, stamp): - orientation_list_next = euler2quat(0, 0, state[2]) - - posestamped_msg = PoseStamped() - - posestamped_msg.header.frame_id = frame_id - posestamped_msg.header.stamp = stamp - - posestamped_msg.pose.position.x = state[0] - posestamped_msg.pose.position.y = state[1] - posestamped_msg.pose.position.z = 0.0 - posestamped_msg.pose.orientation.w = orientation_list_next[0] - posestamped_msg.pose.orientation.x = orientation_list_next[1] - posestamped_msg.pose.orientation.y = orientation_list_next[2] - posestamped_msg.pose.orientation.z = orientation_list_next[3] - - return posestamped_msg - -def ssa(angle): - return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py deleted file mode 100755 index 1ddbf8cf..00000000 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ /dev/null @@ -1,106 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import Pose2D -from vortex_msgs.srv import Waypoint -from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler -from std_msgs.msg import Float32 -from conversions import odometrymsg_to_state, state_to_odometrymsg -from reference_filter import ReferenceFilter -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class Guidance(Node): - def __init__(self): - super().__init__("dp_guidance") - self.declare_parameters( - namespace='', - parameters=[ - ('dp_guidance.dt', 0.1) - ]) - - self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) - self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) - self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1) - self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1) - self.yaw_ref_publisher = self.create_publisher(Float32, 'yaw_ref', 1) - - # Get parameters - self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value - - # Flags for logging - self.waypoints_received = False - self.waiting_message_printed = False - - self.init_pos = False - self.eta_received = False - self.eta_logger = True - - self.yaw_ref = 0. - - self.xd = np.zeros(9) - - self.reference_filter = ReferenceFilter() - - # Timer for guidance - timer_period = 0.1 - self.timer = self.create_timer(timer_period, self.guidance_callback) - - def waypoint_callback(self, request, response): - self.init_pos = False - self.waypoints = request.waypoint - self.get_logger().info(f'Received waypoints: {self.waypoints}') - self.xd = np.zeros(9) - self.waypoints_received = True - self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets - response.success = True - return response - - def eta_callback(self, msg: Odometry): - self.eta = odometrymsg_to_state(msg)[:3] - self.eta_received = True - self.yaw_publisher.publish(Float32(data=self.eta[2])) - self.yaw_ref_publisher.publish(Float32(data=self.yaw_ref)) - - def guidance_callback(self): - if self.waypoints_received and self.eta_received: - if not self.init_pos: - self.xd[0:3] = self.eta - self.get_logger().info(f"Reference initialized at {self.xd[0:3]}") - self.init_pos = True - last_waypoint = self.waypoints[-1] - self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.yaw_ref]) - x_next = self.reference_filter.step(self.eta_ref, self.xd) - self.xd = x_next - # self.get_logger().info(f'x_next[0]: {x_next[0]}') - # self.get_logger().info(f'x_next[0]: {x_next[1]}') - # self.get_logger().info(f'x_next[0]: {x_next[2]}') - - odom_msg = Odometry() - odom_msg = state_to_odometrymsg(x_next[:3]) - # odom_msg = state_to_odometrymsg(self.eta_ref) - self.guidance_publisher.publish(odom_msg) - - else: - if not self.eta_received and self.eta_logger: - self.get_logger().info("Waiting for eta") - self.eta_logger = False - - if not self.waiting_message_printed: - self.get_logger().info('Waiting for waypoints to be received') - self.waiting_message_printed = True - -def main(args=None): - rclpy.init(args=args) - guidance = Guidance() - rclpy.spin(guidance) - guidance.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/guidance/dp_guidance/dp_guidance/reference_filter.py b/guidance/dp_guidance/dp_guidance/reference_filter.py deleted file mode 100644 index 45b243f3..00000000 --- a/guidance/dp_guidance/dp_guidance/reference_filter.py +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import control - -class ReferenceFilter: - def __init__(self): - zeta = 0.8 - omega_b = 0.05 - omega_n = omega_b/np.sqrt(1-2*zeta**2 + np.sqrt(4*zeta**4 - 4*zeta**2 + 2)) - - I = np.eye(3) - Omega = 2*zeta*omega_n*I - # Gamma = omega_n**2*I - Delta = zeta*I - Ad = np.zeros((9,9)) - Ad[0:3,3:6] = I - Ad[3:6,6:9] = I - Ad[6:9,0:3] = -Omega**3 - Ad[6:9,3:6] = -(2*Delta+I)@Omega**2 - Ad[6:9,6:9] = -(2*Delta+I)@Omega - - Bd = np.zeros((9,3)) - Bd[6:9,:] = Omega**3 - - sys = control.ss(Ad, Bd, np.zeros((9,9)), np.zeros((9,3))) - sysd = control.c2d(sys, 0.1) - - self.Ad = sysd.A - self.Bd = sysd.B - - def step(self, r, xd): - x_next = self.Ad@xd + self.Bd@r - return x_next - - def get_eta(self, xd): - return xd[:,0:3] - - def get_nu(self, xd): - nu = np.zeros((len(xd),3)) - for i in range(len(xd)): - psi = xd[i,2] - nu[i,:] = (self.rotationMatrix(psi).transpose())@xd[i,3:6] - return nu - - @staticmethod - def rotationMatrix(psi): - R = np.array([[np.cos(psi), -np.sin(psi), 0], - [np.sin(psi), np.cos(psi), 0], - [0, 0, 1]]) - return R diff --git a/guidance/dp_guidance/launch/dp_guidance.launch.py b/guidance/dp_guidance/launch/dp_guidance.launch.py deleted file mode 100644 index 1f9ad872..00000000 --- a/guidance/dp_guidance/launch/dp_guidance.launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - dp_guidance_node = Node( - package='dp_guidance', - executable='dp_guidance_node.py', - name='dp_guidance_node', - parameters=[os.path.join(get_package_share_directory('dp_guidance'),'config','dp_guidance_config.yaml')], - output='screen', - ) - return LaunchDescription([ - dp_guidance_node - ]) - diff --git a/guidance/dp_guidance/package.xml b/guidance/dp_guidance/package.xml deleted file mode 100644 index 33547bcb..00000000 --- a/guidance/dp_guidance/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - dp_guidance - 0.0.0 - This package provides the implementation of hybrid path guidance for the Vortex ASV. - vortex - MIT - - rclpy - nav_msgs - python-transforms3d-pip - geometry_msgs - vortex_msgs - - python3-pytest - - - ament_cmake - - diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py b/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py deleted file mode 100755 index 5d4395d5..00000000 --- a/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py +++ /dev/null @@ -1,57 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from vortex_msgs.srv import Waypoint -from geometry_msgs.msg import Point -from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler -import numpy as np -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class OdomPublisher(Node): - def __init__(self): - super().__init__('odom_publisher') - - self.eta_odom_publisher = self.create_publisher(Odometry, '/seapath/odom/ned', qos_profile) - - timer_period = 0.01 - self.timer = self.create_timer(timer_period, self.odom_callback) - - self.get_logger().info("Odom publisher started") - - def odom_callback(self): - - noise = np.random.normal(0, 0.005, 3) - - msg = Odometry() - msg.pose.pose.position.x = 0.0 + noise[0] - msg.pose.pose.position.y = 0.0 + noise[1] - msg.pose.pose.position.z = 0.0 - msg.pose.pose.orientation.x = 0.0 - msg.pose.pose.orientation.y = 0.0 - msg.pose.pose.orientation.z = 0.0 - msg.pose.pose.orientation.w = 1.0 - msg.twist.twist.linear.x = 0.0 + noise[0] - msg.twist.twist.linear.y = 0.0 + noise[1] - msg.twist.twist.linear.z = 0.0 - msg.twist.twist.angular.x = 0.0 - msg.twist.twist.angular.y = 0.0 - msg.twist.twist.angular.z = 0.0 + noise[2] - - self.eta_odom_publisher.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - node = OdomPublisher() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py deleted file mode 100644 index 033afd22..00000000 --- a/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py +++ /dev/null @@ -1,48 +0,0 @@ -import rclpy -from rclpy.node import Node -from vortex_msgs.srv import DesiredVelocity - -class DesiredVelocityClient(Node): - def __init__(self): - super().__init__('desired_velocity_client') - self.client = self.create_client(DesiredVelocity, 'u_desired') - - while not self.client.wait_for_service(timeout_sec=2.0): - self.get_logger().info('service not available, waiting again...') - - self.send_request() - - def send_request(self): - req = DesiredVelocity.Request() - req.u = 0.3 - self.get_logger().info(f'Sending request: {req}') - self.future = self.client.call_async(req) - self.future.add_done_callback(self.get_response) - - def get_response(self, future): - try: - response = future.result() - self.get_logger().info(f'Received response: {response}') - if response.success: - self.destroy_node() - rclpy.shutdown() - - except Exception as e: - self.get_logger().error('Service call failed %r' % (e,)) - -def main(args=None): - rclpy.init(args=args) - - client = DesiredVelocityClient() - - try: - rclpy.spin(client) - except Exception as e: - client.get_logger().error('Error in DesiredVelocityClient: %r' % (e,)) - finally: - if rclpy.ok(): - client.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py deleted file mode 100755 index 5be5c27e..00000000 --- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py +++ /dev/null @@ -1,98 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from vortex_msgs.srv import Waypoint -from geometry_msgs.msg import Point -from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler -import numpy as np -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class WaypointClient(Node): - def __init__(self): - super().__init__('waypoint_client') - self.eta_odom = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) - - self.eta_received = False - - while not self.eta_received: - self.get_logger().info('Waiting for eta...') - rclpy.spin_once(self) - - self.client = self.create_client(Waypoint, 'waypoint_list') - - while not self.client.wait_for_service(timeout_sec=2.0): - self.get_logger().info('service not available, waiting again...') - - self.send_request() - - def eta_callback(self, msg: Odometry): - self.eta = self.odom_to_eta(msg) - #self.get_logger().info(f'Received eta {self.eta}') - self.eta_received = True - - def send_request(self): - if self.eta_received: - req = Waypoint.Request() - # wp_list = [[self.eta[0] + 5., self.eta[1]], [self.eta[0] + 5., self.eta[1] + 5.], [self.eta[0], self.eta[1] + 5.]] - # wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 0.]] - wp_list = [[self.eta[0], self.eta[1] + 40.]] - req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list] - self.get_logger().info(f'Sending request: {req}') - self.future = self.client.call_async(req) - self.future.add_done_callback(self.get_response) - - def get_response(self, future): - try: - response = future.result() - self.get_logger().info(f'Received response: {response}') - if response.success: - self.destroy_node() - rclpy.shutdown() - - except Exception as e: - self.get_logger().error('Service call failed %r' % (e,)) - - @staticmethod - def odom_to_eta(msg: Odometry) -> np.ndarray: - """ - Converts an Odometry message to 3DOF eta vector. - - Args: - msg (Odometry): The Odometry message to convert. - - Returns: - np.ndarray: The eta vector. - """ - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation - orientation_list = [ - orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z - ] - - # Convert quaternion to Euler angles - yaw = quat2euler(orientation_list)[2] - - state = np.array([x, y, yaw]) - return state - -def main(args=None): - rclpy.init(args=args) - waypoint_client_node = WaypointClient() - try: - rclpy.spin(waypoint_client_node) - except Exception as e: - waypoint_client_node.get_logger().error(f'Unhandled exception: {e}') - finally: - if rclpy.ok(): - waypoint_client_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt b/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt deleted file mode 100644 index aed2ab9e..00000000 --- a/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(collision_avoidance_task) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(njord_task_base REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) - -include_directories(include) - -add_executable(collision_avoidance_task_node - src/collision_avoidance_task_node.cpp - src/collision_avoidance_task_ros.cpp) - -target_link_libraries(collision_avoidance_task_node - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - pcl_common -) - -ament_target_dependencies(collision_avoidance_task_node - rclcpp - njord_task_base - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions -) - -install( - DIRECTORY include/ - DESTINATION include - -) - -install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME}/ - ) - -install(DIRECTORY - launch - params - DESTINATION share/${PROJECT_NAME}/ -) - - -ament_package() diff --git a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp deleted file mode 100644 index c6309cab..00000000 --- a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp +++ /dev/null @@ -1,79 +0,0 @@ -#ifndef COLLISION_AVOIDANCE_TASK_ROS_HPP -#define COLLISION_AVOIDANCE_TASK_ROS_HPP - -#include -#include -#include -#include -#include -#include -#include -#include // Required for tf2::doTransform - -namespace collision_avoidance_task { - -struct LandmarkOdomID { - nav_msgs::msg::Odometry odom; - int32_t id; -}; -class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { -public: - explicit CollisionAvoidanceTaskNode( - const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - ~CollisionAvoidanceTaskNode(){}; - - /** - * @brief Main task for the CollisionAvoidanceTaskNode class. - */ - void main_task(); - - /** - * @brief Predict the positions of the first two buoys - * - * @return Eigen::Array predicted_positions - */ - Eigen::Array predict_first_buoy_pair(); - - Eigen::Array - predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - void track_vessel(const Eigen::Vector2d &direction_vector_downwards, - const Eigen::Vector2d &direction_vector_forwards, - const geometry_msgs::msg::Point &waypoint_second_buoy_pair); - - nav_msgs::msg::Odometry get_vessel_odom(); - - LandmarkOdomID - filter_landmarks(const vortex_msgs::msg::LandmarkArray &landmarks, - const Eigen::Vector2d &direction_vector_downwards, - const Eigen::Vector2d &direction_vector_forwards, - const geometry_msgs::msg::Point &waypoint_second_buoy_pair); - - void vessel_collision_heading(); - - double calculate_angle(const geometry_msgs::msg::Vector3 &twist1, - const geometry_msgs::msg::Vector3 &twist2); - - bool - has_vessel_passed_freya(const Eigen::Vector2d &direction_vector_downwards); - - Eigen::Array - predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - Eigen::Array - predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green); - -private: - mutable std::mutex vessel_odom_mutex_; - bool new_vessel_odom_msg_ = false; - nav_msgs::msg::Odometry vessel_odom_; - std::condition_variable vessel_odom_cv_; -}; - -} // namespace collision_avoidance_task - -#endif // COLLISION_AVOIDANCE_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py b/mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py deleted file mode 100644 index 40e7e2fa..00000000 --- a/mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - collision_avoidance_task_node = Node( - package='collision_avoidance_task', - executable='collision_avoidance_task_node', - name='collision_avoidance_task_node', - parameters=[os.path.join(get_package_share_directory('collision_avoidance_task'),'params','collision_avoidance_task_params.yaml')], - # arguments=['--ros-args', '--log-level', 'DEBUG'], - output='screen', - ) - return LaunchDescription([ - collision_avoidance_task_node - ]) \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/package.xml b/mission/njord_tasks/collision_avoidance_task/package.xml deleted file mode 100644 index 9e2ea608..00000000 --- a/mission/njord_tasks/collision_avoidance_task/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - collision_avoidance_task - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - njord_task_base - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions - - - - ament_cmake - - diff --git a/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml b/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml deleted file mode 100644 index db084e8f..00000000 --- a/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml +++ /dev/null @@ -1,23 +0,0 @@ -collision_avoidance_task_node: - ros__parameters: - map_origin_lat: -33.72213988382845 - map_origin_lon: 150.67413500672993 - map_origin_set: true - gps_start_lat: 63.44097054434422 - gps_start_lon: 10.419997767413607 - gps_end_lat: 63.44125901804796 - gps_end_lon: 10.41857835889424 - gps_start_x: 0.0 - gps_start_y: 0.0 - gps_end_x: 0.05243377001522731 - gps_end_y: 37.196639612524166 - gps_frame_coords_set: true - map_origin_topic: "/map/origin" - odom_topic: "/seapath/odom/ned" - landmark_topic: "landmarks_out" - assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct - - # Task specific parameters - distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame. - distance_between_buoy_pairs: 5.0 # Distance between buoy pairs in meters - vessel_assignment_confidence: 5 # Number of consequtive identical assignments from landmark to vessel before we consider the assignment as correct \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp deleted file mode 100644 index 9cf7ead4..00000000 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include -#include - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - - auto node = - std::make_shared(); - - rclcpp::spin(node); - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp deleted file mode 100644 index 9c79855c..00000000 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ /dev/null @@ -1,729 +0,0 @@ -#include - -namespace collision_avoidance_task { - -CollisionAvoidanceTaskNode::CollisionAvoidanceTaskNode( - const rclcpp::NodeOptions &options) - : NjordTaskBaseNode("collision_avoidance_task_node", options) { - - declare_parameter("distance_to_first_buoy_pair", 2.0); - declare_parameter("distance_between_buoy_pairs", 5.0); - declare_parameter("vessel_assignment_confidence", 5); - - std::thread(&CollisionAvoidanceTaskNode::main_task, this).detach(); -} - -void CollisionAvoidanceTaskNode::main_task() { - navigation_ready(); - RCLCPP_INFO(this->get_logger(), "Collision avoidance task started"); - odom_start_point_ = get_odom()->pose.pose.position; - Eigen::Array predicted_first_buoy_pair = - predict_first_buoy_pair(); - - sensor_msgs::msg::PointCloud2 buoy_vis_msg; - pcl::PointCloud buoy_vis; - pcl::PointXYZRGB buoy_red_0; - buoy_red_0.x = predicted_first_buoy_pair(0, 0); - buoy_red_0.y = predicted_first_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - pcl::PointXYZRGB buoy_green_1; - buoy_green_1.x = predicted_first_buoy_pair(0, 1); - buoy_green_1.y = predicted_first_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - // First first buoy pair is far away, should be closer before trying to - // measure it. - double gps_end_x = this->get_parameter("gps_end_x").as_double(); - double gps_end_y = this->get_parameter("gps_end_y").as_double(); - Eigen::Vector2d direction_vector_to_end; - direction_vector_to_end << gps_end_x - odom_start_point_.x, - gps_end_y - odom_start_point_.y; - direction_vector_to_end.normalize(); - - double distance = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - if (distance > 5.0) { - auto odom = get_odom(); - geometry_msgs::msg::Point waypoint_toward_start; - waypoint_toward_start.x = odom->pose.pose.position.x + - direction_vector_to_end(0) * (distance - 3); - waypoint_toward_start.y = odom->pose.pose.position.y + - direction_vector_to_end(1) * (distance - 3); - waypoint_toward_start.z = 0.0; - send_waypoint(waypoint_toward_start); - set_desired_heading(odom_start_point_, waypoint_toward_start); - reach_waypoint(3.0); - } - - std::vector measured_first_buoy_pair = - get_buoy_landmarks(predicted_first_buoy_pair); - if (measured_first_buoy_pair.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_first_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_first_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_first_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_first_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_first_pair; - waypoint_first_pair.x = - (measured_first_buoy_pair[0].pose_odom_frame.position.x + - measured_first_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_first_pair.y = - (measured_first_buoy_pair[0].pose_odom_frame.position.y + - measured_first_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_first_pair.z = 0.0; - send_waypoint(waypoint_first_pair); - set_desired_heading(odom_start_point_, waypoint_first_pair); - reach_waypoint(3.0); - - // Second buoy pair - Eigen::Array predicted_second_buoy_pair = - predict_second_buoy_pair( - measured_first_buoy_pair[0].pose_odom_frame.position, - measured_first_buoy_pair[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_second_buoy_pair(0, 0); - buoy_red_0.y = predicted_second_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_second_buoy_pair(0, 1); - buoy_green_1.y = predicted_second_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_second_buoy_pair = - get_buoy_landmarks(predicted_second_buoy_pair); - if (measured_second_buoy_pair.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_second_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_second_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_second_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_second_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_second_pair; - waypoint_second_pair.x = - (measured_second_buoy_pair[0].pose_odom_frame.position.x + - measured_second_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_second_pair.y = - (measured_second_buoy_pair[0].pose_odom_frame.position.y + - measured_second_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_second_pair.z = 0.0; - send_waypoint(waypoint_second_pair); - set_desired_heading(odom_start_point_, waypoint_second_pair); - reach_waypoint(3.0); - - Eigen::Vector2d direction_vector_forwards; - Eigen::Vector2d direction_vector_downwards; - direction_vector_forwards << waypoint_second_pair.x - waypoint_first_pair.x, - waypoint_second_pair.y - waypoint_first_pair.y; - direction_vector_forwards.normalize(); - direction_vector_downwards - << (measured_first_buoy_pair[1].pose_odom_frame.position.x + - measured_second_buoy_pair[1].pose_odom_frame.position.x) / - 2 - - (measured_first_buoy_pair[0].pose_odom_frame.position.x + - measured_second_buoy_pair[0].pose_odom_frame.position.x) / - 2, - (measured_first_buoy_pair[1].pose_odom_frame.position.y + - measured_second_buoy_pair[1].pose_odom_frame.position.y) / - 2 - - (measured_first_buoy_pair[0].pose_odom_frame.position.y + - measured_second_buoy_pair[0].pose_odom_frame.position.y) / - 2; - direction_vector_downwards.normalize(); - - geometry_msgs::msg::Point waypoint_midpoint; - waypoint_midpoint.x = - waypoint_second_pair.x + direction_vector_forwards(0) * 10; - waypoint_midpoint.y = - waypoint_second_pair.y + direction_vector_forwards(1) * 10; - waypoint_midpoint.z = 0.0; - send_waypoint(waypoint_midpoint); - set_desired_heading(waypoint_second_pair, waypoint_midpoint); - - std::thread(std::bind(&CollisionAvoidanceTaskNode::track_vessel, this, - direction_vector_forwards, direction_vector_downwards, - waypoint_second_pair)) - .detach(); - RCLCPP_INFO(this->get_logger(), "Tracking vessel"); - reach_waypoint(9.0); - // Run until the angle between the twist vectors are between 60 and 120 - // degrees - vessel_collision_heading(); - auto vessel_odom = get_vessel_odom(); - auto freya_odom = get_odom(); - - Eigen::Vector2d direction_vector_freya_to_vessel; - direction_vector_freya_to_vessel - << vessel_odom.pose.pose.position.x - freya_odom->pose.pose.position.x, - vessel_odom.pose.pose.position.y - freya_odom->pose.pose.position.y; - // Project the vector do find "height" difference of vessels - double downwards_diff = - direction_vector_freya_to_vessel.dot(direction_vector_downwards); - geometry_msgs::msg::Point first_avoidance_waypoint; - if (std::abs(downwards_diff) > 5) { - first_avoidance_waypoint.x = - vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3; - first_avoidance_waypoint.y = - vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3; - first_avoidance_waypoint.z = 0.0; - send_waypoint(first_avoidance_waypoint); - set_desired_heading(freya_odom->pose.pose.position, - first_avoidance_waypoint); - } else { - first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - - direction_vector_forwards(0) * 3 + - direction_vector_downwards(0) * 2; - first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - - direction_vector_forwards(1) * 3 + - direction_vector_downwards(1) * 2; - first_avoidance_waypoint.z = 0.0; - send_waypoint(first_avoidance_waypoint); - set_desired_heading(freya_odom->pose.pose.position, - first_avoidance_waypoint); - } - - while (!has_vessel_passed_freya(direction_vector_downwards)) { - RCLCPP_INFO(this->get_logger(), "Vessel has not passed Freya yet"); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - freya_odom = get_odom(); - geometry_msgs::msg::Point second_avoidance_waypoint; - second_avoidance_waypoint.x = - freya_odom->pose.pose.position.x + direction_vector_forwards(0) * 5; - second_avoidance_waypoint.y = - freya_odom->pose.pose.position.y + direction_vector_forwards(1) * 5; - second_avoidance_waypoint.z = 0.0; - - // reach_waypoint(1.0); - - geometry_msgs::msg::Point back_on_track_waypoint; - back_on_track_waypoint.x = waypoint_second_pair.x + - direction_vector_forwards(0) * 15 + - direction_vector_downwards(0) * 0; - back_on_track_waypoint.y = waypoint_second_pair.y + - direction_vector_forwards(1) * 15 + - direction_vector_downwards(1) * 0; - back_on_track_waypoint.z = 0.0; - - auto request = std::make_shared(); - request->waypoint.push_back(second_avoidance_waypoint); - request->waypoint.push_back(back_on_track_waypoint); - auto result_future = waypoint_client_->async_send_request(request); - - // Check if the service was successful - auto odom = get_odom(); - double dx = back_on_track_waypoint.x - odom->pose.pose.position.x; - double dy = back_on_track_waypoint.y - odom->pose.pose.position.y; - double desired_heading = std::atan2(dy, dx); - tf2::Quaternion q; - q.setRPY(0.0, 0.0, desired_heading); - - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = back_on_track_waypoint.x; - waypoint_vis.pose.position.y = back_on_track_waypoint.y; - waypoint_vis.pose.orientation = tf2::toMsg(q); - waypoint_visualization_pub_->publish(waypoint_vis); - auto status = result_future.wait_for(std::chrono::seconds(5)); - while (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); - status = result_future.wait_for(std::chrono::seconds(5)); - } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); - } - - previous_waypoint_odom_frame_ = back_on_track_waypoint; - - set_desired_heading(second_avoidance_waypoint, back_on_track_waypoint); - reach_waypoint(3.0); - - Eigen::Array predicted_third_buoy_pair = - predict_third_buoy_pair( - measured_first_buoy_pair[0].pose_odom_frame.position, - measured_first_buoy_pair[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_third_buoy_pair(0, 0); - buoy_red_0.y = predicted_third_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_third_buoy_pair(0, 1); - buoy_green_1.y = predicted_third_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_third_buoy_pair = - get_buoy_landmarks(predicted_third_buoy_pair); - if (measured_third_buoy_pair.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_third_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_third_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_third_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_third_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_third_pair; - waypoint_third_pair.x = - (measured_third_buoy_pair[0].pose_odom_frame.position.x + - measured_third_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_third_pair.y = - (measured_third_buoy_pair[0].pose_odom_frame.position.y + - measured_third_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_third_pair.z = 0.0; - send_waypoint(waypoint_third_pair); - set_desired_heading(back_on_track_waypoint, waypoint_third_pair); - reach_waypoint(3.0); - - Eigen::Array predicted_fourth_buoy_pair = - predict_fourth_buoy_pair( - measured_third_buoy_pair[0].pose_odom_frame.position, - measured_third_buoy_pair[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_fourth_buoy_pair(0, 0); - buoy_red_0.y = predicted_fourth_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_fourth_buoy_pair(0, 1); - buoy_green_1.y = predicted_fourth_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_fourth_buoy_pair = - get_buoy_landmarks(predicted_fourth_buoy_pair); - if (measured_fourth_buoy_pair.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_fourth_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_fourth_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_fourth_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_fourth_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_fourth_pair; - waypoint_fourth_pair.x = - (measured_fourth_buoy_pair[0].pose_odom_frame.position.x + - measured_fourth_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_fourth_pair.y = - (measured_fourth_buoy_pair[0].pose_odom_frame.position.y + - measured_fourth_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_fourth_pair.z = 0.0; - send_waypoint(waypoint_fourth_pair); - set_desired_heading(waypoint_third_pair, waypoint_fourth_pair); - reach_waypoint(3.0); - - geometry_msgs::msg::Point waypoint_end; - waypoint_end.x = this->get_parameter("gps_end_x").as_double(); - waypoint_end.y = this->get_parameter("gps_end_y").as_double(); - waypoint_end.z = 0.0; - send_waypoint(waypoint_end); - set_desired_heading(waypoint_fourth_pair, waypoint_end); -} - -Eigen::Array -CollisionAvoidanceTaskNode::predict_first_buoy_pair() { - // Predict the positions of the first two buoys - geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; - geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; - buoy_0_base_link_frame.header.frame_id = "base_link"; - buoy_1_base_link_frame.header.frame_id = "base_link"; - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - - buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_0_base_link_frame.pose.position.y = -2.5; - buoy_0_base_link_frame.pose.position.z = 0.0; - buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_1_base_link_frame.pose.position.y = 2.5; - buoy_1_base_link_frame.pose.position.z = 0.0; - - geometry_msgs::msg::PoseStamped buoy_0_odom_frame; - geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - - bool transform_success = false; - while (!transform_success) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; - predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; - predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; - predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; - - return predicted_positions; -} - -Eigen::Array CollisionAvoidanceTaskNode::predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, - previous_waypoint_odom_frame_.y - odom_start_point_.y; - direction_vector.normalize(); - - double distance_between_buoy_pairs = - this->get_parameter("distance_between_buoy_pairs").as_double(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = - buoy_0.x + direction_vector(0) * distance_between_buoy_pairs; - predicted_positions(1, 0) = - buoy_0.y + direction_vector(1) * distance_between_buoy_pairs; - predicted_positions(0, 1) = - buoy_1.x + direction_vector(0) * distance_between_buoy_pairs; - predicted_positions(1, 1) = - buoy_1.y + direction_vector(1) * distance_between_buoy_pairs; - - return predicted_positions; -} - -void CollisionAvoidanceTaskNode::track_vessel( - const Eigen::Vector2d &direction_vector_downwards, - const Eigen::Vector2d &direction_vector_forwards, - const geometry_msgs::msg::Point &waypoint_second_buoy_pair) { - std::shared_ptr landmark_msg; - int32_t prev_vessel_id = -1; - int vessel_assignment_confidence = - this->get_parameter("vessel_assignment_confidence").as_int(); - int current_confidence = 0; - while (true) { - landmark_msg = get_landmarks_odom_frame(); - LandmarkOdomID vessel = - filter_landmarks(*landmark_msg, direction_vector_downwards, - direction_vector_forwards, waypoint_second_buoy_pair); - - if (vessel.id == -1) { - current_confidence = 0; - continue; - } - if (current_confidence == 0) { - prev_vessel_id = vessel.id; - current_confidence++; - continue; - } - if (vessel.id != prev_vessel_id) { - current_confidence = 0; - continue; - } - if (vessel.id == prev_vessel_id) { - if (current_confidence < vessel_assignment_confidence) { - current_confidence++; - continue; - } - current_confidence++; - std::unique_lock lock(vessel_odom_mutex_); - vessel_odom_ = vessel.odom; - new_vessel_odom_msg_ = true; - lock.unlock(); - vessel_odom_cv_.notify_one(); - continue; - } - } -} - -LandmarkOdomID CollisionAvoidanceTaskNode::filter_landmarks( - const vortex_msgs::msg::LandmarkArray &landmarks, - const Eigen::Vector2d &direction_vector_downwards, - const Eigen::Vector2d &direction_vector_forwards, - const geometry_msgs::msg::Point &waypoint_second_buoy_pair) { - double max_velocity = std::numeric_limits::min(); - LandmarkOdomID filtered_landmark; - filtered_landmark.id = -1; - for (auto landmark : landmarks.landmarks) { - double velocity = std::hypot(landmark.odom.twist.twist.linear.x, - landmark.odom.twist.twist.linear.y); - geometry_msgs::msg::Point landmark_pose = landmark.odom.pose.pose.position; - Eigen::Vector2d vector_waypoint_to_landmark; - vector_waypoint_to_landmark - << landmark_pose.x - waypoint_second_buoy_pair.x, - landmark_pose.y - waypoint_second_buoy_pair.y; - double projection_down = - vector_waypoint_to_landmark.dot(direction_vector_downwards); - double projection_forwards = - vector_waypoint_to_landmark.dot(direction_vector_forwards); - - if (projection_down < -1 || projection_down > 20) { - continue; - } - if (projection_forwards < -1 || projection_forwards > 20) { - continue; - } - if (velocity > max_velocity && velocity > 0.1) { - max_velocity = velocity; - filtered_landmark.odom = landmark.odom; - filtered_landmark.id = landmark.id; - } - } - return filtered_landmark; -} - -nav_msgs::msg::Odometry CollisionAvoidanceTaskNode::get_vessel_odom() { - std::unique_lock lock(vessel_odom_mutex_); - vessel_odom_cv_.wait(lock, [this] { return new_vessel_odom_msg_; }); - new_vessel_odom_msg_ = false; - lock.unlock(); - return vessel_odom_; -} - -void CollisionAvoidanceTaskNode::vessel_collision_heading() { - RCLCPP_INFO(this->get_logger(), "Checking vessel collision heading"); - while (true) { - auto freya_odom = get_odom(); - bool transform_success = false; - geometry_msgs::msg::TransformStamped transform; - while (!transform_success) { - try { - transform = tf_buffer_->lookupTransform("odom", "base_link", - tf2::TimePointZero); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - geometry_msgs::msg::Vector3 freya_odom_twist; - tf2::doTransform(freya_odom->twist.twist.linear, freya_odom_twist, - transform); - auto vessel_odom = get_vessel_odom(); - double collision_angle = - calculate_angle(freya_odom_twist, vessel_odom.twist.twist.linear); - if (collision_angle > 60 || collision_angle < 120) { - return; - } - continue; - } -} - -// Function to calculate the angle between two vectors -double CollisionAvoidanceTaskNode::calculate_angle( - const geometry_msgs::msg::Vector3 &twist1, - const geometry_msgs::msg::Vector3 &twist2) { - // Extract linear velocities - double Ax = twist1.x; - double Ay = twist1.y; - double Bx = twist2.x; - double By = twist2.y; - - // Calculate dot product - double dot_product = Ax * Bx + Ay * By; - - // Calculate magnitudes - double magnitude_A = std::sqrt(Ax * Ax + Ay * Ay); - double magnitude_B = std::sqrt(Bx * Bx + By * By); - - // Calculate angle in radians - double angle_radians = std::acos(dot_product / (magnitude_A * magnitude_B)); - - // Convert to degrees if needed - double angle_degrees = angle_radians * (180.0 / M_PI); - - return angle_degrees; -} - -bool CollisionAvoidanceTaskNode::has_vessel_passed_freya( - const Eigen::Vector2d &direction_vector_downwards) { - // Calculate the relative vector from freya to the vessel - auto freya_pose = get_odom(); - auto vessel_pose = get_vessel_odom(); - Eigen::Vector2d direction_vector_freya_to_vessel; - direction_vector_freya_to_vessel - << vessel_pose.pose.pose.position.x - freya_pose->pose.pose.position.x, - vessel_pose.pose.pose.position.y - freya_pose->pose.pose.position.y; - - // Project the relative vector onto the direction_vector_downwards - double projection = - direction_vector_freya_to_vessel.dot(direction_vector_downwards); - - // Check the sign of the projection - if (projection < 0) { - // The vessel has passed freya - return true; - } else { - // The vessel has not passed freya yet - return false; - } -} - -Eigen::Array CollisionAvoidanceTaskNode::predict_third_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << this->get_parameter("gps_end_x").as_double() - - odom_start_point_.x, - this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; - direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 20; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 20; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 20; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 20; - - return predicted_positions; -} - -Eigen::Array CollisionAvoidanceTaskNode::predict_fourth_buoy_pair( - const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green) { - Eigen::Vector2d direction_vector; - direction_vector << this->get_parameter("gps_end_x").as_double() - - odom_start_point_.x, - this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; - direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_red.x + direction_vector(0) * 5; - predicted_positions(1, 0) = buoy_red.y + direction_vector(1) * 5; - predicted_positions(0, 1) = buoy_green.x + direction_vector(0) * 5; - predicted_positions(1, 1) = buoy_green.y + direction_vector(1) * 5; - - return predicted_positions; -} - -} // namespace collision_avoidance_task \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/CMakeLists.txt b/mission/njord_tasks/docking_task/CMakeLists.txt deleted file mode 100644 index 2ecfb3b2..00000000 --- a/mission/njord_tasks/docking_task/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(docking_task) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(njord_task_base REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) - -include_directories(include) - -add_executable(docking_task_node - src/docking_task_node.cpp - src/docking_task_ros.cpp) - -target_link_libraries(docking_task_node - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - pcl_common -) - -ament_target_dependencies(docking_task_node - rclcpp - njord_task_base - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions -) - -install( - DIRECTORY include/ - DESTINATION include - -) - -install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME}/ - ) - -install(DIRECTORY - launch - params - DESTINATION share/${PROJECT_NAME}/ -) - - -ament_package() diff --git a/mission/njord_tasks/docking_task/docking_task_1.png b/mission/njord_tasks/docking_task/docking_task_1.png deleted file mode 100644 index c569b6261d257f20ffeccc5d318d26b68403ccb0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 199410 zcmb??bzD?Y+pQvk0s;yG(xHHWq;yG1NJ|St35awzh;%DM4@ilK#Lyuz^e{*XNW%z2 z58VuPkAm;_-S_@`{f)!KnSIWF_OqW@Yn=#nRrx!3lz7*!UAv>GAgg)p+Ratqf5|Nz z;1ep|dq06c*e=qFTDO3IzPDb71MeTW%IUejbg*>wFm<-LW@YbSXYs_v+}Xmy-sO#h z>*fuZ1aJ`NpMzwaElgdl9qj3~tnDnWX}xiy=N6)uaq|~ycXemD#9&DudYHb zBll7(>Dyq^wQKa(6lJBfJk!^vb(0^;doLctQhnZ}el$oVBfgpS^Sw%WU0Vhu(Ml5K zA13Zo=E7qYI$sKj30=ifD(q1zmXWXJoy0#3k50A9^7-d+v)-v@=)D7^Rnzs8w+ z5}yV7&!~TX?HTp|X_%=R@oSgoYOj~=xQM<8sXBNPdpVDAW1ND27n`0nb8zi)Zbxz- zBd@LxB=sn70an?)bL1MJzr^wP`}Rr3`}>zO3@72ZH;Dgt_-)kx{Ae~=N0IA#(s zCH`}cs~OHP{`RD-|PMY%r)qQ>(J=$0pWB}TEmxvGW2CN`H);)z750GY0aXw z1}+BmpWTJ&W&7{+w;=>&ar{@89{s&Kf6(WgD^ohX4<5U`&eN3te7IflC~^1ir*rww z*#4h|`Ohc$=_Rssd)@(j_g9zp<0Tc@=Kro?d;iD(T%3P=QPsVvzgHTbdGl&cp7Mu_shEN~*Pj*HHh;lZQCxL& z#yNe!5IEXGc<=Y7{&nA<&7+^D?BaXguE+N+BFJq%57uqEt!}s(Hno8`%>W`MxZNVZGJ;GJ8l#`=R$ATn(Mj zws>B%K{U@B_@j(o9{zw3w1{V_gIHTzZP?SC`z|LrT?SS{aZ}n9xq?m?Ib!_{JHHmA8xD;wvQdxEr$l_Y_0W92QTo;x`SDVugbk~(=nstB>vnI#cqQeIGAZpQJenp zNyAyklShs2E}qIH&p($7E0GK-bAc>YI^`2=_z-=dA0PhCI@ojlD%>1>B#`k;tRyH4 zbw_vHomvIQy!K0@NtK3Dz;wiGLsfYCoF*SF>p>^LZSTo!R5twAHqrXjz{g8a(~o ziV85o5yp%yC}4Ix4CIT%j?&@A=O1INkb`0qG{>JPaZU=9lXU8yTjzu#H*-2iC;aSw z?HL4t2ZN=aL5Ivd)^Bn(okdr{J;n{a6#N44VWW@mf@ZNPM&j9ZZehiB&l3cW`Uozj zU9I5FtIh8JaD+B&F9?J*{XJw&J18=Bc0Rl4mG}c`E;-?*XyNSo$}NXps4TNZawd^& zTW}hiriedSYU-k4^4pO8THF(|S4GNmhxI9rTL*0rnn4J!=g3?VZ)Dd|CB*1JP*>k) z=MGv8w(!Ow!$03^(PL=b^GT3@O$g5{x{+}8C<0$56I7$J>zJHTr9F~4g&G$Ou-rYq z5$is>;zrrVbthlE=n?PTI?emg(H7ae4mO&ym%%FPDg(~R^S<$MIaq?5ZXJtKTttcJ zcZ%t)@y+}^M%Lug6`)W=OT=qqiw>ewVty)|BA+(t`q<`b2&mx?K;#)5%r?>2Gxn4` z_DX+6(iB}66PuTdJN#0DK9X@alyZQd(?B(@*&Cp0zOOa-cCY3-jCpT=$Zd3!M#@-+ zm4PeB!YBFa3ZBM7o?D+8^v&1u4{rysoLNGOf5|ua?(wT#U|6a_VmV&5FKVWn4}F-> z5)b=JZz_vPz*Vxr+cC!Ex6zkbqF(+o!-v;M1rA2KGRs>y$0Ckj=$Wg^hr7G_#6(DA zLDhcy1P6viw3Zp_)N>ZN#%hTKLHe?)NNR?KgR02jDj)k6Sw}4u@+!#bU}qb7cE%pi zo$WT`qT-%Is`oTFP3Gow$Gc*3?H%L(y7tjwGsYmK!;r|7xz(9)ArZ`Se>S;j>LR@@!!3tZdB!rF zypBfn)X6?>P~`SXKn_&=a*2#d|GAiKn>{aUr8}z*bFJ%%xn?Rsr>x?&jNl$bHCfaM z<@)!dM$Y{+JIa&Er-aqpttLg3l8JphJoT~`Z(}vx0CV_I+Y%lYmg+_G3&p+=0b9*Z zoXkfq93AHKTDX0qJYkd)56UgrWlJ?Sn)Tgz67-Z{&D+Ub^mJ7>z9%UBz~Lq(wHg`EYDfZ{g?CMFwEK(ke`F?(`jz z-nM!D>b48cP?RPdRW>#+(DZ5h_GQsy{^7E2n|q>R@Um#6_VgV3wXerirQJyt8s98N zNK=)okoVFxP2N__=luxbv)| zwapzRn<>|%)p@LAvxOcf!uQjjJ#9Z-KwA4UaTfJu6I}|sV%PCw8vn6Vo z-wte?-q%>wd(;@o!d@AB!?IYxpn_VBmcS!fs~yrJ#2CV35<}x^$3U2E{HyyS+MmtJ z*7KXs`4}!)af{RaHcsBRpWhqa>BQ9_H7=Yj>3=YZ{>;B8>IRa)#3$kgWS7C@ejk=~ zK5RUusYK<_uz-%n4ojKEXqob%Jw!IVB$7`je(E9fCkuvz#uiO{VgSPhj?&qucE2uA z?&ssFXrDi|I0Ypqo&?Ok@yK9Rn!ytCpw~3|TP`uo7~Q+9eox0jv)!o0Eb(35_^GyA zw@^eu-rIs^zi8V266hl9egltb({!VLQg5H;Tc8B&EJTO}Ta|UgCk8y!rz2K=TRU@k zkwU#28gEd~L>0rswCC`mV{V;DXv~v@i02HzQ@FkGO-dVsMmS@^txkMG_5%90!x1uW zKEAQ!Hb!kuSuv0BWZtvVRIiKU#a@=?o=@y zx>nwkAn?UZ6Kc$x+Bp}8^5Ns;QBw|pySbtqt7#Fh#r%R)?&yrx$b}Rdu_Yo8R=(A-Zo@G`+1lKOc3ZBd+N-xq z(EMB>ypzX9Qt^Vr&?jysq)>~QTxeqYwa@-ta|cI-7nfbjHp#=QOi#6AWLjDE>~#c( z>{4*~h(S@r4cB!cY`{dV54o$xK7QLc>JMebyZ!z{bHx!tjnp7PhbhLvL=PlsD;jW$ z^G@@(5T2`;Ds*#5L?p|t^2M$rL>2YHzXn)-9*uBKO1KIbLDofHNXqi%t16fJH`RD{ zGABcpg12FtZ1>~h=yWq|c{jpIS0v&b#&s0T$$a*!)7rB$61;FBT=47(^-=I?!x?boqY z^HKMc?EPg+^s=XgvYKi_kejjEHAKwpog;+59l8);U9p5n@<1AniNcJdUhu*z*xV{E zY_^jnK;G>wA&kx>=ayh%OG=AujEH@&OT5yEmFEzY?Qe3pwSD7Xya z7b*xxD3ci)EaISen}O=g~x;OjcPEK!-WoT@GNA6s2= zwZfF)wl3%pWEaFE;P&`F=P$z3K|Jdn`tMfg1 z$vciU;l+0`K(1nmh+xb%j$Y{nyrzPy=oOm^wWoWw;>4F=#G9t_qp;0|Idb4#%&oHz zoh^iDHM(-3hXMJXJb-Y~Y9c4IMy+FL=cd{UA98c)=)R#{8?fVVdA7V?(EH>gJ2jV~ z4~Fs?Y&5&6rlWHgSL_T;r|-9Sf08}!4eiFgn^M%?nGy>xlOH7_%4+JQ3;o~mlOfSg z#|JcQFI`v@DW5j0ksWELXY4fP7>MdOkj)8*Gvh5+eqXa|`hDEk@9BN0RwRBFikY!= zw%>mJyo?v2e9X46O4{KZVk;3h*hgjbOh*(Ci5?OWGx6Df4C$vUt=YzcA!(|E5wG!k zIE<}UI%zDqrs?r~so{?zb6{l_J>&h;N3t3XozFzT=4NjL**7n6;7)bt+eo1Q!3l9r zbrp!x{Ov>ANf8|p@56%Mf%m6M3eObyS2wN>N5BXQmv9#j$(L_KaTE6#iZKi}Y8$%? z0T%&!br&~#+l;)6MVdw>H?H>;de{_q$TCbX-hIVIJGfc-W|Rmp%+^oIbLeZAj-)u)BYxzNYAmyu*Q(WcDVPV4tiPca zlqus4gme=d1XV@u8bC7H_2=D>n5C75Y&^#OkJIx5&fbS*Ua)h~A@e4`5kVpD)IgcT z_hR&DKJ#47-hYRv$EfLf^#`~?49!Z_xd;Kd%`ZIob(pTzI%2e;_JW+ST4%*04MBNh zJ6JAT6E%?mLv_MoR%YI5o#z60RJz5+zp*k%;4J2W74BJDVrQfwSz2k*UXdMWVdf>y zSs}tagBazecO48{cS;I11Xe7ddn@LjlE){bxXiBLw>Xun-XgOBJZnDKxqzj=IVM<~ zH)&tGy~fbL;orIIAZd>7kjde0?qpBzeDdmVt(NM0T`*C~2G{0}1_jXH?TM(I`p2>2 z%?SuM$y0a&@JtDsx84^l?e*TbAR3}%p7St$ggE4SBPgsf=Kh}&ko-^r&KcBVlE1;ysh6H_^Gkulunx~IDTxe=$w#DiqM&9_GYJXYY za8*9?12jRu$sb}{5=z%0K1?G&yZ;?1iGh`*1-#z5w&M8*tsNOLjSs!aUwb*rJW+%% z@pV~s@)az~+wjiMdbQn0K9wGyUN6pVeE79mRzm@00s&wKqDuF#!JjaG)CTz|iSzbEH2e(QSO1XdPVjSNgMx&%;>DNUKC3OSQ!(bU?`5)`gonkywj0Zp zay%}Zsa=MvvW+W+vE;R&Y49izoF=TS)*qa_Vm|^A65OP=YZ8`G#iHED#a&;Eok$r} zt1U`=(e=uT3+$9>eh7}W#%-m()TYS_5@%93+BOZ^jLVxO6^LXBH{ z%K_d3La``fz+lKm@K(^Zbs^cn{-1p!Cy+wdS9jjiho5w2{*qo0Q^Y4P<}d+@1|T1> zAHwDkqXA~?+eScGcwje|i6Bc!F`C=)$#g!;g_$3Z$U8l1gwS%nJuzTkk!Q`@@}{ZQ z%`87Sq6M%!z^6MM?y<&0oTI} z_qDDD@Xfst52i~HQ!Ntv5}}tATnheXrNhfUfX7HOfPMvlx}hy{sWQmeT}_*xXlJsP9-Mt?}Pin>yc@EcM)FUYOwcaGhEm$J{a% z<{hzeENmz!2jq@*grIAh8*d#}s^ML@8;^!8Y_tRI)-{j!X5k_EifBpkO&bEdC zoE^r`;N(M-3A7XLXV+kxcjXj+B!A|Q&C3&_;b?oddFggU z|2D9w1MvqCH+Gf#XQQqaN4J6J%;z44PQ|W3rhA`H;%RPw z1FD0&ow99ZlVw9Xtm`?Id=0cg9OD#GF&xZTg4(9%6Y}#v_&BCdQ<@LE=I1_0WFX?X zRDc$XDdS02Wv5;g-&(GOdo7QF&6F0U<8r0%st@FJKj4&{Hei2~dfmZHIbrx4d`<~- z(O#@}(Og9&Vd`vuUEXR=$-6LovJCcVb*sXA=ukvOK$o_BXN%n(BoRUW;m&F8l31u& zUT2JGK!36E8Pm&M{MS1Va=1Zp^y80N`TdD-U?P5ArEM&;Cow15{>zY?SExzCztW!1 z9YnKtlvAFSz?^}Y{lZJi4h{~K|Gj_fj8kf?&FLkn693>hmO;IqIS=z~^!58vpXq_- zJ3{~ji;8I74kXt74dH%>mn-yH5b?e`$WvEXsDMC~P25n+^(o=O2@s1$$3T9O?u^}4 z)|Gv^T9kPiwa5nT+5!#|zTJdx{9nkqOk-LUEu5yGss3gH_q1@3x5x2bqAOI3akpQ~ZU{93?0IoowO0ukT$S^fLFT&Bw0pQ?t6v~p>+ z^-(PkTdH_)in52kRlv$xJ1cyi9@K1kKT#(Abn7$9z&4NKV5sIp<$&TxcAYMAb|L$> z9PKr%2K#=re#cN($`k4h-`1%-h*o}5mo78v?XfG)f{@T-xe2E-FO{Z*PFa)aPJ<3Kq-X6^=A!DPE$EV!G$>Xs=hCe!! z3s5NT<-zA!3%%Quc-_qF2e^;+;Khf=FLb!>Up0vTe5k@*$0vSsPDrFgK$mdrnz{6Q zV5`C6kcZnChCuR~irc<)$W0O)S`CVU0=n{=y4J;AAD}3??Mr0SQY@h(%+mPK{WM(XvKBaQ)`O0HU{ZlJNM9^8~{Fl+a@gX_!|AQ^|LtfQf%lcCNKg zi%7i*biY(30ehlf$e|y&REUpos5K|8MfmVyQrCTU+3Xga6HA4ES^(MY*S*`atsZpFV;0u0?@n#LwYtoZ!4xc4 zwMPH2QwOF4%hcwH0lU_qpg10r_qsq#Q1QGKMCa@ss6q}l$*|?^dBzEXksOw9dTf2C z0T!y!JxFzzyQDH~`+z5xNt5li+kK7C3VBgI95bJr>VNDyT(r&}PzH|1`HZl0Gv^?! zor!T=({PK#n_h@Om|n5I{5s64^_KJpf8?**k-sPt5&F!QNq;Lg7DX#(V8ew9716=W zqT}LU!q%%-0ThR_n5F>~){G{RlN$VrocnGKP8C=v^H^ywUc!0Q zDCOLzy`XA^pbILo=S*R=pkmJ%rsJ~cl0f=PeyNt?KN+yep{05R&m7Aas#f#svs3Ah zpBrSjuegLKCbOJMfhBzgn~s!u3sd`jxP=k-g+?Ja@8L*{JdElgbQhx>ECffj5&BNE@2CXN%zLJ;Kco#unN_rPVJFO!=)2Inl(RF4ahGo*(Po=WO zq_TOXyquFJdhS(?TYMo)Dp2kH$Tg~9wZobhx5V?DOo1jjVVq_1CMdJ>T*uiD*UmO} z@{qut>@d*7$&frmqH0u7%7&XcYiO+$R@eOl9$Z{OQu6V_DyYShf zTT`vi5K0?Tj~Y?Z1M4oe6$nhl-|l|gz)KeWCJt1u`1>#dD~^=_zz}(G%yW)d@OYRh ztLq{r^Os+9Ft5Z&P@4?yoqF1mMe&p6@sIm{8V4HCBcco3kZw{U$qNVDM$cz8+AMe{ z`F#k37o^yG6L$b!+ue_DR0Z{A@H4OeU@CQDf$$X3 z)0a$@pMcVbsyPRT?~eNxV!MjaH*Vx7tq&DDu)CBCMA~`=Ei_Zv1h^?&Fc{;hV;lT8 zcVY{-4QQ%IKUH9sUyWo={|Ro*=sR)xtfRG<+0ELs6*|3I_tb8!2j-f3_!B?dYPhwW zB}xhh3~~p|ub=<^{-)T`C<+ow{!l=}*C)DaOkv{pC2r`X@el6CiV1BuU@y9}EuNX% z_yp)UYtmH zw@xJl@L=Z82*`Qd43o{=;`1OL10_)rSO91YK!L85Vc0+#-U@&Jj+G$3!xqJM(!3eB zG|sjW98h`|50vcF?lglsStiJT|EYc)R6q4ifO z(PMta8sAM?x?EdK;9mP|1O56>2VTCtcO8KKoJ*^oLw`}=cTc;9O4mJSPR>W#1HD}6 zW>Cbl$B?}*x^04Y{xG6dRI&)jJvcm){O*QZ8uf;)I;$=!w&sbhnJ)$@@T@5(5gQYNoSB?SJoo2gDRHIV4N8yWTVhH9CONGZya_ zw~71jXP$HM)&V_>zjmxxJRea>raQm_+{DY_#wLl;9;sQy6+1@LU6*t7{%GhF55OGm zhRUzuY`rjtkTb+OibslqA`LeTV9Fzb!Zl~|TioW$6;G@@udb2qmuX@3p9l4$%gnhR z6SR82Ys=sV$mkU}$hH^rWCzJT8p7%kuf2uL@yRjzQ~Vsi)HG7JcX?@4BxvuLL*?TZm@NSq8sSn)gcg+;}V{HJ*1NGaQP|Pl~TSEBHvam2!aNU+MxDD z_I%c7A=dzRw&4$de+@dykl0u5iel;};hTT)sIsr!$f${lB-W>2bpD6-TbCa~s*%9P z%>Y~& z6tMQ4t$Fisj?Ehyrh_>er?9r-^R@!uw=hvE&uLXLQ!uxN?C<@rn|9l$lUzXXdcN#M z+RaZ^67bIbX;tcD>YC`=;BAnj^juig53hac`YX^B{ZC2~Ayk#MR7dq`%<}Vlf8PLj z@tjXFGoT%?I2#w-n{cWp%>&D+$1oTdIUkR(a3Ig`1E`IXoqdF|gc>u1A<^0K<>N|- z^a`m#{`QHH=E_Uf`*OmZhfacvrh(sD|9*w zkTQ%R%^Y@x>(TvV{>KTbC=)$mXaX~yj7W?O0K@?Gud>D4B;^f}5{OEy@Qi4H(C=V( z>hfboARTRQOP+1Mc9zqj^+fIp>b7!&YYC{4Ofx=Al)v%gFH zf!~i=#djg-pkya)mJBeIUH)h48M`TiV(JzKBi$EgoEa^11EqVwkl0Gra-=g{D?fGx}{1A){YN|kxagg?5Wo|@LC zScfpn&AP9b$9*?gDj1>!*PhW3qW%8qV^CcANGJZlJuc}e{wx9m88$Ezm`k&yPA~VC zugo$oHi!s{1e7uZVq=!x=zEC;*6CW2(^X4~#wQX=>_stK;((Y5kWk!7!=3EmcybTw zW22@$Tu7qtE&|CHsP6$F{MzF}B;bdN7QC!^%T=qMjRHwRtTmm3HowL_-Qx_iyVmlk z=J6iwbO1Ummv0L_M6fu-5l+-e==H^UH?=;n3N*7&;E`B;?@@G3|^RVFKzv#cRALl<=PKKj@2bAWl4e`xfbZ2@qRgzGol)^l` z^~1p7ts8VU$_=76Kf4(+5I|-7wfBi;MrxZZ9?AG@&exImBM;X>n9A4#Ad>vFrn&3c zDD;)i`p|h%h&zQfXVShCX-5kH(txKS(KjNMOP&Ecb9@0p+D*cG$G|!v*GU^sj&MCe z(TcP{IjysWAx!j@tysOJdkF3lSqOs$sR>X1$h02rORQ&Oou6mMnYsF&{Q0mZB4=a)}j}R8PhT3KaqrQ|f=KzU7FXJJEE4wP{ zfmkMj7z-*7qdnZ+Jj9qhSy4_?Zh9sl83^gie*SH3wgJ9a^cjTZdFhu!kFNR;Q8vOB zunu?9P++0>_9i%>X-p&j3|?$mR0SW0#S~`Sm8+aO4L&yRc;Zmx_AC%3Et0$JCc0)P zVQ#a})0-=P>Q8PCw1e04I~`C>!kxsS67r(xsf#sO`SINK>5;pC7`jopdQJeC)1A5C zuai#Ibi-P2zh26-P2@fRrZE&GES`2HE6xDRKWC4DD51XaEw^w4(KN(osS>#CJqbPP zy`J|p8_NArMkLwF+bMMtc5GFoy(en-y;vm0Q!Ul zaz{_j`;KA(*yDSm%AkAnI(J0o_Mr&NXxic z`t1p4=bgnWojm+HY%p6(XwMb{R=Pv}QCkTBK{nm2e-R0d8Ri;7m>|%(FXbkv5#bSv zKh(zCvWkNQ@h z{!Br_Q{ft;B_da5@F?TQ4Z@L5^E(Yd7=xdW-~bJjHSHSl(b>G~`c^Xlh!}qZWC1_z zF;m1R^VD4?raA*{^!heo(s`Xb#iBqE^+BrvOw-$h-94q^y#nLK6;- zGIj1IyT`5B>uFGL>gEGv8_?~g+BO9Pu+zz171Gd3DqLHr#Oy5jg(?LlfUZhgIMe3Q zL`U56i_`H-o{nJwu#$M(fMZ>OOv%qfVj_e~@}UkIda-a5rYOxcR}mEvaElXl5kEFa z^7iPThI$N^C5`cD?%2jEjpDc)^Uuj3=C13_bTk~tqO70nMF*j@%~DcOOy8*bEmmUL3KCeI-vCcx~hm#G{BIjrom*HjoY0J zbGWmH#LNOt9=&2G0!+aP1h#FnGDQdtAesTNcVSr1{3$P>2Z;rD%96n&Y97!mZ?Un* zLh^{1>Efl`%sG^VU++G8TajG5)8er0?I%*4TYIKV(e8(->I6z|}z5>TD94~As`H%i1pJ*NVI zmdv`WFCj*+EssSsh`DarfH%{+9-^sPv7l`;oh|~#VkQnK;h4taGP0)vkqTDj2{uE9 z$G=DQ%)?~7_Ul+SQ&?jtIX?L;-WLtHbJ+|t%-sC9OLk)m`*8y#?XUBub4D1 ziKIlIB*yucFz9md0(#>=d9cw_VcMRzs9hJ}cE&yZGjq>WfU;GpTAd-I&#deSls>iOQ(<5e9Z|UDlR-A^E?zK5Sn6IX#5o^%AJ~)aRYU=8Q;9YS zZ^5h!_dn^woE3{J@}%Td z)Tmt5{IkSOv(xydm4q;7KP@Qq_?=q`59%sGpmP0O>Vo`Gf$ArgFFC6^AB z#y`r1GR^%EOnFEtSy>gytraX09H>|=PMfca84B}{!<;kkH>zMk>D1;OLyZ4hV&;}}>+GwTosW zq3%Zrde6#SSdG>Gyo61*R_XJPePpSihVHq2qiS8+fz8eqpWlb-tUz+`s(ew{)PKIn zX3wU=mUx~1mKZ?Ar-U13w+^>j42~P#n;G5qnf`1mA(6#zS?jnyrdI^0>bZSZL>ld* z0P_^_3?J4k1K@y$0x7nuU#5M1%g+s*iJw9fG&tWoWo0$)t%3IvMuG4qU!rHZv7=HF zaC!*1mY}jS*N48vTW7b-RdHL|aHLVUk4=JJCXWK!qnDSbqFMS-AbIBG{*buYPrbKQGN_pGC_zhHAeb~0j2h?=I-`L>_rxG}cUY%M2A1%AUt{e?1fvekZz zE;fh`;Nu^J)|*n?3!N<;^y4NZ(XT{lrlpu1FP`X;XcYAB9-#xwefD{#7a24tXt}uY zoYck0$|uWqbqr9Sc@tQy^uIfW0XX zXTPVY?W4~$Di+R>kj&FS?(jEj4nqwM3VIg$65lwV?EDHZyoKAqP_o$pQ)wNPY~TuDS5IjX zhVadB%4ZPRf?*E^x*tKfc!UEYZ0G=)i0w#PMsokwcROFGU{P9)`+&$J!lh{~K)tI_ zW?;UuC_i<&N9S1i0*EtP-fz|i(AC!fW*4~M1ON}Fv@o6oh_o6d(b~t!0Q#GZj3Q^& z2Bg!<&XqM%`A!M|%jNe+m2x}k^LL}WxVym3Jc4p;fJl<-BbOR?qbG1*m0xS#E4m8D z^&3hgd~FhIZC)2e zK@!b>)L3im*Ko_ONA_+^koc-keGdmHq#L3bZ(OIq2`_dZ&Z=vSq2j13-~f1=Un{k% zl3PC4J`V~5X2M&bRCT7i&jxBZXm5!Q)cVrgGsmAcdiZtI_8pJs*^x>VpjrdOy-5zf z?NbY{=JE1AZg=8iMXwJMde7It-jM^W3lJ4?ERIEwbc@tKr9aaB_WR)z_e+$|Gx;Bk z{L-XF7H|DEpa=ydIX9q_W@eU76rN#$YJg}FP>qvzgIP0LWD(9^;dbBqZzZEX&C7oH zkzD7Bmgyz|_`8zws-57BWHq4S8ngx^7PKR>ta$^f)Co!7z{-6_f9~DsF|I>Q{XzqM zY;T)*r791P&ut_c3kV*pTFd}d4>8P6$8O_Z4xrUFc$J{4k(M&=j9~)2#PDMIaINGo zxs^NZ2B=xRGspr*)xH(PwY?I|hzXjQcv?07=u5IaZ%wjZz?E$HjXtpa0@hprBb4MlD5|5By2nUh%0m2w7TxbBa2zHu1BIx+%I zjlcF9e`%@-M52Fx=VS$t64w$y13`~>151(-j{pT`gXZRA#*uVJJ#KrO4{L7ZbtSTK zfG&s^-e8JF0_keD9vGsI3+X-}5+5`SbAOMM63t=N`poxek#idBDRVSHF*}wvM)KVK zg%DnZHo>dn&I6ANYY=2M=4TkSSR}U)*HGwI)T|zzfu_iq3%>L-OZnNkM~pAL z(z04aBr5@299c;tkO4Z;Rkv>tk=7Q}b~lO$tCi?UeGn*oY73rr>g_q+{Z&Iv&o0-bXSv;ZLRzSO>L11$K1m~N zAL&~C%*{K{Hk(vUFTH}wMoQEgFA;H(*?8z3PmS0Vliau_{e>)1TmO4u;jcYcqAW)gSz>O;?qnWkyf#B^KgM=qaEqk8@LAB#PeDJyR^zO%7cg%jMKJ^XzQ>(<8y zkC~XzhmQ+;qObkNBdk3c4LEnv*`UnHYU?>Q)BS4v9Wl5nqa%5gp4xGr8Xm&}#;INA zVROfy*W}PnHP&C47J^d}<(OQ>%j?&Fe z`S9n>dwN5!qKnyjfUB_vPQOO$p zse>T3NPXnOmjZ+Bj}LPJ&R&PWmiMU*VJp#XbrMUU)oB_bkBh*fNh=_75{J6AqEuMerD_8!Xo@=A)-bq?dI zkpJr%fqk}YwqBlkT8q<*V+7>7zRq@c6d<`jCCgBaVSQu8S;m<|y*}%0(I9<)-Mc)q z=&v2$JLcWZ1`%UR5o4Q5b9ZJ{N{h0Q<*D=~#%PZCy`W&TQKU;vw@D`H$OT%Hi83r9 zCo$&ttmU)TcIqN-*0gY&aA}`L^hD(WLGFsEmx@9k8Kc9E zkQ6?kG?0xSjob*iA8BXPT3CX-rcQOVl&FzLErhfFxFn*>&J04W){2*7?Exr8y zUSI`O2v3q1nED}W?MSWOM~`-|zwYSpJq&Mu%m?LJEmjVheE!jh$4nZGTuYff*1=04 z$yYnnDW9j6O1b^=@06s7OW5Wx&;P!jjRa3F^O4Et4rIM=S(@zC*7jbvp1UDKkvc}_ zdC!rtcp}qu%#8~fGilEj$rBvNKy68KFKRhxFonER#a8@IXTI8=+7PKRiKXF;IWGvI zvf~%vHQ!Cn-W-OL(>%X38DkjpIwNBW%FQiXfM(bk{bEsA@;Swp@z$N~I4PFBO|Jp= zBCDH+UGKKRD-rhe%x7<1k*A2@xQvI(DzchfJEnw6qxui(qZom;03Ndj^wN!&zK;E2 zU;mSRuo6t+8*lgVxU0hb6PitfFqW0BdlpUobeggcqIS>G@)diVk^4acEmgDg5_ig+ z=@Jg4wm)0*D;_W2TzGwt>9!52@ecE-qhFePd`t0N-n(eKK=*EXvMOx zW`=dzV9Wz8IwI^$xE40zn+OH-)+V%8TOs8@L>wDitt=^d1`R ztv$&+Y!vJ4b=v2b+5lf!Cv}esoWc-bEJn^e?C~TeJV2hwqan}d>bC;2TRlt*JP*45 zorV*V+kyFk8su25(m7+^W4=ff_U6CZ=iIzT_=_mRSOQehsPU5$8QK}#d5NoFy7UZ6 z4q?EJVW@se7a+OU8jLC8j~>B;zkDN~op3fI`z^rPtXcQcKL7K~jfdwf?FY4lR-Q3O z0(~a|fySFo`z+Tovi-8VeiX;9t3Z?tL|Oxy1X-gRp~i4MuD^5UrY-5Fm4>^0%!9FB zM(wa7ejizdM3n6J&V7R2x8mlgXqJsy9*)* z#HPZ!+Ha0*63OF$E-wisW#R8$cX~el+x4MBfX3DS5BA5w-)B1;y!M7w+e%OGd+%It zJ;(HSHS2HBNGYQqWLlin;qMFZ>68-5WG>D!yaIGV2XLsK&%q5*Dya1cky-Z4Glx!# z8;}34d2pHON!i(1McJ+WHjyrKg}BRgqQ6C=#9PA6>>iTKs++H0>jZg4B*9E@BS~lT zC5PIq5NK`iY^MW*e|J?Qi$*TrsS8t>FLrhS+wFC@h^<;e4q3xX9A3LVx^#&J;!(kW zRyOpY9?etd*xl$La-9Io*}K{n$J)aQD0WEne|YC<*>jCV%}e_Y*Q|y$_-x) z+~Q?DVpAmU(>gV%LhFBEL#AW)STgY&ms(*Hca--HKqFjHt+wlk{9fN;Ff{q-1{>3$ z6{i;7BZJFG_ixBs?;Y=H!L%C@%e@7nkfV9-<$YNTGhFqeARPW|Z(>kmN6|HhTA8w+j+kbaGkGjDuHMN5=ufU5;mri|hI&0{gx$^K* z?qz-PzeeT6&bua;t(Ps*2yjqoW_j;X--8)sHGGx27wVepgt8@=@rv20N7=dgMPFEa z3+5?{^S-{D8!wUv%CEzvuLWr)*V~l*+t~jW$)t!prQw+* zsOr4c2>JR6Iwr!3buB+C*5JQv@9D72Pe@lJ^@?eygvV<}w8yj7FPiB0A27!_{FF*~;v_U<^pM=>IwB)BhpvEtukrnr-1ESa1j)EI@E~cXtWFT|*!^ z0|a-6BsgT4;O@>K!Gjar-ED9iLk^xc_Va5Y1yFHU6(h{vRLKQYQ5O zd;F5bH2>RLyNLfgZTug{+86oyKT-7my)}oN%>Q0JC-wh-OB6mq1l)^=661?l#dc2V zv9?k7q4me%j{mU*8zsp~%NFK_}&S?9vd_P5Y5ij`m{j~|daN!`xLLix`U)7;q# zAwMH$ALZoaNyx27-`WM;DBeGEiTztB9VzZXrHKCOmN9xz?bx2W4!uoN*qzn3rFMmV z`{-ca{Xr_+4mak#WSWs0{9$ktNJBW-&e_+@ncaN)^_S0fs+@;^cR<&-jK{1tMcFWw zidE9?Tu#q6XC26PBQ6br3miHm6M7$I#?`AR*+iUI%km$irI#$;ir6=(A?;NZwP(uT z$S#&-mQ~Xo`>waRNQpKnp2vGPbT8{dg7VL4!icD+)G=)9ahe!RZ&E|_xL$j&q~3GP zk9}c>jk$?9NZhq=nKs4$d^$`13fQ-E^ZS{7Q9_tOHjh9>bV5+nJG7(B`zHSkPMtMf zgPTxG4yu2sg`ZK0u@1Sv`YiW{OF6+^E>6y$OVh=;JMH{$bX3N;pcd!V#^hz-?Yq1H z%O@t;kBv<=2h04o^@p3>|F7E1+45gtDF;Sz-`4E{Tj(GRv;t;koDnCM4zP)kva6bH;I;xL{D`uMhOE4MHXRqn{Q!Jz~6inc@wl>{0UT znpE(BvstkRu2JB6yKThXJNydd*`bxM#0WEg+C5tnyxaZ%7%f^|JRMh$FLt%E-vP#w z=6(*dKW6B9_iOnx;D{}3ACZoCs48^xgld0x4T=JQTA*Ul!#dIa&?3f59aokLESDLw z{H0-4v~am-y;-(|JBDScRo;8BR_CUgzz?4!SS?~Orw=IW&7DV%#fv;qN{0>;&Z7O5fL8M|V)x9r=0 zsb`A~%P*QsFt;H1zWVl)2uA9ck#F1;*xjxmQMWR#n`^(c9||g2@dBXoX0;oThRGD% zg>?PvveLSgLJK>WH;^egyAWIY{b5G(hjz{^?gGgVjT7WCO72U7RQ2%00m710n8keb z7HDp6@*DH(?m)G|&h<{LOsAD901ah2WlcCvF(#J+wNQZ;9^qOZ$VmvqIUITZRsB1Q zL}eZRgZD#}z)YXqj>pQh zDwj<>=A-0IEKA-h^}qW}B~t{^h~=cmIfwuke)YZHF5msQRQxRNscIlRP04QT?JaHoo`s-NQAbu9m>cBWjeXuzE z<~axsd5Ye@lSS+03awtf&$8+2X~1S8+#0xTXtbO?@s+mvEu(patdMOf{)py-F-fkg z?DPKLh9LnPCZjzozorPqQ*0a|{@S;8_ppv~qj|zVO z=Wl2qkIO(nf9r=z-Z_7GFp;|d*afTuD3;|twpli)8)N_LI2GCT`vkF68^w z_gB{qGhWxrx+3rs=9Py_@4U>8ZpzFI+rXvWF|$p)N)>79HsRHWq$v61hXwR5cmI{FTgrvrqe?T{BdvL@2@D#P zTw$g}pi*DM8MH%R$^>1Jep2zjYu5Tj78Zpei8C%zrnzWbI4#hV_9-xV&HLsv^6S6F z+r|%tFY|d9T#f?Dxv~7|Ovx27-qe~9B+m%MM}Ul&x2|8yZyKySXXRC-0NdE)`ZxYq z1{@EN;79NhAluHq<sFu5){3zO7z!}>Ow27-O zCB*l~9D_ne_{Y5OCMmt)m+D6q9dyyutrqj~`E@Z9LXF?cqD*gr+G^pKS0TnTS)%Ki z&oG!?z#YwYv3`n^cwPfkv2a}#ix%E<{&-8^AyDIRT<0$A8Abb(>fG<3UbJEM z|EBSA{5;qd+HaSuE;>_1z;c0Unb860{! ze_EBMGd|${GxbzcXfw*#u9v5)lv{h{^*YhNKM@r*|Km;DPhpz%+n**s9eft1nGm$_ zD-X2njKwS`D&C4FSWI9s147C56KF__wqmb^oA2y~&ZBUb5G{#m(yNy%{)3FYZVxVy zFmRj0<3MEQrbU|gE=?WXG4!Sv=Mj~~v)9gTufkjjFzPZRs=1cAD-=4k%&1>=NUy

EaSC26fiM(5hP%wDV-g@S1OR!_#2D)#)HuzP}GkQ-=6<3dE-$a7E(}?Y&%jW0Ea{ zbIWtc``IT`#$2IUXyo$YRSdsVHhILtC>vaOCR!Xr2!H6K4PlC2i%05yv#e2N=#Og! zAQTdFQ%{+?eD%Y8#eypJEEP%!jX#&8oU%4Dose!Z<5S5 zk`rdr{G>sglpEy>*)fl66b~4i?RXS6dA#L~@G*4H!0cm0+UjkSPSV*j&5>m|PjYNCNf#<0gO`xG|bVtl;1b#n1-sbosG=z8I7PnU;7 zGSB=>et%4jl3m>)NEfHb3R9AL$~I*z&)lRy7IZDh?dQ~Pe(sjiLs?=>Zb83L2a0d8 zP#L>&>AE`WuPqIz)3NhU&Q%xhcQFrhF~}B67${3|{sBMSJ3&7-Xw8FnDA0k8#%fE! z8#hKRk`6H1{WqM64f7w|l$V;G)A9{((o7Aq_0C^QSO3F70+@p4f`oH4cpLn_4J+;I z@X9CjO8?t%i|Vea#{|ZtQsCqrTE8k0-{O#H@0fnpcY2ZUVw4b;>3O?t`pC{;t@pUb z2^T&jJNe>LRjU~2WW2%?6E2L=9tP1D6X5x%mu4yBmQ`VLyY63MuB!7eaW zYBRaaqa%tMn9#*~@AKCHH*Rrp-nC65^@unjBC=q28W|XK2J;wj_H<9j1j;xs_dS zpL%Z&w+5?)9DA~RKTbA2JdB8+9z=+bJ5Hg7jNbD1lTI^S_`0Y4-4uo0hSSo3lbW!K zvRt{FVNhlUZ6<^g3j2HXiGBInI7(vJYl1rVhq4#t$isqQzp*fF2`IHke_lmtOGVt# z8ruOI(1>Xuya@T2Nhpp^_|U%9?>@9q{aI$yEJ7ag z9G)IV@Sv8s9LLX49@)jQ0%Z~2rnsr2M)-u*`{rVZpHa8OtM?T;bE7llS)y7*_PV@s zL#-_8w~O*H+X~>V)$zldDwKwPg++_Bgn<$H%B|(o0}iu(e93LY%gv-E40=?Zq((mTEQ=HQUk)DB!)rZ8QG^POF% zd4Kp%sp??eGA%H;n>i^`Dg<{3AQwXM%q(UvzhB|PB%saxL-ZY+caZiRAOHj9$nvHS z_P{)fGr*vMjik#mi_?|OHxK+W8)xiNPNtO17@hxy`{Lrv%J=*4gvc(2dSuOXNFUJN z5)Hhx&9%UZ*sflxvkPQS=Pp^4sll_@BLF^ebd0$o~DKyo| zGWJz2hUQP0&gQo`e(_r3<=v;kfX=BS>f^qbhEArZ8}QZY$7@0G4i$RiiOa=PKg+6j zC@KOFXwNq({guCXChi8@+bSh9@t^qqzU)#Zt!yCSlv|+r2f|$e5dP|KymYlzK9^{M zA%l4rPChRXI@VYCM()MHI(U_O7LN05>&l3TmcO{HQM!fZe{u7vsUCe42Gc<^o#y7f zfZ>in(|-?}p!Pu@PW)_Ue3zB~&K{<=(jBSkg46inY=rSZa8YT@{@q6^?dD?xC;`j~_u^ZR933|=&*B#Rv z)0ZrymHrk1K5-$-m^Or|`pupY;5G%jc)>3qb6$_!q?M2w=v@lnM(y|xK(12#a_?=D zuC@LVA)`b95yBGkk;W3ER=M$ZWA)K#>G^UDRfrsajX?S6OZnL*ldx*S>2Gyr2&|%&7d})*iE|j=vCydQ!_T9kMXeV={3G`Ksa@ zrBS)@geZX@R!8aq;y0;=o@MlA4heh>S2#!kLQfQ+`0@&56)b8S5qgh0;E;g!Y5 zUtHjS*>AVlpLk0X9PyA}F4V)!Fc`iVPd8V@>xTfTT8G)ydcV8D49*r1<+GN{8A~QF%9i|oM|3>U0?6{muUrj-+S_Gu7q6-q z&TuuT?3!m}4vst&G$HM=xOo(MQE#~Zk4rLQ1YGI_2E}7ap-mI~*3EGR{*B|cGns0& zmc?fBGMTN@MfueAVbb{}qEy!D%bClCTVJ)IP$Cie1h0tXwY9 zh|RC>8*o!sXb1B4mAFyuT9tmL&PEMw)KNdaQ2lB~TnO}@gR)(ZqJOw;;ji$r#cIM1 zJ$73Us-0bo`rOg4tytqM~YElC0+yp*5ZTu~MTV$=MjYoTtLj?6XZ!b3TEH zdTw2P=!-AH=1PT6vMtDH;jSJThBx9CPL^?=V;#Uruxb+jh`m=m39j8biIjMQO>JBo z`xH_bdG_Zp5o=FjqmQ8!b%lTX0p)rr@u!pE)}(4ah-AIEUEtF2;k& zcEDE>h*K`ktXROVwhc(bh*XZUcup~WGBY1ep{wUWELJF7t|F~ttJJDsacv^1sWr)U znd`w8s#aKCKQ;TuhAp@-GV<4=D5`rh&@5qR$d8z0*L7Wxk)hP(7HJOwDx-3-$k0KE z`X>qs0L7hO&qHo^Iv&Y^^F9NFzm)qt>pIspEC2 zF~}YoJVx-4={#VGN$SMGtDK17Z{ygUB$RLvZsUo;X#YsE$MdA9>H@{TgUP|oep2FY zqWP87=Yp;y&AsNO!NQ0pi{0-rt^|lbmHpw&(%{+FlG)=N@U*Dgie zA-?C-8ybf*xx*&qP{3OE>fRzs(r4W|Gpf%8B>Z9~ zlw}&SOCB?9C9%St@~^?}R_L!4xO@I2#60gZ7Q{gbnNPWSm*WE@PI8-U z?y@dsSsaD$uK7_q+*(+1Tq4FRWRrm5V+l}y#1*iKHF+^4;5cxt{-Y;`7mzHw&KSm( z{;{VznIT}-nJjVh&FO^D_Jmc(8t7AoZZDU;V-O&#@tKI``&Wg)8Ufhqb+yTlKlowE z`%)0dP8oFzosAP1xQaEvO*A9d*ji)!(GQ*y09!zLgnO&{uFEAFU&)Q7az%Q%IRX^{ z$auBtdrBz-#;zC8XH1_IibZ-mM+yC*aTTBHKu1jkT4y4ue);3^mSfz}3^n0)Q4LP) zn%4n&98@yqv;F=PV;|Zo*e3t1*gmKJgB}9^ZgV0GXHF7Ig{GLw{1(5x5*6T9; zSt!CK0cBBQ_9|`wB&0%TWEuG?WHTkLqdL0tmvQQ1f|#xCBf&`b<7U{srE7K|16Qc| zx9DKAM;c@x2Q$fSZ|q%}dHcd`s1%5VfEc;igrveX#4*?F@mBuEY)nGD)S2NBQl(#6 z4So7?-^pga_Ah((J$#wGfA6NR|(s5tMFEP?J-#B*29CxJ`|bM7T^s4EZPMBOSJT~ z#=WC^vgu{r#le<%RIfjaZr1_?3q(r{1I8o-I*d>JpQmm-1(p_p5l{$yRcH6M2G4im znk6dPNq`KjB|euuU$ows5t=>5-|=V&V3erf7qe~*y0A4u5uq;#-#rX-HHuHceUPVQ z+-6`#wd|%@LaO@H?~@(UR~_9)8)%fi*OhurnsMbv#!M|;T_Z?@PRllr({IYZ#_4R1 z8!R>+;44uKdi+qcWLwEoCm})%f1SS>XXdd4+FGDmTv_m5^P=|6Q#7}81C|{+Bnxnk zJ$d)R%f}|PDG0bSF7mY18QgR=DF?Wp$`r7=@88dq7&9hV^jeKhZOiIEE*|AYQ;JwT)o%nLNqV`oB$23|pK|Y{I0Dw?Oon24bdfo=Tk&hL)@r9HuP4Qn zSejUF>YC5{=D{c9>TUiY{^W%+5FF&aKNmm)`mA%#DUXDC(vu?DoO*lmC+c(fHGiy{ zlYr}92^?UORD#f??RnEgm($wiZRhe%^_65q z1KUuhRAn_ZIJqcOPeo9f6XV>HXGEY(fq+SBfm$9J=Tos)^M?dqMBiCx_)Nz+RB5a> zt~-29H|FIQ?HE4jR&RP9fK=Dr2M$h^jcf{Jc>@QDO1oSn{Nw;IXTaTe$&@>Gu^CC} zgw<&(;og2(j6j!OMDBaM2GW(kI>h5;l)zUNv^osc9Aut;eu-gH9UINMG&!&`$J7QL zpKx>_bXT*n5TF=U#N|2=p?&38PwYabA)v6(m{0 z@AV#1t1s-XvRcD^hE;47%nC8NQKyIO{vG@$26hbqIIHUqE5CNs)gn&Fi{xqw1-=W- zjMjXscDx@36=7>FvOB3xT%!C#gKco*S^h$tgNyVkJY-M~m6V+`dbHdZNWJZtOB*!+ z6N9yuo=)8;0`5|mS_2XWFERZ4)r9g)IODTt-|=S}{u^0sKb8fQqD*||0fKs!^938^ zq1V(6w#ly9VqeV3!QS6XZfeipi9}_F<=u{mi`lWE^)0Loy z4<7ooE-gfGR%f7QdrME`_rzNxbwF6_8(V!qSMPvFgKE`0Y=TT5wpUM2YfZU%g+mM( z-h_dqI6Ss_6zVB{+)bS;2F*0Y-+}{E#UHW)ZWo^u%NyC#3{AG5=uOXiO&=5fNn0F| z5YOa4>8TnuOph|`7Gvad`o-$QSlN9&LWyn)(E%o^%V0(qW;~;t; z2dd^#eC+sN;QZ{s_S$oWlNb6&KHz%R_qNycU%d@5OR2W8dgQ(oa=U*RfQX;a@xbgT zexDSUY>3bNp<4Op@bjM*E*?55bLToq+&AtSWDb=V*ZUB!o8jEaPk?778SY2QhTkT*dfesZnmgMuq#?`!;MSF z+d|EnxkYBl^>h%yC#>r@^{=kWNcx{3_(SqbO^MSUj*!V_>Ma3Ty|j6(q&-pPnr4X( zbQY`|(9<(SPZI+A%`EjLAVQ6KV>za<$pkn5zdXiDOaF*ZLec2xO=1dFwBaqy9|6~?`CWZI*Nm*>KTb@b2aQAahq(&a?^!`5DO1~QG5s(C*_p0Z=BdnK{lo!Hys#+ynx(c8 zW9Mp%wrJMj*{sK?@<#Rf)s=T>(gnLI&G5geBDa^bO<_GA;IayKEdziP^L{8@<>>H5@7U}wU3sG`klVjRKV=D_&F9V1&C@H^0%vuy;%~IS zNHpvOB2}LZ)0#XmMZHxjY91)P66z>eE-@U+np-dyep|U#b9D{8Nu14NpiXTcfS)`m z13E+0pJTVDp)#6(b`}w4Odf_Oot{zidEAs6(+mM3c02!h zD~YB<*wiD$^9zBYUM9t?fvrLV=e3;@-<8q_K8b14_t>Tdaw}jmjhW+wT$kbP)%qct z02nmP&(BTeD3}qNZX|9y5HO}~wPSgnQY96V(;zILZPa;%_Nl)Yh)@~EWUP@pG9M@@fRfZ5QKNrOl#_}|Da zKgq8X;8zdo`ZyKD!O$h4S;BVO$?h**%>#HFue+@qhc>^_>w9IB=SyCopCDe| zK029+*^~0(tnxV{0`eqStIYgK9yhF@@FF!f>5M+3Yd5hsstg3ENDKiJ&RTh8ahKE}`(^fjHOdu0L83ZGh}Dgi%i7;e&v-UI zc@pv7U0%IWv?Cq&2nMaQ5*Y*;JFarx(t$Dq6_&4-%}$YetB{%2pI_)-<9}BJCVS`q zkom<^ZNK86VEerXx<8^)W%ZbDiz$bXNHv83?;w<*HyPY#lvR|T4%>f)4>tEE9F3Xk zw>8DhoTG|~^0;5Wc?X{&-{#!eIPyTA+S2pl3o)S^NG}k^OjD`o+%G#w)Jj7{ol6^r z%QEty<<=B;+#gD)NptNdc`H-;^!_#adrqt!F!s1EY*I8kSb=j9O-t4<&G=;*ugAhK zTf380;8Xbqs`UnZl1Ps~D_g3~xO3;Lo_XF z|IN57C4H(6_I7baK3kEMw!hi{=p|Th$*$|0M*68mWugF{camMadA@wWbX`5GV^2^@D~gis1I2DCt^8V zydhofo5q#H4{^c_`KTd$!1Qn}>*%&}Pnn?Q{t}V$B9*afuS1GPO20l}Zgw~zLH*JO zQzSu~FsB4eg7%wfPeh)Afwy)Py$x;7H*7Ky1pR%Te}K^_ev2{2td*W_Yj=eJw0_`0 zCm25#09BmQT>_7QDfPm;wn+0Hc9|Cn{}r-V5CDeoT3$b8yInB%`WxQg5of9gmiJEu zah#y}Z0g5ld;QGXY>n_Tc$TDHE%Qs{d>@H+ZJTPz0EuGd;H`{{93S?J{SPE@vdl&D zRXVz@W!;B_Q?EjhnXrbT(Tg9wL`t=?O+w>iY)>z~MXvONE5i*!Zw|I{WAf;?t=2I) z-9=M!Sm7cMapiQjU_4P)Wqd56d_L39Wi4}g=#~hsC*FVH`?oV`ZZqa7J znhT>&c3xIBxkdno(JGb9QRSBdv^R;t zhdS0f3j@^f!;%Vt;k+)3j(ntz1WKKkORp0A?}z{^2<-2;w|`sj_W;7flNA6|DX!kB zhgXws=qh8ZS_~gG2cydOeK1=kfSF;{U4v~2ce>A$&q1q>n!urcb_}xcPGP9p=4@;o ztbB@i{^w{MvlToXgU+Y8BwrBsI;h_>Pm}DV>TVJsJo>3}M*h3+Y(On~C3nH@GirZ# z)E@ZJSF8v0oF4S>muj~+%gTFqeB5V3^ypU8g^5j0;!pUzu~fRaP3*B5rTdyJxxWZU zt0M^(!r^&AnfWNv>Te6w5(<1WXA4i+*?GbKD{9=5ces^M`LZOxbj#qEF-Wd?Id=?> zg|50+2In`XnF&mme!c`Hu0OZ7z&|?Zn?2r~!4w-?g;X<8mS;N&O zqJ;1Gj?4JuS>TH4&rg)w&Riwc3-K2pW?FR#CGJRWJmUV%97f}{tntBpZ{gp!{L$*@ z^N(zOfeMxL-K&P6dBxATlEB9uRVW+CT#DWsUVly2IQ($_#fHeqVD;J{pSh(ACy9xJ z7y2peCRz9eKAJ39|45i_iCVQQOd4>~l(!28Ew=sKXaMmA=u6xe%q$h4Q{gYcAoN&* zgJr4jQq8f4-unkr9Ph885T`>2C+YPTB%7U7;8_KAw^zBk zesk?B(&Cf#1f^`Yu}cAYtH9&L+kf>Qn_F+uuXOc!1O(d<=E|)12UvF288kPJ5VmuN z+Ez1+^p)>4gAYV~W8Xo+8tg&ZC|5&5|*Bb-gUP z)3*3nrn&f#%?S?Q^P)z&1lPwsL5kKg<{ONX+X$`epsn|K*6AFZ$yRn6gY5y@be_n` zYCVkQ6Bm>2m&rYvW4Ui{J!^8vb-q}ZdQZnb#<^%n zaKUskwYZ{|8$)~5YVEq+Y{lIu%etAO9F#s~sj)i7p7lUf)m9Rr8T!^kDM2h)OK+@%fEy?FQney;E+4=)d5Lc=Yw0nGu zugi}Le%)YDH4_-ecXaQKF!-C;fa=KIEEiQ-wp&|L(;bC2e>O*-n9?}rFb`R#Z#)Vg zJg(M5d{h#Z(}2g$l=Y`2(qHl5gxclMA~D4)kNDeBBArFmm2XPvwVVKJRHWS-Pl|NQ zny&2(HAM$WEDeXDm*EQHZ zl61X`9io~QZThyecYlZ*>`XE@%_c`dyWVEzAoWSA(cr_fm5}TkBI~zCcCz2Ht8RMQzbUC2| zAHfhoni7VARw|?MjT9GxU%iHe)i1m*|Ng2KjVa&PaoGo5BV5A-*+)U~!nENPIxFy} zBcc}xK3FJ`$rv?ZZ%03QFa@aTFpz^;Ry?VdlhmU>amg6TDN_7^0Zj?21a8YF0jlDp?)UWa@y|Y+p z@fQ{0Y5$~?D!UP7c=-dqa?`+0+07TDo9Bb+#!{*oI;qsCcs?49>SzjDWmXhi!=YXF z;hWj}h0Y@3_<>{vIgv3F&a#fo$outg7RLqR><7gT_rs@Oxjwg%KZ-PEY*ueBY?Y?0 z3|U{EP2U-Ler)F2*WJ3W(xo#Fc7AE(9dnOPv{xzDv2UcvV*R0FYtAd}o1aHj-`kM4 zAr}g6u)rC#ModbO!D-7&dsb=T9G_=bQQc3Za}M27=qL9ahKM6~D#5Rb)P+Nj3ycNN zHgG_T4Ol8Z;}~loL}mlcY0w*677A{Z={9ugGe#kQIXnCoi!a3bv)acv20g^Ew6KhI*30}|4&;2Kdfk=$-d`+XF~T89On)U<6bl|Xt+!8H-_LNpwdv?<~u}e^{egm;?{c5a^M_D2% zJ+LAfaao5fxz_WG?po?weAQ;ILDgOs#($s@j+C@9|c<_;OhGcl^~1LfXOaTr3u3 z4N&KZUgpo0bIm4&*r0AKk>v=8{b`Hc+|BXL2eu>6U$_$Uyh-2+S}ajmt)o}&%4Xtp zSkIvU)`h$=qeqVRw{Stnq1FeVNdrTEgkCvvzRapgzEI=>(OwG@okFvM7jN;f&3bW1 z1&B?I-^8psF3w??mF5@0!#RR~Sn?ldG0mZ-xfeevoMBb)lT3DI7r*UyW;$FRwDK=l z1>ZF%E{M->*;_XlFe5Sfi%pC~M;MgF+fYqDw~K?VM_eA4V;t4*b*--Q*Q7SZ#ntF{ zqIz@lJw=4r;_-G-z@Ae#%N#9ZyG1&Cx#OfWXI)=AM!&6+%EoCUcdr_DZ<45h9x_== zC)@hByx%5YRQHIK+eWOjxv8gSB^vHw`>V;#B8}?!+m=^?hEagw)Z`FgT6E(Wd*v(5M_x@ zRJ(m~(JHz$XOQ}v^W{7awHz_r6fb8+D|`xF+*-F2+Y-jH2o_nr3_|!LK&z4Cdi>h_ zev4U**5X9k8)))PT0L?8IO}qgtp{_&I5+5i<6)N9E97pv%(upD8vNfR5;s~Ip6K_lE3$x8UuVHMR~rp%!uge(MR4Q546zf>x7lN9$vOeybm zaIAII>ZF;G+bft&_|1nJ#rTZ-SH@oS`hS```mJVIZ4@lnzxLV36l(G4y)tA@C_5j& zeRd~u(5i{z?WdE0u&i#3-$w0s4zPD9(+Yc@1ua_zHpXVlF* z;_OF3G1i9a{V(kaSr($+;A%V3<@(F~lMPPAj-mIE&7_}yYYYTPuN!)N@O!4IarcGG z>EagH=%N`?O7WElS}jn^!Nb4cd4y4R!@ED8T-uzVf3ZqBatrM)UH;h7BgNy{(j9t3 zKa1t4Q$FXZo@4lnn;P9gY>fQBpJC9IC%72?yxvlOXAxhBmB3H{*(j;iGfZ%vLH*#C zg9v2pmpA%aE^JxEYM|b49qeK!d1Gd8*k6U-O2z;r+4>4$xm=vwmsC(G7G0g=&D2Lf zO0=$h5~!@7XCHtm>Ko zHEy#?uC{$_{X`5HF!}EEufYd~WI)5?5N(tw<_g;+9A7wYkHPcU4)sQ08?n6|XM29V zabeA={ho5$;A=+Q%P^r&QCp{-=8YQRXtX4{g7UqmEY`18$ya#H7f~p+Qu({T ziYvaT$yYk$FCx~ysq-H7E7YtcD0(JF~_gqEA}EKk!B7yFzfAF z=F>)>S@kX;%o4RjV2&U;{YCqH5nQho0%`2j#Ju$nWr_9HQ#C0YdqNhkgDbY$|ArDe z+^4Hr)BM78!%SmjNEy<&!l}St<~0E0Tnk0-dUUJvLgoMXzd(~7(B(l89%wMA_X$ce zI)tudaTcXu)jp74laWoo$$Dh`6LlH=gbYi&Wg9Zi0vC?GXM1Me1Qj*b6SBpaq~d;$ z0?^5Ij~kG!$^PIkvR z*)POstGdQ8yGY+fT-r>UJN}j|Vv1Cb7#V1B78*hJF9e?3q!++XZ$qNp&OFZ8aWL84{3M_Sp;sP2VQ1aV+GmyIEe@Vg*6D<&ezMyIzebzwd z|K*}*d3*=)f?p=IKAiz2-76Dz{4PeR2S=}%J;k>|#-4xtYv+Nwhgb~IVpIuFVQVfX z48rV$LQ|WK=oi1YsjPa+M_=hnX2bM4KCKMBi@7(quwd#bbjdg|^jE;ACt~@!5ik3C z@%20mOZYSgdF*o)sRXG6NnH)$jNDZOv82OD!6yY~z*)Sr5+B^%IU zWH)5L-MpM5*gqabm1fp(s9!4-60v`K&{y&EwKnj45fGvw>l{<3maJw}n28%xchHgB z_hVK%i#k87r2XSPtE}YPgbD?S_^!Cuos?VB!+Y{#PMM2EF-XP^Sy zasK)EkC;<$6Zd?d!4k|JfANd7Xkaq~n_)5m1v{*b*n8UMN+xk>#m zMG=7)QW!o#;K=!Xh7<$_0c7=lwmw%py}SE~D@759SIiBB(dWxCOWdC*L$ms?d9Lyg zKeGw1r>gN3ofKZ$K0ItMY9@O>jp{hnUYXKDlBN2ook{4>FpYn7m|4I*#k!4$LBiQ$BaoN(3WhZ1i(59S{h zs64T098r?l{eU>GIwP?`SAE#$8K@hS>KZn{2@^HxfkrHPJb}9L{Hwt?eo(Ns5N6mzI$dY zE^TA(Q58#a^T|?O>gPtbJVMOg4BTaWHP(57)}+P>DlXJ8$eoCDk(&)eH2*;yBkSiV zQSv)25L7!d+DDoNJdZ@=c@pMzwS|4;)KQC_62(Dh;bb3Y(Yk*r@C zDQ`*HXkNeRx6_A8@~WqEdH?%+O7E-gS%>!fsQV`@y7Z_?Ea;)rCw|hpx4b|XKJ$)K zSx$1qM2qc<71_h#cWs%>Ne5Z0^@Hk$dllk4+Vp&FZ?B>hZSj&1NN<0#Bq{r*7oVVa zEQ($8fc#R6mSCNo6p&}09iv?0!aVT_8q(muk+Sy!P49vWut`}D@$Nj(KW9^EEVsSA zXGP;RKPS=(vo?r=M>o9ef>eZP%EqH0KI(Jr{mU&sO ztt0heJbKNzpJP$0?4IM8mKwGXm7iBDBJoJpxOUN)S4N)A#?yDS?&2%h#UZS)wt(Y(nab4}IxSPk=(+ZQ&4g{7!x4V-7it=&ZqU?vuXq zSMr!!A;LKWIZbr4N(2=0G&TuETqyBjTc)BP$vU}k9n(#G)tJF93VP17xDV{gQt?Or z-Q%#45rKiV+yTqqkH*WB+(%hdpx+U+*Fos*EWdT1q7ZKt0^fBaHNEE~t`=SXP*TcN zA2}TP=bJ8+ON@-0NH)HSTu~FzyR4J5mqVnDbFDY1eLWk(eEn^5 zvjrvRrMDmQUGm3?45fz?fCk1&xz<~_xH7y4suJFcH58y^Ou z?jl}cYy26|=jIzTRU^YKe33NLvn9WK%Sq&xICC1m<(7;fIWI)4>^k*RctKa4F@)8A zh4Kp+vhec`uC6fJ2Cuu(P5kvwdi()ExLR=oDj^s{z zy0IXsg65m{!4(;o+BjF*R5R3N6_xM}VupD~|I;mXY0TnSv?_CMM z3iYEGmk7O8;(+9mSt$OHF8-YQ97lxRK^sT&)-9%N^$Gvj?%Gr|*VFe;cja@?=sKG} z&ab4Mg=3#kH|6(=Hu|nLl59D$-KV!{2;WPP$NLviTY3DA>do?-nod%A^p8T?HLzQk34* z4nO6k#u|vOSEK*-?yaSLr|Lo&lLyuq)AQZbVFjLR}E;~s1EsetuC8}_J zUM;6r=}hv?h}t@V8AKzq@6f69e}5QPUjK+q;kmXiQH@7R4bMt{1goc%Xk)*_%X{@@ z{-+Bv7!h<|_2&0ary`&H*kTG{2uaFnyf*)jeA$E`M;3beth#lMhNod;$mndzATVAf z!JJu(2PGUX-Rt^V)zrwDxPV-q!c>o7H6bCLNqHt7)es{!o)q1hPnn>sa?ozGn}iVz zfhi8=XrSg=)@D3GtrVJa=t$fxyH{{bFbFCwh~HU)ihfHrGKS~MR1`n7A&xDC?~?D)=ErGS8=(dI3WVmq<{Kp@@JhNf6kAWV&8@o7 zI17QBFrEkgbi`^!bk^aR{bI0vW*vQInv_3|o}bDfmh$AW4`G=GyIyT&690~m2;Vh! z*#+S!Ka~cp8z49~m~pZM^NbN|Iq9mEnR2fuOj!IaudJgV<}a7@PBm<7X8gnxrundK zC9=cHCHJuzqc`J=@34tx$H&ya$kxU0B#2V%5&|B&-OmTw8>tdY#_MZH4%c<$(i&{G zVEbzu`;YxUFCVrhVlN$ex!$SX4Tf#Gs=63d^im91nQ=ww^4GHv?GfwKot?c$Hs!0A zp<5*=s)m)&>JTS>r)8uY;?RD%5OuE!7GtwE8GWzem116VG>BZwNurkv+RBk*+;c| zlbBD^+*y!^g(hOQ5G9jHn&x(FiD8%V?L8`;YBRBnqq=J5dFI<19@;pIeOqrqB)1Dc z)i<*4@mSuEm-Ju3(!o_(si;=$er&U)-01`I)Ua9kwq*>t5DH;-ii?LrC$Fb9`vthL z!^&fDCd9Azm^~n77p3*vjstX`oaGn5$U_rJ zqkk#J0v@>4+n_`XJ|5m&J&Ura{Nug};=rrl`Bbdek@0ODItz;m|9k^>qBEwnLTfojHGq zE`lDKSvl1&wx;Y~ObMgEI~y5GCRP-Y5$t8WXTnlN_X=kGG|ygZARXfU0(Y4pLJ_(R zieA?teI3Hyb~VT(w6x!-DSJf`;|h|_B~IO@FJzA{h?k~O?l6*W>P!s@#_OAWeevwa z)`CBgB`@vb?S>;w&l)&3wUT=*pdQ=vevWdG8=Z;)GJopxe`q?#;JDjxjmNgtxUp?C zPGdXS*hV+lB#j!|Y3#C*YxG;L*nKpq8uHBJrj~jEgH>Ti6(vV=UJJzqs{E2qpWt;W`|OO_#-AA zIJsI;huBGwMXjD#N9qi(7K0P5a&W&1ntZOV2(lly=#oiNpj+|if=Y668KAg{r7tBI z#5ZU*~=?n$G&dAMmfX_wLm~5WImZ{s>UfHS!h=tp9I0b76$|ZFkD*4bsw;rm{wP*G@ zk{>A>kxveYmPT2S&6-r_Ht-i`ca|$=+qODA{&jC=M$>FLPO^wp$+mgMRe2F=j~K0a zXY6z#%lue`IZUo(N@mQ5u=YH0xl_~DL&sv%>QadPkLnjq0(}LVCCLODSe2aSe?JUU zb6AsWGV~7uUK;ueMK09<^L!>z^Z}Ri_nXJXGd7y}(idY)R}OXarVy!iycXc$j3};X zhm~0`R*T7omXHyS+4-NJ53iKBZYiDlL<1%O`^?|mMfoC9tb2vSB&Eu*rBkyV6&K@8 z^q!@O$jPY_|8fkxj%i$?MwnFm11&kJ+(Yj)YH%gk!fGEWEaMS-8Q?HS(BzJ-EIL|es+ zw-d&Xb^kZzwfFn#vgU7X=PX{`Z^E(N&lzMNPsY>PGF)dfmn0^2s7WjaYJA0Ahc@}* zIqQ`rBFk*q4wi?RiP$aB(*7#;6PG(V>#>99dD6>kiD%#j2 zP6rMP!cSe$$}doL1-(gz8+*5<kTODVKNHF(T~&Li@*U8~E%86v22H%6_k(5&!$<$Iip=sx33DdLQHA$>FW*jd%xn}(h@}e zGUS>9UCz=zoc!euc%JE@~8xXD3r51+}QT3TxJ(-({@M^0tUG7yo>PKx`Q$0lrq=&jX#)9G?XzE zY1H)B(-^26`2fj`!iAEv!2oaF#3Fv;Mlr~#Kc9-(EvK9|3<~pO5cd|px7`D3ndZkK z8>mnYMjXkcZE4{J&b?@;zB57Ju6g(5X_`_?%`&MkJ3sp{MGW4Lzf1^b|;{8=l7(BJTLx|{|&Im+}uVw17VAw`=9#V zVkyXt+?Z(f!@2KOJK)7t#-m-x?B;q)Y=kTPR#-V+NwXOGJ|s|L!C$S}CgY%IZ!b({ zIJn%spCoN;RplXhb_xqJaf)o9?9O#x#B$iFi&of-Bk`fzf^C(MgZU~G`g5=!d*DQ| z#5f?LmnBgo1-i>37i3QmyT?A0s~%7kqiD>Ipy=~Rf2_pOoP-FS`nz?k7%J7#2tHL| zwedUHe@yfBsE*zK#+^--#?DCoSO>-ITmE25F}m70&na4^JUBH z{$@=JPwe4y10GfAJ7jFv2Yk$9GymGU>EphcfK1qcYEpZjONw_9E{z<4y8y}g`OU#U zb#O-D2zt>58`?2yflVtKYrZA8Lk#xaz-2ymQOAUe4c)wW^m#)U2}wtP%7>3-@}1PR z-=|d$i@M%fW|CLb1HItED>8-NQg+`R6jEwiT%$ql6FV!YNrx^S%@cFoZ}I)Vnd_MH z8e2wtFL=NPx!ld7uMr{1b^%>%5FUhDfvb^Lcs+kihU67>1EP}2yJeI}3#q~G#XjTL zA87rc#T3=ia^a9eyO-Zop4q~d;M-1hK)(L*g`Snvx4*_;p}vuvC1!R;*Z)QJp!Sh5 zS%p620cUlJ%(UQijIQpPWGye=q zL~hWdZ;nOisAnt?CT`+gKh>Y$^9*m91q8^h3|*4d6O_1a?KtaqUEji2@=I4Vz5GTk z9Rj(d@TMnv+_PIXqO24(NUlQVA5>q(CcFKwhmEXHT7BjQ*4o|dejveGHZctVcCm3& zIJAzno}Rkp054zdLl=s9FuHwNhK_k?3NN7O2ZfyW(6$N0gxU+gWE;K4Q4`HNk=1>^@K1rBc)<>}HrY9_(2AjC^VZ|cvsuQ4jWx0u z3w*qmE-dsLbG4vmxGQfu7nrk5To0r@TAc=fwF&LdWQsL8lUf`N&rrrLeHU*&?g%9& zg%>N~CFNWz^K}U9hC-~}6#$e^JixzH2NjTKX{btA8fwFQ(9K6WYezQYL^g9u66rs& z)y<<~of#-AA9`7B&rHM=}Bvuv4ev0!T zzb0cL^XW?r8HxYWOi`b|Yj2eBtJUEJ4bFpBvAto9aBko2xDN5lcr&a;TM|G*3j6N; zr(FYnd{Mn$Sk8(=ReC=lR#0t3HOZAYfnf>52$aTzxF$h?d_>L-j~Uvr{^SUDh(d5*7N(kfi3)|QKsO4RU@6b7Wk2K78a^jA|q&s1Sw#M>t@wJ*X8efd27saBM{CMWqJdy7(gr)|Eh+p z>lGGHm{MhVv3H!7+J@%%B1;=c-49~;ae3jw+NfRR2~_#Dam}`lFJ>Wm=k{`%&(ui5 zbz7+{sstn@?03U5#^MStZH2DNDa&)@RLapsqFvF*1f zO1dYW63;po*eLK){8n2M>MVowS5}Y2CR5Et)(+L)Z(zE_)=oc=^(T1!z#3y|PGQo* zRFbCx;cU+69UN2VFY;5yJ+jc6szYKzR<^D{IR`RUsE^qHi{L!AY`;e zEr&8$6hp$wVZDhV@2+Oknoo{SLn2(B`s^HV%t$DMUlE#`~a_L!>Kli&n{5k7ZLJUN{nn9y;(bA5$QGEi`!O?II( ziQ$0jiVQVaPCxr@ z!#>VZ0f*Z93!@@-=)1`9t02jx1GWsb(Z?O?`TLjb&nqk2=#(04Rl0Mz&qZm(wx1-<^l7kwv|X|MkAbT5h(>;ZXjbIo3vs`R(Hp$aP%t}l%_r%fm_ zDkkWp)(3e$HjAY-E@~Ss1hQX8eIq3B#dTR`gwLva7MnHO9zm!iN#|aBC%;&>MV8mi z9z0Jm_SJ!5|MyCJF@%=cZ5U%JeKT<)nTTRi0x%bbroAyI;1eHK)x%NjPt%Lpt+Tj+eyZ3(H{>W&f!bXZAUzg5`Muzr!+<|o<>g6E&pu)IV@N9!&MMO zrLbf{>w6~sy0C6JO9xVbk*sAl!PWgQ_=j1Zo8WknZ9-W2T_9-}>prs}-H5nL1aOOM zZDHxgTfRV;Z0b>!#}K$t!Xj{MbDS){F!cw2=AuJ71eX-E_sfg;qum1Pcbt73CTUL0$aF&^QS>Fs+dhf3FA<5 zIiQT|`>OZt8~7aO`aOlFqIVs%gMaf;#L<m+ldfc!spO9xxg#hb6|<2@8AK5n z2A{9`#}j%wDKp2wkYsKxo~k)(o*%a-u${ZmVAY=M08iJbTfFn1DbO$4yMCzm0_}GJ zv=R7rL$LFG(Eg0M&wNDegAl#@LbzKQ$qprXj|J)dXojewH^+9^FR$pE{fgJjbFgP2 zHiL#_hLsNr3;LZZ^!yPEek9PQIb48_6eUbll$6m_KfR#?z>lx>((vQCq|iRys+`!i zO#(GI=tpU-NBnLt5sNI z5jyFa%Z2~}*;L`j8O;{I55nLr@myiXQF8R{Dblg?CHI+!N{Z74;X_-))~p3cL#(jm zDmyi&8)%GXHgF|w)v~r<6`K864)Z&_luk|QrmdtszhC;*Ubv3Xd6}MADF^y9tu zZdgW@6kyo|N4T83X7-##7Ar6^KOVn86bwp%z$Q6Lu0j}zVQ&vhdDVjQadatmV9Sx%03Wj!-ZYFgollwl6$g+_0 z@JA#^tOTpT@TnRN&Z03i*@vKMsQZM!Y2sEtqqI`>5e3Qh>@ur(N$0Bak*Dl&Fc0bu zpFO)PT@HegBD-Iv>5nryDzfqVzb*IVGb9Uieq&(a`c4q5HajfT4Kv^VdbW~BS<)iO z0oO1KHSYt0oX;`4$e{V@^<=%t+8R(_nX%YU=&oO8_Ul}K7bpOK5((T-cttFdTHJ#R z_=8#0z^!p)Afv>^)Z)=VyX(4iS^N0%RSxI+=+vK*=)u{iAKVQ(WV(beomwOyr1=g$ zf#!VVir7DMrFERPfOBzJ#j zgN7vU**P75dzs5_*28*QM>N zdAxeVm_k>TS9h9l+x@71-fQkYeEF?^Cn)248~aJ7-1W-Y&Pp0DDS@X|?eh6&)8$Y* z2drI*ejF=$WmVo5OlC4b=a$05Ry`M6VOzXhT{_+&9mpy-sb3jdmm~1FfLr z;%@fClgUvx8wIyh;SJ!lu&sUTTn`(`4Y;AHLW{tPDmuiw1jl4>UwD>(rNGBsY{`1Y znt3|ScmUn5FM+6jHg&_rT-E!g_}G^c)LNZP(`33cPODOM zu+~8$K&PCFU9PaGl|);BWI0#%3eDB&m0*gaBLC|-gpju~Gpn{Ufo%+%_ z@xnJmhSfW9Ld|7_bj+H$G}Q_4d;&f;d=sNLqR=T_wx?u{G+$6zt!1g4$CNrb`C z$NcP1bpN{}l|^0B4?dG=2Tl_AW3QNSIN@1pEUF+P)~#c9GxLL4`KXLeNhr?Cq0k@= z%xFlfgs_E&!t5`%^~@{n@p9)!T?|v`bkkijI!@rawwS#;_%h5~cSj{;cF0?!1eaX_ zCg6IyD@=>DfLYka%%4cb7+|7jc?c{-Sy4^@vp10X;11chH0m6%wg)<)u|__RuDowl zT&!bj7MduCUH<7Wc)*&(c2IO~Ev(xWhkuhC1 z;F`E0GJg5FAaI#V>wGko)@I?MKhzApcLVYpGF3*3sAw2^{24Cx&L{C~PXB-^i^4NC z1xC~H?GEGQPE)he?G61jXzOkl+^(}0wqVQI7+Tr{tRl?vhYU+K9-8yh$_M$?>)K3I z+<E1{xSOo4p&PrAxkC{}?n7$wQU4X6^*FOH0xW_&$Z!VkpzjA}`6v&zy?`W{Q zcE%Lwk+&|8j3DL_9eDr5N_`!dCrn{dx3}i@Z8^c8r?v`$qn|CL6H>@A^78Hw^Nz!s z;Yc#U*%sxIi^=w};dL zW_rfpLJiDTG}ERnKo)*1F-?^R4%3Y3Z0GoeX&qNhN`Cuem;M1T?f-bj1VMDZf?Xyj($ZVsV zWBzL_GYBF3aZuDxw1A-dS?+jdBCwK&8D?6X=Dc|=HMhw194EgGOQ z^g-x5j0#nSv>EhBOz-DmN@BLA-mdNyt(%CWL$2sKeF9y+kNZHT_S8`$`ks6KExQ6c z;_r_8^1ZZAJc*{fFTL9sE8$VhR(mX$oeg+q_0^FkkDS0qHQmiXST81wAW8&crYfg8 z&s`R~%w}*gExW_zrI<(akhGJO?n++nhHFCDigpH2Fiu(6#f{pb&J_{!vFM(O5!P3% zzVUrdpjXp&M7tfR$#uoE1Pr7J$s~)X&zk3y=)<1eQ}=({fUzP*#SQ&)xtko|eQ4DG z?nO#INr*Bk_Wt}+L7Bd~wymcqJ?E_5IpW!_wT+orYcFQ3$QaD8bj3ZMBmQG|U{!yf zgeuKuVL$nfj!#{peo!N%ta%4m^>RC?P)q*s^VZWC3Zd4GY8(kTd@i(yP(f-iCaG$% zO>aqcuSlpJTp_fxIeSOOm$kC_%n361=Tj~pCio{UURZ4gz?!lFPt%%N+noG?cpFGW z$XSxKt-zODFG1i!Mi?xw1bPt{cT877dpxCbnn)inq`YKJW3t#PhHxJ#h~#vUw4%ZO z3@m~zsf$85gvRLkLOX^3dkM9StOlIN|0g^BjaKf;WyhZtUO5~w?%KU>_TTHx`3&3* zaxfBo58S`EYd9e0I><1$X(fjnt{-ROBtsFeoZLB~LH?8Z1o~3s+vh!|2jTao7aj7? zj;X?^AN`biCQjNe{$=eaNf2+`Yn#h*G{<4J3O3_cPy!WB7-q*AJ*g*b)CrXc&wE0j=I z9xyU>ID_1EsiA0?U#fAEPXafM^IiheBKo>**B15)7O=2E7%{baYxQ+5*m&n`A?uL7 z{&aA0ixN^(V#OALar6?!xI=X??`oiIwQ-w%X$Y^AP*^gq2uC=5#5g?7B8OB=6N!x? zC&OwcYk)tG@Z*ck@925+_yfPcoAJScj#UJ@bee(}0n+{Gq`6 zzXTP!{pRKh9#8r_LDl9$7bT&o5-l6paiDtGJS}Qq#h>d(w@ezVdwb@ps}Zno4ERR$ zvR~^EFW^?j(to^-n58@-X>a6|1s0|uZnjg<8)^)N9nvRI7j1sIx;pi@yxhlblX)FH zxXfa#n@c~rXoE&ZbwLV?H~oXI3?Hh^E;*VjBnV1gU(;SI{=2Xi;>DRwu+S9BhTro$ z&y*emUs^BL$F)UX_;V}?if+y`o4BxME~w;7e`42;2NN7uIBed)FP@bgCjGS}maozf zADuc-IV`ZDq1C0&voXbbFfgU{5ZMdh7VP!@=EtM&%c|(Kp7l07qz&a%4D@udiKbDV zfRc~M)FO*`1Dt5Ec=K3hr32|(X0#I|opg+D>|Z$r?^d+YE}g92-f3sH06I)R+9+V2 zA;H%dbsHDLeV@VOkmZ!K_wSP_4GuG45CQo3UF7q$d$)e`9T%CA7zLhC?qtPdR%<1y z(0cf%mZy)3bj|R0HeKo0-0#|{2J=Y}#HkBOd&S91!X;eKM&=1l$F~t`d-r;Xu0Tk_ zTZrud4a0!a!PipGeEMhTXAV(iKw)%sz*_j~gd+1m`hdo!daEHN@T;NsfrmqO^ZSTJ zCX!>cX&;VR$&z5ETx^zo+L!0C!2A6#&o(v+%GGa*+X9yE>%Lti^2$qzOf;byniUp@ zN_O&aiGkkIT|&GnqM%Kb(ryz0b@=fSwSVG+WcZ4gK?#3!sIpUfN4~e!)+r>{kB2*w z5QPH@g%vZ*3&T+*6#;2<>N-k`P>25zZ$=zdUZW9gN|l|MF!AQ zf5Bmb^##r#NAR|*RFbY`%5c$=9<%im&E>%|WPzmR=t||v8OjRY^IE--!)L6cfjSD? zdptmAGW_!OyNa0cq4jd${Z~SdFz0WTc*I%i7s<`eps}xUtF`y&K1rj+1OpD=Groy( zJ`q*@!|^~RRohbp23e+&WWisXPoSPK*u|NvwjR8{(w*X@xU!iQ~#aFD| z-0fZiMb$^h)Uetr(;@Z^STIJZrg; zoNbG)e5J4k-B*Rtyat=S9<4XUSuU;_@sfjnr3X4vevIebqX=R)a6+<=?RxF3%s~>s zY0yAisv-$W%pi|JR&eDO7mn5=$|*3c+Q$R6=pokWu+8pvtF~3OcZA(JoORszrT=n~ zPcN~jSg>(L?9SI%7;Z5w!d9^M?4C&1*sGWS11tOO6Vb2TmLOmtd4*h$x6HXVjrO$m zAc<(%a0{yHb@5Kh#4R~O-X{MLI^MERqm+(aoMgXgg7)EY^jEgOP2Ww27cay>p*AkG zIJ;xk^vV-q8MJNV3e)kNK;`_1`?A9@FcqM)-Z2RNQB9s#6kmiTuriU@eC1t>(2LaT zs1}uZ78Ka@4nw}Q+J+P2=NzpC1KDfbGWPnoBy6180Ldg2mhg-OaPM} zkQ2sXp!O9kZ`j8K;ZN3w=9pm(a+7NoBwrzCYNm2Gzbax0R^vmnD4M>k^SnD|a>;>F zipq^bEvN8qsmkd*T4LTxra0qa^i#TX-9@e(*YgW2rV18}c_CypUY(7WiP1*E;Cq^N zDnzbTJ2LEH7bVwbDG@*ykGYL}O7X&tQ*4|Smh9NOgBm|Od6hfIjATsTF+>ou_7(6l zbx_?hkTR(3sohJSo7-c3J#jVI>>9Rgi_0IlXQ|0!=7&DD}u+ zEV&O_6eZUkMrK8wdSsEl`A>9 zUp|Ts9!tQ?R~Q*hf1<8!tuKy?gX?k<{jLb#5rd({g5;P3G@ueyps1zcn$9~Hz57ay zosfDm&21T=i6c_Ro{2-#>j@bu;cLd<753L_(k zi)Bz2IqAD-o&oH#hL}S2H#R2*S?~fTijJvGv4N6 zt`9KpHZhz-As1Cz7Ju-iKl}FD`RhMjOwJqn`l^iC`M9CJLg)BS#*H4@la3Ksq3@#3 z3Pa<+t}Xl#&;>$=4UdZR$vFS~iB@YJC0*xAb(?M4*%_Z!GWp}AgOH2+tv_{+m-f{Q zCuA=wpL6IPL4!BH+Wgq$Dv7OWwRT&Itn0L=fV3eR^Ii8I1fcIuqG?d$k#?uy`Q^6R z6PcHX*>f+;j!ytpIsI@qhJa;$*xC2bIDwklB~&eR9`e5YMZu_{nf*c@*xK^-rWylX zf#$T7n!V3_)ZCEjOXC|S#c})t!Tt9?7*tDaQcAb?+a55`5V58pcYeB%Lvct*G=4M)t4kwkTl4kcM6^LGCl$!p~~5JxpY`7-XAQjOY1w(keD(P9(o%EL}a>i`qPK2`r|Y6kfs{eyjz(O9lvak1rKn z{jrI2smJU24d=U%)B@XH+rnd8l29es9G?NT_Y3uG_{tW7)u-?8rZqwUx86c{|KLvs z$2Wd`KuRk%g_JVGCvqYRAMJg9eaydWt|<1MNjb(k1>gNdMM&H#$@wK?RisyFz+Cr{ z+&KIp!(dca%wIdjWHrW9iPIq}iL^CVxM?JliEMWlqcoXQ9~!Gv5?YeyDytv}(c@pe z?yTYr20>=ij2cy`!mxP`a|l$@bu;4?6z-z&Ezp9{I(L9p7FueC{lD<}Z!AOEnV);N zva&dtL*^0#nsS6!Y{gea_E7|h^$J5dg8Vv$jLTe;@j09fc6MU?FYUXl;Y{P zt(zxu(F0edudWf*7yA9$BPt?^UbIzH8Y}BWyk9aWS_4L+>f96nzcPj{M9eN8yeyK8 z>59A@&a&WGj0nTGpr#D`Pz^)|Z7$gjce9wM=6<3YCuj z$PtkR!i#kp&Zh=yn~Ceficgq1x`16)yq5jnv%q`~-x9F*VgY->al>?XfhPOX?dl-l zcPi@==q2vq1MPl6XpA^>oRt}K9x7j4vU@ep?Fnw`QFo&y{4#8uxvI9NLrKTZneFKI zVBgF$d%gHJv)gQ+=qV6t?$G&={tT;V^#K2PXK_8SjfetU*!tdbnF1=;#y8OFl zTy<%eA<$aYJVVK7?8y7IrtNJ6o?caOdc5MTFT!_j$e*WrX%2E939kO8FeGR%(*h~3 zpyDrAcI!tPJ;lfJ>KnCeSt}T|NnDzbTB@!}(oX@<% zLS?@E;8tl+hZJroKA94LKi0fRV!40EQ!F)`AQt1bo%@~$D~=JZ6O2*3%M zxw<9^LLEdwuA`7Xz)%~N(-VPQh$uNH**N-GiCmgB?wXBh*eC;M_Ue0~E*{)yLuH1q zaw~t#v!4yHX#|8)!ykm8oUUL5oZiA87+{9t&2J&Pvcqf8 zC+XjPCqZ`Owkq+)($-3u78OgBe5P?l&TYIeYMjkiXmyFrp87n>E0cOix9q1%^oB37 zNrz+Q;)ULNJE=^G_nMZO607XI3Mf8)PuH?*u^wltxE9gHXr?MfJgB%8-ljbKe+$>w zJ6ViFDErxDal>i}#t($qySRj8-KZ}2fRbu-keOdo1*Le&{2v@R5QlYN2UiB^%0=GX z#%}f+r0-RV$QX4QB?&u@BXd3rU_u|zl;z%AJ%ZsI8ca8W_>EN`%@Gb5qn4e2M7ko; z+2RxGEpMfd{!XaehUa$qk_H_dx-+n+nS8h#-#h^&;(t=$_7qk-B-novbj^`NvF=!B}l0Khdpw_c=T)X3GF zfL<8gx!J5Msm{%wwMhL#_Q){pC39LuL9uP6%_OVy`$je2lHcIRgBzH50^Y2XUHeIO z-_oz#E`w?Z3!FX}os5JB%?a(mrkowGu(xC1O-y4pIxG+OIl-IiP{VG%CW)J#l ztA1jwS!GPRUS7f~I)9uMgGd3a7isS-)h~T(zSCOvs4(u)^l9@@&N%jd>H_{);l?kk+<-}5`%WUNiWeZ6 zW2kdhPo|`=<>6K~m?s9ot{{+ONti+KdQZ>SYy=duqT>w3*_a$EWFa|+QKE3*3ApqY z`p;sDHQPZbi|I;04$vc?y&#?F)e`kY3m8=Qo08LNRxJh>z0w3J@$mQjt~IO8?A0P8 z(yP_uofmc~*kN>Ob=!DwV=-o-`Zqj0Cy;rX?sdLW#HMC#ABq@PN5+jTVA6FB(Q@^-Io(mW#xwLJ*t;pM7= z>h|AhUe+%a?3L-pf|6iDf5rwg3XU`lQwi!M?*^d`O;_!kXDLmzIG z{*fCK$n36FP%A2Zt|91aY}+RP>1b@=VvWou@YtHIH2`Nc99qFxC7ghA!P;Yrnqb|`hVg9!z1RwJy$5DQAM>l(DLEerHe3u9oFXt<`CqGuG9V5pooq?1M z@K-o)FGg=BidaQ;vCEGSC{(f~T98=!j@$Ux1WPvhJbtzYYL+pmiQFlR5RK}GyFCKO zwT;o^P~h{-%X2H+k0~T~#r7#wWrwo6boK@+z^te}SY%uJLPTkyti)H43{`}(fDJ+K zYP#A*DCDpBG~K7p)KRJ;dfX+R@k|4m_kS44m~UOmqQKAy?qqhl=X!( zEu6Wy-rOsm%9uZ<+6m29mJ$^ia&oe2=hfLt2LCM`-tttkm`ku{7wOXD{iS?sX^L&OI*o(>T zxLME^2hPd-OslgjXgb#`8-G|`$ zrgOhPAZh-Zeov$l@VOv{XBE1xbKQcRr}aF7=MZ6<^%Oz&nsLC;R-$HIBpY2*;{|tB zTJU2j z#Zn7-GqGu*NQ9J=Cx;E|)X5^(l%D&$YWY(5UhvpYtCo#mqAFP`=*q%OMuE^+f4*!Q zB!1O8-hj$3Qy*(S*+^{KbXPb#}c-qxq zNppbU>J4HFd3vS(^l&cv8#wjJr}6#J8J-;0yBPb65)AGeS>7PyQI#f-WPPv(-4$h8 zdb`qyQUSxn{5$}qOeaZ2aFs$tl|aX}&!3`iVV~i4&7!-@;OpGv@T&_K1j^W=V@*rb z$b|C$0g(SekZ+@KM?|A`bqy|I6F&cuV&LQ|*{<1Uvf5PP7g@0V#wU`f{fvQ*<>|FC zm<6LZME+PLQe5DZ3a*A@Lu?hoE$j3kz)+TCG*V5i#;*HPC3qUtL}U^xPuYa<>vflm&Rsh!j*wZ`J?+3ai=?L{~y0X*$*;ZF;S; z?I|`Nq(8)j8F29xIihHqbttPcA!klTr+cJJw*sJI!C)OeNr*pr! zbN2UlQL3|%5kR4QrwEBZ4A#yu&jJcE#UsA>s{t8+_)HuwZZ5NtS(W2vw9-tnm-K+= zu8ZCv9^b{lL%*+%QCuPZj+H7adjZvv#I=|V7xUYRp?QhDV}GZWaE7IKP)lcD1MZTk zgMu`mvZ_2$0)GnLn%;3$MQItn-4FyTl`HgJic`=t zxb=2^@-6uM%*zARqcxwJGI2_SE(fm!Ad?A&BI3H!YeTi{_ReTcJ2KU4Fl5(KBf7n; z%F?H!=C%(0WWbl?SPO-0b6q39V$o(LVH2i*-}7%{7jk0EZq6fKckJmt%1YI~xMvDu+)=7@-(BL8#LmX*5npN@f{)$23O zFnl{$gv`|fuSz94E^)a?F6G2=Fp zslt8>2&r3s{U#9kXR-BKPN8#|y!4;$a*Mc*T>_zt`D&K2<@%ASKJP{5nmRbIxHls6&;C<>Xosx@Q7y46F z;9)cWhqTX9Flv^Yh{OC+(O3liq0K2hXD9&532tZTNSwF{E}}n)M9h5{TjQc_R7WDy zbOYaup4S$v(?~N^MWdPZlh;d=WCAiy0TSg&HIe5(P&6=4`xir8tP|MxQ@5w<|!u7KfS;XP$L43Br?u2HxkaMw!`PmVOd zKV<{Y`MLzTcS7EB+bCB)nG`qF%Zhy5K08S&JooDcs1q{USRT>JE9Kss*pZ(`u0?B< zWk*4whrhC2A{T(z<6Pob5pR4!BWAn(7#8q}3w6K#Rv_iTVm_%@EUaam_c_wHlvTy=pA%V+^lP0xV>;_95*0Q$0T7FTcbr=(TH$@ zoFZ54EcwFWyzlE)j0n>R{_A(#Kbg(4JCND<)7F(+KnmSraIwy~=)~UL(O0P~Dq};h ztz&1(fgV!a*z!%My57?dJD{djOvP>o94owlE_g-m*CEa9ad|hnHK*Jyt88GHj7Yww z14=28EiAq6f;*}O=7$KIB+qXEl-B_cVCyMHC3hilOyuQ$wfUD8WFLnteJxzMmkqN> zlpphATX^0o?^!d&x%?M?a?&E#Avefbd`Kx_3u02_Wy`unrQeeVWUK4WwG(6p+)d-UfE!S{w0z3a<7u= z(mREK9i#^QMhgfOOoRG~BIYz-kh#;Bflb(jjfZeWSqQ=683gryr z>?Sg&zTZd-j1{5L|1H0k*FM?_qJcS?c4YL`i0as2xlK zR36*uJ4t_gJ&8jjhOQY0-_B$cMzdAX$s3J8BNj*K`upO=nPxvQ{9`nC6 z8jnZI>SCgMuX9n++jb%>O-}g|+iwXdR$>Zq*y*ZaL-o1Ck?S1IqytcEvc>EWj3{Z3 zQSxbvNM>VJh8%&3Vtx>dK08_!TFnxHUy+xfaer5tq7O_(u)(!!!w<6j{&0lk;fN>%2I{v1Ak z(r;uNE0yHyz0dD>L<%nar+)9(y}a&-1pV%6Q)wcLiaPnctx^_!c=-IWzYE$lHu9O+ zlSqo~B+EAZR9q!iBKQ1ny2uT98RbnIqk9>`{nITcNkONZ)8}MiKD)R2+ zW3Vm44f+EXAfqkC+#JOr49LflI)WOZVDi9oKVWoU3c}gvU9xVabno81OUh^8FYev7 zbbsk%B9zS=UClbxuR2N1*}R1sn!%Duk@cW+eA(}Z-zq5e(OOv_AF+Pr* zroh7GTf!sOb@i$5-EjERHy-xE?s=~LrHB<#tRa7TX!_h9_EK~gj(=nN4I%wg4W%Ilux z-KX-9UJ(u~08{T*4bY;Eu!T;m-VY4{x4I*A>Xrrtg(AeH_6u7^(2fg$N$hF*#$Tbe z2+8bF5Y;fw%puWYD~*}IV$bDcnFEvM5et6MGGtdjZ~Y@-^LuG=EeYNDoi>sqGv;h5 z%qLZg!@Kv%Aa#-Wp|}YLxs_A-NPG$C=_d-T zs9C%&vL0%RmC$%?=nIw1X2z8_qoyO<1HJLbp9!Q-IZ=zyUD&wfpqYyQQ7D{?TH#Yh zIzbou&9U#F?ISYkeOXdk1NAUb%cU1lh}aI~!KRu0>^CCy^mW0ei=?w4(I8mB{?sFW zt8phyrl1HY?GZ$gF92Dg2-!x||ok+?R_|%a+ z0p36^f3F(vzj%9^+ahM`oE{gPZgLgwG2|)GD9WyVU;6C&&vQ)qRQO~rP4ri0Dm~&B zud!F4`7D!5!JJ_w3R(T%gf3xOxm%6LA+BHil0nZD;<3elgEItGH9QAY?@tBkorjKY zBIX(LaaFLtLz?S&DqlviFRN?&u0^5iJ9lRdvTm1Yvy%)}zsTV@)u(Y){2H$CXhSVr z#9`?D1Q~^w58?~9ubc>j_F^mLUJh=39i!E}4sf<92M1`gvF>7yf6lPSRN!#!9M8+# zez`%nRHjS6Ysz(wkVQk|2dhhq?7*iM{usY2{))a{#U+h*p{MI^v^Ng>E{WvCOn(gt+5Alww9E-K zj)(g4i8gm$P2pc+=xa$FM+_BOrfL0m$*|q6YE-T&^{~HLSjmvRo#%{oall0kuq?60 zh4#COE9ybWeJtm{I}GrQldl4I&5w_u`ut)41VcGvvL)SucEyd8Z#o(JpKJ`EpEkB& zlb(xKwu6(fE>ToT(bEo@Y6G@{&jX+;c(CwKpYw%f)&pj>=li<5&)JrP(4)R~3Tx#w z-u4Ff8d};55IekWcfBMXE*@yKVOeCD$D%@q{JpG zDxJNaPr(mTo46|33jL6II9;o#($Z>SWdW^;J$!_%KGVINBNyNV+V-J&Hir-hh_Wyd zFgsp^5M{%f2@w&6@VgOWD7n@Gi4W1;;JWDizlZ9=!bQ9~kLJ>}hZeWR6vpUQzBC6j zYS!h`YiG0)mM$+}P?NZWFax{pdqmLqoxi8WrFWsItO_*O?2Hrl3gPw&g|@ zjYt8Ud8JOKqF$t|&fWbi>c4VdV4Cn_)LFVP@W>Yffb9xmWX*p5!`|K%0Yb>2 zYZ(_Mo_X*2wws2h7cx$ZfD02EET^MG#UQ~b z)x}vDqM`K}H&o(%aC`W|_ncBd3m;OF80c|AYfB(!2msjRSeAYA_6)=c3>ytPum5&_ z7w$<<&ucUQ`G|@?7=!)GYwygg2p+kC4CDp|!K_HJDy`1F!=(iFf7HrQJ1MJ4e=yYN zFE71UTNrmNG!?Vko~mZgmd`y7>(%40nU|!Oc+P%!T39T?VQ4X&Fa>n)We8_S;HVgk z%k3#pJNGVhNFyqYWC_CkgfB&20k)j$7o>6n?e-tg(s3F4sO4JkYd7)%PzT%gB7#J7 z+6dGbs(os_3~6{s568$PGmMX$saU*p%?${MrG7NmzEB~tTny` zWet+j{LUmi-cmuUlDReC@1Cr0Fh4(%niDb_D8@yV>q5nl=}tg0n=pfqm6pM@XVFJ6 zt$Iqu*;o&;H&>6K-d)#@ujsL)Q~=^IH2qy`!q@z#)}O!m{8_ z45>eWkbAQSQUSV%OC~U>&hjWQzb@fs=sv9Cvks=ukxAkmi8o1zFz_)o3Zv%Wq)?sW z6>MK!=!mtKM>T65+2yyb>DkP*^Hf%Fb^MxD-u&TLGUQrSaD`OLJ}A-evCqJ#o3>gB zo3xXoJn`AeCXtl;wjbE5;oq`hrDqVmy%B?iL;4D6g_*(zwFQ&qyV%PR8Xtwo2Q_w_ z$^`HS@{?Wo)G=uS?_#G5EBlSi_-L$fh|>mj4djk9SB#Ugpx;?&aI-2yo@up52YFSS zq|6uEeR*P2ylQ2R8IHw6HycC4VEsKD&L2Md2JiQv%oDJMK}ie_edUY1y$MZ75dIJz zR(n3fdJL?vI6q-SU3~Y*-zG#U20-1}%O{Wa_lUsVvzFmxnT14Np1GD{QGBA+^m;|?OVJQjt`&lEY44Y(T?c9D{>*KPfBAU}?M3Jb zG$vHAtS#4P6ySe$$Fnmu>?gh0ht#c4dWePq)U+#~=xL}_PLk89RH;tx1wp!`Cbw}Y z$|r%*@xGQZT4)UIZiqJ#7)5l+-`FbFUgsefmt)4y=FMnDvQ9+NZ11#oevBV|LpPfN z_I8r^c%eM_Ae77hVy|nZGcv}ewb#T-mDl+Psm`gUTk z)n`q92xqA@qs8d8`p22d@oU)!pytdn_PL=C4))~4BcxOyP#SL#;(;3YbLu}^9% znlbi-b;f%gy}Fe1bJD!=G+uJi>u&wk{y+L+g>L`_kMpBIoc-mzL!N1m9<3vDjHN*yxUq<6Uw!8V}Lk;m&MRXI3)EB z{z+IAEDRCV6lCrLgS4pOuSR|Tt)ik=`@X6}-QnVW6UWu&pl3b-{l`b*(#TcSmn>NN$%q(67abpRI)9hIiJ_O!Q$83=4hs@y{$)ELJc zV7i=Z(5v#A+-5mL!+L_|JLT9P)GiOl^Mp zci2+X}&FJWCWW5}_ky!}8>p81tSXN_|vWsPKaGyF)|X(YNvztxqx zEAQayc$xXK9G|-O-I=4<4&l?R9IwIP0>KM!lr!E$6AmMysnGy57qk)gFedaB5r%(= zzfJnBR{P?>684)V{fley*A$_OeB*cSZC20`AAP#dgjJ zP=lD>2DCT88ZDA(&;a*F&fo!?0!NUb$k(*;uZ>lAP)Y8BdD>QeNA||&hkGzE5!IUh zO==SBBI4hSP6@SIT-5p(I$EB$XLtS@@abk3`^1ndF_KwE{c=w4REnR%TESF&u7ow~ z-3aY6H)V)nsG|C2?$&({Ip-yeNggOna@@$*&|*Gf=)BswFSM)i!PG$zU;$Z>?U$=e zpEdDIY%bFuX}1nM^3IktD|12w;nov+>!A5IDOy+RL=DwRO`R$^bqEI9#;i>5&J}#L z@aZn{62g8hqa4`)HK!fh-tQ1B=oM+{=kwe{`2&v)9+F+l;x3{)6A+#?2Zo(z^~MIcdAAE&~5&|Zr@g+^RkhuBR`4F zz!%*YZp)>e(rrwdsP-qd=%}qx@0g`}B^1&S=Rs{VIXY!e73t`*YWM~`kbAG+y`^xt zH`KrjX-B%JwP?>54)&422Ne!9%^yQAaSf6eWV(Vw-$ig|iZLw_-IGXrrU7n}J6(|O`&mrh5%U0dYx}Ls`(4jt?t_Q&PkvM#EE8_33@+Pn~7#yCy=C zXJVLYU?}v!Azy_%({Ym-LRqS35_j+R=LUwP$ycBrL6vb;B-U6SQ1}y?AS0pQx^==*NM>H#KsM;VHbI!t!p^eOjE%A zFD&~B6_ib+T2C%iSp0OFtR;H(c>dG;ms%E765_$bA4U@=nv( z%+6!1np0NsuA2I2@2_qq0o&YdV%0imikSW*uodFZnRcSZ0I-GANBlPe9K#(6xOaMx zJ}NOqNFxo_d^jQxMU1X&vFAdzKM2S^atE0DV}kO-cX39F@!6MeEVE~@CV|;{ViV}! zUJZnnt)ACdH<1HP;MJ>djb?R80+`n7&Fn0XVTyq4kt+%F4_xV=kIuc4XPphqR^!vb zYW!nJ_u@9W@K0kL2d7x}r$-E+Qf?2HIz!cBS0@BpoL<=T*cb+a7uTv6S39Vw#lmvU zC(k#|OGhdfXY!ihasbtl5!oV<8_JL2I@Hq015hVcK-i`H0I@q95jErhsxI&3Ew{v{--$GX^rkqr8NH{p2&UoS@=Trkuwq# zQb7uqg9$a$8d8pE)BshKD*ik=Cj<}UG>6v=3s1c!O%+EDrIWiynDgf4OkBBr$#h?x z2;ZR7od=Hn;JwP)`Ie(Wl(Ncj^g1MPlis1dLW1Y{hKB@tgf6I4F!ehW#4__sa0TMU_`1j3bX;BZhHUBb(zTqWsm@qHKzL7g_4`CjnY`Un6sKXz#cG5h zV=+3Qb?7yBnN;m-LOUwrlclAsM3T14Z_bFj)QCzqV{9t~b(0X9CaGd+U-6Bk{Drch z(z+JTPGRonqAn~7&s3Y15nq1!OCN>A_16B+N|^jxuMI7ex|Z)@n^HS8EioE4eq{54 zOO27`NDn^DXUs4>P}*YwLe!(=(AQs07qS^YHTMFt(TY0&6(&agHvk*1pUnT!0#yF% zmwxx6AQ0x)n~S5DuY)qduebJW6Iw9 z1m_jBsQ5`f2cgKPrwSxVhE1iBKHU{Z=#=4z8Z+f_G>&#^Z%*_H(b9RW<^v;y^FEL< ztZzEu1?A>PR?ssag~W>;!n*(llG)akCy&=?pue2F-RRO5Q~r5s%x1~G`%SelvuR?> zxSj%C&Wl}8;tqRRU|?;4*y3E&5PkeMF5a=AC!5wL_w0-uXbn0rD?4Cydxr6oUNp_| z%HbUA@>z7tYQX+7{KbcZDe6KRifpNH_tq=-uihpd$3+dvaWgJ+A^uXGnTk`wrA`Q) z@tXJ@%KzQNUpW7v-}SKU$)+SHmG74@U0uH* z`Q;NSFaAQybVFWHp$tY;y41YLV|N>zCVp$C@)y!_I?~D%`%Pjy+lGLr$%>=}86bML zv!dK<)NFk_NG9AQKV#Mgjr2p13iwrP-r47ZhJlpXtd)XUS;;Ux@6LinHHs6%lvuM0d zphP7MNi#GV_%w&-VzsvcHVXP%5YG0w@AI@*S5#d}2v&?L-x>`_SjJdhO^z5`$^}ew z;4@gOC$Mjnk1`e?LCAqwLru_10ub82-VWMHb2T<{uYN5$za4hNUord}sQfQJ4^>K# z(QfAT^DnCZc3hK7dqpPM$K`$Ojl^oOSg&ezD_326o^;Z(F%exT4lP!j0OIjUzz#`M zD)DX>;yqjXFWtutO6AG)p*e#qAS-(>#C=sef3vA<-X`e;3=qcvY7cCSFzBz=>mqe( z3tQ@KA2f4Ax{SW9U!f+LVGO!q+vsMj%)%~I?#(Z|RLn4KYF5eTexc1DvhqF{HzusY zh^L|1;W>){DgL02{%=Axtf@cLqUP^1=5(F2jUO0vGw{`rMvZ?}hakkJ#d-MI7LnN6pknw+t z%=7=+l2H$7^2}M8=nfykM9uT*5;oRRv2=#vH*ZfHa;kCwENl6gQR?pc{1MTCHIlQ5 ziOO8KWh%m@A*fq|mTl>amn*7DlcdXg!ud4DAdmUtU)imr>+-LZbG|9Gtgy3;v;vLs zZw#VLIq5nDR+g{w0i3xvYlJfOV0z6dw|jwFTlKFXA0`;dOC!618b5Q>qTf37?xp5U zi6*jPF|D8!V9V-B75@K|g$7C5jCK6)p%Z7)6kE9m@4Z}e@dR=1rrU5?9Jj{fc zutS)XYM-~YF)ILKz~B3XnAXAx)moB%?QIo!D_npm%#v0Ps1>ovKyLTW13o9DY&o|b zwfY`gIThR%Dmh3F)9UD7HC_>zKt!XGpSvns_G-ez=AoIdmSDS@?-TwRMbt)ncLrB; z;2T0`uC?<1_6dyHvKsQR7YiW0EM%^M&&tfOwu18asPXqe-ewFxvgDlk9{J+9jkK;= z!{n*n)YZWeY2O=Qq}iK7VBbED=k`VV#i58GCDP*6F1$YHGVjg1d~qTLSK!%RE&o@bRTo zB)wTMUC8PZNQWInw#dd?h)3QC5S%}*7>fU@Aq(w6ng=gI(w}(KA&Cbn{qg#?dzU625|sl znf^+{mL{9!NfmpejS<2PxH~vi%tsYID#Fd6zeY`~<+qw8z^r%qfwI?3TbE~8Rf5q& zW&;eoX2gb9hx3yD_;Nm!QCZuj?p%(6V^mzd`XeB}`a&WC$c)E3v54SD=PwkYmS)XG zT&1=$KRh}}qi6i;5lY1P^JhW>_kau;{yA)%qAUn=e7cIfIdA)2ZVVWQLEy2JrB#Vv+{=#aMV z$?zhc*j~yjuSZ~n2yz>@Vo3XlWS?$;izloYrYvE~thu$=55A~y9z@ucfO21)>Np^s zT9~}UI-w> z{c-zoz3;Ll8t)-OqaO3&f7+)Fk zS5wiji8}tHnG{VvW+Q`7AIM*}I1@fQ7M#-vxA}IhhGa$_VlW@ zP~o8L&r!1Zg)gVRIS&4@p!t1}_NqYZ?^pXm(lilDR75#*L|_x0KJ(WG^9Q*Ub-G=6 zkcp0mO2Ja1+v_Co1`-Jx@lR5R@dlmg`p<#fY{mzh4*6p13iI`C zD~}Rn!(8M#0b9hngL~2dH4{F(46;FNV`+0iNq*YbQ@~(V@Dg+^z)ed>DO8{8EQtxj zsUYjUHx5`vG@(3ec8iyj@$c;rTR++e&05?SAX}$ zsl6ZpU0#5&&2*bva)_6C+RaXLP}Y<|pnzFH_Wj1-(UgujQ`K;I;!G*==kjUyrEFh% z7GNPPro~idC;wL;D^Ij8qNSN=9#IGEhrNQgnw}q}7D`4bFO1j<71U$zOCZ-%Bfhz16{Jz`F9&?bVuMP1@?AEijG0FljrKAdvjauE!! z$7h8}Ke*~1r4i`!yM7M8=BApeCt^A=%?!pP4%9-SUC_$=D8%@cp=l4VCL<1s*!Dko zZz2-^P;LDn7&aI~?_t=`;+LL3som~#$>W?chmu zWDQR3&+<~p_s!(q74#Z=$)dn^W)aYq5eJw$bl6oIT<7BJM*u|FvX^=Z^zxowvx$TL z!+VEODBC}eQUvru+9B$m>bKaFYjePKNJ?=D&E z&-f`GNEgq%-h?B@T0Ebeu?%dWm43*cz@!Xog$OV)iQb|`xCB^9T<6EQ1<;=*GB%i# z$Z-3MNUuwS6PGJUAD!ka!?c*X2}1s%FfivK?M-6ehSL2mWHE7t*%pW>)=*NOUhyYk zUP?NUlHLB({a&zGOyIA(i=_v;cv;xA}XU8`-7RRN@zlcVR;bTbN?J5QJ zjP=9an1@x#p+YAdub#Ji{EnlQh6}?3(+Uqqj`A1&lc~34kG4e6d~GAX=jGE&vGC^Q z2Gct?F`Jn)hlAaTy9m4LK@V;te0p*%I5@)0pj#E6;)%l6oh^-y@3_>u&QF@h>4$d& zXT4^YzedAY7U(32k#vv@%g?*uU#QEb;Sry`nKh| zKffHV&;QR-_%fOIAC)>(eHE{b|f4TERxga}#vn%uR%)(S!hq2Pm8-86< znk{B>c2ABUhmCHgNGx3a*|T|d;hvf*m*rif-S}W+vuBShX5Fl=OLc`J!Ap1Oo|sN; z+Fm*pxSKnKq2qUI_N&GrR>t>arT(cC7WCE-qpgj-AuYeZCw2DQbspykPna_q;1}cq z=5PSSQ&@N4*g!X*-V$|bUZ`aJkgW^1)P`hUrz{@Rl%`yDKIl;PX(M}cnuY3kuUc!$ z&5ofVox8fhk#6batVSdDdljEBi`~tmi(>&7q}oJ|tN6W{om@lfFeWH%AUz`aHq&kO z%HmbaPQv!jb8^Senl2h6NKj6deGUiVLrv<_)%*Oq2Gh$#<{mU^Mp?12PX7Yn+T9eA zXa)3|cWgpArxPokYyD($!bo==XvJVCRG4-7n9-(M{XGPrZOY+C=RFc=H&);y`8r=8 zTVc1(yt;Y<6LbY(I}`Hh3}34JsU-(W`dEIJEUtH998MaGYe3)v-+fDM_gq@^)&q(R zVh$d%hCmZEgSDWFPZ=uM&2M>EAAjO#f=Ctpa{;KM&^+wktXe`u`UOd{W^zwLV*)1P zxd^|wG#e~h@|tpHb(rwN!y*5jNj?WcQ3e9*xa+D@M%EDGny@B{!P`MAeI+RP;4)V9 zj}0G@(ZA#LqS~aF#Q|fyiJM55HV6j5(+X}59nnWb zqq8^JPh0O`bz}7yOdn3WQ(ihlw!_K$Tt2Ud1|#WgALu-6hXwxai6~Kg+gRb$=>eP4 zx|q?N@^c3_;m_v%YC$UJb;d81rLvCr)J)HV@QQLt#bjUu9S}PUVfrHd{=|`io^9zp zU8935)K49h%C<3)zPYFcT`~Gvzdi9v_+ZI%vns!FCLRv6s^|bKt(j~6)>k-?3ViZx zCrNIV;(98fO{d`<(fzt?=E*3`xMNEef9S%8cafO(ieYeXajEqsRfB-$Foa}X7S^nR zB|LH5>TW#E+|V81ifPl-maXl=8diw5Ge&Vx!Kvn_`&kGQKCefhiP`4Cx%~|$$sy*? zKVa&ux%J5nOB-lFqJuxQXHnzQIkXhb0|Wj{Z%Jz>K49yMY8h&;UVq%65}BE&`N^+~ zq#x1uOeo6m;W3K4cnPADs=2B$Cr8P%+{6`tfhX z(3}JkB!p|k$mo4_-0Egef0j_Ah=~U)^s@O+EE7HdOdwELqq>%{0S6~txmk!|#{x>2 zbZ{kV0z$)0y=QMjQ60#RCZ9Z`IoDQ7T zN!9fC{E2B1wY$r$(!}`Whg99^{Vl{o+5M#M50vF($Lr`}s;L3jkA~!VOcf$nY&d=w z^{!_-{a7WIn{_GZzgtzgF$JgfbAgq7pNNDUwkA(H@;$B3&lUs`YCN3Rz};TrE!u2< z%N;*sVclH$)y4+qWR*XhWB9X&KC7?JGTA!R%q@v1K~4`7h88wI+ztw{Kq1;zg}$>l z<}W9yDl~r|?03ej&b#~Coa3Qye1z*nkU%4dR`T+M9Ykqs6zW#a(iQ>q$>DQ3P&I)_GaI4AD6kO8aZmr4YV)o{)Ux7|tlBB7J=F;+!vRpe`$Rx!!UaD5| z7D8-K@o;Wi+htXiaWcp~Xi`449S)5$pu85?nv4#QU0>?e z3jAnNAJ&GQRVCmTzb_7hlTSs(i+`ObZphiE3z0|y{xdA^K$0%nO75dHC=ii@sGf$w zJs$o1{UVEK50dE0(aSn+J+B+l(@bzYY7b8eJ_hg{3X;~)zHAJ^hu?sWK1zD0ME_<2 zL;UZWEEGc9-k&A>gYJ=s^P9uPRy)X)1XSZH9Yz?c?XQGmU#&4i8nB!W2KyX_tk(zc zhNJ6Y>Lrr?*xSyK!e@A8QnDsqS7~dNq1NroT`V=Xxji^BzqE(bMCfW%w93+Gk_TP1 zNEc#XmaKn2KX>A=6|r{iJag;2lyF|^X@cAB5l@6|sqkG=D_J^4797@k-kxRTQY%#4d;avHlB)cgRJ2c|c0$(y5>S1L`X-GuDMpObI zOzx_f%JkQYrGlwT=TO#QwsW3T?{~9AJfFvkm}xUVi1)7}ZhfMdqEj4ZLcd`WP10cw zN$zja_@K0e6>4iC?oCPoQ#e!ky zQPYIv64mRSpW?;B^TV=Oxro}TRa6E>(Ul$Q4MJ(rz*xfi8YLKZ>w+h94CO7q%j*XP zyWU-Su3}$RBnD-jl@?#<)$R?XG%?46Az$zx$bb?!E&9a~i$4x+P%ApQuafHW&20V8 z(`4oJUy1VvU&;WDvX&K7skZ(N&W2ufDCzXZsvQ`lPMBQ!t7q-2A~U!3HNT>g^nuWij>eeNO=0vwdNn7kEVX zfJ;ZHrJQ0Nm6{A2!`dKDp!Ul?oX>T{5M>CUsWjcubw$(r6sD^ctK@bo z5NEb)oaozqZBi?boqi0pQJ-OeT2Dh3?1fPm=sIP?TRe5?g8Dz|@ zu^^cwYczjPBNuMzH;1w8u-GE502|mHg$dKCLO*XqyxC#@@#Wvt7V`@+{wj`#sk8Vn z1E?OT-I1x8B}Ay-{ICgck(s<2RmB&HP>Wd5g^Od76XR%3Ko zdaySi*I-~=>2JJ`J)L-3FAfFJ#{uD==c{X4plfqO4rI-Qdx>|tHDlXmV?VQ*7Pg;r zSj(MOXTzFYDDMnl7hSJ#8ruF<%AG%TcqQk z%%|p_-&=Av7poHQ7rm`*59@|yQ{~BZ@~D1Pt$hR-Wg*>J*5=MxG%3^Ax)(;Hjp=of zu8vaAVH#>5hT9gG*^p+ZyuA$rx$a;*HvQKEQ_olHlGR##Zsn{P5RI@~V?ob==|V-< z5`Fh)%6LiGh7A}?$34(*uw$Q^5hEy8d&PK$NhE`&R#J7oqki58 zsb_jo6(Ne@zy^vXV$bUu&8iDo6S4gH&Z{xr^WcI^*u)&?9o zzsk+ek)-^6p8|#It71FpjS1nE=JWpkh&#S#UhmQ#mog_^C9?a_;{NZ=4hESouv{aF z!Yt)dKv2q34fiP_H{+RZM#&c4FEYqk3r$ zFNY;gFAGK)))4Rao-e~aX1yDQ90OaNdry#3o{li@fVvsWYi-hBKH^(%&h3RDWvfcV z@bx{|PS!5=Fg#;rHa$7nTylp=3ogz9;s+u!o+Y?_ojt~{K8|U3mGIi`6glc?n>a7} zP|A79dxSjl9Jo&*)t!#AvvqRj-pUKhR2dSD6^ep+xFN$_)kx)nBhFcE@OPIuJ)jVb z7@UXqHK+y>VG7U$}#S{+W1$0KEf5q@^yEwfT*~leC<kG8QXQY4iLu%-GnQ2b>7!a3*WIUimt)t>vl6^UaD{Nr@4r zQSD#^@i5}r1+@fgbvlA*%gB)@Dj&Jk9qBB(8S&neE%RxAidz~Rhmqb=Yf#s=nqEoU zMF)OXUVlU_%}TVTCS6TWzowI*V_B&P1t`G5eQSV)-9(H?HJH(a1BW&WM$;dhPOj}f zojrARcr}NV1`(Js2>$)q@(U^EaQ&~vL|-tW=YO;SE&7o5o1MbPGv#+IzZECdbXdKW zu=;-_YXcJa!Yd>Y@!LLG~$dZiFTj1<}wryu}LP zQvOA1^1Tgy))2zvsgX)b3|fvg+j+KXiv`GHD2niHjJ_1fm(B(O_t5Jzx?- z?8uA^OMhrD4etpEdH+GeboiT-E*Egi!1K2KFeiOZG(F45O`{wjavsOFK{|kN&8gQVbqA0%gPWn7IGTDc~y`Uyut{=kLGV7zAb z!XjhQCoP)hk)n8GMHCRbd_PnQ)>3bp*Rsv?;d*toJb9FiPJiB&fo2|=z)R?Bt+kQ$ z#f!}2JkQ{78tLQ;6;^T>8FD1WK3&=YiyuM%?qGt% zdE10bt4;x8-Cf@Br2qFxqz&?j@zs>f8=^uOu^TuBAr15OzB<3d9_7u3aQpLp1+RbT zGlW@d7^Ve^u2KL~UpvKY&)BzI##OiuN~_kW3}(`6+0Hb7H!=AOV)V-GMDWTmx);Sy zc}Y@UHBQTrma!!-ntMF00BOPrTWm5b9I-7D#(civ!qBj~4km_G3#p#H{ds}Z0}{)X zF;Pbo(;>o@_KxY1Rh}|2Gml25>M#<(-lCuU2RV>`y#)*qL~lKK?H5GRL zS#+}2O5f3Mk;olcn&Q&Dd^3r=+fGef9{aNx45xnktqac!xp2mi&0+tV`h(Wu9X%%) z$iZTru$R@^r}-8Ra;I&@zmeNtb_HkU1FuYt#wyHy`v`}8&kbL>%^G+9UX zIvH3pzE^0@VdldXYB4{N43yGyg|2QkKM042o<$ks=v2bC*VWj|^%w0r3tyZ;^0eRc zmb(haed#)L|H_PyuU#qy^apII92-sie_~9Qi*hWN63GlDX+de^Y;6GcS0)d<><536 zpVT@x+YUMD0~fyMq1m$}wARATzX%Yomkp2ANfPk2=jlMt`z_2R{?62yW16x)Z*gcZ zK%$Rk*_r_&SSaMVa6}0^W}__jF@dZHpe=eyWQZ;J;@wVzD@9U%wOmGlNNPD5GBr%; zz}TRQ@EWc8^x9%q)2Exe6@*wHP%nzEZ7NTRRalrBr5Na6y!t1_Uo%i1HW?nH#^UqO z?%&I0&M)5sg|?JHaWUWyn&bKd)_d6kt_y}R-#5G+D$xpbiL0&{O3Jn9zsqUoJ&w$V zJrW&iC-fWN^-tS*-dbG#`+O1*YnNPS_>U^zQ8sL5B+;o5x)sDi5OkMSD7}bh)xiHAjU! z1y+$c4S9W9L|;&}ce|9WkW&1&4aJ1+-i!|=I!SGsJix^u`#@Gzzj5+cWBp=^p(iS_ z_>9k6=f230oM`z(i4$oZ&4>dXd;PCj^1GLoDgW_$OBbrP;ZAdFft>j-c+f42OyFnn z`8IIDT}ARjjPk{Ij0_4LD^@%m4cMuqod-5{#lIu63(@Ct(N5PpR>-4y7adg*8DskD ztIrVr((^<4hO6vQEu5)LRzK8(s#hKH##DL6t76i7YO`M~&7cBJW4^L-I7~Gt#cYg<$Cs4Zx%SSYzk2>}d za#n$j`YDl%4#Sn^3`~5!&?uV(!fToa*ohN;Z}|9fIxYQvf!~`o_}VbtluFra?3p@- z>Ye&8@Er^4h*&ly<$&PP#)4Zdz3mGU=^E>QrQu&*wp4f|dIHcUuGwTWD0T(V?o6-v zJ_i=vvQ^*^1$33^v;|MX5p_{xy%G_hjZ*mvB?1lTSin3lxcNY$)p0q zuV6g6TDrD66A>w8Y?Jwj;LoU;*1P$ zaJxn#;th`wGpI)Euq9ar(57dj+5MwdX@+hg0>dl$&9&Pj5=(xxV=rwJ=AQi{=UqO# zF_Ylfi!P#e81u8WQf_Iy795*lS7lzGho4jAuY`k$=tptm$tky8TI~Yr{xfWnpoo1w z;un}l>FiNxN8EFqnv`Op9_@hxDdA1T37d&Xjm=$W4dZ@kewaP!gOu z4HddSj1MSa=aYg0UpS(ROC0qLwRD@QI+;Amxf05H+{>u!GcO|V-RIYw)z^oILWJmL-lkOT$OY#j|gL*`$hu4|AH6fbA%Ku3!v<|F18Z-JG@ES#8fp8IHcNI_C(%x6ibE2?x9u-$D zIx=0mvnAmx(fG;i-^K{#ICDk`8ct83W%t*T$6Vyrs{l1$- z5zZ$T@4wCA!5Whw>gI{Z;g;puY{2Jwie)mj(mx z3&Nqa&G)i0s()d=LSu{F{2!laoc6)v_)flVUJP8-=mU(^Cx?`-(dVx=mpOBgDJgoS z^Q?K8oVtf!yQw(cw4l;P9Og-&|NLZyFb_NlKeP~sqx>6pKtb6wr`4P-#8#|=scD@q zNMTj}wo~?m=qBvV=VP%GQ+u)|>p(S|kX+g1oXGgEIei{x$qq-k_ucb+8R7V`UxwOc zTr?vz5v0qUS2`#c?&b-D_WpGS_1o8641dEm(Nvqfp4+M?B0tIiuV-qj-Y#RmBxG(U zXjwD*rx7;d7x*|Ve8uljV1Wkij@_Nn)iZ!lVb0Q`5^(kkL+Cy*zA5Q5H%2&VplIy|7-NI`U+wlm724H&an@+bdq+0+I36iEyVZCD0`=ZRk5Myb{QX<>Sowu0C#B&--D4atuNd+h-wx%TrfU@X3{LCMn>2sqX~fg! zQU1$s2edwoNmvJ0H}#+0}2Mbzmal$enKye>GY}uxNHn ziKg0|8WBjMnMvDAmB24&y|Ua;Vx+rROugss=FzTQuqbrDTW{sWnvTAH77Oc?DlqWR?T^c5(_wh+r`!cOr%)f#1ntU)vb zoKz<@hgGjMIkJR7hb9|$A}Gv?Yuj2*s1`8le-UsvYsuj04*CA^A)_ld)AOcU|E%6| z3g_&lK#t~ySKimT*avvscFobNIALSQyByG{0B{+uF3G!P$L_~M%4V3kkU7dxY<2;2 z$s)8n$9=HTfhAl*JgXUdd@QC#?Vd>MfP$mSrxWkc)>ZC>T~v!b3Eug5Rufrh2?U?t z(lbt`_iBC3x#?=D9%1|}?`s}CLk8fh;yrd>hnr%@QoPfHG?=0!_r=~*ui;>!1pE}$ z#CEJyy~irC(@+Y#AGauF%jf-LA<%h46K+x|F`jooGb6RX{;wnJ!yCp$l@la1uuv-M~{Rk6oT`OjvsJ7KD?S4II*R{_%~ETNC$_!FTdsZuW$= zbwz|r%{b@zhacgbucko#&&SB(yg4^>Xx~g$1f-qr8=W1S6&c+bN@$drF$G#`5`Ss^ za=*e2%j5V~N0e>VmfG|tjltIP;FPmm)#h)cKmp+jgSsU@7b%1G@3 z=+L~jai^cLB@?pe%i?c?`xk29pZQ8(r>e;+E)u;0(@V5I{EC$l+lLhei)(n4%SeImIyCPY1yWE>s6?RuA+;7g^G^^Ddxz>D|AW}o8K`*kS( zU=s5LVq58|BeOj2BOZ&tusmHA#xtPr+|4iNE1$XeEQ}w)cVv;`d-C}WpCyL9x>q*c zyJ?S(Fr>CWyY3A>HdwQ!s zvsJ65p1S3jLG-Nf0OiQM9mRIN*vM;EB;`(!S26wbh9*Cp&;+3%8y+JyQUAg9;~Uv` zgecnJ2JW$bVcZ=kpy{t97{hsH7kL(1G*O3K@ToKTf zul-|L+f6=vO5IWg&FwMktgz{$HDr_QmBP)oNzDi^vn!DYPT_x+w`9uh}4RF>I8>MY##De)&Ib$);t!Tnfo$uPG#9_pe;nW zma71~d|Z|>(;~mRO0ji5rqxt)Er0yQc2vYs8v>x4OW1|gC)yV-{qDhuy605A)KnaR+#+hb$IXgUC&lu*-rR2buJ9UGgW?) z{^bWry&mE6A(HdCkNT3kI;4!!Dlx7%e*$Ml=bMx2azka8S|>%K*jeK;9NKkvus!kH zEiQ$!VHPMU?D6{qpjvBZWHS}n9k2%}3&yBUUzc>{4NDUC>%=)bN5YX69>VnpR&iFe zK_m1LHXk5iB9~V`_!zWDKLMDuL7vDwm@j3gPWpPhVF|t3i2UJs$J;|-zOO;HHHzb2 z58<4-1of)|yH{QhbtdOftKG*^=P5j944ob>{CwVXq^@5xi7NztMO=$5_|B7}x@dLP z?c3H^2cLLf(bDjiBDPkQRF zyzIpmA$_13SZ&w#b__pfpKan|gsU>9!^4zR#wh~lTcjG}Zl*>a-n8kWCB@#z}6nnLtoQz3DVLdYWf`MJhYB8MxY=U6XD ztgNDk+)ml)RJFgG6IF41F5zf!q&1QU$}DtiM|6*3X6Lm_mp`wGZ{~w#$9+VgmfK9f zoY7ROUN=tzw9_56{p+2A$^*3`sliDdu_ld{?OyrJ_Xue5(+|`zc#^kSyGB{&>kcsq z1Lk*=MumlA@+0(qv065)tLnoZvRXXA-n67{Kc1~Tgva--?{Qkj2)}f(=QI_Ib&p?^ z4Rb=yxfUWmJYO<+G`*c=R3%hc_QtC+>z0~(vU)V*YF#?H{`DG76Vu{X0$^45`lo%2 zv>WaTca!UWgJUWu>aO0rDRQ;@Doug*H4v8wrZ=WRtfso@b6DPBu-aqQLDO8d^v9+k z`vt`Zt?d_66Q!PtVDQC5&BQ$I@6$27wy&{;`VXsq&NKuCzk_QL?rl&aIzS>&K=+6*Zbkh(vf=0km`2G-CsQZpj{wWX<%hom$cXdu-vRK$(0j6raS17 zEaG+3_}MRJL=8%I$}kIX*vXQ@c4x|u4Z8Ms%gsh}Ha7=a!)crK4M|$Hc}5Zf6e3YZ zou=S(@r`P$V4$jTDOJjRrb=d~tmCaWk|j)IQ;`AsE}!|&b1FecB&Pva?_8|U4WF?f zMP$%ay-YTs(1wobr*9DtcJ@tj;JnLfvAd;8ZmfInE7faRy(yH(K*Qrc+Ld)qKJ2bE z&o-EOp&p6_OilBTW5#gA&+en8lN!Fjq%!xhH`<#%b3La{@!yIbr8)YNU0=o3I^l94 zt;9Fqc-J%kE3?5dzVOa!9rn?Lm3^Lq&iS!C%BOSi9(9Czzl*K&-MbTVAGsrEwPGI3 z9J_34rF}YSqQr0+=|%kfx&Ls5n0Hga6WPnR@uqxRqsavJ@%3i@-zFw;?;yih`DpE^4T9Yy`OX^?(|t62UgY-m zd-A38?yFd8R9R_k-1pg_@xE=S*O+E%Gw0eA3ojCH7XADpx3<}4-QIQ=t0Xih9caZ# z2qgb8cWM0#2t??5}{&2$_wXOHgt3<1zkG&JJDY_Y_p$m;=ktxU{OI!%@3W<| zh7!DGu!Vl5X>Gj&?lP63gPRmG(>R;yipm|sWXqBVLd-OzKqYA6DEm2 z`|E1q9}KCl3exRIw|p%QY6t?!(vb%C{|W1;zU_bBx9j8iklOoO)lL(g^ZZ%VLlNBa z*NoH@>Db(aE$uM863G9IyTP{ra0wDl{_Sn+kf{CY>ofPyg429`$!4gjG2gSy^^Nis zaMr`~{>)&}*>c8|?ec0nt@3Ja-8)6Gay}mwh-T!3iB#`%aMDF-2?Z)XdhMXDPSitm zl{#RrbC*@)m1ULVRx^Bi#M*cAj=JVSHWD}C#Xy7nS?sfO!d0K-$lFLyY8QN#*9Sdd zvd;%uVcKR)_mo*Ll7lw%9G_)6zO2=K$Y1f)IR6JMuq@@BjqY>L7zV?AbrKuf!b$mD z&v3v4wuFdg`!+cW-?YrbgRTw(9m!M#p0^>}uh~*~Y+kMvZkLsU%&$6~j$p7{Nob|W z%2_u@ay`cOgC7D@V;FP;Ez3JGE;q%+Sl1A@AHB6d8|{lXY_=TLdf@b_P00L z-6AUQbvWF>Mz?28cex7gCqbA7)tVI{Os9*9AuQTvz8HJe1ZseL8A!3GdHnoWx$52QN$Nzk7D%zg0C? z^ja>(AEcQy2eCST;1}r-zl&|2+nJ6j+WAF!qb=ahC*?J(#)CS&NO`MN5*SII^>=rQe|@Ih9`#KnuoHFZS8>HtiwiH zI*8-v*6AJL=~3;0VV(MT8xnD?S`Y&oZzd0%m2OJzH zBTk{jKp~7j(h71`8@Sb&zRjNZhGH{TJ~s9&UkT*w!AlZ%K3R!Mt^_9b_nVPDUfF;l z_+*@LnaTY9aj=mfl$NTh>hjR(Vrp;Tz~CK+4VtPR+x*DuR776InYC zERW6Ec9#(N0Soq8v_3O(N~(5ZP7Z&%-<7Z5TvE-;CJis(9Jt8ArcaLo1xW1ES+oPh z8XWbGT#_+O4=qit8RAG!Zx)Ys-ajDAv^I+26u~(BC>sj66>(!DdOd<}=K=1pUANI$rlq#FMB0LNJbu_7t-bcnSUr*_rrz6YLv>U|Uswp}U zom#a+_?;0w!G6ZrGvm1Ixg`)a;VA;rR&*vql$BwTk<3wmC-=jq&<+Oxgy_%k)KLp`lCu59GEwEN24PNUoL`mn?9KSb@H^; zJ3k=MxLFCr9bFzm^`{eRS(95EYJ{Q0&0KI@dYSi%$-Rv&PyP2Zi!EK@5JC(8K0P(s ze_rPA(?k90rSLyp{rT%1_D^^Je$32%{pkOE$no@V6Ad6vDz)OqRDmhE)a{+YQR@osvb$B{{fzjSsE zeEIeCKMHf`OaxfkYhO}^VkukM9t9{p;F%{A|o@sYm&Zzjy!}sYk2TaPiqG7`n%%dq_1TOO?O|Or+Ck;!r;U_ zBJ{UWvQQr@`R*OLk4+eyoi78S6@kC4$cwKUh68g9zf85w*MtQ(Aakd`*VKHdyTb2^ zE^43^Q4SGxV74j}42xv0!z_QP3h{3o9opTh1fcn=CGwlzhOaK(i5(}@NLym2)V;r} z`;5?g^dIFUJfHN0D60{b&W}okOEzOARjJ9;gIH(IVk#7I2}q^D_(s%nh9_Q-SX(ul;;$kg!)BAdpg+h^qQ{S1 z11~P|tDXM%Gz(D1@oJEIs8pSLm}t0&i?zFi&()f|;6o1Yw|})U^O60D1e45md0qo; z$8V7h@_;u7T8hDV^I$w4Hy3!kz_zyS_Nqjhfk&mjVdp*0Q=V<2ND_lM6Yvc%>>o-E zT9so8e|UrD6Ye<>nbOeTeCI|-TcZ8VP>=YG38hsm{_r-TwT79~3&wBC@yFvqKjg4C z-xH-gk`iit;DM`_>yy3=>lw_VRfOFg%_kO$A40r=sKs~CN-VG{q%9SW(c`onHhA+-BI|lZojcwk++~rIG?_m* zp8a2vQ!ui$u$kk z__rmU+eoG5S@5Oo_BIIG3q}*_V~wT@y3rf(ZT4j&4^%+A_xwKgSgh`{AvDkKOV}BA zS5B;q$HoJ&Il=7&eeBFaR#dGvU%Vy0#JpcAyUlR>3wq4HvDS*-92+>=1c!r9=zv*L zM8+ns$0uC#G=^#vcT4p8eWmgyTC@nJ>#tH4q_a_WI}wLsduKF(tbrY4_c|%1$vi#V zq$lKKWxi!FzW-_w>?5*>D?68OynI@ICA!bmYX0Ij0C`qjjePo@Z4)44Xl>cGiEd3 z-~5J{uRDOBP*DS3{a+n+IAL<7-+uflbO~lWpPb&Kq8X)@Axf&`9}27j9Ue7|D|6 zS&q*(B|%z?clSmP_P&w#Q3}bF3q0Bga3hKSgx+8{0g-L`o^+8n8W@cGPlvE;u`s1r z@|0H)7AIO!h6KS6DZY0+)$p-r>3i<~yu|Wdy_h|L^l~d{KnsOpugaMMAHk|!QJM+y zEPtYKcr8H8F_LVQ6TYIRa+$NFth9nF1Iafk3h-?rZR9gsFYc7Bh|>%Fc9ASD-cVcK z+&jIz9j~SLXqvL|)L2&o9|27a9`ipr_C~}@3lP>ONe!;K)$@v&f@4$GG~&#ph3$tl zkq7qFZd2UDBGke4X2|{8%%GOS(gsn~%7Neka0SCVv7DzVd!v0vIZ7>#_}|qpcj#EmG#34#i>xxl{wP_PZ&PW}93TI-eh0IPyi7)00J2PKl&MtJkY|(jBWFt46pzvt zDPgUt*{J1>xb=$ozLE5JdC4SwXTjgU=7(zw^IE0GAT6bNcrRTIFkon?mX&a7-BJi2 zEb3>bU>k)1b_$ovDZ0!pEzRMhEF4LDrZ?Qof|n+iElMY*gX6iWh)CdFQ+(9i83Wx6PRi4C5fVu3qMToS zX%Mf|hd8Tt40?H38gH6PI z>1{>db%m2KFoj^EYuYv$ja*if^_IkN5VDB<4|{9DU;&LtDF^;H4NgTX;S zreFJXY0D~|vU!mea~Ux(;y>$&y2o^IRmMF4ZtlBnOzfN}y5`pez}cd31(tjJpjL&- zMAQikYY9OBPe|v@X7{2u_jJ*$Z1_vdHQ%Xub}eRfIaJ%VDBP!CNj86AJ#`9Z6DZo) zBx$8`LoAwHMVpHDOVH5*==13LhIzkjg$%fspN=hyZ!MnmuY7AKuZC`K%Qyb9w%zs* zov#i)?}(o4Z)biJSZ{b`+af+=#xki?Cxy-NsGIRU(IZpr>Ecv`85nPQkZ^g)Yo;Jx zkciSY97c{mP+@(m=J(3jGYWBb9%Oh}%C`RrTbqm&VLUcAG0ivdoUgW->h7PtTn>0> zIiYjz3k9`UVuOndl|)ye5hq{M6)VGaTam{&kQx4f?T_P`gzoQ_o>I)2Xq@b;S2ic2 zb`{mcQMVlUHaC+HKyV3?Hv?cFR8ps!{GBXsDXX0h)LgVOy85<%>);VKX`4@k z>l^j#wv?%Vi3J~g1?XKfudD&YBb6@2<)kY*2hGccm@XHh79!}5%kv`S z!-}&bAM%^Qat=#dYf~4h=E+MMm|LMugvnJTxH9j+%c1kqtk!3+$qsvpf^8jDzKZ~n zq}dqBYvW>TzK!5bt({=s&}PfPeu7gNAxFkb2`DutcmalV_|~mAz9uxC3nLIfqez8{ zY|#tBN|R7avk2F1klA@>MWg@uZI9WLko>xXE~gpNIsgP-c41a?ez%uX#447(?^QTX z5$l1nLbtP23v``($Bp3)rH1zFkL>Eho|5BXSkf1^t#g~W(}0)%#68}Ouj63DcB+o& ztb>k+a??ML9CQVFkm1$QW5cAXSW zjLAN7)-_hLIv7luF_(KB5$WWPeUuM^ zT&ZF)PXFcVIW#@_v$>RBq!EZI^aiNcrNGcew2Cosc1CcTneJ9QNaF}J0ynW(2v#1{f$H)&(lGoFIt86K^CU_7q}$(JaAQEUIQw=CKAS{ zi+=7?T#cet*o?lH7Z?BUaY3b3n zS@JSo-kQ3;bApZIt(h?!$ij$d5UsE&P0FCi;?*#h4+-Iqx?AU-sr~fmOld2y&OA$K zrKN-3>BE~UiaBG^3#U%ZF`%(}*{JCCaLVM8GJBA-g*|Rky`?n_>Ih?B0Lm>W(dtgP*uK6NeHc7#4;v@(w>4ju<-Z!g#fwk^vQ}-NLYO zUczz?<=4^}z~3)S-KnU%1mEX|S0$e!Yn82Wkb=L#MG*xT2_R=~zkQ5GsYPsr^xdOu z(2rvKw}X1O=iNpy-HaD(Da@W(2hhfAM>k3q_6($A2IHNsju?+aCh|`FNe70FlFU>kzAsRB1sq6vM`3?;hweLMrJ|;s zQjC?qz5jyD&bcGEM#bL|qTHY&#(1YoQrn6Fy%3?Ga+n&UbLZ~#S09GiV+yRNc|lE3 z4q=MSFD>LfCSL>aEDiMsEhoRNoP@F$wz}`p8jOE*se{f!DlUWcatOs7-2`2hr2&~G@A~vFO@Z$v9a$$Bj!y{2Fkirv7Y^dSKmx)i zmrgpDRSThpfL$-+C@np+y$I67`? zBxwP&K+6_VX$PQ3>t{4BWpSTu<7X_b^4&Wm%J#ozFrH(B=gxmk(ydtjOJ?~v;@-}> z+yH@^#EKHJTbaC`=wCM!qNxQ`6)D$w{nJ|Bwg~(>H~=fY8BaQ&h;f8bpf@V1p4eH1 zWMncYEP0}cLgP1&@Kn=nU(oyoPflA_uFxzKz~^hoy7?y~Bb8ejv5H&4B@!*eZ~zB! zw4_m4r{=&&)O`sxJO+`QiMgh2?zEDI-L}o1wOdRo7g~DE6knGtQcgX#am_1+{7F%f9v8DTX0vbQma;C%p0K$w~4W?nQ!+mPYqi-H=49c zGhvTmV{NHUgNaT{V8*`&N_|Y~L44mkxYOmQv-%CHy*I0ho#Ld+-H%rlU_X64iKM5( z#U*7-BM7C*x4c} zw{3^%gr4>*u7#wxd&_dBBz zy40h#e9hkrES(k03o9~hE0qcOMI_Vn_YXSmm3byEgzOv>hvO)Wlc~CWX_1-v<4)Ew zf{YP>uO`Gy;iviBOYJ^&No?2uja7Ijp-T_+SbG>_s=C};&Q|7o)6+bmOjrt3Gy`R& zLw9dPnnDk1Gr+(;D#GCSbJlsYX236`y!0ag_(rI`D-r$_0AJ}s5F^mD$ZPZw0@;4{ z=gh=u(uXvi@aeq+9fj7Gl~X5v&v#3e*Jv{l8#*!jeyjYvMifV9Iqp>=Xs;TD0{{G6 z`>x_?B~||OMLJk2T6oUwE9iI4HH?IIea*g(5iy01u&>$l@kLHf+brS4{=I;pv6t}} z*5gLRlmsPM?N>nZ678dKxu6qpYwg7b4-^DHu$T4(*_O52^2d(`Z+Qi9oTqt-AJ2|F zRAhM0YB<>Ns2aGUnq8+xWKlxXC8|uNZ4Nu;B2CIN)KF@Ht(n>O=x~*qH}?AnpUpKL z#_oQ~e>i{heZqFL^yzYZ>&jqC&aYjMn%eqySI^6s8YE1WO6}B9b=O$lCposy&{-ly zL<*7f=-a+h-1~o!uupnCH#is7qX{)Y$A|g6%;2C1xDMOfZN0cubOlKaH^cGzmb}%k zNo$Fg4w&EO=;C}bUwsGoH|eC|NHy6>X9e~fIH{+YT8a9eOE zp9R%4M|#I#L({fWULuWdXkqI?WCFo&MyPJwvER z#`bvw!|~JzyNT#SS3zZZT~EQPT40>Xx2C5((h#V#l28BZV@I_`m$I(r&qKL7<8uqh z+4DCwNT5RNbaY?1xNl__C_SfDYsT>C7A5gpz0O%exd*H^>#$NJz$*P`$?Z~s)m`Uc zv%RwIwW^2jv34@KH3uQVjuCdh<_>(+UzgN0LX&qLUC7<22Dcnj0W&+ecY?>_#>u~S z>n8zkQmk9`ZvHHByhA=ta?Luj?n*Z!RE{Sl96+Q+r7DnZjK)Z}^nFmKOR=Z7z0r{{ zofGu)7?<(3eixF@Nf>j}MXWSnp>Fq1muR;M)L+#rl(0~`usG5B07ZR$@ZG=Gcj) zoRM7~y#xM=3y-qWeT;wb&+eoNBYB^=#_{`occ4BHMDVfP*2tBGg)JJ5dnrGs@8`k* zDHY`~Bn+uEKfb+rv6;ng)aPe&QW724ciW>FvsX#Cr=veSEuXhr@aG0;bWT8U(Cu<} zP(R|4Fl?V@&^$PH9P`N93~ujiO!5GpgtVOKHt7(tAw;X?_)``SbG$g#Ge})&)}elI zc*$9?0<+ivZu`JLX9aNQ1mg;B4xWd~dEFb!kg6nvbH8`J&THaNMp`d*b|`@J)q!cn zuy3J}K%Y;sbQUXhyLiWOVY6;fd=5?jc6J&w+gQC?*LfWIv!_8nO#K^!mWUODb|(TG zk7V%^1#cQWPtH@wVyQN)QA#L%?}H)(KG5rN!f5C;@C2y<(=QH7=acf z|G>QhrKLKQJbdvU7e6R}H-V!!!DlSeo`G$vS;D;zN{)9;v`WlQ$EVE*t70%NOf_LD zex4R&v75brZkx<{~;_@-COrwCs?U^VoY_6 zFW8ohr*f2${HsX_mVXI(McM9|dWHn8X_y)qXY#E#t%gn(e{t`t$;!{rn<7%(skt)N zkD8{eTT9ZH_y6v9h6nD(gzIX%0%8wRELM|R1EOo_DiV0s8yz2xg^H|BXajO6U(<%M z`?p?v4hBUZubNL~G+@nQfIjaIgrNwnx!{btSagvjT~FdQ^u>v{Zw2C%7h(85a&X){ z*3=x+HrQ8bO?Bc&oJHFG|i?23HEQu3bkNZ zEO02DfSn}I5i>LHn2kmOy_evt0XdXk>Bl&Bf_y`jJtGFk73z2AzH3V#;JWjwYf)zN z7x{*OeN#`bouL= z&o?Vae;|2J^t*IFVczj&%{ckO-Q5{mXW!WNT=;f8vEtgy2UCSXsVii{LY-+QLpEh+ z>h;Jtx8l92yGe%%oz3>#IESA>BYE|~AG9j)A$@ZJkm!oQ=&mGR!Var>7Pqmoxz9@2 z+gZ2G3M+n@4t@+epx7q$PrbAvUP&*Jq?5s3^g&O~urmJZ+>E#T^1fY5Ny_o0kZye| zkylfvL8TVB-^*6jpxBw=mh`-H}! zJd_DX`J{XycAmMhlu#stSI!==@z)ZFjg$O5;wf9Z?Ah^dDDhxZoH=X8 zzAP0af?CVf(MYzr9WzND^A3scd~-19J8a5HmYhX>G1e6`{)F|-XTw+8bz!MO>N7Qo zmuyNOEawi*rb%jmaV4HO$qMMHt;OXa)@U|>)z(IPXbxcZ(Mjeo^(N-+$zt7>!nJqo z#SEF7m6gyZW}eoei$|7#hbbi@g!k$z$&_tqpZeeb_HeV_;HDGrIN0uncP-DimXU7qqH=~9=F(~F`c16+L^EO0}F?Iw1a+kVcsjxQ_B zv-};9rd828z@&7_lnp*ItxZn8vBapzZ&Df7N%loL?`C8FLZBil!Y59E;ZvOY;}~+cVD48(v2(i$YyixE z>kb%utw>~2Hk6BU`xJIJX?x|%@53wWZ(my0Ces_*Nk%R*DpS3vtDKaav-wqL0@Hv^ zenpwxe9}i*T`)YbFSchnlett=E?e5$8z~MNJY3`+)S5*>YdR}4n1a72~#~06` zgHj1`Nhr|g3r4PHv&Gvxpv@?r{QeCNXkSC<3sAHvn`n8drRValz!(B3-VC;Opb@d> z4nnCHm}r3GdG;sinhwvVp=WeTtHWc`My&xKG49Cb{i3jfeThsLy&Ul;DzG7X%JCm_ z_hhx;0M^eEo_!ngR&XJv2`VpKx@ zd?pj+vu5-Xw6p26z;g{6o%Dd}yuxKeNgTx1A(YedaB`qToeo**b9Aost*Vi%4UM;M z0-*ISv1RO5lR|u(!YN5dV~R4&UlR$q@;CR&5_G_JS?g+&G`OSB6WTTPflj1;urOyO zD+sR9+mCUD$goCj_du|=eiSivc^Zn1B?n2z;yx2;o%Ron(?5OBmiQ8YLnhzfjgy7> z)88J<2%P)$J@7l)e zpUQTEqHx-mvbJ;Z0G(B_Bj(Myzg&P}2~Lgs8V?lb`fm?9PNWEzvx;}2Ar}Xlr>CpG zHhm{RJNqKD%`LC)i-N9peir=RU*7K3cLlbx_MnjNPij}zK5!D+jkeD4pRf7K_=`E1 zN`e#u*C8L4Y4tDV&57^_G~Z51iWg=pN?6hmN%A$xe&6eJ1|?N}?ON@(AV!2cre!^m z@tJ`UuiuUgww;^6qvY{6yutMGSWWloM%8fwSz2YUkv}GQ6<3PDufi)5EOByi99Rqu?p4n-EU01q`jDS zhbxE|<>6oCC!#=mgRvlg1m7uhJd{d!r}12h7ZFYv-qW5nJ(N_`N$ zva>@}XlUcOojU;xWP9ru0^{2x-&C`QiJm6i`zY;3@%vk1%bEs$yTJ&y_e57?H`+kU zA;lRooI(T<#h&qV}IB`zO!nHYh) z-FMEP2R8ZZKS)4##^lb(8^*5J&6Ledn>F0@<>CC3AS4 zQMzQJ*5>M9r;|xS7CxGl>B$Be?B^Tz+rgJbXz4M{vxdIx!?}vr!{-FSc;2*$eeG}GE1`@1HVWS&;rRLDghGU!h>J}uB6Fj${6SA>K64-Dt5e9bf{yQqYV)0Ljo7~00_B)(R9DPJ_( z`4KgHVy<*rct>+rLuP+^vHAN|$17MVDdGwjOB6F$ z=>B5f(86B;PNryjxi_3r>y7f-G$MHY=FGdEJjSWdY4T|a+ z=iq~18H3}O9pWAAoWk}76;3Kyyy8jrC;8>^0}X(q=0D*@E4|^cj!9VW2v&q0wxz=c zeevjTFpvPhi(kF)7@~KK)iL%S~oWYk;isI(lvqbifNe%xW;2CSD z5tYz=`TdqbjTZ`^;P3v!xRm4%A|7eWnNQiYvF)wk@e$`0~5E1O>8W`=nd& zh7xos@OaqyN`VA{*?UHLdE2lROW3izn1r;vc;rn``E}=L=3uP{4RAOU_4baJ5buzM zVQ^9_e+88$jSiKh%=tyqB7#-LXMqe%ClWC!|DvF%i)WWZl$?$QpkM9N zc0I-{t1P6X*rP%I%>!TNiHuT9XbgO062*0cgEkpCY@*mXwu}3m(sC3uE+n6W&C@|- zAF8{5$kAO|y`cldm8ikC*egm&(F)`0Kg$?h>Bx;8M2x~igud=rO*d#}UH8#6->BV20BT|rGIES{{%GDe>y=Uw_eBz=sC%dZ8 zDqLE+k6==Aa9_Gb$ii}zPc$BGY!dM9RKsCjfX@5 zZd;yrol8D(#M7HVPnL$h1q-7<{fdq+ee-+v|FTd%@7ufOcN96!bk+*+b==$@-N9OcTDOJ18YtvKjReO9^sI-EY?A@J1;zP}pZZmY;6SID6i*W9W9 zb?1jdADDu0cCJRPj1`Rw2#pjd3pWK+T81By>bLBeJmTgr=(H{T4rbOKYAZ6Mb+;^- z4nAkvW$%-hiXL+`UTc=v_s>U%0$3;v;ZdG4%fi_SCZ64k`yQ{%QX? zBq+X5Y*>PwC6<6S6EX_|T30Hi?#FO+G^*d7xbZFZKH0&Zg*7aanTC;5MfJb6;XvkT zr0RzsYKYlLtckDr(H}k{`@xcEcP-%keXe5ZLdkRqlju<xGMho>K`ZZoSyvbP)_b`J?pR8$*(mvT~5E@ART&j0(UJvy04L| z-H`z$Of{xJC^|eu0`Ko9Fmrofal;}rU;iC^wuTxv%*W1H^AV%imj8NP!@8SNF6=1e z_?V|tnR1tffM@RtC&a1GGo$1J_t_*v0=`#UY`^)##ijUKHp$SUHEIBdm zPI%}Q+=~nkP{xZtKhYMB5et>^K5_SNXe3@sv42S+V_G|Kg}9q4e5kP+Mc?CCvSC0i zd+W#7r>ZY#{}m|?WG*yak@LBFOt=EcEXC}x)a)@WY)~rHB4X&x<8-bi1c=C$Y&=~) zgTTb4hZqiJn6So9-k15Ir2W4M`^yE##O#N_A@&b@ogTMNf0;&ot$}GcIz|T<3^<9j zHIWf`M^jKuR-NQH!djdn$6vVU!{B$7!Uzn*$`VIdWY5S~O(3z5;IMY8&H%63}N&7u8hCqKH31#gj31N^ojhH-!f1k3sZ3#DTl;1Ht6?B2kq|(=mKofB5pN|BaW-ONdiTPd!u-=b;$j=FeM^ zr=!kEMm6Cw>O!7=yKi&@U__ZMp+Qf6P6jkj9HL6GCnJUJ*B)$~ZuDN!d1)}tpZDig zC;#1(5SYpi^@*&Q?89sV>Fw9u1{YxPX0i*e@_11yHG&Cb0O?=;ISL@}`gdM4y-^^j zzn!tV7^BFV-`bAb$mC?+#H6*%+9!JkYkECT31cG@YwJ^O*3kyqk5Re7p9v6K`ZeXS zx+?+5+zIaIG$f&yc6&y)1sfYnizRimCE5R@`#1kL;a!*n6eORa*Xd{OM8U-5mkHGO zIh6J9&BXkh-6o5`Z-seCm9ue7+Sjj&-9&W~>`^X8G*+bp1>`4Q_CdlGancSyuEf3j z{g__;G`F^^by5?k|C=y|o*K=+oYRM^FeLvvgt{|0KDgm|1q@<1jBU>Ar4mu~iY3@z zTd%a$#ajbi_8SA&7u7i_f4{pQBf>)z^?0@kV@iS(G8VIuS&J=Mg`2O(wp)n+tQGxU zFfsp;8ppJM;t1+bG_HOjI5sUr6 z?Gj6C%}n$Aenki`G%QU6n)RLc9)lV7GM`HI&;cE(Y4SL%Qe2iK2;bszYkM(9kp zEZgvxX-33Wx7C#nDqF!N$hGdfR{|9iwg}6kB_1AX!TJ*<(!JWJe{+#|wB`>@GcWI7 z8@~Lr*dRA!-*A@&C7bD!_l><%IVIRL!orbYIx>`73Ba5Pk0)TnNodH#>`PsvHmvTM zSp|(c_4t&$n>6GlDuHy%npwA?OD0743BdL08H~a?@z<{Jo56*BKn8P;@ILe(X+Vno zwJrtk9cScFp$9^q0C9rdJEDI(O)#TmK$hVJs$H=TldMLIf+3UKs!CN@)8jG}SCs2D zYZ>Xs!k)L^Q_<+Gl7{q6&jb)6XBzcR=t`WZ6kA9=&O4^S=3xMcfv?X z3^sXro+(#owT^t5a~I#hzA=s>zq%;%9p&`OQ}Cs0UF0VDSD8VTv+9+BnNz~ffrsoP zyQe4dk_#w?`^FQ3d@{uD!mJc0Yit-qN4O7f}sTvrRYl+=_S$<5C~FC zfB>Nf2oNBI0HORBzfZq^#u>($%$$4A*?soe-Dl60doo0pnXlsUA+Rv4-#v-MaH8d> zoMQqZ(g%3AbNC;AiaLC}-AW%NE4(TG5u51L24&RaRtuo}_}X5d3m-Mbdx-6u3cxcz z{fPmaW_8VI~{e*^D7N+jUw413fa6d<8t993ao=^q|; z_(^`7Cu(o@3HS7n`?mD+aHsVCv;vyRif2AvXUCHsmJ8;87~=|SW3mTKc6v=IW%6sR z9RGP+ll~`dWyCHDpU#jJFyGzQ~2;03M}PdFUr1{!K^pPTO533_+xq#(q{WtkSWW)MR)g$W`L=g-k4yu z3wu#^dZ^J`M>Bgyje18Lh|w?>`Qw?d z|6^K|=n7ly`*5eUUc0ou^#k!T4{I~1<$C_OMQ&}476bsCmMfLFV`?7v5_%twV3kPg zaP#mkV`SGVHZkEiWF4oV0Md|P--;RE)VYbg9*q&<_$qewCPLC%v;=?E;M~rb0?6{r z;-`v+fQzK4Qa&&~(S@V<+^{iHLpJ8z<^*^#8r0nUv?7~Ik+L!qQ z1?^)yPbIj^xk}7j4ymZTJNlVL!17zEXd%%WktnPk9;O~BS$lP>H6)5tkLp zlKhm>_q);uGh*a^a2^R0*a&*kexxSMBI4ibd&T3I0PFsA9s1Jb;rw9~M>_wp)%d7k z&AO4W(9KY%``G~J@<@&t8)E|olB2x5{EYk1XO;U0RWC(eakFbSlbWzn&jfz) zo1f)YROkTi)o6HjP~>WZ_n{&ovgWa1_K~L&#elVctOXExxaEFpDu`2R#a!_9KA9v`yo@u$bo{-+XByc;~M z5!16BUtjlqI0P(Ps9kGWfPlWh$fL3{m=)SAGry z$-v%=6)g9jO@xo!3H`Gxd@RDYw?rN%?X@$M$4gFg5CcRwu?6z>{;m%8$7PYk3aaY z+pK3Q0K7-eVEx}GpHeNvp;tNai?<%cU1GtwKWU(E0S&1$op#?p;PZ&%Ir!mx;__P- z#+KOC(^o#$Jbt}9d!!VgxO2z5l`&6$W{cJP>quprwqgdUN1fvU&zuT8o|3=)y%Osf z_9E0j;)&cPJx<^N&`ik%V(Z*==TR>V*ZbdmuOR9VRdN(b_~K$QkZ=<>Am=34UDzA6 z%nzu7b~5>Iw;mk_f4u(jF%*D}unOF>6Mea3tV|8mC$756KTaj^A94W#OFjR)$s-ii z2MJ7MipsSUQ5W=zH+B=OB zjn5-Os{z>&#)(Ivtc}J*!ySv80|54BX{6+T3!*p1)C5Q}{XT6^mFxh%c*>uKJ$vG^ zXYs#|_ECAVWS}by5MwjGxA|9hGX0!j#=tLs0>YliDdh*nhQ&4_|NS~0f;4Rhm>Nw- zPw)uIdYr7zb3TM}2jB>YDcb4Qw@M{vbzqNBk`}9n!?XVvbmNN`-tByKGC<@384uH* zKv-zX>4$e2 z;I7^C%i7+9bK73IQt@A*{}EQX)2}XRvPFPA<*vHw%OtHlkr|*5zIpB>oK}Lqt&n@o z>$-&Yixa0`oH+XN=!S6`PgADO1yaDRCy#MB>$sBH?U#Lzcz7l!Cv_@L!+XiS8Rg@X zA+2*uZ1pPip|Oy-F>}#DK1fv8tLCrzSG(lO^{+~Iw{e)wt$U{y7<(j4TA-UMxhZ_} zKYk8_jtwYDB^)0pa}l^R2fLPkci`JMcO%qTpL^9eSL*lj!#QLa&*%Rg)muN-QOg1s z*M1^`@O;9%S$S6@$-707-yLONp(3fXGsZLKtUZuCpEh8)fz(qB(#_#oY}JZ=iR8r9 zDM$KsIs4y6seK}>%-bVMtSD{)cl4&k_|1jI9u~&oiTy|ZcLw}6GWZE&U_ym=U1$t6LHh|A*D=_@c*rz>t}GLXPn5;J??|g!*7GqW zOpP!>#Bsb@_`Xdw1L8D+O(jq!k8}V27JDMdaUr-j4N0|^D)MDx|7LB38kVMDQ^Rl0 zd0scC+1uL)hfhXtF22hr{`BKR(lrypTJ5Y3sl|oNfBtt6ySKdZt{~*!5Z+VylhLj< zPXxLynL4tJD}LKUgx25WU)@9GFMBjK55C&0{G5vH6n&?4y<8EJqs|26FpC_frcmFv zK%$3ig(W~Nf(NG3naFNqZrAee#=Zm{7e)MAUVxjb^DSX%8gm=&CUp;d|Ee$6^-!5? z;*fQFGljk;&xBFF!(=hw7>fbXGj<9N4|jX?|90<2JMGqwM38P`=%}rFhS)r2R1xoe(rC$> z-~>-LNB-22ZNaOe$(AMjG@dfmRB>o#!E%toS_9Xn$(6jwMoKE?rBp9V3SH02aKK?v zbM)(0+4$h2LmDEfaQ^p9JirP~z$9&nJsG3eR-x9}5P)QTww+5vo@-iO*kRpszJnU> z_j3v;@?GJO_pUQAi&=OHLbQq(OUvK?XOi4=@y^eZZVNHn`lYy|KU%a5qmT}@vp$hc zwQ=QD&SVjVa-XRP-sU6|WtIZ|Qi&n|kf^n_Cb4s%nBp}09LMt2L+(HIAm@;t0?Zhk z#F{s)CggDXHOyIn4m@~m-B#+}tVnF@d3$qR9B`|(pBmr*c&OnOsFnB!{Bp^ggXBp2 z%dgW7P-3}UIp{GZ%G&sJNSOEj{Z-QK+VLY*<1UMvkRzcX#ldRNN!(}<(qGP9FPiYx zma1=Y%+}ruQ zYU8-K77aQ+z5n&r#1=P2wdo7<8IjBbVB4-pUio{3e2|=4pN|DxXYB0-PKEECXLK=d znqJVK);xU_!rJ8S%EB(Fukg^nl*l| ztOw(}_Pg)`efU3Og|jfWVr}yjm@_XEx==5C@8p1oj+t`@578&WZKs6T@3W^}v0#b@ z6^Ylb;D#+?B~Q|{aly;+8b0lUev7Nw(=Ud<_SG3gyzGyn;I;OWeCtxQP$K^PA@OZ> zn$kvB0X*&Hq*|3+c}j?{A`CXyT8@N<8ATOs@f||xzhC}9DYByy?I!cqD+YV$eAO9dTrOKAXH z+1m{pI&nN-l*0X&acJ5$cmJBwh3=elSzf|u%~X*&S3BO4RJ59XRtIe(KOf_5!eS?xqLFup=@FC(1!F#&`%7^=`c@{qFwq zoB5(|=zM3T$VB$BImmd!mj{ZqTni~M;n#1F=L`m9;1#WPtEUeK_UX zVzoY#DH--mkf;UkTKjCxKqn)Eh(11J;Z4NyVXOBagX2)@u`3cYgXHJeKVUBzvW@IdvNLwXwF zTI426^3(JxG-D|hEsM6dHI=&*p10B}G!SrmfHuTiZNfAZ8Vx^o7>jd9PiB+oyFtiLK1DTTvTN zd2>8S`R!&~k0IQ#dysJLoeKh^XVuC zm|;hZ=9@##ak#swIoM|gX`o|Nx#TN*zjQUKqOG=BskYlYYoqPEv(76ai;A03DQq5?~XY`(o+mdMma$m9KP8Kc)Q z;qW)eD_|!N#e~&L)4-4L-vxLg*Iy5^1G{+X=K4l1DM>Bssqd=BzjkU?PN2o-yF>7S zwkivICcgcGpcr2#DRb(xr~G!uI7ytJ4Z={v|+_qCHm>l0EGQU)6| z656$S$U6Opv&M1=S$zOQ5Ii-i?i^8a@Pq|j`!)5a@3%c*Px?14(>-nLNG8|B^uRW) zSDOcYSbD@hM8cJ$qgPl z8liRIjM?0kAeA;^hsW^4NTO)knn8?j30rYUK+gV_z{Iz=&tseGzJ8b>Af8v`5Z%(k zn|%5uzW~Z9TWL9dg9^4m@t6zPpd#5KPvyaTM}A`y-`}0RpO&oK4Eu821OxE8(TZ+P zQM7j7aJ73}ovUs{i1?sJljYtH`KaBGOXfz$-7dL!O<5pzM`lsovX{l$xH`c0Qd=Vn zYV{k2w+j3m+9>OC7o2$Sg?Q%Ha(K@$xL&2yGnDS@=T9lG_fF_p ziLF{*tGvH|jrj~afkNAtF{{cObDJ-a1#E=+&}%_EJ*ulkHEvcRL99!k^SFBT46RXs z7=P$J-go|b&;6*zGh)<-bqjRUScYD{>m~FH>F|8D1!0PUoPmEw{(%@vKG%Ts&zAC! z?qz6);#Ng*gP=~ixV9QQmH=Q2aQZv=p-dEdZpm;VmlD)KH%iqzcv4geL{C4yx*mO$ znq$^&1We5Opw?#}bN{(0lb7xc@|ov8X!|#2s~Z2o8fKG-j_@~5=^(l(3xgSWi7q)H zZD0UqTQ;0oKD;!qV42PfcU-!s-y?3lkkC_FN>6Dn%)1V~u5Fc3b+dhos^SEnivnPp zH@)iS*mAx1POCaCfH=|6Y8E7}Me-DQ{IeG<81dtnJN9*t7HUk|=(xQ=rzqlb4f_7^ z1F|@OytLw1%3;YSOYu)64u0u{ZMz@cLsgQBJ5E(Sr~NrmlGPgJR59EKt@5#k%jD83 zdGM8T-q5cd=<4+z24f)3>U`!TabJB&os2z+=q7a<%^Vs-g?mN&5PUWn!@~b;CF6?Y6n=7+`E<*Iji^h>dhw^X;s7tgP4j+!ofg8 z6JRS{=wwiXCH!dc&Ut$8<}SwO?AE)#XBP8LcKdBFyAII8y~<#!eP7~jGI$m%eqbO zeX8E{*lnOIkvwrp#gB}Em+v6`@CcFUE_~#6AohnjZnLUzee)JBxXYp^IvAn(g9m8G zDguch9y-sExmCrrkiGl?N4QuwZM~rBkL9Ni502McAmI=3gil{d*)ud~P(vjm-xbJQ zy1qFsoXW3qN)tgiNnlEC&isT{i2>OkYp$?E(pA3mEWjzekU{5R!?wi)c1P8B=eD$r zDlALQMD}$5x{#(}RPs5`Gi^?nU#D^DDK=7BvEZGa6{Vueo2?eh_qcaSuqzDBn_YYjL@!Y$W8ImJXIUEnNe&L|^_YlNMf{-+sDFWg zfvvoYD(jZ*QawBOhU+*QlwS2|Sy_SL4McQn)F)TQyHmEOI+{+8IcNI`3er>^S%5%n zV#Fe}l25cM?>33zh@O;sqA2)0%}r+ezS~yOa6N#Xk`>)+73)B;>I zQGb`U2fG-@igk6X#&r*LeAUnEVA-n5BUf(_`X3NC`GPGSps}?fON2s)#`N#A=N*@2vy;)9eou_4Bio4*&3i*Hb z(+O_n4dUcJXIW7$Bmtu9+!!)(V&~U4?sG-HHRu69p&K3Y ztY`}_-PdJ#-g|T4{zlwk)e11#nW`e%H@^W%*#SAJ zc!tGYwzsTUc{z8$Kwazie;PoG#rbKaxxQuY+pcOQK(^`YwDp|-J0$k<>9ePvME1nRCs(eHjUT`l$sue?S7N34p89 zglnq#T%zlcY(h|rRoz0SH;~F!`wjT3VXMu^0+IEwv*DfUe3&aCur3VU+V}+v&Z5X$ zp7*p@blXLgZ7$6=mL7flYM*yjNXFFN&9lDoQT>9|AP{~E~pQmWPS zMP!{YpSIz))P$TqXBR+dN>d|Va&UiOx$DlzDk1*#hS9C^BbDGY4SOO+`dha zbu(CgtEh>cRmyPvz7y^Tzc)a4nrwy5v!I8xUt#Q+eVa1Pyd7kon|eX6Y5U>=%o zff>Fv2`x)KIIReWH)yTws23&F*J>Vo{YzFAFOl@46U(}VGD6|)H7J1PzUJ`2O@?R> z(By#ZNKqU&qFtf}P8s-R`lZauXgne8$@1ihCYJZUN~Ry74Ny;mFyAcDm2v@!Qx?FE z&72)hZwY@<0tz9=lC`#$Pj0^Bs^;#I1>g1xX~ zc6^)igb(i~3*#`Yoi01|9jHS|Zf_Xir$^XG!JSq{QAkcvCojXdHglZ#0s3Rf3Pro3 zb_-L7=S3=93n2q<>)tI8fx_Q30tO^m{PC8CBGpLSFTQL5-4%IMB4c^LhI>zu7j@P; zEk@P+FO1S9Mdv2FEthEc4GE&y)rNn)DBXM%cIQtJ+hv+bj`XxQ7tZ&zZPzQXWt10jn$^ErcOBDF6RFPPJBrzxkxItdLX-DPY9dW1e;9U?D72&<|WQ+01 zmUK^bg}OEO@q5hvcJcqYAhy;yUs8d4)>sCwnmostg?VQIc|b!?e9sCpv_5RPWNu3ZN&jYj z2M)HVyS(DfT#v{yB^fQvx3S1;^tX&i{c5AX+) z8ao*p(@C~_4Z%j8Q5+W}H1^M%W(Ri!_`ad5fn98ZsQf^ZbelG|evWn(pFUt(j8blt z%--!E#AvK~x+?`Qcao_#OTE5-2zFL2dAJDjKnZ>Kbl%g8+Sf&r&5$%DQaSdWF;F&W zE0bE0U@zpNQB85(bGuY@|&yYPJKYw@RsYx0gRvdr#LS zmZNc3f`L+ZxmtbKFa*k>edGf{bD#?3xLT6us`zaT@rAE!dS6P@FHLyP(=JW8f3we; zCizU{Fr*YDdZW!ea2$y5`%|y7_x6bEq=QNLvXDu_)b-q|>!}XcB=p4lC7nPSo#Iasa|bd_Bpt+p1vfxf4PvMUVEY2L@0P-IFo(1PuU*G$Dte)# z(QkmwxcUA56=*+K4NwhDP~YM~ZAA!rto5C0*q$X$6%u!jhtg47P$l@XFW{9&HTIg} zbL-Q#p1?15H%`GjH+R`=890hJWw|tYIn$4G@TIb-P7BbKpm&0&GFZ`O4(VqAt7bbQ zw}+?#N(rPRET$K_2hLN0hrY4qKZdBP52NxsXSz~f{Y7N&cU0*N_3$?}$(GS)k8fE# zS*dpMA@I8s9g7{)To5qltlM+0V%C$Q2oPw%&YdUsS$CkrGl9rvSa;U10|Ap2sN7Ft zU1fpfU}5vB6xZ+kn9(58-tq_D~+wO1N3EpVd zNHm_<9nWZ3TVXUX_N!~DJ1TMpVdV&<&61hQUuXLwe^>r^-pyUPt7;Omq9-m*^BE0n z{!Zk7ezOj-5PqX+r4?6~;}fIEI$kXY!PHcmL8OKXMlnW~Uhq*y=#ZY8()=dwj-W)4 zWbJyF+*J;7)XU-8wGZ-+zoyAIMzL&)p_)G zdFZgL1uBG;cq%d($9Ge|V^F&;p&NKOx?Kb{MUxDP%U?c(hy!4I@fruxqCzW;djf(X ztKR6nlW4QhoiNngg@aY?(7hP=H3`o8dU^n<5qb{Mkh{z(pl3DatcQoL zxkpf&xbg)bUxQ1*=5P$bc^_0FXAohx>Qpu3v)0}YzCn0X^@+C7f5IevGw1xtMGs3v z6qn<$u3;`$S9wp=trnie7M)Ipf79H)q@-1`$xV_U zb<6DdqEpwPqvu_u!{XhpU7 z^V8vbdJ-Dj@{BH{)B?3_kwn{rT|p2%MQq^w-X_YSf?z)t7I#$g++X{Os!R%fbYodG zPg&DXojXeVQr_!oHuA-BSOe9J)CnFeNKuf3@W)A-FVzEeQ|!u5Xs;{7APVcb(l<5{ zVU@v0Z;sR2bLGJHFtR@qcF($WBrHv9m|yGwmv?hc|2ouoT4Pmt=Bg{A4H?2p9Rs;oP*)QsAFoq_@c;*oD`jNX|d_o1bgekf&i zM}?A@bGz$3Zp#x?v%7UQbld3Q`(8q*4mK(X3V)-B+Eve}kVRJHrrsm}dVXI7lXCq# zfWHT=?7BI7i|_E`fhpBZ>%J?aKrLqlnE>DR4qY2Sk@oMPo{ZXh(Bu4v$3nssn>OTE z^p9r;ZygofZ6&T1!gr*ttEYD>@!KQS`#SCtQVkt~q(6izSeIRL1$WLL>*Qf!W`;1GfhJYtVu+BvvNJt|^^bO3klxFvpkrkt#c| zj|AItFz}1B~3_n60#H_cmjU9XbfU{<|#~Hae{@CR{Vr32L1wo!FxBl8Q+Uleq zBn$5U5~NN_?w_StHf#`-AOr***0KzT<<#ktUBQpEdCXf0}PhJ=h?Q>4k`G-PYp-1RCsY#g3; z82Jy^elb@fUmEglZc#S+kys-pHqarHAV#$c^_Mw$!5P%k*marr_3mShfsn%9137bM6firy5NC!poX< z>d&UB%HoY!@bNpKmdnqC@=heDBJ$l`>$3960D!skx?w>Oh$0~!1})3?`%r5FR1p274MK+W+h&-t2%J z&8;rk)+yRvd@L6YaKDOnf3;=j*3DgkdTPCo zbr@7kRADP0XjHfYMJ>lPZ4-?y!2|30?fgGsfHeE_fuQc7t{F3Jb{$Im<`n4$hzPhz zYSQ8T51@txB_-BRt8SwPQKgx#idO>E^~M8*9a(abSv_xj(4+<0IxhY-0PP(IV$km|Zm&IlS z@`Bx#yU3jZB?p$o5oyP4D*%>ynk(gXum93Xx(^k-?WG@)dId;mO?y52>Vx5*8;{}a z#H>Qsr8TyD;b`{#DWk1%;43oWUCiU5lm7 zIB#~!<=NKrAwmd=Y@cotjrqQ4cfrHDc|k_sp2IAz@K2)`_T?lE)N=3ko|kcvWbGmj zkx?$fzn+k4lHqkDQCa$k74Vs$g)&s=*WJ~m!GSBjhO=!-V$3GuO$8h;tfDszqDJq5 zttAH&j(5o=Ivjac4evoJ)^Z>D(XXS{hm9EGIYI&gxQLi+t<5>xy7B3pyV>kL+aio0 zF}&c40(t^@(^O2(dKpcnwV}k2ZDnt9u%kEnfoh*sy@z`i)8+-#oQuYe0GvTONQ7?@ z6bm1~{%8SEc5qmBMSUrCAJ_!w!6a&SjE-v041#hZ^bHFL;GU@72zLSe34x1NsD1hd ztad-x>?3O1GsMe&EQV-Q`o~8q9vSiClYaPsge^~{;T#%iS;@F6uaaIbYZVNe|3$?{ zHcxS#K<)U2R6&aFXpHAdbDX-zKaiT@fNXE!Hytv)`mI@>@J^Yy+~j9gM%n}PBTnDR za)>K?s+`kWhCGm?(^51zISuX_TzySfAy|24^G@uS?X}BxFY?KviSkYlu9n7Jz;N%7 zp@jl`6CRaXc=|>Lt0U@iH|I>OGy-`=G`tm}F}7t1m_uP(~^)Ey>(C5Vs9Hu=Lpj+Z#eE7?n2}&nrSDmE=G}fuC;A7AO1{vxm!S zV`SnOWJ;H6c1Fr&^{GZR(w`b246!~!^L2c?bk9FPvpAeXr-v9G4F z+jWp3xZ5sB1@326Z(0bL)}wH0PXMIwl1qzKlZhF5-v2k64kO3f{Tn@b91O)g4ZirGNXU+%uY$B-Fu&p3X61--q38KzMW7%$@Xd^5NW9HclA-$zD`V6> zo#k0`MNAfQfj>FPBgx%-XkxCDes5vFO8apPZK!e@ixktkg>Y5AwF&g_Yl(KFov}wA z0s~HorUvLhvl5Bzi2{uY0AY=Gx1jsU@clJ9PpO1Q=v0Bm)=l;OU%gxP8VtG;@twnA zDPE;%g9fEa9*iAy1QB;b6!zJ$=2L;Q6M+VHKXM;VyBS~MFL9d&jitzXnxCfXBS|t_ zYAIU#${(u4YI=Q@c7s9csKD}f3kiR1Dx58IgLch34Wpdwd-PltcLgC-o8bPG05o9C zNF~-c_RFwkNZH(nOu%=`o8MZsC6?pfStU#mLyKPtbmzRe8d&(#?EBj+VV(#HMzkr0AItbNvP<5)8i zIZX-ETn%6F)UncW-d0c;kiZxE>X)bK(t+Qg(vi#?N=T{(8( zxgU_w=-IK5=;I%WS&qdTR?GNrYW_Cxj6-r>4{hoKVmP)sB5ON;8PG%~)k8z0K-@rM zm6Paa+x;>K{ePQ>i=`{|icNROu!s$4rjoR=K?8nycZsyEs#Zom*D&Nxsx(GsuhJpp z46KxYZEZCmLk}OI+q7H42C=TEm=sw<01Vk3$+?Y{?`;HxR!#2YRYKs&?{y)h=9hH& z+xcrV$nuzC{#~J_f@S5pP!q#NfNr=-nR&6uOSmi}WaUJGcf#PRD_ZCh&ju> zi)9u7^OOL-y9#JR6lXXB$Tv~#b4x47p%k6J-9y=j|BOvq6m`ih>0tt*Q%9U^qA@*} zpSR<#tcUS6&>rdHgT-@QK7&iP0}4};BLZPtGDczpZ8EI-19$&9>ZrTcxLhq4V#L|? zcIbZTk?Xb8i%eBu+lukx!sB+3sL>&9(TJ?XrV7+%0gq(%j-njDegah?_p;nJhIkp$g>GBmJYB>$# z;QfV)z68RLCl0Z%VGg|EU)dRY=t7`{GneeU?*nUVWEa;gMM3bj)=5~R*nFknr#*vm zMpMa@<(Jc;(-IKzamAm|Gnf0Jr+|{E0H$@nk;{Yx^uE;N__AN--^R|SdNxh&Rw@o< zEeDVrC6oHS0z9IAsXU*mIT}OdclVE{@}q|WU#5Q%_2bd5{kS@ANhzzZ5Gr`#&yV%r z$`{E$hw}EL3)miD1>a4*s$|9b%Z-^gcJ!91)wcpzsUWQn`d*8uLQ##o5r5SpJU?o= zJ6l^+jr6JCRgJ{#(dKYasxyRCTgmO%Bh5W`&z5wSPGZ@XD)W7k6|SsW=|h(sFsrA_ zidN?FX*fAVnpsYH$v9~i9WRoX&)8IQtDKq0AeT09FBU{4a}LzFz+ug^7{PN|K%HNK zAnUibV*l3NB7zVa&it#CM#MYTtN@6yRnm%&4Zp^ts>*~S2= z&i88mvHaGdI*zv=^0ymT|033H{o;2iImJ;R$T!`8Dy6JXRR<=}nz))mcmkfqo}5}H z%m!A#<~(~QoKoG2wCy{2$YOwAR$6Y1$&t0kev1t2jPYk~ZxrvYdNHx(VPVJ=Nr&Jl7oqMN| zEp;4!xc~z~V%#zay143>8|SO|eZAHzg}AhoCz>f32m*q{+Dt%z%eZX22?Cau?Y17uV2NEv(c`q30+O@(7T5dJhPm3h4jA-YjqUA=i6SxQ`ec)U!wuUMBO1bj%n z=(~-~vLI?U<%=fjZ;qK}sZy(#+ZC2M1}P6&uCb-S=5b1KwtIsG`68L_QTioovWHU) zbp2MKt!^9-a`b3A>ewOr{%*!M^u(aVPLk(W;S{z|itc78-hEzyuWmU$)czuxmB#L(zvk4d(!`mn2aTp*YW&nj#dd^*B5!pS)eD6T$9 zyVLSD85s!aI#VH?7o`3^Al-^iuUWNYD?H5^Yv~oe!w#-2HNsfp3asa1%F$ zb}$w#_IH;Zf2||ZrQsRUMI5Y%N*md{6`M0RqtS2|+U@yZqFmcW&|g)3 zpyIIjtYh?!r<%=IzW0OdzPSy3bBc@n$DOyWMpZi-8qE$|)i#o;i1__v2Oh4BbE3D4 z%4ZZ1CELjzqOXpURMB;iQVz$@?fVZU6RMRg4FmB9o?B;PE6q%mN!9}nEOPF5usoK` z+je)sNmIt5q3J9c5o#WB7nH#zip(b^(XMrX4^U_BovKHoXFL8+rf|W&SQH-ZJyHNZ zkL5AeJ=D!;{&N(<0v18iBbLp{WjI0J8E=V``?s!SR3o^f)%QbaAgYCYQzH7m2OP--MM_wy`NMhGMDEI+uY z94vIMU^lI#@iCUi;Tk$1;G(8Nt@C(6orzQG^MyH-(5HhkRJF^X#^SuXE9?H;5sSkb zN>2qj60Xe7^70^-v8L0Q(EC@N!xI6p0<<4!u+S2NP*sRU?iQ+}?g`C?HZC}OL16egg zXcwrrEDx>T^{?A~?Bq#;E{K=O+XMj?Z4K6mDL&7sVAB=oS30f-9P&16@X7c!Jb4NtX= zSCwY3l|=yu8JzQXEiC^!AzNxCG_J7R;JD_YyEvF#5(7F!I=EZgNb2$fC3%f;&~cn* zRb`Tf5IV3OH(u0$Zr$S z(lf=D1G;W9{{|X_fC=Mj_#DOaRsVtTkIMr>Lk2gAFSc@qgZyFmjZ@m0u2+?8&vqK6 zxs&iZVHrUm1%3~U!Ty@g74!PZ;+yR|4e^hAX43DVxUt3cS@h6um}K+PwzFn^Cw_4+ zpDn!-EfCGX38(%G5AY`h*zwpaJ#0N+k%bagzJwZra-q{k3g@y zIqqN}I>Sej!d+BZ1sl9jjL0cRTOg@s5CCp`lq~NGR{tfRC8{ zvEv3N;hoR@REm7t$~YXa72GO`If*IRiFm*I%jx$t75g7BusIU(^_#RAX|1W?g>yCETjK?wr*4KX+819(!4cg9mK=v zxi2h1aXiN2tCGMd)f>QS&7{!h5O02-aEKflai@fM&(9dShDkF~_>fSK@kEF3-T7R* zCO4SR`ATa#IsqYR=FwlJ8gu(#?9NDK=sS6hf?G%zhl}^heFnmsaTrSyL?Jk=kZ0Y~ zT-Q^hVEQJ=gn2jFa1V4(RG@yL6VTPjv2{=1H52X#FT3S%Iv3CfqzRFc?88>uDvk~#DtD6gFh9n3*D}mT2CQP0mn$ua(&Ke zDJdIk8Mc&MjNTo5%jIb9U!go1^NeC7D%q~B#Y!qgoDMm#GH zRqOxPhqZa=+fi7JK+29QrYbl1nhOKkXZtJ%Y+UGHv82ho?B*{$SKr#MI!06MqaV}kelph$<52n8F!*=r#59?$UrI4hRd)iQsCK%0lLK** zB<5b7LtJ@UFMv;u4ZP03j8&`*8n??-(`FTIg@fqQcVJdKph+-quP za1#QMH#2w8AgSuOAH|Lk{8BIxp%ns@7@X#pWD;IWRVWOWLQekP`o9AMHa=3wRL42UQ!>+y!V#X<|)9YKj_EYWE>)&gY zhuc)3`c#=>=saV|KzMqEUBjdqivs z`tU~!U3~(X_qj0jUa5R@M2+TsHc+gBF(x#;e=bWWaQ+_}Q_}37)(f1n^F+q1`(4S# z!-cY}j6Yxay`Wa)9U%|;PDQk*99_>fPWb$C$ZhU_v`$M8-d+Ide45U-eiHvc+R`S1 zgPHz?*~az#^Q5XcU1@nR%zUcxYM0}ac+u#Gp9PaHI7PRBoru5I*dL*c0&8osHz}c^ zR>m!dtF%oU{zLnp)lPeIpS*NJD^lAsvtp-kd>}SFP$HLq$sDL-b80RAAe!tf+eMi>l4`o z9m*>f6HRzZPIp$%`*JfK__-yr;`+mj4t(+NNqDZ!H;1n+aN+-Z?b+mfs@YTa6N0P2 zl~ZAL4(gd7lF=%6OnQuCkkeaPzt=pWUY*KjOo)or4-R zf`zVep20}`-im}QuEpOE^_pW!%n(tyY`^z$6$udml}-_*VL`eDUO+%V zLb^fHrMr=oR7vR$rMp8)1eRW6VTl!xT$WmDm*1n`d0#&>|Njgy!|crNv*$kNI(41< zpcvYwB%zJoNDXB~c~RMhl~0I}uZNj~-U1Y7T(AOtxkd*O%LJqB<{V;shb)Q%Qu_7i zbyA2e@~$6B*xoRLz0!62avp0hRRre9Y_RgVKlbcb=j);BKS%KJ`QIb3E)_U?T|$)> zOhOD3Dl6zHN>?AMYM+~Kn~?yoz#5fB(aK+oZb@=Aod-=mYjsO$ ztf)~91-FG0j@t!;ub{L2JLT6VGoc25^eXI(?nR!-Z$$>aBP6);puC~SN>pFt!@3XG zOZw$$3LcB(p&T*^QqBwen%0#H&}J z4i#eIw&z;X*hc*pnm3WsfA7xFqGzt7oTsXwK}=0FFdfS7Ay`sreKvqFpk=lJrqHpr zUnff_7m+5>T{*G=1UEOq?OBsn`tdsHBR{S5+taudGl;ZDwh~ZR=mKp3$b@*Puyj$}rg)cnw{4I@w|Jt|b-! z)d2Y(^Qp(V&(<=g$2-xL_c2KMW#{+)KNjb0^grJr`9=s&m!#)(#`D7QwwOorl z`YrJ&{ulq9F{(72r@+KAmIb>e^B#^MBU$aSlKbiL^BB| zdi8@VZy^=Y_u72M&bCUuuF3lAu

QM~FK6B>R8o3ZN7RAsNR^!fOtQ%}@4QtRQ?M zVIdbD>#4{jRYuug_$7f14VzJ`ZJ` zS1)1+u>SB{xBB@0$CBT+&>EF>zdx+k_r}=6WJ8CfG?-#irgS%cyKrf<^AjJ@H6wH9G|DJ}u(E74C zShuM@oR9RnG`Z}mW%4+@u)Oq;o<|#N1fAF1TIJ8?e)LDy=O6x)btW}QQ&hWA{r+7T z)A6Ot1-$(Ircw$%f~N_x92D=r=qW8Aysy2O>d2@Iy|3N17kIGCPSPOZsPo(M*0?_Y z_eTCQ$Mjn&^5>+t!h%U|A|-qLWDigdVGoxM1{<5(QBrqJ*uy7jDg=~w9?D-N%3oF< zWgML7@uS+y*Y%3cSXysDYQJ*%`BnGWS4Kj*kFNV0o$<80X8x+qu zW#3N#uSxlATJ)IUCQVXV^6IOlP-6P6s0Y&8u85ik*FygsdM|7EACLJP3ceMsP-*Ka zeV=M6zZGyLmskHSjVSRAS%0vrhy^d5 zR=i?TFWIEEE@4 zT5H$csT*RN3Y9EYj2tOZ6gVM*0dTLm(YDRM+V;$g;HEZLtJl(=yr@}P^hWPFOK+TN z1foK_9Auf=!DkCr8(0f#Rz(6nVtOWX{^y5+Q1KDg$mt`|N-$GcGaKOf9p74HP%u8X zp=KE#s`CeLb{~h|R-@x;q7^q#oPQ9_N2VsXnDX z)i)V?EXCS{FXLsJI*Hv5AyQ(%imaEZqQN#Rk^JeKg+P&_Csh|| z4E<#fZT`V0e(K+$bfu8|?sp8lp znGbuSBtV)G9sBqb_F#+1&LNA<4o%Wqm0-IanjJ0!+FK!9AiH!L8^d=he3Fx5|7YK^ z|8ufaHl2RJKE1}W3!;npiJMcQn6u5}D0`v2%8mLHy4mU5jIRbhkUtHT;VMn-Ws(=| z{wb=QQGa8_xv{75UZbOJdUSF2;24iLRia#RC{yTa+JtxF#DEau51tbVa|H?% z6J&F7UhMnU#3S_}VJ1gKx?1S_bN%0f z%VhtHhpaES6e7uJr~14z?0)1*$Kqa`|77!=8Y>Z)$!sjE0f#o-Q)Yi=`Mox?$99o& z2EUijM?ZhWHLtT@$c}37EfBc;BblSA>vHfFOlt(QfKfF~>5!|wo)97j8zM6M{Zy#H z&#Av_Q@8zkO{bQT`gGm4uX-W3Vng7B4oP1z^qf7q=Z{B8vU)T7*=cetWQHI!*nQ!T zB98o3#7qriM?VoLCgAFVN0{MOPbf)tW7%hQOBbvyQYi7ajo?ooQA0$_8(i4F@`Z7Q zu2h@gxbVKsrX-4x8^|zxyPs_%0MDl)8fRf9W%hVS;=*Sp^+rlEUt-ZlNYqM7H2;fr zy=u5u%8De|7>)BbqqtbrMQ~|0Bq=24v2#5OCt>tz)T(zK=!&zV1{^b)Zoz9+-!eJ? zB`!d*^qU0_JxiKgx@}xG`scxG9{q)$2Voxwe-Xn3w2!D0djsP47uOCT+{KZ8fL&xZ z{3?&4KKRVkoGAog3;HAPLf_kqb~df6k+hxxymd3+ny}>t##?JQgPvd4ilecXyb(RThrW!#`PO(O_f8uaDDVYrQ#gs zKLHVzbKmL951I{e0;?R{+@$$3fBRiRuWeR;19xQuk?=zEqYvwBMMH=0dYp;+RvSZ6 zEIp!0(6oFrUMjwYs0j&u6SLOUx+N@q+n=Y+l>hs*gVU}(i+%}UOOq>q*S=$C>HZ}; zOJnCWz#0_ zO#*m-75lhB;v3NcE4GJiClz3q#(w=6biBP(uU`VX1_sxUtV^kGQ5B{8xXxE+axcO* z@#Ff_O7IR}jn@pXPsShzOOVR<%B)QYyFy2Rvv|*`|Ppyq0bNojOdQCK&m4J5ZPHjYVXj z$tj;i&tMc?lCu6OUZNQGwO5uV=J9|9yXDsI?V>gLKmI@VZy+I2Y{AgpE2<)y!!Idj z=2}gOa3r=^i~}3SP5{9pfI31=V>K-S@fISFZ>S0;%%j$0xyB|MdKMxY!D(YGx4ZR+ z{e_su-0Jx_FG-ofGz+cymDayuq>=z`SxcArCMop3kLYuDz{*;)wMJQ9#uM9R5IPus zIVY;PXFz*s@jNBfN}g-nYeHPV6T4acz{5@XrqrXF|BJoa{>EMj$|91HSjX0QRApjk z`dAtRy>oq#IK>v)i9d&KQb}0SdY@(3 zrBU7?RFfuLwnVxOjL&$$UI4dEj6UA-s#^1si5^11S1Wq`NS>!q{*=Gd$8~POYt~$& znzHG=$re#CCsRel6xLbSRrXb{G-r%0k~S%n0%^276l5@X)zQhqP`=MfzmH-@QU(J- z;LRP)k)I;W*<}Bk*xKE4Kg-MaZ84V|OQB1QF4ye|p(e26;&jzgmdM}E^_&a_y+O7f zJmDRKf)XPtjPZaBqu}0Q{y%ncXvdEym1N3x^?Yeu%r{yu7t_d8>I6y!CGp>pJm=cR zf7=yB`VN_+%ct1+2!s!)R#@=RA=b}!n{d{^2xbP0XieAn?SGQ2c^`oYlP0!MCtD~X zhwQ-}M<4$(C4Xh*yl)t8VeYYee_{Zoe;*pCPCPHVmMW7M?kED&rrWCt<_ySG&d}Ky zzKZ4=qmCD*cG|11=@|BQh+OpsZXJ0t9HS@j+FqkZEmXf@XejzYBBhnSOv<{IkDB$f zB&GYJD$C(V#IVMCRaLFgYH+9&sr4l098_?i)De$q?V1UxdGDu94@X z`$+i|m@Rg5abSwme@$2<$TG$rH(*5_x(cZ6h?&S^5aCN}i|l}3UJSs*nPf>iJ)iiZ zjZ_AZ_up{m*xljDe$-b=C+}O%#X77K?fP6JMFiFd%=Z_4)*Z3Ys&ysB|6xttiu{uL zVED6hs^dj-Zr$0%LLI$kDrS2vfmC*J7?Zl7o;k{#*xygh@~^+^VM2AN=XEuTo=fvMDMWXrf;Z#tzXYoDaEL&k7#i)kI%#f3c(# zg%U79ceW26Xn`2K{yhjFOM5}-XPJ?1FI1GS24n~6U%rW|q-+A2bm#;N;OhZY=r^N^ zIbJBV@L5O$rk4)Dw9F0*cu1~=KFGNQ2+)SmiL_@P2oT}Paky`VKqZ(Rcoz#bn9yyc zg_Rlw2yir(7Eu3%K+)LnfOm2Z;n4K+pHan?2R)NQ79F>zI_RxoC}Rn5Lnp8bc3f$x9(&XceY5#K$} zkL#%Xrlxbr3}~Y*o{Q3H5Gj&{-qg<)+e+o%nreLKCF0=H7)NPi*&e3}WG;vA+Svek z11`R9`suw^zUIcK!*^qcCJ*LgbICpr@#*h=(=8LJs#kD$7_(P8J%c2GmtdmiFX5s_ zuyH84s_2?9CU#HM*RTQQFkDAO-qxofu#gn|3L4>Mudrhy;gK*6v0{YOE_ zfd6?CCVO3e*Lmzvo+W_e6I!$9aadw1mMJx6dn>=S-KSy->;Cz;>^l;p`Am&tgfokW zgZPu`tOGgat(Ph0lNP}cBIQ9fKP0zM|KVJ5&-`%*4>wEH;2=i+CEak_9rSDlDD3QU z^DN2?^D@>0MCq!1-{zzHe|Zrzg{&Sew#Mi!flAW}WEC9Kzzy$@_pA{7JY>P(pK^kG zfp8JV2$4TWd&~rZ&s`;+NEBX2k^(e{%>D%mSYvZzKa2ut@XqVTS&^edvQV`|us|Ww z=3K~F)8|nk&$V_b^4T#sbnKZsFKeK}8k6CEv1wp@maAJs{an4ZWsDzQ1B;d-U|)s| z1_4(h55H;7c0A7hoKpD-*VG02KUKfx%Cr?6zP6=-TP;mBZ4#bslt14nnGk=Ek@*oT z%+}!l*hBY&IwNRiH#s`%NFhVYMk=tKZ3C2WNfh87uaZiY^RR76GShppXnV-jr4R{R zsM^uoXI^8+mkYMdNC(P~V3ic{cPO^^T|QP)N_YWANKpn*p4y_LXZb26_B&*8EL|uzdA`hL;kDn1r1VyN4}?7h&T}U!n|f3$YE{i@P|^K4*dL8+1;sK zN^rxzZ2HR!J^||7?{)9EVo;)=-}jn%=z5#N)n=&~HOW*7V)B)OG%!n0mc)|02C?Dq zhnZaJ_eB=WDh$cCVH_0OQ{9G$qkd|fwFzChfk$SAS;f<{U_NT)<)?Ux;of&x zV_apUK+V(Sj4H<{4@H^impC@`H!pBaXF9G<220)XGCuLw_RTZ@3&UX|y%&!=&>~JQ zkhQllbuQH$!F+tFoovcLkd;lzcWaP~vQ`&=&tv|O$@^Wxx<`e;kJ9E2E@pn}V$Mm% zv1aum^tjEO`)tXf|Ms})KU>}D0$J|ji4ijtF3U%06$uWPl@}sLLV?%twz&on_)`MB zhCFS3`31V|vp}s~Mg~7L^IXVfn*;q1>@F zV5!R6cK2{6Tn-N?ZPtmLC4zL0O<|Ib^8;ECD!`@VkR-%baTzdSm%EEn4w%)*c_Zp$ z@`>`Pk4axyA zC6WhR0bv*4F^GIK)uDB5;@8-eo{xRsa_O41ybNX19K+~fq2R-MkI{+*IAT^B)8&KEz3!gz z!X|I6(i0;WbIWYlEYrrU{ns<9TpW*gCnJ!S)C$@7vi>(cs$^~^#`8CRX*r+pyV^ir zM+gUD5{v>iP16|mYl1FmW{wFe?w$BUQaiC6;su;bcf>Kbob{-mbM_TH{ny4iqoC8` zZRQRRcWc&SMX2#QY2I79l!GPCcOut5I7QWmK(@}8|L_HASv26DDk9QCM zWQn?o!RH_@p1}jxdQY$@tg;0>vv}0MZA*_Vn6%_fb_9{n*Ey`e%Db>FAT{%J|9;}N z3~VX7dgyeu&DeZ?T17Q``SzD6RL!|*r8kKEM!#8o$YgX}mK$4z{)nas>Jb1H^+kwS zl92@BmJ5h@I|dfCHfnl}BfN|iEZ8Qt6dcsSpQ8;y2MOQEr6bm$8Gvn$8xz7Fs#x?g z=>8iCt;HzF_~*1OveSd4&J-IB^B=1DL*u}E(mP*}>`qMyy`mm>RdP-yL~rIq^x)~# ztQ&Y6@&hB#iK-KlW(^)SF8~+t~mF?0n<^}PAkc%cl-ReFc?9WIaC|GJd z=g&Bp{3>l`x%t=tl&uutiAj&7koB)&BeyYSdt;l?-BaQ4=R~Ei)=G^0Hqo(P#QhzF z8EmZM5ITN+en?D2P=)=C1z(76*NGFA<9_y)*Z4zO_J-1_2rNr>naur)Mt;?;1}@(y8BgcJlBPuTMRZ1~ z=X|cxRP1c6q6s_8DNc9UTblJbn!QdFGeV#JP#DdTc;oGf!@}L( zvdr7&n~~u3EwXEPoKV3q!^juw5cZ+J4k(mGs0SESEwKY7(x)p~Z#FAyW;Ax)wz0uJ#7aMV2GJfae2lfM++<^0GG?+=6~>_U zPVeFwbE*55ud7|(D6Jt)jZLnFdB4~{JUWGFj+HE) z9RU00kA-xX&vh$1k3dN$49jAh=ZPgmL8q~ZbJYZogEbbts`K+KYDN`dd^p>d`fG-h0S1^knSlqND>1&wz}}6)O{#E zYxyyIif~P32Hm;RIX$!-;9eE5W#OIarUyj){qqz^aP-d!+r*P)FD~fL%(rOoU-8$D zJLh45@9SU9N@%$_?czN@Gmbc-^|#YTv8umm_Y^&tY}}muUU2KDvdb&E+~Q85&5e8g zmtEYpJHVC1iJ{_ijnP>r#Oxe*Q1U6z=nVS-0GG{OH(6$v{j#=3(Nqwrjed>M9HYtK z@6|v*Sfs(FLrFtyFP~f7Dzym_9QEs>p3nwndyL%bgJ@w;c9H{)phS|%4wHcOU|*eo zLxL#Qo^-U;%>$;+Ho@>>(J#$zdvb8>-q`ufGDh0}V8E%gI5Jk)%enE5uXBvk)q%A0 zR!geI;(KTSS95#brGG`HZ`XAAY)Mz);^P7`{gjXExGbC(-%fsPU3&Qi!NI)l#(O3H zl2RPE!wGkm$3c$~y%ybXe@UH|-=&o9EUK(~MzLl176!EKfdcp2yG*(D@pm8k+~*&! zli*gf_0f@fJ2(|Rd7b=js0R$cI+4y*9k99<5;y-Q>}Ajyq;Z6>p#vdrH$Efafv_~c z!V7fouX)Wzke-87J^O`Oc5@dn53EmcOW9gAVf1>!Um5N#L6@K?4%ZLNpSns4`?{X+ z+Mqg{5ikX(YgPkVmPo%{-c?!{_i~W((dL-92^{tIO^DgeA`$6eM#;T-u1B}~L8m0m zr_ic77z|;Hq=1WVC|>?@#2Yp?(N@b!by@SYIWlXwK(-V)9slE><(>3zovqK(N%f0; zlv8hg1S!|3|Bt;L^;B}Z-DzA|1> zw`I2tikLV`W;iJ`Dth^aF87bwf-ksyid278nEH`u`XE9&aY=r+8YOxzljmrVOPZ@# zsm4u7$e&`%SlL%k5s}n_WoMThE-WM zX##&!RfoFWE!u&@wZ_d$b?QK8(SyUvO--*%B`-#5D)4RDS9H)DZ67f&@N6pktRRP_wu5+6}RN{3^0`?@e z9$&aw?^~TAut^=su(X1i4T@uRb_aA5yCMxVo4(f)RcIwC9mMcHeE%+x?Skce9dglj zFHA)j6pfA<@x~2v47J}vc#u#rl50OIJwg7otn`ol@_y+6wPrvop39$L@Z*QEap;R0 z4&q*Hix@&Oi(p-45MR?y8g~PIH&fu>7tOv+d*CUG-YCUP^FT; z;t!MB)d*HHd;P}FWl?)a2McfdO(kf@=H?-?U8K$JjD-E+H?Mwebz4)LUIqsG4MYmV z&8sx(JtQFBo0IhF^9kw#AjFZI0UmbksQ;V7;;be4`TnjjH0Ls2eT=?j#<9G<+nrMSzmT`7{4*x#_L8LRI%DSBv{$y$kaUm z^{J1D8Izzys87;1SH9xF94DyJRel*C&e||$XZC0s%tTMG?z`3>E^U#!FqhH7OCV^{ zs~;IxQs!75r#VaEj$XqU6-p;yaacVR?Qe11a0^*utQhcmLfb7J;6}LoVyr&yDBiu| zsn+{(LbBKqm_NK}8&-6k%2I9UYXXP1d+MVW*1LzrS;dVKq@T~C2%YC{cTbu~TS4*4 z@6EX{pV-84B{F{`RgX`+o+M!6kn%$@CNHA1mjh zdfif`Hw_kc@L%pchv6+QAVv@#FW)bF{gT!ZGr8zNDW8NQ8Z=Qm9DT6GSw0;JqW=Q9 zI|WGtKSd#8j>ed@^NE{XXG1~jOUszpdH6jOI!aq{N9Gy6~p!s2{ETzHij(9f>9`I@W~x`_@bO2G)R#fjpQ$g{1eE;n-x*Y( zN~W_rMY=Qt@8p>pRfFSzyV2^xRy35&V*xG@%NXL(ynwXKGY{!D{CUw9Lyk^AjiUwf z85?=0&6vj>+mD}8Y#G%=6+b_hx50RBmkSfZ=ez`7>xqV1EHP9O^GM-vSII_=S+%_E z`mPfR$k$H;wLpFrVrfSpEA;0bh{>7o^Vl8Ef z91w&bcKtj!^?+YvKxIzty?1G&k*1ngE75Ch>OJRqHeEW}6*9PrOZDy2Edzs2di6qY zO`E@&nM34W|H}oa@Ar3oagbe9?0_0CYS%B~UHI?@*N5*Br?PvNxiLMn)dqj4{O#DgZT_^c3*E^x-3Ze7y-$@5OO#gcbH z9H)__7hG7b;h~sM`E;6OPxW=nXAKsvr9cjWqh9>H)y8Zl^RIvFaOm){{>0Kt^9F0? z08vTu16}bQ1lZ_R`@NU2=R4~A+1vZeriCv+eTbdot~MMT@uIb4V*rr1L>@K^++Ng_ zSey*dp+LubK{-Y2gL4l|P{h1j@%#zq9Sgt$z(JY)KAv+k_t*lrEsx}J&@Wgc9(Y`> zi0=_i(Y<4pi#3h^+9(Na-T65J&kc3y7Tc8tGY%TBpU(UY9@?XA=he9yq$HB|#p`ro zat=}HlsfKX0li| zASDs#OFRFgHCnpyaG^E&BOkO_%`3;^gTRlz?^Lz1E2(0085A%9peUw~um9y$VJWjF zOQ6>e|1RMGolX=$I}j8dbs(ZQ|(cW<<7adf)Lph7thD93P3;P+fk!*?2~ zkzb&1Eo#`Br}a(0B~*Ghx;|oyjUN0g?jh2l-9dH)4`8&B0XS}SV8PybmX1@ols6;s z#dEDpg%%xd+(%nXKrm+|pG|?pS@@Y0$_GS8rM-y>Q$feCy1GC#|EBt=Cwki~b>Ptn zxGK;oP8+i!lIxEtTECi!kT=gG(82KQ0cnoG2RQV z9|E{H&R}{*o({{zJWCS87vUEGg@HO&LEt3K!t)=kF@SmSyN9EYvMx}-2iDRuh(QxS zbO(rr-kVwx0nz1mq$pGxw(F3WY1B&|10X}`iPty9xJmoowd!y0Bli&~sY0gudtl>< zVqBbegH)kETz~XLxr*dd#eN?)=bGH%+L#_5ZyQrL6-6G#p4jh_8?#Ebw=XJn0c!gw z%lN)iQ*D%+vhi0?B|8<3w<9zBnwP=R`KE~W2$z6L@8e6{!lIHAiKk)S{(W@~t-n$n z86ayr7#C~&K;&drxR`~(oCMoDgOW3Zoq{L5XD79fu&0_&0&YxWJlDCO zI(vm*^aO`^)q~{(#q+@&?J;u3mVEI0?U7_k1v;%a6B_Vt*@i#E%X$N>SWOwc@&ngu zSy)rR2nc0$!AiKkiR*lF(jZ=+_@P?8YF(5-z@|a^mwccXmBYZ7L#H`rJy(2_OyA^H z67!GMGz*c~`_d|xkH@<3t8&yyrB&)k$yoU9Lab^;YK^M=1L@jJ(LiXt z5Mh=F&LrZTZ+P&tlWEl5kRYtlN^+7?*Q8RA|E2LN)+Fmh*~Od+5Z4@+g?7evJipNK zk#^GdMt)%&96{=KC5QoLnlw3hqQtox%YgC`>}t2`I>-ND!>QVX+y;_s`-M8vPq@rl z(Jn$&Rq#?8AcIocjNa7k1TF?dE?*;m$}2V2#SGo+2~Q6HaJ(dOfRz8sw^C1Eajvk; z?5)e-@R)SybmMalOcUFpU%9k;NKljLAwn2z1EuMHRz>^8ZYB55v?B^nJGwC8!|&$J zQ6?|dGg}2B1IqBUK$U>S1%pkMQwm>Vp9f@%9KL|@>VZ%3+?V%Yn(gyUd)I&XbAVbyT94zMs$CB3-kY<;;e#J)Bhfli zeXBJ&2fFn}W5@_dJW;Ih8Y}+ES|gHuzDt8%mygz3&I5JG61p_asjKLuTiU!gfSURYd@c~CIia4Kp59vSGe`e4ra`i zMW@F|@gJ_;)?;_S*%GlSL>_eBz80ywLQ5&J85v^K|G2~8h^1+V0S4HP-w(bYWVcn# zZ*{fpweXp`e^u_mPHY`nssaVZ=w}pl!>a&$>JG%*IL0f!vCM8;)I)OcXAN^YsWT4+ zLwi#U#P1eO_=Fh6PF#;|Vk@2caJg!zSwK^Vqhao)ML;`S(WTmTV`Z7OmUSflOOjKd zEXkKaXTC#NbgzZs10_y$Pm~2I{?La0$dn|vw64D=Qy7^C4#e+EFT0>{QV?I{!!G405@8Hf_>)kgiW7!c)H^~$oQgo9dM#gJsno?H6040L6z?SKuop|R`*vaMcB9d+qo7PwayrtXC^pa=Y`5|oJUE4N(%OFNeMLKOgK--o_0`t_9$Ak$KnM=gU zs{1}t`QPeXrUqIo)+l>o-eb^-@?aOw|N6nzFS|2{yaj7OV{xLC{ywjvJG8evF1cx- z-;lAfrVfkkyb0}!p~x|gz8W9si%R>=Jlk&DuBg+4Y3kO^(dT4LKiShTm=f~fa$KNr z(rPk0SSq&EasH8{%b{?eZkBS(cy|%W)43zI%O6Rpfd?P#4?TDPw)-o1sD}X{p#cmL zfNQxTDR1{Z`b@HaIG78>%c25%kn>ffdOpgh24~9O|^YyS|4ZJbR5HU8m*mS$sZ6t#j!52mjI$3|mf;c+2FmJlT>15Jd z;HkcF*pN)DtqV6|_0VO&#fJz!V8y+(bj`p2&6UYFsprTLC33Wp2M}xf+ag2kUSZap z%_fGfOKl3d81xJ3GDowH+7AweMZLD3!G*$BfZOQ%q+h=PmPfr|R<;8oC9S-`U`?9k zAXVbhzm!#xP&I|vQujL;+WK{iOIKD*9qmEtST9Kqu|MKgCSz@5qExATH7I&`EpsPeBvL3npY9cr;?Ef(E!|h6HLATMjHmM6 zPTX*VmnjnfN7wiS*AKS2P(qdASI<0Hni1?q?7-a^cV8UXCVc3;kVL)TF*r}LxuEw! zbYau>npn}V2_OQ!8Y>prT+5eDptZeG4`I{D?ZD43dB{ybN$WM z^`3!fy$~5dAa;)BJAXf$t9a^Lul)kpr&O_Y6gz+mnTa82Eh>tYqKG{id;?alb;Ys# zf!%FNj!X=ylN9(bg>(5-`%SmfU`pw_cUmdPoQjHuAUhebxQ&i=lm*7s22&sBW$WL* zz5wwhuTv|Ezc@D>nbf9A1bS+C1uh)zO~5d$T0hEO3ik!>Yzo(~3tU5&Ki!_8K<;hE zAp#SSvkxcV=}<8Sdc|2gb=!!tBPNfroZ@zJ*sOLnIqq;Yd5pBNmUsON*=<39sSRQRGWJnd{#ONy3h0S$J1qT zUFlOc#{;)LXV?1`$woRXx^&w8O3$5d`NheQZs57hVEjlX>2_JRgoKZrf@4waqqf}-gS zmV(%aC9<6Z4rL0XPR-ouLkt{Vrf3mG)QId$a@^Lw0VbMx`>uhzZa%EIqHI&^WeQH= zV{B}3nKQ&9R(~G5Ft{gHIaPGGb}#0rQ0-#+<6Z!;^iyl_X4BEu{i)snVzeW`$IDfg zzu+1Mn8^c%u(xk>EZr{Y3gW;&dPoyDZGlaZ(j*SoQg{bExk=_g{hr_Sg2v$5Mrh1< zjZ*sD;S_#Jf4x`0-n1DXEOX!6vsc49WJTM6l!$XI?-E&!%r?yn`m`q;owGD@Hn
9!&-X^mt3stYkH7r&(4>zlz%=9si@^RO2-f<%S~@%VXn{UARz*m30iQmIe$rCbmN|1@|unCO9_aP zhyN=Q4VbPs)?s-gL;A`TsK!^-24LBf;&CR%-mN2=&?_14i$>T%CY7A!naQpN$UqZ1 z{J$0)!$Y_qz?BEWQnW330ZXHbw>8nSA7P+((N19kJ*wwEer3Q9iP;&5Z-0HD#El4e$3SV41#1zSx`&uAvjC3EUGlF0!@z+X34MJrw&YBkH0nAXi4qt*X_c{nOt3bHS*o(#JCmu zVJm_E$)o80X?7@+%Emwh$x&u$f-8K7c|-e168=siMvBW=GB z?kv=Qko}R?w6()fpn8o$#c0?!=q-=Zyhv!e5=7F2Y4PXn6Ib8H4BgEHe^w<582nga_{)Y>1RBMfCZ7>TgtKVlU#LCz>Gk1gqK7cLv_7j|uXgFGgs(UVPQ zkc4Yt;1tjzUgvf%vy03drR1HK_U0e=`!YeI_eBuk8v^UzeaVzvBVV_x<`z4?g93pT z@b%^I4_I4vic=c=Y?*c9fK^`hV#N%WC1RP!@WCPTJ`pxuryR={Q`dcWgN}_oK_=g< zTx>mr-<)t!)JyM3WyQWu5~)#?`7a8xTcUjmjAa1C12^wY%ePt@H2qr4qq<(Jm#H|* z0UXYOeG4xy)Gxme_g{pBa3bbrJmNRwDd#83C^kN2Az2abwRl%c2~==j#14 z)wf#B<3T-0cfK&NoN$D4W5lt$8P2ayN8|v*KEUr@5V0a5D07(iOF)Vt^@(!!@J57Y zB*=GZYulgi5Fxo{-aaVsERVCfMfUXnNX|lyUI6j3@G-^=*wPk&zZrwiCNE*DYE487 z+w|Tqav}{bz!Pi%Rj^4eIQ zC277*Ify&T7AL>DvR=X5nI?XR2Clwn@YwCvEU9mKKc<*&{((|HwnM2z%N9ItQ1E($ z@Bm^74hL9Tjkg&Q`1O4324%&L_UXm>VEbB$NYZir=j={50U3Yto@>q{DXMH6x=a{OjGh=I=)!;8I^e-*>pI zCoeCzos%~|klMU%n%j7Lg9sSoSf}j#)^&c=W#O=SOZsxi6a+v$x+B6nlMkr8y?>EtqiD@@$VB{w95*Wb(@-`t=v9J04ek69?)f4(g zyu9h{T-31^lo%+)N5~7(uXdpk+cq>=Z8)SRM%(&0c`}-7cz#hz9i_SJEx9XhqGf|Q z@y=17*3XGPe!cB%yj56b$%^?c+^WFM$o8{1q|5oaJZlG{eoW;AI+-8v$cPZY3@ zlz9FoZ=7b-YQ~j45~68?(X{R`C>yMD6gT$CPsAF`DoK9>OHX-Cb{!~IxuWJlpvQ1o zJ(`*PeW6*l`kQ4|K;t(s&*ct=|G68;P!i zPT_4zpAavPu3G$}rix5@rjdkFbgfnW*;`-|Ul5O_Cv4o~%=b4#&Z}J3eYvu$$fUnf0)|wk}2O zGNONce~BZep5)Dfr}pYQUM)-I&e|oOJ~;GWnHCQF)lI7VrEdn@98 zLMH)?>#fj9`|pIJG;tu}F3Pck(rm7y0**Ck1|V_=_Ee;WEQ4R82u|KT0+yT;ygPU>HT#|JpBXW?5}psR%#J;e8;${J zR>wx&v)(WAi4W~Vb)q9&{waDf#@r{Iz_(VYYf`E3ATzlwoDOBO1H`uTb>$a{2gI2R*jOr|rO=G&jEC|J zm?sU;CxGmm4bW;Ays|srPNW5Bp8pfUnB)2+ybg}~(i=R`M`WWsxZ}B6-lZk7^A+n6 zVAJ7w`wMADM0h>o*_PjZCCiUfsnS$dlQD@Vq$Upj;&K|amMi7b0b7zcJey`2GxbgD z`PbPCu84c15=|1swhSY)jxnc@I5b;|Te91ygDcWJ6Vlr7H!9NU>&_Mul*xAd781OIZ_3sUU0FG|fM+LF2SqWbxtm@Ach%`Z&5>M1ekSozIBk z)o&QWCvk-?OPUo(ZkIdVCU&?SfUYt2>*yYBrHg)3y1pqLV9NqbfN>Ihi&<|*>}w

oas;#_!8>*m>#TV@{C zbj={gwQw_%dNN5#_~Nr+u44KdTCWxKAAK0!{sV!in{O+NlXXg2umU$$RNLI!od< znIj2Nx_)-o_T3{1_FiUhh0K%GWgOf#aR;N<)xHiQb?_6#gT5d4?Y+Vhv%e`Z#))lJ z`S&fB9d$atMQK$I-Vmj{1H$_&xTn_tG`pL*IVsJAnERrhFkWq#PNk*c;W{ka__x+T z99eG?eHLt&%+kDbg^TRXJc-OaF8!uN66Dp38j?J-p87b$D(bP$%;SH+2|r(V(igd; z&?tIsHA!LM_YbUE}7B zxh><}KwTEcw^rUIvR&S#VGXqrAtD?sQ&6M3XfzW@#1gP##KQrLww_El#px{H`!U>V z@@?voY{&nh;iLBYSaBm`(-%_)8fC)z5`NT42s1C7U~%2975htu3Sft(fc-6+kg;-U zhxku6gNMMvNwnWqX|z zf$UptrUd>q`z(>IPp7RZ}hcZtx7zSn!13|iZ|J7QlkF)+9ar~}>Xcl$d^3(VZneH(Vl z*_$FZHyH~oV^R5Sd~9HU!Eo79w9nan8M6Bd_hqQgqcSwd8Os-OpoXV;ltayrPS|oT ztM6XFu)$1M*L99s59TLrX#YRvzB($(EqdEPQc93+knS!CDe0DwPATb*QMwU9atP^0 z1SE%+kglOShVF(LzSn#2?|OgVTHpWQEZ4Bm@qN$iv(Mhoe)e`WP{u|ge z_ts10CKsxM6Pz4pt^T!p7`aET#XfB^J|baJ*0-KBQE4mM%Cc?=5;QkgMLZ4xL{CTC+Pem^i@@B zHCp1(&6khWMt1MKt^Ex)7wgx+XGTCJ{qoAf65!^JJB?kddh;BW2vW})3`SZ8$Xt+< z-g~rt350N^P<4e0@efCwB#I7y^FEOAvD*%Nl$d&{0DvLMc8-)oeoC z^>eC^!@U2a1z7%6Ek@=JJ&d-7E(L`!m$@%*eq6EQpl~0%Ux%?~3N6ZWpmwD9RHdFL zFFp>nGfzxC|EA_HBZB9|QckgwCoaC8mpuBRqd(}+_j~3H{vd?7DypW4MoTf$T@4!3 zQ%%)7%s3w+{%+uE$xK@z*o*P8=h%HA=~0vK(SFjY$_)!8<*&Tq>ah$C!a5F? zxbNa$I+`@Z^xK5ww#>lQy09jfP*~0l$X{}Jbnx)d_c)-YcP|JEGu>rren;Ui^7Ka~ ztt!gU%}>|ltMJx*A)$U4=$>+>af4@9DtWA*8e?buTrY{ebvUdQ?g_r{8%{Mc=me)6)aYsNg| zth1jaW*Cq+^b!-#?!6mhHN_K5;G)DHW0&{> z9?x4_v0Iy9|74v_HF~(8c!H7i%~a6GY2h?tV<_^QwV7iYT~w*WpO)qBjOCn?66R)U zJjtNIM;CQa$lCePrQ1c_=q%@`{`$nmWy9Tyu}h27So(;>Ohp6$Sc0i#GtNoyh)5E6 zz_E9znUL}hJbOs2A*+DGs`jeF&>0qC~)2XCa z2vbHG|7A`Sg)5W=>cpr8^6m4$i?yZ`&KdapCe*fksLwTdx$-_~bAmPN z9)ADxW?{GMi?Cwy;+l0U{Blvg^;aiZGl zGh#)pBwg!(aJ5raFSa9~%n7ax#dQU`jaQkTV_7ZB=ChHb!YZn75pJ|}wk93{ES7;M`sbN4tL zZ+}wuN7j)=A|D3ta`W1A<$NRS<2@NMz$UR5i|nQb`CNRRXRGo`Cf||}j2Ryj5TieD zXzjcNBi+?|R2Wb@;)d0G4xMn%jypZpZ^LqzhPrCgDr1fX?lUhC6s88bLVTTFdyn*4 z?BDx0af-!N+26?NiA#DJ{l~6omR69*iEp%a*>bA+9$a|xJK+!A1o(^|Srd zmpkr5_k-L&VDJwW+^dTDAdiw@m-2_` zBuC@|XnRysY)f7eCjja0_^sOnCOjZhF#*gkvp6N;^K5wiDeq5Bf7m5<9C`43F+I0jSqfun{?tN%*X$GRDmHKBH5W zcVx2P+*@!*74v*(L+As2$%J#uuSlJljE)OCW3pPYo;s8k!{f^83dXdITj7^IwR#$~ zQeGs|MTCD>6v0!`xf$o~WZ#+~4;c@P;DNX4{qy#PDO4YB=z!rOR#8Ch9lG?8@G}4< ziiMb1`j5)trNoo@a^#5z+>Q}m;p}h?`Ro z`UTmVXY73(@BZf(4mJGJU(i5z%$PD|OP$zD9ot4VJTHcPdf*8^c=Rg!B|Qo4cxoaz zO+z@#vXvQ?BaaOJQ~KD%wrK2GLt#M!H`^gE#P0e*Qg$u`<@j~2eL~YMssh+xi+ zrL59G%C0lQ^^lXxH&CHA6zfb)w429A3i#3wrBr+{>DA7J(L{Dj?aSG!Ykkr=BZa&X zFX>?_&wf6y!MZtT=@#0#4G)aprqt4bNjk*2Vp+4@&b^(7Ts&F0pg4!};_*ZcfCXh< zpku&ViHL)(qyD70G?!_;rS-0vent~VJ52LO&}2t7M`afvLOB5??l-RswzLG-MR~`l zhF-oRD%9aXIj(N8U-n=r7RoQC4jt_W^rfBw%QwlOHY95Gd8!J&@Kw~@j_-EMYU&C= z#fUF;Y%}!r$XUP}YH1Cp=ok0v%&;?7E9NWavnsAFK+8cbzq6yzRr3fBBVX&R66ztw z{sk+m-P^Z!TH76-S!f#$KV+T!BkiLp^$c40A`?%O!|RsH$jPDHFa=RsBxpPktGE~Fn1%LA$h zag8B01}Wfl53m`U*sjpUyurw44%wm~(viG!W|Ex{408!l%kr2uYU(^WudF}b5ZBB~ z**wd~PI)UFS30E@tm~;;1X|_uYKsnCa|Y6TdQ~f=eb7~cq2P?NJF@?dtduH z%r>RlSF$Bo6U$dV)~$I+z8uv)JGxiW*>f`OM9znZBW(~5?;=iLup<~(!47m6ifc;tOg55HW1XZ-8*y&?Zpd61Uezydw%XESba&y-a~Gt~ z0-j`ijvK-yNrZ<40Aka?wmQ|5pRc`C{=`Yg2bP;9tv@E-m2*jcsL*!Cvfl)rgq?Wu zb2?!C{5g`OVB|*$%` zH2XGK9v)jBvlcdi)dcC5Zgak5d+%|jWAom&TgeQ9tIX@p@XiP|@77}ZM=5c$`8;~C zCnQ-MZF@BoVJ3v>mW=s8g2=B^^&0x`2-mQDVQm|h?YfZ|T9fxWI7ll!t5w{Yh1!@BLKkAgWr$@vv)&A{bG-3QjOA?lD+~^e}H}LD$m!njK{>-9r9V<($>p zqQimFZPLOZQwL+W6_gY+^Lv8BT|MQYT-){imp8hw-tp*35%H15Ihx^TWwl{${*qqM z{|tx9pL$PiBIam$J~V= z7#Fd_Gw+^rlfl$sCsidh=lgd4ZjEXyT;Mj)Y8PWZ_=z$nSOQDBDhW8?qbmJ2OVMghox8Ww9 zSslQu?B(~8;u66q)$u93t`e;RRLaL71iuIy$P`tD@F^2@>!! z^MtisD9TmnnIEmF>c5E?pc5#HPNqgrI=+S8W%nZr6OiV=KD-HodDdvEGx5>5iC5@? z3<$0|c{(V?ey*(c%BgCKiQP4|qN+O>V|V|s%!gVW?*XjpiDtnjK}0Fv32Tqu?U;-b zT1R>)i?Bj1RN8r9ag#)yEP>w8ga1Gis206f$cGHCiDS@=mH7MN$G%!I5!O+TN{JU4 zXnJX`yG`O%esGqHA8$}XO5?}F{!2t6t>|f5WV5)RJ@Mae_Qa<>=mDS8-ghRHK9YO4 zdoYqA7lrSouLF0k6l+{F0gi$Mi@dQQ2SLmj#lC{75%em(I)OcueO!#oWZESItYE9{ zj^~@n1jSHNi{hWNh=JQ!-5FA(6>sj@*jwfb;CZm!jLh1dP}?YY1|L-CJy3)@*LLsq zxq=(d*b{gXg1@A&y6V2Y@E6arzgo%W&2QI}OB4S5K3(56@ZuoJF&VrQ*A2vGFN!~_ zE$|2|sIQ_HWLB`DjBT5qG`lWgLNTzf*N5j{Y|cpOMHdPqmRX4I=Y1TxMJfBlol;S= z#f7R@bQ*O()G`}!=#mN0PnRZHMs><*XT8-=?bv2Ty}%!jPI4b;HyT7K7b!~^lxsSw zjDf*lq3=EyRN(ArS8>F+2cr#V$dM0Fc`sMehy8GjzZRQ)QPB3$J(lW&t#bSe2j*I2 zQ~cZ-$@EVO_v!~E<}T|d{jTTmI=X}cuW&o-SR23R*1h&*REZULlm3f=v^rne+J2y z?^Q!9E>VMQ4TDwQv91@_C6+@g-voE>xXbrBru6&U)IrVP3nSwnSjM+^9%`J2+|*3v zLRzJ)LCHW{L%uSs8aVtgKJC#4Ok@v67(hm`Ab@UIJut{EHUD1IkuNFt0P?V{hB4>v#y7H4hyony_Zqn* zX^B`U3G67xI}=F2-bUdtZ!tQ$Ca%&O_0$Gi;K`w{eBQvdcY8W@VKOJ8gT1bxYHy|q z@q9CnfqqETqM3sV6ck6yKU~Gm5gAY-Rjo$bs#pgf#icKCdM^y?{T^&os61wu;MTOL zT&a9tP$Ov*Gj_ARSb1bd^lZ1;$O<(##nwD9V&+L8Cw+;Q`Y#i ztfsI4x7;B9i&NF3PDyqa4DuGwOc*8qp%E);SMGUaooZYWmrKmr)Ec{?gw9Z-;d3-2 z7I*|B(@l@JMDxyEDU;cK;6M||NfUlIi1d|kKQ04yOfA+C7qs*PJ)hRL+{P+-$a!-= z>ub=5zJ>(pNo0K#!9q0!o~^KWhbwcc5I)V;D9p~Vi6>XnSEI%0O-zk1=aT}i_ zv-bDvD>7~&%g9E_Dup5tx#8y_L6rF;^i+{%z&f9_O0b2<0|b8bsvOR4NHRu9ZML#B zgs=0i7=sa0!LEQ%RkN5+O_&im@d}q6>Oi9hu>;kEw zR?7}Eu&85w9xbO{AB^|aZCgCc-`p=VNSI(2pl-&&{xItS$~`qZ6&|6ctq`BxJaE~O zf!h6TVS^jPKa9cewD_Gjf81t#an2NE7nCum{R4XF{H;m(^V1xiKq|VY5Q(xH3OZ!` zD$b$VSG*kGr*A`yh!fVZm(=A*X5(ICuyaFDR{g4)ze~W@vMH2DIF_QwnTMu^f7a!u zo*J36k)3s$1b%2kN}*~a3VM^a7ChBbq$x*vZCl;X)e zk7hD+l4kXLp@v>oy>9a=$wS`f_CePd0!b|<;kJR13XYR7sykiA{K)ZN*{|zs^>80f zQ7a2GI)sO^nmi*1lXxnvT(uC=_6p>Ybk{A)Lqd^gZmGjpy(XjBgc zup%@(jEQvM#xm_n94fcGfw|9?P%e=Hkxfzdb#_U5KBF;f9i@yWyYDw)xm7YEv)C#hC=dqV02Sq`evp2(O!;We8^>olb|JZ9 zj0QJ7m32ohD?B@VrUox6-q>t0wTZZAuv|Jgk79K62WkXqrqyEQKU%ews9lt>X_GwI zEyT{CG~%_C3#5;<)3Vi8TLv@l^j?|OV~gv&4uu?D{WlXnRoBNudgyG>KAFDmEt~e@ zLkAKyL?xY9`I~n@v)@+eef=nkT(WGc_>yO)T@1gZIH|F?gdfG|V%XV0{B-J9m_N!# zW~H4*<6v#O818Os^yH3fDe{Yo%2CSJH~6n0%sIh8&*Et7SU}89*Bt06Mx(N5uL6w* z{TH!z!RlJlc$mYb__nfk*r*)TAyio+YS+D;Iq_&6uIr`_hh9xW0j*0d`n~&3&POXG ztQ?;^cXv^IVxo_k&1$ez#qWf-$?!Ar|7}D`;$%{nMC&=@4ne>1GXO{Z@k^^KRk}!g zKI{E%pJ%c=ZP0x{S*RP0ey(pF19Xws5oltrxJt`nngNF+Z^IuB-NI! zcs!59NbYsq;Mq(78hy7d8@Dy(>APCOHjn+5I1);FImlt`@fhkG%?T{Cs)pB0b7PPtD8}R{XYVv7b?d zCiAk#)sLxTzcZ8`NqOM#iesMA^}q7w-}vieg1qjtFFM7Kv2-Ix`8VFu7l`sJx3_Nhf+*K1jSaT?he!C^{f z#pEaUroHU}N`k8Kq=(7*q4V3=gxZ<&U%l29DC-g#yvoBRE9uvU_EE$+M2%xz##qWZ zKfc)7`~*FgH)It*=44s=YbQdebwX88r2B;ikDBE-xGqQutp}z$BQkwrCzN z0KG(X@F~qpQCHGp4NKN&4yw1^UBGjY;s|$o3j8JRd|S;;Ub}&QJZh#wt$YlW&;9ZV zPtzGJYzXbDb4FYxaI{A0S!dbcN5o<{xQfQ&Mj;MD6rC5$Wz|RAG(b%zimXr^Kz_1< z!BQ?uy79db%5<+2qqFnuZxU}2RbfS+xHBAGY~27<(0__Su79z{0mFUeAe_Z|q4Cw6f_mZMuHb z3J+1+HW>Sfq(U?K&){^CSFev{6tiGsl-H$2B-cF4#NDR=NwJyzfpvRKXxhqmjiR*9 z@tgA@mPq>LYhi~PE`a-`-&EoBU44pCI6R(~$q?P#xi}NXZf0-u;Na{2UJ#PnwwIqF zlW+y)^~5V?;~qy1ER9O>F0?(@>7uyL_&9ZqBiFSY?4NpE&mG z{%XK>q=%5|iWpf@BQFRQ;0hh{x|RA z3tpBDwtqx>)KUK{!qxF~u@XTz1i9teRLT5G60(j}_R>g{5;X5UW#A0K8Y<2q=>7Cq z9}_Z!857DOeKX3hXy%srq9+48^E^BhvFR?i;8~B$kWcEfaD{L?a|?oH9R^Uth6V6| zIJSbz1bI!Mc);y4MO%wPS0Ocg-fIy0LM|6oTe`^1%#d0``>A^kdem=P>i3@x#n;p> zkYaL?*}Oy_310@O^{gi)PTa9> zpr`(2LAaQ4*WRVaB64oa(Zn3vgfA&0XWLX4J8GfqeNM-d4*IFbq%E0opZyL;!;wbVW*UrTWQ3gw^;yltb)#c+Y+j z2A#^nm3dB}Vq72u34W!CUOdE=C}b?S;Sk&TwU!-yJg_#2Ro)p&c*6g&9ZAe%Ku9JN zW+5M1jb$F4)qQ0-Js)-VI7|R;srYrI2R8B=c^?DgQd|cEdgJhhfRPd`j1_gh5gyj= z=gzhC^%rwRN|^b(;Nab;a|NcvUgu&)X)W9a8Ag(Rh<$Cm~P*_m@j?4^_*=V z;_w5Sq}Svc6SM$)lonO0=8dihv)8O>q3gH)i&?|ZMq!*4W`PgN$pN|x0p$l8za@M- zNU0M0&t&+JZnEV*M z{Ba_;KQ+jt3gm=4`H?n&)A(K()4$x3x4Q^`lRQcEba%N-rr70W#&Qln8fJV?RV~}V^es`hzBs}&Z`|w1J=n6M+wF*J7Y~s z!^4`6&}?EJSF?`|`PKFniKe}4E4fU2_e+hP4maz76(u>lt5L2B@b%n| z{%6VH<|C{G9t`O(IO$C-uf@IW#zJFU=Px{mnl}3dY080h=(Eo+U#C2~o0CWnaQ3Cz zISSGIp`xxb4_DU2?eZl^V>C=MXjEOtjIgcQgi%N68G|p|{Sx26&wIzfMz?LAR4)u` z*rB-^WnuifZ|nEu-mI<6-S?)UsdhP=weAHuJ^~iutE^Qzo{R7oxiDe-^}71ZJDw>Y z(DIuTq4XJYjYeH=e5{y`Y-dJp`_bLXt!W3JYxam;6SpT}=E8zf+N$^sUVq1z-|t60 z%TbLaISOBjf8rVV-J91y!Gf0*;~0usyY5Oj50K3t{bIXp-u)jfz?~CbFp_#O)>HjB z9i(%C!*G@9IV0H6q08@!rd6LH#^`N;{?l!cuY(NFF6{2TpJ!0y0m>q~{;8RI z&xFBS&#k?ElX6SXQ!)CdqqAzo6X7i9v>H0nRQ_bhwLauFb;`bu36OI4A8!r;R|QO& zT~nZ8HwI3#@;OF|i6G8Vz%$Q5L7yMeJK@cp5;F1hO@Qw!&x*`ty6u`n+ zYF{~d*3W?^S+uMs{ccsv^8U zfxp2qGLFS3C7+)E>i*i_EKuR4Re%kJT1vJIj+W$;ya?K1ZjT=x^&l9s2zIA-RI z3Wa!7?R+x(u3zZ$VCRm$8Yjp}JH&s-eUXeUX5>+dSdO}^1#1l@cS zXps#91%C>a+4&G35Upnx^j|18rEo3}%pLN8ZfH!2syr@*Z{L>Mm(802AxraA(GB~; z`Ax+@7CuMOZBc63lqGO{(Z;UvDLkuLt+lrCRnl3wojbVt_h6Q0&dhw$p6Qe#Nxml1 z9u;Op@42}mv*z*1plBk!)N645GPmU4UdsU*J~?-sZUm(( zsCGW&uWPx*TY~p9lo{No!55TISYJe!3Z{?G%&(a^rt``JeGOZP43R7#RGtl%AsaTm zk#Xr)$yj*`Z`TUKeN+KnS4u1oJ(E@wd+b z@;g|l+8?@?GfsaLd2-VomAVvZGC`%ZC#!USocPWUwpemqM;4^fp)b*lJ$Kor2AW8} zMX{OGJt8w3B85P2H2Guo;?fx1rA|*{ee92<^2z5dR|}w(reVa#^4caqI97kPSXmE5 zZW|S)acpwu$JXw|AWed&?t)7Ann23fIcf5{5{F|JaHuZ|V#lECnngKfCV&n39tBnm>U_0=;m!a;i#L=+@dI)0H zm3N6DJTUntLm>Z5wcaEG{peqS$r}96hXJNFB_*?9_T9O4Dc`iHDabP+>qS^FU907B)a3rXDX z*ta48u>@eUj7gEH6hq<8fj*hYmi!j4ibQ7gnaw;vne8c22MOwYrbEJ*|GMec!X}xu zAK(ltYQtBNO|?2_qYv7V7Da?yD6E>SetErvpQ1vwDBVn3a><# zOhlUcWdqV*Hpxmz6wx@fTrL(%a=>24{*;{SOGN- z#;v#uT~$e3%E%5}ohRRY041mcT}9q{w#X3=-lZxwpZ2$IuHI6Pznkp)91I_5Te#Ti;C6M&9+ZxdS6Tf4GF$WX>%Ynv5QR3S%hqM7?OiOBBd7TF%)1VYXT!%bB~ z3#*l<3qghfSEq-%5co>~fOjZ-xz3Mb&4hFmM;(r)83C$(udKvFOh@G3Rgya6H~xI{ zI-zf*WYcVoKa>smMt_-Le?dJ?jAIlqqF)5bk1~=mIW+aPEYMXMx>yq{zrg!D>;dn= zI_WuXqzX&`t8ZUk;j~r?+Pnp(XL3$l%|Yp#+p*yyVy05)J|XVi-~gTsm(5Nj5iZ}l zRj5>;&vwGN7_3WJCR4=Th-4@7>9c(GcLumUxruQ^>P0P5jp8*ex&=p9SHqK^-J13e z?W-X_2HF?-HLDo_m+g;mO$JsNg}EQ~3RR|swQCTt7=b?FjL}jSOair)lgI+yqPwj!pQ&&OMr z=S6fB|ZpIepNkTerl3Ap^b?D}?D-XW)DyG`9 z&}`GHT{ymbqL(@5a5%cK^kUnMQ`p4JBYyiDZ-PX-#)S?777e0`wyB{{O*1yzMUXdU zJoc!AZjq6F$|t5rRaZ!W#ZndV_@0Pro-%MshW5MqWPoq$+*+l2Diw*J9v)7m;eow( z4a=0H{5wBj&CJa9Y@od@^=!B}hs|GI@XX%MNl%h!Vicx;#bt2bR0&1zGm52kThT2% z{w-9|Z5e{@aNo|6PLQwxPdK!R%kynRTnGceq8`8^sd|FSL^|6Z7xD?gtcpOA{8m>h z>+2X+uOUY|I2KYD&N6QC#${(o`gbL0UZu;;x;%B<7u#UHf%5B^#PX7ye4^ll9_01wfG3DnNwfwTc1Fgp4&Pu4amM*j0(>YA_{; zpSrf2j$W-xTV{6myF19dsII^u>LEI6PKrW#ApfHD%;J){gJo*Q`E5;|fSz$f`~Km5 z7f|ycIS=ebR1ai{4NuIYDXCEpNBKL}>%6`u@7xr!sZ{iJOb=R6Wj{dKynz8Uax_Y+>r&2<|q0#(ksny!OxWKxzoy@q49(z;gbjT-@_K{jsC4!(kF z>!))1X20K=f%a(@f-mhs`6K+a+!lhTUDu;~qEq76P4Ss`WWC2P??__Wv$+6S!auYU zKs3-KV%=29MU}baKZgo`XSMX-z%qu9-}hsK?#Sy}Z}Ie!I6*r!r-d9jHCDd+Vj`q0 zKl!SFB8&@=(EXasG*x7?8x(dDv&N{RC>jksHCNnoie|6< z-DAvad_$EaH+f$}AdzA|#QUm@s&FoINE46hz)K%FBKcvE2WaVL77D`ZzG{RVtp3`N zOk*5FI+VRl?(NFe^IU2g>=;(U7J6wM}X+7U`jQRHRaAa|bx=bro z#5)s1{5mE7G&wcxt;8--dd$9g1=;r35&E{22l+sweQ3VVwfVf-_~^j+4qOjiKftbQ zxZ$`TfMwiE0Iqwp{uF?Q{V1*1ifIYeWtH|$=ck|oNYh~ZBA_Ddx3A<#pwEyb8=tmj zYexLGcu)2h>M=f=$+jY*@>q86HC0|I&rnJZUqlp|y)!ySc!bs$iI8{sQ9}VXZvQ%W zuujUW5ANQwFq62LFLU!(JDTtPPKle+FVR@EPegO8Tl)cMG7Xf-yxTy7)vj}5ULGi% z05b2o%*mmRBB}fKQ4IqX!CkwjS1N(+HI|PwBA8)=684_Z&xK3?N3!kydK}4V z>s57Mb#&ew{q~Ot=7-%W?Kv5w+c)YVXn24M&D6{)9+1ufI%-4PLHX!ZD{F_S`dMH{ zqo$n_Cm-l@K87O?U#m2v_UFMS7LW-R%C!ML((Tgoo42s=o>X*(BmVEKNVL%)g{Lu@ zFfqs*Z<`%4xelOfU>ne|d91igd+YU>I50sz9P73+)$w=u^K z?XXw(g=O@FB$hBnjh5unKgMqysHXf?Lr6g`pazS&Mb}wxV{vE>S`@NRGCj5bhMRFZ zu74OgT#cU&12)Sk>Ee56`>O}k7=3(Y6Q+SpsHUKqkH)9(YZp*Ey57~m66_p<2de;f zhzvk?$bXl5x($+PH4&xm_6NFvk|m~3=wzTrklg448yOr6FIbC&ZFLrD2zfnKb{7Xv z8W3}Ie5dOpsJQ4=UbVX_Kzf`1N$v0h3PYc-r`6<)X)>hS7N6=A6J$1NQ z2jhWf2OR`UUK}08*d6-O{ZkK@Dwa(PWE4fai6Wx%gxiqF@WAYt?eL_uo;ZmzR=2w^2zTUoZW;tp z6i~EmF_~-WXOY(R6ksOFz_p=PjO8QU_~d-CyxY#-s$~HABau!KKZBpOGLFfkkWrK9 z4*lNhm3(pvTCS6#Q}R~}Ll03zcBe&YOP~>Ud}#@6^wtbc?6mtCfX^#xG4mx=`v0y! z8$5TJ2e*aTn=e_c2;c@ip6A>ieV8vbx-Xu}64eKYV%O&c{lbztpXPEl!km<3T^byD z)+Ko_`@8i=7W4|QD5W6NlF2XmF#v|==mwkC*JT`ANO#-Wg5LWD?SNR5$U>|%IKG>( zf5$PhViuGsDv`rp`7c-wmo$iw+6Hu0A$rm8RE9oT_gzgq{z`GZbe*Bc4SHHdVr(VzT|DQCg?lc*O3VbQ8aC`r5!@8BFM(tB`S> zj{)Rb)6=JTNL{s1lk;_9_^}hZHMu@1b1hJgBE>-Q-`W8r!TYLOpX7`fK7gPz4P@pY z2Dlk3JBL*GzWaApQ3-%ZZTT&%D~Pf@VmKb+)z7(chb3>4%s4k$oD?8QH57K&vNzr4Ey8`$== ze5OLvppj81FVFw6@xCrOCFR@Uc@&tCE4D$Ppl9WU-@--hI#fX(|3w9r#NLza9|l9I z;@?Zu9_SP|gs-1&Bg=n9*VcOy{>$&AZ;314Y%gYWJSzP6s~2P46w~4zOo@l;sH2fN$TLcGIJ{q*xt0r1 zUhCg5M8%3vEkBB1i|WS`$WAE)xjhV7?FsnyE|7A*vQ;He=%(>5o%PX?0&StapgEb$dW{=Me%X~&G{#`D25Ajyy z%`|Bb54qpzNJ#(nGD3$38Q4AT4kaQbDE|?khmee_^Fe{13sHK}Wb3Nc=zg0>>Ab~a zh*KMQif!*SZBN0fq+h0_KOl}R8|ZB;TG<_S=eFou#)YK*+TSG-IC`L}4$@v*DGoqE zet_seWA6vZO{p}u2E@7b{LO)@&jNpK6%)Y_*cx%Rt3`r2W)xMMuG4VZ>ACm5Vk{-) zs}^w~p#169-pl3_7sZw0dSi9e8Pq;a!|KUiSX1eE z&NrSAmv3S@7wZn2Y<3YdPwI1}w}{6r`ih^p&Sj~KpPX{nV`J-Xd3fFWKe}8MI_caO zzpFw@B!(#9Pejf4WD_68^x)vkq4SIqv!~t%Rhh&#rmlP3ydXW`N2%y@k|Dvme-y){ zS&OHH%%B&}?!@d9R(+hsNt&5!(c_d2Y>oNr)$E7dGXYl@Y^yRm^2Z8=uP9^tzdMw} zZjTdPTJfjyM0~9y#0ZF`=t;k2C1J+qwjZMewLYEbE)cwhR&KdB*)Ko_H#6)E5KH%M zgDhFx{6aylLcop_w>cCdyOfs=1I4F@RrP)mF7DCglj-w!7d#byHPhs{rvXmFR>loCzLCoIo+<;<^wiXU-WHSO?PSuOTc2yA(O#&_Xz9Z`=SNI#Y z5Xx1;hXJ?QeFLk+Lq<2BHlkYd`1xmy6-FriW8gDqiw*H1@h^vj!2L`CKj5!9jhp4z zhn@A_41A{qn;pqW9G;ks&ao=q7@IT7Zrp`m8XBRa@5oT(fG7wQr6*Os<9rEcG>7Dy z^%Sq`yxa8b^A99tWyR3N)8NPK4tlcL;R=piG~;UQwfD*_$r#&kqxtJ~GDeEY9ETF! z6Bk8!c8wx-@eT*n`b2zTl)rmJ(_~RIbLZ_Q3tn9`nezuuZ;m1QC(7s9P8eE$Mtq2sRXlWVXsslT? zAzo=z(JA1U^5t9+m}Y*Um6h3Pq{5;5KvBi5nzS;b`=q=%Q^#BOa)`s0z=7f<2@nZb zAqb45CBLj|%}Vu>iF(I}i$PTn)eXM#^Zz_sB?k8ws}A5K+e)Ce=qi5Ir!@Cj;mucw zsVaW*tClM|;%Las;dKMl#1_HpoyjHCD(&D$LfS2ey@*|~W{1^TDlG}=m6{(f6VNW` zkcK7cIb0>^84icS%+((k!^<7a+#GPOh#*$-~G65Fk- z_+D47GPe5+>EYxJ60pZm&!DAuZ%nUXGZ{6P=#X_|hpyi-;B~$jfZfS$%@bI`LV$$e zNtYljLP5KLK*f0lzBIPDWcgPKP>Gwovo2IWfR|hX1kD01CN6%y{_g!<`^u$)+B&JU zi<_t=exmd=7THCxXy;xZ&qPn*H%Wt<)3^*I7c6@m&GZzjfNN1lxBF*@ww(Ho`LW%J9&1kMs=rDS9pJ0e`b{cn zZbe!>#c`@!V$~7`#pi@|Pe@-SUA#No{~b&ZZXB>Ker&nV5xoV@&P@lK+amC5BbZiQ zi0vL5sI;bv-*mL;16yX^?|y(Uuz3i)c`)2H@Ry)x2h$$s)5Wt3+W&%v|PK+BUD zfQHif+_mFELC)`i-%XL7&{v_!~>?o&fc$P?c(zdPedP^ zhvz&q)dIwax}-r|oj>&<{e7nt?5e$|Nal}nG{{L4LotZ)UAOVRmxWvkL1lGG@q%AO z6BARJZ4FneM%@-Vd}jq7(;LkdY$}bDgqp+5AAUP$@<7Coyfci1U1iUiopu<{0Q!(B zoOzn6JIzdK*yQ%4Cgu{$+}vJ`2GuMY^XSY`b=E7{R4E*jCa}oQ?RMi!PPWmy91-ts zyUBLlo}9{iEQBS=o%=y{3LDfoY<_Er|L>Xhe!^G;s`WNzC9;1UR2$mKb=n#7@CfUK z%ukF+WFpJbqz#$E&L1SHqZwV@huzv-kBvlaB0BWEj?%G%kY}G7HS+dt{}I>iB_jdl z(9@ykQC$Qp!&KimHoGXVz4?QLu79qPP4n z90iGs4i(Gamy9uy#P^s)Z=>u7-Di7|!CUJNt3Nf1Tnt~n9WnZQrLS9_@5%KsX5R~O z!P~kngRGOCS&5EmqiWOQ;{wcIa)`x(LBxZ`H>XDnCZi@x*h_G#JW5s^gLxyQ?SMS{ z?Klg53S*?R=+LvUIQMl}0|=D-oPjuUpDXD5eecEdN=nV0+{6UbGTVRH0H~&WqAR%%GJY#t!7CDTiZy9Xb#PBZysFa~pSQ7+Uj5PqW*z9v{vrCd zvHS0w(iwyZ3^-#l{oAK$I7-FY zFV!SCDrs(;1^&TGzi+s%T>v;xgP?(=!N2!6i@r`BFxElPa9}+-|L6bz{xKYH1zeag zt(N|OE?Ow=(S3K!k5C0Q!xCCWw`?B`E+&Vm=l(O1f89Vx<3GO!@Ee_NUPraSPyCA1 zIsRPxgbVOaeotDj zYuZ( z{r_-VQOSaPYQXGqivJowlp1{-FmeF;zau|EV$1_3Y1ess>vtd-FyIf|%!vJ8tF1FJ26~QX zy)#G7I$r+L2bh-Z>%X&+`5||^>C%K%*N*eYRrTevOWQ{sec)c)d4E2_8CSb}Y@rwH zO3bdx)%`5^c^M{G_wsR?7vTDV|C-juSVoY4W4V6^qm|hv z(uV%yxvYq_=EufLH04Jjq+ld9=xBLP!uIMx&gsM|9O zkqI;`Gn?uvfTMvnY=eMotd|Bf$R2R=^T`1uRS}e>j1V_oCmslEVI+HG1j$Zfw${#0 z#W1|3NW`gFdC%XgP3Yk2c-TKqPS$EHEdIr|$Mj5nKTzzfje`)r6~>6*g+Y;Zo5+V_ zMq%ZXq;Nffk=dnis<2A zo&4>BxVZ7w8_u!DcR^`q$+yZkm13sWYgp{VH-yaTv~kI9bN1znWMXgZ9K!@a=Tie< zgKk|-q;uQ94EP&x@=Q`TGX521P6C-{k4@OFZuz!bNqDzU0|b|_5m6l7L{gdgZdhr+ zb-D&Eql7d_w@A5kcP}XoQcEn7 zBF)k)pmaBq3nC>g9m0a7#L`PF_1@_7d*APm`0hhJJXXB-ikWlHnQLaQ!mzMS{InUb zmJhF*+#sjtIW#7J@n}V_xgKOf*+J$uom(GcKf0b0$9Q9fL|eJB!H;c6&CYj}xVl_4 zeYk3_R&uN-?btbiSEd55{0DMza^!cH__@5Y;@*qcJI(#ZtPVH0=7O@QSXsGoH)8s0 z64;c0+Hj!@!#eI;`4Tm)hCy*ecTb!qxB4uB7u_h~*$dIa#{JLO^4F6EU&a1T^0`VT zXZM&_%wyU}To52E%N#etLUXmT+vtA=_7K=voI~(zh;K!NgJS)5Zv) z78M_8vO*h%)k+24r5m)r7|pk}i*P-Y>@gF@CL)N-k^qEyIKkt|UpQXr0xMQIm!3QW zaXuT$Z952qE@M_>8hs8~hE`?VkYs^e^pWz5bV^Mt-jh&Dg(uj$6sDLQ2gA&G>t}jR zJ(HVJX5UQg6ixpOmvT;h8W9oBCnShV2CP|JnAcBS4376WDh%KA!c_@n|JMrtINbjn zqAxc!_v-6H?(XBXqbz*uQ9zR~oLiz!g4M$JH)OEVe%Un~izB6;+)XHP&sGoNVgahP z0Pp4?_g9wQf~60Nv73zT=gh+DQpMG$RLL6ZAkVNKm99D8hHLJ`=o$dox&Yi+;hyIV zl4zB%@N%{sjrFY#8&7L)FNA+4gP0`K31dnt+Xs!g;4;{MSKHX zRv`1#*gDJDhh|Tg;z=S)q{=Xl5cHF%f*xZ3vCprq6}`ZIji$0-R2@Z74l7I415qw< zF_j|2t{BC}kvG-PpC44;5Y;C+A_YCq?o6gUFQu7R3-+l#HO~ZJ?_Z2$^h!=k%$ko3 z<^Q!mqO5W;5_mBRaEesW!q(&TBk1XR+d4*vrl!WF;Qq!r&&Gb{>?!d&^6Dc1Gh|t5 z;Po8`{@sgjJ8LXnT$6;s1TZ&+i=~`vq5#^5jp%t&;D{+U-yyOt7nEF9m+tlLw=HV- zV6EaREfv<^RRz1XZn&Rv2`LMcW21Qb@x+9?4MoSW6%48apx3~ zRVi1y&Q3)CDa$%=LE0^i;3K5+*B^mgm8#%e zgd=x}e=v8Ft3zbS7-=DI_lAeTH0{c9BG9(MunVZ$%Z#f#4mA^d?NDkn8KX3;#-eks z!%Jgi`t=&uW;+T%5t|$KldvnWzJV{n=5ZpL$#!xynn2(G)+pKyZ3_HYxP%?+ z(QHaWHjV&Vck5TT4`7yNY)~lI4IOBgJ=98!`r^m_xS451C17FyEv#uJbo~Rbrv(So zpldu^PO|0u_yldvFR*-iZ`ma4(*ZL#Ik?Dee4r;kS1z$kXI_dcbH3)M@a)QK0)o4@ zx)RgFR0`)of5FCk@{@k}(_`T4;{|e^+(>zAr5)ee7JI{|ZPl+Y8 z=T&gXScRFxx5>@!xLE&{@I9}{D$OnMcz1WzWMv!T7P98xQir#G^2cF-qkE@ zjLH)V>&XuTx7UO!xP9l%fv+UMnQ(7s1G5R~R3jMGVSU<7?hgDSUj~ocJ!M(_NLzW@ zE^ZHn4+T9wKUFzIJ^5=Mbb?Q0SP?d(2JwysHlrq2krEd`W+m!?X&YGpGAUAVUR&Z6 zy55m9#7sB|xg6hiu?*a4mxKln`DG4}WDOkR?z(0RT0wr(?_R7q0Uj$jvVHa0d2{-T z?WEJ>2>`(lF4Y9e5E&h*=u0cVYtzkV2rz)PDoFrX5#+FAjFZMd5MB`~{uBIZahs}N z??&wFW_D|BPYC1LUi7-D@BTQX%vuWu$>t>!u4mF#%IMPQe&?damFmlTh1O4$&1paF z6eL=H%-CHrUh-7lC6()b`%wY=JDPt{kIY+O%$T^J@+b;8VHZMIaZz$@W*_Ipo~R)KDB`&_zg*b1h10>2U7Gj-``Jhn}OtvwUp7_qO{TbQ8(s=ihq z0+U+HXVObaU+bI`1xE>+sj|r#uxa;Kg~WYgC;2Ctge@m2{;*b{O|0Rrz{d62^^U+s z#69E3-`>Lf&j#^7FuJE{efFL+2jH(8=f`*?YvTM~;N{TG>rd82#MTC`D zKtE;><0w2@%K04v#zkEkarm814P=&r&sDZ4FPC3Z350+A5g{dsxE~~n(53yFLBCxq zTusiFWmx%^$~F??*DL))kBz-BC5-X+9UzUE6Dn*!TCvdd zsXwguWQ6gunPwl;ECXyDw?dMjAznL}9!ULZZU-zpV}ZI;Wu-550Knw1fGmW-`7*va zo0_Yg!QZ+WK%}FatUKSDIk6-lI_Jhz<$N|aeh?(vB*Cwculw!C6WzCNDkaI4O*Q?A zA|-Qk(~CPpcsK|Sq<%3!Z8Zr@Dsv@wKEbg<4yi;7;?fIs5n}A8197KzfbV~gE!2F@ zk^E7CTaEd7^oz>abGQc(l`a_caEqB=6E9HVIKUh*TNN;cFW*m=4aXDE;DP|Bf%i*3h-3NqsG z-yiHE62795by!va6RQ*(khzg}_FwmLX zH|;kplD5CF3C?wp@J2!Lt?8Rcx7g6rfw=X|Inu`j>#ux=0jcGP;EL0j=X`Si~8w^#Z7?mle3S3(2&c(+^8Lp)%&skX&`N9Bh#63IN)U(-AviJW!iAt zK9DbTe6`kx+0(=0>|%|*ysV<8u3jZAe|(9M4m;Uv)FUPcapxdc!RA$W->pYPPuG|> z2cmRR3lj@F3BK^9%*8Q2n|J)aV;qJ?-Pzfx)m^ZyYDNoVUO8Jsi&OvBn>`fV$olinz$5S{2GirX%E!9le5sj9FwzK z@z*1M)cvNSW_T0r1l$m8r++TeAWDP$gUC^*Ss&=~hh6DFx$Q@-i+Y-~nzA;Z;kI+9 zm|ssx?R;9UGX==9093)x92<Z2uYb)JwLD$;BGg-9%L6xo(R}quMpEvai zg9YQoNvZq75o3Hw>pen3Jl%7e8R3u+S4x6=B%&6!vIXgtzwE1;Ml385y=8SqJN9+c zHjM9U5=!UW1?3Fc3GO9D!j0>DX4go_q{{R=W&<{gxLH<55B(2U9&mkK;BFf3eeqW# ztXC^vt0zdoJuUR!WF9+vho0a?GH_@W;-w#TDPlsvN|^Ik7Yz@1^Y3qoWnNuZdHgJG z!%6{+j82<{r&}T$>Nnzr3Q={gTxOo&x+*Q;R&qYShd%i9(&IIb!;yIv{ad%59m%i~ zPwmDgk7U&4ofbFVB|?4y`m%(T&r-8 z%h{t#l3F+FWRZB@oL>qv^G+_FHN6w*6u!qOhgL)tFmpzn^IPcs2P|losCUn-KizW> z@boy_!91>Jd+{lWS%&*-8!8QYXfIkMd%oxj+!%`8bqgoJZILR*o|d`u#>I&%=+fAB zl646w02hr{Z%d%E>nk0eKOulI&iCEPOA^8hx#n~!vJFXTR<`EZ!>`*JUZ zg)>g=v~k!>v6=VRp>t9auU)i^ z|32n{@~{feWFbM18nmGH`gg>7Ks)F|_S4ZV2Hq0`wxKAVk(b=YoCIl4wfTi}=kbhi zT{M{@**{d#gT8mKk+_h(5))Ep;7}_`1bEki|48H^X-HKNA2y|4e4I6vC^bbwC4VPb z#BZkZCWagT?0_heT0sy;t*Gy4$yT}bilmvs^W{`GR*Jsv(s!S>wQU2Mqx7R13tKfs za;5pO7gSk`9*^;U4}8{xuHEj|6)w98It)%wcqQ`)dB%@9wG0Ez5#$reyYG4lV7%n& zwE)X-6WMB6!4?7MOoV4AjwliC*gLp@{etQcezUgn?4RztzQupl3(aN4!aJ zxPl{NF`0DO`M48z8U1R)HJpnrjk0UoOTf2SGD!G-;PcX+Co%-fQNk5j;VBtui$~#l zR!m#I7ZVQR*N#DJ%3-L$C>jn0aK<{Pq{p~HqA&z6~Q66>jrr~{U3DT1j3W?rv^_m+nI3q3e88ory!AO~!(Fd1ti}6XWDHV&>GBbiZd#ef14`7TSiJJ< zI_($H%U8NY*i%urW7h##4KwEaGPV($-__Ddi(914!j_dn+dpJl_WSe1YFMJ!s3S$` z7lNq=fdQQb?}d%{`Rfa{`V|$&nUfV>l=<;V`A6fCX5EYIX^d31%R<#X<=1hlu5yhM zRe6?WYzp~J3>^QcWu<&ic>*qzeRty-|KM3gv}1E8LZr#M5{e(kE90~l6&ryRyQ{* z#OIF}Yb7NmrKDyoqsQFFS+ox3)R^a&N@%{$EP$AG%=^#}^Im>moUz?epLxkGAR<$z z{zdcCgF8MASej6rqgFTC)glUqb=+2_Zq?eAj29c|=?1?H&g)Chxzja#`vrtd664J| zwBHpK@-m_gym7%VG~xH(stae3u(GoHo~`O!>~-MJHM)gaxy7*c7fu$#bw%4} z+}6DgM;YEFV(%L2(koDH8Q{F<7d{d;vL_=*eWE4{6yCr`u zxk1>Y4s7YXhm+L);=|PSpzHUK``Nmm1QVz+-=-aIZyp&P_~U_eqAb2a;TQmgndd7$ zY@dT|vt_xEpx9cj|z}185kIP#m_HWr{ATkP%yT3z6)K1sfe$6ww>dY8#cbFpJEuMGJ68_ z7?C7%K3E~7@!h-C=Cw|R^jc=mF8p4X$c_go*><1JMRFOCGvP{&vH>=3+Lr&B(|FMY zOM(K!+#fsb^s6Et-5S4I5KVBt%_!3YAf@u`0(yyG?tko~T><66-9VfGNz)mV)Ig*c zR3CBz)7|RLLO&w9F>N}tahC@891`~V=U1FD6r&Qg3h##nDWTVT$M{@L5<7-f_jmT! zTdqCCtAs>ve8nhWWo#dN@XL7$yRC09GB=m;pQHGZBDd`xw6y05ftsaW{19s98rn#G zXloadZs2)(QKQm$fO_fy_+7t#Edw__$mf0EafIbgbu?1$QhI0BG&Xh>Wcl~)La%py z_g6``clh-T4Nc9>1J#S93+Bx%EkkzO+T=5>b?qAVW`&dol$_1HlcMJNP3Gqc#vGRq zu|HJlf_tmsmzN|y=QsoTV_jS1wZ_Dw63|N$#H|)LfT*7Ua-t(~E(X-eMNT5|^hP^= zI$T%5Npu%0+X)M7z7}KW|PD;z(10nHNzy ziPB_IWs1l#fo7aMZ9IwCLZ|hAPS}qi5V)9U z(xhZWiB5%ikj>MCqesKrA{@TW$!TG7D3(krGU&R6wE zYH`?&9+PKB{07#ze?FuXTu;IwZ|!c)>$&|}ZQ6L$97UqKLNmo%c#q;I+qM1n`N0w?H4HT*Np;@ow{ zH*b~r{P&~O8N3W=AvM$eMsQnlY#uXO{>Pj}$k~Q)0Ta{P1!#;(i+4B?lXCoeG{scK zVJ}mbDobK$jFBPo!MtN~-Gr9*?%bDoM~w=BhW8tUPHmU`j~+c5f5#U{Yib2a!xujd z2nzCRy*xJ1FXNx8STNmRVMao%KWH!&q*8JcA?I_Zwg@~3-+?V}Q^c-E8^@2Lj^D!W z8n8${Ia-&c=14pG47g@pbiRK&a%%t+O)lYHaA=qO=+KyL!rXBwX6zh1&9pkue&~$=uzlMVd2qJ8|?6Y=u+sp$%+tIHSCIGxnqQ z{GhP}2x4eug&qcjNvrYBiQBQ~H2{*$82!R^eR_W0($hLjRDn6fax*dIolx$wsP_(6 z03Wsjw=shf&nRz4mVF?DKw1*5CCK}4^y1HX|dVOD_V-Xn~Dq1;`5ORnKSVfT*A=y|y6$*rd5+8j?HXzzG;IpQkT@`OLl@rrZ_POM4qWStTaNXL4GOwz+;*2IU2@*%YM zLr?g-pM=t>5fh}4G@t!17a+8&Eigkl%%eG~pXd8GRrat11-cC7DlCpm2QDmRoHesv z658OB_&4{vF$vZ#MTj#8)CN#31)wRnS}GuF@W!KC>kF5UtvOnUw|i?YW04CyO>h4O zlYl>r*#X}_#98#4DDq{~g2cJx)O=9KZ}^M5Gd+=7q=>w@=@u>5D# zp;4#px2rq!_Ik41EGVP7^U1u;QE@Z-0Q0yXh&7gFBrbUCiLMkgUidOrQ>>2)_qQI? zvi3z6pzX!eD9p&ZD6%E~dyc<9_4?MOm=Xq_I|W886ed(UpPBi0jY zz`M~?u+G(W_GL*lX5>T>810UV(#=78oK6q`3TL=@Ru_vgGFx-rVV+@bTV)&d-kj!JiBry%~J!t0@CnBQyOa8E&fcMc(DH&}i19MpUU`0o9fd3y5Dqf1!?4PO2;q!qU=+XvwI zqQ{hb7>IBmPzrcaf}?{^@WEva(ctWt-)FD34|o}r#$H5u-o&e5`OFC87}5TZ*&kgS zrJvlT$BXA>QgYiP_)^s=`Pzof0pB(gb|iP&05~CG6-r-rFE6;2YRzCs^1yG48CwDy zPJ3rYLwgQ>C_;8WWvOUrZj8kKFMlCUCmmQ?Z}hLD zOS+wap4*eUw|_Z}{W3oW>I&i9N9u9{LbzKI*q0qW@wocOW@PFWQ?|V1!De2=OVxMu zGRNN4+t%sRBktOK=PTSHQQxXJ(){L??bFS_3)V)Ju54#mx=PV`s~^|I(kM%tLP~c8h@oU`t9xQ z%92qVbg-jiQiJAmhxOW7W|T5^RTp+Bugb=e`qy9V#ockjo~QZj_q1-!C|y7_=kO|EOBoZctH*CPF-(BA1I(bv%Nk&*SA zQ-?EdY6Mh(a^X}#+5O2lCkMwXA)(&(c5wm6xy6Ikel~V?f*X8sJ=qa9@jGMC zDfg9Yp3gUSa$Xy-B~gLXyH$?akJzLCno{_s`|?ME0b$Zn6p; z+i{kzAPTNY(~s;hu$_`o&xGTKgL-d2u($kJv2>x3%G-V)|q(Iu!WqM}#6kSBazRH^OLMltF@+ zYO(D;CtGg2Lnx03SwdpSl1=C{AIVu!#>1m+aLE^O`1v&Wy|)bIb3^ z-Mf-pa4j0${<#ZL=zcO3RHqGuN&zx|&_!yp2mA(1C{hH1pjJ>&xL6ZtTg!>LN(kv9 z+V*_adSw<>Q8B;zd7Vq`0S)=Vl_F$aOii_RV_9$;2Q8C>bQVm9?r{*KAXJrN(Jz!B zk0wbDS7659F@2B4ucxkiCx+#KWBIfrBvB{>KnH-sWhLb49;eV@w@>G)@VrNJ3P^%d z&f@(A8(;t=+Jk~1U^YoI!n<_*_p2t~J*ND->p&)gx9urd5)EYPpLA7Mo0*2+RiV6F5_>fY67-0a12~VH+TsQ@zOV1$Ac7I;1291im`2Nf_dWl^^o70It#y!Ve^erS z>)CG(0qt}Pae9s(!8?upy%~4zxDcy_gXPU$+n|1X(JbThEXIL_&Ri6s1jf5!89`z| zJ2zKUNzB>Rp-=kJH?AQ3z$ai_Q2pRgHtkl`S}LYvMp1c*dlLq|Hp6!um;SL{Mk5ij5$Q25%+Y&CdvBLen=N}>QdIg z@^e9bAF8Bvm-HVjc$A`e&(_xXvrG_DIr>W%srNR~?fZTXIXSg*ONv~n90ld;L z#MqIEU~-pE|6j2y&obD$?$ITxf9wYzpRmnk)GGJeWl3g~S%TcqeBfbHK9Ww^Ls+Wfqca0Z@UCQ%RfGSM=SEL@-La^7upwRONQ zeum+<6EPwrRgZce`112QMyG~u2 zOc(xP+`AO5`8Dv+B7q53RF4HlKO0g6s|>vd)f5$(sb?7%fUy#N}Vw=dyf<&!zV zj)g5iPN%wQk6D37)AiqAFP$~z4s?CXGWUK^Q{TA5qqA%QK z6ePGp)rm(7lZtfYgFjHUzHwW5O5KW2T@ic5v@f_0lz*_H;;c8Eqk`IdlV6n3d8uSt zcl^3xAeqI#^@XT+4fnaHb)L=LGcsbn*{STCoqBMmJ4U(X76$Ajdlh+dck<`UWE)NS zU^T3-WY+OQa;zZc86dc{@b#o}M)|a7Sf|0SzPGHox#b-5+$qCgrE^`KmuUB+3PxD z9_=#KiiE1k#TFXsg2yC@=&QYHr5!#&7nio~=APJfQ+q`}`%hIxLd9UB&eeyY`k?;9 z7fj-zZhr=UEJyn;{GHq<@Hw~KLy9i-nqd#0n=M)i2hyZC!_N6PV_3U8b?;25OXnD7 zllOE=zD*)swv0DlWjBAsj~8D2bd-P2mii}ZC*CT0O)@6qlB1s~KP97(N4rFoS*BAh zpe|o`Wdc8KGTKcPh|Q z@E88kJ}L}zu=T?4(zJNrP@R8#l2&K3UsHIa5N@Zfbs*_##e#?#u1~ic-?ja@TllMY zWq_Tj>8W*r7w0jGeUg07cn#hZlv&<#94db5bgkiCU$Jlnh30IWN^1$l;Jnqpp3ZPI z@tJbm#H3410x=H59F$*Tmtkvhts#@?{#%J{0cd#>S2lb=*;FBAZ9TuApLFQoBK_tSQfXSXjpI`l;q1cWGr$kgUC~XzUm6kUI<@ z4wLXmYAmcp1{u|uM-E~QF&ZcNov9e_;(31^oXc|R=mPQ*-BeEw)(Y@oqDX;b5(~}h z=8?MAq57fTmyYW3m6Aqrgr^E(rPbBiH5~RxQaj0UFA7Kg`*!0O!9#aXQWRYp~Wp@Q^+WRy1KT4 zOC98djgQ=_A$O^X_n_AaE*4g6#^ixC8cLMfzrq&BXKg5NjSKWZA_umdlOzygNI6Im zUn$CWx*xM|{>nXG`qyPSu;ueZ>Quvu?dD?976rXQ{lA1 z_ku+hmUL&BBrOI3PD{(YCoKbTw*G{*M&S%YTP9e)MlVRFke5|quvp!aXf&NY(_s^i z@yM-7fD-_A>#WbEX}=e}q=jl-pUeG3a~A^nLWvZp=3gZ()$ zHDmF+Xs;sPG0831h?1q!7SAWDOy&0P&j%dIMS_ymlWj_-Ms_|Is0oby<{c?5CVrz@ zY1v?M_8FvRldnD`l(&}Pp3#&MJzyP=U#%NZu^42i$DB&)s--nGstY(=fiU@dN+pfO zqa{Z!1O_HFxX$j81|hWTiU{5btBM?ynhHfwbUeL()|&b2HgQAFI;PT;;%n`+8iGJg z5@V}WSrHM4AGcmx!NS#Maubh`npWaNqyl4xDCDyQ^vE+U8efY<1K}qMDD;IQpwL>D zu_uGU2A6%)M8#=XSl{yM#e-ZoxnyC3726tcKB3`g9N^Q4$j{WIDm-zme*$flMh)Qk zC!Zqfjt|+ee_xj474Rf37d_myB+11bnbBy5l}eHXz9O|KvIvK15JtQkuxK~8F-edo zBn`sk%QM{7YA<-L1cGKYf3?Mn2liVjqi#w#`G&C%CE6(h*&vZ<>Tc>MZ znB2?LlbiBYEvP5(0sHyg&qE)Gmfg(4P~7S`lG~@GEpvbUCTYK2_H~g z^(~)TID0*h^6LuC{YGI6SR3gD2a^wUfItQ$6G)12z#$*ST4nUo3#XnN#yABKrZ(vn z&r7O=!HQai#Ygrp4>sJcZ*y89TD@uP3OmMZP;&Sk@vX7y@9Zq>q4+ve{it1usP~ zQo;s6Yfl52Rm^oRJ=zK!nUd3=9t<9x+qQd(=o%%2@jvpq=e@g%NIvtrRx$Oce_bnTGsQBdgY_4bh)Z^^x z#o$9P73QzrmlnU=d?A{@Kk`NVY-*t}C5iz6ImL>$!w<5*&L4)5WS~*&0Gd}u2i(B6 zD`3p;)f&FG%R9ER%=Uj~&opE^-di`nYtuY6F;e=o*S%L za(GPEGi|9*1LHjR6rWl-Ri%5|WuF}+oI5P+$NbitT4d)C85*BUvZ<)AzMfFm^ggoct-TKMnTTx2P_y{ z;nnaGHp;s2c+Tg(`fAW!UW^JUqWP(*ZfQ@S{>08bas?aj7eOVpM8>*0js<<@@b zSyyrsmUZL&_uP6;@u3tx7*vISk;qt8adj-dBR66o55a>iBKKvAy$IgIbmIL-4vaq9WAeThBTVU2Bb0Oa|

QhKd$?>*~3G?sTipH>LZpDhA|ZtX2mCCZAUaBZi|UU}&Vm5JTUAqJhuUiXl8f zLb!z~^=NNsojmK6UY6%Mx<~oaMG6#O;{F^UCpQlV9bk&ymINJrEo`l7detpdu&C6| z5D~sM$1scXbQfhkTFoJ%8j^q7`ibt{m_u7;ekS23D69y@sCh#7Bef{GV0QbYwq9ub zxUhm(X{(QdG^?J$)m;@jnRh9AAJqnpfQ~_`jg#Jit>u7a(6|SaHSodg>9mbsz^eMJ zs*$<-`ya%wrWS0(saR8Z(+7`cn`vxn>ne&9R+3LtNZJ36^4sbgfF|TU5CUr{A|ma? z+>yaYR{qbX({;3_`sjyjbv-9`A47ywlZD~ReC#E{u~2z0@=X848pky$)g)nuqvlyE zzWJlZ_)g}OUiQr9x;N+O*w!n7QxBCp3jq^24uQ#RF7k)v{3qobQS`~Z?_Ir2)VE7x zU}#A|+B!C*t@?X0r9@2oBJW@h%zkQ zw0+|}^>F*wLQrm3c8%9islUTtlT{@9O9X;KoG^fb2E{ISbf7siOjR}Fr>8Xn00c6% zT}Q3eR4L}0^G}sHT>~7A^&m6ohE@X4U*t>haQK)?Jf0?)%znKCA84M4p?m2fc_})P zXr=d1 zkbclIX)g=0Tt8)L$ z2uH}t^hq@?s;tqtq^6Va;eO?4%hZU8zkr7mQ$YeudM5`*uNtT zuD*)O>cqn=2MOw3FYc_TQParBuhkS;!3-eI>yI)h#m9a5weBYlWSr4Td=t3+cPIge zuL1eDt(vw*+_$fpeuvu}6nL*UaKJm&1bb-+ZlV0DC@Zh&>&R4Y>5-}Mn3`Ibh=g60 ziiq9N*?s_DaK4Diru!?SU(Yr$L!sF7is2DB_~U7x>jCu>>W#+;aTIJ6wFq&RJvgah z+*zED74?gDvgrn8xq{|=15tYc(vy4p-}nd!#+TpN zH7$ZGk&1(1GE=_JB&4u9;Zl4TH9bzwb$FiraWZ6R2q(IH5UncafKUA|#MF}iSp;Y`U&U}jpGwho7*L<4xC!5Lb9ed*ywVo2= zppq*b1h?4m{w^q%;r+Yl2Fd~RypO5h&9jkHL}+wZ&l8F9EKiP?+<>8VcNm{)DdkNa z)Y4950bMcA*cMLtXxVf%(_JyRCu&xd&`NWi&wHZaAjb8n8yMCys4FE^lDAjJ82SjU4rIcgToazZ809E zZMYlhy>Qr6EPBcU|c?5()_T!KaYyRR>yW$ta;R5>qebCd93{HG0jj z(gh?|UX3#axLG_DFAs~gC=#BJjvrJ?1!+B9VJ#}w^c(|79q=r^dJcp~`qg|K$QCq4}-fk5!1LNvmIH+c0izb%m9D9_`tIFux3L z2o3zMSHV>A;_o=VS>B+B6G8mfJLfVghzKKeM2N*Q!{mZs>Lcz39aRCP+F!fx3^g&E z7R7pcwy!ufA#H^pp}z4*Vq)Sqv2MyiykkSMCOY+Uv%e^BA|>z=pF;<3zFouT)*!?w z3c`ulJX+9yaWuFr)7i|K|3sxE7T-)wqt;6aa3p}o+4e>Oz1b3+WTqIql+T_>%;RmP z7pFgg6T>f_m*N9^siVH67$iDz^*Lus0Q+I$6sm8l;i!70mPtdxHCCQxd>CHt`1ojZ zC$!ui%}2f>Psmg;ZQvAx{@E-efZ$LPpNe;K2^*&}CeJRg+p!8H(cYN{mNx_Z?$H-M>$*2#N(mD%EMaPC=-bs9f zy8uY+(D+PV@5c0Q_N}Q3cnH+B38`mVy4NWixh)eipc$?Z6H2?AwmwsvqfV{N+;NZp zE-qHfHejc0+1@~Ev?aXY3HW9AEHg^ZH>6@qww&2IWvrxD69o$-Gv5^~jMPQ70_MtF zt7y;Izl;iLv6)+R%#Yplm2Y&YutJj4BzSM18wM1PHE(1LFhulXylCk-_)8{dum(>6 zxx0B5S7lXW6!IQy<&kCAH*YR$`c>cm^K>cL-_Vf%EwZRu+DcMcGKg&=Dyp^k$$MT- zzM`8D)004!dwEv!FkNpqu&2h<2|w>2E7u{CNA2CS=~mSe%ESG2K!E{iywSDszpue+ zxd0}$!A1htnrB!jZ{u!DaTS;fKH}iu-C9aO5MO5&DF3>Qwo04|>NRxRMLUa5l)hY* z0%VmU!|*&^3IIRASJuhQi-`8NcFbF|vgDr8R`F}8ZQ4jzrTO^Q+~V!)HuGDjmqs6=bVy(fx&H_R7RPe{ERE3*Yf!V#V6dVGn;qHd$VS=_ z=`TecRnkNBZu$NkMpS7JcLB5n$rPE6ItomoGaehwP`~@xpTHRYhO0MToAXki2I1zK zG(L7*2Et%|-X;+HH-6`RK(f^z+R=c?b~de2pA>8wNczo3*614Y2TL2P@2PJF@;9ZH zpOtsleBqCmo+zy-bIjiYVuX)<H){`|$MIUzL&gpjv|G54SP68q#K2s-ai9iN`R?x>`gR&uW#_tw%h<-Blzed(^^!sBh-&T06 zK879rPXK`6qk<(tpMb^@D-k+=0CeDfPuWO_$gs4*7PBe!k?8ngym~5&R(q~#6?p+# zH{U8vJWgRV{494~CW}qV*Y0GrIZ?*q*D%$8FYE%`6om>gHJXCo%e;OU`ZijNYz3Q2 z;rgh3*H`zP9=2NyN+Y_?JtiNn!z{TL@HTWd9S7=~HtOJ&|A~w<9RQW;4jhKDXj+Wth(^wP>rV#-^@p(u(O)P1f%)Od_XWCDda^H%oNf^n%f-Zi+Ym+!CzC z>=%VNlbsLY!o{A-O1`^NRMR*GpQX_Qh%`d@XW>#O?9HukNGiL?AA^VEdl)>K2z&jGlo>^4)7^YD5%Hc_k%EB1wl^ne2>QXFZqwRgQtX}WBoRP3ym3MqGihF}^p=Kc zGri#Tf20u0kmN4=ndJn>tPnr;wk^bpHWPKz|hQ zBiR!_FKus~7nwNBuaz1|*f}p4#^Cw<`@+Q~+CrSx5M3pw>L*9K7XI>okeP-5ElmJF zuoDKlcX!J4)2;W79yz7_U+AUwM5_EZVyu9EAV7OcUG{#E=kh;MU3_-4s1LaRHwY_wDc(f&VEc7S<3DjQro{>i}K&@9+Qr>u(lfNDcD;_RRmK6(i^&UFij4#FzS% zj$7^jEx&m-wb$+FLV$HG%6x5Yn>B5FiltqqqM8M?V}n5Y(9Q&EA_muG+t%`F3WOO)aQ}`$q@A(DnUO;2$`XCLVmV{C0HIJ5pe~6X9oyMcT3at7EL* zZU3G9IY(;&Rl;v;ED2G@zgxTbhE@|RV3+Zu19QOPcWE)_;x*EmGC#v>Q6hQZWnOWe zm;JlOb^ZP<0sWyU&Z4L&yD_al z78a2~yF%aO^)&U6;q}t&@B@igUU&csGYQ-EQ!H7DP@5(WXqP{0`|4p(qs>HZ@E-U! zQGEBxCxN9%>%>%xL27%yGDzF|s$u=zVgD0HA1p7mQqR~aYsv|ymF44Mi?1z?eo+%A zClPIMf2dbg={2>1a%YyuD$f-q)Hd_#tYGnksr3<{O%yiKS=bSzcH+%nrxjzF8&*ev zrZ+49hrKrsYV!R1MzvO{E5Fu-S~j&TB1$a#o}yMwAfO;1AVe*TVV8YNQl&~Q0-At8 z0z}IuyAl@Jf)oW4NGOCw0wf@t2}^(^gzWE)+Goz3Gv|+Y<~`5MnRytQ!OWfezOL{6 zT|eK?bzOj7|2FUYvYk_O{gBXIMe^*%Y5-h$H{;lQ6-mVwgGcobZkf2m*j0;?+U_JV z4zIm>HGDQY2&F;WGvY=4SZ=}g$Jr>EsU#0_;=dw_LvIQl?! zxhS(Tkl@)1=>n#R|)+yJe-eOngU;Wp%%M=bpr!j~r-841 zd-jhvWs7fQL$xgTEa5>+!QGxiQ&}$ycTE;YilMiMcdfKR58n7|z{wa`Jbj}iK8M`X zbLOu>=h^DQ@n%X@%{g_8o|&0A1?rB2V|vtz;8ebv`tuWWyO=%WVvp~8P811L+Uc6N zf`mA+su#sdy-CY;CBWu~pd*u`fLH2!eZWc6V84rVCPzcP9IZ${A8YHzWrobAC}%l3 z&QPGx@3Lff{JpGx;IjT%1)8_5Xcywtle-lXZn^lDb(;C+d(g=7$jfvp^7VNhyYaJ3 z5^F5w7zvgqF3VR)1LUi;MA69#p?j4(+#WoBOl#8>0Mx-WZ=HoNU&FVMB5s+i^-{nRc+8?;*ek*++53UOExW&ba zk9#?9n{OEb=KEUsw4f+<=-Mdxu~--yw15Q%P4{G&h&e8TWtLma%FpDelRO`&QC8cX zIeg0x-1ff;-I>nvuCwaiiQd>M@2m-ume-*5SFclR*iXac<8u6nwL_G%%|D%-$O%39 zQ^{<1!WHK5OY~ESxpT3z;{(fKcUDMXVQWeLB5bKNj1wp>F&%fEHO+MC@?ZTb$Eh2@ z5F;n#GeXORjruuW?tpPNd8Weo=*IUHJlR4w($V=^|B(%#vkSoN=w~+ppwVZpfPYKv zpAI5mppNxkj^^+4T_8L|LKy@k)xrBkYHIs^Fl>Y z8%ye0*e5SaiYSUQeS9#|!`0LbgSlecQ$NZ^NGSS@g!$5D;WYdstxt|muV!UBxsb>8 zu!1csH*3WY1^(AZ^>$SFWNEm<+4NEHNBRd%0b;c;;S%cEF^8G6M~y&wZ%7G|5tPgp zQZw(AcMr2aQG_0%F3vaO+A2t6kKO-RK%YJ=JChiUAw{oPK#T+_c*c8Xwnb^}@k~GU ztY4mCI}>paGp_WOKzZq=G1bqhv@E#0DGwfR}bL z9t-1Y@$S{GUg@+VAh?B!Dw(pvr$tlNwxbc8rT^#m|5UWONh8jV|5V2Nc*hcZR-0G$ zXDjI5Q*1@eEhg)F$z~$JDu6Jx+z)adIvI=CeqBOuC^IAzEW`pFRk-ZlI5XpQV}Kzq zhp2mu!__q@)KZ*q$1a`xMQ`^})!O!5liI`|ey7RKWt6uxyCxNZ`xI^jn&L7%OY!Vi z#m|@-U+BWyU-h@g!`q3%2YD02gPN39_k+8NpY-@SmIY8QGs+ma_o|wbB zG7cmapX`Z_h$=|U)Hu3E|{S!Cl+e-AoRHK@x+j&iGKj%#~^#J?=YWE zSl{+7ZNn(s@@~TQG(C;O%8qMgnQ6K|Ovuhi6l)pHtL><=Dzu_%4Z96uM;d$3_)XQ& z-+0^Fgze#2cj2%K!{CI6hn}h?yQf{Md7|aV?Zo%n7(TFTQBQ#QGZ}Ro{Zsm@L~Qvv zsID$U`T-0qqz8ZGuw?BFDg6Sp`+{=;rSt(#=2|aw$EP|RIw1%}mgG}<@Ot@ag4sR= z=-|YS%Ah(I|F1^j+gI))8tha5dVC~*LrEMZZx_suvN%q>_L0uDNDBzQ;gse)SEc7o z!acD~tIKI@_Q;?nCxh2MWUh9Cn4XsxLo^$V^eR7Ny$$}SonmzCmvSecuTy^+pc*qq zM_Sr9xXmJYpKDfe-foJOqc9X%&XOk_=N6A{oKqch7s%lXSr$O8Z(|Cd!Vu1)o2wCV-Di~SRQROsim3!t11oGE) z$FfUGDs%_d8_-N z?RjbHu5ATpFcch~ShH(#dw`DUQlN%T!c|nY(|mh>DGxme$d3B=OR?z{f6=k$?th>o zQvb1eeBer%^SAy9Gs>CAhXR%fL&V3;Q-}T6c*>lEK;pVfOvw)rVb#r1WMRS=0&lQOw?W>RHRPhNb zT~;TW)r+GqX`cLJN(+!prPHhl7I*Y_^CMiXiq-*-A_ z^2e5@|F|W%*ff@oCq&F~ge$?j;W|ezmItMuSphl*>=}Ks`!m~D>geuxc-aYkSJzgO zFsNxS2p}LhUCaLdRi?u(15z2eOP9k^KntA|&*fDaSqEJ18M}txe)*qxO%S#04fSox z@W|aW7@+SK5m5x_K)pz$Y0}JlIWYR*uF3w(fJ|OvPwlYJBa6;V6mHu~YA)^%KszgF z0Z@v^9=5mlT=<;49PV@AL&#nCn5gG+cX;4*4>)LJXdXI;9VP`H$=I^;w-E%G`HSY_ z#M$mpiWl7u3mFf%rEpooZIRula5EXiydMqVD{7I;s@X>mtN+S28?a2 z?X&~%pp93i9c%747h;QVKGM7!_91^`VQ@t0b^O$-4G^^gU*c`$qgG^5Q**|0g<>e1 zGcI>vCnsX=CKcH4y8Z2FK<<%Uw;5%l<+}8jJ7U2@lfVSJe=W29<;3@6aljZjrMYrN`ei2 z+?ZD-oI8kwEui0DeB(CFYAf={`FKNAypIBM-GInMN7_*>@4W{+qgv#QV}EDo`R&?e zxi7_XB{u5P#K0@Dr)}@dAj8-*Z#m3hDRFv2p4!WC-BC0fbkL1-VkqP`cq6jY@9ZVz zO0PWVwdax2OB2a8S?ac#_`Hj|b{Uj4j`fLC8%*jCXu0%#IO`MpBip?TcS__x`$Xm+TKq5@)s6QN^j2c?SLbT-$hXd6cD_Wj;XX zT0@jfUR(S-@$U)3kn1j>v`A&w$=aTp&)gtgv9xtFwZ*v3#>W50;~PV`&`<&$T|76+ zKUaOiOH%sv{w+Rg3qgKws>Mgh72n(ghC~lPsnTkgdrej!=dEWQ?P)N#Z5a1z{h9OO zt`mQsv%QiUWHJVUhWq$C#V;K?^-T`}zY+N+D4nakU-0_Z5B=f0eSoAUD>7Hu16z%q z*r5dCWmTS-Ek9w<`xX=lh%w2fqbKX$HIFwqJU0qXMdM_XXCNN${*G;hZ`rtW??`-n z*0W0ociEWQfe?b%(`FXm9G*F=t$E@r#_l=DenBb0sG}n&Ns$AjSM;c{T$j_O*IB{a z#%;@%Us{DW$Z3!VshVDCx15qZ7ry}>_wu6_-p+w zdD-;Fvu+C3Oy)b_Tpv5v2@CF^@#y_mHeMD%wkNs2uHHu8*@xA8kUbn@mgBf{%dpvB z!DevZ4o=KH-Sg`Mlsg0$_yBSL(Mbd_8Xy!Ds$KgFsAfi&dUV-l`xe}dE7AwIw)I(+ zTdKQy=n!6slk{Gy15nFrV&sEzN4RUF=E!An^~vQI3E9tkTTbw*TkYL=yVl*ewm!v0 z*W^1qDCS&+Hwm;XTz91k#TAXWEQ1){yc!xyNTr)yS#@d#L00!>RDcXcTCSt2<=+QC{}_G6 z53=~Z--*rS(&=E~S3H57b9}i*d@_5GKZ5Q&-4PuBc!uskoNDmz49W5!<^5Ep>y%rf zv~sgt1#MFHYt#Kudzr62t(N}d#(xfQ+(a&YN6JUI->d<L1foLz0^c>pq06?4vhK9Tl{}OBCV0&x+*g5AT?&)b)T75WNmqQ zXRMH4#s2`y7*xxxS}V|L88KnDmHai9?PPue=urL^cI09;6P_QujA%{tCN6nrB|p=` zW@QzH)wZ9={m=}tbIuhSGuxi6GWWXXFMs%H>*OzoI0{_Rcml(rCP=@jI$iahM{Uj@ z4fQ<;2aLxxRKqus@!JK#XYtE_nqn$AEkMyY>opQkE1%`nVN@qm&R(zFCVk1h+$i172e5T{&(3{=%EZs-+INb+%!4Lg? z(8AeAXOPF3v1RXp9PIVIAqV6R$Xs&LK!2R%PYv((GVBO*Fj!H3H*Y*xL--4ppS$ip zZ5#fm)&0^xt!DoK<<3BVTH_VL{i^>Y*J-@&IU2idsVM9gDADU|W5u|o;0F!Ta!rQ9 zs^y5*&J8BAaU zu?FkT9PFS9qoS5d%3DDyS6BsmtpQCcsPqv26R-E`3XgedTDD**?ROfoX$8d^UN+39 zoTzzgTW*zT+jBQt^eW7&xXIr=COV3zZj-PSlD`Od&x!_kPses{0g*~*rZ3Y?2Pzm9 z%)xqo2uyaTYv?Rc+Mh${7d0lFAhq2~<@VWUm0!_&HyDWrCRwUFDq%QOm;Ks#HNW%x z>g@XaJ6Q4jWL-|m$PeHx03355_5FBm3Erw%D2G#Aee#o)pryK$bWL)(vu)pj0j+BF zu!d`EV-9m|S9<;Y^xWgAjq6eTLggq0NDftI%3r1nYkEq(o1nLi11~?`geuQb`LmSF z5)LU2n`usMh>hmzDt;kHl}IaED4Oc;sPhs2Yw;RFcWM>w{?$jW#p6i^`cSg{+bkP( z>0(Wk0<4Ht2WkW?RegIl#no;X>o1AM7SzvFySJ3P4rK|UE)q7ZEnksUetJYx#tKr? zIN@GODl%+Jpnv~_+kN)ZvorM5ZjZ`--L|A{COALe{;^&<&uOWO!_Vz0t=}T7TQc>r z_HT$wI}}i9voxXx(kn0meQ+y?SA4~-_N`U% z`nPcqgUdtQK<43(8$*rJ+yHcygsUh{uW2ob4|`s;E5dd#?di;B?pf~vT-?Hs4ovelo_MeaVrN$5a>{xHqqOzagUxp3`&XhS9AJA%luVcVH z`uo!>>Vol-bqu~aS8=9h4ovr1zaL?Cq^2#(%`%-PAy0m_S%3e!8shLFdFH!=$UGLWshp0G>-X~3lL#G7^P@VXKuXIXq7}x z$R=x|HyBnYcHn!+T8P_L`(6Pr{^?TPgWeL0pS~u$+A;lbp`i;?UR?VKoDUM_E}!KZ zi)WOf?WBsCnRB$dk2m-2TEiq2RB)7y+r;JjP<*2a#VROj9whh7@QJtgi+#_418vTg zwHIP!gZm}_vr7S7@=5nVb)>067kOEls&R!7w3u<__uQiH!jXdejN2p8HnNw@MR}G_ zj{;jAzzv*i@S}YA8fA`rpAHdccZwH$r&ftSy4C!AE@M6esD12Z_GUR)r~OpZ*yt6v z3$Bg21Pj;&{12hhmS$nQvnOpf$(0411?Re+f?M+GU)2xetmG3oQ+V#eCf!oT5F^^_m(N6 zf0bEb*%aZe>s}q$&{SAU{B}Oa{BNMA@f+9f`2{KF#PL^GuxTM#VX~{(Dx1A1KEdRb z?15OBTW%CznyW2CiypU>x6PC^3ghLjnr@p4u$>J-v+*`Kq2jh6< zC50hWB^!~ki}R2$n#tOv#u_|))y~-Su&8{kdM;TV7E`k@Bi84rfiKyjilS_@Jndvj zFN#;^NF{}*4$2CWzEepzK{Hw3Zb@qYt|!YA2#<(Ii$No-!B@>SW#GYa!t-6ldD@g~ zPEBcpkz+sTxDVFf`7RRkM$6{6d7#9OJ62)!^YrD&`Ruec*MbFK#h+bP_*#yuhwEFr zcmU>ljJeVgeejx&%C$TSn)BTUV1Td9_6yy!v4YjelK7gMzY@>R31=v0)?BTLbcdsm zgV(AbMQLRlth*o1Q(CF@0&hG1`%`gM^W8kaXTY5Ij$gDrP|o3~2Iye1M2C`Vj!jxF zLk}50L6j#gHZk&!+HG}$Cauf4>$BeQXQ4!oYkeQ0y$^OG-A#5g-k6==9Doop>+nxP zEWNZW38(kU9~^!74c;RNZDIfo668M`+V<*?55}!zJ>dSdgaEkNMbf^Fvu&=%Hxst> z9R|90=canwbF;5~n`ym7bJ1J-3;42uQf^i) zYuDt>m))j80j*(;LoZNmKy)F60eaQw*98g$u5Nv$F$NF$N_YLn)=6?sDKM5#V)M9@ zWQ0J+T<^ml0WOlR(3oJP=hb!%*h}#4$yx++-adHSR_UY9ExJkPBjI2D7SL_F|Ftr} zCD_%2Yy%-l)GJ4Hm@H!VC-J{xtIK#_IIC~$JZbH{<&Lx_1Lq|?cx6;mrSbum5Fht z)Dm{IAN}{fw_Ysjep1!fBZb4wi>0r_I?jFWPM>`IrQ2<<^;vsgf4B2V0WQfVu&M{OOM2+@41SyMx2Aygsa|(@(k~Y~J_vxlK`+n3zvJwG zDUUz;`R3M2P>GkQU%GArGrBf_ovZjF zX8F$r^+xj47Z}t;6GjD-i>1>NbKDHa8j5V5*+!O$s^OD;x zAfVW{H^pzy$>O4Iyyk0WK8Azced4VG7E*~RqC9T_@qw(!W3dHnyE7y{DL!|p)rr8b zcj6=kpj^^xY5*yg#F*6EHa$)O>HhnI()S4?xpOH^g8r7#_;9mg1Q9s}?W$NPLlPK5 zb`bSeE3Tfp^w52T-jJNxdrWroD(a z_!Cj`p<)DM&xPsWg0@FhyR!olKL#j^-~4o_xw>a299+*G*mEDD)LFN5+S`mQU+&~j z7N|#{nqG+;pI5k!@Vto>tN18xsK&U=7|@Bs zb;*^4QJP4lI)FF+wz4v~Nd4g(am^R!V6I(IPy{M28pg4saOA1WIu}yP*nqm|RN%c1 z@9Ac@7^FyA%E~-g)P<~TPV{Yl(7|*(uV2w5{jdKhVi zrRp_55R2XXQobZM;#E6VsWGgGD+V6kLz9ig#|iz{HJH@S#Kzi4Ma(h_SXT(K`rcRa z_iQD>LZ~Ex3tIPo_EV0z)B8!LkbK9;s8`vYZTV`;Tz_H5#2~3>Ik`pjF&_=3@7uh; z7VzMiHG$dMIzbWblfY}SQ2Z^Ztj>pwoYEP?P7^7#3_75@?92ONG<>-o6MQFLbT#$mQUt>xV_)Qp zMy!`637-;cn?F&yVC3W76X$0`K9?2Cz zE`>Xz0v2ZgnjV8%S4Lw)6Eb=3K9sr9p2O)~2%HWQuIaQeANR(#p6kM}8y+!@WWROi zbqP_A+!Zqt2;D#WS8Yw%n(k;5S~oF6!*}8D-SM+yiW6Z-1v~&5lxZ|Syw=~M$V(A& z=a$oiMi!RuraDkM*yOrSQy9X<-dPgZEI<4u9=Qa5%SoBO+yL^XEwE%a3|hmrCwu|F zMQoA|G}bmW<%cFbB4!)-q33;=5;h=#^|Vnf>5}Rw3z^yRB7eJFJi32a$E1E882I#% zAUK%N(`4@+DZi&e7m{UOW<*jvuww;2*CD;OmXvD*c;3ZX6(XvH%%tT97uXO0yUHxd zb((jg$9e|_ItP-%A+~2e1>DyNVvaT3p*_Q`3vhAwg5g(tqG}1D1Eral8B@7?&Ufxf zK@OXySTI;H`kpl&dLsk)lUgWgBuZ97hs{O?jP=E4;sMD>k$%WiyuqWr&P)9|#UggdvNQ{NYEPGfYP zXLJNY50|Pp9InwpqH}h;!zt^DTz_-5V!T5{U3)gU1K`$DTjFJlTh}($N{sbj_@bg= z49b%pOC-ySw2E^lnunvj3Dp(s7Rxa^aE46J!l#TkW9KjgPW|HRDTAJLV1qlnLx#?1 zBLB276qTdORT!5c4d7_~d4u*GYUDhip*AEZxP)J?aQF`HKt=oLgLFxpU=Ql*_F{yF zlaP>?3xXghm1Ab{H{(b}MVXc@u-+l`!7q~4bA>{xOt6K8<1NufT+# z=qzB6aryzYe0w^-Nk;A#@nx|?KweQ__!CROU>-M%s9`=F*9{N`(!`b@I95<+w0D_4s#?yYnEJP%sb0R z5=h(t{fI#H2gciKsV$OKyiOEYIT>%n&u}Fu)5q<9h)m9N+P@@=lLyL8*bBk6$XA9m zPS<(B9E(GgHr;L%y>L%=MU!Z%h8}wXm~0|2S-5{fR8H!M4>dJRj-Yvya6EKr3uk;vo9XuBqTnt zRW`!__7P~YEv^HiQSXpDt5>L4Qanyu+8sZytI$?nPQsx@V`xH&x|&m~kdCNsmAa8+ zDhhia$ufLKx_iX%`}#6jaG>01Ynu*??rZ)-Kwa&V{y6RAspG}Q#MoVPVF&6uZ{MFD zdYGPWs8&^k7<=n1F)k(pq4t(>3-!8rLATiVWmUz1LfDF{j_@w#(yz@X<5#@rWK7NR|Rc_sxy&wuoH5Ny}?D_KJ7e!EVtl`$bRREWrX`Xpv5RQ~t5%-gyU`o_|Pk(P~aar*p zh%E_SZ^fA=cZ9FEiZLCxPR+yPECBJ~HG>*s#AEnj-xM~2yW^7Du`lHw-HFY;;2`%jHHGBJmteh1w@~cP^M#{_|HQMTw?&;_)X(uj4XFqD z6GD-tDKbOqv`;MskZyI`_6?tuY&O^ctVz>o|L0OH>6F8OBIXO4@n7}ei*uicz2*PC zWc;5?K1uTbQA+=hT>kgvviFJX(?Yb#g)7kb4-(G>Ou^3A?!W%^1u1#AiC?@n8@&0~ zpqAcBH)TO{r4=B7Z|bqx710`gOWs$Xh}@)S4w$ddmHsv8)QfOToXyvcW}A^>Qgx0k z>JOMLThkudX3cW!O8AWq+brfccIZJUWQf=qgBjfaGVr!+bqV_I!X5Cg`jb8iTks^P z8veEX!`p8+wtc%ac_VF>tottWimYBA z6?;R8ATPTstorTFk*T^*c|YkW6vNP>Hx!xjdYW-D1EmgcmY=!B#mG8BM~BaSCCTAM&0-r|5#Kv-%)ho7L*|&%v$CQJ z>-n96LH~`o?ddUFlg{Slf}|if{FKioDT#tk1HwSmnQ&`!Mee4Fr}1`NoAmN*g2iPS2%`wyyfPaVL7O z2k0(!*g?%;U^V|{(&Nsm$Wd^tg_TOp{M7E6+3O3}#Qr2~lM@uI3Em#JZ%9;KGn$eL z4J@-*jExKv`z*u2?_|rGNDQ|LU{nBq`Cr*V+-X-*b?sSbRn9u#I|Z;A@kbgJ~|c`tGuD74nN7NG!3X7^`Wd!7GL|G z^h9MxSU^h~F3aC2q|08Of4e#7#lPdzwT+Fa$>hTfnTa_(t~`>apriZlwM&#orIYy& z_r=g1_qmjK55yAh^}rw@N^GUV;Pftw$SG(83P?VARhlz&uw=oyb=|XMI*U)eZt4n; zJjO`<_BA_V?f0i9CaKxvl1x6^h4?gk^0moM_vslc1dxG3%{k6TtI=B^^a;)fc@XTl<?{&`!>^9!ui*Nc{Cnf$_}IR3PBZ3R3pE7Y8g5u(;P zGA#(!_cS;yYEdd9N0DzRPQzg0sKX`|h@Kl5!4hBKq zRF+A_y^1q~9ywr??z^%Pk^P0U#Xkpi@4utRlk~My z>g*%KA7ufCDhYt;Obf=*t4nI3`F&9#r)1XwudwfR8Q*+3TMMaU!4XC_N{(34B^p{z`930Js8&6zp(<)x3+&WP0j z8I%$KVQRrf0sx;Pz?($?z>R?ySfP1%5ex<2a>0wv-5Mz#+`sz=4mQI|VITumK2&+g z_=S1#!qQO`x75Ni)>c^4mKkc@`;_oY1?OwxYrR!|sJK@pyAGlwLuo{5&UMX{#)pZq z;~Z4Jj&_jWkJ{R@U@}#TviR072!oP)6k>E>=MR^1m3icu0FMyb+=rE_%EoM?llln5 zIO+E{7b6QZi|VwgNsUQX7k4BVscXULMVUI<+Qr6*F*`=bkz_l_7|ozUCOuV}vNDpDydmPz3zGO@3;!gnY znA{{U-or439qv8!@sAgMS)}zctU(zvkX6d{--?PbCr4RASh2)tGbDjMa~#_#Glpe1 zgapj{IRUVIrlI1>y=XxaY!ia`OI?Z@u>RbaL?c#*t2-nwPyjzq6D7TCc%8J_e3WPI#0^ z&xmN390#HmBwhETKC%gBM*4`TP@h*jQw&9yko?YrV%7YGLbU6}#cYCa$HdRHDHuocY#N06np=eoy6pJR+;JM?lZ(Pg}P(Vr6Y@4(a;_gx)3e z!bO{ALW(dnpspR$pblV^#!E-0vjlt2PuC@PU{8-?WcN9eBpPo)>B(6=yFbWH&?d+Pi#%b_@9ZXZ= z2U#@7SSw_{n$tMQZN}QdO!0ozM6jB^rW4Z4@N-tKg=_jZw&0WHa_McAjK6ul==mGA zB0=BU%5s{+K4q*{a{qB*80%drw;c=MkVg4x0O)0ByYV)I9!=kK9ze1R&0tBHq33oynZ6^J%F-YbcmI{-kRgCw4B|Kd?k%B0}s%5lgzul0y zC-sUqDs=Ra*;D8OS)s_`y~24R~CxMlnADo47(zq2-2eV#l3p_9&l zWi0@b(B9XJcz0Vmt511*OC$^a56zp;$rB2cQ*Z2%P^qH>j#9NtTu* ztdR>_#rfdAxab^zYX>uB0U>`yDEzI)aWhAX+n41@chp6cVlx?vW#u^Qo1QMs2wbRG zU~TYP^C}FMjcB%n8KNUsH-3NVBsFfA(S-y;c+{$+Uwje_-ZIEQ(%RyDq9(KeFtrwd z>DFU4{4H&u2pllT^z~ReOsA~gX#`daz)^IzR&l1tYtlq@Y~ILX+=&JASNPjTf4I64 z^4SAqVRU#agAHXN_)LEk^(J4|xO>O!8<3+T{z+|)TRuP@$U8*HyV%v}^qeLd zh4T>@W4%=RJB^yEvQ$Z*?z|=;GST)1+%b>)qc-etkzUB3Svq~i&d_1gM&7^}`pFap zy4n2Vw(Qw4y|`rkj;f4$oM;B7&}3E~6Xw{3Fu*B*d~BG%u!P<;pVlXvI4*^eo9X|E zGN`(wA)T+jZD*a+SC#}>_i`E=Gkx#3|?o-7;V zwO$>2Pe~0+vI?LP`ev?!O-wpP>0;+VofIXD_sGopw53-5FkzOlz@ps(Fl!8nOKc`( z@j%!5fKJ2h=R&jJYLYiIkZ?+t$Y=Z)%M&I*ili3o{Z77qFRJr0s`O_#ZZuhkol>Y4 zTF}>#8=1TBLPp_mtM3CKMS5Yb*?n?gM6?TI3hSbp}Yu@il zV~4YGCmWc$5WL-es;Ao+@vxZ}g0k{v;aRLPLAPQ-wP~H<)ntg&1CTrfrXPxR0&lo# zFxzqy8*Kp)Su-FyQh$UcCAJEZ+j}X2kg+FGNSI#dp8Ki@rD*_+T#A%t6h^*UY5AO& zD5VY=ndl)08mAY-yx%eJsYJvr-z+gRa~UX(k_AHn=BF&_^2y^otTv7H)pHn@%{d7m;=9Y`R<&d^iOVw^=~%;i4psz5MI zJEzR?V=55<;8a;z-~Eil#e{nLWBIzrY^2pVzfaT(QZvTmdCk&siI>XSM~eL@k}Xj4 zA>nYQv*DOb(|Yx}-70fW49CJ)nMDg2k`ws*nU&sA>sNAr4bsN>5`EQ-d8eM6us3AT zZ+^M$9k1ff*3QI*NuPW4tgj@{_QJ^ti3Xj9jxd6)w=)hvy?z3rYhTiQMxi)kgyF?g z+LBSHY#foAbqxXUX_`wIrid~h4h&gZ@gFGZ6(_}n*_J|Cx^xJku+tz#$$Ou$MQwRR zO?@~DYf!)790X$tfp*v2$H~(dl#u(BF@-W*VW;gxozoK7>tj$=30qW^yJ;2XL$>qh z9w43V@nlZIy1_NF%#4Ix?Q9ukPS{6oASjEyF-BBkVFDW{_jKt76LBUfDxkS3{jm8C zfmv&77)e(K0!XKjRUpP92=Hd5YmoM?!K)4eKh_g<`Tqtj$VlLigeFH@$n#wL?rUEvb=ImTTH&#l~qa&Py_R+j)6JS~DxR}>YOJ1m%xjyXx zJzTWpzrT=Js1I1ymARJSW@i_}cb>%jo@964@CFFW7mZG#dkf``_YeE$T;AcAXSy_R z?MyYsE{?Qm!Q6Vz8`Rg#4fVrk#2=MB!upE!GD=r9o#?6jKJ2uUjWP9h6aksl9mYCI zB81rE2()4?WWmPZ?>iGn2A?Mc96Gnu`mMu=>Dme-2qBCmCM*E*xWa zvu?n7`*imY{Lahbda7Dg*ldR?i(JK(;m6}8l2LG?-b8p_49*xC{hh~T(@$0*!FbaU znsIgnlHcaQxeVkMhyj1+So%mIpFJ?NOtu0!evQLi_UKC>=BF;{$NTZ;<8SjPfRYb$ zn#WRPx)uuW9qMRYTifW14B_Wr(`YRinsb2eAdb8dM-Akom*4fwMwY1&g0A0Y zO!b+K{o#_I@*8XS=kkMi@tfk|!GR*x^vLk#o6CDI9czN~6heeeH(JyYx7T1LCP!ZSBrFYG4CSzsWYv2(*usA2iLK8=ow$ zG{GZ`a4g1O_^=XCIvX&S zhM25EWaN?8Cudv7C*CD57cQ>H8nK1+VtcG#P`v1 zq%M0aN#m2sMh3sMY;O{u7Y_|X&1xCfdPmn2)+O$dmo)tcxTQ|n7=I0p z8Icj}S7niX!vjXXna4m!41VxsOi0)44i_Ozi31Pm^UmTZ zUIr*D(-=WH4Fkpntuhh3>3jk*-`AJz3@zWkE1dF)SJ2n;_FaH~;)1;%kVGI_o-p#> zBNT&6_|TX$j8O=mBYpF73(Gq70NmN|cnI_`)%J1J2l4dSa8ZrmLE4OeO?z&jO}y}zH+Iu9-s(4c?*7a!lvg_LZ3v>G|af9%~t6NcI+x1 zHT>fjG$#Ky@F4*k4!Ri+KdI^DdQ_=b!B1cU4)ZX8ErrgutoPYuZE z#G((|R;X_2@VSD7kfh<+)M5)Km?x;@rHZZ=(54YZto2o$WRA=4g3pYFZ=AjL84TS( zn!kiM`L4f|&pZHaAwf}sKSi+mZ`O}lm)?1>Sz3ivzLih(wM$1?+mh({8#DdxSA%$! z58|=>zS00iybeJa+o3uxJqj!e0+7zxnK5QbGAzw489QkBi^D)s`*t zi#uyxv#*Z$jRa{u$7Ph{8sk4T9Bjne(Ua2t!Q1xRGqY+p0Ub~*;W`q6LQ zyj~uf!RtQ%N#D1`(N3P(IQEe6>R7bK1cAgE{g&YOFKMXBnQ}X?rrtL3X4`N)3e+u3 zT)Ukfww`66fB`MLr7ygvzB|7-fV7=pRKis+7yColHNi%THOW>tOp*Jeb+ncI(!V5& z>S3HSrUZs7fgsj$nLdY921l19w(RDjb>41X+)X)qr?@*CEPC25`n~nNV(ecgPz#2S z%qCCSH7_4W<`)gi$u|6GxhoVmP=vUEPzpjYrd*WWcp?{pNj(Y~6u!av_j!S)5V$k2 zlKbP4&1xSYOpT5u=CEg0$U8T10OfCNkKqqhzQ~0nnm0PZ%7LdU!iue$d zQ6pJ=gN)}SL@OG<5H<5WwSjg2S}78%rd4JikFCU=nQHR_U=e$Irgb{U1j!08R|`P? zst*v3Y${X(h|MnrvlrsK7~+9m^kuMO2XP^FRl@p0E)D+^@clsjn+W167P-70O-0Bi zflEnZg~BwW^JZo!amOw)M!4iIIBR z2U1sW`|lSM7XqH4h5_Q{FkSI43zZ&hYm5dsUsh(eKuyb{(vqzJ$c^Tcbac8tjV}wv zk=DHd_M6ApCV9>##42w6mGlfu%%JXbom0|>z+m8f8sQ8!a}*NXbF8pb3~4fEPB9EI&&Nhd_XJF$aeMT-N>$l(^c->U;mwYW{VG z7wG`0&(|{7hER=HC#vr;R8w}rnX(JU@AKl@K^p&R>|^XtiNlQxCm%WiY#()P$Z#WV zZa^@?<1{Lwt4V8{w5+}#k@%93fjU2p5%&yD9`Pk=T_&e>j*CA|s4W7Jyq`^Hzxh)5 zBY<`Q6sB1Kb@6^-R%KYoyfjPy@Wbf`vnsVOqYVI3 z1S2L&Z2_#4G?l_&8u{_@;0rzz* zcLHR_NKx?!Z&+D=LO)0q&`r=M-5gG7r*teQ13XZb>b5*H^osi?&ux|>7%>`GE9PQI zipAA<%;#=;I5exIC@fQ<^hhZ|_Kl%BAF=wI~#H#08(=q`GjA$QSWHa#XNWUL~t4q%`OrVXG{_4EPb zkU%)W{N=*x??_f@U5ML=Sp-{Lu_^G!*_(_-TySVUISno|-efC=&EKH;G~>)j!mzbr zq2icoKsTVAZU7SN#QVsc7;js(+ay-Uy~qY^3}=T0D3mcnJN?>!`=XN+YoE*5%mLri zKln*L%(lwgH0PwdvyJi{YMWo?+k@Kj0nYUx3gd)|wn$Xw&dCT0b72?~qYBWPmM2;t z+k^Yw@-KmjrbY;u0$=zCj$xhTkV9)RiQI_dzV=`{s0mOxF*i*KH*nI}7#e^vLk zcCF;X1czOqk+zWbAkuL$^iA-aE>xH72i2cIw9%ZL-1Je6k@a`=S0ZLpPyX*k(p`ff&43ajQqtWF4WkDT6_k>Yj-iHviprYSL7EDyaHMU`+K=06|jkPH1*R2zsK6$L9@e&-sU zP-9TpMDE?S7v;*LD`Iuh<1}-$p|{thWHgW*Mg9p)re4JPT|CKcWzpw@@d<44mMrTd z%f6Qi8ZHF$ea1(x6o@_WDfB7%4sPW%>;cq=xD=100FKare_P6-9-z{G6K>SKFvlz- z(fSi-*LpAPd%NMB#r`+Vn-dai-ooF4GJ=!Oz4+YYj-c_68e@HwHgQ!#x1(90P%HVc zcAH(w?6dlMwzRN=&Hj$VXsgEp7jGU+_E{LK#b)R#Nu_`G+P}tf{%by&akI*6pWkrK zFQV72sU6*MO-OvEIV3&RJmlNfrjsaGxu8)SDwu;bXZ?O!k7ZW$`<_#VY9aqje6|Wc zY454!j@@d#6Bh6Epb-VKEpaoqt@e183~_LjbZrLRztk^l zp@YiKI`hK^Ij%SaO~>oJI#I+`Cu^Q7h-AK8IGYgs&rb3JFlxz=b z3^Of>UaM1f`pX|n);_lce}boZtQPAWqvH=hfjgW{LlTna0+Qa}FkCB-dLvZez?_~? zhjjcF%NRPo^8d?`>(4) z+s5;Hm(rs~6_BEbgXJK9VavWZ{@PJ=tQ{w9I0|A`c5DAYDM(;ycilp86rE7-lndYG z6Xjc(?UF33qo8;H+3ADX+{P`JdZzBkw6W3~P598mu1GR&F_Tu@rTDgBxTlJC*0Wh_ z8%Ss`a414u$f;gg)A5ySxe&!S`%gq=-Gj2yjgbsd^}wRxFbX51Y3ub7C(7ANVB|o? z_Q5rvy8sfv8ow()EztZBYD4u#6mnMp=bQok#0$J`?nT#1coOgwhwbrcpkH5Ifz>ta zA4`bCs2*awA&qY~)UPxb-7%W8Svs?PVILcqOim}j%noiFOa1`m-Ouh&#yTx^Zl6DO_x$m(SG`r9Mr1)KcHoZtt3KR zFj}Z!jmA5WH%gozh}jQsxptXB^+3@beL>fSEtdCNzy&HJxm1JghHe-X%=j@0A?%@;g zn>h`h9n3PjP)>$W zG*p=ax@S^Q>xGj-vb#cmVr9`j!E*n(+w)2{#}tsG?HO_gzgX*w^DsY5o5ut=k3gI2 zH)!r&z}!e=-+#iw?g{_J1`da}H_`Vl?s?Ak-%H~XyZ+C+9TfGV@9$iI|BIUae^XQU zY+4Ea4Ya;@6#bU4+&)~x9h0cK#e-6gU6JY9`RL=jC2>!V`;TJ;Fy|5dbG)OVaQ<^M4s&_`lwG~o z_79U7e2XL<_|im2Kcz{s&|)ngGc~REkFpm&dsFV{Cb^1{%4uAN!6x3(BWKw1Jo%f_ zVZGa6;RH``6O2MDW;j!Gq{`VtV|eBi8KscS(&wx;T9mCW28I(8?hUct zcqIu{xRYg3(F!KVY@7BfjUX?0likejqg2|Lub+u6jfF|ha)IcPbUIyJdX=8z*RaYD z$2J8SFon1)=&hLiwb;ru$|}|B^?N8j1gJ!3v~MJe2b`R*WZQoWDVnDtg4VzN##=Gk zb#3^)skDUx^Xo^_Y2Lf{P~>fY5NIG-wLeGa7W}g1*wCjaqlV!08i6^x<&{;8kEZa90jc_Yrp!PkOUgeE$z(Ma5^!Jrl&p2OmTxsFI5Whpy+DsYeDz(9uZSq z2P6nxp>D()&O9iuG&HxH*$@U>!S!!-uZ79;dZc4dFEdQ`Ixo9b#-+30kb31F#ONje z4S&?fnTm~T;r(L@(z)6x7zH@cX2juE$HL*{A90=C$DdzO=hg`$T*@@Q9;f5Qd>GIsv{5khgeK!=+ZMBf{GFU?s-uYEq>@DBk z;*ALjNyn`x`PWHFSs!^yV;1S1#;y)5mwOd8bo$`0IUOqpcs#lbwLg8&AxHWra(G`$ zcO989x|`YetRXP|eKfQ!9C}71E`NIOEEPDhYm=gvJG+dyzX?Tmh1*tCIl|&lqJ$J5 z9_SX7xutV1v32^BYo>9JWUNQpKYffzjyaIZMcY??a0?skYz-24SbSg677hKst57aZ@O z54YN=57J%eF7&p7GbqwP`EZU25KwHo^^~R-x;q#pe-h&yq?QcU0sZLmEqvl_mXjop+ zve8LhFVs~AD-nG7#;gjNuHXJnb@`Ro@j7Pl+RyI+>N-h(_+h%xCACXv+BXKu9^PGO z8pDR2{qv+vb9~g-q|Do$5ir?Y?x%Of$S35KUeZp9+Zzox`$leNp8Hm|1?rzs=?<^j zuU|G0;Cfo^a{A(#0BSOE1&oXO!pG!Mfaocxg031O&j$>b-Rx7Y=YCd^sbS7~g)|Ru zzg->ssLjkPez4a(_Eu5qb!Dgm*M~ROglt<>_v9X2bSHJV&jy%}t7N97182ulxw{}h zXL=UnEPU-Ym`}~<8n{i#LRaR_<&@D6r2ZJw9_n? z-~aZ~tKGQdJNTN%dp699P%B5U?_V3cH??9fG--FXe>izGTfR1rd*>|7NyXH48TR^d z!hYJ>t%tx#0k*-3vAS%D_h#GV#O3i+Rqr1h023yyv8QaU!etYB)Y_&nt~=ir=Oo^~ zer?1vS?sp2e0-xUsWo3CVu_XOw%Y!Q=sr+jY92XF|IxyAh4j^zm#ku>js2u$2HRdP z2Mj76q1wB0(%GuSE~6|n4>fQfepQ;k{GDK-x?%qEROr_OUzu|SK(?6WPVWQXHwM>l z>Gn;X7CC6E%%`=87B!bTtn6>GThkq5-Yghq3gR}{aWZw>oOcu*?N3`(<=%EKZ)d+( zP$D+JQ>`)ol#NhLUu#7E5WQ|90Q%*FrGxNr-+!qfLXN&e`qsPYh0m3JWlJ5)Q(4OI zl*!^mGfuAV#kl0gF*_}!A3iEzF8mn8{A`pZ<-F)$qGhK>G89((z+B4?vo&kAGe(1P z4N*^Hid9sy=TX$FxpXGs`kC}pT_NL+F1W%gb?&{u+C2W<)>l(S9(5yoatc|=bu)ny zHWxR`)Fs+9|N(2 zd$lK@M(*@kt5IC$zVTQh%u_{NiY=8nHbGBq(=V-kY2}z5^G~3@T7!LRt%ZZqb3SXv zDe@wRLc4ngQci8S6Y(Fe$Xa+y@gt>l+ffIVoD6dY?UmP;vgMTRL&hx0eMb__s+Xp3 zNl|oP=YE_ogtX1%0jrM^3M;-LKRbEx=G^&s02J-L=Y++CIPMN-)@5A#3v!oTisi*g z0ni0AsN@NPJaEP~*Ywz8RG>vup5;S!|D>ll4C2$xP6GhU_b&PF`d((ePvrc z5*URJ|L0_`iu%>MBUcQY8lmnLA)jY-E*=ZZbw_iVXTUnNfJj1X{|Gbamjg{V;pg*g z9bF#TbatS18MRHt&jmEf#N=FMVal%1Ac#Fp7C4f^Cj>{de;L2KRY$sP)%7;}ZrDbP zg85*C4-Mu`=DO8~?jNuTr1c-tW7owtszr!S)`J9m3^jn`N!q@~VPSGtk$F4eB8 zvM|@AfX|APEV>XJMWy)nPWfzL9f{62Vxx!9>VbphiLS`B++AkwtCop+-iVtMwLr>D zVfC6Q1S4AWBv9oR&ifZh`cMJ!Ro1YRaV<&tjiaB_ZXs=K81t}QAixqLEW*tgVoRHgEUpX4}BB6Ul%C?wcDew@`;k&Ecwwr4yZY1k% zP3<$Hj}2jb1!fFFGQJy4nf7%%#XLj)Sg;X&XNf+-1?Q1KNDh~qF}1zc$9Lu8tkPC zo7arL`}&^{6T1g8gSJiIn~&~A^z&1RcuEtp4F$EK8D1$gxjmVrl`gTRF&g9_zJA4$ zSoB(r-AC1Q?8e*PkW0U_f7rZv;~VD_RlP6HqCbD;P0*T$q$65|TXh^$Ln=rYSTZ)< z9i3Qfb3H4gVlIPyzZ~r`FgEQ`$M=KbAw-HCqB9bzGe*AxO7!}12yK0voU{Rfssr7m zx1+GmFifNzUT5p_A=1$Jfm8PRd@_>2MA@Sd2RXD|rk{0zd^)4FiSz3E z6b7F$T4#~_Zy{qK#k=x_<)pdA`QAFlyz%Rf?{>UShxJ$(5Go(BR)0mIK4DoI?bKNB z6|Vby9B_ZX+nzM(zaKFZh-I?Ph3NK8Kj8#Vk4pHAY!{oR?t7#!V@O%YQOXbV=FLwA z-Y#YL3d(f0|2o+i^Zfy?OCHmsr$_785{a^y*i(Nf*Blk)NYTHeIAH4&9(jBync3R@ zJmjq`My>lko=>`aB0732?cM73d3OHdNyK9Cl!VuOo?GVy=Z+BW; zX05nAKzjM@STt10Zk0Mrtw$t9yTD~UlLS5eL;`@gw@B{go;SI?<&`G`nI8K@(DvDh zxjb~~t!XxHl)l=wmRWFf`{~LyeeH=nvM%Yi_o8%CBgj&v-3$Rvx7`qz_>|8bj9bc1 zo|oxkKyDhTkct_k=nCh;Eal98^$*PN_#8qPbXQMqR_#BM9^DVluGpCL7%lRGG))PaZGXkR|29#v!{HrN3E6?h$r6B+4$rtZEt9w>X&b!guA)4nahTnJPC!KxHts>L% zC+raZAKH@UtXDT#JoY0%Hmfzhl2bFBHk)kk7AkTzVV%27xywd;_!LDp3e=DlFZ3-4 zbta**E1T>7q1?80MSK71Nk5Y<r&In%DPR(aMumm2~5y!KAMCHB7Dz=W7l9tc6;vfYrap~@(B_@msd%GlhlwV}O! z(qf$PaUOSUls04K?ZJvza|^KyyN@nIk?fw=9+BLBwWzFy?elj(_d3O%4vuncnY8vq zobQsGoNrwDKz3k{e%E9yI5)pMRlwqLIF-b>F=s3Ie6?n~wR0t$*IIdwxz&|^yw3dB zd(as-jk$m7*n6<_EVIE_wcT^R5GaCWwC=?pRLdaRhsdPnw884yo$EWi7lnxyuONtI zC>Q1Sx>>Uw?B4;~gnaj|?)I+l@fOVLzF~PK+o5xoKpPT+Nh7vq<+{O+@c1q`ZJX== zR=uhdHkqpx0r0M5B5d>Lj=R%242E6Z#EeIS*QZAd7Fg*qO#&^W!+uYVb`a1I@6jG)Aq)7+>TPhp-@dUjN%RDBa+KZm+43eNME@-vL$ct+Kq*tWU6jP zryu}R`ZA2?j|gl8Ml_5vSco>#w_a-d60kl_5JAJ8>y#NrS+?eL(>^$$jbwzmk3xo! ze~Uk{K9`=UvBp$Jajw9I*yVD5($Euw*-|A?dV(Q9@yBQbw*`7~KQ4lQC|t00OVXJX zc6_O~RvEhY1*xR(trD(K(E4pW&k!(OyI^P%JT2CSPRJIhWebH;Outfxs3S#VrVbS~ z-g+gOTUA$phd1l?QK{|up)LWzTm%1>gkh^4mbIrr^xLume)_KN2vfwf81eK+&!Jx? z?yC>J-09MZ_P6Zv;`w47^ku8miE(pV*Wg}}!(jg1u*LbDNb<-lVx2_Jx;fM47S;Y4 z3B&a1X3@#Zofgd)(;pWf=<^WpenJ@h=p}{Z^*vMSCnJ%Zax42+1BVBT72E`WedBFY zWqI$vgD7|CViBNqQY$a`{wFA9FrbFb9TmU!eQKQeWzDJAsj5i> znDI4-gt<=4Vk;eoku5haI(>kSL2V?y;>r1`bT_bB>C&D~69lDB_wSJ0h*$5Y{Wy%v zkEjrNxg)P&1ZY`#hb8EI&GL5U|3A%e{O@H1sG{qv6pGIddSf_HT0;M0tn2UaKpbY{S@nXR#yom={=h?L z{rH}`bmV_d2K4uZ?~GK~NK7W3odOOK`k%`IV`CS-6hQLriaE$ZBxKi*0UHOSiN3f8 zL$ve49e@hv&i`)ImRM*jif(mNn&WKdK(*UkxVxOq>n*^+uLF<&b~{v&I_hgo%b7Hx z>VjGvM)JZTQAPfeG31+f(^Qu*s3UUqI5X$Jzr4*`+}&T0bS7U|5^i0_zm)$H9DZ`V z8cB^uiMNmjsgFcnpeKi1g?Jcwr_?E%rJF9IoF`N5FmHd8{hP--Xw!zXeVf1W0nmcK zZXZyhTy<8Dm5&~0r%;k~9pt(Vi7$k|MmX5(=BSfG^prHhuCZC*lNH{)qGdNqjJJ73 zdxf?3Q;6ej@c!ECv(R$YIN0h1&r=U5@kZ^7GvPC?wQ*$@o8tx|&7%PnM5hQ12)$z3qrQjM zAe56r$~9Z(tt3q>TwK26&~IwSSb2G`1kCdLXVf7#qG>)SaG;JY|CB&Ryj}=31}}8m z)eG2u0iFJ!xGeMq=2+hd?ZVCm2Kk(jO&n}_*adZc_0}*L6Wx5_&lVG*0J4q(bmhF( z9#sNYhsgA{l--FK`xu&g-J&(!(411L*)?fwA;v3}Aq`a5z8pDSQvLx@s%4ZYB+KD$ zm?{)H@HvSiUHQV4ns+WoX5q7AG87EPw0h`#56F*B56Cj-8+vJcx7|;6UV0emK8466 z=Bs}vH2PoxrBS=1@lx6xtJaT3l^m7i+8W>@fZ`0kr z2^ODFy@`{vw7(MQ<)=Qfe_@lb)i3elT{ygCRP^OcGnkAV@ z6s+0CTt&wuCyJEKHuTSi2D@%=?Tv)*HyxujO{81f?Ip9i*da`6*S}qo}BoL@>{pZb2Ty?LltA>Shf zLVF$0kR*vmn9ML%3ZX?9y;0P3LRV7|*?gL0(VQC_Odg8^m&|8=EKj9haoyCW)p0_1 z-$H)uvdV0-H$Jg3sHh4gzg>a}{7%tbsU}W;oA@;*4kmIjDFjB-6`FnEQ6c}3_$}=f zWOtOn_RK=Alf=<%f9lNKJl7ywbBXHQBlvDqXyc)MzEyxxJMXj-elIY|{}8+16yhJb zpPK7)5EL7~8Y>ARh#4ay{PA}#fG$v)h9ew0(Tv&+RgHrM=5s4aEJ0u0puX|splJ&` zX!N9a`d9mK{AbNNJ$K1ePe0L>_`w#SM+0Wb(sQyVHe+o3r%N)Sc)*& zN{7~$Tte|ei}uw004jc`rNsm`1v#YF43il0XDlv98lRp{WbO~P9;G??`a0<5fJ`iG zYE-#yFlyUvuzWZ^or3JiS`BRvuOCdgL>!Bx1)z!Sb~WQLd>1oPP~q`@_gl3EA85MH zPA^W)GP!PVi)3iitw<@8jK;_9xhp=^mXzo_>NcJ}?Qay=M}mUiPn0_Ycw3p=-#IO+ zRiI~EMW?E&`q2i+cC3w5$|rKCH+3nm`e(#1$7;ul&>uXb^QOa)yklE^A3jzNJ6OJfwSTi@$0zDx`HV{07Yj0p%w#_MvAVZ#TPs zEjFwLCS}H|icf%+p{MHxW!oDfQfm~7u!(-w8;tLx>l>?jH)VY3fJr|tCAv$d=uz1q z3PoUkXxfD*3#7%5`x8HwAVpSM9`3wZKiu+I{YjAWn$7mHk{I_V#t%lk^m8qh1DWUD z_d+lSRA#M}q}5()siTh*?-9jcah+@G?qq@XjTIXnql?Rb%+BV)c>7 z7@3%!0L7I=Ci60rhHrt}&7Hsho1eCuyQ4<#r3^6+mTmixm?7%h@qRsA<};S!#YaD@ zf)5VqRdO$-Yd6+An@-F$)^!fpcO5i#PL$R4qzf5Zv%=3mXD0AW4kJy<$L8Tr5Sfd!ohgVX79!f z!~QY$3fTeYM`T>iG;1{7t1k-+w*mu+7@5FDDq==aVYEru32ya~y1z{0#?sfGA`!hRBfdyVv6Z9Aw`QZh}DnMEjlVC@j`{bYdN zZ-?))dR&IwUo_3q{U=|&ujgH8_pL<~p_(--#cdcoKShgWjwYML>_$U0@` z`y*O*Fj9~?x&g9I%>q3Yl5m~*d3wCxbh6cWiYWjsbAgVpw=Kl1Pn5^&T1uzxqQ;7Y z0=Rbq_O%dOL4#*uQR($#K15HwMP)NL?K<47N6%z9q>8!N=WG&m9PZ8EaR>H-= zWmKoC)(1{+nX6JmY0tGLV$9+&CHp3Vzri-nOo98Jt|QQU4hEi*>7M$DVFX41@yg+M z(lxCdA#Dnj_E!RT&rMGfvt~qd~IWP&p1L<_K`E&f?@oqGh+ac3Bv|&*RGX9 zB}bn8c)B9>M^lem;XGvI(UOdH`#Ut{Eo8)=O~lpSw#Xg|l=s7vt{rTTD?+wKD{ z{+;emY9_Z${eJJ`N@k|x=!BSofnLAez^YJ;Hy?n3C7&|ZJk6!M5Ev~>nZVtRD}OV zufSfHeO~Y9dDb|4I4~KW;n0Zstq_2L1_r=<+Ij{rpx=!CIeB{ z*1!}AyLQ>y2zI^SqcAgEAy)Z!`s8K#=CB5(r_s5;Dq!ByH|^HiKilZr5OO9BI++}_ zPkC)<9-Q#?#h1A`W|oj6R+gZnOMt|3@{b=Ij&-l!jBF#$W(rl3DQ=m6um65WTmT{OOS#^$j{nP`^40S=}oSH$5Q?ESc0jS^6Ay%j{_fx(g)=gd~HiFJ?NU&?cV+j2dXOCbCr=*RT*VqNRmgNbr;J5l;e?(Xgi zH#eICvPjRZ4!RyUGYuihO3AZb1_WB(w>!NPy8UJp;z%wj8W~EK&6IsdyJdN&Yc+<$ z;rMSXDdYRLh8e0UC^w4o&?zzv5__#cJ+*A4L$rUrP*2(w4wBSg@fHaV3`{-8nV<90 z2$)#|*XHNAiJ;@RMpg)aXI$qhWMY&(u^xMNzB$|L#W)FB z-JNfcFvot69CUr+Y0R!!Yb&(ae!s~*j0RuM)#UO0dze*8XPB((WMZRnscn_I_&mo; zcu$iZwL}#&L8@bKEy3k^a&~q7OIW#R#Pkbqs#P3+| zL&`qF45mv14`$<+IE{vEh0jm1mkHdBDE@3^>OOXS>>ex$>$eX09K$u-um3&EK(I7! z-Fpn>T`}T0$@3ma+J*{#x|o~mP1Jn&@BtIba;m^exVgQ(4xDWj-~9-%fNVT~;`-4n zpz(QNg->?u|Ls!gv;1nyG6N$bDYMZ4z(wS7O4E@4r`HF=LYCu>KOu=+L|3nmz3+uXuB zj-i%!H3a6<(EZ(t`zXKFCRRd-X+gVXb_6*KdNCR_US&6^pO}X~9XvOIVhy2r!}c)R z+&(StdvZ{`NkKt@1~+QF)EM_OlpI)#0^MxMm|~eDd{!#m><57-mD6}iP$}yZ=i5x6 zk9VE?1>2?Jn~0YCc1Pb|mOFV85Y+emIroO3o>N>8qa@ddxMap}=sCz;_X&*{s*KW5 z5AdoSU9>C5F10%`Z4HXL!o8@3x5@xhS^F(rC=t20yL;Al@OB`=cROQiuDO`vpA5Oc z{NF}|+o&4E{A#~JrJPL;I(>Y&HH-90yaQ$u}xtV7hz(5Z5Zy7T{&@z_OpG1x+ zDu>(i6Z81Zc{EMyQ8=?)04!P_rv^RkgqHlii0`%z9>{*jb#KD6VWfD1D*&^UkkKOf zWKVJ4AAc|g$rxdwU+&i` z9@8)f1s4gh$RXRf%CW!tHO8Z;dJ2&~dEvg#^R(GT#te$!^#25Gm%m@GAb5Fr<>8<6 zT_#>S1*VwEWc$TXnxGTYGtx7-0veNE_ATm%n<=%F?2sr$-4bzN-olBc8As zDS_oOT%+$cy8IkhE|xinm4NO`-~jFh9Q%#e{|&R0R{9eVX*szu0D7@B&|Ml_w@ygT zHuU@?MXSSK%DJccxbm)gKyVYF2-y)3^x(0%oPX|Oo|Bsyk9gjP#ysuD1nYb_k0`SN z{a9Wf>|>ls2S%3QXY<29%i+EN3&jTAY= zBUS#g!-()bqb?+7@k!|uQ|_A&o7*AUI?8ecKP!O)seWU^&lExRZeL!DJ{X)hMLdDi zphz)4-k^9QG93M^5PqZGAWx#4MD7Up2y`|U3{KIFf0B!;@76S*P0-aPepofB1v*_I zuK~o(JYC{i&yvrBbzA2E$c!3Dmq7%d)E*oh=ugxjEaJ>`Z$2z8L#8h31@G<@h2R0z zcj_=ZeacDMReSf4ce{E|yW7+t$`P)M%pWd&rPvP!R4}eEw)@Xak(hQQSeF zN=T3>iILg<{Dt{pOQ4QUhV1Zes#V@cz_h)+yYg>G1$F{+ zYw?3D=yN>!#u?I12QyZsm(XtlK^BTkRIZQusK*uYYrbFCSVZVH*JzDX4$L0!TnEk= z;%E5h&)G<2S*(OfWu8BoiWNiXM;ofqanu!hMky)HpQey^&Tz#O zaq|;%)vZ|1ftz1-ZrVuS+~q&oAb}GiH#@b`8Q^q^TY`s?J)-n53Z* z;1N_Z*djlEemoZ3ZQ2b!U!K!AK@JCeDqe2Yr_AfA?6ElA!3Bf0lSTr*HLu5Mf1m-Z zvPK@d^9WdJ?R7RQ04+8@TPgZ5ty|#=k>0vBV^IghJj zR@drWRMfic0 zE*-cX_Jj+Ciqw118jx#qm|(^zfJIy9f&6jLecd)3pyE&UsNlVd%D1Q{;XNs%xf1MFP@a5j43y?~%Ha0)TZP%TpbyJ2q=u=D6M`MCyM^ce_)MrMz zp6S3LnM$LDQE=N}!4SmIs*W}m+Z%yfJsHHz8jGB*&5@oBr(cuU9gpz0Un1`X7+wWJ0ai_LdEdb5N{nf0-bl7vJlC+Mey7EmD<;RQ@=sxetzON^Xi-tIK``ea~iXk`VmeI}J zq7N&JjH5~6|Cr1H!j}FPc4E+M{p*APcDg)cy|=R*b2Ryu+VrtlMj@P`Jzw|TNRF|? zWR1ogu>7>;bv}<6!&pi*!~4{k(99XCfmtpBf+UuUU}YHBx?J4n{03E1vt$5Q9Ksh4 z;{l7~Tee6p8bmBfo0V3RJTxHYQ4P-@GjWg_{^ebidA7Qolb4XE;klS$Mo zG%L&W&~Z;)3gT@>P{OF{gg6?wT?BnnxT3-Rt4PJt#A5BvwH%P6asG+k@=%=4b<*5f*a4MBJkOBItibmBD2?uIL7-!~Xf z=y_85O!;2?(>aIXS}B-LQEOjNySgr4&Pu$>2{Kj%l2`f0Q{-5N69tya;)4B8{PtKc zI)0E5zm5fJ5OI8k+&4DgJI*q`$Kb59?cB5>aVE@dIP2~_o77RxiK2VTH+XpT3=DW| zYxo*;CyF#Pl*G3@mCBDkVTn@-g2Us-=|T^ClQM(gIw>KTjUcHiy7Td#&ffvPst- zpYLJyC(gH5cTLZ!iF@v!G+LwUX&AS9uOe>g`076{ija7mO>G$)AsDVfv=~d4&kk0t zu@uZ(BJt33iQg9J;&U#eB6z78mG$qw^e>BDiQ%&oZ7auXTcY#~{^Sp-wOBuq=)~2%o73Pv9PlmL1}Lq-aP7(|8&4$xwBOwfTd5!ixV1ne<(x5}b!EZ23Ct4+y)#iF^Xq z5WVUwBU6<7%$oh^8WyE1Ky%}PZX6$meIMAfPOLOzqTw~xN=H}NaGKM9n!7q(aiSZ? zoSIeo>&Ul#_3T-&smkn3CTaz55}*Gx%7g3Y{B_iX+oLp9(C_bFZ#_}{9a;#!7+AF3 zHoDeD!&B=# zd_Q?goRv6uTe-n=j~_t{dKBj;s_<{V;I2tP$J0*<2u#fWvK5Qff}T3A!TnzoFO&+I z#;Jr^lT{3gQjxK)TAlDn#r3Pz^MY|1yz8Z)ahfcPBBy~77rR!cz5bkM^N3N_7dqWk zl%nMfOkggAP^P0iQ!z0s5#C?KMO)H&A7NW7x1pm?PEbGkH;Dgow)+)ywjuoyBw;bn zVsHy02nZ=XQc^TZDKs&5B$3pKIk9O)Y516hoUG2_WH|4sE8nUCF|Edg2#*xD3R*^4CMzPpz$3!_M+` z9U2BWGq2$FcoQe9 zGuCQVi9c-ac(|1>F{Ye-Tt7TKr%zP%7M}AXAlf1@CB(})VFkKAafRR~+u!^reChNw z^)jaPUa@@GL1L(XWF-a6y`?QgKM^64xi%1#lh3n%*|1?reVYd%o+pg-pvfk}16`;J`#()iuK8oE8#+#yy4p$PHu*uW7#wDl-nmoxksRb1Hi zq;$Kl6CpxZ^wv1`+cxo6m-U2AXQ{J>0D5zf=|s3Ivle9i1w5ahQt<~mXCCKU?~~rl z@(v(^+Y!HV1pf63f>TxuM69O)=ku&w1i8kVEKA6JwL9*gtL;Bio`$>XbV?i52KXa} z)KfoNZx>EYHem#3S|D_82{TlRL-l-sHK982k4~y8;FVpg*W12LvzE-0uKJp39>~l; z0`d*vi+$N%I=vt+n>cqp7wR?LX4M%iFBm;}EE|^g*|1{hy?U;3RfR1$7jfV7vmhaM zo47q<0=Kf>8;>{jq!L=|aL#EY3Y1O*i_wjdcpPyktd(=O*i_1wY}+O0Nw^?TMXi?z zOrHLA!aLIzXU8eYr6Gr-HdI^+nla;S&SO)bI-9I~WV69C?B7v*?`dlS5@Sasof{$HD(rRKC=FHV9laj>iTk zZ?2Q1SxC$A5E^(@NZ8_D|F<8r zSuzv7ep6aVUcyg}eDx|e*~Unff=vybXq_5+pdtYq2$pNWms;*(D-Dwxi|>a5_&+<9 ziME#?b~Y(~W8nq%$=_Pp5S}a@IO}h?L{NYF*SO01#w~)$noMmS?gT{OPh3fuKgt%Z z!`KKnB@(ES6&aa)K<;DSDmB&(PiS9gvTF;NMi==WODnU44CxytxUvjD4&BR8_4-c; zu}tEP5Lx3JHg)qaXhVGbL$SPHOrRO_y62ek=;owaFF&fgThVPw zySPlxb9>!;wBP_(2jnHz!kx_B;FMqjp@RrAp$_o+5(gbA@15Fdm(Li8S(ms27!+~X zTeTlHr&9X?_D`@K>OM^K$9ecWeCnH#`*S*QqTWu|yBP(O_w;V%u7H8Sq9Rei&J^XC z!0OD2_T^mn?c9i1E)z=b0OuK>=*`L}JIK4@9B`-f3mZBks&E;~yT3z`+X|i_XKH!2dH*Dktl1W$KIZktwxGnsnZqovn;`p+7|E>C6`79Q+ zeIJKTnYOMdvL5a&%Cz3baDwoXG}NOy!;*^0R&3|a!yARB1MVz5h_t>$;5YcUfC(;b z!<{N983up8JHJ(A&QkVMJ>xZn9Edq7+!aj!U`l&Xhx)mXdZa<|I8s{nuqnx>f7U=h zkIc?$b`prZK{?0uPj2N_Q;ked#)K54;OGGqFjQM^z@D^opn)yh=+LbpW$CBeL z2WC0Rz}>I%D@zS2+~_vsf7*SSU{3SmJ9ilr-6Xh^VR!X1;7JWZ&$gzB_N5A=g?;@s zcoavgNf1J1*snn);`eH}QIN(s-K}rsk%~a_y2U&IAY1-q;Zq|@4Ojn%F2p24X`f)x z>u+(`xJhiqrxGH9pTWu(S)y2NbJ$pOrh45BZ>G|Y$*Xvp?rX;l%a#1t8p&>$eL5#3 zJMiRAH%!8%7@)ug8t;UewCQK%lGodEmW2&#$?QaCG5p4vUiddal8e#8Xr=({j;twEE(u@;;o$2T#IVP2w zl_fe_Bbq@IL=j$010D1d2IsH8P}Q^?oZ%c8NClOjlrOr*8Vh^-BP1KU?GDlwmB z-k0_3PF9Z@u)xw@9WFHGW#8_k>Pt|3tf6lE5b}{MRRdZrp|?B|x&#kJ&6QpO8sVr< zC0u0wypdrvq9qkS6Bf)e6!E!P&BwQLZo^scY)5PXphtTb(Vk#+sR6@EJZRC}=bMkF zGI214E_z}UBss6D=T;8&tLNS|MCsnrsyi&0N{!P0{u!X7mx4BuIx|X}D!gu3Z_aT1 zMiGI(;%4N}TRit2BfsP?s7J!Nf47}+YPG#m_c7TedEm2YQ|0QLX#K8@?MJC}Dl2ThteK8;-NxU)R5#Jtl=%LP8$i7q(d%c-d&w15ytm_d zq&?&0-4r{YJKj>OO0GSunR4`k{A|`u+xTAQ%D$1l{Dq-G8(N?+%-Jk`O`H8mXTpB1juijcC2k~+^@QwZ^|O%&AfBJ@+$rnxVL|6wmh&0 z)CVb*81`IMTXWvj%rb@luT!LEMZC-X_Aduj!`JOBjO2UuyZ8;;S4YJg*Gy_dZp>PC zUuT-ER?#{C%fcs^8P*9wW3HfYw#xOYMRx7)6~pgpoNP-Eij&(tbC&*;D_q>mvmD#!-3z>o*2WP$(q-1%$Tydf}6|H>+h63 zE9bA1mz!DSF<;035&!+yhrjwh|GQiLW%;YT7fXRX>qKbmK9G66`RnG%uV#CGQ*@br zCPq>0@0IG=&A;rH+z+|G;`|5adHwveTQ@24Db~I@e>41UM*(m`!xmcLH{>s0zfsbu zjZw_C#h_w()U#*H^6-8ifdj(4~|I^ac zri(aDsNQdLE%;yfzDk{^yXO4+9`kq09<$)DuQ$}~b-30hzH`QGVB4P`7OH>Z+BV!1 zQBn1DoaB8_^?dJ_${BX{a&_su&C-kKyj%hvq)>)=Dq4Ek-P+~n+$=MupFZ>Zl=Nhi z8r_p+=QrL3PT1T_du@2toT1@$12ou+9LMI?mlZ`Sb0`vhsNLkI&WbwNJ2Re|vcQ{W7_^Gnbu@pC1{p`)VGrmJEfK zzYPC;w>kfs%O$gUkHs~gdD5@S>z?bXTYcWsxlEejKq<6E$Z*c`wqyObj}Kzf|3+sO z?7JAf`^EWve3OB>V|72wfxrb;a=;^&TVed&d$-M!eh6#y15aRn4GSqC8`j>Uo!fCI8)796^(|4RzU0)rjfASX71IPiuakd0Hj0yMOM zYV-u46$Fqi-T`$yg|4H8@vXb#znB8u2nGeY(DoXTM{?Ma>bhV5*Lt5**b_csGRSUE LS3j3^P6L*gZ-JD%~A|QUa3FB_YzGNP{C1BOyaeBMkz|(5WCH-5^MpbPUos!T^Fp zNH?5~zP|7G{m!}0ALs8g*X1w_^E}Vq_p|qXuY0XEhHGmo6XMa~VPIeosytBC#lXN> z1OG6t;evPWN|0RxKdv~-t30{}9^TiUg@NB`U6c%59=@=0afdirVpu!8u(#xOws5kv zba1wL;j)R{ECW7t=kh}ePL>cC=nID%kD&IJ7>{gRZ}5xVxaVqpLy%uk7`&Dcx+}pi zazk6=hJw<=M|3U`^%xj8FjN%pJ@QCfoBIK$*RTI^w5ft0BmYuCf0xzLh<9&&W3{oN ze7S2-agDiwZ+}jGt*gIrPS?FJmO^DDt~Ou3_2%0L&ihL2+ME@tJOx}4ywc1#k9cmp zeJO~kq}{x;=sb3DrlDXPMIwKVCFtKL-}~cc^tZ~X|M#0_3F}sDW&g*URtXRz*#nyY zjs>s6%%xF3nEd}e-S0o%`ah2RcAabz|9@OFh-4Gv`daY+JSfcF|H}UV8bgPq9G2Jr zI%t9MMnL2LJm}gM(VG^t|2dl=kE;)!@cpk5e#ZX44F~i8AI{$p{l7-1!#I01^iRO~ zDt$LFe@8m3R+-SbEt8vX%tCf|NI1kjnC^te>{QAZXJXX^sU+=oEW6{XjY3JGb|m&p zcKJ@6DSqGA7YYB|;AS#at~e}9M^+x+hvEyL_VezccYkp9cq4AUto;btE{rAdIHVc! z-upPQnz(T2m2akFC-WKi1xkX`)#-*2x_lz~;$%K}2(3s{s@b!Bmn0_Uf_e{vcezx; zMAiSSnJ{w+9Es@gwlGcxk}FIwhOAMWt#zDzy7scNHG+htCrMNxkgRIYz-*819BLJG z5+}j6KupLmM|9h6_2NST0{p(7C%F?UQ5+o?i*c{SqOYCF=&X>3U>bF&CCsMgMlC&q zZ$?7W)O(Sm2IMJX5n);WPlh=NykFg*^iLB#EffW}mH^ zf3fEJ9r$lj|15vsp3&a#oOoOH_7YKaQPX#PN(1E^AL&VL6y0H#9XmsfoqbsSFit0$ zX#LbFqpd^u)6?sSs?iBEn%(f5XS3%cTemiuUtp5sU^Z$Bok{KgYSi2|djxJ}BnU*8$dWb2iV7BGKzO}Wyf%n-t9`DE&jgNSm8*uD3H z8s&m&9|hHF47y;Z1PH}$j+mRjr`RF(cZ#i#rhRWk;JT#VEifLM^4|A<+;~ideX?ju zr}0s+eYE;OM|IvxNX!A3v2mQM(Xu^*_u_$pEY%?~HhMVERQ8GAj`$2WF&H;OPWJj* z-@_e~`Xka1$U+@aFYM5I2pz8~|C19BIzc?;?U@<4t_LC+*d&@#Mnk;DVMTQ>&~nBg zIyk2&^Hk#X1qUgvOw&XzLMjVZrqx)l&{(gOUE|JLHUWvl-6zt#LkJGx)Eao0Z2I)6 zRY3Tv`1TT>1A7cq!awLwc3QCfCwH7aWh1PP$AnDg=^?GH_gG?@PcOtKjTpy$GLLIW zeWHI13B0Zf6cvi>LpqEYGu;`k9M0Xj zYFsFFJaf@^d|)16U(f_M_!I=Ck}4gu4>E+4GC2u`b%MqvlAj9@5Fk3L7x2L19y-QPf zNAy!rDLp&PmbY)jux_9EKT6(5Nv!wGa=|Kem)5iB^+ZhD+fWmc=ipww?-z@Ewn zG2GDdmHC~V7BVCMW+z7@p#(&3q+7;g@lx}{qk``l`k&7i&hPUTLVUgM`+9HpKxQ%^ zw&~29uE>)D+9-W{_v7{J*&%$r519GZ_}0^9JT(}be^jB)w2ia3>@T*^2dMnrD^H78 zg<}l7b}6}$KKD>ht~MiYMrTonO8i`k5gZNmv~?CSQmEClULQ#;k~*ZB? zh!%c(9*#hG54?vm+Su<@j=wZJiJyj~TW7i+B{rZR(#EgO(|3=yEBqk6UbYw}^b&j; zI?ZYM%kw<>p~k@0vmRK?e&xdz;VRj^J-EGv*$aXfiMl-pxA1U18t+ukX^3Qa_wDy$ zOAHw48v5<0A~M@$iM`OjmbKXCUQiIM97r(O6<2J}Az!*}3e{xFzH@z~5tCeW@{}dDj=;Y*GYLP4Sw^DEgz@*-=E=}Z|%sYPpq@W z$rSE!I^(849*HrnLdBq8H-h5ReS6=$!RfqvU&M3n3|kdKGfeEsuwGKxxcz4R2eH_s zS?fW4={4lt{Bml_73ZA2Qa@ijeu7`erXcwldhN0eZTbw3S6=m4U$)U#T?Q- z`-O)mE2%!LoOB4`WB_SaxS&Cp$Q{bCMz%N25sk3_YUp=LnG4lTZhXKldk^}YP81$$ zpl0(2===k34wVpe#gw^AZ}>C>D_!E38ZrgN-7(ww&8IYTKo+%LB8_TibOB+pz#wSv zfK|rjEgRN3r9QP}|CsyuXWXz(w(B-XeL~<^Zzx&2Q`nGQq!2-6w}OLMbOS;*GsUqf znQUz&M_jp^)@QbM`R*$7`Q8}oDYgE^*X%s=Pemn6$b~#kJ=t%yswYfJFmir+*P&U7e4= zvaTN_GId_FT;aQsa22c*heDOc`o7k2yawACv1w%1gagT;4+a)8TW8LyRIGL5r-kZ6 z=Vy2?XHCwp8Yy1?jRnlf|E)k(xU5n3mihfWZ+obcKRL#?9E{4HRt<~1)a(|JEQ|@u z3Tv8R!3)&ECr4EDEcf-BHuWf99Q&1*dDA1Ry0A{*Ck!&y4Tg9*cvJG%XL{e=Jd?LP z4X1n#S3QT2NGE(Hs?;d{+QW3_NmEr={_8;bkqL1jvfDvkn{Um^Bc&aMMv?n`{U)wu zP-6Z7*ur5kB4V{BWv$p}==_-ccy2Jf>>EE}??dO=^F=?% z(O@2rS);~FPa+P~Qv-I1@hgFtXC>XO`?rUfPvfgY%`3LfM1{{9M+{J6Ae3d5$6?!J zJcvkYPQBcb)mz=ECk%gTxa`fpA)K$mZLj*WsQS!FiQS57*eIqtR@{>GVR1N$ruc4f z_(hA8NgO1)Mt}f&2F8_;m)z_f*;nfu2{oh%5h%!&9Vo-g)DaW+hJ1E2Bkd;Dp&(Pn zlpq?01&w!yR|=}fK_E$P+YdIWv^iqWCf;dw9m9%7U~&{}F&{$x6m``y@6g!N(1Zj- zB~(_5G{2QQHqGw6-WRusfe*bV^iXF|?wH3Y&L$^kZ};Q2d`c%00!?okUZ+nC+HY+^ z<=6LZOh0sc%nYlm>}o;hVAds9L2pxx7$IyK^|LaIa%I2$C>Ql|_a7I4d`H;d&0n|I zE_EP3{I=qXaQm_x+0SDJBFbTMu|llQYjR7UN2rNHfkA;`b&-e+A>S=9&@Sz{j;llR zdi>R|#yiSh^ke%Xzr50`zK$6i>FNK*%%2f|H)%Z0<7b)fpLL z9OsX(9-76Z$A?x$@2}SU?C`xJM?io#pm~hZ zmTzu?bx;HIq@*nE5^YYcH{5YOi@gS`taU#-QH|aa?`}jZ$Tw>t(F-}~Cih_az9QmihlK{pG`hjN z=9#Wa7P_RmW^sb0ViN_SGl&L7yM4Bke$ru?GSwvhOb&}cV{@I+=!$zV|N!u#%2X5O`4Q;pFJv**oXSQsT<6^roSWFRxKm*1X zBPes1`GlMy$>GU36`g`QS0sEtX}pz(NYeH`+)*DieJ6ZZt42E)iGICtBP{#Pz2;OY z(O=?Z00l>Y z*S6@G&P+WZG=47x6C0TatJ89|4PLh<&UcW>Gn;#lkb3&v_4;U;@1*JjTHFxmd>yUe z(bM`QhYFI_PI_jYWaG90D*+99#%7n5iC<36X}5|dXz-5bsEmi0%gzt!A2Xk)Xa5wD z&>UwThP_T5(8-#osaXh!b zA}kop8mW)8IPc|U&!vVwaxY^zFzFtC;nClIwkn33Q#QwwEdO%XXWZyvTAHQPJlRsR z(we%}X%xr7-N(=uoWbjL&+siPf}JY-*C#_@aaBGMZEwWzMiSUGV87F8&_57!!IIBS zUG`+JoKoL>FLz|WU1mRp|JPQ%#`6YxJE>3I03KZ8*Xa$3bSqxNHIZwv{I0KcadV*% zaUxB2W$5z+PuEnpGjI!sPUDRqSy6piR#O>*QyXUxbEC5{N@tex>j#z*SM8)5hxRM4 zGT4=lN)(K4)*vF#D`K}aPZqf?#9-fbIkiAx{^05ohTNcB(Ti!(I0eO z^&57COmWhE-}7QvD=9yoqn2to5C^a{$8d&TGP<>6SL({c5{-onp7k%Q!<808w z?u%qp?C~UfgI;X7yo+ChXXd@s-63$_${d$KP9CupjeWkq<561QHK_O9qk9L*Y0Y8l%bMvJ65{tq8}B`l?gl zg017Q69z)qKIfXSc^)moZ#z@hZH;8sosTdl zCna|~F;H+!@E?{-f7klVXF4PY_=(q0CdP{6Uuxq_J}P;JY6a&blQjxWQk}aCf{8Eu z+WJQYqkiu9HCo0>2OdMcP&R}ebP9&gw%L|TXWIs-Ru7CUGHbhfo^h$GKjhReOPAW3 zbo8^qx&)$$h0RztyiAGQ#tqIkHl@y% zhl*s+Bf45QY!q@R64D{T=$FIJ_Y&YWE^(&Tz zjKF(3itZ#8Br~_nvw^(bhIO6%&1JlPle*r~YRP`{`upXA3VE7UrLN1ATNt{^Sx&_s&}@TS_mS1$nsiBM7k z+Zp=z`n~d(%Ji5a_|>3d(0qPq+jOz*SGKiCUtfG|1BV!7aO)+dG$_7&9&PgeHN@!f zj48c~rMl5|dJlBg$`tHN=zz@;u}Mq!b+2(|-*B{*V`b)>o+V{SqizHz!HO3nk-Lb; z@w&MIh=s&nyKvD&_IJrtJ^^v}fbe8CfR~}am=U(30Pg6PIv|SH=MN+>6kGir3I_qs z9@@B|L^zEyEypi@Hv1tFHAnW2f|J~P2xa3Jy4QVhxKVl$Rjq730v)@La~J|$s58u^ zy5oFsaB)>s<`^44vGgr>f_6x7w6L|C^U+lk819pV6{0Qz*>l+ic!CAR%n0T?h|Dbl7X4h-IQc1Zb(3-fd|{1&E?G)BLYjPT2CpE$ zGNoAkDf|!TyyfthB>t>t zWP9951Zb1BBB^J7JFEE>W;s61m7QeqQ*6133CYbafxT;FpW3d3KtX?PRPD1bN_|Z|2z4JFMwDkpMbWqtcU6+o}nZ~E!p%jb8bJ;_Lv=lwOa@7ue>&-U`h+$OGjzo1Bx zNapG6B@l|#H>h_DUCpsneYnGceKJP~a%B108m97z@u3;Vs=Mq80?JfTdmgksCY9)L z7Se0?rb=31%$9nR8%KiZ`mqn))rKPJ^APKU^WJ5+*O=RQcos&md@}BoeDZ2%pUKP3 zT9b?K=5-z@?1*?So*B02-9A(6Z~h-BH5K`>3xo;sO%W+cwq~y}XzXh)N!7Ee3w_RwLe1~G~IWrCU zIpJG+_eVWH2)YL}_#KKSY@Q}e;r9XTB*06bLs3q&HO>3{1Wfca;>E?;1!Q=W?C?xy zhO{~J(Jw9g(ox&m@HR113}O2@G7r!gi-QhurhxpA3Fb0uA*EmBxYVdsw|~A_cYMwP zAbN+BAWiOdIWd~y&AmxeANdmg*6DV>DWtb5#kfugE5X$&`m5k9t5xX3rqI*@2bKaoHxqaJ zVN${ldB~oM(o6nppciA>8Bm@nzNm2u9k_Q8ePdyF(I``J!`arr*3N zIF&m?0P;r+@s0YcXS|4yZq;UwPR$k;?>mj3q2Lzk&z(v`5xlkbR)0;@8$ibS)+&%(nXF0QjaTt%bvJgE z7@!OE-Bk30VTh($dv4*WO>8>NcQM|sjO^HgNzxX>cN~S3j?F}&O7OlBD8o}Dk?2%5 zIG=7xT4%-Fg!yp%2WDwGnk$EmOaPey7Kj?~cZ6Sye<%jTL|eq^)=f!~3#pmZZL#Rf z9#Y@=b{e+5%cIGD01w`~Nq1_N_;Va@W{A1G`RMiXJQL(Q2RVPPQn}zjFg>&5>hg=E zv<5lQ&hU3f_9EX_9d{GlOxGHg0MK?=Y|soX=OL^yi4PTS}2@MW$PJ*Cn}dwDF?q z`AN{~q52Hi#DFMzyCm6++Ac=E;npO`GaJrFoJ{0(f3@DvdZ|q2;OhP30YjHCaUm%E zd{QUNLu(~cS*C?#ulGuZsCrH6&Smt!og}Sxsx7Ko614lyd$Ik-yS@YeuGB;QvcnRc&?ZziULNcZk2n^O)z z=7w0vZkV6}z$I>uLgIp-$a0}<-7obxQ{DVWVCtZ+6pmhTmt1=xWqCHDzg8^UimnMm zT?=mOS>}42YfyhKGa++GT{3%~)Z5SMz@Ev&Unt(laU#oU>J`sV^WAkCfa=PlMPoO8 z<`LTo_`W<|ntF84ks4@qIv3(o)dx&l+=P1S1}U{;UVPAex$(o<+w?AIh+ozXEyGWw znlHrBNI#$1cJlyB@$3c>a3Nmm7ru(n=TtMWk+(s6fBLbczxgyuLhB^)!>h)y)IKuw zHO5dI#`lk=W9BN#(;055#j;=6q;D95X7JQ3wQHWG_?X2kPlW=vXOQxZ12Z640ogLo zNvUU}j_4RzhE?Ja2%T}4Go25+7`u)$`(5wofORZv`|DaptCzfnOeN(8zi+8fS4VWZ z-Cm3kU*MF7u)S}B0Is$Dfv?u|s!g2C&3pRoO74^}h6UG@)mNV-ABkq_%y~vn*pmkL zuvUgZbPM_&_X3g5XW6n9-Ukn@5)xp7FGq=ZR)4(RIDak4>}!YNK*PV{p6?)Sfu52E zL41frJ{vx`KeLEO4)z!efVAqCRz#wyn6F(3+KuT3S00u|cb`6#*c9m6d_bdVU?U8p z7%5oa*-e$*Sv&s$lC?XL`(tJNjVw5onh}b5(#Os3Cq;6kk%H(Ly_WG?qOVaHh z{y405ke?uKDJu>im-A^=ba=Ku|1X+Dra7y~CnO}eED#!!P!=5;G9?z!Kh(N*4F0@H zf*Bnj5sN6GDZZlTFxShilL5_IlgafK?1eOZ}_V?e^xBvKx7}Qla z*)&T#rLAWXR$(~%I&xj2ryW+#NG*hZ7ngix7g~Da;o;u$NuNp&P#^7cca8QGSXiF^ z^#a@uo#7rUoB`!3D1eyGuA9x6Q^05>4tjaP#wqewR zGHs07iiW>xb*!~kePtJHX$>UHC(qBxI(rNSk66zfQu#HHqe1~7-3fF24)AH^_V?07 zHOlQA6{j37JaYIY^P;K#HZv*9AURh`9LSLYsRK__?t3a@v?mAZo74E5hME-)9-~SH z)X6V-pZY6-D64$mRdZm6>Kq_Nw`l8~5#E&rnVR0ClPx z89}o(FpfCcogN@Zr5e@s2iv-TazWo*zBAX-L-F8;g4o-^n9%X(YZ9;1`+gl{vLgJ_ zaRSVXie513z1$wieCfOC{Fv!{|HLDH245udnE>mC=2xm4Z zdYjd>01DAW-$r%2`B{+^Sn%_NdVgvgYg0-n`PPaS7N0cBnJHr9by)}t$b59{o^n^c z53I;9ixJNv$rsgBo|toL#ey&b7}2rI3VN%!JgYB>96gJw25z`Hui^K0iB02$rS?yx zUj^C^r{$kA>-M%(Fr=v`*u7U|*O%i$FTRY?c>VcTM~ukY!RCP%!v^4#_mk$TH$Sf1 z3$7fDemiGB(hbfjbgu*9vq~({E>h1zxp3Zw=2>v^k5b9;iB2;&7_5ps9}~RxfEv>` zHAG0AIX z56dH9ZxRh;E8KQwtT@8TU6Sh$S3MC`%CW_M_kNKA@2n=f}#FUI+_s7Jf7adxHfIv5XjRcyqoFpFddSM zs`qCeLl^VU?&%sbBMR8!_l7on27BH8P>xZ zCm$j?FI+7C7=JE4|69KY%A%(aJcWr40G{sO@$!QJLU_$y3a~*OzofJHuh8(y*(N>S zE>mZaOuVCt&Z=hr1@HAjqM7)lyh7#|s_}v$(-ggMlRCGnN~5?OseA~jS&(DM4}IDK zv?zkJgKY!L{GRH3WdSKmNc_1MJzzmyQk1Wp`v)({z;~zt!)2lW{cHS2cE*7P<%(yK zxDO4{N=*Dk@2YZ&pmQtjBX4e1l}>8uAu{Y2UEW1{%+Fi#l!~D zQK>y}K=V>_@Y{!)HK_3aifxf$ghTwHwMp#T3*TrUoDt zy7Agw%F-Efp%;tz2=z~?b-?5igB9g3IC zNm?;bG590X4c_|4yyU@szqMf8h3;+!^5pf8gNt5}q4L*DMmF89;rc9a z(lvHhtyL|lkpcwRv)r=?KovPib>Z1>tXxC08#RdBCstik@|aI?jIhhQD&Wr@``oxR z`<+0=TPu-WcC?NM^9Jo`^F%Jv7UgByRuyv^`4tT_3P6fY<{<461$v+@tV#`?JxaV_ z6g8H6TjR>II&<+8AGcRKDVLDI73eS||vKX^I(+J+j z)5g65$ppy!5B=-^bJ_XS%(^NaG7aP*^}gIWv1jgQT)EJ!*N_L&Yk{Tw1WKiIi%)EW z!S=&F`?l4ptAb9yD|jR|;2Ce%6a<}GjD)KXn6sJ}y;Hvlm5h@>55C%pINJGKf{+w_ zWOT@^EHGdcpNBk6wmy)xUTQUu;YN3Rj#G0BU55PRV)fz~Si$`~rds|B8SewkPuvTa|qcD9&f!4?b?bs05759W5wFy+Y3F{rygL zUdbTx<0F`${F&v68=c-|I3zfg33S1nz1OTvft#(|JkxdH3CR6~b$<6%_TX)bWD};F zgh=yFeszGujX57?WL$lxH`9$4z8;7PqaY;$tQ0no&RYhSlg$je1}M4WxF;Q`a>U)~ zGn`H8T*?hIH4E?WI3|KyF*iPfkKf+aq4VHUcU@wMYL~8x)_N4j@u^Op3%ePavLTozxUCbQUUL-+E-D(+BvC3tgnw{71&Zj?E5pe~)! zWJ(ul)P$RzgpH5LD0`(c0BQ?p&l3)E3^CC_;{w?NC_UxPTS1@?c^FAuE~M$yuYaY# zCmn|zblE*oF&7+hx>C&tv?pjSAUhGT@<~T`kDOCE)U+(lf}Fm*y&mIOY53Gf>fRPR zr<^j&nc3A+pd7zSJVsv)&}#7Y-*g4!L|a8V%2t*^)d*Tjp{S!MwvxviCDC14xG1zE zp#ca2r@G3Ns(Vh(&vukbW{=_kQShx|XkUwDEk0j$&oa z5WMsv@W+9S#ZtqwfT={!ajn5Jm$N&}r+F9s+v!?KhM$8{d)~*fN&n#%J3jqe1b&9JP&`rn4krcx3ixAvxNfcnzc3s` z6T=?)l!Rw5=CO8F**SIM9gR{vAqyc5^vw*|ocEGGZWJ-g(>%#nwSNHz8h+g;DIid} z&{gaBop2KTlF%?xJ{dcxwP9Qr5D*G{-}|GW(5*uJk|Nw%4xK$5*c8x!i!r=&Rj(r> zIrOcdaRBKCD|%lP_#Ey!+);));ffl!)R+>qduHD&kG5!gB1d!B0XrS-W>6;e9c$k_ zun{HFYVu$$^ZObQyWSq4H6u$SZ7)e%(Rm%T?O>-?MJr1(;BNfvSZ98|YtJxe`aZ^7 z(v?4ZP=gL2rUJbwZ>N~ci0(ni#+x4*%2u|FRyRN`&p3EYn!;0io~DRwB}=1`yYkro zNMnD^sIXl9uhyBO4v(@EZevdmD1aUM>fVJ|kyd4&JT$g&^CANPJaXz1WR}$3P(g;c z3hj`$L1UyHc&`meiCA$nx?9lcD?S$s;`v(zp6ONWBNFV<7FlAf(eewN7>wDY!4CGK zfr7DA(h2tPfu(Q`_OPs%ZEAfcTe2VOt5u=TGvcrMS)Yw0^6@^PC&G213k8KFYgn0! z(5SH@)CW*qIjPJ}>w!qv{4^m*DiQL+5kM*e7+NgiK8tJV+_RlkX`UI#^K-!|@9^-| zm$OB|(a|U6&9t(D&wS@<8=rdRn4kL=RBc|s)V%Hr{&wt2`hHn}^*m2Cv zGrvgwahZo3nw{$^6CRD{A@zaiuQ}i}e{3pqi1M>K+TZ}}AW-Vy8GG5Nw3k$TnKf|> zzZ3Fl!QZuMw%VDoC;R-9Iu7U~y%@bq6)1w!R;+;@YAE9uK83HJoRo3Xn3EKjq?;k} ze&L5$FOn;})4bDr3ZCl{5HYdt8?kRsd}G|CSG54-1g;;)VP)HNp65mx&#boafI5W` zgkQ&w=3oL|(^v3;QTu8erpHsJKBuE23u3IlPI?v<8Je{p`Rr4=%n|_J7!;rn2FYzb z&%#Tf$LU000e>XPI~~4>%P1L{seM8C_e0`%Y#IX&>El=J>D32!xkmKG1iv5rLdNq4LI7t6bhGh>_fI%+< zd6D*;2QC2pkdEu;)ShyWJtF4w)#eXAJMc1^^eT}N323)PRvC|^>1kCEwCPmeHisg_ z5|Z@T6Vr^KMr-p^2?%tBfI2=St$|enSbU&ASzZZ5l84;W9oggvI4fiQ?D}YFs&Pm= zMCGHPLJ5EPQvK?o){)#GO}pAm&JX*fo}&byiZ^(ZcMs&+V!hT_T`4CP`NF_j7hRv9U_E&Ypy} zr@I15{(Ch&(B)q~&*)rg09rXPd03=c9UC&$j?n6jhq3^RM~3eXGpGzNr*W;<>#Q4b zXWY272AsBzPd;DdU9??hVYAO+sRQyxhJN1Hcf$*%eLMH~I}>l1|0Wtg{6*5%5;_FF z8``kC`9A6Tc)>Eet9@>oLB$I5EqTCWD>Zx5_zQ$aWJP$5eU*?|3GiE2*Gh^3C(Q%| zgWYjPy!wue$c{bY7Rn~R?SJ2)Wv9#^D#jhv-hxLU1j z?&coo(1GH4r75whFZ`}qZRdHH5Ma@E5*=cD;*-*hS>b#cn=P^~d%{TxfLG`NUXTvR zgM+r}awYH3fU`o)JJ2Bu8eE@J1=N!mz~=gtIzMI6r>Hw(t)wm$YN%3w|A+<7_te@6 z`b*}m$<6rv@fa*4Wj&Fmi-b$eZtA68st0t?+d^s{=|n!IeGA*XfMn^h7MFQeym~M# zOte4gz0C_+ny%?=KJiII4Jt||Nj`+SOpM+8pwxkESM|yogTyq(ISPZ92h} zH!Uy|R@f7u#Mb`6lxEF;TW)Q#X<}?uU~!`C1sqgTf=||%0ruSE^u2ELJbP`sxQg;Q z6W#Yrw#R0vkL_d9zbZ?e9rG3WT@dd5q?&KhG8+8+T)3U+c5VQ~sH42gR&-VPc@JxW zKN4NmycO4Ne(HYe8v`&-+isg>nw<>PHOm52(1C2}noQ~MTlyE5EX8UK+M-)!g9bQT zb3PhIz=eS5khYH@ai>Z)Cse#UVB=T)t!4#w{e`ybhAT~4_U^#U@&Q(+FlLKQ0u118 z>4EE%8FMEDh$=@xL%~mex5_4<7w-JLiQ9`($tK~+!Q6Zrx@n1!Tkfi$wOT8Z>8xS| zB^Ma0@7u@5=`Kc)o7C@->H|f5^@t7lWuH84FmwvO4$Q&)l=DyIKsZ5I-I)i4=bGP|Q?C6XmctrMi+Un{9svW|cbfG8S7hjuqPS352WRGLp=k}hF z&3BSWH-62YfU>_o-|vNaWh} z{k=&->0jc9=5oD1xA`7E%%hyX6C4gzg*yY&g`KDZ#g%L-akIQmVCHyTb=(B2OAIPZ z&|py7o!0pIRRd6rmB+vEpEsZZ{LZxmhIymRreAB}&blfwhd?*X>7fOif++igswu~N zAAq>J;4iLYX9i4U*OXm4r>Yo$56RU?xL{_j#AoRE0O?p)IryYVV%EvTsZ$U%sL3hK ziclx2J~zbkBkIy2?mHI8%8Y@Q9H@+DmvpsS-@plb(Cp$ig+|9dRw!p4k(jmyEp%Nn zL;NQ;!B1FI-lTnQv1eytat5~i1o)!1sAatT-nBxc3$CR<^d8y<$lnj=OlYI*(@?;m zJ}Bfhf4h?U4yYw0${+OBJjZ(3BXD=8U84((*NUXA+}8ylB(JGhH>y*2>v?DGGA?mR za;K#_LNQt8323K4x-l&TL?II>Y4eWmsGQ#8_hNW}_1X(G^-QPAwai;j)odus3*EOF z*d5ga_%C?-*3r}Xx`{tU$CNfvA!7B%NB_!SVwuyjkHw*2E8WrwY}dN7m!aR;Tnb87Qh2K0R=?jibxEN z$J{g0xm{07e71y=xutEdX3LFHSFl119@9 zeRux{&wo=0TWGew6hcopj-c!7@jS%>BW77YD;(bx!92rUh`7qq(JT&_yb9J()#QOheFzth0xbAb|XihvXEp^yYBl6N^ji>-#?MsuD?S%4I6s*2cvv4Xje%{EI zpdI+T1^u>eZ(f`uzff}kN|Yxx_JBn(_C_8lGigri=hq8DZ;;rT__2z-tR{ zLiYHk4vpZidXKsBa9gB3Q~CDy7Z_k0Iy$pOki#b(Hk1!knKfS#a4%oLJa>mCA3{4l zCAfqM*1PGCiE9z7irUav`ImU2sohoUx30w8q`8w|T%?_r#I9fP-D*eW!jmw6>n1VB z`=^8I(?g^kna9U!dI#-4zS6=?|FlRG5B~YS9*n)u=?f(Xr^W!pfC}6i3=VjV6F7kZpA8=X*|6JI^!n*}{(^yR6zj_v=+vYpU}LPoS5X{% zPdR!K_<%^MNqz5I7XO%wo1nd=iZLRdHqEZW#-5BHJ<<^XWiK_a+mcB@JvCxKata2{ zF3>07`o!=9hohq~$g12(l70Vcb= z0N_Pt5LGDA23&Y3ax?u}OC-Fq?h|Y7dR82)*HyU8F7c^lLZqn7v*Ard3G+LGKu-1X zD>Tsp5%PeW2dr-WfCNb896S3BC9j9j1cXC3;aVku6HvxlOx=jlMTTU%3O5XzyvHb8 z!rWwUEpBHu0NRxRRfi}7E+?ARIU3-s08)(_Py)LLo^T2!Uwd%!ojxQS-lLP2X7$vH z%MfwH_kHK6;ro-qG^#JQoEn1w$d&U@=kz;osQ(eQWB+Zj5=7M9opFHfZNa|+tUiEW z)P4erdJY}{VcWZtNT}(%x~~)D zxz~kCg;su^Z(fXj2Fk5V?M-@%A< z-^i*dd-c?kNj~D1v5XlmD*->>ldVDQ*|Ssq{ISG^5R7X9!PO^8=2QOqCGti@Ra774 zstrX|F-!Ejw!19y<=!LL-!*ws7k9ConsL}+@S7fvQR$PeU=%NtdLx^6Hym*lleZEA z!4>ei56ZTt+&T4r2obXOp=0=)y}O@v81KKa$M+XpiYvHVH0Sr~#X0ICqIZ_{+u1g? zFtPi*7!%dR25P?c!vDnlln$R8J&#e_ChQwTcq0LorEA^+;0Z*W2@4rI-ugxH$eTll z^&pjL-%8j`?r?eZSYa%2LWfiD#s|W?RWr9(!J}S-ri=sp%)->^FEc2aAIvV*$$zAg zc-NPh1S0`->d0X*o_X71mABzU}3SRuR#MnPr)97 zvlmauGxsv=Q?MQgl30eQ@FCq@gUOo6=RVg94`c(ECIJVe;V5eo9sfy*r&v~wi@}ll zwt4jRX=T&KB0H&9$j$dn713@x7{0P=_a9)Z;yB`4_Vbps^2XiBam{s@AL0{rEE;`N z6X|4++2_uzfW{a&$+X1U|9WJ}Q#7aW-Xu6~e>fz>Mp{A>v$|7H>TuhOLRIh4&qy1_ zjdqynn7)YP=FVn$-8n{1uhfS)n7)b;zT5+`lF;b|JSOiwW^HprWfGC$4>MVU*c-X^ zqdi0&+zZSKiLlwPZ{=KlX(Me9v^j$bg(9{*FYnBZ=IQ1I)31~_Vc-$6ePZe|tqi`^ zq}_X^2uD5fj{LM^8rJ8hV>#U(_f8I-A+j5{uMKay52Hm!&{e}|m*LI*?*}Kts=;&f zlaBazX5}#*-ec8jzgnl#U~FLA^|IfsC(M5rlMnacx@DK=FaHsDhKn6@>r|}t|YpDarbw z&3AM&Y1pUO?YaZ?^VxSemcC$mgoDVgxIJmfxD4@qGAxpW>mglR%bX;y9cRa$RJ5uI z0Se;65fYmgP^O+k@rqOK&aDq@{jyvOS1gNlT`BzEjJ&HG`BJu=sjH*Ab3I;t;^v-~ ztsGA72}4es%wF)v&??JGSs&pWcDl7TWn+*XMw}uIzw@3dLE5Q#7nFcCO!bBdhA#Wk zhuF`k4eKmRt5=9@>P|xqvNElUD95?HMC{SM$K*U$Z&?N_S!d)IsqNqW>jem-yPboVgSF{qcRADRmNvS%QshP2DG*A>9; z^oFWCzT@+=S-BxyY?3ch(8uK()2jzDgdUY3;7~1OI3cUTx1m$U?~C?4r{a%~@?5xu zs!~4gF$YgIzSQ^B1xU{B6XLH40 z^CyX=Bk_rMeUiYlBQD%{T}weAHm-LeSPvL=6_ze=={m1mzfnjQZU!re=jK; zthK)1FT(EN{Xd`L_jweB{4u@$9D4bjMcd*p{rkedpGFA&S)hL|Z-dMKMC;FCm(Rl3 z|Hq~Ogyf&Y2QdDR$^A3(|1X!F)HlWaAA_tU{nT9jQZH$udOXo2a|aUgzlL1*ir^%_ z6XjWL_um~DDWVk}^p;*2?f(ScRWemf=*JZ_OYcjr( zBUf(HIlC=E^>`DqI;ap=;ELg2cOUh^%)Z6U<{Cs0-B66{Ci}Uou=$LM*-WN$W1JrJ zX!AGmR17ZYc#Z!%TfLl7DDA%dJM9vP?@LBo;^v^obZTgD_JH*|Ata^dA|CcW9Tt{6 zR-+lYWNPWwHs3{r?AeZAXK13ILalv$N4ShFztR6#HsV-iJP5YI{RZ=Q@mLr|-~0%= zcQA5Fm4?qFgw`~!HBC4`tat8yYd+=v)_lla^G(VEx%XdeglREta?0yGnX7{-`}s=W zKPn`4`7BeosHb6Y4HA;}RW9#q$HublnnTmQT0~HjNaXL`|7ZPLh2R20!A-WQ6@0?Q zNW&($^}Vk3SUk=e;?ei)8R+{N=slcPJ^j#sb{qIn^3m?|RCo0XK8K4XrWc_czR(SS z%b8zzN_A$Wx<(VZ)q($g*2-T_>PkVC9VjfH<*z5$D?C7mp159ADJwlKHczC z0szJWwl3rLty#VS7UbJ(Yb5*M3Boj4ukiG}TKC8wPb~NNf%%i0-VtBv8q6nljZYeU zpZZJFJDi9mI$)HGbZ7_+Oy_lJuxHqO0Y+}>w)fv@R#HeHx?(~8`aJI?nqOC6HD4H+ z@QaD!K!WeL2$?owkbE3@m@QVFQU2ik>$6GaDwT@bIWlmq71wDShP4*%g!1Mc%=GcC z_{aYvG=E_@DUL#IXI;T zUA=S4j-OdKeDXoHD#}la_&;7T0Vd(pgHfP zccfU~-SDoa-5(yNBav^bVNmHnOwvLThAdDnVHf6pErmz;Kuvhyd{7s`3^YN;VQdf-%SIG4Z@BO#JQv|U1VIGik zve8#@A|I+SuGtj7uNM0lmMnyg>7|hR$m;Z^FI?14k^6mrX^IF z(F#CciqF5PF^hZ=dk(Rl>RSbA^%UrGsYFKBmo$H7+1=i=Qotl7jf(lVWJmBm0y?LW6uZl0PilDji##7{x&(bbP`M-$z>ZmHapkKPXJC$ybZV*tB66r>|mF@;XLMiDksY7=Q zNOwthcXNolz4w0a_lIk-lm*9U&z_lI%sl>AlAnD^T*5-t&_G)XPzv}qz8LK6TDffX z*Nwt40}ph16)3cdxH_zt&HBzd+tpD)J|JO%sxk576wM5P5cLCeruY&_I$d6kMNwEv z@ygC6PNcfqYP0abgB?az9Tg) z4>SKAvofr7`Dj}p76ii3ndg5$?}5t%PEwqbble zWtWbE{rXIoqA9#I^L%#$Mt2|Z5tCO)OTk&kasSr>G2(Ig^g)-X$mF4Oo>26R$5WA( zhr#xB_hY$tbrY|hN}J>a@@haMsy`gjNRK4A%$ zAq~_Dr^qAk){%_`I7ly&0fB5SN#dE+_wMYur$piz{cjZgRH8;@(`VwXzi^&Q{B>R`(Fn$nTr+L2*l2hUasOCS{3V`OdrupCDFN(cS zF}@hram@B+*XJ+)(EGtHj4~Mg0f84Bix-ZZ^6GKPCZ8r)*32HZhqL`(OlSH%OPF3g z(Z{p=JAEe21AynjSkt%_)tcoAeKfYXv9A-VTBy%FI&7Isn)Zr=8>Ie_@uOdq#Z*+x5WpeS(EET`Z58HmSE zR4*z2)lq%*ZQ{zaTN?!{W2Z;j>$>H&VP!wr_64QD$71ny{fh9A>1GHea`Z+P_?^v} zK4JF*EVlAe3SU2$VO3}a)Ufd?l&Oue_($)@A4nUZWUbhkIvj}c8$Nzpim9Lt?wIJM zx6Fq;UJHRIK#St7zLN;1U}VbL&}b2&vE3kd=MI943(~7Ms=ub3Adk2psr0svw(~yT zy&F^v<^~6t0s{`v+3t0+#E=dxU6auvV~-8t7nb@a(C_u<~x7 z(OHDu`ziGqp+dcLrkVKPkFKBhS3}FCn|cddr`x;!7hZik5yyKC&?!3bN87k;gCnl_ z_}B29d6Knl!Hg@9GtN@ZOC7^fm%e+IV&s# zObF|iL9p+A>qAK{O^RE(vt8;SC>P6C0kD$ki+BcBMdQ zBNeQ7X^8m7*hjy84^#ILZU95>l^U+pZtHp8Q;l02AE0vgd{xfK8)LBlD zp!!kY-_3?kfvy)QTLYqCzfGicV+6QAQQcr8Q4oAH9G&<3?kbA-Zlk6n!Ck06@WUF5 z*w0DYwv~2257i?sN~0Q7Qp}btSg%_;koO^r0RPk4Bs9tW@e*5babC7qIA#kFW?D;< zAgU9$Xs9b+!Tkd z=T`tj*5kupJLgZ35)f|{7!c=kzH-p753jd2SN^e#)}ZpQ-LrhFmXzFTPu@%os?im% zZL3GL(gDd$`vVpDgGn@dlDlBtQFYkBZGeLN6o-T3q$<4so5~+ty{OaoWw;g{^3SrD zN2g!tN|R8YHKhFQo4;||T8I+A(1dO>A;r^`&MVz4dE@BfobGMY=sD9(a1WO3XnRHa&-UYjH&a#`{ zHBBcKY!sBi$qKHJ`*KE2+d9Bn>uOPX!`M{tRl6CH#Wel9Q3WMKm^ivi_rZMj3=mqD zjayNgLYL6JJ)iOVpqXQK0^=ujW5UaQBVn3AWdJ^aZ_>qRR`WPKEt=z|&R)EdGKgru zR>DF<35bB%zw0m3q6zG~gBwtT#l<<@4C~g_@zyMWYWo(d1cua|9^&A4eKk^TM_d6S|t@N_(OG*}d79;r?zPR1M>QFXqKF(afj7@C(|rASdUr)xJ}D!vjJ4f_}M zB%9WRh|c+Z#U!eB6G6oKUUc4i>Ix{#8y;<6Cj#_fVU4x79k?4>mk@-9a){|_{Y?Jo zMNDxD7~#quaSD84Rca(HIdHy9Zh_@Z_v_k28k)#*a_iXb&-~ghxV*_b$UhJ_=u1XHwQ%&eLi!WoA*W>{9$=`vica=zGmrw4qBJ< z*58It?a%*8ggErd@Atq`A5;7wgi=6p5%0~qN179lt*q8XQ@@txu3NKZeRTFmn1w5& z1^#{3lA;PWDMX)|=uD%lryZtgA_(-1J=(4QUxiXhHvE0wOFy+=V*Z1!RK_>s`y;X2 zWbVY$U2uTA**<|E!(otw{>2mqR?Az^3gaOiGLI-}yp@8A*Jvn6|J`^4*XznsLUYx| zMaEFq;WwXeO@(ppi{Fk&L ziinVkD-5hLI%`YVyjbG`C10;*X5ucoC*$6Byx7D)<`>KUm>yX+g$q47hR79u0^S+1J1b=1mh$tYZtF?E>Pb+dMA0>u`8WM8aE3isOGxdrhkR>081n zqC`lGFL6Sn$6Os|y^2)U3zjdDIMO+Qxp8U;n>mApYfln!z>o;Qn4-6TG2`U8{E&>b zCjWR6L&lTD$ObA72K;rsUX!vdsBZPs{wpUbt3Gqc#<%_7&B9L$mRv;%242lYBh#u0 zAsU-QRzSzz5LQ;td3s*)9IAX`nB(IO)^WrQj=eiy9c3W@lb8tBj&#X$afbec{JIN{ z=2aq0_DFzBGBVd&N%W7$lh!%IClCn5S!=_;aw9vgsDo!N zvmrmA2*bha;_+$LNkpXGl+=_IFnNb%(JN|TpL&Jn!8=>Ve_^!%cnR2K`>Y1@s3Lm! z>wcp^og|`CDG7CzOxHF_G93Y2qdDb`xlGp!MXi}8(g15Hbf<_kXv?@DKOJ)VAH(ci+knAQf4Vxr;YH-6LVMx82W=hNsu_ z;O7aWz&!`=tbJ$?C04mjD=Tb`tL!7}tLdwfOCh<4r^g%|XwYz_{ET}?u^MxzH)r?%VT zh4rWAJ^ezO!))f17x?Ao&F!Y#53A9k*K|3|DA@TEpMh8mxnBLEeh>)+&EgxEMg1W_ zVW>z^10u$LuoGYg??>!s(#+>=>Pkpe#Kxnt`^{jC``M0P_x3?y(FkX*R)$QmnpE~~ zFy?&$`bWZB6<=x=MH*hyGHpju>@Sh54bMFeV|K4j@Av+-ITCB`e#k%-cF#u(^u-Pn zNwuV;&RIDzxq7l_xaqk8ECqYSR-^m%`B4jl7BvluH$q$HTZ72+{r4RPd%Mh+FK99m z4ifH&6N=Rrr%U(vk2i;W0nf#Ubm&mKoBid>cf4D`4E0%39c8cB(VzOj0H0^hq^NZJ zEF#QYd6-Ef5-3&ni^HbhkG8oVd?nhnqmNfUYPnwQ!YnxWb@t7UIyPM3rf-j%jqYDz zF00L09b6QYo}}_nJBuvE_g24gYEO@*RZb%qd_{5M2UpWOvBda~vehe-(xFNcz#T`7 zm6jHyvYnMN^QHCG&%>S|1FMEpIWu#BD`C=lbtH(h@@dzG%|SF!({W7nZ_o@{z>2^# z9Iw5OcQ|puL5p;8;N6ys~H%IbVM4*Ia=k}D!;m_C}Hr_{+>pdsc$DiAg;u98#w^5IicfBm}CLXjG-4q ze!_rWkbjM>`oTN*)2o$St#mLk6$*N4SiWnw)3qK$ruyqQezAZs7eHyz78 z`@k#?JRxu0;Avz9=WL0O%;5n?<($%|BK7>4+L(IdO+cl%#DV=ge>MrjyN;3f$^ZX3 z&j3g?a#ORFlnVq3{_FZ>Q=2EDx)t%hhY6OJK=}Gqsn=rIIJo2Lo0(c-M4Oe?hG(hN*(###K(~di9_5P0k{>mgj6!0#&Jnf&z^l4Wc5P@8>(O>a| z8o}u}VeA77g z3rJ{=sciA3&fllVGs70Ex5N}Q0KNfiAoi;y>&h1Mgn?t6v&SVPVk-%6>=6O`bzZ5r zW6!eYw6$Q(vch(S)}R-$m;SSRWCe<|EdYU_k7<_3NZX)^)f+@~C6|$3Npb)cV0)@S z+4=p*=^GW2AaeRhRf9$o+);SLIc|NJJ8K6Rrp7H1_uJtW)2(Cr3p-}oynoL>;qJA< zU_>v{0H!!=S10cmpXh%AYC0IMu!Q>jqhq?(=lX?9osP~`)y^{ua>cHX%ep9<4Q z$o(p>`aU{}H+9H4#oK8Q6xys>|ARzG%@d4$cKH1O8AZK+Pi-?30V~b~!h~JDKhP>f z@3&9Jj%q+QQAsxpJxEyhuLrWt5HJKqGR3oKwS2*bW{PLIc*CsR0L%(Rn{6UZV9!7S zh7R`-pic1nOxRFjD^ZO`!U5zAXcrLP=p_e^t#qJ1tTs?(y<}Se#0i|J3KKyjaOPjI z9so53=EAqO?c1v|6}AMLyx;48W#YRS5o%0Y{snEf|06rGevQH)xIR?&Ype)Q^p=z` zTpJ!5+t#f&R0L`zh&8}>8Cbb8K-y4Ven|V2+O{+^9$gobfw;m7nwz9hy#Pa4s zla_VXmWz?tOc>B=g`!A7jE_0lk)w41;jROpxbcqxR4a?fk7~y03e+s>0NxA*Nf>bX zGz_J)_53Hq^PG(qsBA4DtmJ*{fPo4fcn}3wJ^17%8y-;sb)nL@8L`1Yt1Z?Tvg|GJ zxap6Ti31+n1*=Wb%mGRuRUTy3oeKNNMkio$GLT?{P3 zp>6Rb&dk7o>tLqk+KF(OYz&l1e3On07#Ov;S{kqhmHw8cqO^(~PnI|M05sQaNHd3k z6a~CTY^BR#@%@vrZ(*fyqHFOD2wubb47#c$mzW-{C4oLw+6Hsb6|92g%^HOqv~!`; zwbcntz6JwL@A%-6lLHRe7D91mw9zQo6U=i0VF1Lemxi0Dqs=G8o+8#@FE%xYv~XKo zHX)gPxUee!Ua^3o*t=+?cmDDIr31N>m#Sz5=;IjK$)gXlwbEsZg~Oz|_YFyAPxKUI zo!K$l|J2wKmmk3Kcx%G5jD`25>cIvMtwH`hgN*toX|FXp3JI zQ*sZ|gB9eH(aS(-x>gXyEyaf^gJe4V;4?ot+8&L2qHL{j7rg^wHzHpIyRzzxbbka0Y)vU`HsfRnYr}!XKFrM ze3S>pn_C%sLPZ+=;zileG-CPprbZ(}J+|c|WIB;k&^1xy?!nnH6)9_R#-C5`dB(O> z>=CzD+Syda%1@pw^jscCMIKp;{iaxhVLr_j6?Hw{HrXRPi-5;o$uvhrU4=m7{Dahp zL%6G>WkMwY44L)`jd=8+C7y_f*4Uyjs~XP#W40d+EOx)@vxd-MIA^%hL^Tb}CYvbc zlZQ0}k%$6JfuPX-%f17klx66WeChutj{-}J&~!u;eO>>rQ$Wt-2ebMAD)8b28K@tL zN54B4UKEi_B5qsoRP?{zFzOKZ%1M3blDF`cjlwMS_3f~l9M;dW!k@zge00hzZzjD3 zbn_Q1ekpcl4VN2Xp+MEpwchKhFSv+@E~~&mpY2f-11Paadt{IrbPT1k)loBTzn|g1 z#3%jiz)6!8ALCmQ2>>P7h-I0txNX-MDqRUT=(OIKUdZ5iFJGcJzi}M4V`rV}D#Vyj zk)4aQ@Zo^FUxe63UBUKu_ro@?YVIY;PTF_;0p-&cC9PU%6oh~n-+C}79Rjp59Jrp( zm()LkWl)6?uf6h@ZeNfqaSHDM3_fLpJ+479#-LN2L)rxr6YoKn zD5YlkG+?H1u-opvK(`~?s1a5}nRcKq?Yw9221&6mm^vCnr4|d>X@4WjIgt3|4KRF^43dOF~R(0s?_S8s4`^-*X6qotLIo_x#`XxR&nnwX=> z@_S;4bS2*Mr_tPt_wcPs4u(f>C!a42E3b})^303@ zIdOaJ_pk!-JgIJYek{t^llnYtvrT8`ULbj{jbEAsVMFhOiu~VJ_hOP7*!Wh7-rXKT zpM#->V7Y1+nqGxv)m|+cxrpM1Y+6*hvV8wfCF@p&nS0-BvB;rG z1Ry7^0u66tBQ>+~c@LiDBJ%eB1mH^SHicJO=|nJ8Wa-L@=|sk+*|K1J%Gxp!zoB?f zHT4szRC|~gaHWD)2=o)9D!*pBAMJkFncqeQpvX^Z{N?Mqb<)%o5HPAx2fLfH18MBj zNQSyrd@|4Y43iGuT^+1ZA%LR7np5ZQ zPRVpDGJy~RTofk2o2$N)wCWym+FxcH60c$28mjCDd;imz?0GCcZ5z&_?K(0FgM3fyY^GPsJ6g-`!9BI02qRuh{XNXY+x@n_ z@&3~7HjdOuR+HAN)k%lps;BYr(Qg8pMLQeQ;L%rBG7z7cIH<#!dmT54cPcl1@QZhW z^8lJ#SMjER`eo9utQau(PZs6{YEnlP<&@@0wwiA6Q4>Utp#evq9We3}AgV;R3!2rF z!_*3F2+EL}(Z*G;4b$d@I>&h%Q-d_f63sTY44)~(nyG;cwl?G=@OvQLh(EsyCIE~y zjkT7TVv#h4hZrFw%s&?J9UH5Y$iUV~acJ9h76JMsAnb*EW%>aOjD*&M7Rc2sy0602 zKR1lb6NY>^8fSsCa&J!G*LXLWYTh9(mRSsQEz8T>Z83RyjT{^^D;Gq!&4GZ#A*-hR z5u+t8o|==FHZ;j=;zoW-bH`BFI6=NquIti_(09tV4`3MX`D1ygB^7m+o^Hp|Xj^x# z!!vCD+u?5^P9_}&7?1Dhr&Y3HnGy2h0e4neeMc5>vT;;RJ$B*>Y4F4b|0%*_zbi3o z^pfqp1Xbcz#!dXfq;LM=&=1pZX;pkFPKSIS;(h4avoo*nA~|;A&uh_Ep}uovlc(h0 zRL3W*G_BNf$!Mx0m0n;L1_VaT+VF={CK5Pc7FS@q4bWO2((i;*YX5PBN-XyQtObh+ zG@!5=TecOSZ^(vENdJ7dY?{N9iUWtW<89Vtzk?vO47QR~s#lhV#L{#q-oW8w+vXV& zl>Qzhsewhjf0D#mg>NM27z@4?BOSsTC)Q9^W!RgyclUmhE#G`LaOcpA9(sGql)= zP+;)+c<_$|*DL9AU4o>Rp~79uP`!wSi%sN7 zMktmVU0=_ws{-FWzOwz*k`PJiBH#kI+U4_ET&jjpVSJ`D{+GW9=I)uIs@76id;69e z@YiZ+T10@3f-egImIOij&QBQZ4n{|v#0?jxxC6BwOokyoEsU_xhHMha#U1@8*mAwx zavaH3sAat{j`aFg<})GK_096dg>9)Z25=HcHaD3sTry{(Vk94A1I(Ho7Xi7B82sA` z9(Cf17<1ba1Dzn_#>jo1eC)13pzPar$>?qRj5Fsx+I5a{)$GFQ@HPWyuIj?P`-SK) z?3>g?C%?0T=61uGb83O0r_1{^m#6ppJs!;{>E6v^D^AYmdz<)?L%Py%zU?88NA>O? zv7Z7HM7BzCb`vBJV5Sf?Txs_|28{T_NFXbG-ir3y7gwHgMfb-s_DWl2%#=Dox zCdJIIFc>=YDd`~l?A}TFm0C(2vQ>YB6HSIWpV%Nz6v3X?NC3vo8C-HM=12exj%8LM zUlZ&YCc?vMC(=8Z=&4h-j%ozkWX>Hp2c1v?pC#q*d(%SJQBlX9mQ|zaH;->51e#zj zlu^J57o6A*qE-8|&y^$Bs|4#ZbvcP)>}7BKuF5s?xL18m3p%r{s<85*bB%_jka*S zG}7Kq_-sdI0J?@MT6S`S;=lnt%G!H8mJ4*0hobLRx0u7lVfRx{M>jt1!~879ZX)J} ztXb;C|IqPOLh~E?xta|pg}+43qIDxK5~gET!7#w{trI2Vtc!Wu$^od5+BUcftr30s z<;0`4y5W=e^q<=e>#u$)X{^>M%xYc+n_c&yr)ZcrSu zF{P}pnw*yLCAYXQ>@f?aRE~4SHO6bn3)K*BDl-L+6`$}bVH4( z2L4}fK(`NF`eQhx-}PNZK4@d_7D?h`W8R88?fV*@D_`+K9?w_zLcgwcV09uI$f?WX zqQCsGH~QhAK0G&!clN%sn|WEwX$f-osW^dSw9M`|eXayXU!jj^EEeU=2#d#WleBO zm#BXDbzOf@ExlM#yBh0{{I@yd!k) zb*O5}>9U>Cv{^ZN$XyHH8xFMyE{_Q$Sw5Z*w?btfG)os}t?K^ucT8x14;F25pVR)T z=l1VU$#&6KdoW`#bY0SC+8O=P;NZY!L?*5MgS4^X(AW1oHT;RBWmL4v#rl+y9MV5y zA-nq0Tiz;b9A2F7)iS?-j9nJ!tJ(4MR+!*llb_~=vcFHFpO@dU*T^_}-8eA$5X*7K zStEW{m(k`=G50N(L(Z9n)iuq(RcRpdS*gA>pSBiT4SG&T zQF)(rXh~m)JxJDM7CTbM*myES5rgstl5w`>npf;hcDmoSJndo6L2r@3Y?9+iONSy6&C*B zt!BXtTf#YJ;|2(JmA1@ANi*jlb6{X}?ks;=|3EKVo_U&Vd(cEBSef>yBlRh;NZO(T zHdKHh?FDLV-k{Qrd<+Cux??D50#1y2?!>x97|DQ-&lM^u!et0SSap5%7bY=+!QIda zmbK-l(x_L{Lv{ZaF$O|txvy6v#^eD!SE_L=RSeBz@$E?fMz?C_knhvw-z|q6xg*E% z;r&lnB!x)bg~iGZmLtB$Y$FS9r?*?OgRCD065Nn^pgs_+(PPr~CU~#0yI=UzMn;b@ zB9FTl!i1b;`i#|@FQ5xWSMe*@8&6PWop#Ntf$9JbTzTR1KHR1^!7zbYF}-V4*Yzu= zJKd~v(cnEFuOJh(f6`-k3PT>K;>2__ykGJkyZqf+yqAa1l5bZQ*XQ1>FLC}m;?=98 zWV-zuVsefoKo;vP;nT#TQD9M=qZCX^vKV1=2=Qg;*gwfMNF!##bQMA zBqnO)_ZNXt+dhuGwrw4ONX&p;IYYr2KMCQW5MNO#FLM)K?TS*sRKhDLO~@i|aF|M& zw?sgfqG}7%OX9ekGohAJKl0_EMU{W6=`r#L>&_Hh1ZwxBt!&cvhB>unss|2N^V*DJE{ ze(obhk-Q-mb`sY04(G#h{RGMb@hOh1Q*nM(PlptvLdISGHxX!pGH3sp&?;#-_h0@l3c(6SQ$JD2QYGy&s$xJxfeFmqT# zCcj&gg7QG~q$q6ZvwgdVXfHQuJM~S8c#nsX)2BtwWLI_Y;_!kuhxGH110!tLTF)k# zqncs=qj1g7&YSM6)aZd3MqA!E3Y*;Orf3cGeNl z!D1Ceb&sMf$AS-fL&I}*OnM*&$@|jTb+NSc!evrNihkfUef!n-P>sE#_4p=?`Ojhc zA7z(jldHbZr;Un+e$>Lu|8abV6nGFvm{1PU?tLp`c0WN-*n6)G14-Yu^4rQ8GeFR&uBnQX3Tf$Y($jSYQH3>iWm0Ec-Ylc zQAE_fcL@i3YBQb(iT_{|5fifv4#9`|En;%-cW+oV1wfcySYiXi&$mbA6X3Imi_x21 zAZ>aDgw>QcL1hR=5u_>INq&4J^Q&iq-eOlKpQR~HttUq?Mjm`TJs3F!zU#rjTSCdG zbvv3FAPdMTZ#$CKYSwK`qoT17UH$cme_klw2zuNv=2mXn7ufF|^W0sFKU_E=1rzNe zb{H$?&LcO$8oe`GMjXIeMTKQ`IdgF$9p4Uo^UaAneaiIPr&mXJVk>9j zVk>l>dAGlNV%z&0N+~VW-RXTEQn%7?Kkp|>IOJtIf9>Xx`Sru+DDUVp?~2Q_`|CB@ z_jkC%D_qe!G2MMRLQ7*(iv@IkPd~TpVhY~SwCp7k<(c&arO2BWqM+h?3(HS24~&UC zzKK7pYrfnmY%J@Q%Gy2IeQ?a+piqT__4eGKsC8PR~%V3V6p>&K1f}ro3!V37&H_u5VFiteUzS#N@Tr7k8*F}uH)(4 zDA|zaBT>ut!zSD|=hg_uYYKl|dKa=tgUXP((GWjE5i?_mbHusl|5%h7@PH9g(UUx3 zjQ)L|A;;-UPTOOz{!Ea3z$Jf4JkJ5=AAb9&YM=0_+%?3lnVf5#ohLRPmp%ulozZ?6iKC&PN zg6BB$#H<2L6WZJ!j0gur1MXM!pLA9W&$T6Cx|X{O!m;asR*us)GhbJDUjEJqWH#dW zn({AoX&TuYEprRGy#++yr86|9w1j6pdL)bBl?~(LK0?M{a^-&Q`r&VZD#-KYy^yK7 zU9v&QrwX;d59l?hEZT6fW3hD5EIX4S^7`WGFH`rzR$|-mxOb0Ah2p$%uc^KDMN z7jrIBG3X5iL^;VT5vgb@5Wm`{oRUm;!z1 z_%#x`-Qh|DLluz_SeA5D2@jXt;P5=#>P&>QYyQ2bBypYJgVHZB>66)@hk5b?u3uRc zgWTd93JKM<>Y`)5STu7Ea_qb?^2-RVDpCyCW#kE4_<+4e2}$rAc-h-&8rIk z3SY_+iQ7ox)naEK_g%2tusEkFVQ_L9%i~wn{B40We^ke7UMcPG=p^P96>&EGo6hOY<6T-v2(`&^ZS)p#!T#1o~m0y6zi8 zk<#zvF5fli%fs#pa-?E)n4K8cXfW7lrBAp!;c=g$=fGGmO5BjfT-~i&6$I>x?!*`~ zzD{R-W-|-j`ZV}npleUP!~6WrCK{F-rO&=N4~q(ED@RCprd$M(lAbvqrXnOHSn9(h zV*@7b3m$YPmXmL=UbO5A3!$+ZRbuj(j%Pl*o3LKEV(Bkm=EWOF>I$xS-cDW>_ORw4 zeWEREu)QcE#zFoyiJ69vL+6pBs6`YUo6O-7YDWbBCD!*BnX0#>Bw8|?1?|R$=ag|2 zi4FBvC6i)ym2NhRLH?ah$jD>^6@hUQe;3c7(4HKh^^@YrkSZ1(kdcM`Wb*uxM`~=Ug^1+hI)Y zzJTB>gk%u5L-+R*5dDV#=R%{C6{a)X>Id@MIXLke{|(3V zX}u^{n9@XqM6~YaqMQJ`yrVzJR-TP27G>JM_(0mQrE3s9MCZf%wt!WSu@B)5C8eF@r`57bxh9ARpC}+1+svE> z4#fz+z9f-8#xsmDn~`WKGZutMFt)-IN=v>)cO}t^8BFOi%~I2et;0t?>^myBzWn@Q zsW`-JhrHz+DU+Le0)-JVucq7IXNn8Y=jm{WhsZd+8|m9(jq;*WXxb+pSg%2Z!+b@! zYhgVsJjP*}{RCQAFXmkTRz&6>os%Qgqn2geM|08jTZelB?Vmyw70?vkWdYOOFtLP; zWR6FJgb70rg!pUu)Da47c+pD&nNgK)18RyMXmGkf(|3~d@rJ|Q*3%SE_qv5)x9>g{ zsprFnJ7WEY2BE9-1z#o}m@#i*9j7MUCviYbrT8{jVLzr6%fMt6BVNO=ejwIz49J0Z z=T~fpfLB+cN8v4nA&^;KzlYp-eAxK7fWbhq2Rnz&R|nFarqTGMIhXPGCCvib{rV@i z717Dd8YO$hfX)7dp;xx|pQ0g&yi%qzGuwP ztvEl6%!xf8*PR>c40=n?ABs25Dx(Na!s!gI{D$j}&!!wG34D1W`7p(5LpSXBFyTB` zLp|@!W0lI{CPL-NCORW_tEGcsBe}E!E*s@7uov-<7#Nsr<_nhZR`*_PaAL+&nCXUg z7BH18B6y)EZ)HBRmrGu!tNVWl)lOt^q+d7t{s}WPdxVZmQYBQX^+hfr%gv{iSjLx1 zO#CD3@paWOVhR7_1;9tX`f;6UEVZu(Yjx-EQ=_kC9IL3Xsl2&CkiIA!Y9pruVPhQ1 zN$huZT<&(X)~#bhmLBND*iih~n`RWBZKn;fL+<*KT*Xji)P!_qBmGuuoP<~==-T2Z zMwevOmgHNJWsZ@*PO5d4FNenJK2;54wyKJl!INL_F~N(*WmDE-?giuvBo-O-iTh?4 zKbrCEd@)0>#-e?3j;XtYOPLz>N-`uQs3 z!<<27;J?KeXI&>)6lF+}#NpJGN3JPgkH|mq?dE=L3_J|`%5al>qzbJ-J7SuxVZ%db z7jHz08~>xay=LDM^qP<5k5(v%_}uAuZijXQD`T(sAmuiuZhn2>eKNT$czxb8-# z`QHEH_1pO9yrBH7pF@f;R@HqM35s`<|MIf*?1g zxrO!VifvpahlU-!#v=Cq+HCWeG;|29|L84==!-Q{ZXG0)OOEg)hHnnh z8B-N&Zg-ZSVF-AnCkWAZcto*WG6e6)?%5`U&nHt3&(G?m)Lp)8B+~Xbe(Uke+;;`1 ziNL$cwr1ibE%^svn}Alx)TP(4u`Trf4fS@c_DUT@=`PK*#JqP!F;so2+v8#$aq1(J zBe39_0vScwid5Visw{V^ytwbzbS~<@@)3!g)O+|DH|fyu81lL3joV=IGWxfZRC??g zrSfieoa^3CJ@KEHXRQ5;)wX7}^<5_2mt48`Dp=@`LJBx>uQiX4$Z|i-9CJh^wDz>l z9gLn=83jsNK`6EL$8;8Rz$-b}q3c-Hb^1$u8n_G|w6>6W%sY5bk<*blr#Kzzac&7@l!1xic7h@(IYi-JMXJBEf)2+QU*=E|5TCvk-P+}FFgkIYRG zMX(?H4M?rozkBDicnxCR$Ek4lZ5%{B2dF166unR4X7j-*3k$uBd zI6K~n5)n_o9lndJ#s9e+;v~cPJHb{Vx_R={{QMP~`TUlRAU<@6TAVSsaADyS)<@wF zFy?4SJZ65C>3T7cOdQt6Ca#r}*JxM5e-Md23pZX#n>VW9m^sKVkiVj(A53Ja50D=J zH61T}S$3VVlQvMAgHE`Uu+UPTHksdKsm$|u7Zsr^d>bsvuDw2l+pQM~;q)^&JV|sI z`(jc}<86U<+7zhYdSTKYH|Xtkbj-iNt+KyT9q~1Fd1plGtE`6E#0|ea<^U&$UW@?a zVWg&m^VbTdKJ|sBrE9%!Jzv~JVzk?&aF^Fwd#;Yt%XMLRA6dS)PYPwOIQvzf80v(a zY>Af+*`cU!w-NSfJCjD5Ihjfjm(ye#FL#Rrm~iv2wo&+fXb%{_nh$Om{cMR*_82Eoge-B4N9J;h(Rk^S*+l6cX`LD#D-Aqy^CPoK5Km?(q&tSV;cvmq1YqDWM$mjG#C*fw+NfP+Cm+!ERkWcC z89n(g-|#gZHt=m5J{)I4D2U5mFxjNw&^)3$pI_B-l$2q%AErVz5qjB9OL zyjv~1k?_c9@NwCFL7OX0x4)>6E#a1_Jxpd+5E$gpIEg;`0e>;RF}24mQPi4c`HM|M zz}TZ)!KqZwy;p;bopkxiT1!Yz||J9d7oM2N+b+JX;bPS?W97J0nU zxot6b9vK%=;*pw7jirl5QN3_OEmGoEixt8z>5!X10Xg*nSVAqRjjnMo=M^(Tp4c*U1#r%Ag&!T%BdSQQGQ7h|aAF$c3 zccR9Ind+_X3Dewg*TQyrvCK)$rAaJ@nih?@Aam=Bk{V!Q`R7k=GLpKlXNpHt6ozxu z+!qQY$Zntws~)T7OGD3fZ_b z{^9o{lsvOsIu`A#7Y~2%8xNz*_(>xaeA??#c&*Y_^xo<&m+S}+c)VPa9<{Jk`Z=r) z9M2)NT81%`vLXCw9a^~7t z2Cc7Kr#oHkYBYXM4cV4$<6Zx9KeMMAn@-cLhg9ceCZ(O&BcEjH>islz2v8h(bKLLq zhn?U-=i7cNwZ&6nJualR|BR(&IHVEN?t-0QqfDhX(k*;{-8YL#B9cWBEpcnYmb1F% zIyQK5FidegR|au#FOV_#ss zi9^XJsQUm<6W#mb4MJ}gJ9+)gk3TfAziEvHUwozS$YMwCvol&Kzuhho!sa}|-6EgT zB~BbFx{X)W?y8#T(B0;XI5DL9)6hsMxrec1Yi(|@f~2j*B!+oO-#wx*bp;Jr7cG#j z;tCrwlP)jn|DX~P`?Xaa1!_htOciqDp-ry8_9(fp{ngO*h4r$gj9@fDx3}1a=vHk2 z$!?(|9y1o071YE-6`2_xFnU|T(aZI&jEu6q3`1+`?LQlWw#;cF8YdTGbxYNbWaeR^jNQSD#UGt&-i<+_Cx8Ndlpa3E6tZ zpYedPQ0cTt_Rfa9!9Yfj(;PoZv*uxd$*mwH;gSVE};8%Dm&uaTlXnCJ2WKiXLL2FIe{`?rF z{^0s`>88oaGq~>dwVU}gdqz|5(Di-dedMtF$y1F(jk#y^4Yj)KnQrrC{9O)L9Kn<7 zpWMA>=lfOwF1`K^&M?Vqy-~=zGLYfMn>00A;i228JRkZAEzp#$Tr@i0aex|tjg2_T zwcI;o+8T}ix zb3=mgjWwQFKAdsl&*mR$QmWo)hu=#?!owkrZFy&YMe!!f#ThdjSP>4rVM6y)p@*N` z*aH-^e8XWSH98cZR;ha2($K4%CY)^}i|ykxqYq_YOwJm?gX_1%XvFq7lMauV%6#|t>{ zb$dV>vp9l&*L-QMmDk7HBQC!L5fw8Q+fF;bBBb)n1d>)fIqH8Q`S13^{a^iNZQE5C z+n45rtrz=HgZrfVK$}Si$peA`_6j1Y2JAY8bYp8R(^Ws{VEd^Y?F z@g41i_{1#MNbH1>JHWnt?cSt-^Txce-kQhz_WpF;6SurIkOx`bMhaa8#FU#tG=s-O zMhX$Lf%HIV=2I88)`+~Hrrjlwj}kb^FL^)Gzt)q?<>Bfy?S~WC%p?* zu$QmGjy6~hk;>oD>NBqqHf5Y~uKa!XAAN$WYB1an3cq9?wlg|4DQehbN{2s7L`+;@ z3VdYN*mFt!SuIvB@b=a`Ya7+GNN*aOBo}zaN~>* zS|3|D8I1;C{p9jALpx&^3EWHB`XTAZ;oH(?tTIlmmRsnivkq^RUWtTT zG9nl88Zc&n&kj5_;hovXip9L6-(~a07y?LPELCD=`wS<@-8~K_7}k{=v;yjnNsf88 zvJUbOwy>K0r`~{P!_>9oVfE&{1Gwo#HBTrQ4nu%)Pq}-6qfso=qdZ#aq))a;MlDZG zSs@r1J(tn|`4=&QcoyOL*Z6M_ZhDFH(Zhh7>rf^8zW-u?Soz>eAR`wLJ%`Ks;eE`n z^}-%zp-yTS3Z^{CEaVna=phKtx^l=(IEa%L^wqT;S12VLg_|dJum3O#1XIaXHJXM~ zyWf%7uy{bA>KPr4;suQ!@^8uN=fYL=VF4e-+ad06i@v5n2|$A}QacwA(#GtZ6&8E_ z+4m!A(Y)@ppUA>ltyVYnss=KBE)9K1HK6FHZB^6ak_!Q9NsDc?Rwuut3p{Jka=T*2 zK?;4DA~~hBRF!dSTM^Y^&Tb>J3Prv<-*N5DOErE6@KnF5CxHDdTy;kc?0YsM?fzyH zEn6lE1i+0eht=Uj|? z*163Ndj^goGk1;&Z%ZR(`91hBa`T0(94~e%2WBjXCV_8lPimblncTC)=%EW95@L@Z zVdkD6AvBs;p_XV?FvC8l`ISOG*l$!vkX;0N@*CGziZa7h=*5KG$VQb3N4vmr^m8eb zAbq&?smuIow~f@j6n}q_%%du8%FQuIAM_CR9ck$odtcB3t#qS|gC(Vnd$IA%2Ajwg zkpCE-t8;*gn)^2!=#7%;JmR;eYFvk=?^uD1hjR;Iwix4lzwWMmzvjhX?60dxC>k;m z=^cDyf5s?6HwTUYM|qfK5!$kzW+za>@X+my~A7g6j19zCk%4l5a{gn@@ZhKb;P99+4yxw~szFsSa^4yD1Y(+xhR_Dk^K4j75&gyf>j9|w*Nfq^%^UvqSgW{bN5}>%E|3= z{=%H0g5@}OT1Uh5Y7$yYd-;A0gYLE9vdY_T>?pQ?K(}M_p?2(bSFajbSJ8JFt_Qhu zZE-L(PP!C$G{J~T!#Igdw^2HXf*1l$UTs1ADB>jj=izZ&4ng$_1?!qc^O}Re(F=#) zyB-ax=TmxZfoC2f{t)2%EOk{YZTdo~+(iLop)32IQeU|(WDCZ164`HZ0A)*X@w4G3 z%isdx6OiTqy9uIZ23tK{ykK3xy7{O{O)M!S_R2bm?8?37A%^~7K0Y?LC_BSu!iPJ> zzcGB_YSDmoRIxGWU^RzpGee%|SttK~8V3BfRTnp)pla&1%NrS>jyDwt{k({%;2a55 zOY0#<*I!m7{VdpzrtOzfV1Fy(;H&7s`i@KUWqaw+=^aBG{1TpxMDoP|KEzr5(GEzRv&ZvAXC7w{}dfb;6WzN8^bzH)W?DNUHuJetJTS59J%E@+W zQcit0#6*4E`cHYq)DlaRVh|cyENR74zzM%}dFH)}AwhkCQ*=7@+jNjnwc56i5uMfu6X1Zz?>l zlhMRA1p-8|8pwc=79wMlnUTbVJxXf?M~T9thFX}B9KM9cnJ9a?@s%X!%eRwsM4E>4=9H1E<~J|Fcn(J z;0A-L!2?&vY`oH-4b9XO=jH7x|G6-NNn(zyppbfrgSs7P65i|=0rF1ki0Vs0vT9&R zt@F5mJsVWrv5(qaFlPYeHKwGv=h=>b+=j!TRr#+ClqUdtWVHzebN<)s>Eotz{zwS2 z=Nn5gr^6o}tegi16)jkcY(s@HKu_(iuWk6sbj(V43*XlzgI@#`st1wYF!Go%)_~*7 zdF3|1(5whH5OsI@gYQy*9cV?`3h*1}d^yg2^Ca}TqSMMQ`hy3S|H@hUGDfe4Zn#|E z@fIhp|DMwrs*mjtO~vm2EJooqpr~FMj}QJmmPXZg;7^rq7|^pDnLR=T`!U z;_d$+u~)dxI@K z{V0Bex_Tn|H^~LwIJEYK@trE#INow|R(V4SeK7N!;IQqa(YB11L^N1nh}iu@FnCyN zP7(eT>?oC~k`ldYzCCwLjlSX{F_T6f1%IoLAN;wWkW#j`ljp36_jAEViv$^>X_=T` zNgD7JG)|xTZ@PAlKDa=Mat4>423GpX^lD-rv(dYq3JME@F)>~(NYw}^?@6Y`kP3S^ zYp|%bkSfgd?#{n3K1-E7aHN%2K!$*O@NVfFO`H_*MjA+Jyh`&63On88w zR%bByFL2?aOwz3FY49mcT|U)_C$0pYyvz{0-{F@V2DXdvB{PL|K{|X+eU9eqm$Dsu z1Zj?nMdQIpHBE3w1v-ZCBd!j=EyjXPsZ5!Tu>fAr9m)%-`qxAk{tFanQu6qFhtHu# z8cx4VY53^uj%HU6Qn=n~AV+VQq3w|Al;ATDw_6ZZbo`?%4Se&)z|}yPyf(+scl?k6 zTH0*GBQ9F~NEe5g@p0BO4zPPRE-Wq^W=e12FLo`f88#9)FNE}Nw*-`6fn5XSB6{dU zL)lcytA8NhN_^bk4^?9U69pR9`ErJJkBoF?T{qUC+QweBM9-+5sDlA1TCX1?P~eoa zDJ_k{X-lJ-&qeGhXC^jgfEB`0&c?aGz9Zx&_T24GEJmp@k<(lcy%oo=DgRQfl2Sia);&?4JuiWgA@ z*@wXF^CB4ALlh>&=-b7--)pHyvLTm;UNVI|sQ)UepJ1FLDm0wSb6sO&a(p4mDBZ_N zPuGox#)b1OD&+Iq*CKXlD`X7~O~3)Fp{REclSS>yi9za_tz5e$r>`bz^!}&Na1uhZ z>QLMvCe(MA4iFCguO!ns-b1%T?!a@{Z8we2VKP^CA$5OF75hH7`Mce9VfWDyO`_HE zrIW^*`ha?rYRvl!a2p!8siB1a;$crjFebCbPkkksK|%=iM+uJ1>8nFngoG+=7l$D} zpyEGVI(}dj-_gEEJ;70qyVLXvIbD~OX85)%RpEprZ2oBP3JUTbk59(Bi4UHNB9SER z`|fo*OJ2OJnOES6V9^(ulqN$N=H8Iq`1oVwzO9^5`vJ(imcspv>2g-npR6R8Lq zOYNA|{)QVOdnyTaioM51^mynOaKKIiiEUsYK-X6D;LzQ9D{8vV;+*{6d70aDWd_S6 zKwO~Qu!V+`=NanBje}SFoH9qwhF!chC!DgDm6)|dQwQ9X$u=)2?3p+~+!3P4$4#wH zldKj}_+K9S8MfU-tSpiz$8-U2Q~=th%DI!m@@CkqswrzjxuqS2Djt~}b)^j2=uj%l zkBw|gmxNmJl2eT!j{#uBHmW)_dp=wgXaNU&UBNjW7h#pQxaIfS_~*zQl&5fwUfID9 zH~f~ZkQyiV6F4Fa1oO(;3awf#ltc=2--5j7-!5v~yTYou|0@;SZKda*=)80u3SpmL zdT*o>B$6fLEmZgR+mm5Vy*4G$w~;-aQWhUy7T-q#8GU8VoTItmxR!h-51QY{T;DI= zdmpD6R^{JC>SqlcoU_%;D8NfHSCo6Wz=r7LSVfS?|I-3ouY3YVmcnI=JbGbIi(peT zoK+WxHJNYd6|wY^r9nFbkg;D5z%R&rFUPKKx0ESo#)az*ES5rs zIxdJ+!b{yqSl!B{n8`vkk%^tMUBG?mWPqeB_H?x0ullLS$36vbP>&3GsyT|J7gmUC zd9+hK9p4M;3}`aZ__=L>xROGRJSd+QUxs`#_@pOD9U0Z0#=EczPCYrW(`UJH+mwNPtHady))2 z{mJV$)nN+n^2cN)^VNJHBiz+@*F8=UeH|*0{BcGQWB+h0Y6rRT2CesFVus z1d}=C=^ZBI_AVJhbO8lDHG2U@H2A7AlC!I-uUGU;1Ql}32u4e|w0cNe)GP=xqmyAu z1i#b2yK-|?AsBsv@i*)ZX?NDB>1`svWRVsaV`3QaC}{2J90ng=F}~$Me1Q?C<7X@z z!^F|=xz#HC*lO@SXq61=|8lb`w@ zHC~RQM-SUhMSVuu&7~z14tBPV4O{8FrbbSFuZdUv%;l=bqC8WZAbGCdfb9Dh$JQfy zKfoO>CRg(Y`eFtktV2DPv9UkBQ}C$(W+r3`a97`L#6zos2%cSly4Vz;!i}7&5>j1{mwK zH(%rHg=^!vc$Y}R5f!GMY~3Bw_E-a7;S{M5{eyh4dTHd zrxA>{R}`1fOFm^CB5|>#D{dS`jlw-J&i3z~+8=}d?F<#}hT&*)Xi zp3(Ho-5Eqs<&4b+Z7{}1RIl1Xw~ldaN?AoW(ND&@gzv`P6zJ+y3*2VAhA;I{D*;sM zYUt~CwfeMlkB$bW=a7N}+CssZ`5tn=FR!+MsG}dx+pt!)YEpp9XClT%2AXEd<`6+N z)FGhnijtiDJYIKoOyA)FZ?oEmrRj%i64S(%a`s~xNy`F%7#Lm+EY1ka@6T)9%rM4X z6jLU4j=h1LS=`w%`O8%f`}0HpM@5tb$}+w+y}^Yv9ex_iw>gC+X>=)H?nqFX;hnp! zpdxiOhFB+62rXlqKNtHzSzMR$KoV->(w(yA@)XnT`$Xg0T=A6 z6KHo*bRc+;c6hM3>8=Aw_!&k^8JZ$gA4?rf_zp3dr~ViFxJrhxZ$i>F9MuKsH#xef zML`Xlc#%S$jY#x(4J;JFF7v3>vE$XM0#VSg_zw{iA3N}I0CMC|aK^BLBN4F}H#e$4 zw3{_z1lt2T#;AWoW7_4{fnUjWGHI&p42wH;0CgEBK%8L=~g>DbeMWfr~qwBE}kBI9K;Ge%Jk_W|hR2&GU-eOYIy^}xdUk4I{&MB|4Tt|U^D^EzKstq2WK0a>pc!uz!-qkoLgUa6*vDZ zuc4vU|LZbT7)g(R|Hlc9Klq2iJ_G!?RYU4RjdiR&{uDnqX|$2$1V@$nu(K5#7Uq8x z_?z)LGdCX@>?pt#yn4FFB@0Az+>xN~BBst~Vv1n( zM(^Il2wwUlz}$WAW``^;JRB&8h_4Upsi;P1I*40%X-!VF*vK}P%(K*#bn#T^O`~f? zQ_OtT@55`dX(NF(`3J$M{|H;}ZhP>D5bk5ExZULW-+*zN5rTpqW8+r>Ji-?H z%|HzGV==Nt&rud@uBq7IJw&AsthEbjexnv9{nP>K!lHP$_GOHi|lKz+c&s;rSKM95<52Qf_2KFs3F33@fICV_;A>P|JklwLA}m`q{rXqWJHkuh6W&Ep_A2%Niz3e6e=lHnzKVlsI3fW7***bJu?F2( zU`he1km1oJe~gR1y~kz5#KoILeSKf);vjZ^?*Xzk|10a^a64Bc4`4B@q1P^6zfKZP z3ZOM{9tMoCp>L_&dRSV?aQZnniS8_qd+>-C`vY9++MGD`lH<-_i{( z&{thFbk=`ZoC)Z4`p>{A*IB147;+z3Q&OqufvPFtJXk$v_UR80g)SU)>exXoJFXi| zdBr7(1_wrF=38t~tDzLq=zUuKbx_)}XtP~Nj>(jJ+YMM8!Loy~aQ$BlNyJednRnia zLhN_??@)5?wv&-zwk!G)X(WrsM#u{sMe_m4RtP*Y-&KvBR^kK!BK>O3Iqjq&m(-7l z%<4Xupb;wzoFCq7N?cWE5wR)vVd zui*00yOZ`wsKsL-LqEb{q31DY4Y8yU#vs~g*X9>(>V65#$hilz=@iqqSYx;L+U|m4 z$7Af4ZYh{>Am_X%eNC9{rC-bv@XT0Fz5Es{|5vE~Lm&TpA@x8G>-i}T*SdYE?^<_( z&};M+m>#j80IHe!*cHb~-ojN)ayDq==wmzNO$B zL%;7+EYRq#y}zV--HrPK#ve0UIy+a8_n501`Fpq6{xeVt;iKew(sEo zW7z&pYUufn0Dp1%6)9EZ=pEr)OmdB@2~yIu6amPYoz&6s3=Y&pVVvew2jlNQ z3sa5*RNnLK03Vt|ouE^s5u%7-_+Vfj7EK;jpG5ty)0*Jm5>jwk9d+J?Yf+%o5KyWqCLV{U5zlC!k!~3 znL&9%68aGoUb77f$&Syrz{#k}-qehZT5ZgsIN?byyex;g&Ua&UdRXVfZJJ znwLEv3Hmp1dfh}W_Wamnv9pL0i)mcXy@vqYq#yX180#EvX&fptSsWX>$zWTN23X!% zEuFzzTyvX`aFrnZX1UJB@1BoHjbeD!8{f5?P#UF-+6fy?GlB6%Yjy6J-deolb^_#Y zPqPt2S%W@jt+>2-1q_plAB>C+&Uu)2B&}%7xv%5i6lhX(0*mH~)H>~G)}B$Bby3Uw z0XhLD#K(FGQPie$7ygk9*3@7n!M_g)hJ}Um2EnD#4G@;^=yo4RL}+n6bR<$|oqD?J zXdn%gYz(7NX&#LwN>yEoR;cJbI%c_;){{vMy*v{&ApH{;&yo_F*<}XAH$WClx;+S} z6L?p|v9P-Y4QiSGY&f9hX{wNuj5g>^m`5*hl2idJ=BgA@s;UtrATrY|jjPOLm-dIJ)Wd_R)o>@a^@@h(;!^TGdxgzla-5? z62U8J0f>s})}OKMvE2vt`Y-%|lUd=07UoR-h9S_kx#oIBgO`5n)>mul_=cS=089ly zX5VBatsw z-0YYs3mO0^ZGk_lp^@L%5@6!_gKfppoW7!_?J5W+H+KuztN^X|-dg?wCG9H!xP+Ex zPbb7Td;v3izTGVvQ)`tqP}JdHdiUm=gA7egG75?nyX1x|cIF?n9+~<-q&(z84rslc z-j~V>Qs~Iowg(8P>w<7?Jip0j=jnY7JSN_vMmquU>vl$WBnh!y@NC@FOYRB&p# z^y}bAi4oH%KoS-nIUS?81ASFEy05kmglZUDg0 zd}V^e@qUte^*Le{j`;md0g9w*Vp3(KjDi9Pd$hX-P>fzjF*aB_HX2nlxF!kim^c=i zb#rf*s!+`%Y-MQNIk>ePJa~9Hx9E@%ArO+XM1&73Y}0-(lQbR=?m3TAF1>V{%Ln|T zud|t&Wyb4pMoHw1?3CkzAF5d9Y&4GyX(UZw{1*pc{B?S>i?^sEgf}mB)zQ3igU}@8 zBD&J35OU`VihkuGN;er6K;K){?NEPIv(}`a7DhdMyM5&Pjfj02Z9oTQhTu9p<$|8= zD3<{L#U~c)kltVfN_D71(dy5$I-$feVef)OLIGAJVcdrU6()p0BjI6XC!HGSd8`f0 zM>taUOvaM`SG>2WBCYJrLnN!3`@R>nu~ypid({Pb;{Z$qaK8_5_E?CR9K$t(Sp^1+ zqy;wK@rsS-|1JvFyNL5K{w zk>JP6ozxco5}tjF3N;@uuC4h4K*Q$(Z#aPH)!YTsVNGj6)9f2>1kZwlC7^$#l0`h5 zWU=k+-8qJ?Qin0gvjNZ?0^t2{<4v{Jp+$|)fTjcvzEU>>e!PPJ0Y7o*7dqWNA%LSz zr6`k@G1BIZ#|@s^38d}u8LbD*<5q~)U_|UNiJ9cAB*Ondv9(O<^C7-qXDu>_Fr5A( zE)UWbP?LfE2yarQ0c8`5zVN*^vCo0lSU(NM+DtP1nSQj#ej{PtelxM_Of+8@oyezLZ|5|ES7Z>tP}}Y?{(idkV@8Ue4TR9i98v7ZJ6V zEHSh!yvz#O{aQ;3I*WW2REm)PuP)*&NS;V7#dA>LVE>z;afiLaU6rcMX^oMm_3sCR z4Klc?KGy888Vs#3 zeJ=J6E%?&fb#G?gN100`+2gRkD` zD{n0PRFuvx+X-Hp zUb0}m>*fp^NA-Q;rgb&yAqT=(?E;)v-zJCthGwHVha1_VM^FT^ZWl9i4)kd6&koJa zODf(t-SR+8s;dP>-t7={cqArAKl=fm#sKW1(Znh_=JCZABG*4U2Xw6E!8`Pb=jrNGgwr zw$ESa)?iQQ7otPgUB_hw=irbD{JxHAY=UtO9unWxRy>TWPM|s|U|7R?$mpNm_T?le zKebQpt1hA-?X6;5BW+$&{N19}wJivB}>#j7S;T$ZuXMsxHx)h+Br9 zDxyq0GTT3lm+*UvkzwL11OyK`l#7^Wu=&1NrDLom(BBr!pFBMM2J1vrFi31|Z20m?zPB(UfIlo>!EgAMi|X=1Q27QBS;(pDAVmx#f6#N$eirv1s$N|n4m zQLmLCesmWlDVjB{Pt|%b42SOZ9ThX~NkdKSF73kg>!S%4EJr$;?{y8qx>kzS&A_-U zgbL@NUW-6^kruL?5Y`zZT+kV&r2!J)C~~Bc-et$PoLC5k2) z*dVy>l)l!~C<`C9WAtZAg1I^^EXhgzx&N0bm@6(;mtLR;Im%#E*=;{Xo3f^AU_zz7 zIoi;@8}%6v972q##K&RykqyaT!T?0MK;7ELN<}4gd_u!$BD*aPkcGoInv9ru>XpfH z@;YCnV+{D?2f_nIR5o`V?aPSaR28eHdMwCY7!hU9@q4pz-BRX!H{1o*X)JtJ$6$Ne zl*3&`EXCPEkcQ zuvnsL)0Gn`rX8m+u*axl>4zGhyw)g?TBnz$e)KomxU}bX=fn$ZtQE-(e$Of;fR_$} z>6LhyvxRopVNAo#4oKRq?tS9S0#dO)0S4%u=?!oRQav9qD{5g-jSn|H2_hft* z1K?n`2ubCu`>3fM#sMQvnFfj?<))7iN5)L?GG&~esMznaO_STFwNfpYjxwX9*vB?V z$p(nXiqOUxW5eg)NUPf-T6;(+qWhMBG~z4u^9(vwO0puJWM2lVqQl}8;-Jxk!)ndK z2+nY3_N*wRW)+vDdP9D>T>!{r{o%+Z9}!43?-)Jfq1DMtK_eclBq!3ptrER$la^9I zS|OfDUW)2`zj3shgicNe5V5M%TVE}Hm0^^}Y}djFk_vtX(|h~=rYRq_+spF7{>q$9 ziPF4f`%SAV!>iJ~iRkGMoAic`+J@uQ8EI=dijXr(U%cJYDV<*Q4^PmTHi@(J$8@1- zZKOXVZ&tVq^~vAlEe*qWk2(hbB#nhRtH{!&SiAkj5ewI7__|%Ba8wu`k2*z7g7JzT zBQ%KY{n&Cqi(5;6Fo*T0@o?IJNbFTFnbpwcLv~f?kw^A=v|A3{%jXy`w*_Tjr}+RV zejkcYoX7h0Tqx-b(B*Y%`!yYVCRGO65hK%O<-q=ev}y+KnsCU@1rfD|#y{v#NSdui zX{_0LT>eljZugfK+(+wZ=3--u+H(aEd4ymI4=!L&qKT?h*pK>_=X3At@=GUb@Ro4;|Nga5Xorq8~=|caI(YJg_DWOfrKEL}+9xPDq6qr_Kg8;Vj>O*!X=Z(x`o1D|_VdAH~ zoc(HVVz%nXAv|xr6feET+cVaWDJ*QD!SxIj1OzRi z6(02YDy~i1uMr&=i7Pl7Qo>79W=$b%Sr{D>o{!b%Z?{Cyd+_0orOt;O>oxyc#eZyr zQ|fp?wZ>YScwZ86L05S7V9X0iYV^Zq!vLWnN@Vr`wLdZY&PQF!llVoNowQl`LB^G) z9|qIcl9_WX%c-@;}=}#izds$p}gY^K%QhNr!%`?V1tD^vQ_&Psd^Mj7i zEhLn%f-Aw5v^$^)(=)TZs-lN0%6L#(Cfqm+hm7>o`oJ&YQshA;5wllz^_6nNqVW#z5NO!_S>MO^#K~x%nkZ6KV);l;@H{+l>(Lvm`|Aoc9JW9y zS4Z~=$ zO7hwvxxwH>hc-6}E=j1vY+uA-PAaSJ{!J%P9oZP2=}vQqMWw&tquF{R53~_gV=4XEy-Bb`Vfn%l?zL<7{HW!^BAi9POK@Y{FU3h-F6?ibh9tM6`WtZIq~&58^yy&!iXCyIrZllVUgDdI74xhf#8L{-DXhB zNm$*RVsV$PdFw_h*S3xerXS5*fh)e-Uz3d|KuRk+ubf9oAAf5+8YFK-K}QuuGRF55 z_!*Nm6G~ckV7NzIl6X>YcHn!KUvAj?Y;E-tPG1UR*Fi~q+*W!$>}(2+7%gk_*7vIT zBJ#4+KwQ46{@09B=XIgz*QW&=GaKXiV3?fG2x4U@ZQr{d%(c+U3$y(FZ?K!Zgzf&l z*Wc&pI+AO89Q@P`mO!MJ`t`fbBZGB4w0)ey%oK9@DfQ*kvsLPv{Ifv|@5DBVtB2{* ze7;J7A8o0~m;!?}Scg>|HC^HZEb+G%2zNYAUw?3Vrucwobte`FWz5l9d@7SQW~Y-= zov*wuTwl7Tnx%GKw;vJJWy!9>FBLPgAfYsy_^va}>MvsKaW4Gxr>Co*8v90hGH1?; zNVNiq=dZj~dh#7XULSW+%<__JqkH;bBXwZRLSp;Fpua3CjpSc$+?y1SFHiQ&9Q_G< z<>t%cy3njPd^MPd3+mY7h}sGvl6Hmen~#!Hys#(0XgD9xs@Qq^3GUw~*5Q;-qp7vG zEKlMZW^zUUIh=2)PqY~+cYaFp}oft}0lPQ;wnOEQkbFR`sKMirYn;?AQBjXZ{O@Qk0I=#cvPhAepS)5+PPRct$6yM6TOC z^Nq5A;!m{n2X4MdN9bz^YeIt!HD3k)B&gNS>-EmoPyYJrxt^2KisMmVfzO&d*vgAR zD7)P|cjcDX_i9kKJYhM!%JsA``)Y-UScVK}4=4&w*Sn8I`XsT$@~d2)U`MX(>CnTrM zL__yn{_^5|Si&tCJR$L>My^k@7xz6_@o#f5lR2q&Hfoqf zO!W9f)v&9d=*?;M&bt5Pze?iFtX$-|gluIQ=~=GF5yfoZ!}NI*iSSxJk1cvH5vEq2 zCew_cFcqj$>`r+JN#}nmgq_DQ&eu*=ZHeX7)f<{XZuj)@on=p z@5*M0rIq`8rq}Cj@pMG+y?KjGvs987o*o=_9?rTLS+sj(<`_y@gS2x44TurGwXmbu zhVFEq9g$O=K}-Z5H=%EtP(<byXuZqb`7g&8vXeb)el%%qi{^j)4n8#FJG8+^tS zUYZP2BsFFxpV1(73T{pn2;&sfM*A5Qv)-_(wFPM0=p6mS zKA@1n^kYmo&4M1Yl>QC>l)U?gBf=Az6??X_XbZx;F6_OsE$okpYqB;JEBcdf*a zi^czTU@`r2`Szq1TKVagm&Tu4^)w6B)BlemKT1 zcB?(y$CNPi3S-FhFV%aN@TxDm)#?VWIWMx2FH#59;Z6JCHtqZFweCfP0d|N1_K?Xi zw=^#EWEAXOwJf%E>YdrY)+vYx7&eqlNJDz-;KQ4BVfXLujJ}H3jkvm{y0&S=#+B7b z0u0)UGkSdAwVxoDZ6y1~py)YfDdI@>)vitHlJm-}hwrWv&%zKmE(ZKicsGH-lArXn z&(juvg8j?*573(7qn0)bd$`A1FELA^V zCD^{7c&shH2CGam(-GXvJO&KNdg=}_lDu{?&>Cp<-L&owlWog?o6zv>@Y{s~=V%fH zg%_$4D-WN0Am}4COb$>9)5BF7z8=3JK#KCmeN=jHbOOnd@tupkWwC!w9kpA0xi`xw z@~*M>;tP~tm`-QF8Bp|08Q&jkS-h#>Uh+^%z_P-^IGw;pj%A;^zC|aTxb^7#F!sa) zs$6E6TveX_N4MgukI?uoK=?aZ5v}7ewa?w4DYeQJA6EU_16QC$*)uz`U1VR+^`aZt z<);^sy3n~(XT}je$Kt;gQN)y^x2h-G!g^%#Yj%=@%dMrWcJJ-ya4i^u_Ju=}hn9Q) zbK-?SBXWCA)N>QsCA%V7l+*mC@gl*0Jo-srL^Zc`=K0UmK*%K>w*Q4%$WYyrCj*Clf4SA>fiycCNzvHOt~dSg5X)ABlm|qf;#-pG(|E4(K+y%^xW=a@8^Ti zn7}26N~cp}%kH`q%LQFb*l?^)GR7FTN!BxIQ{@T<<8J8K^*B|&>K|HMF^)^)?EO!V zI;}b$5^IVJF7cKBg`=KLL}WJX^~yNRgUl0Zbuw`Iy*r%+Qt7u&qZt{l-10B~1Q9JDoT zu>Zx@2{xwebe_pf)5&;fZVzTK#z526ZaL62D0lAJGUrxD+Kq5+6L0_AOucZpeNpay zz1H}bHmohzWpBsZ<_{|)V%4wf$|6`~RCh{a-4H)!o7oA5=V*RkWsxegSkp>;0G!<_ z-;BgV`+ww=%h*mn_Uu3cEp9W!Q|F>N4(axfzanY{%{><*fu~Jj{iq*6$Xn(xu#2?a zw!a5HyFPwm@#iX?UF>L-F0tecp0xd_*VYpf-T*h=3fFDMH@<8q`B2_#a@3ZBotNlo z=6yHb%=?<_<1so@f{>9n`}BA@Aku^)RoUG3i9JrGc$q?zXmcX+)+wH1C+INSecF0U zK411+?P89j%h%JyjVxOREtCuWM*4%kgP6(sdeVOO42$WSoAJ-8bKI2FyE8t`oWr_L z>-VjGEbS2VH{&<-rHg8K6kNI2mtUI}-#fpDWswEQ66h7X3X3p!msb(67baOr{ zwF-l`pEs}^!)Y`4E*@$k$!BafmShF7*^3WY*ZK5jW+UNQF^S9RBXK6f2 zx;HPPppmu`26B!Pk=1$&zpSJpt+({P;fUnJ?g{kd7+>hK8sCH!q|g#jf~{)`<}GTx zMU;wp8_yyQJxZUo$%SKM{s{{)jlbYwTH=CwJue>r?pc722kGuT;(N2@;zpYlOYocD zdev`rU*8BC9>@r)EN|)S)NzWc)$YJ|q+tQdBtQR-lB0G%;nRo{Y-T?PZ^c~@k1{Ra zzI#%GA5o1`b)P{TZZud^L{Jxz;C0$w3>XW^?(`ImD8PdgD2W}7&z3$KpZK#@!BIRR|JC~?R;9^MY65?Dg3mJA zz?V(0s}D}p7`Ma@eHk|TwvwR`=2r#Y7J<|)kS>HvlGhN7Kl1^gTBq z&)mq#uRiU44$jqDaPyq5Yk=Pa5A9JJe~vi)E;9JuUQ=$9__#u)VooHU@ZF0I9Bif_ zzbwS-2yU9_-7kza*^tZ@r{{_2(mBlI#xyknyA5Y@ayL;(oZLR%gE1#Ck|egToywoS zF{zq!Hv)%8x>5DH7ax}A^fDiWf$3D~Cl61nLNfB+Z-VbM#Rk{CdS-K>8eH!cwmQ*M z__-pKAZM#g4<}mj^{14M*E8?tM7Bq7pA=k9Q|N?NV|`$EbNzi|kF{-NVoAxnBNmY1 z2EDGCS~t5cat(S?>vD9q&-Y0AAqa+Bv-Xg_>8lGju4edGf}okC98-{92sft>z9yFm zx+j-G{#6R#qd3Cf=1#YA?R@Fta-fg3xHV6w7R{-wmSguxLlxPzYa33bmXfsQ^)R6~ zM`C@TXZ9Y&7`>xlP*}ufw5b;T-pJ)WiDfn~DBBKW{$=5bro};Z8TBSibbiZMDRPY< z2xYw8Nkx=LHHop)X9`!7=*Em^rcCzW744LA<6bTDd$43^99nk#Q8SwCP_u0vt#2~u z{3g#Gaq{rJBm@y#?s6#(mg7<10&Hf$wbI6-&~5385L9nUH=;_Vn^_cR!luMIT_a7} zZP9m6aO?Bl6R8TS769pUvo%*GS{T(jN+{$u!-XO}ezT2ygt#-&k{xvQh+GQ3_=qoh z6p-I7y;H2FWeg7-rjdwks_9Qflm4|~LF6q+r8qXvD%w*Z6l>pdn(#IrXSZ8Mb$)CC z*3Z3gJM)S%Q+B$5B$o%Rkow}vU#|YB{NP^v*hz*jy-4z(uJEGa8?Iz) zVTvnB`@-vM+3Ql|VHHV^+X}R604Tb8R>EAH|1N#r<04`pXFRy?4n|(l0M@T8*?RWZ zo^5Rw%o-Q7oxZC{&z!_BwY0mRxSq3_AhnBrkK=?ql^Zr>YOSA^>Z^I{j_0EkQdY*? z`#Pd93^~Gi$*dZnpCEf&>rJ9(mQS?#1Hg7d{+XM0Uk@E*q{lJJJnB4VV!XoCA%4BM zf$JJt=#6G#*5U?YUQq&Ib7S*Fz>&B1HItR2`#EqE6`gk)Osf#%V}u^GI_K99--Qe zjOkuTe>+P&Q^Ko8s3V?ftS!&%(YkfPy}(=AaH=UHxAR>c4~cd<&uM==g5&wl889eC zKAnvJXQaxe=$Lhno47}zGpZ=AWeVf9i!Q@Gu4|1K?_L0=h40QyCXSCex|qv}kJN zszIhjlk9|s$(OdD{psc<(?j49G7%9++vL|coGs2Eesrhk^Tx?beAVc0B1VyFx5BrC z^t_*BcKMVcsoRfcF6G^Mj}iz;rr#m$1ne87Y|2izFkvB;^cbxr-X_47JW@$J zMyGA|@i$0rFUQo+A z%n5icXrggadV)RKGd6RHtMK%okv3Oj^lXI@MHYKMQ2l^F-Sc;*Q zWV5jz1MIB>r**Xi{7#;mYV(aaW#?iRsNP9PiI`l%pph_KXm?LY$T`$2F07~`{M)^@ zNoJpUltP0#jG5yiL}T4Pj4jOxhfh+w2ZjwCq`f1W-cO*k3F|tSU-~}OzIDttrHnh6 zY9H10`V^PHcIM9d&zxbY;e6{`J!ez2&0jn{QshjR#v`#pl{jJW6~yTc#%ojbD=u80 zfGr-*mraWI8YMxdNBD@Y`5@{(EWWwYOnmsQ*5IA{Z)F8saQhg}Sl(xvych}#&ovFj zi5BusIxTawMz*Y~mFf>lUo-^M**B+Ep(t;dkA^@AMvX_&u{h9vqfT>B9vg8vx~kjf z7&k99Rw5EUI?fC7JuwP}<<0?In19yJV{p06p2eT|#ul&52_SJbz8u7+kWC1hr^*6- z-jf6dJZ%#R)9jM*ra(`CKD<2a5N%LyOLp8jk;~uplcpShxy#@7a4l>|75ZBB^;n;dBGW-84oa;hdfbY0 z>oG83zaGtzvb`ZRw@x8@6&$tWzCdzK|_%X^7x&kTyjK_gG@ zS2rU^IRkQ>towYJELnLGWxQPxdzre{CmhA4neX{HG9IX-pxVHsU%yn$C$tex-^z-l z!sQLF-S-+{m+(iB|7n61k9(kS$aoZJTnRiHYBKvy?wzz!eJ~V{lFmuuMWDbOJnQZb zmo%c`BlL)Zd^D4fH>dg5Hb=rY^L}d_w@rAKae9?_g=$l3Dw6xdz}ksL^O)Uy(2>aR zS8IPp7Ef*~8f6av3gKR*_GR7R(|er22K;U-I$hI(kUQo zIMZt3PL`mog1)4>FBz-SE#5R$zw8O-hpDW2=ETwdWX=TN6zghUDbj6t4rjpNyIok_ zZjPrym9&Wz{ONn`zEZQ`)#Yc-w|68?$b*SBk zvjQMC8x~T&WsO#RAvu=6XKBhRl;kAMp|F0vN>&ohZz>8JIy)CX!h037NRKD5cV&fhj+Tz)_gq~)<`Lk~_Mz1g2d$$GO4Usz9xOd&Oh^+O zY-r!nVq^#J=+h@K4CR_Ux&z;Vk-EwMX;=fHBzt&Yo0nmcN?<4s=e;tx9`#)pKq}?? zC6bB5s@I)LmyBUt7FFXxX=ZGUxTj33*Gi}H{7nk!Ekv)KZY`!t`vFDY*Km)ot0a|; zEn1s$U2$)?V9munGc@{>#-*@CQmE=9GpY3GMq$3miz4m1K0hA@jB?pbp|Hvrob2+l zQh29cQpwm=Lde_9tv)(4C&lV{bHVL`jNFM%i{Y$%smWQ$-bgsvd05aTwQnCi-$Fnr zKt*%!x~GnN&gZc;{53mPq;~pbehbyMUSm-YEw~G1%Em8yQ0G|1c%(VECy|$1`o92<@%;ykvC0S-99d2IoT!JHHy^rS!f@O&>I~ zO(fVPtN&E<-snyj&z$QfS%L|2enmyEX$7ni7Yc9J4c`>X7a6It?@Ci- z3ZM=aCXuG;JI=S|a#!G{L|$T+?V4p|#X@{?w+nb2-r8OnPqUw8-V-+SQQw?0EO5~u zXZ{J5T#cU<0SZQrqB!GQvFM@%kX6b`##j+`*pc11lB=H!l#ZI?H{T&P|>U!VbO1pbao!OAGa z8o%d!7bxAJ2K7&`UF6nMml(4g&3n>J5S`*Z4vMYp}R5?8O(v4E*~3f!9c zy|g(ql=ne9p7#>jyfuh-rsk4zbtlqkYe6oh)yk1~iAvNF?OA=Pddn}5T!ZYotqFJd zKiNdMAGIz1TG%l{Un!>y&YB|X8*T$uRiE!z<{F$?Z7%R^WR=G`i3n~wmJF)5zFhcW z=HmLrS2gj+_{tte(>FK=R`h0ocKQo#k|Uj_&qtf`PS&)kXIC#)*2CFJp~8GD>9;6G zQFMtWyXL0f*lBI6W%*O_PI|~3jCl*D3-7-5BR$QrnL}J ziXyd{nhm}z0dskLKx?!o*-&lXWCMp+RR0KNQcgg8MSr9%-@FM4=3~2Yd1N9bIk!Dj z#lCn#1kT)etSm3;%QpuU4&i$PrnGGno5;X}Ud`)5w&H8QekTZ`jju~8D<1Bg5=Vs; zV~{vG(ZcIwnH@BKYMru9JsIqeoLgNm6-Ac2re8JZFFnI5Y!bWLk=0!3PlePAO~3ul z)z^55{autqLKGYi)CDIDFl(T*Y-GXpd{+mJg;J?#~vxCuMp( zNFWwlCT%Kki zSKFjaFS&`%a#rW#GLHS%u0IloV*BTuOS_s%<&L=RRbLJHqP7=(7+OS6_xy`3l&H)rzh%Fv8^KNKw< z8|sWUt&h-VWtdb|4azn76xGkAPRN%|IT)BMS7T9#Xf*n88Hw(8jc#&8hew)R(dyR$ z(Z)+a+mXDdtnczg(DNi{Ob+IsU(UxmcGvF(*Rr{>djyJ+oZ!!509f3ij$@`W&ezL! z%=uBT-v0sgatj>>?(l>-I&MBjkl%yIJ)=fO#PA)P(lpU93go`yTSi+S-SgoV1o13X zmWf}0gJ>tQ2O=)ZJijA{Y1`5Jg8#yWM>*HIHrvQPvXAoqj5Ccvf!UeF0>^I8pG{t^ zAEN|{g^fp<1o{YK3%}XrdJ?_*Oo94S4#CV6z@Tp%eEk(IwA)XY=(r!Cj6!PwE|^%v zghXN#5a6_BIB9XC_0P1F>BEY31-}8^opLQ-Y)eSdaqh;~Nhn#w<)NSih)L%37e4j% z#T&P~IaE1Jes;TE251tMK@Rl|&j9Au=|qEf?svcU*ipHLH|E|ypQHH~WNy12{poTL zEnpk3GbbM8v>G3pU{u)dRKN=G^y9>Pl^-tauBASAkoq!?)|@i+UpXi%bT}dybvgF=1A3Wwlh1MQ)vw+o z9=xH5x}V5viXtQ)0~rQ;6*xQ~WE@>atHvo4*@&dFxcNqD9Egb5!D5%Cthi56H_2UJ!UNH)M%DVih@5+nY%eTG@dOj`fyF{ok z-g=JqC={*K?l$vtp(K{RX(-p}{aQ3HzU%&}G16qnQ!#!g7Z8U-OAJ)=JC&&&`1$Z_ zeIl%O<7J(IJJQqLZWcr^`n||-+R1LRyi59_(M;jn<$#}KEr zLd&N^T+);V4LVC7?i|Z0TTw}a zM0mRg+2ak{0Z>{vi^P3({?~@PgZAty!G$Y+CSHVGxf2q^&2M5KI^a;%La}NSWPw&D7+hQ*YE*9g z{RDT)^qXrF_wl^{c&p(wg#lPxubTOs`90ZdVzKP&Ao_rgFB@5spJpM8IUZ5Fex!wv zFL8FO$XjJ%>1`>*a(DQ-ieUo0-)Ok8nQaQeubRGIwvnFjb)6THdKGIijD?f$_N#;i zox1;6P-mff!zt^^#<>?PG=aB|2*}NGOCz;kgql(QM4RjM6?=Qt`D~C=ntd%^;^5dRk^;j*kDrUK zQ3rso)|D_$W?9JS2{>9O8`4hCneIW?APyy+d|X!3Tj`R}!p)X=N`9zRJ+dyc+T-sF z(u=@fw<~X|$xEU@&Hds?rz_5I6lcce)J^`*9o{^R#w$NTO4pAL@@s2>MIO)f8+l@A zs0rE@AiMW?Djr6?IvieL2mS<=w~mqy*)8+36T5tMuR{NNiq|B126@VZA#w8VLr(_1 z^PT#5VPcee(lKf^Ly{`Oq*SitvEHBn<;btRF_XXpaxU-Ukjt~NWY%~bZZGr6i$Y$B$+urg^aLJpK>?H~$sOkt4}49GJg8n3%+HRvJ3WyZ zg~Hx#`kG+*EjlhTK72P|Ku;~x{bYciQ#@T&{Se8nF&gKBu4PbV?=bhKG|x1rgSk-p zLVm;Gs4b_jdc=J?KEzN5O2cp$qCWj`^(hf>D;pTHGkA!r%39F<+eb?^ubS*pl#-sc zj?ZLJ5%J|m_p1g!p;eTOYUpCM}}Y1s()HJ_D0KzAm0;2at^M2 z^f~Unq`zls?Vw-H5m>@mYEoQdB)IGTY15WrPqeeP;2!W~{-w3lq`*T0Z+I`aEHP3*m z&n5F}GKFebAkg{k^=423%a?!r&;Q&@UYT-& zXZ4QXQxu={Gf>o-G9(H`o3@6_42e&W)Z~H0#Bq=7BBF62{z!Zjw0*1>d7q%?q{O-f z$Aad#h;-8v1qJei!ga#s6tQtJJuElckB<1civjaN+kkIS$wON! z2^f^|jeiHl%R~5X=wTK2LyvD>`d6A`B6Vv1$+f>CC%nRzo#xt{A=6&8fWkr5N5%lv z%OYFt;porpo%Dx7x0^&2ddIxa%hcZte?_tB324TeHE~#1=;GV)Ay7@rD&Zr#9b9^r z-`C8T;OQ+(A=a?q&K9YOL6i`|AZRTT#iGp>YlYJAF^><=MX~x`jVL54$@WN)wbj;o z7?4%VV^yo=l-%C)htBk=roHO<)@d^T_{Bxj+O*$Age?)Z{xzCs{!)G5|&9+Rtv@Tx5wu1-{?Png;&5NJUyr`WH~w^uA6F8B^hyTIbgLw-})ghEel z6MxJ45M$N#RrM}c;N-+v;B2s_Q>jJMbc0Ph>@o5%tcl`?;q?OIyJ-1R#{e3icUzjh zFbvrpKp!{@m}T!)KI@=ruzpZ1Xf$n5b7H{@dhWN=wR8QE{A1~won3ie=iahe!0#_k zS^-B9t~Hg{)kmE-VPj^q(Z~0d7N^(1!+I=*Y5{+?(2nvsoZELrww|l4+RHq9E*l|K zN+jj>h(+0_(z=+eckQ2B37e$)zXgr8gmD*#tDuvvpLG<>sUOyZ_D5&xPXKOgQ^<*BR!IF%lzbTg3wIj(wKHd+6T1RGCq>^V--8t7ykm~ zckB}8?=V9KW@WTkMJbQ2Gd+goWU5(;h-D?pfuwG$QxmWX(Z?>ZS!*JZqN8>_dB2xy zo;zE()6+Jwke{Jic3>eAP#-|ZTi<`asNMT|AN=BWt*TEu{?r)FPzS%p3<)-vNsZ+3 zTKS-8Qk=E9(Hdk4S3k2o5GDOdji8g>*5B$Wka*;%nm~-bEcFBZ*tIT zia?bab|#kUvr2(d$Q`3Bu{eOEQs^kkXTG&2b*+ki8I(ngr?X%b?TO`%qsg4ks9dx? z8u`WdTkf+-65_0BaY+ku{zDOs((`?u6(WUR|4H)NR^8a`vN>rstZ3pkWs_1%mLQ%QHW`!1@TcjJ!NiUBr3HGByv6q8IBrytpr zi+PjW_%a3x zY2jubK67TPp{QBcPg7esVw`#=#_Z@JXoO`6k)Ec{7bd;v+XIyv%+dg=NA5{PhXiUj z7^9m)-o4QUgv_0tA5XmS*vK^IM(&i7nvIF*?m3fM5yGlg~F`63g!^0x}EpTHe7;r@UW)G*5z-_X0+uK z{2D^GZi123BlT%=UvT~uy1uw((A7vR`(^W@{O0A<7$0b|oBHN)=~MH<0aU>^vkZ7; zSM%)pGHz&l4G>!aXjr@Ea`n|-DPV!sl5CnDY^!8;%k+@;&1;{vDmRYJ+qPwveGYi# zg!+;u)-Lur;O*TJb{b{D^(>EsuXA*jVU!zxNFf;kb2^!zs4tpkx8e`?LiI9P`KZpG zNM{$im+DWd1IpmNGZpXshoj%84FM9}HbnzJ9j!v>WWsGGUGC=8&jf81sD?oJd8t7X z)6VP5PmfQV)8>Ih!s`T;W8^?MRC9jU<>=tNmAL==coPHnhH*{Cw1=x-vV@zyW{OYO z{c%V})n5Nu4_vO{Z7*;#A9~u#(2BS|ZT%2yub$9MC9Kny3hv6c`(Sy!Ed+(W<^_M5 zQ?TOkMmEhEpm{)q!Z|~2LpzNaQ>5OVGD$vCUu4|j=?gXL+|9q0d6XXwUDV;PQaYSD zu*^uKS3b;Zs(E0-YDoug>MkINFC4CekmoSq#O^`gOGF`STzb+B{nc8-Y~QI22MMz@ zX8ul}|AH7>k>x}NQJ0?g=cqvvs~V+0TOT%q;<6Rw$yC@USK39mms(Ghcu-z>lk0GQ z-X&B4B3jQLQ2btgizA^Pbko1saR_4^j#IR3I(0>`hw)Twau8qt1=? zx0@E061tiy#2Pz5gJZU=vlZ6J8NvnA_xI5tK6>qD&vg-2N{MLEff%5gme=K$afn{D z(8N*(^)DpPwYxX;+$!$R%bGrrV-!GuY)G7)^S9t)v6Es3A?1CtgvncRw@@ZB?XI#7pwUE>=4&IWt@zNlV^Rlwj7i6dn@1=0&YwoI5;< zmB?@lc;bTrIy4rvWsL`?Wo9rjN!%V^y*HI~vi$b2)-=|LWlbxWjD1*74^e90{)w79 z5(&ZeJrX?epshJ@E$zCq_&f}~Ph4xRtDdX<{`uFh4t8Dx%(t(>lYv7{qoppkRxiD_y<;8JqulLEAi22;^uxU5%#1f!(1(6YWS~x zfNRj(rh!XP$U5#>HpP=;pW$C}KHzQswO^5-M)vhBkhe8I^Gvcmra@Gao5buQ;dRuK zp`k8*4BJ$m-}`^H>`CdM-)3ybo&($#%v{#On17un3 zDhPdF@H)=`(<55qaGq?TpSlM=U zYkd7<2n|7v9kj7>&}jW$Oa43G^zmBp2hsBKBC+wzT?M|SK|GKTQ{L7anl@3UI9rUQ z&qMqdyBnn$XFs*7<7!$Spj?)ko5>c^|IRo1`7NFyOJ=f)gAE)%CL@!lNb%?m3h>n&eR#llKk zYcH9rQy5n&&Ymf9rqu-G-4}cup;J?}6s-?4#=b1@`(V;nMRL*b0%RoKJoPjD^JzdC z55AIx*ITrg5k|;Xno?psn`YT(gLM=RC5Mc2c=zr_@+XW@+$1`djG`p-3k6r<^4(bb z?@O_z6$aejq$T`_vc*tQYZ7G^H`*wmQd0QC32mzhB;KQdKO6)vR0793V9q(@89i_> z6uSF|lPzlJBJR%(5zY}bAGVTF+-d76a7|;k9huEkH{X=dLk*{_+*w(>YwvkYT@9~- z+FfQ}y)_wUBX*9#$n@5MVPUK;Q_Sj!ARV=PUeMicZUc2ktY|$^eQ2yUYiXH$tH&FFHhx6 zJ7-qn`Il`e;2gXYf*iY9$ekZ!OdJ$$?n_ux`hP(rpbzbykPgLAtfJxF$2Ld`=DK?m z;!A^(Ry$5o3`8cd|8bfjL!H1~fADcS?`Hb+F?8`a7Eu8QbDS-*kKp6wHlNY@_liM| zaZ}F^)nh0Q!Zy2i<|lzLK4kONHXNWpl9TM@pL^B##;|t%_60l>Ytj5UmY_owS=PUQWYy7{AZ&UdRF&$-$> zhttM3oyqdVWXjO5-_(sWkER>;Su{#Emih6VXLJ%Ln2!X^NU?8kn`ez8S?=-RlCFf&QoewX*_OkHlMnsB& zer`ov?Tr7ZS%2qZJ1{1?b70hXP+Mw}XeBsnrM{#iMN@x$)_#qsZO$}C5wed}oud*=lUI{b68J*%Bjw;oHFTNLaJ zdf-maA^UJ+H4IgQkI%!YY7+E$>x>xO?1utLl$`ORSK5NZ17r~7;m*B*0sxaQw3MLo z9tsN35D9x#;fGVhcIBKu9vzfL`%q)^4ig2y-iC7R%6s2kLE&1n%_0AxW8%Osed-KU zH0(>7OgD>k_g{HJT>iaBM4kJmTOpWOq;b>Z8jR}h_b?y#PapY$Z|?Ha)d@H*X=9m7 zAkmVDed+1KpllDa+w8@w&y!2f379~nQ+*+c!7#4J*`9kDs%lpj5@N;OuhyOKUTo18 zLV2VgFASY6J4Sbb=U;!vKbws8*j@jkc8S{8owFSfX2mcl;GSIH(%zqH%8-KhlHtr3 zu{Wdk?Pr@St}63Zd?(K9P|mQI+S+xD3!9?ke{yc{0(KPx&cnK@ery=XoGQ>}2vLY` zA(TXI5P>WHT|BVuZ#Ygt*gQus0a4;Y{K-Wu-XGT3enuUw+{7&Yo*2h)NaM=3dpBQD z+iJ21l}qFFd^;U6>TMu2`7=yvEv_&xb^VTSb(g4`T_7&&1qJU^$~ z5`8c&17aeD?h`&+ghMSQ^B*G)HLJ5xui#7iogm`sp3gWm_1iO?0>FLTvd~ zPQQW*z%$r_44+>(o!(Eh!_{s{m(&hqxL@2`Z^VH{o0W~?=??2nbdQTb$H-d_ zt%sNOs|QWWgc2A_Om+VhK$qTT!_>&w@P-ao9&S{VJ%5Zp{_(gd*!WiF+e8#>(u*wr z^n?_AnT7b7OhQS;6lrS#vQrJbHaE-9nc*(rFxjCtnh`k z$oLH{``5Y4hxhiXu{pX!=a9pl3*&2 zc>m)^uz+<@b3guQkc&vOAN8BMldopJ5*4vMbQE=`PHMDY{UvZ(^rbJmE521k;22&z z%`&7^cZKbUQw;$kW`Z(qmWbSo8r_6n^TZkn?)m8CsjQP6q>O!vQ5Dzz8LF(1Ot7%f zKIG;S-oU|88CN(}-$>L#!JjFS%e&D=R8pLWJ=snGqmf;rY}{#o95S4&q0>?_G_Ae8 zug?=my+t?)IJ3J25xECsyL)821!XrR9XOY!6F%HI{$s!Os7MIFGmx`FL7@L!eEvfa z*v5Y(@S{Q!UsFg`1jE!Sw>^0iBgl5jxwm;a?*cwojCvgRl)d@AUCLFb(L61EDv1-Y+Xad#Vw z3pt*TGA9fDVf9E)3LnL(G34i=QzAbi2g{*A+HrcKyX($`7IsB}uOshke)xWj3R?L7 z1RIoJW}g*n@4l_zqAg6)Reg&v6{unZ^CNxj4}fie@pARZVbAaEK%q>}^I+f z$!iy}wjIXWh|Vy-Kk53%;-ngbTRJn~U(MV}4+yZFoSEXSL=CBNw8DK1^wH{csKTun6~T~EbZ*i3CwQ(&QCFE$X7Tv3u$w?H_nm$BO8mT zFx7Ouv2nYkKX1w)29=mg+F!o6oldP)J=!OqxDfTtD#t@CRGWo=&fX1Rd4_6V zgt~CM#qv)tz}i?s2hze~3mAjCj3Z*cxmw;uc&<~{xLTi~cTb1NZVcr`+X9# z%M(;Thp$Lvspc7C;`)i1heQHdWge(cvNwSF7f3JWhqxWYo6Z43C>3M|8uMks`z&Fx z+D_jd8zRSl=Sp;PoJySprCpDdSC18V;byF}Ro?3g9+>B3NL?*17AjLIfaKJ8rU)OZ zVjqow4ELJMOvy!$pHxHZ!(XvG8@LsRou*9=ET8*g;gzLF;wLnx)8b|z>Yrf zJG`{oRB9uKnnr<9yfh)0pfNMV4o`%1)4!k@t>#@_s`y;s`e2it^a3R3?~yBH;4<_U z>8^UQ=^^g4jrAT00l>%FwvHQ*iU!99UJ3gCiL&9x>#n^kxF^3&XSX_;Qngpl3zKeg zsk|-K(fg0IyF-gd)MNfNUSGxgM3zO9G$it--Z5QUXgPfn4Rf!UqiW|<$O=djdFQCv zk`+~%EOp+ z2JzAa_UI-2*fL);hYJz%gl6sKgR+kxl5*VXgX-d$7F2$(yn;oTO5H1x@_REAL5I7> zr)$HH9$m1h(wK=$DRnEUsjX^E^doW;2^+q7$bIDYw+R6X$B-HMc8|N;;|F`F#fAQQ z)LVt>l7|KC77=UMFFWR7iuk%zFsK%J0HzhuYfky9$>3@f8~LvPV_G?~tp0?vjWEhK zs<7Em6Xe4~d{q5n4>r8M_YpRopQ_>5Ny|m;cl*MSvN+*2F-q^CT*UmL;z?tDKacP7 zeD`kkJrSBILZ$##p~?2nGP+%Hbw_bwQl`Ujn#rcGtt&^0)O@V{oOerh#bkp zR9*KIe+U6ep*%c5Q&L`5t)J$>89os~HF1jA?!w@w|V-Odh#dP6kfsT^6`*8 zZ=F+12BkHX(z&%P#^L>E(7EXUi}cy8k^Q)+7BD5KAFgd_ZSBvd;OTq!wJSkfp6Rpq zzCkE+OZQOGybOH!hrn|a!}qX`os*1tk2EMXyZhYc^$sfAoQ%tWrpP{K4VuysAg4$G z&ukZ;FVREYif6>mdeIi~+n=bi#VN?)pWSH^f~>qtffYph+p5|~8M0wT3GV%(u}g?` zppdXQyuui#tG?;&%<15e22Mb3klxbGtA9cgqevv0#a&73Xeb4&2Ws&F+}@rd6ju?$ zvty_9xI<=YYu-IZL$JY(&gDa|+IfZ{C{y4XkKf|84zkN`ewe4L+;Z%i?YeZ{l`mKO zJQx=Le|F%9thb%hTib*n|8t(d!6n%98?fF`*S!TF%i(zVyKms{wXPJ%G_{U1UO7~3+pHCw{Pp41bR}KuU+e!O?0L)@l;A(>{}JLd z*kAt7bl3d-j>((4^UaSsJMd-yp8tEPyD)eEzsf@Y*awoKyibrt=zpH@?`IC7BkM)~ zwdQZWw>|lX|9?H!?1@_4zZ%c}v+@%z5|#X~HUHuV z3R3C+v*uqS|Nlm0vQO93|E37=#^iccVj1@@X85xF4+Dn&$jU0{HC@|ud6X>u#lcg; z|KX(jCNat`XIw$AK0y*zEOP7Y|C{p^qg8w_cEm6{G;hxDW%#AUS!qCvogZ_Y ze59>A>l0!anbTi`kUwzGK!SY#Vd$sUs2IDpyuknqv;w0ayJAZB9g;UYt;lPF5bWLz ztAM7KpE=`*Y|F=4AhMBKxe+gxv}~^m@xBG{mv~O8|3}_!ZV@AQfw_%{BVw7rOL9xZ z&V1mdW{=(VWKh($R|y2O%EcTd)Tve4uCpGUBs4|eDew?t_1A;=;{QkVodOnY{arc6 zz_!+5!u7yG8RRjrTg$-50B?rC;UnL?#Yt)XKeEivqXQl;_z+V2R?3<9pE;u9bdTJ* zm)HK{=wkDL@6-Pk5X}}_;}YAuIm6$Vk#=)HI63eEVtIKigOI|M2^{nSQGKG&lNAW~ z(df#FX0*K0hhpd&G~jECD|cWe*T_de`d1bHQ#^8f`@b}z6Gh%ms!8!;y7N!g$Q^q0 zeaFKY)bODn_?YNr2loQqWM#4CRbi8#mHzyMxL(v;LG-)2^-lJAJZ>TBZVf64B2#Sp zwV6%S6wufpg16mC)VTC=?SF01eGCgQZcj7TX5CZ*SMd z%g(GIkZFtv{L1inX8L%){;1S_&j1`h=|62_f;^0lCiirsBkrEB7#p4k0XXysS{TM} zI{-2l{S3^spzVICV5sY(&(x9Xl-!QsvE`W3IoEyZPfiX1N}QfIeSY_H-t^T8$Nb%Pwko7MOY2@$+eJka0&`+WhBUOXpPr;0!%mJ&0=mIT_#$&-{CEPuJ|b zx%FkwszKS}Q%lW_cpfFzC*ZLZG2rzPd+<%a460k;8B)pr^z3@?NF8sZRlK(h`pYES zIR5hmLtM;^-Jc8BW{>m*h)4LPJ;(H->Q9XkP(#%WXwLy)4&dp0-iTF$7orYrQp!}j zFSYzCDl|=>eciPFhV>2Wg~X-_5J~xx@^1G8*$4%q3lp&li5sWhjs1^Kb-oNMyKchL zF0z<{rOfhwqBO@;kYzR+p+E`{lxS$o#H={ru2|^M?LV6f zQmJbrJjmZ?=>Eo~xO{eb`=#NDVb?Qk?yA{f3YwumiV8|~gU1NpBq+>o$?yq!z;xKUI{n4fA&%Rp`Bt|v_dh#yLx)+i zs&yiNkQOS9TG<_>kVG^HwvjIV;qj}XE^Kb79XSQ`k58*r&`;yG(|sO|Nqcg-tz7o& z03RF1tEM1=^j^FIOf1dZA4?ipf*($N9uj*HJ-3goh=g6joH4&? z-N1Dh)w>K>s;v}7_rirC4(4(q;KBczo&G_N$Bh7%dvOW8?QuVbu_M!9dECD|_PNbN z^Yi2w4v^@Ygq6SNfQ@h==de$QE1wFTWV394@u z3p?1amp7L9|Jhjf+~)F};}^f|CNeZ2cNPU==21dH_lWh*V~Gfcj^Up5vF8guKuzI8 z^G7**86y1c0%m4WL+w@tgTceUkg**lZergbA%-4&2-Yn?o;b|+C(h9pBnVRG5%##|<cBUGpbKSZtDnzOSISi<1BHGFC zRidhPQ&hAENS=5SY{pQ!2J-btq5@2l_H#z>O1~xd=2Q2V1mV>!BwStmc(8j{i14IW znu@vY9=v0?6D3>>PTIW^cln;f9IY zYy^)z@D(TM^Xk{p&uH?BbSjfdB(`@Z{c`w7o(8BSv#PvKy4jfiMJPV!KNEVeZSf(5 z4}NugBPF93Zb^s#@YVJ#Ud;E8+~}PYO!$z&3qH1-0B{TD)QRYQBcr9VF)dLyd9AMG z&m_h9893LolxWIGs6`eee(d%A*vn>Z-F=>)y9ANmxHg}5dU?zsvoqvl+2DDNk(2OB zKA9-_c=5iUlsqomXy9cX1KPEqN2wot7uX+r=_()=@TJPcx~^!lx1aKSHW&kjq&1nK z&I8uTDkWu1LRE`a;>d5F*i`XZx}TO^)k-q*CudVd=o<_N8#mT{KdkU4l8Z7Gm^byW zk3IlWijg@$`yKvujb%|+i<vPdFmE^kSM5@OV;!rD*k2?Ykb0YZeHZPwY{d%Ry738WDRtx7Dp~q z3^P^h0q~j<_=X2WQoaNC^3cuw#YYG9xd6@0S1Nbzk|!K@>{74KR2YD#-YiANk@Y*Q zwxifnRpLexwr*a5ae}?w(K(6ftk}lQ^34{d(!v_tILSh6r`HQBM#V_E`gnx1j#6c% zKd+oqX~bb-RbXzuGrAT03F8Xb_%Tqo-68Rr(AJ6%Dc1d1R)@C+ZkVdemsqOLQiJ zAUZ*GqD2tBMH$gMA$sp*jBW;F#@Ukhee?TzKArR7zt*gkvUYi%`@XO1u6u8<0V4f; zg?JxPmS#=aDdQ>Lo%(G(qR`%r|%NMJX^tWYw{&0~*Z1GFofLzAp{~ zVwxsr`LY7)K;X~MV9&msR9ZqtepO4yziR$I@Z;%mW16b!FlpP_2gLb(q?57P!R`&f zghor+b}Fh=8N?nt*hdQXhwB^Tl}P11%VNd?+45SkNL2M?l71!>?CXF1y}5}^Rvu?f z7nMaRr@>fgkx;7F5__|ZEaTnl@=y&=|8QSok<$y~m;lmf{8vEpTeHqztPoISR~X1t}+l?4Zt;Ci5O@#1|&8bB)K98H^}V;WIT{5 z+W&J$>s6G?d%bt)7*834==ok00Apd|y0X!|+HDWv9$%CpLO;Jv&+h%Sg*D*m9jvr2*t>LOZXUZA)0Y#Rb zL<8aY<0_2wUTe1}wn<%@J9`Yj?;1(BiFd}tF&9|L(~o!CH|&_1Lppi(pe25jYre7f zd4R^kp7xsmdq2-rNdO)?`9ebdy}FC*BR`%#`|>(o(7$Kz5~F{_gc}bkuc#fEhb7m0 zu2M7)u{95EeAf^Sj(b7<*`WAoF$2${6AlQ>*lTL<7vCvS2OPzp_$tH4A|2S2sLQSc zo3%9{D~|WBo-THA)2FRkqUS9nJFEbtB-Alrs?%chR7=>x4l}6s(3phs7N}k-e^GJE)Wd={&Z41&W;fRk zocCb^azGnf*pvaooW*vIP}P!g)f2sYbZc*u>}7GNy@ z<{T3>o{tt;5V`OCpoSqsBwh5h+?=YKY)Jb)5uU?YDYhb)G%Y8y{I$-BuLc)VxsD2( zuVeWCxD*XganiN(fdc_r36%J)(CAns8R-r(gH_7L<+aFc3D=yp|Rh^-pKMo;h2m7tiWr6}rtur_1KPQTAc&&btwinY22gV}MJ1oG%o5>%&rO*EV6 zEloi@vKqs@u|l(7SyA`42Ho=h8B!FvP$q^P;t_K_q#X<|dc1U8Vp5-1$hvc?BBs)k zJqiCYJ!fif{=P(;^^xg-nivIp?n1h$)hZIa=S<^X(OreYIGS6gA=bIL=SuR{_8u@?QshFQwQuaxzj6aXIWZB z&lvHzMY{3TP&DMG6}L_Yi?CJ4Gb=gZ)NX@;)s zA5;OKaDSif$~0$5)v1-t!sZLF$Ycbu5F@MVnS9P(Klc!M=-3 z<|IL=N0@s7F5Ou?UlpY;;}bu-;0y|Nhcp zq#82P#n7mKd2@5&j5RbSD}Q>%aU$&jm?xG1~!(wz*4%clV>_afVu4=}G!+VA}%T(fRnTck~@qELxP7Cn9F?4zX#5^*$2 zvCU_VBPx3yXA9ZF(1HES;-PxFZ^D(|aP=a@bRIj_)c zFQ6mlaA{P)^tV?B$7cpMhfiR-{IT9MYcixQt(}SV-oIC#Yby=ChMOeuK@COpQ(Gy) z)ecQSGvAuedRkZ<7vqHAVxZoGKX|UYaLJPn@wBdznkq>KMG5{vYy5Hg&JB3KN?!q* z3kz*b`6eprwa(Wz$_EjxrUjDgswkbBxwaKHBjdNjyRGbXb*U$cA0;oPHWx0#+?=Fs ztk4Ma_;^qh^}ko6l`OGc1WdZF=l&h+9h;fDYkI%%0^ImrJwDk_ni#)>)Kj0Wc>ol~ z`%(XLQ51<$Y&zd9`zRTZ^s2eR!peW@h_*H>6a>y~{q3dz;$uQDQk1ddu+wX6#84TSY* zcrPo0MP84&}(ncmgUS0)MUUw!E4e?|yNhdz$vH6(LC?14wM>Zq9W;n7)(~_95*eq2AEJ^KJ-zJP7^qk5>SXDowqbu}-8e-KGwffiAL>PwAOhoDum| ze3Pl2Dlo+Lu}#3N{g@v-U$wT0)X;ho7SwPO4{GN4_w=4H!`G)~V;Jj~`XMq)u2r2; zJknN{+j!Rli*hB>PAv6IiiARm2X>8(J&-$o;CO|&-*yK?Je-Cp{yIgYUj`}OG%bc= z!D#G`@2kni^ZTB^9%{b4S=MxAl1E_B3pM^)Y zoG#sMHfsiL7k2#fU(HmXa2!EkX@`H$Uwcw~6rY!@d1xVh?JCE)`n~)IK|tW6^zWMD zLAJPm82jo7LF|8Y0{nye+CSF<&W*zQ$FzZC6yE$Jec%|De+?91ft&vV6u3OW|6Tt7 zH^PZMb^i75EWnkH|4+?-q;MWi5L8B)@IS`P2n@!&`6D3!T2{sD^kk=J*|Sb!TOQKK z-jTxpVPI5%f*|LjB_ktw*_#$~U{CLfLPgx12>S0@+eH!fAty|@ve?0;u>t$Z1Qi!S zg_K{Mq`66?X}rw-0xt6lIb+vbW*(0BQr-(JUD((lutL&vaC~IxA$sh_ME?jz{x52M zUdg_R#eM&%aMq!!p+&*hA>6o3MM?Wr6kBO8@TLqla4I>Tga-pD8bd$hx3nNJ{QpsY&fT@_}{VZJ`B~AU8)ik(p2{>=h)59L9%r*GLl}V#? z9sQ83iLas>1e}~V2FYyI=|45Mv|U8))gO0S&ov+(CK?1OeLr1eN-dQOf;)&TBZXHY zK2a?cTwt4nhTX76xtPH5aITg&pTgF>ABRvg?tv5J_P5|z)a zs%`4oIH#T;6lMZ{7U{lcr_MW(`bpdFgWHj!O;ht!%-88?+c`lZyoG1sp{W2?a zea;Zlxh=A=e%df;qpq&CCTad-{agfM<>nuy?i&S2y;?+P+f(A%uh+6lf&7YO;Os4= zZP`P&&6zr*i?MEZ;faru7$RF8v)9XxV&>MinI#66h|_zdSzaM5>?ZlM(`$#i(DmJu zJ)1J_IIBAaWDM5319s^FM{=Zel3<&yPv&B={{BR`35ZDu@^_1p-1bU!V5UJ<<)^&QNC|oz3Z}@&fVQg zK}g!G+}@2DB3M6CzXd|`rc+{4uiif+5ryP<{}x7hIe<3Q)iX4HqsI$brGPykenz7< zsvOdr!t(Q%#ZE5_BKX%j*+rDTRVgP<{3IFo36gVTCy>b%13Q#}Q^&VnyNSbZ=uqC( zPlXP4%}|W{By6maoKzg}cWA`>_j*TO_l6E{0D>sOg;38lts3bNUaE5t- zIShqtrb%B&0L78dF*Ty-bNqDwBe~Rsr@?vYK@2RG{ZczKBTJh~H98ir5LAW6zM)9W zIfPft?Zdh+@(dA^<$5_JbdHt+Vyx&a-vrM9G))01gN1`j_#Woso(j^jveGLVfYn6O z-7Vxd7Cs{Dh`&}|^j;<`j*2CX#tx9e#xE00&pJ?Tz?2pP1+o;0Bk@8LNOErB&{GDQ z#gs0|Oc6yT{%IF`0SO1}|+f+%;XI`uVaJe(}B2V$qr6-2)W@ zCF{3+(4lrWAMgd(`7nFo<8P(mOx-=;Ve7-jptBG<*2THfA+H169P>7 zE?$HS4tkw5$A}__%78M4q3XauyrqzNg`1y0mTbyoC|&W7cR&{PS;X*X>O1ORQ4TC5Obh6ob`8O_%v(lw=1$2VO3s8HcBa!F1P;yr$`Ixt#ba59m9nX zko8`-Ui68PbL$2_h{CB8QvHb>?B-E~e~6eS<(ZyOUA%=MbAoO*ME{#V0RbeL07psp zr)+?T+vd}Zn)k&PF$aqcueY(A)zf-Y#G=y!5?Kn3^Q9&}^`MTw*#*bOvptuMcZm6U zZ%X%V%xsYTN~U4gV`T=<3;)eh`r_707Jdgv(f*tGn(X(~ncwAZ88Yu-g^CW{@aHdA96p~D6nsP$3)fK*YE5R? zKUukliSCH|Cs0AZ?4SxgT0%9e=}QM2Ib)*N0!dIY2~duI z+3|@HtX*l4k+8X#GeK224LF2i@_3T>^D(!u0V{gBZe9siQmVy9>2)0h27|!;Ndc+x zpx%9wH>KrLYVqISR&z3e34_S674q$!{>vrKmzl{~^lQ4-2DHQrjUUP#f$niL8{WQN z5k`*91okk3(v>(ll{A5ZKPcA>ogQ-mWhSt%Lr6-)m>@f8T;6kY5|Aw*-PDFLrxANK zm$i-u)dge;RzlfL0VP7fQ$S_fMH4CFe(6Bf8>pJcQAho$(OdA_S}EnRT$&? zs|Hy#ky^sTLmrQ1M&3boH!>$bAGAM-sXPG>A~x45)69*ncGGbSAAnXbRJAT>Y9o9{1f&+F%0Yar!r2aq8pnNat)xJa!Upecen+kLRgTz6&cYm z5P}Hi#6QF0FmEVY_R1hvssyeIZwT0ry%rB*DcuR{VCi6v9uTGdn;oJ}T}~fjuaUQB z!s;gD=3QLo&frU>WR99^jr0yEv$|ThrIVnnI+e?qlltMbzhd4F-=Wio?wvDK&j)=C zc)4{M9D|}|jcE!02NhtSiN7)5|7EfOSpL6E^IJlBPyZxc@yFGFN|;yw6vVD%KoKhW z#C2T#@#g?(ec|7&hAVjgzt&qUdP+lP^oUXxi?QppwVz>f0d-Z227W%SK=4LN&dCwR z$6lzB$4E{90=>kI|4vCY`_B>5>4DA16h6-cq=IE`5+6?GQqH=sP@|`=W9xs`zOnjD zE`#eCp#+=Vbp2IB6!>KVC*(a=k&5q5mr8Sg|6DlY`OFy+yk50YFNabB7dY*Q5}Qph zA+_yUb82LQ7CX~Dd$m8S8AV!}@-%nUT20_*GZs>c==VCg)ggtB(0h72i>42u8WLaQ zy6FRDSf70kWn10mdb$m^dQj$^cPL)!9)16HeC&-h1pZ*(KMSb_obmB!#M)YQgyr2M znz?QN!SUVAIkkgkF+gq5r}xg9yG2^ag*fHxW!L(ywb35ryK6u)6>->gzS~cjuMyx1GnaJ8@7|fL zPd2ftfJCkT!cQ+l3xPc{eMXsY2!;K)ZhO@$0wTa}Aa!jnkrk_JxizW3WwNS()G+!L z&^NxzP;X*X_|#rAM}bu_Y^5rK^I!dW_@T%l!KO!G-Hx@wU-xWdfN)+Lrd^bbl5I zj#>ExEh{LA!jUG}NspdH=l@J`k-Ki?4g7rjx=t{gaLix$byQh4ssT@?{ylZAMy- zzR57M;t&b1P^g?aBpuR}+&NmSu+zJ0djmg7HXPq=_R`1+xv9m-y>4#1vqyHFl0Ncj zfD&C4ivv)#73pD5qk%8S7NDI4)xBq_h|jNV!ip8z?&IP#^>3tfY=egw)mR8>Je1%0 z!KpLNrPI58lXFUp-@0E%4{Ac|m_Z))2}wxhad&8NOh%pDnN{#} zFdh?VCvtgJ2T!_*i-d<^!D@5e1*Ss?{u~>Er?1T(D>YBa{6|RW;Aud{+ThQlN(DA~ zW6xxRg57l=NI9Zi_Mc_Ig#*NtLb;xOX0;F;<$TqP3;z6QXZO!N<2|k{QTv`y*I2G< zUbjs!_Gb&@c9Z;y6}>+mcBNc? z!_rf(XSQqts`>06!5X8=R)l%S+c$fsYo>Mr4LK>x;A1SYBg$5yA2Ln(CDjEoSo5vg zuX@&3yH*ACqH;f{P4DB1rt8~;>0!~-OqSpe>h=j^_(bMm#i5hxGDWXUYq zcF;Eq@QKbUw)f68$-~EXQUGO=dm@0ZN&%@Amnv^U=Gx2k7ty*4R{W9%+*U9M8!LEJ zE?Ck8Sm0WHh{{X&kzr(g(-=4}7)C`kc?di>0OaCWxN z_aN3kT2mNi!Nu_&`wd8~zrS_^S7jIo6^|(*_>wMf5Obc4e2Nz;V!sv1K1Qpo;SBW* zNn9a?n^imvArOWD7~ED_82maSijzZMM!uu!rVUu7^ZXe zp@4X&cm_aDF*3^3z~|tfs&##4SNoC!W))OY3Z0E6^SZhb{`;H@*J*uVS2#D29JMo8 zD217wh0QP-0&d!kNl|Cr`G5h^p zdv)riwox`->plr12CIOS?ZL#^m8%di^0+3DWWav4aZp%ev6pe`+ z*ZfkHoMeDMS1g`Qfq#p!{@SlnHOCGaJM+|7-1y@c@0%30p2#ZUu>|KAwgc=?pTvn zcU9`~@Xdf;EKAoP3^M39c&AwO;$5QkhD_)}>a197-;9;^IPI-njGYMU?!8!VE4*ww zp8-a_+D_L5y`dWM1$s4;V}AJfH#cYkZZ z7i`YB5D$i39#=SmZ89OaNKWyilxup;I?8>P=C(J+0{RFY{Li2SQQz1AVR=h!Mq+pET+=>~BkteH521HMaIY zlWN}Gh(Qyb+%o?&EJv6KM(JoS!mIUP7_V&P6Z@eGV;T-OWAvLbLi3#x9kH&^t^F5v z*kQq%+&^{fqRG0q>|@5fr*h%w-lrpgZgTI)1Lb;F6MdjW-{EE8+%&q5-Zq~qk^T`` zeV@b-S0ArCRTKQ?@tBmp-rB}s8BiaU&lQq~PUcVAUWS$J-1A-w(D$~ic0B-UI4bAY z{GKL(w)4v^n5O-8Ltc6Xos7$KLr#D?(6U7f27dYSq|3x+U4{0QEWMi#jc5G@>T+Li#c@j2S8Nxx?yp_N)JL-)lQw+_J8o5o-vjx+Y1S@eI(QDl_Ui|!2c`2eX=*gZI!Yj(NneVCLOL0ex|v`JL$UuM2@yA@}A zjq)xF-(91r`u=t43#W-mT_#-h=5tbu)u+LN_vkO+K0A`qK94wn#jcs52*Gb&5wUkI zUy9#6wb>jvzs*5ksg<4Dx-;c5hS8+MocUxo62Q<(T02m(CM>y&wrKP>ET%vZCp7fs zrT|Yn3WOUkNUqID7W>5fCSFdnudT+P9$Fs0_u~+k6WWUvrL`U0V`8eZHuNBY=fv5oGom9(oW&Zp?dsWqjDWm`fk|ZMq$f{l1M#) zO5rgKB$@fMTxSmMp7swTfA3m=UdC&B{kh5c8{VZKow!&D(zvx%YIyrZ99aGPPZ|WV z1)T8l8{goB@q~yOYT5dF!cPME*PZ` z3TdZz!S)0Wz9@b0l4;b$aLIXM11@J-g=uGr)4iGreJJ=wPUy2*GQ?zuBo(2OtiM$a z?qve&n;6qpHz63dqK>!MfEY=UkE)?%H3psnYfL-Yhgqy6Zw*%JUp8CAZ=@oob;hJP zd5mcV;XI{HUTUa8ggDa4#NzbBs6Ukgv6{3aCDxu*7%-9r zPd>N;pwt^yGm3hoDSLIA;u9e@rhsuD0D`z|A2q&|)wcX|=qkLFwlu z>gqaDqJcjiOTkwOvIUDRzfOOL#Z&$pV44zk<$@RkG0wT&%tTi;Z zUH=i+Jhb!I%A<^^=c7}zjuXR)0x(ZuSSb7L-D&@+S3g5>zApN&9vVKpH&C6$*-t3& z`m_)>@*=oq!BpX51Fv}y;jD16t6Oeip)vg!r6|Eu78B*R_S5^YsrxwYmir8hSMP~1 z$t6=8LO)4jmnLwVg*_!jLf*putzhpv;?r+Vh_ye?0$wiZ&PLBWB?Lrtm=4h<5uD3z zT0bvbdvM!e^1#=B<>b*$I*@$-1y{bYmbY07uQc9H~J5zz0^BFdXi9G%T zs93vH=(7$iIF^)VEBBMs1Yqk`w5e$A5D66;0jp<8OBM2WwECY%l)!sQY(TYJ-I zndP5(?pDO(XH=wNi=^FTjuYKzU{KBL^Sm0WG8#I?Sr%YA7JwWB>JvN-R7_xjUlyyX z^x^|?37cD`JY$C6246uEi;Lq{2bF{#L|~6=20&iP>d)+LmRA!d>nu%6xU-2HKW4ccI<;JP){c!~`eye&k1EkFAfit_T*V3c5@=er!W{~u% zX^Uj;zH0!*ANx#XzAf)<;9WOXFll=$B3&6fs>Gy!?g7lI_1^~soOm~ML;VKL0?e*|4x5{Mo^8R<@{88c zbI1`nEnUzneh!*PMWt5gY@_{oii=H$Q(&r_2k!EIJC_z1t}JW08koggujG&!&|;90JB3fC0pOt-xD@t;5#Kd+PF1 zr}Tgo++B4pKtD)`gWhodOsY?C`%>E1^PH|Tu@Hr6^i&liOAU#o3jS`2mkht340A{7 z7f*aEJnuw@_Kz1jA-9e3XFnA8;XUFd1mUJW6|;^+wS*~gvUaQy;y`1P?UleOoZgl% zJ8o%Qcwoc)dD)nvnd3#?xp1ji}h#Ez8xu~4A~*2 zNWB&*uQ{@G@nIbicQVk1YrterW;!Wy92J!~{Ip0_;Pr*zY8CrFk^D>d-Bz28 z`>JQ@$EGrM&vr(HzTGPA!0&CFbAad=##;@=l6qoN(!L23&(N0^Za+4zlDYv?Ogo{N zh)8DoSD8+6g@dX>UD+SH4jy$`Z+P!{LO{tp@g0PXWPUAE_M>EehqY~Z8~vKE-<_Nn ziol}8&-dEPblYDKXih_Jlw!5aYx%X{U2euL4>86R4FJtf=1mAnQT@ef?BUeyzR_*V zeb?Juz`mQt6_D2>13|)5j+qB3-tRd58njnYPz*a#Wba^GS;>MN2hg};Dq=W8|FSjD z%zu`-Xt?|o11wWJ+Y=D; zB%18Up+0&_`hyam>KnY#}d!l%on` zJW+CK{=%ivgt^Fi@cco$75c3ptTW~>MiS|s8PuxfXq#=UG4Hd#Ul$i45lq6txRv&* zOJm>ed#UEzPkV~dYJWH5WnPD0ZUij!5=4lQkkw(B0IRlpi;?!O26Q7ckr_fK}zo+Nv z>?aD%&`5`fJgMfz66i70ttw(s<3Lg2>D5bZ=_;==B~zZ?CK)U$!x;tl`T$Fqstvs+ zUW%vEx^ejM>z<>QNyHo6+yKDRr%I6Ev=i8V=exm-7R5T=?zt7#yFC5_y+;hZRr{ps zL+zu&Y;jtmSF024v$4q%f)lIXop}I&8y+nPpN(7}eSkouBE*}Me^{YwO7tPcCTv9- zfbVgJuG^MeN6g}%KlzLt=OUw`JBIGtdP#qs=?QFl3taS1wM zutVx+Tx)yT@lr&sL%#|msy-x_!xv6VLJVMUg@dKa z{#_lFQv9+iYSq`|Dfi?lrtFeI#n4~s1ppvJ&Edjamzl@kT6YTDt^5H()TlbeNHcuY z-}4H=F+)2;U+Hm+UU)KP%KwYyj$5ky_A`wiV7I1fiK%8v>2_(Rj2is2ZDX*X>#RKt z5)en7+`FKfiJP&TfV9}UG-l<#5bZkX?B_B*|M-+r%5th({tN`ZZKXp}cP{pJMbIq*@CU~sWcRWpqhae<4`_mp#xQsPkRjO98sSO(j<}WW zz-CP9`^RAIhOSbc%Y^(=-wR9Dm$}xbL1pDaehrSNK87yEmN&->#L#IFtAj&!F5`=^ za0_;4SNVANR8r-lW^_@znvd=OlF&u8q&lxp(9F;*u65*K+Dz57$PImMmtSjfYpGN5FHC+%EE~>Pj%N8l5ztm;jm24w6 zw&Lgz_zxOhY!*GK2+|Pc#ObGdZ?r!=urN~mp+|Z)T*D@T4r8&NXDh=H zJfHO!$I{2i+US&GR1T_tn1xJ(y ze}z|g=L6{G&k(@vl2}iq{m>FoG}1l|w)R&K^Z&aPJJH#NmjZC&e4;A$AuP==KN~U! zt2NRPQuwKe+TPTY{rTvXqt~Z(>u4+V+_s7lraem(&>oxB=7Fo23|Q~Cm>9OECCJ-v zB6HPz<;AyJp55ErjT+`-TV4wx({DPx2{3oZno`vQRYKyuN_6h>q2#c`g!E+t;eSqT zCFGukrm}??{rWj5Y4zHOEqqr9?j$I3&oD+aPws_WZWx8v$XlL?r3kgOS^t=!t$}l- z)Jp^a#RggfLlDKpZ?@Qc`Az^%=PDmsk#Z*3>gj4T+x0PcY!97u=E=>==eq*9f#=fR z2W$!dV2+?-({`}@$E@0f($gfl79)WACEk2`rj~{nTz_+EU^lvhvdI_K*K%iR_mp@C zE&0Rr%ctA@M&}HdeMQ6!yi&fvdnR$JgHNtl)Qhvd05yUM=pY7CG%!i6p{c#cwKC05 zQj%m;uM(<0e0e?BY=!<2gS^=ETyj#BB4c_7#2Hs-0^YXznBN&-bYB6u)`)|Mo@_;p zUj%IM;V;-t8>MTiby!_kieN>Zo1nGk>P^)Yn{EWin z;tkaR6jJ)L@&?bt0UT&p0fNmOdU=)<4-X<=iDgea*=U7p*mW>W+l!bgDPD>hbEBj( z0^n|`zzfMeoe!pe@IVfkEOXhS|8uM>`26^3h#ErsS-C4?Z^WBOcKQL|&90HJ;qU*$ ztCH+Ieys!H0{U))%;Btm3|vKXTk-bkvAEjXfEW&v@)t4MuW3bey|#Y{QOR&}WhfNn znxR!Bg|55M-aPle2uKd7i<-FG*+*sE5?0Gqty8(j5)Jy-FRds}In6wP$MPhmNLBP$ z9zi)i*ij{vpPWj4P2QZ^J~G0rX{0yp1J1VFpg5rwBY}oV@x?iK)#x9kj~vR?Ud<+z3ggv=vbSpk#dbom(|9YmE>Kvq3Eai25iBn z^0cI|d|RFlJB`P;No^d7iqaX8dazUWaeSs2KS;91RT~lEc{5eDei-;ZB(s&wWYNi>|yo9wuf90|Md8qb|*1+N<~LH zQm*VA<45JX*+MoMQat&k&7Dq(+B{J5uU@eKeA8*~4eJ@hT6HqhDls6gCuTBE(kAb| zxty(Wn+kT0mAvtx;3$@~j_d7IvTGI>FPFI&M!{s8+L*4`dw_{CAY3iotfX@rIqlH?wbM`NbvBENxdL3l;vkK zh7bXF#?E({qwV}fb!IN~;`p`n;AI?{yVuU*(_J1zJz)~OWAVo0Mu)e?>}a3qwXnYm z08GJHEdTcX!P$L$I{HQEU^C5Rz01i{`h|wajTPZwk}j$hj=V(&0G#+1B=ZPTECY!T zUjWk9NQ>j?O(A!}6ENaYLY^2 zq2l*y|~ks`!>Nr~PEU=2xw+8cSZ>Go#a=s-&gLAvt>!*$?GuPkNK@iyAW- zlFyQ4m5>krXOve8xtGuQ9{Vk&%>;v0d@u*$){f(?kZTH$bF}eIcb`pE*C|-fkXcU> zGc9m!&v}&!C0SZD(tp)$JxE8tL-U-d3fd5-r-#CQ^lF2&Coo{;Y25J5t5o73fu@B} z(z_`64*{$=9F?pFxmz{2F}UkNVuwngXVXw1$t1Iul3v|sj+Vol7fyTNSs$J=`6`>l zrr+PZp!ug^h}Rg0``w8Lzzd{#@!~;D;8A#_chNgE8H9d&L*4AdcMz7<-jlbZI^v5` zD8;bCxPRj*fO4Q5`L|yV`anj^j+QBqjz)l)XIT!OTSkIgKcZVrQiAkRJ-pl1wtIU* z@W)zQlaIewu%sbm!zPVAq=sGGp0Qh{h_{64atK^`N?BVc8Z%Xo%3W&++*)@Z^zKQ1 z1n|km=dTjFKT4i(H<*W*=SQ#C+ukq%hBm?Q}Q!+>7CQA3}l{d{Li zLKm_J&x0C)Jn5H-9$;-A7Q_h;hdu_tc|YPm^YG(yZM=t>-4TI{=YniY~{k)mgtBWVvA!e}X?+2ddU? zmp6;HRSn-dn$srhk4~QJ>>647I(YN!SK8Bb_s8)h@pX+ji4a~~!!oe5pX_vT1L}sL z@a>wkU<{+fj7VK&K{J!d*K5u@$NFDJt0`B6_RVw9&TWZl4|v5q-@5{|ba=%c$~Q_) z)FS3hTRH4!^Nq4p%dn`jWNjm~5uW)aakmI%X|Jr*>H6!nEMyrYx*`gdyJxcC{X1<5 zCYNjH6IBjc+%88fkzP?)*SJQN71~PpTbt>$u<=(JOTYI;Ap#(mfnOPE!LzW~4!v5r zuX_37QiB~k*l~MRt^k357sr#M4I%M)D6F%Koi%DFf>ll`2q%lFPFkg83Xn8rtQN5q zb)4qze3jwX5XncVY->L&!Ar48nJCHQ06Zdg-Do)XS#f-3*zw3rz^^+>vTp!xX$pDg z^el9db`-h?wQicCNu6$Fc)iw4^e5lK&FiMtx;;YUjz~K(c`=`*pOx3mI8({_0eIKC z#P_v>G1WmHU?I~!$5=JZnd)6y)ZGW4e-`SAqNJ$g;yY|+bE+EEmXKlCSwp+!wqzl$ zbi4x@QprQrlic?W)3w`tm;%&{S59n3A2@PJcrvbv^gf{XPjviTPD^PRsC`zgnZ=k! zb6^A{T?aZ|GmYQBd%Y;y3HqTQ%d+P^%(X&>uPk-ur7=_qX-IMZV-R5`P-`KDv6K@$ zh-ekYxk?JCCHJ`{WBAG*{+nLz% za9tdD{7RvG0SCy3Le)^xV@`Y7nl}CU&q|{N;aLIpm5@hOmp!s!S^fRcmPjT#E6Ay^ z@xhFbl_tS4KeR! zoFSvLkG&S`_l{9Jj*k@V?3`W>#r`g8snSbFtxxIIc4kj@QOVUc$Za9VQV@-XPjbj) zavtI33BVt;ryvXpb5dzDfjYVm#twOBolHTS?BEc{s8$}ap5+IOW0kMT!C{eh_FLmJcb5vWJ%?f|rjPGCPSUd8{426kLCCS<*vZ6wHyb(UGzhUhW z>|O@k<%o$rcSe@4d&M!^l<~Q@ZLTqyY8MQfvYG8|9_}@3#cf& z?|WPj0Rag?LP`_~0RgEYM+8NX5Rh($lx`SOx=~tMQt9rF8R?Rt8OZ?#7`o&4@V=k- z`(OWOvD8_v#hT}yd(YWt?|tu$SQTLdxt`Na7>(<19Qv##zYP_NkH_sP%e-Bz9T~2; z8o&69F?d+OB-&O_O;=1!w+(q$2DkNn%}o@xj1$Mq_8QJB5?;}TFBD$!fURWo1-+=X{eQ7KV9Trge)RWbR?Npf)l;a{ zVYy1l!(G%0k!q5wg;CDTK(!#RzVT^DEOs~a|6wt&*rw}v;%@718=NB!`TeMn4(x+qVr+3Q;#?IgaC1}}^ht~^ui`yUjzW2i-(_n97k^nR&}51;|dimo`!-r1?Oy{@{N zwmOlm(ie-keNWSDkjBJ>INkfLOCrR#SgXSd%%@Anj4uWS} zY@PR?ZFT25F;t~He6XUMWkNNq?0fX(J&aOMsL)ObgC=}#kgiCEyI4CTw7-@APdgFn z=b?zm_RAWdwn8AUH$Nka4LYdf68$+--!#a1ZyHv$3JL>VB$k%Nb#Yhl)Eb+>HG7P?w>izZ-{m%=q^oW|ipQHPKVT(>_ znId_EaC7shpEtC0iaT;mjm9j?FfreM!8`TNh$aFg7YdPj$ePR+3<~Cf1@byz_o-Ou zK&Ch9Q`w?74)L6E@49MpG1%Cuyrle2SLS5>+5Zf}_s43&Sw1@PgTnu6cHGur$>iTU z(PA!uYc9&%-yL!hi7IJnIOwZ%5R0kU@OXK-6O-0(c~Ptd4p?0Yg#?U&iFqr+0@aX(tBamhC%KVK7}>RK4+7a0RK;N%Z=EH z1@V%?$mYKczbon)J7*!Ot#z*-H>{g{HsqS(g6(QtPX4h%mdZ6e&Y4c}A>{deb&4PU z_zy!9SZevhY<_S?DH}S^)C)};*UsB!>bJoSdD^L^NQ3`aJt8=xOOy-n@o4+E&MyfR zjKwfVhvG_zqKi1^FJos7^i~(Q(ObnBA#{SND%&J=s_+oN&*vIlHDOeTQ;r<0dBU*r zG9sHZRASY-luV;MXMyI3Et?1XUPl(;Yd5!4!;xol0BJ}Np5RbjE8Oayv9EdFve7X( zyiZFG)RKQl5WBBk<{2TrN<_}M68)(iJ^S%gwpB=~?%%dFm?n^CD%!E+ihCQcBqeg?7Wi9x1h`fNeRjQg; zJM8tyXMwS;IDw$Pe9Y!4|BItIJ3w>u?^@cHRY6Z!kop&HiLGa`L$));O6G`JkbZQ< zcaPg*285EA(43pz0zvEKzHsUn5Y59%xW=Z_xj*x6Q& zT9476TJQ@JBuM{cdq;xps2m8PZBA1dtzv_wR9bzj6DZ9afxV_6$0hRy=Zl6~oIHa{ z`ZLnVQ^cNzgJegd?}^Oc-+}QuP{c9gPP$?PHsl25VDG zO`3|21Khjvuih~N%(zcmSJo`^x*O2mPp*t#BwSfA>9gm(wsNu8Bdod+zRT?3dY~%E zd_jKyxQ!%1pC7ETwkAmqtcuXHSWyl9#EoTnwc`$*G(Ub}k`JMGCdr zSVrsQj|1|Y*f@FqUNGs1Hfbil&HL0{YXkdix-V>Zx@E9|*-_Z{qpK>4MxFuqXCtD2Mccgg?n*zU#<6UF<6t;p$%17tK2Gn~LUc z;W4Ub zomooMr`--X!Onk@QpHKiLQRFi=rZZ%vs?W2AopXSa<_WXVAr-yVr`wKT}?al3KyZM zdUuQ8f-T`>;J_#A1#$glHxx?P_twWE0DE!IF(Uw_U0)lNrv`l|tiz0O(hYB*JCgeb zQFWnI8rZ!)lQ_|+JKJnBZEd|uw%f6(7~di5c|P|BtL!h3gBb(2wgf%O&++3sTsU>% zl2-T8z64!-n_ULxuV$W5Gnvo*la;!aSgaC76u->;q2nt6jik=>~Wy4&ifTTTwdXYmq#&9XfEt=eHUg< z_4`r3ML&$1y%~S6@8{vEU?jFk)9jY5IG~W+6&bYYY=^r#eS!hX=^!t!XRNU)k&Tuo zyt~|Y4ggW9hc8;JS!9U8l=DAy%j2xSldPVd^^Vehr2;d6y>OR8Y`PX=Z0B}@Udq*-wgP)SQCe@vu6O2O zI&KE)m41=R@6}x~s@YnTkW-nD(=}eQbW0@A)2o-j27P8(dne&$U0%J{{d*a{=Kr=j z%~{TeK&zdnSqdoov8q(Q9~vZjvu3kD{c>8GKl!c&l5#PA%&oyJ>l4P~MUBXl;dMn+ zYsh`;zTe}Re!jxX!_%@$%maF_2 zVx|T3(076y6TB)SleoZE4t(W4lQ zuZ*X8I*8zV23X3}IB|0YAhR9f1(_>;v^4e1qcZ{0+Q4IX9%&&|Yp+XUGoAG$`f2u7 za#KY^->(y&{$GS`J%g*6;;Q=FNjt~M*7?`#7MELr>kAke@YTW6;4PP3R*@rks+=}L zLo~JFJY`*DaSZ*+46=~_N_xTcg6JoeK$2qg!K_nNvncqTc$uZOgjwGB5AgbOJqZT99f|fPf(#nC$Qxp+zH;+W_%`SE>F*`i0Y8`(X z>AD+$)vCT>EllAw%t;BvBv(0z`c~Csfk-*yXD6%W3&ONSXd`*SE3|#NR#c3 z6VET+b<Cui5r}u$IV%tr) zDPPz2a-o`v{w&lUAd%3*TLf z4IS-SD_}8R`rrv^7Y+&_hh97ZO(&0lQy02jw0uz)j`Ie+$u_X_JdtC2%~U)!bG zH;w|T{g)^v2Rf_s{P^MqM_2Xrm|e$YTa!H&?itW#7^L;)?s6@Zi(GM#04|{`CX-P*(}1Frk0vSFM~-YV=_rvAXj4Ze9%)6w8ZV zS_}5iFk40zlvo?d=2W7Nvso!Ju0U@>8DYtXg*Wqq0y|z;Gyokjim9Pa{SI`;H{gNv zhW8DElICt9q6RkxfKt$LylkqD0fd?rVYwO+)g5-a;UG|x>b(}*xDdC}gTDx{g|!6s zZ`fpHMXRIG)vE-=q?+L8TBObBoId1WQ4cyuzd7zS!1+%G3EKO9kEp!DG31tU2NZ+K zt5ZPv_}=rO&D{8y+4hH@i?s5~E1J9K^McCMS=F7D^mLQYh@~1!Y?gSsj~!PCo&$+I zaho&{3)pT^H~)nAF6V4lKNWkTRlWZ8FIGGgog*kfN{3EP@qDDW{Ynm@#=!zi6u?ga zPdhX2hk!70fBnp~FwJDP{h*wd4MZ>{<*X>U7S5 zg%#Kf8^}X6JvH^0@#YlI_vQk5c8)I7LTW1MykcPre~Or#1pYH?!d9h(k|s56c|MN( z5SOedm0o1_?i+BoD;=kdr1845=a%5Cvxr__-c#XWTCFHOu?Q7FhIgPCVRQx#wq9C@ zvYI>FSU{cMxUw)mvRW!H^jYENRKbDSv zwvJ^c`++n^k3Iy%k}2I5_-Td5XVq~Ej+!lZ4bupCO3U;K;6Gz_h}^E{uVcX6JDuh; zsyD3R#qD=xpJc`AI^B|8LpJ>W&cVnhC?3O$%bkfD9TVDc0$_@jIe8oMS#+1Pz|JUS z=NtH>$p+B<$1QjKR5WX&(5v{kcT-Yh@UJ8kOJ93f`~O;It(d1vpU2&XQ4VgyaRRUs zmsLs?cm&+SXSz9q93R04ul#Q&yN4wKwOAjLW3EUJ30+Rk02>y}=TC1N$KvQ%hCYxb zeMoZtNsd^9vOri=BYRwP`Fil;^2GN_sLpmi8%VgeZot|9f2>@9f1*WM;haOAZ?&dS(itW=IGccs&m0O|iHN4yfg{hIbw9Pw5l)%W@aQEff92!tFVEEQJqhv?4t|@!S++i6|ZP`I0g%yq-p@ zN~Hw?3c5%Xm&hQOd@PSdQ~TVK2Mdbwr($FdSBSCh>eMecOB=g#K%528C%+)$xTdeP zz1i1NDWhYh&eeTWv$bL6lHtFr5rICL=~0gul2^jM&fb}++}(ehzcy^Ynt5Z;IZ~iY zUmzkO>*9zlWd4mAcebLuI!NVl(7PWaaz7H*+Fe;wwd^Xtfq@{owKn$61n4l!HcwWb zcEM~>YsI5z!19IZo zn4QXV`=F8%uJbFbjHGAb#*OnYTQTIpz&U$#z(#wp&%k_>8?X*djEugxA-$7Gz^8uN zC9t6(Vx;#8JsOQJE-Q07SQGm6Lg1yYdrS}nGN7d4UOjDGMb5kH9WKid#S#v|2Ir0g z8?=tuAAR`SvWZ+Tn*}^-Mpf5~;yVlVu=Fx8WcB!{#jJ(N27wBH1Sma#+d?W&;!9G0 zPWQGRS1aX4DF^bkfL{_cKyKIa5OMmo~hARW`C? zaP?>O@W<4&=LW?i3-j4kO>eEt<3>)HrVX4aiK3Itz04RO(W-@rb(a)r2Y?0<6TX@E zQ=yDb&3PsGwp6o|I$+uT^_#keuAQ|PMx?-eqZ~|kiR1-@G{6lg+4^ZP+;Q`_LKH9Q zVUu+kR}~b4+mV$dcm~h^C`X1Xab+7}TLh@UvJc-MzmBb9aZNPa{iOX887vyuEBHpu~4Z`Utc6$utu}?kvT~m#O#kEFB3&wQa0ee-UVLx+lTZylye0v+QC&3zl5)S)$$9X8 ziMkEhgK-O8#_zA}ee`05;8FEzZq8@pJ5k%v`m?9{KO#+qiJ&&nZCqv2+z5Tld{d{Z z7*MO@1wk1f{?=i|Q9jCa7u-12>aPU4Ai7~DS>vTA#%@mO38Ds7NA6(&(n+rg5o^5> zpQ!|n`w{}&m&q01mqGe3_1F+}nsbm9GNQ1s`Wr-}?!@KJpibt3C`wJ0ugykNXhXlm$z_0jj$+*i!F`C9S)o;6(3l5k+S z?swhk*{#lF#qE7pE*4Nq#z?R)*%XHbtrUzYBuORdl!vC}KRL<&371b4h$AQ0lv93@ zB)b&&E=Mw!*(>g5EzJ94i9iO5mshwW??X~KXG9G6Iyr)h+i`1(S1#`%a8YVYi$a6$ zJFAE<7(Qwuzd)6R)u~Sj8+)&i;?{ZFS0FtX0HPYUGV)NL0ytP!{+Mnhe%DAw0Bz^% zB4C9m+bXlk5@;~~4io`G)BZN&1LeY4n1<^8*v?HDq+GSphEGBmiBlT$PXQ0YHvZEwM2ZKRs^|OeINjcSqp@t7#_cfk=}M(9KkS6CSTWGuDo!4L;gv zaj?&7=}0!g0LUXb!@%Tv>ycy7Q#422w3lGSk}6^d`&B{YY0ww6RJ*1hGQD46&jC)1pjNn$y~{>8)c5p?pg(@){Wkqk}k z{^tr5Plqmi=1l?+n$mUY$OcAhNEIsIUi|>wS=mKPfu$B z{X~OqG$?w{JJYuAL(@Erk%)~Vf@1$39sGG_Kd-nlebnM z>wV_39$2VadMfv$CdhPJ zGcjt+l7>e%wqT)*Ne>Uf=KhUR2O62q^*S{d%kyDSpyX}5&hEyty+QN)0_6qBPRgWT zneT@?J4&8I==%68$?qnve&n2_$<#O8IrK~exzPxrvYfl84s{=Q1EdZ*ZXiPpekqy+ z5E3O1$l;2&q`(gBMMstl9q#n79q2`cP#3#oCC!D}Z3UW)=QZO)n1z3erdbESJR@gh&is8zK`{mrh@Pttc0t@ zY68OZzpOK9n~D}`Sxo}hSc(ajPi%DIm#f_cW@eTvF%}?sR8~ICUiy7sz3V7HBw|bg z0BW(-FMrhl#lsB9?QBFp6$VCZtt*<2@QL{A5Sj)J{ihYGD-mkIY))LJ+mk-tp)29i zGOQRp?BvR;pC&o|M$uL0RcTpoJXe8MH-MWD(IYTD z$wov2LR0HK_>u=!URBj)&~_O++;*BaF`;HXyViL|sN3lL{c}yQ=`01IaQHnE84jM_ zh1Afz!v#|t;DqEf*~tysbsdHo%vGttX~0W#KVbR*q*}W9VmG?NIN-xT0@7)-(~dgx zJKW*mU+?fO7j>#OKGV6kU$4(#qb05Uo+7AQ8J|VAObkhzPELC6joqe1Cqpr(+ZbAG zT0mIV`Ch$L|MWcBI(>$7N+@|M>m$fEdT+x6Pz}ead}Zi@tjkL5F)%8zM$fpK1W}@M z`oiV$6@s@cYl?=RFS~~|d!(?mhKzZVyGdEYyL02ngIQm;1139W#rF@TjDvqEK^>Pu z*-*XkSh2q~{}2%0jjDXg^uW!z{keq0u<7R-&{G&c5#|vn5mv$F?3%5`NEQgT>KGmU z>hlwcsj8{z8MJF%oT=fi_r4(3e+lm;XLZN4oVcR~Xn^D_$4&?Dba6gE^>mt}&@Oo3 zS=lZT9d|g;K+`I^j|e{QLb}o6ou(6~76He)Iz)+j@}ZM>Wz&4N-&b9CH(`KnDrY3X z2(lU!8!oX@CI8Fz1HGM=+Kp*xFuoB)>P=$+?9AzBg#sO#vJ&JZ-uSsRQ7o?J)Tagx(>hhW$ z=sV{Tf}&^+fI!IRF*Ns1*qFE39Ov8ki8LpUA7){WjAvI6oCt0qdS^=bn?;L--_h&F zn2I2DGW1rBeMcO!VP-;TQ<*3}eF^0t+zPP+KeJQ7^S@-g@T#q?Jza|6u7@7I$n-uI z4(RZUz_h^ad@pof8nh0~&C#8CWk`~m+P&;@YTXdcQs2OSnFwqxj@^oMINqx!6Ss@) z0wd%2&>a|Oz#Y(k>*3j(E)Z)za+xU}Hall8sn`k_JRe7a4ZU2k;DeBdHEyf_%49%4 zw^tPRmM(Eyr8rW#Flu=$F5f4qw~bC)N%? zPrIMJL2T8%UkdHW>NgyD9`@|d`x0->daos!8>|qtovX^e>pz)rkS7;Oh#p+fLL3HK z&y*sqN{SAn*m%R)6hpJ!R|c5~hYZ>mKQ;iftfggeFjc4l7&jr?xhF)}(St+#iy@5r zqcV&Y<>jyQRIsvy@bNoCpM=Xkto-i0f!9Zx8ps@upZyg?_+#*WJ6Me7DnH~z>V;?y zqYTsa6F|wn}k4(^+oYvA&RI;`IGjI+ly-lxMHRm%8SUvu?sS%L+s&IfiWvTi< zUHcyI9LUC^6;Ccs(hgT=6=FJw#I|FfVD^J|-o0-e<7gT2^^$UMX<*`#{hC$J^C7vz zkJ@e~)$zMWw7!Yi_Y4NZ*M6^4Q2}huN!x|E*;Sjp1`_6`0SO#Jpqbe<3*S^$10PtI zC?h#eV(ufS>bjnJr3KU|Qo_v3>wQ|SW`kqg1cy*qZ2agbmJSK#m6*sKh=t2}v3`SO z(QH|fKRWXaxYx%(0d)~;EV5{iVn31a&fMF>f&=DMMq9pg242nCcIqjgOTTpZ31|k{ zAu5xc*DV{trpPP!v+LBOzx}d+zagaP8N-d2Be-d9&?=IO(P+`<@mLdxVk#od%I7#_ zmOt(YVO-oXm5&}UwtlUZ7|C)k$5L`5CCf)*`}&nHBDEFjx4mrx5Lw{e3*=1(@@mH| z5)SdXKFB;6zPTK}>~Fgyv}-+MnYVR27VEEEapzxk_hQHK`_Bu&|J;os@;N@Xydhij zGj6)PliaxYW?kZ~s9%W*zYrrwRD#uDmr9>GdBjavjL!+O{$%5VZbza>>U6dDJaa$< zeM4>qFn({fyhXb#J$DB?7hRwJ`+##~z z0khSf-q9y@XO}tPy_Fr5JPb}GSn70yh|z4@!!`P2e9|i=aXRn2+&e$q{#ekiwFi8a zbQ6#}QAp`8aKQ9?^FB{mS_dTHYwiR~q%lkePptLgev=a8?#hn&mRM`Q;qv1*p_i9d&4V-lJ_J3Iamb_Zh_Q7`Fr?14~!-8(H}RpXx{ z5aP$&-+YBUy+gXlL`6lt4o77!uTR=gS0_l+?)ulkK?R?^euMoYgPWApI@{3_R`h`w zD?aCVwh6}Y_d-W#GY;f#+LyUU@+{AXKGCku3CP^xR|8M{Vs~!%Y2=;rE1x5RJ#NoI z9Ju5cT6uk?W$w+Vm08;a4BEkFjBn2@=8{VPVgp>|di1E$l~H3zX!n1CFi$@*jO@Wp z+%b&OSh&2mybO5<$D`Y}IXK-ZpxY6DX65!hGTH-*AwF~NoSD#QI6LN^YCfX8s!{0V zF%uIryBO7D04|DwQFpSLy%H(i4E*)k&i@H>BzbJL0x3LDIkHAlfS)>WVsy(&L$mGr zvw)s&c^9Iq&8T>BI)Hnxeb+E)zJLU)e!q{ldHNZ%&6XD z2_-jTb~zr&*E@2=tmMS`&&q@Ki_~m1t*d^@9aLR;xttIV{1H_u4jMg8W_7c3LUk2Z zT`v?c{)sy}7=qFU6CsLv&+Afq&x^`PVI^9cU}Tq*woHb<7a7^N{XsnmmJd`F&?|3Ir5z})QXfrntH_G&xG9%Z^Y}J3X+ED(w z^0|)abyND%^4p_|M;VNEFH0h+Q!^)u%Qj4-3WWXL?RNNWLT3+p>D~xl+1Md#!jtMu zAO?1f&pttfc%L=u4Dum=$``1*o*dtnarcvX@cvE{#_{6VCYx<(Vp2bclQ2WfTTVH? zz7prI5p`u{WhYR!rso+C{`N8`)y1&s))a`GL|aO%T8J^>bG4{Pu;JrxdD+>khN&J^ zsHHr^e;Is+=D`g&?z8vmuDUR=M|FW6FOa_QDfB&+ z=seYAo56Gg{R>BNn*15C!jcje&z^BaflZCh^Pg%k9=?@c!5gs-$Oe^sHYYlLRWL!j zyRtw}?3Ss#Lf*;qW87f5Z&1i%+q`XYv!rgF2v`T9ok=F!FH`FlSR%HywNcVkyj* zS#;t~x6m}5N6D7HY|n4x2FiU8d1{BBPejLN?l)~zz2J9Mkjao_jGWt*pB1mpDT>vAU;ewWd+xlCF&dmv(%N3!UggWF|ru#AqGr)q5 z!lF%H4Y8xFRd(lo`qc4P%es3{&)t2qjyK+?*GrE@C(cki&!0Zsr{DjJb~y&h<(u;& z+U}b}T0y&(p2h2z%Zt3G63_9Uj=3he+PyY6O{kuDjt^<()1>9mZ0j5LIcVdfA^Gw` z6H{8Oksvfg`i_P}!}eP(p04*1oEid5f?g5vPM(gW)(n%Fjnd+8CNxEHwzc2$vl<=;wk=;YS;N%GcS<7j^gT zg0{t(BJdAR!ffW8I%rO$b|G?5GLB&^OHw*4VnFJbPH8UH-$2TlSEWewbX0KI^k`H5bLE zc{6YCbe%feZ~5#+MMb$8=QkzcDQ`Kg$ITFl=YUzw$?Trn zk&>Q>x}>zU^vjk;{ZU*TBY8S=X3LN#k-qD!EWpZDc{BqlteIV^R#U35s}xKJ&i^W9 zD;IncsoKfbje{HT3y-AewU~{IY~_%#Txli@?E= zeIIM)jB8b&{UlSKJ>ntc3I3aGP{vWe&|Cb1$)Q2FK)6IMC$#h%VKF#~6+Q4-M^cX`D`3XWx?xBPk6p}SRa75a} z{w~D_G_Q1V*HKqP^nFd!qY{WMAAZZfKUu1`iUQxzAhdzRSW>rOY~zvuie~SRov2Y; zfTFpgL*UIvh*UL$G+s?BUf7W6am%ytry}W;mgen<>%a@qXAIW#PN3u5`Oqg}$sc2z zK6`Nx-L2$jQFw|?__ux5!^}=BLeCX;5p?I=t1zRrj0H^+@u{L@En4$mQo;Lp=)Je7yMooOkv0>i2ks;Cb zWAnPl@9CU&{gt%uR}FT=Hv5AM5Ggub6QlQ{1YsqspkemDhac2<$@ADfBvu!WLvQMv zx?--618pgtuKN7gqNk>&00(H6;JVQ1L@XQB1^A+FEZoqV8`#P<%9|p~iC{ zV2@76M){Dyo_>;sw5$E|*O=|vF|)4CmiIJCoAY`O^~^CA+w+@K z*0iX$4IU+xq08XI3Tg;`$@7Re=az4{!@g4p$7P!=5g3z?Y7MKp0y}A)yOCw&fuijAM13(`QYRk1MLt$8|I@(bJyCp-=G5ZZo-wMDmn9z7^nTL;>{7hVH0h8 zXa2PfpN_6{b_21;dxln>3nSmEpLO!*^Q*aeFVf+@&;F|ZaD3?D7&8oE>cD^`njSt; zqIKFSvdCQfssIE!B#l@L|JRGn-wotYeVMip~>$X^ZJa6bg}Q8F^JhTZUuK=p#* z_4PX%YoGf6K;Iq!rR}bcNUrZ6?b|YZbCD$K1$9fB z|MBeot4@BHOt8auzL#=o2i-=mIl?w-AI@0HKg83tQnXeVhh3Nr()>N^>KqC|os0}? zpDB!Ink%bC-3HqJvKhMfb`nT9UYJ(Q_Hb|34cg}#e0(O3e9KMs{q}7PB{n1s0$euE z&sFY9OTI44C|BzAYo|^-i67!+|Il2~EgO20vAh?E=c!s>)x*Z!OTR@IUEjI@$(wzu z@HCbUU`es9cwAe19f`xo;;5dFTMq46basq>Z6a}Vzlp>1+65Ze#;TsG0Cve4miOek z7HszN{m=?6%zb}9#l$fya}!x<3Y>}H^=c~U5B?VSV4i5p6Wn40->9n+nJuw~;dKk& z+Gl3p9$Kh{j~B?r2Owj}uYM#&hx_17BDDW3BmnX$lI|IW(=(i@$o z?xN?nGN~D}rFl>Mk{@xf!?8e4g0SM}Zlx)e+#i~clG{o1nZ9o<7N|1NDS|G&Qt?Ek z7V59QG9tB?kE}R!4KP&_>pbh#7J2s_EGl4s#pDnCb4CMO0+@7W@#cIhyV5;}>%DzL zW=~u_8b!_Gb+Xly>eK&9_5Xe(zxg7KMW49KPoEvu~Y7_tmd;ne8Yjzmbf0jF?=Nfd(^Y<>vq?OVtfYxt1 zc9~`Wy+6v(4|AK}q?uT%DG+>|j*c2C^&X6c0>M1^7SG876dz56NbV+-Vb18`s2i!A z6Tbt1;5wqME1Y2>(JF$&eXqI3Lvr$DJyCM`ab&pE*19N!64J=#;Du#Tl#`m;XhhRK zz@uGOCC}SlV9CJ3l5ROK@)k0jHy_5A zniz|HsgSNiXDRmaSAAD442e&al%`>(eEJtK?;Eu`NwwRx{@ z##`e0z|KSD7t3}!M=$(YEhBOj>~rJm(A*Zq{lUvpvjv)7J>Mg|JH*MUdEsB$gep~& zzHOy81THR>ymhip2!Ib-7OAXYy4!dA&Rdm1Jw7lq$!<-d3AG%M(6Qu*cl(5#X$q)>f7<}7H ztXyQ>V#v?f(7UJ59|$c!J0c^)4HjMw%TDbSd$E*q3|$<{J$?KOtTZ=y0wr5raW(@z zJE`&?O!ky$Y-?heI##RD$G`Taxtv9^(2z1pa5`6P<5cDjFEzGR?^0-jek$+-3a9LZ zsv--n&6$dQr=NcsW&~d>>AB#%p*F2j^o(ReAD*oYx2;Z_$ZM^*quBNV=5yy@5`29E z{V}sC)ZFM1P(0sr#_|<>b+@*0=k{x(n@_L&iARbWr7 zp)`IGL}K&7sKqXWidp%+&d`#9FJms1HBTo&Kt zZ+e*5-m%g926<8*%t@?eWlkrbZyf`hSgPWF5ol09=Se~=CpP5gz@QU9)%;|@$y#e^ z!0YDAiBGG^MrZrPrA~}n#uI||T;2dhwUF`Nhn+mh+^?+GM41YG3k&HWFYVv^5@0$K zT2rdq^nqNwDSH7Jh@C~U2b`MS;ys3T#zVX=3vN$O|$+jt;$i#s++6yUnZxcU7(x=zbHXgZj1vs1m+{N<&CqU*#kWAXXJ5Uc>jhzZ+N#Y9pWCdAB{^i}7tUvRDB z^X~?39qDP+ZBRl3jk(BZk~>pYEZZ#*FX#60UTVMM@q(GRDnl&WB~gFWfc5v!;|RMQ zM!ZPg1I5$n?BWBSrt?ZtR~^-;zqC>mIER(VHL`u$9HyW_VQQsh3PQbOgCa%fpfSi! zAGBwyh||@|P*X=e1~M+0G%GA{te~qUsV$dM_F-k|=t=%J3sy}PqI0tD2>{FhUE82l7($SZH z@>TWLDQkd+sj2Rn%1&7NZkx5>m$O=(oE<9N^i}nypXGXgyM^kEZ6{#ri60Wwh|<-TD* zQiApt2S29jM8$_yDS5h<1wfS2e?V5kry<_t*UxQt_JD3NDlc8ynn=X@aF*HObna7& z7XJ32G51X;c)0DlzW|sc(hjim8l8zDA`*a}WChOkGFK>QaiZ`TS{M(K^}M^Fqh6-V zYID*q5O*vGumt};L)f=vSt`v5SYE#|F+8x5%~C^~_tZUs4*oSpHL5ZmEw?Kjcz%Dstp#+2l$kb^zT(e! z^+7QLN7XHGZ5hfSAT^Eq7j*H4czkhc-R!*ye?2#>{dqHU(?HA=qac2NhlvOqH$C!5 zKh=lhS;TT7fjMO-_oq|MfQsReBbZm5sV#TNbgp-Nh0hgY3#>P@anS48W-3C%tuTX; z@chl{d#wT74b7+d41L%;qu0rkwecIsL;pPA7!z-?uMN|Pewut4hr`;J`cyqeyhO>w za*07L%RHh3|3!XQZiA;4Hg7J^tUq5pjaKFM3E zc!i1_QAlpSaMnsXqeJyKstnr;(;o1EQ;VW9mPHq10_ZV0mCW4zYgkHG-v>X5ADLVc z7N9L(=x{nvkJ#l|6N?m+%rMJ-5-&3==zODT7o`Pivkz?VTz=6A4#LxTLt$>sDL-Sr z?K5u)&gXP$=ICXG;3Jbu%klM zf^pO{V$O7saaB6+5Mgw_YJN}-aDa_K?`c2rl+2erq?{(EMiX7LUZav-u5XP<@vyMl z8awHa80x&F6;`Oiz-B^==_}NgW@rt)yk3(A(`{ouQVKi7jv6#+t1U`9xJU=&qyKN( z65USQj}u0iqOy-lQ^N)5mOV9)9hYXr3GOm#v85|65Q{cF@|V+ARDrxBe3ajOlTK~- zW-wD@To1F?t=#^ut!qLu>2^JLZk}->j+5_L`9o(Uekw*>9TBP?7s5O$)1RxY9(As?>9Kq!s`Z4nVW#E6X^x&I|O(HdoYSBT6Cf^oq6V^ueG?6#=>}qYNEh{GfM698tP}&8cPn+PwUo1bvm`Dz1jU zM_dQYWr;Ze8WOYrf0L)`cJg?bn^hb)w=K9AAAvBu3;uP64-i_-H6INa7B^VPCL3wu zDoyNOt=6;c4Tk3{d|cp^Kks~Neq~MehlhZ=`c>(nsCBV3jk8Xm@<)xMWRYCE^G)-R$QiY6bAPJR92GkOoqo}UvbjJ+teY-Fd*{_2QvQCBwJgjp+uA8k}zVq5me{Qnm@ zs-Cco!_SSa1Zz=NFU&8kI~TMg;BmoN0A?~$t`tb)VW-crRNQ;uKew5PYxN}Jnc%sN zARayLc06ZTxnQ!!BB%ON+ABS9Edp-6iM_6&x`&nMCf{^KYgsz$%W^MlD-*gKk zTvs6BHsD8gxKqBl$cMMy-?jl-a(f_#ZV8)0Hc%riLN4NUsvqIkah0Xt;n-10irGjH}kpMAFKrSE&rIn|?|2 zFaG_{3vdk)6MBw8*F4Y_aA-R$Dzz!_c-cTuazhz3V)jc(DWZ_&Si)435a^40|SuOzZxpxL4 z(!}J->-H|VcjmGQr%}rHS8kTu`7~Slv}ST?Zhm1Vo}zQEZ=K0Q4!vIjdOjBo?~e)- zlZ(v{Qg&OXE6TkJqltJ9h`oguP03H=+SP&+9QuonfH#hjQ_A{1@1yZ@0;L^-$IVLgaoIW^`xRtCK-zei#}< zQkbyB-C8&lR{VopW^UcZo*{VLUsgF*RHlKa2ZPS=K#9vzlp}1MTt*;}ksruj z{I2rNAt5!5E6%JstXc^gT2A>1ZjHqO{Wz}=DQW+e1Ysor^6OIIM%uA#E6gjA44pw2 zwL%;Kum{|mUDUWGp21I`Wz|v5InAi>`vZKzL~f%{@Z$3~tr!8)X+Oo3a^ZDHm%RS0 z^jH`f0@uaRKx6R_{W)TWoaqYSWE^G4kNv_5JP%4mmHvKewkGzeQPf9cJD=*GJnBrJn4VUa)Lx~-jO=QM#k@5h)vJS`Te zvOUiWGDH7A_TD?HiS!KvMO|f81lxiLf(1dOgkA&^yHWxRtRP*GBE8oHqKk?Of)XUs zRhkG05{i@%5a|SjAQ(z0p%_9EAS6ISk~_God+)jb+;jiB_nco&ILt|AzWL_;p0_>k zo0;xX&F(`?V?1-u!cTRaS=ovi`v>u49CjYL7&G-xl>thEJk@T>@e2 z626FThf#JnwBOWriWpk2t8V+cx8DlMI`Zi6XBYP12Lg3)f%zmGb2XOk>#C576pFXO z`!XikeYg3Mp0oZXADDU3>JHMGL0+=UsMx5lQ~yK^K+NEl55x?giZ2FwEA~bSU8oW$ zEo+?CZ=_{t?@B1O4c)F^v|Bt+b+}IH$s5;mox6>%={>$lsU{o=xk;|Np8WkX7kAMk znj6~qZSk=u?DoP`ROlAX`Qz0wTL6sxM*YTveeqv)s%am>nJ$&kV~J~b+hHOF=iJOY z+~r#I+9S@n9T61zbQk9bq!?EfcJJOEugxGvdGGl9+$$%21L>h*rq06qpU^pA4oKQr z(}5+`uq~nWrb>j66lKTD8&kinAG(KONR`O#F)%n}Uafu=2)T-1Nk1s5v^ZyT@$kbx zhbR7eq0!_68LxMnh`luy9v~iRW5{>@iqoU_uQq;npJ(?*7_W0L^ z9C&VgeTxTkPbi}JE_iaH(#Ma+76VxaxGC!R+d@iF-;RiY2t@$vk6ZTBi?d;NkVM@B zgMCie4&sUoci9hQ)|_4fn1nbq{$nxEeFvg_k5!`XAhGLAm^^tJ81un*nY zXG?0Jx12y%?VLQKj}2MI@uAP0ASnPudrAspoIiofnSXJox1$+Nj!5QDT%PRSQsNNk z8JfYDkaq3(Mhhqw=JbGiL&|9)JHLEFuSVrVAKZdhyO43Kda|U*J9yFOHG$3AyBBx1 z^4D?}ZTXd^bd@m}gx^?KK+7&uUIxG2v4O6b% znYW~gW){m$D78f-EPs1tlUMy&V(ERObb&Ctm@_gLgKl+?eQ-20@|f`6LIM|+llBvr z9r=ZT4kfFGieXd>q|p8aGy4XEnM7+P)-Is_`m_ZW*REFYF1j*WXVtC@IZcQ#IlU#_`{`dA!PbHc3}v8k{BKFi@gZ@`;VBKqGA_F3(=G#(-;m+I)-RCF%ZE`^HTtGonJtFBC+2Zh5=< z{0Db%i)ti;-g_Uu&Nh-eU-vGX4NWU%E&4ns$d>r;3H^CtmuEt@4p6m>V>a?eOhzPt z(5b4L2IR#ejfrCvEw+s`*Chb5cLArrDfr;OHF3HIsF0iXUW%9;Rjlm&j&OQp<6r^p z1LVWo#h_9pPl`g zA(+0qe71I*mbU%JZt}!0A$D|c?O|M{ZH(FxE0+1>GQ1J>j?#Q)Oye#K2 zj{*(ch9P=9){%Z+;hq0Z3rPD=jNsOR+L zbHGP-fBs@5POdlrwU?jp1RRJskaYf9mdi6jK7n_ z=lk#2UI73*|Ghi_1pasEb9KP_{deg9PWzvrf_B}9tRC9`Q&!L8TaKVZh#W%r1Sj}_ zuD^!!|7Ah$eEM&>8r6yXTv>8PXEb!7yEPvG%CFb|UEW5&P;p?RUDXMJv{cfZ*u;pO z3~Jo6GN03LjAEJ}p-c}mABW_x&4AF=!=zDV-<1I)8^LQSL)mExW(q;GK7ZTtJ~LJ%zl8@R*))t`WBc<#}(JP zua9C>3s)FrfOn*1cw0DKRxViRb3WA55+8Qku?>r8ySrNmpL;u-$j=n4flPOs@jmf- z4<)F6-5vX3*KMU+OYirPG2ve?$Jl%kFA3y6%>kOR_@7Iq`G4zlFA2}?3!2)BJ1a$r ze?^$-96Ka^>+F$mLsqM~&y^$L&8*tD>Jc zAQoYy@24f;@%!(O%4OdQj`YlK-tl?&h4?}@H#a)0Mrs-85C#g-aaA{}o`y@!@9BCY z&@>_-@!L&_$>onH3$ZaTD#vfTF?P8Kl5fem5%O0q!q z#ea-#*{NPt^_+q=VY|ckkq4Ihw@tZT|MPH!cLym&-y2!OIUIPm=4*6dPXS<1d(o|@ z9vbDSFUOg1pJl2F$DKH?CZ9$=fIN`?du(&Y>~yBFY9PHR>7&|xXOraWJ8QdUkj{T} z=v@P6ser+g5fP8VPkR=dWxD23flgWr(Vak>h)un*WZ_K-Ma62lpbuM1Q}Qm0`>pf- zY5AT5a5-Ew?^>gm>Di)tTv% z+S#@BH(UBLQ(wL=tlUv}pLF{TX-9I4K{EGPGS_!{>cE9;($f=tY6aulNH6xuVs6N0 zfv*g;Jz&~iJ377pd2pA2@HJW?aCL!^O<$-%Gu1~nqGb~T8b2RQeyMe=-e^?#4q;mn z^+1N!y~jC}dhGWGpwrEB&5H1mck~L_nL47hUE+J(6<8aEJ`_2TH73mF>S;q;)FDbIZ!4ydy-3TyEL1%PFR(O%Urr!XWZFvvR*7 zZ=W%?KXQIQy!Cd(n^=Q7X>5}C(?I9@%+q#2@2S4wy#KX@rwj0tQ=k9XGMqtSimP~u z`Nv5jWXO6ZkvqCdi#B4dZtLeMPc0p>6J#kvJ77E7B!+eBc;{ec+>{pYGFAI=8M8|X z#j;9F@Sm4^-_M@M6gjD$Cl^ZADuq3%*H4S-30vODnZ2FJ1s{^tq5r=7x=DQEF_qun zqYq%3udd5_Pi0h6ZEqZkO^a6cvrl=SK`>M9iCu|9{grxb7gIaN$j0QuJvV)Q zSK!7@H2CVZupIF5oP>`owpj#so}Xq1;qAnHRrvVnz;6cvhh%+2uMe;`KU`7P*^0WJ zZ-X2Dc{mT&cLdHmp^&qQ2%$w^t>7LJd$|9%2=95ih3QJ;f(lOVe#`BKeK(Jl{8_0d zS?Y9qKz#Dk@BTiRrEhEN>>rpIUEXhcHBS%_%7il)iBhf9Swx{gw>tz8eRemwE5xp6~1QTx56tdjkHoRr<=86R8TCgseT%}3l7)g*>l zWD5U99A4y`a^uW5stT)pCM80D+rvjM2gAM-_JwtE7O$D5)$OKCNmRz(aLK^cQ9*TH zpTH+6!X0(73pea8?QdENNIK*9fiVO8bagr_7hTcJemeY-*Jfse3EE*D)sSl|-P9*- zIit2k9ukf$4eg)V*}1$eOK#0(V|9W&zM;^(9w9Z9QT97!TSD_~Rng1`0UCLX*+P>j zg~dj;dYeLq$vJ~3Iz{yq<=&aQKa8w;MuJZ-?D|^xx6SpW62z*q0-gVy6^lKPm{`7X z|6OJyZ~7kj{vC~zW22{|R$eaB<|}CkQyuWh_0BQ^mUZ?@eUf|zNguEWqLyaqkrNsn zNB2Nn%HSQbEX_zL4YrpZ#p*+MG#rXS;NLknp-II+v4L6rZ{ zZW(b>L;H7=^UUTkr?=RLAa;Wj*RgnFz~xgwP|X8X(c31WocuGNH12&0U-!AvtF;A& zgfcN*RegHM3<^TO#`-qMZTBZAmm#zy<+`q8h{lGZrUlG3_h{jdF~UgykH z`rzZl=!aQ}@^-%PBgt~|%0>ZN+iz>Sflhj)s!Aml=FN2YGS2W7oJ=Q3v{jJ4lArqW zH65evE*CFucRF^1me4XK-;ley^Feo7-HA3>LwH_+@hz8;<~`3lmHTs=&jx6NqMGAT z5{w|5`yD&C(xHQ@Z9QO10{PL<(4`SG+5tLsf5O%ErkK!uy=0LmB8m}l;=+%^YVJT9 zjX7=Jl4b8ndYFYN<-Yo@=;nEFJIqPs_IZtSt^xG25`KGqNdsSSBpfVzWZ$y+{hETt z@r!Q<8u}R3=SY%=cXkdP8Uja%kqq#(vu-2KV|MwEEE5<}h%+ZWlAHfbRu60U5nKDH z9*F|U(kraNua*v7oMu~z)~o1B?Z36nUw_-S1FF6A`ikJ!pPv(??(P2Q7jxmTD`dua z)6u}`x%a{!%cibOMf;CHd)w$uA-d)+ z@s4x&)=K8B2$MSOm7vO4r9$-1?jJf^!uWa;eNAwc4%}WoOXLn@{qKnI`aP7`TcY1U zsto^k^3j&i5s@TM;EOk3U-L)IUY7VglM|23RGxrdo>5f&^W*MovzFB>ponLDL9-`) zY3x%?&W}IHM!etNHb`BSays^y_WHQTch)y~`U+Sk$&U4s$`^YuRP6BW_F`=5nSc5|`OCUlqRDjRNu#p#q)WWw&n*f zp6C#_KKL|FhNI`&C-sb`-#UknberzpW!&}`m(_Ob$=~1a6$68;`it;8Qi%nhy(H!rWTyAav90lzdJ?X@Fw-#b6cE%fR#8n#)^eZE#N!|8j( z3jdIMyNPs(6OJuo>@rb3O_-1!A3d3#@Bvr;h^X+SL!7j%BUvjQ{Fk4qVz{c#$xmWd z-;Q7#UU>x*BNiZkZ`*mgJqC4YE#>*>iSTO?U36+_*3S6I&*Wk4i8GaAK$aLp{)@wP z*q`iiY^B69js>qTeNz;+JwoD78SyIvpGMDLd$$!BX9l88I{&zjJh8ldqIvm%yZ-xw zDej+m^hkT50>$0!?Erv%vb}-4qgN$~k@(3>y#ACd??|kiOjGv3*I8`Uk<5m%$p9dh z_yBQTNb2Ao3Gdv_=^Fk@cuTwbTjlqXjt4A0I@|-AZ10i&a)KlUquIb-zboG%Q*WpR zOB44rNlr5nW8>?-ymUP!(VL(#j&onrIDb%Y&s$8;)7B*AlW48OLw*O%fhQ(DN=OJ0 zS$lMs(D`T>!teEjG20R$EL8!_UnVeZE z8{tSfN}gSuGKrYJ)Y@4T>EQn|m%3M12cg-nH?QtAdb8IR`@PZ0jAM&Rl81^St4wP8 zW+%s9NcuTkAC}0|IWH`*Ze|Ql$S3#4dU~ z8U1x~2I-1}aZRIS-{%6EaF&``w?@1luUx*Rgq>Z+%=#37sDa zy{}9L^0>p%D^13iZi#@Z@O$pc(asv`lEp`uFx?T|96kExS?aO|H*Re}4}86TO~vh; zaTF%c(;`Ek_qP?LVmp>%F~=C>9cSREi%zgpPQ1;D+3p-(D67w*r(wd(2pM#cwnL!2 z&Isx?+85%h5T_>=9Ks7Ku$v#KM?l<&r*R@>aZW<*x(xwJabjg|;G-Y~sCTHWZ28}* z(13?-nK+iq(CA*zIj-VB{@O=0I|zrYVB;6qD?Mv88L|bsLy@c2YZVL@8!0B->Gk;L zlc)dT%3ZnLi&G^<-HVviWLu*G?~KR*Dz38qnz()ga-=#9&UR+6(F=M!=4;woVxsGObk$j2_&(g9v&Q*Mf)thX*W{Dg>8s|GqetaBd&Io_JhPPt0Os6CDz3E(~($tDWGTlaS^G?{XS5p*LeR@VO})`i2%T z4DkJ2hFw}V_8CUPf4;XK@j}m^s~_$JlLOXZpsFRsHv6s0(o-RUTlrtzq+3Q(B>nwn zi4;iZBn>5wSdZBij=h=fVB_sI${A|ng(T+s>2$4*Jy>)=oLY0_8`7Mj4bb=|savgs zwq|pTGh~!uHB39Gs6I=95gu$WJ!87&nMg6;C0+>Syy$e7o&X-`TCZqS4UJ?~=A#Ca-!<)v5WZ)S>84o(&Dgk;92<!4I(ui$-{$)ABsrJv;-sV?8@hcE1L1kGU)MO z0*otHyMf{x`*9b`D#FRO?zziBiMhR!*P0e=twum{+2dZId1f=#7}SDP;nRa)%QlG% zF>FsW*+k!{EMg>dz%UJJ^}!gd8?qpnVU=@$3osvs~5=0E>*3X5YlgDOVrv(3>Pq(+s= z2gl3f)G36AMz5zbe)a;u49TW-)ECJcw_d)lL6X^52b8kYL$sivDssLzr)c)^lXo7C zFmk(~z|xzDmEh5e&K94BY}oKb*RrdDU_GjWIRztZuIH$4Oy>&G5NX8z7^9l&z9S(R zXTa#_j$tw}3T5*=ZwRi1;y*X_LPD1ammC7c>-C)5TT(r{`;sO!N?cPycLHI+bx$5A zQfj|(pnf<6N-(}J=;OznD_I5IC!y)98G*9YFs;$z+ApwEVp0BcE_|v_m9?58#i>T_ zZXsYWvYM`i%LTkqSx`l(fQe?_7XQN1O!0Hx{^PrQL&P~LTb_pYSR95 z-^cPL9qW=iQPLg}^zK#RwNz?gx@>&24xUMs%&yPm8f&jC0Umb@^OE4I6ki=4Bs+)n zxV49R?`yT>hxApW3_}9$Rf&gvHG0Tr9!`&<;UKsR;o4~08Kz9Rw1&5hcQ~5h*jY;~ zCU}*(V=&JypjnxJYu!G+1m6zRiES)h;hNiJDC?G%M{PP!r?mLX1*tql*lO zg@Zm~GRUsUabn7a;UUk6{pKSC4V+7=t&u3kd9KBIjubeTYbct_RAlE44rEt0>yB4^ zk%|b+v>K5+jafw#Y3^Ty(02Z=Bgfo_j=7n86p;t=pmuDsb}k^yhi*z-HKQ`29JVmc zBxG{*_r-f<);YeJq}ZJTO*S8dsNP<9SE2++?&)l<0@|L&Lro+w;fK! zDKbTJ$Nb?P0s=ZnFN-=4$llZtC^ChYJDM*`PF|T2PXk}v4tngNKr*wNOIcafC3lDB zID(3jhIXuLZHG^u3C1!hL>obm^0rHu5wT1^+>Se0YEF zzZ!rCFymTa_^ivCdw zbm*SJ)^1I?0S$R6ZuxM}1+{V@bwHJsI3fp?4a1L)AW&R5{t2@`_b73`a!f)SXip-jbab&llh%DF!wpph}3s zKT6P)cB4z(*T+ar58MOr-#D&a&TMm5Mw%4O054HkFqB?E&4);OFve!>Bbjo9Z{Qra z&`pLd36A{|ZDRwBR9j}BRRV`QAF&weQ52B}ARH=g~BeEclQ#5|kJ426v z2{AzM==?SN#%0s=0;rKiy|O%Xq#D)P7N*tb5ass*CNHNHQm2g1ZCQkTM7F|Hc>yWa z!)Ef(IXp>HzaitjovVstcZ9Dx#cos{ip-6Tm83yBrdnX+x-0Bl!#3p85?@*#MAE=3 ztEKzGKWKS|Kf9jZ>XS;!jqUNk3tdI}WMMPAwTZo7wPWgp7A#Y5EMa1zFfk)B5Z_S_ zJ4~mlz^(@!&Nl@|(zSKJ)uZ}H*cE57vh8u4$bfnGIfl(u@+q|v6ElbQ1nx&L(@gDC&yXsFNV=_cF&lVztckx$4^D*GN9}`j#p-FR z>w%3%Df6GIv{z_OyuL`_W(!>}hKZ&KhW!E(x*X}edvV*=!X>+%hEvsYKnjaV!Ym7K zR9rZ=qY%sSrA;)Z#R{$gYnIEz`Q%_TjfW>XSQ`rBW&ZiY(lMbKrwvoN6p=OQnQk*g z4)9wFj!?U|c&V>zWp&y}Xc}lvcMO{|nuB=82?zJVqcW?WW=S4fbPIz&^ZU@z-`uh| zUXLmHl2hVQS)EPYi$Yg_P$)qd{{GLTDta2chuy##Zc!o6xPsw9OG|!R8sORxFDU+ewTyZ(Y5(t$qsoRBYB_yshzL+i)l+*IWu9;j zp^GN9z&DoZ!0$OHfQmynwnvW6mlbo2JXO}nDfXc`_PBui#Bw&3`KwO+ONqi#Gjr{9 zH_E5kv50vTSvjOz{kO*Vr$siIBWj%0P55X&Y28{dX8vKv&C}$u_VX4w=)ZX zXE(xB>6@+CzTQ@AwVY#HLS+Ea_??RE4WD!nBoYE#6|ZlMJtMNQxyDa%^na#M#?#2q zL#YwQOtHt5VOg&Vwr+E_LRw7+g7KTnq+2vYk;Oy`sH@-(1;u?gZ*om|&e zsubt&LUP`O+6aT(&Z}P9eM1|2b;s&agk^IumkXI=c}UL?Q917X=B$JvS$Uj_jlKs% z)36k{&NA|A;B-qa z8gx)Iq}`(6FEN}NG=I{x8f1UnQmf)jq^VtIdR~FXY2Pwb!LYG~MSc4FVzQ`a5wmC# zGn7Y3es7m&r`Qrn-t0F1#~x)y=F2#2VR?Q%03s;jUr2EmPn7c<9>P=B2Bd}Y8k)JC zwz3kV;$~U{8oq$_1r}@r&N{QsXK`Q5UjERX0It&G&bn@ppD-&`u;4}+GlwoRdv^JD zgz7&kd{@0VB`63};sOCjyv&fspw@US$lUkI3%w4Jnkc7eg<&Gw^C8LENS-{0mIODT zV&m2-UiPUSd5V`70^XMCEWO!Tx$!`$@iX~`Mv-S9Ar>MF;NCa5!SU?K*Z-*GUG;bR z3~y+u1E7a93=tlTHnw@D2Fy7lLXSxsydn(3CR-T!MYAt}x#Rx9Yjf@L`JE$kwLXjX z9j>?Kd`A&6(=jeKrzi5m#z`!#BvJ5(&BsP;ITXZ*P0?PwFP8eRUyMz4jxIF>xGF=> z5g{P$PLh>31nx0)vIfk(X9;k)Eul4e5pPsgOBhX`lKLXwdQt?XACx zXqpk58|byRX)t69AfWC__`$C5q9Vgi!@Cm(w#3lxKl#O@H;|JI-7|Y{RV_y?6OQ*0}nkG zu?DJuk!{<&CFG%uw*rm&3-Z`SwFy=f;2D5;pd+9m8*pj`zQIT9w0$$C?j$ddT-}vx z2)xq^qE}5as&Th8dhhreU}6xp4OD{eqf zae%gQCN>M<8=dsvO1=tg&0o$iDmJFqq4D|NVPelHgfAULErs;W)yB*FGTxTr(Dk0_ zva$U#qw{61syS|y%~`-@7Enb}VIWOz^BKE(R8Ro4(fm-1pHe?&@!1qnv z@vSv7YNKS-vH^7QUDuap z-!Y0^mk!$O1zFYuq1hSA=)7h6oP^_-+(vywLU=sF?Wc^Gc^mN2)q(fEfQx?~^|~A0 zm?@-XI8uc*O<{vEX-`@ z^pCBgknXFRx}LuL&0K-)Y!vd}SlxTD@bC3&_{F0Kcct4&flI4Wq$%XWA+br%vp*u! z$)q=%-^>Ien*XsnlM*QwQa5l6kahM)0kcCtQlTd42L+&}wcNNZwdiB|Xp(cYSDh@~ z*Z@sg=>2>7T`{~!afS9=bojZ^*e!Lu>;G3bIn>Y#S~RW}Ri-KL7Re{CToZ{3Q#dLu z0IExt6Re3^0Ts#1?Q`tP{Xh$Gm{MQj{y6`-ZIxo|`fR90UAh{@LNq=F*%Z@XUF3@# zNpWLFTT8FWzVF04W0v+r9MGpHys9fHFVl4JsvB{89+{jj3r!y=$TO6OT2O`)!g@8> z?u7`021usE5A&5>EGu7Ste%sfb*#_;e4YONI3_lBtF(CqHIkwTL|AdTJ?$jLtE3)5 zWC-ShmuIqt#XBR*$=1ItQbSj(x=*5e>SB8qoD=I?dKLobbWNcsEzGLgb*#ACpKU;; zBFgQh7A8gGDpqC!j9Wxo_D2otWc)C1@Ee9+NDZ{jLlL@1&|19D*yeHMXpURJnpG4B z<_VO#3OeEXogug7uFWgE@jods-xu}yYYHRCU-kYG;kBqYR@ft+w-AlKA4R**up zaZ}jG9JL%dXhv&53ZX45F-+N=-sLUc7^NUm)|kp=pwt~EjC`XEJ$r3UcQ!0}iQ3G3 z9PMst6{}STs0AD%W0WDX5)h~!H_kPo^ylv3Tu5^r@N+B1q%#0Uxse~$%k*>2)y;8C z%Y*i*tjBsHTL>q%EUga7RN=UHY0+tNdNQ%!S*vkfMh{4E=J_ms?;IVG(Jz+cv9#ov zPS9}s5TQ@@!2`TKe&0jF2H&Ujdj5>gYPDbzU3*)r5_{KznCIsttN_~k0c;v z*2*x?Nw{&D*&Jujo}*MdEG=HP6eFA>5+tn@EpXS-Vk33M7r4h7B7@=Z_4 z3nN!#-mkXybMp_>!~22qheFZ{;JN9Kau+%N4)oPJa4;5oEc2L$?|ne2 zCw0AS9Ha0yZEJoW^CEl$6pw=Fpg}h06CfNF3h$f$y5idTx^!uN`=%e;Dad0vFj<*Y zkve<)TV9_2La-BZbs#@4uUliS&TOR*?m_az?){M|Hi=uC#iXPQNr5|PbpDBYI@h<&37*cn1*wuZ9CdO;j8>;7 zuX5z$BOf}!Q<+tWUY2xB2D#VIo#gCJTF!N32^1HkxDFkgAuhJS!CYT(I#<>yO!_og zO_5Ydp6!&W%}j7(7EOJ~O94vuX>&Rvb)c2bRc&$YQNp4_cb;yJmTzNYF30#y=C0-L zwJJ}ebXm`lJdIiN0IYqXAzs6CG`U)-+v?(JL#4ib6ekwYJ?*-KWH<0pjp|zUq4@4wI=#0vIs-HeM-P1q)@sN*wZ%J5+<+#x z>T_E#LQ-Q;J~0bu0FZ~p)@FadM>YKM+tQ~Z zIlennhciAd84`~F8ksI9i&8Qx&GGM|SXvAeKvBwZ%nPhKJ>5l`+Yq(TTMJ8m4Qfp8 z@o3-T1ZgqLHo%`DKoS+CXyHk*i{o51Z3rCHE(lBOE2s@-kc6%xfZpX;6Rm1sC4P{Q z^_f|Ft$(D7T%Ix8y&3<**?HJ28IncG(#HxA<)|?F@!30dLRUxT;&!s+MxhRQSyFk3 zhMX|4oz!o4?9%p@1HUt?>nM?$FQ?6al#;CikNY<&4(mY|uozU7A?_pq3(xVsN}5M@ z?qCSw^YAjsmQ`$4a7`q5E02vstb;bjC-~>q0N)^gdfest#-h$Ojz~o~Kg?@*7P-a) zZ}Hpor^9#_$FP*}4fE0)=vOG?FO~r;QIBh|dZ;<{e7CXL7O`>OqCUm%Ip7Ix6jPg{ zR$73cIL0Dv@gecRrxp}XyW2ZiOZHiI^zHZq-tq0vA^j2B%Uad4rqpj$t_ zXTa_G`^H6ee+c+xztUWql=STPNoV0T8fXWHR_Vbtue*Y>tp`Y4lO$n)m;|``A!Q(s z#OHE>bttNWxr~AsFu@->s9rXhZeiqwFQ70j1u=OOvgk;Q`n*brPfL|@OWHu5mKEw zq(%FP)58<&$S7YBkZl*HXrT##!cxTUaS*uI1Iup@iI41cidND?y?3LG%?h<6fe+d! zcC|}g6y5NRtL#`qujrL%u@T0~!L9fqGw`&b??_J^sYlk^rld4qLKxp$*!e~_pSNVh zDZs%SQV9{!kt&n3U2pHAOpwWrZ1ERWLfta6dHFcc`z)@@^z2lptnWj&WOMZM3+QY| z@%+y2bN3bu113&mh$;XBYH0)zFIE6&XPGYBeoe3zL1Dl*iD$z7V=@713E)nGw+MpS zVPfydTo}OEv184LYI@yn&%5ku30(a&Rq`Xw$rN|Wnhqc&*5*}nCCmJ0BPjOncB5oj zZ%+}HrD*qK53IjQMTvYV8Q^36v-gO29#-f!eUGO zsqPKc+2h72{MRe#1v3Hk3yqtrBuQ!7{2GE!Nh#I2k+YtWS-#y0Qd8p~h5LGfWQc ziKwxBP&XTbCc9;pvMjN>j7PM8EIxi$btr+(u{kk z&z^@NKC%Szc*g_cR%GUyjMMN$9m)YCkD@H3nEr?`Z-+WbkMtBRJ*qP)Of;}vBo zx{VQZ7!IC*8_HW9zmA?a0gr~&(OuL5EZcI(^m;vA$k&<6AAvSq~ z=_#_^=e`xTSEQ$>D?8K;rX+N^SC3i;0Ub@ss}o&$1XDXU=*CSxRB9(xZn$dZ@ska_ zOk=?i?Qs)?T$emzuzX(U(mw{c*BVBU>$cKF_g|$sAhdD)=4~|)V}DiLdhSHw+9a{! zT%)h$C`;j9G})`mCsk3DY@y~b(TfR3ya3p?n%ZzKPSS^3;4LUWPWH=zH=N(KzMPeK z8M+IIEOduqnq~WVYVjt)8U`H~#*IrEDJhj?kX&B$n-z}6yBv>H8-w1rJ~J+yC`Sta zY?(0c9EweM|K_uKy^Pjw)i}2%_toPaS$aam;|EW9_gdujVdkD=i9(RnnN=U>5E=7+ zhJ)*hTsPX~mJ(>wZ51qEkCCVy<9j+D+YD7^O>_D?BTeP z(cJyRqcz=@t0J4HKxinJy6NjH?rI-~UuBcso&}=Q%+YDgjr^@(CXLeN_M!~#p>apqBX0n*|qZOlg?5yG(21fIdu!`{!vU2=bq3y?O&J<$H~q< zj_GkIL$y$YH(Zvw$FLy=-u|0vBv_4FRDd?71p~Bo0c|f}NjE_GTE^pb$8>Xmebe6i zyQ$jST~(<9%asV;G+-9(TIk+i>(oYAaKOEC=yRvr`i0b>hsV2|!$El3an1Njao2Ap zZUc+17z5PPxr;;TSwb04R|kx}bkm-t`iG6E;q+Pwr?-;MN%?$P&2A$Okk=6IBSKulT8*2w5o z!(gx;Qw2|Rb`OQ%X)2lyQTaKJ;vQd0+<^DZp@Hnrewu~tN_}0oBCH(cHUodK9}!)i z4?J}@Y`l^SP~?c@jeut@I2;gu4Azyz`iu+~K!;hD8DYB1E~vbB$ln}8x_PUeLAHeI zRdple1w9j7SY2J_-TIW|WGyH;-w?QA)$BKGX$a3J;Sq>KiG_fFWTbu8W`!@5@2T8ChLEA`DZf(BF;=Ha&WW} zZp;ojI<~jyE~*pXLaIp$ND}r+UHMFUn$T49gD_gkg4^8`Sz0MJ+uYh zFP25tNq;*3XrB;JIR9f8$g?LFlO<}rZ3v)={732c*3N&q^Z#uC^8CLDfe$>@X|=Xg z(csHwa4$woq4~z)7b8KXJteKP72RLMu9+m|=ANp9^i1dLD0l0!(^GQ_rP|QZ^r*@CRYjwvHsnZ zQ4)y7{MZ5b0C>_j=zqKae?>9_HL^Vud1bx-@7JUQ@m=EhzY7|Fss{lDtuw?nDp1I5 zw{p}!i=J2bARv2To<7NNfmh%ucDH_g5$q{S*>PXM%!;$rL$`1tF5^)Ntf?Xhf+zQ2 z|D031^>2=XyI}v09%bTnH2p82v=&@UD;`BZn9BB-WJAfWfvC`_Tzjg~tBnSNj(-%|1h7qLIVH8_xb% z$EaVE+~X{|xFg}=H8^Y8zjlMe@61Yy>0WN$;R0lQD<>`(X~>&bo?Ar?M>1NlZsm2I zw|s*|nU^A_TsQHD*3IkQl=LPm9k+3$`5xob9n`MZ%6RGVcnu9aMF{zrQb*ONf%f9Q}2Y z&x)0WDQ}CDJHOxeI`?{<`XP{f)SthOYFHi;oR8}}c-;MY!5yREKxu!`ly2 zwIZJ%K%Y<0RjqdVm;=tg?2x)R&QE}FDSX^APuN(2GIJ%qA~Ptcyqh+j@t0fY%E(S- z$7qx`WK+VWe;QbYifGPlDI(^I<}tipnAtyeDR@`vwQiob%@&$xIKwS2Ieg8O zq%jEX*x64OT>uao*SVe&R?|nrm_x%ZpIFVueXL(S_RBm+VY1?wt9L%dXw0n6JgYlf8&2snKo`GxkxNbxB3g8Op)HqY`D`W za(~}5DU7g6{m5lY>lD%f68`o<&uTvp(m`*rvS$`9YcyygD@-RM0QD|&d%)BR@Qd?? zi;u)b=;PB7h&bR04I(t+E$Z|iZH00g&`ZBhJy3{TUHL!B#z6Ie^qs2u5+8dPU&bD& zX1?~#*=eS9zs*sVV@cm8YFo0B5Z$aogmch2S2cZS&(K)jiVrBdCuo51;^DO^uuA+w zWdED5jA7#I`br~9 zl>W!ySU}G>HDEvjN+aW^i3A=@OWEPJr`uD-z8q|KHX$nWr5K65pUmo5(X#13>0i~klAtK- za-W-ZOp3@!FheJhH}8%tlU1P(%U6nLF8vOSoGONXl|89n6(^v&pJekAB8{8(QH}Zi z-XuH2I{D}L$roQ0DHM$C$ak=l1!W=XG7@N&Y_FR+E;L^%0F7=*;9pWmIi62MOyLt}eadPK8Zv zb5Jb!@Mx}&{-}Cq0a+}_yKYJ7Xg&qo#d3zTQ8bGwgPAgeKk&D~ZR+4eR zq2i?>vFD*p=&+UbiRt|fRyA$RqK6&quuM`~_9%JAmwra#5cljC@n8V%yJBLOx@qGQ zS=fT*Y5ecgWBV=qCgw-&LR%00@vVVYhnV`_Re|jmBbFzz+Wf`=h=BpeNn(8c(Omtr z!;DF-xT`~CdIr@S=Yz`mO48-F$GG+n*rc}yAN16I&xlBA-Df!OLpBdp`U`f&y~5n@zpV{D?Leh*+(e$$a}!deTQOy}NKf-MRC5PV0z zrGbpSwKT!!voy8!g9TJ+7)WqZgJlls$35Hw0v)!l$e*`afr|e3MV~$D5;#I1=ZsD_ zGGc$b&Ex#>DMa8g%gXAQ_5Sh7dpgo60PzvOa}B%3mJ2OST4N)ttyY61_gp8~C2-3`u`k`n3MoYZGih|h5<*dH}S&^mDt?21F91ohDmO=&YFH@`^~U${kGnmq9| zR>cLakw0o>1&;vS90(=z3{HD-0PaWwNM;YK+GsH`17E%o2HYG6{jib*v44A*KWbw)+sIqv zVq$IQ^EQ1@8XB@agX_{=rInAk=<=lvdYp3{w7C)QmDAe4L<+chW(+y&C*LU=@L||- z`vdR+ok85^vXX}W_PK*pLhM_ zNixu>s}^JPsiJLvsTP=xbT`LBmK=y7=mzf9HJU=Nq-&)H$$?YBhmMVP^(8H_yBcv94l`h!Q1V5gz$8-pfp!sui6Powl#M6D_4WmXGzu0CntBU~jTr>~7@Oax~XIpw_ zy94m@94u=5d1;W1oL7LfK`N#VpSi8=)O0Z4<6OCo6L|0!e58RJqi^2|-RTfET;09L zfB>j&muN(jDv-{k?{Z|P}h9qmJd^K}Dxpo8BoP7G4&F;)!)(E`suNy_vKmWUm z|M~F$_jaZKCz=1>Mj${v=K>fk9&}=IJ!SWaVKm!fxy2t_mTuM;P?~&1UdnY8;}4468LXmF{OcV*ga556 zEIxy8EA^Gad(;z^ebLF%fvIl4O#2yx{*)r>o}49zCXY28grK~8Eb7H|m3$9Edi+2a znU#4Km+$>~6G%__;d732T0g6&0*GtFWQig@JKLF}^&16dq@1ek=(27@fA1Ak&$C_L z`R$a<;`Tp2>-(A7dWwtPT>ENbv_3L8k8Ox9^Smru^P8ATB_Ela7q#95Z+}8DzglGEeysoT=-?Pfm(r1oA?iw*r}W>gt66WO|Qz|$ER z{4qV-QQ-}Sb$Tw!?b)LQh0KAdke6Q9dSCN+Oy5}j^oaT5*(+MUw?_iEOJ&&5d4Cj> z$Pf-OjoWtY~g9onM&JyZ+06L~Y1_=OCHpOyj5&bBEsaB3VK`2#7; zYMn}~-9^1(?2GKeNd+z+?z@F8Azs6`d#|%+QqIk{u{|Zy0F6`B1><3H&VjOid!U0h zq0js_7Tc`7w0wEak((hZmnMBtL9l@K2|w`se!pin8syn`dw{wV5^YLjgTT?TXUshx zU*f3bte-PIAgfqTViKpy?T?p?-R>YW|E?dt>}c)c&J~F2C`7pn1tMdQgi{2)t@}-* zQ>{-dkCpU$`-N4+o%1l~PX~vA)&zli>w}i{#qoNz0N001k*P2@ApO>c7OAe36*BD9zWT99;$9t8p|83T#qOLhEb`A zpexOVjE(c!33YKS|07D5vlx}KWI?=7tgjM1fJ%J?oBvpO|6I&T%huZ$t9Q$7OzNp& zYKh*YMRB!CY_>>Y;@cy&txvyV(3uRvf86G|fjfnooGH>{&rAPsztZuD2-PR;BX-Vb+2~bh74l{ zGOw5DjsIHPsq~j5=&i-nPm8Q#p7f`yFjHuS19xrGvz0B{Ma+LN_4oEt@zAEJaZ=hg zGrw^3x>m?0eZr{|HNMM<@!7Gg->ZT}l!J)^=TS#-zjoqdtfFkFZ5*6Zc|wM#O5|m1 zxeaR{Tp9_SX~dHzdPOYu3tPEde|+D#xS03L&%OT2yTK%EG?$z3&mSNHr-x<&OSH$I z##6q2L{W3Dgfl?-t`zZ0{B47~kL|?ZVCUU^M?7PAueoTgvUpJ+CA?&Vo9s~0Chala z-gMUH{O{sTKQbo1u<(S=xq=e{Y_g(zt({weuI~_5B)b#uj@2ymTPXWZAgEkQLo<{|NFC^3P67t6shdnN{;CKEG}ul~Tl2;vsYWrG@6 zP{V4ZEqILeow^N6dr1!!uE&&)>-;YdRoFZw;eq(#+C+}0`y*TBet&4)PKa#0ngAH| z@o+3{d`7Uij?ObtSe0tn1vD)~;0m(~i@6crD+#r@N~3Y}L&~kL3G1M}Gp~Z9G_{{a z?@HLN0lJ;}Sf`kv&=X+RwT5NED?A1Z{&9Xg4jHlQedB1w^@f{Uh&$#6-M`v^QF36u zbKh^L<=&;-wFOsc*aRYnHeTuL5$iSKb)}l~_qg1cXRanU_w95ZX1Q34nL&l^cO~qZ zU4KYD{lWC!b`F7a+wJwA6Ka2EdM{D_F!Y!VYyZsc31&%rY z#S=RaU*h*{?GeSpvdG=#QTAz50`4<78?_(mF*#WexSuQdpi8{h|@2(G$y}?Ae*VS>PO6#SdrFXs6Ud$5* zdRMAF>DTgg-20_0f2`D<=W(l>2t;ya4xd@rcv;WjpCDHZqDqPVjfE)@o@zqO(UISm zBFs+D0%*KeBXn^fEuKpP?G2eC@3!Os_K}ub-<4FLr%(QZ`VLTa7+=IJQa%1%Wh|?O zhkGI8XRmGSI4KY4*gZ;62O?l1&G&BXl)v-~4pG%4R*Z+T9Bv^e&NNB8>g{Sj((F(0 zJ3~!=jwUVlLhj1}|DftwSLQ>xMCklcmz^y?Dlt3FwtP{b z_;Xo}%!K__^PloON!{5V7>BF!xGr|BJf+;*Su`IXi_Ax+6-OsyhBq&6Zyr9noi+Em zBbeGWsC6Fs78eO!gVbwe&Y!?>H=|Pc`|P_Px9Pb8n)vc;$ONpf=YF>fe;G4dl>&h( zC{tUg=RA1ow9M^7+;_QN0L{#GC`tb?3I(crd%tUhrO7{Mit0X>wmxCgiV8{QwfwfN zi1lZ`I@R~06pA-PS_?c3dC6Pm|0RHL!p2wngt>g>l^6ACD65wphNiVqoP|DX5n`9M zx5i|ph^e`}&Z~ShxF*wS{h24lWPnS#VLGxb<~hfeZF-e_hN{F${o$%_KxI$)HT4|> zP{eLfx!TqjZ;`B{fu^=FWL>hN5-6%K96sWf;H95Vp5jtP$$93okY!oZb1x=pxrSvC z8RoOW+v|JVb?fu%W_~*eVd&05e@D+BqLIiRO#-RyvV}+V;pqvw^ZuzZF{V}SGY5L{ zABxiM+T#Do8}#JpU{pQqC}lVsT^fz|x4F7osIJhrCagc{Ulwf{@N<6nMe}@^_Q>YW z+jBIpuTe<4#b#A0fr~I^uM;o44t=;gsQW8w?!AOoFr~^c2Vv#xbv~A2Xgg-F0+whB zCo-k~w#p0fq=zM=^E3TPmjq05R(7oIGL?ck;8~ipQ8a1xx8_Rq>wOY17hMEzI3|<% zEq0I#QI|aKd{&YkkDiqsFz-p=|WmUD=dxeyHzs~w3d+UjI^xVDu z;O+7qftbf4TwTa1+Olbi`~y1O67jo@N2Gl*rV%J%WiIJm=+ykUTVz20lxUSv=KKj3 zbq}ucH+AtanfQcZ#Eb5k)ILPeJrNI{FJSahP!6t1h>hdTFgCREo;7(_T;FO-xD6m; zc=crnw88Nu2(-J~X^Gz9d%VrW!nN@249_q4_hJkM;UMeA&j_j+b+#Kf?3K;G z8?;qaMWLI0y;5Yn-b;A^HH#GgKJMh4<0Iek2mROeem(vjPskrSerS7;Tx9ZVVdRjB zIOAd)d*?jM@%tm!NRF9c-l#cDRu4Lr<4QY?dFI;gHIpxmYh-~PO)P|$eM^#RsWKGM zK=C?L@S?vE<{4-gJwb!+pNlL#FdH_wtKTkG)la-`@lHau_D%ksvp9cI^)sv>m?Uvi z9~ycg=>AW`O^XSBV(7(#(VbtC2Db3}$%#GZUF5feiaY^Pew4S^0GPNg|JUpVC%I7O zEoGJ8Hh$6Vf{u)~+?A76&wS^&BWnUD66Z*A@7_h~UNfm_Z-^v@qge>aK|Je%t9!$6Dt{pY61t4)EzyY=Dy8IM^Sc}Hxh z?|pZF<-_)EID2h)R@gl5{p^c^c(lM^>61ye4@%pmJt+UQY0n?W#n{L{=(`j1P;}CY zMe8=|U9Qp1xLSD4o(n+qzbyv$4wRyI&uPW=&G2P+hrT$=NpIDMvlszyQ9Aj^OJ_BL#ZVz_-GF9$!bWIVk z1q0M`Bz*UuQ=e5Mje0-JSMo(Ij^SH8p?5NN2Ypf}@9xS*+T`Z7xvOI@ME$>Lz9paHTXO4CcP1e2xJ0paUX4pPXXDOslXz; z8_15uw_x6-FZ`eL!|X*mfz3@z=Q(kdh`Cq7YPPnoX3AtyS&j(Y5q^cQw%ffzUc9I? zrwJPL6byZ#J;>hF1XrhIcK83(n7!16q3f#V9{$@vS z=wp89mq9Z-tDuG@()0O@Kq2)Q-|NtDlFG?Gen_4fujz&SI0DZOqT|S4zEu202?jJ2 z-2@HWJ}Sr`K2y5NjF^R}V+GvYH`Nq)7*%Uc5({o+dVpRFC9gf9f7=FREW@+VCTvCR+6|{8To>vBX^W zS6KXPW{m#uk6$wcD7D4Y>3*y8j!};l{Lot`dFQSqFR&=OJxL&YlOuo^zbNQY3@j{uRBX3e-gxC`^Z^|l4LGvq zcuXyBwHzIF9qJcfJgswPx+o*%u0l?Hjst$_M=dIa|5u_EBB?Cen5zU8Jr-hemk z=6F)?|41TVRDnS$qU-($^Iq$b@whMcAY`P=)cjpWr*HwCC zLDrTUIK$()w-c*Jt0Zvm&o?hR94sN;sVgY^4#Njgw7!xe_0By(k?Dkt!(YP&hc?na z;0Ff9Sf!&hm*@YMf}%6^Ong`L{`s zPR>d#yPxI~%BBR3_+zPT=$CJHqcf2jM7c?le@_8qt}Fa>`~k`;-OFMy(T~n#Nxu5$ zn9U*`t+#vo#A`pL{$D3FI+JwA!xhQ4I5@A&^zR>+J#FM2L>*6WV}B&WMf{zA!syoHfzMTr-}BEC??NbiZ4Nj9pTl(^Y_F@aWwLA znzT-ag)hBo;|U}7efD%^;V-3nfE^515FGVpFFG^iq~xPCnO{oSQF~dEcTGK;>5_(o z)J^yMOzt9*v$j%m9+FmCdOB)y_B00Lz1%$Ed8{h0-#nzhwaFIdnSp|!PsZ|_Ob)dPVLPt z-mn&>-xl^)r^d=phD38P7iwEu$416yf$v&dTP+RrZU2~eclpNB@Qi=NL&|R)@0T7y z#0MbqcXMH%Q&E8IHL@L@3rYsoH7zBC zTDC_yl=^R<^8R1BN!d@ z&qSS9tat_gr-~BQ984-C;FiHE`+vvP>h5thl;z#Ou2EGP__cMfvijgD((jOrDYDg) zH7e(!aw$Yym1irD!-ae6NAH?~p^(QpryuTff;iYMn9aaH`O)aV*?5fUcE{t@k?3&OFRx zY`1IM@MxT9h#Dubj#5PqOW$yF^_EQ#mK^6VbWSp@yk+@9lKq`V=-B=aWfMk-i(Lz$ z4~1rrIx{W<0yrT^(RC2k&WXIdIZG+C!=JA-yqI058~$NtX1SLdS7xDL${DZotu01Z zB2Al(zrdsV%g|I|#6Lx**FKetJ+;W{+0sHf*WQYFF~F9MiAGGtqk~ulCop2Pdxf>|oLP_RvDpyXof?yHDO8|bmsc|)8OQAwwLYW zQep;18*qnbz}7rrD$35_bIk0Fnw&%;O^XFy$B~jEz#kq`9{`Jh5@yr_JXUWr48<+L zMuVwsrtz8cVmko+#yrtw*Q?YWHa0QOnq-aM#}tOGFvY0mT+LGg3I+ymGCABl{2SFZ zU8fqj0kY*E;98hq8oeWTk`!mU@Fo*(ob@z$Cd|D3b0L1zkIKi@xG_RJ(lSm$Q zs>nsHtvQ9H`hfYf$BPW>8yFbBo1H~SY!>tCZO-JvT-GkH?i@wv{OjVIXXQ;lRS16= zEA=!BfgW$(BTfdQ8{YaUUeXA)wCd`2NAY%C)6-U0ytC#E{&1H9kLFMDI`n6# zXHx>7JE0(WUA2yI!t_;UCT8GBTJV&mD)U+DkDPQkZ&{vLV?+M^q&@yEDloczlCXb6 zPoPM~KK@PU3Ro&DkGq7o-x;QVgSXGVoHcmcc2eXr1R|91MnXcuOfrSW;)C9~fTM1C zL;03P+e>Fa)v$J3DGtvE!MxES4^0MFSeW{ky?)euQm-G)8sz_8()sp2fS;%5v)~ps z@CRAnyd}-%e?DIgf~)6T)p{vqIRI(iScv(O_(}Bp{=w+-5fVyxRqu05k>t=3b%TTqv;vrm!5&GnN;>b#?~ z=?q~HJQD=tmw9el++llUS|=}eIZ_Ur*3J|WOAm7+#L zrFGwcHszy0ymFg9kMiHTAnrW&b7v`X)JuS?@Z~UD4ZU^9%QDjuiuFG?3 zAmY9S@n|0Wd&RZmb3s&~pIyt?YWLN%23`qmWpgd7H{WS+GQH(|qxixr=hhv;TyAv@ zjp0eKW$$|5I&87bXcwl`Pjz|}rSARLjB#8J;sROI6+ccnb%m?9pHSy=(9L=)J7ybS z+u0x8E8eJNF|`5W#OCIeW1>s_EViB~db|aUs(#BTeqTz8twUH$T)eF$8A{w*f6R47 z_UV15txZLiO_yWh`Vj_9xEe^rpni6mi*jVJCIUf#Rw>C8^ylQx@ND` z&vSYM*(wNVu zj`e^&qk!$C$&JMvR7q)R)8?iSaF!(7%*7>g(F3Rx))f7ioSdf$-2dlzH)+y!yXA64 zW##bXq>W@szkRv)##3aU?IZFR>N#CQcwv?==;m(X1(tD%nFSA7(L!6p%R&)RG))-o zckeW=y={rHl7Bx>%~FU~TGT89C|~XASyt%i@POY-VCL+V){HCDHR-JHLmq33t14K8 z*ZkU24uc64x~~4>?m$y!V6Lu>RX4E^H^{uaDyn@~xf#U7s|kyIp-rqlnh)dEHgX08 zQ--9xKQ$Ex5*$DWGgkqPT)%XFjJP7^=LeiuLvXqt1PSNe6%t*>uvxORF7 zv(uBN1SCmW>p+@q0hV(^C_(j=P;{#|Nb`67L?wF1mxMiVIbN;T(xppcsI2l_rbqej z2#xN+=Ac2X9OL&YD)=`ekM|>EtlG*QuvLER%&@hwsqT~q=a?t&ff4i9gR&?Y9pGZv z!-7jAD=FR1>nlbHR)=zZGjVanD?5hV zj(#es#!QkgA&yQBBem(-qkH`Lf7QD1`4;nisEoF_sFjJf_+iq__HG`pK3jTuws(P1NwfdQ^#`TaYN^paZCfhDEvY%t)cXbw zyc$e>%x|6VXr_BFI+H*=y_nVMp4e6>ur<>g?e1kjmd@X}OS`|{)Z!9$4T3FiFX{E4 zxa$+mskemCQYLFSycXYjtz4g12NK(<>S@9vrO+?)X)RR%%lnaEC54xA>5W#w>)_H1)*q5(ITwQvgMI9$Zz`7aIxdyS}5PFG)m6I z+-<2`<{9pc++5|ZJT0^M&`5b5ov2)$N!*BIg@D@k$WB$(b0O3^akFcn#n~Il$W$J# zmJbI7TH3Mhvo7bp#}3OkEO`MUs-fi*>DqYK?YL4YnvQKuY1@b!E(( z+A==^A?zIMCG0}AL;bR!C%kxuuu;WY#!?LQt~WYqX-Z-``eQ9XaM8Kil!FWxmIhzf zLIO8dbDXeTJ~vSA-`Nt{)Z7s*?faX%YH0`gog+z^Aa)Zcqry=Tc8OL~=$ukf#j=%C zzUQd@X!(-1z#bddv9t}oB?geV?nwuy=h%C1z8^Xpf)wW(GO68R@N3!Fh--9mF|-3- zN(_38CdN(!dv;R88{1wPt}wk(Pv3y*zGhz@F1@KL zmMDBs zJ2(2J-FW%PZc)Ulg6A9hvc<||{nKJ$GABU)e3Dlh zIQvl1DUC%&qJU*1eZ7X2A$+8Z2VVJw(IYXIa>a0iV1H?@ z%NV@Onw+gh1fW~`Q{CzbtCvKUi`G)ZKQ;^bnD!+a>m}(rEz!jw(Jgs8C`tH%M zXAG4J$fjB^EHf0#%isEG@uK8znK8eI|CeZEL*6;=aP#O`mH=QkVZgtxchf$r!pXDD zIFCER&!jc3XXKSUG59v7x6UPySl*<^ zUP4AP2F#)U*Ct$p97IJBhBr_{Q{K^1ZS17?y253%NS!K;q@gYY6)6-{dF>zTcy{)i zvzQ47)x-l#tM9r;T0P&zLC;tXM=f%hZJ&%i7SKScjRrdmeM~&GPgJmu^IHjXxRx}x zJgmAd?fn5t41+Gs>^};`ML=yo>vIR+YTH?9jwLSkG2j=b1#qhog}xCb=~3YyDx3!W zw63IETjd%+a=G1&qoX^1-OmLRV649vO?k5D4g1h{2k-!CUdvdORNAhAm=J4u9pxg= z(Oj4veY&-L+LK4<$JqiXr=3G^K;d>qf^J9kT&bb_6|1Jkk?HBHggNp0K6_6#*kThE zcwlpL=_%bxd{S-Zta%!gu%yS`p~b+!;Jd$(*4^Dbz==iJ2<+qt)b%Mw)(y0xr28Pt z6jtn!*_K5Uh_Ni;^Ih$-YLGl`m4x99rp{~=S1RaCy$kmmos_*zeCpi zQ0~aq`FOEVT*J17goNHr>5+^MS})0$9X2RG;IC`fH;@>c6@H&L#gkbn_y!JynT2cI|<@mtBy57KJ6wC5xj=;_& zv14)xOxQ|WOJUCC$}>xZ9|=CJ9T*Fx@q=memsO6z>lp)K=&CQhEX3K1gta}?Lf{eM z4q*zx>02iR>5~1-ACsQzpiQ}g$bY4oFQ0*s&oD5!`wnr`Uq@-<4qV@?3^@_w(OnBy zuxB!f zy6tfd13tywi0E2NEeO3K-<%=ejad1CefvWVw#@M`va65wNS^I^$$!asD>4T|BGw4ys{<(`Oi0cU~6H!ICY^9Fu%Umz@dyD6cG7~JO&f3yNs>0J32n7D>*5T>?-l= zksmOg*{Zvdf|AGnXr@!e^DMj#v$w5Zf?NJ|+lP2~02n>y&yad*zNm5-PC zx>fg8_VDlEDjD!co6n?)Ys!-*6^1Ylo_WJ=p4d`ZxS+{yN(DjCKtRu3&rD`JZy+zX z+|gdtGuKwe0{3o}#t*TK?>8bLnE;&z#+Kdp6>ZWQ^{r10hq81%3#}k_x&?!~XnahE zwXf3XfQlw}yQ~m>w2G0qiLuFI+n@pz1YKU(3{j6vUAEM87}T+Em(5~Z_bX4*Wk52* z_l}e`Qj2lp5pRnO^(eB;A-TEsAvI5+@rLW=i+<&B0XwJ~i^YJZwu7k4z(8z!dXxd> zJk=yC2?_Y}>6(JvqxjmSj{^iP!6^#J^n}Ec-pzu31MTGlNLJ2rxo;18lRsCZ=|DNvAFm!WLD%{!!O68=Y1xHavEo9sRtC#c zMk@4{{CtvVYfL|{(jA%#v6Ck7o@>7Sv4A4vwzAM&u3R;AMUxKDK_=}jPvHKOcspHP zNrP_BFY)=@7fDOUD@mOx7WP~QEfs>PNW2eE?sI>o5?&?iY5h9Vl{ub9=y@70WUEUr z2^I0JnOIov;ISLELoP*v;qet3=!!VYA)fEF7rE#)OATUtDEA9h!kzMe;KJ?h774Y@ zE=k!Te6!eaYs#rHW3er>pHqt`r33%@z?-gH%Jic+PHk-jS%hX7CEZD^+cP@Go+ml- zlmFBB7~r_^7!@(Jkm|_%DCCM~|A1r)Hc>At)v00$!DI3OSvzpqQzDvHHud@5Q*OQ) z9~hFBO)n;iCkp@(`3fi0fKO4K+a5f>tK4IpJ>*(h@}Q)t2=HUrX-~(xGC8_Gse;&f zgd=%AdisqMKb&uw_~t&&CYR|j$U*X^FMY97v6tG=Mj*xL{JZJzkZ-%+(Hn}D=cPX(ugLEh*1)dI z`6cNPt6r@E@b+)#Mu6{pNi;D>^R)JX@>pco%x+JxH%B3Rik1v*0Noc?+G}ei74`gJ zai8rqjmCfOP>9Zosa-x~TlO<(_Y@b0>$`5_RAjPics+bYu)}?_Y9_g_%)DKi%)(j& z`cvbXNnC4BReHI_btLRq!#PRGnJo4`=t8BHj8>GvlPekYp1FH*&rF<+q(_}ShrNn7 zwgkea57@UHzOh{DA!H>rsD6HB*v*|%OfPBB1)Kl?C3LD4g-=9NU$M*Y4l206weFnJ^cVv@fknK&qE z((MU2g}doGvP-?5_0z?rVsv|1J`0c`{M&5@_+)3nmNBf$`m$^G_!?#rb`KFk*cuj| zn=Rd=f4kOYV+e_Dr*<#z&JW$qSXWw-rXoykeD%+%&F4+;*1ITuU!s50Jlg=m9W3|- z`9Uxghj6pG@xoi6$l8w1jN%NEjDPJ2U+qTeW zdC7Z@i)pu!>|R%g>>k70H9kg&d>V^rg>$qc%V<)h_?}xG57lN;0zeQU+ROgsy^DRu zR+^7X#=5`+>#u8!XQu?eg2}fotpBgYB$!>2Ci~$M{N8c~bd8MhiozVSmPEx|Y%+{Vx$i4vAH*=$v8`WfiPrb1Gze#Qy!F_1?qK%pv z%ShO*-v*&us=POMPujTKV?rnI<*d+tCH)ArxY%vbY5yy6M*7!qiHD4=F#sUH!IM~$sZORhKRP?k(@Q)vYhy;g8CLno^ zJLRw9mU40MLPP6*Bh*W-Dy}SlBWnOBsNT)htoRZR?D2C_x)+sP%X4jUA=TIURAm%%2uI^+#(#$cMRv-6*YABtJY>gW z;il|;@`l#YhL?Hi089)Wm}i$Br~+1f`%B8J#w?S{00{kWORh6q^6s-W)eGPJlsE6% zu!8Fi*i-l|YVd`>x#zu7sVgWK)y6|s^1U{VNMTsW%u+0ErwC7mdXjSeeZ!CI2+%TO zq%QEHUR=(OoSXFNzeUI>JdxrJSnM zDhr@fg-JgQ$>>z8u7PEA>JM-~&eGfRAM>bJ*c5%|kB~7*E|e zr9;Sc36!GzEs*^Sr@CZmW~zpHU?g#vrP^wc&Jm<0L+&H3rk<${+vUD(_NYB0J<^lHXvr2!aF;0NlTnUP*W-8P-CMIFe!yd59sSr}%+1F${2;Q{W5_@D!m zhv0gw5O3PQtbSA5#f>Vrw_FK{F#*wFVM9eyj;N_l>0SJmZsdZ zov@3|*7*Rka$uo1%3w-Xmw=cEL>W1{ZsXFt-`E|?r@dY2dEP4ugpq}I`h(u zCft+YrVkl2%{Co3ZfQ(CU78rPT8o|^)hv=XS!$XN8Ovla$??QGU~tNa*Tk73;qyRV zFb+0dB9z`4e_;BNzvHMzP8?2NVI;^X`{b1-Yl~9UvAh0y|K-AiTG)f5Obn2{@Ba0e zF1G5~_yCkQl}r^W>9e**Knm?M6wENhuGqQV=zRWk_4)D&E`Uwzjuv6X^&ARo{q2!l zp&zojJzdHN{Bjf}jk?N{AwHj1io$K~n?HFa<5ubIHDVQ5NDR z_%ny@zO@<61a!T8QlMpUW0mBWH(-aSk27)O9%Ag7ba&g+#Fa2r(&&sG8;wwhDR2nS=G~kJCy0`I>&+&&CqB-TS3Tu z-prd#c(;-XuQZ@r_d3O?_0@*<20(@G1^+Vp*tY4m=&0k0I7LN@~4oJ{2agirxQL&@z23_ z^G?TwoJyr0IOQ_mTim@=Wbb*TUsOoUgY|Js&^f$Z6bji(FPWqQ5xfZrNmts7^J#O` ze554tsRzK?Gw`vn-CFrFABxG)))Gs45?ADDf?H{SV`RPRvi3%_Eak5*I96iaB9PGQ z=922SGFv388PTp2^1;X8yJaFatNr%cgT>Zv{585?xJCTtCxQn&UgsCk98vM_hgpLm z^+<=6pv=st*&?dLMn43SaZ`_r%($E=ane2@nNBmmD;9Ovr^~ZSBnl=qu%iu`1dqNI z{gBdHkStIzy?GipoRJBRD`k}*sEw6m#MFl)O?Ib$8OVoBE76LfzJU(g6p&lxt0t2d@MWlGG~rwm#25DZ?uMT9opHV?%>V(O|^s( z1<0JJoQ+oUK=PGh?8KWi_^YNX9B82R3kgY8#Ho|7-jF>cyu?ZkGdEHvuN!)Vew{FJ z*KUv0iJGY>?rO^!Lp5<|<0_I$CN)MQukMmv#i;`HcS3VOg$=MgeM^LI`(4^DvVr()|2 z)mP})*ipP`H#wGPqp4E1*NWQS5M}7P`9DPpU}yBneHIH7zC8sVo$~ZAUB*2UmhT$` z?(f~wyTQyblPz_<-tQ4g?!eMr!(HVdr|cpghS|oX;y2u(1dkfi8=QMeKz;(FIx*!c zD-N{|$Ly3{`b?oJ7RlA7eWqnAYvt6Li4N(N?t7P4sG9KiC0aH>8VgtZfk&1+h25dq zTzu1W{}Bs;dj>^$$e7v%X(hXBpQ#SkjKiimV557Zkv-z0IINLi!YZ;SH0Tp?q|tzc zWbN*$%v4M%R~56FTITELxcowHsoYRpp|d;?Vs^jTt@Z+~#_j55vKt1uZ*d(UYp|QA z%aso!;4oY;Mte7RGj+;UIdY#UQy=^U=Gst_aUD42vV#NFOW#`h0Tu;ZShEyBbEHoJ zc3coqJwL*zB&I&XGsER4o*)&5V|ITUxUus&DS)P5pQ&L zh$%LE@m~mjbLu)UXDCp-!Eq$aw=?OP-sb@q)LdtAIhYTSJl+rz0F#kCuRWPKLo#+j zY-l?E@_RUc5$Y;{gBMPv2wS1=oy+xQZ1Nk`n5d_1_^l-3+mqQE zB(Ug&xBu)-OVnRxNfxAN9-Pb|5=hXsgp$kZ(1XfJlT)w`5L)g z0?WC`LgKn7bkcDSnDTe?Yw*8gK}f2C;$HzA<*@1GHSou -#include -#include -#include -#include -#include -#include - -namespace docking_task { - -/** - * @class DockingTaskNode - * @brief A class representing a node for handling dock localization in a ROS 2 - * system. - * - * This class inherits from rclcpp::Node and provides functionality for - * localizing the dock in the map. - */ -class DockingTaskNode : public NjordTaskBaseNode { -public: - /** - * @brief Constructor for the DockingTaskNode class. - * - * @param options The options for configuring the node. - */ - explicit DockingTaskNode( - const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - /** - * @brief Destructor for the DockLocalizationNode class. - */ - ~DockingTaskNode(){}; - - /** - * @brief Main task for the DockingTaskNode class. - */ - void main_task(); - - Eigen::Array predict_first_buoy_pair(); - - Eigen::Array - predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - Eigen::Array - predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); - - void grid_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); - - std::shared_ptr get_grid(); - - void initialize_grid_sub(); - - std::pair find_dock_structure_edges( - const geometry_msgs::msg::Point &waypoint_third_pair, - Eigen::Vector2d &direction_vector_up); - - /** - * @brief Iterate over line in grid map. Returns a vector - * representing occupied cells along the line. - * - * @param grid The grid map. - * @param x0 The x-coordinate of the start point. - * @param y0 The y-coordinate of the start point. - * @param x1 The x-coordinate of the end point. - * @param y1 The y-coordinate of the end point. - * @return A vector representing occupied cells along the line. - */ - std::vector search_line(const nav_msgs::msg::OccupancyGrid &grid, - double x0, double y0, double x1, double y1); - -private: - mutable std::mutex grid_mutex_; - bool new_grid_msg_ = false; - std::condition_variable grid_cond_var_; - nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_; - rclcpp::Subscription::SharedPtr grid_sub_; -}; - -} // namespace docking_task - -#endif // DOCKING_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/launch/docking_task_launch.py b/mission/njord_tasks/docking_task/launch/docking_task_launch.py deleted file mode 100644 index 20434096..00000000 --- a/mission/njord_tasks/docking_task/launch/docking_task_launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - docking_task_node = Node( - package='docking_task', - executable='docking_task_node', - name='docking_task_node', - parameters=[os.path.join(get_package_share_directory('docking_task'),'params','docking_task_params.yaml')], - # arguments=['--ros-args', '--log-level', 'DEBUG'], - output='screen', - ) - return LaunchDescription([ - docking_task_node - ]) \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/package.xml b/mission/njord_tasks/docking_task/package.xml deleted file mode 100644 index e3dc286c..00000000 --- a/mission/njord_tasks/docking_task/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - docking_task - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - njord_task_base - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions - - - - ament_cmake - - diff --git a/mission/njord_tasks/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml deleted file mode 100644 index 121924a9..00000000 --- a/mission/njord_tasks/docking_task/params/docking_task_params.yaml +++ /dev/null @@ -1,46 +0,0 @@ -docking_task_node: - ros__parameters: - # map_origin_lat: 63.4411585867919 - # map_origin_lon: 10.419400373255625 - map_origin_lat: -33.72242824301795 - map_origin_lon: 150.6740063854522 - map_origin_set: true - gps_start_lat: -33.72255346763595 - gps_start_lon: 150.67394210459224 - gps_end_lat: 63.44125901804796 - gps_end_lon: 10.41857835889424 - gps_start_x: 0.0 - gps_start_y: 0.0 - gps_end_x: 0.0 - gps_end_y: 0.0 - gps_frame_coords_set: false - map_origin_topic: "/map/origin" - odom_topic: "/seapath/odom/ned" - landmark_topic: "landmarks_out" - assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct - - # Task specific parameters (not derived from base class) - distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame - # If the distance is greater than 6.0 we drive distance-4m towards the first buoy pair before trying to find it - distance_between_buoys_in_pair: 5.0 # Distance between the two buoys in a pair - grid_topic: "pcl_grid" - dock_structure_absolute_width: 4.39 # Width of the dock structure in meters 4.39 - dock_structure_absolute_width_tolerance: 0.5 # Padding on each side of the dock structure for line search start and end - dock_edge_width: 0.13 # width of the dock edge - line_search_distance: 2.0 # distance to iterate along line when searching for dock edge - dock_search_offset: 2.0 # offset in forward direction from third buoy pair to begin search for dock - - - - - - dock_width: 4.13 # width of available docking space - dock_width_tolerance: 0.5 - dock_length: 2.0 # length of available docking space - dock_length_tolerance: 0.5 - dock_edge_width_tolerance: 0.05 - task_nr: 2 # possible values 1 and 2 corresponding to task 4.1 and 4.2 - models: - dynmod_stddev: 0.01 - senmod_stddev: 0.01 - \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/src/docking_task_node.cpp b/mission/njord_tasks/docking_task/src/docking_task_node.cpp deleted file mode 100644 index ca84d750..00000000 --- a/mission/njord_tasks/docking_task/src/docking_task_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - - rclcpp::spin(node); - - rclcpp::shutdown(); - - return 0; -} diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp deleted file mode 100644 index b58d8a38..00000000 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ /dev/null @@ -1,637 +0,0 @@ -#include - -namespace docking_task { - -DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) - : NjordTaskBaseNode("dock_localization_node", options) { - - declare_parameter("distance_to_first_buoy_pair", 2.0); - declare_parameter("distance_between_buoys_in_pair", 5.0); - declare_parameter("grid_topic", "grid"); - declare_parameter("dock_structure_absolute_width", 0.0); - declare_parameter("dock_structure_absolute_width_tolerance", 0.0); - declare_parameter("dock_edge_width", 0.0); - declare_parameter("line_search_distance", 0.0); - declare_parameter("dock_search_offset", 0.0); - - declare_parameter("dock_width", 0.0); - declare_parameter("dock_width_tolerance", 0.0); - declare_parameter("dock_length", 0.0); - declare_parameter("dock_length_tolerance", 0.0); - declare_parameter("dock_edge_width_tolerance", 0.0); - declare_parameter("task_nr", 0.0); - declare_parameter("models.dynmod_stddev", 0.0); - declare_parameter("models.sen_stddev", 0.0); - - std::thread(&DockingTaskNode::main_task, this).detach(); -} - -void DockingTaskNode::main_task() { - navigation_ready(); - // Starting docking task - odom_start_point_ = get_odom()->pose.pose.position; - // Eigen::Array predicted_first_pair = - // predict_first_buoy_pair(); - - // sensor_msgs::msg::PointCloud2 buoy_vis_msg; - // pcl::PointCloud buoy_vis; - // pcl::PointXYZRGB buoy_red_0; - // buoy_red_0.x = predicted_first_pair(0, 0); - // buoy_red_0.y = predicted_first_pair(1, 0); - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // pcl::PointXYZRGB buoy_green_1; - // buoy_green_1.x = predicted_first_pair(0, 1); - // buoy_green_1.y = predicted_first_pair(1, 1); - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // double distance_to_first_buoy_pair = - // this->get_parameter("distance_to_first_buoy_pair").as_double(); - // if (distance_to_first_buoy_pair > 6.0) { - // geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; - // waypoint_to_approach_first_pair_base_link.x = - // distance_to_first_buoy_pair - 4.0; - // waypoint_to_approach_first_pair_base_link.y = 0.0; - // waypoint_to_approach_first_pair_base_link.z = 0.0; - // try { - // auto transform = - // tf_buffer_->lookupTransform("odom", "base_link", - // tf2::TimePointZero); - // geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; - // tf2::doTransform(waypoint_to_approach_first_pair_base_link, - // waypoint_to_approach_first_pair_odom, transform); - // send_waypoint(waypoint_to_approach_first_pair_odom); - // set_desired_heading(odom_start_point_, - // waypoint_to_approach_first_pair_odom); - // reach_waypoint(1.0); - // } catch (tf2::TransformException &ex) { - // RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - // } - // } - // std::vector buoy_landmarks_0_to_1 = - // get_buoy_landmarks(predicted_first_pair); - // if (buoy_landmarks_0_to_1.size() != 2) { - // RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - // } - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; - // buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; - // buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // geometry_msgs::msg::Point waypoint_first_pair; - // waypoint_first_pair.x = - // (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + - // buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / - // 2; - // waypoint_first_pair.y = - // (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + - // buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / - // 2; - // waypoint_first_pair.z = 0.0; - // send_waypoint(waypoint_first_pair); - // set_desired_heading(odom_start_point_, waypoint_first_pair); - // reach_waypoint(1.0); - - // // Second pair of buoys - // Eigen::Array predicted_first_and_second_pair = - // predict_second_buoy_pair( - // buoy_landmarks_0_to_1[0].pose_odom_frame.position, - // buoy_landmarks_0_to_1[1].pose_odom_frame.position); - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = predicted_first_and_second_pair(0, 0); - // buoy_red_0.y = predicted_first_and_second_pair(1, 0); - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = predicted_first_and_second_pair(0, 1); - // buoy_green_1.y = predicted_first_and_second_pair(1, 1); - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // std::vector buoy_landmarks_2_to_3 = - // get_buoy_landmarks(predicted_first_and_second_pair); - // if (buoy_landmarks_2_to_3.size() != 2) { - // RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); - // } - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; - // buoy_red_0.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; - // buoy_green_1.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // geometry_msgs::msg::Point waypoint_second_pair; - // waypoint_second_pair.x = - // (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + - // buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / - // 2; - // waypoint_second_pair.y = - // (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + - // buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / - // 2; - // waypoint_second_pair.z = 0.0; - // send_waypoint(waypoint_second_pair); - // set_desired_heading(odom_start_point_, waypoint_second_pair); - // reach_waypoint(1.0); - - // // Third pair of buoys - // Eigen::Array predicted_third_pair = predict_third_buoy_pair( - // buoy_landmarks_0_to_1[0].pose_odom_frame.position, - // buoy_landmarks_0_to_1[1].pose_odom_frame.position, - // buoy_landmarks_2_to_3[2].pose_odom_frame.position, - // buoy_landmarks_2_to_3[3].pose_odom_frame.position); - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = predicted_third_pair(0, 0); - // buoy_red_0.y = predicted_third_pair(1, 0); - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = predicted_third_pair(0, 1); - // buoy_green_1.y = predicted_third_pair(1, 1); - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // std::vector buoy_landmarks_4_to_5 = - // get_buoy_landmarks(predicted_third_pair); - // if (buoy_landmarks_4_to_5.size() != 2) { - // RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); - // } - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = buoy_landmarks_4_to_5[0].pose_odom_frame.position.x; - // buoy_red_0.y = buoy_landmarks_4_to_5[0].pose_odom_frame.position.y; - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = buoy_landmarks_4_to_5[1].pose_odom_frame.position.x; - // buoy_green_1.y = buoy_landmarks_4_to_5[1].pose_odom_frame.position.y; - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // Eigen::Vector2d direction_vector_up; - // direction_vector_up << (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x - // + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - // 2 - odom_start_point_.x, - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - // 2 - odom_start_point_.y; - // direction_vector_up.normalize(); - - // geometry_msgs::msg::Point waypoint_third_pair; - // waypoint_third_pair.x = - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - // 2; - // waypoint_third_pair.y = - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - // 2; - // waypoint_third_pair.z = 0.0; - // send_waypoint(waypoint_third_pair); - // set_desired_heading(odom_start_point_, waypoint_third_pair); - - // geometry_msgs::msg::Point waypoint_through_third_pair; - // waypoint_through_third_pair.x = - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - // 2 + direction_vector_up(0) * 2; - // waypoint_through_third_pair.y = - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - // 2 + direction_vector_up(1) * 2; - // waypoint_through_third_pair.z = 0.0; - // send_waypoint(waypoint_through_third_pair); - // set_desired_heading(odom_start_point_, waypoint_through_third_pair); - // reach_waypoint(1.0); - - geometry_msgs::msg::Point direction_vector_up_base_link; - direction_vector_up_base_link.x = 1.0; - direction_vector_up_base_link.y = 0.0; - direction_vector_up_base_link.z = 0.0; - Eigen::Vector2d direction_vector_up_test; - try { - auto transform = - tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); - geometry_msgs::msg::Point direction_vector_up_odom; - tf2::doTransform(direction_vector_up_base_link, direction_vector_up_odom, - transform); - direction_vector_up_test << direction_vector_up_odom.x, - direction_vector_up_odom.y; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - - std::thread(&DockingTaskNode::initialize_grid_sub, this).detach(); - Eigen::Vector2d edge1, edge2; - // std::tie(edge1, edge2) = find_dock_structure_edges(waypoint_third_pair, - // direction_vector_up); - - std::tie(edge1, edge2) = - find_dock_structure_edges(odom_start_point_, direction_vector_up_test); - RCLCPP_INFO(this->get_logger(), "Edge 1: %f, %f", edge1(0), edge1(1)); - RCLCPP_INFO(this->get_logger(), "Edge 2: %f, %f", edge2(0), edge2(1)); - sensor_msgs::msg::PointCloud2 buoy_vis_msg; - pcl::PointCloud buoy_vis; - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_vis = pcl::PointCloud(); - pcl::PointXYZRGB edge1_point; - edge1_point.x = edge1(0); - edge1_point.y = edge1(1); - edge1_point.z = 0.0; - edge1_point.r = 255; - edge1_point.g = 0; - edge1_point.b = 0; - pcl::PointXYZRGB edge2_point; - edge2_point.x = edge2(0); - edge2_point.y = edge2(1); - edge2_point.z = 0.0; - edge2_point.r = 0; - edge2_point.g = 255; - edge2_point.b = 0; - buoy_vis.push_back(edge1_point); - buoy_vis.push_back(edge2_point); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); -} - -Eigen::Array DockingTaskNode::predict_first_buoy_pair() { - // Predict the positions of the first two buoys - geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; - geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; - buoy_0_base_link_frame.header.frame_id = "base_link"; - buoy_1_base_link_frame.header.frame_id = "base_link"; - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - double distance_between_buoys_in_pair = - this->get_parameter("distance_between_buoys_in_pair").as_double(); - - buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_0_base_link_frame.pose.position.y = -distance_between_buoys_in_pair / 2; - buoy_0_base_link_frame.pose.position.z = 0.0; - buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_1_base_link_frame.pose.position.y = distance_between_buoys_in_pair / 2; - buoy_1_base_link_frame.pose.position.z = 0.0; - - geometry_msgs::msg::PoseStamped buoy_0_odom_frame; - geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - - bool transform_success = false; - while (!transform_success) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; - predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; - predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; - predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; - - return predicted_positions; -} - -Eigen::Array DockingTaskNode::predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, - previous_waypoint_odom_frame_.y - odom_start_point_.y; - direction_vector.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 5; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 5; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 5; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 5; - - return predicted_positions; -} - -Eigen::Array DockingTaskNode::predict_third_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3) { - Eigen::Vector2d direction_vector_first_to_second_pair; - direction_vector_first_to_second_pair - << (buoy_2.x + buoy_3.x) / 2 - (buoy_0.x + buoy_1.x) / 2, - (buoy_2.y + buoy_3.y) / 2 - (buoy_0.y + buoy_1.y) / 2; - direction_vector_first_to_second_pair.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = - buoy_2.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 0) = - buoy_2.y + direction_vector_first_to_second_pair(1) * 5; - predicted_positions(0, 1) = - buoy_3.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 1) = - buoy_3.y + direction_vector_first_to_second_pair(1) * 5; - - return predicted_positions; -} - -void DockingTaskNode::initialize_grid_sub() { - rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; - auto qos_sensor_data = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), - qos_profile_sensor_data); - std::string grid_topic = this->get_parameter("grid_topic").as_string(); - grid_sub_ = this->create_subscription( - grid_topic, qos_sensor_data, - std::bind(&DockingTaskNode::grid_callback, this, std::placeholders::_1)); -} - -void DockingTaskNode::grid_callback( - const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { - std::unique_lock lock(grid_mutex_); - grid_msg_ = msg; - new_grid_msg_ = true; - lock.unlock(); - grid_cond_var_.notify_one(); -} - -std::shared_ptr DockingTaskNode::get_grid() { - std::unique_lock lock(grid_mutex_); - grid_cond_var_.wait(lock, [this] { return new_grid_msg_; }); - new_grid_msg_ = false; - lock.unlock(); - return grid_msg_; -} - -std::vector -DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, - double dx0, double dy0, double dx1, double dy1) { - int x0 = dx0 / grid.info.resolution + grid.info.width / 2; - int y0 = dy0 / grid.info.resolution + grid.info.height / 2; - int x1 = dx1 / grid.info.resolution + grid.info.width / 2; - int y1 = dy1 / grid.info.resolution + grid.info.height / 2; - - std::vector occupied_cells; - bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); - if (steep) { - occupied_cells.reserve(std::abs(y1 - y0)); - } else { - occupied_cells.reserve(std::abs(x1 - x0)); - } - if (steep) { - std::swap(x0, y0); - std::swap(x1, y1); - } - bool swap = x0 > x1; - if (swap) { - std::swap(x0, x1); - std::swap(y0, y1); - } - int dx = x1 - x0; - int dy = std::abs(y1 - y0); - int error = 2 * dy - dx; - int ystep = (y0 < y1) ? 1 : -1; - int xbounds = steep ? grid.info.height : grid.info.width; - int ybounds = steep ? grid.info.width : grid.info.height; - int y = y0; - - for (int x = x0; x <= x1; x++) { - if (steep) { - if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { - occupied_cells.push_back(grid.data[y + x * grid.info.width]); - } - } else { - if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { - occupied_cells.push_back(grid.data[y * grid.info.width + x]); - } - } - if (error > 0) { - y += ystep; - error -= 2 * (dx - dy); - } else { - error += 2 * dy; - } - } - if (swap) { - std::reverse(occupied_cells.begin(), occupied_cells.end()); - } - return occupied_cells; -} - -std::pair -DockingTaskNode::find_dock_structure_edges( - const geometry_msgs::msg::Point &waypoint_third_pair, - Eigen::Vector2d &direction_vector_up) { - double dock_width_structure_absolute_width = - this->get_parameter("dock_structure_absolute_width").as_double(); - double dock_width_structure_absolute_width_tolerance = - this->get_parameter("dock_structure_absolute_width_tolerance") - .as_double(); - double dock_search_offset = - this->get_parameter("dock_search_offset").as_double(); - Eigen::Vector2d direction_vector_right; - // Rotate direction vector 90 degrees to the right - direction_vector_right << direction_vector_up(1), -direction_vector_up(0); - geometry_msgs::msg::Point waypoint_third_pair_map; - bool transform_success = false; - while (!transform_success) { - try { - auto transform = - tf_buffer_->lookupTransform("map", "odom", tf2::TimePointZero); - tf2::doTransform(waypoint_third_pair, waypoint_third_pair_map, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - Eigen::Vector2d line_start, line_end; - line_start << waypoint_third_pair_map.x - - (direction_vector_right(0) * - (dock_width_structure_absolute_width / 2 + - dock_width_structure_absolute_width_tolerance)) + - (direction_vector_up(0) * dock_search_offset), - waypoint_third_pair_map.y - - (direction_vector_right(1) * - (dock_width_structure_absolute_width / 2 + - dock_width_structure_absolute_width_tolerance)) + - (direction_vector_up(1) * dock_search_offset); - line_end << waypoint_third_pair_map.x + - (direction_vector_right(0) * - (dock_width_structure_absolute_width / 2 + - dock_width_structure_absolute_width_tolerance)) + - (direction_vector_up(0) * dock_search_offset), - waypoint_third_pair_map.y + - (direction_vector_right(1) * - (dock_width_structure_absolute_width / 2 + - dock_width_structure_absolute_width_tolerance)) + - (direction_vector_up(1) * dock_search_offset); - // double line_length = std::sqrt(std::pow(line_end(0) - line_start(0), 2) + - // std::pow(line_end(1) - line_start(1), 2)); - double dock_edge_width = this->get_parameter("dock_edge_width").as_double(); - double dock_width = dock_width_structure_absolute_width - 2 * dock_edge_width; - nav_msgs::msg::MapMetaData grid_info = get_grid()->info; - double line_search_distance = - this->get_parameter("line_search_distance").as_double(); - int search_iterations = - static_cast(line_search_distance / grid_info.resolution); - int result_index = -1; - // int line_vector_size = -1; - while (true) { - std::shared_ptr grid = get_grid(); - std::vector occupied_cells = search_line( - *grid, line_start(0), line_start(1), line_end(0), line_end(1)); - int max_result = std::numeric_limits::min(); - int max_index = -1; - for (int i = 0; i < search_iterations; i++) { - int left_edge_left = i; - int left_edge_right = - left_edge_left + - static_cast(dock_edge_width / grid->info.resolution); - int right_edge_left = - left_edge_right + - static_cast(dock_width / grid->info.resolution); - int right_edge_right = - right_edge_left + - static_cast(dock_edge_width / grid->info.resolution); - int left_edge_occupied_cells = - std::accumulate(occupied_cells.begin() + left_edge_left, - occupied_cells.begin() + left_edge_right, 0); - int right_edge_occupied_cells = - std::accumulate(occupied_cells.begin() + right_edge_left, - occupied_cells.begin() + right_edge_right, 0); - if (left_edge_occupied_cells == 0 || right_edge_occupied_cells == 0) { - continue; - } - if (left_edge_occupied_cells + right_edge_occupied_cells > max_result) { - max_result = left_edge_occupied_cells + right_edge_occupied_cells; - max_index = i; - } - } - if (max_result > 300) { - result_index = max_index; - // line_vector_size = occupied_cells.size(); - } - line_start(0) = - line_start(0) + direction_vector_up(0) * grid_info.resolution; - line_start(1) = - line_start(1) + direction_vector_up(1) * grid_info.resolution; - line_end(0) = line_end(0) + direction_vector_up(0) * grid_info.resolution; - line_end(1) = line_end(1) + direction_vector_up(1) * grid_info.resolution; - } - int right_edge_left_index = - result_index + static_cast(dock_edge_width / grid_info.resolution) + - static_cast(dock_width / grid_info.resolution); - - Eigen::Vector2d dock_edge_left, dock_edge_right; - dock_edge_left << line_start(0) + - (direction_vector_right(0) * result_index * - grid_info.resolution) + - dock_edge_width / 2, - line_start(1) + - (direction_vector_right(1) * result_index * grid_info.resolution) + - dock_edge_width / 2; - dock_edge_right << line_start(0) + - (direction_vector_right(0) * right_edge_left_index * - grid_info.resolution) + - dock_edge_width / 2, - line_start(1) + - (direction_vector_right(1) * right_edge_left_index * - grid_info.resolution) + - dock_edge_width / 2; - - return std::make_pair(dock_edge_left, dock_edge_right); -} - -} // namespace docking_task \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/CMakeLists.txt b/mission/njord_tasks/maneuvering_task/CMakeLists.txt deleted file mode 100644 index 6f3e7a2a..00000000 --- a/mission/njord_tasks/maneuvering_task/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(maneuvering_task) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(njord_task_base REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) - -include_directories(include) - -add_executable(maneuvering_task_node - src/maneuvering_task_node.cpp - src/maneuvering_task_ros.cpp) - -target_link_libraries(maneuvering_task_node - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - pcl_common -) - -ament_target_dependencies(maneuvering_task_node - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - njord_task_base - pcl_conversions -) - -install( - DIRECTORY include/ - DESTINATION include - -) - -install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME}/ - ) - -install(DIRECTORY - launch - params - DESTINATION share/${PROJECT_NAME}/ -) - - -ament_package() diff --git a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp deleted file mode 100644 index 731d5920..00000000 --- a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef MANEUVERING_TASK_ROS_HPP -#define MANEUVERING_TASK_ROS_HPP - -#include -#include -#include -#include -#include - -namespace maneuvering_task { - -class ManeuveringTaskNode : public NjordTaskBaseNode { -public: - explicit ManeuveringTaskNode( - const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - ~ManeuveringTaskNode(){}; - - /** - * @brief Main task for the ManeuveringTaskNode class. - */ - void main_task(); - - Eigen::Array predict_first_buoy_pair(); - - Eigen::Array - predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - Eigen::Array - predict_next_pair_in_formation(const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green, - Eigen::Vector2d direction_vector); - -private: -}; - -} // namespace maneuvering_task - -#endif // MANEUVERING_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py b/mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py deleted file mode 100644 index da8a4eac..00000000 --- a/mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - maneuvering_task_node = Node( - package='maneuvering_task', - executable='maneuvering_task_node', - name='maneuvering_task_node', - parameters=[os.path.join(get_package_share_directory('maneuvering_task'),'params','maneuvering_task_params.yaml')], - # arguments=['--ros-args', '--log-level', 'DEBUG'], - output='screen', - ) - return LaunchDescription([ - maneuvering_task_node - ]) \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/maneuvering_task.pdf b/mission/njord_tasks/maneuvering_task/maneuvering_task.pdf deleted file mode 100644 index 94df6470062bb5ad4f858ba2095992259920d75e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 120289 zcmeFZWmuG5`#x$Sp&}qHEnU(zbV+v(Aq~>akSZYE(j_1uQc^>M(%n4_NDe)~NY}of zSA2f&uH)EW_Lsf?d|>97x$kwaxYiZtd9I;Vm6BoQVB^E2U0xX;$K;}XMd@hz783-* zWY>f^Sx~a;s+hjDFmuCX*Kjv=`_CtG_9j*qnCv<>=5E%M96X%-nC!1DY^`dG&q%6!F&4G3mEgYt7wFOF*2&+R$6`}czd@^HxC_y6nl|KSXXaAX*B zUmP=!8m>=w5gSTd5_D)UFt%E=!7@CZ*()9SP97Ots0>Z`-nMs&zWMNZ+8bH&GHt0Z zIEVY15_OjZ>X6@+ktM90hK+{G$(O!jYBBTs3voVD>kN6E;rl;qC}hrG#|z_-+e6I1 zjgr5m*v}1ke7kI)_id#R{s-7N z%}r}0h2dOktf)O?CMPGHJqa#x!fJbRDr|mbws;aka@)a$c(p(ptrxYMI{}#=744^O z72jVpoy@E+J|+Tz3tHXtu#ho37jy=co?q;TM|ZuWR*(~QQ-&d){&!?bRXP`Pf0tthVOe~fXTw|W!kP?AiT*0`4U#gJ zXtd56&Cw0XWnZ)Rt;tI> zl`!{QMq!DXu!_k`&};%+R|%nAKwW!b3DS&#lS^^3-G0;8%dOZKfrCfJCE zbLTVNiJaqI{o(dn*w{L#WVCMUTqtkbx63-j5A__=xxu;XXxh>xQ($r4t0myEmFjTb zAjBX%k)s6BJEzr*zvU6wFJkPoRMS{UERcB#2gX82<{fU7+)ahJHP_SM*0^X|FA(n6 z(&ZG%9u%!#FHVDnyVy2F9K@#9)P$XjIq%_5Ryzi!op+g1jyZ zc+#o=rNlDKgv;NcYTC(~$q+4GxVg7MJ{i3^$wL@Y{*@y-&_&PH$c5aHC_h==xkaE` z<9tbL4~L*W$&Pv-Q^*`Tu+T|U`$lQn3rhg|&oum?k3z+)T$<|P-y|DM<=jN(i%Y43 zbef%g3?n})fY641Q`f3*!e0ut&g|%ghq*};cecz@{wlz<;kO@a!_a%udL3zZ-#_&! z8Y=j`k~X&(1qOr=A=6q!JHq|;zi^`7$ZaGJrXMmt9$(S`FHj? zLd9R^V#%!H*j!iG9d+&g&O=*jdqUqwft8AoI>F!M;0Ad^rR08ZKLZ`x36Hg(ja+`ioO5(8J%59M2w8pg%Q21yd1C3R zds;6Fdm+_FAc?!Z?}$nL%yccPHF#nI;S9@KIdv zX`lS|K=55%KqOr_PPnVf9K?j!xH+yH1LE}!QEx@6a@DxEk?l4pd!rw)j8wA9?qkb; zKsQo)(QS#3v&LQK-LehI<-E7N`NSf5B0QAa=$q5~9qgQ1r#&0&QhDk|67LvRt@@{a zJvDh>VFHaOSCA*Okjk7Qqi!M`7xLT9GV&@?{;|BlCpF@8fta$+*fKc56NstHcBqU( z!dNUm+RL$fS?=8w!%sqYMEx{Y*uCQu37Nr@d5ZWam0ID64oCirm|!KuQkdr5v>%iphLG!AE2OJrcrBwh`#Tiw&m`2 zl=G-kfl)jxt(j9leLrP29+mQ<#(+3gS-TDxe=|Kh6119(lo{CK^ey-Wn0AloG&zI; zWH8%c&Ffbf-Vk%Z=qHTM= zvwJ2&v<7O3$F&!ZDfAasVY7*3Tg6cP#hZ2RIe%E1xWzq*6q{LH{k3%f->64@U{6TD z=ync&jn~Y!?Z{2Z;oA9L=&HcpjZuvao0zUC-QB z^>tUrv6N!_G5kKRK+ZBl%p(O4x#n$hgjyL>J!E$43EFUyeZ4sB14cUC#jC*!e!be% zju(F|{zoBetx1lXPHGQC%KHk{@m=N!R6ru;oObl`dxVEU(ShD0s`nfTDG`HTsJS={ zOLbpT;++)|r&iAfD^#erBQDMluXe4+iB>W~2Iz$ZAnoEOV~N3Igj_{IC6XnXQhJ1& zeVw>mOKi`X!AHr1-GR7O@*jUc8t9W8O3*r8B~%b1Jb(L3h1wr?d^a5fMqT@MV#x<0 zrT?_O1abPSzfD6DSusfpwVB8X0W9^Wj-Iqs+Mf8x8tCg^KlX0AuPBQ?Iivd}aNE9? z$I_J7N&H&x6WVokCI7YM)8u zF|=gN*Ys9Y4vOOZRr7S>JAjbXdqV~fS2+?)YIaZIMVS0{9NMUXjV0b0rPHh^F=m-| zT{dlSr?b7o;w|36ecF^hOj|RmY7E(kO2td7A5_i4LF3UF`NCj(cNKKI0b)HL#-n0+ z9=vp6K3-r_t4N{?8KtFfh|WBM>4p(qF_DA?$p{0HY9$$oUO&`FGxi$~xAO4E1BWE! zLomJ0mGA^MxJSE$EF)O6dxOcu3e=~m_ag1NZ}2#_Ol2($moCo(+$On7(SMv;sa_|P zCPZsGFkpSDUU)$xq6f5q7o2&cQ}2p{5LEe>@%iDC=Q zenT`duVEg6>EEd*hGq@T>39?5dyI3!2=pY|j~9U@gRY^~UWn9glPg^BLB!HMdA29A zpAstb()Z0}bQlOO*S;I^DKUWghG}CSm_>kLLN)|=7Kvzn0&fmKlpf@G){)iD#@fyw z3$fIf0!C+yeXaM_gsnpsNF4N7m()upa(Bai>hQde1nj~UnJr6@MdFf}N$8t8Z;&jz z$u=Vhx%>t~QY23f){F)wbU#gX{qSFt&FV^SXgPMdeXYk@RZW-{Fg{Gr0@+rYW03P; zlqN4buKz+u2zYVs0*7MdNTcCl3713Z8;aWbUzILdq1=bVZ0!M)?sFyS^4di;M}oUV zdgNJ5SSqv%@!+_`M;%q~G3Isd7Z4?1ho`Zpl_%y_1O_xol47cn^Yp?w> z`et{8wjCq8pIHVnKS?xI*^5$u$wYDiW9wD;YpP6 z9cmxT=>AS>#aXi}5^u1Em&C#s>dl=VYp7l>aM|4#=-GRH+FdEMXSsJ3h}DuyYAs^h zake86V|!3p<;!l@JXAXG*(g!k6$vf07FolIlMUTL?FWDRRh)KrF}UaJtLUfy#=)M( zCIqN~q20WI%FRNuJj&ueZapg>zu{wvvUu4qj9!OMH9PTX1GCQQ$77N!WcGn9R0yiZ zit~~C@Rapx_!JKm_O)J5F2cjbiIo?QScQ_7jI2eEct==@xS4y*okTOe-@{mhJS8j7 zu+y1_R~U0ESq2?Ie@`>c+sf81W7T%WuWi?E+C4k8CQbGewe){;kA5DM+^tQ>(9H4Q z&PsjQtZAsnEqniY_V?$&43aG~MH7q#8LU9zZ#>~$6wF`MDzJn8PgYI`;iARdtm z-@&*=EsV5`kW1PNUIphaV0+f;H1y`{7CrUzf6-_PTgPJeoVTo9ELg&w&4~AS^d$*Z z5#z8?W_O?&y$k)vqY>ev;Gwe;Wx1P|siTpT3s8i$v=qr~v1e=r<-ab!$)`UO~FY-?b?(G|Dae-T^GusXlNJl#8z>Y{(+^-)(>VC`mvwx8RxBQ#7QHmZW zqE^YR(RZt$Mu;Z%mbCBQ(>nJ9zRi8r+GKJ!R7k3Ib*rj|j=6W{wPF}{3(>ifOKc2S zjWsmmx)!l*ysAfY`8oGv<)-YCO>zo3ucY*fA|GhDu?Lw_-vF-aliQOD$Z|W83`;mgXU*zJ=sVcB=&;-4a z<+B)uxTVL+etAgeWQ?80BqeCxTLRU*EE|IpY+Hz28Y_T>l1(j00%o^s$qw#@Ro|0! z%*=R3omzqzRWyzBM+{5+7z6UNsKRSnp*%9xtnqbFaQyLqEf{m`HNdW-ib(K>VeDoBEE1uqHYF-u_k$O?Wa(PZ{)BJ@m{khEm3 z-fQ+}l|_ph@im78OJ1*eQ1mC)A_R`U9kG-!bwPJbL=fzdnXx1e`rv=&AGsich?D&i zY_MYa$biWxry(|cBpp*OLidyKB$aEQcJa;bcjBvpvZOTdR+`jWJPQ4Ch5ts^eieh+i(wdgusOEkT$#Ad_ zc6R%37iuv?6B$yobZUI0A3!q?VzwETVd?DCvAuS`Sx87$nRWA0ezW>9;l_^tN>uA! zqut39t|BQ{JhfugH!g*6uCq zRX#qW*Q-*|?E?ynLOVf;W2i$!c|O7V><4n0je8BV(|){}S26S2ghxPP&3&EQ4QzH` zZ_t>Fq&<@hHIP!gpy=vl%OByG&zotpZ4Umgm}uVx8+#HqSDx`&xsQb;mH8euXat z5(CtBbLNe#Br6@x{uL$JCS`5LL;9u91TpLz8=_P5Y%TLKtWL$k?6_A+taOJH@DDbY zKLhdYZdk@ny!Qp!aW_w6@D%u5vLT^&e$3SWHTP_i!!{jq6DIaQvF(Ic0K48qjJnjT zXzVjjbC_;!m^M*?X|rW<%%#nYUWGX23Xu1uLpch)$?b=)1s8xk>LGM(kHDKKlcd1Q18{`Vob)~M1LGSG+7=K^ZO91EZX) zn~{4tL__RJyh!TWCyKusqS8tOJcGda%wYG6lw z@_5dEvyh?MgWy+j8^wn~>9q9G~anAZs?biW6ixvXjYot3svZ`4{W&GKHzh5DT<8><{^IQmv7 z!ax4^stPq{6gJc0ua$gq3^sr(oQgBV0(%u3|d9g|DpwVh}2{(dT7Qp zoegSa?1}R}s!S3@Jo2Ug~09+%js&_2j^Kj zIkmQ*q^!|a-;9+Hk46=jEy~6^%~t64&R(0$YCNAa9-(#L*J>a=@86#KrCBoYaV5Z3 z$b{{ke@gEPS<(XTpD;Ro-`~~RgBX- zI-`J~ehP&SSOQQ+ePDIo^@+woh)5^l3!Ma1#{Nv0T6>WB7;=DO=YZ#j6e(-IyJ9`Ku{I>uia+Zuz?U9>sYm z4Bfw?wq$kGj)O8QbIbkHPJG9hJbF!!nKTq!ss_^v*(<6%es)_azdMX5u%N=zMs9Jr z@BT36!v^MYgU@Un*iFF$RLS1tWcq}ke(co^4m}Os5OB5uVE(tY8@jty9sn{`3g0QF zGd{X@_&slY#pcl3`>paqYqfzr+FR9^X_SOpHP;31wK?#vA&UPL0>eNJab>3G!VDMN z`ndhYGkjr|ILqyk<>86F~|-%$eQBFQ?XgA9NUZ+H8mFHfK)kd>@&LobR9q1z#RTMF#xx(q)V>mf~#0y&FhZEt%i6p=_5SV-@pH~Z`U}&&8bD^hKO>n?ro6M zF1-(b&Adf_3)$aks^!n?KhI}BV<8!Rz$u}_D?vwdIsU4;*W!^P+nZO$0m*Kp%NFJL zIGvG)Hz`D~@K-iGSS|=qO49`cg!n?Yga~bh;UN0ltNnpuhkEJ>P0gxZ)S>F0B!Z=ykIEqlDkN8ef|Eno&WE>V4`<(Wg!U% zaiyCbZ%3#4rrPTKTfMTG!Gzv(NJwjc~VI>nT~Zz#53nhvnC&H<%0 z;oy09<=54-Q-FaD9V9eLD${4*{%-bvxFJQPK`!~&jkIcH`puW4Tx^EVluniV6S2j1 zH>p{;MGpl=^d6D@_Y?kk)bCNHwy@Ux(o6twVc!(rE{*voJYJANfof}R#qg5~@Bj4Z zPuDc4l-u-LQ?4BWe)XCI=+)O#>8iv12>>zlXzqHxi}&RJ^b7|&{ZSgzDGDC|ncb4H z{ty!g0=#5gX0=0WVl$rbj46)1_upaPj=vzrX?gf0*aNIZ3eC$a?tbpacK2cQi`maF z%K(S!_J1*u3T4x&$$Xy1nAmD>lK8nvz3Ip zMUkJudH0lI>Dgb<&Nno6>-P44UwlJVk8Z2hJNr43ZGW%bKNz(TK@a%%y8rd&|GzVU zi4K(J>}hY@r?-rG1vHSH-8Tv2jp5^E1R^gXKKC}FSqgNGG9rhH$oVn zb~}m73Am5N6MuTmp3upgalKejb<^9fp@qRsiJMC`;qKd0*|0b&`XRi>4RHE6F7szt zMc#3gZMXe?M=d!t5*S~{6LLeuZR^@-WcHTZ-neQuS*dkX)b#!0IbOg8Nf(dAY)k-4 zqv=g^OKXYkxw~kkQy}l6I|--z@|`xBp9nZ##-%L2)x3*rtB~`GQK; zYac_4#?N7&rL+mMV3LrUGPa9X*YIg|=$K2L4mMGfo>wjXPdLMF%hU^)u z4rmbZ8#2&;;a(NHTy>Y?-*AVy0hA>-Ow<=ZmhhkRu%8|RLq6;nq2;2|(Q}yqQ1YNc z@gAP&7u_R12K!ob%PQyx_m*evYq+(5ln1fVrjh~ zOcB$_G}c-YEt^%k-0~dVz6`h^0O9bogQAUF%=}+h1DVJRdDE4m0D4FM*Ou`- z>~QzYXkSgl zn}tMWv^v&z`3av|E%~-R# zD!<{Q^oFS-UE(EGL7z`Mf-cbj7HYJ=Cdk@4<$k2`&2$ihS>no!KW9$1*5=EtTe@?$ zOsEvW+#?{gL{ZEbPi3`DHC=Z`HQKfbSvxNUR4+8s-dR3It?q;->!0<~gV%XMX~(fg z=vT*~w3dDaB@>BO*4?^=0>gV32CX)v!93jC6RlpsTfYsmkuxFJ7AZgqSG~!>1Qu=m z2dlQP@9L!)Q|(2SWf7qnTAiFa;|O3TY4hH()#4q zj_KaM3^Hk;8rd2YYV5a(m%uQ~$10 zuXo_ekyt6dPSC>WQmS2DDiT6gU0K?VWvoP`${(fQ?2m_=pgy^S-8s3q@WP}3QJ*>% zYHINbDF}SrK5Q$mSwgGnHzUxPP>Ldz=s`WrqRM!N9BGL+1vD1=0Da%9_TAa7Xt6;d zSU-F7kk3^i;^S`P&6Z~`-$zu`s9T*Tz$3x$v9^+No4z+j_Vx?+X;R-jXKFD}NJ5Hu zFKp1hCp(X%mo;F)iA${A;IszN0|^SHjS%8LL#7;l-1qX--?@sD_=bp$A@0huhkFUs z46FwL+*8wEe0$*WGpP;$fG|)1+t38K7*Qf^F%axB{cc-LX#@87cCIWekKK59Jm8!z z=SO?KZYpsfm$~wW@YKaJGyKOMPr9$T!|Qdt)^o$>Vb(6@7D#T$l}P&zIwl#U`aWLW z5&d-;fEr#Meq%aHOD><)<@?cK+j|J?`Okg~2S=d%AW25kzG*utZ3!W8OZOPLzf@%G zx*wnA!h)!^D2z&H_E=Q$nYMcQWG%2cCbmN{I&Y;ONTEv^;|awqAzMq#e9qO0#^H%0 zbytNszUYMQ$FoK6yr$?uw5OPSd<@^DI!W3XF9D!$a}AWCIy??wJWF#0i@$lc1X4$a zAw3hoa2E)TORKH`Z#09@Q0Vd$_*ojB2!F;EU~8c%Bv_?Io| zX|z)VYqK**)Peviji(lxV^}aVr(_iHrk`gpRDbo-50K<23CDy^q3l}QnVxJz+?r3S zw?x{UdRYChJ_=y#$#}>q9fw@P{nKp$^-qq7`A#LPL>?7D#QkG4#v?$u1*D#sa1p{Y zvs0Y$0PhVpESemj%zz4jG!xiWOve{=o4cS_h%R`B5r4nh^hS7YdK@2G&5Ize^(`Jb zjTit(AE?h}pSed-pF6>B-7-bCT+D@%!-hfdh?+{~dgjy%KZZ!C_G-*+v9TKGe^Vyi z!j(;;5hFmax$+E#{4RRg)HUB0j>9p<`xI|_m%b1ofel*K2f&~8Ur3-*!*R)~=!aTw zL%4$}R$sJQRy~#YI1ZU8nO+B#wvtjus9tostojCY!xhs|`{Vy;URJ|$?%Q7k(PH@t z(I6Px=rIJM0U7e4;yyxXSZzOFTjJ;B91XU+(wXH&$Rj%Qc*NsU1P&l`omi%vYgRG$ z-p6+U7zZFRN;vCe)-?Xkngq%;PgeJsw+tE7ct86#FLSHrI%Bz`J`PZ z17ed`lRRsSP@z3zL`Y9y7|5Q|yvfs0UuGKXfbL63bkttlm9=Z_-6TAVH4~5iT!z{O zyt8;1i}A2Vy5)Q7QNSet_q`V?qh*e#RnY!=0E0d-9O82G*B&yPaZtGb{?&ha`{P1I zXo`2}S2#Ht4P&(Zq{VmDuab|?CuQ-6&lTwcrpd0x`Ctb;iKvY&2tQ!sV*tG@9COlXP9-EUgc@>|Nce{X z%O%tK#%*l~pu|wPY?mQZNX)WMQ#)?S;66ocsY@-<{mmcWkEb5pY)pO&`L<+rsJry^&kA@3s=}s$k<9 zN#hzCy(;Z@K)mQ;0_1c5B(K3fk;aNc+d#Grs7F8Fe|4Y{wQg*|zE1T>pFHdBC)CI_ z#MI^nbPnh}8(9I#GB3heYzXyqvtfN`#w7||$NIBxL5^Rrxu68Sx&5qrEU#*X_hPq4 z4^ZnVIWkQe_^o0ef^x22>F|6-97Of?d_$O2PNPjOT}#~ zab4azT3YgozMNy9>UbkJyAA2V^fTl1Yg4a084mOSODk)*byM^?h73M46$8^xLxp8k zbenB2zhwe;*FWDT&#+7)vY5=m=vL37dAcv4;~q;i!33gS{Y@eM!%~ z)pa0lr|}v;9zRKb@_RzWR@F|q)-W%lP^lKjB@OjmLUbUib)$&Xv0wBfUYf%huqD?d zd865gbei`!jMC}KF<-9&E<$&uYIsw8J)0h2uCzU(BR$Uj2YdzZ2`x%7GlVf~l!~Ov zWhqDuN!oVBTQ{yJrao zq-4OPQqmuuNBDdy7IfYfseE_*$$W~ z5@ok;FxsdoI4L?b6{tjj6B-!JO$0~&{^7re*Lv5Ytg`F`uS5@%XPnLhNUM`;vH-|F z8!*re<824#jEV9YAFIo7`>R67tqW8dKw)@i28gd;bf%$Wx`AI3`nIW0WuGV8Spe~+ z;Z^Ja{msip@v_{dn1wT;*@o{vZRkjupHE$~YQIF9_P$3kXmkROzX_zX*J!^*mEZlE zNHg~!vbb_7T|*Pb?w;J`gnN#mVWRR{j73Dmr+A!GE2 zZNh#BO(RtkmAQ)JsX#^n+r|$v*;-TFB56RdJe_@-4q`pp3V3$($7MHRxqU70*#W|R z4%%m}6BBL($7C?pbCH!0!wqQxLZJMD)r-nL&6al*(UY;LOHoZo@OyM=W+P5WwTjk2 zyGe3-Hb;+^T(kZlkmMDESl`#j!>+?_a29qz)ii;{wf#D-oeD&;;XGk91&%DAk0=ab zq0Hja)q32_3K86VnqMGKpM({er^2OW;2<9L4C|5cQqpcC?eqA=uexUNQPEys?zKPB z(ko;mES;L#z8gNUgmo3<+6(ONej1HnSFwyH!rh!+)JnFyprca)k@k2OQyPzZyu9irrR!KG55E)UA+2BczM_H2u;;|x~gM!X<(jsjc%Lzj2IvtD)a4cI{73uD(A zCkvdl)o-K}0a6XXQ_VFALJ@zgMvSVz5Au<9pU`{vvw(^h)0};?k3i;zS~3ISZ@doR zyfYo1bUzGass82NZx&uDJ)#fK`-xb!nPG90@V-?+*9I-+>GhVEt2&b(?SrzSdNp$# z#^+;{m9blT5psKcy_-x|q$K5Eq`R4(=qn7Kd1t)^^!1>IT9Uu)J5o$r7`r`mpd<47 zhe#!a=8vd1F8jA{*0@Tkyb$DlAk4NlsOJI0pRb>(lZ|(N!AV=+ z(nIdXx8@N%(wh@R&*u!Z@&dG@Ay(DC#{x)PIBjLEXsIZ-by97ILHMl<=`;G~G%B{} z3VY>prS4a;ohmh{Z}9zh)Z#|(-s|XEn5SjFT1X1j^V_%!F+roZAUJCwM!v;`gAaKt zk9#EZoF{`bx~)e%{_I@_Cz+LRz{2wi%K#R@Og+AaV{=DU6A-|yA`8ZEs>Y*hYuu*Y zEi`4~kSREaM&i<6fb%34jgG)+BGus)iuTQz+S@}$$alPm1N`$lHZWK2)D_Y|ip9I( zDV$90sCh2nbWkgAGO~V?upj@-XzhD(^`+x%3mBM0bH{$oV)km7x>P}4${%0|2%ueB z6cs;ItVs_c$O<3@xgmN=W_#y3>mkW+!JD>)V8>cog3=kEZNDdPHeu30ZV8;G0*Xd+ zt9~uV@7Ep18`-xl!SP(OfXY&0P^{na5iq`s?&SwhJ~PA-QkEhFP#tYU$~|wA_Zp5{ z)D7Yvfy?C7i|hNhXAgcfN(>JLbv<8|Vxf!!H=L}GocZ4mfP*BR zmC54@?5z&6cyaHZJ{Zx-2C#vFu~;hCRe)9I%k(=38oo!hdl0kIG}KB|&)~HGBKyGB zBd%XY09)VjkHJ5`m$&f|Z>SRx_>+1510I3PYy`)}%1Q1>D|kI|Xl;)tTpwyI37Egg zXdH_t2TF}@!|0ve|0K0SNwC!9rQtn(+4~H* z8D)5-C#|?L&(SZ3Fq2O8P<1 zba?!WV2Ju7&Me=Bi`-6^6Q@Qq75^_+ku#k7BeDRHq~lp~720HMC{Y|Fk*+mE-#yMC zamn>`=ZEs-VDH%SvQqMmMF!TNrMq+IhY(eQiW?qRWSvx1V$2Z@$(ne!Ae4Kg2y`Yh zb%|k71F41m3xTM9tsCLDsmXacw4p}aFtgheqTtO;J8gI{`WvsXoM^!H@R9qBNxw3Yk}eR(F+ z4z?)5?HWY5;+Elb_%X~(FR!x%%eH+(v9oAHt(inWj5rQ*#Lg=!e)I3$$oG#eW)gUE z3T2}=)W)<2LSHJ|Cwf%OZm=| z_W#ZnT=f^M7MEH?-HJ7#K~ebvdJCoJ)AsaxV2>QtV;qKyjRm{C1% z7<7H>tsBb1#P_1r^*YC@@_S)%py%<9YE(3omdyw`U3~aqf%)&!oG7pE{$$Ot@ikrC z>8GtA-8|Ladh+vbHmWzKrxba*LVVuu!&s2SNytH6Y)Y1NRlQBD^uCRJC`&j<#lPd& zIH-iVi99ARi%p-iC8IPYx>n0}sp-|A?->+d%T6k?PO)kY`>}daPb-~gISZYTEo$xa zl_Q>*MTz0)upSHU>~S`crOW%|{wY>_cu7`7b7E?0ev~X+-!NHu4pq5<&34;SUwQMx zFbB7d1KdGvktuAsPmlbzX2vSMF{!lVOs_cm#-t@tcMWV7|C)?D-Rix){5nApY_Z;fNod&;erX>&2cSA9ok z%9CI`oHju~Wn!f$kOUmYv!+Urv`(GV)C)bB&I-u6xph)^ymZtNQD0ecIF{8DtUi(& z;X}#;Bwh6d#{aW13Q|_-ETwqKLSAq_b^TOG-8Fbj*1Wutt3mA8tfP0CtGAL$cmj2L z&J~#oUgWCg(Uw}R#eeyjhEe1+KI}S)ru?Lg#)7EAKKZ`mjDpH(VoMYc{5qu%GN!}6 z(}z&roBX(|??R{a_2ZW6c$l`NK!E7EwYbRNL)QD^eOBE)-bS#Vp4YdV?^U!ob%OWQ zr?CYmLFBkmqM)a>I|ogrHR<;ztt%53iMxO4MRmPFZ>!hmv%jeakNwb3X6kW>NP{Uyl!LR*R9=oh1TUrJMO4AhZolADm^wVx4Twx9iCiHk$SS$w#5E-EQmzPEr|#r|syD%t8|d|_pZ z-C}Ej8uqp$%uTFb?3zFjmd!hii+-eao>h^h4$|Fa6yJsa+yx|?iM zw_Qln;|(Q~WWR^&miF9dwK)h+ZB7r-H8*T(&po(XJC*5MZBVmvajZ44Nyn;y-MDeJ zeqxw_)4QVEIlE#Ae`%g*X_=G&4_{KGkFQH)*xjhHSlQ z980Z;yw;{d~e(A}z zqt7|R23hUw27O>dFip^JRwC7 zN{+5}&r#~K=BmvNF#5YDaXmiWJ$i>DJsU^dQH7?u(gR%O&cZee9j~C41j!|(6Y3F~ zt~TscuW{r+3R8+>7c|sR9>>8~^}4cF12zltbQXQ)CP&`=hzDoM7b!fqG=x&Y84gwgmnO+i@bWUcxz@{h zcn-cv0Slu&$~xC6i}1$m2>y=z29D^6o(yQoZKe?beOW>XCM}1YlaQED_l~+*nXgYF zchZF^_vO6dUJyb@l}z4x=fJd~V<>J>exhrz%V3Bb7`K_jhko^sVeK#Us(uvK!}As_ zHd}>ylhN3&md2?q^q6tBulG+pPFP+_vU7xGV|(gCMMZPif4$b>BuKgJXA!Gn80PG9tSt3Q!o ze6E=s+Tl@Z-e0Ah;aY;giGOi&7vWlrAb#=Do4hA)zPn#zXK?`<-aJCB_7YlMUBQco zaZ~AJ)9Al&mb-W>BI*eai;iO_W>1)wktt}_EfNIzw@kmve$+>#O`ZVpm!o_Fk$;Y; zdcI9@_NH6u?GeRQq5bDiSz7XgEYvk75kS}LT`P7&C{ATC)|lht+u0FZQ@<|Rta?^v z9+e6e4YHz04r7&s_zS%1nqv9MFwtSYQLLfyS&>J!XpL!M9ed5)FQ$+d8$4Gi{v-Y@ zg&WXX7d$Rl(RN7sPS-Xl-@zwZOZz^|weCw;$O9e;p45R#1%x3OaCX~y6`XY>4K!oq^-#d@db5XD+fA$-dN(UA z(Wz}}^J&I%Hw;&AD)WZD3-$rzHRuJ&D!zb?$DJ}&!vVaWYwK)>%ANLi*vz-#1wQ1% zzco+h*TRUTNOmiQkm|~>HBMPh_fpDbWM)sLUHwNl73g{LtU@id=6r5EO~S&>Qgmp8 zP03;KbyJLN;DAVhV6R8g;-SE6jG{?0L&K=3L1vv=8{^RfXa_k>;cC@EXn=XcF#eu= zs~jV5+HX3FjLdOXR&rhkyoQ@B7RJ`*-%hjZFA*GwQQispOrbA=&C~c+gAu=~W4_Uu zsy|R1Izz_8?FkDFO=AM2o9FXPvln!O#^S*hVc%)oxbPhq=pjzpX_ z_SI3~CsZs`Bw=Pf@(Z6glb-c&v=Jj$Jau-OYg`sfJG+rAg3eZhP=1v)Nb1Q!Oskq`55;i=&eW&ed8$sU8H)Y!V{4#&5=*I>a4;Ad09`oCwh5u9;7TJVGr zu_MZL!C3P{~=VGu7z!+BVnS_#=WQ)?%^z~O9;wZV|S zZldl>muDQ764+FcU&HDrSy!w(kAs3!(>-~S1IeUp-lHi6bZl`4mC6OEB&A!?U z`_pxMi)m#&h?9cm$R_=Qj=)Wi+hPPg45cOH0LONS?9-1q>{!rlyQEOeNW4$vwS92N zTTZIDNYs%(=TjVUeXH~mBWR9}1K>UXJ!Zk6gqfCYD!rfo8(nYV7ghJ}4dZP~-84uk z3P_iLv`R@!#}LxpIkeIt-7uhZBS^Q@(A_XJ3>`Dnz`(p4f9JgCeV)&I_yb_?&9&CL zu5YbP;XwDU8i5-x?@rEgy4VB)fr##dye)3Z1-L*ii{Gr4Vtm@vzY(PrJ+iOu-Vk!I zDEq2*q`L&~(+)k#@VPSht^?%M9C^~bgbN({c{;0{yJprh3>MK%n1<=LH^9yL z4p9Z?<|Z{1IPrF-NxR#}$u(Wy9vKYM1cUpu^!9k%Fi93wlIm*n{c{itXTeIjJl{Xq zig{Q1im-nPFD2Q`=ext5XJSJ}4W4ZrmO%%wmG%Q5#z*42n*+4C#RDCFD!ia)HrV8SI+xEUAjO{kdob`SaU>GMN zy!66Gh9ts3qj&`=B@oTb+{yUWOQ+zF5-w~~7j5(Xym`17p{-yts^_fMh<*m7eL#ol zy*@TGt258RMCQ6ti4mQzKukkm2~wd&5fx!wtzfW9(XL_JZ<7&q0>9hEwx5ZdjWx3! z9sM79#%mOEqJW`-0C(f?pl2Wl;xa@v9ootgs<%7b5CEtAyT;nR7!+o>oE{$)JTkXc zj%EZ#sr;N>?`dN%ondQ_csJUmu>GGa$C0H3Q%DFhH-YKQOd#6NNu)r^obG8tTi|EyF!@>$we7q0>dLg0LO-;M@o3SJYBd_8Pe_&iZ)w5sEEx>8I zqpqoP)J0-Pk}3?QYbAZi9#e*O_4ni`mX${^9{4Ixj?e>J#HI&WSqUz00lgSzcip3E zWB_sh2N=SVR0MBndHpmtflot{r0nY@%<{p9qVq@T^GVfgG3q;i%$uD+;;l|J3Wm`a z>UJZy3p=Pju*w;GNnV^EWlc=CGhm8EwX_u%%A-iw?Yag{$joaJU2i|qkil`DI%AOR z3wUA)dR&e(`aX#suWHW&uRZa z96iu~-PVFiB_4I^ru~+o=#EQlUY*VvC()c8rIx{1PX?AAB&N$P$%TzmY+bE{dCNqX z(`KSwi5_GTDjxmQq&VwZWbur5t>n?+pbH+pcb z1Q!-$bCWcNN&0zd$8BX!r4kYWRaMFl{x1^^xnV@oQ-9&Cj={z0T91~r<-zBN7soXs z%y0oXSLmw*$BbPgr~Uf5RGj$z!_10blD5VDJLj_?Yi zN!wt;Qv`n)!zM(;y^%A~jejOUj+gJD^NMfR37Rx0e2qj}HSc4!H)YSZwb+t)4cW&U z$;nY#^C+|hlCs;*j0hQ=-eq|m)AZ7O-o|X(SG|+qZ+jspZ4$OffN`#{z;x0hl9BHO zgssLEDQ;?4d!lYGFwQLE)A?==g9B~`iEQ!1oU2F^L_I&#)=MFIT8A{p%0JNCTV%v= zHKkRj9ax|C%)=wbr=HvLPcW$ZB$0kH{Pbz`fpWU9N7#P)e>xeRXl5Z_@P3RNo*A9gYsWpeFUF#eyMgt4nX>)(pSjL^ z>T1D2jrs)&a?&NfNVXjKfxhsKcMta%=AaYtMSIV?71>bKd{>|eh|-MQ4xL%$x95{| z^sSD5aXz>^YSLQBaM7b$jmhB7vq&#$HU6hV#eysPJho4B@FVGYw0yF`CSLEzE$E16 zUL;m7lPeZal8?!~C&-#ag+=lmGpOS67zzH}jRx5I0w621c1TZ|z@{+a7 z_KdicBT1E|fk$Bb7|b`79N26_`VEHHW!=s$qV(zxF%UN;iHip}NdKq4V?7lb2qlCO zV}-RTvAb{Vt{Y|yzv&Eos-A4kyEtSC?CMnoNiQA=g?%4C!0ZlP&P4atSy76W^XM*1?aTM59zBS@BQuF;uNc33i zL=>#f^r_8HQr$n&e^yNCUKJvpWMo(5Mrv}aS~qU4@n}Le zvy>Dh;QA(kmTP~@+GYhs_PAC*Dz*OD?wVamR$b97mxE%jtz;^KmU}bzVwF0C2AJ}e zxitwMy+cNS_JSF4pz}Rf-|vADv%E3N!^oBN%$FgU(P<^AlgOaSNvl>Vy?$e_NQX}w&2eunpErq+q5t(=DzxjS*VzQF4}sm~6ot9JeE z$$qW@ASiq^uGX%4f|er#*#Jn}yVz{^>y_=edT~o#W3M~F3jf$Pk)->`!L@+5*oM4W zA(p;YBM>hZm;&>Qp(LJ{Hv0esXi-CV4lSY9&NjHR!j=JYL8sqxwFM zZOpqY=64l>NiB9?s}pewZVmW%}r`UK@@@TXi~x-GrcS zZ`7o$weCxOyF74*##37foD2QtjrH;h*yazy02?5EMtAFB{Td{UZKG(|f4^aUe)g65m<3zL(yU-ThkHA}z7 zH1NEv->rH75xYz~dn}b(-qZ;r=SvN?u|tt-icHPLW)z5VXl(8GQ|s4RWHo*q;LY$6 zw=8%z8_Aud;-$*cRT`ii|r1eAxE6L0jamc1|MW)Kl+@zmM*=l6Y zE)+jVQPZ@Bl-wNJbfkI&Xi`s;n;F>_v2(z#W48rg64H} zE_6)kI@IS!^nGIPq2C_~272&Q(-;Wh6;6XoFT00!8uD%q@7J|dI7#h2xZDQYlW)CO zd`*wrwj68bp*}wzrZ7k@wa?{gr8RcM*j_-77@PbvWOEq<&1>;~sviJUS#`HY{s^z# z@_85ZEIl9odn`tW8LZvBkxKuvN+KZ2bEz@qHHUfBM`^*}Grk~=E|pK#)3mxvVst}D z(sa&UP2SF2!8th7hNB}x$i5%%F4eh!3?7}qAW|_)_}Gvj(|)5uIO?X!?S?qrn;5yB ze5$LpUonFc02YKq?l6sRV(lEWkgHqZNlm!_PIb1lZcz@cr{6=5kYOnPftI!`NKq&8 zZ#kJX=%!|}R+GTRsg+)&(o0v!RJEL{#J-Vd78(SxZYA&-lVIiah$)R}Zv&aD zp?a!7*M*6xzcQ?yY%G^j;ODRY7*lH(4@i zDs7}~O>goVDB|PW7X_ARPSC}X&s?H?bz(G)Kj2GGBoN^+*x7FMky5C8W`nlke*hc@p;m$HV~W)5p<~h;^c9oM8$nVDp`@!hq>el3PK3c+ zf8p7i`C}e~arQL&?ninO!}mYW-Zge4@c@h5Tcn^#5xx!7HjbpJhr#A~us&F~&N3}8 zCF5$7A1;&3Ju;9aP~#E-G;$ROv|HatF2m2&Dhq4vjGB}ws#sD%7t0YesJ-|e1x$&? z0XVULgeBpXU)FRn@|Oa1-P&zJgNG~a!+~=9ebm35nYxt+fL2F=h==CS60dR3_|f&@~;I53?cpF#Jy$9+C)tMViEs9!i6+zvJ-`esjuz5 zlu!SD?bcQ2OjuL-?OdU0V*>bh23#_g+dL_-H(txH)7Rhe{30nNI3s!>?%fFBls`=O zQeO?>qlFO>@6aagy9TKCR5IH>Ch*93>1Q;1zZE{Eh-YP8!f#neZ&>b5``r6Hm5xS9 z_%cC4m+=2UV*NXEGsTKqCAv6`3U+fY20v${T6Q!nN97Zr`KBdQx=D(eSAH^C@T2Q_ zcpuCEqxga?z0(5Tjbokh2){*Lo|}WbsRReJnrMS3zcw(aH~NYR^wKVU%b!YQW)5)s zMb*i?t|ci9b^j&-zcd9AzTgPV|K>}u!_lAMwSH1dmtXPH`rn>~jsYrB5+Il*!;@+x z&o9$yoe0PiB+iZqnXiwV(K(CQ*?8;bo2h-{qI|Me53k+ozVDQiF12-upV-PW zSQ4y7a~I;azsFV^nM^1gF3v+JaF9TskDdaCz1;AWHbIxAjIN@p=e9pPHf&RcfsJ;R zx?t*|WcjTvebco1Kux1_`WVS2PyRZ@GddCPjuCSWzn>SQt(;_6;^{%6K z-u(3)GDt4bEOw;cQhoUkzu(y>e-^yrKZ4qj-k5erJGPt*bsBq3)~=G-jRPFQ6xnv2 z>fFhIaY(f@ClME27^v=kdNy~<`rI!RH*aU5Tz%d;! zi;jJtj!L$uMO6CkL?M1CQG@2e?>GDSU63N07j*^LnifrfUr#U#{yStcakLwB21_pw zHCHFohi&r+1{kWy68jyrgZd7KcW{tOFovdD`Zq@3hmYR9g)zT>)N*Rl6d7;bTTu#^ z$EowOM$;PjN#EC(F4cU3R!>~G-`>q3-6!(OP0yY{r`w;ES*cE ze$HnRtLFO}=t^AUq||Yad#LxflrYQntxe;Si6NsuAzO!oXqx?w2FfjQ;?1mM_YLU| zUc&t=7Ig!;+yN)haSTGZA<>?=b*@$DU3zfc%g9hh;RqnExJrV3`_e!&Sn+nGBPuNQ<^)AZ{{j{;6odB zy1mne#%2Gr<9lPi>iHZX0EMB;R1GxG(EfeBTUdlY?|L;dEs|AtU>DWq$=IUz0vUzxgZ_|fG5C& zR=0;v`X9f=JmI||c+IT0&>PEBiu|y0KgNE(Sa_^@Ax| z4kqA!8tzvumY|!zDYCI1=dBIP=(1J(0&I}DbwH1i!5W)!0WhD-?Q^v4kNdXe<6C!8 z?TitBSYW4RSfZw^i%dW`IIBpEV_P$Xv(E~>3t+7c68aM<7&XIau5+EV>XznUVgq^!cJy>{pONB=<9u#EYqXoSFB*P zCLi5aO?>LLKN9hA8E{%&#z>0jRhxJVh_+S&B&xom7?Ff%nqYk@E-j+II+xlEGJkqRg2&BFYQQnf3tjEftSme<6fQ zWicZTrWX&%P&qgVG{n}gPpn_^YsLf-V=SgCj@@xqjPocc+(savWqgIo8X35O`$kYc zCYy*mi)^?XI1+1xwyRjVtmS9juUG#@K|cH3lO_o}u$<{1R!F)TkDCYa{E9`sKBA->z9+@D(?3nbrW#fU36b-i$lq zMD1e|s?KoYemGBmeKrNt`1K_$@!GDU*X+-RTQw}nb>^i|pV(2UXrk^$G>{eGjm3Uw z`vE%2?Y!sJ1HjkatM{p`g~lSK_jOD!!_EZW&h#6$_m`I740!yD!16v0{5{s7@jNsUF!~W0JhqBh}Wh7R7|Sk1cZm zz%>37j2v_m$CQfFd+h{Le$%II0>FibFQeIal|i6f&b7jV*fv@3Ltys+kryt`= zE9!5@s+BTREK9Z{%2Pn$KDF}=RJlBK2$e*rC1UCV>n`c=N7c;+6eYE^w>+BRE%6DE zDNqzaiuB%tdrKI;JLdul2(ip@+v69yfXj{`!ijM^H6X|I~;dNcu^qH4Vvv*qFySZ+G z3JyW95ErS_Z28nz`XMOM)Md#@WTOU$B|>N_J-ysTW3> zrK*Zymw8@&5zZv&PkRk`5l^vne3N-vh#Cf*`5l4Y+~lOWb&bl#k_iG)s7gKE{gKsd z=TO`M9!r&XJ#Bi&Nd@6Eo8O%Js@us-BH`nY|eve7m}6FRdDmRs=p?^N5-<1%N&V_>$cg z>GSLLZxodK!?zNKIo-YRL4L{Dk)PNQP4|ym`>(<#j-2vYzrvjMM?lNCy}jb@T65|0 zabz&DvISIE(9}iI1HH31=`Z3nHNGrmGAHKI{)rc>m zQnvk2Qkcx+PW(%?hzX*p_|}eGUF(p&o(1><+?zVAddy`1vb$ode<>I_?D`B*=+$>M?!jV;SpWA z0ztuMyOc62r_5d63ziY!z+Eo%uk$WLyJ_P1_nmbKq0kHN(0Eu7oSOWc5juZUMo!87wUgqOjxS+NV1zKqt1$Hg41)}A8m`3eSdxiZn z)~|^GqhOwSxa4({g5F3?1cf@w-M=_&Xua7mp2{QEjtg_Ff5rqqnRBl{!iO;@vKbm% zvIEm>$*t=`*maWL8$>4n%4)l;^IV+_)$}q-j<@%1M68tC%`kERpNxJ~kWsOi(F?se62sB=%EE5WWxp%n4>9z={y;GeQ^v=X z7oBMP-%0o2`lK!oAqZA@k#* zSPw2&p0LZ zgFWafVV-3Jf7A?h=i(vPAd+;CHugNp%V-imnzqdzkus6WrHpiW}LLVx|?E`=a zYer&M=@HVRX?FBY6H*|cN3j|B^SHVhXzri1dky?r$#}&fSV5T@qHd@Zku2w2-GA-= z1#zIsmWb9Y9z3b>diK+qChAdJa*j+3Xn4^=d=j{8>H zs(O9_!28o2bF)!0y}o9@YX$QZYagirV{Y|_deAl6)I*;{7#SfdS4F)lQL|#1B}CJX z-K1}ThzzB>ihhONDs7Q!6qlYtFy}($)cbBwIWwG`e*x%nbAYqPN@)^US?+1_QZ!JS zC*i$=en0ECHau>;jsN8-TH6#&z%6@eVhNTAc(itbE6Y`N2o>55`gbsWU!DuCi_goeMW}R}p2` zM`Oyv*gsMjOU&iO5H-TFNHGk;Ei0A^O!P)iaB4{qf4PUcx9%jF{V;^s-1O-~eS?Jm z5wjC~{|(TMI2qh&=SVlR&7@QG#fO3cFfa;#wvg4GbO&IlmCkPd*D`BS{ZS$;2C?VO z%8Rg5>E98?KqHWr7s%>-l&&u%UctQnu5F;Bk2;&i-}@LCw3~+ymr0RIH98dDC4DCD z?lt1)hSQdFkujyBR%3SdpCW~eRzg%64GQ{%+8Yu2(MM0slNz{}TPyF6lpI0-S6${Sm$k>LH*pFT`(K}||rNJ$UbCq%nB2Uvk zU$Hzwp*;bqz0B0}j^fWfcSkdyT5M5zg%cwmE%2KmoW*ztlJ)kbsYH!f3DmT$cSvPtg^T+;2FHY6FNdyk_w`4;;ZU3gS|g8+NRCCQSg?(kwpm!8(|#DNJ; z&|GVbkU%ca;DlLOuU*NrUoZ`=Lclj$+D73v=uy)TV__cqI#p&hbgQa8`IrD$zC`~U zj4Z&4ahhw)D@&hpuB_wc`CWZd2rAhdm+{hn&X%NZ@x1dYs6a|lJ2n=R4>CcSNHv~a zic7M#8UoZ4F(=7#3A0F3q!|#jvvR}v?y_&#G4fs7BQ zK75$=MCWK9NgS^AzeqzS47!U%#~{%amORVHZ*42~pWE}&9Oj9aLAr4|Kkb(5(4-#d zC>)8H+4+q0Mxfj8VR?W451jm=FopQ9?F-@aBdqHVPQF#QTCV1`hbc!F;91$wPqW$@ zTCx+`SuR{!kF+_-&ifF~`Yae5aII~q&?86?CY*y)v^`HC0(;ZJ6=`>eG<4G;)%O>3 zP0#~R+N$km_&Q7z!lm6rz@tM)yZMT-9Y^H6ID6Y7iausa8%P7kTkE2 zDa`Q29&QUASXPf4%_s;m5+X^&#l2k!=B&c=0_deFj3P65S?m$%amS%`2o}d z(VQjR_CLcsGb6l3qH{Nn2g0+(5`dS&N%hXA9I};t3qonNJ(mWL7DSbW5~ONodcU&v z{mW;^x*H{zU-R!s9`+M?ff8RJ>1l38D1r_e@fpBWwk?VOA$Jp);`i4wK)tHXTEThS zM^?D!HgYlabgRIu@`hZBF|r{`jBfVc8{Sa-!kFBXlHWBRIYw*=BMb?C<$6)&Te5#pCnbT1*U}vwq)3irieF6&V z{a-KIOZ%)mh7lb8W@dywCU^_yRn<26ZVqIp#gLpD^?CgGn%XirFV|9zk&nxmyb`)y zWT?%;brtmOypkyMIX{>H<)GXo$l&EMS{)L#yZs^|AkJ?b4=gOqO}?d^!*^ZVJrqR&ql24+8qXkRck-#| ziPps6?-k~QM5r3Bh4nq2f{KsR0J|+Al6k5paOGSL?PY>Tpr8DM<*hZ|9IcmLckor8 z8V}P-o!V6wLNe#FR1zwGk<)ToFifGFsDVsCf!*pI{VxAs5;Sa{gX$X{kYDqdraBr=(-P0Nb6zh;jpW!zj>#6+{?Ii zSYM0cZuoo3|FiJ6Muf9OaNo@DrcdCmwI( z()XA)MN7;-N<6BA%qN{X@Fjm5qV7+$=Pj>*Q)ku9(qXkT%C*_3{I?r-nxw={p-}5K zuf+O!XD$y8>KfZm;bH8CcHbIi($?DvW_KiUK*Bm)KY__*V^$bRHk35N9mYwz~bPwXvq;$`6ADZ0MU& z6NP?sVs&XjXL3anwsxt*ocvdj&jfCxS-+k|JXwc|ymj7l*SJj^O5L6rUExbe+RcFTU(%eyselG(xDOHE3s#g6Jjn8l zX_}KYeKk-*GU=|*Bjf^i57}SeOx2^rTq(t@#u*ITH#$Y3J~`X~Z9Bh`*WL=)2>+#{ z#`NN-n|Su3%$wX80KX5J)1es2@~gn2>JXN$LPo^NB8n%CuXNMIo-I~qp?;=t+_3*e zcYv;M!y~NA&)ZAFm@;K0w#3}o1z1g7{fWAWNSRH%F{T`D{SwYv>qf5!tr=1@$-q^M9IIno&ly>= z+y!)gvsL*5dq&ue=%6Q?QLT0|ikabR(%p z^CJUq8`%i&%>TT4?<2bG)TJ`#L)m}aTL~~ZW>;8p&0%EC4@$$E_Kq~3%Hr5zDqvcv zRF@^^?vyn`X(c1;1&j5lvlUxeRa9gpj3^TMr_$SW#m4Bq4sQ)7L&-rL5zHYTjRSyY1JT87WhvlHmT9m z&QD!i$)}!{a_7grCMmu54rEifBDJR!izl0H1ywydM{ZYxVt`={*cM3VS?Pn-C34F| z5zs21x}>8v+m6f`)3&cK|K{xL?kvsqDlUnd+IUo!g|5<^?dpDQb>9~6o1J_9 z_}>jCk8UW_cp^WII~*Xi4hPrXBa%vLr%xL({lirvV(l9DvD1^$83ohhMkba$HmPN| zlvU#4=@X_b&H%8wcMKScZf^eZkB%&=h{)(GIrY5nH3W2-SV#WH;M`DoP5NIEwTz7k zZ=fj|5!W+m63f-efe^xy99E&wpNxlSIhvlwb30zMBqX$3knN&loOiG1K<#t8BF@Z=J+nB7qF%C13qBlg%& zk>6-pgsyi)8T-nf6;))q%TPcl03B)abX1?BVgsY_KTe{EVl}2H!!?H6{78At9+H0h zxBUT>#c~uxXTq^{*Syu)Lk(%yrII4}Sl$-BAyJ6I%5&49zUveKDxTdP!Vrh`f%c3+ zd+I8c)EEL)XvdX(i#u#jgV6kzrzosJJ&+oL9PM$+P?IOJZ_4kCHnq6Yx{vkdL4(FR zIsd~cN-c@)Zi2hK7GhkT5h5ttf=|we^dnu_1Qz8=Dd09`xr7SE+qBfwr+@HLm2R0F zIP@xkYf+Hm*?ncA-!h~L1{lsM7*E^d-TU%nn@w!+ZN8s0sJz4fXrgk|s6pps^xyFm zmwF;3BE_2GEL^X+D{i~D81Lu{(RUqBV_DV4N+WBIOTMKTQb|OnZowZu-&*AM9jJlX z+nH6canulS86DtW#H&r(FqY~M&FEe`S8-1Let@J}a*Z!#3zq3J?Ok};`7XShx^(}XdoZZ` z)(P=JW1D*Yemf&0=v_|HIDu5Ax~^HG^7GcKfT`?eV7^?!&TuQ7V{Jtjq+c|U0bxl% z!kW!88|K}xMJhgoRxH>4a#G#uF%-Qe+~Lo`L4K`wedJ-lRw^y-=rBUYONHAgoh_gg zZMSDX5PmgwAFG!G2FFH~g!d=VJGIqeG}w~1E)J5Jm4zXOQCART!#5)utT(TSIJ^LN?D8e+`U;R_Dih?G6x(Wt%t)glrHkmt zg&t-2ZKtyC@9eHXM1<6Ga%{sQX0FeQn^*XsVI4<3Icg_}PhxC(2j{oc{A;cic4Yw+ z$!td@8TKy0$LW%Hi7J=%4%K#yf;?|BUv0!LULh+;vpl#lz01zgK$c=}mNUMnPW92) z_)T;&^`-UPPiTpv@Emded2|6&jNe-pZ(H`=CSViFB5he#SMcKp!nNd+k*v$ks@qtr z##a>IGE6UD|MKIrFG-SJdI6UKw&pJ>6V~qi#@yJ=Q~ury;mp-U;oL(>Vw~F{jEIUp z7)yj?Wbf9%hikF8c3dn`Cf~7njZUupL)*#ax)bw3Q!e6zGI!hJbNBqWHFFq_4xiMAx|x7dS%{K!U{vqjVR^j z!W#4khwt2+bKi+fc-KQA+QEfu2h|i$Il`7QUt@MyQ1s5;>9x|)*zg3$Kf%=2=B}-| zGpmHj7d7{=nl*b1;`%b+uh!$+Z-KsJVM7@1r#im7_|GsnQ|Y#x{2-!WwJ|Yv4sYmh z5=d8>Y`pys7Bj5NAALqVM=pnDwZjI(@w&xY+?^S--n~L6uPmi;u!0oCaEC>4l9aRh z5N~ukiBj5!gfC}){U~A6fE053x$jUcEEYTxP(+yb^rxCnG2h%o zqh4Tn$A|`HNUM%aR1bkRh2{R)0r~vd5+;aU)`;PZ%)MNZW4HoEH7_+qB^Y+q>f!xJ z1`1vGe>b~>9w)=3#TfJM#ZD$(1a?6=e1H3;%S&|P4^7@snwR1Ex!2j-Z?$#TC3D+i?sTt#Bx#ZMt*qN-k7<2vz!0Bzv6MP z{qZhQAGlYb17d&G;)oHZz;S;4LOw}rG+O?$53d_gb*mVCsxQibD=t5}8;f_E$$HdyQ00*i4_RR0FPN#GWj07^K3T#PFHEoF_fgg_Z&GOjy*|f=u0DZG$d9w{ zhSEWcg~Ha1Jc9!_1@8yOo~0c}o|fM%96sj}A5g+$zcF?QbNrAeT~dq_KeO|@Qpey( z=f3G#NdNOFwb~o7T;9t}GYFZ0i=Mvg@s^Q@rY6<)oa@O6E?oZ+eO36UX4Mw2jHP8( z?;q0fR8Cz%hc&!}Z}qGmcUh;wV$C1YUarjEYlp6^4})D^HVa3Ih-oGP4g}0e?VaW! zGogsk^Aum35Y&FNFR8z$4w8jVTU8sXpVJPF9N4YCHY2xeaC}OxM~(GN&ch~po{tWp zXtJDp(~Kpnwj}QNm?+Sfw2gBRWzt>nC4irN5fG;>@`!!CmG>;GW;EoDlAwpXX$s-Q7u{sp7$|TkYWZY zJ}c#swY!0-MC)6JU+74(zs;&Tn}J*etXV>b1*%DyuO6=&=Pg5{xYPLhUrE1wkwkW9Z9$P+;+Y?r9Z>Y}nDImSr<_;aruIMz z;&JA6s54DK-8MPj#xZR**=OrLH1E8VyEz4e-JtGNw~49lSNDHo&lggzP9ytDothLd zO6kl@XFgJneHWkzPjRH`KA=>T3@J+z_ED1b|U(YR&VvC+; z=M_bniIhz_x0lxe#ZMY>v@9ONxK*F!){GaqX_VjXl0Wu3e~CQ*iZdVmKEMBSeh)b^ zSZske%NSbhTBx`A&^)dtcrPPvFK?pl$E>{K@i0THAs z(xA%1Qj5v$&VNFQ+?FWF6OM@kr0Ua!Hxjt%QAgINU7j; zj?yB>A5HiHM((Z?4Vw5agr48}0-Wdt(eL0~q@4`7Au z^4fA2`2O#xnk%_R@Cj`cSc(Mgb`Q&gNwtkbap~J&@h~W*h5r&zW=_P3?*&I>J#{7) zN*-10?m8^v&nIUu&77+c{vOeuuyq)h{`M86N~gz$@UiPbinBx z_Xt7#R-CBsi@`!3q59v+po+;51JLLVI>2}*wO3os&Lgr-MM7O%dqk-){9Lj7X?U}sY(x{u| zns{YfD(IlPd7EC0i?BH{9>tG{!q)=3rMO3q_UMC#O!~hqyJ+w1oKoC^1v%U@8$^C{}f!FovhDfM7XsDYrIXjJN(TDdTYz+cCab0 zu9hzxme_6f2;y7Cf7(B`a%}v^u)4u(OV z{Dx!l3Ma<7$5kgL^qb{Yd>IXHHm4wd7Ea8CsbR;SqA&Mn3nlv@eNPB}FD z_32y7VSd$mL{B9?0^c-mlG?rxjLUR)P3U`x7rMEjoGKnrQKJQJUR#8b5~*ttc@OpU z?){l>wIPm8i4oJIgu)?y3R3X0~0}%vwmcLYF_oY8bP($idVokZTtW^h6B4p@pBc z;u-rK_?=ep+9q4mQcO4|*00;k=Tz2sjx2f!BBG%fvtOyKP=b!GDvH2!gVc>Sj0o86 zZ2IVxxt$U7S(OFnSTRZ$?k4vf4`I#y@|Vl9tLar4$C#HZY^4nwe0Jck$4iy8UgYAdmaxt*K@wMU&1=)y)9y zeNYsh6{qlz?&uF?43GGpAMGtV=bgFB!Mw`ZC(QMVME6*5W+xq&zN!~wKJ20lbPc{= z<$NV{AMg@gx;W8%1rF;&*W-pnuf~MC)T7LEK&QBkUh4)xdCz68my8$&esDV57Gl`` zn9}`i!3^Km0aM(oGV*Zr$pioB2*7Z>8rHHmcMM?xuFf^m5cqyEjq56soA#$+koo`u z8M1t(T4bA;Dbh^yM*kwez^7uumw#)(3;l|x*WmdrdB)>aAkG${;P=E8rpW+Y`}Vd)aQ`zkOQ`;KzLr3(79v5B1^G#jlmj8#Tui$F43)U^rLh(}ELV;2!F2w^B zC#lY45Ax35GxNyI-b8H`K7V^q3$kZjfYjF3 zI<|eMq!e!a0>Q^~f1ou!A1WNqj5M}DYp~>5ZlBHW7k=g^uArT_L69%LYu;0=pW+)L zs!yH8M7m(S!>evbrpw`eGgHAG2w!zxwvGuQJRA*+fwBX2Zew3G>G>yY`xyhfHc zPYFn1gwsP+^MrUI>Z$r1PUX* zJFk$X-XCfnnM{U_AzXBOBPrt>_Cp>D3`UK-77}3-d%0?h%cSnwY1Ejj-mpgrf*9g5 zwXb>j36+*n)?I0hM9mBfaN}1SJ3Z@{Z;rnL|r>CPHEj|x!RL}>H{9T;< zN!$F%D56#G2aA&_fa2WUvz_t)IVzFIQ!Ka)&SVx*>^QvU_j9OIO%n!#7ajCw={B~0 zmN?JxusvJpoW0MjoPN!6_T#zOD*r!Zz`NEr{;o98mq_u#(O2);Lqm1pC+;YGE;_+tcN{X<31AV$O(XW!iX5GXAg7 z>8UBdFEo5?;d3`e6NUS8FM;vP;VdO=W6ckLS<~m7lG<|~^@wghtuyfoJ|RW?Gc0~Y zZoxNvY{RhmQ<3-f>0w%^2=eN62@$htxkPzlkqAW|ZiYI~li+tdYV|nk^f@LC{2;WM zcY{oDubEO{zqwxm{iTX5^eOOTX1~j3wgKI4ucCB07H1DFl}-Iba*8-c@ww{Nr!4&5 z5)~Vo9d+d+oabv`w$o$L$BAOVThhE^#vk90@6LT?S@&5Y)Xny=$ITggQ>0u`oMF>Q zAFIn4K!CM}YIm4%DI~`*r0J_Ny^LYfk7zZRa&&TZKb5l(eug0IqMJAw@z0-$A?>jB zF&^^0xhQTGFPYQ=SG(u_^1DHSn{7@te40;(v6Ce1D$ z+Wog5^?z9rnsrB{iyMM>C?9n>u;T?^AAjZEE|BQ6>!|PTe+mpPzLmsCB$fyTCw+@` zlUlvsc|eWG!NSah<{k=Vj3hE~noV>qog~CA%BgIGg<+Pr~sfYgQ>>z&WiQQH9ENq8Q-^opmTBF*iUY=-&oB{cvneo$9xA2 z_gz~!2~E^pN|mQ|8B)heW|#M&MZ6L>%WC=Kh}l{l=xjpgh*b6-(McDJrdx7bgYcbC z8`uyqnkE0uIlXCdFrKPh2NQAGFRYOat-2TWhmaxz@#3burT+d#kQp&CMY{fL(enMj z=hE@IZlVR!cYowa_$R9Bu}58d%aujbE`U!I&xv)i!>Ti_-xp(CzLp#sJc`TDt{ge1 zMj$ESypt>RivnRK!@B=5?)$>WH0?Yu6xvrsJzsx! z+HiCe9vh?f=h5ulgCIIi?ucpjzAa(ml!afS0ld7tmd#|?!4L?f*PyDNm(^sg72I9s zM{JRuF{8u^g|<8N)`8R;l2ngM;z@Ol+%U<)_PdV&u zL(zH08h*3^+Rl#xiQj$3w$0aMv=>SOiD=0&*rFG5^n6(gO<$PI?)A=Wb;)I!E~>y1Z-c)yHz>sKc>n2#7B{sS|0Z- z0`T}4gzsAC`Rmd^6*g&7ZPCV!zg3Nibi7grzG{N0AGWZ@Gc^Cf2m%#Js;02ha_34} zJ3FaF);Buc)1qNkkH~uUA6;bhfn)tlWDMb*h=>ckOv^5^ zEP9v;2uar|g+NY!2`@1deu16b^krS{?V9mMVD{n^+s;=WB9W+Vc%(x*t=2 zoyF`@hxeN=5+(&DCAC!2m;YL$6+pA^Hdmbn!!M+rJ)s5&l8lzUIoC|~B-3pfp688& zS^8#mGvy9DBG{YXDbU;T+alRZgs&g!Jp#v-O$#55uIx_A3I^&}@N*m;S-p__Jje{(W{r)B z@w4f(wzgKS(7{dEyni-bsMPOP7CL5d(g^Vih)T#e;XWJjLCwACU#!!bu#Ku7-Pv%X z^svN?Z%Ar2zU%@Sc^gHW3vq|MmUkZ$c(YOK?opq%TJd$wdn)|t`PD%?-N2qypUO0? z(ftF!X%T&1lUOmBHAAZPQeHFE#d@Tv`St}x`r`HOT09e3GFbwFG-bTrrtScWtf?Wp zrcl0~ehNcel_ZaUb4;n+8u!kKLWoYuFqvtnxBUZ~aD08Cdv?4zQr2dP-1}pa ze;I1L*8Fx5>f?}n)KjVnDOoTUK1LR-3mhrNSknUkGacZ5;?#C63eiW8q?GyhZiDY{ z+CFJ^9@o{^+ujgTrjZ2I)KDnF)ddM`lON1kM(qLH@i7s9y&(qInY!H}P{2T-tV>}) zP^B0i-SQ176WwZV??+FXZ7r|9VHvh@0RvNQ4H3D?!4^eLSHfJdQG4LBH(E`pIxZ61 z60*bpQAsb_2!FaM*pFTwZPzpFRu~_JtCuba%j~PqHD=;@oiLm&)|kFs#@as0Cg3!$t2f$a zUHF6NO4Waei%HM6n5Rs<(ovi>E53B$g?rgqwA7w=8=`|sQo@i+1*R(IV-#UCJw}N$ z68r&E?m(9IeoG9y+ihNxW%h>PVg-Q@U@44JK1!E z6sV#J?^Gw1WO`#mkCT&A`)=uximzEb=w?hr`}?#-SnZ~f`7}5@@V@SzrEvI51uVP4 zD5A8MeN1JNz+8-XYnp@LHQdL-DT55llNTGeNgbxN6KE{dOv25epezdHFnmDK!Tat) zzR?vncasEOo#~&tjlXR>C@M47trE*UjEJkk3eHVw7cY}=MAfpx!aaFMMxCQ_>VuqI zE&079@_7?{2L73C*Q)5a-&)Hae#MHe0QL#$VBk^G<9E9HiXCPAJ_u23H3fenhz@bV@BMNs9*Mey5m9 zVdPiZ+mA93c*$&ozOy`X>$#WyNW4gVRO~+9d{kVgp{o71q;WULbG)_zFZe-l5!s!N zQtCr5Ty;UC>1vL(aqN@8e#VYHTT=Evl8p6O>aUzbquNdTZRdAh{8kO`Z#g6Nq^l{t z_{7jO+D5R-6T7!4%tnf$vC_q|cWh3-$=e2CF2YJq_Ya{UR14q~WJ`jc$WBxm5!a<`Vv*Nf!R)F?sgWGh5L^ zVJ%2wUq1w4B45}aii3+g$qlJ78FhPm<(sAv(sbJh|Cc>-fP2+8Z-KEcU@;mj=>3GQ zbGYtZG=5JiA0bYb#^mKFNoQA*?S@K;t&`vBOMXSd{ZxOG-jUm`OHiOcdAYk-ecj zo}p~4tCZQVRh)mHZCEkb3EVG%XVN0{>H2}$7~Mt5Q>ge#h!FdFGr}Oa>JnJL3f>xM+qBwm!Zs`o@*4Tf#x%{{R*gf0dj@-O1sDW|?>uJki%Vw@1O zp&mvS{Wl3t`+_()kLNhVayj|*FCp6Sq1GauqjRb7nl>NJ8rTcpblN9LFbzuCFSnaX zBf8+Lw+zYEn1p08s;~ddBB`Kg_-bfqz`#vIU7fPLykI$CsF}gXXk*HmPBwd$WY?$V zaonElvBBm}}A4wyeyOre#?SErA z@(s~SS@oh-3H`(j(*)-@1H_HTc!<+Lv61vOZQO{w8Kf{)NuC%qf1>&OQd}rV0nA0x z_FaB}ptyq4C8r49RcrBL-}lY0ebWa?5pcS~ovGlpaFp{BDIwlFVVWer=Nunq$MaGx z+3;96@z}ik{_^}lyz80QI#Iu5v0vAk0BsNYXD8@`iS_%?C{?+?>e^QmIE6)~wX1v| z#QCn{bWOaHT>OwB=2V43!z_m{KpmCiG9SLtp5=3t2Rj?r)Q1c??1P4r-)?Xgxm1cf z`XL-r(I60r`s>&Cd^F@&U&I9qp;C*56E5+Ol?Xj5?=f|Wf%eiDmGN-cF26KDeD>}4 zSVZCfnDYD*KDuUOta4?8h-k9pLI=Y?4|Li;^__W?FbI}$esEp`0Bv1xET>oua@A9j zHL<4{*|rl8n;CMUznAN+t`8>H*48$>l$7GKv$KDv2Wo_0)cvjAQZPFV2F9*g^h(Hd zps$D55!O%HT+n9nOY*rhUa9G2ZpJA19J71&M?Y(=D`BAGym|yg4imdt843QQa!j?;Ur?{Wx zM0^p5le)FFwL6kHO(CnMM!;|-mh3H2M6(xzqA+0vk$7l7{t53HqS(rUce@~`aCu(hp>iBVwZ!Hfx>K(%>!vc3N8 zkiEQ~2G=rR#lvlFX?9MfBS3$bR!x0&|KMWQZzh0w*5(hpL?2<7h_!R%N2PTr8n^sK zwQA<$4)nk*k=8Ebnux?JyYqpTNPLsszsqA8$12UFm*Qhngo%4zwyW!N&*?7m-@K{2 zPU7nzI+rp)s$Nk1x`tV4V3#PlcKx_bzC-~mqpf@( zkLrJdtxAxNDE|1uc6^!7$=)KoPC96W3zH||BzV~r%^-Y;?-3qeC zkqk;p!iPYaAuD$|vW*Cc%PfIgA>=%mUHq zg33y{r3cC>@V~)}qg7B394hXk^#q{adqse}v-E5+LozpH!|d;k@rW`af90lDjqbK#sleYPCm5hEEv!qw$z&nzOSQZ$R)PjWCRpOtGmlgPRDAclE3MUhY0Bb>>-Jta9kY ztQ#vBEId8*H%YN!64>RuH!0Y`92oC#mUe3xezOlGrG7e%wCJGQ!L+u|bfOMo0R{2X zK}du`BqiU&!x4;y<^r;i-`{E$WViXC6UIgg+`r)j&(23hFP>ylZ0om4rU^k#86_1`|?M2ko zu`~t)@{#{IWiWlfv1wzKAhC&3-79@4%vDe*$Y!{~Ba;sA5~DS&DA_hPL+_?WAdq4k zI6MGKt4$g63^x(*$2=IV{S?6?U$+q9=RBRfO;1I-hAq&^r?0Run=#$9!2@HUU4pJO z6im1|b!}O%m!N{e!ymKdKrPlhkv?z`eVxWJzA~J4HvW!2y>@BZ77w=_l-8@kODHF$ zYNXte{-i?WkjfhOIvuMvAG-4`#ipbq$O|ApnP52O{#3)H9lKU~!*uU8Kh%}LvI^gn={vlii}3iL0cWJ=HAk~~u!;r-}U(QuAi zr3&ajG}9lR`gT}q+q|WGum~vyB%x~^+>Hc8b9mZ6eVLL6**g&S%OcE)0o`@;es zMllukI>f#X=(T@^<+7D8?f}nZ#1-VNbi|OW7S>-2rk4fm983 znVu+9(l~@eqaBM~N>-K-Ni&!3c7Xue0f+cI!PV3;p?dPYK*^8byCUF{yGb0k4&F*C z4=Ra8$aU>eF*L3+h@Oo)aQL9IUx%LnEk8>iLwi$-8 zbactS1HSDfcmG-T5g@UaVqfWCpWFE?UUF)L9ZnUE1IcN@EBno7P*A1H)7kI%c~dB? zP6<%;t!;zjcayTtqAoHmAAHwxgAG2Djug_Tm&fA@K|w(b42<#_vcwT5M@KYFOz(#) z|ASrTdVDzrcyys0)6&ahtA_ zta8oTJR~x1s`*lynjn<@@4_%CD)f9FY5l00_q_yPOOIPp;BzHo)cx+it?^}swj3%) zKy#7|q)4%5tjEL3T%V;Bx@2`K4pS0X&F=p7c^PtE29{%xG<|bDSbyE?;_RaD0$9k^ z(PcG=0su3d?nV+GjY8N^sxF>!e1y}bO2^}rFW!f9JUAdT3p~ODPdPc+wf)T)>We{I z({}wFZzdQZ$s6*hv(gt3DZDzkUU32?ji(XaF;|R;tsA3Y_a6dLADnQ>*Au z=Z7Etd#CCA-Lv5$yXHb#te@dMNeb-7?%#qtIIWcX_2>Ds3*?Xjm<7&xzf4j0z_xk@ zyGOv91AZZB81?Q^a-(g@;H)sSHlNthWg9$dV>{#!<*3$w=G{PUgLCj3cgBm2V5mbMeuep7!eyN;EYR#>I!P>T+`L_H?DCrRC-2 zMMOkkjT{UoYF`%=6%B1xg2IWMPM6pNNDFIgTcy6x2skln7M#QTw-akVH`??bz1tPX zgFW7bORZig3tRKD$riDnu}lRN@OrINyy7i(EFpLT7cX%nEs>VmNzyq>yl%uiTfYPo z^Fg|s&oNF-0COY2*7;_=twiCI%!V@u^baLDg^fx=RYfghl(*Ey%l1_5Pm18E2iXx> zsU;$AN9Bt~X{=TJi~0{*2*jy0wna1PlYC{9`1JW$Mdk>dhE|ZocgH|o#J$(EXyr}! zvzv5ewqfM0oD7+p?IIzo@W?W$t~gE;`YS2MEo}o6-3?gR@I!zV=Eq}K2uDX$ymG^q zS(Ge&0+j=B{7?fQZA6_Ke`%EuIN7!Q)ss6}^L&8~6dx(X>=e1ui4abq8vl6`1; zLlnh4(n%pk|0=9z@h4n#^S>Ux4!R4R1b;PI#zswtv#S5#q>f$4dc*aU_%ckXvb)|I zIhAyHMR#FiVL>ZDt((eWbp>@0FYZQdtE{a2^vtV&3Fn_NG8>RY9KpTfp4>8QgQO|N zU&ws%X<9%Z zhJi*R8y4)~AHo0L`77~O2?0Ns@QvaHD@USWc1;!)S6ZX(Il$`Fxa{uLuUSoiI&S#~ zqn|6=#M5{QzL#-6GFk2I^fr+yS<(8zaIGp{PYoS^Wd(NjBx@B1EKz*TO^kqxTsBd( zR96RK_BCSqzW0Y4Ykk=62#-Lg2(^G)u^ zq3;IrG=hNNjFsSPyt}=X;Gz3~VSN~S2K~N9k)dp(DKhOg$eIzRBPYQKjB!J2XeqeY zq7cZOQBbYOcwxZd3f4lpOFv=YzyO%!If68e(s}!H^yz#bj zpr@zxNyw;X)j)^wr5zg^8&OeFu`{|s_qt|gW)s01y}iAhHaf`vM3ofeZygu;?fwtK)1j1=4eJC65s6=mgg|d5ci`K4}ska(ZB5u=d?3p}rc@ z5&_JaBXo`|vr=}^eEs2<4Vvi3IdD5#w!E_O(RTvb2dZ|6Zv8f;;Lkb`XgjA$Jj+Ku z%cl#3P3an}rHTrCmFO_u3lN42X3}C*R{7nut;dO$PZ$%|q4(Pztmc#V8#pvhtNgq; zlcc}IEb(_5S{bi|?JVyWnYIRTux7>h(k+bryV6ViQxD%$vobHi;O|1!+#Ge~CY7ur z@}#}4K|8?$fGt$N509TwpLrcvXp)wN8(TX=>3CDQSUcluM24>IOA07j`+&aU7nFg- zQ-=Ql?hIOHIV|gGQXTmG#%q7Hd3itJkUkycKUV55(@TMGkdZS+B%9NhQ7S&LGoXnr zKwo#m$MA>pMS&+OIYh1ei{&K}uLJV?!dhy|H_<@(k{!2l6=H+e)r$HTRq*#0lcKBC zysI7GlVChPRaeqbcSo+<=$B+uj9@1HahKu+q$hCh{C9Zts3Hv@fnyj1I56H`8w8#? z|LK_d@|^KR@u(>pzCF6YBuy_zvm^CkeRbN-I#S4 z`&8cMsCyGu@^<_UQ&c-;TDC>yNpM4MJ4IOZF;C#|iIVFRt|cu#5dqH8v5-E}yR&y; zXe04z#_#P+w@Nn_o){C~XWP1Nekc{y)q)WhY~-7WzyA0zOwUrDbo!%+@BTZXt zIrMxTCyE5k-E3PV%5BONaUX|=Ly82LA)tnXh+bb~V=80ZU7CWstD7g^0vqaPMisK( zFkK3CvTM=G}eotS4S&TeNZ)gu6KS3e#&*^5Ap3k{ zqjZp0GeF6x=Gxzq8H_#%AL*JK&!tPX>d<~7D?H_|+o7sw&ZLp4G5doI15ii8QJ5If zT%DE{Q!F@c9m_Bu;B9uY8{G8+j&W_R5v={juU$7g%XK9&ykw<`nIX_e@sw0cPbuD1 zcB8wb_dA!vQaWU(rkwl?K?=8sW>?1dPRlOAgMr7aL;KYMl{s7s3ED=OD`tT2 z^77Dvf4U;oO=mkHICj)(sg*lH*S0s=oIj`InLnH5bGeQODSMTfEYiu#0OHp%?7smf zo>ranG#{f2TR;W?l5lT(dMwdvX3{UrO`O?(ZE;TO(CX9{=S6Keoz2U|5 zPH74~sMa$72hRKeurehxmFD`#GOOX${mRlpS!PHkC~~<%hf{hQgVJJ6UWC-3V$&Zr zXnNr855)0Q9hi6zKwKor^%@y+*gLU>m%Z$*aZ5UNg3ZR8f2tgtZgiDX9S4Kq=jn2x z%2Vt}5o216(K4!H7eVeA1V23&)rVax?G<>rZApqH_J=&`?X10xhvP;K=xAxb|AUAm zXuRo?W?T`ugAJI&KXS6V{z7u96|P*6kSM0L?_L#_H}y+bSGcLzy~!^w&vgPc*hYE< zkW?yG*9OQsdwyFQN!q47s+MilNu1=9wcC4-tsl-97)44Y2qfN`fHc*d!!UrU5+^Z7ARQK9+868C--}{v?^aWU3Qf)(o#X ztbwfR?aB9v;}I#A1?%380^`ga9+ByLVQod_r*ZDOD9y4=q{#JDgabFi1;poyC_Hji zWIW}r5R8D|crji3XO6cFpGrsB6bp3!KUUU1P7MFHNqxk*z@0^LUXW4%c^Q)aQ zswv&6OMGM5>%9v6v+(vMR%=2|Us}D-B;sSuWHp_MGV}CO=**j@TS~e~1wA`yYH}%u zE*h5E`7QgNqlx6WcMqh4dA@1OP5+*$2RVz;@S|{;nccCW=-Y2H*|B~{uFe+cOm0{v3!v~eGT1p zFH;~kV7%)o?+R^eX0(_8Qw2oAlITz`mac+%ll`P;7)pX*8Ro4n)RzY{{@^dIr-Z9g zcBs1w!alxd+r$vikqdM7=jmxx?;8vZEM>UWLMSnvWOH%>pdP0+tHgUD%el^QguS!2 zmkaLFZaRkePhdX@(z?g9l_rPl19?kJOHEBppPyg&2|dUq_J&~Frq**uPXOK{m_Lr$ z2g zufzNF>O|iBltTFH1c0@!KsyvQA$k$H2c4d+XNcPu>ddd^sAmZh<)3kN$B|Dse*P+Bk zS*Y*g6G!)0}^JG>@*2j)(jpd9Qr%77i?42@1ChxgXtPc*;wgNzN7ZfMc+ zlND#Zx;&mW7hQTWz84yNK(p;1UG}4Jqt)h{c?S{^UWQOB?i@W_`+xBulgD5?YPgxZ ze6}1Ohu+IwuL?h>?q$s*of*Vg2kSA}5gLunC-2HgafyaiQ3O~k6IGRuOVzZEWhI^g zOTT;wo7hFU9uMU_N1%u)ps!&iYW>wdkMi)EIumd*OW>>r5ujcgcZezY?56xnA#&?0 z>gwC#^J$6+sTaZ_ur?EE2!7@IvrF`gaBD(Y)0Zl^2}2|Kj}FLcSuhGaY7C*SWg!sn zxn*M9S7pni0FSPz&AGE&^W+i~L^emi)Pw!;y;x$b^C5E^pICcr24d@{@`DDOed|hU z9?1Ot?Ljh6tt3e61o8`;c5kG1K9a?3ra-cD|xn^8F^5Vlz3EQQ$dh4*p3 z;b-L{w;+K^|7YIMpMM*4S(m%O)r7O&LiLC~mqLH=C}WgX6CL3c=10mjRU3>kxrM~F zmaf@M$nxzJPk_@-@h$#LQWTumV&`qi#%q)lenDD=AK{P?3DsjB{gWL&9j3=21FC6L zkOwM_;+(jw!Bm06d7kDnPIfw~ND36~XT24oy)0iO&Kk}yl-JxY z8ErR=F1bvq6s*P8A6#qyTXRYF$c(@8!2|MtE%!xODeU+8kAc5K=@ zIuy3bdSv2wPTu9W0ol@kfbkx)FQK!|t-_z)3be9tX2%Sd|5%9#AnbWeO_4Q@)lYwp z>Z4{18>ec<1;~{uZ{Z9MmR=ConQ(#zS*PmwB^-h=Xpg|FyQ~%hUECZvEk@vvx|1~iG?;nxa~23|Go49IpY$vB zxPHi5_Q+AM5L?DL)n`)R6$ zS}#u`vA-DgjRlj6ci+h`|Bni>OTJ_~?z}sy<8|e7_zlhLHDQllI|qN#`67qg(TPvV zXYJgK-So{{xpxDqeSnsX%deRsB-X?FRhk&w&kJiFt$t%Y=u68Xim?!3n(GXYlg9;t zwFYa&(mbDL1HK81Ds}O!w(9I`5|Chy({UD^3C{}Fs2W4$1w;a6Pf|=B(}0-(SX%wrr$XlJ{`D!gzt!_-8(L+$6 z;hDPMdL>3%dlzTJYW_QeGZy_rJmGz~NVBpVk|u$@mIs~QU+%Z>-3?Ck3kkx0_@;~p zS(OSBE4fuD9JQ!aoeQLS5#u7ASYK}Q-()>Gfk>ah(i4$y_vUZsLD>^Sz7Sxyj3%tW zzq!zKvF;poRT9IXKcohhrT;kC&}M?n-t?^5l7^imkX>LL0}oY)Fe1Zp<3M2cYu(;D zku{bi#HCovS29iWb=!t{UYO?QTD^Ap@GI3#9L2g!ynSTcJ>?fVi4jQ*fPi@ItG3}F zhd-I~UFz|iDPbh+IKWmSs+BRPnVzgI25u)KmTm8^XX8qMYbB}%eeFzXIQSC>JIdg* zW_mt5-25}z6WKe{gWns%<;T$@dJoG5#_eIgiTI;0b{7iFXpa75s@8N<;IA*hRKO0T`N7pkW;B_xVEn^AyErI~M9eCCr8$f))2`p4J9^w!=I{&mdfiYz;#X|h`4 z`K4e=(%iVt?H(xMezs$b5I-j!Z|6z4&$L?ErewkF96I%hRmBX@CbJ?GhPW7rYr6eP zvc5iVVJjV2(hEpyI`q>k!vgAC8fBHC@p-f%uJW(5H%KU~QnuoLQ{PnUjj;D5sVs6vAFsoCuSf&4y) z^$7p`@?tE|bK%nN-Si*5^a5nL9P=jfc`>IBc4lx+xAW7ukbrFQeJ^2RflGYgF4?r~ z$j41_K6W7_lA;~WvcY|hSBgHp6g2Y>Y;Ij1CiRn7;X{xDi8ANs;(n7Mmxb0B`1=MG z9!$?4wrxTg&OA4|`)9`Q2;Z)}jk_$8vrw={%xW!%D{uFj9t1 z%YAeYUG0SX-X>GG{=JL()^M`xPpi+QeTLu3Z@vhON1^-RQKQlel?$a)UuD4JSI8G@ z_!&(<{~0KhUH~Qo8b3$;G2Z(2!%W`ab$^{gV0<&lUC$rzv1QF(@e|8>mVMu-gOcvy zq?2i5KUq0*EV%Roi;>kGA+PWDly1kHoQ@9T7jZasa>&BB5s_}052P(G>m&268~0-Z z7177_zdR=}@_hD43rLBpr$8=#4UC)6Z)_RSqO9b-%%?GYKR%E$L95OU3)Q{DtgrGvU@5zbwW6Iyi0 z4YjxYDI&d`C@^QhwLdzIm|AXtgwC;vpk~z!=PhAAM5ubv3H7dndgb5>X__(2zE#uv zCI>36c#I^hfqYh`l8X$zTQ1(J80nN>`;c4cTx88WZsnH%kT`;)w1$~C3K!N)VN3w* zN^%o88`{`rIQ8Ca?qS8F&z0!6WB;vNP=2t%Ts5{1@Ln_@s}y7;g)u-9TiqC}7h*v` zjcH7q>)}PRG)yS?o)c4KozLy@vg$oS_X!N1&i*b*lDshyB1Qv@X1zD5J!kf<)7QOm zs5lS;mWaXZIhA}Mh(l@INoP7Z;%Ed++K}-h<#;T`F^s@c`nfgqpeA>no~_{m%K7Sf zb-kPFaxQ>}kDsXE-Ph-Dv+n77do>%sjH!jN^(eb6RGVIO)#DaVZernk=al699JreS zt1Df_O1NdZO1139-Z=EV+v`9Q&vlNo%c|aeCH`^V=*$9G13=^%ZIFVY7kk?x#mKio zSMV|P;SfWs_N2x&GEf$9NbaPJky)>Qgfs_Ep?1UpouRKh`AG-BY$dfBv%7}^( zTXctxrMb*|Af_@h4`nqdp=FEU9^=VZg`6>jb47~CFFQN4)V`u|!qK1xw`e5!!aQY( zPm$h41EYl(gMUrzfhjPU@wNSfP~)syTB%`wN3iM48>i;=R({2Tib_ zQN5{9Fe8T2ukp!yYPtR3R@0#l@Wu{q=tgUk48F1|IQS5u?$1kNTMCU6(m&dwqkjeT z(Mt%{a4<0UdJ*~7j$LVXthW#@jMWg?`wCm%fwKqgBqBT5w-aZ`uhIII@{S)FKLren zsXhi9+MJhu?9Vc}R|aOihXG#V@?U#y@9*%r;F_)Y7Ok|IO^59ap5+T~!&U^ZHAK<0 zx%JxnJlM*crjhB!KU%7`KTQ64&`k4s?vbk7htKZu$n!c@Mm$vk7_GTINAsfsVJJc| z)gX_f$lj?$x(d-!3BaMz@ZzHiv0?r?mJ1h1t}PPJq>ylJ?Z z!Q@+{s_0@B@!u6Ij)@^dBQ@e^(7};-$e$8z?!Iao=* zd%t(iEk9v1AWmownXMcYv7Tc&tS&v0Pq%2BAvedw9Wx|!Tc!Pc1XGxVr8^Iwc?*op z1!UPo@)5g7aDx1JbD#D$hxJ2d3X1u zE-jaELdLXU=U!Lhq;a4JFN+!9qz(I|CHJI0|D+xHq*JoTd0jC=LlNd$3H$Y*$pc~1 z0$w!r(|K?(9d7GyhAT_KU|GXqa)Xn#Oc%e@58Blz9dBNQsumyKKg5KZ>om3eZ(Cw62VqWR#PHmI)Yw>7gs_wCRDHJLB{&IS?RbVEFe6k-h}Kyn?VV z8DWpJd}T9~p@&}wkUY@xeWJMpkltYZ`rfId)lcI*^B2#*8hJ3W;Yh5|R0e@0=paF} zyh!A~-uPRa2t$Kjw&b;^t(PUC8)-44pt?6tn&Qi{?-Wuono+_G*kqXn+cmeO_A5a3 zakToJDAQ9Ayn&6G)(X$RN_$oBs(2I0_}w4UFLfXTO@Qy^KtN6(h(0R8Ku9!r%X7>i zhB0i3(5WuBIC+00%f<*_UOwl*+eIu3=5=BB{F1ZG@}p^zMa76A+3G|xmtU3#aHslD zH=-JZuO5xFT&eZgR?E@?9JWp4mC~=h_b*CcBI|lX0z@>i>G4UVfjD4<-!d?&QT_oq zE~o1>kbAd^|nBhmtB%PG4mIHwekRyM(>mzWxyCzzmOwAda@G>w?(|igj12Dj7gm z^D&|OqYctwgLLkrRaKl@%A2Y~M_F9z2(^p+&+2fgbZQ0{Hx~l!5d|{lVfsTc*5^LQ z@No?m?_9*004_JhY@_RESVn)hvQSvHTDLJbGIv@@(Pv{@O~_20MFkY6$TAMAl#`TaV9~EhyDtHNwl@OjtWyEBk3CM- zq4avXYykWAk6PeK-CJ%{cw=O6Sx)3OU3|sfy`31~r=0?6gS_jk{N83|k@fL}XEfUU zj(T4<33lHJ%0uR!H50PtG16KM07*5}G!1uiUtmUvIUdYw=%AQJLh;&P7Y#Wl%@Qqz z+xI&ssEqG2QTboQX0%fm0-cd%i$0kwBn7AHbdS*NvbK^DxP1+^Uj6k_FSns!Ob&RP z>#96f(N}eO+_xyZ3^0dGmDoq4vfh3|C6(rUyj$_AKJNb)@%Fx6d)KmdLed+fVsvKv z)Z68ZbG_~usFT^Z;#1<;u6sZ)F!UD&!VIf%?XQ@f0O3V*rFKv5W!Ebw@%p50H+l=` zl`>tBg?>IhFBP>0|CHkCW1Prg7O5mtN4=ig4pGL%W8^$Qf*Krt>o^5o%4I96#mS6s zm(qWgnmFSy4}){?SIY&=r=2Hp5*dK_dAmy@9cl61bzP`tAzo3D&ceWQb!`oa>(LXb z$zhDm$jzDoT1M~0MXUa}ZCsW38c z06|$Qwuimf^UrsPw#j!VqTgPN>is|JX2H3rU-_zO)1V&`=g7XmtRKPh+P^aJ`iN30 z9ZURW`b~%nQsHe$%&v`LKE-LV42cIVJij&UwA_zPbgj_QFE8PL&pVNN04Q^dAj<=y z&tdIHGO= zSa}>Uz?F|eSZg`DwzB8;Pzm}-o+iFM0)CkJYq4XtU#I>LUvCu^SJZWB2lo&h0u&zH zEkFUm0>Rzg-77c*3&DfCTj3DgEd-a~?(XjLpSnPto;`M=`t^&iwnuW$T+jdbT*Ie`;EPVK2!n??XT@7uFt&c13Y- zd}*JGl2)i8d(_WdflIe04+7*zl;+KHWED5yP;c#La$_ccRuJ^Kn4G&{xDcdv1$9aa zYM!6}@~X_%o8+E@m+>7ZTgQGcQ@ouD<6I>(+wI4q{=84GkvIk_FOq_6D4z1}6@i?A z9>70YPE(PKW04WBw`Y^g9@8aP?E&OIc{hX;XtaiZ@^|dr;7_LV^*FsOH8We6s17f> zjruxVubIdohz>*CZpb&kQ<5)CxNYLDvZGk;)IN>PhRjPI7}R(RsRbbBNaFzDhn{nR zd<##fJXRL;oyB0>i~)^IMgMj1$I>F@02$rlGQ)?}O42I{aRVe@R)&In_;jPu`Sl<$ zxQY-4!)T5qrXOeD3!#^F_Oe#RbBZc#59>3#z=t+Gn+aUsNy@E*_O$>#-;|K<3XJ?* zy=-Jf#*AHeHGWKK`y4Cw`|O2UF`K6S9e+NrKo%u_OrRWt_ouzFFa)Cb;itv9<=CKv z835C4GwE--682yxl7Ym;!loUgnFgF9O!E5k2U!+p(ZaS->XD`)nYr(sv0O;9cQ_yE zY`S4)8r#a8y0MuFn`9yK1eM)2;-!SL`c6hbwPguiz7-LNbQ)vzq0Q^h|eh8MRtbjU3~v6D=e0ZLnq$Xr?9K0`{! zS3jp|ii!z-OmPLY)~%Z<1Ig|xGrqRszr(-i^rgalwm{#yBY zY;$*NSFePzWVQz;8@{zA7KGe%s&TADx_w`S`%0yHR9!mg#DzGCOid%V2Sv{SFPK;$ zv{gO|sHXJjeu?VjY}#AszRt?M)Xi#{c!>fWU4ZvKH$~hsj9z)vgwha-MwoMoGR%8BqS-Aosi)QQ zM+ZHlsKgYcM8?7h^tgH&(zz}NmJV5&7DVY;>We`0G}(@sywFahdygfFl%B{o#5DNf z7$$UG?2@lg*F#dg-ltXIJ|ALy;$LTl<&04#cloU~RB|JaFRDSw=#TSUEI=)s`!9`@U#jqIr37XO`|o`EbWUk0o6?Kl&Bo=-f?l~QQlo}BAeo}1#~cB74xOK7 zX~c}OEbHZ4I-YK+6B{iO<{lp-P738P9B5fXiz^bt{v)kTq`Y;byDnU+m(D2SPmda% z4B|B8A0)z5WsgLJ=zaH?V&FDdNQ2|Ns^w=zTOAZ^i1acdZUz3(Hutnuow_PbpK3*p zs_gH=6u~EH*goQB><(^>ylrq?#zzb+#@wJar{0WWV+YL>22n=m!m4bExN*5+%+5>P zhjMs`8!=tud;HceZP5sWD;Pf0Cp^G@tb`uUBnbN7w>)t$i(P&`3J)-Oy?+b2x*SLS zMx$~Ab35%PV0kxN?bitW12wmZJ11x?9|^z0p~6;U0u54crgLG*)1%Zd92=)$B6YV%W2D_AYO@65DpqkFFIGE6Qa* zAEuxIIT1WoCpM}~M>m>2D*eeYcg8#7>>f719i($88#-vfNF zqO^S345TcuAzQLGxlbx>t3oK48rx$FHz!U~K9kDzLeY)uIwD^p^E+%i?R9;wCmG$_ zZz{T8C5QMma~2NcG@P%xoLR$uV9tnH&z{&UgzL&h?4kVvCNdyb0ks^W++vV+;se$j zsG=-)u!w~BP8MD7J_oY?tjt9BhrYus`C9c36M=;WEa5Mc_ynw#KA?nj@Ur=(+EKDE z>gVuS$w0tIvuiG#`RfD(jh#kS2ZR_yygTyCif7+5hk68`a+`8MGBBj*Xj$$9BEN zUv?;P`5l^fDX=wv3$O<2z)X%&)p!+Jd`{_M?n>EII-Rg1_D6RKYfy>jjrvl!4CuH_am+Rq;=n;G$|NMmln&V^js0> zo+y8f`Xem%+}IU*0@S~qmdi}!UX^|`wI;jU_$t-Rbu%7>)yxUXZ;rYI2XaSXlbBD6 zy`FxJz+K#AHAY}3WA-i8@I50#7DQ=>={kO-DeZ`ifLR|^NaR*O>MDt2x>|(7CfO>4 z7JtR>@*r-V;8uU^(m$w+_x$Y&+RtU)Nv(?|Yp7hto`u-cBL!R(=Q7WII##^W!|jgf z_UJrPg6I_FzT07~6{We6KKpCP1IY6Mg7UyeC665Qjaw&6=!r=x-9?yfRC+)dgVH?o zv+hAmTvpw7qtt=b6DiK6nqrzaC~GJ{ObKvywbw3o8hB!4AQSO`M3?skot;C3dg?Dm zJ+`M6QYg$a`PRbUW4z^p)1_g3t%(kjAcNS`l*pyglgG`tv{Ku8(;bKh@E2?18Y8;2WI-C3?!Kt@(Cw-22GFbw3BU#yN%G?$s#cnsfla#FikDvGi7ulaUH|Hq1_`VYv%=+ z#O2iRH*;y#p&?%y($1SgGCsH&lX7KGx{RU+D2?{n-+%UkRbea4RgSG+DSj+>ANkXf zhX4E%7@0H2^Ca9BO85cfVwr(?97k13vDN;J`!>oecaE@n-K~V)>@?)6t9nRK@^d-? zh}*i4=H7iSwmH!TfnkOeNCAy<7?t&P8q^ z)80(iFnPnVQPg5sJ^$lkab^nW{hNlZMntS%6nYq?0c=Ewzn8Pogm2|yO%j-d_j=%G z%vj5gBCgTHNS8?CzmzUum-jOE%qg`bbD=sB%*ErL_%iDV^`slDw%wL-NuK{If zX1!ma8#l?NGAgUm)h8R5AiYypus>rPF)k=vPbFNuYoQxx?bzlBFy#4P>{y?(V8YGkA;j9G5E%PE*g*UH!k)Xfq z69m;^iCe!{z%0FPWq<6%e);QmW}xy?Du1S=s+QHzGyht`7N^zhMSw^p66p(a>_=@B zv3C)!gyzH63vP2!E1=I}(Dmcxc38{5kaaI4qsCej8rN)KXVCWA|n$VNK3^LPp5r#=88HdYhGS zQ6CgFoDfNSuQg3mOEgL1_^)jJ?9%YBY;8?F{ZC9Z&E;QAbm)NHiMwAm!gUM_@L;Cb zpn6{%ZZ#l&F!Y0o5B>DoZngZfGNn}w`ofY#FVQwbMgbpTaDnh*k-19z>O~~GUmVG< zAeP(-_Tv6H?5PCvKC*p=jc`~4t$5{E>LVh6g zE`Noy9M7#qvPZWA$(G6hxQHxNo1+Wf+=w8*jI|KaJE0SjN)={YDBZBvNfYAoN--^u zTjm3KMcJ!`v2ppX2dimN_v>n^?6PwjW2WrSDNhGk1fL`27!x_MEhj!jcP?{@|ANcZ zCkzCe$#w9R#@$7p!`VL273IE;KxUW62>x{HZyM?Z{;F|1N_>3`BH~br%(X3Jd=f~fnMRc4i)RJp1T5EoK z?;InqhW&|)PVqSQ@ZrtQ&pC29P=t8bQr}4Y4%~Nw-W5?*U$t@!MlIv()}B2ee-H80 zmN?Lc3^^5i4@-LC5&oKF`=f9e5F{I`l}uV}y2METYk1bJ2#(3O+~lIF3hsw+Ei%@5 zoZW>i4h5D=7M%L?Lb4i1-Sd<1nO3ga%?^PGy?xGul0uQ9L5refP`iDOzeq7Qpg^uP z*tq>24o=eV8Y(@eh^kOL8~j&ji&5GOgLsYa0_@vgO5*S+slj_qSb!TwQxwr6!p@D4k=?7NDb& zH68M^4u-Sga@+o1@GpNi7a2w^6JjBsrQ`>=?;)ca2v{q{V~-gAO2hmM5Nwam@oTe& z%xV@3AXWnA92Ajx zGkc!}AG}jdP!mg)=OOG7p{SN;=D)y6RI>@5FPyfg{MIQ-zkevduhr1XtB23ONo7E~A{> ziHKB>4c)G>mDVN%RGSeJnKcvW9Ib$Qz~$98yfdAK2;ZdC`h<6V)5=mlKH zCX~m>*I!wFTuugtg((L3v8QGCyh^Z~v8U<9zFZr1?vZ(Ge;=`uQ(`(^m;J6@5hI_` z*v@7<#z|!-O*`jO&Tf)~yX;Pl;tYQXa#Bf{?%Y?HROS_t1?IR-|5YH2pjHE#v`>G} zM+u(l{^Hv?ZzkEI`H6cHr~;)Max+DCNjW1SBp_B;Z4Len$xpAyJB(KK*Ot>H9r%}e zcY(R@c=5PhR&QjYFnDZt!^x7g?)Fpa)s*dFSl?7@l~^BijB1SyeU~>Z_W0BB{ko5; zw41(i<6t6k8xpZ6GU9n6_t3-H)F0@XrI3p8v9|2p0hm90wn&-Qf8KbqG1%*hGsukV zKl+!A@W~%T1}A=g1srv&lqlzld4KhyP?|X58b+3*b-V$3Jey(y>)9p5Lm=&z61(B0 zR!;aTn6KE*PQS2gOE2=dmEZVxoi>|YGYI}OBY z(CfZ5$<+qWPEmqrF1$S|gNMVpp?Dl~2W$ois_wIgCMc}@GS_O7eyfl@l()Ku;J0a4 zp$TJLFDIc^gHwCEB07J!@OIX;Xw0Vh1kyV(=P^ypYmeqiU0!2^M>JugpoTpr^25Ic zvf$I~lgNBwSvAyWtyf)s)(Ri@g}Ugt?K67+Jiprz1aYk5sL1>#Ak{6;Br=QQ?fd^!giZgF!R$>UwD^?#syoQAn_1w)g(6LfCI;f@h)zbhK<9u)K97pirr9NL zB0Ur92i*zAn(k}V{TS`{-aA?sLeXiPFWwW+Aq}Yyr}GDxj~FdrmtH=b)$B%E`+4e9 z?0pKsMM88$6<&sDP$pl#WW}5GqMT1C+FK(3&W^x{d4m_Gx1+Sq>W;<81^{%0zxx)r z-lwVDVoE%RyMp2;;1DlbT9q5T%sLO4_x}mvWlc8|zrOu3WxSytTD=K6zVN3UW9b-f z6G;MUK3hD`o>ad>Ktow}?$3TWLN zKK;>-j(?j5YlCcBk!vsG!C{;CeIrR{%tdqC6{dCF{*@`d6J1D3wHCnT$n+J<>nQH# z#7d;ei$rRGP`;aIs%{7Wd_t}EV~>O(#LW}XkC5#2v8?WAI=#JgO_|pDaO_1Q_&j%r z&EniOm2v<95iX=i22jl@aH@Z^Q)miqPBfCCsl!jW4zNA5u}AhD5xfl2tA!G%-%MN# z(sn(%5#OPu>gy{REiGXH4(EmzS~pU*F!CUKuF0EabuHk1Ik#R!7TlbXI@@P=FO#vZ zE<%^VEL88uy;l;>Zk_fDFLsXBj{I_6JBYloio!`!`7>)NCPhCD_Pm*SN1FxkE~mkT zENN#X1v_kKE`A6^dz2^Oe!%|2rN?1lr!$AC{#*_51Xy5!WM%R=GI3OmRbw?${wiKE zl62REM2R@!rcwY+nw`p?8J_HKOyYy@9ru*bvtsDpXbIE&TCC&YwYUfuP?K( zu@z`NKplYylcC47vI*1j{0n-7d{B!~EST!~`S8f(=jx)~Fv3;ArKpFN6xqAacc?AS zeZmcV7|eon1xOMt$@gZ*7Xx$08TC`#DVklS-D6ZUN~G$xk&C?U<*Vw^TxX(|_sIPw zA1)Q`>KWoZlAn_m<=?YD$_Qk%^DBCgTTj7Kd5WJfF+|?uaYo)7?$yE3RvuwA!>j-H zePtr-Z;eMQZ8+H8RZ2I_xo$a*hr@b&=KOkZeu^@Pu!y2oi?CQs#&`YBn~T#0mEzX;b!}_hWm{90^~o97BU5@9o%4ldMBf1f!6LF~7p=-^oCSZYRlcW@oh9 zvKP12JUP#Om{lA%MzAvi%q%?Tj8;y2x?u&N5qTM96j4A)M9sVWbG)q2k5>ueQQ%T+ zD&{=7yvqzWh7>TJ%%08$`ggH*;_`pLoi&pI?Nh%3`ppXgfs2vpf8_#eLn`C{*jZG3 zyxlTB;EPx-h>8%WmkN`Bn>8h>T>`y*eu#jegC`|u5FAFfqh03PT* zt27Be+PJ+dI=zw)VP1f{fSP6!cbjH5_DNxU8UR-rhdcYYD<%NcXS=&KCJb#wsB5^q z$>&6n5hk}$6^|`y4J)04!lxCMtlH%lt)AP@*e)a`Wh<$t+5Bqtqj~)EVT2tWx2SG4 z-;VV3_YD_6@nhco=!s#|T->>$(hPSyNdWGau5jF*LTLg0fujnFnTX53bE+*+#KN%U zgF-~&N48V7r@XdI{~E>j#(T;xy20su=pizD$f_ zj+b`iT=+6Q?XkE6Yomi>yMEEP&^5D{cp`Z~o|@Ovfl*$L19QfMO)ci!bhA~YB;G!c zb8v*RVEsb{>{z}V8=RI}ou*iuy>fJvxf9xSNTA=1@Z;dM^6?0GT25WUCvhu=2>9F| z$eYDnAIbFvExd)aVb%rah-X?VF;4%v;u%-QWGWb7lHB+vGl3L8;li|zz;qw_Ot264 z7{Ho`yQ(Kd^zWSa$ah`9`hY)0ReY&gpX~ipu-E#b9oqJywMJtOGY|B?YEzK7mxbQH z9^K6obWdbXi8j%a$GU##hz_u@k>QWu7nVE9 zSvOI9OIDE}=bZ8=1ET9S-(>8<>Ym)gPt8dUcHFy$dGTm2fnL`u5MdK9tQbda56qE|w?1Zu zucnj?$NlBCG@wqBs%wnjJtsXkfabMuRZV$=Za4Cv9x?hml?&cK-AegDu+;w4-s6F0u!m~72mQ1zu!Y>%^p3Dwb~!|!syMpG&|2od z#ug(4|9{4oQU>j&X$#}|Pj1FR8qPWOh)aP7d%OO72W=`7{8+dBb>GTN)B}@U#a6?1 z*JY>;)YuFHx&ZlB(h@VdX`O8u8S&Y_rVA1u7sBW)!L!j7QgQ24G@+*$#7d~lN7FN< z((?P&b~AGU2%q8l5TxH~f7f3m)2-rVODb8BUffS;O-w537_H=hBJEtN9)JM`MCkcr zh#l(ENlNn&nly~-(=WA;T$d!F&q*AYsvj=Nhmd${YD1-94Db?mkO+-3^aGSV4c}kh z&-RU}@@PuZ3qA(09HaElugA5o3U9hCa#nfTtT+)VV-{Sp5!+J4`_{lmzcycP2?<+H z2AG3@quCr(WnTRCU`zZrL%;u4KFqTJv5 z;vOcW&6XuK?F(ACKw=imR7Hb0%3xA<8exFn`u1wOodoK)ur_{kJ_w6s688mZHWWZh zK1_IoJ+X*QvL5fw@5R1z$2kx-d|+m=nBsx}fs=Mur`rZp=9+IhKq)uzXgss}coLEF zQl{}rAowUu1ZHp>h?2_U(&yo(p<3OGGJ!&m}DT6V~<~+Xu+N z#WH%(t^B`9*gJ;-#Khe7E%@<}VBQ}I?J=lpf`@6&uGzkHm*v${>G3WxtudJoK&n~3&)~@R_;++3f#fj_Y!Pq23hG!hdVW!C*vJ1CW z;a(fTeI3aPg@1f?GWi?>gZd`WPCH>RY|Wu}xTNEqtUI^MNZBLKEqw?bL$z6wcOL*L z3COZ!(XOR-(nvrrKM7-&D(}Kb%H&^KTB>jFQkPag z_-alcWDRsh4XY>V1zqu0K$Y=z zCsuYmh}bm+OoErBxJ95`wDa-uZg`nEt-eIQ%s?LIQ}nYBj{!j`GL|C?e+2RC=CSKl zlHPgjojAMO@t8`QZ z3}QP~F>zUzx& zKMuIcF=Fk*RUVIZ#)FzYfkx-T%35oif0GpD+;QBm5yra7dYs>2$^1*Ge4X;+h_dgm z=|(*YK%RATFF3luN%a7XWw4|Em=a4uxCo5(z$Ge)E(IyYLy_|Xm$010y|=`;zl@_= zn6hW5q1K$(@h}>&8B0G|?xPHH-;OM-$N6>&`#i+B8xut*pTg4!XzyyRLFqDk{sANG z7ILf-l@z-!rQy>2`SSc1fTTKIiX?6sMx~EfhNQXx?4HqE{L$o_WfD!|(aPKCOr5AexMr{W+uWlmq2$S|O=tF3Ja%r30XRB4e6$we&l_ zz2v%bA6t*>j7dCs{M;vw{Aj0i+`QM!{5`7(v-}c%84AdbLJHvwuApIpwK%_g)AC}H zJzVKuR{(0qSg%~vXSoEi1F(@&i|D?t!mie^Hr;259DHd$I|P6T>IT!w_BPK+JZZhk zX?2DAd~L~NNK4eQu^(JjzweM7SJworaAqddwswcI>f_2iF$y}h)icK0H}#I-wH}Q( z3Gqj_+w+NRG0$&6s^85psJC;z`1BdF$>CI!xoL}9@k=nO`R`0}V-&W0r15)GRA|`w z7A{3W82?(6peEqxN}l^xUTdfY+Js%NWijTWnC`9UJJCfpb62+eq36hqjBp6B1rnr> zXb0)m?M${!Wrc}nHVm7~PAQj^U=S5D{Qge87p3!oWw0=q-xyS(2GI+u zV%9CUr!0b%Z;;F)m&i~cAai1FcHuD*)#~X>hg;yXwJ2DeK%5L`r%hW)9)pyyNfCpD zXrp2dGM#iL7zmAQD`S&`T}TFil#-!f*s5oALQXiu#X(N(QANHRW`SGy#pGEUrVGRO z=vw^IThn%D{RU~#C^VAze~VXt1h*`k+gIIJs9-jyJI1NBp5leRGtc3}c$!0jWa2Z6 z^DE@6nkDx{TzE{?Uez~#$_N?V6}bce9nmhy9T>OLjr zTzE-IC@Y%*?1zd)@D}m}uVzx{lT{s(-Bz^_Jxh|K93^7ly*0GEQHF;!Mcb1nn>s;a zA8yJ_dN({laK=tS5EjluwGH7KMzw7_u6mNwEa|p4IBIt*@qBBC#6scMQR{C=hD~hB ztOPEwV&oPMjw}9gRmNz4So^?Le9!$a!{I<$F89vNbSGx7Z%Q2h@9Z1;>H@VJO$Am7 zdO<^q!0R8t3a(+(%#)(GRKLO>_3xtHHoG;SHGmhjQ!)Ejga)I z$rF6_qzP10ptH+4dV)!_fEM@n1I}U)CJg zm%mkWc0p6}$PKhAX!;PNYY&!?Y)w`W)YLkR*{($jN2%7?^Xi9r^IkN2(L-}zJsYhSy&2c`$3i?OCxzesgrC_^vdcZmH~5x`89*|mS!-|mCgYIyj1 z8R_T)Lh*1DxW|>Ktvs<-0-!<~b)_xj zr!lafgRL^OT#0})+Luq{Nx+!Wx@4tM@W;R7HBr^?o3(Wa>kb=9!dzoPS3C%iU6rn9 zs(%lDms52(v3Z29=!{QqCcORpEYPM>G@n6XPBxom*Rf{Vt-o4(oT=JkSAv6+d8_a^3n8F_Ri--Mh722*Kf}C`B8#vTOx-3Z31({oWQvsSO+&a> zz3QG6ulL6GGxRtk2Id*Y}{l z%SdfmF4jBkLSz=OBm#t!htVpvzV^IA`)y#(F}y%|3mT@qaUFK1N7Y-W{;m7w4|dva z`wdH!l{|Spa4hq zX!jn`aR}#*x~+rTT)Wyl9Po+pM+9S_>l4?ZNO^Zbt&4@_Df~@5532W#=Tae=nC3%B zbiXuF2N{@vHT}{k@~G`Zo$$U``2Iv@?E80p48i6?yAhmO z`ItCG+o@8Y9rXp<2w3y1Sb(GACijm{Kg)J3yozU(QrAM2r_7wmGvYyY`>b%^60c3E z#h_4DExjc64>MhW`!eByTZnS$f$LlIU?w?dpM_iH9MfMDZJmv%ev*Ss*kS9UO?Sbz zndFgc3eq><{<8|t>m^I2l=4TH{-TPI5>tk-d#TVyxauzuAy}G_Dfsi3aF|xfq=Wgr zBNtjLeKtRn?);jgXWr&M@CH6rqK+pH+->JZw&b3LMR24|Mn|Q4x?f$HIDY#42mQ|7 zvska&rOGZaOT%$EU)SzT5yh9R>ng;A>|2%$=HH?rCdhAZ`}d~)d2iy7S8FLnD@tTD z8UD^7n(Wq=f314r+r^3U{jdENNq`B9>Do8Y_kcnJEu-(RU0Wz@cmb!!&P6o|u$U0> zq5v&Lcc*6QhfFV@K>-@K<94qeU!QZI*`(DvKeMHsu5tFHL_2|F50uY~g*C>+_Q0 zy=m+`2gleiNcYW3{=g87#A~X(OHNoa+*j z5(wH`F>!EkSh;o$WN#%`Z(EdMc+AgvyO2<2;S?c4(Ggiz_MV}{W=-c!=-Z_E)8in{ z;{7kpg6e8;L|5b&uz#~9lX2LxtgmRk7lZc0-!JL9QADreVcO5MxgE1+Dl|yD zyI?KJ+%a-1&knO{!r6;iJC zbvh!*Q8Ca_uOaJ*m3g6WZgWP9ix4uOp#@pZ+jQ>CyasFjV|c(;o|RL<`dT6)O7@8# zz2reeq#cNQ`eU^Ln4CbY@rgiK{m!3cy&P!gLrce5!3%6kzR|>0efW4Q#z|)+DgSr? z<|^wSldZU`SDf2T+gq9M?0bro~2hPO6WKC1+x8gAy~>V5_jZ zwBbq}(jM>qS|^(AF#`OD(MS>swHOtCJ=sU~td0`Yx6o2yIVdQyl*|R7g7C`hTps=c zE+K4l&<=wAeyN0A+dOX-nxk>cG0ll3}&j#pQ+Epu;B$tc+{ z#HM{S2km5yy~OOln0q0d!=<7J((u0@dUACe=*c@n74QKj_X;`tMmBDvc*hBY51y$C z7N;mW#eu>Smx19v!|U3LAM>`aOU8nK$;tmUad8GPWP>$@{sfK0@}@gIxSQVtxKb{) z@xNc_&;s!?Brkpqpdoo1q}4JMVRs0>e~)WrLvUaX8)O{rFOq{b{D+?U$CQt5gY86( zJ`#SY&3wP)R;G6Puc2$RN?)`1->QgeAHEWW=zYLERUu=zJ=;aGD1Ywq?v~ST9bpmr z4bTrOLn_VL0$AR2c)3ifzne-UE^MI&HwYq>le=EyTX)pazIhKo20nFJH2BSAc0N6K z-9w>B{2sX{0UGmv1cQB}fk^EFpt2Mv#`=Wl-tZwBbvCNVcdTh>M7F*$~z^C zXp}&@?wsCDtDYoAWGUY%+^f9)up>Y(UuVre?fLRo+~qW$6hr{PmXaUMkI@G6#6cy- zj60cjOC~Mwjm{}u3$25qVC5alUQ$d=HmKFLT9<#-p>vot34+HIAL$Zh%r4;RDa$*# zGr$8by-I3!0JTTh+Mpr0Ybsc}X^dH6E$QjR?C=I6U2Jv$rh#XipdDc@%GoLZoKZAo z-B)RCo9G=fvdG%GUp+cmo0AW+zWhWv=4IE$!S^pea*e{*uN~KPOvjsbl^yKU7$(eSDpU)}TfbqXis5Ehe&zyr9t zAYL|QS5g4sY!%JxP4DwO6^_~f#kPjbo`dqe#}KO1&KsQ^QH`;c&L_a)>;>m?i=eka zpz8=EMu*02YFB%J7aw2J9>e}HENowCK7oBz)CcxlUcc1~+gnKz} z#Va1b6#f2}qY$kKsD1?ml^|4wel=idl`k923$3k4%2v67(CDF<7VQjQXIp+Q{#P%3?T(Ha*u;O`a>^*NQkdE7C4Uc&Zq4IckD4R(s<&z~3^nJqTgh3V{ zl=`0aID&n@*_CWaUrYz+g@K=iD*-r|OVyoi9}_MscxF!)Eub*!+K(sCb$>Y=7Gibg2J{-=uSWBFbS-424$ zNC$#UbM6TVfUNL3z)D!USjvor6_*fHbo_?;>9ZOBI~UN|*(;7w7^HS3fLZ{kNftMU z8CsLQRIg{Q|Me?ThF5XkaHbUVSX-G3rM#2g$}#cC9>wv%3BWms@35T*B$vI9waPa-3^k#Y!W)ZreMeBdpUBS1=3YP%-6RusU! zW<{OBFJSJ1W%;J{iAQ=45i_7@@ACn0SAQAioH9X6*zDq#nciU6C>0k#ME@?by-!xc zb=ARg*6}$&iKmj|owBVodSO==Kju!2(mc4IS>fGwnml(}1)4|AO#J$ zqk8_{0yhRFeKRg|pXBn_n(~znH2`|3 zP({wD6#FVu?hI({o7jXT@l>Il=Vq6PLh=|4urn|)eC|q?enRPXZr%Ex^$VFaXk`?t zt5<;0GM$q^(Z{t!Vw|z7({s;2gztInaSKy07T&-(-ybI83AD`Hay?|*FQB;oxzB)iOwX2gX>UlHs6CJ`Dq1mI+?@Rp2 z<9gyp0Up-m$OiXZTf35w_cZevKB4NXF>`+9c|WII;1Fz1frM*=)Ot0tzkFt(_uf`D zpjEqCX8^L{nY!`$(6A-Bb$EBjX0o>EW3$bY`Pt`SE4r%D!25J$oAqTQS(X7z7Wbku zv`rN*ZX3>r7fWmH&+m52o{TsYMhV6}7}7USL1B${%)OZqs`ab?9SF2NLUP8W@V%JaZxKL)-n zIw%F{^@j-?p{&ZMRy!QMyUUgRZsNYn%728~r(wSv3;j+KkYn1L$D$r*5JNq}{u`(< z8lMN4u!&ubWcU2MV?BpJprl)j5vz zxRY6gQd48$m8#sSSo3OcP=;lTbzQ?r7KY2U^Dp<9^s}_n)QeVig%th-Cd4faRF@nv zo%*u7-JB(o?AaMtf3z+E=nFj8m^zg`vA<1HAdesa8(D}~3hVuEW}$lA@4uOa-$i5p z<$>~CgMn<)7m<(U|D}PNwhHgVJc9$O#!D5vo)imJ$%GZ?!=EeU7AL20%0W*IoQp9;q^=>z@m-HOg&P}=(e~SKJ=O6Rz!ppjTmFPm ze-_=Jq#JrwlCN$|Z(VZ|dj)g75|A?Nq@Z}d97C70U^3DX{1MlQn1;2h8B92I2^;3M zYan3Y9W4&3b>5GLI`Oap1#9TPN0&q;**R1pHQ&QX{X<#KEIE&|;0D*ms8eOL>pY zP-SC1e66f-iRR`h-6>z;qp;20&lP_xE%-7uOO57zBp-nAuaAZi?*R zF5X_bXKRBjH@^^sfg`peQ5UKMA>UYAL0I;xh8&uqQe`I1p0)}~N;1ISFYRC4dy8ry zD&g^V_eDTH#Un_*ycSz&2m51SuLS~tZnl9v1ShjfM|E;%McfQE3Ro*sP#{TeYik22 zh;PxJ{8hotzohv@T5TuymvY|B(W!7G7BxrKJf!QGLT?7)>W`eqB*K0HVuTC7ebX#k?eIPv^K4G^23~QpOc-ywf*FjtG z0pmH2^}snH_Tz$h6#16x2T72HH$1Dq@Dw3zMJv-xiV9q`dMSXLu*8?+uj z(;#3B6rpS!;=gAlwtcS_)wvzp`Fd2UnRGW)GuzH%ttb0#8y?2~)n@tpVXKLt>?u$| zaAyZqd|R7tE5x;OdAho!5ax8p^UIp{r)GUD&6MYegZh_&Y}yyFZ{~Xw$6pM(l*Se> z>;-y^CTcVYQDK?-y9SbFp&yf%#!Jb!2QTLHu5pVm82?8h3I+&x)s{p4QK?3KVZXB& zR&F$iq4Tx#zXaM6vvg{mQnBu&{?B1cMn_7ka1P^Cqc>>Wok)%k@&ZZ>LQel;;D+Dl z5dj!u1#II+Yp2r@qQ@wGa|C%_z#?IB6^M9nDxMu}#ICmDAR8lvYN}*{Wgx~uveUSMR`$r7u25O?LJIgVg7#M3 z#w{t zEk_j+2|Mg29zTEdElHiF_5#jnEY006E<|eHtSz&+6|3GCf)zwv_rNomm$eLKSkUec znkhtyR?wPbgoDF@NF|^_nl=1Pc0m8v=0G+UYs>BZc+*fEOpk+nrq_de#Ob^S`s};; zpYu%If7VPB{yT0@Rg8wVvZRmwT&MxctZY4x#MiHv$5| z!Wu*wLj8iAl0xd?kqMAc@S{P2K?Q(FYw6u01S*$QVh+F6eN(;aG7ufEqI48A9H-!z zyi@_@czvBc_mP;PjZ8(Rq_u(;7>UQ3qO>q^epyMBF{MwgSj7{j=Pd0T@*dihkCy|H zD>sH2)&B!La4O(|dlQ*Hk?bNXe&pmOLlu}}4%M~^4QI}u%CHs^bv6dP?L|?2;6s6I zDajheJPDLEy>7}ky-e!W0m{Hky%ruj&RLSH_OrKjSJrGXMLUG)sFEk2RoD<@{%35S ze@|_^q=4IKQ9N1wgXL=;L>63usL%C3vP{lZB7;Db2uH>6UNQQmV|+9Ee5S3;y+vc_ zyo}#z>F@_l)C@_1wh}QRh3Su^0ENzsbW&+F-Zk!iHP=YGiseYsjYw(CLeR6j;7hnD zmuMcruPbW)%m=Beuq&>v)bfNBNr`GH6;8A8HXE3GGyW12wrlFmn^uYi)v4)h119I_ z0=4X`k}SIIkOE#8GjJOW0U1VfRiu|ZqP0>GOHIUnHytU_Q*t z1w-E!B(Rm<1B1Q(*OM2a#)&PEz$-M7oT-7}7`e~mCE+{Jt6D2DjnS(sv8byhfOdLf zghZBzZ-rljUiS!B9(+iE)pO*yC~{LX#&Xs+d5Zcm&6S{8Mofw6!%1Z!=~w9aY7NOO zL*4!Kn2C7N{ZbX}eE zI_JFbgxsZsJERU@j^lRm0xMTFF#VhxR3zpV5LF!CrYwHzob^e&IeH1tIl@##;X(5! zB<636q>9uyfON?|QaZKe{8lBlUtxpv)EoT!UBfQMz9TK0r;8(%mit8mZ>>---t`<0 ze|nrX(@R170Auwtr~!8JA>cY|NC30a&;*BOM`1t%vr$ui+W=$WgXNu=?3FaFab|aT z4$Y2|v$Rs)Z)*oP)JXk|>eXbbSLMM@4IiAI*D|i#LT8uHShl4)DY$A_*tFIx~=%~Qu&_lrL#vez3!z9;fJ@EmypzERiFOxa69 zF3LhUl|C#(g~{U zC>EIQDkJBxS-oLW7KL|pBKuaXL5{c_mn&b8DF0;HmhG)wWF{jm_3s*90 zw>k}$?Vm-^2o)i$_l0p>NJvvoGE5Yl0g>r;!>Q>lI*j`6qtVYZ`V|?64YPvr9zW9 zw3Q9f)@h;#DT}W7Q`p|bshPK30mAP-JuHx7*deqWc?Rf#D?V@XzI1m9_zU{v#kih5 zS>a&{Z}1?6YC%n1EAD%#1`hLo{t~227^r0Ncr)zz)h?GDN##=!-%6d;n7=MD$9&?R z3QF|j`11t$RNi*Hhp|G|trlv%C)rn`G50oGOD~psqq0{v&iXD`lmCY# z+md|rIl>}9uIPVCZ-9{jG#&uCOH1hkgdJ>cL}56iO!oJYI+#18Hn#7r{|w({t#(_@ zp8K49_2<;#>3%k_U)b~lrp9gq? z0U!@Xdw_Pus{gA4AlAU^o6yl_O>bRSRiXCux)S`j0@$*;UkCLZ!gqk$JS)C-U#ReD z*|tYpZdG@xE6Pg96?{P%AR#n*|@fe+I91N#}@zNeK5=O=uW`~38$IY};0I@`KK;=LXr z0dk8j6mtsr3rr6{US}4=X!OKzU$Bmm^4QXj<0j$GV>&ro?kNv%gqA1rb%b1p8Btz{ zRsA{(^8s*O;~4qBu4@H%HP>A(*W+09HCVHvF7mnQI`5`y-~&xu(YlC8Zfi*J*yT`u|0H6$2!JRwBL6ZK6ZT?M^VLN@Yz|L3=-AT z$IF4cFpxx^@A{@eMnIeP`pjq zo<#b(k;(Hms#j6JK+JW3O-y1O>r9e7dN1M-pEH2eU<+@>kl23a{p?}p!*ZNH)*ox+kC%iBBn4v5B}2w@lQbx zehrs^5Qp#t2P5dyy>;t_=|i_;>*rk0EF`lf#|em~R%x?s0X!>k9@L-)0qv_kEWcL2 z_k0*J6X0;ytiGHc!J&5Aw8$Vbcx|hjk?T4(bK2g$4gJ%k5V}YY8`g$WtqhD#lpvzf z2nx|>i564A^Gxmc8-Zt)2^^jOld6ry>E78hK4V@&3Zm67!@JDC#ezxiDv6|*0pk>uavLY( z*uQ@fY2s9TatGY9dKZ}iVVd&n>Ob;ktwbPnpGDt*-3K>Yk3N+isV%gkHIS#@XVpz^ zO+y*h$m=5JfgO71RJ#Djj2ESO1C8Wc|3K!V#LT`b_SNqM;$D;KkH41VTLTlFl=O)C z(^`y2oRtz$zhD+aJ2Z^CFYh^1ru0FG5te~Gs^6+Fw)iJib1=0QR{+usAl(zLP7+RU zuGszB_tY^i*H#@01e6T!`>~`1%}Sk}$8%>e0Q!Dek4K=p;>jz}T}T;t?vQ?aa#avDoQ_1KPA?-`e zFUXALuSL_Gn&XSYqg6L=3>V8cPq=guL^yows0F^-$Bpc=s*DEvv#>&mpD`DX1Y9#1 z+bhCllJm<^l2UXgC+M7{P>CxM!ClT~*)B<8Xfo|AgUYDhxr25JH^WcyOrd{G)LI6( zS!aJlQF~p;F+wH49w0qCipQSTztBH@XxuT{LCueX9~(-oRakg#Vfk-OFKZ;Q#J(T& zo&aoBZ}9s#1O@0JqBg@=>`w*%3Etb)?MZk?)Lw40Cw=w*p$^Ce zm080Kf({(GkN8`n=|1MyFMWt8FLx}Z#Fv^kj-L|LX#8}TMZkvS@@6=0uj7hCu|C?p zEon(gj~Am^7?8>HXrA1WUr#P|Kk_oj19}0eSmR4qZ4HRT=BtFPiWp}}zqRh92(0Wulk72{%|o@D zK!6NK0fWQ~G(}r~08P+2`I$hUbJJh4p8{73@EP)(d;mi5(1em%XWXh%MEzp86SXo9PZw{t8GW&eh zDI$2pF@;&-Rm-lnhvOKM^6}|JN=R8^gUDkCwL5C* zK%e;=my=w;Bt4m~PS5o;+5XGS15aX9DQBfkHzNxIl-H~XJ15Q6FH4Qea!sHpTKaB5 z&hddwRet1bee>;<{*Gwwa*Zi$@ympfGa019N^ z|AoKW*6Mo8)&g|+Q=Jj>zaqWT%)0v=4(x!p%FWVNJdi_w(uyY~?`wZXR*qlZkDt=a zv}|(1Ec%AqT&FW03!|5GA52E$MmCf;k5*FroTlH!N4YGJb3Odp;+cty?cb)UK36e| z>ojyXf~#(#47bJ+#mHNaZ{F8k%eqS==cZbc;h zJE;N(4;q=4glu3ryNKT|@BNThca4VzkcLq%$37Hy=k{3Nk?E4gkzQ+)1l-n`;>gy}tU`Ca~6 zHBSG7_u}C`N0YV|0axoJw29Z$JgAAHPhC^i@|a?;Y=<`a`HRpx!Y=N%HF=adDTnd- za{pRXhecLtj9&uz32}%JDfFG0?unlzrn{4oFMUCnYTNeU*h?W;xxvzpBR|O@gOr^fIf^9^m|nc+qRDL%yGx__S$=h$P&H0RGN&V1r%Iu zVpGGpL=Wm?Xy8Jt9$q@Q z+S?Lclv(XC8<8))T6I79VCtD#H+!)dtZ3ON&AlP52MpXSY_&h_mYCY|9s{E1u1U^Q z#>x3}_Qtt*B74iqF=V5BM*vo%)0xDEMWA_T0+|auQw2eq$Pz7E=CsDqkP9ClATgNq z=8_bOsE9?5FW0I2RWe#+_~sQ%Es*ANg+RwfuQbt*PL3Hf;P zPbR8QEB$KqIHUs0;I|#?Y>0U+;`grkh_N-zl4yzkF<-ag6XmDq&M==K1z|#+t~jPm z|2_oF@&aq|i2-w)V?n-zFxNJNbe=fk+hWtF{h#7zg7!YmX*~-b#6un`0$WMzoML~O z^OHqY)3j9Uv`lGR_Doa!d23hIcI~KBO(8ks7qtkBr$**Hj+6J^6KMQ#GeE90)h<}F zrc2GVb0QZ_2FZc7Mc$0>~@TlkaoDKc4vNHWKbmK9k8(?iA_r1MdW6& z2@bKF<%3IHjNR_-nit?#>euz>uE4yAxt+hVNWZKN2p{Jqy#q#z1>EL%K z!R?LXy}KzAYSB6Y_Sg7SDVpXKGx|CN4_qldU4uf+NTCpM$7!_CW>1VkQSaI0=Y=3< z0|Y`N+S?`A{i^J35KA)k$!m|Ua$q>G@d}7SP-Nw7Gk%YWy|5PZf5CM~=9m19%pI+llD?cAEHvZ0y?hT@-Hbd9g^1oAPFO{Frub z`qkW8Q51auCa&wK*6d1b%5%@d5iHUDI|WdKptsG_T)Z?33(>4g7VWTUVaXSJZufqi zu`dxOIe>zE6Ml`%_SuG-o~4^xOgR^WGu8eWkLStb7l)+F7rk^>qe7I=b} z26qRhb^UOh&;qg~k@5IN64QpK@9S`$y2=n&OD*#(_|*AXOq&qljN(gzE2v;Sb=d(< zww(;j?cV|;80R^v4f;7ZbH;feu2DxE67P@%zTdZc3*E31(GiOao~0f-Na1iV?d$yD z60dAUkJ9vq)DA>*vxSUo(qx{hqRk8~Ts37K2gwgQOfjKN;t`E`Hk=z?R|=1n{mY;l zs!DS(y(9H{s^}+AUnZOpXBy9~ew;{PdVcJLlJde>w?PjqLn0kdMw2(L!%kFDuXoOF zGxacd-w!bNdsj$n0O$}%@HG!kF%+EAi@z z>jWf8Vgml@Tb>f=Kks)uLN^A|<>>%-^a3c`DUClJtz=)Lr6?I}!b*vj^_ajYy7F@$?NqPJG_G|F{-;<@|z!{24N8${^|{%r!?nh zCj<4kB`bE4>hvWzDwo>T7}po|`KQQAhCY;y1U9gA#AYeJz}O?Yw?(} z&GY^-rn`wxp+i&9cKIi{W#;6L)DG$l|D4Irdh{%6IFU|47;@ekng12 z0=uF-Ky9Gyx757NypwM!Z<1sjScPqc`Xwt~HSP!MI#Q)-DP0$*4Gz8w-h{f2R)v>| zY76d9!1RGpRlfX$?ZMno81uJ?m0vRm(y30r5q>sk*t>Jn(8P-4t1?U58HW+_UHOW< zN8RLlq5*8;WR95?!Src{5Q#*$9noxmm|gi0XIWN8gWu>h4xh5me{C0(^j-BW+A`fOhq&1{fxQdR5m_fuLkHRP59b$V4 zQ9~t05#|Yoi8(48!lhLKT32(h$Dcp#_Fl14StoaFN6ikgkHxB#frmw1lkxs56>{K# z^NFaW_u)trLP?Zw>&uOE6oS^$Uj&fS2Pd^trcmy2wg7+2 zIS*j;@|fd+x*@is_*w$Z_q}Xj*$S=4!{{2SqG-BQUGV659OutWf`;ysC7b6f@UBa= z>ll>Qms=8#gH}Ecq%&r_9b=7lhby ze}UEy2}d`Sa*Rqz4A6OUcKb`qCN*`83&uJDVi*_l&Re38PC=qQb`Pt8dR*V~VSIN-0@~`4!2IZy!*j3$K z-FM9!YQ|%scqUoxh_md|{};tD?@{~(aD%=kBRU<9;e9P++yp=_R}!?nM`b7o)FgrRd>0`^a7F-dj}Kdiy6R(vvKh3=|-!<+N?4oG;+`Z&=a zKOQBud%LzusU989MuxHUh1t>Fo_RMWAtMGbmb6TKGh>|bNSv4Zc=S)Er!wFuCDRFE zl43pIny1&1z@+oj>f+_*s_k3aI~<%*|Mk2Dj`+BA)UXUW^Ei}_$!hPQ?ESo3Fl|A2 zBkxB7zctPobX}~qH^`7j=$#+66{oVT`V9!JSNon)#YUbn+9C$mOd(_=qB(t=CQxa9 z)n%=f-8^#S^{+xg>v?g+$ooHK1&hIpBrZ89zm$PMQy`(eXuPqq+Mixzqz-!$kU@y? zftUc@pvd&;e~h^c?J!hJ=VfyC_>Co?-ScFDH<&(*0i)&Ph#a*(>}_j$cD15_vo- zi*|#*b@zaUc9ucM(7cSZ24f9yqx${#a@k-7bGF9Rh+Nb*$yYcq@OOg=pmY|TRO5>h@JiM zxCzjueKzk3cW5cIRaUF`MYttXDp&L1FC7>&t`FBUlWp(GeGjaZ>LGog*5%nRHNZ(G zz`CFq1E!l{b}Epc7&IKtqV3SjNrUIAiP!1Wde)@yzJ@=>IWt~Q#{oA_WS7Rt!aE-c z0XY=4$nwdnKK(8SdkB8GS@HMW*oMOASfw%HK&jcg8T4aupifH5O`ety&@IKLujsH| zXZrH@!|^W>F&Z@vaN~WK;RA_uRtX^6T}>VbNg<3ffiqy)11jy+u`jJLoBm3t*#+Da{ho5#pnFR@xy+DlRT!B$!pI|y8kvWcs!lZ#14bvI@Vv5W18nCDN5fl&B7P_WYAB>CzgqM9-Gi zfCPg@0Ai(OoC^jfBwx1`w-RfYw{Tq{+5dQ~cUfK8s@?s<)Fq*PDf-wnhZ5=Dxsr zfh2P697R%!TdbQAUnj|q){TRa*G5abqIBwHo&%Cvq}h zPYcs`TaOcg8r*ySe>d%Sy%2QwePvWS;)IP^o>)M{$SnJk)-ynk*}7Lh9r8OyIKU>6 z^v&t?oN8CCirnKi0dR$R<$*s#N8u{ZW~O~t%;bk9@`h(?o%)^^8~u?Ze6IBmYXI8d z;~vY~qFXLlWu_vVn|o*FW6>>)=!VqFnXA!ghgU%fKLN}0#?n!1WYb`a-laG*v=lTF zS86c?BgQOY;TYn1t}5NI*JQk!lA^-5MH&t3vkS9OqDlV%N<| z(in-mMp{%h9vc;Q>PtSgth*vj+r}hJC*-)^5T-f9bjvRfk8p7|F{Yq``O~((vFgk5 zYIW!A5{zoGMH>aWgdf6WXmlkrXrZwb(I$V723*soiZX0iyVd69dJQXbwH=RONvIX! zePJvcaBNFiVh0SO)Xa@4)^h0py+`zhwmyeeuQvORKzF&%S0(1;&Yj{ifxWR!JovUG?EV4|3=2?vr`qA^{vu|F*n~qm1)3s-SBWZ6 zCEyFX7f@WYMOLW8L#*|o0}UK_qOaf@{arxlWv9Da`f2I)>5fqgx6IO8SsA)HSak4$ z=a=7lr`IE#6$>SRL*#Itto<8KiSJdMVq(KDj47NKXRz)}f|F#ZGM{O(U!8JnynUE_v@7%PdxyC`pL%u*rB*0f=e8wjpeFmPQRPA%FdC&}OLxMCgLJw7 z?kBRJ8HQjQ1Tk&)aIMYJbQEE1V&{?C7UEXt5*YB@ojM#;#@-fZ_xTPI>wb+>zo9Bq2g#$|cPYfYB zV*4=$oU*cUh8j*lN3e{68M$~FNM?}t**_i2r7P^aNYpa7e4F~kD40Vdk;xkVMqg7| zKd=|&)&!*RLJpj+fy8 zmK!PAj=3hXs6<_=AeF3oTm$}qJ3*H9g@IjTVs~jZ&_cMhoRVgRf$Lx`U~|;J#s)~F zj|fAYgR$pSN;ecmy|G&D4W0Yw5Us%w=s6tsyuK9$&V1FBnb(rylp z3n{egb+yX!Rbu=;6|^a-PB)BK5mqQP8H~%O;zFK!Pv86DG<$fAI(g3|%?6FLK{2#; z)=$OMBg^TzLk~XC!V<`P01yt@8+W*K_6v12zTc*Z;(XaohlGldSH`MFjW;G>xg;I! zuFR1=%5W#id^b)5-C~yP;XNsCuxyv4aqbumB5-+g$Pr{ATlv#Mr4}n1m#xYz2qIvc zIWgR(&af#zHfPS7j5rZul|cf=U7S&24%B^N$Y=e^kzUR^P9fb_9;qz@viWO(gQYjP4}Z6>9Zh|1Z8=+^-YQcwYawy`su|n$fEn z3r^l0anD^p4WOqJ@Lx+KrVYdWfN7I2_*jF{|J?gQzN~E!~pR3O+qJGK590I(7sr`ustGHR+iu@9@UT?clN*ZTWpRklHU__(3?p5$?2`v0Z$jH`u{wr18QW^I!#>f4fnzRD*gev@I%tvnGTy+5wd&v^K?D^ zC{YYSnupbik(S$*=6a@cmaHN=3b@HMk6I_fQY?ZsOA@YWa>D<#aXOIHRBLo+8cyyQ zm9h7*=jNh3U5LAV;)W>JOp#_80J0z)IE^gL4Uk>!_YHR<+;MFx?iH7Aw!qsUrPLhq>lnlb8!Zy zHE<8|;Z6{`uH1AA|E6$Y_TnKu(BHJGCbAVFk;xA0+lXT5f(RDm{ zka|=Wa6dqQnk~C(oky;_y93z89fF?4*u{1{R~Pq3HLI=wpW88jtZemrfct#jP=Q{F zsdbZs9OFJfI238`E~RzKI#4YRq4<|u3B@$RJ*3~o(qX-1Y`Hq%PCoOCvb!;;wg>-Y zI=VXO5xT8rgp(+vc2J->?>BDU3seX7XmuV8?vHlyqz~g5{(gKSA7b70?%`JRwWDUEt${X(>1fR|Te7v0y=@s(Z_ixS$OsqK1vesMEsUMM1V0-xkP5H^K>x|;Fi6jHdCUL@ubYNzJNP^LO#}et`6p_fylKQ}7yLg^nd-uS zX2<|!MdL6>7Qolq1Cywutb4ZJuOPqZhZ`I%+sY-!vkRLvE$ba3!%i$c(r!e;m=>dG zyxFL{LS#qZ9RqThbdPd6y!aT4U}&K(MC!~#JoXh1uuJ&N9D(F82!LG)Wye7=drtK%$>)p*_eS3D`lER1|;q#dayko z(BPE3+#RNkc&rctH?cXC=Mzh@xp1|-?y=APItH`(fXRmm2IsADqSxQ}$wi6w_P^6! zg=adDEquPA&aI*WLwvKrjCunv5Wo!gM+yo(*J-_ORJ7gzB6ZInIn}D4FiBH@rIdZr zWZa8eiLv|M&m1v#3*8V-;CQH00MLs-!xD4@7C~5MQrXYUO(o32v zE$PTj-_PJUJ=CM*@OCvL%Ru=9jV$N*a+ELne{EGLufj~&2tzY~3al;bai0GKc1b=> zApIU5d)At)qA7N!*F2K5P^DY!OHIyKcBGkQ|*TvV(i&} zd`XCG!!1mem4Z~F<1p1uj96Uy=WU`%GRT$&15x>mG~L{%nfT_^VWje2+6b3s07g7% z;SjHij;jTE?=-$NUui;IV_Ucfv$vyorW% zV%(qkmrQlgiNLHFUpP>tjhWHT$MJCIw~^oPi${tL>y0#_ib*fK4t%fAyLEaoEPcbo zXunup1l5~yrX>?N~OTi4u)pe9?K$KY-F3A`M6yHZZ*qU zWviBIUdibs6uvY&)R|H#H$}@u28I0+zhw5P!A2s}w=9Apy7YgO0ljnaiU%!Lrh;hN zlq6FvSKZb20!mrvpB}J*$!&3*hlSSuB|H*uYE2vvHCEa8IT>ONx-rpuMcAVFBEQNf za%<{W*^;JaY4bO$m3eYX6wPgVvLdHjYdp?s&F_1@NIC=)%sE>zS{L=qWsjZ}I?Ziq zDqBmOKLJlv82V05A&Xm`HgCs+K{_#cFg!^@?^KRobq^JIwBgq1$Suw~4)PdS)R?Prhx zoORr#`b(KqEjWK?*l-k<0xS|rgof#k1{hvNrIOFqB>22}PS@WOhGW{|&blLyrzsvw za*RwTdA#PBPJ(lg;i&*DCe~g@qS{yLDJBijx^T2i?SE6{Swf#`03?#|O?5qVe26ub zN;WI1l0ya*cabD@F46B3FNPLxc5^W@_aTY0R5JIWJpXG0({X}U*Wc+o*ihqYqi7fDem1C~w9c_NW@ zHmMiKVT!ca;?LGLr1L2eqttVK^S}BVY38^M7%syw_006g3zH;zoh{h;*#JJ6KigXUjj&qI~TBg*Jh;0A%-GH@AjU9K(Bhpwd)4Q*R}1Z#zl)+6Hsd2G;}5$3ci zZ4|z%WsJ_gQw`A%_c$uJS#9+BspO}mH( zz$ZY9IT2YIwoKA238!!8ec1n5yXt6+cV^FMO2&Q#W})Z$Ga%V$A8lcF%Tg_C7zfl) zA9C3{r_c5a%>(3mT)nm#ErbGWTUdee{CDam+#06A?>|a@8pZwUB;FsQEe6T5O2(-( zGBfPaPx16?1P3N}6x8Ao5A>-Ok<=^*mwmtkPKm^*8R&pvTi%-Ko3hK2v9MQ9Zwgn$ zyFxq5CJ9?~WyX#%Z@*P~>h;8@=Fil>B98kJjez$q;79iG6ospWqQKfW{!G5JC<3na zn@aNA4?y4uepdc?$-ERCYrK|$*&P@ruZpDh2X!%Dv<&W}H?ciwlySSrTRko9Amwgg z(6^x8FKEAg=Xz?g{g_(M3_E;{z1(GBR87N|wFv}Wll=7*Dfs84{9?2YBiYkfQ{@iG z{lcgKg5I^Mu6@GvH-elcueXi3=31wt{-)XLwHW>6gS7M#*VgrgkD(u7TJ5!;+1*)3 z>gmtdkaW0<-PC=ghW)q{nO6)uIA8MZd2|ISC`1FXHa|#{02M3ZWv|%*E`}R@md34@ zPc=(P`!Ps5B*XTng_NsQY4J{C`s8))(49(@fv8aL)0J450jtxZE#4aLC;wiDVU+^63dQ z@)3fH{`ftHD`rH&1K05>TTcEM*159K$Y8d|`{C9)#ib`KkV1R3 zhy4NvMiG>}9>#i+s`&QnSxfLO^3GGb0T%fa^wqFmyey`Hu#~TqDXQF!i`MV~v`e=A z&QR(RYMNb{TffCPeiX8~Ns=3rtuj3ALiPysug@Qva|gSHp|~g>%vn*Cz_AlO$Me zu758pExN&2fE)+#ACy5kWR-Aa)iGrCGqC3ahR<-Ku>$cA2=ii`kDPt_>2!TaADF0H zpf$^COTX4JcyO4ghyY3#z5BIP4Zno)GGb!lga^5S8)mk2l16j}t6lLwm9Kd+17j0y zEkNr+*4+4I+mCwGL`M6Q)6P*Z7Y)RJ$cJNy{=pui-oZk7@=TWf=9x$qY!`(Zne}A( z0|Fwx5hgG zq24mZ<$PQO1_)#`m^5tNd8UvJn2f!$#+cKxi7_f7MW0PB6p)m1d>6j|&3ek@ky}Yq zfOAwC1_51^_rGof*{u;Ts)~~rmo%{OUzen93C$!L>%a8|_M|u*S~QY>IY8*hE%f(u zHIU|jS95)}IBr$&2z-&h-19VGp*;vz$wxHsczmjtTZ(i+qEy3l#S}05fLtPUvVHvaJZ4M>Fyad))>+Canotk`q|_^xgRPd zc9LZsW8*Vgw)%2{n`0i|r|(l@+8P7HY}C4c|CR1%Yrls}ufDMR9)c<5H>Dr6-8#{o zb4MZC`W+*PMmJFe9xS`XW=#;GEJiHzLOHzwSp%G*phSmuz0=>g+W`2I{>45uGT0X2 z>RuvLwE)mRtl||~OMjvL)hSKdBST&>AW6}DZe=U`mqPgdNnJdJzJ;ZxFw1)+%b!im zaQrs~Yltwpl^uF0-ha%30&^^kr~~=hTIivvI&?@n--iQWK_l{0t!}~geS6fuYi@FV zTL_;{01C>-I#h~fpyYAehQSd1wbLxts-Ev$DM=VBe#V*$J)whQx12f?(Q~;Lj6wwTD%6t|gfqaIDZL`HRjh4$l-;}Nfw&%YxN>JKHvEA9uct^Ie>=iLYA8MpjT zJ-ta&#A6zo*F~BeLI!9M5S|KX^&KkL{i#085}%AKIYcA)bewpoEug1_r;Y2WO2TZu zSA8E_8`I_Lva-?;$%4)35$R7mP(YfS7u4!>UH@odV~DHOI_6S-)PKtDJZ3L)zQuXE z+4hm`q>9DQ_(Z<hJKM}hCPr}qUP`sIHtHm86EN z{9kB@Z)eJ(#f$B5(eh5SveLyQoJ;4ZPXsb z?%B|l*~b!X)8m(BTm4yBt}?-KLW(HLX}`K>mtoOXgOW(M zJ5Sn28dgF=LP9;|s%#M=J53+P#6WFUblp8K&us7z+M_d(WAnQh39-gaqn+RHzdhQd zSvNJN_{xn6Fc19wtfgh4foW#J@Y2*_a7TfJBrw9{;up*HOHAVUWg7y*{*xp#KZQ@T zq~53;iSONUM_m9urO(0FdV~pv?YMn&fGIkq}n5A;ljzan=Bb%8|&3EHiLoUn?YO@@K3$oFy{S13GTKvExjNhbPH=r81{y??}f&S^kPk=kQ$a09zQru zQItI_NJ(QRtS<3Ls-0xJz4Z0TCF&(@Y%)Jj!a(HSW`fC#>1OR186U5X3RZzF^xCR! ze?09_aGbB^wc7yy{T*o6mDU_C9wrqACtEyy!k5(?KKL0QS7MA*eXp`GO zIbxa&VQ>!PsN49_qDQt59)igM^{fM0j>Ja~okHpN+-KHhFcQ->XmX|J3 z+sTG5uc-4_yX&9i3e1m)0s7Ybt}>c+LS+yjA#PHL9+6j)WtyqXb9K#okEf2;@C9KP zCbg-$k?<7>__eVS!*YELD&omMzU{4{aKIN%pC$27Fd0@dA2F&g=(Krx7)Q7gOlU~W=hWvBzVcPWcFf7V;ClU& z&x8d#_lk1tW{p7WCu7;gUpq=ff)Eg7;?+^nZmCp0GFB>nU{62n9$3Lg8XH;{kk1^s zI&4Fgk4!x1Tv^VEbrOJFIe+d%-B)qDHA-=khd=nRKouauqA>xi&q}xse=|^MlwF&i zlhApSX{WjE0(SfqqVx3s_j z3QC8xv@{Hz!;lIB($YP2cMKf@(lG)}((KBNm~fD0;>HCc^@up*Jc;eosiAlf9>=r=nE! zm~|!GF=9aFf<3v308$wEUgy)QTrTBQ%?IX;;Zv!yrkaspDFTjX4Xh7hk@=({N(Sr4 z%7%s`gzgfPF~lQjBW8ix{!~y2&1vjf;c3wpD57F&9dro7Nsq_3RH7rzNi8Z^h}x^p z5hfEJheYt=2J90kd2Q;9RJmT-mF-2Q$6Q;WLFRMU&}_~(^-OBkJEv#<3kCO|cJ|Te z>7h#<;*+8mZ{gtFY@o#p*F@v?4D}IC!$DoTAefj}Do+&Mce9O$8JP|P^NESx3=I$E zS0l_V{SVnbGX_7iw#j4+AX|}vi@(x8w1$1Q9JYS@{mQQex@?xr644;3ojmLP8AD9j z6`87CvC|WJB;lQ{WxGL1+xeo4*q9-RZ=K{8PJW~T3gisUfA^X}j4$ga+Teph=pH2p zZou~^du0>)Fzi8$M# zbhGihZq=fa;&&nC+t~dH!u%|1zwdiqyVhm4N@_D&E~)gZyM@tCW;SUu_gZ}QDx!@P zWAX?R)h&p)3@@55_fa(mz;I64EV0dVJVT(tx3oqTxYx%O%VyAlf-^0L9I85yOZ2v_}xyl`#ZV-PAqYQ&&T$*8ueEh#J` z`#vxl{4}R~PJG?mwAhb`Q~_zRpl2lyMmY7&M~%4z-|=>98)x@7?vfrYRft%gjLL$g zPfqdAV^n4QOZBJ)(4%=0-bF4N7(-uhGFh^ne94~{*Z^1+CI+fkv2H~d5+d&s-L?uTX%_?wJYacO zgGA4kR_O-XaD*X_Emx8lO;9^Le)CPIY{^JreBFiLE!A@NzIJ z9`PDNIB%+lPDZt=zG7Dnc_=!SFIb(y_PUW3h>@JvhWhr7|5Q$Rv5Hhpzhq8m-sUm# z0#df{kJ|=S+MBEJa=q6x(?!i(vDOh->P3CAiJXO%42_J5aGI@PpJC|ic#yC1PCK42 zO(vpmB6t%^fBQ>_&P5zFb^645VOdsHTs4PAXO#FhO0+gF=}wELh!VZ&`uGzl*o8|y zQL54uZoo9%4*Rt$Y$sinHgtFuXL3aRcLQLmc#{mOc3Iz4Lum$~Kf47ecW2Ci6;|1F z+x@OEh|X`V2104+*7s5saU6_LL5$jU<3eZSu4BBF6wiY-ZNp+hpelhS*n$=VW}Ld1 zN$hoBrlrk_UO7$X%$4A|y51~VfK36eABbiJ|9Yr0SLt`KB{nMIs@>5^S5*19hj;*lVgIfmC0ApoqbdE5 zN&l=Z11e&tZ|#9TyZaU2j!Y=A&b+JrZk&q9B|6(wY=Zm!L#j-V)1z-b&X*<$k2;w4$m-Xlqzi2tsB!BbIB%C5IK zbd8od1pXQazIyyj`$9QYPrITIvlrP&O(OcygIR4t{2hBa#O!NKK%$M!oEgz`lir+n z?lvn0B;7fEx9=J4dH7n^;G~ycnai9Eb+45q)?ru?p$(pFpN$uXpR7H8_gXS+Xyy;9 zCLDZ$t}A`%v&_bJX0c!TVAv?+qt*ND9Lpy9+jjT;vaAn2ngw| z&H2t|R?RvTBMtlfBr#-*f{|e9daoojZ_lsD$>RwON zm~kxySob1CmhJdsd&80e1BH8)()@nEwI;*#bmz3TOq6wr&Z=|$tJCR7k2J65KZse$aQqo7pD7Ib6mNvqtYIfSJPu5%(*){yKTbhPtws(9cfvzLK9D9QoR zMt@474P96iNCO?VZMIX_#0bq2WgyWv3*Baq%~S-+q-$IQ{iCs1VLKu=Um0t5k`uSu zr2@KZfstuCacll_`?#ZLdU-}3eD-guw$I-=&JM4v1ro+w&S~R(;ov6NA)6M`on9t4 zE+cFD?TCWWXVmZb^ZCC1^cXK#=kA7F#%bv=f^x2+@XP}~RasP?cj2HzH7aGxBp9u% z0#jF^@iNWlXXTFNd^s5Zoi@5z1c>Z0(~g}b-&ob3_YP^P0s-21K*{#JLLn-_x-uzs zkmp0yRo#JW~_up%`=x>Fx*TSSelY*^>?l*a+UXT1E_LYcYIpC7K%>XGwN=_C%H_kGKJ}7RgSN7@zwk;m9YTwlH7%=mHR)U z?nxDkb@D6M)8CB}IBK~qx0c3qu>H?Noct&7FWp7#IsgE(;1OgmmI2Bu-@zEBBu`dP z6-Obt%ayzc?aEcVJKQ_KfBy8R%UUU(W><|e6WiHLEXa}QYtrF-lY1b_F7WhXT{aFznK2I#No=5^HVYM%@k~`kw<)qqL zI`mW8MIx_|OdXW&3BXhtbxT{hHBDYb(oP8tt`OkFKh{)~q_T_E+pJsu*?y-YR+E~l zjGW4paeFSG@q;z(WPAMcD+jqmq0Knrn^XH%0wwysC^4m9FKura_<)AT)Mu>e(L5@K zo2nl0+r9fH+`FtVT?v<~X81O(Jv1P;EgC*>#fJ0o+3t~>Wbf2qPWXJI9hk1G4pOzd z-QbbLzFp>Az53Jpq@WocN&53ha?joWqGb8O~Na~%o- z%H931`)7*~ZnBEI!0+qkQ>e2E-#tJ`xd`HIiWBs9kc0=4V_`AUO4~38t zy}+RX7$#Llnyv~23~B~O*zRN{@jndVg>QpA{}}H9kIOZ*82p3tJ5Z>=sI18_GLDHa zI;&)H*wy%C@Jf0G6w$fApK-j=xq5`U!I9}vOTJ{?1n_bOca1$a0pERm-ly-KGWjaD zN*7s!Tjq2N(bk+Bf_nxt%zZC=82z|jWy%<5wY}u*B4{wZhL9a!XWx3ek93E%G}>Cy z@lP<=&-&FMn=2-4r>@IPDw@mn5*(bB;<#-?=x%NY`;7zJjV#w&&6Zfa1%|Y-0rV2p zNDhSpu|7M~s35`#gk4h%Tdh`fn}J0HKO)2=X?dF23}Fsv8zo8aNUWqc)?BMBAn0)J z$j#l3n8#)+xrz*%r^it<_fef4`$p8J6sKG~2L9(~j(iX|OX;*%kxsqk;>_epvOcDrvT2+)gzsV_Aj0Oj0mX zxoo-x5K!4N|OX7dPmGAT67rwqADt{uj z=!I*N)>0}ea!7dPSQ!n1J)^vsB5Evq36Z{ho8ZbMtjfbfi>CSq2qgXuK*iD8NmrBZ z@G%rIARTm63C*{lO!ejaxiuNSkDm&=`Xuf7pvBfF01S|HqP>rB_c_7QefqG^wza7s zr66ci*m-E!w0ngP9njpV%7|x4^=csyr%1n|Ge0HJ+g~xyW5r^cy4_al#R;c zMyM)0Go#0Gpi)(QqKapG(uoFvRf##=P%Y1k93|axD-c~x*5xK7PjvRt>Pnd z-Di1hEK&1OL(#uX-o-p(&e18@ykx#vmfv5bx!U{B?>@+^ujhLwY?6MV7C~}x$aw+oJ2uN2Im$ecegvh$aoI!Fqs{PIkLQc1 zgv;kqtz?&@je&9=+241oVpG-G15({J|Clfrz#=bUq3|wg597ht9Oi}HUWV0!M`w`= zEo4+rMS)Oo)Kokjp!0v$97AC5cPfl6X_Su^61B*x=><^GgC0e_Xi#Nn^B!P8D{{qC zXgKjwNvYlHuJ*;UEr6SiKjqmQqjMuA%Rq!u)wfdamr8cWGAF@VslLYrI@1&CL1b;~ zRb%$3T$6SiRfQ(XN7?FMS9*>_9uYvjRbqpkJ8jZF z+gMXE=P`Hnx{1(JCbB)Gn@}=@y$IZyl~RigpX3fIN!AMUa$Tqy!PE|3KyI-cG&?Ms zI%I5qBhIEi^!)8-~|b5aGX$>*z>a9j2yIk?wy5N3AhP^Lto|3Yxji#pn#~J zd^Haq37>I9{LIlGX-`U^V-O%&)?TSjcDX8NUHaYpDBoCO9&=zUm0SItmmH5qK|86; z-EF&2td#ZQib^o_A)f-%N5b!;U!K1eDaTrM{SuY!U#(-+$ISAztJNCjy^v$6l%t%= zA+pVe-26AP?;Wej%z~=}k{zcJLf(@iMn&OW z1o=gSOrX?7Dww^|!jC&ypTDp3qmx60>ZEYq2wb)rcwcFo;#z)cSJ>m^EF9p!eBpYr zd{`1!7?27EzJ24e|`o9GdXcR{6Lb_r0vX zWe@A|NY1oNOa5I}5VaEYBB_RHp{Vfnq+y==_HT;axm1sdn=^~!ZP^jLt z&NwzE9C%T+cbC5zy#zt*{gAsMMMLs?-{0)F9y-SmgR?up-W)ehe zx8*%jd_E}Da2ljQSQCCjnyyH>ma+;)>#=l;%6|UPb31(~XK#Sg0UDaJ%+&?)v{-m} zsb$GYxAVF-1~#Y7u*aWcYC6OQd9XAU+OE2>ONk6>9wQJwo{JD{$~`Sy0^dyIAGuM3 z%xq9I;72KuHViS*NnB2i9(v56Ds!h^f^?9^Ohh6gG&7>>*aG|*FQX*j)C;{DL)q+q zdUp}}?z?J$ST*sqxBa!({dmNXSdzXjhg~e1v>|Jk|G`gs+m4fgmR`pNQ;DphYZS&i zD~ixS{pxxtQ;dU&NzXph4bZzZABpK{UiAZoW$sc{OVwj=VCqHVqX=>;b{}{M=G0D2 z@Me?cOS&H<@?KhM9WYyJB`*%gO3YX?W@YOJN5wU_@_yJbWsx>ir}d*jPA#?nW?oVjy7!*$;J(;zE&-pTveg4oLHG|y|x>_P7OOOAGPFM znZO#~@Q3!wXb+1s5VGSd%5iVI&ch4WlLvERC3?AKj&X3cS8Z3;hm}*w`(r^4w7Wj1t#+|j zM*uG5Ks4%4a`0B-0w)Wg?TK|{P(kB&3?pCm054x(l*}Y6FHv;SmzZ=k$~E#v-?xD$ zs%U(0qPkh=I#LV5RJnPU3&VX_5RFC~U5&!~52x$I+(2 zwMkLGR!l+!8>&DPac$4}_Dfdnh_ICv>zC(p=B`#x%pT(z?xcAgNMDOd-oz7+6NvC# z5Ie4|_@y-#t?wzRUFU?olD4XgLdZDhI6u`RgS~gD_lI+%MyJOW%O3*3#BN;vmSoNS zFQVZn{9u8Hs5fNo47K?KAIdD}-?XI@2=npVQc!_EVl9xEjDn?t3m-tt!NK_$k4y>G z4z9}ogB4mivE}H>j|G9}!i6icV8S8L6s)X#B|N;E+^5RvXe5)fwxuc*OFrH@KC4Ji{TW$%o})f>g@}pIYq1B z16;1yS&It)+%I|VLq&-;1->5=9GPf~2pFLE?PZ3|g66c=fs1S486yx*P&L2I(`fiP z>b-I@G!A3EF)y5Gv*SPKqYim56GfpDkBBo0Ye3VJvX|yj zC2;4WQgJ$`InLzZ&$uXpSbT-AQBro*|Pd*fu@_ky=UgRE)u}GJ3{@i};eB z9}Vy9d4I~-A-VzII3yKEONRhD^Y>449;|0Ul1ILjQlSd$$w}|E!izsqVvY)iOCC=P zf@Ox&kQ^eRGygm$EpyN*bfM2HofA|n$Cc%Z06N}{9$95F?4;z+X~|$?<=kr?o$l!r z^9PrmO8vd8U8E8}%TXEU;DzKTc)c>9B#wNU1n=&H_b1^L9y-D?j2x$j=ZwL%aZluL z;YM|&qI#K_<4+VhNhTZD2=L&fQMzLRV9~tbLRcSVj7&@se&DXRz1FZmw)@Q(bRY7w z*sx~fT9{!Xhic{lL&c2clwDM}m9Go)S@6)tYtDrhc(LO+J5D}(nJkcocz|nPiMlFh z?I^TXQeCVN8Q(k3GD1FYj>`XVH_ZZd=jKR%2}yV^V;ZYAAZWN3<&=|zk||I0tP)bM z?9;SO9QAtlZeyGq{@`9o!mT?JbAfM=#0RPo){50G0Ge`Be*r5}_RQ=}dTPlR^|z?9 z5Bf-$nn%)KfdF3;HoM|IfV&?Vp5PZCuas8P*^_5Cct^-Z?I%R~w+(D^33zl2fh@xrO1&VLjJ`;LoHg6=B;u8RSaGKiGl%V3<%_5(2mA4~zOhf7#kFdn`Y z@(n*+0sFw1IJQYEyFUCn`LZnd@sI;IZ?mAepV=VT2L@O-8h7d#M#%Ni4mz9Fx{f|n zMfP*BK#e%hZtE{Mp>W(H$RVl%r5HLD7>_K5zSn77GjblBF_1O{=2y0yzS;Te$H+IW z3BxBJ3=|scy}rMDBlS;SfadLw_eY%ODQCI*(oHP?kAh|^iLVb$njAKtxW;3+aIrKx zv0bN{YF8AnT!Sp44*@E;MVQy4%7i=r7J2Vd9a&WqU09pzn)XVS50RU&EEOSYMZS&^j z=Rfa49ZAS!g)^NPJXhGN`Lue1JT~)90gp}6HE&7B-*uayxyn=9kghNlDzsN;_O49^-d(`0*+|emIAM$3ITm>udT-)WILp&Fd`;wEgeu*q z&X}@ZlB(V>yAbVIHV_+3`b`VeY3=vt*C^er)sDk=HQ6*|l30>75BMCJ0oy19A2ge3E`XUUiE zx~+d~cuxI6J6Sq1CJA)_vMt3gdwN%Ag{Qk1a76U+>bP60V7gUzDPfOkP`1G2UFgk- z{IdIZc?17eMn^Pw8@%S;k*7X9zj7$svm=17+C1|4$a#=_M-lsZ)pRh_muA)z8wnT1 zx*P|KRjrzI0yY)eU*Fb@QZ(bKY}&hf@O%97q1R`F+N1{`m<=RU~NFFdG~tHK*FEWl{8c<4+;Gd>#Q1?Vpilf)?ag zO+W+_2*Eh)z+?()`ZGJ@o)}fVF5*HO;Z~4mul`D@F{@Yx07=|b>-=_ThO_WTSOx9> zh?);x^e^NCrD|5utv6zxqfJ(tpvUZl8?salLyfE9a_7mcRH4yj#x3qztL2=7g&r`Z-2AM*Lc2TyvKDT{hjzuOur05ZpWU||~zg6?Y;)Q;>fY+eW5psp`M zISj5>Cu38(o1Z?~ACBL43fQJt;DOkeQAW}DK^BA;e+NuF!lbNjZmPy7Yj)frfKsJ@ zWj?dmCUW#KGaf;1bQjhQ-RN~4|IzDqTF6Kv=pq`!CcBoZq6y89dzu;E@jfD^0Y(i6 z-s?zyuiddKR$|y&!|#Q*^~9{t*T%A>7i!?%EOPumWv(J4X~}YwQS4FN-Q(O)!A3PI zPvvehDk3P=AMvHxuaj6lV5;(y;0-mu8?GItUY}5tIsApMpySrAMVvqnxsGC97Fm@|#-BpKV;FlwcCY&& ziF~jJa*YTXPSI$9YS+&%)oS3}cxv-%S_x%-Gk>oG9^yNPEFOm1K`}rgr!cJ1U zRJ16!&;v@w9VnnXooEAvz`*h)1EsswKnuzJy`>+XpnNiT27~7;lxh?D1f@RVLul_)eNtYn#@+a%u!xmYk*lh{sQZtKv;q6<9;dW-k|gR!kK@AqvM|%X2-)SQqU460@ z8rm^BdW!#vOaBucT?;tan~%ob!E+$9E}=M%3Q8p>9<3%gXiSmxTScCr;vsOCHPmN| zU-a;Z-00{yy6;o6__OVmf{(p*v@0etQ)lK4J@+xj)3sH46UN>98Nz$iRj9;3!je=? zdv3YJVy*haDFHJ)TEuNPVNBizLd?aqth7QoGQ%*8ygRpML(Fg>GfE)Dv(yqh3*eSh@J?zt)^tbCO9X>c)79*1YkAxts zIb7H{F=oH!yOEyZ02bAf=L;$sG%BLw(%M}HTNzg}1Zv-X$E)ACg#m!NHM0v7&;=6< zTtq`=#4&T;P}O=HMYV+tr*IBo2A{Y5+w3qQLuvG5AU!FNMqlPOA6}5vtWtK|p~=4H zLlYuq-3I!LOALx#^rq~Z72F-p$18*5!>M1dx++vNFnnL@D-O@QliZt%wls`IRqGTc?)_D8a;PhlqtCSmj3o zQ>t7=hV$JGuoq;$CVf46d*mU++zh8EJ}iR!p)Z%qe&HTZj9koB=t`c}GxwfoV}HUX z9cObXM5)hmi*(j!3)fTL`XN+GuSx`~wOW9ZEqjsM_HW6>+ilizv7*Vc1dPl;fnz{z zW_lDimWY;lJm@ffc9udwNIf*EojW*nbED#LkjmC{(`u_-un6yh_A5VgB@~+lEx3Q_ zYsVQb*7yf+?)s1RCBnNO=ZoA3=ct_=_YLhXi1yMxF!?Sg{Oy5n#tnmOA|w`+M()cldd1}s9#USK_e#`m=Uva;b@^MN z{C;~rj_!Gvd41H#2vi<(^U|R*)eLk&y`{=j>f^Wb7Y&Q~7_{Y+Jn5XQwC={@d|CQ0 z4034@9(uhow|esC$(*@Q{9@DoLIhqIFD{J(gi^X__=1v(l`T@NeJdC%?7sKNx04o; zMV7M~FXYTBV>wJa+i*H%!-&uT^2xu;HXpb)_QnCY)djY_--kWZ>%Kq?veiV{a@(Um zZLB5Aru|5-+43(pL+pwC3S&m(oyKLrl+j1~6?A8<*6H zo(rZs17RJs?a7Jq&3jaQtrlbTES|bhn1SugY13z}{IrE6pKoJPRBbGwbJ#=vE}NSm zfBJJ~WqZmM7;9h-2!t;>wX^0rL3}h2Pq-w!Z{y+@SBSa9+BEA0czSSWl>x540_8s0 zu4|EVa_M_z{2pMO!$R+w3cCJIIPx*MfB-h-yB9yMsUgT<7We>FlA)VF5mZP z!Rn~0u-ilOxh2Wu#%erqnuLiQEo|P+-~|JHtu#LHTX4eF8bMIK?babnp*vDiXH)8| zqQj2$*lkCTdS>&&$}$-Z2nech72{OladeKv@;miBn{j%vqfvvx(;W<#+n z{CJLbbt6Owbj(aIVo|p(HsozBovq9GoXr?8a^o zG2+Sh4?bSjYC7jmhVqmxi>>;sv!7HJOKW?9=l|5xoHENh!o3g1nW9T#FPKaD$D;EU z>*@8h&feAu?Zvrtwj!}rH>EmGK7Fe(SGF4(C!}wtS#|v~zU-f=@jPL;Y|Unt+qHCS{1dF6b$8cqtky!q&KuDJ^_4 z{wrUHaG=1wlKo~lfHL2FHUMy=zc1d5w1Q(sp&e3Anv!X&ndNd zj402z|69s9$qOjfUF0zS!m_1t@m3c)=yc{y?3{O`*cjOCH#Ds;3pex?S-3|JUK)If z_^Td z*Wqy(m%`q;LMVM#tg#FduP`b;Cqn((r1koIZ+}S3yzZuTc~iyN4I%Oa4bp6>FJ3R+ zGl6OYFmVK*TBuI)u|M(n`+BJcDWh>=b=FiV?G7E3v&Ui2{So|}j zpALeeioef>zB{>e|22@j@)(sePbO0o74|y1W1lR23G9EnD5A_B_j& z8H|)<21SsiwA7Vj(qwq;ZxU!!BPfSJ|G3#)WB(?3e(Ups-J53@dhz7`_*2>BcXQ=l z#tSB!)EssTD4gBRZ^4*r4fUqq!5o_pOgS<1 zs?qNX^#hu(4zj477r5Ra$%)PQB(*bvq1le4We`hLPgYKZKS59~KU2FJPu5eqrJ-!P9H_w3|Zm(MV(%977Zzktz<)DCPRYs@dHG?Sht z^)&wI1IK02(8 z7@dNjEf#UTP{x)tE}2*?^w-gPjFn&d2kzQ=d-#y4WF=#3XxMl&g*}5m?n(&ai1;Yz z@@(`ZgI7rDWcw|??^xa*4pUGZ?7R58V(#Bs~xO|DE5z6;6f8>|ImY=#BIOlg6upno0Z8J zj&#r`PzVTA`+KA4{$}YAf@hm!f}+CfuaRmIy@iyPOB6p><&VFc3HBNjEjbA(inXW) zljS5|rdBlbHywib@C5tgTOEr=oat-!mV8En$-XlEFCHUiqx(ufZ;d^JBC#8Nuk(e! zQwpA8v`*bTCuXYyD}!J89nIB7vI6)oJf6wRYlT^;pbZ=#pcd zpl?3O2;xI;HrJm_#t;4~q-V1gDW%W}Tw&F8Wx*nY8btr}qf-V!9Qy3>M&IZ{g<~`p5HuX16Q99^Nh#ZS>(DJO0LQBO!@&wFekLrE7KOCBI>o~p4| z@uvsZ>HG5>sjwmRPW(S=-I%?p2z+Hpj6ePC5ylaL8Q48_`pljYN8(jdZBSa7SkI!& z3ndIwBR7E~wpsxTC+e5Z%~awb1B9~`SVigm;{Xqq66K62_;guwb$&$jRA9KKW>~N! z+L2=msO%IP^W;UN>H$}5oJxc&>`%81yN^sSz)|p4)fvep8cS@HZOJN@v0~^O&-(8J z1ucdDiTAQgqia);?$;lYwYf#QVNb@*rlmHHuX&f+7kk()TrX~4C$Y%D>M|@t4x2*^ zuksz_m^PjrjRetxVcoT3v7BXF3BPI#Yw7?h#o`czocS9uxRXqp1z)=#@Fw*Dl^^rf z)%n#m;7CFL$A744uM+HE8`oO_yx^m+s@t3TIR{?iZkYgzP~NVq2y|5xu6CGhxeM7% z6LZD(KO-!4bJI45hHw4|pD4p}>CNm4A1jrfN0tExKQ8b)Dcd*popSJaoh_H7W`VOPpxePDe^1m)3&(cG zL2n^Iaec@m5a0Lg6tKyT9%YlHJ#cqrXN3ETC+=@;Tk!;ZlQx9Ndl)vR6OcWNF*G_% z+rwp#T7C7})(uKauV#1rGlTCK+qBsJ<3FkRMK|e!!$?bP z>7q7Ys!o#`u>3^ci6#mK4lpU6sfJnH{#k}HMDhopyuUg-&g0jR2+Gs?q%nt4`O)Ry zDQWnbf)Q+h^%-a%q=`sgC4di$hwKypbRuA_>$A2>dzJgXg@`2~MZ_B4FfuGzl5|Wk zDAZwR56+GjJAd-W=C5LXC@uCM#X1%7Lt+(yW$T?6wAm$x$T1P#l`1VOrH>J+uh-3F zXm$a>&@=&POkfkJl((RO0!nN&`?Hy_v<90%v~4eav)Vs%`g#7i1Qbon@%p%HlHAk* zEG|vTXM;bR9NM3o@^7s7tj)0c88C0FBXa&sG_liPd1p8TerE{l5O1fYjV@hxUzUsJ z_a5Zc$JnvNrTG$&(3U0OiAwHJo?i?fxUnX=)_l`eS@B8*Ym3*5-370qlk`r{ zPv#4PFErxmLI4rcZF_%)jGh32WTn4xNCLfvBMe~AXQJv@vhCCs^|Mne$QqHZ)7j|K?L-kj1UnM`bSMBtu_2Fb8zBpE>k<@I}P9DH70i8Uy8=cd{3wZ`! zm1)yY!IKR>9@&22*{8DZPD3L4&uRF9H!k@|n`767l;oexykB$GRoPFh5hS#8xe4Lz z&q90_N`0;L&#yi**HJb4L;{Zb&Xc5|z^AOJgc^7Z#yMU{T059IQe!z?Z&_dx+n-O2 zocu4gYCDt`%^fi?<7VMKZWvU3<=7QTwjs5)gDG>r4wdi-s38#OVb3TfI|ui&)GMRw zb;K=;Y>|l0A9K&1l>{AVM7Q2CGsTba$be72yLclnRLzj}UYH_E_OEFE_V!Zrqp0O^ zK(8HX7xSOi&i&2|e!fkWqwaO!H^o3`4ZT=D38N2BIvMJu&pQmGX}$g*ZQ38=rEJ@i zb(G;)?8Dh?x5aP|BA3sAvVE{B*dVL;m4g|0P`y?!c(RMFW+c~L*RTF5T#&kIl56DE zQBUJRSi4l28KP=4FojL&B#pSpF`ajk)sZ5H+ z2+Bl`GmNYPz{C$S-(0>QJt!-vu(O~yZ5`@F(}?84zIRwih2YNItxv5hX_m2OWvA0p z&2|E;f-S$mal6;KBBT18XHKq`{+d;6J8W(@dkBY@SLGMW-By77m=P`W@f}o~0VzGQ zQi-x4fJ?HmUx6n7oc?bvqkX|=$H!xTQ!=11SZumwpBr62G5-ISLN|Cy)OYlmm8R*G z0Qe4!E9ZlOF5J90rME(oKB#xMHpOGa31Ie0)Qjogl|P(=;CYMs_-M%Z3pui!-MMkK z$X+g_y*aX-q11dZf6 zD=$C5*`0)o#-76z@fX=j@_gdnaL?OkB1S~J^ zKN9NHLDyuy@+bq%z2ma)b#1ID3jC$m!;GOHFy&XRqZ-e#I}OdMcSCd)X`2_S?RfTD z7BaQiOE%&>jw{%H%)N$<9Oz? zoTA{?cucbTCf~;p@_+Y6$I26I;V}qkHsfBchIGyaf$J3&R78R@b^m#8X^8>$*;hLi z40T>;3!AP2k6NTiPdWP@DFZIePFbRl+gg{gzM0LnQAzWB+T+!iY={wn34z2o_*5_-Yt$UX9 zm%>SFZ*1Z?7B8D5)R+SR5X9$+i1}m{3N(*U2&*19Z2~5|5f|inkoKBu-1x^Wy8mH7 zXc!6pqtRyfZMSXQRdtua;x`*jS)sP6o+W?-Xvdfk#@75od;j#Jueh&k=r&{s0r9p4 z5uHfGvGbUr8Bf8T^NJ>2voG00QJ_8n^;@IuRf5gVH31>AY3iv+v{+EsuY@O;ut`T1 zj@Ab)rCqTnhSO{}!2(ZA2)BD(d}sP}7dY6-GY5U>{2OqvRLR-6paQyXMovZi3=dFXfXw2w^-d0IldD=oiGp9%Gk1bHKA{jH;H9 zs(+iN;^&P4L1%1*Fvq9Y?-txne3$~vb<{&yo_l6~`cNS5O|?_*UuTYeuOp+$?6Ov+ z9P9WbHukfeI!X*|sNf8jw=9TLuS1ECD2CD9U6vVt-u9}=eQ6n|kd50ln{ZtWI=jg^ z-tT8R)S{dyvS09qg@trVNbNOmz0pOB6vF~UOW0|7|NFifgX&(R)F7uek*>ee=Z?td z|1Ij1y|b3+sQ6og{U`-WFvasG6(5a5@l`t{Hv(`~nN@+}FG!jpa};F*73yf^uMhY) zC=;Fo8N_N!lonflBze3s@^>IOCc7)4(<>*#9XhLykF0_lZKV{>>VjFDl1~zfdx54W zn#|!A;Zo1$KZLt=!hZ($i&27G_59rda8;vFNA50CkK-R3MtXox*_|o{% z=3x*2Ql`_I$9MQB3jS@%9r3D#9{STBV_t2x{iR=fWH^m=Bc^;x8$irIK!`Z*?sne@ z;6(8-8($Z)|2d%KPaw4mvW*>GOn)Mf_ld1j zV}DhGI%oCB;7BJv6%b61Maj~+zlG%KFAZPyA4^+ztcuC4iikuH;w;);(M?Ss_q@1p zZ==?gp^7-ZQ>|VKQc9^3i+Da%RG20uY>aBK4O{KaTG|yQ)eTvj&gN8n9ttJ_+<;HK zG{(oS3{a~7rEkZSEj-I3=?*1AkNr~{A0yp()E?DsrFDwur+b=1JU*L6uy`bqVOdNpb2 z9dDbwclL@?pJp>Jn|z|(R=v2CYr*mkclVmgRM)25n_$uUtRiROe@V>GR33w)B&)3Y z_39j6T)6}~*l0{y`!KCQ^`kOdo^Y>|0UlKkS1anVYUNMa1*WLQ+teO*n6IN>=jU#` z!|8ZFR@M#Bek;Cw)505hX#E&axAoD5X+;mJs(!GETe|{=a=?E)vc1_s*ZlVwzIB<+ z;Raf9Hag8~V*2<qAYIw7!Cl=`M)zv?8F|+K9Qwe0vSb6qmrnRNj?pq3~{&3Lr zUF}xZ5mP4o-C}Rz``BdN56EO(3ykFoB!Yj}#tP00GILsUs7tw}7VPu^*Q_Vl zdu2PU8l&F1(CT^h6Rwoo(;H&(Zju3ahN$L;Xk$0X(PC9K#GrYv;4Y*IBa@^=I3IRG zC^^H z@hp(apys^?dU;dYjn}mkr_#U|j>ArC$;{&hn-miXSu0TeY5!k)?;Q@;_pXhn`$Qs= z2ojN8I0&e8DosjV3c=DzTb1sb2u`PSeTKie;d7ky$_s+FH*WMfZf>oAnAGUa@PZFB^$K856E?r2v|Lafc zrH^r~#E)7XoyF1%lE=rotDWLR0LKh;xhy3&p{aJ2X)IfHd!@6%GQ-Lnzp`SNV&#M`TBnsupQ4#lzkvr{oWUtt$#rygwB7j2;e zfHeh^Y@)y~J6DV2edF{d%DSTU_Bt584=AE6*GGM7RbP(L4=wIGJa`P35SE$zvQoc< zs`D;TJfc&ByL9d*8ppF zwDIe&dyDm_UFcd=yzr64&DxEjSj>wwjn1ng6qMdb+b(TnsMCFeV)3Lj&icF{CIw~H zds83v0|CDxpp4JDR{6jCqx}Mb<%$8TWSULAAD8(2A8dlyixd5_8jMso-`Ibgh=1`K zU#ZCe1e4%jrc>w~iDBwPR0b5Oo>lEvz=78!Wc08}G<$k&(2Un5Q2#@1aF9xX4NDgw zf*Fp|o=f=jDKielI`8k$?R%#4x|OV#go!Er>!_#vMX1JG4^0D% zwh(=JTK2i)dMjOvR}8%@F~yVD4zX-`iU6o07Yu;edhG60Q|0>YPjK(#E(X`f3ww9n z)EN7_Vu`y70+eCAs0Zg_&iFJ9HqO8UJ4Lw^^v}8lZN?UIdoJN%zDM7Z1)T1iy&?Si zn9S+N;|G1i>sa`L4-;CnmN3ZXK;4Qy90L5yQCh}iTl`<~ZTP^rzTB27A^6qs; z$*Pbwxx-ynzNfT^0#@3MNG)OfoLlE^;o;GC^v=506C+A?q)#4ue$lSjU)~tJ=|+21!ZOwg_VJ2KxGBp-@w zrA9VYR@YQao@HR&RkTEFJX8uwlOR3W)}cQv_~rB&L{|=cB>c|fH6+bpSMq97B%1q^ zS`rVHrO3zGj<-;%X8xvO2;ux=L&dvAJz1%~milz?Wahuvv*QyV`M4hu{7|+4_a$16F7g%i|`0t|#d(s>GS2@gQ@I|l5m4k?SS$HVS z5R)e3u4KF)1F(uYQZNHZ;J1NZq%8-t3b1*#DC&|ts+?Vbo`gM@+}`R=?9|c^@jZtR zO>P~#&zW3r@Cg#XdW$V=y_NT=Kz~fT&tl!D)i2Ai*4zeJyBYf%VI-MHe$wDwyc}Vc z_APuMMV9I+V5=v11{qCCeIQ^@{w)u^KEcKMWQdKzR^1yB$}dM2&J);)Vyc0|2cY=S^5$BcyAeWz{(&9&?^1OjB1wo_068g>c39}6x~Wb`hgGlAt-WEG z1Nfm-5wv`pVv(&)&I5)z9z{I4;CGw<0m#!IqY|cjtpF_2u3b4EN@1JWtUKAUtmQJ* z6DWy(J3cfd8Ma=*5uWl#=xx{K25#m|sWpR<+klek&kTe)IZDWh*2aVCuDZxuh(*xi%MuRtxPMGXW z{@K^&n$ln>T_|;J;^a6?Lvjfh)PKZh|FtwJujtaPd}fMlq01P_41CDML}6GfqP+}5 z5V-v4!=?yTHTP>t!6X}_3##1~J=AX`_J=9jpZ?G{SAJ(-EBYpDb@~S0zu%5WZy*!( z2{_rDHa!QGdS1T1{UFU~8(Hz9(=iR^(^-&c6)rtBl&ZNJM}0KcPY|I7@WFhzz!UvB zOyf7YsmVsUP<8~dN70LN4GnQzF4}o?7Ogww*Uv$+W&7l>Hr{q!C2iUcU<^Fnb-GoX z?zI}LKN#=tohH+Q(Hv@CAln41 zov)<=n!R?s_Y$Sqof`@BNE`dGI!KpEQ;v*#ksK@wa6=dRGqZ6VS^$WGw!iNDOY{Em z>KZNZQ5Wni`da9do;ZhXb{cgh`{{`rjLR`>0ON$)^ij|$buCYqxD(d!(L zz){B6;gU!yK+mz?eF!*=w%=uWy-(D&RQI=WdL{q_;k@zUgrfW#T&Gcc{+~-NBlr*K zj9E_J@YH{`fJ(I+i6>J5oHo6`ym-%&pU-$L%u%u>O_aV)gqXc0SeuAx?25V;0H$ni z#2t7u1H1Z0=lI5t1cLCk5<4d80*&4yGe#f~{~5)pf@MY2!6M^hC5v5+OLw9)Im$T1 z?SAL>{_xj6E2o!L5vs8>NMZnvqDF+D7w1t&X|E-MPa1$z14l=MovwG4a>yiGFl5#+ z4BLu!vJ{7kK$P@hM>`2XM8(zb__FmF!5kmea)G)*mO2gquBzdfaQZykeM-jC7#hG` zqoCzcAeCf3bN8n&Qmaz->O@WCE6>*y6lX2rrRHGn&U`1PpQmiF0jF{vy;y(~V|=|4 zq`i%w$X)uY=r+i(7wqwlk}V|SlQm8rBuBqlyfz8=cuJ^mfA?f~1u zp8t)!3U}^=+ML&X_?u$W`%@_IibVU^`vbyhD9Y2El40{+cBAyW1D-E*5PWPhcR#K+ zB;xV_L`_aE974-3%i53S z`H>s)6jMsYr5rsg!ta%QjF!0F;}cokW&ny!1AwFNA{oAWF~VRM|Gc^3Bu_k&HP)E% zaDYjEv_>to^nf-c4Wj_%Vsa>VJ)Pk*@!;CDX+zkn$4OS{?#sBq2yme%ADR^|l9h?H3FK`o# z9jthZ8CmUuq|zs+>|B}n0H(3b_`IvT8yZtP7GDmNv`waZHTRo3gFrog}_iqMYUQqa;AzYGefPDj5j(;v#7P3Q_U z*M3tLTX4HUX*Ejlkc40&7(Mi-LGv2F1!}YD zIH;bS+TJ&)@54dy?c-xXVwdMYz-%)(@N?^>W**AbHYyCm|6+8^Tdp+U9CvUke<`4` z^DDE|pE>6f+J-JCdiYzi(Bk22iGKWEL%R2ouk~^Uu%yO4GuS6rp@r&fEBd8-9RJ}? z0=V}q_oB>weh%^OkVa18l{XR8Wxu%PpR$Mj+C3a)FaUTN%V;Pbyi9(b=725D^21N_ zE7E+>j%E97sAs`z%R?P961mxr467vOKrgCxcay4kzAq>Dp0twlv;5{>oou2Z@HN-V z+*?ph1jNw^opxCUc}06J?IEYN~myW-(<8vR8oQJs96qCb*) zd3AX(ssL?Q_oO)I9k9~_`fEH7#_;)RA;ss>B5W0@@HsGxFE*olUUZ?gvV9~UhvF7o z)I^t)t&BG4YrcLR6}L~^Q0tBjo&#cStqiGIx`~VBoZ;z8L(H+FvJ3Ez~hsGLidl2nSt-6QA?*RhV#weq;w~prvLCtEf z^~1{s#RlOs#hbw;Zg=fCWKC+V9oz~h;RP#GZ;ry6jknOx45k1SsA!7Tz-bZuIY#() zEpdRmc*D0dcYaDVq2bTOYh&!s6vj8*?YCQo^v3uPQ7zZ#P|a&4pw%@Wl_}D~Gt_eU zcQyjbtvxN|ugt#SS6M>9MQrSCe^Ro*eUy>SeIx~BV^7T4OOUVniew!M(MSq_&=N9; z0Rqfl%oj0XlHkxeHYvUWhRMMsLm);fg3LS?+|)n7+09Y9MfFYgf+ZSA1-x;w-j2+t z`?FV-u7#S`<=T-^rN=5WHfl1jRRQKtFWdpTKsu>NZn;rmdQ11qi|lFuLa9W2US0{2 ztJATIv~kZWe122_#N2nKvI2CIWLpCAXzKOW+f%&iNkig#P8I90%}L@63xgCPZk@)J zQRx((J&(U>%fY3#vPJ(nwcpkD73o(MPkIKLmi&>m}#-4eD%H z0W*%108FM_YSw>c`aCsc-(9oja2v0HVN~0yCX88ls(w z*4Bt#R%BKGmnVLZ(;98fFS?D~mOga_Jz#s4#Sd^sWmwo3$Cr0!$NxY9@eFp4)JjVC z9lcBd<>&RBR#06)crsc~XYch|#9Tr1`yvn#Zr`LM!q*V+_y0P4u6>)rxkByDZJHMc(;7J3(u-BiZY1AUTV` zAH4xo5fsX(6I|v=i~dv%pwG_5-5tw15Np~os3-uaH0actbqE4jy7TGggvI?LAuV6$ zb0C2LVk(Obpd}dJRREC97fHyZ=Fv@iLq4!+Z(QA02pvq&?E z9sqxa{f*MR^i_g|oNXgok(NiCRU3h%lA0?&*JcTS|9BDK%Hd6)^T`o#^vtMu+;?D~ z8EGjdYXKtB07g^#APF}cR&erPwf%4)$|J5kRS;;ko}$ms@4+JmOAvs?GJgpo5K>Ae zQ*97q6VLINZtc4I#X_o=JW2iMsV=3C9AS7<)t?Gl!~?9uYIp>8S+9*P$b-Abg1>mA zr7JW9$%tIm#UTi}=3dQ6Q46oXi2(ApU+Y9r`P@zm56dpZ_a?|B%37 z$3pej$Z$sT{nO#EF(!J;%gznwh`ZNn(>xKUkJ~}gV=0;A#q+}wv+wC@2|*&I`L5+z zi@q;+Y2762@6cUF$Tm9UbOjxe4@h6+VMk)+|2!<>4=sl{JhCM9Bc7V5N3aMa6CPyj#O;RoM`{}qZah>vw570$Jtln zEh)_5$!Le&{*0K>9W{^AXWxxETsTKRd$QtTM&!62h{$jaMz6^7d-m|8R1bWX0@7 zbQQyQ<7hp?Cxhx6=k;T=wxzE;>i4pECwKRrVp_?O@ZZVg18LvJHe78_d@4m>g@L)A1R8 zf%v(nb!A`OKR4y1fo}IbJFHhUYWWI%SWQ$paZFcI(i*HcHLNl8d+Nrj*h)5s1R9rE zE)ie@Q8oN>!(@45xZWx+;-KG?+~?>tsT7M>p9gh|c#%Umlr+sV<{4P{9mZwzlC+8H z`1K6hq_$y;5sp_sePSg))qnK7>8YX7H;bO2D?T~osWJXeM&BIqRVTviV2ijL4_)ld z9Wi1A>8>d{;n}l_S23qe$uUk@MsDR&`oH!Fp9knfhh}mlPm#u5*yFXMc(oUGXC@t@ ze$LmduuRtVzFF|2{0PUY&+|glvjoi!f}b0LV2y^g%@U?FQ!{4HD7%PEv>I#@><4xz zF$=m5X9e&2x~%I@X73@^W37wTG_*6iU76Y~0$Yzy-(BH&K*FAuwMjpHtNZnd2lhDg zh!!#ez2+V;TP)Z%6v|QCK!TIfmP+ZpWqR@r<^eHoca1u#ryses!ILfmt4JqYrm0ex zOza?M!`>yVx{hjSOa%<0#U7Ss-yTp~Zp$_wqMP;n(ZK^v8~lZ;z3TxfDS5*F54CFQ zj{abQUd#{B9b)`|1x&>g0i5g#vkr>)9f9SFIY5m z;GBi*n7YoQB0-m_N`(E&@wA&>3-Fj3zuXuN@!=&^Q9dS08{>D!J@ z)t`3$oxo>JTUe)B%IQ3-bNovYtLL+7!g}CaZ?`%3j1}))jN};q*3#NsiB9RL8!|qc zCoT8g@@qG6ID(wj%?=Ff3o>Kxm0Pzu6%_v3^ue#r2=t%M|oU=4ahX}8>>w==MrQM=JTVe2lo zy|!8i2KhjYO*@s7HjP^xCr{MLOdh-87o|5nl|^(ZA4E1%oH(1z`SfhLsNIG;?kx7P zBaLgGP`;l3J@UTKlk*!6?u^-278uQLM&l;zonf@i_gRitKCR7Phf3^v(uAM{r%^%b zo-z~1Kh|4_vTK8L0&dnoUyPlN(b)H)ujkiDISs4AS=u`a+vMxD7jF$E6h8fw(D3b% zQ^NHJhJ+~py%HYu6Welz@yo%p?OpB~pNt;a`)O=XoyRWPW5i51PcmPLCpZxJ3e1OT zC1S5w2O6f-)hM)WrJXO6D3w_e8 zy;Ki5<3z&ke9q3N`)Z%gyg7bf@{53@ZfY@ZAz+t9&l|nbg1s0|4t24*cDW~wP;;Ji(hv&n%-akT&dP9u=4@!xmB;Q?pP-r9-Q<*gr1^aJ9dV*OH-ZXbL$|gGQJ&+7Z&`bF zNn~$zyvRJ#13X1hD)OV=j&ANn?ZI@t-CUM-(_s!uCF|YOWZ4_#>K4_V16kK*)NY*W zPFzw$PQ5*bKgGSquIO^iXSTO?Q5nj3+ZKf?n>sHz2kI7-wP^2=)I&w@m)_VclSX1L zhX=WlenQ>oZl@bZ(T1P`@7oi?zg(3Rpy6U^Fy`be;12X$;Ea>og+6U9RtgZx!jZf9 zd7$cr5G!&uGr=7*aZ8>wO4`~%{TXX7^@p1LV)|MwGvE~d6{nnch9)<^3Yj}n3fsj$ zT`i~ia5S&Pf|8(C_GTa7^;;@AN-z#BiH+cwJFnh2^c$xoz;$v`3hG^F1x(I%OTSBU zOl%(^5uEWGhddtT=rcXy8ngpTT2rmE`uz%FgU{$@uq!Zw)XFDLrze{MR-2n`y!PT} z-o7&uEXuwXKPgz0@66VW&&GaR+U7(sc67Hl;XB3_(+ym(HM+8DNepy4clJDLb_@1K z;pVhPT))q(gJ#4X4;z>u+=mfz$Kj9XKtIG!Wg~8B%-wTWK+h0O595lkjR#F-2ImVi z$0l>Na}{m!tZ>BK)?jm)Em$u@z{nHBC{S4Y%$U?APav9m+bo=cn%{{E)e!Ow5dzL! z`If>5E^jw&;CpAjpR;JruT@!B#8G}^M?GH{QP*q3_3`11zj$EK$DrWr3~W!Q@!z5< zhA#yM>81w<_S)Y%en9*4t>>gu3eR{NTKz6M>yCn1;J7-bf8KwY9#p1VVl?KsZzr{K zyD?2JDu9NfG@{N;wl^@}pYsS67KsXf%#S|5uRM%14uEjHo;!Z7F|GM|HM`&Z1RAx! zG$=il!7NuV87*hr*H>G4v{)VIcQ#wOPkJmf;k0(wmS5w5iq8m}iMr_GeJYFQQ%-UX z$YW;pWzoN z5D6FKl^-y@LG#|-DmzQ0VN%t)VGoU3YhE^YtO@N=8`$%a%Ut#NG>_TvszQLQWWnQ@ z^a762-g$0P;RH~ckm=~DcyQPT6qopZ|Iu&5Rq;Jk`uH^4^=>0iQjOb?G@=5{p<+!D zJYRjQ7+e}ZU%U2;fjifH_b~n^`v5n)@@4zkr+Olhs`8(0laMtqhqoRKnBaODBjHKpN*~twzn8J0w;GGGmQa9Q|#a}M9$`u0O8*4$Z_URGM^6u@0>krbrh$3aZWxg zUV#0%5Nld+Fm%H?W|kUx#2$$#8Q5*;OpnFE<+b=FUWH_*1(s-&`xe*EeIV~TE-n>k z)a|D@Eh32zAgu>TwW<}D2X)szU|!y^CtO=u%XD;SX%O=koxeAo(VgA8Pf}m+vv$vN zT%_SF7N6l7Rx;w^SS~%K(alzQ{k~_0HpuCy*wBGKut%z;VIxi+gb^e?wTBL)9-q}) zjGb*#I-XlpzNTS5B^rm(e=nmnkT5zt>(Y3C@@moy?jVYDzGSSS)RMcr7r@ICp=wfX zy@uUXF=+HzL*nhRKN)HJkczZMCmX;%x8t&ufVzfhd1SuNCIf z3&=6WZ;T%9Zz=73T@lSU6m_%1sl?uHUi@k1u1f8S&feKEiX=fCwHM1y=O*|vY(!sK zayl*Fl$Z}P-KYC$-ljrRzM3(K>ur0{JhS|Jttv+)!C1jjq~2kMrBbCLCr-XN<{i$e z@}{~AyMKH^S=XaGs}D3s@tX})scRyx2xc|PZ@kX~)z>aNIhvBUDHtidk;^)L-J6-I zm{@YFTwV_z+UD=%K3%%l^N0aU)hs8dNj<+w$l;LE4BpV;GyMQ?X3EY+qvf)+@-$$ z;+rG?k2}mQ5?F~&oPJcwH_JFhv#M0}9tq4p0QOax@Q4K!|87Z;^n`&6gi9FGr8~^8 ziP0uHKT6(@yj6F)5r_As3%kDQyVu)9$L_s}2iJX(Xmy$c+vAIJOedNR4eS>1U&eh! zVGCAc-$U!fYMjB2aL{3Q7Q04?_(783hOgDcugNCt#PS9>t|pzDNleo98>CrYeRoW7 zPSW&CY)#2&bO}IV2G~QnY~{=Xe+oDa1u#_j+!hjb0MO#E96=ktYn=|J;goICe4!@D3UC zOy`b`^Gw$$zfB5D51m+JkPL4)PkQ36s_yBo>o{H0?k;wcg6%5OMoBf54pmHf-EJtf zpUa#nYuuy@MIfHfeM0LPRJk6)R#!#aU`Y3ui2)c=CJEA&w#nm#dUq zoi#?^Hn~(6bgFZQXJt_df@mvb2eNW^x1Os&#jY?CiIUi!uZZz?`;=#BUY$lX5_z39?6Lg1UJKXu|&2av>*>+kV9k4qTCZ^w|X+)EJ^KixZ zdUL`$1}XLhb}z%@9`$61?nljirWt%2$pO2&0%JdRW$8qUd{|AQotOB{_}4`>x$GfR zS?KBIb5&I9SFQ2OncCW8D8yr3+;=gnwdtBH6C)UH@T6hQY&F?%8o_d)BB2#FPZ$0) z<>uIp6@M`got9Ton+EuSvEka}tL^QN6;aDMdfM~hE7Qe1jaM9--P{{Dc}dEme@d0L zfh{RFd+>N^X%2T#_mqx%{;hWm)axu2)|-ep>sZd;5UGr{y$s27jt zNk8!+Uhi(}#z~2aky(^vB)??EbsXu+yc(3!o;ptIH?19S&SQwzO8A)uWX+W!Na6&sSwA)SXyZb55O9Ou0@ zP4CzdPBrmbYTw1mdqVaa!Mia{-Y-Hx%# zFkZTJmPfp>_{O^x+?wbJKc^00k0b4$z+2j-Osf5Bb^$QbM2y~}sPK8hV}qbnSLvT6D}qA;b~sA~+>L(v$q zOXWSud_A7zAJeEN*P@QQ<82z(NKVuGF~7z6Ln7jRS@=JHCRVIo1@>@eHqQa*!?q2Mjs0BYF%z*T{Cw41xZ6`VVT+jWaHKw-9 z_s;C`$$2hi8sJ}|c?M-f!t(Ch6Fn~;U69bV%z!u?r!f%R$s2#|HB}o(xHuf;d~?Lo zJ-&})tUcJCU~hF_wcM2>7x5%81n(5k6b;acO!r-mLLD3gllQI1Jki8`G-VL`T zKjR!ngxa0OgH7g64|bKW2+4uehrsuWUyBc2hdiZd7*?bF&H7MTyR{B6S5Ws|gY3#+ z%iFTu$+Xm|WthU2Lg2A?jIcHoiM6!-^=R_BI`_akvZv<1p+EUVIC8;5C?n77MfC~k z(y~~awgN(Mq}RJ&Gm-X3#v0P&bGv&D)(NxwdsuhLY8esIFhZSqh9@o^tt-)DL8uw8 zSMkC%(~CDA&M@hI;sn(V4RYL`Eh#wxEVP7`6WDS~YmYi^cJskpBZVCZKbI4wbDaAO zW`8a@8fZ#$JPwRe<^{*CrPa424$zS3Tz&_kOxh3$a=s&ZNYDfNslHq;J<24hACqU& zo3XBK1u`(t!|uu>^RB!(U3v8JFu#b-m8EJ|E$VCh96I0!p5mYsRT0XnIC(uzJ&n;4 zN=Caf?XNzn(0e19I%|ACp8NTGY9tM75_Ywc(6+x?k4{s)<>WC{MWJU}a${1^#=1*7 z#+2olz$(>K)z0ClUov1|@~?i3zwA82rGOM`u{7*k`ZCx&>E2p2v+NTv+TwMNnkieK z+-xj%Y9RDtHyd^uMg731Z}$;8fEMI>NOKB8;2XWt5esJ+h7B*bC7%)dlzMJ;PBEYVdxa#^np?U1{j_PCmjPus_3JVqEjM@*3bUP8XnjRwAt2M41!S#={ zMM?`+h~tQ|XIdQs4$TY~j6))xL_U6b-G_b*CZsL=oK@~?`}OL{C>`=&E^hlllPRJJ zYUyLFFWn?8XtK?H6gFddPPL-TZnE_({uaxkaW>3rHfs(pgHn%8_6fd-tZccObnEt$ z^W5O=HwB!7`1G17;Jq2)DWfLhku8oWh*&{sDs?J2e5`7|{Vn{f5el0V@BO2Ot0QUh zN3uSA=<3~+Q4tgyXQHSZ_LtZD$>stFTgO?I!n$ZRK0(O~KW z&bpI1U}1e*a~i8Fc__JjLW+EKDJPDt&ih5jq(&l3M<%tZ(2%`l^DDix*Y|9u*F;y} zM$U8-}2#fL<17$TEI-1kP0YWZ~eWfxa^syAV0 z13kCqd~IfBd%o{hUlD-$qH^F#nv|`(fGdPEr%yWP>VbE%w*&)jeiwVtyfeF*u;QaW z@Wvivf?h&Y9ZvN6_>IuY58VF!UY@+m(ylpn;%(NDZ(-3KscR@#+H51WcyRLCealty z^#azy+woU(jRLCW8zR1UUQ$ZfH_lHF_AC~fr(Pn+%7gq0nw5sQ;l1J%*ZdJO)j**T?R#H~x`*{?m{TYi)=p z$y8}Z%8u1P^XyvtWhFZH;D%8gwQ)JkChXS-ES`VCshVPa1beJd^1W|~fYnbRDuyA~ zPTNHweh_2qxVHu5XkM%A+1FC2P3Y@=--=mkiXZveJXh~7xus{AxOh&7wh+d?x{Z!W z?n1Y}+{n1%`0MepkEY?JJ)fN9l5FFQ=-qAJm8W3em<*C7?kg{6gwbVr5h9w};8y58 zccMDg{m@w~R^q)jeEjR}V>gd7gsR*Aq$qMx7?K(~PC}F3Cvv_%3et|7Dp&H(CzRW} zzfh~L_03qCl(54slH_bC!QlkUv4xjM!j%4o)wJY?cklsYA%y09U79pE(ST{LO>2p- zr?aqQ8tT0={3DD8on5MI-=So4`1+>I%GX?N}PKD-6vFR=u9~qTMks`6GE|Rg7)t@ z6{FV9Y<%DB-)JG^&I^`s9OO>oZs&bg=;|KCnxQaL<(FIPp+g|Ajng89I+KDyKbYF3 zlzcq#DZ-n?40z?luu`+1-{mBdb)N#c==b_+Swn}}^t-%vN#S!mne!Qp~N|dwz9O_ZAVcrIQ61;%aH;cyR@KXGhFW&qaUno10r0`20Ip z;9t37V#J)Xwhj1OIL0aGycyO@%@*& zwP4Pc^qg1WM9FK=f~9Wa)tiaIO|xLR5f1AR@*^&g2|fS;e2pWce(PaZI21^9*N|NNKU=|A(h0I^@Bf9G9Xa|1*D zBK<`FJJh z#RO$#h4^H-goTA=dAMZ+xrKmf#n1OjKtPa>hhISYm4wmXxo74I`6DObd4vV|i5VGR ItH~1oAILv!YybcN diff --git a/mission/njord_tasks/maneuvering_task/package.xml b/mission/njord_tasks/maneuvering_task/package.xml deleted file mode 100644 index 4703ead8..00000000 --- a/mission/njord_tasks/maneuvering_task/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - maneuvering_task - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - njord_task_base - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions - - - - ament_cmake - - diff --git a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml deleted file mode 100644 index 64ca40c3..00000000 --- a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml +++ /dev/null @@ -1,25 +0,0 @@ -maneuvering_task_node: - ros__parameters: - map_origin_lat: -33.72213988382845 - map_origin_lon: 150.67413500672993 - map_origin_set: true - gps_start_lat: 63.44097054434422 - gps_start_lon: 10.419997767413607 - gps_end_lat: 63.44125901804796 - gps_end_lon: 10.41857835889424 - gps_start_x: 0.0 - gps_start_y: 0.0 - gps_end_x: 141.45540473589617 - gps_end_y: -0.22386536008730218 - gps_frame_coords_set: true - map_origin_topic: "/map/origin" - odom_topic: "/seapath/odom/ned" - landmark_topic: "landmarks_out" - assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct - - # Task specific parameters - distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame. - # If the distance is greater than five we drive distance-3m towards end before trying to find the first buoy pair - initial_offset: 1.0 # The vertical offset from the second and third buoy pair in meters - nr_of_pair_in_formation: 20 # Number of buoy pairs in formation, excluding the start and end gate at gps points (NB! pdf is not accurate) - \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp deleted file mode 100644 index 0c56e330..00000000 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - - rclcpp::spin(node); - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp deleted file mode 100644 index a8137e95..00000000 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ /dev/null @@ -1,468 +0,0 @@ -#include - -namespace maneuvering_task { - -ManeuveringTaskNode::ManeuveringTaskNode(const rclcpp::NodeOptions &options) - : NjordTaskBaseNode("maneuvering_task_node", options) { - - declare_parameter("distance_to_first_buoy_pair", 2.0); - declare_parameter("initial_offset", 1.0); - declare_parameter("nr_of_pair_in_formation", 11); - - std::thread(&ManeuveringTaskNode::main_task, this).detach(); -} - -void ManeuveringTaskNode::main_task() { - navigation_ready(); - RCLCPP_INFO(this->get_logger(), "Maneuvering task started"); - odom_start_point_ = get_odom()->pose.pose.position; - // First buoy pair - Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); - - sensor_msgs::msg::PointCloud2 buoy_vis_msg; - pcl::PointCloud buoy_vis; - pcl::PointXYZRGB buoy_red_0; - buoy_red_0.x = predicted_first_buoy_pair(0, 0); - buoy_red_0.y = predicted_first_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - pcl::PointXYZRGB buoy_green_1; - buoy_green_1.x = predicted_first_buoy_pair(0, 1); - buoy_green_1.y = predicted_first_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - // First first buoy pair is far away, should be closer before trying to - // measure it. - double gps_end_x = this->get_parameter("gps_end_x").as_double(); - double gps_end_y = this->get_parameter("gps_end_y").as_double(); - Eigen::Vector2d direction_vector_to_end; - direction_vector_to_end << gps_end_x - odom_start_point_.x, - gps_end_y - odom_start_point_.y; - direction_vector_to_end.normalize(); - - double distance = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - if (distance > 5.0) { - auto odom = get_odom(); - geometry_msgs::msg::Point waypoint_toward_start; - waypoint_toward_start.x = odom->pose.pose.position.x + - direction_vector_to_end(0) * (distance - 3); - waypoint_toward_start.y = odom->pose.pose.position.y + - direction_vector_to_end(1) * (distance - 3); - waypoint_toward_start.z = 0.0; - send_waypoint(waypoint_toward_start); - reach_waypoint(2.0); - } - - std::vector measured_first_buoy_pair = - get_buoy_landmarks(predicted_first_buoy_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_first_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_first_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_first_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_first_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_first_pair; - waypoint_first_pair.x = - (measured_first_buoy_pair[0].pose_odom_frame.position.x + - measured_first_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_first_pair.y = - (measured_first_buoy_pair[0].pose_odom_frame.position.y + - measured_first_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_first_pair.z = 0.0; - send_waypoint(waypoint_first_pair); - set_desired_heading(odom_start_point_, waypoint_first_pair); - reach_waypoint(0.5); - - // Second buoy pair is FAR away, should be closer before trying to measure it. - geometry_msgs::msg::Point waypoint_approach_formation; - waypoint_approach_formation.x = - waypoint_first_pair.x + direction_vector_to_end(0) * 15; - waypoint_approach_formation.y = - waypoint_first_pair.y + direction_vector_to_end(1) * 15; - waypoint_approach_formation.z = 0.0; - send_waypoint(waypoint_approach_formation); - set_desired_heading(waypoint_first_pair, waypoint_approach_formation); - reach_waypoint(1.0); - - // Second buoy pair - Eigen::Array predicted_second_buoy_pair = - predict_second_buoy_pair( - measured_first_buoy_pair[0].pose_odom_frame.position, - measured_first_buoy_pair[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_second_buoy_pair(0, 0); - buoy_red_0.y = predicted_second_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_second_buoy_pair(0, 1); - buoy_green_1.y = predicted_second_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_buoy_2_to_3 = - get_buoy_landmarks(predicted_second_buoy_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_buoy_2_to_3[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_buoy_2_to_3[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_buoy_2_to_3[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_buoy_2_to_3[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_second_pair; - waypoint_second_pair.x = - (measured_buoy_2_to_3[0].pose_odom_frame.position.x + - measured_buoy_2_to_3[1].pose_odom_frame.position.x) / - 2; - waypoint_second_pair.y = - (measured_buoy_2_to_3[0].pose_odom_frame.position.y + - measured_buoy_2_to_3[1].pose_odom_frame.position.y) / - 2; - waypoint_second_pair.z = 0.0; - send_waypoint(waypoint_second_pair); - set_desired_heading(waypoint_approach_formation, waypoint_second_pair); - reach_waypoint(0.5); - - // Setup variable to navigate formation - Eigen::Vector2d direction_vector_forwards; - direction_vector_forwards << (waypoint_second_pair.x - waypoint_first_pair.x), - (waypoint_second_pair.y - waypoint_first_pair.y); - direction_vector_forwards.normalize(); - - Eigen::Vector2d direction_vector_downwards; - direction_vector_downwards - << measured_buoy_2_to_3[1].pose_odom_frame.position.x - - measured_buoy_2_to_3[0].pose_odom_frame.position.x, - measured_buoy_2_to_3[1].pose_odom_frame.position.y - - measured_buoy_2_to_3[0].pose_odom_frame.position.y; - direction_vector_downwards.normalize(); - - Eigen::Vector2d vector_to_next_pair; - vector_to_next_pair - << direction_vector_forwards(0) * 5 + - direction_vector_downwards(0) * - this->get_parameter("initial_offset").as_double(), - direction_vector_forwards(1) * 5 + - direction_vector_downwards(1) * - this->get_parameter("initial_offset").as_double(); - - geometry_msgs::msg::Point red_buoy = - measured_buoy_2_to_3[0].pose_odom_frame.position; - geometry_msgs::msg::Point green_buoy = - measured_buoy_2_to_3[1].pose_odom_frame.position; - geometry_msgs::msg::Point waypoint_prev_pair = waypoint_second_pair; - // ASV is at first buoy pair in formation. - // Run loop for the rest of the formation excluding the first pair. - int num_iterations = - this->get_parameter("nr_of_pair_in_formation").as_int() - 1; - - for (int _ = 0; _ < num_iterations; ++_) { - Eigen::Array predicted_next_pair = - predict_next_pair_in_formation(red_buoy, green_buoy, - vector_to_next_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_next_pair(0, 0); - buoy_red_0.y = predicted_next_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_next_pair(0, 1); - buoy_green_1.y = predicted_next_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_next_pair = - get_buoy_landmarks(predicted_next_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_next_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_next_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_next_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_next_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_next_pair; - waypoint_next_pair.x = (measured_next_pair[0].pose_odom_frame.position.x + - measured_next_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_next_pair.y = (measured_next_pair[0].pose_odom_frame.position.y + - measured_next_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_next_pair.z = 0.0; - send_waypoint(waypoint_next_pair); - set_desired_heading(waypoint_prev_pair, waypoint_next_pair); - reach_waypoint(1.0); - red_buoy = measured_next_pair[0].pose_odom_frame.position; - green_buoy = measured_next_pair[1].pose_odom_frame.position; - vector_to_next_pair << waypoint_next_pair.x - waypoint_prev_pair.x, - waypoint_next_pair.y - waypoint_prev_pair.y; - waypoint_prev_pair = waypoint_next_pair; - } - - // ASV is now at the last pair of the buoy formation - // Should move close to end before we try to measure the last pair - auto odom = get_odom(); - double distance_to_end = - std::sqrt(std::pow(odom->pose.pose.position.x - gps_end_x, 2) + - std::pow(odom->pose.pose.position.y - gps_end_y, 2)); - if (distance_to_end > 7.0) { - geometry_msgs::msg::Point waypoint_toward_end; - waypoint_toward_end.x = - odom->pose.pose.position.x + - direction_vector_to_end(0) * (distance_to_end - 4.0); - waypoint_toward_end.y = - odom->pose.pose.position.y + - direction_vector_to_end(1) * (distance_to_end - 4.0); - waypoint_toward_end.z = 0.0; - send_waypoint(waypoint_toward_end); - set_desired_heading(waypoint_prev_pair, waypoint_toward_end); - reach_waypoint(2.0); - } - Eigen::Array predicted_last_buoy_pair; - predicted_last_buoy_pair(0, 0) = - gps_end_x - direction_vector_downwards(0) * 2.5; - predicted_last_buoy_pair(1, 0) = - gps_end_y - direction_vector_downwards(1) * 2.5; - predicted_last_buoy_pair(0, 1) = - gps_end_x + direction_vector_downwards(0) * 2.5; - predicted_last_buoy_pair(1, 1) = - gps_end_y + direction_vector_downwards(1) * 2.5; - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_last_buoy_pair(0, 0); - buoy_red_0.y = predicted_last_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_last_buoy_pair(0, 1); - buoy_green_1.y = predicted_last_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_last_buoy_pair = - get_buoy_landmarks(predicted_last_buoy_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_last_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_last_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_last_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_last_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_last_pair; - waypoint_last_pair.x = - (measured_last_buoy_pair[0].pose_odom_frame.position.x + - measured_last_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_last_pair.y = - (measured_last_buoy_pair[0].pose_odom_frame.position.y + - measured_last_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_last_pair.z = 0.0; - send_waypoint(waypoint_last_pair); - set_desired_heading(waypoint_prev_pair, waypoint_last_pair); - reach_waypoint(1.0); - // Also send waypoint 2m through the last pair - geometry_msgs::msg::Point waypoint_through_last_pair; - waypoint_through_last_pair.x = - waypoint_last_pair.x + direction_vector_to_end(0) * 2; - waypoint_through_last_pair.y = - waypoint_last_pair.y + direction_vector_to_end(1) * 2; - waypoint_through_last_pair.z = 0.0; - send_waypoint(waypoint_through_last_pair); - set_desired_heading(waypoint_last_pair, waypoint_through_last_pair); - // Task is now finished -} - -Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { - // Predict the positions of the first two buoys - geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; - geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; - buoy_0_base_link_frame.header.frame_id = "base_link"; - buoy_1_base_link_frame.header.frame_id = "base_link"; - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - - buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_0_base_link_frame.pose.position.y = -2.5; - buoy_0_base_link_frame.pose.position.z = 0.0; - buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_1_base_link_frame.pose.position.y = 2.5; - buoy_1_base_link_frame.pose.position.z = 0.0; - - geometry_msgs::msg::PoseStamped buoy_0_odom_frame; - geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - - bool transform_success = false; - while (!transform_success) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; - predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; - predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; - predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; - - return predicted_positions; -} - -Eigen::Array ManeuveringTaskNode::predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, - previous_waypoint_odom_frame_.y - odom_start_point_.y; - direction_vector.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 20; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 20; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 20; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 20; - - return predicted_positions; -} - -Eigen::Array ManeuveringTaskNode::predict_next_pair_in_formation( - const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green, - Eigen::Vector2d vector_to_next_pair) { - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_red.x + vector_to_next_pair(0); - predicted_positions(1, 0) = buoy_red.y + vector_to_next_pair(1); - predicted_positions(0, 1) = buoy_green.x + vector_to_next_pair(0); - predicted_positions(1, 1) = buoy_green.y + vector_to_next_pair(1); - - return predicted_positions; -} - -} // namespace maneuvering_task \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/CMakeLists.txt b/mission/njord_tasks/navigation_task/CMakeLists.txt deleted file mode 100644 index 9d462689..00000000 --- a/mission/njord_tasks/navigation_task/CMakeLists.txt +++ /dev/null @@ -1,76 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(navigation_task) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(njord_task_base REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) - - -include_directories(include) - -add_executable(navigation_task_node - src/navigation_task_node.cpp - src/navigation_task_ros.cpp -) - -target_link_libraries(navigation_task_node - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - pcl_common -) - -ament_target_dependencies(navigation_task_node - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - njord_task_base - pcl_conversions -) - -install( - DIRECTORY include/ - DESTINATION include -) - -install(TARGETS - navigation_task_node - DESTINATION lib/${PROJECT_NAME}/ -) - -install(DIRECTORY - launch - params - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package() diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp deleted file mode 100644 index e224c3d4..00000000 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ /dev/null @@ -1,143 +0,0 @@ -#ifndef NAVIGATION_TASK_ROS_HPP -#define NAVIGATION_TASK_ROS_HPP - -#include -#include -#include -#include -#include - -namespace navigation_task { - -class NavigationTaskNode : public NjordTaskBaseNode { -public: - explicit NavigationTaskNode( - const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - ~NavigationTaskNode(){}; - - /** - * @brief Main task for the NavigationTaskNode class. - */ - void main_task(); - - /** - * @brief Predict the positions of the first two buoys - * - * @return An Eigen::Array representing the predicted positions - * of the first two buoys - */ - Eigen::Array predict_first_buoy_pair(); - - /** - * @brief Predict the positions of the second buoy pair - * - * @return An Eigen::Array representing the predicted positions - * of the second buoy pair - */ - Eigen::Array - predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - /** - * @brief Predict the positions of the west buoy by using the two first buoy - * pairs - * - * @return An Eigen::Array representing the predicted position - * of the west buoy - */ - Eigen::Array - predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); - - /** - * @brief Predict the positions of the third buoy pair by using the two first - * buoy pairs - * - * @return An Eigen::Array representing the predicted positions - * of the third buoy pair - */ - Eigen::Array - predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); - - /** - * @brief Predict the positions of the north buoy by using the two first buoy - * pairs - * - * @return An Eigen::Array representing the predicted position - * of the north buoy - */ - Eigen::Array predict_north_buoy( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6, - const Eigen::Vector2d &direction_vector_second_to_third_pair); - - /** - * @brief Predict the positions of the south buoy by using the two first buoy - * pairs - * - * @return An Eigen::Array representing the predicted positions - * of the north and south buoys - */ - Eigen::Array predict_south_buoy( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6, - const Eigen::Vector2d &direction_vector_second_to_third_pair); - - /** - * @brief Predict the positions of the fourth buoy pair by using the third - * buoy pair - * - * @return An Eigen::Array representing the predicted positions - * of the fourth buoy pair - */ - Eigen::Array - predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6); - - /** - * @brief Predict the position of the east buoy by using the fourth buoy pair - * and the direction vector from the second to the third buoy pair - * - * @return An Eigen::Array representing the predicted position - * of the east buoy - */ - Eigen::Array predict_east_buoy( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const Eigen::Vector2d &direction_vector_second_to_third_pair); - - /** - * @brief Predict the position of the fifth buoy pair by using the fourth buoy - * pair and the direction vector from the second to the third buoy pair - * - * @return An Eigen::Array representing the predicted positions - * of the fifth buoy pair - */ - Eigen::Array predict_fifth_buoy_pair( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const Eigen::Vector2d &direction_vector_second_to_third_pair); - - /** - * @brief Predict the position of the sixth buoy pair by using the fifth buoy - * pair and fourth buoy pair - * - * @return An Eigen::Array representing the predicted position - * of the sixth buoy pairs - */ - Eigen::Array - predict_sixth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_12, - const geometry_msgs::msg::Point &buoy_13); -}; - -} // namespace navigation_task - -#endif // NAVIGATION_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/launch/navigation_task_launch.py b/mission/njord_tasks/navigation_task/launch/navigation_task_launch.py deleted file mode 100644 index c2dd17c7..00000000 --- a/mission/njord_tasks/navigation_task/launch/navigation_task_launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - navigation_task_node = Node( - package='navigation_task', - executable='navigation_task_node', - name='navigation_task_node', - parameters=[os.path.join(get_package_share_directory('navigation_task'),'params','navigation_task_params.yaml')], - # arguments=['--ros-args', '--log-level', 'DEBUG'], - output='screen', - ) - return LaunchDescription([ - navigation_task_node - ]) \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/navigation_task.pdf b/mission/njord_tasks/navigation_task/navigation_task.pdf deleted file mode 100644 index 9c0357a1226a91a5892fe7cd459b1474ace73d85..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 99398 zcmd?R^;=b6*glA@prRlkt&}uMH>e;D(hbrL(jB6rfOH8GA|>6ON;lFC(p`tndEoeY z-}%m8FxSlaK`&tMwbx$j$@{+7A%89KitZUb8!GwKbY~AL)1#-4EVPYLxw%mpG8}mVq8BgQAhHjp3tbER5`^48nRw28K3|o-wj8qcXfO zvax=xXZ6y;?5%~lp1I8JYs$c1=lcaiYPJVo~9$z}0f(M^*Y3 zmdGN(${M}ggL_?c_Gcr@Wm#o*#amUFEqS3gm)`w7Qj0TTu%By-oZz@^pl=?#Uww8| z{dGd)y1>LWl=BaNmEB~yL6rN4SZBPT4bG+CXO}L1|7gNn*n#k#E8D{M6%3+_UwItU zKRTp;jE?*P`zFtx+y>so4=91oHgm}PkU#%_>t912K^ZIE-^)MInmrXSBVT{?CcC4_ zq8FN$W#_f*tH}4|z>eH6a?G}5y*oQrr0yf`Lh8VS{7cijoRWE-C#8!^xIXp4*p-b( zM3s*nM38^nSNioVz)8}ohmEmMnoZrc)-nq}j15)eA@a|83^OYsw`hv-BC)z1C&Xzp z4GszK`npm$nEe|&`7#>F)rD>nn&OWBuP zlAO%yFcyyj8{gK@5?5mu-*QBbvE|$JVL*(HOx~c4yr`1{_MW-cig1wanb%ysc4+c%mSOd`Oc}1^UUDNjIvjn2AIaX-{8vUtS6# z+yYCa*>m7Javr8zoHm6MHd6SFTU6;k2aJcTE#)dEKmIehqhtRGJVu7DB7tiXO$i2u zYUzRjf+ga?usmIH_XMgq2zQiz_ln*b`!+rIXZ) zhGoc9%TI{BoWH@c%+Wtsk+Ve~q-46Rq?H&#!akQf*@$$9*MXUX2eM&agmoC+9q!+R z$%310??}+nX0D3Kn{_#F`H-7*-iRqBCffUPW`mCDYhPO;jy7~VY8vOBtK!BLOfpZ zz5f{b7W~bKnz%yMop512nMm^>?Ie6#JXRO!pUb<%-KNRHPUXgXo%N!MVZw<^rA}mY zN8&aUh;NfSS1#^;RR8<4^~<^IZD<_0PP@4zo0%k397`{(mhObIm93w&H{`ms+{|uJ z_sSmL@tq9&cT*vF4eUkenFQu~;!S@?xisS|xn)lR@>mubwfnL(A9!2zSd`8U@?nk@ zv#s)$Nq(!U63Rw%Gepi<_`c2I^TxjjGMPwWS+68> zC}f!F8dE7sY|55MoEdrIAfqpepT~kT&yzCTsIMk3Vo7$BBC@LQS)PW?jpq!eAYMw@ z;+Fn&$;1w4t-td{Tz2YFkW$8|!`4!aCoyurP>!X>aoJlNd%HRHuy&MBg?>-Peu{O@ zd&-@>{F5AOxgf8ILB~`@jIZ~ZA{HrpS8P{8{|F!vRjoeW)Rh5Afmh$riL4z4@ z|J;9#$lRE1x6E7LD$iC}Ah}qHu&5vnQyG4cJ(i}}d5b>(ihtJNX2Rr+|I7w_ z=PN#e`VT902`ADZ;no3T=>+BCygq!@p|HSZ+|L3(dN=(AD2nN%b0va*lSe)U1;s@aEKqqJnW7(Q`ii?uThAPh1}=n^yZMX1 zxs1#b9xhbdqp|~;6Sla8O<>H8T8N6f8dw!%$YER_2u_WFN%gqwMr01jK?+DnkZtO> zoSFK(v5)g#_};>StGcYEu1JmT@j0?>Y`gB{tw2hY=NE`}pLLwHO1Y2t(vG&AXuLCW z=k3sO=H%>!XM4B^1nu`PPimedJIWb#jaQlOKuo@?dXK_qn_vu}| zG?B!--p2vLWX5cU$?Li99~dw$W;jDExg1Q;()YkyfRpow{r&gCy;}PfH9hE8vn70~ zg2xZzN`~wJ5zvPVGqpJi zXj?9y%2uGC(K*DZkZx?BKuQnKP@AY3WZp?FVo*1mTw(d>8{Tzn(cH45e}e$0Lgiu_ zfdOC|gzH7*wShuCU)pFJ%gk?cB3^IEAj;44zCbs8m zj$ie?-73~P#m^+Gge-1qFkOt*q&asrHiue;(f4PHT-bGA5*<=Z4Kxko(^>18;mOid za+-l2w!>ol009zPT1}JRKAA*E^gDgi4> zpeJMSVK*?-eC1faHGa5yz09HkS;{9QIEoYM3tT=`7+ z(jR*v+PgH0`Kj-6m0OxamvKW=`;{Dd6<1Hm^;&n|>*M|`KYaKw(oY2sivYoXGX6p* zE^~ATvE`?E%S{bAdMB#CBe!MW?u-8V{LfA;1Ln^dW*#II^NLQUIum~C=wo8;kA9ge zplu{up%>?E{r<?wLiRD&&TP+!DA>`8sJY5)@tuYi%%28t)F6&L zMqE)`JEm&aPlymakLn%P^Fe5p=qCp6^1;gW4++i3tuy>|9D{__z%TcIzy9Cn1l`J3 zFVaX;Dz?Q|cgpYy!>ghU{i8}u{^)SLM!5>hxY%&qdX)Ao;jsbJ!$A2=OM_H3MMtl8 z)FH}ZZ}zjL6ZF}>mKeS}(Ur+AnR8`_UQuzLauf`T2~;%Sk?u?TA=;I(+4Yn1;}O~) zv7$R89k)1tEo25vRm~T3ylYfkJLEV~hjP<=#62gcXrQe|tz0iVjBCOS+hJ*3ytC$cVp4@$JAf~(3W<{j#n$Ukjkv)D?KAB|v35>h! zixQ=*{oJ2EHiFs2o17>7eSQW5QZMJOg{BY5<#(4QFXBfkqz&kAjG|1C4qVG7KrXyx zkhjI?-tpY)_#$Dl7TTp;J4!Aax7S{1Fa9P_1_z%=jZ>-o2v!=4Oi@~-HXk{|aacNq z-~$ywK|zre$Gl8rmVB-ZB#l^NCzXx7i#6bPyHB+Df`=|g5L;Rbs_40o0zwp3_bY~7 z=(ta3nd45V=1Xm>o6$%+?r0Jyq=~vlu9e@)71Yknb^PmNt9mDPdEI&2Ym()naPT-L zpE^!IT)iQj|FBZ;P4ABZ_U!lWw=|5aupJKFc9PX?dN~4=NEX@J6O01>K&?nMubiB2 zF#Wo!-NOjH``&+{ftV^d7*>C%%mFu7DY$+i^B3Z{b}AN4!;BXTY9**Z#3wqP=myxB z3k#quq8f9!EkR4&Z^Y8CM=EB5+*~!hqNjCG2l)O`oi%1reTbVb>_p>`dLTWyc%zb{ zMBhO>iP7<~Y9nl0x%r941}E}1yPrK}0a;aA$qB{Vcku0ru-|SKP1?FS`XtW4&i$U& zoUsZ1q1$;yk)U-z(pprIBuK6EyLe%PEVtdKXuV{Ui>=yd&VXvb?`0`%+4dl3IPq*E zElUR8-P<3aQoyt!F>4~l#*f4@=5q*dtm3=MCw?)| zch~Ekcb*pKfu~6qb@$_|=%RkoT%zB^cMXqn37Y2@|8yD$ev{@udx>GS%^L7 z?HE!FUorVKKE4(4$<(83#{2je!`%~58byqUj6#vkg)%Mk6m4xYyzWyAY?DGQ|C54xzwJUnZlgtk@L zgJ`82Znc0P$-2%WxxvX-N-n+GGEHr&_=38pePxg(({zWz#DDDgXADhn@D-^Eoa2W` zFL{RnW|utiH#lrrKiukb-PDVsyVGMuH#L@0Fy1bW!$W6HSlF0zUlXbKKMu(2GyXH3 zaEQ2g9h95o>~qrV%`~V(oVwUH!g^)1CdC_EZeklU))rcIL)fkqN;nKOA@1Mtr9{8Y zRFTr|mi||W>o!6FV_9Km@A;!j5>yqWJRmK$wd3*|a>b`Mv<$iI!g!;ivQ5eWmCmjb z<-I>{jTThFUv-c@d<(*FETJ)}%VSrj+62QN=26KgJfLW;0!PT`=Zd>Prw7 z-Q{yM81xK;=5z@^(nsJ2`rg!iU%z&=0TZ@2TTAN1h0&ZBE5JYgw`lbU@@13l4pS?C zqE+R1hywI!*cH9!{d?{s?&R98QUyUVfDl$}rlpC>Umiq36F@(*ANJo&W|Zm<6lE)S z!?T8;`jRE)WKOSmOlw&>I%!Nb)vQ^xR#SZquo#mvg|$NL6c0cuJmhTbb=+$|867L?-c zigU#xdi;>Z)Hs;CEq7Hv)UP-5TcWpGZ}q$H0A(% z-D|tVmFJN>|Tjxa|c^G4gN?KM?W*#1KR+xEYsjkbUR=^DHe)pTS^gr?^4## z+{}>+QI!L4k)~Fi=`S2=LW|+$jcz!Gb>8%dg!4#$ZtB?%ojzTPh5L2*EV-=e{?s8A0;F z3c9Z|{r&;6m$~UG+Q{l57uGWNOW>r^y!nXB~}MyyOkk!r<& zD`?y}(`7}rE+~TCssXMMtpnykg3;PYLI(XX^A4J zWkpmma;fmiLfcD0BBf)(iv@ecaMRcM8O`L+EUjcFB42?Y&~J$&#`l`zRD!nq&t@`X zL)6f4Y(raY^S@G2+A^^+jM`1w6q`jrd3eih7rAR&cLC_*-yVZp=gSzNtXp;n-Js7H~@Io?-OYHEKIbu!s=zuif>m!qLEN4?b}=M1K;wDLwd1C1qI_~=27f~Pedj} z0>)OQwoIShn4-gwFXI3SPM4lpb$ZhaiYbIP@8pGSP<~62*xY@o&ygts00P-QE)rU3 z%7=Xt-x<|0)nL!hVj;;bu_^Q_C9cG>nfq%qaRsW)97^qo_76f?21>c+%gsTVh-p<-fg zljnnx-TG}3AoJFTzVOe5#8ucQ;6U0h(>rOMSpM9*E_Q_@y`uEvdhvwVFWSBj}KGcd>%T zjHcg+JWP+%srAR_F=$wuJ}6S5Z*srY``u!OwN%ieZMoYkao$ipNFC2up6TPNo#u7A z!+_j6-JU?lcK|{Q{P-edA(F`WX{&IwNL|Jp7kh8i5m5{s4l9l6w;{vqvsc_Cky%yh z&7y=}Z96)Bj|o$Hsd@2&y#ac7i!^-uYGyfS4yS z(rx+QG1_RnzJJ6xSnqYBF*p z?!JiqT||B;Zn?){#dDs*|3OhG5p)xM7JQS_Wt$6&=fJdNS94&#)B_QVYkbJKhC=&= zec*e~Jsq_4l~3FNRoscinS>4~rvw0!f3O_1Ax9E>l+wBWRq@`!WF3;fNe7q=`z*G8 zG!53s?vY|HHvF^C3XbPbmJ9LUnCm?WYRQ`jNqLoTF4j*01=)iNH{?<}5TBN>%k4pw zS;URf<@@`GSB7HfdVNL)5o=GeX6l_f3=um<0w|53BuMk)Trl8pmKtL{ z3AZ>@*|>sSx-KW^4xt!>D$8V4uO!~vk(FzSvN)qV=j1zw8{)-}<-+B-`fy07hvFKA z7v@2A@hlH)G+*bxWvfS3vSVWMnQ;*Cs6$k77)Ht_W)9+0YNz99_MS3qhi2*=&=Se~ zkQt*QDp%G)n%1QCYqF{xF3WQ$k7Dc$+BS^63&&rIuhFq=u#t|J+Ap1Q;2ho5C{NZgg|saR2lTw zoD>2P0hWbyog^8CUQr0t%o0`;@uM;uA;>WTCqCBmXcivLLo)S=$PR*+Ckazl!ZG<{ z65Uhw;WI*6A}67uc)eHN$*X}gQh-M$vf_E;FBjwXL~h(C%?!^F%PF9u$M9wv$=CTf9HO``M^=S(T!4Iq znI|I6z>pc-AYxF>PbEW`vl>OoC9D3>j)8@F(aF>PD`Yo%L0i{2(#Vr6WWao^ak)$W zuhc(ywkn9gWzMbq9TR|x226jYS%ZW}jRq?qOIaUR{4yX4P45#wZ%C-bX?<+)iXKr@ zDK8+9{uRjAc8l}x>-caWGkjTcWY}^A(nj5on5s0673Oa;hR^J(byYTV(!ott;(g#& z|C9L~BT1-+mb?0}D=EHW6=8`b4v9L{VIWBCAW1;x{bK{bOci=D68<)YTV{=1JXeVrFO>+iuQH zF{j|ic_(Jgx&~?`hSyrar9uI*+N#i+K?E?HY{klohmL0)vX@7B{-J?a^`2Xie4Gd* z48~=;kRtp5aSiZp-Vnu7(*t_$HH6)?Nc(!-YSEQs{3zWh0g>4> zY9j7Ctt}pRF`3;E&$p$GgI56G(oHJkc~Sx><#wsTbaTPVpaIZgL+qig8N$GcO)I$_ z%vq3lJ6(*xe<=rQA<}RGXtw@58p+@Ms-nWS@YEsr=q~j1R~f1#&QhKhO*L7-}=+H-AggQ zo;gxpJfUuC11LTNY3JN-`3Q@{sDE?!2*1UNcq^@|=F5xO?)v~!j0}tjYGFW!mpOis zPtL?}v!k@Qc3PO-kQ`uYqRv_3)_&I+wCtGkuZJTL*&TFu?3MWl{i#eOH&-RNPb&11 zP_og>)XmKX>ONNzuP@82pAy%h0pkf7vw!M@be$P{$Q18V*10SWvY=>URIh~cj3h{D zqN^_6r9Bo?ZOe@t1NP?O{0?^it}=o2?DbV3Pn(93nEj&((Ypcei@9f=T4?Xh2wJ=a zeyn_ey&LULc6jgq9!Dq!r|fh^rE)I-mbPU^IY~*qk#>{UhJ_m4UL0w?O@SnfW*d>8 zd>`c6j_kBbRwOJoKoX}^E?uKcY3j0eti+eXlQh9>l{1MJ4A;}vV$rG(2RQp01J=dj zKRSYt>l!frkb7VvBhZZXB~h-DwzhPl(cT|mpQJVptsgP(4kh++-I1WnXKw(28)&n( zR_i;+h@1G=aZ>c?aejW}#!u(1&!Tc^Oyg#6m#OHhU41DSZB_5=eVqROZ`R~4BvE?#R}p3 zfDR(fjJV)jQgCR-_jTi8B-RqUX)I#S0-}0f6qQNnjVUPBzg?2eXMbB7`@B)hy7HX* z_(zT@TdPDU6-s}~cnNT_ij-Pu-+WWyId#IY-{T|hFkN)wUf(L$Ikk}qeTU|Qh6SWw z1~xhpm2_(@M7NRL)v->ePr#;Ak9$CX5H=~>OuUc1=aSc$MQcUmEufMqGJox_!T;lwZg@JLIlLEM3!J%=X^&Dn>$ytYvbqozW~hvjX7hz50N}`@}uiBzy%*8F7GgEI*U<< zC>r%@Xr=7&gwD+gDja&?snegu7bGY84% zkf@UW-`?=69>kUK^jX`Fyz5D4T~aD~6|z+r#kRzqab}a9w9H^QfvOzWMx`$5Y=o+*XO zcP6u8jPOm3E%yz}q7K(B-0dgZBlH|Vwv7eN(KVQ}#;iRb&_O-`P-bL>(?Fgd&L{et zS${IVi7QH3E0#ZJ;@LY(6{hB2Hkf^-w9*5O-!~N%gK)$}>tI^#v`+lrjJ6V>+B;{B zK?!B6#$?_8S4MuxElKg;`k%n2Tel9RS;2NLLv36gq zrU~nM?Ch}WN3Odu_*T2#lS$_>rl}I@=azLf(V$xvR~fE*^aE#`|8z?2yoct@M&~?v zG`kCwyr`ndPZ1(;$>dSmzJy?d1HxQN8F4afnsu{Y$H6r9>YHwl4w@j45VQUYd!lpX$udTM3W^W{}k;yWxYRafJJonVuQ4OOxY>@crJMEyGy|MP4OMuSCgoi z6X&_sdL^#U*HG@=3Xt7K1qls=^>Dt zrmy|PfQnhcngnH^;_9JM+Cc0aPa`&+G66D2fZHH1Zwyep#P@*oImI!ceQ922 zT|r#j9SJ;pJbF_tV4gzxe$nehi=&MgH}L9?iPQNO;4@p-`xDnrZP}yKpg4Yl^irzQ zp0u=+AByt>(B2ZWv**@|4#&Rni1#*kzAz8OL0l6eCr@!RP5F!1_7j(ncoJmjwf^{5 z0Qp4ZOjUwF{csc+ohzny-JGCL>FR#mRCUUE%cJro`U%&Teg|gc#<1&)vXf`l~G>lk8*dTDHTc@gVx z6`>k|YCbCUtL>dH2G4V9zkh<^KbIz1YNvL$W7ipj=c;&XLTrQMahk{PkYyh}_KzqK!F z71XO%u`5j`98&uS5xxl>ICRQX`BjI+rX&)*O!~Q2fTn7o97N6kqMH%vRX;(j zov&Xz(Rc`b-L*< zf}K#_3?;q`_=U3dihjRZkp{JLsYVpKU)l9nbWT$HlR9BrM>qDl!w}#Tnn`75-R@rC z+`Re+oN=}8DT{Gn*$rniG0@}#JQj8;-I8!l33A8I7Ng8WAH`}C()9PY-8#p@0TCmW zu_aUnT^_LPQ6~Oj>LQ@VljUQD*K*pLiQQu4t+$rKPAFC@c58J6c_KgRM8be9Z&Hf? z&0+LdItMdN{_$qV(72hDBcd)gW(nCZK;;5Uew>UAVKvvwzlBQ6^K&nSV$8l{wMiP6U12~)ru;n6^bR1|QeD%F}h6t?L@KgvN* zQ?>C9rHc_TOpS6q3!W=Fw!I0e88%`1V~s#p=0JN?L{4p(#R@$wKsmQG(u!oNu4RlK z$d>=gml5K@NG*Wq5Af|`8#sbTFRxHX!g1T8?PN=$JXcwO`gq3t;BcmsqQ!6_WCYO` zcbaNE%IZg7)Y9In#65^MX8g2*7T{c^yLtezZWukXCucg4Gt+s%FnQP=Xz!(>#gT%M zpXNFd{?=m!F4Ky{qw@aNv3mH`OMJEEwSgQ)+p!f^2gw#zZ(oBjrFe~x+*P4R=Xk3I zeWw{J4BhBfuk8Rj=~Vx{`08u!ewUio^$&>YR1;F=veHY700)o!s=By@pR8tvI>ITr zGfD$e99qz6?(tMpP3+^@Jd{gy`vzT#9Z~69XuqV6QZQjJ4Btt8LqMLCz4oYw2au|D zrTB1f!Ta7$@!`zvP-z=B`^e+lF0eKj4SD%$Sq{a#q@Z0Js_5#0-a?wUf4J&Po@9H* z{nT#))GVyUc?`0duX?gvlQU%2_9ExSp{p{V!U2co0Uhm$PS}$n&{KIAE6;_JxP~`1NOD*Y} zYLtMC>@i2n>ZT>%=X@z(j>;d=HW;Y~gCfmy;C$z}7cl3zfx#u1FH4nN=EWBDNjf75 z)M8>SP8+H?`b>U5_C}U&LNES_frEm1S4UUx3o>l?xAWS$b#*_4R~0Jfy`o!I7-Bh^ z97B_e*7*AB7+0gtr6YZKwpJ<;U-OASK$yT(Fg0g0)W-u^NK?nw0u~gnw z$2w-Y&X4vX+L>`|oLg_V(c#W$#+DP;GBYBkNsqGnFv`o=u{vTg=wj5 z<#I>?CB-YNkfiCY!NX(pJ~h_a>i9jtgJ3jdlf9M94@8L9LWqgZw1JOZ45)Mpp+8qm zo#I{j&ccm;%rS0j9T9}4x)U>4HW5lR9h2g)vdQNtwDB9P1I5rKIQj|g?$Cyy7CWtz)61d6F@xQlaL@T%Z0jHmVle>uQ`MWs}rXlD|7pgoD&p#QFg! zqM7;$bR%`nR|*Bye{`Pvxd9RDBCF`>grOiA27Z)^T*gS+dT(DVtHw5$CC&EAG)tpJ znfaO9;O{`FwOeg=y17%mOXcI5X`m@h9CCl#me4_po!n_g>odr*#B7r`paHf)(vg0e zOc1ouvRo4fXo$Rdm&QO1W&18`eJh8>0joqrtmzy#ASV(UmFY^0X-2P-fc-9)2?Kq= z1L_&+EvR!Nf_%H<;xNLNoQMx;xN55|rtD$YR9}iJ#-HvcK6c}lwF1pgbBB%%Uqxk!@FrWIO}?HFsQ z9iet>HJv~2ey_$*>dIC?^#xp*T@p18gK8xJTHxTKpR_}{o zHs1B%@F`Afe&WJhmyxKkVX;&g!X=|Q-5#K`Z<)~mEZoNFh6K_I8w8!cc}o+?yfj%Q z&|L}gXH<)dRuRj%%wR_c$=M$dQw1ril5QnA1yuQ|H`L*j5aY_oI6 zgn6ApP7B4Of|l`b;g*E;hT7S=w28w`ed8Qt+~9C2ihiOTw5<@d&kFFv)_c7V-3N-N zPt42K4{S3KhafP+d-yFWfQqrrv3J>wPPjTZS$QPF2v`iIh&wd)Cy6eov({8`(mMt% zD?5nhr}Ln+qg2c{ijc;o-AW?G04Fksl=kAJ;(1v;SJ6~i7`RFmuz90M%ps@m?TJ;Hi&XBbu=HaLt_cf zy|0tp^*VcLW7WDTy|=B22zO}NaPMN%$Yk_-ZalnHUJj7HB9P(RFPlT>i?qjB(@G<( z*#hsNe0SwWS6gJX3W`ttxHTq$$a*}`6NhgV{1doZ{v(&)(N92c4-B1T?Oa&ucwNQk zn5v-Etg3i%MNR4uU-ItlKLV`Qv6MT}wNQm8#@*&npCeT;Pe?cTo-qNSGN{^Ogy^?B z^4!H>Z}lZQn{_Dp>oliAhGJTuIz1emLfS3}iflk_-Ybe5W>~nwWKy8G-YW4qnc1XP#X5_Bzn##P#l0gEAl zFcllQ9=rtrF0G;2tDXrka#Ioq^8rNJ$OkMgqc3k@OG2|}j|e4@1iJS>VhI3x=)W zHa<6xB?@5oT+o*QzemF-e>Zm8LQj#gJzRO7`&=#0y{itkn6vz|9>lTCbh;sK_hh{! zs0T9Ts#ljPtCJS{LTo!oF}!VdSqgsH|fZwNNA?)h@1oQ zr!Ov}%-n~PICQ-T>{-VCl6+M@Buu^VGiiWp+i!8kZmlIB-_FrKLJ#uZz6pp&7e*;G z%2TnnM5SFBff9#L>jpDk$Mm~i@7Ri*$~AFQYRmJ}EH$m1$z9Kbz6~av?6{t9OBpx! zn7GYxoNo~=u66+EReiUG#oP{jr!T7GC9N9}8t}g1v~Vv<@6;}G_3?mMc}ZO_2qBzw zP8}RcTH}Z@E+hg9-2b3+3})`OM%;D$w(o7`?U%9VDjj8KUhdQU0f47)E=!H!S5aoj zjP<|L|0t8JhAoz~JDY|Maja}7c!Gro3~AEq z$TzM12#TFV6mu3b)_KTxor4g;s1{nw^)kZ5LB6H2;r3F1{IHH&5FbO^> zBMW6CC!)r7d9N1xL+DSrCkflHSV5b)BrSH<9vgvF$+@?_qgr+@;m^yJ_(JtVK1j|DI@{tJqw$48y!u z;f#SSfZGKxiu_$uvVh!(J$wu(Q$v|4Gb2NWGF!aW``uPnwKwNzwFfpVeviSHs4q!< zH+KaT_Rv0t>|7C|_oh@U5V&gRV8f*5Z5=?V*!MGMye{1*kAOPtS`Z zz}RXUo9Df(87tJD%333#skE{x_m8??&fV~ec0M$6r#v@DhgCZ1Qu9N{Omap?5W>uV ziluTO3nrbYhGNa$-Mf?;g4zk*L z5#GX9_1__`zu?c0BCH}2HiVUACo|jrg^7&Jns$jHbL%{wo>Mr|vU06AjzFWVY3_s%$A_@=`; zU$%CcRei|2Ar^LQ@g3fBCuw;ZJ5^XL*XOn98__faJ$NM6Tjp)qCt@fb$AUqj87^q} zrDS<3(~^g2f`tN6_NX9IKi6|d;yi^qyEbs>RdRR>ewalG$i=rftmxV$fsF!1WRbpU z9&i9~Tu8|on5{?Et*|rJp1zRDn9;LGisH_iN0hmZgGl=0U(k6ouL}R%<_LhE$?Uph zzHP=Eg~3+`&sdU#Bmf^6AgGQUI)x&=6P+s|?$6e;_2qz+*dB^+909>Shl#gSTK+IS zHSTr%IkJ1^8cZzIG}I_tu^XQHhpaX*#dum0#Etji2q&CH1C&Q4!=|s-_nH!v0plj4 zD>c(lcv2TDXME1?`;nr5Z1Gt2({t;7fy8ArphR;%7FNc_i-6o6+Gz55sCk;!VTW$3 zK9osRf}Mm?(RwP)43~Zvgkjpv!-zYacRQYb5Q#%vjV>F2zIqK{F%!FM5mrHh7qf^Z{ zI&6{elqoTU17tvY(kXunbcH}5q|ecC2PN=a0}o52oFv^CYBbu0IkwmCTJ@d6zYUWZnPW1DV5`{p47&3J^rJ(7QGH8uF0=2OYw||vP|B{ z6-}c6Ne!HyK8+L9o128 zXyUBuxKoyLj_=^ zQO%=3HpR@kGq-bofo|Ofx3sB7cqjI!ivcKC6Ty^>cF&O`aQA1om>@RTL2Q`LJ+p$& zPS5$=zwbuhf=g8yS#Hs&S=@Uq&cKQmuffG0Sy(6 zlWhh)7Vw5HJ&GJ>&k0J$T}Si88Dpc1t7H|f>Gm$)LitWu=C1-8kkrVyl+~I=2+Ju$ zHAd>%fC#a~NQ1}%sHiBdPv;D;;U~2@Zc&K4&;0qzk;+92ET`|${PY??r_fP5KfjN{WHQ5z z@_h`>AA=_8y_%w?5Q|N#esA|r<}vnK?KsfoXeANp()IME%VkS!L;_(2hkQIb$@Vy; zK>1Dhsk8I#(B|!LAZ^J~KtSuk87jp!Nt18e2O1x|^`fCHIXxiiT7pT2Ok2`I3t85m z(bxVJJBLpWCH81d6&X)9)AUQDD=8%64opeF(`MRi;!cYEv6wL#6@QLto4>V*BH{wI z>=LJwH(*bVK84OwD)(Esxz9^%nmq^wK)@4vwHJaGM}p0RMj`~xMP>Cj8My|}zvB~= z&x)HHheYZJqAA!6Q)8pdC9x2{0wFStWn8=w_Rv+R7)U}&KU;?*D2c=VKH%5(E)Ym6 zgY=cJf#7)wtK&hi%xqc(&u;&wGjgeidskC3|CbLTpB;X=P4ni zQ>|J(YCd!H`xtFCsWP`OycbUcS47%y*J#?$ce#;p4vi!%X**r|^yNV{V`*$`d~T5P zz4r0^Vo7gx4YX07q=|X}v40GchB;-fb`v6grgXtnE7Os3ZNxf0!1wPXbP! z?-P{vTUUrhRnCr24!OZBZ=Et;L2*%Nu}1(c-#qR&o=2F=PD@L_im?RiOnLJ~CvPmK zgG7#Z9srH9{eit-rLbIpOG#-aPnjvu*^ zl_1tMB$R&=knA`P+9x;%7!&gZUN#5WXIJV4y)Do=u)a8D@+5K z5#8%_5y0tl9J@$11YJVeiU7PE4z2bd2MHb&ae*$9gi3kiyncpH;1y7VGUeuUOJ`pm zGOD|LAx=S3sVE$i(6+W?^N=x?`uhWbo*Lv!dX?e7KYkZ>;sRhEAe7q@ZK^xpDy#;i zJR^{LKac2Jphh_Mp_ApYhi^KF9a8xxX=}pq)FFaXvMpq7$mjq6;9q_v09K&2N51N{ zcjv3mJcqCxaxwx#de+(xc@;i}#g_E=7b1Tb8iz*+qjMml@F-%*ul!+@(!vrx;MV6D zA6&=-VOTpUozvo!mE>>XZXhZjkT*b$yw?#)|!M_#~CY+Ib37$}Q?L2>m}V z%TdU)sswcUK6g?x4^a9b$bG!MnE2l_FCLAVp*+kEgrCMnD8(L}5=~bsF6>GP`F{+g zZ>~2F(k-i4;8Q?Nb<360=>M4hRTGsE1RQ`>2FZ?-*-`8EZ5tbb7jcUJ_g6G5U>GZ) znD>@p$gFai9sns?>Ue>Pw~s9l>| zp|TM5ze5HoqFrbAv9%Gn&eIUrVcdGNeuZRd)wqlYyG$RY3vZqNH?e7Ouy4eZn;LW| zE*$5ZKGT$bZF)E~bvVZq-+h2~UP)m^LtQl4MYb=qVjo*}8>QySmbR#K$ZZ@>viVD| zYl6TkHk1WFNao7z%sK-6!*8``F8%75e9?V~~comW~~x+O=ykzb$Ik1^3VT5fnq(UREYIZ?gL_tIbh-44pu1F-bc;C<(_w54}c z`jw5uoh6MglxAp6ytjNv3W=g>ZaTqN2idqxc-bvI_9b!j4#M24#DMQy=;DoEbCtI& zE0HVz%tdojTei2P9{)laO;#sTfHoWtPeq<`%+!UU7|re0Z;(c5KIB067=696NB2ar z-P3SrUQS8>8@by=LbUO$tVrZ6eFodrOzbkAwD^R0&gr0s14@kR>^(AtW>vFxXxe_<(8z$zYl7=0Q>D`WJ zyH(!6Er+rZ$@6Vcg-2MyRNl3jI8wt_rAgIt49O*X4S zLD*#9NuOiWf6Bu`x$id5)|U0zoGf-htEy(Uw4iQUppY@&+wBh8>rXGE@P-v%;_~;4 zWKCJjS=CT>zs&JKxlgH(K0*WGcWJxe>g%WASWTsMaZZYI^=x?*CRg%DW$tJGOhf&k zOHOh2fXZR0CIz#Ut-vR83I9W#U5!J()C~{r`M-&*b7{2W##3ru=9efRf|t*z{VpDB z`H5W~SU89gwG;jf-{hw?(UE0DljC|!ag{5Ui;xJ2*Gb6kfHh}{c!71aQ}9z z(*g+!|09da`J%34Y2%Ifi}0g)8Ei6I?6_2lT)u{t8x+%D0yXdw;bVT-oT zD`uNkzdSIO`ZNT#Fk)1rwP6!Ak|o6$vF@2eu3bzH}=PR2m%|k zAOA0^&O5BhrQ6%M-HM99RumN^*bwPTl`bN^N$(&cy$MK3fPe)M5D;n7rFW1T2vwBc zd+#McfB+$c07<^de$RQ&_2nNg@iIKm%&b{!)~t2^s5Ktz`;|N{uGqI2Gvv} z&9ok{cNlY!rwSMY-u)|$OLDtT>?3K34NnL|%4i(06*PzR8>KE!lkN{MK=MHU8vFS8 zpDg#?#Gu08~(R2(_lGRng>sgGALR)~1Wk^qt!96n8}5 zP{2FbW?X^-zvkgno!ME>ks$IfGhWu0i3b9JH}dxw|chH6bJ9Grlx+ zeO@`7J^nQN+6Y2*j#iZZ7@>>z-YseTOaS`spoS)34e+$&nWPz zx)ICpHRj^MhV^7SIb742MV5VEN zNQJ($*B&LQ&q_D04P@V~C>^yUbyQCX2w+DbQ=%qL6Nf`zZ?QMvokd;6r|bM0++dcH zxC`FIeeH)BqSpoAmA&u0x@x-k&)Z4I<$dQqA@+KH|F*DcgrE=fGd<};V-<=A`aUWu6|&Or50?tzMiN{=N9bJly@b(Dj7+kS)!Kk%a%|;Ji11T*oDb z^U)?kwi`{MR;0h}=&2SY-5@^}CfrWfCappXX{V$(bRMX+L?`ZNj*p%%US>S+B7WfI z=>;MG4lr87$akpFOpa|0{%rrW#v(Rl+%d8jS>iOmd`77y?aZ~e({!eib=;e7g{k!0 zw=0gWzdWu}$I?X4dTuq<3onK*Se<>=Sh3ghQM<4@zXTx{W&>@gs?p5U92$x3LSv_1 zlpv^f?Z8q#_h*d(0kT-Ld2@Hrr3emEzLp3)!u&83(pqM@&Gp^-@iVhGCPm}w8jJ#g z1Hx?*p~n9lXjC1O2}y2ikg4lUPb~#%Xivb9CnJBEL5739 zQIU1)VFfky=@rg{#k1(;)U@46yw@xvD~{{!d|A(=)uqvcE6+jmAfcly;J!X{Wv6uq zmUok5?QNSCu}y&fZ$1cBB^6lR|J?o(S~r74Dfs;!J<9apTeUn_%G!}FIo(JumL<3A zjj^6^y{(c#pLJlk32M$KD~Zi%O;s3t)?2;%e*`EifI4GI+MewK)w3YrK8g{GGtox` z(uR#`pPv8u;2|Y#Jx=_cBTcYRA?~bu66{66RESBpdtnt+Rp_YBO%W!gyt;I{CUL@c zn_b22q${9T{!Y_<)S1e;)Z%#f<`Yhvw3-;W%eWV2t@NDqr917y$$tCph(*!$o=n$l zyIdkO?*;fZs;_vbrzcNsQlNq)$3c(&G0fl6m;Q6MD)ySeH{*=d+>^00d4uaC1;rm& z&waI|9!nulbmnXMytZ;5a==Wzu%zgT;c56Lt-EVOl_b;KFK8aBm8+ukKX84# zx-(wfDa-Vxh;H2w(|;a-R}y8ckl54qHJ9VM+f()iF+)U9TaBelR+2ry=OH z0+0Thq8pTV5a02CppX5)$N_6XJ<|c*MxgT*I+MODK-MWtOG-xBHfsWF`l_-b5ilnH zQ)&Z2cb4)Ec;N=TQ~e{#Kvr*zG<5gYtsUtvV30g?FIs8-ol%ZbgugK6aHRY#9l27c z;TH)P0v;L^rcu6W`X=_VMbLP3%b4iV9~{`xS}%5wQCJJ$iHwsD4yUhRA=kK~DljbO zk%sl#pr(!TU;Vn!+kr|Yc;%SxN!(%G6?X({{XOsq&)^DuA0@WJ>*9StmF29>?R{15 z@8^8)DRl<7^qo!?xzkCxn%a#K6csCXgQ-^fQk|qt>=L8R-`81jvRo8v+&SfDXdfMS)>I+lj*7q4yE3Ft&(_h?s!w*JICB1ZY@W_GDJ5+{cK<5%M%l zz;K5rvF7;}0oq1w*6Gq)Pr_7^F4}LTw3&fx`UpxYI{k}k60vjI?XDh$;2b68p|>r$ zrfbg0BxPVOnMqUGo^gY$SbS*b{%~WL!%6z>>e8oG`t48~e82r68`!ETX8S>(&iAUZ z`b=mV+P}ihvk4|qR^IS<(BtG+)7Znt>-FAL9SYUpO6c)08Z7Ebdg#^MS{F&Ta9{(z zS63uDe*Yb5_&48zwh{2E<2&Zxa%#;>Cyzb?>zohdQ}vv#Lgxaf{CDZ?Bb^xYdrPPy zLauVk)=Kh?hsYg((IAA~2f(|p7)SAULKZ=&;!q)J-v%DDdv=?gtB1ZTQ~?@+$<)yA z3s1rp&iTIXaw#&7G?CsYLyz~#?!ZDsucp7KzRFJ1kvzKmPRPe!nzXBx&yO3shbaiR z!X4%@Tl}qKnd2LsXQ|Tu5siHI?IQxSF`Zjs`NYAmwj?z_#VNK#O+le&8n5HJ*g8V@ zu)y&b{BVhEO%rNq5xd&O8dg72Ie)Fo_^LCntI@y@Pd4}alKC?uMcyBo{3LR=y|X%w z^BH!Y`jptsr~4^Ui{C#h8PfA?zGbjcuT)RFT8!l+nPJ|q2OCrpw*J)Cx=v=;K9>Jw z>ZK?P9P(@oxlph!cbqlK?$DPMR}t)0cet%QaYKlIBwBF=`LGK+S-Www^@D6e$<^yy zGrBnv@k5)E)dh|19IxSt!j`_V``JC0SMEjq$(q{roaDRHH2Du`v2P1|(uZzjLTtr5 z|0dgxE(3GpMXVpH#HY`HF}t>05AUy5VeXT>Ua!6&V8}{-JzkQgUOI6)YLJ^&7g;lt zfi@4HHn$tw7+3$;#4IfCsk#0>?}!$a`hty(#*C9{#R0u+wl%lcBv8WK1gTg-IscwF zcM&H~K|2t?QK`mP8~sgNj%#eX=-VibFjF9HMiqiUX{;M4@+$kc$PL}gh7r8%ULB+- zqsA9Zjg556ujNh$E1K0$jT{AJT~0f$(pU(cVpEm7B~h`Ne(!gvhs_^?&FRF<(F973 z&R|L@aZg1{%eHy)vp@cH%)Bf|Cco#f=sy1=eHYUBuUjmd1<5Fn-LcQ->CWdL-W?Np z()}jh?ovlPuz=Zjn{#tK^N=r3eSCX=$u8O)I+0-Ad&Xb1WX_Zc@Aa;&i~Cf=bA9(% zFN(n*Jb;ngN~>8KIlV2Yeprn?te^DVKTIv2hg{M&b)gSVokDD&ofJrn^^d6r^j0Ly zjh5!MeRRikgD>1GvqO9iO`N{nQmHHxKWiyBBA_$7pKMrHUkpYV0%3UzEemNt$K3*qGP(d~{xp^>Lob@l!G$_VQu8QSkI6c=ugk z_KJk5q-=q4qjW+9kG)sI0Fqo;t;VHTDfyoyq%EskDj#}s=pt^U^ONGZYx6?hG&zDw z8Mec6WeyqSq&yW3?aRdSPCj;;7~8IE{u4uM=1t(?aLJ!~uc+7PwC+FFqL(~j)UFz9 z{rRAKGzC7ywbK;HCh2OvY}Bh)0(TsBPLeW=jF^#h(HmZV6ya3tkUm%8ykQhgR}!kE zUV+jzqvkooQpQ#&CFW=)uT}ev(jxIpt54`R4+JKR4r%(TrCf%0r!?FdsnCO`> z*u{K*ylfyp+{;E1a8cE^+@p4fME$A8bZqwzVeUTd%KrqDzhwutLFv%bn~mz5UpFw& zF^BO~T=Z=b;pWZRY`y26e6*aK_~D^ff0A6!{+NR^jV|BaGAC1nD*krHcYW2Ws1eUU zfr2S-=T;>!eL0pNb^UEB8Nitge4fFd_hDBb%rcF)Jiq>{!@+a@`^IUrCOA|I3-q|b zAuLf!Sa=fu1?Y$wFRQVvP=Jc#@C19dibgr+!&pJo%kpF5;hV-}nI#YQQWggN?eDdO zKI(_=eI+JRE{spQ9pcaZE)?1{C=QR;X1u7nRO(ll7i=?nai8E~p5hN)veP7huNaOAL+u8BO6!D{)gfuUF~5OkwEY{+8Yg zPe*q-qTppxCk>6S-bB@*di{Fb{NFK1u5v`{yfuzGN~_aygG3o(_SyxESN|t7{Wa|? z*7wP;oy04u!fM1cn_8>taozrr%uc7?Q4JvvnJ&mu(~hQ@xun2XgZXsrB*QvJR__D0 zG3x$NEc`{!Rrr2*Bip_mrUeh%fFCrS!(!7Ad*^ zB+f5HB%{JyZ<(74T@0@$b}%I1AcLT!7CUBXZK;dA>mqPE(bRvv%taxLKR@oyjZWII z(ZVq+Gyix7=XP^=-z{5rH3gK+SW3emde4OQq~5Tk5Wc3`&VPQ> z@ADt|)q(>zmW}Jn^>|qGIBLtg~Orx!VtR7RRZt zc7CVdH?|PZ91I~@kMSeo!a13gN3iQ1-H40zQ1r8!#APd!>TTI!n6`W2{xa#2VX|$Gu87N;oa~+m$PFf1$kz- zRm`ME*q1%pq=UIrMX=vjPZP$33no-wOIyt)aJvco?{fl+X?;X@PB$j(-Ma^RXJ5Be zOl++zuC-l&EeV< zvGJ1A%5$m59ilsAIn_Ka`tW1!%Vh9Vc_As)H#2r4i!q(hxtcGN!#Kl-CI*^B8BOmlq}Nqx&{$p?_C*1d5&tc%eJw2Qd`wb*UYoyi>>a! zz4*Z)>5UOpfUoV!w+jFc!SB|bDIz}9^fAp;}=9kO>y?(_fLuJ7A+ z>@syZE8=3So&9oCx4ohZI${`U76SJp!fBMQfBCdCJKSi0H;VK7fY;3v-OWLZ(DnN~ zTUG_LVQO1bC5dGxnw!p#Lo4D@e5qPfs<_j8b{l#!^wOhuQ9IE28uh&N)WBS26t17_ z^N`GBqQGLKq%vaa%W>4S@l~V7f4ST`Lx)`;S5#vS6buJ5erkWm^rT5MH=UyZlp~l; z=n;%dZ2qmh8S&!axggHK1dwkO^ya^B(sGSGqLMSc)R5N1Ub9#^*tPiha;ag}r^bg% z;C{iL&w0N2X*U6?B^1>&(5TeQ%V30O8ULY4D?q5bYaUe9U^3L(e7jmQLtWDpc;&Jv zt3Xc=SsSiq5M=)M;>_sz)$Gq7;hUu?`u$oj+XmcI19ni-5j^DxJdR)m!bl*u&jxFJr zxL&K@+Xf!PGCBeKYojWNv%O{qmqc(zZeq)mKG)cns6FzE(nz(VSE4Nv-V=Y5I_-l{ z$z82jDI>ugc-JnMZnW1ecOyUjp;>}u;`cW-8Z1#AKU8spC;IKHZVkoUG$4)2GyAqo zizwVRzXceLpls&vQp&|;2b&wj;=zoYo8efV`gIin*#!7qn)aY7>SOh3(lf~%4r6qI zLh6a%;VB`UX{4;0KVGHvURi3RE`gsX=khK=^ysWMy8dvwve*l=lg;8t`_AgBsdn`1 zDo(HO0uv`E0Yy2=eGRvzF~hp4eYEy5PsXAPf?gBZt)a_Jc0Os14c+k^cJ^6(S5#IX z$`x~YIf=S1FJ+dFFt13VI+0XV1m=G)Un?(*YRqBZJXbk8dT*HyJH+jrDu(KGOK_ul zRt>U2z_E7&FDI%*uNTCUP`lQ#)hkk=Y?VM?(LRY2Tb z6v6E(dA#rgEbk?)d4G=r=Q(PPa;?$^Gwj_`koq|Hi>X{e#(WJq|dzF{jEV~XLwk8pEa(Hl?zhd^s?ajx_H5gRNE;W@~*uVF~ zMnTCamC%VDO{0b!WXF%!A4Q)?kquK%vNgVInELe24z)ml?Uv1<5x}ntD73(CHXNL*ttvkp!Xfq=1SeLmL z>#A7tdp`wF?rZd~PZ;0_o^nQoz7)Dj)BWOB%G;EBw zT@$i@S*LiHMzU7b+F+vmMYI@khTW+)O)fz8{~Hu~svph&9nCXT*9`vq>wp{j?`u<2 zy?pg=xI>vCa9a#;4d7jDZ-GCay{C*a6%~(c-^YF#*F|#6)j9bG^;hbzQgwja2JkOn z+Uec+Bo@O_8}06gL`mOa`i6VFo-}G(Oumg?FIygAu$c|Z?Zu+fZPIq3OTkt?rRP%p zN)(kDK|*`r-`jf1RLevJveg^9F8w-kYQl${A}wF~GL)`&@luEKOgIbqgsmQgk42Bi zGDEcNUKmt82|bP$_Wa(In}*(tGAmpD(3pt8?I#XLUi75`!41V~z;sfJ{&STKTm0pJ z%+ctu;Zu#AUVM#3B=t_%ia9m>TB+K<81-%M%2L@P^8SZnAD_v?L(Zx8I&ZR2_W5%$ z%uG72-nI>$%;MZr^Iog@omb|4ApUob%BUW7{CBQ*HSb-W=aq!z>K}D!bj* z9%gc&R~YWeE#F&-z7}ZX%q`_oNStwEvv$dUYH69`dPS)HQA|N(_#;WdpSG+nNoJ`9 zJvMJ&1zxehh|uiz2+$~CgvY4tq1D6~4UVf1a6 z(QL@2&LhR&otmER!l28q*SX6YU52I!|5AQ4=aGb9`Fit!mgHE>`dY3|d9FX<%w65! z;PIObhMoprP0iCX{OY$)PGWCfGjKYcf^cc@&SDWuME8sEda6rw|6K-e)S?b{SY=%K zs7p`MN51*N7&8AX-}|%N;=#^YWaG^H#ND{;BD14>QPs_xCFA#>jdU94675r}cZL{C zf^hLSIfM-Cr#f1Z;Wi;94$AmTdHfIR+txpq%YFlH@r@#+2vE#IMsXbT4%l6t)o zKVY|jbX4E#vFoh#X=;5ugWM>1P2+amLdD^(iDlF%?cwX1Q z&#wC|%9jtgrlwUoT?5RAMeEj_3HSMX}=_fWv^;`#tFu7 zeShzYeDwH~`uxgctBUQ>}qvNSO01^>o!(DWLdiQWY___)SS|n?lb$r zQ=H(KQm*>MHpst1#`&<<=GJ(sg@H|~>u;;pDIeWURxK+7146n?>+$gf?$Go0!npTX z1i_duIL)7-m5P_j{O|u7Tsb2X8Mc0mak6%KlAQH8`KFzypXpwLv9iXb{hYMw$eecW zD*v!&m_4nrsIT28Oit!t$h>(i{G_#8-$kpc4$n$HLYtpNvLlaOFanc^=k4d#aMnzt zrmrSPs#NL8Q<>Ld zin77|XFoa+91OkpWDb|FQ=&%SDnb37f{UIR-TQcak%V>n|r&m z87|JM>aB&i!Ds%^508F#LoPws{R_H>zocoTq(Q&o37RuJ^zH4NKXj~Z-}*;Z+%4nF z_(tM657@Q;ec7z^1aHHrnA=R`0*#~bKx@aJ-;sT8o5-u~2)*R0-Wr$B>eIT%#W%)p zZ8DW{9g}`5uwLKKs$_+e9NxFDxox9vjV4&lys|5!lL}{dDtLOYa%ACIw!LL$+&1!R z!tTS(zf56Y&4K*Jfz&AiV0&W6Zr5>mOLqM8-+3q9R~ijh;5a=kvhf zEk>kO$*ydmY*_&)-CEpkp1a9rNlO%?G2< zZ%j{LfC9){>*cGqr_nb3!|W0oX;KYtt6jD3C|^H6(-EF~_m-wznuw@0f3te;D5*`t z9->en`MMtN;mxrWn^}7gq9(d9Jc3(9Fza;StMdJpH5Vd};7;JZJ*R7;^!rI5u5+HV zbnL3q0p-Wz)lHyAwac9MmxC;&x&$=%03#mo7BNwoO<>U2?AF3UbPA~#r zV7$ZwR0rW#kPti5sJZ?0Sj9;OF;?M|?rliHh}FiRP%Z@23x;U*cNP^yH&YY-fQ~G{ z^vEH}eO1f?V3j>lH?Xbh?e2dCwGIE{urxW`7xHpfLjaTZ1~yCIR%KGZxT87`Osm6`cHLiVC z8}_41G`_Cpuc@mbd9c|D!eQ`ul+fl!x{|k`L?-KqL3Ptaks1?i@*x_3Y0R!pdCYqX z`LQfAO-dyVYf|$zU5$Cvi!DJ_uhz{&`qV)OhUlbEY~)Ke)E8F#OBF%|-t?^1*#I-Gb6KjQ+ywHpq%=i7_95fj<6? zwoef*$Wz$8l>T2Iq5ZYEA@3Mdr#mt#s_~))J#Z9HgD0E5?qj5`>gqm>fi%gDjcViu zYKL8%Q<^8IO66%*OZprtKDfH-&^9=@2NoTibV@*j8E=5$wk9h!hI5mWlDIINa}6i0 zVK;`Q4_m|HsWbQtMwl!Oyx@y*JKH&!JRw-Id=lZLK5{-x)tkt>X;A`MVW9NloNQH; zb0v_FoNYfde_EQfKOCsVxv%x?;qe+3&0{%gNn2Z6F?_jt#YP+=-H#t0 zulf5|vM$=|I0VU6T(1gxA(T{IJ2rt+u4;FAJL{!Jh&41!Dt|tDAOk!Z>o)-V(3F#G z<^K!=T?e2e6_CUW2ju><3a3EW)Ww$th;IdS0wCj?ZF4~Ed+(af=s0Y<==lPn@HMaZ z67)BRPronmw6*(yBiuT>*cjnF+vs;fBA(donAUrjWoKvauL?R&{A7h6-Tj@6Ds@1t zmChA0Ax9^&-yi}!-Mz+Hnq({wkJHXtrUIQB0AYb2#33F3VqNi2>bz^iYQu;F<+Z{9 zpz3r&{#07=>c86z$HIvF>;o<^fDh_H=hZC;xrKO?Vx7va!Z%f->AztC#otNM>uas! z{X#q(uN;14GYOIK{^!;$L*zTVkQZ8cFVhDG2I{;7*6Few@eqlK8MskXm+kWT?jj?f z12fo0`e)?=?sG9<(Sr79{o4(?F0E1JVP3W)=Jon> zlFxEmhs0!O8;_rSjIAC*fFZ=`AJrxsE&K@ea>OMX`CJ|6+eCE~zEbHO0&{RvC zCZRmRo;=dM?zKV~x9HH@A?xs{MwDN~JZ+?ZdMIp!0ryky%6@d#%Gd7-ptr*MseU97 zWXbINO##0iiP3PnK^D4|kdjpu4M(#_x)jGIb;Au(-w!nlYL%o9_uF(^mlr~vU5(os zv8{ZIsIP0vBP_)+)Hj{1mTs*`=$Dub>La}7-_Tx`kGOq{nfc^sZ@CAue~e4236s91 z2eCJBd2dyrJ9`6S$8Y5j-Dw;Xc%B_Xy|=P1G;|3!3wnbrsIxN#ZFmT>1#b; z%&qNVN;DD#H0+I_u$FPdJK!i{u3Vlr{<1qpMsm+azST6k%uzf2;Li`Ta zHLWAX$hiJC-Czf|R0vI^U>dR^Vd@!TeKgn^fG-NTH3VSj*#hG)bI;-5ZZn=ZhPLxE z2baIbb-l|^iZV=ZGnIc@ta26E@c~rU9PwXk#|xILd*CRGbD`ocilatQ01LEZ=pyw! z)-HeCq#H|$mYy}DvD#%VU5#nfZL2#f;g zX>M5(G=+`GB08X7=P7C!HEt?{->$Q1*(HV*13(VoS%Ig%R^Raf*!I!YZ_a8P2CAd+ z3XXuqTKS1mjoTlRoV%cfk^oJNf&!0&+oWiuP97Zf$i{#4ANLg3u_8!?%c}>k%%1fy z;w_w+7u^V!E|11$*$oa3vMI+5Twb_DL(~0JrGJ^d-|M#e<2;F$Jkr%}#=@YQWJgJN zui<=g&dZUu6}th`jfKF5w2%k&%*fb3-;6u_0h|hydA}@rcO6g?HAe)#<)LW6cuF;` z2o+Kr=V?QhA1>0^2*p!sz;{KXu-8m5;#k}BPJwL&V%Wb^;0v%_Oj}{3zSmso;LvuB zbL5>A;S$DT8wGaa!KTM~E@w&Kqun}8qqytRpM33HUQ=Yu+FY(inqIz^95&R=)3eca zS^ijZrk>a=zW)_ITGmDzKV6MuT4S7jvsummyM@~aR%KKTO5Vka zzTMoC9l0^3ivaTu8tO~|taqp|_>74;ohbcoEl0$*{j2QFmizn|{>^5G(tpd6ea%)R z&V9{c{$efaq1MO12pU&EyoQ`lMWQ@+?Nu<^LdkfCcLx!|TP*kNQM}F)re%DGzcSf3 zv=gPtr^DUdg}Q}$+tY|NQTH|C30`~tq*r7A(FYTiC-%Qf3dO`(4WSG60Pgq8hP4Sjlxg@X1%!7%g8{3l9t9{< zK;Yw+yrhbV$v^cSl#0uv`ftwaJFjwgy0vTChMKP3nEQ7cYQZ!-;}8X6Y118eE~j&8 zeplbE$t>j8l~9)m<0W)qgP=|uKk6nrFefr)Jz=E8&YEIP(^fol)E#>nwrtV_pY82c zdl((k`r60mq`A4-rID~!+C)0uM$BJjU{DBWva+_$@fDq%oOGHjUj@$C4no=-MNfoD zk8UBG^A2x=4RABk%w~MPKM&0*TXiPYMvoN}koDxm7Byq>}cfCjY&C-NYYjAVPTZ; zt08~-`kZ$ceqn*<7W1(0PnD#lrHw6R^YbH?S+)4qc_-#(M-!)#l-vEs$LfIb2QWgQCo%GADM(WDUl-UH(jBRI%Kba99WXFl z;}?028yV^qi$R&1uT-ezG}=zy(eNFsyJGYvC6BW^Nimm1rSG~Qt97`X^4(KU&I}^- zS?Z^_9;w^B_-y;;^V}!JY0)HRF;>AH22OhnhaLa4=;=aL5`M@=C=wzm=C-0(s8`(6 z+dEtznx&PWnP~}rs-7m5Vque-g@L0*JEwoF306FGW%hF>DKr7xIec~WdWK8u-Qno$?OT2@fD zRj@R>{ohH6ySGsc;tSwHK`7GmyII={C`BEL`8u^AF)!}RddPpu8%}*0Tx;wUR2=KE zH*Qy3w4)gLyKG+4XOG`+1PemBup4|4%pxUl;_+yqK3}PGmw%<*aNG3s34Y$+pTPf7 zVYcz)vF-Acx`)GHE-Iau2j8B`qv29ueJ4aMhE<=|tW5xAM{${440XGNx}m9QRNhX1 zbbAH_5UP0f{_uVIe6HNidABoXR^b-V_w+wECc^JD;o3o=L6Ta=Uav?rqoTIz%9|3b z!;5n?H?J5o{rCNJ038ez`)+nZGcZSKH5AFW4^Gld>Ys;JB$Ss1Ev{Q{Ki;_UaA$$T zp5G_qWQ~rY;-jo4X^S z4C7a4iJXxkiWi|VOg&T8j_rJaFtsZU72h3^>>c0%wWb`}Pf%saxw8uiKNM z0lwEKAzNh9Q6o)K8h$ukVyVwOp7>_umD1p$nMa#^0&Z9JBJ@jLE&X1&VJ(JUx6ge^ z4{S7;+;vTqL?Q?5lT?D8AVyTLBsXo6?^<}=$DBf!5sMDTU*^+OpKUq-dq+})#iP})k#I&;{LnduPqqrMj@YOP9ax*GR? z?3bd`;i>j$#~ew|1(zUqNX60p`Jg0QE07@0b6(-Ic=44gBeSgEzPSsg{$_RHXx-KT zlbT4C^mko5B_FHr36zKq=FPRV@XU4}(t6&!_8ln@spj8AKsR9}yj$_>6i;v)>&&_4 zrQ)0)?7W)K2Hd!9T0NkC2oz>~XsWKd4jf8;1I`s4J@~oU3lDbS7A^R>x3hIhC*vFDB%J%6QCeR<5!lq^hI~?({9ji4ID0@&0qKokH^+v>LmP#P1*-p zd4fK5t3NJiDzK)+b@xp5B-B}-n2%#X^3hDP-@N`y1hM+$DgAO{G_VOl2V_fGoqzG3 zX1**W+elkP)QKn)jXh#%B1oa2O_i|J+X%Tn*^|p^Z-6Z7k%KXSF;b<)E$yy}1?q4_ zc4VN}FMqoYs_FB5_ZqiL?6bDyf#B=vGH zLVN0bddyCGyJnlbAxFcg-FbkLV7TeWzT5M0p(0Y&@)kml>k&{S7L6(0XhiA%@2hMuuVe=`?%FY z1br&9X2L<`ccY3UvxZQy$nU(l=io#ZR*fqXds^S}`EeKoV)q=9>Aa)MUhFlsPb`pZ zvUh_t`J+&p;nSf{(-hjB2)7%Nj?4NWc%dpU9e*GK;`WWwW`KwXqM9Ysyd37+6l2mo zbcdjL=GFHmD2VXOYS^j5`ezs6PXm6;;YSG*4>9~}l>nM@?m58W1DR_%6Jfqp-pd7BBaE&CI2|h9!&FAY^}eUhL*{W6Rsa~mq@)b4Bw&Qs!lTmFT2!hVY0%L!-hH)M4Z37}Ls32!#5nXx7;Hj4?t zcGP*=AEM@4C{g(APz@{kvP4e+^mGfaM4{t~2j5M$g>icy8(K-DHUzA)!!1P_+zT&H z#ujW)g5eH(W^s@-Cs6)g^C2m^LCzl)K`quEV_kRb8U$i=ZnMqRyD5# z`alzMm`k!ZdMsz>T^55dGoGXspcL5u(Lcw`1)M2O`!YBCqw#G{XIQO*<|(3%xv4ao z!!fK4oFjHs0EA9a@4AscTb_oc5Uj&A6LJC_8Rb(MPTvMdM1q^OMGk3GZo{OP`DVrm z&Qo|Hpc!CRV>|s`;4>`1H#|8v;8+9&%{J2~DuG}O81i_EG|$XIc<({@M%{s}qI}?H zWcbhI%95jl-c@Ty9#MGoYV!ppeSh7x;X#F&vF@6v3RhX1=(dtECkbT_`aJ$e8JoKu zS|2}z((YDlZO2M24nE#!ME?5H+h^==P>Z&~H#9+6kO0%3NbWQS-R}S=sD(#CLEV-T zjJeg;NR`EAK{@z*mpA8Re6~`Jcl(%7_nCt{Kc!1hG(uy?8BocG-vr zb~$-FsqHh{Vi#BJLh#uY=zsk7momp?K(g>b-%8Bd9=#v;sOc;P8HsJa{tNy0R{^k_ zy`+RCzyr?XP@2VsaBYHaD>i@=##XBo>$42AFr-@aVWqY&3f#P5Cw8~X(Klvv zh4ye*BtE$@`)GwH`=MjO(+A~CFXeDo7FUBl#ofGV0I8^G`gOL4MlM1EklD4^`rVceFXFx(}^JNK@ZevhViQ!02Y?KMCcv^UA01UfED>y zXvX}4*&CU!y~*vU-KxaiIxdllM_Lq?IuJ>+`#Tkh*SxnIGndW2T`#$OPxYaEf|`+om_>1Gjbajsr0jWi z%m9abf=SS=$GX_0LJAANL>iTEFt2Kt&fqHANz!2R2oH!x0O98G1C%D59IoiilHUpa zBo+OPu4h;Ln}r4h@F{?p4eET_<*@rJgy#UpRs^8SKs_TS3D~YgpNBSGsv<$pIM*Xh zJ|!3sG0N=md=!c)$p!Gw09{%7RS*K;ssLCCP$QCXH#2={DHJZf?$nQ{^GCPSBmbdf zB)=knh?Gxhh%=D(81j7xR4|X_A6`dS0m7ih?!7a<+-|8Kl%irz!6Ni7K9#@awqy6Q zC-cWJ{x9{pLrK^ypq>`uiH!iGRx(XSICdjwP3Siw!jE3{MF)8s{IC%IkRn)g48!A} zE|zb9Lr1X2ng;?_#i7_c-B@hiG1it**FUEVssh_7-Yy*%LCfBBpx#8`s6M*q2{?hV@i|)(nqg)PG4vvjg$w5` zUIKjsz;g`CF5s)XnqTzUFQM=LjXru&;!fZmymlH0wEL*OJ)9utF4$Z=dI!>Y5(Yc<{JhWm<3)3OMda#}5( zYTU|f#^%M+(P-=iKc@GQg5tY1`*ps@cJxAb`-=iBEy_MgnCk$@UF|P+6@90?8Hy4* zfPLG~x+nt-3+JeVc7swpkj2x$&anb0&X@)WlK`TSW;*uWBlC*q+l)eq9zZFjG;;^y zqBl`(wA=^4NFe?ynXFIR@73ql^t)c|$}KW$BkfQk69!I<&?o@f272xs^QlArtTBpK z*@Z%hl`!_Rq3?Y^fz43)79u+}14Y#7!Q`wDau=5{A*-e`s+O-W%v&EU5qFJ8p+3O zBdf6UU8ScLr;%6peW8;*(1%4c_U1VS5%zG1c;V~pmHcmFV9Ntce|*x;tyC)oZneJq zaf&q8Jsf9U_Gf-Xt7P;67+NmjB;VdI`d%wC(zx*vzbC+70=>pdH+a(PH3=KAfh{2r z4gW2z*~|_)xK}j)rR|n$5@h7=sh!Bk5#TEZWrskg4aj00{Y|$*1NOmadwA-fHy}uN zH3NL0WFMxY)QolwA-R5!{A*RvJycVmSy{HGXj+a zFjLtDg0^FR@4)_)=*ePO?j{+c%FC$Y7#g3#3KI)s2Trz2uS&C>A{^m-FhjPffLIFp z^YCalgxJSHaIrk=J-<0=#XDADU3CBhT}@H&N_zxaD7E2Fl2`q+fVSC2?ms#Wnp01K z?3f9GMTQ_s0mr`qZtoQgu@VPc*8_c{LUMppK213^iXDstB?#LfXTWat91JRDLPGd& zmmaB#Mrw}hdY&a7mr6A{V#oW8`RvX+anS67JIwh#f55G~C>G5x+ON|4peOXtn>MGr z<}XR`UfPh7ksnUS4n@{WR-?%h2P;<;h20^5m?6&>z9}}9e=@NOGT0|9O}79!AHR;< zb580Wb1VSjtu_NchX=S*AF-kA=G+|tSu2rw{Psz1d(CP=39VZLaDKTFrOY0XoKiD% zHMS$Lr|mM;5r50F8TSQg<3g*_1BQ#7N$*3TsHiYK3{jH*eI9kkFbL=>y+r%DdUbk0 zZj3qw{?G0_2W-akbiI6g2#^rN>BIP>94a16o5G|xQBN=%nhrzAb7)@P`O|Rpp%+(y z&+VF`Q>Im~dmlTYI->MM=vUTHZBj|hSBnh`IMkvyHuarGT-66IJ~U%|32eIO?-b}$ z`~2tkV!=TV$XInDMtZ7MJE2ntX>o4LO`^cFV^b})&Bqe)H*H=y%7aVqqZW`W<<|Q% zZa1qi-|>K$djsfylw`&Om%uQLqk`T7N%EYHq0H0_1%i{{O0u;F&IgFEl`IA~6s@%Yon+xqYOWf;}xsNLG-v zMVkS(@4&=JG&b)9djbB}dv1#U4B)H*jlo-i60lh2%A^}^pmuG0#b%zCK>c*8|r$uN~yfGk=Q9>iDGPj!=>h^qWd<&xp5~&1Xh&eFK*oQ&!X2 z0MAR3@|0n6-XAgDmYAC)NZA$_5;w#AOo7G1DKp22( z_^udD4S){==z8Nj;yE^evgf)=7JwEFBOlzac)pq3?g%1gN}6VS!Ru5nUNel_Db(fw z#YqGx_5K5Ckncs5d)tZbA*W>I#u1gki8bQW7j}}Wd71SM=7&WdY zWi#_udP!S9u`~4qXTjYc;3h?Wc19)q!mTCf>&doKkaKr9?*zau8K?G{W_D=jR|Q}Z zaTTzW>N1X}!dlYg?@0FU9$uD(0MX8B>k7Hhr>khgU9qU%D z#l7HG$+V?f1;X1X#W(3ph}ex5O3haQQ1IxX}s6LC;FGXgjh8s3)jzy4b9f{7sy;{q$>6+DOCx-J%g5CKN9%m=) za?;|V1Q5|WQh~sy>u;0f2w51tDKV(Aq3TaqHVCI-aEK8XWyxPw_oej<;L{f9&l*4ga6_;(RK z0uoID34n{f1u&||^uksS!YP0dc|<(^i)#OZTk2mb!wKwGp^#NX->$bueCqd@$?q=p zN#W4`QfG00Esu@KqRpXUuipF&0pSoSxjxGwKaW+{D9f>AGvwYrq)FoD2aIL$jh=wAp2t10aI zo-R{$eAh}Bj}uEbf?2}7qrMr)p-qd$=o8mp?h`xlg)Av%&&=&sd#|u@boinY}T@;Co0pf&j-f=dnSFLQ7%7Yug2r z-{YO?Yu+rk2KAkIw7&oanXb1PXIxeXt(cpP8kHn%9fGX~;tB*J?a!}87u@>(@il}~ zMcL4f3-^QPPJiN#&r2xrR6_$2GE$ORF<*WR)u zKn{N7G9vKUdC=$iW@I}LOzp8SS5|!?K)SC;9DVpUUQlo9v)Jp;;R5Y8e*Cf-%8E2< z^i)YO;UkXA`^hq_G8=AkY*AeRmw}id=LTaB^!(&^5ZvL!Ns;5}g$2BlK5*UMp$cN~ zs&S)X^#)fi?gr8)_vl6-fKI&YvU!Hk@Ci8D1Q& zZTRdb?7X#_4PPStuDXJc;w$$NN}d`b+T3#lWP+E1r1 zGvp6AK6%X^&dz+b1^TH#H5`B-`Nj8A_TP6E7HVzgC&ciD09h~Hu!|(TXYZj!S-;RU zw(D1X+0GK$Z=OUNjZESrFCDK(F7}hRPS%uBR{xw%l_|6cK3&WRhY%`YcEL5zF5sRmRvr6|}|V8ooOp107FL5VBPnO+YnT zpSK?Ekx0DtUH7gA|6Th1z>ZHM;6e__Yb};x@SuGAR!OV~cz^6%-K%7~#dM)+e?P9h z_E`k!`!Oh10`E}YF!Y2_>Pg1yPm@my!@up%yO1|`kCkm!8$`(83ufG=$Grfyjq}O#X%nO;O|xmpv_aF5^4V) zcb42gXBr?EB}{eO#!E@&#WT%oj$1o)TRUUob>O3UCjWZfS!2yB*-i}=XEh|3x9E-< zCrwQ?pBwzLSm?+4?Ceda%%&{<_CWQEV|;veBG8F}h#cpqBGB#pkV4tIy5k!+>*U6G z#`>aj40}@Ai#n6tK8{~@la#XaSdXd7&2^EG)^6w-qwmhIgZS)uh|7l{nvey*Qf%(8w5DH`ePyv=mW>OF3}gv6H;DP_2JqS&w- zXRp{l!!mB|Tq~$n)qI@5;h+(Lqq7rqNwXryUfh;|*423>Bm6&jb*zZM%JH8S2>2t1lCiJeIVc?VFZK7VXwThkljf1o^rLe*TCq-BX!3HB zAgUf@Qr!v7Zj2NUH;H4j`DHuq(s?;tO(*PauL1k9`d|5sDb!p|#SPhzp-g4TL_?Ta z-I1ib9#1{)?OPWoBBRB7bE;-W3zZdEK(OOSJmze z*(rhaWExsKR9GkQfaN#!OQImGy7BTF;YY31>ueO8h{)s0e&Zs14=w}q;H%gm z^ymIcymvtZy+axw7`)O3q+7=GUP=gHwr3b1iuaxu%0fYGf9`zV(<3-t0Z^b&4= z6S+I!t-EqDseq}+-Aw4af{;Le_&J}J_}ewPhSuG!n8{Fv(k9xqbS#x3(>q9(dOPp2 zWbGvW#{Ed^Q0ql#g!zG~$7b|8^|@!)x+5i6kZ)W%nR;=wU*J#UW4NwWi(*B#pC}!9 z_X{R9V<^vNVXoC5ZwULKBMUPjnim8gNDT4T3wgCy85;9x%)fKpJQj!^Pfe~Ad-sVy zZ}C)iji?8ohno1WOL(TlA(9(?Vr&9n$E(`IIv=6DeKRm9u))n=5@cKJHO=1UE%Ff0 zGokP80!ROkyf+Ppy8Zvgr(C5ZijZWBsBGC~FWIt%ki8PJXE%1)lC>0BhV1(~mMlY2 zc4HkoV;{yc7(2s#PT%XgzQ6nbe|SH+kK>*Pj&k^X&hvcl%jDpCP7mz6|1&4H+QNlFGycD)xHq{ctaCMO|`WgqCmj%AsOXq^dCo6JOy3Y!{E8 z#`s&`j@p6P;pF6sd#}G8h4sg}b=21RprMOaY|&8oL1l|Qs2uM9(qRRAw#>GxXOV1Q zUDFHZH2P*>v2r3{ih99h*@)p%uXh^;5=;fX-bKQq3VAPtfwwg<~un>V>PQu3Z(0XSXXg8}yM+m-i> zCZ&TfM8A3wL(vrK-Ky);-LEe%QzFDK>~1^hH+JaUWRZ@$7c<)j-N?9Zu?$^vTCK5& zbnQkN;=S$jL$+-ZLOuh*d8Qn%DVP>$dY$@MJI_q@NoedMk&KJ0%xm=b`y@R26HD{* zD+l?I6HZo0H`dU zuA9``Zn`v~-(eScq8)fM5QNPdY_r#5uQy9qeCM|Po65EgK6gaJ>_}d4(Z@7NAMwC* z)LXvw`YWg+nMWsMtcQJPuEcNj`RrD8UapHME9+#G!}6=>)i}mqMK%<-eywLzG5|bV z0zelu?Ud6ckCoZe{!!fMR&zA$8MOd9Qh*h2s;xsUkeXO8ZabSFGn2x3({fFCxk%rF zpn@s0!VG$BA+N5ta(a;ZBqS&b63VT4b{H<{=-Ezc8<)Ax%U~P=p$#r6d;@%b`n$%f zICB4Mp~%3&#j!8vn46On)+FPjbg?5{y8DJPGSzvl$?wL-aewLMX^9Zx;euw+-Y6&L zMibP|@XV7a8lA=xK0`@Zw4NhM-el%-iHZLA!$Ge;$}9d$$rRGI60ZY{BUXwSZqO7z zEy1luc2b;j9{QfWdmTn9^VD}uGKDpDP|j02*YXb|gMp9^{Pd$+BPa?3TNl)2*_g?M zn+7i8Hiw<)(sJ6e{qf>A*`_8H8p4DvKq#Z* z6f(Xv6BWYv3nufW_~rO8z{sRYR`GQaMmOM8B-CrBk{-tWNuayKF=Ih>}wB=KE_^QH0d1{xvJDTK8B_^97w83jmt z0KU(%>48OD^0`-%tz9~q-aS?}N>Z}@X?PZKHLRv_ZI-&!dH}H1QH@RycD)Zip2jhG zexLt=2=;0sTA8AR80Z|dd(4npFF3-KesFj#sCk5^+@4D{sfSDCMfZ#0xu(b_y2PH3d3poxC%XifT5c?`L&L+pk%{C?vrBDVBJcr^!n*7E^I<_~q<43(XDBI0# z%wE4?NfqbbgNK{W_GBalQkaO{KIA{E!~x8^ThdYjkHh9yZH*bH$s7;99UpcgL#be- z`~ybB!!kW&o;W+Dpl?Rl>@U%C=sxn?8LEiHTDgw=g_|zx_)&~J86xP75oRWu zmzTuMryt{{+ijY%3nh4LvklB_#%X2(u)+!C^B#6p-1XEMx^>sM=olpRY;+d~y8p6J65MTmT^s-PS#jmDd6ABsvZ^fgjK zv?RqTwZ4pq_aJ)n1MAFVJI#Z)5{nEC9)Dl>V4CIhn@2Tb!S06n*t$v6h!XFy@aDMD zE^rvtXA7R`_Ka&6l%vYJvnn;Vwk;e^!+q14+5BJ2?<>hzj-%c!STp1(bH%o&r`n2O z1>BIk1u2Eq6zuoJDPL_byvyrV>ds}_0Y;~I%4x_1k)`jY66-0FKQz^^I>`35Lj3jY zy)W%H>gvYsp4dZp(|N$4VkEw%jAd-QbF3|csf#)-F+p+F;jI6)Xk_3s^PU40ez%mp zVH=*r&uGb_23ODKtB7*H24k)lUit$O^Ss%40)d?GeMj4B75M?if7T0K6?&(Yocj?oPF3W?WETk;$YUXL0a+z5AdZAY` z_~5hNu$a-g&Y&)wUUIK$+p){&OshI6O3z{Kz-ecwizoh>vp|W2pzGu^h>+C`7)7An z%4+io4KvBstUE>V7JlK9?(s~X-xA|maS0ni#jU27G>Wm3iCk3;>w!yXQpjCm=+UtY zcQ={DUPIt7R2ARRy;G79&`_vDl0=%5&y!>zE5-n?u17OFe0F45Pn@tBeCKOgF6k9 z<2s6jomQN3_M86t6R({(#h6`e5nX4cjlki`>SJktDt=y6kPhQEFgTP-Wkp6Ix1L1P{?)Ercb#2QC6uHWUE>^af!96;lqLrBV zK)L#UKlj{>tH3Mog1D z(fSR2nByxDT22)4IiFy3(yX7Oc>`WvIj)X=Ya+&Y#YZOKnm;m7><#ZEusuV=!b5t= z9&@uL2+x(DvxNMD>P1|wu%1M|q7)B)@G?f|6jpXnLZ5V+Qn|C0%s9O0V{E44{5vlE z@B>eGmt?*bRLI6QkC*mn3!Cmo3dytu!K=ur2?Nd5DHc!AtaXn}$k@8zM@J_9HF7N# z{`e){#cC&Xx2?El?^m2a99DQ}Se)jrW}DHz+{*a`bsP`#4!yleQqAHYhb1d8?AdFy z(lwd^Vyq>GMP38P_X8&8ci=8(8$Tcezgt-@YpM<6%@cOqO^2sEvrQ}rKR*%nXjd<$ zHm)pAz=nZc&Nm5I!6g{-`jqW#4cEjw@J@JfVrdMETTSvyMzp%S=4i~>DPg2Pa1TMM z<-CY<#o1R!IV$x4LqyMgxrpbo$~6%XM3lc47$dm;QvbDPK=D9z!AZa9gD%JyO%pDX zI8Ra*Lf35~7SaNIRy3Nm}2+ZRN3p|z^p*L*n7H={H2C=C zH$G~Swl`MaClSwotvSTvCBO3@{|JT?qdMh{3*KmF?7cck`HWQGj)e~qZZ&nP8&|+) zmBD81(IG=`v@`};rCJ*^Q_Mqk@)Y66an{95sSJ2GlhitV?MCRqJ7dvc2DMav4i`t^FJYW}OXO$EJmn?R@V7{sJ_WZivE!JquCr3|8<~k!m$9;J_Y;I;^VUpbQ|ca?+TLjK=&5r}931T&e+Sr)Y*SrDrtTlCIPF2QU3X2@-WdIT zBXJxZS}o}XMb&DT$jk6TKCvZVQXPLgRpu%K%(+Z*Xvb?OZRgHUs3e{dpQMC@fS`0d z%Ne0O%nHgd(R>Ki>40$A^;-AdUfyCm<-z+@fHX~Q<3J#!b#JD;IXbvbhz%MP32yr= zP}uUO@1kx`mW24q07shBox^NgS351|foIn08MuUMcKaexRpT83M^-le%fM&FH)(&0%j5szW_$izLGID^s$T(B##QJ$>o`Z1`)7)dJO?Js$EGoDTX{=AozC%f5YlC9n=^OG2 zW5tIi1k57xa#YvAutoCLg+rFZ;LophT-+q z(CNl3Y3odFdM#p4ANdr;S*tTu`;E3jG*i_p~KUrUFOy?|dPq?=M2%rn7?8KgnVqKFYJS(Cz8R z-`?@wxZsIn+E+L9GD4Yo2?fa% z8}x$<(+WUN1%S#MyR!?^(o*M^p{Z7M)$Vsv-#n?Ys*K$!!F|J-1MyFsPFWUxZlKmA zYYmqCt3bnN#B*^9yu8PBtJcSEteHX1bm9=dv!?z|uUZ1}9ktWQKmbg4oGEWDyVP2h z4E<4Q0QnTvSw=^&aJct8+ady!vkMJ(y;n4c8}~M}gwWa3u@(23{V4d1 zjYi{+bbJ3r$fsnlALAc+8?tU1E-_q}myz%ORPX{XJz(j8SfzR5eEyXJCcs`u&!Om9 zPRf_W-$N++R{Y8c6QAqu=6fxV28MRF-4l`@PsG89Q2QsnHq3+Z;E(rY20l(xmNE_9dqZn-T5EB*_a0;c zlz-Mw7)T36CGM$Oeo^MtJAPYi{kf&k8xci~2%IFD3TsG}Eq{dhS#lI-Uy+B&^u(s{ zz2p?Io+p;Rq(O2IVRyFYG(oM2ahnYpk@*BNYe$Fa=iT5@s9|z^u={A6@E36zs`$OU zQ!CErL(dpzXw5$xK1RzydeZ^YpXDM zmnSC1*I5L^{fwh!(Co_t!=tO}_GcXA(aN$eF5qs6d?02JS%a3^Bf2}ravZ0Q=Xj-1 zo+~0ehus_G2khWgv&7wL@enE0pH3eBwHmLG)ndGTInqzk=S#b)YIi5f9^<)M1BCni zd%y#wN+L0oq6CCq%}n`WU70>|6v=N7n!QQp&?tLCEROITaqSkLP5<`qd$lsjBcGQ_ z)IW^px9ZQ1_Pt^A;!SeSo8M4r(ctdKXr1Xj;&K182dV{{Lnx^K7M<@ZFGb!YsOqAB z+%av(#_EMa&--2T(F;IF{C5}e0^F~b(?TaXx!@Q+dQcn@>d?=%Eumil+2@Cb0%FY( zT;mODBoHO#2{)#t!5W`4p*O7tu5*#AstL|_>tre7tyopw{X&bfGh#A&&Uc}8OI5p< zFmR_}-)vjxIEDVS%hE@1_G6KM$mxIW)^^{B?@kf9Ie&70+q~758?J0Qa$h2heGYr- z)HXYLu0yQFLpO<41t}7U;d`**Fx!ypHC-3%waz`_W4PT+>MING$lh;CiCTaDct=wu z<#*ps;QkTGuXo^NcOQ<#YBGk?w+m^uPGoz)$-0lgGOUmZIxTg=n@>?P&XTr0$#sV?gTzhRTy z^cZc(&bsGcP5t>{RMFv$>j}T^=X2U1#n(br4c%L~=JmqS?~JP%Tp8m*Y%jHw^^(4P z-gd(0o#M1aUn7bCc^Ii|`Ue*`hL;Y|=`564k4<8n$r*w| z-1EMg?7z7sSUkM;B6D9NB6FT*%L){DcTTV8nK%$ySN8i2p6lhJ9V?Pjk4l6SSi|?` zd9_TADg+WHrqV{z5n&?FFYL<`&)MdjQyx@#Je~wfa{pP5_tnp(MBrORB*(5{d20l1l49KUnP2^v13Ud|qz9MxmV@ z`l~i!s|}qIzs^v#-4@Xtfl;RU{PLrU53}gH^i+R~Ixt6HJylgz9#ma#IG+A6SK{Jz z(f}8+XxF)eb++hAv?&a2Tq31-(+h>LN5v#bS&ww+WTdODiZi(y3<)xVBQg;wsnR{Q`7Z!CUFn^u)m_2}zQUQ5|C_AT2sk%Ol^UaGhe3h6$Dt6GA zqOJGfT8ZtO3Rq8}OD_2rtfD7N_+8 zg?HO+QlpN_S9m-CWw}Tc!SZNo@da9G`!<1HdK&iQp7#qY)s=SRLV8t&*)0kRi~0gI z_Ol}nbAxPGU*R|(i@e?}KF-Ql)R6Mbm;dfmxg_emdE#}^vx$Gbl}s;}VF$`BFycm! z<2d|ymdGUK03?Yrutxr$5lcGVkA;(Sf(Fi>s`!lQJjpqo%dGSC6bP25Z`B```h_-p zZ?%zb9Nd8tQ8%L{B%>Q<`FgikCJoUVLT`tn^L8J(VQs zKvmw;nXX|mwJ|MWgRW7xQLgy`ja19S5IhamHRAB?fJ@xv6EGlf=6|PJ(77X2*S2fB zEwt7MM#+0IvO#~|sruvSvsmHIYc;JJ6trHctW^t&DMITa(lDLur3}wOLBaTxvgLHW zC5=UwM8w(?RQ6=!isWM4j`(FdkTntIAX63-zzzgjw&`88n8h=9ri1`BxRgrstMM&D zW^u3%-P`uHo5oXNjpJ9wx7aiix062u>wEmh zo_e;E=CJAk8EE45bFmNAc*foIcv^zLAWr!SILQC8of zy!c%5%(aY?Y#$hYB4=aByCw`rF%sov($~9^Azmzx#c(?)lqc z@-oE#LKD-m{*aOfrQS$%@)asfT$ge~4o@$~?lLd{KH-I|Bn)Grf^ z>=TjYRweDAq)yjNLNZl_mBAk&&AwJY*6EAB&E*e zZxr}C=RD1wLPYw|LD=+a8pPL7&Yy9h8i*)3r4_AU3g?`8~7k3Rk! z#CJ6GBaf|)AUDI%veW(Il`7p3lQ-+%u#)F>>S4bD#o5-+;1o(WxO8h?M&vYG(CVsr zs+l-#{G^Q53X!j3u3f6*WsfySrf6(8pAiDalvs^#e_9=uQuW}{uU!q3?UG_@DdQ-q z;e&&H3G>PLeoVk-@Q!DvJ(`!?LQgkxkr}WZRO7vVU-Pa+m2I>C z?xFYn0%?ITPXD_%%;QGA$M7FR&68E)KsdXY_=_zzNuMUywAhNdb&>G{O@{@y9-4bV z!&L48-})u+9Z-&2W72%_)*(za`bCkQW{oVJ&IEP|R8V&aG-Tq1*zI)u1#iCOU6JJn z?Y&}ZAccE`h>Fz>_6+tOl+IlFO9N!Rxk>t#$cF7+HjqI)R|YWzAdgZA=vq50E;e$M zJfPQ54`pg88&WWOgxf7l(bRE%%|TzFO8I@EBaBlP8^;y2^y#T|Nnf+L zw8~COvyLXuPFYvsM3k*8UNX8e6sYck(C;ci*8dC9;)48tu*(^BedkHz2l7Za?u(Hx zNwQM?5R&=S)WOZ4m-+1=koT!=S@7}1PMzh6?;D@PPWWx-%B+gzZs?Hp^WAsmaHx3? z12Ja%Ptvsz-ecD3Hx1+w5tqIcNWI>)SHms~nju^lS2aaTE6S1(Yaf;r&lG0<-K2hX z)%pAvJ#(f&GCOyLUsc`juIlK--?$E>@eZ;^?hM575@M<**)t|HWumaH*VoWQo{_WW zRcv`ZBEMjz9j;@%^_lha`y>!r=HW7s8FBjNNRHUfj4YU~?pWQ>Fy9Nr9LGNstT(b553Kf3su}?NLj5og#ANr&Hx7KSZE*}TUpa!RD8c& zDK+C&`;arKt#`|UL2vgGjbgZt{hQ<#?!$y~qqLHJ-wKVfZF*1jO{+9f`kjS^gn@c^8Ky; z-;;T6cwX~@XYgueu2WC_fUa+{QYF=|a121QIzJ2NLr?CR2N?9Ws~m!rx1 z_GpP(17T^G=f0_Le0t>f(tL~j-1$@|YCkjt@Hx$UEnLY5$Nij=3A11GN72?F0t8iumm>`J#`k}?_K^8 z7+lv(w@Q%;CnUZYic=93G5HF>JM$<`PR~$s{qh_S6h}x)b{R~9At}3P(mrM>^!%R{ zHY6TuA%MEAnRX{!}*J2KM4pnW&%w9#tA&BNG-9Y&(1i*X|(@< z&UY+0722)QPsM@Iz^)PWW3GXQ-Cm`Lnrl$^*ZEk(r|l^ri|4Ezg@`#&27Y^ z{8{X2`S@eX&{tnnc?x}>b8?jdWmbLi%}PBjqa37RizZ?JFd7SgXf7-wQ+Vqexn|@JZJ1e z;j_rJ>$~8v-!9`=4t2vLlDV~-q7hVJHLG%x+#ZoCYb2b zud4QktW8Hnp;&_MHtcbJFEm0Wg!@74E!>+%@<`PuIu5LV@6tL;l75+X*X`N#OgX8+ z^Meun)vn{fS4Y^rJ@j|U?Xs=cK}%kj*lA}lqr%&$T>s`T6odpvJtVX5>Hx0mlchrq z)5%nmemmHoyCFYIptZhur;~GP_zCQc|F0})Bct!EY;Yi>;A+Qa^HEWj#*p{Xy*=rM zA)eLCjnA4Ko_3UFrF~TX1in%U?&2yzK|@d?Y7&dimN-L${`@0m_Grc6V5D`chO47w+-;0zRL{M4(=Ht+vazB-dh@RH zH*;^_Eh0%A01lSZd$ReHlX789Y@`!*Wx1W)#?F!MD2W6T_U7@AfKNwBEqH>+QOc|t zNfa~)LG4rh@f`ttS({x#7QDRUGmo}N=*?T%uwx85> zJPrBDrVj#~yzbTM&fAni5t{}@*Do5_qU_8`)?W2{jrl$b zR%B)M>aQP=o>IzmoTOHMRw$Fuy0nQH0+ozO{F()?DC>+7jz`Fq9cNj8&O?Dvx=JU> zwPgI|n}L+rvCH}_OM=dX-rHU+p8L^zWM2_$9q%S*!gr^P>v2=FguVEi9;Kkz3CxS3 z?|c}*5%9K}e1~}ykh5*E)DVx?F&k3{lp*{yuQ;UC=QjzX<@rr}NS@P0AA^-%e{yLV z^)~!KoBoXKk^VzJ&eerSd_wN`dk12d;~h<|-58EbS>YVN8{}uZV^xsO5z!jvQ&8X*k%`QR`LY zyIeOJY{&#-J!FN{(!cd~Za~x?&yVlVW0xB{ZVNrcvo|tNj+qFnwwAtzyOy2Wi?6m% zE4VdTcy4i{;yD*kFCiNYJ3FTF?haWanE9|30{EhMh2he8Q0s&!f%I5OD)|vpg-q@J zG=K~+YAzSFNgxz0hE7ls?)V2eQF`gky+{QlD&D2ihzj-Wp=kL0O>cYRzEoYBs*dN) z^U-40<{TjC?wjMk9ueml^rXr!NcBQaX1SiU)4A*#Hb`z; zy)zZx-W)j-DR{l})&8CihiH3W6`>>NNm=6qNDR;_=bo~<`I36{$ucItlSnr*BHhRz zG#u$kdycMl7>`#sg1C0b{G$I>#EP{A0Up_+ zwUPCX@udx=Ka8<&m2_I?#oNq5lVNhHVZJPon64f1DlA>AhTEp$%rSgZ5+qfMbqAMC z=BYKP5pt4&W~PKI5UrcUT}NCJTQ9Btgk(mPukntpFB$!L=T%OMMYN$YV0J{E6NOj< zYK|Rq0kT02xP_V2*2nA_axotTv{u)IBEpY!i0D(T4Bi^DFOfOoj3Hvo3kLTgd7yx$ zB$Z8Soa<2Ogoo9sImC$9F} zb~(0kHK3{qBT8K3&R*$WBxN*w@{4ITKI*Wc8FLPOg>VNVONC`-N`UH_Oinqh98e@M zd*gHX%8uJ&vE5z^+|N1kg{*j93!v)vbQ4yc=lSS&ipSQ@^GI|h%RscQ5((x3JC@{~ zemsS!CC^1x)p>X|1_N#VmIO@d?_J@+8NDvII_dUY3@!#6{J{1Tu{*kF|MU+Uh-2M46RJ<-ZA$NdcVPM;EXUAw#YqdMKs z5;9#)G&~*sF`=+1-p=v@*>^z7_UD}>2i<+!cmS%pH_~gQZc#DYV$%5jLz#mi>Cqee zQO~b<8*URwA$eD(91Iy@=8bi49-$o=7?~>X%oig|4D%H(k+4zE9A|Y;LqYHU{fQ5V z3j2U7RM@|QTIyZulHO$0hV^k;9*{7$ByUjr^aPmlOYA#!%Mon7om%mn=#JKg{gaS^ zcSujzG4~;o#6PI(QjPxHQ)Y(LecsKx_2{?>^y??R%02fdE$eeY84KX1m4bmm6Ok0c z=EV2ppskYnIQ4=_7+cTphd1>6C`n>+DPBzTZPGZ=;G6DPiiMAyoYy|v?7NUj0?{hU z3&<&8Ui|{qYe;QCTWqvzdiyogKg39B_#S`J199M=_rr`HnO4XM$aEH;ZNe0M_BIL#Wnnzv?&ybBi9lRDD3tgT5NwG2`FORY~lfnc$yM%&1=t}J^iqq&KGHU?o>)( z`l-yRYhaWEjk-dzT48THo)S(`p^y_Y54}) zFr`{jRjv4WP7uMK-6@18V{Qq*KR)41t-1(NzKuO961X_^&!2+3JXx$j9{s0t1hNjH z|Bwg#8o;nYrMH6Ef&m>VVETziApVE*;>LYLJhV`?NF*Ic8S@_0 zfk23EH`6*kCUO+?B>?;qlIa1Cr(_L&ID)SDIj~&b2fZcBp6Te?%?pRqOdCj^=^_fy zG`o2E727h3I{+0|;Eps%RVbyLfTs{u*+rXH?5(FGPM2Q%EFQ=E*#EYOsPjwPas@IK z;{txnDqtRxDPVq5etvn?BrVl7L-|atBjq4J%Y~pGr{a?mmyCNCeS2Kz{vb0nT;N@M zn_!N?CF98z5Gr;@sQIZG(Bvn968?FuqhOaVyBLBcceaG}Mx zW5}u?>6Oln1s+Xd_Zc$rw%5293GLD%wDjfIv-pYB1bAA=d(dLK`;`~Q$*q+Ke>%zV zPOr_fCq_eDD^g+q8-r`GM+pOf@~@u}*P@A_c5vbFuJ7w^LY#I@9LL+YKw;ObTvF{l zd!7%QuaXfr0M{HYtIg{xcMm=fbJ5Oxu=lvfJV3o+7U+k>=j)F-^@fYY1g|Byd0UeKya*NtB0}zG(JkL-MV`#zOb0V zD|8n0&id67E0)cjYNB2@DtVfEiVwsRs#t#LeK`L2{Yh2$3(U)r17FR%jjzXu7pRVc zM(sHOyl@I`z=j%f(47?4@Y}a!TLm{>sJ&{^)Fu{bA4Gjij*WWq`t=(t3^$hBemqTc z_{J98V3gMT(J0N?R+IBW!~Ws!7vw#-&Z2E?yMAe*T;G#gfZlQqK4f&&uAVCm@rxQ# zYrU%O7vb2~{ViA`L~O0Ed(ah(HPvru%fWURx%AckNM;C8GBw4c-)^xF5DG6Vi9EaO z&)1OdomJWP!pf{9$rc)p;WaW5{aqk(*RU?q9`q*)#$V>z8&H0o^j*3&xt-XMjh^VP zrJwV)ZR%i|?84XNrO-a71DCKdf!j~-#^B5*xn;Hn4|1b|{Jf1_D`$V%z)GMll!Ke% zY5{s}oFy?mGQyTeC@NIq5-@-)?V!={i^m!>6JGM{$lN-`WM5QMRNtAVL;qWZJz0E- zVY!0y744<9ym{YVA4jo3clW42M~zbgxdA4LY{aRLCd4c8j0c+ zi<9DUP%&Py*lh3u`Z3&HvP}vy9+wnf{r#m}XSt0Fhu`o{dL5-oeWwo}r@t_E{aXbG z+?Yt@=IU?fQy6@wj>+Y>_m$!PHsFlSHwx~&C^JP%FW{auPv5E~p*@t6QgxDSZ()C^ zf;qYG-ns)?=C1I!JuvyBb!^M>bhCLwwCmjJ-URA2%YK4n3M&~WOMip)kb%QLl_)HK z42$6)^sCCSZI*#7X$(h6Z;#Be2fSDuatwyV{@#!of+zC@_}_jvQJV+% zGY%b)nqKQDZA)$V1t?{(lbQ!TxgP0;H*}n>Ttbumk9V^>gM2ui(YN1odUcx~-Zt;l zjj0I0S1V4ouq`!Tzyuz24)#uP*>c2lJ@boc=eAo7<(}g4#vWN==@GMUK!NJj(d#?j zuGh<6kA6ITd>R6gA+|XG5Rc`7Go`m^%i8m)S)BzkAG`U3G(0F&D%S2V@);X{H|C88 z=x!TeG_|Yhf%_7ozPMO}(&_JOTM65Kx^&2bwk=r?TFIz$kvw(K!=0xfF+?tU% zi+9s&q=2;sJ%X^gN>DM~PN-3*3GUD2F|1pYOik+JkvLZJFz9RK`9TS!j~Fh2P^St= zbiR0WO4kUtG2A3Jr%>t(zfJKH%<|V(;DL$`wS5H=fabqJfx8i|_B!vET)&v93JaTe z`n_7DH|xOzm@ZCkn!YSav*Hfs9e%MFb2S2i;2B}$eO`epUW@~XjEFrla1nkT7c{3!2Zp1aM~7!*wTFVNsGqukmQ2;EjP^*Eq)&?lB4??z+H4{^!Eo#X zGfVih!bULOp@mAyfg^1#UfNl_G*7MGzkrb=_U~$M-dh-%=x^Z#9biy9zxhFLo&Wc^ z)O6$$+WOxxFy=Y%+EI@96|w%iA|#9Ugys3`kSkvVxV82xs^0dPJAQ@{vYM{F5I8=!&iWK?qy4^N?BHFMRQ zOCN4NAErRoF`SFkpFIC)jsUCl~>($tVgYe4#G3^8Gm1*Zj` zFvT}jd`>X7dKUa>b+ zEal+<^tC`jz{fJs6S400s-hRFeiW##VAUl=XzdWG17){+k#&{8k3X)=ZpmIu9Mmqh zmk8OO*84}K!_2p^u~{}%5o?S(`%ZTZtP=QEu9EZ`zZ35vJJp-ygl5CbuZoCNS6r=f zm$<5@0%nO^!mw&O4;9iJfIC6|xDV6ea81{YMD&RYb(oGh7PfBY&IS8^TbA_1()(iF zZi)cg1BPKS>-8oLCWWzw~wF8ymuW+g2nn3^W z{{-)0=ATz)&YIs07VS|Ty_x8e%2Ho<^5RFm-Db_k8OST@QzU>MmG=~0PJm-jwiDJr zR>4wpDe}beNHtH@wz!g@4G!JRR|sFYXq3w?Bep{0d*7Hyol)BT`$EN_2T-bS+w*Nr zl6EKov=w&gwLD0!*MH5O}(u7pw8#mQwZ= z{e3^f-Q(NBoZQs9=Px>x_WgGJv_OB{w=jYS8;N6v&R*SDr@t`+g1>pxO8`uzvX40Y z0g8mMihC~DT0x*A9wDVa_+vi&g5$sMOW~xP5k6C|Jh25?`PEC6Z#BMe6jSZR*1y2*OP_7$f#yg^UyHq#|#!KqSsUqk|{yQ^us271+2Bd1u3{cPGbVp zg2rAmdOT0MPvWtc7r^oaac1JDhySH+)^LSz0zA*x)uhlV@HXi>=k|_gWJT_H)T_!(Nvw(9+~+=@n^ZeK3;uXu+B;@2SSfx1O#Y z6$W2$&fvdL?B{!VAWPGXg=vltliyY;uvWhpy;;1(=@-f*THuG+23>)VrU zCnoyhWcS3m8W^+ZzT21qzDr3o1rtv_JmQSbF}SD3|P!P3JGSGL|24=^SWR4 zAz|x|Y_c3%Jy1E}Z!sTznN2vO6`bmp0{B8OE3`pAG7r8Sp{m+BY$xMr{x-edrQs85 z8|CnXQBmyoZVEttKL6WR`~L!N%m_r6Gm>}w@^09X^4-xcsC=9MB||nZ{mYHPH8I=4 z4;dz`sdFCRtMFg-pD)}q;#EPuGqDZ>pvnW)vRo)= zWzT1l)fe>TVLG$4KbmD73HLZ9M6tz#p!d0kFm<{9-BwyKY#VVJ+cV}_Ln!_I&SYTt z|a%H;+ofvWug|=QwDAS&(d5;E z+eYV6KPdJKj!Jw@G_)Xk%hs}721>`V46Dw?WkCwV+8!>UMQgSHLE^_VO#4-YQh5=# z8eo|%L42-SG~_ByUNr9b9MB$O3)bvoAH|OEC-!A~|(|v?xHZukeu4SKm3>n+UyT@$S=*pmXsy9^i;I)7bh92Z;ZsxmSM2qg8 zDHtx!yjSL$qFm4vgrDFexv)T{?W{7sdXRBs7J=Vl%g=KS+Pgh|^I+$h^ZwULCNVuc zJ8x^sUUKa{xwxD8X<_F>0G3<#O>f>KLX^G}?I*77V20pO@~@5EJNpGAZXmV!SBR9m zM{&@au{9xF0V;9lt@!r+mhK7|CkUv3v#hLET$9Y0JQivUrvn+KUf+|;pzNGq4sdb$ z&U7L|>TK8R+Z4Tyo>Thm#3Tth+K9gXxz(%@w06{Gl!}nOTk}Z9?vR{OksBr5la6p7 zr$~v1KOL>09U7wy0x}7JAUDrFaiz!5?iJ!PjH2I|D0wi4dmeo|CjkWNLN#5{#M`Yz z?XmGAt#9b#7a`{;6&DEaM z8&x;=f`n!fOhZ1+)Cx>2I*Q)ea1S?s;99iI!1uUG~ z7Afy_8n#If%#W{N^bdCsO24Wb=Gw-CP8pc}DB zZKpLVY}3KVfuE$e3q%L%ONQ)a-sIutuBQ@{7JvJHtlHzU7kg_!!`o>N$w-=Kbr^a^ zT>3_FH7jND07JQ(1Rot4H1g0EzAwa7Khi1{cZnH130VT!@1M*^i5nzMmE9?()UW$3 zyR<-#aOf9N*jpOQvDLQVG^3qsk(G2E_#jB{<$rYIc`_lTl3u8XyW^Jd7$3lGrB;u0 zb^BSBU_JoI{{P$>HSIF}u_MSx0Hm(Es=FApJ<_uxn)&YT8~ zpCgVJ&M<&%lh?HB6qgguNCJs@_m9I>eoRlhOtYoDJqaq2#0uE|dD8cPVKH zAz6iQeBMCLcsfFN_b`RzJty$v-~$5UU(BG8zeyCCKc!r1zYb&RB5}%saenDT8e}@( z0BF&PVjaICNt6KQ1^f}QnEUD7R$Gg}q7y)wDx-ANU(Vtv{XKwEgAXVZYf!y^E>u9-}lGvKjN$AeVyAOSw*5q`T-#bp(5hV|5FzZLjtsxVekecs%LC2 zB452nwD^f`ewW!Z``_tLHyarsF2w3Sv1)(!8@}+t&Pi`@Z-E|NJ(v&jbGJTa*;0q; zRiAwKbuGsIe)_VLeI%eQOFJnaY?)hd<)Ywyl0V%6;`5)@C2iDw#GLr2*LNJ5o08f6W+UB&Nv&K`k&4&-RBlUXnoN$o89$ibZ(P_sNjDuj{l)a&e@e2GZvL>Gyk zDCZfy5^hiD0^?t>!_)A}4R6JfEjf?y#$qvt2Y829YazqgR-@bIo}PyZ;-p^h2~uY^ z!vB^jwRD&zF8I=(eZ;}FDfV4{$+E-iQ$ojw^9ao8lZd;CG7-41ih%ohv`IwngF$y9 zhm~VrVMe`Xng|SRGNGcXZ)W-quTAYw?eHHVjrUQoO|;?dzNHHu z)&#?xmzU$#?3GZ=;ys}ZmAX9lK1ss@O4TAi=!%luQ$*~wcQ+{VCimJ|e)jdUvC!a( zy=yC;{NlU?i#hO{;1eTc<(gunN9RrEKqWl5??KCFI`PPvEKW3Mx!mdi+-R`lgUMTd zP+gi9U^Md0)-imgl}jE$WPD@scYASsYE<2|Lzl%{xR(+@c$HZH;nyRW<#3HniG%Cm z%YdR>SSy(S2KNVS@|e$}p?^_2P=LFsZh_lMlqmlD0A|%3r!f=hmn)borpPTJJ+ z83yzY$+wW&Mn$T|L!-k+6O9q~^oab*I@ zLSnMn2J{^Z<=!=_ z(YAMJQjz#=i&QhZ=>P9}ACr0{50_mHr^3lDRU6?;26HlCX#o=>N+vn^J2I>~zGKjn zI{uBNV95WrAQjAVp}nm#2krfRQc@a*8R#}RG3=n_`>OlBZ$j0t>4#Bg4vi4FnO>5O zB1ML0m;dY);FbV^{4@r$yLsiiRRLTxMaMIc*RK4|wQ@1> zo=6+#suY6WD#>3g(*G!`I_3bq(Ak-x(MN5DxjI2vAYSO@=Hoim=lgpEdJ8v*@EEn* z$%{^BW`Xqmqo+aQ(YW8!lru6WwV)bwb%G*atIXvPZU$>5hm!q(M%A zm5)iXC}HQwKX4G;-(d3P{Rphp{+q*%)k56_TfOAbwjppS8?8*L*0z%r(d35Y^_<0O z8XJ3r&qDv&Epdpf-TqbBL$9>0QQ7 zh#`-*c~9B^5RwSEtu+BqEa__^+}s6i<&&sr`K6;*(rGvT%(4SjWm~yUw?8lC3ovgk z0KMacp8+A$zq|$MTfmoqrol@aTq%3G9V*i&vOLp(mgr0I$XHjqM$LGChBeybgC2R3 zdNbw|$dBd2K;9az?+av>&du>7Ey2s{u-h=)yS$@sBZ09-b9%ye9<-k`ff#bM04 zHJ(&=t{4-#9~j|!n_bE)L*=O_LSVHGjJW=O`V6!Z1FYLXnsHmgPYLu$q3|}M9N%jK zC$km;0*IF<8DX|nbj+K!BMh<6xujNovH4_kG+_1vS`Ap)L@#t2@aNI8^_O@intBh* z077QS>J7|Yp4T&+xo(D%wIu-X@P;JX&WWH;ni&jil>vH-FfGSBhjt)=1=lW5`r#Zy z;@;_fRDZe2*yVSVt#$Nv*3o7`*c=9zD))UL8hAYNN7P3-33#P7%BaeisRuSS$zDQU zw6G&H_+d7Ms{n9cS%1pRph$!wi-KLueWzWH#+*`Jf995afZhCeMnK-r*5jH-4Y;Y~ zP$7NOslv8Z^m(<8@G|%28sRN4VgP9lj8M>U81~E5-ndXf;JBBWSk2R%-&y3hKHY+lb%}3XeA+R4n#mr*FU>4buO0&s@={Po5=Ata zzsfo^Uk0~H!A|%Ozvk@{b0wxG)oa9a42Z9XvF ztr0zg10!SU-MV>2Ik&0U)PZjDr02(#`I5iu+gz!5SYr$0ot-4!lqg)({|_Vq0)eIW zW=!6Ls4%CH^*+i=K{msBUy5feGd;Uh(gOgj3p9h8AT>j>FJ1-GMJd`pi;5FEeEtSm zbanu%wQkVnzCdOkR!%SgP@BY01|$e87hS2hYSpt}Xlt_bKepXL<>hgD0Ki&)?q@WZ z2h9YTIk;pfgOE@EClY8}6f~R)LLk6{AlzAs8n3PEztlY}LoV?_(%MP40dJI5V)mHW z#f6%S0!dtz+S$%!m{qb^SB19a5TfLYQ0^iO2KjLJ>zl-N1b z@x*7w(SeT1huU<;x8lQ5v79neycoZRHv{x7yu9ng+3tJFifCjn>WU{m~oL-xoYW0-;5+Pv4$pHImg}$?VY5vaVXL1R}G`;?Q z@lg4|TPPBeX;8hqjxRV2uWH@sF)w%15-O9p|9>@)HL`yZKtq|H^6&Tssc@HE0yDv6 zV2WqdaX!`50N9Xku~!ra@cAt2rvmi!ek5*VwQXk2qdi-Aic5OONs?90)e^i zAdhT;UUP9r+C5dW>C`Gi6NJY9=?L&90kg$Gpi5%MUjOLB%|D^#FxH64Tb5dt@GHg) z<#Hla^HdM7;)0&Hc4#Ia8q^#z_QhgTtM2=p-{MbH6f9GTp!TyYqAMl6Tlpr}(%EC) zp2z&)+JQO_+MMn?lS^MAql|bc@pLkiGeV< z4;2E4lhm+I+g*DTByC+JanC9mLBd#4V0{1UdgId1=AOYTXv~!Xf+)0Q3kimTQkjMz zX*$*R`V9bHkdy9g^Z`v*F`{5;7$ltWz-*~;M&kEO<#p6Y;4{Q?S2Ih3tJEZxBUcVYy#8v6n$iIWYzV&q#`8W`)M=5FyQ|& z5DD|<|M4gPJJ0@qOfG$UcYXM!gZtZVtG7Wr)PS|) zpHQ(~eG~)?!)9?b(H|dMQo96768@C{ViE$$_L1rhn7IoEOOt$%LMs0hMD4)7032$6 zIHd#>Um8+2OU)d*5BndmFAW)j-cuzsjsSN5(@Sm)@|1)J6Og?>4+aibw?O~>Lv1DS z#&(yecVKfVN+97Sv-r3+OpDSl;H{Uf;A>cwg9qqIJxp2!%m7zh%5dQ3*7+caUqS*( zEjLcS>-%NzXn#ISPP^b$Pls4zOh$~e7*a8iR&y=mZuNHK>YO5xJm}8<4VRo4BZonW za@ou48=egJ|BRPes3%D7ePo=8WJ&?Nu@gFD+`q;t_-`H6n1&=#KWRq=VqHZ%8$8@K z;jXW#P2SO*8j<{eaz{4;0PWAh`r}KTXTFTtBVIv_#^d9;E|!7ohTY%vIwf}fZKSz- z1{{{0d}rjXs&5{27=KXm^*@)v(@&KgjmvdmM^FRWjp~Sazk*SlX!w``&&jYizu-*S z*HL1kx&KU2wP6#8%wv+h0juktKQ!`8qoxZc+TUbPT)y(p^`8**epYPl6lljXE8IRe$(At8!AO&Q z@jt)5m!m42_>aL!z&WNAQeJEC?Y>-AgfwYOzEkZySHss_YfBe+_UY=A1x6MkX2yR{ z4*3UH83%3UBbMMG4Ey@gux&ix~Z8O`+Tf%y%5&1*aN!w+Gb2o~T==1e|XCAYp zVTZu8+iQ7idQ?O#Gk43XW_Qz!og6plRFkhs|tRUi*Is1oJpsS{Em_S>~?Y-6_JaS zM!0E~yDg)dDS%VOz2kpxj?%;Ot6o9dff9FLNfml?sRZ4M-DXpH4!k~8fv@Ro)JC*^ z9F(-sWBQlzUqna$Nie?Po=4TjD7%X1B`|zW*2Wm+)rA8oc>c@w{;X-V7IQHG!{eNd zZlIP8ffyoCGEveg&8&(fQ9$Po*qnO#^~6Vuc5;yujRl_C|LKjYk7+0laz~!rT2pwr z?m%Nwfeh-M#zQ^S)!NzY%qnk3%-zU-#a7aL?z`AYt$O)Ofm}fp)VfIx7IJA^%3uMP z5~h(}YrkvTIa-@F417$-E%ozbu@NpXJ>x`DSvMyqq!eiLSA?{61P^p+SP=O}!;5Xg9pGytU?(In-t!+WL4;kpH$AA61Yrdw z21n=~N-^^995=;OCPtha3%(Yd8+DP=KabcCqdfaY^!xCt&uFPKi)MdJcHfxkekD_S z=Jbh2OL^iie!2p)s3yfZ!gb@1vfSu&z%}l&P<-DvLo-g>ffyZ>Gr5_r0W1)}b~smDz9wp>vVhq%0EB3y^- zIL;+;AA#Zl$~K}u{si8;5(C#8=W;zP_pwai12Shh%LufV#`rU<9HsO}Lb4pA$z^N2 z|GdVHVL9fT5LHD1Dl3|1Wpiu*l?BZHh?>VyHWFb4dl3(uKE#zZU9J;36u^nOt8jv& zLjwTg_ws*jLxfFtd&eFbmhCSafs{ZkvQ!kdUMO~_wzZurHL-S?d@rHkkzd>2gcrT* zPwjNT^n{8kA|m8sVD0Tg!OT0cqKw|IAw}g@@{|S!aN4eDVEZ&vs5q%82Mhsre3d9EJPeIXt~n>?i!&@ z=||Vy)*&$pGb|_tUTrys5w3{wUNTC&G?DaQ_wVnc_cfb|Z5uk7O1PN-eWE}#pd0Be z1P&Gsa5_eh`unHRd6#-B?Nf#o2*UhpOci8IQ>_~K`HZ`5ThA?fR`_h(b)GVAfwVP{ zF#yw5P>a}&+$$GQ0?tuDQv2SbDN3+9uc_<_V?0jM2vAObuKgySKQq7*%eh_MhHRty zH%$u@V>*8}O+??Y(i*=QZTmN)b8p_@$jHFdS*2Bqetdz(Q`hBq&1Kbou+QZKrGd2C z$r`i--YSLAwqNTWj^{#JzCwFE%2Vr6_EXg!^Hpk^o#^&cbD>}28(w}BpAz`-tg27- zH6Z6*^{WAw4uFD`AUvcO9T!&Qzna?{Pxo$DyLatLwum{0c2SbYr4)l*)1bBy;^%oZHmU7&s=WF~EB&4Jf*ilVZ&lnOAA+_Qwd(G29frqDa z(yo9OBbZw(#fh6|Dme0FC{1;g)nG!JlOPZLbfoBsG#*fg7dhr7eG*maqbiHiy?!QK z?V#6}OPuIlyekKGIW<9o<;MZ%#>;?*WyerluR$p;F7xReb8%pZu2^HKjH5=nlm)*6 z-N`(htmblun4Sz0`jMAH9RqMtz{maz0EB{q)uYpnjy3>e@}7SYPwzinbMjXong;FS zdAC*r+a{;6dv;9TI9^d#Wy==zz{x?l1URbkerZbX(JP><1jyq}M@^7d@(`kJ1H~{@fV&-&rifxclKvQv6UpeWTCV!iJizKl#>LRr{p zjdRDSo}2w7m03F=WlU{ppB2%d5iHJbQD)$Jbr%mM&Oze(5`Lwldbt#kl@wP4~~Yyn1R z>hZdm5q6s>GjwhMA6$lZwPj%v?w!ngzTqD!rCt$5E_y}U_NKR|bZo<=q_0e|bQTG19SP-mpPKH9^gj#~oG#U@iJycEOk7@cps^ zNKYa-B>vlFy+~*l6lfwsHMtBj9FZ;8{A;QmJ536xYOq+2(QOesg$09Z0{8#B%R_~g zA-6OGZp(VYCg2F>ioAgW*(tJe++1Sc96pIjpoML|#U033QNx=of+eCDgzJE8 zd}|TlZ2nPIokp9}UjFGw)j3(o%L!W)=MCrby#i*$M4ziaJs=xP!X)hZtq^+A<^nc)7p3Vv%%F8nwVAgdiB81|>G7tEN< z?hu!euUsKKsWCXvz~>n4OgLrsKNB_tnuY5Z8CD;vco}BY35ysXt1WQAYG^wGgJ~l9eK63`lmrzzFbM=xv)8!p zIq<+N2L8;RY+88+#g`19N4ZH>E*uq#Fw_GP5{Ow=xK*pX;yV1SoWi}ei#N=>SyfYI=U-nlkgMoL2q1)y_0~hZ$#X`@xjUvhdDVUQLG&!pjx=(1>p!r|Wc1prv^YeopXHn-RLf5XqV(6hlhBF-SM@!O4p(S-h;FD( zRJ24?6v%Dd(bG(N$({)k4Y_YNp$;5)Rv-! zbaxWLb?495Mw1*BNx0X0$EF*VP|XBD!|odRzt$WCRZ}hysR=A+EIJgRgueWG)A~Rm zwsd+`{g+)O;YvZ^i$#}1gZ(8DRy`wk$*x8~Xsx_erYa8^fvTTKtx*`0k}#I9dOtQh zvB_ZWCs(Y}`4m-MO~h$B3D&v|aPu z6TV2~wn$H1%dvW<*Ng(M!|lCT+=)fJ3pqvlg4cw9oLy6AOM*BictJb^@}eqD9W zIb@zU>zH&jYYpm^T6M9;q=~OrkEItqp-};EoBZ@u3RaSD8Pi~o7BBV18uniGlQh#y zw@<3?v&GN1Z4Z6^3-onrgN$V9p`WkU{6PT67}U&y{;d7Lxyh9Ea{p`-x@TS5@>^hK zAgTTiv&uW;HHgWF6?+k^G)Tk~UDc(+nk=>iMD-c?fQ@IIh9AGvgzFf_Ikzl4Su_y3 z>-&RaX{5CIE_e&i`w#1roEkKpnI_ulckmu^Jc-C5HBE12l@jDxz93P{TJ8*4PZxo~>&#}`j-?O*70P*M_Wsu# z>I;D%E+h^zx7E*nRj+X``g~5F-Fq_Ra<1MeJLf7bEV(f-&8@AD>cVjOY<+-}Gmnwp z0uUU%f&Y%j zMC>Ui_j{!=19V<3uCKmrz@28lgG1(hziS}6tx#yXko1Nh{sMiG#KGzF8IP-HW#c5J zV}n+GF9=JA*`#rMkyEL@vAj%aBH6(kzGE8!bEwb6Yyg9$r3cujuKR)d{1pWbW6crH z$y7Z<&t;dKK;(h^VG4@VuAGolSPk+9C0C|L3B2u zM{jutyk49x_>z^&I#S&|GwePsxv!a+p^~Gxn%HJg5(;ra-imenF`-2eTIY9`AWS?9 z9uK0k5`LRxsQFinhxt>!C2Nq8I^mghFIC+Cc-;&hk2(4hw-G5x1$tfjmPd({YqPmU zs+0|6ApYbV=802kye{z7=vL@T%?Qv^%C(UIj7A0(| zVlBZw#`*Pe3uHC;5AgF5L)PHa<~FZa+X@E6vBOoKms0V#n||)>_(1*%LUbeG-J-!c zTwNZeLi_&P5PXagn%Sl>0_F_sFmxu#+oFQbe;kT64-KzPs#O+il`CQfOH_?~E>JvR zIcpr=tIJyjwnMZY`UV*P}i;BD(WcKO88 z7ypXz@cvx?v($Iu3PY{e*?gy11Gsv~8z~#0=kk+&C1gPO&q%2X$=qy48j|9M#`W#j zs}HB2_KwgTmo+sj`UX+qxCTtB*B+agF~Bt__cN6{i)kJWfobz}rgg*pBcd z=wl%xX1-lN{tmuvKi6wLNw7_R9#{AV z3Q%KU%HrF2FvG&0H7hvQHpjFIu;07)_FpkTCceG$?o))6R|dkbPlzA3C)7`S(%Y-o ze=9TaGfukYo?83%--#VH9ulCSF?yUeK4Ng(dZ~{ZI7YRXa~v(=+M=qr8p>STYUx2R zynSLV$a;}z=NefJga51VMg8fIXLYO~Von?DKRunBd%ztAFay~2XBSknAF1%h7An&d zL_B?lBI|~NNUW!10lexpk=sqL98FbM&7~L`9j&jDF58{f$c9u2w2J=zPliMbOq$Q} zvfcu?f8=+<9JJ3JXY^4j#^j-KhlUGGVBkQe-e%;y-jid@FLw52wF+$%Ny03ees4ru z*X8$SvKg@fun>f{YcV4U zI{r&GfS|G$S46D>oV#mJy8?jy&fBF9K)N=WSuf_!@EpY5)mKz-qkWV6h9+xLRfwC^f+h~|BiotLEUL+ z@$2rzSct6DU*OV_f_6VV$$g3z)*8Ed_I;Px^PQigf8qNq{{NAww;(S;YN5pQ|niJ?u%>==WuG@4mM84{qA@ZAwHgCP*(>z+D>h{{nR6^ zkx(q*qeQ^(v2q)8Z*LO#n4E-v`7SWJb$zt`{tv+ zMH%syo7WH8tm;W=m4ZyRvo44W-MSj^dz6ZUtP$EY0;wBfp(bt8YWhCweI>5r@A0!x z6s{dm=uYQSr^=x2g_u6OiYUChH8RnsP~{Br)8E-th8@Abj??u?Ih#c2I9(c3??Z6G zC#)z_S<48{ZYIZoVx}>g||-aX4jJ zho-iyX!Ti2ZYXzJf!%7xD?NPL3%6DlCWo(mwB_oRB|@U20chfejdj-eW7iGk=FstL ze>0Xk6awow8tpw7k}x}k<$3O9uH}O6Bn5Hag}UN)5ErUtTn}bQ1F@A&WP{Z4YTWF) zY`pzAt^4ev>=IyC?k-oH_x_5MNnc{ecI>g8B0L zy&nV;#+pDe#k4@yf==Pfckvs)!H6yH>Xily^?#?$&g#;&*)lAh5V}@3TalA$P?aiM z_To@iPXdri-;Ua#%Wk(YQA_n}V`ZsN#>Z#^js_q4a`@QT9f}0(hS3La>iN9HHiC&f zT;9J8-RXKr9-0u~n7{;6p~Z_*)(!f1b$q~k2bcZekJbRZPDcq>{P6NJqmwY={+>0Y z86mi|O>d{t2o3_MqP7MzVTf|AE~B%0o2W0)F<*y7pRX(55a;jo7N6+V&-|$nrhx z&o`F7P2x3*KxC4JIuK1qF5i%}f%L^3J}C>mEk-ueDE92=@dHrJfgK4Lj^}lo2P0jW zUCUy?86}xDE206!vm1Q~-rO79MF21W!^V=@p1aaxAm;7VnA27mZixg{8%9 z2z4inbiB%`Rj~Y8f%@s!K+Jp<+-O?H(AplQB$DNCPW-ExkT7FI*M*4Hda&%&f;3wg zg|@HHZL9^nIX?Gul!I;6u;6$uKtbu~Y=%g(oi5FjTv!*c|x%rew zfCTNv3$;}_#`kOD-7uX~puUU09M4}xr^lZ+7;p0=8}aMw>2jq$VG)LF*zLA6iE+2kR>lhcw*Vn|tou~j{%9OrYtE+o=nz)&mH-+Ocu*CD>_BK zaF{yUf3KV2XnY6neZO5gj`K(zPp$JTjWTo%6@|nCpfUOixSfOF=UA{L8fs^FU@Z_7 zxmH*5h=xeuBcada>$U;HmFt*h+lJbOqf^re1v$=Y8~fxjMPrtcjfvia>31Y^rHu&m zM4%ZcuSH;v5aJB*MCtOfmC+ONaTdAh{M-7dtGn!WufNq9aTX_@gZh9>jsbL)m}3ug zs?jf>O88WsWe0nk3*PUjUZHw+#?f!LynglaClRpoJp+&Y{(5iy#~3kc%u!qw#T&^s7!@Tod4zmuR_`;4)tv7Y!F`X(V)G%aMs1 zoL0wja4w2V%Tvpm3tH04pmbr!!9=dG4;tdBk2QN{4^FBFV6Lk18G(Y!KR4@AgKRYI zCqzN%m{wyW3IPmYRLM3f@Y!fy8Fv61B0EWq*YD+G9zb*&sJ5HAn$K?)E(3_>m4E*n zlujF(`eFYlY{lJyTB+A(vUE5%`BC@|)T6L74WkX}0<;J9GwH^ZBzsoc%&-L;vQWq{ zExPGCggwrmYICg|pj+-f{IH%3$6}Wbm{|S_IAmLXa@blxYx31+$36RvLRVWzY=;(` zAzNc&S(Axam_60Y$}}l2xPrwrFa6$J<=@Kt>*Me^i9%gkv%n{`6!5zT*?MDlYn+E$ zqF(rprYAT}jKTbnr(~jiN)c2p#KaYfG#Jr%hvwHmajZsVke9%5|A}UP1WDwXwyX7) zPn74zaL4`N)clyTGN=}Ef!mgBQUWQ(zoo5>Go}}_f3vpmH+P>BrHkL$W37169x8{& zTPg|q00NXI0a9R|*#M{cS#iC#_$|0`LETQm_%O=`FeA#GeW(LmR`|J|VrK}uW?s4n z142=}+D<}D-7lfMed1gcDcEmNQWF?amGSic$mrXY0~QH!^Xo{%$bKeKH-;A^evlj| zVmldKEFNx-6ROqcu`_7dv`R%KZNRPzUeoQ)k_hhQ(=U7S1a1~dl};@_z>zAD$`z!o zC81%d_hWPLr^O3Ov5P#1j+#?kI0ic-eaFPcqK*-=J*T7V<@z|7GsFDQzWk@lcGm)+#SRrJIy7tqwg?e3Npsja zVC_c@n2Uqrr0GU^&K?0!#?l3N&NAaDqRAx@Ytx4fB$rm&A>3PS6K zSWVbejNjS&({klUL1P1aXiz=m{6iW_f3&n^ZRF-DG!kq+*w<*2ZO8_ z(tiEXD}LFaq=?&dr_|*aOUSL&)y@p#a*G?+I(OyYu_iAerV}Ntkk>0R4Q8{k&0^n; z!xUA6jsaL`11`fvZz*li`NH7T2y0Lm?)ZvX30(xG!pFI}8-{A-^He5R>5AMr)CQka z(I)K7MZm+JH*TnV#nO#ihWWq!M4T$82N_fD&UwKudO{{~XQb$62Kn>Dq9m>Jk$xui z3D_F!*a2(x#!!^qiWZ@;ib@ya?rAebRjNjOjM1k~_vM9M^@WTn{!qW7;uxJZmpx$# zA9{z%nXC;Ty~#TM?Pjo9qAcsG-XX2-Qo${aGBj+vW62i{F(xyRLZ_0g1dF|TIaYreNC^3-AUUc!+vV8Sd`ykUBt-m5lZ|JMXMRsEu7wOknR28 zp{$)O&f;{5v#lsG0x8^miYs3vrz>*hC|H9WyCjzwYca1AR5YZGm9p3@nY*hHwOa>y zp4S!Jx7jA`(I*qaK2wEBHYpg)YIkp8wc%}B|OQ(s~CBw{oAY*wetyo6{9nQiz=7VCC9>-)%FE(C)0-iEv5xyox|*;$ViY znPf+R#9=* z-d2K9C6a#Wj%^JA=St@U)X31qptS&RY82;AC&?eus!CNye51HWysU!S3=vc-OA zrJ&Dw4e>%FdwA`z80Rq)zmiGNxd0&)VR)Z@Ihi3Tz4sNa(-!&#eG}-z6r%XzyOqj?izIxJA7^+eb{C2!yd2VE42=;9p^CoPibqdl zlovPdbYab(NBA&AxQrT$MftBpvSuproJGJeqb?_7pai{5ma?6YWv}CH( znh7*XQ*oSQUT*bkO#kj;RENJ%6Qs|FQYD?%#DFydH#JRQk&}oRAL`oJF6G>oJy)qPuD}fJwtEoQS@5e}u z)AWtXynYFrz@CXMJ~`}AxvN?CT1)2GlQR@fNuBu8(?OES?S-lF@Ul=v?UP=qw6cKi z3bB$;F6g+eR1^j~n*K zeC{e_c-J{42lEv#%Dg5Xh4~V+-ml(p+%fRT;l?~{d-Ky;7_E@QXXLCJUVkr!1}W?? z?;>@x6y=hPP?I1pmpzywwymb4+dCi zXw~G$D1W%P*c^Nl-iV^qkfUkaa}1+k_hj=FUsgYh&Fq@Ef3Jk9bipggcomXX_}og1 z`yj4qb6Vsw8%W^V5F83_&;df^$h#9I9~D9VN3rl`MFc1@4*Wa?%qUnxZUHr}DJC)4 zinrK5px)yBg8{gHK#<{7`EKt>9BX3I3;tEet<32SwO4{Q&tl3No8|5%)6Z(pC`)a* zBV4aH(2i$owq#s)%^Ry321JB+mhbj{j9(i|xwGMHdOC(*AbUoy*DUuH%}(VZ*1o*k&LqzO-3rnqP+)bv`Q>xv0Eq%)JLp z6Yoh77L#e^Li+}=mo8uCHu=)A865kJm}wD|`s3bTzQqu-D@g+`R7gVq)`?-xinXM; zA*GI<4Q}p}pb&kr=zf247ZbZ11N+s+6Dz1Xf-l)k$!9BSnOAQIQKHFjw*n7o#B@{{~d^lW?yJs^`SM}jc4JCSPsUx6|2&0R)hVj5mG@Np;7g!b;5zHIq z364B(8NC91lN$HcQX^%68=Mv!)(oOj>B%NG{7V~a7)VBNIoQRHNT7I9>o@(jr)oT_ zr%r2Tam6st5~ZtWJ|_UY0!yuIeKhy`>_cWTv8c)#F0@Vd`c;y!bxxElIs3bD@3Dd6 zR4UPf{wxlOH4d&s>XGTzmr{5fS;0n|xecAiTyE-@6%5wsmqx{QmjETLw}&iOeYI&{ zIAr~Xg+`0ni-l#fFU~D5C`^wc8xut{;`Ya##8NLu|522va!cVVflC+JFV5tgif}aZ&}@dzVYd+| zkHw0ceQH&x>^JoWcRVJ5--7tK;*T*+w?7Daq}MlKHjBBf7>cCMY!Tgew3pufH3(&9 z!5VERt?!khNJxs&cB!4z5sPD{t(81W#S!;|@6@F`!DcORB&+Y|X47PaA~h4ZKGY^^zN@pKJuI5a zb1xfySUk+uH0>B&+&&TJ7~=oiWT0ceXs0R!`YQ-fT=Dn&akEmVATejH~RQws#+{%#vOOvYjJ$j5Z%L~Y!KiOFw*xQS;}?sfgrbq--*kfVWIUUMlDUq>9TF*y zM)qN(o9pD7K{4Br$K}?lHMn6?6^~o*>WxQvL zTS}X3{vKz|%TwJu>$2GJ?d!pDf_T*Wi_?BL8-w;|3}^1eE-)iuI9_bS;ey(q3Z*V- zPZ(LeF9S+nuaMORP@wy(q!oNPXJGL6fU4Q_sX~6Y&mx+t~T+ zMgCEx+f0sDanf?PV{%8Aw8@wL=@N~39FsVAT@9p!J&7WCeBPBfh?HHsB;0|;&TJmB zuQX!G@um5^#?gq}G&xfgIQvco|4Q51x6_7I2?_Jbo7t*=mu`GC^JRV9!q8nh37{XD zSAE3l%x)rTUK3dPAMqvl+l*w=tON>=L#7q5W9%)qH-#{c3iDB(XHY8NxxIPyki-H; zEls-$VISIPinP%POSWP>Yg<@R6<0}tXAw&m5HPFm+*%5{+>vl^#KY0kg|-XKUx8wB zx92A=AhIW^KDy}%X`9y)NgyyT10@uITyO>05@G2t-3TCTxIhl=hv&;ZV;YkRFC6}u zV*5p1LR%4#ESnf#gRA7*JkW5{PlQUPrHywXEg37I&G%Y>qtu*VdAa$%rOk-dr`^Yc z6OSE$@Ifp>4;XCHgQ8N>Plur2nX%jPe{0;nx0aw&e(!+uyMf_}l}2e}TE*eC(_(6qSg4G|EQ4RU9+(s<8$--(iP@n3q3()j`#CI@ zYo?GB0YMexANIvS4a^sQ8TbiBReCw5rOP$u)Nv`bkT9Ly4yX<~3jVbf2A{Iq8XFk& z-QKgNxQJirUtAL!NX=%>=uSm@rhA^Vgv}GzJ)PqN%D#1OAA#`8@)y8N$2j-5TgDs5QzxwH zFTwSz%n({<1;UPogZE?S6Oxy06#5c38Qa=`zy1xX)XPT?GY*5!R9h5zgi9ABS&sr3 z;1qXk{deby7FvKcSgV95u74`J-umyrr&1j%GdXKqn7E=Tz+e5tN^T?ar%#o8FWGA> zA{KCopzKKKrqmMED^91lgE31?zf6<~#;|$wA5lOA<$00%KOX|DR$BqnSY&_#l-wm) zfgc|h;4ZZ`zXc4cyBp5EgtX%c={;3&RJ>dZa5!2WisSeKD}y~2dd$by_d2fPWg^V= z#L0_V?k7Q=118D6n`fqZ;LjmlzzG<*rPeF$^&}O(vPXI6ti;!bec6UgL4hNH?3@Wv z$bTv_OFsau>wX8?g1|3(4ZQ79ssm!y4zKuJr`OgoEif(QJN5C@1JV_zyV1jhn`J)G z>c~F>n3~%kvw=pWMMJ&A!Ns!F1)9XKD?DocKO2exu@TfM{ZoJOPqFE@&%kE&>^o@~ z_6m;g35`yZkq?mOJW8AHUM69p7y%28Z;n5@oNy`n@8cmc@F>Wdh)aHiVcJChCU9vr zOnbm&=sTk;L@&PvFzvS*KwZZS{GIN_moc0J3)aVv6j)=s&A1>o!@9sY*Y!9i7f2gr z=k)(L-rs}3THLpKQBN#?%hCG3Klaopgk!blp< zJ^bn1UA7(;HKybBy~Tb`NS(YY)#Jw^>6{dbVY~j+JRl0Q2>U3W?p3;!&P=Fx_~#RL zk|W+o>hzQa)@DZ-WH6p-fXco)$Q5ge*9U+?DBPwW$)g584b`7JEV{$` zhZi-e8F)k-v^#EY^j#|Er(flkM+R7QFgVtVaSb3$bhjemuAVMD(`|6)iJY9+gQ{d` zO_+L@M*4~46GmAn5+ZgWS|<$V?`mD|RaMOF$R>R#7{(q`+72^QTcdKliXlh;f1;a< z1dnfd+ccAyEZ?nhV+I3{{+3M&m@cVdvd19D;yEW(ql85xKdJV+MDj}4396`G{Vwr|NDxT-OI2Y#sj7W_XRUF}>QCJ<0d5L`HfM{TwN zVLY_4n&_Df>d7$8vTO5jrkPfo&{wRn&37J%4X(73uIxT)N6F)C++uYlwW-1i7s)`4 zo+(C_bPfC3z%GSt(sK3BTE95xP;hx{9N+vwf6Zw2mk)HR%FrHZYceJjhDM4~t!@pO z2W>B*Jtf}jask~rSfzwD%S;b4(ttF{p;p!uSXFFLLg=k0FyDv3GF;HF4eEL6Cow@- z5TZbnvp_@p$Xeg)9lKBXWiPmoPnQq$&Y|`cnQ^+qAdX8yTOU6k2iqt4@LU1A-j660 zR9_L%)bjiMtkIbRFT0YO$`sC2i_6+1ECgSI^AOjD(RG}WwphvMKp#g%6Zal9u8crfzlXj|V)h(TJ=$bd?<9lH{-mgEebr=i^o5pZWZEL7 zY}KQ+_3-W)TieMJj4jsCKzq=^2oWhjB6K&QY~l6DEn+2wC_YZ%+47$7l!#Q4N`2ML zdhK&5!-^{1lqy8)COV~ue0FedfU=~E z$4RX}`G6A#sE3$dw-xSlk1_m09w@W>we`IE!K!+5pYS#1NwZ z-lR*T7~W}{=V#_iq*SCr;kSt@k7~Q|DMP#Jk{LVCZ?`zU-J8i~^SJl^1(TqMXYoc! zN$S^apMKIGTo=i%MBZ&0*sndW9!`E?O1>w*GoW3xrJ0l_&Gjq4Gv z#eAbVy{`*hWE_%ucM|8_ha^_AaupG*Y0&A+1p}rzuO7+#Q~9?kZ&NOJJzIiEv$%wK zIsB$Vm|29=Gt zl+?hWdOK`o4`N8fsq^7=xkP27#unD!69{BLqwm>)*~<-w>3X~BEuU?&G@GR%``@8Y z`z+Umbib4asxC{yB08XBG#gsZHHMS-$EbpkRcY1m*W-pNlU6p*Bs%n!$Vm#ieAZ8FP7*Z{6@S5#mDa(2qQr?|<}^QUSHo!{2Tw_{$Za*9 zx!OVeTMD#X7h)=z$ly0M%g=>(;|<)qRz03nukQ3MOwSW;fKEtQt9SYME_vWxm(;oc z7t;pJGVUZj7kLnj7aD*)J%Ju4^^V(>l^duO;<6+*BHc?Ntxc_b$n~sLA6fNHEOrLcAD3v3+tWIG40R6U22jIs?R_C(Ti}@UA_P>_9L4MIAU(Rs- zl7?$cSQk!CFa6ci_kgeuP_UNGF-j7K;w;yc70#( zU7={LcCQ6{s!6YiZM7%M+b6+lA&%f1b(l@xzixd?q@(#qpSzGJpWz7FX{S`9E$6a1 zpB}cJl|4Jqsgtc%e4}^(8USM{aMAMWpz(e;1AA6LFVJ}WTS;y+tQj(Q2yVf*5h3aT zn=?5{Yw1?x`?l5@$k{u(h_(}4(I0!!wLEgFvNOozqr+0$K~{*qgc^?jMeN^9OiXNS zG0Sg#)f`Ls}MiJSIJ#QT%?y8Ad z)vg>7wuib>n&3V)RDE@{QYiQE=;$$(L-lb(YwH4KY0y|+47Tk_6kOVrMkfJR$~jyf z!H?;Ce1qqUVieTZm+jqKBo~u1mo+{6$fhw{x9=e1T~uz&bocdO2L7SE+PSxLbE$#GMjXjig8~@(s-siHpsFgKrwZ$YrdbbaueMScs|KL#euSi{`o!11fJ^~QMMVWI9RstF_KUbg zlKq%0HuzMOZ0DeTRn`mX_KayMsrVT~w{J={g=mPSow)4oFW$Mwo~h_GF2C1@-}rd9 zXNDsj`9cFSv}db2V;sJ)sy&%)KL3mx%1ARobqYe25A2`{-m7c{eSenzO{w@n_bW(M zsKl_V_N>yy>!UYF53Ahymp{My-g%Cb*GPTL`A7Yyd~*`@yKzPO?}#K8e*3Qe2L1e+ zhfC)(7?WY@vNL<@lMM^KnRJZYcRg#fb8~HNY%-F_=a%~R5_soi=B47}3ew+}9x>F! ztmZ0=tNgKx$4d-q0w*V!%75XLD^fR7Fl*se8{C8vN^CF4A^Z9z@ATQ3%{YAuquZ5l z0ob1ynYmoNR47eeW{fHrex|7+n=ZyJIXOLRE5TV_PvDzM2MO_k>ys{DXU*lI2(QgA zdlTG;-u4rbM{1(8Y<4WK4U1A+FDP`%GgMaTWgUt&o4d8-7O;?E9@`ytbq{wfdb_)` zHBz6Fx{_++$<^Lh$_HGG3!}v;@emH>f@eg;5*l9 zEl7${Jdh8|z>V5e#7vipO`4KLb5Uozc`**?S3#C5et3zYhys z=b+B>FKbW~KZmjlEO&JE^|#lE$GOnZpRT(+wta&S3N}6!5%Tp_x2<93nyIbh^**wY zq3OT(AKkyhsHhz3xii4XLdhHRyxDYlv3`xx6J?vQTJ?mFaa}H)eEsvN-8=U6Hc<>!rvOOK&T7(as8@L_7p#$SpwtEQ~|O zoH7>{X`^BgY<&i0#TzMdl_=d@H!+Opn+0-=Lk|N|*cb=1ACXX1?VM@~5;3aY9VxY? zJ$vaTKk2K?d*p<3ZZ*9ni@l}3Lxo4?o@|2Z@GUEG+tDgoMgT?pXq}EzG(y`T#H*ue zK^3?PMvpr|Li2>I}+N?NOIT8;IlFx25M7^3RuJ z;#!1P$2|5iZH!D|R{Vg6BO{4`q2+TtFbx zakg!?H8Rsg-$&IF_6kI(VRwO6OagAX*A$6d0zuc6%d=pKNhlO-7`f^yw)s>Y~ zJgW!$3yAV#!pdeXHi<=^xUxT~R!|{(m`r4Mn$B7H;q>C&S+UPveV{O1;a*Ht?CN)Xb3cbc=oSZ$;7rRgj>r`_$mjqs?J z!|yT!!l132#4e$Osu~YffBEv|AjVKqQZiRNe{pefe|rT!>x8B|hQV>(@Nrb+R=h|- z-zYA*t}eQ_{K81bxTr$|U*`HCLgRg4mVi7-!$k}KDzt?^iH_8jATk^pn-T2_vQ^XX z_N|#yuRkWzL4T1T0>*8^E1s^l2UJ-v1%X!ASGQmj4N2p8hu|7izvF1OK#siobq*)! zw#5(4m#N!_7g?#ws)y}z)Tth!mub|jGeXtoYHvm41gB4hDT8;stJoE3)*QXw-Yz^d z+^9;j$sg17YC=4;_cf$g+O_r*#N3+AgY4X@gFRyOUpg)Hq+9o9%(lmfsH0(z11=%g zCqAw}1g|N8H_9s_W>bq}$*-5~2L~tKEL|J@YsGRcaT3)gx}5&b4io;~L|Oe&3^$9s ziwc1k>}8^oy23C%Cfaq?hF)KU__rb-<3D%WNHBglUd^!BWz2DhSyiv5X>Of1Dw!dy zBVd}xEqQ->wV$QHfstxcA9;5L>6W=>EImh;B%85WOjDFvijzM30H)lF^t|MDgBQu{)Z>tdM&mn18Ude+K zK2G*~?DRn~*%2viE^v^B9?fRb-YVVLZLZ05AkZ0>Eaw+moz({{Pwg*?E26YGuj z>EzH8US*hW+(-cK6H3UFqhnlIerkRy=ci=;=&jopdCleXd#f7io-L8J zZ;F!du8bjVC09yXpe=F%|sZ5i3>~R1?)c6 zy+Wop)J=$JfpY${iL$jDf2}>IF)p_tHJ$3!*2?KUXeaD{&3%iN7r(o$&$Az7Po*h3 z2W9+zGr1&6w_}=>g~YirBDkDx|7Th-0WW=TyTG4t0>3uMzAQGjYekKd^+EsqQGc#kLrR08PMx#C=s=pZH$x#{MTN?27rZZT&d+q` z_olR;{@l2heCfRWBX?cgX~hI*P6?|3JuzOkA^ly#tGxx9ui#7Z&Z|Y# z&$6}r{8E{Wl0+-5W-r5)K0Bu@x7BAs%)Mv@#MNaPFQ10@XXR+K9WQu@_STmx?t}w) zu2T3-$IM@s1)+vblnd!%WNUpV=oQ-Om%v3_tKmyhi7n<4QQlJ9#7x2_uld{KGO4 z?FjNR!#VZT#bfC+jxata?t}&(>BT?B?z&%#w3I~Nvl<;nTozsRzHJi}#K7 zrhEY|j&CShGH1$hu`rDEfY7feP=vO+@rL zwRjK8#A5kTDYE=6t+#Qp%_wwI@x?5jsVK=S)8PQ=rCgNv+xcO?XH~p)b@{8Dija-! zwru+e)O%a!U&9wU)L;719DLRs3etL1Efz5H4v7`<^W#%DHHn)9B0>EvAG9R+9b@ol^n4c?s;Of);&=&Z!?R_Tfp0tJ_7r@)AhN(xQJ-76W^s93mE zV{z6TrQ6R%iYo}ML^y~)+^8D}z&ebaIpXC^(A-s-ynj5JLh`8gDDpJTyZ^{g+TYE| z&v+}!26UI6YN6d1H+XgD-TR=4<(W6wUVGg*{omC7=GM;KjkD6FSF4n6)43C)gP}jwE@D^zocP+9Xj|VI* zu&u(JGZt2b?)XH9)!4~gW^SN}wo-RVC)_ zt)x2&4=0)nZAL49INcQ*wEqz)yB5RdZL`RJCh)u62(m?@cO~Qp{xxU~9T^`+6Y-uraYUl_Ncd^udQV2RMihqirdJP(rX|&33cz z&YI>>Rn*k0B1Wgi4Hd6U%%J^!>>Jxi@O4AmZ!TJXFqBtEd$6+s+dFk52zf|!*&jwi zE^!~KasCvhZgG_%;#P1YCGCGC&RP|DXGy-v`MMJRIp=uq)8g$oa!2WTpiRwQ{|RU^ zH!jighJUOj6I}BQ%8{M?Tr%`=@(m^h0F;%~hTSA}5438g7?c8BFlaS~1BoPEXt0&9 z;7vzD!GnhF!@#508uw%O?Y$v-xT=ikkaZnkhIpJSTd+*HFjnmlxe!Q*5mYtl>Mb@c zRH=iA|B2kI3r1qq2s(S`Pb-|x{K@8I&rq@|r(OH(yrc_7q(Ic!ap^*>*!~NBPrMUSiv2TFK=RnkM=$@RMoR^muBV#D@zbGjlO4VMjxMy6PuHaId2dz(U z{98z|E0yR#z}I@fxehe%=QQTS0~4F&=6v5B5Z@99enO)KwKsoycYp>1cw=j>R800w z3$eWmtoJQ)^luoQWJQ5>L) zT^+tQ?bh@!s7*Fb8Oy^>deK}rgI)V@ukPeK=XjWpY=#U}GVk5F9ma6VvP-TSDlX)R z4s}Cu-FHIpiVuVrBirQjhmqEOOwAW0*{yA?eUL6cFc^)0=GB8`FU3|_B)qH3+oU}k zMjK9kAh?O*b`vEj_aq9;hj&ias(Lz540v)x$0Y}KEk6$YHCRM0`<_&ycXz?MHf2GV zJntku{yY6oI;ov9-_0v4i#>mZ|3$onAPfc*7T&ACxUCU+7-V~0zEvVphtKF=(^<6D zvCT`kIdhqr-D=n?{dk{tXE`Jg8fw}iF0kiJSsPBtoSq<-YTg+$Ka2%Upzmfy#(I#(~zh&I_*=a zfA*joIiF{+`7_JrQ2Q$(1xN)jgzBJ_w9LVZbZxq0KvDTGn>1|TKay?|$F4=NKtm^n z{C(vGdIBXruHhGdCI5zZ{!hF076_9Vj6`+|!y0 z$Dn+Tn?qYwLp`tE(NmbXN%{!7wRppr%)Id) zw}`jL*}2gb{(?_!E1%*FC)`UM39R)Bga5KY!Ac#U`{W-3)4iQ|U7odj3SO^bRonWs zp03n+vt&s@(3-I@TFzUOnJ$8S-B2p&OHkEMg{Ck3qNIfZjMXyELcOb0eQq|Lfycpz zj|BB-a-y~Zno{+8_}=(Z{Y7ivvKm>2cqv3BHvKs1Zv4*)z@(Xt?t!GJqLULjrRZN( z72qAtL_e|}hrqR;kiyB=XrR}ij6HLLjL1e+_y!a&OZqan0ac$+82Eftbg4qNf%+nf z56k4lO?fZuZ)uk&WuI%|fHg?cu`P`+t#4YJtNFY(ZV6ii>|aAZ)#U8tyi|7G7-hpx{%kTQ%( zQM+{#j%e3Iep06nm^v(!Qf7*;#FV8aktOnyi0@oklp^(8IX4jpQ?Hs!oX|@;5Lfg* zd^Yp^?Q=S)d}@gLa!^*Tv3uUfTulpyC??FumD4;B=L>dY)-P?ORYzoyJ~^+cgB$Gw z%hhR{nP+HPkn1i*eqL}8!J?$g3HKB&+zq3Z>R@k5We*|N4I)_Ey(=5f&b&bkna015@wzDv70!gqFnP#pj3I=%{V!#)*gM3Al}9*Eh?yea#6|r6MSot;`pPN+%*TvwZZW1tCgUrc2Jp;4MTyn#s%48cX4v? zNua`gKz~>5`!ANwOEt?}UwLXzvrY&jPvpWPmq#6gzd2p-NxxcV&bIq!?x!x^bk2h; zoq4tMJmNSQWzgO=5^woWq(0-40avigNLO;PHz`iY&A7&!I_X<zCDx z!A2p(1staL_IyP5v$0xxbs>sp_4xu|Q%@*22537@`zPF;bZ1&ZayFm^zKsDNEm(^n zySJvIM<7);L2JT?0*io#MfAP>uo2yBZMRCZE+-gAzW~0ylq@BT)I_|q)=7KqKYk?|zFeES zDexB!6UhJ9re0F~j$oT8v?W7bAPP;5K5`pe#e&Q5S}z2-@pmHsLVEAyRGNjZUbieo zOI6AgXj9D*3164WIKLf|Cwm7SrhCxkM@J2xr5&cYVW_fxk*Sy)LR$$Pf9$KC9J56- z8QyK9cf2h>#Bbe_I@p{yW!20#ZrQnGRQ!Eyv`BXUC1*bOK5PM`vuLurU=B*(^~x$C z;_!vFmGz**D7^aGx6X*pX__9z*2K(F2XatMzJA4%%JC~Yw^kNrn717_b2;6XU^S_Bw&0o*ZQ*Z1}-yux+{ z9;Ltm?gj$cD&y+Qs9Pjt%uffE?qm|q|2PRy8?4ksdt1~!JrICNxF6f~gfjtE*~d1y z1NMd(*Rz$Q9rluSW8NMUrC6W#26Q7uW5dTUwA$nNXpV<5QJQA~)|U1rG8@U!*=>uv z>65F)vVZ^ewMl)mFqfV%rNk1+UqwxcT%g~IZ*}6HMBkPtDd&;C?pDV#KrK&JCH71&As1u?Is&2-YF*jcy)ry2Q0JqzUt9VuWPI>a}Y>srSMU~+O>SlG> zUDxv%{o1ITS=K{k4(!HYK1WpjQ~8C_ZQrcBI)6w7Tb^j9CgFXq-o+Z!;C{)YVva&0 zGw)J^kJqT>(K6G9O+eELKa}Yjl$Mc^k!x&K{Ub(mNF_>vk!GFGkvpwZb39S0B3row z*;u*wR%C_(V{A&aJmzvDF6Fu!9d(Mn^7^zCqY>Kmqy6BRi{FKk8_;OQEYCxG%^O!jK;ZMpRXv5IWCD>ogG!{Bw9rnO~+NK#?07S zC)W1o?DLK#ycmvk{-vGrs=@AaESD(3tI2N7MbU?w`pbv&6Hhz!UJx`iAw>bd(~0zh zOZ6JYER`?hZxG$$9a}jleTeZUJG@rZ@!G|icL19FhU5DX1A5-}1?Yk|-7*lY3x{N_ zGU;Kx3ogE@YJKBT)ANf3e%78h9JxKEKX;)~jD8&UqVHX3*1<;!_ zP`rwwTqszrK9K{sK*yGh266)<;>2BA*cCr**xo_Tmi5v3F}Z4UGMX_?&dsfS$`(16 zq%*Tr^tzTj^(cvz*lB~$d>neHWfZS%zTh)zp?)4x}XnGcc6jYlx`$T`@QeD+$@`Yk4OiPmVB3V@;ujrl|Gj^2DQO0ScKajk$uk zUO58}>G_dLkfyrq#lWfYawJCr8XnjxS(1Nuki)*KAf z^#ltKjPR%+3^er9++TxZR|xI+QGKtjZLup-3k>htz>MgUXO-T}VwqZgah$8H zY0X?aML(})u;-q!3NMl#zgYEnEM9#RBQ=t~dp-BSYtUP-mG`#HP!3^=z&|PyyNWAv zCDLC!mMv2{Dhpa`IH-qCmBRaE8JlqVdC9p)e3LSb?(GX1h5;AK{aS@t{iu!ls1+8GP*Y7mm3#*0kV z;!lK87Uj9}Q|Yev2UH+4NGFe$NvG!F5`P{)r`>k`q_erlZnj#PH^lv7dbhmq#C(al za;YXq(kcn?=uI{H`XiM>gSMjm&I$c>RO=YT>0gYWKpVTx4BzsMAi>s-I+35ahdGZt z4nHaktkjy9?Tq9k#jx7T6Kp6S0l;h=LBk>mE&?9&YMXx&<~5ube{S zhu}eag|*OP5{$Ix-B5CaM(!jq6`GQSH6+)`?Y;vV$D{%|~bNIfC)5$T(H^b%zbQYFe^!$#& z7u!(x%De=kZ|hKkgi3a`vfSekyd=z42n*(pmiGY7KeqrYqTHR*4B4+4N)|YX@+{dm z9ZhA-V>q%dJT4m^x3zUOlp1sOPJzEz{faOSMje4`GP@LQKiQ&{5qr%ttlHmK)2=Pw z(sWI0c+>lse-wYwnBci4Ir;2}tBhse;J%t-*YuCrTue`{W_e4+EQg8yHYa7)-!EDX z4X`X`82MHD>xVUGag2HBA@#?K{cj z#!NZWM2XL%w|UlX&+oto-v#Wz)Z<9JeBwKIaGp*tg2JXKzBCgTKL7Sq@bS zx{aU8#p{ny^eJM)u3bqN&risLYVsPH{6*GzdnT1boT)foRwKW`P4fKk_e+geY^-XMMSvzOoC-yJJdlFqQS^Q)R56EI)^6;w4( za@yoGEnA-kIuy}&`|)Oj8ZP?IlEbdTD?YakI!Cy??t7Givbn$F7m za$iX%gVk3%Ut)oaj&dX4xJ-;!9hL(k3oFdwAX{!iZdFDUqBP}-B}jrh2@M~#P6>Ic z#n~zFB$JnGYmR=&0qnw*Kt9(D8pPWf4Y@~g-y?SxzuJ#`J$bGXss@xY+IHVqxKlT$ z|GEXZxrp+O-Omk{>I58u)6HXogf;wrclt87=gvsmPx8kEb=<2xL7G-@J!G+NTw5#V zpQpkl#f+$kscAVA1u%7C7bRx6l7;L2wFBMcD#?N7Fw^MDJc1Hn{_MchpYN}=d&?>S*35cTW}k}?cv+z?-*;I|+8G--e^c>cd zC8o(>%xN~TzhPFfgQ3u5=DfWGC-}3sDBy#w%>Rr&40q-GRz`Gxf}?8O{xx z_~C6cHZ?HZlpJ%uIn3*SF~5IQ(qrz; z7AI~!6Xnnf7w~pUIG3e~8W-=-u<3kD_L8OC@K=4} z7I8r;f1M#eV7*Ct$S~uynNEzets~t`5nhfl9Jzt55MTUc+_D$;p_r{K8I`u#K-*)% zv*rG)M5`4OZzHw((}Fac#&`vzMjHC7WI^`FgYbxK!otDpY}ZlTm1rcVlic>xS<)Gr zIM6S(sdONjd;62>jiCYZS2A;Xv|i}2l>@51vdX4e5kL=ZguH0KWKLtAYTNSobDb_t z^TeeV7bSWxHTJJfFt&kZT+zTzfO&ler0{%kH8d3d+MqropC=cpE>=&jL^b=P@=-R2 z<)`&@eJx^#>EyNdE}v$43Qlwlx9@-O2$p4d=Gxl#etiw(o7>#i4UPZ@j2tq^eeROr zr6&ft3kIjvdlv_+`VgNc&tiXlu5Ze3Q@=nX#s-oDi!uI+f{RL_OFNhqcrFd3`sKr= zuVf;w?I-FNF1RZd`yvHA%m5i_^+7R{PnHe!`zk`-x?0k^XFz{11G4mZDg*L)LLe|v zsr#cI&U%RsGN(KP1aJ3)K#OEe>Jl>Wy6wHcxwsH&w73i=9se!9q%jkj^C*bHeD{1a zaFtf4In1)2IKq8s`BC#tu5t66i0<7cEC;=6e#C69+f@nGiY+GMh1Zk?ng`m~0oTd= zxOEnSlCKnSHD%322tG$~>vA!xpgNe!5Z4@__7!`L2lUMV*=GPUTho!dMt-hTz#|0C zV^y(GvA(BcufRk6h&;BR%pK+Sdc%`8R6=<*?g#Axr7f1+BAqu_qS927bEDivwa}|De z>@rq#4OfIB@qv1HNjh0~60&5;r9W>=9~aeu45TkxwVx?Bdz=R*c7a_mfRN(_5F-(6 zX#6Xv`!*^sW&>`gCcB z+D12BsN0WEdx-Iz^KlMow=@pz?=E${n%-`WR4Y9+ea_YTWz zay`;PL|n6nLOhQovBE)Iv4@Bz^^xbd=fc7KOWES!KNbo%$o!I7_V^QJi=1+WUZJ-RlHIaj3N%|MLZ9!a|5; z(JY@K!~$&y<%a@-baYMnw@5rqz<3-n2yUpiz)4Id-;u14QbYnD@Lay3<2Y&wg zo2E3u$z=t{0T{4Zu2`P?0kHR%JT@FeLp^(0p^P6S!cJCIgdpZy7=Gr*zU}Vr%*?=q z_vC+z2TG^Ho+yt{V}!@dk2Xeb8XN~OmY^ske2@&X-QH^carD+fjge80>8ghp1=AiV zELU&ew;3#2b2o%F9w*uWRsV4+`cP+2WFt^=%(GrsUd{0C+}E`ZC;Z&Hv8xhKAlJ$J znDl7(gniDUdk_eeh1WUsY7QUmu?N4xI_)|9PR&S?bpT`!NtRP%?=9^wx2IjcTYjO|o`{GIFTGjc}$weuXXN%rlz%U_a^CdEp07Bo4JPBPG!k2)ps z@l|4?>fx_g&K@CbymlP|Y)-X&1VS|74MJ>avzw`FsM_-H0|({M{*NB4REIdBvkvwO zA`Igr&L;kD%U3_Ii#d6Ug^|BswGnqDy!8z}W+i{&irq1Smm4p4@OmmvF3AMITrPRs z_*WNXBJbWh)tueDb{~8J;ACrDCpnKCBURvJ(H}zlh+<8`bSftzGY<%FtDNvJ21qFbGE(7j~{vdc8Vq>D^m9 zu)xI6WiXnae~Up=d{PX%8)vLGpZaQb)S|?asLf;^g?je@0SJbcLa-r589(nLrh=^9 zF4$<-h2&0Wyp|J@W0Rr72LR~no{E`&NC1f$hSVs(>Ua#?!dU6VE&L}lBEh^~g_oqE zGuV=!PzwquNGlMXnuj&URc=@ByR3JVI^`V#hSF7TTYoka=w<1EeEmV8gAguFVA~Yn z1kd&8lr$xrhwU-sq$y(dEpH6y5CAw+Z;GX#xM`I=EN|S2yukSLpuffEqsPojm4QmAs)Ajeg6&X!L+8?b;P$81(IqHdvUZuBUP+7@30k z2uLv}eL67Ti?Sr~=eHN)mM-@rqI{0B(bgnbr=@-TEGOcpsQ#`FKwaNhR$_Jx3`uU9 zyB#3mw_9c_xL!ieT|MfVjU9?>mfNq1CEv5S_`&fc$gw|V_whfG+Q6m1hoT~c!9%HNRGCyzCJ_>549Mt` z8^s`zgG>|zAXFfIT#b3V^zRq0m+~Ec!fhiwz(Cf>p?LAin85H^k;t_nu`TG^5=seZ zF3&f?%JUh>8D->X zuU22X=*bj=@SZfvd^Ic6xpdf*s#t!xHUw@lac!1;PkYw#0uAwmWPZZW^HX>Z78rr9 z@VmC8zU+HKeE@bsVPNkoecZI01E?3PlM3u<|1(5yp% zJ@d_yuaqNNQ@IP%ns3Zln>@BqDZD{e?UmDE!Im>|h>2`{IdEC2s}B@iGEMBe;s|uy zOH~}Z!tdy%hyE^gjO=(u$du<`NQ1MO(l$H9@xEExvx^*;NcUkA zEcAgGpvMZvz2E*Yo!E^sI?RlBzIwkD9G?O6dc72b{_4F8x35tomc;s6NF{CfGc>p} zCyT7ULAF#M7Es^&<}&z9F$S#z)G!epy2qv~xC9H}1S_PIE&rxhb_12;4LFp03;`yK1I-EOI~R&`5cuF@J*qj3ByHu>7fA@4oai0$@G6YltX zv@b!^OYyIgDd@SKBJT-mkJASkIsNkbYmx4Bz1U5mfhyRXuy!s?1WG^M!l#$~9 zt=%UJH9VMr43iq`;OFV%l4Wjn>F(4P18}a}Q%p|TD06x$Ked|AGA?=&L(Ug+E6tS9 zqH41~PMjqpKK{Cksj-DFJ!VV8$0f${@6>UoOFVlakfyto`%QOU6h%V?nn14h1+M6L zdh${Cjom`f!WHKq1aul6FIAWW6b598wv~(YfuL2CHa_J-A3TPnJ;oLBxT0nHF~18QFH;3~V!XiD#&I8|9jYw0(e2BWlqkJa+Fqgf9IRYh0_9y4-T!PGX1m`^z`TjqzK zW8O9ZJCpCR<>B@|dS3txs`n~sTLOS@gBvtOXJ9QaHlbhNu~AIXHs=%)^w*4cHZXhT~Gu#?TsnyT2<-ep9BwR?gCO~5|SE`cJg;n{bE3Vpv5ZEIG)od5H0W}4_F7f-bM*S{jI5w2m`Mvc)ARR$LLe9XH~_Y>5;HAJ2JY2z^?x52 zev`EEJh2d+mm{zhXR;j)YHJ>-JeMkL3y%jzyret43WQGch)xcBsP6gR^>jkGp(3W(6ZN<%)K%EuGg;PXq@^E+YHLjlwT4 zQw{_}QHTOEwiH3^ETtlDKeYwDOyPQMF`&67Maz`9zeK@qs3+A=(Pd*5Yp%$@J-F_I z5~1CdIyw4969(38R@F#=8xkxc;+UdQU{XRWy2diB&8yKndqR6baoe ztf__+pBMsAp)&6H#fjw8llVX}Zt6ci%u%q-hl?V$V}-MPjL0dsz*7$bfJMO_Y$hNa zwahl?0Rblo`)%D53i^c)20YBlvE?y5)Pns+0}K7I9W zstQw~_rF*{e!>AU4tUf_)agBAP2Rqp!rinBi7|CZtY^`{J2n4Z_)|$^l=tPq9??%= zIUU3Y5{@npEQ6VdsxJ#Db~R)`Xow;I*@n9U$$_)isI9y>ozRMp?XyEmekG*1$MfQo zwVQTI2gRC0PLtCc=CI~*e(lX$46g)cPMshau!56N+?8iG9U!C z3^5UixjA(Cmgl;mNlRpfp$dg``houcS>cSTi@XQlwKB}@HRVg5%*?|fDRW92v1!3E zUr{t>!vFDdT*B_#`SbR1vs@nXO__4%^@Ye+-3@ zFSn|=!I+MW3;Yb8QJOD3NclfMVwU}Ba^-ibfSJ6JvXm=GF%#75|MMOX&M@_G;2T*x zH}&*e>7U{m(!7m`M3TX|dp=Av7&E|MBUAb6=ckvVaKqzc-&I%=ABYq@jU= zM)?2x9}fm1-U!ur0?T2!ugQ>SvQX@WUe9OUKjCgR;IlPd; z2u0j?wLPk@RORRs{;p7v5l~jGE)_x35e zW8V;a{h&H>olpp65tyYwcQyI0PNBtg=Z`<4u~YM3vEw=WX*Aa(!4F!*lDQ1#Vmg!F zy`}Uoi4OaUcDgjuvD){dcE>Qhull_P9_H#sC36)e16uQi?t-M*K-H6^&z*OSG6L5P zW2BAifY?jmP^b=Q?f}lTY^^h!Ih+sOhP%a-cEJxm*vsO$px_sjX2yj$i>ey!-Vnv-(JE=+&|9=CVq;YE zF|SXZiSqIDd!2Sb^WA>;euQ%^&&oAdMEwVgJ&9NDvihp2?)~8&14ZCFQ+?^wsz-k# zt9(|fr&Kbly#4D&RU3{Eh4B9jU0xBgx^skSXMhXCXr6(y93S!~t}zXy#!t27JW1-} z+Ukp{;0|I~GI;g4!oIWDz91ndlqW(0QR$H#D5*(}+L>j?juAw+^elLuRuMzDVW<+1 z&Rs~b%_7R**41Ia^7&`)1|N)}aCz0#id84TD1$|Dmp?e0&s0tAT5?R|surh$%(}OVkC@u35d57)^}V~Kxo-`#0*qBA?byy3up-ET7pRa`~z z8`W7ebFr>sHZ?ymLmU}aW~ZA8muHF3}Is3ZBwH&b!{82$tDX7 zr&GGen{y32KW*0b7u3wKa#YYf)BKGQGI6++pUHcadYU$qb1!kq)Ie@7>d)Usk0n*U z1x}98elRV|SYwmQZ0$CblEE^>3op~~p8_4-t;$sHB$I|^r>g&R)r z_8RKEC_`&StaST;O8rQ_J6C?$$GHUiO?j5ymZs&X z$IEciOy=bnZ>~{Jk{A1xQu(U~+FD~07=qWBhS8%_&p>|oke4w-iN&^W)lc#7pN2rD zUwd~vm}_sLb|>c<5&1DtdJ%HEJEvJyB%R%#-iROM8|6(u4i)9*3UdHN``w zdNs?QzVjvW3DnfYiLZ5GTw_Oi=?cUgjTTFA| z4+r`i8RSMJS;tLB@6%X=R2_g@ZhMDV2&dV20hY?39d->K&uCvb< zZL|2yR37L2vf z{MwnlWD!!u* zzaPp~#f8L@`wT77+f*eSt5)NKXPE;LwXH~+XS65r8a)ifEA=j@HimSL$TicYJ18lv zPq*YHUo8LdxQ2sGN2fhKL$9)P|MOTH&C61JZ-d*wIjdaHmehN^b}_{-vRmicYP^4U zE*d>Z%r!!-7L|~4JUSr!(dS7Hhz;$2buJn+aHfSu4^r%eR$pQ((<#yBycQO+a`20h zPuxfNX2?!1uNvW%h@}17*hO{v2Wl2<)SnA=56^h2q)U^s=ZP)%7r%)aKWc@de_vU$ z{sM=4s9t)$R1A${=zvszuz9^yc*B=mOEaPL_{3w(QF8bkLHaDxL)Vx}1vcTX-k`wb z{FmZn%R35^G|*vSr@*7&UXIOk7xs@NZn-C^E)$IpnV#dkWfB-Wh4?3CWwAK*!?!%z z-!S~4zTT>`45vTX0gCT|w&onFgZyV>nAO8Vms5|@KmT;w%n|E3EdDJbL^bU<|$8*BmqG-r4<(td;vjZks^D<2uKkSu%ZZvhzhdrD2r?YMq*gL3E0-Q@AG`` zkN3y-!+G-L-kg~^=ge6q_s((#$IUHeI8Ymu&!n(J6JDZggNHBb|9D9xuchTU z82tcZo<2RvP3_A*ci@esx;2SopOgTfcmMEG?_BF+?7_;MiyFW&(iD^CQ{NG6jx0vD zdkx*udshBnnrNFST4_s7l#(A-rNRo3yE-a*R&Lbn8I=?p@__TOQ|!zT-7LyLjQdc! zE&8%0$zASPwMLKT1D2t{dm5T0opOgNeCFr%&VTx*$Sl>-*z9y`YZL)q%J3fxcc%tz zO23B4B8O>=G-M~1QT1mqO8V*p#Czi=`ZQ01VYYVvnoFxyK zMRI?HH(2s6Dd|d);O(1Y$EAm|;bxSwM%9{tdMi>Lvp@XgPTiQFmmSPcE=hkV_P&+B zeKVv8!V`Trx+|FUgMWzaqrUep;LyIwFyW%)?|-G*yFDVXixYAeK6Fq7+xLk1w~eUl zdwK>vP54negIuFH6(;-oj$hhR*QHot%>6cp1TQKFhi`OqF-kyS?f_r;)?l;(^@O z0eKFxP2V5376~Y|VSbU4+aWO@E6YXu+Lz$*p71g2$2CFUtVV)ULfCRG1uuRJ)7>D^ zyrth;Y<{#Xyl}xWHZ-8aqx)_|x|18MZ*=zB7}TT~vMM4ov~=Jl zXMeh3uEI9z?c6-ZtJ}TL-hhoKPN1)dBe}F~>~h17D2@~SeT>hfOezzE$EQd$L5sd+ z*FPybI^?}k)^tw#<~t$(wS5_xjd04T#LUY zB|FKBKY4ja&AjlZ!~)5dUkL-Qwe|nwQ+)5%{%g!7$45QaU*7Z#F-f_8Y~|9F^s090 ztBi}P4j9!`g8X2u4e?q}hNcLtt@v_i%dB&MM72IyWi&rHBkFi-K0EAcbdz%QJ!I(6 z9i?nxYCh}7v9WhaR6ADp(!V$ut8u|Cuud?gedSf4%<4V0PjA_0j~w}A!V>JY1E&DK~K7&VpTF`^}QZ8QN^y%+fAiW0{_g`(Tj)rIV$ieQlvP_t8p;+b6aO zBiDL7Cnr7k;T0Dr^E}4JS@|&uH@hx&KphD0YRD=iO|zDEfcXvZLQ3BOr1{#UUuL_T z_3o=ku=OT=?%8+SoI+F%z7Z4&5iS{HrNtamo+cdP?vL3kD5w@2-JDsFzV$wQN_;!g zy{DrBtu#LR^yV*xNo>2zN>jOYhk)dRXZ9MBYjzlg3(FJR9wIMORkF%ONa$d7m`$P6FuH=5LOgIkA* z8*En53D`~Q=Vo&VHzcdu&xJ<8GEfSg^MgBWZ8kcH_Yq8!30*J4N-G4Li(RD z?MY!ohEo)4dai>qU`w#@I63ZqKGMB$QFUNHBdO}06;~(hH5f`(;$hxN-|bO)8)j>c+jJ%sbiMUBv&`nZSQLs>RuM5F;QiKk zlEAfxjgv(~S-+j_lU!+LPxObpFWUZm_=Zi`F4&dJ*aoMjC~U{d>`3Fvv%_2)_4{nE zp?98!{g#h8Hn;B890LBQaQ<{CQbzPyeO?>Up1-7W zX5qSoUxukc_tx-H;hacs|9C~+Dhb$?2ihEV@?dbV+=-4c=NiBgi(7Vky1yd8AXn=^ z-O+D*CuLG>G`9&r3VeHsDHj!t;@ULL7W#Z!qtv^BQozS7B{5{_7d}jD2&y|*vA@Bo zV96K;@#smRz86A0!9D{m&e@X5(#BXp7(WzMJMS zctxPc*c=~qkd;avB=&l)6uO_Av02lGjDJqw*=fEuWLg3ninjAL41;BWKW}LC&gD>q zN?>T@On|i9WToC)5s-PsX}b7hp-8OieYRO$^?W!ET4kW+JQv<$RDH#VHVrVpe>e#3Tr9ESg#@;czWLiRh` zvgxz8OXN9~Py6^DwpH_@b}y%X(KYO9!fG>ZdkdqX$b!%*eHG3hUgfU?hI0Ml9~WMT zUN$L@Y&!SxYT>Dj?gRv!QwUWu;;V?FlCP%ge;#Ty(cQM;wAuyi(5~RZIeGrLZvvQm z9x=FGT&65f;%zG2S5*@r^|74HX&?7>YwWoI1IKnM>AMefmpAmO#zs$w-Mja4WiK^k zRAQb5Ec}s)sG*wm9KZur^1J%IpmS0sO&=5P@8i}-1K&=XWm&eAB@FE)EHa**`BgT? zOj2UP=X?Le)0+=oOi^jp`7|-(vcEz$ji{UvI=)=Gqo4EeV>meX61~BpY`>15S6<4Y zR!J=-U@Z_>jVnM$0yjH)Q|);a7Nyt9u)EXtAOnq$crAn}XYAszW3*>>2j|bpZz_&R znM?~>_)sn?5dZN0c|oGc76&x8-peuX$>eP4-s6-LWLi~BxGmR^Z-t%svaq*rY-{AYTK}1k63xuoz7XXs_3i>LEHB8Ay+c_t_D@}=|SB0ClFsK>7Bm$->Iv*uy2)Fiv?mhVM+L71ui7rCxQ z)+M{+=R0J=S~7WCK-4oE{hNQuwkxoYYY0+Vo=MDiFmE$g_yaE=ePUyx8$BH?&jx|3 z*b}+&ztdbJeT2+%<9dS76xKn9=cm9-?P|G=x{ZF?U(qWO!R8aYvYDJltLron|H(71 zVeh{w6i?t{&)2MB0+CSdW$*V>>$if%F6Co&O`jXOc2QPPozkh85;U`i&z{enj0afF z21R)=8MfGy#z3Y$kMsdV0|O)p%d5= z=9eG*==(C?b%jug6LQs_ydZuEy(juC?ZkBq>zV&Qz72#{L@@DWVIi`Q8-qzFyLj{O zXs(_@Xb1uEuQ)6cjrbj?^%K?( zv<~tmL+TbT6hP-F*^TOAL<@w72v7op)KtS@aVU(Y78V7eai|kQ>Xu{%&7V#pGax>* zcIK|0WD;}z9sy~BN|t;U!ABiW^I?*Gm<)*DI!hryfewB`$luF+76|`0Um$#Rf3m_5 zQa?gkig*{Mix-W;Cq)4a{+$x$iwbz46DExg?bB2} zNTyOem<&fG5(yph1~nlj4YDvcg+K!W$aKCkwA3`T)DUV&$Z_APQ>WB?-P~1~G@2Jf zjYRWyR8$0Vqq!LabwT@#wUG!U3W3o?XlNpl8faw%QUQTb07cd)QfWT_S2+th&CQ<# zi2i4+S$UAXyuM`6gUR&OR#*SGeEuoCKswo72mxV*5Pu{{3ys!9L+;zwVK^*C3k$8~ zLOy@OG=SLo=?@sV*8oQS7Z?tU#sKO64TjSECq5J!_m6T|4WOyt>oVysR4+1p%{;8A zr^$e2bxRrzm;&E0gwzlDxYHoMX8HHV5K+enYiO*AH^LYhX(EuCrW!_CMmR$gJjMiP zhy>J(^dKEWLqn7a0*^33fP5_!28TeInjnpgH85xmgq9&j@5H*BxzL$wa>C&dShUcd KJtpR+LjMbLVc&%S diff --git a/mission/njord_tasks/navigation_task/package.xml b/mission/njord_tasks/navigation_task/package.xml deleted file mode 100644 index 1c06d25a..00000000 --- a/mission/njord_tasks/navigation_task/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - navigation_task - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - njord_task_base - rclcpp - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions - - - - ament_cmake - - diff --git a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml deleted file mode 100644 index fdc8a50b..00000000 --- a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml +++ /dev/null @@ -1,22 +0,0 @@ -navigation_task_node: - ros__parameters: - map_origin_lat: -33.72242824301795 - map_origin_lon: 150.6740063854522 - map_origin_set: true - gps_start_lat: -33.722553423946906 - gps_start_lon: 150.67394210456843 - gps_end_lat: -33.72253671663162 - gps_end_lon: 150.67413245758723 - gps_start_x: 0.0 - gps_start_y: 0.0 - gps_end_x: 1.1791211357704299 - gps_end_y: 17.485397720327114 - gps_frame_coords_set: true - map_origin_topic: "/map/origin" - odom_topic: "/seapath/odom/ned" - landmark_topic: "landmarks_out" - assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct - - # Task specific parameters - distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame - # If the distance is greater than 6.0 we drive distance-4m towards the first buoy pair before trying to find it \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_node.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_node.cpp deleted file mode 100644 index 4fe227ad..00000000 --- a/mission/njord_tasks/navigation_task/src/navigation_task_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - - rclcpp::spin(node); - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp deleted file mode 100644 index 9398ecac..00000000 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ /dev/null @@ -1,924 +0,0 @@ -#include - -namespace navigation_task { - -NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options) - : NjordTaskBaseNode("navigation_task_node", options) { - - declare_parameter("distance_to_first_buoy_pair", 2.0); - - std::thread(&NavigationTaskNode::main_task, this).detach(); -} - -void NavigationTaskNode::main_task() { - - navigation_ready(); - RCLCPP_INFO(this->get_logger(), "Navigation task started"); - auto odom_msg = get_odom(); - odom_start_point_ = odom_msg->pose.pose.position; - // First pair of buoys - Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); - sensor_msgs::msg::PointCloud2 buoy_vis_msg; - pcl::PointCloud buoy_vis; - - pcl::PointXYZRGB buoy_red_0; - buoy_red_0.x = predicted_first_buoy_pair(0, 0); - buoy_red_0.y = predicted_first_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - pcl::PointXYZRGB buoy_green_1; - buoy_green_1.x = predicted_first_buoy_pair(0, 1); - buoy_green_1.y = predicted_first_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - if (distance_to_first_buoy_pair > 6.0) { - geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; - waypoint_to_approach_first_pair_base_link.x = - distance_to_first_buoy_pair - 4.0; - waypoint_to_approach_first_pair_base_link.y = 0.0; - waypoint_to_approach_first_pair_base_link.z = 0.0; - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; - tf2::doTransform(waypoint_to_approach_first_pair_base_link, - waypoint_to_approach_first_pair_odom, transform); - send_waypoint(waypoint_to_approach_first_pair_odom); - set_desired_heading(odom_start_point_, - waypoint_to_approach_first_pair_odom); - reach_waypoint(1.0); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - std::vector buoy_landmarks_0_to_1 = - get_buoy_landmarks(predicted_first_buoy_pair); - if (buoy_landmarks_0_to_1.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; - buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; - buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_first_pair; - waypoint_first_pair.x = - (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + - buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / - 2; - waypoint_first_pair.y = - (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + - buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / - 2; - waypoint_first_pair.z = 0.0; - send_waypoint(waypoint_first_pair); - set_desired_heading(odom_start_point_, waypoint_first_pair); - reach_waypoint(1.0); - - // Second pair of buoys - Eigen::Array predicted_second_pair = predict_second_buoy_pair( - buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_2; - pcl::PointXYZRGB buoy_green_3; - buoy_red_2.x = predicted_second_pair(0, 0); - buoy_red_2.y = predicted_second_pair(1, 0); - buoy_red_2.z = 0.0; - buoy_red_2.r = 255; - buoy_red_2.g = 0; - buoy_red_2.b = 0; - buoy_vis.push_back(buoy_red_2); - buoy_green_3.x = predicted_second_pair(0, 1); - buoy_green_3.y = predicted_second_pair(1, 1); - buoy_green_3.z = 0.0; - buoy_green_3.r = 0; - buoy_green_3.g = 255; - buoy_green_3.b = 0; - buoy_vis.push_back(buoy_green_3); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_2_to_3 = - get_buoy_landmarks(predicted_second_pair); - if (buoy_landmarks_2_to_3.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_2 = pcl::PointXYZRGB(); - buoy_green_3 = pcl::PointXYZRGB(); - buoy_red_2.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; - buoy_red_2.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; - buoy_red_2.z = 0.0; - buoy_red_2.r = 255; - buoy_red_2.g = 0; - buoy_red_2.b = 0; - buoy_vis.push_back(buoy_red_2); - buoy_green_3.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; - buoy_green_3.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; - buoy_green_3.z = 0.0; - buoy_green_3.r = 0; - buoy_green_3.g = 255; - buoy_green_3.b = 0; - buoy_vis.push_back(buoy_green_3); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_second_pair; - waypoint_second_pair.x = - (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + - buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / - 2; - waypoint_second_pair.y = - (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + - buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / - 2; - waypoint_second_pair.z = 0.0; - send_waypoint(waypoint_second_pair); - set_desired_heading(waypoint_first_pair, waypoint_second_pair); - reach_waypoint(1.0); - - // West buoy - Eigen::Array predicted_west_buoy = - predict_west_buoy(buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position, - buoy_landmarks_2_to_3[0].pose_odom_frame.position, - buoy_landmarks_2_to_3[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_west; - buoy_west.x = predicted_west_buoy(0, 0); - buoy_west.y = predicted_west_buoy(1, 0); - buoy_west.z = 0.0; - buoy_west.r = 255; - buoy_west.g = 255; - buoy_west.b = 0; - buoy_vis.push_back(buoy_west); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_4 = - get_buoy_landmarks(predicted_west_buoy); - if (buoy_landmarks_4.size() != 1) { - RCLCPP_ERROR(this->get_logger(), "Could not find west buoy"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_west = pcl::PointXYZRGB(); - buoy_west.x = buoy_landmarks_4[0].pose_odom_frame.position.x; - buoy_west.y = buoy_landmarks_4[0].pose_odom_frame.position.y; - buoy_west.z = 0.0; - buoy_west.r = 255; - buoy_west.g = 255; - buoy_west.b = 0; - buoy_vis.push_back(buoy_west); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - Eigen::Vector2d direction_vector_upwards; - direction_vector_upwards - << buoy_landmarks_2_to_3[0].pose_odom_frame.position.x - - buoy_landmarks_2_to_3[1].pose_odom_frame.position.x, - buoy_landmarks_2_to_3[0].pose_odom_frame.position.y - - buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; - direction_vector_upwards.normalize(); - - // Set waypoint two meters above west buoy - geometry_msgs::msg::Point waypoint_west_buoy; - waypoint_west_buoy.x = buoy_landmarks_4[0].pose_odom_frame.position.x + - direction_vector_upwards(0) * 3; - waypoint_west_buoy.y = buoy_landmarks_4[0].pose_odom_frame.position.y + - direction_vector_upwards(1) * 3; - waypoint_west_buoy.z = 0.0; - - send_waypoint(waypoint_west_buoy); - set_desired_heading(waypoint_second_pair, waypoint_west_buoy); - reach_waypoint(1.0); - - // Third pair of buoys - Eigen::Array predicted_third_buoy_pair = - predict_third_buoy_pair( - buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position, - buoy_landmarks_2_to_3[0].pose_odom_frame.position, - buoy_landmarks_2_to_3[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_5; - pcl::PointXYZRGB buoy_green_6; - buoy_red_5.x = predicted_third_buoy_pair(0, 0); - buoy_red_5.y = predicted_third_buoy_pair(1, 0); - buoy_red_5.z = 0.0; - buoy_red_5.r = 255; - buoy_red_5.g = 0; - buoy_red_5.b = 0; - buoy_vis.push_back(buoy_red_5); - buoy_green_6.x = predicted_third_buoy_pair(0, 1); - buoy_green_6.y = predicted_third_buoy_pair(1, 1); - buoy_green_6.z = 0.0; - buoy_green_6.r = 0; - buoy_green_6.g = 255; - buoy_green_6.b = 0; - buoy_vis.push_back(buoy_green_6); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_5_to_6 = - get_buoy_landmarks(predicted_third_buoy_pair); - if (buoy_landmarks_5_to_6.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find third buoy pair"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_5 = pcl::PointXYZRGB(); - buoy_green_6 = pcl::PointXYZRGB(); - buoy_red_5.x = buoy_landmarks_5_to_6[0].pose_odom_frame.position.x; - buoy_red_5.y = buoy_landmarks_5_to_6[0].pose_odom_frame.position.y; - buoy_red_5.z = 0.0; - buoy_red_5.r = 255; - buoy_red_5.g = 0; - buoy_red_5.b = 0; - buoy_vis.push_back(buoy_red_5); - buoy_green_6.x = buoy_landmarks_5_to_6[1].pose_odom_frame.position.x; - buoy_green_6.y = buoy_landmarks_5_to_6[1].pose_odom_frame.position.y; - buoy_green_6.z = 0.0; - buoy_green_6.r = 0; - buoy_green_6.g = 255; - buoy_green_6.b = 0; - buoy_vis.push_back(buoy_green_6); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_third_pair; - waypoint_third_pair.x = - (buoy_landmarks_5_to_6[0].pose_odom_frame.position.x + - buoy_landmarks_5_to_6[1].pose_odom_frame.position.x) / - 2; - waypoint_third_pair.y = - (buoy_landmarks_5_to_6[0].pose_odom_frame.position.y + - buoy_landmarks_5_to_6[1].pose_odom_frame.position.y) / - 2; - waypoint_third_pair.z = 0.0; - - send_waypoint(waypoint_third_pair); - set_desired_heading(waypoint_west_buoy, waypoint_third_pair); - Eigen::Vector2d direction_vector_second_to_third_pair; - direction_vector_second_to_third_pair - << waypoint_third_pair.x - waypoint_second_pair.x, - waypoint_third_pair.y - waypoint_second_pair.y; - direction_vector_second_to_third_pair.normalize(); - geometry_msgs::msg::Point waypoint_through_third_pair; - waypoint_through_third_pair.x = - waypoint_third_pair.x + direction_vector_second_to_third_pair(0) * 2; - waypoint_through_third_pair.y = - waypoint_third_pair.y + direction_vector_second_to_third_pair(1) * 2; - waypoint_through_third_pair.z = 0.0; - send_waypoint(waypoint_through_third_pair); - set_desired_heading(waypoint_third_pair, waypoint_through_third_pair); - reach_waypoint(1.0); - - // North buoy - Eigen::Array predicted_north_buoy = - predict_north_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, - buoy_landmarks_5_to_6[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - std::vector buoy_landmarks_7 = - get_buoy_landmarks(predicted_north_buoy); - if (buoy_landmarks_7.size() != 1) { - RCLCPP_ERROR(this->get_logger(), "Could not find north buoy"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_north; - buoy_north.x = buoy_landmarks_7[0].pose_odom_frame.position.x; - buoy_north.y = buoy_landmarks_7[0].pose_odom_frame.position.y; - buoy_north.z = 0.0; - buoy_north.r = 255; - buoy_north.g = 255; - buoy_north.b = 0; - buoy_vis.push_back(buoy_north); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_north_buoy; - waypoint_north_buoy.x = buoy_landmarks_7[0].pose_odom_frame.position.x + - direction_vector_second_to_third_pair(0) * 2.5; - waypoint_north_buoy.y = buoy_landmarks_7[0].pose_odom_frame.position.y + - direction_vector_second_to_third_pair(1) * 2.5; - waypoint_north_buoy.z = 0.0; - send_waypoint(waypoint_north_buoy); - set_desired_heading(waypoint_through_third_pair, waypoint_north_buoy); - reach_waypoint(1.0); - - // South buoy - Eigen::Array predicted_south_buoy = - predict_south_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, - buoy_landmarks_5_to_6[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_south; - buoy_south.x = predicted_south_buoy(0, 0); - buoy_south.y = predicted_south_buoy(1, 0); - buoy_south.z = 0.0; - buoy_south.r = 255; - buoy_south.g = 255; - buoy_south.b = 0; - buoy_vis.push_back(buoy_south); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_8 = - get_buoy_landmarks(predicted_south_buoy); - if (buoy_landmarks_8.size() != 1) { - RCLCPP_ERROR(this->get_logger(), "Could not find south buoy"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_south = pcl::PointXYZRGB(); - buoy_south.x = buoy_landmarks_8[0].pose_odom_frame.position.x; - buoy_south.y = buoy_landmarks_8[0].pose_odom_frame.position.y; - buoy_south.z = 0.0; - buoy_south.r = 255; - buoy_south.g = 255; - buoy_south.b = 0; - buoy_vis.push_back(buoy_south); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - Eigen::Vector2d direction_vector_downwards; - direction_vector_downwards - << buoy_landmarks_5_to_6[1].pose_odom_frame.position.x - - buoy_landmarks_5_to_6[10].pose_odom_frame.position.x, - buoy_landmarks_5_to_6[1].pose_odom_frame.position.y - - buoy_landmarks_5_to_6[0].pose_odom_frame.position.y; - direction_vector_downwards.normalize(); - - geometry_msgs::msg::Point waypoint_south_buoy; - waypoint_south_buoy.x = buoy_landmarks_8[0].pose_odom_frame.position.x - - direction_vector_second_to_third_pair(0) * 3 + - direction_vector_downwards(0) * 1; - waypoint_south_buoy.y = buoy_landmarks_8[0].pose_odom_frame.position.y - - direction_vector_second_to_third_pair(1) * 3 + - direction_vector_downwards(1) * 1; - waypoint_south_buoy.z = 0.0; - send_waypoint(waypoint_south_buoy); - set_desired_heading(waypoint_north_buoy, waypoint_south_buoy); - reach_waypoint(0.2); - - // Fourth pair of buoys - Eigen::Array predicted_fourth_buoy_pair = - predict_fourth_buoy_pair( - buoy_landmarks_5_to_6[0].pose_odom_frame.position, - buoy_landmarks_5_to_6[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_9; - pcl::PointXYZRGB buoy_green_10; - buoy_red_9.x = predicted_fourth_buoy_pair(0, 0); - buoy_red_9.y = predicted_fourth_buoy_pair(1, 0); - buoy_red_9.z = 0.0; - buoy_red_9.r = 255; - buoy_red_9.g = 0; - buoy_red_9.b = 0; - buoy_vis.push_back(buoy_red_9); - buoy_green_10.x = predicted_fourth_buoy_pair(0, 1); - buoy_green_10.y = predicted_fourth_buoy_pair(1, 1); - buoy_green_10.z = 0.0; - buoy_green_10.r = 0; - buoy_green_10.g = 255; - buoy_green_10.b = 0; - buoy_vis.push_back(buoy_green_10); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_9_to_10 = - get_buoy_landmarks(predicted_fourth_buoy_pair); - if (buoy_landmarks_9_to_10.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find fourth buoy pair"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_9 = pcl::PointXYZRGB(); - buoy_green_10 = pcl::PointXYZRGB(); - buoy_red_9.x = buoy_landmarks_9_to_10[0].pose_odom_frame.position.x; - buoy_red_9.y = buoy_landmarks_9_to_10[0].pose_odom_frame.position.y; - buoy_red_9.z = 0.0; - buoy_red_9.r = 255; - buoy_red_9.g = 0; - buoy_red_9.b = 0; - buoy_vis.push_back(buoy_red_9); - buoy_green_10.x = buoy_landmarks_9_to_10[1].pose_odom_frame.position.x; - buoy_green_10.y = buoy_landmarks_9_to_10[1].pose_odom_frame.position.y; - buoy_green_10.z = 0.0; - buoy_green_10.r = 0; - buoy_green_10.g = 255; - buoy_green_10.b = 0; - buoy_vis.push_back(buoy_green_10); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_fourth_pair; - waypoint_fourth_pair.x = - (buoy_landmarks_9_to_10[0].pose_odom_frame.position.x + - buoy_landmarks_9_to_10[1].pose_odom_frame.position.x) / - 2; - waypoint_fourth_pair.y = - (buoy_landmarks_9_to_10[0].pose_odom_frame.position.y + - buoy_landmarks_9_to_10[1].pose_odom_frame.position.y) / - 2; - waypoint_fourth_pair.z = 0.0; - send_waypoint(waypoint_fourth_pair); - set_desired_heading(waypoint_south_buoy, waypoint_fourth_pair); - reach_waypoint(1.0); - - // East buoy - Eigen::Array predicted_east_buoy = - predict_east_buoy(buoy_landmarks_9_to_10[0].pose_odom_frame.position, - buoy_landmarks_9_to_10[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_east; - buoy_east.x = predicted_east_buoy(0, 0); - buoy_east.y = predicted_east_buoy(1, 0); - buoy_east.z = 0.0; - buoy_east.r = 255; - buoy_east.g = 255; - buoy_east.b = 0; - buoy_vis.push_back(buoy_east); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_11 = - get_buoy_landmarks(predicted_east_buoy); - if (buoy_landmarks_11.size() != 1) { - RCLCPP_ERROR(this->get_logger(), "Could not find east buoy"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_east = pcl::PointXYZRGB(); - buoy_east.x = buoy_landmarks_11[0].pose_odom_frame.position.x; - buoy_east.y = buoy_landmarks_11[0].pose_odom_frame.position.y; - buoy_east.z = 0.0; - buoy_east.r = 255; - buoy_east.g = 255; - buoy_east.b = 0; - buoy_vis.push_back(buoy_east); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - Eigen::Vector2d direction_vector_9_to_10; - direction_vector_9_to_10 - << buoy_landmarks_9_to_10[1].pose_odom_frame.position.x - - buoy_landmarks_9_to_10[0].pose_odom_frame.position.x, - buoy_landmarks_9_to_10[1].pose_odom_frame.position.y - - buoy_landmarks_9_to_10[0].pose_odom_frame.position.y; - direction_vector_9_to_10.normalize(); - geometry_msgs::msg::Point waypoint_east_buoy; - waypoint_east_buoy.x = buoy_landmarks_11[0].pose_odom_frame.position.x + - direction_vector_9_to_10(0) * 3; - waypoint_east_buoy.y = buoy_landmarks_11[0].pose_odom_frame.position.y + - direction_vector_9_to_10(1) * 3; - waypoint_east_buoy.z = 0.0; - send_waypoint(waypoint_east_buoy); - set_desired_heading(waypoint_fourth_pair, waypoint_east_buoy); - reach_waypoint(1.0); - - // Fifth pair of buoys - Eigen::Array predicted_fifth_buoy_pair = - predict_fifth_buoy_pair( - buoy_landmarks_9_to_10[0].pose_odom_frame.position, - buoy_landmarks_9_to_10[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_12; - pcl::PointXYZRGB buoy_green_13; - buoy_red_12.x = predicted_fifth_buoy_pair(0, 0); - buoy_red_12.y = predicted_fifth_buoy_pair(1, 0); - buoy_red_12.z = 0.0; - buoy_red_12.r = 255; - buoy_red_12.g = 0; - buoy_red_12.b = 0; - buoy_vis.push_back(buoy_red_12); - buoy_green_13.x = predicted_fifth_buoy_pair(0, 1); - buoy_green_13.y = predicted_fifth_buoy_pair(1, 1); - buoy_green_13.z = 0.0; - buoy_green_13.r = 0; - buoy_green_13.g = 255; - buoy_green_13.b = 0; - buoy_vis.push_back(buoy_green_13); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_12_to_13 = - get_buoy_landmarks(predicted_fifth_buoy_pair); - if (buoy_landmarks_12_to_13.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find fifth buoy pair"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_12 = pcl::PointXYZRGB(); - buoy_green_13 = pcl::PointXYZRGB(); - buoy_red_12.x = buoy_landmarks_12_to_13[0].pose_odom_frame.position.x; - buoy_red_12.y = buoy_landmarks_12_to_13[0].pose_odom_frame.position.y; - buoy_red_12.z = 0.0; - buoy_red_12.r = 255; - buoy_red_12.g = 0; - buoy_red_12.b = 0; - buoy_vis.push_back(buoy_red_12); - buoy_green_13.x = buoy_landmarks_12_to_13[1].pose_odom_frame.position.x; - buoy_green_13.y = buoy_landmarks_12_to_13[1].pose_odom_frame.position.y; - buoy_green_13.z = 0.0; - buoy_green_13.r = 0; - buoy_green_13.g = 255; - buoy_green_13.b = 0; - buoy_vis.push_back(buoy_green_13); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_fifth_pair; - waypoint_fifth_pair.x = - (buoy_landmarks_12_to_13[0].pose_odom_frame.position.x + - buoy_landmarks_12_to_13[1].pose_odom_frame.position.x) / - 2; - waypoint_fifth_pair.y = - (buoy_landmarks_12_to_13[0].pose_odom_frame.position.y + - buoy_landmarks_12_to_13[1].pose_odom_frame.position.y) / - 2; - waypoint_fifth_pair.z = 0.0; - send_waypoint(waypoint_fifth_pair); - set_desired_heading(waypoint_east_buoy, waypoint_fifth_pair); - reach_waypoint(1.0); - - // Sixth pair of buoys - Eigen::Array predicted_sixth_buoy_pair = - predict_sixth_buoy_pair( - buoy_landmarks_9_to_10[0].pose_odom_frame.position, - buoy_landmarks_9_to_10[1].pose_odom_frame.position, - buoy_landmarks_12_to_13[0].pose_odom_frame.position, - buoy_landmarks_12_to_13[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_14; - pcl::PointXYZRGB buoy_green_15; - buoy_red_14.x = predicted_sixth_buoy_pair(0, 0); - buoy_red_14.y = predicted_sixth_buoy_pair(1, 0); - buoy_red_14.z = 0.0; - buoy_red_14.r = 255; - buoy_red_14.g = 0; - buoy_red_14.b = 0; - buoy_vis.push_back(buoy_red_14); - buoy_green_15.x = predicted_sixth_buoy_pair(0, 1); - buoy_green_15.y = predicted_sixth_buoy_pair(1, 1); - buoy_green_15.z = 0.0; - buoy_green_15.r = 0; - buoy_green_15.g = 255; - buoy_green_15.b = 0; - buoy_vis.push_back(buoy_green_15); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_14_to_15 = - get_buoy_landmarks(predicted_sixth_buoy_pair); - if (buoy_landmarks_14_to_15.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find sixth buoy pair"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_14 = pcl::PointXYZRGB(); - buoy_green_15 = pcl::PointXYZRGB(); - buoy_red_14.x = buoy_landmarks_14_to_15[0].pose_odom_frame.position.x; - buoy_red_14.y = buoy_landmarks_14_to_15[0].pose_odom_frame.position.y; - buoy_red_14.z = 0.0; - buoy_red_14.r = 255; - buoy_red_14.g = 0; - buoy_red_14.b = 0; - buoy_vis.push_back(buoy_red_14); - buoy_green_15.x = buoy_landmarks_14_to_15[1].pose_odom_frame.position.x; - buoy_green_15.y = buoy_landmarks_14_to_15[1].pose_odom_frame.position.y; - buoy_green_15.z = 0.0; - buoy_green_15.r = 0; - buoy_green_15.g = 255; - buoy_green_15.b = 0; - buoy_vis.push_back(buoy_green_15); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_sixth_pair; - waypoint_sixth_pair.x = - (buoy_landmarks_14_to_15[0].pose_odom_frame.position.x + - buoy_landmarks_14_to_15[1].pose_odom_frame.position.x) / - 2; - waypoint_sixth_pair.y = - (buoy_landmarks_14_to_15[0].pose_odom_frame.position.y + - buoy_landmarks_14_to_15[1].pose_odom_frame.position.y) / - 2; - waypoint_sixth_pair.z = 0.0; - send_waypoint(waypoint_sixth_pair); - set_desired_heading(waypoint_fifth_pair, waypoint_sixth_pair); - reach_waypoint(1.0); - - // Gps end goal - geometry_msgs::msg::Point gps_end_goal; - gps_end_goal.x = this->get_parameter("gps_end_x").as_double(); - gps_end_goal.y = this->get_parameter("gps_end_y").as_double(); - gps_end_goal.z = 0.0; - send_waypoint(gps_end_goal); - set_desired_heading(waypoint_sixth_pair, gps_end_goal); -} - -Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { - // Predict the positions of the first two buoys - geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; - geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; - buoy_0_base_link_frame.header.frame_id = "base_link"; - buoy_1_base_link_frame.header.frame_id = "base_link"; - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - - buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_0_base_link_frame.pose.position.y = -2.5; - buoy_0_base_link_frame.pose.position.z = 0.0; - buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_1_base_link_frame.pose.position.y = 2.5; - buoy_1_base_link_frame.pose.position.z = 0.0; - - geometry_msgs::msg::PoseStamped buoy_0_odom_frame; - geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - - bool transform_success = false; - while (!transform_success) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; - predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; - predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; - predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, - previous_waypoint_odom_frame_.y - odom_start_point_.y; - direction_vector.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 5; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 5; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 5; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 5; - - return predicted_positions; -} - -Eigen::Array -NavigationTaskNode::predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3) { - Eigen::Vector2d direction_vector_right; - direction_vector_right << (buoy_3.x + buoy_2.x) / 2 - - (buoy_1.x + buoy_0.x) / 2, - (buoy_3.y + buoy_2.y) / 2 - (buoy_1.y + buoy_0.y) / 2; - direction_vector_right.normalize(); - - Eigen::Vector2d direction_vector_up; - direction_vector_up << (buoy_0.x + buoy_2.x) / 2 - (buoy_1.x + buoy_3.x) / 2, - (buoy_0.y + buoy_2.y) / 2 - (buoy_1.y + buoy_3.y) / 2; - direction_vector_up.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_3.x + - direction_vector_right(0) * 12.5 * 1 / 2 + - direction_vector_up(0) * 5 * 2 / 5; - predicted_positions(1, 0) = buoy_3.y + - direction_vector_right(1) * 12.5 * 1 / 2 + - direction_vector_up(1) * 5 * 2 / 5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_third_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3) { - Eigen::Vector2d direction_vector_right; - direction_vector_right << (buoy_3.x + buoy_2.x) / 2 - - (buoy_1.x + buoy_0.x) / 2, - (buoy_3.y + buoy_2.y) / 2 - (buoy_1.y + buoy_0.y) / 2; - direction_vector_right.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_2.x + direction_vector_right(0) * 12.5; - predicted_positions(1, 0) = buoy_2.y + direction_vector_right(1) * 12.5; - predicted_positions(0, 1) = buoy_3.x + direction_vector_right(0) * 12.5; - predicted_positions(1, 1) = buoy_3.y + direction_vector_right(1) * 12.5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_north_buoy( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6, - const Eigen::Vector2d &direction_vector_second_to_third_pair) { - Eigen::Vector2d direction_5_to_6; - direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; - direction_5_to_6.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5 * 0.5 + - direction_vector_second_to_third_pair(0) * 2; - predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5 * 0.5 + - direction_vector_second_to_third_pair(1) * 2; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_south_buoy( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6, - const Eigen::Vector2d &direction_vector_second_to_third_pair) { - Eigen::Vector2d direction_5_to_6; - direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; - direction_5_to_6.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5 + - direction_vector_second_to_third_pair(0) * 8; - predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5 + - direction_vector_second_to_third_pair(1) * 8; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_fourth_buoy_pair( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6) { - Eigen::Vector2d direction_5_to_6; - direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; - direction_5_to_6.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5; - predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5; - predicted_positions(0, 1) = - buoy_6.x + direction_5_to_6(0) * 12.5 + direction_5_to_6(0) * 5; - predicted_positions(1, 1) = - buoy_6.y + direction_5_to_6(1) * 12.5 + direction_5_to_6(1) * 5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_east_buoy( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const Eigen::Vector2d &direction_vector_second_to_third_pair) { - Eigen::Vector2d direction_9_to_10; - direction_9_to_10 << buoy_10.x - buoy_9.x, buoy_10.y - buoy_9.y; - direction_9_to_10.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_9.x + direction_9_to_10(0) * 2.5 - - direction_vector_second_to_third_pair(0) * 7.5; - predicted_positions(1, 0) = buoy_9.y + direction_9_to_10(1) * 2.5 - - direction_vector_second_to_third_pair(1) * 7.5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_fifth_buoy_pair( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const Eigen::Vector2d &direction_vector_second_to_third_pair) { - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = - buoy_9.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 0) = - buoy_9.y - direction_vector_second_to_third_pair(1) * 12.5; - predicted_positions(0, 1) = - buoy_10.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 1) = - buoy_10.y - direction_vector_second_to_third_pair(1) * 12.5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_12, - const geometry_msgs::msg::Point &buoy_13) { - Eigen::Vector2d direction_fourth_to_fifth_pair; - direction_fourth_to_fifth_pair - << (buoy_13.x + buoy_12.x) / 2 - (buoy_10.x + buoy_9.x) / 2, - (buoy_13.y + buoy_12.y) / 2 - (buoy_10.y + buoy_9.y) / 2; - direction_fourth_to_fifth_pair.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_12.x + direction_fourth_to_fifth_pair(0) * 5; - predicted_positions(1, 0) = buoy_12.y + direction_fourth_to_fifth_pair(1) * 5; - predicted_positions(0, 1) = buoy_13.x + direction_fourth_to_fifth_pair(0) * 5; - predicted_positions(1, 1) = buoy_13.y + direction_fourth_to_fifth_pair(1) * 5; - - return predicted_positions; -} - -} // namespace navigation_task \ No newline at end of file diff --git a/mission/njord_tasks/njord_task_base/CMakeLists.txt b/mission/njord_tasks/njord_task_base/CMakeLists.txt deleted file mode 100644 index 2666fae7..00000000 --- a/mission/njord_tasks/njord_task_base/CMakeLists.txt +++ /dev/null @@ -1,85 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(njord_task_base) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) - -include_directories(include) - -# Create library target -add_library(${PROJECT_NAME}_lib - src/njord_task_base_ros.cpp -) - -ament_target_dependencies(${PROJECT_NAME}_lib PUBLIC - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs -) - -target_link_libraries(${PROJECT_NAME}_lib PUBLIC - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs -) - -# Specify the include directories for the library -target_include_directories(${PROJECT_NAME}_lib PUBLIC - "$" - "$" -) - -install(TARGETS ${PROJECT_NAME}_lib - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -# Export the library -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies( - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs -) - -install( - DIRECTORY include/ - DESTINATION include -) - -ament_package() diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp deleted file mode 100644 index d0c55429..00000000 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ /dev/null @@ -1,162 +0,0 @@ -#ifndef NJORD_TASK_BASE_ROS_HPP -#define NJORD_TASK_BASE_ROS_HPP - -#include "geometry_msgs/msg/pose_array.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "vortex_msgs/msg/landmark_array.hpp" -#include "vortex_msgs/srv/desired_velocity.hpp" -#include "vortex_msgs/srv/waypoint.hpp" -#include -#include -#include -#include -#include - -/** - * @brief A struct representing a landmark with id and pose in the odom frame - */ -struct LandmarkPoseID { - geometry_msgs::msg::Pose pose_odom_frame; - int32_t id; -}; - -class NjordTaskBaseNode : public rclcpp::Node { -public: - NjordTaskBaseNode(const std::string &node_name, - const rclcpp::NodeOptions &options); - -protected: - std::pair lla2flat(double lat, double lon) const; - - void map_origin_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); - void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - void landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg); - - /** - * @brief Runs until the navigation systeam required for task completion is - * ready. This includes setting the map origin, getting the map to odom tf, - * and and getting the odom coordinates of the start and end GPS points. - */ - void navigation_ready(); - - /** - * @brief Spins until the map to odom tf is available. - * Stores the tf in the member variable map_odom_tf_ - * Then initializes the odom and landmark subscribers - */ - void get_map_odom_tf(); - - void initialize_subscribers(); - - /** - * @brief Set the gps_start_odom_frame_ and gps_end_odom_frame_ member - * variables Requires that the map_origin_ member variable is correctly set - */ - void set_gps_odom_points(); - - /** - * @brief Get the odometry message - * If no odometry message is available, the function will wait for a new - * message - * - * @return A shared pointer to the odometry message - */ - std::shared_ptr get_odom(); - - /** - * @brief Get the landmarks in the odom frame - * If no landmarks are available, the function will wait for a new message - * - * @return A shared pointer to the landmarks in the odom frame - */ - std::shared_ptr get_landmarks_odom_frame(); - - /** - * @brief Assign buoys to landmarks based on the predicted and measured - * positions using the auction algorithm. - * - * @param predicted_positions The predicted positions of the buoys - * @param measured_positions The measured positions of landmarks - * - * @return A vector of size equal to number of predicted positions(buoys), - * where each element is the index of the measured position(landmark) assigned - * to the corresponding predicted position(buoy). If an element is -1, it - * means that the corresponding predicted position(buoy) is not assigned to - * any measured position(landmark). - */ - Eigen::VectorXi auction_algorithm( - const Eigen::Array &predicted_positions, - const Eigen::Array &measured_positions); - - /** - * @brief Use predicted positions of buoys to get the landmarks with id and - * pose corresponding to the buoys. - * @param predicted_positions The predicted positions of the buoys - * @return A vector of landmarks with id and pose where the index in the - * vector corresponds to the index of the predicted position in the - * predicted_positions array. - */ - std::vector get_buoy_landmarks( - const Eigen::Array &predicted_positions); - - /** - * @brief Send a waypoint to the waypoint service - * @param waypoint The waypoint to send in odom frame - * Also sets the member variable previous_waypoint_odom_frame_ to the waypoint - */ - void send_waypoint(const geometry_msgs::msg::Point &waypoint); - - /** - * @brief Utility function that returns when within distance_threshold of the - * waypoint. The reference waypoint is the member variable - * previous_waypoint_odom_frame_ set by the send_waypoint function. - * - * @param distance_threshold The distance threshold for reaching the waypoint - */ - void reach_waypoint(const double distance_threshold); - - void set_desired_heading(const geometry_msgs::msg::Point &prev_waypoint, - const geometry_msgs::msg::Point &next_waypoint); - - rclcpp::Publisher::SharedPtr - gps_map_coord_visualization_pub_; - rclcpp::Publisher::SharedPtr - waypoint_visualization_pub_; - rclcpp::Publisher::SharedPtr - buoy_visualization_pub_; - rclcpp::Subscription::SharedPtr map_origin_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr - landmarks_sub_; - rclcpp::Client::SharedPtr waypoint_client_; - rclcpp::Client::SharedPtr heading_client_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - geometry_msgs::msg::TransformStamped map_odom_tf_; - - mutable std::mutex navigation_mutex_; - mutable std::mutex odom_mutex_; - mutable std::mutex landmark_mutex_; - - std::condition_variable odom_cond_var_; - std::condition_variable landmark_cond_var_; - - std::shared_ptr odom_msg_; - std::shared_ptr landmarks_msg_; - bool new_odom_msg_ = false; - bool new_landmark_msg_ = false; - bool navigation_ready_ = false; - - geometry_msgs::msg::Point previous_waypoint_odom_frame_; - geometry_msgs::msg::Point odom_start_point_; -}; - -#endif // NJORD_TASK_BASE_ROS_HPP diff --git a/mission/njord_tasks/njord_task_base/package.xml b/mission/njord_tasks/njord_task_base/package.xml deleted file mode 100644 index 12b43f79..00000000 --- a/mission/njord_tasks/njord_task_base/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - njord_task_base - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - - - - ament_cmake - - diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp deleted file mode 100644 index 8a547d6b..00000000 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ /dev/null @@ -1,532 +0,0 @@ -#include - -NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, - const rclcpp::NodeOptions &options) - : Node(node_name, options) { - declare_parameter("map_origin_lat", 0.0); - declare_parameter("map_origin_lon", 0.0); - declare_parameter("map_origin_set", false); - declare_parameter("gps_start_lat", 0.0); - declare_parameter("gps_start_lon", 0.0); - declare_parameter("gps_end_lat", 0.0); - declare_parameter("gps_end_lon", 0.0); - declare_parameter("gps_start_x", 0.0); - declare_parameter("gps_start_y", 0.0); - declare_parameter("gps_end_x", 0.0); - declare_parameter("gps_end_y", 0.0); - declare_parameter("gps_frame_coords_set", false); - declare_parameter("assignment_confidence", 10); - - declare_parameter("map_origin_topic", "/map/origin"); - declare_parameter("odom_topic", "/seapath/odom/ned"); - declare_parameter("landmark_topic", "landmarks_out"); - - // Sensor data QoS profile - rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; - auto qos_sensor_data = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), - qos_profile_sensor_data); - - // Transient local QoS profile - rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; - qos_profile_transient_local.durability = - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - auto qos_transient_local = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), - qos_profile_transient_local); - - gps_map_coord_visualization_pub_ = - this->create_publisher( - "/gps_map_coord_visualization", qos_sensor_data); - - buoy_visualization_pub_ = - this->create_publisher( - "/buoy_visualization", qos_sensor_data); - - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - if (this->get_parameter("map_origin_set").as_bool()) { - get_map_odom_tf(); - set_gps_odom_points(); - initialize_subscribers(); - std::unique_lock setup_lock(navigation_mutex_); - navigation_ready_ = true; - setup_lock.unlock(); - } else { - map_origin_sub_ = this->create_subscription( - get_parameter("map_origin_topic").as_string(), qos_transient_local, - std::bind(&NjordTaskBaseNode::map_origin_callback, this, - std::placeholders::_1)); - } - - waypoint_visualization_pub_ = - this->create_publisher( - "/waypoint_visualization", qos_sensor_data); - - waypoint_client_ = - this->create_client("waypoint_list"); - - heading_client_ = - this->create_client("yaw_reference"); -} - -void NjordTaskBaseNode::get_map_odom_tf() { - // Get the transform between the map and odom frames to avoid the overhead - // from continuously looking up the static transform between map and odom - bool tf_set = false; - while (!tf_set) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "map", tf2::TimePointZero, tf2::durationFromSec(1.0)); - map_odom_tf_ = transform; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - std::this_thread::sleep_for(std::chrono::seconds(1)); - continue; - } - tf_set = true; - } -} - -void NjordTaskBaseNode::initialize_subscribers() { - // Sensor data QoS profile - rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; - auto qos_sensor_data = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), - qos_profile_sensor_data); - - odom_sub_ = this->create_subscription( - get_parameter("odom_topic").as_string(), qos_sensor_data, - std::bind(&NjordTaskBaseNode::odom_callback, this, - std::placeholders::_1)); - - landmarks_sub_ = this->create_subscription( - get_parameter("landmark_topic").as_string(), qos_sensor_data, - std::bind(&NjordTaskBaseNode::landmark_callback, this, - std::placeholders::_1)); -} - -void NjordTaskBaseNode::set_gps_odom_points() { - if (this->get_parameter("gps_frame_coords_set").as_bool()) { - RCLCPP_INFO(this->get_logger(), "Using predefined GPS frame coordinates"); - geometry_msgs::msg::PoseArray gps_points_odom_frame; - gps_points_odom_frame.header.frame_id = "odom"; - geometry_msgs::msg::Pose gps_start_odom_frame; - gps_start_odom_frame.position.x = - this->get_parameter("gps_start_x").as_double(); - gps_start_odom_frame.position.y = - this->get_parameter("gps_start_y").as_double(); - geometry_msgs::msg::Pose gps_end_odom_frame; - gps_end_odom_frame.position.x = - this->get_parameter("gps_end_x").as_double(); - gps_end_odom_frame.position.y = - this->get_parameter("gps_end_y").as_double(); - gps_points_odom_frame.poses.push_back(gps_start_odom_frame); - gps_points_odom_frame.poses.push_back(gps_end_odom_frame); - gps_map_coord_visualization_pub_->publish(gps_points_odom_frame); - RCLCPP_INFO( - this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", - gps_start_odom_frame.position.x, gps_start_odom_frame.position.y, - gps_end_odom_frame.position.x, gps_end_odom_frame.position.y); - return; - } - auto [gps_start_x, gps_start_y] = - lla2flat(this->get_parameter("gps_start_lat").as_double(), - this->get_parameter("gps_start_lon").as_double()); - auto [gps_end_x, gps_end_y] = - lla2flat(this->get_parameter("gps_end_lat").as_double(), - this->get_parameter("gps_end_lon").as_double()); - - geometry_msgs::msg::PoseStamped gps_start_map_frame; - gps_start_map_frame.header.frame_id = "map"; - gps_start_map_frame.pose.position.x = gps_start_x; - gps_start_map_frame.pose.position.y = gps_start_y; - geometry_msgs::msg::PoseStamped gps_end_map_frame; - gps_end_map_frame.header.frame_id = "map"; - gps_end_map_frame.pose.position.x = gps_end_x; - gps_end_map_frame.pose.position.y = gps_end_y; - geometry_msgs::msg::PoseStamped gps_start_odom_frame; - geometry_msgs::msg::PoseStamped gps_end_odom_frame; - try { - tf2::doTransform(gps_start_map_frame, gps_start_odom_frame, map_odom_tf_); - tf2::doTransform(gps_end_map_frame, gps_end_odom_frame, map_odom_tf_); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - return; - } - - this->set_parameter( - rclcpp::Parameter("gps_start_x", gps_start_odom_frame.pose.position.x)); - this->set_parameter( - rclcpp::Parameter("gps_start_y", gps_start_odom_frame.pose.position.y)); - this->set_parameter( - rclcpp::Parameter("gps_end_x", gps_end_odom_frame.pose.position.x)); - this->set_parameter( - rclcpp::Parameter("gps_end_y", gps_end_odom_frame.pose.position.y)); - this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); - RCLCPP_INFO( - this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", - gps_start_odom_frame.pose.position.x, - gps_start_odom_frame.pose.position.y, gps_end_odom_frame.pose.position.x, - gps_end_odom_frame.pose.position.y); - - geometry_msgs::msg::PoseArray gps_points_odom_frame; - gps_points_odom_frame.header.frame_id = "odom"; - - gps_points_odom_frame.poses.push_back(gps_start_odom_frame.pose); - gps_points_odom_frame.poses.push_back(gps_end_odom_frame.pose); - - gps_map_coord_visualization_pub_->publish(gps_points_odom_frame); -} - -std::pair NjordTaskBaseNode::lla2flat(double lat, - double lon) const { - const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) - const double f = 1.0 / 298.257223563; // Flattening of the earth - const double psi_rad = 0.0; - - // Convert angles from degrees to radians - const double lat_rad = lat * M_PI / 180.0; - const double lon_rad = lon * M_PI / 180.0; - const double origin_lat_rad = - this->get_parameter("map_origin_lat").as_double() * M_PI / 180.0; - const double origin_lon_rad = - this->get_parameter("map_origin_lon").as_double() * M_PI / 180.0; - - // Calculate delta latitude and delta longitude in radians - const double dlat = lat_rad - origin_lat_rad; - const double dlon = lon_rad - origin_lon_rad; - - // Radius of curvature in the vertical prime (RN) - const double RN = - R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); - - // Radius of curvature in the meridian (RM) - const double RM = RN * (1.0 - (2.0 * f - f * f)) / - (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); - - // Changes in the north (dN) and east (dE) positions - const double dN = RM * dlat; - const double dE = RN * cos(origin_lat_rad) * dlon; - - // Transformation from North-East to flat x-y coordinates - const double px = cos(psi_rad) * dN - sin(psi_rad) * dE; - const double py = sin(psi_rad) * dN + cos(psi_rad) * dE; - - return {px, py}; -} - -void NjordTaskBaseNode::map_origin_callback( - const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - this->set_parameter(rclcpp::Parameter("map_origin_lat", msg->latitude)); - this->set_parameter(rclcpp::Parameter("map_origin_lon", msg->longitude)); - this->set_parameter(rclcpp::Parameter("map_origin_set", true)); - RCLCPP_INFO(this->get_logger(), "Map origin set to: %f, %f", msg->latitude, - msg->longitude); - - // Set the map to odom transform - get_map_odom_tf(); - // Set GPS frame coordinates - set_gps_odom_points(); - initialize_subscribers(); - map_origin_sub_.reset(); - std::unique_lock setup_lock(navigation_mutex_); - navigation_ready_ = true; - setup_lock.unlock(); -} - -void NjordTaskBaseNode::navigation_ready() { - bool ready = false; - while (!ready) { - std::unique_lock setup_lock(navigation_mutex_); - ready = navigation_ready_; - setup_lock.unlock(); - RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Waiting for navigation system to be ready"); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } -} - -void NjordTaskBaseNode::odom_callback( - const nav_msgs::msg::Odometry::SharedPtr msg) { - std::unique_lock lock(odom_mutex_); - odom_msg_ = msg; - new_odom_msg_ = true; - lock.unlock(); - odom_cond_var_.notify_one(); -} - -std::shared_ptr NjordTaskBaseNode::get_odom() { - std::unique_lock lock(odom_mutex_); - odom_cond_var_.wait(lock, [this] { return new_odom_msg_; }); - new_odom_msg_ = false; - lock.unlock(); - return odom_msg_; -} - -void NjordTaskBaseNode::landmark_callback( - const vortex_msgs::msg::LandmarkArray::SharedPtr msg) { - std::unique_lock lock(landmark_mutex_); - // transform the landmarks to the odom frame - for (auto &landmark : msg->landmarks) { - tf2::doTransform(landmark.odom.pose.pose, landmark.odom.pose.pose, - map_odom_tf_); - } - landmarks_msg_ = msg; - new_landmark_msg_ = true; - lock.unlock(); - landmark_cond_var_.notify_one(); -} - -std::shared_ptr -NjordTaskBaseNode::get_landmarks_odom_frame() { - std::unique_lock lock(landmark_mutex_); - landmark_cond_var_.wait(lock, [this] { return new_landmark_msg_; }); - new_landmark_msg_ = false; - lock.unlock(); - return landmarks_msg_; -} - -Eigen::VectorXi NjordTaskBaseNode::auction_algorithm( - const Eigen::Array &predicted_positions, - const Eigen::Array &measured_positions) { - int num_predicted = predicted_positions.cols(); - int num_measured = measured_positions.cols(); - Eigen::MatrixXd reward_matrix(num_measured, num_predicted); - - if (num_predicted > num_measured) { - RCLCPP_ERROR(this->get_logger(), - "Number of predicted positions is greater than number of " - "measured positions in auction algorithm"); - return Eigen::VectorXi::Constant(num_predicted, -1); - } - - double epsilon = 1e-6; // Small positive number to prevent division by zero - for (Eigen::Index i = 0; i < num_measured; ++i) { - for (Eigen::Index j = 0; j < num_predicted; ++j) { - double dx = measured_positions(0, i) - predicted_positions(0, j); - double dy = measured_positions(1, i) - predicted_positions(1, j); - double distance = std::sqrt(dx * dx + dy * dy); - reward_matrix(i, j) = 1 / (distance + epsilon); - } - } - - Eigen::VectorXi assignment = Eigen::VectorXi::Constant(num_predicted, -1); - Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_measured); - - std::vector unassigned; - for (int i = 0; i < num_predicted; ++i) { - unassigned.push_back(i); - } - - epsilon = 1.0 / (num_measured + num_predicted + 1); - - while (!unassigned.empty()) { - int customer = unassigned.back(); - unassigned.pop_back(); - - double max_value = std::numeric_limits::lowest(); - double second_max_value = std::numeric_limits::lowest(); - int max_item = -1; - - for (int item = 0; item < num_measured; ++item) { - double value = reward_matrix.coeff(item, customer) - prices[item]; - if (value > max_value) { - second_max_value = max_value; - max_value = value; - max_item = item; - } else if (value > second_max_value) { - second_max_value = value; - } - } - - int current_owner = -1; - for (int i = 0; i < num_predicted; ++i) { - if (assignment[i] == max_item) { - current_owner = i; - break; - } - } - if (current_owner != -1) { - unassigned.push_back(current_owner); - } - - assignment[customer] = max_item; - prices[max_item] += max_value - second_max_value + epsilon; - } - return assignment; -} - -std::vector NjordTaskBaseNode::get_buoy_landmarks( - const Eigen::Array &predicted_positions) { - std::vector landmark_ids; - std::vector expected_assignment; - Eigen::Array returned_buoy_positions( - 2, predicted_positions.cols()); - int confidence_threshold = - this->get_parameter("assignment_confidence").as_int(); - int result = 0; - while (result < confidence_threshold) { - landmark_ids.clear(); - auto landmark_msg = get_landmarks_odom_frame(); - - // Extract measured positions and corresponding landmark ids - Eigen::Array measured_positions( - 2, landmark_msg->landmarks.size()); - for (size_t i = 0; i < landmark_msg->landmarks.size(); i++) { - landmark_ids.push_back(landmark_msg->landmarks[i].id); - measured_positions(0, i) = - landmark_msg->landmarks[i].odom.pose.pose.position.x; - measured_positions(1, i) = - landmark_msg->landmarks[i].odom.pose.pose.position.y; - } - - // Check if there are enough landmarks detected to perform auction algorithm - if (predicted_positions.cols() > measured_positions.cols()) { - RCLCPP_ERROR( - this->get_logger(), - "Not enough landmarks detected to perform auction algorithm"); - result = 0; - continue; - } - // Perform auction algorithm - Eigen::VectorXi assignment = - auction_algorithm(predicted_positions, measured_positions); - - // Extract measured positions of assigned buoys - for (Eigen::Index i = 0; i < assignment.size(); i++) { - returned_buoy_positions(0, i) = measured_positions(0, assignment(i)); - returned_buoy_positions(1, i) = measured_positions(1, assignment(i)); - } - - // Check if any buoys are unassigned - // Should never happen as long as the number of landmarks detected is - // greater than or equal to the number of buoys - bool unassigned_buoy = false; - for (Eigen::Index i = 0; i < assignment.size(); i++) { - if (assignment(i) == -1) { - unassigned_buoy = true; - break; - } - } - - // If any buoys are unassigned, restart assignment process - if (unassigned_buoy) { - result = 0; - continue; - } - - // If this is the first iteration, save the assignment and continue - if (result == 0) { - expected_assignment.clear(); - for (Eigen::Index i = 0; i < assignment.size(); i++) { - expected_assignment.push_back(landmark_ids.at(assignment(i))); - } - result++; - continue; - } - - // Check if the assignment is consistent with the previous assignment - bool consistent_assignment = true; - for (Eigen::Index i = 0; i < assignment.size(); i++) { - if (landmark_ids.at(assignment(i)) != expected_assignment.at(i)) { - consistent_assignment = false; - break; - } - } - - // If the assignment is consistent, increment the result counter - // Otherwise, reset the result counter - if (consistent_assignment) { - result++; - continue; - } else { - result = 0; - continue; - } - } - // Loop has completed, return the id and pose of the landmarks assigned to - // buoys - std::vector buoys; - for (size_t i = 0; i < expected_assignment.size(); i++) { - LandmarkPoseID landmark; - landmark.id = expected_assignment.at(i); - landmark.pose_odom_frame.position.x = returned_buoy_positions(0, i); - landmark.pose_odom_frame.position.y = returned_buoy_positions(1, i); - buoys.push_back(landmark); - } - return buoys; -} - -void NjordTaskBaseNode::send_waypoint( - const geometry_msgs::msg::Point &waypoint) { - auto request = std::make_shared(); - request->waypoint.push_back(waypoint); - auto result_future = waypoint_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Waypoint(odom frame) sent: %f, %f", - waypoint.x, waypoint.y); - // Check if the service was successful - double dx = waypoint.x - previous_waypoint_odom_frame_.x; - double dy = waypoint.y - previous_waypoint_odom_frame_.y; - double desired_heading = std::atan2(dy, dx); - tf2::Quaternion q; - q.setRPY(0.0, 0.0, desired_heading); - - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = waypoint.x; - waypoint_vis.pose.position.y = waypoint.y; - waypoint_vis.pose.orientation = tf2::toMsg(q); - waypoint_visualization_pub_->publish(waypoint_vis); - auto status = result_future.wait_for(std::chrono::seconds(5)); - while (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); - status = result_future.wait_for(std::chrono::seconds(5)); - } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); - } - - previous_waypoint_odom_frame_ = waypoint; -} - -void NjordTaskBaseNode::reach_waypoint(const double distance_threshold) { - RCLCPP_INFO(this->get_logger(), "Reach waypoint running"); - - auto get_current_position = [&]() { - auto odom_msg = get_odom(); - return std::make_pair(odom_msg->pose.pose.position.x, - odom_msg->pose.pose.position.y); - }; - - auto [x, y] = get_current_position(); - - while (std::hypot(x - previous_waypoint_odom_frame_.x, - y - previous_waypoint_odom_frame_.y) > distance_threshold) { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - std::tie(x, y) = get_current_position(); - } - - RCLCPP_INFO(this->get_logger(), "Reached waypoint"); -} - -void NjordTaskBaseNode::set_desired_heading( - const geometry_msgs::msg::Point &prev_waypoint, - const geometry_msgs::msg::Point &next_waypoint) { - auto request = std::make_shared(); - double dx = next_waypoint.x - prev_waypoint.x; - double dy = next_waypoint.y - prev_waypoint.y; - double desired_heading = std::atan2(dy, dx); - request->u_desired = desired_heading; - auto result_future = heading_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Desired heading sent: %f", desired_heading); - auto status = result_future.wait_for(std::chrono::seconds(5)); - while (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Desired heading service timed out"); - status = result_future.wait_for(std::chrono::seconds(5)); - } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Desired heading service failed"); - } -} \ No newline at end of file diff --git a/motion/lqr_controller/CMakeLists.txt b/motion/lqr_controller/CMakeLists.txt deleted file mode 100644 index 874cf293..00000000 --- a/motion/lqr_controller/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(lqr_controller) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - lqr_controller/lqr_controller_node.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() \ No newline at end of file diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml deleted file mode 100644 index ac497221..00000000 --- a/motion/lqr_controller/config/lqr_config.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - lqr_controller: - Q: [20.0, 20.0, 1.0, 1.0, 1.0, 1.0] # State costs for [x, y, heading, surge, sway, yaw] - R: [1.0, 1.0, 1.0] # Control cost weight \ No newline at end of file diff --git a/motion/lqr_controller/launch/lqr_controller.launch.py b/motion/lqr_controller/launch/lqr_controller.launch.py deleted file mode 100644 index 03ceca10..00000000 --- a/motion/lqr_controller/launch/lqr_controller.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - lqr_controller_node = Node( - package='lqr_controller', - executable='lqr_controller_node.py', - name='lqr_controller_node', - parameters=[ - os.path.join(get_package_share_directory('lqr_controller'),'config','lqr_config.yaml'), - os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') - ], - output='screen', - ) - return LaunchDescription([ - lqr_controller_node - ]) diff --git a/motion/lqr_controller/lqr_controller/__init__.py b/motion/lqr_controller/lqr_controller/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/lqr_controller/lqr_controller/conversions.py b/motion/lqr_controller/lqr_controller/conversions.py deleted file mode 100644 index 92f14cfb..00000000 --- a/motion/lqr_controller/lqr_controller/conversions.py +++ /dev/null @@ -1,59 +0,0 @@ -import numpy as np - -from transforms3d.euler import euler2quat, quat2euler -from nav_msgs.msg import Odometry -from geometry_msgs.msg import PoseStamped - - -def odometrymsg_to_state(msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation - orientation_list = [ - orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z - ] - - # Convert quaternion to Euler angles, extract yaw - yaw = quat2euler(orientation_list)[2] - - vx = msg.twist.twist.linear.x - vy = msg.twist.twist.linear.y - vyaw = msg.twist.twist.angular.z - - state = np.array([x, y, yaw, vx, vy, vyaw]) - return state - -def state_to_odometrymsg(state): - orientation_list_next = euler2quat(0, 0, state[2]) - - odometry_msg = Odometry() - odometry_msg.pose.pose.position.x = state[0] - odometry_msg.pose.pose.position.y = state[1] - odometry_msg.pose.pose.position.z = 0.0 - odometry_msg.pose.pose.orientation.w = orientation_list_next[0] - odometry_msg.pose.pose.orientation.x = orientation_list_next[1] - odometry_msg.pose.pose.orientation.y = orientation_list_next[2] - odometry_msg.pose.pose.orientation.z = orientation_list_next[3] - - return odometry_msg - -def state_to_posestamped(state, frame_id, stamp): - orientation_list_next = euler2quat(0, 0, state[2]) - - posestamped_msg = PoseStamped() - - posestamped_msg.header.frame_id = frame_id - posestamped_msg.header.stamp = stamp - - posestamped_msg.pose.position.x = state[0] - posestamped_msg.pose.position.y = state[1] - posestamped_msg.pose.position.z = 0.0 - posestamped_msg.pose.orientation.w = orientation_list_next[0] - posestamped_msg.pose.orientation.x = orientation_list_next[1] - posestamped_msg.pose.orientation.y = orientation_list_next[2] - posestamped_msg.pose.orientation.z = orientation_list_next[3] - - return posestamped_msg - -def ssa(angle): - return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py deleted file mode 100644 index d9507b67..00000000 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ /dev/null @@ -1,42 +0,0 @@ -import numpy as np -from scipy.linalg import solve_continuous_are - -class LQRController: - def __init__(self): - self.M = np.eye(3) - self.D = np.eye(3) - self.Q = np.eye(6) - self.R = np.eye(3) - - self.A = np.zeros((6,6)) - self.B = np.zeros((6,3)) - self.C = np.zeros((3,6)) - self.C[:3, :3] = np.eye(3) - - def update_parameters(self, M: float, D: list[float], Q: list[float], R: list[float]): - self.M = M - self.M_inv = np.linalg.inv(self.M) - self.D = D - - self.Q = np.diag(Q) - self.R = np.diag(R) - - def calculate_control_input(self, x, x_ref): - tau = -self.K_LQR @ x + self.K_r @ x_ref - return tau - - def calculate_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: - rotation_matrix = np.array( - [[np.cos(heading), -np.sin(heading), 0], - [np.sin(heading), np.cos(heading), 0], - [0, 0, 1]] - ) - - self.A[:3,3:] = rotation_matrix - self.A[3:, 3:] = - self.M_inv @ self.D - - self.B[3:,:] = self.M_inv - - self.P = solve_continuous_are(self.A, self.B, self.Q, self.R) - self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) - self.K_r = np.linalg.inv(self.C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py deleted file mode 100755 index 6ffd5420..00000000 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -import numpy as np -from rclpy.node import Node -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -from lqr_controller import LQRController -from conversions import odometrymsg_to_state -from time import sleep -from std_msgs.msg import Float32 -from rcl_interfaces.msg import SetParametersResult - -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class LQRControllerNode(Node): - def __init__(self): - super().__init__("lqr_controller_node") - - self.declare_parameters( - namespace='', - parameters=[ - ('lqr_controller.Q', [10.0, 10.0, 5.0, 0.1, 1.0, 5.0]), - ('lqr_controller.R', [1.0, 1.0, 1.0]), - ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65]), - ('physical.damping_matrix_diag', [77.55, 162.5, 42.65]) - ]) - - self.parameters_updated = False - - self.state_subscriber_ = self.create_subscription(Odometry, "/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) - self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) - self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) - - self.LQR_controller = LQRController() - - self.update_controller_parameters() - - self.enabled = False - - controller_period = 0.1 - self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) - self.add_on_set_parameters_callback(self.parameter_callback) - - self.get_logger().info("lqr_controller_node started") - - def update_controller_parameters(self): - Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value - R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value - D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value - M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value - - D = np.diag(D_diag) - M = np.reshape(M, (3, 3)) - - self.LQR_controller.update_parameters(M, D, Q, R) - - def parameter_callback(self, params): - self.parameters_updated = True - for param in params: - if param.name == 'lqr_controller.Q': - self.get_logger().info(f"Updated Q to {param.value}") - elif param.name == 'lqr_controller.R': - self.get_logger().info(f"Updated R to {param.value}") - elif param.name == 'physical.inertia_matrix': - self.get_logger().info(f"Updated inertia_matrix to {param.value}") - elif param.name == 'physical.damping_matrix_diag': - self.get_logger().info(f"Updated damping_matrix_diag to {param.value}") - - return SetParametersResult(successful=True) - - def state_cb(self, msg): - self.state = odometrymsg_to_state(msg) - - def guidance_cb(self, msg): - self.x_ref = odometrymsg_to_state(msg)[:3] - - def controller_callback(self): - if self.parameters_updated: - self.update_controller_parameters() - self.parameters_updated = False - - if hasattr(self, 'state') and hasattr(self, 'x_ref'): - self.LQR_controller.calculate_model(self.state[2]) - control_input = self.LQR_controller.calculate_control_input(self.state, self.x_ref) - wrench_msg = Wrench() - wrench_msg.force.x = control_input[0] - wrench_msg.force.y = control_input[1] - wrench_msg.torque.z = control_input[2] - self.wrench_publisher_.publish(wrench_msg) - -def main(args=None): - rclpy.init(args=args) - node = LQRControllerNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/motion/lqr_controller/package.xml b/motion/lqr_controller/package.xml deleted file mode 100644 index 55cd3a82..00000000 --- a/motion/lqr_controller/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - lqr_controller - 0.0.0 - This is an implementation of the LQR DP controller for the ASV - vortex - MIT - - ament_cmake_python - - rclpy - python-transforms3d-pip - nav_msgs - geometry_msgs - vortex_msgs - std_msgs - numpy - matplotlib - scipy - - python3-pytest - - - ament_cmake - - diff --git a/motion/pid_controller/CMakeLists.txt b/motion/pid_controller/CMakeLists.txt deleted file mode 100644 index dffae1df..00000000 --- a/motion/pid_controller/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(pid_controller) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - pid_controller/pid_controller_node.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() \ No newline at end of file diff --git a/motion/pid_controller/config/pid_config.yaml b/motion/pid_controller/config/pid_config.yaml deleted file mode 100644 index f600f9f0..00000000 --- a/motion/pid_controller/config/pid_config.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - pid_controller: - Kp: [130., 240., 60.] # Proportional costs for [x, y, heading] - Ki: [15., 29., 7.] # Integral costs for [x, y, heading] - Kd: [160., 300., 75.] # Derivative costs for [x, y, heading] \ No newline at end of file diff --git a/motion/pid_controller/launch/pid_controller.launch.py b/motion/pid_controller/launch/pid_controller.launch.py deleted file mode 100755 index 465924a9..00000000 --- a/motion/pid_controller/launch/pid_controller.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - pid_controller_node = Node( - package='pid_controller', - executable='pid_controller_node.py', - name='pid_controller_node', - parameters=[ - os.path.join(get_package_share_directory('pid_controller'),'config','pid_config.yaml'), - os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') - ], - output='screen', - ) - return LaunchDescription([ - pid_controller_node - ]) diff --git a/motion/pid_controller/package.xml b/motion/pid_controller/package.xml deleted file mode 100644 index 1cb6be76..00000000 --- a/motion/pid_controller/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - pid_controller - 0.0.0 - This is an implementation of the PID DP controller for the ASV - vortex - MIT - - ament_cmake_python - - rclpy - python-transforms3d-pip - nav_msgs - geometry_msgs - vortex_msgs - std_msgs - numpy - matplotlib - scipy - - python3-pytest - - - ament_cmake - - diff --git a/motion/pid_controller/pid_controller/__init__.py b/motion/pid_controller/pid_controller/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/pid_controller/pid_controller/conversions.py b/motion/pid_controller/pid_controller/conversions.py deleted file mode 100644 index 92f14cfb..00000000 --- a/motion/pid_controller/pid_controller/conversions.py +++ /dev/null @@ -1,59 +0,0 @@ -import numpy as np - -from transforms3d.euler import euler2quat, quat2euler -from nav_msgs.msg import Odometry -from geometry_msgs.msg import PoseStamped - - -def odometrymsg_to_state(msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation - orientation_list = [ - orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z - ] - - # Convert quaternion to Euler angles, extract yaw - yaw = quat2euler(orientation_list)[2] - - vx = msg.twist.twist.linear.x - vy = msg.twist.twist.linear.y - vyaw = msg.twist.twist.angular.z - - state = np.array([x, y, yaw, vx, vy, vyaw]) - return state - -def state_to_odometrymsg(state): - orientation_list_next = euler2quat(0, 0, state[2]) - - odometry_msg = Odometry() - odometry_msg.pose.pose.position.x = state[0] - odometry_msg.pose.pose.position.y = state[1] - odometry_msg.pose.pose.position.z = 0.0 - odometry_msg.pose.pose.orientation.w = orientation_list_next[0] - odometry_msg.pose.pose.orientation.x = orientation_list_next[1] - odometry_msg.pose.pose.orientation.y = orientation_list_next[2] - odometry_msg.pose.pose.orientation.z = orientation_list_next[3] - - return odometry_msg - -def state_to_posestamped(state, frame_id, stamp): - orientation_list_next = euler2quat(0, 0, state[2]) - - posestamped_msg = PoseStamped() - - posestamped_msg.header.frame_id = frame_id - posestamped_msg.header.stamp = stamp - - posestamped_msg.pose.position.x = state[0] - posestamped_msg.pose.position.y = state[1] - posestamped_msg.pose.position.z = 0.0 - posestamped_msg.pose.orientation.w = orientation_list_next[0] - posestamped_msg.pose.orientation.x = orientation_list_next[1] - posestamped_msg.pose.orientation.y = orientation_list_next[2] - posestamped_msg.pose.orientation.z = orientation_list_next[3] - - return posestamped_msg - -def ssa(angle): - return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/pid_controller/pid_controller/pid_controller.py b/motion/pid_controller/pid_controller/pid_controller.py deleted file mode 100644 index 29624c66..00000000 --- a/motion/pid_controller/pid_controller/pid_controller.py +++ /dev/null @@ -1,62 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np - -class PID: - def __init__(self): - self.error_sum = np.zeros(3) - - self.Kp = np.eye(3) - self.Ki = np.eye(3) - self.Kd = np.eye(3) - - self.upper = np.array([40, 40, 40]) - self.lower = np.array([-40, -40, -40]) - - # self.tau_max = np.array([100, 100, 100]) - - def update_parameters(self, Kp, Ki, Kd): - self.Kp = np.diag(Kp) - self.Ki = np.diag(Ki) - self.Kd = np.diag(Kd) - - def calculate_control_input(self, eta, eta_d, eta_dot, dt): - error = eta - eta_d - self.error_sum += error * dt - self.error_sum = np.clip(self.error_sum, -20, 20) - - p = self.Kp @ error - i = self.Ki @ self.error_sum - d = self.Kd @ eta_dot - - self.last_error = error - - tau = -(p + i + d) - - # if tau[0] > self.tau_max[0]: - # tau[0] = self.tau_max[0] - # elif tau[0] < -self.tau_max[0]: - # tau[0] = -self.tau_max[0] - - # if tau[1] > self.tau_max[1]: - # tau[1] = self.tau_max[1] - # elif tau[1] < -self.tau_max[1]: - # tau[1] = -self.tau_max[1] - - # if tau[2] > self.tau_max[2]: - # tau[2] = self.tau_max[2] - # elif tau[2] < -self.tau_max[2]: - # tau[2] = -self.tau_max[2] - - return tau - - @staticmethod - def cap_array(arr: np.ndarray, lower: np.ndarray, upper: np.ndarray): - new_arr = np.zeros(3) - for i in range(len(arr)): - if arr[i] > upper[i]: - new_arr[i] = upper[i] - elif arr[i] < lower[i]: - new_arr[i] = lower - else: - new_arr[i] = arr[i] diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py deleted file mode 100755 index a8927ea0..00000000 --- a/motion/pid_controller/pid_controller/pid_controller_node.py +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -import numpy as np -from rclpy.node import Node -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -from pid_controller import PID -from conversions import odometrymsg_to_state -from time import sleep -from rcl_interfaces.msg import SetParametersResult - -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class PIDControllerNode(Node): - def __init__(self): - super().__init__("pid_controller_node") - - self.declare_parameters( - namespace='', - parameters=[ - ('pid_controller.Kp', [1.0, 1.0, 1.0]), - ('pid_controller.Ki', [1.0, 1.0, 1.0]), - ('pid_controller.Kd', [1.0, 1.0, 1.0]), - ]) - - self.parameters_updated = False - - self.state_subscriber_ = self.create_subscription(Odometry, "/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) - self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) - self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) - - ## PID TUNING VALUES ## (OVERWRITES YAML FILE VALUES) - # M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value - - # M = np.reshape(M, (3, 3)) - # M_diag = np.diag(M) - # omega_n = 1.2 - # zeta = 0.75 - # Kp = M_diag * omega_n**2 - # self.get_logger().info(f"Kp: {Kp}") - # Kd = M_diag * 2 * zeta * omega_n #- D_diag - # self.get_logger().info(f"Kd: {Kd}") - # Ki = omega_n/10 * Kp - # self.get_logger().info(f"Ki: {Ki}") - - self.pid_controller = PID() - - self.update_controller_parameters() - - self.enabled = False - - self.controller_period = 0.1 - self.controller_timer_ = self.create_timer(self.controller_period, self.controller_callback) - self.add_on_set_parameters_callback(self.parameter_callback) - - self.get_logger().info("pid_controller_node started") - - def update_controller_parameters(self): - self.pid_controller.error_sum = np.zeros(3) - - Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value - Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value - Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value - - self.pid_controller.update_parameters(Kp, Ki, Kd) - - self.get_logger().info("Updated controller parameters") - - def parameter_callback(self, params): - self.parameters_updated = True - for param in params: - if param.name == 'pid_controller.Kp': - self.get_logger().info(f"Updated Kp to {param.value}") - elif param.name == 'pid_controller.Ki': - self.get_logger().info(f"Updated Ki to {param.value}") - elif param.name == 'pid_controller.Kd': - self.get_logger().info(f"Updated Kd to {param.value}") - - return SetParametersResult(successful=True) - - def state_cb(self, msg): - self.state = odometrymsg_to_state(msg) - - def guidance_cb(self, msg): - self.x_ref = odometrymsg_to_state(msg)[:3] - - def controller_callback(self): - if hasattr(self, 'state') and hasattr(self, 'x_ref'): - control_input = self.pid_controller.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period) - # self.get_logger().info(f"Control input: {control_input}") - wrench_msg = Wrench() - wrench_msg.force.x = control_input[0] - wrench_msg.force.y = control_input[1] - wrench_msg.torque.z = control_input[2] - self.wrench_publisher_.publish(wrench_msg) - -def main(args=None): - rclpy.init(args=args) - node = PIDControllerNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() From 4a3e8b952882b4a0e3de2b5221cae489dcc7073e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 13:23:12 +0200 Subject: [PATCH 58/67] Remove unused files from install --- guidance/hybridpath_guidance/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt index c85f4eea..18909c31 100644 --- a/guidance/hybridpath_guidance/CMakeLists.txt +++ b/guidance/hybridpath_guidance/CMakeLists.txt @@ -22,8 +22,6 @@ install(DIRECTORY install(PROGRAMS hybridpath_guidance/hybridpath_guidance_node.py - hybridpath_guidance/waypoint_node.py - hybridpath_guidance/noisy_odom_publisher.py DESTINATION lib/${PROJECT_NAME} ) From 3d0e8b987143a02b336c27385fafa2afd7dc1fe0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 13:24:43 +0200 Subject: [PATCH 59/67] Updated waypoint service call example --- guidance/hybridpath_guidance/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/guidance/hybridpath_guidance/README.md b/guidance/hybridpath_guidance/README.md index 3a512865..2c503518 100755 --- a/guidance/hybridpath_guidance/README.md +++ b/guidance/hybridpath_guidance/README.md @@ -9,7 +9,7 @@ Or alternatively, run it together with the hybridpath controller using the launc To run with custom waypoints (replace example waypoints with actual waypoints, and add as many prefered): -`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 0.0, y: 0.0, z: 0.0}, {x: 5.0, y: 5.0, z: 0.0}]"` +`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 5.0, y: 5.0, z: 0.0}]"` ## Configuration From 57ff9e62f9ab4c6a15e03643a65090ec0c432d83 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 15:41:01 +0200 Subject: [PATCH 60/67] Boolean value for when the built in heading reference for the path should be used --- .../hybridpath_guidance/hybridpath_guidance_node.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 49fc5e4a..30268f34 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -53,7 +53,8 @@ def __init__(self): self.u_desired = 1.1 # Desired velocity - self.heading_ref = 0. # Desired heading, 100 if hybridpath heading, else can be set by service call + self.use_hybridpath_heading = True + self.heading_ref = 0. # Initialize variables self.waypoints = [] @@ -188,8 +189,9 @@ def guidance_callback(self): pos_der = self.signals.get_derivatives()[0] pos_dder = self.signals.get_derivatives()[1] - if self.heading_ref == 100.: + if self.use_hybridpath_heading and self.path is not None: psi = self.signals.get_heading() + self.heading_ref = psi else: psi = self.heading_ref From cd33ea538e2ad20279b42d1ded92595a128ebfd1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 15:41:24 +0200 Subject: [PATCH 61/67] Removed commented code --- guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py index 1453314d..39768568 100644 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py @@ -359,8 +359,6 @@ class HybridPathSignals: s (float): The path parameter. """ def __init__(self): - # if not isinstance(path, Path): - # raise TypeError("path must be an instance of Path") self.path = None self.s = None From bcef88885cda2c625d8c9c8a5ba9f3fb0075c828 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 15:48:00 +0200 Subject: [PATCH 62/67] Removed unncecessary if else --- .../hybridpath_guidance_node.py | 87 ++++++++++--------- 1 file changed, 44 insertions(+), 43 deletions(-) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 30268f34..ee42402c 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -167,60 +167,61 @@ def guidance_callback(self): if self.killswitch_active or self.operational_mode != 'autonomous mode': return - if self.initial_pos: - if self.path is None and not self.waypoints_received: - if not self.stationkeeping_flag: - self.get_logger().info(f'No waypoints received, stationkeeping at {np.round(self.eta_stationkeeping, 3)}') - self.stationkeeping_flag = True - - pos = [self.eta_stationkeeping[0], self.eta_stationkeeping[1]] - pos_der = [0., 0.] - pos_dder = [0., 0.] + if not self.initial_pos: + if not self.waiting_message_printed: + self.get_logger().info('Waiting for eta') + self.waiting_message_printed = True + + return + + if self.path is None and not self.waypoints_received: + if not self.stationkeeping_flag: + self.get_logger().info(f'No waypoints received, stationkeeping at {np.round(self.eta_stationkeeping, 3)}') + self.stationkeeping_flag = True - else: - self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) - self.signals.update_s(self.s) - self.w = self.signals.get_w(self.mu, self.eta) - self.v_s = self.signals.get_vs(self.u_desired) - self.v_ss = self.signals.get_vs_derivative(self.u_desired) + pos = [self.eta_stationkeeping[0], self.eta_stationkeeping[1]] + pos_der = [0., 0.] + pos_dder = [0., 0.] - pos = self.signals.get_position() + else: + self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) + self.signals.update_s(self.s) + self.w = self.signals.get_w(self.mu, self.eta) + self.v_s = self.signals.get_vs(self.u_desired) + self.v_ss = self.signals.get_vs_derivative(self.u_desired) - pos_der = self.signals.get_derivatives()[0] - pos_dder = self.signals.get_derivatives()[1] + pos = self.signals.get_position() - if self.use_hybridpath_heading and self.path is not None: - psi = self.signals.get_heading() - self.heading_ref = psi + pos_der = self.signals.get_derivatives()[0] + pos_dder = self.signals.get_derivatives()[1] - else: - psi = self.heading_ref + if self.use_hybridpath_heading and self.path is not None: + psi = self.signals.get_heading() + self.heading_ref = psi - psi_der = 0.#signals.get_heading_derivative() - psi_dder = 0.#signals.get_heading_second_derivative() + else: + psi = self.heading_ref - hp_msg = HybridpathReference() - hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) - hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) - hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) + psi_der = 0.#signals.get_heading_derivative() + psi_dder = 0.#signals.get_heading_second_derivative() - hp_msg.w = self.w - hp_msg.v_s = self.v_s - hp_msg.v_ss = self.v_ss + hp_msg = HybridpathReference() + hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) + hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) + hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) - self.guidance_publisher.publish(hp_msg) + hp_msg.w = self.w + hp_msg.v_s = self.v_s + hp_msg.v_ss = self.v_ss - if self.path is not None and self.s >= self.path.NumSubpaths: - self.waypoints_received = False - self.waiting_message_printed = False - self.stationkeeping_flag = False - self.path = None - self.eta_stationkeeping = np.array([self.last_waypoint[0], self.last_waypoint[1], self.heading_ref]) + self.guidance_publisher.publish(hp_msg) - else: - if not self.waiting_message_printed: - self.get_logger().info('Waiting for eta') - self.waiting_message_printed = True + if self.path is not None and self.s >= self.path.NumSubpaths: + self.waypoints_received = False + self.waiting_message_printed = False + self.stationkeeping_flag = False + self.path = None + self.eta_stationkeeping = np.array([self.last_waypoint[0], self.last_waypoint[1], self.heading_ref]) @staticmethod From 883f88da88e51ae4d714ee06112152f998eee2cd Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Sat, 17 Aug 2024 17:39:09 +0200 Subject: [PATCH 63/67] moved map-odom-tf to map_manager and added optional grid usage in landmark server --- .../config/sitaw/land_polygon_grillstad.yaml | 8 -- .../config/sitaw/land_polygon_office.yaml | 16 ---- .../sitaw/land_polygon_sim_maneuvering.yaml | 4 - .../sitaw/land_polygon_sim_navigation.yaml | 5 -- .../config/sitaw/landmark_server_params.yaml | 3 +- asv_setup/config/sitaw/seapath_params.yaml | 5 +- .../include/landmark_server/grid_manager.hpp | 18 +++-- .../params/landmark_server_params.yaml | 3 +- mission/landmark_server/src/grid_manager.cpp | 10 +-- .../landmark_server/src/landmark_server.cpp | 37 +++++---- .../include/map_manager/map_manager_ros.hpp | 13 ++- .../params/map_manager_params.yaml | 13 +-- mission/map_manager/src/map_manager_ros.cpp | 79 ++++++++++++++----- 13 files changed, 114 insertions(+), 100 deletions(-) delete mode 100644 asv_setup/config/sitaw/land_polygon_grillstad.yaml delete mode 100644 asv_setup/config/sitaw/land_polygon_office.yaml delete mode 100644 asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml delete mode 100644 asv_setup/config/sitaw/land_polygon_sim_navigation.yaml diff --git a/asv_setup/config/sitaw/land_polygon_grillstad.yaml b/asv_setup/config/sitaw/land_polygon_grillstad.yaml deleted file mode 100644 index 52cec359..00000000 --- a/asv_setup/config/sitaw/land_polygon_grillstad.yaml +++ /dev/null @@ -1,8 +0,0 @@ -63.437946025438066 10.507175268806458 -63.43787563495969 10.507121157686353 -63.4378360402396 10.50719002638467 -63.43763366637102 10.50697358190425 -63.43760726967404 10.507101480915406 -63.43713432473124 10.506752218231092 -63.437239913116464 10.505876601923934 -63.438056010214204 10.506461985859616 \ No newline at end of file diff --git a/asv_setup/config/sitaw/land_polygon_office.yaml b/asv_setup/config/sitaw/land_polygon_office.yaml deleted file mode 100644 index 90a4898a..00000000 --- a/asv_setup/config/sitaw/land_polygon_office.yaml +++ /dev/null @@ -1,16 +0,0 @@ -63.41489120183045 10.39705552070149 -63.41497265487507 10.39783732516908 -63.41538547016246 10.397651181248118 -63.415431749532054 10.398250978326427 -63.415048554100395 10.398449531842004 -63.41512075073269 10.399185834462275 -63.41495229164123 10.399305793877936 -63.41486158248972 10.398540535536645 -63.41453021396673 10.398747362115373 -63.4145246602717 10.398631539231284 -63.414280296625655 10.39877218130482 -63.41424142039905 10.398197203415958 -63.4144598670836 10.398110336252891 -63.41444505719146 10.397990376837232 -63.41478938520484 10.397841461700548 -63.41472089017681 10.397167207053899 \ No newline at end of file diff --git a/asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml b/asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml deleted file mode 100644 index b7b44874..00000000 --- a/asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml +++ /dev/null @@ -1,4 +0,0 @@ --33.722464875303054 150.6733186790048 --33.720864277234156 150.673353826611 --33.72115033638088 150.67502276015122 --33.72271743627459 150.67461276680072 \ No newline at end of file diff --git a/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml b/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml deleted file mode 100644 index 63c9117a..00000000 --- a/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml +++ /dev/null @@ -1,5 +0,0 @@ --33.722576743831986 150.67377683501326 --33.72213785615945 150.6737273159938 --33.722119332734295 150.6744244880215 --33.72265441348879 150.6743192836287 - \ No newline at end of file diff --git a/asv_setup/config/sitaw/landmark_server_params.yaml b/asv_setup/config/sitaw/landmark_server_params.yaml index 92002733..9d31f351 100644 --- a/asv_setup/config/sitaw/landmark_server_params.yaml +++ b/asv_setup/config/sitaw/landmark_server_params.yaml @@ -2,4 +2,5 @@ landmark_server_node: ros__parameters: fixed_frame: map target_frame: base_link - wall_width: 0.1 \ No newline at end of file + wall_width: 0.1 + use_grid: false \ No newline at end of file diff --git a/asv_setup/config/sitaw/seapath_params.yaml b/asv_setup/config/sitaw/seapath_params.yaml index a5406a83..fc8848fb 100644 --- a/asv_setup/config/sitaw/seapath_params.yaml +++ b/asv_setup/config/sitaw/seapath_params.yaml @@ -1,7 +1,4 @@ seapath_ros_driver_node: ros__parameters: UDP_IP: "10.0.1.10" # Source IP of the host sender - UDP_PORT: 31421 # Port at which to receive UDP semgents - use_predef_map_origin: false - map_origin_lat: 0.0 - map_origin_lon: 0.0 + UDP_PORT: 31421 # Port at which to receive UDP segment diff --git a/mission/landmark_server/include/landmark_server/grid_manager.hpp b/mission/landmark_server/include/landmark_server/grid_manager.hpp index 0f2a78f1..539fc043 100644 --- a/mission/landmark_server/include/landmark_server/grid_manager.hpp +++ b/mission/landmark_server/include/landmark_server/grid_manager.hpp @@ -16,20 +16,28 @@ class GridManager { ~GridManager() = default; + /** + * @brief Update the grid with a polygon and a value to update corresponding cells with. + */ void update_grid(int8_t *grid, - const Eigen::Array &polygon, + const Eigen::Array &vertices, int value); - void handle_polygon(int8_t *grid, - const Eigen::Array &vertices, - int value); - + /** + * @brief Draw a line on the grid and update the value of the cells it passes through. + */ void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value); + /** + * @brief Fill a polygon on the grid and update the value of the cells it covers. + */ void fill_polygon(int8_t *grid, const Eigen::Array &polygon, int value); + /** + * @brief Check if a point is inside a polygon. + */ bool point_in_polygon(int x, int y, const Eigen::Array &polygon); diff --git a/mission/landmark_server/params/landmark_server_params.yaml b/mission/landmark_server/params/landmark_server_params.yaml index c334181d..c95a6384 100644 --- a/mission/landmark_server/params/landmark_server_params.yaml +++ b/mission/landmark_server/params/landmark_server_params.yaml @@ -2,4 +2,5 @@ landmark_server_node: ros__parameters: fixed_frame: world target_frame: base_link - wall_width: 0.1 \ No newline at end of file + wall_width: 0.1 + use_grid: false \ No newline at end of file diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp index cad2313a..5e67b56d 100644 --- a/mission/landmark_server/src/grid_manager.cpp +++ b/mission/landmark_server/src/grid_manager.cpp @@ -7,12 +7,6 @@ GridManager::GridManager(float resolution, uint32_t height, uint32_t width) : resolution_(resolution), height_(height), width_(width) {} void GridManager::update_grid( - int8_t *grid, const Eigen::Array &polygon, - int value) { - handle_polygon(grid, polygon, value); -} - -void GridManager::handle_polygon( int8_t *grid, const Eigen::Array &vertices, int value) { // Convert to grid coordinates @@ -41,10 +35,8 @@ void GridManager::fill_polygon( for (int x = min_x; x < max_x; x++) { for (int y = min_y; y < max_y; y++) { if (x >= 0 && x < static_cast(width_) && y >= 0 && - y < static_cast(height_)) { - if (point_in_polygon(x, y, polygon)) { + y < static_cast(height_) && point_in_polygon(x, y, polygon)) { grid[y * width_ + x] += value; - } } } } diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp index c4b896bb..7300126c 100644 --- a/mission/landmark_server/src/landmark_server.cpp +++ b/mission/landmark_server/src/landmark_server.cpp @@ -59,6 +59,7 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) declare_parameter("fixed_frame", "world"); declare_parameter("target_frame", "base_link"); declare_parameter("wall_width", 0.1); + declare_parameter("use_grid", "false"); // Create the act. action_server_ = rclcpp_action::create_server( @@ -113,19 +114,13 @@ Eigen::Array LandmarkServerNode::get_convex_hull( cluster.points[i].y = landmark.shape.polygon.points[i].y; cluster.points[i].z = 1.0; } - sensor_msgs::msg::PointCloud2 cluster_msg; - pcl::toROSMsg(cluster, cluster_msg); - cluster_msg.header.frame_id = get_parameter("fixed_frame").as_string(); - cluster_publisher_->publish(cluster_msg); + // Compute the convex hull of the cluster pcl::PointCloud convex_hull; pcl::ConvexHull chull; chull.setDimension(2); chull.setInputCloud(cluster.makeShared()); chull.reconstruct(convex_hull); - sensor_msgs::msg::PointCloud2 hull_msg; - pcl::toROSMsg(convex_hull, hull_msg); - hull_msg.header.frame_id = get_parameter("fixed_frame").as_string(); - convex_hull_publisher_->publish(hull_msg); + Eigen::Array hull(2, convex_hull.size()); for (size_t i = 0; i < convex_hull.size(); i++) { hull(0, i) = convex_hull.points[i].x; @@ -184,7 +179,7 @@ void LandmarkServerNode::landmarksRecievedCallback( return; } - if (grid_manager_ == nullptr) { + if (this->get_parameter("use_grid").as_bool() && grid_manager_ == nullptr) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Grid manager not initialized"); return; @@ -205,8 +200,11 @@ void LandmarkServerNode::landmarksRecievedCallback( "Requested to add already existing landmark"); } else { // Add the new landmark - grid_manager_->update_grid(grid_msg_.data.data(), - get_convex_hull(landmark), 200); + + if (this->get_parameter("use_grid").as_bool()) { + grid_manager_->update_grid(grid_msg_.data.data(), + get_convex_hull(landmark), 200); + } storedLandmarks_->landmarks.push_back(landmark); } continue; @@ -218,14 +216,18 @@ void LandmarkServerNode::landmarksRecievedCallback( continue; } - grid_manager_->update_grid(grid_msg_.data.data(), get_convex_hull((*it)), - -200); + if (this->get_parameter("use_grid").as_bool()) { + grid_manager_->update_grid(grid_msg_.data.data(), + get_convex_hull(landmark), -200); + } if (landmark.action == vortex_msgs::msg::Landmark::REMOVE_ACTION) { storedLandmarks_->landmarks.erase(it); } else if (landmark.action == vortex_msgs::msg::Landmark::UPDATE_ACTION) { - grid_manager_->update_grid(grid_msg_.data.data(), - get_convex_hull(landmark), 200); + if (this->get_parameter("use_grid").as_bool()) { + grid_manager_->update_grid(grid_msg_.data.data(), + get_convex_hull(landmark), 200); + } *it = landmark; } } @@ -354,9 +356,10 @@ vortex_msgs::msg::OdometryArray LandmarkServerNode::filterLandmarks( for (const auto &landmark : storedLandmarks_->landmarks) { - if (filter_type == 6 && distance == 0.0) { + // filter_type 0 returns all landmarks + if (filter_type == 0 && distance == 0.0) { filteredLandmarksOdoms.odoms.push_back(landmark.odom); - } else if (filter_type == 6) { + } else if (filter_type == 0) { if (calculateDistance(landmark.odom.pose.pose.position, landmark.odom.header) <= distance) { diff --git a/mission/map_manager/include/map_manager/map_manager_ros.hpp b/mission/map_manager/include/map_manager/map_manager_ros.hpp index f8065220..9bd9b0c9 100644 --- a/mission/map_manager/include/map_manager/map_manager_ros.hpp +++ b/mission/map_manager/include/map_manager/map_manager_ros.hpp @@ -6,6 +6,8 @@ #include #include #include +#include "geometry_msgs/msg/transform_stamped.hpp" +#include #include #include @@ -45,6 +47,13 @@ class MapManagerNode : public rclcpp::Node { constexpr double deg2rad(double degrees) const; std::array lla2flat(double lat, double lon) const; std::array flat2lla(double px, double py) const; + /** + * @brief Publishes static transform from world NED to world SEU to use for foxglove visualization. + * + */ + void publish_foxglove_vis_frame(const rclcpp::Time& time) const; + + void publish_map_to_odom_tf(double map_lat, double map_lon, const rclcpp::Time& time) const; geometry_msgs::msg::PolygonStamped processCoordinates(const std::vector> &coordinates); @@ -62,17 +71,19 @@ class MapManagerNode : public rclcpp::Node { void fillOutsidePolygon(nav_msgs::msg::OccupancyGrid &grid, const geometry_msgs::msg::PolygonStamped &polygon); - rclcpp::Subscription::SharedPtr map_origin_sub_; + rclcpp::Subscription::SharedPtr odom_origin_sub_; rclcpp::Publisher::SharedPtr map_pub_; rclcpp::Publisher::SharedPtr landmask_pub_; rclcpp::Service::SharedPtr grid_service_; + rclcpp::Publisher::SharedPtr map_origin_pub_; bool map_origin_set_ = false; bool use_predef_landmask_; double map_origin_lat_; double map_origin_lon_; std::string landmask_file_; std::string grid_frame_id_; + std::shared_ptr static_tf_broadcaster_; }; } // namespace map_manager diff --git a/mission/map_manager/params/map_manager_params.yaml b/mission/map_manager/params/map_manager_params.yaml index 992cda10..559e5a3e 100644 --- a/mission/map_manager/params/map_manager_params.yaml +++ b/mission/map_manager/params/map_manager_params.yaml @@ -1,15 +1,10 @@ map_manager_node: ros__parameters: - use_predef_landmask: true - landmask_file: "src/vortex-asv/mission/map_manager/params/land_polygon.yaml" + use_predef_landmask: false map_resolution: 0.2 map_width: 1000 map_height: 1000 frame_id: "world" - # Map origin for office rosbag - map_origin_lat: 63.414660884931976 - map_origin_lon: 10.398554661537544 - use_predef_map_origin: true - # map_origin_lat: 0.0 - # map_origin_lon: 0.0 - # use_predef_map_origin: false + map_origin_lat: 0.0 + map_origin_lon: 0.0 + use_predef_map_origin: false diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index 37491c00..bed3c5e9 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -30,6 +30,9 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) map_pub_ = this->create_publisher( "map", qos_transient_local); + map_origin_pub_ = this->create_publisher("/map/origin", qos_transient_local); + + if (this->get_parameter("use_predef_map_origin").as_bool()) { map_origin_lat_ = this->get_parameter("map_origin_lat").as_double(); map_origin_lon_ = this->get_parameter("map_origin_lon").as_double(); @@ -45,8 +48,8 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) map_pub_->publish(grid); } else { - map_origin_sub_ = this->create_subscription( - "/map/origin", qos_transient_local, + odom_origin_sub_ = this->create_subscription( + "/oodm/origin", qos_transient_local, std::bind(&MapManagerNode::mapOriginCallback, this, std::placeholders::_1)); } @@ -63,10 +66,12 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) void MapManagerNode::mapOriginCallback( const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - map_origin_lat_ = msg->latitude; - map_origin_lon_ = msg->longitude; + double odom_origin_lat = msg->latitude; + double odom_origin_lon = msg->longitude; map_origin_set_ = true; - map_origin_sub_ = nullptr; + odom_origin_sub_ = nullptr; + publish_map_to_odom_tf(odom_origin_lat, odom_origin_lon, msg->header.stamp); + publish_foxglove_vis_frame(msg->header.stamp); if (this->get_parameter("use_predef_landmask").as_bool()) { landmask_pub_->publish(readPolygonFromFile(landmask_file_)); } @@ -135,6 +140,54 @@ std::array MapManagerNode::lla2flat(double lat, double lon) const { return {px, py}; } + void MapManagerNode::publish_map_to_odom_tf(double map_lat, double map_lon, const rclcpp::Time& time) const + { + // Setup the transform + geometry_msgs::msg::TransformStamped transformStamped; + + transformStamped.header.stamp = time; + transformStamped.header.frame_id = "map"; + transformStamped.child_frame_id = "odom"; + + auto [x, y] = lla2flat(map_lat, map_lon); + + transformStamped.transform.translation.x = -x; + transformStamped.transform.translation.y = -y; + transformStamped.transform.translation.z = 0; + + transformStamped.transform.rotation.x = 0.0; + transformStamped.transform.rotation.y = 0.0; + transformStamped.transform.rotation.z = 0.0; + transformStamped.transform.rotation.w = 1.0; + + // Broadcast the static transform + static_tf_broadcaster_->sendTransform(transformStamped); + + } + + void MapManagerNode::publish_foxglove_vis_frame(const rclcpp::Time& time) const + { + // Setup the transform + geometry_msgs::msg::TransformStamped transformStamped; + + transformStamped.header.stamp = time; + transformStamped.header.frame_id = "map"; + transformStamped.child_frame_id = "map_visualization"; + + transformStamped.transform.translation.x = 0.0; + transformStamped.transform.translation.y = 0.0; + transformStamped.transform.translation.z = 0.0; + // NED to SEU + transformStamped.transform.rotation.x = 0.0; + transformStamped.transform.rotation.y = 1.0; + transformStamped.transform.rotation.z = 0.0; + transformStamped.transform.rotation.w = 0.0; + + // Broadcast the static transform + static_tf_broadcaster_->sendTransform(transformStamped); + + } + std::array MapManagerNode::flat2lla(double px, double py) const { // Earth constants const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) @@ -215,19 +268,12 @@ nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() { grid.info.height = get_parameter("map_height").as_int(); grid.info.origin = calculate_map_origin(); grid.info.map_load_time = this->now(); - // 0 represents unoccupied, 1 represents definitely occupied, and - // -1 represents unknown. - // Set all to unoccupied + // initialize grid with zeros grid.data.resize(grid.info.width * grid.info.height, 0); return grid; } geometry_msgs::msg::Pose MapManagerNode::calculate_map_origin() { - // 63.44098584906518, 10.42304390021551 - // 63.41490901857848, 10.398215601285054 - // Office traffic centre origin - // map_origin_lat_ = 63.41490901857848; - // map_origin_lon_ = 10.398215601285054; // Map centre is (0,0) in map frame, origin is bottom left corner double half_width_meters = -(get_parameter("map_width").as_int() * @@ -236,7 +282,6 @@ geometry_msgs::msg::Pose MapManagerNode::calculate_map_origin() { double half_height_meters = -(get_parameter("map_height").as_int() * get_parameter("map_resolution").as_double()) / 2.0; - // auto [lat, lon] = flat2lla(half_width_meters, half_height_meters); geometry_msgs::msg::Pose map_origin; map_origin.position.x = half_width_meters; map_origin.position.y = half_height_meters; @@ -338,22 +383,16 @@ void MapManagerNode::insert_landmask( int value = 100; // Set this to the desired occupancy value for land (0-100) for (uint i = 0; i < polygon.polygon.points.size(); i++) { - // if(i > 1){break;} const geometry_msgs::msg::Point32 ¤t = polygon.polygon.points[i]; const geometry_msgs::msg::Point32 &next = polygon.polygon .points[(i + 1) % polygon.polygon.points.size()]; // Loop back to // the first point - // std::cout << "current: " << current.x << " " << current.y << " next: " << - // next.x << " " << next.y << "\n"; - // Convert map xy to grid indices int x0 = current.x / grid.info.resolution + grid.info.width / 2; int y0 = current.y / grid.info.resolution + grid.info.height / 2; int x1 = next.x / grid.info.resolution + grid.info.width / 2; int y1 = next.y / grid.info.resolution + grid.info.height / 2; - // std::cout << "x0: " << x0 << " y0: " << y0 << " x1: " << x1 << " y1: " << - // y1 << "\n"; grid.data[x0 + y0 * grid.info.width] = value; // Draw line on the grid drawLine(x0, y0, x1, y1, grid, value); From b65b35a13fefbe368a3250cab7011fb6fae39dd0 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Sat, 17 Aug 2024 15:39:34 +0000 Subject: [PATCH 64/67] Committing clang-format changes --- .../include/landmark_server/grid_manager.hpp | 9 +- mission/landmark_server/src/grid_manager.cpp | 2 +- .../include/map_manager/map_manager_ros.hpp | 18 ++-- mission/map_manager/src/map_manager_ros.cpp | 84 +++++++++---------- 4 files changed, 58 insertions(+), 55 deletions(-) diff --git a/mission/landmark_server/include/landmark_server/grid_manager.hpp b/mission/landmark_server/include/landmark_server/grid_manager.hpp index 539fc043..8bf339a5 100644 --- a/mission/landmark_server/include/landmark_server/grid_manager.hpp +++ b/mission/landmark_server/include/landmark_server/grid_manager.hpp @@ -17,19 +17,22 @@ class GridManager { ~GridManager() = default; /** - * @brief Update the grid with a polygon and a value to update corresponding cells with. + * @brief Update the grid with a polygon and a value to update corresponding + * cells with. */ void update_grid(int8_t *grid, const Eigen::Array &vertices, int value); /** - * @brief Draw a line on the grid and update the value of the cells it passes through. + * @brief Draw a line on the grid and update the value of the cells it passes + * through. */ void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value); /** - * @brief Fill a polygon on the grid and update the value of the cells it covers. + * @brief Fill a polygon on the grid and update the value of the cells it + * covers. */ void fill_polygon(int8_t *grid, const Eigen::Array &polygon, diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp index 5e67b56d..b8df5637 100644 --- a/mission/landmark_server/src/grid_manager.cpp +++ b/mission/landmark_server/src/grid_manager.cpp @@ -36,7 +36,7 @@ void GridManager::fill_polygon( for (int y = min_y; y < max_y; y++) { if (x >= 0 && x < static_cast(width_) && y >= 0 && y < static_cast(height_) && point_in_polygon(x, y, polygon)) { - grid[y * width_ + x] += value; + grid[y * width_ + x] += value; } } } diff --git a/mission/map_manager/include/map_manager/map_manager_ros.hpp b/mission/map_manager/include/map_manager/map_manager_ros.hpp index 9bd9b0c9..41797e1c 100644 --- a/mission/map_manager/include/map_manager/map_manager_ros.hpp +++ b/mission/map_manager/include/map_manager/map_manager_ros.hpp @@ -1,12 +1,12 @@ #ifndef MAP_MANAGER_HPP #define MAP_MANAGER_HPP +#include "geometry_msgs/msg/transform_stamped.hpp" #include "nav_msgs/srv/get_map.hpp" #include #include #include #include -#include "geometry_msgs/msg/transform_stamped.hpp" #include #include @@ -47,13 +47,15 @@ class MapManagerNode : public rclcpp::Node { constexpr double deg2rad(double degrees) const; std::array lla2flat(double lat, double lon) const; std::array flat2lla(double px, double py) const; - /** - * @brief Publishes static transform from world NED to world SEU to use for foxglove visualization. - * - */ - void publish_foxglove_vis_frame(const rclcpp::Time& time) const; + /** + * @brief Publishes static transform from world NED to world SEU to use for + * foxglove visualization. + * + */ + void publish_foxglove_vis_frame(const rclcpp::Time &time) const; - void publish_map_to_odom_tf(double map_lat, double map_lon, const rclcpp::Time& time) const; + void publish_map_to_odom_tf(double map_lat, double map_lon, + const rclcpp::Time &time) const; geometry_msgs::msg::PolygonStamped processCoordinates(const std::vector> &coordinates); @@ -76,7 +78,7 @@ class MapManagerNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr landmask_pub_; rclcpp::Service::SharedPtr grid_service_; - rclcpp::Publisher::SharedPtr map_origin_pub_; + rclcpp::Publisher::SharedPtr map_origin_pub_; bool map_origin_set_ = false; bool use_predef_landmask_; double map_origin_lat_; diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp index bed3c5e9..1552209c 100644 --- a/mission/map_manager/src/map_manager_ros.cpp +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -30,8 +30,8 @@ MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) map_pub_ = this->create_publisher( "map", qos_transient_local); - map_origin_pub_ = this->create_publisher("/map/origin", qos_transient_local); - + map_origin_pub_ = this->create_publisher( + "/map/origin", qos_transient_local); if (this->get_parameter("use_predef_map_origin").as_bool()) { map_origin_lat_ = this->get_parameter("map_origin_lat").as_double(); @@ -140,53 +140,51 @@ std::array MapManagerNode::lla2flat(double lat, double lon) const { return {px, py}; } - void MapManagerNode::publish_map_to_odom_tf(double map_lat, double map_lon, const rclcpp::Time& time) const - { - // Setup the transform - geometry_msgs::msg::TransformStamped transformStamped; - - transformStamped.header.stamp = time; - transformStamped.header.frame_id = "map"; - transformStamped.child_frame_id = "odom"; - - auto [x, y] = lla2flat(map_lat, map_lon); - - transformStamped.transform.translation.x = -x; - transformStamped.transform.translation.y = -y; - transformStamped.transform.translation.z = 0; - - transformStamped.transform.rotation.x = 0.0; - transformStamped.transform.rotation.y = 0.0; - transformStamped.transform.rotation.z = 0.0; - transformStamped.transform.rotation.w = 1.0; +void MapManagerNode::publish_map_to_odom_tf(double map_lat, double map_lon, + const rclcpp::Time &time) const { + // Setup the transform + geometry_msgs::msg::TransformStamped transformStamped; - // Broadcast the static transform - static_tf_broadcaster_->sendTransform(transformStamped); + transformStamped.header.stamp = time; + transformStamped.header.frame_id = "map"; + transformStamped.child_frame_id = "odom"; - } - - void MapManagerNode::publish_foxglove_vis_frame(const rclcpp::Time& time) const - { - // Setup the transform - geometry_msgs::msg::TransformStamped transformStamped; + auto [x, y] = lla2flat(map_lat, map_lon); - transformStamped.header.stamp = time; - transformStamped.header.frame_id = "map"; - transformStamped.child_frame_id = "map_visualization"; + transformStamped.transform.translation.x = -x; + transformStamped.transform.translation.y = -y; + transformStamped.transform.translation.z = 0; - transformStamped.transform.translation.x = 0.0; - transformStamped.transform.translation.y = 0.0; - transformStamped.transform.translation.z = 0.0; - // NED to SEU - transformStamped.transform.rotation.x = 0.0; - transformStamped.transform.rotation.y = 1.0; - transformStamped.transform.rotation.z = 0.0; - transformStamped.transform.rotation.w = 0.0; + transformStamped.transform.rotation.x = 0.0; + transformStamped.transform.rotation.y = 0.0; + transformStamped.transform.rotation.z = 0.0; + transformStamped.transform.rotation.w = 1.0; - // Broadcast the static transform - static_tf_broadcaster_->sendTransform(transformStamped); + // Broadcast the static transform + static_tf_broadcaster_->sendTransform(transformStamped); +} - } +void MapManagerNode::publish_foxglove_vis_frame( + const rclcpp::Time &time) const { + // Setup the transform + geometry_msgs::msg::TransformStamped transformStamped; + + transformStamped.header.stamp = time; + transformStamped.header.frame_id = "map"; + transformStamped.child_frame_id = "map_visualization"; + + transformStamped.transform.translation.x = 0.0; + transformStamped.transform.translation.y = 0.0; + transformStamped.transform.translation.z = 0.0; + // NED to SEU + transformStamped.transform.rotation.x = 0.0; + transformStamped.transform.rotation.y = 1.0; + transformStamped.transform.rotation.z = 0.0; + transformStamped.transform.rotation.w = 0.0; + + // Broadcast the static transform + static_tf_broadcaster_->sendTransform(transformStamped); +} std::array MapManagerNode::flat2lla(double px, double py) const { // Earth constants From 04c0bcc1ab82177f268df81636b3d7b8f99e4798 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 17:40:58 +0200 Subject: [PATCH 65/67] Add coriolis --- .../hybridpath_controller/adaptive_backstep.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py index ecc0008e..891aa677 100644 --- a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py +++ b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py @@ -99,7 +99,7 @@ def calculate_coriolis_matrix(nu: np.ndarray) -> np.ndarray: [0, 0, 5.5], [82.5, -5.5, 0] ]) - return (C_A @ nu) * 0 + return (C_A @ nu) @staticmethod def rotationmatrix_in_yaw_transpose(psi: float) -> np.ndarray: From 422955273656fc80560c7d431ed8d94b0b36defb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sat, 17 Aug 2024 17:42:39 +0200 Subject: [PATCH 66/67] Remove old force mapping --- .../ThrustMe_P1000_force_mapping_original.csv | 190 ------------------ 1 file changed, 190 deletions(-) delete mode 100644 motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping_original.csv diff --git a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping_original.csv b/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping_original.csv deleted file mode 100644 index bac8cf21..00000000 --- a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping_original.csv +++ /dev/null @@ -1,190 +0,0 @@ -"Force(g)","PWM(micros)" --12989.13 1100 --12792.53 1104.49 --12779.61 1108.99 --12635.04 1113.48 --12547.48 1117.98 --12554.38 1122.47 --12528.53 1126.97 --12484.77 1131.46 --12327.03 1135.96 --12337.55 1140.45 --12202.25 1144.94 --12147.19 1149.44 --12097.76 1153.93 --11947.61 1158.43 --11948.59 1162.92 --11878.31 1167.42 --11685.45 1171.91 --11630.87 1176.4 --11660.74 1180.9 --11514.6 1185.39 --11370.6 1189.89 --11293.55 1194.38 --11221.2 1198.88 --11213.27 1203.37 --11064.32 1207.87 --11049.23 1212.36 --10945.55 1216.85 --10837.99 1221.35 --10678.96 1225.84 --10754.31 1230.34 --10588.09 1234.83 --10503.28 1239.33 --10344.9 1243.82 --10226.64 1248.31 --10190.24 1252.81 --10081.85 1257.3 --9959.01 1261.8 --9948.11 1266.29 --9759.36 1270.79 --9753.3 1275.28 --9619.75 1279.78 --9525.81 1284.27 --9362.72 1288.76 --9333.51 1293.26 --9187.35 1297.75 --9078.53 1302.25 --8851.82 1306.74 --8732.14 1311.24 --8632.45 1315.73 --8490.62 1320.22 --8411.14 1324.72 --8312.52 1329.21 --8047.98 1333.71 --7948.19 1338.2 --7934.35 1342.7 --7780.13 1347.19 --7709.89 1351.69 --7375.4 1356.18 --7364.05 1360.67 --7233.1 1365.17 --6972.58 1369.66 --6845.09 1374.16 --6759.8 1378.65 --6633.6 1383.15 --6337.56 1387.64 --6224.88 1392.13 --6132.13 1396.63 --5939.88 1401.12 --5682.91 1405.62 --5527.56 1410.11 --5258.96 1414.61 --5074.12 1419.1 --4823.63 1423.6 --4685.91 1428.09 --4500.56 1432.58 --4308.91 1437.08 --4012.16 1441.57 --3866.78 1446.07 --3608.86 1450.56 --3235.68 1455.06 --2964.98 1459.55 --2782.91 1464.04 --2522.36 1468.54 --2112.45 1473.03 --1955.09 1477.53 --1551.28 1482.02 --1327.8 1486.52 --913.3 1491.01 --506.29 1495.51 -0 1500 -614.46 1504.04 -967.25 1508.08 -1501.92 1512.12 -1965.98 1516.16 -2315.18 1520.2 -2789.34 1524.24 -3218.91 1528.28 -3442.74 1532.32 -3763.95 1536.36 -4118.76 1540.4 -4473.32 1544.44 -4733.06 1548.48 -5052.11 1552.53 -5306.67 1556.57 -5514.31 1560.61 -5789.63 1564.65 -6061.47 1568.69 -6337.72 1572.73 -6516.65 1576.77 -6718.8 1580.81 -6901.18 1584.85 -7112.16 1588.89 -7352 1592.93 -7634.54 1596.97 -7720.88 1601.01 -7973.84 1605.05 -8136.83 1609.09 -8386.24 1613.13 -8569.13 1617.17 -8745.66 1621.21 -8921.09 1625.25 -9081.5 1629.29 -9306.64 1633.33 -9397.61 1637.37 -9521.35 1641.41 -9746.14 1645.45 -9890.84 1649.49 -9952.96 1653.54 -10218.41 1657.58 -10275.6 1661.62 -10446.95 1665.66 -10584.48 1669.7 -10754.31 1673.74 -10864.61 1677.78 -10959.78 1681.82 -11109.23 1685.86 -11210.52 1689.9 -11367.21 1693.94 -11528.98 1697.98 -11604.96 1702.02 -11700.44 1706.06 -11756.9 1710.1 -11942.16 1714.14 -12045.06 1718.18 -12179.5 1722.22 -12383.12 1726.26 -12317.95 1730.3 -12531.05 1734.34 -12581.61 1738.38 -12716.21 1742.42 -12825.03 1746.46 -12832.83 1750.51 -13053.31 1754.55 -13181.88 1758.59 -13207.91 1762.63 -13247.19 1766.67 -13414.53 1770.71 -13466.28 1774.75 -13573.65 1778.79 -13696.17 1782.83 -13803.27 1786.87 -13847.3 1790.91 -14029.66 1794.95 -14067.08 1798.99 -14156.74 1803.03 -14283.08 1807.07 -14300.44 1811.11 -14443.34 1815.15 -14541.8 1819.19 -14560.88 1823.23 -14584.52 1827.27 -14665.63 1831.31 -14718.97 1835.35 -14901.01 1839.39 -14973.02 1843.43 -15054.56 1847.47 -15149.7 1851.52 -15239.98 1855.56 -15272.75 1859.6 -15281.37 1863.64 -15384.03 1867.68 -15419.57 1871.72 -15593.75 1875.76 -15602.55 1879.8 -15724.38 1883.84 -15696.74 1887.88 -15764.41 1891.92 -15910.14 1895.96 -15958.35 1900 \ No newline at end of file From b7f6fab6e2085e3cfaa22dba9a6c3f199b6fdd7c Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Sun, 18 Aug 2024 11:03:21 +0200 Subject: [PATCH 67/67] build fix --- mission/map_manager/CMakeLists.txt | 4 ++++ mission/map_manager/package.xml | 2 ++ 2 files changed, 6 insertions(+) diff --git a/mission/map_manager/CMakeLists.txt b/mission/map_manager/CMakeLists.txt index aa641539..f76c00e3 100644 --- a/mission/map_manager/CMakeLists.txt +++ b/mission/map_manager/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(PCL REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) include_directories(include) @@ -38,6 +40,8 @@ ament_target_dependencies( geometry_msgs nav_msgs sensor_msgs + tf2 + tf2_ros ) install( diff --git a/mission/map_manager/package.xml b/mission/map_manager/package.xml index 6db8a033..cee4d527 100644 --- a/mission/map_manager/package.xml +++ b/mission/map_manager/package.xml @@ -14,6 +14,8 @@ geometry_msgs nav_msgs sensor_msgs + tf2_ros + tf2 ament_lint_auto ament_lint_common