From 761eecfa6c18a23f19c14c88ce0a7e6d9a6568d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=92=D0=B0=D1=88=D0=B5=20=D0=98=D0=BC=D1=8F?= Date: Mon, 22 Feb 2021 20:05:30 +0300 Subject: [PATCH 01/18] fixes from 22.02 --- .../config/lane_controller_node/default.yaml | 29 ++++ .../config/lane_filter_node/default.yaml | 23 +++ .../config/line_detector_node/default.yaml | 59 +++---- .../config/stop_line_filter_node/default.yaml | 5 +- .../src/stop_line_filter_node.py | 159 ++++++++++++++++++ .../unicorn_intersection_node/default.yaml | 22 +-- .../launch/vehicle_detection_node.launch | 6 + .../launch/vehicle_filter_node.launch | 6 + 8 files changed, 255 insertions(+), 54 deletions(-) create mode 100644 packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml create mode 100644 packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml create mode 100644 packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py create mode 100644 packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch create mode 100644 packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch diff --git a/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml b/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml new file mode 100644 index 00000000..29cd017c --- /dev/null +++ b/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml @@ -0,0 +1,29 @@ +#lane_control/lane_controller_node +v_bar: 0.19 +k_d: -6.0 +k_theta: -5 +k_Id: -0.3 +k_Iphi: 0.0 +theta_thres: 0.523 +d_thres: 0.2615 +d_offset: 0.0 + +omega_ff: 0 + +integral_bounds: + d: + top: 0.3 + bot: -0.3 + phi: + top: 1.2 + bot: -1.2 + +# TODO: lane filter should publish this information +d_resolution: 0.011 +phi_resolution: 0.051 + +stop_line_slowdown: + start: 0.6 + end: 0.15 + +verbose: 0 diff --git a/packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml b/packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml new file mode 100644 index 00000000..cbc5b5e6 --- /dev/null +++ b/packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml @@ -0,0 +1,23 @@ +use_propagation: True +debug : False +lane_filter_histogram_configuration: + mean_d_0: 0 + mean_phi_0: 0 + sigma_d_0: 0.1 + sigma_phi_0: 0.1 + delta_d: 0.02 + delta_phi: 0.1 + d_max: 0.3 + d_min: -0.15 + phi_min: -0.5 + phi_max: 0.5 + cov_v: 0.5 + linewidth_white: 0.05 + linewidth_yellow: 0.025 + lanewidth: 0.23 + min_max: 0.1 + sigma_d_mask: 1.0 + sigma_phi_mask: 2.0 + range_min: 0.2 + range_est: 0.33 + range_max: 0.6 diff --git a/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml b/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml index 398c01db..3fa655ab 100644 --- a/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml +++ b/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml @@ -1,44 +1,23 @@ img_size: [120,160] top_cutoff: 40 -# should be an array of 2 elements. The first is the name of the class -# and the second should be the parameters. -# The class should be an abstract instance of LineDetectorInterface -# CUSTOM CONFIG +colors: + RED: + low_1: [0,30,100] + high_1: [17,255,255] + low_2: [165,40,100] + high_2: [180,255,255] + WHITE: + low: [41,0,100] + high: [164,100,255] + YELLOW: + low: [23,40,100] + high: [40,255,255] - -detector: - - line_detector.LineDetectorHSV - - configuration: - dilation_kernel_size: 3 - canny_thresholds: [80,200] - hough_threshold: 2 - hough_min_line_length: 3 - hough_max_line_gap: 1 - - hsv_white1: [0,0,150] - hsv_white2: [180,100,255] - hsv_yellow1: [25,140,100] - hsv_yellow2: [45,255,255] - hsv_red1: [0,140,100] - hsv_red2: [15,255,255] - hsv_red3: [165,140,100] - hsv_red4: [180,255,255] - -detector_intersection: - - line_detector.LineDetectorHSV - - configuration: - dilation_kernel_size: 3 - canny_thresholds: [80,200] - hough_threshold: 2 - hough_min_line_length: 3 - hough_max_line_gap: 1 - - hsv_white1: [0,0,150] - hsv_white2: [180,100,255] - hsv_yellow1: [25,140,100] - hsv_yellow2: [45,255,255] - hsv_red1: [0,50,100] - hsv_red2: [15,255,255] - hsv_red3: [165,50,100] - hsv_red4: [180,255,255] +line_detector_parameters: + canny_thresholds: [80,200] + canny_aperture_size: 3 + dilation_kernel_size: 3 + hough_threshold: 2 + hough_min_line_length: 3 + hough_max_line_gap: 1 diff --git a/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml b/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml index 4e2ab54b..8577300d 100644 --- a/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml +++ b/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml @@ -1,5 +1,4 @@ -#default parameters for stop_line_filter/stop_line_filter_node -stop_distance: 0.15 +stop_distance: 0.3 min_segs: 6 -off_time: 2 +off_time: 10 max_y: 0.2 diff --git a/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py b/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py new file mode 100644 index 00000000..9c5dc0cf --- /dev/null +++ b/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 +import numpy as np + +import rospy +from duckietown.dtros import DTParam, DTROS, NodeType, ParamType +from duckietown_msgs.msg import BoolStamped, FSMState, LanePose, SegmentList, StopLineReading +from geometry_msgs.msg import Point + + +# import math + + +class StopLineFilterNode(DTROS): + def __init__(self, node_name): + # Initialize the DTROS parent class + super(StopLineFilterNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION) + + # Initialize the parameters + self.stop_distance = DTParam("~stop_distance", param_type=ParamType.FLOAT) + self.min_segs = DTParam("~min_segs", param_type=ParamType.INT) + self.off_time = DTParam("~off_time", param_type=ParamType.FLOAT) + self.max_y = DTParam("~max_y", param_type=ParamType.FLOAT) + + ## params + # self.stop_distance = self.setupParam("~stop_distance", 0.22) # distance from the stop line that we should stop + # self.min_segs = self.setupParam("~min_segs", 2) # minimum number of red segments that we should detect to estimate a stop + # self.off_time = self.setupParam("~off_time", 2) + # self.max_y = self.setupParam("~max_y ", 0.2) # If y value of detected red line is smaller than max_y we will not set at_stop_line true. + + ## state vars + self.lane_pose = LanePose() + + self.state = "JOYSTICK_CONTROL" + self.sleep = False + + ## publishers and subscribers + self.sub_segs = rospy.Subscriber("~segment_list", SegmentList, self.cb_segments) + self.sub_lane = rospy.Subscriber("~lane_pose", LanePose, self.cb_lane_pose) + self.sub_mode = rospy.Subscriber("fsm_node/mode", FSMState, self.cb_state_change) + self.pub_stop_line_reading = rospy.Publisher("~stop_line_reading", StopLineReading, queue_size=1) + self.pub_at_stop_line = rospy.Publisher("~at_stop_line", BoolStamped, queue_size=1) + + # def setupParam(self,param_name,default_value): + # value = rospy.get_param(param_name,default_value) + # rospy.set_param(param_name,value) #Write to parameter server for transparancy + # rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value)) + # return value + # + # def updateParams(self,event): + # self.stop_distance = rospy.get_param("~stop_distance") + # self.min_segs = rospy.get_param("~min_segs") + # self.off_time = rospy.get_param("~off_time") + # self.max_y = rospy.get_param("~max_y") + + def cb_state_change(self, msg): + # 3501 shit fix + #if (self.state == "INTERSECTION_CONTROL") and (msg.state == "LANE_FOLLOWING"): + # self.after_intersection_work() + if msg.state == "INTERSECTION_CONTROL": + self.after_intersection_work() + self.state = msg.state + + def after_intersection_work(self): + self.loginfo("Blocking stop line detection after the intersection") + stop_line_reading_msg = StopLineReading() + stop_line_reading_msg.stop_line_detected = False + stop_line_reading_msg.at_stop_line = False + self.pub_stop_line_reading.publish(stop_line_reading_msg) + self.sleep = True + rospy.sleep(self.off_time.value) + self.sleep = False + self.loginfo("Resuming stop line detection after the intersection") + + # def cbSwitch(self, switch_msg): + # self.active = switch_msg.data + # if self.active and self.state == "INTERSECTION_CONTROL": + # self.after_intersection_work() + # + # + + def cb_lane_pose(self, lane_pose_msg): + self.lane_pose = lane_pose_msg + + def cb_segments(self, segment_list_msg): + + if not self.switch or self.sleep: + # one more almost fix for 3501 + stop_line_reading_msg = StopLineReading() + stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp + stop_line_reading_msg.stop_line_detected = False + stop_line_reading_msg.at_stop_line = False + self.pub_stop_line_reading.publish(stop_line_reading_msg) + return + + good_seg_count = 0 + stop_line_x_accumulator = 0.0 + stop_line_y_accumulator = 0.0 + for segment in segment_list_msg.segments: + if segment.color != segment.RED: + continue + if segment.points[0].x < 0 or segment.points[1].x < 0: # the point is behind us + continue + + p1_lane = self.to_lane_frame(segment.points[0]) + p2_lane = self.to_lane_frame(segment.points[1]) + avg_x = 0.5 * (p1_lane[0] + p2_lane[0]) + avg_y = 0.5 * (p1_lane[1] + p2_lane[1]) + stop_line_x_accumulator += avg_x + stop_line_y_accumulator += avg_y # TODO output covariance and not just mean + good_seg_count += 1.0 + + stop_line_reading_msg = StopLineReading() + stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp + if good_seg_count < self.min_segs.value: + stop_line_reading_msg.stop_line_detected = False + stop_line_reading_msg.at_stop_line = False + self.pub_stop_line_reading.publish(stop_line_reading_msg) + + # ### CRITICAL: publish false to at stop line output_topic + # msg = BoolStamped() + # msg.header.stamp = stop_line_reading_msg.header.stamp + # msg.data = False + # self.pub_at_stop_line.publish(msg) + # ### CRITICAL END + + else: + stop_line_reading_msg.stop_line_detected = True + stop_line_point = Point() + stop_line_point.x = stop_line_x_accumulator / good_seg_count + stop_line_point.y = stop_line_y_accumulator / good_seg_count + stop_line_reading_msg.stop_line_point = stop_line_point + # Only detect redline if y is within max_y distance: + stop_line_reading_msg.at_stop_line = ( + stop_line_point.x < self.stop_distance.value and np.abs(stop_line_point.y) < self.max_y.value + ) + + self.pub_stop_line_reading.publish(stop_line_reading_msg) + if stop_line_reading_msg.at_stop_line: + msg = BoolStamped() + msg.header.stamp = stop_line_reading_msg.header.stamp + msg.data = True + self.pub_at_stop_line.publish(msg) + + def to_lane_frame(self, point): + p_homo = np.array([point.x, point.y, 1]) + phi = self.lane_pose.phi + d = self.lane_pose.d + T = np.array([[np.cos(phi), -np.sin(phi), 0], [np.sin(phi), np.cos(phi), d], [0, 0, 1]]) + p_new_homo = T.dot(p_homo) + p_new = p_new_homo[0:2] + return p_new + + # def onShutdown(self): + # rospy.loginfo("[StopLineFilterNode] Shutdown.") + + +if __name__ == "__main__": + lane_filter_node = StopLineFilterNode(node_name="stop_line_filter") + rospy.spin() diff --git a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml index 0575c538..62bc5dad 100644 --- a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml +++ b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml @@ -1,19 +1,19 @@ -time_left_turn: 7.5 -time_straight_turn: 6.5 -time_right_turn: 2.5 +time_left_turn: 6.5 +time_straight_turn: 10 +time_right_turn: 3 -ff_left: 0.1 -ff_straight: 0 -ff_right: -0.6 +ff_left: 0.3 +ff_straight: 0.03 +ff_right: -0.4 -LFparams_left: {phi_max: -0.5, phi_min: -1.5, range_est: 0.4, red_to_white: true, use_yellow: false} -LFparams_straight: {"red_to_white": false} -LFparams_right: {phi_max: 2, phi_min: 0.5, range_est: 0.4, red_to_white: false} +LFparams_left: {phi_max: -0.1, phi_min: -0.2, range_est: 0.4, red_to_white: true, use_yellow: false} +LFparams_straight: { phi_max: 0.07, phi_min: -0.07, red_to_white: false} +LFparams_right: {phi_max: 0.2, phi_min: 0.1, range_est: 0.4, red_to_white: false, use_yellow: true} omega_min_left: 0.2 omega_max_left: 0.6 -omega_min_straight: -1 -omega_max_straight: 1 +omega_min_straight: -0.1 +omega_max_straight: 0.1 omega_min_right: -2.5 omega_max_right: 999 diff --git a/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch b/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch new file mode 100644 index 00000000..3486f2dc --- /dev/null +++ b/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch b/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch new file mode 100644 index 00000000..3486f2dc --- /dev/null +++ b/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch @@ -0,0 +1,6 @@ + + + + + + From 0a4fb9ef091966dbef847372c100c09f2761ae4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=92=D0=B0=D1=88=D0=B5=20=D0=98=D0=BC=D1=8F?= Date: Tue, 23 Feb 2021 14:08:13 +0300 Subject: [PATCH 02/18] add v2 --- .../lane_control/src/lane_controller_node.py | 267 ++++++++++++++++++ .../src/stop_line_filter_node.py | 1 + .../unicorn_intersection_node/default.yaml | 21 +- 3 files changed, 279 insertions(+), 10 deletions(-) create mode 100644 packages/dt-core/packages/lane_control/src/lane_controller_node.py diff --git a/packages/dt-core/packages/lane_control/src/lane_controller_node.py b/packages/dt-core/packages/lane_control/src/lane_controller_node.py new file mode 100644 index 00000000..7bfe7c47 --- /dev/null +++ b/packages/dt-core/packages/lane_control/src/lane_controller_node.py @@ -0,0 +1,267 @@ +#!/usr/bin/env python3 +import numpy as np +import rospy + +from duckietown.dtros import DTROS, NodeType, TopicType, DTParam, ParamType +from duckietown_msgs.msg import Twist2DStamped, LanePose, WheelsCmdStamped, BoolStamped, FSMState, StopLineReading + +from lane_controller.controller import LaneController + + +class LaneControllerNode(DTROS): + """Computes control action. + The node compute the commands in form of linear and angular velocities, by processing the estimate error in + lateral deviationa and heading. + The configuration parameters can be changed dynamically while the node is running via ``rosparam set`` commands. + Args: + node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use + Configuration: + ~v_bar (:obj:`float`): Nominal velocity in m/s + ~k_d (:obj:`float`): Proportional term for lateral deviation + ~k_theta (:obj:`float`): Proportional term for heading deviation + ~k_Id (:obj:`float`): integral term for lateral deviation + ~k_Iphi (:obj:`float`): integral term for lateral deviation + ~d_thres (:obj:`float`): Maximum value for lateral error + ~theta_thres (:obj:`float`): Maximum value for heading error + ~d_offset (:obj:`float`): Goal offset from center of the lane + ~integral_bounds (:obj:`dict`): Bounds for integral term + ~d_resolution (:obj:`float`): Resolution of lateral position estimate + ~phi_resolution (:obj:`float`): Resolution of heading estimate + ~omega_ff (:obj:`float`): Feedforward part of controller + ~verbose (:obj:`bool`): Verbosity level (0,1,2) + ~stop_line_slowdown (:obj:`dict`): Start and end distances for slowdown at stop lines + Publisher: + ~car_cmd (:obj:`Twist2DStamped`): The computed control action + Subscribers: + ~lane_pose (:obj:`LanePose`): The lane pose estimate from the lane filter + ~intersection_navigation_pose (:obj:`LanePose`): The lane pose estimate from intersection navigation + ~wheels_cmd_executed (:obj:`WheelsCmdStamped`): Confirmation that the control action was executed + ~stop_line_reading (:obj:`StopLineReading`): Distance from stopline, to reduce speed + ~obstacle_distance_reading (:obj:`stop_line_reading`): Distancefrom obstacle virtual stopline, to reduce speed + """ + + def __init__(self, node_name): + + # Initialize the DTROS parent class + super(LaneControllerNode, self).__init__( + node_name=node_name, + node_type=NodeType.PERCEPTION + ) + + # Add the node parameters to the parameters dictionary + # TODO: MAKE TO WORK WITH NEW DTROS PARAMETERS + self.params = dict() + self.params['~v_bar'] = DTParam( + '~v_bar', + param_type=ParamType.FLOAT, + min_value=0.0, + max_value=5.0 + ) + self.params['~k_d'] = DTParam( + '~k_d', + param_type=ParamType.FLOAT, + min_value=-100.0, + max_value=100.0 + ) + self.params['~k_theta'] = DTParam( + '~k_theta', + param_type=ParamType.FLOAT, + min_value=-100.0, + max_value=100.0 + ) + self.params['~k_Id'] = DTParam( + '~k_Id', + param_type=ParamType.FLOAT, + min_value=-100.0, + max_value=100.0 + ) + self.params['~k_Iphi'] = DTParam( + '~k_Iphi', + param_type=ParamType.FLOAT, + min_value=-100.0, + max_value=100.0 + ) + self.params['~theta_thres'] = rospy.get_param('~theta_thres', None) + self.params['~d_thres'] = rospy.get_param('~d_thres', None) + self.params['~d_offset'] = rospy.get_param('~d_offset', None) + self.params['~integral_bounds'] = rospy.get_param('~integral_bounds', None) + self.params['~d_resolution'] = rospy.get_param('~d_resolution', None) + self.params['~phi_resolution'] = rospy.get_param('~phi_resolution', None) + self.params['~omega_ff'] = rospy.get_param('~omega_ff', None) + self.params['~verbose'] = rospy.get_param('~verbose', None) + self.params['~stop_line_slowdown'] = rospy.get_param('~stop_line_slowdown', None) + + # Need to create controller object before updating parameters, otherwise it will fail + self.controller = LaneController(self.params) + # self.updateParameters() # TODO: This needs be replaced by the new DTROS callback when it is implemented + + # Initialize variables + self.fsm_state = None + self.wheels_cmd_executed = WheelsCmdStamped() + self.pose_msg = LanePose() + self.pose_initialized = False + self.pose_msg_dict = dict() + self.last_s = None + self.stop_line_distance = None + self.stop_line_detected = False + self.at_stop_line = False + self.obstacle_stop_line_distance = None + self.obstacle_stop_line_detected = False + self.at_obstacle_stop_line = False + + self.current_pose_source = 'lane_filter' + + # Construct publishers + self.pub_car_cmd = rospy.Publisher("~car_cmd", + Twist2DStamped, + queue_size=1, + dt_topic_type=TopicType.CONTROL) + + # Construct subscribers + self.sub_lane_reading = rospy.Subscriber("~lane_pose", + LanePose, + self.cbAllPoses, + "lane_filter", + queue_size=1) + self.sub_intersection_navigation_pose = rospy.Subscriber("~intersection_navigation_pose", + LanePose, + self.cbAllPoses, + "intersection_navigation", + queue_size=1) + self.sub_wheels_cmd_executed = rospy.Subscriber("~wheels_cmd", + WheelsCmdStamped, + self.cbWheelsCmdExecuted, + queue_size=1) + self.sub_stop_line = rospy.Subscriber("~stop_line_reading", + StopLineReading, + self.cbStopLineReading, + queue_size=1) + self.sub_obstacle_stop_line = rospy.Subscriber("~obstacle_distance_reading", + StopLineReading, + self.cbObstacleStopLineReading, + queue_size=1) + + self.log("Initialized!") + + def cbObstacleStopLineReading(self,msg): + """ + Callback storing the current obstacle distance, if detected. + Args: + msg(:obj:`StopLineReading`): Message containing information about the virtual obstacle stopline. + """ + self.obstacle_stop_line_distance = np.sqrt(msg.stop_line_point.x ** 2 + msg.stop_line_point.y ** 2) + self.obstacle_stop_line_detected = msg.stop_line_detected + self.at_stop_line = msg.at_stop_line + + def cbStopLineReading(self, msg): + """Callback storing current distance to the next stopline, if one is detected. + Args: + msg (:obj:`StopLineReading`): Message containing information about the next stop line. + """ + self.stop_line_distance = np.sqrt(msg.stop_line_point.x ** 2 + msg.stop_line_point.y ** 2) + self.stop_line_detected = msg.stop_line_detected + self.at_obstacle_stop_line = msg.at_stop_line + + def cbMode(self, fsm_state_msg): + + self.fsm_state = fsm_state_msg.state # String of current FSM state + + if self.fsm_state == 'INTERSECTION_CONTROL': + self.current_pose_source = 'intersection_navigation' + else: + self.current_pose_source = 'lane_filter' + + if self.params['~verbose'] == 2: + self.log("Pose source: %s" % self.current_pose_source) + + def cbAllPoses(self, input_pose_msg, pose_source): + """Callback receiving pose messages from multiple topics. + If the source of the message corresponds with the current wanted pose source, it computes a control command. + Args: + input_pose_msg (:obj:`LanePose`): Message containing information about the current lane pose. + pose_source (:obj:`String`): Source of the message, specified in the subscriber. + """ + + if pose_source == self.current_pose_source: + self.pose_msg_dict[pose_source] = input_pose_msg + + self.pose_msg = input_pose_msg + + self.getControlAction(self.pose_msg) + + def cbWheelsCmdExecuted(self, msg_wheels_cmd): + """Callback that reports if the requested control action was executed. + Args: + msg_wheels_cmd (:obj:`WheelsCmdStamped`): Executed wheel commands + """ + self.wheels_cmd_executed = msg_wheels_cmd + + def publishCmd(self, car_cmd_msg): + """Publishes a car command message. + Args: + car_cmd_msg (:obj:`Twist2DStamped`): Message containing the requested control action. + """ + self.pub_car_cmd.publish(car_cmd_msg) + + def getControlAction(self, pose_msg): + """Callback that receives a pose message and updates the related control command. + Using a controller object, computes the control action using the current pose estimate. + Args: + pose_msg (:obj:`LanePose`): Message containing information about the current lane pose. + """ + current_s = rospy.Time.now().to_sec() + dt = None + if self.last_s is not None: + dt = (current_s - self.last_s) + + if self.at_stop_line or self.at_obstacle_stop_line: + v = 0 + omega = 0 + else: + + # Compute errors + d_err = pose_msg.d - self.params['~d_offset'] + phi_err = pose_msg.phi + + # We cap the error if it grows too large + if np.abs(d_err) > self.params['~d_thres']: + self.log("d_err too large, thresholding it!", 'error') + d_err = np.sign(d_err) * self.params['~d_thres'] + + + wheels_cmd_exec = [self.wheels_cmd_executed.vel_left, self.wheels_cmd_executed.vel_right] + if self.obstacle_stop_line_detected: + v, omega = self.controller.compute_control_action(d_err, phi_err, dt, wheels_cmd_exec, self.obstacle_stop_line_distance) + #TODO: This is a temporarily fix to avoid vehicle image detection latency caused unable to stop in time. + v = v*0.25 + omega = omega*0.25 + + else: + v, omega = self.controller.compute_control_action(d_err, phi_err, dt, wheels_cmd_exec, self.stop_line_distance) + + # For feedforward action (i.e. during intersection navigation) + self.params['~omega_ff'] = rospy.get_param('~omega_ff', 0) + omega += self.params['~omega_ff'] + + # Initialize car control msg, add header from input message + car_control_msg = Twist2DStamped() + car_control_msg.header = pose_msg.header + + # Add commands to car message + car_control_msg.v = v + car_control_msg.omega = omega + + self.publishCmd(car_control_msg) + self.last_s = current_s + + def cbParametersChanged(self): + """Updates parameters in the controller object.""" + + self.controller.update_parameters(self.params) + + +if __name__ == "__main__": + # Initialize the node + lane_controller_node = LaneControllerNode(node_name='lane_controller_node') + # Keep it spinning + rospy.spin() diff --git a/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py b/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py index 9c5dc0cf..7db88656 100644 --- a/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py +++ b/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py @@ -56,6 +56,7 @@ def cb_state_change(self, msg): # 3501 shit fix #if (self.state == "INTERSECTION_CONTROL") and (msg.state == "LANE_FOLLOWING"): # self.after_intersection_work() + self.loginfo("CB_STATE: {}".format(msg.state)) if msg.state == "INTERSECTION_CONTROL": self.after_intersection_work() self.state = msg.state diff --git a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml index 62bc5dad..63c23523 100644 --- a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml +++ b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml @@ -1,19 +1,20 @@ + time_left_turn: 6.5 -time_straight_turn: 10 -time_right_turn: 3 +time_straight_turn: 13 +time_right_turn: 3.5 -ff_left: 0.3 -ff_straight: 0.03 -ff_right: -0.4 +ff_left: 0.01 +ff_straight: 0 +ff_right: -0.2 -LFparams_left: {phi_max: -0.1, phi_min: -0.2, range_est: 0.4, red_to_white: true, use_yellow: false} -LFparams_straight: { phi_max: 0.07, phi_min: -0.07, red_to_white: false} -LFparams_right: {phi_max: 0.2, phi_min: 0.1, range_est: 0.4, red_to_white: false, use_yellow: true} +LFparams_left: {phi_max: -1.5, phi_min: -0.5, range_est: 0.4, red_to_white: true, use_yellow: false} +LFparams_straight: {"red_to_white": True} +LFparams_right: {phi_max: 1.5, phi_min: 0.5, range_est: 0.4, red_to_white: false} omega_min_left: 0.2 omega_max_left: 0.6 -omega_min_straight: -0.1 -omega_max_straight: 0.1 +omega_min_straight: -1 +omega_max_straight: 1 omega_min_right: -2.5 omega_max_right: 999 From 7a45430cd72049e6b228c5d8890e932433133bb7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=92=D0=B0=D1=88=D0=B5=20=D0=98=D0=BC=D1=8F?= Date: Tue, 23 Feb 2021 19:50:19 +0300 Subject: [PATCH 03/18] last version of config --- .../config/lane_controller_node/default.yaml | 2 +- .../include/lane_controller/controller.py | 172 +++++++++++++++++ .../led_pattern_switch_node/default.yaml | 18 ++ .../config/stop_line_filter_node/default.yaml | 4 +- .../unicorn_intersection_node/default.yaml | 12 +- .../src/unicorn_intersection_node.py | 175 ++++++++++++++++++ 6 files changed, 374 insertions(+), 9 deletions(-) create mode 100644 packages/dt-core/packages/lane_control/include/lane_controller/controller.py create mode 100644 packages/dt-core/packages/led_pattern_switch/config/led_pattern_switch_node/default.yaml create mode 100644 packages/dt-core/packages/unicorn_intersection/src/unicorn_intersection_node.py diff --git a/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml b/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml index 29cd017c..5e0fda70 100644 --- a/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml +++ b/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml @@ -24,6 +24,6 @@ phi_resolution: 0.051 stop_line_slowdown: start: 0.6 - end: 0.15 + end: 0.18 verbose: 0 diff --git a/packages/dt-core/packages/lane_control/include/lane_controller/controller.py b/packages/dt-core/packages/lane_control/include/lane_controller/controller.py new file mode 100644 index 00000000..4d6d0f60 --- /dev/null +++ b/packages/dt-core/packages/lane_control/include/lane_controller/controller.py @@ -0,0 +1,172 @@ +import numpy as np + + +class LaneController: + """ + The Lane Controller can be used to compute control commands from pose estimations. + + The control commands are in terms of linear and angular velocity (v, omega). The input are errors in the relative + pose of the Duckiebot in the current lane. + + This implementation is a simple PI(D) controller. + + Args: + ~v_bar (:obj:`float`): Nominal velocity in m/s + ~k_d (:obj:`float`): Proportional term for lateral deviation + ~k_theta (:obj:`float`): Proportional term for heading deviation + ~k_Id (:obj:`float`): integral term for lateral deviation + ~k_Iphi (:obj:`float`): integral term for lateral deviation + ~d_thres (:obj:`float`): Maximum value for lateral error + ~theta_thres (:obj:`float`): Maximum value for heading error + ~d_offset (:obj:`float`): Goal offset from center of the lane + ~integral_bounds (:obj:`dict`): Bounds for integral term + ~d_resolution (:obj:`float`): Resolution of lateral position estimate + ~phi_resolution (:obj:`float`): Resolution of heading estimate + ~omega_ff (:obj:`float`): Feedforward part of controller + ~verbose (:obj:`bool`): Verbosity level (0,1,2) + ~stop_line_slowdown (:obj:`dict`): Start and end distances for slowdown at stop lines + + """ + + def __init__(self, parameters): + self.parameters = parameters + self.d_I = 0.0 + self.phi_I = 0.0 + self.prev_d_err = 0.0 + self.prev_phi_err = 0.0 + + def update_parameters(self, parameters): + """Updates parameters of LaneController object. + + Args: + parameters (:obj:`dict`): dictionary containing the new parameters for LaneController object. + """ + self.parameters = parameters + + def compute_control_action(self, d_err, phi_err, dt, wheels_cmd_exec, stop_line_distance): + """Main function, computes the control action given the current error signals. + + Given an estimate of the error, computes a control action (tuple of linear and angular velocity). This is done + via a basic PI(D) controller with anti-reset windup logic. + + Args: + d_err (:obj:`float`): error in meters in the lateral direction + phi_err (:obj:`float`): error in radians in the heading direction + dt (:obj:`float`): time since last command update + wheels_cmd_exec (:obj:`bool`): confirmation that the wheel commands have been executed (to avoid + integration while the robot does not move) + stop_line_distance (:obj:`float`): distance of the stop line, None if not detected. + Returns: + v (:obj:`float`): requested linear velocity in meters/second + omega (:obj:`float`): requested angular velocity in radians/second + """ + + if dt is not None: + self.integrate_errors(d_err, phi_err, dt) + + self.d_I = self.adjust_integral( + d_err, self.d_I, self.parameters["~integral_bounds"]["d"], self.parameters["~d_resolution"] + ) + self.phi_I = self.adjust_integral( + phi_err, + self.phi_I, + self.parameters["~integral_bounds"]["phi"], + self.parameters["~phi_resolution"], + ) + + self.reset_if_needed(d_err, phi_err, wheels_cmd_exec) + + # Scale the parameters linear such that their real value is at 0.22m/s + omega = ( + self.parameters["~k_d"].value * d_err + + self.parameters["~k_theta"].value * phi_err + + self.parameters["~k_Id"].value * self.d_I + + self.parameters["~k_Iphi"].value * self.phi_I + ) + + self.prev_d_err = d_err + self.prev_phi_err = phi_err + + v = self.compute_velocity(stop_line_distance) + + return v, omega + + def compute_velocity(self, stop_line_distance): + """Linearly decrease velocity if approaching a stop line. + + If a stop line is detected, the velocity is linearly decreased to achieve a better stopping position, + otherwise the nominal velocity is returned. + + Args: + stop_line_distance (:obj:`float`): distance of the stop line, None if not detected. + """ + if stop_line_distance is None: + return self.parameters["~v_bar"].value + else: + + d1, d2 = ( + self.parameters["~stop_line_slowdown"]["start"], + self.parameters["~stop_line_slowdown"]["end"], + ) + # d1 -> v_bar, d2 -> v_bar/2 + c = (0.5 * (d1 - stop_line_distance) + (stop_line_distance - d2)) / (d1 - d2) + v_new = self.parameters["~v_bar"].value * c + v = np.max( + [self.parameters["~v_bar"].value / 2.0, np.min([self.parameters["~v_bar"].value, v_new])] + ) + return v + + def integrate_errors(self, d_err, phi_err, dt): + """Integrates error signals in lateral and heading direction. + Args: + d_err (:obj:`float`): error in meters in the lateral direction + phi_err (:obj:`float`): error in radians in the heading direction + dt (:obj:`float`): time delay in seconds + """ + self.d_I += d_err * dt + self.phi_I += phi_err * dt + + def reset_if_needed(self, d_err, phi_err, wheels_cmd_exec): + """Resets the integral error if needed. + + Resets the integral errors in `d` and `phi` if either the error sign changes, or if the robot is completely + stopped (i.e. intersections). + + Args: + d_err (:obj:`float`): error in meters in the lateral direction + phi_err (:obj:`float`): error in radians in the heading direction + wheels_cmd_exec (:obj:`bool`): confirmation that the wheel commands have been executed (to avoid + integration while the robot does not move) + """ + if np.sign(d_err) != np.sign(self.prev_d_err): + self.d_I = 0 + if np.sign(phi_err) != np.sign(self.prev_phi_err): + self.phi_I = 0 + if wheels_cmd_exec[0] == 0 and wheels_cmd_exec[1] == 0: + self.d_I = 0 + self.phi_I = 0 + + @staticmethod + def adjust_integral(error, integral, bounds, resolution): + """Bounds the integral error to avoid windup. + + Adjusts the integral error to remain in defined bounds, and cancels it if the error is smaller than the + resolution of the error estimation. + + Args: + error (:obj:`float`): current error value + integral (:obj:`float`): current integral value + bounds (:obj:`dict`): contains minimum and maximum value for the integral + resolution (:obj:`float`): resolution of the error estimate + + Returns: + integral (:obj:`float`): adjusted integral value + """ + if integral > bounds["top"]: + integral = bounds["top"] + elif integral < bounds["bot"]: + integral = bounds["bot"] + elif abs(error) < resolution: + integral = 0 + return integral + diff --git a/packages/dt-core/packages/led_pattern_switch/config/led_pattern_switch_node/default.yaml b/packages/dt-core/packages/led_pattern_switch/config/led_pattern_switch_node/default.yaml new file mode 100644 index 00000000..4b3eb9d4 --- /dev/null +++ b/packages/dt-core/packages/led_pattern_switch/config/led_pattern_switch_node/default.yaml @@ -0,0 +1,18 @@ +mode_topic: "fsm_node/mode" +source_topics: #Mapping from cmd source name to topics + coordination: "coordinator_node/change_color_pattern" + joystick: "led_joy_mapper_node/change_color_pattern" +mappings: #Mapping from FSMStates.state to cmd source names. Allows different FSM mode to use the same source. + LANE_FOLLOWING: "joystick" + INTERSECTION_CONTROL: "coordination" + INTERSECTION_COORDINATION: "coordination" + COORDINATION: "coordination" + JOYSTICK_CONTROL: "joystick" + NORMAL_JOYSTICK_CONTROL: "joystick" + AVOID_OBSTACLE: "joystick" + AVOID_VEHICLE: "joystick" + LANE_RECOVERY: "joystick" + INTERSECTION_RECOVERY: "joystick" + PARALLEL_AUTONOMY: "coordination" + OBSTACLE_STOP: "coordination" + OBSTACLE_ALERT: "coordination" diff --git a/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml b/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml index 8577300d..cec53ae9 100644 --- a/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml +++ b/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml @@ -1,4 +1,4 @@ -stop_distance: 0.3 +stop_distance: 0.25 min_segs: 6 -off_time: 10 +off_time: 16 max_y: 0.2 diff --git a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml index 63c23523..c9813643 100644 --- a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml +++ b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml @@ -1,15 +1,15 @@ -time_left_turn: 6.5 +time_left_turn: 7.0 time_straight_turn: 13 -time_right_turn: 3.5 +time_right_turn: 2.5 -ff_left: 0.01 +ff_left: 0.018 ff_straight: 0 -ff_right: -0.2 +ff_right: -0.061 -LFparams_left: {phi_max: -1.5, phi_min: -0.5, range_est: 0.4, red_to_white: true, use_yellow: false} +LFparams_left: {phi_max: 0.0, phi_min: -0.5, range_est: 0.3, red_to_white: true, use_yellow: true} LFparams_straight: {"red_to_white": True} -LFparams_right: {phi_max: 1.5, phi_min: 0.5, range_est: 0.4, red_to_white: false} +LFparams_right: {phi_max: 0.27, phi_min: 0.0, range_est: 0.3, red_to_white: false} omega_min_left: 0.2 omega_max_left: 0.6 diff --git a/packages/dt-core/packages/unicorn_intersection/src/unicorn_intersection_node.py b/packages/dt-core/packages/unicorn_intersection/src/unicorn_intersection_node.py new file mode 100644 index 00000000..c25dd4a0 --- /dev/null +++ b/packages/dt-core/packages/unicorn_intersection/src/unicorn_intersection_node.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +import json + +import rospy +from duckietown_msgs.msg import BoolStamped, FSMState, LanePose, TurnIDandType +from std_msgs.msg import String + + +class UnicornIntersectionNode: + def __init__(self): + self.node_name = "Unicorn Intersection Node" + + ## setup Parameters + self.setupParams() + + ## Internal variables + self.state = "JOYSTICK_CONTROL" + self.active = False + self.turn_type = -1 + self.tag_id = -1 + self.forward_pose = False + + ## Subscribers + # self.sub_turn_type = rospy.Subscriber("~turn_type", Int16, self.cbTurnType) + self.sub_turn_type = rospy.Subscriber("~turn_id_and_type", TurnIDandType, self.cbTurnType) + self.sub_fsm = rospy.Subscriber("~fsm_state", FSMState, self.cbFSMState) + self.sub_int_go = rospy.Subscriber("~intersection_go", BoolStamped, self.cbIntersectionGo) + self.sub_lane_pose = rospy.Subscriber("~lane_pose_in", LanePose, self.cbLanePose) + self.sub_switch = rospy.Subscriber("~switch", BoolStamped, self.cbSwitch, queue_size=1) + + ## Publisher + self.pub_int_done = rospy.Publisher("~intersection_done", BoolStamped, queue_size=1) + self.pub_LF_params = rospy.Publisher("~lane_filter_params", String, queue_size=1) + self.pub_lane_pose = rospy.Publisher("~lane_pose_out", LanePose, queue_size=1) + self.pub_int_done_detailed = rospy.Publisher( + "~intersection_done_detailed", TurnIDandType, queue_size=1 + ) + + ## update Parameters timer + self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams) + + def cbLanePose(self, msg): + if self.forward_pose: + self.pub_lane_pose.publish(msg) + + def changeLFParams(self, params, reset_time): + data = {"params": params, "time": reset_time} + msg = String() + msg.data = json.dumps(data) + self.pub_LF_params.publish(msg) + + def cbIntersectionGo(self, msg): + rospy.loginfo("[%s] Recieved intersection go message from coordinator", self.node_name) + if not self.active: + return + + if not msg.data: + return + + while self.turn_type == -1: + if not self.active: + return + rospy.loginfo( + "[%s] Requested to start intersection, but we do not see an april tag yet.", self.node_name + ) + rospy.sleep(2) + + tag_id = self.tag_id + turn_type = self.turn_type + + sleeptimes = [self.time_left_turn, self.time_straight_turn, self.time_right_turn] + LFparams = [self.LFparams_left, self.LFparams_straight, self.LFparams_right] + omega_ffs = [self.ff_left, self.ff_straight, self.ff_right] + omega_maxs = [self.omega_max_left, self.omega_max_straight, self.omega_max_right] + omega_mins = [self.omega_min_left, self.omega_min_straight, self.omega_min_right] + + + rospy.set_param("~lane_controller/omega_ff", omega_ffs[turn_type]) + rospy.set_param("~lane_controller/omega_max", omega_maxs[turn_type]) + rospy.set_param("~lane_controller/omega_min", omega_mins[turn_type]) + # Waiting for LF to adapt to new params + rospy.sleep(1) + + rospy.loginfo("Starting intersection control - driving to " + str(turn_type)) + self.forward_pose = True + self.changeLFParams(LFparams[turn_type], sleeptimes[turn_type] + 1.0) + + rospy.sleep(sleeptimes[turn_type]) + + self.forward_pose = False + rospy.set_param("~lane_controller/omega_ff", 0) + rospy.set_param("~lane_controller/omega_max", 999) + rospy.set_param("~lane_controller/omega_min", -999) + + # Publish intersection done + msg_done = BoolStamped() + msg_done.data = True + self.pub_int_done.publish(msg_done) + + # Publish intersection done detailed + msg_done_detailed = TurnIDandType() + msg_done_detailed.tag_id = tag_id + msg_done_detailed.turn_type = turn_type + self.pub_int_done_detailed.publish(msg_done_detailed) + + def cbFSMState(self, msg): + if self.state != msg.state and msg.state == "INTERSECTION_COORDINATION": + self.turn_type = -1 + + self.state = msg.state + + def cbSwitch(self, switch_msg): + self.active = switch_msg.data + + def cbTurnType(self, msg): + self.tag_id = msg.tag_id + if self.turn_type == -1: + self.turn_type = msg.turn_type + if self.debug_dir != -1: + self.turn_type = self.debug_dir + + def setupParams(self): + self.time_left_turn = self.setupParam("~time_left_turn", 2) + self.time_straight_turn = self.setupParam("~time_straight_turn", 2) + self.time_right_turn = self.setupParam("~time_right_turn", 2) + self.ff_left = self.setupParam("~ff_left", 1.5) + self.ff_straight = self.setupParam("~ff_straight", 0) + self.ff_right = self.setupParam("~ff_right", -1) + self.LFparams_left = self.setupParam("~LFparams_left", 0) + self.LFparams_straight = self.setupParam("~LFparams_straight", 0) + self.LFparams_right = self.setupParam("~LFparams_right", 0) + self.omega_max_left = self.setupParam("~omega_max_left", 999) + self.omega_max_straight = self.setupParam("~omega_max_straight", 999) + self.omega_max_right = self.setupParam("~omega_max_right", 999) + self.omega_min_left = self.setupParam("~omega_min_left", -999) + self.omega_min_straight = self.setupParam("~omega_min_straight", -999) + self.omega_min_right = self.setupParam("~omega_min_right", -999) + + self.debug_dir = self.setupParam("~debug_dir", -1) + + def updateParams(self, event): + self.time_left_turn = rospy.get_param("~time_left_turn") + self.time_straight_turn = rospy.get_param("~time_straight_turn") + self.time_right_turn = rospy.get_param("~time_right_turn") + self.ff_left = rospy.get_param("~ff_left") + self.ff_straight = rospy.get_param("~ff_straight") + self.ff_right = rospy.get_param("~ff_right") + self.LFparams_left = rospy.get_param("~LFparams_left") + self.LFparams_straight = rospy.get_param("~LFparams_straight") + self.LFparams_right = rospy.get_param("~LFparams_right") + self.omega_max_left = rospy.get_param("~omega_max_left") + self.omega_max_straight = rospy.get_param("~omega_max_straight") + self.omega_max_right = rospy.get_param("~omega_max_right") + self.omega_min_left = rospy.get_param("~omega_min_left") + self.omega_min_straight = rospy.get_param("~omega_min_straight") + self.omega_min_right = rospy.get_param("~omega_min_right") + + self.debug_dir = rospy.get_param("~debug_dir") + + def setupParam(self, param_name, default_value): + value = rospy.get_param(param_name, default_value) + rospy.set_param(param_name, value) # Write to parameter server for transparancy + rospy.loginfo(f"[{self.node_name}] {param_name} = {value} ") + return value + + def onShutdown(self): + rospy.loginfo("[UnicornIntersectionNode] Shutdown.") + + +if __name__ == "__main__": + rospy.init_node("unicorn_intersection_node", anonymous=False) + unicorn_intersection_node = UnicornIntersectionNode() + rospy.on_shutdown(unicorn_intersection_node.onShutdown) + rospy.spin() + From aa53d125c5a70db69dffd158eba56ddd3436e2a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=92=D0=B0=D1=88=D0=B5=20=D0=98=D0=BC=D1=8F?= Date: Wed, 24 Feb 2021 11:11:20 +0300 Subject: [PATCH 04/18] right turns are off --- .../src/random_april_tag_turns_node.py | 133 ++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py diff --git a/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py new file mode 100644 index 00000000..a51fe491 --- /dev/null +++ b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import math + +import numpy + +import rospy +from duckietown_msgs.msg import AprilTagsWithInfos, FSMState, TurnIDandType +from std_msgs.msg import Int16 # Imports msg + + +class RandomAprilTagTurnsNode: + def __init__(self): + # Save the name of the node + self.node_name = rospy.get_name() + self.turn_type = -1 + + rospy.loginfo(f"[{self.node_name}] Initializing.") + + # Setup publishers + # self.pub_topic_a = rospy.Publisher("~topic_a",String, queue_size=1) + self.pub_turn_type = rospy.Publisher("~turn_type", Int16, queue_size=1, latch=True) + self.pub_id_and_type = rospy.Publisher("~turn_id_and_type", TurnIDandType, queue_size=1, latch=True) + + # Setup subscribers + # self.sub_topic_b = rospy.Subscriber("~topic_b", String, self.cbTopic) + self.sub_topic_mode = rospy.Subscriber("~mode", FSMState, self.cbMode, queue_size=1) + # self.fsm_mode = None #TODO what is this? + self.sub_topic_tag = rospy.Subscriber("~tag", AprilTagsWithInfos, self.cbTag, queue_size=1) + + # Read parameters + self.pub_timestep = self.setupParameter("~pub_timestep", 1.0) + # Create a timer that calls the cbTimer function every 1.0 second + # self.timer = rospy.Timer(rospy.Duration.from_sec(self.pub_timestep),self.cbTimer) + + rospy.loginfo(f"[{self.node_name}] Initialzed.") + + self.rate = rospy.Rate(30) # 10hz + + def cbMode(self, mode_msg): + # print mode_msg + # TODO PUBLISH JUST ONCE + self.fsm_mode = mode_msg.state + if self.fsm_mode != mode_msg.INTERSECTION_CONTROL: + self.turn_type = -1 + self.pub_turn_type.publish(self.turn_type) + # rospy.loginfo("Turn type now: %i" %(self.turn_type)) + + def cbTag(self, tag_msgs): + if ( + self.fsm_mode == "INTERSECTION_CONTROL" + or self.fsm_mode == "INTERSECTION_COORDINATION" + or self.fsm_mode == "INTERSECTION_PLANNING" + ): + # loop through list of april tags + + # filter out the nearest apriltag + dis_min = 999 + idx_min = -1 + for idx, taginfo in enumerate(tag_msgs.infos): + if taginfo.tag_type == taginfo.SIGN: + tag_det = (tag_msgs.detections)[idx] + pos = tag_det.transform.translation + distance = math.sqrt(pos.x ** 2 + pos.y ** 2 + pos.z ** 2) + if distance < dis_min: + dis_min = distance + idx_min = idx + + if idx_min != -1: + taginfo = (tag_msgs.infos)[idx_min] + + availableTurns = [] + # go through possible intersection types + signType = taginfo.traffic_sign_type + if signType == taginfo.NO_RIGHT_TURN or signType == taginfo.LEFT_T_INTERSECT: + availableTurns = [ + 0, + 1, + ] # these mystical numbers correspond to the array ordering in open_loop_intersection_control_node (very bad) + elif signType == taginfo.NO_LEFT_TURN or signType == taginfo.RIGHT_T_INTERSECT: + availableTurns = [1, 2] + elif signType == taginfo.FOUR_WAY: + availableTurns = [0, 1, 2] + elif signType == taginfo.T_INTERSECTION: + availableTurns = [0, 2] + rospy.loginfo(f"[{self.node_name}] reports Available turns are: [{availableTurns}]") + # now randomly choose a possible direction + if len(availableTurns) > 0: + # 3501: turn off right turns + # randomIndex = numpy.random.randint(len(availableTurns)) + # chosenTurn = availableTurns[randomIndex] + while True: + randomIndex = numpy.random.randint(len(availableTurns)) + chosenTurn = availableTurns[randomIndex] + rospy.loginfo("Turn type now: %i" %(chosenTurn)) + if chosenTurn != 2: + break + # end of fix + + self.turn_type = chosenTurn + self.pub_turn_type.publish(self.turn_type) + + id_and_type_msg = TurnIDandType() + id_and_type_msg.tag_id = taginfo.id + id_and_type_msg.turn_type = self.turn_type + self.pub_id_and_type.publish(id_and_type_msg) + + # rospy.loginfo("possible turns %s." %(availableTurns)) + # rospy.loginfo("Turn type now: %i" %(self.turn_type)) + + def setupParameter(self, param_name, default_value): + value = rospy.get_param(param_name, default_value) + rospy.set_param(param_name, value) # Write to parameter server for transparancy + # rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value)) + return value + + def on_shutdown(self): + rospy.loginfo(f"[{self.node_name}] Shutting down.") + + +if __name__ == "__main__": + # Initialize the node with rospy + rospy.init_node("random_april_tag_turns_node", anonymous=False) + + # Create the NodeName object + node = RandomAprilTagTurnsNode() + + # Setup proper shutdown behavior + rospy.on_shutdown(node.on_shutdown) + # Keep it spinning to keep the node alive + rospy.spin() + From 7b05e7121d6c1b5d76df9987c1835d1dfd05dfcf Mon Sep 17 00:00:00 2001 From: Konstantin Date: Wed, 24 Feb 2021 11:26:07 +0300 Subject: [PATCH 05/18] update stop line detector --- .../config/stop_line_filter_node/default.yaml | 2 +- .../packages/stop_line_filter/src/stop_line_filter_node.py | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml b/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml index cec53ae9..7d4c7fa0 100644 --- a/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml +++ b/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml @@ -1,4 +1,4 @@ -stop_distance: 0.25 +stop_distance: 0.13 min_segs: 6 off_time: 16 max_y: 0.2 diff --git a/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py b/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py index 7db88656..3a6e2271 100644 --- a/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py +++ b/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py @@ -96,6 +96,7 @@ def cb_segments(self, segment_list_msg): good_seg_count = 0 stop_line_x_accumulator = 0.0 stop_line_y_accumulator = 0.0 + stop_line_min = 30.0 for segment in segment_list_msg.segments: if segment.color != segment.RED: continue @@ -106,10 +107,12 @@ def cb_segments(self, segment_list_msg): p2_lane = self.to_lane_frame(segment.points[1]) avg_x = 0.5 * (p1_lane[0] + p2_lane[0]) avg_y = 0.5 * (p1_lane[1] + p2_lane[1]) + if avg_x < stop_line_min: + stop_line_min = avg_x stop_line_x_accumulator += avg_x stop_line_y_accumulator += avg_y # TODO output covariance and not just mean good_seg_count += 1.0 - +# self.loginfo("stop_line_min_x = " + str (stop_line_min)) stop_line_reading_msg = StopLineReading() stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp if good_seg_count < self.min_segs.value: @@ -127,7 +130,7 @@ def cb_segments(self, segment_list_msg): else: stop_line_reading_msg.stop_line_detected = True stop_line_point = Point() - stop_line_point.x = stop_line_x_accumulator / good_seg_count + stop_line_point.x = stop_line_min #stop_line_x_accumulator / good_seg_count stop_line_point.y = stop_line_y_accumulator / good_seg_count stop_line_reading_msg.stop_line_point = stop_line_point # Only detect redline if y is within max_y distance: From d5d75ffc6e25b195472f436cc3063e7df5f789db Mon Sep 17 00:00:00 2001 From: Konstantin Chaika Date: Tue, 30 Mar 2021 18:59:06 +0300 Subject: [PATCH 06/18] add apriltag_postprocessing_node --- .../src/apriltag_postprocessing_node.py | 180 ++++++++++++++++++ 1 file changed, 180 insertions(+) create mode 100644 packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py new file mode 100644 index 00000000..c963c7c0 --- /dev/null +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python3 +import rospkg +import rospy +import yaml +from duckietown_msgs.msg import \ + AprilTagsWithInfos, \ + TagInfo, \ + BoolStamped, \ + AprilTagDetection, \ + AprilTagDetectionArray +import numpy as np +import tf.transformations as tr +from geometry_msgs.msg import PoseStamped, Pose +from std_msgs.msg import Header + + +class AprilPostPros(object): + """ """ + def __init__(self): + """ """ + self.node_name = "apriltag_postprocessing_node" + + # Load parameters + self.camera_x = self.setupParam("~camera_x", 0.065) + self.camera_y = self.setupParam("~camera_y", 0.0) + self.camera_z = self.setupParam("~camera_z", 0.11) + self.camera_theta = self.setupParam("~camera_theta", 19.0) + self.scale_x = self.setupParam("~scale_x", 1) + self.scale_y = self.setupParam("~scale_y", 1) + self.scale_z = self.setupParam("~scale_z", 1) + + # -------- Start adding back the tag info stuff + + tags_filepath = self.setupParam("~tags_file") + + self.loc = self.setupParam("~loc", -1) # -1 if no location is given + tags_file = open(tags_filepath, 'r') + self.tags_dict = yaml.safe_load(tags_file) + tags_file.close() + self.info = TagInfo() + + self.sign_types = {"StreetName": self.info.S_NAME, + "TrafficSign": self.info.SIGN, + "Light": self.info.LIGHT, + "Localization": self.info.LOCALIZE, + "Vehicle": self.info.VEHICLE} + self.traffic_sign_types = {"stop": self.info.STOP, + "yield": self.info.YIELD, + "no-right-turn": self.info.NO_RIGHT_TURN, + "no-left-turn": self.info.NO_LEFT_TURN, + "oneway-right": self.info.ONEWAY_RIGHT, + "oneway-left": self.info.ONEWAY_LEFT, + "4-way-intersect": self.info.FOUR_WAY, + "right-T-intersect": self.info.RIGHT_T_INTERSECT, + "left-T-intersect": self.info.LEFT_T_INTERSECT, + "T-intersection": self.info.T_INTERSECTION, + "do-not-enter": self.info.DO_NOT_ENTER, + "pedestrian": self.info.PEDESTRIAN, + "t-light-ahead": self.info.T_LIGHT_AHEAD, + "duck-crossing": self.info.DUCK_CROSSING, + "parking": self.info.PARKING} + + +# ---- end tag info stuff + + + + self.sub_prePros = rospy.Subscriber("~detections", AprilTagDetectionArray, self.callback, queue_size=1) + self.pub_postPros = rospy.Publisher("~apriltags_out", AprilTagsWithInfos, queue_size=1) + self.pub_visualize = rospy.Publisher("~tag_pose", PoseStamped, queue_size=1) + + # topics for state machine + self.pub_postPros_parking = rospy.Publisher("~apriltags_parking", BoolStamped, queue_size=1) + self.pub_postPros_intersection = rospy.Publisher("~apriltags_intersection", BoolStamped, queue_size=1) + + rospy.loginfo("[%s] has started", self.node_name) + + def setupParam(self, param_name, default_value=rospy.client._Unspecified): + value = rospy.get_param(param_name, default_value) + rospy.set_param(param_name, value) + return value + + def callback(self, msg): + + tag_infos = [] + + new_tag_data = AprilTagsWithInfos() + + # Load tag detections message + for detection in msg.detections: + + # ------ start tag info processing + rospy.loginfo("detection.tag_id = %s", str(detection.tag_id)) + new_info = TagInfo() + #Can use id 1 as long as no bundles are used + new_info.id = int(detection.tag_id) + id_info = self.tags_dict[new_info.id] + #rospy.loginfo("[%s] report detected id=[%s] for april tag!",self.node_name,new_info.id) + # Check yaml file to fill in ID-specific information + new_info.tag_type = self.sign_types[id_info['tag_type']] + if new_info.tag_type == self.info.S_NAME: + new_info.street_name = id_info['street_name'] + elif new_info.tag_type == self.info.SIGN: + new_info.traffic_sign_type = self.traffic_sign_types[id_info['traffic_sign_type']] + + # publish for FSM + # parking apriltag event + msg_parking = BoolStamped() + msg_parking.header.stamp = rospy.Time(0) + if new_info.traffic_sign_type == TagInfo.PARKING: + msg_parking.data = True + else: + msg_parking.data = False + self.pub_postPros_parking.publish(msg_parking) + + # intersection apriltag event + msg_intersection = BoolStamped() + msg_intersection.header.stamp = rospy.Time(0) + if (new_info.traffic_sign_type == TagInfo.FOUR_WAY) or (new_info.traffic_sign_type == TagInfo.RIGHT_T_INTERSECT) or (new_info.traffic_sign_type == TagInfo.LEFT_T_INTERSECT) or (new_info.traffic_sign_type == TagInfo.T_INTERSECTION): + msg_intersection.data = True + else: + msg_intersection.data = False + self.pub_postPros_intersection.publish(msg_intersection) + + elif new_info.tag_type == self.info.VEHICLE: + new_info.vehicle_name = id_info['vehicle_name'] + + # TODO: Implement location more than just a float like it is now. + # location is now 0.0 if no location is set which is probably not that smart + if self.loc == 226: + l = (id_info['location_226']) + if l is not None: + new_info.location = l + elif self.loc == 316: + l = (id_info['location_316']) + if l is not None: + new_info.location = l + + tag_infos.append(new_info) + # --- end tag info processing + + + # Define the transforms + veh_t_camxout = tr.translation_matrix((self.camera_x, self.camera_y, self.camera_z)) + veh_R_camxout = tr.euler_matrix(0, self.camera_theta*np.pi/180, 0, 'rxyz') + veh_T_camxout = tr.concatenate_matrices(veh_t_camxout, veh_R_camxout) # 4x4 Homogeneous Transform Matrix + + camxout_T_camzout = tr.euler_matrix(-np.pi/2,0,-np.pi/2,'rzyx') + veh_T_camzout = tr.concatenate_matrices(veh_T_camxout, camxout_T_camzout) + + tagzout_T_tagxout = tr.euler_matrix(-np.pi/2, 0, np.pi/2, 'rxyz') + + #Load translation + trans = detection.transform.translation + rot = detection.transform.rotation + header = Header() + header.stamp = rospy.Time.now() + camzout_t_tagzout = tr.translation_matrix((trans.x*self.scale_x, trans.y*self.scale_y, trans.z*self.scale_z)) + camzout_R_tagzout = tr.quaternion_matrix((rot.x, rot.y, rot.z, rot.w)) + camzout_T_tagzout = tr.concatenate_matrices(camzout_t_tagzout, camzout_R_tagzout) + + veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout, camzout_T_tagzout, tagzout_T_tagxout) + + # Overwrite transformed value + (trans.x, trans.y, trans.z) = tr.translation_from_matrix(veh_T_tagxout) + (rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_matrix(veh_T_tagxout) + + new_tag_data.detections.append( + detection + ) + + new_tag_data.infos = tag_infos + # Publish Message + self.pub_postPros.publish(new_tag_data) + + +if __name__ == '__main__': + rospy.init_node('AprilPostPros', anonymous=False) + node = AprilPostPros() + rospy.spin() From 02f228c2fd6e83437f04808b4209fabb72dbb30f Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 30 Mar 2021 20:33:49 +0300 Subject: [PATCH 07/18] add fake_tag without hardcoded values --- .../src/apriltag_postprocessing_node.py | 36 +++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index c963c7c0..5342af88 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -67,8 +67,9 @@ def __init__(self): self.sub_prePros = rospy.Subscriber("~detections", AprilTagDetectionArray, self.callback, queue_size=1) self.pub_postPros = rospy.Publisher("~apriltags_out", AprilTagsWithInfos, queue_size=1) - self.pub_visualize = rospy.Publisher("~tag_pose", PoseStamped, queue_size=1) - + self.pub_visualize = rospy.Publisher("~tag_pose", PoseStamped, queue_size=1) + self.fake_tag_publisher = rospy.Publisher("~detections", AprilTagDetectionArray, queue_size=1) + # Need to call fake_tag, when FSM_state changed to intersection # topics for state machine self.pub_postPros_parking = rospy.Publisher("~apriltags_parking", BoolStamped, queue_size=1) self.pub_postPros_intersection = rospy.Publisher("~apriltags_intersection", BoolStamped, queue_size=1) @@ -80,6 +81,37 @@ def setupParam(self, param_name, default_value=rospy.client._Unspecified): rospy.set_param(param_name, value) return value + def fake_tag(): + tags_msg = AprilTagDetectionArray() + tags_msg.header.stamp = rospy.Time.now() + tags_msg.header.frame_id = "" + tag = AprilTagDetection( + transform=Transform( + translation=Vector3( + x=p[0], # Here will be hard coded values + y=p[1], + z=p[2] + ), + rotation=Quaternion( + x=q[0], + y=q[1], + z=q[2], + w=q[3] + ) + ), + tag_id=tag.tag_id, + tag_family=str(tag.tag_family), + hamming=tag.hamming, + decision_margin=tag.decision_margin, + homography=tag.homography.flatten().astype(np.float32).tolist(), + center=tag.center.tolist(), + corners=tag.corners.flatten().tolist(), + pose_error=tag.pose_err + ) + # add detection to array + tags_msg.detections.append(detection) + self.fake_tag_publish(tags_msg) + def callback(self, msg): tag_infos = [] From b5c8625de77b2e429acb82ed1eb87dc8c47df403 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Wed, 31 Mar 2021 15:46:47 +0300 Subject: [PATCH 08/18] add fms subscriber and timer --- .../apriltag_postprocessing_node.launch | 29 +++++ .../src/apriltag_postprocessing_node.py | 118 +++++++++++------- 2 files changed, 103 insertions(+), 44 deletions(-) create mode 100644 packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch diff --git a/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch b/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch new file mode 100644 index 00000000..54321794 --- /dev/null +++ b/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index c963c7c0..5cd2ee7a 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -2,12 +2,14 @@ import rospkg import rospy import yaml +import datetime from duckietown_msgs.msg import \ AprilTagsWithInfos, \ TagInfo, \ BoolStamped, \ AprilTagDetection, \ - AprilTagDetectionArray + AprilTagDetectionArray, \ + FSMState import numpy as np import tf.transformations as tr from geometry_msgs.msg import PoseStamped, Pose @@ -16,10 +18,14 @@ class AprilPostPros(object): """ """ + def __init__(self): """ """ self.node_name = "apriltag_postprocessing_node" - + self.timestamp = None + self.state = None + self.max_time_waiting = 1 + self.last_time_apr_tag = rospy.Time(0) # Load parameters self.camera_x = self.setupParam("~camera_x", 0.065) self.camera_y = self.setupParam("~camera_y", 0.0) @@ -33,40 +39,38 @@ def __init__(self): tags_filepath = self.setupParam("~tags_file") - self.loc = self.setupParam("~loc", -1) # -1 if no location is given + self.loc = self.setupParam("~loc", -1) # -1 if no location is given tags_file = open(tags_filepath, 'r') self.tags_dict = yaml.safe_load(tags_file) tags_file.close() self.info = TagInfo() self.sign_types = {"StreetName": self.info.S_NAME, - "TrafficSign": self.info.SIGN, - "Light": self.info.LIGHT, - "Localization": self.info.LOCALIZE, - "Vehicle": self.info.VEHICLE} + "TrafficSign": self.info.SIGN, + "Light": self.info.LIGHT, + "Localization": self.info.LOCALIZE, + "Vehicle": self.info.VEHICLE} self.traffic_sign_types = {"stop": self.info.STOP, - "yield": self.info.YIELD, - "no-right-turn": self.info.NO_RIGHT_TURN, - "no-left-turn": self.info.NO_LEFT_TURN, - "oneway-right": self.info.ONEWAY_RIGHT, - "oneway-left": self.info.ONEWAY_LEFT, - "4-way-intersect": self.info.FOUR_WAY, - "right-T-intersect": self.info.RIGHT_T_INTERSECT, - "left-T-intersect": self.info.LEFT_T_INTERSECT, - "T-intersection": self.info.T_INTERSECTION, - "do-not-enter": self.info.DO_NOT_ENTER, - "pedestrian": self.info.PEDESTRIAN, - "t-light-ahead": self.info.T_LIGHT_AHEAD, - "duck-crossing": self.info.DUCK_CROSSING, - "parking": self.info.PARKING} - - -# ---- end tag info stuff - - - - self.sub_prePros = rospy.Subscriber("~detections", AprilTagDetectionArray, self.callback, queue_size=1) - self.pub_postPros = rospy.Publisher("~apriltags_out", AprilTagsWithInfos, queue_size=1) + "yield": self.info.YIELD, + "no-right-turn": self.info.NO_RIGHT_TURN, + "no-left-turn": self.info.NO_LEFT_TURN, + "oneway-right": self.info.ONEWAY_RIGHT, + "oneway-left": self.info.ONEWAY_LEFT, + "4-way-intersect": self.info.FOUR_WAY, + "right-T-intersect": self.info.RIGHT_T_INTERSECT, + "left-T-intersect": self.info.LEFT_T_INTERSECT, + "T-intersection": self.info.T_INTERSECTION, + "do-not-enter": self.info.DO_NOT_ENTER, + "pedestrian": self.info.PEDESTRIAN, + "t-light-ahead": self.info.T_LIGHT_AHEAD, + "duck-crossing": self.info.DUCK_CROSSING, + "parking": self.info.PARKING} + + # ---- end tag info stuff + + self.sub_prePros = rospy.Subscriber("~detections", AprilTagDetectionArray, self.callbackDetections, + queue_size=1) + self.pub_postPros = rospy.Publisher("~apriltags_out", AprilTagsWithInfos, queue_size=1) self.pub_visualize = rospy.Publisher("~tag_pose", PoseStamped, queue_size=1) # topics for state machine @@ -74,28 +78,50 @@ def __init__(self): self.pub_postPros_intersection = rospy.Publisher("~apriltags_intersection", BoolStamped, queue_size=1) rospy.loginfo("[%s] has started", self.node_name) + self.sub_fsm = rospy.Subscriber("~fsm_state", FSMState, self.callbackFSMState) + self.timer = rospy.Timer(rospy.Duration(1), self.callbackTimer) + + def shutdown_hook(self): + rospy.loginfo("Publishing message shutdown") + + def on_shutdown(self): + self.shutdown_hook() + super().on_shutdown() + + def callbackFSMState(self, msg): + if self.state != msg.state and msg.state == "INTERSECTION_COORDINATION": + self.turn_type = -1 + self.state = msg.state + self.timestamp = rospy.Time.now() + rospy.loginfo("State: {}\n Timestamp: {}".format(self.state, self.timestamp)) + + def callbackTimer(self, timer): + rospy.logwarn('Timer is called') + rospy.loginfo('AprilTag last time: {}'.format(self.last_time_apr_tag)) + diff = rospy.Time.now() - self.last_time_apr_tag + rospy.logwarn('Time diff: {}, State: {}'.format(diff.to_sec(), self.state)) + if self.state == 'INTERSECTION_COORDINATION' and diff.to_sec() > self.max_time_waiting: + rospy.loginfo('Bot is still in intersection') def setupParam(self, param_name, default_value=rospy.client._Unspecified): value = rospy.get_param(param_name, default_value) rospy.set_param(param_name, value) return value - def callback(self, msg): - + def callbackDetections(self, msg): tag_infos = [] - new_tag_data = AprilTagsWithInfos() - # Load tag detections message + if msg.detections: + self.last_time_apr_tag = rospy.Time.now() for detection in msg.detections: - # ------ start tag info processing rospy.loginfo("detection.tag_id = %s", str(detection.tag_id)) new_info = TagInfo() - #Can use id 1 as long as no bundles are used + # Can use id 1 as long as no bundles are used new_info.id = int(detection.tag_id) id_info = self.tags_dict[new_info.id] - #rospy.loginfo("[%s] report detected id=[%s] for april tag!",self.node_name,new_info.id) + # rospy.loginfo("[%s] report detected id=[%s] for april tag!",self.node_name,new_info.id) # Check yaml file to fill in ID-specific information new_info.tag_type = self.sign_types[id_info['tag_type']] if new_info.tag_type == self.info.S_NAME: @@ -116,7 +142,10 @@ def callback(self, msg): # intersection apriltag event msg_intersection = BoolStamped() msg_intersection.header.stamp = rospy.Time(0) - if (new_info.traffic_sign_type == TagInfo.FOUR_WAY) or (new_info.traffic_sign_type == TagInfo.RIGHT_T_INTERSECT) or (new_info.traffic_sign_type == TagInfo.LEFT_T_INTERSECT) or (new_info.traffic_sign_type == TagInfo.T_INTERSECTION): + if (new_info.traffic_sign_type == TagInfo.FOUR_WAY) or ( + new_info.traffic_sign_type == TagInfo.RIGHT_T_INTERSECT) or ( + new_info.traffic_sign_type == TagInfo.LEFT_T_INTERSECT) or ( + new_info.traffic_sign_type == TagInfo.T_INTERSECTION): msg_intersection.data = True else: msg_intersection.data = False @@ -139,23 +168,23 @@ def callback(self, msg): tag_infos.append(new_info) # --- end tag info processing - # Define the transforms veh_t_camxout = tr.translation_matrix((self.camera_x, self.camera_y, self.camera_z)) - veh_R_camxout = tr.euler_matrix(0, self.camera_theta*np.pi/180, 0, 'rxyz') - veh_T_camxout = tr.concatenate_matrices(veh_t_camxout, veh_R_camxout) # 4x4 Homogeneous Transform Matrix + veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180, 0, 'rxyz') + veh_T_camxout = tr.concatenate_matrices(veh_t_camxout, veh_R_camxout) # 4x4 Homogeneous Transform Matrix - camxout_T_camzout = tr.euler_matrix(-np.pi/2,0,-np.pi/2,'rzyx') + camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2, 'rzyx') veh_T_camzout = tr.concatenate_matrices(veh_T_camxout, camxout_T_camzout) - tagzout_T_tagxout = tr.euler_matrix(-np.pi/2, 0, np.pi/2, 'rxyz') + tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2, 'rxyz') - #Load translation + # Load translation trans = detection.transform.translation rot = detection.transform.rotation header = Header() header.stamp = rospy.Time.now() - camzout_t_tagzout = tr.translation_matrix((trans.x*self.scale_x, trans.y*self.scale_y, trans.z*self.scale_z)) + camzout_t_tagzout = tr.translation_matrix( + (trans.x * self.scale_x, trans.y * self.scale_y, trans.z * self.scale_z)) camzout_R_tagzout = tr.quaternion_matrix((rot.x, rot.y, rot.z, rot.w)) camzout_T_tagzout = tr.concatenate_matrices(camzout_t_tagzout, camzout_R_tagzout) @@ -174,6 +203,7 @@ def callback(self, msg): self.pub_postPros.publish(new_tag_data) + if __name__ == '__main__': rospy.init_node('AprilPostPros', anonymous=False) node = AprilPostPros() From d642bfe79992ca7c8768ff5ff295b312081e753d Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Wed, 31 Mar 2021 15:49:06 +0300 Subject: [PATCH 09/18] add fms subscriber and timer --- .../packages/apriltag/src/apriltag_postprocessing_node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index 5cd2ee7a..15cc230e 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -2,7 +2,6 @@ import rospkg import rospy import yaml -import datetime from duckietown_msgs.msg import \ AprilTagsWithInfos, \ TagInfo, \ From 84cf7291ba9f0c22ed8a0dcac4866d5c714f835c Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Wed, 31 Mar 2021 15:49:58 +0300 Subject: [PATCH 10/18] add fms subscriber and timer --- .../packages/apriltag/src/apriltag_postprocessing_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index 15cc230e..56fb202f 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -21,7 +21,7 @@ class AprilPostPros(object): def __init__(self): """ """ self.node_name = "apriltag_postprocessing_node" - self.timestamp = None + self.timestamp = rospy.Time(0) self.state = None self.max_time_waiting = 1 self.last_time_apr_tag = rospy.Time(0) From 1571efc689a7f7c0f36d678059a15a4ea6a4e477 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Wed, 31 Mar 2021 15:57:42 +0300 Subject: [PATCH 11/18] add fms subscriber and timer --- .../launch/apriltag_postprocessing_node.launch | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch b/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch index 54321794..022494f6 100644 --- a/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch +++ b/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch @@ -1,16 +1,3 @@ - - - - - - - - - - - - - From edd8afa994885a9a3fcd2668c549deae5cf29fe4 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Wed, 31 Mar 2021 17:47:33 +0300 Subject: [PATCH 12/18] add method stub --- .../src/random_april_tag_turns_node.py | 65 +++++++------------ 1 file changed, 22 insertions(+), 43 deletions(-) diff --git a/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py index a51fe491..1426e5ae 100644 --- a/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py +++ b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -# -*- coding: utf-8 -*- import math @@ -48,10 +47,11 @@ def cbMode(self, mode_msg): # rospy.loginfo("Turn type now: %i" %(self.turn_type)) def cbTag(self, tag_msgs): + rospy.loginfo('in ') if ( - self.fsm_mode == "INTERSECTION_CONTROL" - or self.fsm_mode == "INTERSECTION_COORDINATION" - or self.fsm_mode == "INTERSECTION_PLANNING" + self.fsm_mode == "INTERSECTION_CONTROL" + or self.fsm_mode == "INTERSECTION_COORDINATION" + or self.fsm_mode == "INTERSECTION_PLANNING" ): # loop through list of april tags @@ -69,45 +69,21 @@ def cbTag(self, tag_msgs): if idx_min != -1: taginfo = (tag_msgs.infos)[idx_min] - - availableTurns = [] # go through possible intersection types - signType = taginfo.traffic_sign_type - if signType == taginfo.NO_RIGHT_TURN or signType == taginfo.LEFT_T_INTERSECT: - availableTurns = [ - 0, - 1, - ] # these mystical numbers correspond to the array ordering in open_loop_intersection_control_node (very bad) - elif signType == taginfo.NO_LEFT_TURN or signType == taginfo.RIGHT_T_INTERSECT: - availableTurns = [1, 2] - elif signType == taginfo.FOUR_WAY: - availableTurns = [0, 1, 2] - elif signType == taginfo.T_INTERSECTION: - availableTurns = [0, 2] - rospy.loginfo(f"[{self.node_name}] reports Available turns are: [{availableTurns}]") - # now randomly choose a possible direction - if len(availableTurns) > 0: - # 3501: turn off right turns - # randomIndex = numpy.random.randint(len(availableTurns)) - # chosenTurn = availableTurns[randomIndex] - while True: - randomIndex = numpy.random.randint(len(availableTurns)) - chosenTurn = availableTurns[randomIndex] - rospy.loginfo("Turn type now: %i" %(chosenTurn)) - if chosenTurn != 2: - break - # end of fix - - self.turn_type = chosenTurn - self.pub_turn_type.publish(self.turn_type) - - id_and_type_msg = TurnIDandType() - id_and_type_msg.tag_id = taginfo.id - id_and_type_msg.turn_type = self.turn_type - self.pub_id_and_type.publish(id_and_type_msg) - - # rospy.loginfo("possible turns %s." %(availableTurns)) - # rospy.loginfo("Turn type now: %i" %(self.turn_type)) + chosen_turn = self.get_chosen_turn() + rospy.loginfo("Turn type now: %i" % (chosen_turn)) + # end of fix + + self.turn_type = chosen_turn + self.pub_turn_type.publish(self.turn_type) + + id_and_type_msg = TurnIDandType() + id_and_type_msg.tag_id = taginfo.id + id_and_type_msg.turn_type = self.turn_type + self.pub_id_and_type.publish(id_and_type_msg) + + # rospy.loginfo("possible turns %s." %(availableTurns)) + # rospy.loginfo("Turn type now: %i" %(self.turn_type)) def setupParameter(self, param_name, default_value): value = rospy.get_param(param_name, default_value) @@ -118,6 +94,10 @@ def setupParameter(self, param_name, default_value): def on_shutdown(self): rospy.loginfo(f"[{self.node_name}] Shutting down.") + def get_chosen_turn(self): # stub + rospy.loginfo("get_chosen_turn()") + return 0 + if __name__ == "__main__": # Initialize the node with rospy @@ -130,4 +110,3 @@ def on_shutdown(self): rospy.on_shutdown(node.on_shutdown) # Keep it spinning to keep the node alive rospy.spin() - From 0f64adbd83e1655f9300afb05dc6358c6e2850ab Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Wed, 31 Mar 2021 17:49:02 +0300 Subject: [PATCH 13/18] add method stub --- .../packages/navigation/src/random_april_tag_turns_node.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py index 1426e5ae..1b967669 100644 --- a/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py +++ b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py @@ -47,7 +47,6 @@ def cbMode(self, mode_msg): # rospy.loginfo("Turn type now: %i" %(self.turn_type)) def cbTag(self, tag_msgs): - rospy.loginfo('in ') if ( self.fsm_mode == "INTERSECTION_CONTROL" or self.fsm_mode == "INTERSECTION_COORDINATION" @@ -95,7 +94,6 @@ def on_shutdown(self): rospy.loginfo(f"[{self.node_name}] Shutting down.") def get_chosen_turn(self): # stub - rospy.loginfo("get_chosen_turn()") return 0 From 24ea8ac902c5071fab269633cfa0f314697b4fc6 Mon Sep 17 00:00:00 2001 From: light5551 Date: Wed, 31 Mar 2021 22:19:08 +0300 Subject: [PATCH 14/18] move without apriltag --- .../packages/apriltag/src/apriltag_postprocessing_node.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index 56abdc4c..21d1ba0e 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -147,9 +147,7 @@ def fake_tag(self): tags_msg.detections.append(tag) self.fake_tag_publisher.publish(tags_msg) - def callback(self, msg): - - def callbackDetections(self, msg): + def callbackDetections(self, msg): tag_infos = [] new_tag_data = AprilTagsWithInfos() # Load tag detections message From b49d739563a8e6e8a971d866b51ee7d13d2887f9 Mon Sep 17 00:00:00 2001 From: light5551 Date: Wed, 31 Mar 2021 22:41:10 +0300 Subject: [PATCH 15/18] refactoring --- .../src/apriltag_postprocessing_node.py | 27 ------------------- 1 file changed, 27 deletions(-) diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index 21d1ba0e..1c942530 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -117,33 +117,6 @@ def fake_tag(self): tag_id=58, tag_family="tag36h11" ) - ''' - tag = AprilTagDetection( - transform=Transform( - translation=Vector3( - x=0.17, # Here will be hard coded values - y=-0.13, - z=0.11 - ), - rotation=Quaternion( - x=0.67, - y=0.73, - z=-0.03, - w=0.05 - ) - ), - tag_id=58, - tag_family="tag36h11", - hamming=0, - decision_margin=63.566, - homography=[-41.83, 10.072, 441.941, -22.281, 43.970, 154.238, -0.1530, 0.0357], - center=[441.9419, 154.3480], - corners=[415.42, 185.73, 464.715, 199.214, 480.802, 108.3516, 423.951, 118.906], - pose_error=0.000000046 - ) - ''' - - # add detection to array tags_msg.detections.append(tag) self.fake_tag_publisher.publish(tags_msg) From 279145b291cdf58fa1b2458b8c89adf9351498ff Mon Sep 17 00:00:00 2001 From: Sergey Glazunov <31547459+light5551@users.noreply.github.com> Date: Thu, 1 Apr 2021 10:26:09 +0300 Subject: [PATCH 16/18] Update apriltag_postprocessing_node.py --- .../packages/apriltag/src/apriltag_postprocessing_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index 1c942530..33f601cb 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -114,7 +114,7 @@ def fake_tag(self): tags_msg.header.stamp = rospy.Time.now() tags_msg.header.frame_id = "" tag = AprilTagDetection( - tag_id=58, + tag_id=56, tag_family="tag36h11" ) tags_msg.detections.append(tag) From d024f799d46a457a2a7f4d329ec83fdaf3098d39 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Tue, 6 Apr 2021 17:42:37 +0300 Subject: [PATCH 17/18] impove bot detection --- 1.jpg | Bin 0 -> 37126 bytes .../launch/vehicle_detection_node.launch | 19 +- .../launch/vehicle_filter_node.launch | 17 +- .../src/vehicle_detection_node.py | 167 ++++++++++++++++++ test-arr | 0 5 files changed, 194 insertions(+), 9 deletions(-) create mode 100644 1.jpg create mode 100644 packages/dt-core/packages/vehicle_detection/src/vehicle_detection_node.py create mode 100644 test-arr diff --git a/1.jpg b/1.jpg new file mode 100644 index 0000000000000000000000000000000000000000..01327a7a92dc9cec5361cc9946d3401cb8a0610e GIT binary patch literal 37126 zcmeFYbyOVR);8GC5C{?=NN|_n?sPX2JUGE6Sa6r%?m>bFCpf{~-66P3a0yPMjWr*? z``+(;XJ)NgYi9nPd%COEsoMMOI?pcK=h>&87oRr(*m6>`QUC-50080T2YB8DSV_8D zegObvWtjk|00008c#8o1NA$0S69Ex`^HS$}SuDel{5|06+vF=DhxwPWOue;J@oC{}_8I&zrBWEL>b1gxJ~boY{;_?TyXYOzdsh-HjaB zIoUYa0U{802O|?}GZ#u@GjmHj5cOGWCpD#|DTrE=SAj#pLBhlv2zx32T}jaxX?@epWW=#l>bt3u?A6VDJWA)*gKg~ z^04u+aj?EXcjKadvEgL;MM(97)W0pfY=Nl%-PNyOzp{PhX0vxPXXg|Y6lCY%V&~#w zebHcb_ONp?a%Z)3rup9%KA1V1I9WQlSlZiB{$tU|*xuCzME!F7e|604#nJz5_y6RA zeUz#5>mEwH?z_DU}Twv|L1}KiQvraUc$DoFY(#)3P1t?L`3}O|5A`% z{>Z4v$Vf=YXecPJP%+RjFwoJ^(J|k=#m0Pt^9CIqn*bXJ7Y`pF9|P+hApsuYTReQc ze~cgiU)mrczeYxWjfaVjiTD4yJa++bP!UuRx`7DP07M)FAP&NFAAsT|&_zc0SNX3n z^<^9JrH`*rP+y~=zbMpW0}v5_Ktv?qKfQjD`n<>iNI1xEsW`=6;i?#+P&?vr1;qVC zr4g_0##fy>r{y+w3Ve-5@Q#p(n2w&|JtGqjFCV{vppe7|NhxU=SvfWJPa2w9+Bznt zW?#%LEUlbfT;0C9dw2!~hlGZOM?}UaBqk-N{76m9&C4$+EGjPfRa0A6-_Y39+|twA z*FP{gG(0jrGdnlGu(-6mxwXBsySM-M;1GIod3Akr3%k4j2NwbW_+PMI*8hd<|AGtW z1s5U`5)cXHA6y8CU;hD)gM>`Q`Rc8h3W|{Sf{6N`LthI zzY5rV(ls!9s9||UsMK+DGHy*D6_am5X`U@MUvbcoEBnPd{1PbL_%=2%&;yB_IV{F7 zoVb*cVsVYgKjT=qq7b;AGqJ66kL3@Z|B|9VZ~rLT-SA6sO`Uml6PM^{omZu4f*4kG zzJ`SY>*(z#$CzWCoBwP+{U@YbqGwNFn6_eC%-~OZ>xuk!`_1<>pXNyt<+kS4Qr3*% zZoa8_6M;jHC8=)xB5?hZp`JVQri-mJi>GfaP3)aH;br~}z%1H(1=`r3+`$I5xw&Ur zUG71$2UYyOyOnQxXG+wIY@5FiZi}2gYPyy--&Tsw;+V>!0c93u+>#F3*v;ij?(e-% z{VZ^(;E?hRAbd0{rXg~jua6}~)kN}nJ?5B39$Td3%!9isn{UVxU~2ij0Y&1w$5#7x zFXUAls7L~N_V@CPP^he#%|P#xH2#~OJ@dHkcah%C95QUKK86gbbWJYd6up22+s)3U zJwaOeOy6At#X@vnnfvCw_87vSb>Y$&mUwa;IIjSYzsmQTtxfZX-=iFU!oNY^@DxmZ<-D=8|9<<`3L~aG@mEVT5*AoDQsju!7B)+Gv zTa)wG%+&Cw=EJux^$d!(e3T4d@qDRk1c|%amn+xL5KFIZRDc) z{9skwRk+BiSWUQ_kCaRqKGL9W*p5sSb_h$eAVSl%7Z}v3C#xCYj!b%mA|}C0g+lq=So{@bd z8i8ldb8WOzFGUimy!H&xF`);~_DJ&VrL+Uc?$we!$e&7DmGFLX&j;j+MXA2pi%%&B zl7b`!$Qe`dOebaOHzVzwWyisZ$OodN6aB0NN;r=kP`4E9-wQgQdZa*xqjY}C%$N2( z&UtIQ>Lo}IuGkilk)$*dv97zD5y2U}^{l04JmMAh%w}PKl%Qu?01Oi9!3Mpj> zUHN924jV!93V%%2rVj)`oy}W`W#7U^gLfdBmev-@7jNh!Kf1$6ZXl}dgfHhY-n!^~ zKIXg7C_C#Zb4FdNF=f($?)M{kVp38HosW_egx24Xn-XdA4uS7!h^pbuI7PwGsLvU_Xsu zbCMEYz45N-bD^Qh%bwkifz&_nAZG|O7kh!G~h*bdc+EGRB7y0y^P3cw=FBM#;)-<7DjAxqh=B{|7Idq3ViF&kd2~^$o|Gdz#@sBnQlT{J3DZhzQTHHu*wzwJw=3ICU={#DJ6Zh8;qAKlBw;w6 z4B|T0GU)5xZhG5hlO1_|k1cjpLFP?T*6n-Yo_4d}r(@DzSDzS78rR@iQ-O|QUQ{V} zT>^`kc%ZNSBJkHSGD5DeHsSmk5ctRzxgdmDpSjw5mY%&f9=6&c^Jtwj%P_VP{KyDsPI7K_jB1=_i2i>Fa z^*!56JG^2K@1THM|0u5rb~b;~?^<>~n2x@kF&k~;f5^$W}}jt5IvJQ&KYVF3w?71yv;$*!iqDX!xkiQI0Y-Zr4j=iuDM~bDHj{6Rr@Jz`mBmi7W z`iB<@lB&iCrM>PRR1qbj*p>`t108fbaf+glS&k@z4*Ubeql^kHX z0QBM3z7Gxdj}M|*)CA_ha^+PB6X<-7TI%PwTf_OJArfhtUm7?o%83C!$VOJ07drbW zkeNs8>Na#vh6?Mn`8p_9HE{mhsL*3~TLg zS!wt@04_hHHp*Y+xAI1GZn{kd4`fguMc4J3@N)-MBOM~1@0GI z-9EHXp&&QxL`{3e-v}5kNo>Wz6rh!IS%>z_lJkxQhm;viVUU*Z&wv&f(G`8C>dlG3 zQRJf;v?8M+E#p_7jk8(k%;YoR{;qPS@}>3QBkRi@crW$SnLWMSGVg(373pz6s|#V` zVYI#zSMO3I^C5A@;)WEe2u!mD#=Pe=xvpd$!CRPI|(%G*SU= z%p1(?J-Yv7-T964(5Z+vr6c`_7FHS9UjQnRnFzsj`k|MV_ zC&HY2#EH}~!}L5w4bi2E=PiCz+q^C4wm)f|8H8w0G+6_K2RU^RYF`ttkay*#rOHy;cc-%KU=n2e;6WlUSzr$)g8jT^Z7E`S(2 zJf5abJv{6{4H;N;AcqQojwxd*OXKBl1uT?fI0)nz6O216x0+p}d*v((Am}Q@ZC#wXc?K}qO;~*^n<;fsbrk1qXUrxNArQAySdLWABM#hLR ze4D?rN{oURDjVymx8Y+Y0v%&IjsGAswU<;I94*iJh|KwxV4Pcqgg(L~4ft@R8x9!c z>KlQG80WGFdQNs_fWdVIFnAKG;P75z#_its!Q{OpSzbb<>k_;v2U<0yzaU9YC9;EO zfI*_(*LNUT0SeFR{v)H0`Dc{vZdsx08v@)bExB!`?!b#9hscnFmzHZ;SJxH8LOe3*JT#rYw%!J%l-QsRQ0MVe$4gQowv4BeUNDd+G z*2sqWLS?xpUr5QIYoQITIYqQPyxP0F@pDjG{bnUmeZrkngV=3;uUh^B--;pU2BOz( zIo8R>vfG~}VI{de5$R%|2KrPKMJcAr(5RLSTOvW5?F5BWC5a3ui&D8Pdva?kS8t5< ze5VoCH-^n;zySXF(@v|4JzjC$=Rvd-`2_SQrQcb1_(MweMKF224|8;!XLmQ@OA%!E zKm+G(=`eYy-a;B)BaUv}=vQ-*I36iM0p~^sBD6nP(}j+ElG4@`by3}1=mryAWWf6{;^QjQ!(m8JHXvbAKfsxAv#k5=|#P2_RQJH0^w8Uhn3a(oaFAt9R#V z*xhab7cn1C(5MaY!)ZzzVcJvuc;MX+KhJG8guR?Myw1PhMxOy8WS!3d97+ACQ%{;B zrf&)UKC;CGduM944O7(y#{xiTF&(rs`0B4o7D;%Krcc0z{;or}tg$CR+c~R`*ez*0 zY2I5Z?SmV)P7}uXm(yI{&+0K;M1gTS7rCbf4d)L*srxaU>!WuO`?FU+PhGyD|Fy+P zzR(}-b6i#AZ+-@tK~g@>wd<&uThdOaT;!?I1QQwmLMIoFNA_sT!moxtSn8Zaq+#fJ z)DmQPG-(iG6j6Da`zs31yWpEsQBo;zR8UFi7fv`4CEZX1jKA_!-KUQWtCUk@40j;JBOokH-cYyMiSfusZ2f#c zYr`$SmZqDABm)4bmYk`ao?2W&hH^;gSkFGLAr!vbN^j-CNNjutROHcKydQ>y2mrXe zCaFZbascoD#MBXt0SN-G@)-azv%qD4imZ<7X>Xxm`EoA>gNTM z^A6U1?L>@6=wOCy@)HZ6aR zL~*}MRlyz;m=P}iYe(&7!)uyN`0LeP)~gL6b*?@jFirg^QkipsI-~kh23f6Nz-6DP zV5OgsrjrLl$oYO!m&a}JA`cG`ngS{*{7&j9*_9wje#M+*6NCEKhcOsBaUk=ykTIFLjO;gn1NVgI1I^7o z%<>uVdcB+I+Dq&9FpDVVK^)ZSkpp2FoyktYyw-uKZ5sUWmM|&qC;e%F!KvyO9E0i7 zuKMNs;#nK3rFC)h$Jhc}D+4^4bVh&9c*lN^FPU4f&Kt@_)K>edf9|FZLc;t;8rVAo z7R$Wwv)rzuv98Nbr`&{eCom*gY*5Gj(01(~(H(&v4cneE4%XSNum#F57c8o?LWW9$EXPHvthBoKV+VS(HO@FQGH^bI(yOG{2=M2L3H2lp%wdj_W=c;2?T ze+r_Hh0=6Zc?57Ih=<0-juJA^pl1Z`zbLSmbYHXCF|>qqsL)!xvpgCCPI@k2cG_`9 zy~$C%TLD226n|@gM_vK~A8+vo{zz9PBn6akUg=qcj+wHnG0U>@sU)Q=ZSPs6@7ViD z=hi3fCY#qc1)+Xnn*57oyh=pl@1OvXuc$W|>f}x_0H=VG=t_E#r+bl3uYBxRwWy82 z`GF|Pq$IL%R4^FU#_Ln0V?crO$QIAIt!-i}t;L1GfrQht&|ytRLnZ4<{qii)=YM*u zgz2|w$o~wWQTgPzNq%1Rgyx3~IR``F(z$heQf}r|enu8JoVKd!v)At~iR~HR5O(|y ziFOWn$RC_pYbm9mR_Etj_DPl{ig^3st+*nWk4M+zuRacGTkIIZX*?i;iE@tW$?$D? zcrzZfY&0lPa3d-ZOWCKC{o`c0%a7k04O1&pvUDn`v3s6aR!d!orE7YF=GGz?e2ZJr z`A))2QQ4C!Uav_zqtA;zMw!#?*bj@s>&NhR=AQvkd+#8NGgNnErmjQBBf%Q)=&O_W z;)q{i;RVM#Dxu5zgX=MJxz7gY0}+T$Br;qIal3Q$B>~1!{_bNO7mRpczM*QE9LSDa zI&jM=f)i0cn)5^l$S1^FMNfE&c4cHSadCL5#`{=efoETbEAfzfUP^IUDj?$G;ryYIlToUiokacTA$ksd~Ob0A&AFr_(r7FHhK4lilc zO9e+}0%5W7p*$xPNPlFh&(@RTFwG)QJ8PyVy6IY{gb1{8FUh+s%fyN{e*ftsm;7nC zqScPhn_L+E%7`j=iv~cGz3@=yG_U+%Yg=RXl$G3Nq(|F?vWoT#voh+SJx{E&-qmZ% zQY$w5vG$z{VVs*`n#|gSsMmBEvAKO%RYfE3gssPvK1`c(9)jObm96@Xve%W$Uae%n zKbgxdAoa-bd^-v%s<8Xggmzw6!}aS|)P_I4^D_XkLfpJ*Iycq0p*fTg?(kjO8LjFl zV0X|i2wdc9Wr5{~xs!b9d;hB1vZ(pXxZTG{MANKv=YYS+%*adC9$XWtcKfYOT9<5g z_GT_0fyupmuU`3~MX;UF0Y~FV$&Z{{@8=Y3Z!C#_m?3?Wt*H=tj2^%;Eb#tmn=4yP zYY1fwAL*r|sdWsBwMxzmt~X!UY{W|B1U^++BE?H7ejEp(s)F0lVE5a-{PS|ADN3!? zy|$Vbrjj?^CwJ573I=0H-wzwBLm$e?W{V*|%AU66+4dJgw>H7YMX_hy**lWx#rjmw z0Bt+?7&<%rxAjhV{rcOL?`nN%@g`B=oV&*#v)SjhZI)(SeI2yz+c`e*=1^w&wPS)y zaGk!Ds{n^C=M^qK!#T4it1G92cn#`m);qV&JE$S=BN$e_6|FSj$(CATW$j_^As8y@ zPNOO{08)Rm)jrPHP?Io=BX_K@c(^9YIVL_6bNyo>-(TKQmTc0_S#i@@MmFmz%Y0Y8 z?bI>NJzYduIE5cNJ4qpOnXOZD6RdJ66mTlZr!P0Xwx%0#x*52J|A+Qs2p`*Jm?1#H z4*9@#qoXlxewRSkI((TZ&*=M5R|(2OkYV&Q0AB>h>pXpnKm3uIC;r2->r78sl;)WK z<5ZSVV6f&vSkyDXiUJj;y%X5g8D1oIDR0lzZ`I~-{)u(nz4Tvh;@}H1$)2^cSWk%C|x;g9R>XUp377c_vWJcdj2TTZi^L3s~$jwQ$z~ zTmHk@Niz3owbpT`qrlmjo?7zYgc{tuB87d{tMB?`mCcSOB;X5p`PyYFg?ftWzSmUXh2pNNSGb*z;r;8u^%7mv5RdNo5iukpy;vYfSL=}MVS=LNH2 zT3H0ofb8UFK$jYvM{_m%yrcu!Z?ba(>-RGNde7uDzH`Zavf>3}D{2m9#NTZ&ll|^0 zE+zffs<6@NX68U9jNIo?O6-)c8Zy%HOBkh|@5}TjP6evCF0O(6&q+jtw60^0;P znS-=^f}2Z{5(ce}W?W+r zsYj~LcT~9?uOIu#@Z|^JYcbV~*s9vtt9-8wW3@vN%3@=N!M zajLM!`v*t9!sF5;(3g;K`flz>lcKk1;CC@J_mO|Fxg}ySr8TN8<{68$6T?vBId+_z zXvYE;=q2xGmF1~osb#k!w5uvEC&V0!BSA)$ie;OIASR%+YV;17;6>3HShi-kO z=;hT^s7OjE{c7*k5Z}qz_7+=J|1 zbK(Yq+or-=-nMREmpGK(<%Q31Wdv97YAC%t@Qe&Z;H$;B_dRJJynM9Y6$YuWUcqu} zF-)~IAfT{SWg)CV256b>PnFdxGKoPrC6p|&;cff3zEzpP*`Ei8T9;Yd_#+ae38#(x zjy*$eco|iICsFF=nE=#qyT~@EXjsY0h6VMaq~X^xZxqvdHKIW=@PBSjvvd~>{0|C2X%U9+e$@4SCrc=@|CCZ zBOO3Nz~zDm&~AGoztXSZNXZfE@xOOhz@xQt*4NFY8z15)w)n%gvpDe%Je$#cRNBGy zS8N4t^Y^yLBMwud7r?*b35I+yY{hkKZxBmC{xjgCUbAgqba{;RUww~fz?S5%n6Tpn zzrUC&tivf1v9+*ouJ83v6{Sh%+Z&^cIGQPh{T;a1+ME^I=}d@@nTux5-t6urDBvG@ zF@$%}J#wCFq_7o~9T`x@&#z+f$=DQF1Z1C1}0(e9D~3 z&TDyLS~lPUtI06dt@??_8h^MZr{;(ux(Ly)$4^lGv#={?Tm1fl#FT;xz1)@v9Jd|w z>p3^xKVJ)B0jPml?}Pj+J93 zb2nl#)Y|+rJtlM(;@3x3;zlv%JXBs{ejO9r)Xmo*2pTb$jIhUbDOd|7gH-LY3x$dE zs-%_rv>(EZ^d`}U2~*dk69~Z4Qzh&=nea4bvRRXo6trEbxzyyhBlY)tb5%+|-aDT; zFd{Yk-PfSKlv^v9x^gt=DDrRo=V?U8;j&7cHM)*zWlllhS23w*Og`sypM?uNwP!%x z)-xar2)};@9I2>{zO9&84uc7$%@UjXQbTR`3R_&88XCTRoas}^Hz6WfAsR_0gV;q92+EFUecXcb_>`Og7cu-w+m717>1%Ox`H4b}6h!^ZuCL ze)T<4=A*8+tcb;GsM)Kkbp!G>Ra8M8V7NZH*F*&^{&AHUBl1MFyx+=OZUfh}G^8ap`@2H3_ zJ0nQO2v{Fa5Ij<(xyWBRk$Lsm-2b=I|ajeAp9&?7 zn7qrmO8!xf%7M$mX3Qc9v_>UJ8U#%2Ng6kF-VJUrJE03n&U6%*<4p?%FhvQEs5}2P zlccyE2G4yf8bzv+GJODbugb1Y4h;_<_upjqg6C?Bv|0}dYNYXUJc1G#=ePxgqC6NlVZ$ue9DJTg@jugp7Z6t+?(*8p z#T~=z?fk_Uo8DtJkLc{+x>y$%4Y$5Qk8vV)*-{OJ=T#=v>=Qw@^ZT&izxmw>hve+y5gtZ z;$7Fk-`Y^5^FA+4P>D8)e7vd(2L%&ZuLD^CSqc0jL2aH)L9Soo1 z0W={z8vxC4g!54A2u+0X8$(F`T2d!b?@CPzZ)~5Bd7XCh@nCAg+xGY2OZDzpF9B^r zpC_m{-ZXMo=616N*)0nPHk0qIGUKgpZDj3i22Qc%h_fTo5=wL+#F;tG!enS>1-(I0 zXd01#|AmXUL20YMO*EuJNZYLkb&>@H1}BKgap?D5bF9ZJ~3d;1^*CcS6CC}Gnx zpfS9(?-^hm#t5zLQR?8OO8u^fg}mhzgPNeH){-}nR+Q_Q+Nh;((y`W^D33H-HM-j8m?4*q+7$AO z&w$%$fc3uLve%6-%=LVKv;Qpo>WN!<@>+t&ZC;~<8CxrT<|VCA9rTNRfV^op7*V$(o0G#nN%r-LR~%!I4ld3a1?5O|$A%fcAE%!_}H zKVlex?ppv`M+@A&%hkXs4#WW`AP;o+h{qH6C`3 zT^r}tW~PkP{I^;Z^Ck85Eh~2ljw1dTc-W;ckFt)IPle{Ja&e)blCsf0NbJSQW#z$j z*ip{$4ZdPBGk%x7EPapsh42Q5c`#-eK^e>u$vpNOAm}RUhwvSOAa~m^Rp&M%;&!QT zhcj^DeGw3KfznhuQ<9lVo>Fhz8D+A|nDjD`t1=12cU+a?(qejfFR0YgFny~sU4FH+qKw)hfkGz*4Rm7{aU%mAiA1@aXfyn-h_uXMxX8B>U!5T;#y)p1 zOz!9Q+n3QJ26)TVt`5|QRQHE|u~6qe&-X{z7VNivxg#yVu?@C$B)mmp!H~Z=LGhc( z@Uea+JwY}mP8pS@?7XlRJZQX^pM1XB!%AQlox6I(6R-&05{mr{5vmxZA)oD!HW~Uf zSb_Y0Sb#KJP7|FVUH!XojZRP0G0{-!f@2pECC(yRCF@dZV|{r8d?&@^8SwKHtD$<$ zWX?O{f|HK)*AJDh?-kZhgGPM-ys{maZ7-UsQ%sOXC8^@QU*fIGvLYUQVH!*>wK`!+QVp?S3UX3 z1mR>~3|$y!SX2=U6i_MJ=Cn}DDdNF|@^rPn#nfC8BRK6im74bLOV2H7cJ1ihvDF=k zc;wv0~)33>TJN-3n%EnN*t1G0HT0`2Cmj|QT0P>V)_;KAUCRf1K@FVqLq#Z&a_Lfv(d z>0h#ryNnQ?sDkS2F8Xg4R*fuwXs7EOUvD)nbHw5Zd+d`2td+nb`4tTIy-?hi2T`8l zo~&HmzNrM5w8#gq5w;nHNyLMV{xT((xZFCWuFz%RP82H8Rk`xQDU$8+4f^Di+xkRLXuv#cvS~)sg_Z36|stP%8!(mU)!3n+>=ba7m>z* zO*ANY?{EG!y)ZFj`-Saqa$m5{tc@YolKf&`5ujPKv&OgC1uTA zQL-PCtbTx6x3TD;Ts%lhm#~nNn^~4h6~!@9HMV6cU-jeDCm7b&6UhMdkbjxH;7ySw zVZ_I(1pjsZ5wU%dg~nxvUyf-oYxwJGw*_MMSmk}IRc#2(!zh~)TnNJSkT&@kbkOg_ zN+QLr;U>+zrG#nKFlA^Z+E&Xlc`;J4xH5eKeO!VJ4&@Nof~i1Xgz+ zb@7fp75h7_K3R<=!D-c2DljRf6&#J7@$^LoJ{=qxl$$$}xRXD70+08XL$B(+u+FsyhFZ z63rSGDPEDpJxo#;>2^kWqA^H`8w;=ZrD)1ySUV7g zp_x*~Zf-cvx0KEVlgMVdqD>c9O#Pj&u{_sWR01ct{!()GWhEJQu1DD6Bmxrh#@TMk z{}FW62<-t{n!D1HyTlLfk;+yj=@E3THuHvAZhf;SwY0R~q`+P?=xO|E&nCj}G1j5X zVNONj&_xT7@_OrN@M)`aS-3X~dHzv;b4=>;Q%g(3EUB!Sb&*KCFm`lPvoe?BYvfQY ztKFK~TA|Y`P>Zdnf`*}x_p5H&w#fE zC2mpj-jhp%O;p|lUj**Ncz+*JG;|c1(W2XLj1g1uFmqVP;tUE)w~7c2AZTp=ENf00 zQ?PG&9fV*Nu;&DfZ%sOcb8Y}I|ISy)spLsslWY^)ew_>=y$eQ;qzz<1@@_C zE;oMT<+nfkqc_AUTXH2X`xT9s25L*V+}tMc z9BSY*(P794P#lKu&vyzL!_2m0vB5j%Hw8|w2M$)@%{B}A`3D_bFrQ5ywVuW_7VKw0 zk2DjD(-=3iZZ?xlBlf$Hr&H$>^`BtfZ-11WuL=bWHdVD$3L{0yXv6I_)FuOkfsK&-p8+sT#bP-<^rCiRlFrXUy{Vv`sxwA2HgJyR13ry#DqRZ^ zX(Xn|JOH2u7@#-{CU_K^%G{sz6>42&N@$odY)%~p)sjOe<}-4qXVZ&jAAmiLDQSD9 z<$6zfngqTdiq_fxXFDS1-@5^2_k#(efU)}~P-md6(Jiif~&`+{N4 z$G)TNk;6BUfFE!_cf^9liqz}#`~cqe&w&h2|kdH>k@`Nb&yZ4*X}ePS-zxg>Tp zpP|6&w1`FUR)f;~%_5uIO}_qY9e?XKh9rsS?=ZH$M`|+P&Lyn=!_59z%ae9vq1#O- zC9snBU3E?KteK1IfVN9!XLlp z^4RBjo3&2UMm-(L3*v3he2W|Kf$IDMO--E8kQa@TbG%IPDw4P9V6K^~JFwj(IL|ax z^fHbMx)t+wR61`QIGT8+r=wJKn0#iBl4jJ{7D5}w^;vM>+@tGW^N%kX3T7R`4{=!agcC@1eNAJLCz?2PbX~Q)BQuEWr z3LEi@IE`s5e19rAry&|k6rG1AoW}h_!Q7IpSq?u!)s1%x-b;TKw zcIIq0s?IdiI0FUlXLJ>#f}+JI&0)H~U${Hbeog08oR!)S2_HF=^n<|V1Adtr%>p>Z zB1mM1zZ)V)cgwFUNwYIUmFzbQm5eca&RYG&$s|s`zea?%B2X~S$JSO%6>4N+k+z}BQMmYyBD%aK1LIFh)l4&TGq8E z8Z7cW1JlbkZdQI%^t9xM+_Oy0rcA=j#lzsV#fbi`nvf(3YJ6EDDJ|v=++(YvU{^!_ zmw6Bp#|mrF0cv``Rf&F>P#FeYl^g7f3(CuWa7r0idt-T<@8K~_6xZUzRL273v~v)A zV)BShMdT0|<#xWpJGSu|e7Ul>>ziAqz3o{W@^}ODlR3HaafXN*K8oGVUQHMar9Ifq zuN#D8w)6)d#ol?%ygBO|WQx24!e4h2`}F{~4icS~quXeo0j;n6Zag3O5Z$mzEV~h^ zGe}*#D^Q@dXxTUj34P~PE(&Ppz_=h#_2WdUVQO~$OV)24$D0Ti=d~?US6tr4UZF2? zz?w0zhpwHSO6~T(Rf3bwiF2IWF!!rxXCH`uOR|3ED%x$*XP8(rQWdmdu@6U9i0xZG zix~OaL3@tGp0@@h+ibqr4YGgeus(g1bP-$Z2if_GSa+~J_=X@yku)G`^(hUN$H-KO zzNA>S8bL=_ms=B$xScPzPqeA^I*Jmzf4vlKopMQ2O+trAp;6Thi59|vAS$@N~4=}oS5x>DM~;}mfNey>zTkwf=@`#2VFOYP!9CikvJjq7KAHHEqy2QH9U zMeTqA*6bsC$)HIot*j%huhkuwJ+hOObF9h>-?Xd(Ln2>QGb?fm2o-yzPk-ot7!f;3 zo76x-%J7`2A$*~nj;5P_7p8yJadE~O6fCEYCf!ZBDmIv=AAdD-_#TO~q8bO}I3|s} zRqNzn^*XUnE-TF6ns>BspGe?LG$!`M`e{?r+0Qm%<|;Q}^&vQuOXCafP1f?>J!6iYlh!CMTlDiA36CXOItg4ubYPZrouqZ zIP92Dv^oHmHrB)Z)j6JRg=QyVoBi7w(|O7HTz2SLwo=?y$_d}{hbp;IV%rawQnO6& z3N;tfDvNw4D2UrW;=*Z_?5&oBE7&&zq&$-mY7^jdK4Guwy+mNsIa%7X-lLYrImIut z`NnT3^dg5A>d}9)r<7R{%$L3Tpp$f|K<)Sp5DDTohXWNkBbCHP%44(3o&hGpA7fFIqKZGk=xb`r-^@?_fGje_(-qnNSVf?BAWwCicBlY|02=9Mnh#MWNK3 zR2qIWnARM{Dmi?I6Ld)*R~SA3;vX53e!8%{aIbv%cFHHc_wBlTM!`VyueKle{iu=< z@qs!Ods7pZ51MMp(_mtEU~ff;2yhcUbPz=?W)GIFNSish$8=i65XwV1)|W+c?wkB= zq@Xh77%$fB4=!=-FK=-uDRh`1n>93q)StN`!Gx>|yxYY^(O(*p-UD4Gx-a2@k?7ALt}5bTW|u=@dY=oD`{{@trOJa z8?w(ONfr1imYSgqI_Kp{F_Fv2YuS}n$^#9J7SL5Dg?cA*I$8sHU@y11U%hh&1&nwd z_mO*x0&lfot}utoWz@?|rrdn_%`Vy!ofBUm>b^+YE|L)*brdwpxb!fKQy)d1ASLbU zh7UcSRbl%VM!XCvmiJY^#b4tj2EtJ_cANa)Dw{_NZK61LNT1KW_Mw%^bQhxz6;e450k}#F!K5tFV-<5+1mZ=SC6!?1SoIZmC_qY zTZ=(BxKM(c{i)8l1cs>KiM)TDXR^s7>r3M;Ra@(JHx|-yx8q9jC)y3}rKKsk&Av=C zkj4HR#}Erk%LH*3P3_WeU0Ax_BdR&xlJ2V|{rC8bp_SCw5{8odY3E~C@knXQ zqQg#15d>;ThzJowIQz>iJk4TQ+^l%Mi&=ow8(~w%Us}}uKC9MUY&zu&Kpc# zuKTaYR>$_!&xI}ZET9=ob&4TEVb;LN1S@st^XbLoS%Tnz^J0eP{=iSDaNkga zFLu*xo<4Q2n#>1z>^ED2=Xi~(2xJ`0S2Q<)Q;&XISYauAewe(pGv0DDJ!?iu#$Ww? zuBAVVXN8G;Jy49PlvwzE*Zg@WMt7Smc98ij)Bx*SS7U?58joi9(HTOc{@89*;-#2E zEBxF*$*LMUAqAWIHMS87b1l`;4yTo zCNta0EDz~RSG&z-$~=1c(xX*`G={lcdgyLR*pi8jO)yt zj+gZF$wAelU=D8(?B~{5gzu}lN1|=oDG|X!nIg>^++oy=2%s?GoJ@+7nOoz>Y4`oj z&c1fP?c2~cwV&>j2U);snf}w5+M$4R-QQ|x(eYj61jutxkE)C8x`Rq2$F0paX)HdIuX z{e2-4z@%fFN6j|%_Dt~ibioy}el;a}95g7P0p@`=s*OK}z02@w`p(To5v7(z{Z&av zA0C@M4ab`R9yvn?hn2)A(x~kqgOFzc@~b(9mx%2AaBJBF`M2`=7-`V2n)Hvv`*5H| z2Qv%`iMVd6pDMCWLvpLuu5UAp{^R6yko3NOZ(3N%Si{n z3Aud{`OQponTB^qH~N0kHc7K(ai+?U6=2a8=b7R%InW%7t|#EfWT~6E?c%)yBuP9j zRlQmOTwnIRU^5COXTYT6{mka%&hYB6J%NV@p$T3ZXS3I4|I3IfxTPC?`uW@tnO0RhwY%Yu?a8<4pqiG=B9ZTHd zxA%^cB4+Bzl07{2Wo91)aRqdqM)x}7ZNywXE)`BjOF3ur(Y$!^tC7(?!H#b0ZgBehu2fRD=!py!`nwDh@p zpQ1nTPKYt^i^LZvgD$pX!M8><5ACUm}0OKJ4`lMYL zOUOu>3g-M>m#3{NPuVl2V4*0W;H$|Z~#Xmie#7r+uOZo zooq$8TuXZ>mz{7V|wL@r&WQ zd_~jIt>uo}RkUU{^2zrdN#z*z13c%kuK^zcylW(-PMHiU4m`)ai*MJ4C-kqW=eZ@A zt@;Us1e_GYF_yK!qIyL^gZ{=s^5nD44 z&v3`r74FX4diU>0Hn<9T98>1F?$4BbL*e_K26}RGA@Xo zJ_kL8RYr8-hDBY~p#II&uB`)!UI`j9>Ie)}kVyQi(moLUKk&bf>~2o0sa@OZFiW@W z@y#4T)Q^;mf=_(YdJP_3=AZfIznvBJSNtP2P^XI-3gHLaNMz)Esh0zC~juj{10OYT5A~o8a#q_}1q`x9Tu zELS>>jlP~q$+^`Z?RO{LWBZ`}abD-I{B`h0!;5Fr{3l@+sc?gDm!=?!-AL)aU*h~S zYs7qG@wdjFHQyZin`u@oPx>a8n1($W^SQq;J*p)wERNXGN2))Az7qT)L~jvZCbOz1 zD{piA=O@u^(||wT&(^R!P5V~drLnT`j)M-DZmYH{i`->L`!J`8%c;{CSkmz?iS>^eUHzwC)h=~u0m`kze(g{DF^nqy09HGYGu!D- z@?D9^+66L8X2AinM{+1MNQ~l`_%!b-O!X8rC%NL0Nf8hN@;@p<27jGO){Mgn#2S5u zi+`)#t(Ki_HTC0yRu~g=5If`3-m;X^+|e$?x-W3yny0x9#GQ+y!Tcy+9H30NL@)A8bkN}xLQQJP5HRZn%^^Xs0dX!LTJ{(;V*3`F{^D$|plcD}L01vM<=Srig zE?RpX5xUg!e;W864MW6}>fRod+FHHZ+(&JxD0u-g25c0-T=Rv&$E|*ce01=qi@bmE zzUNBtM!)|623(BP3(QU@k8M+#P1jU5%8ymw10>PP4R}D zV>+I$w-Z73>>*Q`5O3g>Jdc|k)sK!p2!0@ZM9@4FW#OM6$>ICm9h3V~>`OF1Y+#BE zxsUG<%Ex1Hb6*(zKK;KwE%>v<31xYyX!h|H-H5dI83V5-Ltcfge%HScbZ57gT|s5G znkH@bnqioRR>lYfk;gUF&lS+~{hZzPK9Ts%@Q23V5WIchZEM4xJh<@onW9LiP_$Vl zkL@^-S)43?E0zJ2tMSj_PxyiGgT?+V`0?Ry8fl(5xYE2grG)!NiXK(-HnSE`i}EmV zKqb0<0=`dA+l$6GXAtVME>BrB#DovU{{Wp_)jw;Ghyf zvG~&GU4c`VbLu@$;NObxe`$LUh|&1k<{t}N+k}4)`I3N0g`+93e(a%QA19ptbj=sw zw~4gR+B;VGi*e&?o39RC+q`jTC8hayAW(%#*bxXU24_2K0A^-SY|64iwk3b0PbVvj%4~2p*5w?r9W@{Tzpda zS!l-zJUWV=0XW-ic*rC8dy2LD8~DFL_;2EUPs7(?t)lS}ghhWWqiW3%Z!RVn7!Bq= z8`Oc{HSy-1@w3Mt8@?xaX5+?s^J`LRiF5XHZK8)@c>aI5qdD50eJj=;G>{cz!1Vrg z6ZfokMy%mS**%Zat!u+NZnbMIj;CjDeQ5a&6~YDw{42<{FWHvIO}U=)!CL;I;i=db zHf^@*(atyp3jzV@oSNpo0(^Ii#1?k`8u1mvrnLfy?Qf3<0GN!lj)u+@O`z0rwd|PEv?lVx%=K*3=n$|YQ_Gqr>&3n zmA{1BSJKb?zS_lZLLTAOh-4$siuy0&cf&mc;+B|^UA*>Mj34puevzv*f9#Q;nSHC~ z4QD{{XTX0D`H}!_W;;d70@jyn=)?G19gja+B)Xv{wq=OCO=S#;G#(7SfyXN>P+NjK zbq`)o`#^ko(Ldo0@h5|1vDGZ~%iBdoj^Y&Bp_y0`!Hxz6d$yUacz5C)do1LO!=52w zm9@ESTg>0=k=Xll?O!7P(jN+Zb8X|xtz%!%mrjOcjwoifb2YoF3<4y;9vn)r+jOQCyIQp8Y1jT}=>_ts=b0U?EIo}vkF_Y`Z@i10PLc^h`E~T-o|-sZIbrw6co34Jh36j$lcqF*M&AtVR zl&ih8(-24Zu4QlJMSIi?3ia(?ciLuEJ|m4^jv9WOA%|b^{;#3Pr)hAaNWmRQKQA9l z*F&WE@5A0Xj%!^y_-Z8(JeJoM=K)U~?i)|iwVWAHqkQR(Rk;W7JXIJpOB+jrJ-wB+ zyzRqF3@VJ@QU^6xG9W%1(ou^;V3uxrwS@L=82dHM zi6ZchbGvTyf;h_>_A;L>c>e(HEh%3MXwYW@OSxYjyJG(U?5g1PC7&u>DfGognFp`` z09v$eAUQY~szx%c(9mp#Og>(rAQS#ZE8hMkqC?_e4Opa+$}O}AFcpF1vCmGms(OQ49~%5Y(oU7* zU1A9r2ki{w(5rmI^fl zQ$=&A$8if=T6sh4vQ8Q zeuLJy+n(yCb9I-fm)WZ>pzOMsU;3`_A4`hPI?p6*U+B~ ze`O6=#|vow7>h)+d>^*zAS;zW!~yw_=}_c%Q^NN?c>dA&mE*g;YG1_tL zqo2&zN8r!dl1~-gD_sp=#y5oS^5aq<2j3iH>tA~KC-!R4d_MmGXw{*Y#FwKOYodt? z9{7z|=kczq#a|VCCGgJJT3p@34CoHcGfqJAf8Q&@{y3;}J7b4SW5T=x`x#HC!3oso zgGIN%KVs714<<0=(g37` zkx@u?=YdM|9^4UOJMDc-0b{O)INgtg=WFY-%?AyCj3XBZ>IrOG6hC__@sEnsS z{c3xtUVl1UYe}^WsI9E+-sb973mj4PVt>zC!g8@mTz!?b)~9y`osDA#wP&~SOgeS#%7by`SVt~6A0T&Na6b-e9}9lZx+S)i47&C0#m27V zbb`kGLmW9Da1qJJbDnD#;xB{z0r2xhYnP7l{{T{yl(exrq>f~NR>9#1bNE+_Rz}>t}r`NxGIH- zEaVN&c_;C#i_3<_ItuNfPnB5{CN8C_G)Tox8`8Wp#@-K2OI5hMjELG+Ec<#B>-Dc! zx-S0!#816p-`Iy4Bzp9(N-LCH%<@YQ4^O2`C0QLM9Bx8Et?fHep2G4ejFJ+p3zAM) z1MA+pJB>SY@Pi$yBc%l+a*TV`m2-aU#=aMx-J0qfgq)6ZEQBA{yw_9l6{OeYwCK!e z4-Q!I^sa{g06?|8U*1NpD?IpS-XZ(Af`QYXMH&dq8jRB}1gtaZ#bVufQe8&nrSl1A zB$g+=b;+ZMNx}jvM%Kx=AvTQk6p>dbd^zyP{3H6PyDZNuAx+r(p!Kg>g6WtHn&q_E z{ETE)^b!_OX+24D(#E39NdyuY9EJx3A78C~4e)>Vhz?-Td}VdCk|VX&n;0I8KmPz1 z`Pa@7zBn~5{mMb*eGMjIB$@ghEw$aY4)v9Bm0`#uwg~=p1oB$nTFYms!wmM)0@$YuT_or;cwk=!4&A=E7|EG@6B zw0k{9!s0cS8>pB_k|E?UIO$I3G?PsEW8ydL-DP_3dEmRtc;#YUv}>vGrMmv<&)p}{ zxc3$3ng_*Qf5GPC!~P+%8qS{?Ff}(YSl_S9PlLo(N<$3IWySNtjA2=DDJ@1$ld;g@f0WRK}zTzKR5O3-|DYIW}e z>R;PdAqtBlakf~&=W4h9%D^9UURMu-{{Z0^bEQ43)^l5BTH57G$ej8g=UKC%Ni)@c z4t~+v&w)HWrTBJl4%_QGcAo%-7J_pG`v*?BQhzGRXg{=f z#lHjqEqqNTt8TyY>#V{qmjkwVarx0^Lxs=Lo-tk_`%6v__64y17_KEC`;=etuN?5_ z{1jq6I#jpuzk@uUaBp-wwKyHghD;y8F{|Gbzi6)k{7a6nL$;ESiG9FiluLw9S6+botxmEb%9orPO(WL-&dFEVxf}?L8KZ=q_ z7AWIHU|CmpZ1m4uW3>o&WLiYyOSHM2p;^lnO^=LV=RHCGRoG489V`1Y!~P;|HYoK8 z&@^ug6=Vids-!Lf8*$t4u448dKS~h7xsb{i816;}E7WfE+o)s~cB-?bvcR#%PCiwO z2Oj)`ReeS~PmlLwJ$f3jV#(vJN1QOj=u$D4XNF^I&MjeWtm8LC-envZUNM#+A zR-f62`#+r&0UyKP*{@ByzKwLZ+vB|G$5a{;! z{{W9G1rI0U-3Q@cNqL$Vf;5g+(?ioCTyAZ!#yIJoGJ5;hjQHR7vC}L(+iweL@@p3W z?OV&7#cny|f%luQY*%BM&l)KngW-RJJ__&({j%3hi6)71Z?5Dl;A4}xoDZ#XemMQ2 zydU9jI^RkU;w?mFLRym0hv)A|PBHY&eA(kKh`uE8{JXW?T3O-kX9w<}U-$s)&~>hF zgpX>CsyYBTtemcG$dTXppW~m4{8WZ1FXPeeNFlAXr1JM?)y`P-_Udba^CO9wk$z@5 zEP9jfd(?6`_3KbbN$vEcGHGtR4n2iH^N=x&9MmL{$FE9=$&L;=q#{FbTo4aK)4fJr znDS~o@G-#j^r71-2U0y~5c2$BoO4Yw4uoWTRAI7k+2-WAKa z_WuAL)s0En7LjvD(zOo~>el*RnQqs2*93%&4itB0J(PDf?^YkPPlqGZ;7wBh08qQW z+57jlm`4$8auj1Y?OYFqyfg7!o~Km ztvmLOaJeM@op=@V(?@P5mEg6G)g)$RiB9h=bHeWGa4<4!bH#o>*L-;Pvum1wycWvd zN=FjJi@T}Xz4*p!oigRP!4=VmrBy4`da`8W(0tKk9G}9oE^LDXp7o$yvFD8X)J@HL z@Qt@Ixq&T~8yk2PPJK8wFe{~KmH8f(3)^wqir7~3dxUI@#9jioI;Cg#|(u;kcN;DP9(XSZv=~4yiYF{r0 zr7q8#Jc>;c#-ikU_pey^9s5A*Uk5IQkuqvN6qZfXY1zx$llP>9Jr8od54T$KwL%Zp zo${%{>FG+u?tYhOI^B9%&#ZjN@dbJ!`` ze-4%KKf*tYy5GUy5RFGhZ$8pu1@@UE5f#$^0PLVU6SHcy&I!iuDaY;)jKPC0oI!d_dCTv%UjtTGgo9s(sJiPsb}?V|dHq z?}dIU>0e^;2AKu;3Pe-mZ19ua9CAO-b6*&IY5kczMewO+)T|m$5qMpBH&LhhMX>(> zrl>lB{pI7-3QjlJ(lggm*NA=x-stw?`$?5NL!+|qNuk3?sp>%CzM{AZ{67?oc8+s` zr>OS&(>w?9N5mfwWZ$G+tkw*2_7@p=U+LWsxcqC{^uG~!8{>gU$vzwLODN24K=Ti= z=^*@#V&mN1Nbns)!=Rx!01s?bT3?5}D{9iq;%nLOCSUbB7LgwNi8_4KNZ%w&&q7H! z>05uaewX48gSu6<&CQL>`gF3gNvT-EWQsA43!nLBbH_iOaeoRtW8u#hX~xS+c}Aj< z8SYki@L!x34U!1WLroedb)nhX{6f++1W2^MhCUpQAac*BJI^a%b|F+L?~$69Rrt?u zsx(nucueWKe$pgOCgv$zpY}xY;18)apMRo{nS%TDu6Fv_S1L!ap-5I;iFMWZRpL8Y zOqYHq(=Nv$Z!|ke)xCs~K1bq!R}XdY%fXt8may?$Hj)#-ztWx{JXEje8?NPRh}ts4o%%@P_+-qUAn>*3muW8Gmp3Xrwv(4G z4NJxzA+(oTzWZxi#9~0U>k3AvovNS?GmoZf_rfoS-Wd3w;!FK6T=8RgJr>YOXL!~P zbpe#P1WmMpi;m;D=M>sQNcBBS#?OB&7tm?R7NCP4X@Xp=sr}X%=h*chrFFg^_`jvS zn2%Amo;$2?o>*p6o#TB;VjsbCXXfMTLm%Z((?uki?w|0mW&Z%64WIr65AdSAi_5eB z0HNj|{1?eZO_=PvpIP{u#ag$E@1FBr)aTTu+)9a9h~v*>LBfGq4&_1H(xZM!B?XAM4vDO@%5(7FhjxTuOgr= z&t69$nrpTj3{F7f6+#k88*w?sGjZp=F{C6BjCx{^nLs@A)KE(mr5lLkf@x8)$;K&! zsQ}|V_o&^*S#k#M-NgV&EZ~uWoK;C~3zp>Ibfl95&|vkb!w$sIA&0rnMKGM>`cuv^ z$-(XqJkr`)&3SbVt*jDU-A>{-p^0}hvFHG-YEHCcr|MEaMIWzv`Ir6`M~$V%&k5@Lhw%gm zN8`9vh4^Ff9sz~Wwd7OLR_K2(GQ132CtY&Fb~<3xb!XU(V>v(SU!JE2iur@~h4`Sp zCcl3RX;W^}Ead+HOq~xGQ!nrX-BkW(=qt~4xV%r{OaB0|YP0H^Wz$9^@+G|`wh}-j z2RP0<*F2$A6;%ocrE}r(*(s^YS4gQrKfggVWN0 zxiL9jdY;t-l6lVq-kc;H3=VtK815&I-6$BLu%{!ORAl7SnS#@LfOF4WW`Gm!{Xd;G z*yq2$H1gYl=LVY~>DrnCGe~-jfHC^h$pZ%*(_8{Zde8$K+L{h?PbvVx!K7sazgjK> zPZ-Bu)X~V~b?-<)Bx0UHz!*99qyt2Na5^7a>3j$A16%lM;!ACN!}niiy^9MZZT_m} zKJm!C!T$j3^{!>cSNhPPf$u>F>2JZWjlLiF-J%(+57@Pc6n(czm4}%L?#un;C!-4N z?=)i}AtiMlgbMt#@V|<6Zw71jn%;+dCY`EUHe;DPj_Sksl=lX|LcRe0(3(%iy&SZ+{SS1b&}~97S0$D%h<05r;b?y6?3^r!B*rE z=xd{r+#LG*)fwy%^ceb9hdVxL_`&-oX?`ww^?wI6S{AhAyV)?srOSH{`DVwt9D7&C z{vgn`9}DXe>3YqS+GeM2*=QykSx;i8xc3IXQf0Jgn32f!6~%l__+O!XQPKXxu37oa zqxUy)e6zrPRl5Ei4|*+q#~CzyfAHt_kJWrN3h7=ajr>KSu-INqJ;ZC%2X_)KJ05-O ztFZl?ejfOGH3jm4ATt&qM26hlPAwtN1HdO;bSBCDU&PGSQGyKsm!NMjpnY#>n0-`yP?v zjR#TirQx#HY$em~Pu&uXq=VHMaKq_dcdJH*Mr`!w*jLiN3;m+}FY#8%JU#Ia#?w!_ zlwoF+{{W%N-7>A#7Wd9Q>V0GOE|Px|X}5M7o6q4UludhUFU@w4sN7a297r1*M?=W# zS@QNRJ3BMyO=rWp$AJD5N#obkypcPsc2_qND!2sWd{M9^vDBOZ2SZ;pYTgv_AH^uSo(J*x>=FqVf)W722I|%>t3ZQupEx{k#z}o2OaAxnxl}?z75&fjng!~DzP68yk{UgJYC`FHd@Oj4h`j~`A8=jWasEB zfkstQIW(n^j+p+Gy$Vd|zu}X$@A@xa4!`grXs%c6*5C8WFZ5ef_8H1Mjj(Hx`j>Mi=f&8kw5uah64Fr;z2sj)Q+|)8l$6kGp zN1+}G8=1-=fafozjl)ni$FOK})G zPa@3^@DKL+J z00~GzZ4<xDy-O^9g?WG9r&tfcb)aTsSG4T(_mY)uMEpx2h5hUO0 ztu2W;o@4h{=sKTj;2ZX?@P2SKe-bh8i)(-WKVBE{JK{C(#P1UwM&Vx5O}ET?eS;8u zldl{HwUk zNe0qSb4~Mk<2gNPxh}-|aY(E0#xp?BySnEX9qFw2ZrJqBXgrcTeJPGMo}7`K4k!U# zfjn?24lsH8R7LiwyMk1CdN(Q=Wr9l*Nz&dVNhJISMiS=mGt4lk3pZ zGhhHtY9dJl^Y~I8ah!M0J?KNRHb@@bsfD)u$G=KWi}-LUvSS4Cj(XD&q=r8K0A7@V zdEj)UR@yn|(=@{SiS-m*2||3`Mkw92k34$Q6}ZlL_oVZ3xc(|a9$y1F82VIuiKQni z(~nASc7xC5K!YF>I#t~pS-$Z8t!b)hH?ry4rK9)Q)Ap3H_`hhnF0til-Y|hO@DG+d)g5Gu-?>hsuOpiF*No?ZUy=5=)7V|xUD@18 zYk6+#EEg~_St4+FWp1F?-Cwhh?OkW_5p8@i;>lyM@n9Pvy2jWwEO%bZFRFAsSFX`% z_c4?@qv!{SV+W;Ep3oM^AEj!#^Xc`doj~`k4pxq9QPH8kx0(r}j#wp9j})pI9Zv)h zde_PS02)7JHik*{j{-)hd1-V>8*1crFtOl&x_I=(eVHr)jX^DnNH9fFDYVbYySphZ zZtg6vE#*4{q^=ghAZRmh<~#$kG>sAE4Gs_h;*g%sTjq~Lpaa()MM30CZVq9 zM0}5srG;fdfC{%lInUCw<+K~Ijy>z|AB%snr-%G?VthyN()QC!y17q1Y*EAM*MYeP zGBf29+iq9bWSaR8T=;vhcx%KnXj(-djU~$N;4sYmSm*13 zTM{_RRIYRQAMmD0Iw}&_RsQif;;Xgwv21yZp4+}!jK>AsrR0TWQPHFapK+R?MUi@r zgJhAuc^g<0imubC3`Zb!%{78K$VA#YVL%lVB7iczdtgvWD==fh&QAauV>aG0KAGaH zi%5XTjRtxhqLEcl{G+eE0Lh+Co`>AjMnY7dx(E5`L1L(Q!0rA`CiION*}{R&O+@9& z2?phiZ(;)a_ODRTKW85t_=`xl)I>UNnH=l6Ak^+&IkujJs-t5gu^{_bpnL@Q4A$~m z_^-tlr_3kK({!1m`@O`{gWtNZZl=D9mPtcqK!yA61B&@9esSz2$xU6Ibm(&{$oUda z*>lGrF4^=6FgeBZ;Qn!nr*Zo<{6LHlwpv}obAb+|ng0OCg4ftm$k-Wwk3&`Dlq-NZs7;KKLa7DYCcrQq6@dO<&YdIS zjyNC;uo%sKoiyyOa6QL6E^<1ab5JVdEPM3gm7fQYa8FZ6Np-@KbJDv}3~3~;K*0W0 z8Do|MC$aXVkjQuhCOF;H{#3w>=Pba2*~Z>aA6j5a%ot#2AJ&k$!O6kz#YEtOaNK{p zoDa^U-2Fk%)PMEpD43ialhZh+79{7IV|j!(%%J1A{{ZXMafud8F;kv*DZ%_G0!AG% z`Otn<<2XG{BOT0n#yk4cM?XpcQ|vnW(-amY_4?C9n0ynEdJ0C$oT~sk_n>wq!vJ}} z$26Jip46+pPn40*wKha#UQfzTr2_?Xli$4|AcK>U`czXRft6v$d{Ykoer_?x6cS2s z*y<@V4^~UbItL+CuG6=!+HTdr=QQTX!a<1s#?*eP?zu5cs zk8c`jso;MVUIy_UjC*V^WR#UygBu%PR3G=)z7B3@#{v+_NSI!e=0!2 zwNA$~h{FzZ#YXU)5~m*GtYlHa;;V9PW8<&d$M$Z#zrMNg$Azs;#nY*6Q%r$=X|ewR zEu$UVA9;J=5nm#S5UxXX?0bDj^RL(0AXgRhPsjfN*#^tT?{(r22qZdRh}$nUu^V8# zKK}sRbN%nn)`qTa33BLsyYOq{{{V`9315SwOmDO&lXcFAa>T*4Kx;qUE5;HYjDZM8ufhL+jeZQh+T4pfozBXNw58z00g^E+44H4hGIml|fRWj>p! z+cwFqZWC*v?ez!L92%R$9wgR08Lr#wdIqO+qw27nt<|$&vg8tu4-Tgss{$(qt+Cjh z&!azRf7yG)-VJ+=b3<8tOX2hd0ACi_<7<#h1%okJ}1)O zPP_jAmU54p3CCuRPL1^HJAqvP0MumtD>j*04cI*>rTl2DtSg?3peW(O=e9lQ^EVQt z=ZtgOqE^89k0U>&A(*3J5~Pe~x=3JhszE(_1CA;-+UTWJr#a4ll{MStax?ExGZMob?is07vOy|(_vui(V+FYB-`;>77{X`Qp!KLWmSRBq z_BAMdfQ+BO^{9#7z~G-!f1OO7fI>uLbd9wXXDoOX^k?kv@EN9-*T!Bbvtb(IO)pEh zA9(hjMf4q2Pq938@J$a?k3-jNbt$y#U0p3-%ocZrgfcK37ROLJGU8mInU-*{k5HU@=m61k}zYFkUhH75ao&EZ)$J)z&OA;AB{lLe9Qq1Kb3uCV`GJqw8{Y^ zrh4X@%*%i<-;GBboUsJpoOBeDDuyFH`NsyVLWc}TQag}3(JL_LcjZRnkqxC(dS{ws zkGS9wx7VcuK*SaKx(`~3NGBQZO?BEbdt;|Lq$4TES_VnH1I9@11x8)RrKw%|XsF_)|&whIl;& zDKIA7Up+JJO5-OP!5maRVOHspL2^cW9@(G=xY&8=O~}s&BfTt=1@+J8OfQw*cn6G7 zz?VDvbsn^Yka9)=sQlBM(mv*3dW=;FnXpxP>q`U4YjF+5yz*Myz~V?}gzb2oau^=O zR3U=*$j?e$#fPQ_e+*I?_wVez`%TC3PS!sR_?l?#ykl&iyO$eNQjC8Z`baVJFMgxj z=t2lM>5BY;x4W9k<_oJy=C!-GaV!@QHrI{ED#}MfEAP+QhxVX@;;pP+8u2WV+IY&{ zF&vyMdXgyn=sF1p@h8-dGlrdxdBXam=sm=Br4DF3XO4z}&T7*!XmFH{^zG#I=}lGQ zlnftgyNL6@jK2c>JMpJQHu|JrWYuCqEj3sa3E)1N{w2>*EdL}Yc|j4)J;=DoY%XYF6&Z-hT*({%@%#J&?#wXG(TbMUx)tyw6}nZMlAeGqEEz@^QZ4^%sgX<8KgrJ=YUh@Ybt7k*Kc)qB%)d`?4Y7 z$JeK9SKa>r0RI4NEfeC4L85qwL*dUAkP%{c{{Tn5JA9u*9|3u* z=>Gus?Z3i`{dfMw)4%8+Xa4|xSNtff?1gjk`K7}QV3E*bnl~I1k&ecsk&uU@V7a8a}8kZ{tg*Yv^WrzDBqh{oBjOV9QUXk!S;N<@R z7WMNb%Nu_USV}(MGO-OQ>LOF>dy&w4kzZ3>`~diCGyMJw()I(iFo=)jYr@55v@p`; zn>LLZPgHy*C(E&q2PfsvJ?c4_7Hk~jwmVncGk(q=4BfIkC2tw&n5UJ0lT{m!*?Zx% z>Oj#hVL9s-qcHyf*^PPcKEgdNYxQ}f;;rPI@wE2EKPtJ~jo8TPFL4msYs{07uhoMyVk(%Z{WR{HxTQcJ(7aGKOST5}nduZpVHr*uvteVq4A2%ObGNQO zzSR_%RUOSWPESvpo=<8C10-^LpGsk10F@<(IKTrRr6`jdM*w4*k2r9-`EkuLdFkua z(itR^6Y`!Z1D-N{DmLI?l5@$X?g}t-lg0q1GEXoPLE@3vFB#)C9(l)n_oXZt0fGGJ zumiq8H1I|T1XL%3)2%xgUQg1H4wRFELB%zkD9+>Bnp-&;$TVXM$tO9-6u?}EJxTP( zdJe|+Bd!HTOAHL;jP#@+f^s>>Jn=vXA1~9V2hh{Q1_Tbh%|y?a;B6g8Iqgmi26N7F z#XA7zC7*MUGCR~V9JnBidWv^Loa5_FQ=H)74@y8hNEH`2C%$P5XN>#QdBFgj`q3+3 z40S!|0a?c*0At>q4sv?u*S#@83(ihB!KK~^;{zSdP=SU92U>8*EA3UGR6lpco*^rxtWUU}mkc<)wi?j^OF8=HBg zx4D7Du}=vq@i;kFQaJ>A)o(B}ev}s~pp2ht0iU2hvfu4J55-$_@VAMij@QO^$V5_{ zDr#}BS3%l9KJxoyagaWU*^}>IkCwMH+ucERYdn^BmhL5j<|Zp7GAYWkkQRBqpy(t6?;!e)G6PScoS~)B^dM7821O|*ml>@Q zjf6P|=}ump((TPA4N?mZ1B2WiFl*)S+6(qIvhi`c@ehGg9V=fuq@PSbCAHz|a6jdn zli#5|eQW9!Ip_sQ2mb)7iB4St+_cm16FKk>6kl1D_-d`k)uX{*G)Fe9Q!2j4Q2D+(A$SyzwteJiE-bMV_$_?zOp zZ4*rlk+BbVcZ{XHS>=xy&t)F9lc8vOhl%y8Ef-3*^R+AK1aQU&QgfDUbWk!$K9%>^ z!jFXZ9|^o87MX1l+}=%zFE1O1no>IF`=g*8tHj6S79JPsZ%!P{(D2uUw9f%(HvS*d z?M1GeZNfUQGCnzHJ=mT}tIjY01eNE5SLIOwD>9d6Tn_lnQ}XsU*5D4tzbeL8m1((2 zp0zlnisa<>s#><0mYRK(Pd&uabVN~-f5X&HMVl9vy(fs%iRprtMfZejo@1b00D04j!O&m-kf zcs)X9LF@e`P_NJU*f=T9v#CZf@V?Rm;A$++V#Y{)a8$P0<7z5j%@+v=t%d{SVR$zU*fr*IUV*f`IvFfd$}1mm1_phTd82Xjm;rL)ct z9VwX(NhF{rC)oSasup36pl6d(I|$0eK7x=*(0bFj5;t&o$K{SG%fSGU0O{J8$O4o4QY$fFqbJ&cB?kcD zDB~aJ+L{=X=^+G%Ick#L=6gHXF0Ch)&hFjB(A>o3B#FVwvaskus5cGVj2~{IpZp4Q zj`Vo~KSKWiW#8IjAB%P?;qMVk6}OEnQ04)%Id;7{hql1C;9xL(U%Tc-0 zH4AI|E17Kd3rPgl_fR(1iN`_+{{SYxLqBFO+HTk54xG9sw((f_($YN168y2>ApZcC zvHSl3d3zp}N%taAzNfx{#V*|YQuGu8O=)s00V(P!am6Pz%1B&b*B|jm;Xj8yFnDH7 z7sOUVc=BEgl;Ue?J3MXJk4}fZb^@J`N(Ovw{{Rl|Px>FP{{Zj_f5wXYCkNZ|qMn1) z{HZYlNXZ{l#YX#4wr;}XkUNTzryQs)m+79rjWR_f207c)B#Py^sNXSHYL(oloE+ks z$n7$uaLFvPFpemrLRv;A04VNBsF`*$u0rw0Ij>XwoxDllUj=xv_3s$mU9IJ>$*07& zjWkgbIgyCT-Od05j=8KVOO`idin~6N{hNLSFT+gUk8n;Dw1+&?}Th@U+og803F8yP5CGAs&gqv$0yyo)xAApx<;Hx9RS*n zj1L{iBB@{ATj}`nKNXL{ZvuU; z$&tK8sk>TfaIntC#$=d}?xOSPSfD*?;KV|afbR0{%RG{{=8=gcvmT_AlU--UpNW?r z7W`><;u%B3U_a6H%SjFz3w1v(M@`T7+pi}b0Ioo}RV9cA*l-1X{!xJT6#1mJXP;iD zGY8m8vvH1|!!*g55%*3qG3o|Cogl#nDaqty{{T!>F61)|;F0%+Km*g7`sl{U;bN~$ z1t1Szl*0a7oyUSY3RFvOcE+5-6wG1Qy92mCeA?vx9k0Ac(w)$frG{mU!^#V#0fii!0b=fk?_u+ zAS?mF2am@zgfWvUat42&y*XJ(`LcI+^rp(?q`@Q*Il$@7HG$d#WR~QTdsQ$*TS&m< zHqdzKQfFv6Bp!o4)ZDVEQUX77nuN1R%)>szG?>t=!BqbMcj?xUe)EtB9f7GhA&$^^ zAZL@Bi_8I;PXn$!KC}SoR5lM7{w#N<@W6w~^`_?p@q&2Z&^aKE{hFB3D_}xSat1Pe zYBf@MT%7hb9K1NkU(@lRNrmA3PIH=&9e@eQ&$n6$$l9YL*NSU$qa8^nBaUcNLS*#L zK_mS7)r@ISkKY2G%jRvv;DhV$OTksgB>moc(r-{Ubt8`5X#m#&3YOrWl-6yG7Z?Yg zDe?gL9Wlvc(xZ^?>&G~wK?reAZ{d%4I@Yy&N)8(8bB|bvxe*Zsx;&=;2ywrq%z}z zeqrg_kOtl}k_wDxflYxSxeFdhJ#cvKL_miJsq4o|jg^BCasmDxr|N0Q>$m_$dElOu zhS4N|mL<8!;~axRpd*sSdy~`gqzjLj;~s;xJ~ve*M>y=X2tyx~RgMA2=Sk)Qa!3U6 zoKj#F8;(z0`_KklWU}D)#R9Nks_s+%H7YsVf->KwK_MbQHlM`P(Z@YTcmjcsSuw%k zxhAh^daeG4soLsVZRD03jiiEGYnVdl6LXXmK*_-y2 zvH01ejXP8F^#1@ESjG+2pdcm6kNIsEvF^P`>t5=kKgz!xbZu`?@aC^$s_7Sxrs}pb z2`#SS;n0uf4?+(FabKlhvv2Jiru=m9_meuur+Bi)G4}mCJTU3!MV_&-?bS)a>`f=p zlGkI|z{Msg&P6CDwdhTQZ#22ZF!j$gxug~!x{7WnqU6~8p$Y_y%yXY?f&A#2@RA1J zNdN(oI@4V_1muu=o=3M7=4DOHc8p+>208jyBYh35Lh?4_wSgyq%D6o-^)&e3WO(Sq z3xGcHsIXmCmlaRnIv4+2WIEYeCVBNO>f;U-0IuT%=R)EeMbfqi6pB zTBVGsW&nZ!AoWq!qcar88C-GwYbOZHbXPM0rv+I0MR+Zi#!~ibY~|gRcp0N%>&o+1 z?z~H{UFmaa)A)y3n@*qoWrX^i&jaI-8a92TWDYTkspcU#b!>FOIX!=^M=4B@IRNkp z1lDg2P45&O^6oX<10t(7(U6$w_;FAo6vJe20N`^|XKr#xC!FK?R46dykO4ihf;lzU z2t{mW+hi>(sb*8OFgvm-LBJ#UPtA?J>Q^dI@-hcN4;38loCi4yGm+mN1!`xgtsw~P z-~rG8ed&fqXWDQud;b9Ssj|(vurYu+!2Ku$tBuD87{)LS7APc8ga%OUpS$b$(-I+q zTY^t>(v!_0vN=6Ea6Xj@%L2K>1Du)wvH{~Hk%5eVGf_#n;3vwy-(PBwH_9=BdUVA^ zPaQWC&j%FjA)A>A9GrU^dBIXJcLCqneJRrqEjR!Z!No+&A}$FSCwDy19R+R`cmo~B zJ*Y(^IpBJXb5WU5hpXoU9Moao_GYF!>6@6R%JntmAyTxHF2B}ag5|2 z=lWEEFip75NZ{wDDUPLsHb*@@I#P?1DuAa9IOuRiH4fv>dUKvRpqq_X@<1+qK4H>@ zX6Q~2L5hEuypTTf@+vtO05(S7ZfFEKLEp=8zrs73LdqBpa(M)k#ULS{f^&`sy%LSM zBL_Vxj7LAbJo3Ef6!(~CX~(heO$p9h*F7n+qOV?99tJy70p4?9j=34ny)GF+0FW5q z`&40v8C()_LG%>x_yxJi7{vfRiTQ!+oN>)Ck^$o}~7rk~6ae91)BPNJtn1+8)tXSR6710`X#bH>xswY(|ftzW@9 z{g%7o`@^Sd_D&>*aDG$jjz5V=;Ew#(WM||VJBb|+)|yC(gJ&K1q{=^3{sa7Q(f&Dj zK0QvzrLL=P1bUX3TQ6%SKU6r%w@<_nJu1Hi7wXjt{rP@_ z`q4#N*n&3u!|DDM%dhzPZ>XY*eE?#At3O(FfAY$TDCh$R`qTdKsL%X;AEgvffRf|y zulvTMUbInC1EcES@6z3LG*L|fQt$m@{b^zU09<~eiYc+8c|X+(sVChZS}3H-8xQ)S z{{RB0;{O0x-$6wbgn@tK;Qs)-Q%%-Cq5f1+K#>{$0GD5>rZ4<`ztH~xN+_yD7B0T( z{{XxFYLNTG{P&`YkS0&Mzl}J57yI;4KoS1{%R~55NzhS61fn=TD5>ZI{{RvC zQzZWYSf5%bqo5C$?l08S*QFFy0QCF6h^0Sp{b-_w1AC9HePRCq1o8g>l9}Tl`vt-O a0HL)MRKA6+&%Hl}l8P%*0*WZBAphA8pxku; literal 0 HcmV?d00001 diff --git a/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch b/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch index 3486f2dc..3b490d5d 100644 --- a/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch +++ b/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch @@ -1,6 +1,17 @@ - + - - - + + + + + + + + + + + + + + \ No newline at end of file diff --git a/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch b/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch index 3486f2dc..3ecbde8d 100644 --- a/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch +++ b/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch @@ -1,6 +1,13 @@ - - - - - + + + + + + + + + + + + \ No newline at end of file diff --git a/packages/dt-core/packages/vehicle_detection/src/vehicle_detection_node.py b/packages/dt-core/packages/vehicle_detection/src/vehicle_detection_node.py new file mode 100644 index 00000000..335d2da5 --- /dev/null +++ b/packages/dt-core/packages/vehicle_detection/src/vehicle_detection_node.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python3 + +import cv2 + +import rospy +from cv_bridge import CvBridge +from duckietown.dtros import DTParam, DTROS, NodeType, ParamType +from duckietown_msgs.msg import BoolStamped, VehicleCorners +from geometry_msgs.msg import Point32 +from sensor_msgs.msg import CompressedImage +from std_msgs.msg import String + +class VehicleDetectionNode(DTROS): + """ + This node detects if there is another Duckiebot in the image. This is done by recognizing the pattern of circles on + the back of every robot. + Args: + node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use + Configuration: + ~process_frequency (:obj:`float`): Frequency at which to process the incoming images + ~circlepattern_dims (:obj:`list` of `int`): Number of dots in the pattern, two elements: [number of columns, number of rows] + ~blobdetector_min_area (:obj:`int`): Parameter for the blob detector, passed to `SimpleBlobDetector `_ + ~blobdetector_min_dist_between_blobs (:obj:`str`): Parameter for the blob detector, passed to `SimpleBlobDetector `_ + Subscriber: + ~image (:obj:`sensor_msgs.msg.CompressedImage`): Input image + Publishers: + ~centers (:obj:`duckietown_msgs.msg.VehicleCorners`): Detected pattern (if any) + ~debug/detection_image/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug image that shows the detected pattern + ~detection (:obj:`boolStamped`): Vehicle Detection Flag + """ + + def __init__(self, node_name): + + # Initialize the DTROS parent class + super(VehicleDetectionNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION) + # Initialize the parameters + self.process_frequency = DTParam("~process_frequency", param_type=ParamType.FLOAT) + self.circlepattern_dims = DTParam("~circlepattern_dims", param_type=ParamType.LIST) + self.blobdetector_min_area = DTParam("~blobdetector_min_area", param_type=ParamType.FLOAT) + self.blobdetector_min_dist_between_blobs = DTParam( + "~blobdetector_min_dist_between_blobs", param_type=ParamType.FLOAT + ) + + # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + # + self.cbParametersChanged() # TODO: THIS SHOULD BE FIXED IN THE NEW DTROS!!!!!!!!!! + # + # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + self.bridge = CvBridge() + + self.last_stamp = rospy.Time.now() + + # Subscriber + self.sub_image = rospy.Subscriber("~image", CompressedImage, self.cb_image, queue_size=1) + + # Publishers + self.pub_centers = rospy.Publisher("~centers", VehicleCorners, queue_size=1) + self.pub_not_detection = rospy.Publisher("small_msg_about_image_detection", String, queue_size=1) + self.pub_circlepattern_image = rospy.Publisher( + "~debug/detection_image/compressed", CompressedImage, queue_size=1 + ) + self.pub_detection_flag = rospy.Publisher("~detection", BoolStamped, queue_size=1) + self.diff_vert = 40 + self.diff_hor = 100 + self.cutting_image_flag = False + self.x0, self.x1, self.y0, self.y1 = 0, 0, 0, 0 + self.log("Initialization completed.") + + def cbParametersChanged(self): + + # TODO: THIS DOESN'T WORK WITH THE NEW DTROS!!! + self.publish_duration = rospy.Duration.from_sec(1.0 / self.process_frequency.value) + params = cv2.SimpleBlobDetector_Params() + params.minArea = self.blobdetector_min_area.value + params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs.value + self.simple_blob_detector = cv2.SimpleBlobDetector_create(params) + + + def detect_bot(self, header, image_cv): + # if the pattern is detected, cv2.findCirclesGrid returns a non-zero result, otherwise it returns 0 + # vehicle_detected_msg_out.data = detection > 0 + # self.pub_detection.publish(vehicle_detected_msg_out) + self.vehicle_centers_msg_out = VehicleCorners() + self.detection_flag_msg_out = BoolStamped() + (detection, centers) = cv2.findCirclesGrid( + image_cv, + patternSize=tuple(self.circlepattern_dims.value), + flags=cv2.CALIB_CB_SYMMETRIC_GRID, + blobDetector=self.simple_blob_detector, + ) + self.vehicle_centers_msg_out.header = header + self.vehicle_centers_msg_out.detection.data = detection > 0 + self.detection_flag_msg_out.header = header + self.detection_flag_msg_out.data = detection > 0 + # if the detection is successful add the information about it, + # otherwise publish a message saying that it was unsuccessful + return detection, centers + + def set_detection_params(self, centers): + points_list = [] + for point in centers: + center = Point32() + center.x = point[0, 0] + center.y = point[0, 1] + center.z = 0 + points_list.append(center) + self.vehicle_centers_msg_out.corners = points_list + self.vehicle_centers_msg_out.H = self.circlepattern_dims.value[1] + self.vehicle_centers_msg_out.W = self.circlepattern_dims.value[0] + + def save_rect_detection(self, centers): + self.y0, self.x0 = int(centers[0][0]), int(centers[0][1]) + self.y1, self.x1 = int(centers[-1][0]), int(centers[-1][1]) + + def detection(self, header, image_cv): + rospy.loginfo('starting detection, it is cutting img {}'.format(self.cutting_image_flag)) + if self.cutting_image_flag: + if not (self.x1 and self.y1): + self.cutting_image_flag = False + return False, False + else: + image_cv_cutted = image_cv[self.x0-self.diff_vert:self.x1+self.diff_vert, self.y0-self.diff_hor:self.y1+self.diff_hor] + detection, centers = self.detect_bot(header, image_cv_cutted) + rospy.loginfo("cutting img now") + else: + detection, centers = self.detect_bot(header, image_cv) + rospy.loginfo("full img now") + if detection > 0: + rospy.loginfo("detection!") + self.set_detection_params(centers) + self.save_rect_detection(centers[0]) + else: + rospy.loginfo("There is no detection, change the way of detection") + self.cutting_image_flag = not self.cutting_image_flag + return centers, detection + + def cb_image(self, image_msg): + """ + Callback for processing a image which potentially contains a back pattern. Processes the image only if + sufficient time has passed since processing the previous image (relative to the chosen processing frequency). + The pattern detection is performed using OpenCV's `findCirclesGrid `_ function. + Args: + image_msg (:obj:`sensor_msgs.msg.CompressedImage`): Input image + """ + now = rospy.Time.now() + if now - self.last_stamp < self.publish_duration: + return + else: + self.last_stamp = now + image_cv = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8") + centers, detection = self.detection(image_msg.header, image_cv) + if not detection: + centers, detection = self.detection(image_msg.header, image_cv) + self.pub_centers.publish(self.vehicle_centers_msg_out) + self.pub_detection_flag.publish(self.detection_flag_msg_out) + if self.pub_circlepattern_image.get_num_connections() > 0: + cv2.drawChessboardCorners(image_cv, tuple(self.circlepattern_dims.value), centers, detection) + image_msg_out = self.bridge.cv2_to_compressed_imgmsg(image_cv) + self.pub_circlepattern_image.publish(image_msg_out) + + + + +if __name__ == "__main__": + vehicle_detection_node = VehicleDetectionNode("vehicle_detection") + rospy.spin() \ No newline at end of file diff --git a/test-arr b/test-arr new file mode 100644 index 00000000..e69de29b From 99f751fb5ac62842f6e27faf061c3b48362df3f2 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Tue, 6 Apr 2021 17:43:29 +0300 Subject: [PATCH 18/18] impove bot detection --- 1.jpg | Bin 37126 -> 0 bytes test-arr | 0 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 1.jpg delete mode 100644 test-arr diff --git a/1.jpg b/1.jpg deleted file mode 100644 index 01327a7a92dc9cec5361cc9946d3401cb8a0610e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 37126 zcmeFYbyOVR);8GC5C{?=NN|_n?sPX2JUGE6Sa6r%?m>bFCpf{~-66P3a0yPMjWr*? z``+(;XJ)NgYi9nPd%COEsoMMOI?pcK=h>&87oRr(*m6>`QUC-50080T2YB8DSV_8D zegObvWtjk|00008c#8o1NA$0S69Ex`^HS$}SuDel{5|06+vF=DhxwPWOue;J@oC{}_8I&zrBWEL>b1gxJ~boY{;_?TyXYOzdsh-HjaB zIoUYa0U{802O|?}GZ#u@GjmHj5cOGWCpD#|DTrE=SAj#pLBhlv2zx32T}jaxX?@epWW=#l>bt3u?A6VDJWA)*gKg~ z^04u+aj?EXcjKadvEgL;MM(97)W0pfY=Nl%-PNyOzp{PhX0vxPXXg|Y6lCY%V&~#w zebHcb_ONp?a%Z)3rup9%KA1V1I9WQlSlZiB{$tU|*xuCzME!F7e|604#nJz5_y6RA zeUz#5>mEwH?z_DU}Twv|L1}KiQvraUc$DoFY(#)3P1t?L`3}O|5A`% z{>Z4v$Vf=YXecPJP%+RjFwoJ^(J|k=#m0Pt^9CIqn*bXJ7Y`pF9|P+hApsuYTReQc ze~cgiU)mrczeYxWjfaVjiTD4yJa++bP!UuRx`7DP07M)FAP&NFAAsT|&_zc0SNX3n z^<^9JrH`*rP+y~=zbMpW0}v5_Ktv?qKfQjD`n<>iNI1xEsW`=6;i?#+P&?vr1;qVC zr4g_0##fy>r{y+w3Ve-5@Q#p(n2w&|JtGqjFCV{vppe7|NhxU=SvfWJPa2w9+Bznt zW?#%LEUlbfT;0C9dw2!~hlGZOM?}UaBqk-N{76m9&C4$+EGjPfRa0A6-_Y39+|twA z*FP{gG(0jrGdnlGu(-6mxwXBsySM-M;1GIod3Akr3%k4j2NwbW_+PMI*8hd<|AGtW z1s5U`5)cXHA6y8CU;hD)gM>`Q`Rc8h3W|{Sf{6N`LthI zzY5rV(ls!9s9||UsMK+DGHy*D6_am5X`U@MUvbcoEBnPd{1PbL_%=2%&;yB_IV{F7 zoVb*cVsVYgKjT=qq7b;AGqJ66kL3@Z|B|9VZ~rLT-SA6sO`Uml6PM^{omZu4f*4kG zzJ`SY>*(z#$CzWCoBwP+{U@YbqGwNFn6_eC%-~OZ>xuk!`_1<>pXNyt<+kS4Qr3*% zZoa8_6M;jHC8=)xB5?hZp`JVQri-mJi>GfaP3)aH;br~}z%1H(1=`r3+`$I5xw&Ur zUG71$2UYyOyOnQxXG+wIY@5FiZi}2gYPyy--&Tsw;+V>!0c93u+>#F3*v;ij?(e-% z{VZ^(;E?hRAbd0{rXg~jua6}~)kN}nJ?5B39$Td3%!9isn{UVxU~2ij0Y&1w$5#7x zFXUAls7L~N_V@CPP^he#%|P#xH2#~OJ@dHkcah%C95QUKK86gbbWJYd6up22+s)3U zJwaOeOy6At#X@vnnfvCw_87vSb>Y$&mUwa;IIjSYzsmQTtxfZX-=iFU!oNY^@DxmZ<-D=8|9<<`3L~aG@mEVT5*AoDQsju!7B)+Gv zTa)wG%+&Cw=EJux^$d!(e3T4d@qDRk1c|%amn+xL5KFIZRDc) z{9skwRk+BiSWUQ_kCaRqKGL9W*p5sSb_h$eAVSl%7Z}v3C#xCYj!b%mA|}C0g+lq=So{@bd z8i8ldb8WOzFGUimy!H&xF`);~_DJ&VrL+Uc?$we!$e&7DmGFLX&j;j+MXA2pi%%&B zl7b`!$Qe`dOebaOHzVzwWyisZ$OodN6aB0NN;r=kP`4E9-wQgQdZa*xqjY}C%$N2( z&UtIQ>Lo}IuGkilk)$*dv97zD5y2U}^{l04JmMAh%w}PKl%Qu?01Oi9!3Mpj> zUHN924jV!93V%%2rVj)`oy}W`W#7U^gLfdBmev-@7jNh!Kf1$6ZXl}dgfHhY-n!^~ zKIXg7C_C#Zb4FdNF=f($?)M{kVp38HosW_egx24Xn-XdA4uS7!h^pbuI7PwGsLvU_Xsu zbCMEYz45N-bD^Qh%bwkifz&_nAZG|O7kh!G~h*bdc+EGRB7y0y^P3cw=FBM#;)-<7DjAxqh=B{|7Idq3ViF&kd2~^$o|Gdz#@sBnQlT{J3DZhzQTHHu*wzwJw=3ICU={#DJ6Zh8;qAKlBw;w6 z4B|T0GU)5xZhG5hlO1_|k1cjpLFP?T*6n-Yo_4d}r(@DzSDzS78rR@iQ-O|QUQ{V} zT>^`kc%ZNSBJkHSGD5DeHsSmk5ctRzxgdmDpSjw5mY%&f9=6&c^Jtwj%P_VP{KyDsPI7K_jB1=_i2i>Fa z^*!56JG^2K@1THM|0u5rb~b;~?^<>~n2x@kF&k~;f5^$W}}jt5IvJQ&KYVF3w?71yv;$*!iqDX!xkiQI0Y-Zr4j=iuDM~bDHj{6Rr@Jz`mBmi7W z`iB<@lB&iCrM>PRR1qbj*p>`t108fbaf+glS&k@z4*Ubeql^kHX z0QBM3z7Gxdj}M|*)CA_ha^+PB6X<-7TI%PwTf_OJArfhtUm7?o%83C!$VOJ07drbW zkeNs8>Na#vh6?Mn`8p_9HE{mhsL*3~TLg zS!wt@04_hHHp*Y+xAI1GZn{kd4`fguMc4J3@N)-MBOM~1@0GI z-9EHXp&&QxL`{3e-v}5kNo>Wz6rh!IS%>z_lJkxQhm;viVUU*Z&wv&f(G`8C>dlG3 zQRJf;v?8M+E#p_7jk8(k%;YoR{;qPS@}>3QBkRi@crW$SnLWMSGVg(373pz6s|#V` zVYI#zSMO3I^C5A@;)WEe2u!mD#=Pe=xvpd$!CRPI|(%G*SU= z%p1(?J-Yv7-T964(5Z+vr6c`_7FHS9UjQnRnFzsj`k|MV_ zC&HY2#EH}~!}L5w4bi2E=PiCz+q^C4wm)f|8H8w0G+6_K2RU^RYF`ttkay*#rOHy;cc-%KU=n2e;6WlUSzr$)g8jT^Z7E`S(2 zJf5abJv{6{4H;N;AcqQojwxd*OXKBl1uT?fI0)nz6O216x0+p}d*v((Am}Q@ZC#wXc?K}qO;~*^n<;fsbrk1qXUrxNArQAySdLWABM#hLR ze4D?rN{oURDjVymx8Y+Y0v%&IjsGAswU<;I94*iJh|KwxV4Pcqgg(L~4ft@R8x9!c z>KlQG80WGFdQNs_fWdVIFnAKG;P75z#_its!Q{OpSzbb<>k_;v2U<0yzaU9YC9;EO zfI*_(*LNUT0SeFR{v)H0`Dc{vZdsx08v@)bExB!`?!b#9hscnFmzHZ;SJxH8LOe3*JT#rYw%!J%l-QsRQ0MVe$4gQowv4BeUNDd+G z*2sqWLS?xpUr5QIYoQITIYqQPyxP0F@pDjG{bnUmeZrkngV=3;uUh^B--;pU2BOz( zIo8R>vfG~}VI{de5$R%|2KrPKMJcAr(5RLSTOvW5?F5BWC5a3ui&D8Pdva?kS8t5< ze5VoCH-^n;zySXF(@v|4JzjC$=Rvd-`2_SQrQcb1_(MweMKF224|8;!XLmQ@OA%!E zKm+G(=`eYy-a;B)BaUv}=vQ-*I36iM0p~^sBD6nP(}j+ElG4@`by3}1=mryAWWf6{;^QjQ!(m8JHXvbAKfsxAv#k5=|#P2_RQJH0^w8Uhn3a(oaFAt9R#V z*xhab7cn1C(5MaY!)ZzzVcJvuc;MX+KhJG8guR?Myw1PhMxOy8WS!3d97+ACQ%{;B zrf&)UKC;CGduM944O7(y#{xiTF&(rs`0B4o7D;%Krcc0z{;or}tg$CR+c~R`*ez*0 zY2I5Z?SmV)P7}uXm(yI{&+0K;M1gTS7rCbf4d)L*srxaU>!WuO`?FU+PhGyD|Fy+P zzR(}-b6i#AZ+-@tK~g@>wd<&uThdOaT;!?I1QQwmLMIoFNA_sT!moxtSn8Zaq+#fJ z)DmQPG-(iG6j6Da`zs31yWpEsQBo;zR8UFi7fv`4CEZX1jKA_!-KUQWtCUk@40j;JBOokH-cYyMiSfusZ2f#c zYr`$SmZqDABm)4bmYk`ao?2W&hH^;gSkFGLAr!vbN^j-CNNjutROHcKydQ>y2mrXe zCaFZbascoD#MBXt0SN-G@)-azv%qD4imZ<7X>Xxm`EoA>gNTM z^A6U1?L>@6=wOCy@)HZ6aR zL~*}MRlyz;m=P}iYe(&7!)uyN`0LeP)~gL6b*?@jFirg^QkipsI-~kh23f6Nz-6DP zV5OgsrjrLl$oYO!m&a}JA`cG`ngS{*{7&j9*_9wje#M+*6NCEKhcOsBaUk=ykTIFLjO;gn1NVgI1I^7o z%<>uVdcB+I+Dq&9FpDVVK^)ZSkpp2FoyktYyw-uKZ5sUWmM|&qC;e%F!KvyO9E0i7 zuKMNs;#nK3rFC)h$Jhc}D+4^4bVh&9c*lN^FPU4f&Kt@_)K>edf9|FZLc;t;8rVAo z7R$Wwv)rzuv98Nbr`&{eCom*gY*5Gj(01(~(H(&v4cneE4%XSNum#F57c8o?LWW9$EXPHvthBoKV+VS(HO@FQGH^bI(yOG{2=M2L3H2lp%wdj_W=c;2?T ze+r_Hh0=6Zc?57Ih=<0-juJA^pl1Z`zbLSmbYHXCF|>qqsL)!xvpgCCPI@k2cG_`9 zy~$C%TLD226n|@gM_vK~A8+vo{zz9PBn6akUg=qcj+wHnG0U>@sU)Q=ZSPs6@7ViD z=hi3fCY#qc1)+Xnn*57oyh=pl@1OvXuc$W|>f}x_0H=VG=t_E#r+bl3uYBxRwWy82 z`GF|Pq$IL%R4^FU#_Ln0V?crO$QIAIt!-i}t;L1GfrQht&|ytRLnZ4<{qii)=YM*u zgz2|w$o~wWQTgPzNq%1Rgyx3~IR``F(z$heQf}r|enu8JoVKd!v)At~iR~HR5O(|y ziFOWn$RC_pYbm9mR_Etj_DPl{ig^3st+*nWk4M+zuRacGTkIIZX*?i;iE@tW$?$D? zcrzZfY&0lPa3d-ZOWCKC{o`c0%a7k04O1&pvUDn`v3s6aR!d!orE7YF=GGz?e2ZJr z`A))2QQ4C!Uav_zqtA;zMw!#?*bj@s>&NhR=AQvkd+#8NGgNnErmjQBBf%Q)=&O_W z;)q{i;RVM#Dxu5zgX=MJxz7gY0}+T$Br;qIal3Q$B>~1!{_bNO7mRpczM*QE9LSDa zI&jM=f)i0cn)5^l$S1^FMNfE&c4cHSadCL5#`{=efoETbEAfzfUP^IUDj?$G;ryYIlToUiokacTA$ksd~Ob0A&AFr_(r7FHhK4lilc zO9e+}0%5W7p*$xPNPlFh&(@RTFwG)QJ8PyVy6IY{gb1{8FUh+s%fyN{e*ftsm;7nC zqScPhn_L+E%7`j=iv~cGz3@=yG_U+%Yg=RXl$G3Nq(|F?vWoT#voh+SJx{E&-qmZ% zQY$w5vG$z{VVs*`n#|gSsMmBEvAKO%RYfE3gssPvK1`c(9)jObm96@Xve%W$Uae%n zKbgxdAoa-bd^-v%s<8Xggmzw6!}aS|)P_I4^D_XkLfpJ*Iycq0p*fTg?(kjO8LjFl zV0X|i2wdc9Wr5{~xs!b9d;hB1vZ(pXxZTG{MANKv=YYS+%*adC9$XWtcKfYOT9<5g z_GT_0fyupmuU`3~MX;UF0Y~FV$&Z{{@8=Y3Z!C#_m?3?Wt*H=tj2^%;Eb#tmn=4yP zYY1fwAL*r|sdWsBwMxzmt~X!UY{W|B1U^++BE?H7ejEp(s)F0lVE5a-{PS|ADN3!? zy|$Vbrjj?^CwJ573I=0H-wzwBLm$e?W{V*|%AU66+4dJgw>H7YMX_hy**lWx#rjmw z0Bt+?7&<%rxAjhV{rcOL?`nN%@g`B=oV&*#v)SjhZI)(SeI2yz+c`e*=1^w&wPS)y zaGk!Ds{n^C=M^qK!#T4it1G92cn#`m);qV&JE$S=BN$e_6|FSj$(CATW$j_^As8y@ zPNOO{08)Rm)jrPHP?Io=BX_K@c(^9YIVL_6bNyo>-(TKQmTc0_S#i@@MmFmz%Y0Y8 z?bI>NJzYduIE5cNJ4qpOnXOZD6RdJ66mTlZr!P0Xwx%0#x*52J|A+Qs2p`*Jm?1#H z4*9@#qoXlxewRSkI((TZ&*=M5R|(2OkYV&Q0AB>h>pXpnKm3uIC;r2->r78sl;)WK z<5ZSVV6f&vSkyDXiUJj;y%X5g8D1oIDR0lzZ`I~-{)u(nz4Tvh;@}H1$)2^cSWk%C|x;g9R>XUp377c_vWJcdj2TTZi^L3s~$jwQ$z~ zTmHk@Niz3owbpT`qrlmjo?7zYgc{tuB87d{tMB?`mCcSOB;X5p`PyYFg?ftWzSmUXh2pNNSGb*z;r;8u^%7mv5RdNo5iukpy;vYfSL=}MVS=LNH2 zT3H0ofb8UFK$jYvM{_m%yrcu!Z?ba(>-RGNde7uDzH`Zavf>3}D{2m9#NTZ&ll|^0 zE+zffs<6@NX68U9jNIo?O6-)c8Zy%HOBkh|@5}TjP6evCF0O(6&q+jtw60^0;P znS-=^f}2Z{5(ce}W?W+r zsYj~LcT~9?uOIu#@Z|^JYcbV~*s9vtt9-8wW3@vN%3@=N!M zajLM!`v*t9!sF5;(3g;K`flz>lcKk1;CC@J_mO|Fxg}ySr8TN8<{68$6T?vBId+_z zXvYE;=q2xGmF1~osb#k!w5uvEC&V0!BSA)$ie;OIASR%+YV;17;6>3HShi-kO z=;hT^s7OjE{c7*k5Z}qz_7+=J|1 zbK(Yq+or-=-nMREmpGK(<%Q31Wdv97YAC%t@Qe&Z;H$;B_dRJJynM9Y6$YuWUcqu} zF-)~IAfT{SWg)CV256b>PnFdxGKoPrC6p|&;cff3zEzpP*`Ei8T9;Yd_#+ae38#(x zjy*$eco|iICsFF=nE=#qyT~@EXjsY0h6VMaq~X^xZxqvdHKIW=@PBSjvvd~>{0|C2X%U9+e$@4SCrc=@|CCZ zBOO3Nz~zDm&~AGoztXSZNXZfE@xOOhz@xQt*4NFY8z15)w)n%gvpDe%Je$#cRNBGy zS8N4t^Y^yLBMwud7r?*b35I+yY{hkKZxBmC{xjgCUbAgqba{;RUww~fz?S5%n6Tpn zzrUC&tivf1v9+*ouJ83v6{Sh%+Z&^cIGQPh{T;a1+ME^I=}d@@nTux5-t6urDBvG@ zF@$%}J#wCFq_7o~9T`x@&#z+f$=DQF1Z1C1}0(e9D~3 z&TDyLS~lPUtI06dt@??_8h^MZr{;(ux(Ly)$4^lGv#={?Tm1fl#FT;xz1)@v9Jd|w z>p3^xKVJ)B0jPml?}Pj+J93 zb2nl#)Y|+rJtlM(;@3x3;zlv%JXBs{ejO9r)Xmo*2pTb$jIhUbDOd|7gH-LY3x$dE zs-%_rv>(EZ^d`}U2~*dk69~Z4Qzh&=nea4bvRRXo6trEbxzyyhBlY)tb5%+|-aDT; zFd{Yk-PfSKlv^v9x^gt=DDrRo=V?U8;j&7cHM)*zWlllhS23w*Og`sypM?uNwP!%x z)-xar2)};@9I2>{zO9&84uc7$%@UjXQbTR`3R_&88XCTRoas}^Hz6WfAsR_0gV;q92+EFUecXcb_>`Og7cu-w+m717>1%Ox`H4b}6h!^ZuCL ze)T<4=A*8+tcb;GsM)Kkbp!G>Ra8M8V7NZH*F*&^{&AHUBl1MFyx+=OZUfh}G^8ap`@2H3_ zJ0nQO2v{Fa5Ij<(xyWBRk$Lsm-2b=I|ajeAp9&?7 zn7qrmO8!xf%7M$mX3Qc9v_>UJ8U#%2Ng6kF-VJUrJE03n&U6%*<4p?%FhvQEs5}2P zlccyE2G4yf8bzv+GJODbugb1Y4h;_<_upjqg6C?Bv|0}dYNYXUJc1G#=ePxgqC6NlVZ$ue9DJTg@jugp7Z6t+?(*8p z#T~=z?fk_Uo8DtJkLc{+x>y$%4Y$5Qk8vV)*-{OJ=T#=v>=Qw@^ZT&izxmw>hve+y5gtZ z;$7Fk-`Y^5^FA+4P>D8)e7vd(2L%&ZuLD^CSqc0jL2aH)L9Soo1 z0W={z8vxC4g!54A2u+0X8$(F`T2d!b?@CPzZ)~5Bd7XCh@nCAg+xGY2OZDzpF9B^r zpC_m{-ZXMo=616N*)0nPHk0qIGUKgpZDj3i22Qc%h_fTo5=wL+#F;tG!enS>1-(I0 zXd01#|AmXUL20YMO*EuJNZYLkb&>@H1}BKgap?D5bF9ZJ~3d;1^*CcS6CC}Gnx zpfS9(?-^hm#t5zLQR?8OO8u^fg}mhzgPNeH){-}nR+Q_Q+Nh;((y`W^D33H-HM-j8m?4*q+7$AO z&w$%$fc3uLve%6-%=LVKv;Qpo>WN!<@>+t&ZC;~<8CxrT<|VCA9rTNRfV^op7*V$(o0G#nN%r-LR~%!I4ld3a1?5O|$A%fcAE%!_}H zKVlex?ppv`M+@A&%hkXs4#WW`AP;o+h{qH6C`3 zT^r}tW~PkP{I^;Z^Ck85Eh~2ljw1dTc-W;ckFt)IPle{Ja&e)blCsf0NbJSQW#z$j z*ip{$4ZdPBGk%x7EPapsh42Q5c`#-eK^e>u$vpNOAm}RUhwvSOAa~m^Rp&M%;&!QT zhcj^DeGw3KfznhuQ<9lVo>Fhz8D+A|nDjD`t1=12cU+a?(qejfFR0YgFny~sU4FH+qKw)hfkGz*4Rm7{aU%mAiA1@aXfyn-h_uXMxX8B>U!5T;#y)p1 zOz!9Q+n3QJ26)TVt`5|QRQHE|u~6qe&-X{z7VNivxg#yVu?@C$B)mmp!H~Z=LGhc( z@Uea+JwY}mP8pS@?7XlRJZQX^pM1XB!%AQlox6I(6R-&05{mr{5vmxZA)oD!HW~Uf zSb_Y0Sb#KJP7|FVUH!XojZRP0G0{-!f@2pECC(yRCF@dZV|{r8d?&@^8SwKHtD$<$ zWX?O{f|HK)*AJDh?-kZhgGPM-ys{maZ7-UsQ%sOXC8^@QU*fIGvLYUQVH!*>wK`!+QVp?S3UX3 z1mR>~3|$y!SX2=U6i_MJ=Cn}DDdNF|@^rPn#nfC8BRK6im74bLOV2H7cJ1ihvDF=k zc;wv0~)33>TJN-3n%EnN*t1G0HT0`2Cmj|QT0P>V)_;KAUCRf1K@FVqLq#Z&a_Lfv(d z>0h#ryNnQ?sDkS2F8Xg4R*fuwXs7EOUvD)nbHw5Zd+d`2td+nb`4tTIy-?hi2T`8l zo~&HmzNrM5w8#gq5w;nHNyLMV{xT((xZFCWuFz%RP82H8Rk`xQDU$8+4f^Di+xkRLXuv#cvS~)sg_Z36|stP%8!(mU)!3n+>=ba7m>z* zO*ANY?{EG!y)ZFj`-Saqa$m5{tc@YolKf&`5ujPKv&OgC1uTA zQL-PCtbTx6x3TD;Ts%lhm#~nNn^~4h6~!@9HMV6cU-jeDCm7b&6UhMdkbjxH;7ySw zVZ_I(1pjsZ5wU%dg~nxvUyf-oYxwJGw*_MMSmk}IRc#2(!zh~)TnNJSkT&@kbkOg_ zN+QLr;U>+zrG#nKFlA^Z+E&Xlc`;J4xH5eKeO!VJ4&@Nof~i1Xgz+ zb@7fp75h7_K3R<=!D-c2DljRf6&#J7@$^LoJ{=qxl$$$}xRXD70+08XL$B(+u+FsyhFZ z63rSGDPEDpJxo#;>2^kWqA^H`8w;=ZrD)1ySUV7g zp_x*~Zf-cvx0KEVlgMVdqD>c9O#Pj&u{_sWR01ct{!()GWhEJQu1DD6Bmxrh#@TMk z{}FW62<-t{n!D1HyTlLfk;+yj=@E3THuHvAZhf;SwY0R~q`+P?=xO|E&nCj}G1j5X zVNONj&_xT7@_OrN@M)`aS-3X~dHzv;b4=>;Q%g(3EUB!Sb&*KCFm`lPvoe?BYvfQY ztKFK~TA|Y`P>Zdnf`*}x_p5H&w#fE zC2mpj-jhp%O;p|lUj**Ncz+*JG;|c1(W2XLj1g1uFmqVP;tUE)w~7c2AZTp=ENf00 zQ?PG&9fV*Nu;&DfZ%sOcb8Y}I|ISy)spLsslWY^)ew_>=y$eQ;qzz<1@@_C zE;oMT<+nfkqc_AUTXH2X`xT9s25L*V+}tMc z9BSY*(P794P#lKu&vyzL!_2m0vB5j%Hw8|w2M$)@%{B}A`3D_bFrQ5ywVuW_7VKw0 zk2DjD(-=3iZZ?xlBlf$Hr&H$>^`BtfZ-11WuL=bWHdVD$3L{0yXv6I_)FuOkfsK&-p8+sT#bP-<^rCiRlFrXUy{Vv`sxwA2HgJyR13ry#DqRZ^ zX(Xn|JOH2u7@#-{CU_K^%G{sz6>42&N@$odY)%~p)sjOe<}-4qXVZ&jAAmiLDQSD9 z<$6zfngqTdiq_fxXFDS1-@5^2_k#(efU)}~P-md6(Jiif~&`+{N4 z$G)TNk;6BUfFE!_cf^9liqz}#`~cqe&w&h2|kdH>k@`Nb&yZ4*X}ePS-zxg>Tp zpP|6&w1`FUR)f;~%_5uIO}_qY9e?XKh9rsS?=ZH$M`|+P&Lyn=!_59z%ae9vq1#O- zC9snBU3E?KteK1IfVN9!XLlp z^4RBjo3&2UMm-(L3*v3he2W|Kf$IDMO--E8kQa@TbG%IPDw4P9V6K^~JFwj(IL|ax z^fHbMx)t+wR61`QIGT8+r=wJKn0#iBl4jJ{7D5}w^;vM>+@tGW^N%kX3T7R`4{=!agcC@1eNAJLCz?2PbX~Q)BQuEWr z3LEi@IE`s5e19rAry&|k6rG1AoW}h_!Q7IpSq?u!)s1%x-b;TKw zcIIq0s?IdiI0FUlXLJ>#f}+JI&0)H~U${Hbeog08oR!)S2_HF=^n<|V1Adtr%>p>Z zB1mM1zZ)V)cgwFUNwYIUmFzbQm5eca&RYG&$s|s`zea?%B2X~S$JSO%6>4N+k+z}BQMmYyBD%aK1LIFh)l4&TGq8E z8Z7cW1JlbkZdQI%^t9xM+_Oy0rcA=j#lzsV#fbi`nvf(3YJ6EDDJ|v=++(YvU{^!_ zmw6Bp#|mrF0cv``Rf&F>P#FeYl^g7f3(CuWa7r0idt-T<@8K~_6xZUzRL273v~v)A zV)BShMdT0|<#xWpJGSu|e7Ul>>ziAqz3o{W@^}ODlR3HaafXN*K8oGVUQHMar9Ifq zuN#D8w)6)d#ol?%ygBO|WQx24!e4h2`}F{~4icS~quXeo0j;n6Zag3O5Z$mzEV~h^ zGe}*#D^Q@dXxTUj34P~PE(&Ppz_=h#_2WdUVQO~$OV)24$D0Ti=d~?US6tr4UZF2? zz?w0zhpwHSO6~T(Rf3bwiF2IWF!!rxXCH`uOR|3ED%x$*XP8(rQWdmdu@6U9i0xZG zix~OaL3@tGp0@@h+ibqr4YGgeus(g1bP-$Z2if_GSa+~J_=X@yku)G`^(hUN$H-KO zzNA>S8bL=_ms=B$xScPzPqeA^I*Jmzf4vlKopMQ2O+trAp;6Thi59|vAS$@N~4=}oS5x>DM~;}mfNey>zTkwf=@`#2VFOYP!9CikvJjq7KAHHEqy2QH9U zMeTqA*6bsC$)HIot*j%huhkuwJ+hOObF9h>-?Xd(Ln2>QGb?fm2o-yzPk-ot7!f;3 zo76x-%J7`2A$*~nj;5P_7p8yJadE~O6fCEYCf!ZBDmIv=AAdD-_#TO~q8bO}I3|s} zRqNzn^*XUnE-TF6ns>BspGe?LG$!`M`e{?r+0Qm%<|;Q}^&vQuOXCafP1f?>J!6iYlh!CMTlDiA36CXOItg4ubYPZrouqZ zIP92Dv^oHmHrB)Z)j6JRg=QyVoBi7w(|O7HTz2SLwo=?y$_d}{hbp;IV%rawQnO6& z3N;tfDvNw4D2UrW;=*Z_?5&oBE7&&zq&$-mY7^jdK4Guwy+mNsIa%7X-lLYrImIut z`NnT3^dg5A>d}9)r<7R{%$L3Tpp$f|K<)Sp5DDTohXWNkBbCHP%44(3o&hGpA7fFIqKZGk=xb`r-^@?_fGje_(-qnNSVf?BAWwCicBlY|02=9Mnh#MWNK3 zR2qIWnARM{Dmi?I6Ld)*R~SA3;vX53e!8%{aIbv%cFHHc_wBlTM!`VyueKle{iu=< z@qs!Ods7pZ51MMp(_mtEU~ff;2yhcUbPz=?W)GIFNSish$8=i65XwV1)|W+c?wkB= zq@Xh77%$fB4=!=-FK=-uDRh`1n>93q)StN`!Gx>|yxYY^(O(*p-UD4Gx-a2@k?7ALt}5bTW|u=@dY=oD`{{@trOJa z8?w(ONfr1imYSgqI_Kp{F_Fv2YuS}n$^#9J7SL5Dg?cA*I$8sHU@y11U%hh&1&nwd z_mO*x0&lfot}utoWz@?|rrdn_%`Vy!ofBUm>b^+YE|L)*brdwpxb!fKQy)d1ASLbU zh7UcSRbl%VM!XCvmiJY^#b4tj2EtJ_cANa)Dw{_NZK61LNT1KW_Mw%^bQhxz6;e450k}#F!K5tFV-<5+1mZ=SC6!?1SoIZmC_qY zTZ=(BxKM(c{i)8l1cs>KiM)TDXR^s7>r3M;Ra@(JHx|-yx8q9jC)y3}rKKsk&Av=C zkj4HR#}Erk%LH*3P3_WeU0Ax_BdR&xlJ2V|{rC8bp_SCw5{8odY3E~C@knXQ zqQg#15d>;ThzJowIQz>iJk4TQ+^l%Mi&=ow8(~w%Us}}uKC9MUY&zu&Kpc# zuKTaYR>$_!&xI}ZET9=ob&4TEVb;LN1S@st^XbLoS%Tnz^J0eP{=iSDaNkga zFLu*xo<4Q2n#>1z>^ED2=Xi~(2xJ`0S2Q<)Q;&XISYauAewe(pGv0DDJ!?iu#$Ww? zuBAVVXN8G;Jy49PlvwzE*Zg@WMt7Smc98ij)Bx*SS7U?58joi9(HTOc{@89*;-#2E zEBxF*$*LMUAqAWIHMS87b1l`;4yTo zCNta0EDz~RSG&z-$~=1c(xX*`G={lcdgyLR*pi8jO)yt zj+gZF$wAelU=D8(?B~{5gzu}lN1|=oDG|X!nIg>^++oy=2%s?GoJ@+7nOoz>Y4`oj z&c1fP?c2~cwV&>j2U);snf}w5+M$4R-QQ|x(eYj61jutxkE)C8x`Rq2$F0paX)HdIuX z{e2-4z@%fFN6j|%_Dt~ibioy}el;a}95g7P0p@`=s*OK}z02@w`p(To5v7(z{Z&av zA0C@M4ab`R9yvn?hn2)A(x~kqgOFzc@~b(9mx%2AaBJBF`M2`=7-`V2n)Hvv`*5H| z2Qv%`iMVd6pDMCWLvpLuu5UAp{^R6yko3NOZ(3N%Si{n z3Aud{`OQponTB^qH~N0kHc7K(ai+?U6=2a8=b7R%InW%7t|#EfWT~6E?c%)yBuP9j zRlQmOTwnIRU^5COXTYT6{mka%&hYB6J%NV@p$T3ZXS3I4|I3IfxTPC?`uW@tnO0RhwY%Yu?a8<4pqiG=B9ZTHd zxA%^cB4+Bzl07{2Wo91)aRqdqM)x}7ZNywXE)`BjOF3ur(Y$!^tC7(?!H#b0ZgBehu2fRD=!py!`nwDh@p zpQ1nTPKYt^i^LZvgD$pX!M8><5ACUm}0OKJ4`lMYL zOUOu>3g-M>m#3{NPuVl2V4*0W;H$|Z~#Xmie#7r+uOZo zooq$8TuXZ>mz{7V|wL@r&WQ zd_~jIt>uo}RkUU{^2zrdN#z*z13c%kuK^zcylW(-PMHiU4m`)ai*MJ4C-kqW=eZ@A zt@;Us1e_GYF_yK!qIyL^gZ{=s^5nD44 z&v3`r74FX4diU>0Hn<9T98>1F?$4BbL*e_K26}RGA@Xo zJ_kL8RYr8-hDBY~p#II&uB`)!UI`j9>Ie)}kVyQi(moLUKk&bf>~2o0sa@OZFiW@W z@y#4T)Q^;mf=_(YdJP_3=AZfIznvBJSNtP2P^XI-3gHLaNMz)Esh0zC~juj{10OYT5A~o8a#q_}1q`x9Tu zELS>>jlP~q$+^`Z?RO{LWBZ`}abD-I{B`h0!;5Fr{3l@+sc?gDm!=?!-AL)aU*h~S zYs7qG@wdjFHQyZin`u@oPx>a8n1($W^SQq;J*p)wERNXGN2))Az7qT)L~jvZCbOz1 zD{piA=O@u^(||wT&(^R!P5V~drLnT`j)M-DZmYH{i`->L`!J`8%c;{CSkmz?iS>^eUHzwC)h=~u0m`kze(g{DF^nqy09HGYGu!D- z@?D9^+66L8X2AinM{+1MNQ~l`_%!b-O!X8rC%NL0Nf8hN@;@p<27jGO){Mgn#2S5u zi+`)#t(Ki_HTC0yRu~g=5If`3-m;X^+|e$?x-W3yny0x9#GQ+y!Tcy+9H30NL@)A8bkN}xLQQJP5HRZn%^^Xs0dX!LTJ{(;V*3`F{^D$|plcD}L01vM<=Srig zE?RpX5xUg!e;W864MW6}>fRod+FHHZ+(&JxD0u-g25c0-T=Rv&$E|*ce01=qi@bmE zzUNBtM!)|623(BP3(QU@k8M+#P1jU5%8ymw10>PP4R}D zV>+I$w-Z73>>*Q`5O3g>Jdc|k)sK!p2!0@ZM9@4FW#OM6$>ICm9h3V~>`OF1Y+#BE zxsUG<%Ex1Hb6*(zKK;KwE%>v<31xYyX!h|H-H5dI83V5-Ltcfge%HScbZ57gT|s5G znkH@bnqioRR>lYfk;gUF&lS+~{hZzPK9Ts%@Q23V5WIchZEM4xJh<@onW9LiP_$Vl zkL@^-S)43?E0zJ2tMSj_PxyiGgT?+V`0?Ry8fl(5xYE2grG)!NiXK(-HnSE`i}EmV zKqb0<0=`dA+l$6GXAtVME>BrB#DovU{{Wp_)jw;Ghyf zvG~&GU4c`VbLu@$;NObxe`$LUh|&1k<{t}N+k}4)`I3N0g`+93e(a%QA19ptbj=sw zw~4gR+B;VGi*e&?o39RC+q`jTC8hayAW(%#*bxXU24_2K0A^-SY|64iwk3b0PbVvj%4~2p*5w?r9W@{Tzpda zS!l-zJUWV=0XW-ic*rC8dy2LD8~DFL_;2EUPs7(?t)lS}ghhWWqiW3%Z!RVn7!Bq= z8`Oc{HSy-1@w3Mt8@?xaX5+?s^J`LRiF5XHZK8)@c>aI5qdD50eJj=;G>{cz!1Vrg z6ZfokMy%mS**%Zat!u+NZnbMIj;CjDeQ5a&6~YDw{42<{FWHvIO}U=)!CL;I;i=db zHf^@*(atyp3jzV@oSNpo0(^Ii#1?k`8u1mvrnLfy?Qf3<0GN!lj)u+@O`z0rwd|PEv?lVx%=K*3=n$|YQ_Gqr>&3n zmA{1BSJKb?zS_lZLLTAOh-4$siuy0&cf&mc;+B|^UA*>Mj34puevzv*f9#Q;nSHC~ z4QD{{XTX0D`H}!_W;;d70@jyn=)?G19gja+B)Xv{wq=OCO=S#;G#(7SfyXN>P+NjK zbq`)o`#^ko(Ldo0@h5|1vDGZ~%iBdoj^Y&Bp_y0`!Hxz6d$yUacz5C)do1LO!=52w zm9@ESTg>0=k=Xll?O!7P(jN+Zb8X|xtz%!%mrjOcjwoifb2YoF3<4y;9vn)r+jOQCyIQp8Y1jT}=>_ts=b0U?EIo}vkF_Y`Z@i10PLc^h`E~T-o|-sZIbrw6co34Jh36j$lcqF*M&AtVR zl&ih8(-24Zu4QlJMSIi?3ia(?ciLuEJ|m4^jv9WOA%|b^{;#3Pr)hAaNWmRQKQA9l z*F&WE@5A0Xj%!^y_-Z8(JeJoM=K)U~?i)|iwVWAHqkQR(Rk;W7JXIJpOB+jrJ-wB+ zyzRqF3@VJ@QU^6xG9W%1(ou^;V3uxrwS@L=82dHM zi6ZchbGvTyf;h_>_A;L>c>e(HEh%3MXwYW@OSxYjyJG(U?5g1PC7&u>DfGognFp`` z09v$eAUQY~szx%c(9mp#Og>(rAQS#ZE8hMkqC?_e4Opa+$}O}AFcpF1vCmGms(OQ49~%5Y(oU7* zU1A9r2ki{w(5rmI^fl zQ$=&A$8if=T6sh4vQ8Q zeuLJy+n(yCb9I-fm)WZ>pzOMsU;3`_A4`hPI?p6*U+B~ ze`O6=#|vow7>h)+d>^*zAS;zW!~yw_=}_c%Q^NN?c>dA&mE*g;YG1_tL zqo2&zN8r!dl1~-gD_sp=#y5oS^5aq<2j3iH>tA~KC-!R4d_MmGXw{*Y#FwKOYodt? z9{7z|=kczq#a|VCCGgJJT3p@34CoHcGfqJAf8Q&@{y3;}J7b4SW5T=x`x#HC!3oso zgGIN%KVs714<<0=(g37` zkx@u?=YdM|9^4UOJMDc-0b{O)INgtg=WFY-%?AyCj3XBZ>IrOG6hC__@sEnsS z{c3xtUVl1UYe}^WsI9E+-sb973mj4PVt>zC!g8@mTz!?b)~9y`osDA#wP&~SOgeS#%7by`SVt~6A0T&Na6b-e9}9lZx+S)i47&C0#m27V zbb`kGLmW9Da1qJJbDnD#;xB{z0r2xhYnP7l{{T{yl(exrq>f~NR>9#1bNE+_Rz}>t}r`NxGIH- zEaVN&c_;C#i_3<_ItuNfPnB5{CN8C_G)Tox8`8Wp#@-K2OI5hMjELG+Ec<#B>-Dc! zx-S0!#816p-`Iy4Bzp9(N-LCH%<@YQ4^O2`C0QLM9Bx8Et?fHep2G4ejFJ+p3zAM) z1MA+pJB>SY@Pi$yBc%l+a*TV`m2-aU#=aMx-J0qfgq)6ZEQBA{yw_9l6{OeYwCK!e z4-Q!I^sa{g06?|8U*1NpD?IpS-XZ(Af`QYXMH&dq8jRB}1gtaZ#bVufQe8&nrSl1A zB$g+=b;+ZMNx}jvM%Kx=AvTQk6p>dbd^zyP{3H6PyDZNuAx+r(p!Kg>g6WtHn&q_E z{ETE)^b!_OX+24D(#E39NdyuY9EJx3A78C~4e)>Vhz?-Td}VdCk|VX&n;0I8KmPz1 z`Pa@7zBn~5{mMb*eGMjIB$@ghEw$aY4)v9Bm0`#uwg~=p1oB$nTFYms!wmM)0@$YuT_or;cwk=!4&A=E7|EG@6B zw0k{9!s0cS8>pB_k|E?UIO$I3G?PsEW8ydL-DP_3dEmRtc;#YUv}>vGrMmv<&)p}{ zxc3$3ng_*Qf5GPC!~P+%8qS{?Ff}(YSl_S9PlLo(N<$3IWySNtjA2=DDJ@1$ld;g@f0WRK}zTzKR5O3-|DYIW}e z>R;PdAqtBlakf~&=W4h9%D^9UURMu-{{Z0^bEQ43)^l5BTH57G$ej8g=UKC%Ni)@c z4t~+v&w)HWrTBJl4%_QGcAo%-7J_pG`v*?BQhzGRXg{=f z#lHjqEqqNTt8TyY>#V{qmjkwVarx0^Lxs=Lo-tk_`%6v__64y17_KEC`;=etuN?5_ z{1jq6I#jpuzk@uUaBp-wwKyHghD;y8F{|Gbzi6)k{7a6nL$;ESiG9FiluLw9S6+botxmEb%9orPO(WL-&dFEVxf}?L8KZ=q_ z7AWIHU|CmpZ1m4uW3>o&WLiYyOSHM2p;^lnO^=LV=RHCGRoG489V`1Y!~P;|HYoK8 z&@^ug6=Vids-!Lf8*$t4u448dKS~h7xsb{i816;}E7WfE+o)s~cB-?bvcR#%PCiwO z2Oj)`ReeS~PmlLwJ$f3jV#(vJN1QOj=u$D4XNF^I&MjeWtm8LC-envZUNM#+A zR-f62`#+r&0UyKP*{@ByzKwLZ+vB|G$5a{;! z{{W9G1rI0U-3Q@cNqL$Vf;5g+(?ioCTyAZ!#yIJoGJ5;hjQHR7vC}L(+iweL@@p3W z?OV&7#cny|f%luQY*%BM&l)KngW-RJJ__&({j%3hi6)71Z?5Dl;A4}xoDZ#XemMQ2 zydU9jI^RkU;w?mFLRym0hv)A|PBHY&eA(kKh`uE8{JXW?T3O-kX9w<}U-$s)&~>hF zgpX>CsyYBTtemcG$dTXppW~m4{8WZ1FXPeeNFlAXr1JM?)y`P-_Udba^CO9wk$z@5 zEP9jfd(?6`_3KbbN$vEcGHGtR4n2iH^N=x&9MmL{$FE9=$&L;=q#{FbTo4aK)4fJr znDS~o@G-#j^r71-2U0y~5c2$BoO4Yw4uoWTRAI7k+2-WAKa z_WuAL)s0En7LjvD(zOo~>el*RnQqs2*93%&4itB0J(PDf?^YkPPlqGZ;7wBh08qQW z+57jlm`4$8auj1Y?OYFqyfg7!o~Km ztvmLOaJeM@op=@V(?@P5mEg6G)g)$RiB9h=bHeWGa4<4!bH#o>*L-;Pvum1wycWvd zN=FjJi@T}Xz4*p!oigRP!4=VmrBy4`da`8W(0tKk9G}9oE^LDXp7o$yvFD8X)J@HL z@Qt@Ixq&T~8yk2PPJK8wFe{~KmH8f(3)^wqir7~3dxUI@#9jioI;Cg#|(u;kcN;DP9(XSZv=~4yiYF{r0 zr7q8#Jc>;c#-ikU_pey^9s5A*Uk5IQkuqvN6qZfXY1zx$llP>9Jr8od54T$KwL%Zp zo${%{>FG+u?tYhOI^B9%&#ZjN@dbJ!`` ze-4%KKf*tYy5GUy5RFGhZ$8pu1@@UE5f#$^0PLVU6SHcy&I!iuDaY;)jKPC0oI!d_dCTv%UjtTGgo9s(sJiPsb}?V|dHq z?}dIU>0e^;2AKu;3Pe-mZ19ua9CAO-b6*&IY5kczMewO+)T|m$5qMpBH&LhhMX>(> zrl>lB{pI7-3QjlJ(lggm*NA=x-stw?`$?5NL!+|qNuk3?sp>%CzM{AZ{67?oc8+s` zr>OS&(>w?9N5mfwWZ$G+tkw*2_7@p=U+LWsxcqC{^uG~!8{>gU$vzwLODN24K=Ti= z=^*@#V&mN1Nbns)!=Rx!01s?bT3?5}D{9iq;%nLOCSUbB7LgwNi8_4KNZ%w&&q7H! z>05uaewX48gSu6<&CQL>`gF3gNvT-EWQsA43!nLBbH_iOaeoRtW8u#hX~xS+c}Aj< z8SYki@L!x34U!1WLroedb)nhX{6f++1W2^MhCUpQAac*BJI^a%b|F+L?~$69Rrt?u zsx(nucueWKe$pgOCgv$zpY}xY;18)apMRo{nS%TDu6Fv_S1L!ap-5I;iFMWZRpL8Y zOqYHq(=Nv$Z!|ke)xCs~K1bq!R}XdY%fXt8may?$Hj)#-ztWx{JXEje8?NPRh}ts4o%%@P_+-qUAn>*3muW8Gmp3Xrwv(4G z4NJxzA+(oTzWZxi#9~0U>k3AvovNS?GmoZf_rfoS-Wd3w;!FK6T=8RgJr>YOXL!~P zbpe#P1WmMpi;m;D=M>sQNcBBS#?OB&7tm?R7NCP4X@Xp=sr}X%=h*chrFFg^_`jvS zn2%Amo;$2?o>*p6o#TB;VjsbCXXfMTLm%Z((?uki?w|0mW&Z%64WIr65AdSAi_5eB z0HNj|{1?eZO_=PvpIP{u#ag$E@1FBr)aTTu+)9a9h~v*>LBfGq4&_1H(xZM!B?XAM4vDO@%5(7FhjxTuOgr= z&t69$nrpTj3{F7f6+#k88*w?sGjZp=F{C6BjCx{^nLs@A)KE(mr5lLkf@x8)$;K&! zsQ}|V_o&^*S#k#M-NgV&EZ~uWoK;C~3zp>Ibfl95&|vkb!w$sIA&0rnMKGM>`cuv^ z$-(XqJkr`)&3SbVt*jDU-A>{-p^0}hvFHG-YEHCcr|MEaMIWzv`Ir6`M~$V%&k5@Lhw%gm zN8`9vh4^Ff9sz~Wwd7OLR_K2(GQ132CtY&Fb~<3xb!XU(V>v(SU!JE2iur@~h4`Sp zCcl3RX;W^}Ead+HOq~xGQ!nrX-BkW(=qt~4xV%r{OaB0|YP0H^Wz$9^@+G|`wh}-j z2RP0<*F2$A6;%ocrE}r(*(s^YS4gQrKfggVWN0 zxiL9jdY;t-l6lVq-kc;H3=VtK815&I-6$BLu%{!ORAl7SnS#@LfOF4WW`Gm!{Xd;G z*yq2$H1gYl=LVY~>DrnCGe~-jfHC^h$pZ%*(_8{Zde8$K+L{h?PbvVx!K7sazgjK> zPZ-Bu)X~V~b?-<)Bx0UHz!*99qyt2Na5^7a>3j$A16%lM;!ACN!}niiy^9MZZT_m} zKJm!C!T$j3^{!>cSNhPPf$u>F>2JZWjlLiF-J%(+57@Pc6n(czm4}%L?#un;C!-4N z?=)i}AtiMlgbMt#@V|<6Zw71jn%;+dCY`EUHe;DPj_Sksl=lX|LcRe0(3(%iy&SZ+{SS1b&}~97S0$D%h<05r;b?y6?3^r!B*rE z=xd{r+#LG*)fwy%^ceb9hdVxL_`&-oX?`ww^?wI6S{AhAyV)?srOSH{`DVwt9D7&C z{vgn`9}DXe>3YqS+GeM2*=QykSx;i8xc3IXQf0Jgn32f!6~%l__+O!XQPKXxu37oa zqxUy)e6zrPRl5Ei4|*+q#~CzyfAHt_kJWrN3h7=ajr>KSu-INqJ;ZC%2X_)KJ05-O ztFZl?ejfOGH3jm4ATt&qM26hlPAwtN1HdO;bSBCDU&PGSQGyKsm!NMjpnY#>n0-`yP?v zjR#TirQx#HY$em~Pu&uXq=VHMaKq_dcdJH*Mr`!w*jLiN3;m+}FY#8%JU#Ia#?w!_ zlwoF+{{W%N-7>A#7Wd9Q>V0GOE|Px|X}5M7o6q4UludhUFU@w4sN7a297r1*M?=W# zS@QNRJ3BMyO=rWp$AJD5N#obkypcPsc2_qND!2sWd{M9^vDBOZ2SZ;pYTgv_AH^uSo(J*x>=FqVf)W722I|%>t3ZQupEx{k#z}o2OaAxnxl}?z75&fjng!~DzP68yk{UgJYC`FHd@Oj4h`j~`A8=jWasEB zfkstQIW(n^j+p+Gy$Vd|zu}X$@A@xa4!`grXs%c6*5C8WFZ5ef_8H1Mjj(Hx`j>Mi=f&8kw5uah64Fr;z2sj)Q+|)8l$6kGp zN1+}G8=1-=fafozjl)ni$FOK})G zPa@3^@DKL+J z00~GzZ4<xDy-O^9g?WG9r&tfcb)aTsSG4T(_mY)uMEpx2h5hUO0 ztu2W;o@4h{=sKTj;2ZX?@P2SKe-bh8i)(-WKVBE{JK{C(#P1UwM&Vx5O}ET?eS;8u zldl{HwUk zNe0qSb4~Mk<2gNPxh}-|aY(E0#xp?BySnEX9qFw2ZrJqBXgrcTeJPGMo}7`K4k!U# zfjn?24lsH8R7LiwyMk1CdN(Q=Wr9l*Nz&dVNhJISMiS=mGt4lk3pZ zGhhHtY9dJl^Y~I8ah!M0J?KNRHb@@bsfD)u$G=KWi}-LUvSS4Cj(XD&q=r8K0A7@V zdEj)UR@yn|(=@{SiS-m*2||3`Mkw92k34$Q6}ZlL_oVZ3xc(|a9$y1F82VIuiKQni z(~nASc7xC5K!YF>I#t~pS-$Z8t!b)hH?ry4rK9)Q)Ap3H_`hhnF0til-Y|hO@DG+d)g5Gu-?>hsuOpiF*No?ZUy=5=)7V|xUD@18 zYk6+#EEg~_St4+FWp1F?-Cwhh?OkW_5p8@i;>lyM@n9Pvy2jWwEO%bZFRFAsSFX`% z_c4?@qv!{SV+W;Ep3oM^AEj!#^Xc`doj~`k4pxq9QPH8kx0(r}j#wp9j})pI9Zv)h zde_PS02)7JHik*{j{-)hd1-V>8*1crFtOl&x_I=(eVHr)jX^DnNH9fFDYVbYySphZ zZtg6vE#*4{q^=ghAZRmh<~#$kG>sAE4Gs_h;*g%sTjq~Lpaa()MM30CZVq9 zM0}5srG;fdfC{%lInUCw<+K~Ijy>z|AB%snr-%G?VthyN()QC!y17q1Y*EAM*MYeP zGBf29+iq9bWSaR8T=;vhcx%KnXj(-djU~$N;4sYmSm*13 zTM{_RRIYRQAMmD0Iw}&_RsQif;;Xgwv21yZp4+}!jK>AsrR0TWQPHFapK+R?MUi@r zgJhAuc^g<0imubC3`Zb!%{78K$VA#YVL%lVB7iczdtgvWD==fh&QAauV>aG0KAGaH zi%5XTjRtxhqLEcl{G+eE0Lh+Co`>AjMnY7dx(E5`L1L(Q!0rA`CiION*}{R&O+@9& z2?phiZ(;)a_ODRTKW85t_=`xl)I>UNnH=l6Ak^+&IkujJs-t5gu^{_bpnL@Q4A$~m z_^-tlr_3kK({!1m`@O`{gWtNZZl=D9mPtcqK!yA61B&@9esSz2$xU6Ibm(&{$oUda z*>lGrF4^=6FgeBZ;Qn!nr*Zo<{6LHlwpv}obAb+|ng0OCg4ftm$k-Wwk3&`Dlq-NZs7;KKLa7DYCcrQq6@dO<&YdIS zjyNC;uo%sKoiyyOa6QL6E^<1ab5JVdEPM3gm7fQYa8FZ6Np-@KbJDv}3~3~;K*0W0 z8Do|MC$aXVkjQuhCOF;H{#3w>=Pba2*~Z>aA6j5a%ot#2AJ&k$!O6kz#YEtOaNK{p zoDa^U-2Fk%)PMEpD43ialhZh+79{7IV|j!(%%J1A{{ZXMafud8F;kv*DZ%_G0!AG% z`Otn<<2XG{BOT0n#yk4cM?XpcQ|vnW(-amY_4?C9n0ynEdJ0C$oT~sk_n>wq!vJ}} z$26Jip46+pPn40*wKha#UQfzTr2_?Xli$4|AcK>U`czXRft6v$d{Ykoer_?x6cS2s z*y<@V4^~UbItL+CuG6=!+HTdr=QQTX!a<1s#?*eP?zu5cs zk8c`jso;MVUIy_UjC*V^WR#UygBu%PR3G=)z7B3@#{v+_NSI!e=0!2 zwNA$~h{FzZ#YXU)5~m*GtYlHa;;V9PW8<&d$M$Z#zrMNg$Azs;#nY*6Q%r$=X|ewR zEu$UVA9;J=5nm#S5UxXX?0bDj^RL(0AXgRhPsjfN*#^tT?{(r22qZdRh}$nUu^V8# zKK}sRbN%nn)`qTa33BLsyYOq{{{V`9315SwOmDO&lXcFAa>T*4Kx;qUE5;HYjDZM8ufhL+jeZQh+T4pfozBXNw58z00g^E+44H4hGIml|fRWj>p! z+cwFqZWC*v?ez!L92%R$9wgR08Lr#wdIqO+qw27nt<|$&vg8tu4-Tgss{$(qt+Cjh z&!azRf7yG)-VJ+=b3<8tOX2hd0ACi_<7<#h1%okJ}1)O zPP_jAmU54p3CCuRPL1^HJAqvP0MumtD>j*04cI*>rTl2DtSg?3peW(O=e9lQ^EVQt z=ZtgOqE^89k0U>&A(*3J5~Pe~x=3JhszE(_1CA;-+UTWJr#a4ll{MStax?ExGZMob?is07vOy|(_vui(V+FYB-`;>77{X`Qp!KLWmSRBq z_BAMdfQ+BO^{9#7z~G-!f1OO7fI>uLbd9wXXDoOX^k?kv@EN9-*T!Bbvtb(IO)pEh zA9(hjMf4q2Pq938@J$a?k3-jNbt$y#U0p3-%ocZrgfcK37ROLJGU8mInU-*{k5HU@=m61k}zYFkUhH75ao&EZ)$J)z&OA;AB{lLe9Qq1Kb3uCV`GJqw8{Y^ zrh4X@%*%i<-;GBboUsJpoOBeDDuyFH`NsyVLWc}TQag}3(JL_LcjZRnkqxC(dS{ws zkGS9wx7VcuK*SaKx(`~3NGBQZO?BEbdt;|Lq$4TES_VnH1I9@11x8)RrKw%|XsF_)|&whIl;& zDKIA7Up+JJO5-OP!5maRVOHspL2^cW9@(G=xY&8=O~}s&BfTt=1@+J8OfQw*cn6G7 zz?VDvbsn^Yka9)=sQlBM(mv*3dW=;FnXpxP>q`U4YjF+5yz*Myz~V?}gzb2oau^=O zR3U=*$j?e$#fPQ_e+*I?_wVez`%TC3PS!sR_?l?#ykl&iyO$eNQjC8Z`baVJFMgxj z=t2lM>5BY;x4W9k<_oJy=C!-GaV!@QHrI{ED#}MfEAP+QhxVX@;;pP+8u2WV+IY&{ zF&vyMdXgyn=sF1p@h8-dGlrdxdBXam=sm=Br4DF3XO4z}&T7*!XmFH{^zG#I=}lGQ zlnftgyNL6@jK2c>JMpJQHu|JrWYuCqEj3sa3E)1N{w2>*EdL}Yc|j4)J;=DoY%XYF6&Z-hT*({%@%#J&?#wXG(TbMUx)tyw6}nZMlAeGqEEz@^QZ4^%sgX<8KgrJ=YUh@Ybt7k*Kc)qB%)d`?4Y7 z$JeK9SKa>r0RI4NEfeC4L85qwL*dUAkP%{c{{Tn5JA9u*9|3u* z=>Gus?Z3i`{dfMw)4%8+Xa4|xSNtff?1gjk`K7}QV3E*bnl~I1k&ecsk&uU@V7a8a}8kZ{tg*Yv^WrzDBqh{oBjOV9QUXk!S;N<@R z7WMNb%Nu_USV}(MGO-OQ>LOF>dy&w4kzZ3>`~diCGyMJw()I(iFo=)jYr@55v@p`; zn>LLZPgHy*C(E&q2PfsvJ?c4_7Hk~jwmVncGk(q=4BfIkC2tw&n5UJ0lT{m!*?Zx% z>Oj#hVL9s-qcHyf*^PPcKEgdNYxQ}f;;rPI@wE2EKPtJ~jo8TPFL4msYs{07uhoMyVk(%Z{WR{HxTQcJ(7aGKOST5}nduZpVHr*uvteVq4A2%ObGNQO zzSR_%RUOSWPESvpo=<8C10-^LpGsk10F@<(IKTrRr6`jdM*w4*k2r9-`EkuLdFkua z(itR^6Y`!Z1D-N{DmLI?l5@$X?g}t-lg0q1GEXoPLE@3vFB#)C9(l)n_oXZt0fGGJ zumiq8H1I|T1XL%3)2%xgUQg1H4wRFELB%zkD9+>Bnp-&;$TVXM$tO9-6u?}EJxTP( zdJe|+Bd!HTOAHL;jP#@+f^s>>Jn=vXA1~9V2hh{Q1_Tbh%|y?a;B6g8Iqgmi26N7F z#XA7zC7*MUGCR~V9JnBidWv^Loa5_FQ=H)74@y8hNEH`2C%$P5XN>#QdBFgj`q3+3 z40S!|0a?c*0At>q4sv?u*S#@83(ihB!KK~^;{zSdP=SU92U>8*EA3UGR6lpco*^rxtWUU}mkc<)wi?j^OF8=HBg zx4D7Du}=vq@i;kFQaJ>A)o(B}ev}s~pp2ht0iU2hvfu4J55-$_@VAMij@QO^$V5_{ zDr#}BS3%l9KJxoyagaWU*^}>IkCwMH+ucERYdn^BmhL5j<|Zp7GAYWkkQRBqpy(t6?;!e)G6PScoS~)B^dM7821O|*ml>@Q zjf6P|=}ump((TPA4N?mZ1B2WiFl*)S+6(qIvhi`c@ehGg9V=fuq@PSbCAHz|a6jdn zli#5|eQW9!Ip_sQ2mb)7iB4St+_cm16FKk>6kl1D_-d`k)uX{*G)Fe9Q!2j4Q2D+(A$SyzwteJiE-bMV_$_?zOp zZ4*rlk+BbVcZ{XHS>=xy&t)F9lc8vOhl%y8Ef-3*^R+AK1aQU&QgfDUbWk!$K9%>^ z!jFXZ9|^o87MX1l+}=%zFE1O1no>IF`=g*8tHj6S79JPsZ%!P{(D2uUw9f%(HvS*d z?M1GeZNfUQGCnzHJ=mT}tIjY01eNE5SLIOwD>9d6Tn_lnQ}XsU*5D4tzbeL8m1((2 zp0zlnisa<>s#><0mYRK(Pd&uabVN~-f5X&HMVl9vy(fs%iRprtMfZejo@1b00D04j!O&m-kf zcs)X9LF@e`P_NJU*f=T9v#CZf@V?Rm;A$++V#Y{)a8$P0<7z5j%@+v=t%d{SVR$zU*fr*IUV*f`IvFfd$}1mm1_phTd82Xjm;rL)ct z9VwX(NhF{rC)oSasup36pl6d(I|$0eK7x=*(0bFj5;t&o$K{SG%fSGU0O{J8$O4o4QY$fFqbJ&cB?kcD zDB~aJ+L{=X=^+G%Ick#L=6gHXF0Ch)&hFjB(A>o3B#FVwvaskus5cGVj2~{IpZp4Q zj`Vo~KSKWiW#8IjAB%P?;qMVk6}OEnQ04)%Id;7{hql1C;9xL(U%Tc-0 zH4AI|E17Kd3rPgl_fR(1iN`_+{{SYxLqBFO+HTk54xG9sw((f_($YN168y2>ApZcC zvHSl3d3zp}N%taAzNfx{#V*|YQuGu8O=)s00V(P!am6Pz%1B&b*B|jm;Xj8yFnDH7 z7sOUVc=BEgl;Ue?J3MXJk4}fZb^@J`N(Ovw{{Rl|Px>FP{{Zj_f5wXYCkNZ|qMn1) z{HZYlNXZ{l#YX#4wr;}XkUNTzryQs)m+79rjWR_f207c)B#Py^sNXSHYL(oloE+ks z$n7$uaLFvPFpemrLRv;A04VNBsF`*$u0rw0Ij>XwoxDllUj=xv_3s$mU9IJ>$*07& zjWkgbIgyCT-Od05j=8KVOO`idin~6N{hNLSFT+gUk8n;Dw1+&?}Th@U+og803F8yP5CGAs&gqv$0yyo)xAApx<;Hx9RS*n zj1L{iBB@{ATj}`nKNXL{ZvuU; z$&tK8sk>TfaIntC#$=d}?xOSPSfD*?;KV|afbR0{%RG{{=8=gcvmT_AlU--UpNW?r z7W`><;u%B3U_a6H%SjFz3w1v(M@`T7+pi}b0Ioo}RV9cA*l-1X{!xJT6#1mJXP;iD zGY8m8vvH1|!!*g55%*3qG3o|Cogl#nDaqty{{T!>F61)|;F0%+Km*g7`sl{U;bN~$ z1t1Szl*0a7oyUSY3RFvOcE+5-6wG1Qy92mCeA?vx9k0Ac(w)$frG{mU!^#V#0fii!0b=fk?_u+ zAS?mF2am@zgfWvUat42&y*XJ(`LcI+^rp(?q`@Q*Il$@7HG$d#WR~QTdsQ$*TS&m< zHqdzKQfFv6Bp!o4)ZDVEQUX77nuN1R%)>szG?>t=!BqbMcj?xUe)EtB9f7GhA&$^^ zAZL@Bi_8I;PXn$!KC}SoR5lM7{w#N<@W6w~^`_?p@q&2Z&^aKE{hFB3D_}xSat1Pe zYBf@MT%7hb9K1NkU(@lRNrmA3PIH=&9e@eQ&$n6$$l9YL*NSU$qa8^nBaUcNLS*#L zK_mS7)r@ISkKY2G%jRvv;DhV$OTksgB>moc(r-{Ubt8`5X#m#&3YOrWl-6yG7Z?Yg zDe?gL9Wlvc(xZ^?>&G~wK?reAZ{d%4I@Yy&N)8(8bB|bvxe*Zsx;&=;2ywrq%z}z zeqrg_kOtl}k_wDxflYxSxeFdhJ#cvKL_miJsq4o|jg^BCasmDxr|N0Q>$m_$dElOu zhS4N|mL<8!;~axRpd*sSdy~`gqzjLj;~s;xJ~ve*M>y=X2tyx~RgMA2=Sk)Qa!3U6 zoKj#F8;(z0`_KklWU}D)#R9Nks_s+%H7YsVf->KwK_MbQHlM`P(Z@YTcmjcsSuw%k zxhAh^daeG4soLsVZRD03jiiEGYnVdl6LXXmK*_-y2 zvH01ejXP8F^#1@ESjG+2pdcm6kNIsEvF^P`>t5=kKgz!xbZu`?@aC^$s_7Sxrs}pb z2`#SS;n0uf4?+(FabKlhvv2Jiru=m9_meuur+Bi)G4}mCJTU3!MV_&-?bS)a>`f=p zlGkI|z{Msg&P6CDwdhTQZ#22ZF!j$gxug~!x{7WnqU6~8p$Y_y%yXY?f&A#2@RA1J zNdN(oI@4V_1muu=o=3M7=4DOHc8p+>208jyBYh35Lh?4_wSgyq%D6o-^)&e3WO(Sq z3xGcHsIXmCmlaRnIv4+2WIEYeCVBNO>f;U-0IuT%=R)EeMbfqi6pB zTBVGsW&nZ!AoWq!qcar88C-GwYbOZHbXPM0rv+I0MR+Zi#!~ibY~|gRcp0N%>&o+1 z?z~H{UFmaa)A)y3n@*qoWrX^i&jaI-8a92TWDYTkspcU#b!>FOIX!=^M=4B@IRNkp z1lDg2P45&O^6oX<10t(7(U6$w_;FAo6vJe20N`^|XKr#xC!FK?R46dykO4ihf;lzU z2t{mW+hi>(sb*8OFgvm-LBJ#UPtA?J>Q^dI@-hcN4;38loCi4yGm+mN1!`xgtsw~P z-~rG8ed&fqXWDQud;b9Ssj|(vurYu+!2Ku$tBuD87{)LS7APc8ga%OUpS$b$(-I+q zTY^t>(v!_0vN=6Ea6Xj@%L2K>1Du)wvH{~Hk%5eVGf_#n;3vwy-(PBwH_9=BdUVA^ zPaQWC&j%FjA)A>A9GrU^dBIXJcLCqneJRrqEjR!Z!No+&A}$FSCwDy19R+R`cmo~B zJ*Y(^IpBJXb5WU5hpXoU9Moao_GYF!>6@6R%JntmAyTxHF2B}ag5|2 z=lWEEFip75NZ{wDDUPLsHb*@@I#P?1DuAa9IOuRiH4fv>dUKvRpqq_X@<1+qK4H>@ zX6Q~2L5hEuypTTf@+vtO05(S7ZfFEKLEp=8zrs73LdqBpa(M)k#ULS{f^&`sy%LSM zBL_Vxj7LAbJo3Ef6!(~CX~(heO$p9h*F7n+qOV?99tJy70p4?9j=34ny)GF+0FW5q z`&40v8C()_LG%>x_yxJi7{vfRiTQ!+oN>)Ck^$o}~7rk~6ae91)BPNJtn1+8)tXSR6710`X#bH>xswY(|ftzW@9 z{g%7o`@^Sd_D&>*aDG$jjz5V=;Ew#(WM||VJBb|+)|yC(gJ&K1q{=^3{sa7Q(f&Dj zK0QvzrLL=P1bUX3TQ6%SKU6r%w@<_nJu1Hi7wXjt{rP@_ z`q4#N*n&3u!|DDM%dhzPZ>XY*eE?#At3O(FfAY$TDCh$R`qTdKsL%X;AEgvffRf|y zulvTMUbInC1EcES@6z3LG*L|fQt$m@{b^zU09<~eiYc+8c|X+(sVChZS}3H-8xQ)S z{{RB0;{O0x-$6wbgn@tK;Qs)-Q%%-Cq5f1+K#>{$0GD5>rZ4<`ztH~xN+_yD7B0T( z{{XxFYLNTG{P&`YkS0&Mzl}J57yI;4KoS1{%R~55NzhS61fn=TD5>ZI{{RvC zQzZWYSf5%bqo5C$?l08S*QFFy0QCF6h^0Sp{b-_w1AC9HePRCq1o8g>l9}Tl`vt-O a0HL)MRKA6+&%Hl}l8P%*0*WZBAphA8pxku; diff --git a/test-arr b/test-arr deleted file mode 100644 index e69de29b..00000000