From bd79fb363e9871c67b4f1ff3cf5b1227504f75f9 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 13 Mar 2021 23:09:38 +0100 Subject: [PATCH 01/64] Hello vlx_manager in webots Vlx are now children of this vlx_manager of robot type. Why ? The fact that drive already uses "asterix" controller prevent another supervisor to get vlx data. To get around the problem, vlx_manager is a children of asterix and parent of all vlx that can be a supervisor. Vlx are still children of asterix but are now accessible from a ros supervisor. --- tools/simulation/protos/Asterix.proto | 33 ++------------ tools/simulation/protos/VlxManager.proto | 56 ++++++++++++++++++++++++ 2 files changed, 60 insertions(+), 29 deletions(-) create mode 100644 tools/simulation/protos/VlxManager.proto diff --git a/tools/simulation/protos/Asterix.proto b/tools/simulation/protos/Asterix.proto index 9edf73b8..c3a6256e 100644 --- a/tools/simulation/protos/Asterix.proto +++ b/tools/simulation/protos/Asterix.proto @@ -14,6 +14,7 @@ PROTO Asterix [ field SFBool supervisor TRUE # Is `Robot.supervisor`. field SFBool synchronization FALSE # Is `Robot.synchronization`. field MFNode arucoTag [ Aruco { } ] # Is `Robot.arucoTag`. + field MFNode vlx [ VlxManager { } ] # Is `Robot.vlx`. ] { Robot { @@ -138,35 +139,9 @@ PROTO Asterix [ translation 0 0 0.17 children IS arucoTag } - VL53L1X { - name "vlx_0x30" - translation 0.08 -0.13 -0.14 - rotation 0 0 1 -1.5708 - } - VL53L1X { - name "vlx_0x31" - translation 0.1 -0.11 -0.14 - rotation 0 0 1 0 - } - VL53L1X { - name "vlx_0x32" - translation 0.1 0.11 -0.14 - rotation 0 0 1 0 - } - VL53L1X { - name "vlx_0x33" - translation 0.08 0.13 -0.14 - rotation 0 0 1 1.5708 - } - VL53L1X { - name "vlx_0x34" - translation -0.1 0.11 -0.14 - rotation 0 0 1 -3.14159 - } - VL53L1X { - name "vlx_0x35" - translation -0.1 -0.11 -0.14 - rotation 0 0 1 -3.14159 + Transform { + translation 0 0 0.0 + children IS vlx } ] name "robotbase" diff --git a/tools/simulation/protos/VlxManager.proto b/tools/simulation/protos/VlxManager.proto new file mode 100644 index 00000000..489e8d24 --- /dev/null +++ b/tools/simulation/protos/VlxManager.proto @@ -0,0 +1,56 @@ +#VRML_SIM R2020b utf8 +PROTO VlxManager [ + field SFVec3f vlx0_tr 0.08 -0.1305 -0.14 + field SFRotation vlx0_rot 0 0 1 -1.5708 + field SFVec3f vlx1_tr 0.1005 -0.11 -0.14 + field SFRotation vlx1_rot 0 0 1 0 + field SFVec3f vlx2_tr 0.1005 0.11 -0.14 + field SFRotation vlx2_rot 0 0 1 0 + field SFVec3f vlx3_tr 0.08 0.1305 -0.14 + field SFRotation vlx3_rot 0 0 1 1.5708 + field SFVec3f vlx4_tr -0.1005 0.11 -0.14 + field SFRotation vlx4_rot 0 0 1 -3.14159 + field SFVec3f vlx5_tr -0.1005 -0.11 -0.14 + field SFRotation vlx5_rot 0 0 1 -3.14159 + field SFString name "vlx_manager" # Is `Solid.name`. +] +{ + Robot { + name IS name + controller "" + supervisor TRUE + synchronization FALSE + children [ + VL53L1X { + name "vlx_0x30" + translation IS vlx0_tr + rotation IS vlx0_rot + } + VL53L1X { + name "vlx_0x31" + translation IS vlx1_tr + rotation IS vlx1_rot + } + VL53L1X { + name "vlx_0x32" + translation IS vlx2_tr + rotation IS vlx2_rot + } + VL53L1X { + name "vlx_0x33" + translation IS vlx3_tr + rotation IS vlx3_rot + } + VL53L1X { + name "vlx_0x34" + translation IS vlx4_tr + rotation IS vlx4_rot + } + VL53L1X { + name "vlx_0x35" + translation IS vlx5_tr + rotation IS vlx5_rot + } + ] + } +} From af7f2c4e340c2b6b01bccfc80e6a28f475de4303 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 14 Mar 2021 23:55:49 +0100 Subject: [PATCH 02/64] Getting vlx values in localisation_node --- .../localisation/localisation_node.py | 2 ++ .../localisation/localisation/sensors_sim.py | 17 +++++++++++++++-- tools/simulation/protos/Asterix.proto | 2 +- tools/simulation/protos/VlxManager.proto | 2 +- 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 90174b23..37f1b380 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -13,6 +13,7 @@ from visualization_msgs.msg import MarkerArray from tf2_ros import StaticTransformBroadcaster from transformix_msgs.srv import TransformixParametersTransformStamped +from localisation.sensors_sim import Sensors class Localisation(rclpy.node.Node): @@ -45,6 +46,7 @@ def __init__(self): ) self.last_odom_update = 0 self.get_logger().info(f"Default side is {self.side.value}") + self.sensors = Sensors(self, [30, 31, 32, 33, 34, 35]) self.get_logger().info("Localisation node is ready") def _on_set_parameters(self, params): diff --git a/src/modules/localisation/localisation/sensors_sim.py b/src/modules/localisation/localisation/sensors_sim.py index bc5e7954..7a3a400c 100644 --- a/src/modules/localisation/localisation/sensors_sim.py +++ b/src/modules/localisation/localisation/sensors_sim.py @@ -3,15 +3,20 @@ """Simulated sensors definition for localisation.""" +from os import environ + +from webots_ros2_core.webots_node import WebotsNode class Sensors: def __init__(self, node, addrs=[]): """Init sensors baseclass.""" - self.node = node + supervisor = node.robot + "_vlx_manager" + environ["WEBOTS_ROBOT_NAME"] = supervisor + self.node = VlxSupervisor(supervisor, args=None) self._addrs = addrs self._vlx_array = [] for addr in addrs: - vlx = self.node.robot.getDistanceSensor(f"vlx_0x{addr:x}") + vlx = self.node.robot.getDevice(f"vlx_0x{addr}") try: vlx.enable(100) # Sampling rate of 100ms self._vlx_array.append(vlx) @@ -22,6 +27,7 @@ def get_distances(self): """Fetch distances from VL53L1Xs.""" distances = {} for vlx, addr in zip(self._vlx_array, self._addrs): + self.node.robot.step(1) distances.update({addr: round(vlx.getValue(), 4)}) return distances @@ -29,3 +35,10 @@ def __del__(self): """Destructor.""" for vlx in self._vlx_array: vlx.disable() + +class VlxSupervisor(WebotsNode): + """Vlx manager node.""" + + def __init__(self, supervisor, args=None): + """Init pharaon sim node.""" + super().__init__(supervisor, args) diff --git a/tools/simulation/protos/Asterix.proto b/tools/simulation/protos/Asterix.proto index c3a6256e..747a3b82 100644 --- a/tools/simulation/protos/Asterix.proto +++ b/tools/simulation/protos/Asterix.proto @@ -14,7 +14,7 @@ PROTO Asterix [ field SFBool supervisor TRUE # Is `Robot.supervisor`. field SFBool synchronization FALSE # Is `Robot.synchronization`. field MFNode arucoTag [ Aruco { } ] # Is `Robot.arucoTag`. - field MFNode vlx [ VlxManager { } ] # Is `Robot.vlx`. + field MFNode vlx [ VlxManager { name "asterix_vlx_manager" } ] # Is `Robot.vlx`. ] { Robot { diff --git a/tools/simulation/protos/VlxManager.proto b/tools/simulation/protos/VlxManager.proto index 489e8d24..e8ca897e 100644 --- a/tools/simulation/protos/VlxManager.proto +++ b/tools/simulation/protos/VlxManager.proto @@ -12,7 +12,7 @@ PROTO VlxManager [ field SFRotation vlx4_rot 0 0 1 -3.14159 field SFVec3f vlx5_tr -0.1005 -0.11 -0.14 field SFRotation vlx5_rot 0 0 1 -3.14159 - field SFString name "vlx_manager" # Is `Solid.name`. + field SFString name "vlx_manager" ] { Robot { From 8ec3faff51f2c9087bc574c03ea9a89a1f38d9c8 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 16 Mar 2021 23:00:36 +0100 Subject: [PATCH 03/64] a bit of localisation node refactoring --- .../localisation/localisation_node.py | 35 ++++++++++++------- .../localisation/localisation/sensors_sim.py | 2 ++ 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 37f1b380..5d5a1d39 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -24,7 +24,13 @@ def __init__(self): self.robot = self.get_namespace().strip("/") self.side = self.declare_parameter("side", "blue") self.add_on_set_parameters_callback(self._on_set_parameters) - self._x, self._y, self._theta = self.fetchStartPosition() + self.robot_pose = PoseStamped() + ( + self.robot_pose.pose.position.x, + self.robot_pose.pose.position.y, + theta, + ) = self.fetchStartPosition() + self.robot_pose.pose.orientation = self.euler_to_quaternion(theta) self._tf_brodcaster = StaticTransformBroadcaster(self) self._tf = TransformStamped() self._tf.header.frame_id = "map" @@ -59,7 +65,13 @@ def _on_set_parameters(self, params): self.side = param else: setattr(self, param.name, param) - self._x, self._y, self._theta = self.fetchStartPosition() + ( + self.robot_pose.pose.position.x, + self.robot_pose.pose.position.y, + theta, + ) = self.fetchStartPosition() + self.robot_pose.pose.orientation = self.euler_to_quaternion(theta) + self.update_transform() result.successful = True except BaseException as e: result.reason = e @@ -95,7 +107,7 @@ def euler_to_quaternion(self, yaw, pitch=0, roll=0): qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin( roll / 2 ) * np.sin(pitch / 2) * np.sin(yaw / 2) - return [qx, qy, qz, qw] + return Quaternion(x=qx, y=qy, z=qz, w=qw) def quaternion_to_euler(self, x, y, z, w): """Conversion between quaternions and euler angles.""" @@ -112,21 +124,18 @@ def quaternion_to_euler(self, x, y, z, w): return (X, Y, Z) def update_transform(self): - """Update and publish transform callback.""" + """Update and publish transform odom --> map.""" self._tf.header.stamp = self.get_clock().now().to_msg() - self._tf.transform.translation.x = float(self._x) - self._tf.transform.translation.y = float(self._y) - qx, qy, qz, qw = self.euler_to_quaternion(self._theta) - self._tf.transform.rotation.x = float(qx) - self._tf.transform.rotation.y = float(qy) - self._tf.transform.rotation.z = float(qz) - self._tf.transform.rotation.w = float(qw) - self._initial_tf = self._tf + self._tf.transform.translation.x = self.robot_pose.pose.position.x + self._tf.transform.translation.y = self.robot_pose.pose.position.y + self._tf.transform.rotation = self.robot_pose.pose.orientation + self._initial_tf = copy.deepcopy(self._tf) self._tf_brodcaster.sendTransform(self._tf) def allies_subscription_callback(self, msg): """Identity the robot marker in assurancetourix marker_array detection - publish the transformation for drive to readjust odometry""" + publish the transformation for drive to readjust odometry + compute base_link --> odom""" for allie_marker in msg.markers: if ( allie_marker.text.lower() == self.robot diff --git a/src/modules/localisation/localisation/sensors_sim.py b/src/modules/localisation/localisation/sensors_sim.py index 7a3a400c..d439d2c0 100644 --- a/src/modules/localisation/localisation/sensors_sim.py +++ b/src/modules/localisation/localisation/sensors_sim.py @@ -7,6 +7,7 @@ from webots_ros2_core.webots_node import WebotsNode + class Sensors: def __init__(self, node, addrs=[]): """Init sensors baseclass.""" @@ -36,6 +37,7 @@ def __del__(self): for vlx in self._vlx_array: vlx.disable() + class VlxSupervisor(WebotsNode): """Vlx manager node.""" From 722cb393e7fbe3714b356ca32d718ba459ab8e7c Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 16 Mar 2021 23:01:37 +0100 Subject: [PATCH 04/64] odom callback + tf to map using kdl --- .../localisation/localisation_node.py | 27 ++++++++++++++++--- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 5d5a1d39..6fa641e9 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -6,14 +6,17 @@ import math import numpy as np +import copy import rclpy -from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import TransformStamped, PoseStamped, Quaternion from rcl_interfaces.msg import SetParametersResult from visualization_msgs.msg import MarkerArray from tf2_ros import StaticTransformBroadcaster from transformix_msgs.srv import TransformixParametersTransformStamped from localisation.sensors_sim import Sensors +from nav_msgs.msg import Odometry +from tf2_kdl import * class Localisation(rclpy.node.Node): @@ -47,6 +50,12 @@ def __init__(self): self.allies_subscription_callback, 10, ) + self.subscription_ = self.create_subscription( + Odometry, + "odom", + self.odom_callback, + 10, + ) self.tf_publisher_ = self.create_publisher( TransformStamped, "adjust_odometry", 10 ) @@ -141,10 +150,7 @@ def allies_subscription_callback(self, msg): allie_marker.text.lower() == self.robot and (self.get_clock().now().to_msg().sec - self.last_odom_update) > 5 ): - self._x = allie_marker.pose.position.x - self._y = allie_marker.pose.position.y q = allie_marker.pose.orientation - self._theta = self.quaternion_to_euler(q.x, q.y, q.z, q.w)[2] self._tf.header.stamp = allie_marker.header.stamp self._tf.transform.translation.x = allie_marker.pose.position.x self._tf.transform.translation.y = allie_marker.pose.position.y @@ -158,6 +164,19 @@ def get_initial_tf_srv_callback(self, request, response): response.transform_stamped = self._initial_tf return response + def odom_callback(self, msg): + """Odom callback, compute the new pose of the robot relative to map""" + robot_tf = TransformStamped() + robot_tf.transform.translation.x = msg.pose.pose.position.x + robot_tf.transform.translation.y = msg.pose.pose.position.y + robot_tf.transform.rotation = msg.pose.pose.orientation + robot_frame = transform_to_kdl(robot_tf) + new_robot_pose_kdl = do_transform_frame(robot_frame, self._initial_tf) + self.robot_pose.pose.position.x = new_robot_pose_kdl.p.x() + self.robot_pose.pose.position.y = new_robot_pose_kdl.p.y() + q = new_robot_pose_kdl.M.GetQuaternion() + self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + def main(args=None): """Entrypoint.""" From e535ca187c24e5c34f2908f153a4b0ed0ace9c37 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 16 Mar 2021 23:49:54 +0100 Subject: [PATCH 05/64] localisation utils --- .../localisation/localisation_node.py | 35 ++-------------- .../localisation/localisation/utils.py | 40 +++++++++++++++++++ 2 files changed, 43 insertions(+), 32 deletions(-) create mode 100644 src/modules/localisation/localisation/utils.py diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 6fa641e9..62b3ec59 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -15,6 +15,7 @@ from tf2_ros import StaticTransformBroadcaster from transformix_msgs.srv import TransformixParametersTransformStamped from localisation.sensors_sim import Sensors +from localisation.utils import euler_to_quaternion from nav_msgs.msg import Odometry from tf2_kdl import * @@ -33,7 +34,7 @@ def __init__(self): self.robot_pose.pose.position.y, theta, ) = self.fetchStartPosition() - self.robot_pose.pose.orientation = self.euler_to_quaternion(theta) + self.robot_pose.pose.orientation = euler_to_quaternion(theta) self._tf_brodcaster = StaticTransformBroadcaster(self) self._tf = TransformStamped() self._tf.header.frame_id = "map" @@ -79,7 +80,7 @@ def _on_set_parameters(self, params): self.robot_pose.pose.position.y, theta, ) = self.fetchStartPosition() - self.robot_pose.pose.orientation = self.euler_to_quaternion(theta) + self.robot_pose.pose.orientation = euler_to_quaternion(theta) self.update_transform() result.successful = True except BaseException as e: @@ -102,36 +103,6 @@ def fetchStartPosition(self): # Make it crash in case of undefined parameters return None - def euler_to_quaternion(self, yaw, pitch=0, roll=0): - """Conversion between euler angles and quaternions.""" - qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos( - roll / 2 - ) * np.sin(pitch / 2) * np.sin(yaw / 2) - qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin( - roll / 2 - ) * np.cos(pitch / 2) * np.sin(yaw / 2) - qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin( - roll / 2 - ) * np.sin(pitch / 2) * np.cos(yaw / 2) - qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin( - roll / 2 - ) * np.sin(pitch / 2) * np.sin(yaw / 2) - return Quaternion(x=qx, y=qy, z=qz, w=qw) - - def quaternion_to_euler(self, x, y, z, w): - """Conversion between quaternions and euler angles.""" - t0 = +2.0 * (w * x + y * z) - t1 = +1.0 - 2.0 * (x * x + y * y) - X = math.atan2(t0, t1) - t2 = +2.0 * (w * y - z * x) - t2 = +1.0 if t2 > +1.0 else t2 - t2 = -1.0 if t2 < -1.0 else t2 - Y = math.asin(t2) - t3 = +2.0 * (w * z + x * y) - t4 = +1.0 - 2.0 * (y * y + z * z) - Z = math.atan2(t3, t4) - return (X, Y, Z) - def update_transform(self): """Update and publish transform odom --> map.""" self._tf.header.stamp = self.get_clock().now().to_msg() diff --git a/src/modules/localisation/localisation/utils.py b/src/modules/localisation/localisation/utils.py new file mode 100644 index 00000000..e74051fb --- /dev/null +++ b/src/modules/localisation/localisation/utils.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 + + +"""Some utils functions""" + +import numpy as np + +from geometry_msgs.msg import Quaternion + + +def euler_to_quaternion(yaw, pitch=0, roll=0): + """Conversion between euler angles and quaternions.""" + qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos( + roll / 2 + ) * np.sin(pitch / 2) * np.sin(yaw / 2) + qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin( + roll / 2 + ) * np.cos(pitch / 2) * np.sin(yaw / 2) + qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin( + roll / 2 + ) * np.sin(pitch / 2) * np.cos(yaw / 2) + qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin( + roll / 2 + ) * np.sin(pitch / 2) * np.sin(yaw / 2) + return Quaternion(x=qx, y=qy, z=qz, w=qw) + + +def quaternion_to_euler(x, y, z, w): + """Conversion between quaternions and euler angles.""" + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + X = math.atan2(t0, t1) + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + Y = math.asin(t2) + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + Z = math.atan2(t3, t4) + return (X, Y, Z) From fba43184f74736f200d0deaae9f0c7507f9c7848 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 16 Mar 2021 23:51:25 +0100 Subject: [PATCH 06/64] vlx_readjustement separate from localisation node --- .../localisation/localisation_node.py | 7 ++++--- .../localisation/vlx_readjustement.py | 20 +++++++++++++++++++ 2 files changed, 24 insertions(+), 3 deletions(-) create mode 100644 src/modules/localisation/localisation/vlx_readjustement.py diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 62b3ec59..c20c88e4 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -7,14 +7,14 @@ import math import numpy as np import copy - import rclpy + from geometry_msgs.msg import TransformStamped, PoseStamped, Quaternion from rcl_interfaces.msg import SetParametersResult from visualization_msgs.msg import MarkerArray from tf2_ros import StaticTransformBroadcaster from transformix_msgs.srv import TransformixParametersTransformStamped -from localisation.sensors_sim import Sensors +from localisation.vlx_readjustement import VlxReadjustement from localisation.utils import euler_to_quaternion from nav_msgs.msg import Odometry from tf2_kdl import * @@ -62,7 +62,7 @@ def __init__(self): ) self.last_odom_update = 0 self.get_logger().info(f"Default side is {self.side.value}") - self.sensors = Sensors(self, [30, 31, 32, 33, 34, 35]) + self.vlx = VlxReadjustement(self) self.get_logger().info("Localisation node is ready") def _on_set_parameters(self, params): @@ -146,6 +146,7 @@ def odom_callback(self, msg): self.robot_pose.pose.position.x = new_robot_pose_kdl.p.x() self.robot_pose.pose.position.y = new_robot_pose_kdl.p.y() q = new_robot_pose_kdl.M.GetQuaternion() + self.robot_pose.header.stamp = msg.header.stamp self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py new file mode 100644 index 00000000..f76cee1d --- /dev/null +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 + + +"""Vlx readjustement for localisation""" + +from localisation.sensors_sim import Sensors + + +class VlxReadjustement: + def __init__(self, parent_node): + self.parent = parent_node + self.sensors = Sensors(parent_node, [30, 31, 32, 33, 34, 35]) + self.parent.create_timer(1, self.testVlx) + self.vlx_readjustement = false + + def testVlx(self): + values = self.sensors.get_distances() + self.parent.get_logger().info( + f"{values[30]}, {values[31]}, {values[32]}, {values[33]}, {values[34]}, {values[35]}, {self.parent.robot_pose.pose.position.x}" + ) From c97c62ba205fee694c6b02d0002cbb07effeaca6 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Thu, 18 Mar 2021 23:29:29 +0100 Subject: [PATCH 07/64] Adding in_rectangle and modifying quaternion_to_euler in utils.py --- .../localisation/localisation/utils.py | 26 +++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/src/modules/localisation/localisation/utils.py b/src/modules/localisation/localisation/utils.py index e74051fb..929c560b 100644 --- a/src/modules/localisation/localisation/utils.py +++ b/src/modules/localisation/localisation/utils.py @@ -25,8 +25,15 @@ def euler_to_quaternion(yaw, pitch=0, roll=0): return Quaternion(x=qx, y=qy, z=qz, w=qw) -def quaternion_to_euler(x, y, z, w): - """Conversion between quaternions and euler angles.""" +def quaternion_to_euler(q): + """Conversion between quaternions and euler angles. + + :param q: The quaternion to transform to euler angles + :type q: geometry_msgs.msg.Quaternion + :return: euler angles equivalent + :rtype: tuple (x,y,z) + """ + x, y, z, w = q.x, q.y, q.z, q.w t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y * y) X = math.atan2(t0, t1) @@ -38,3 +45,18 @@ def quaternion_to_euler(x, y, z, w): t4 = +1.0 - 2.0 * (y * y + z * z) Z = math.atan2(t3, t4) return (X, Y, Z) + + +def in_rectangle(rect, pose): + """Return true if the coord are inside the rect coordinates + + :param rect: The list of rect coordinates. + :type rect: array [x_min, y_min, x_max_ y_max] + :param pose: the pose to test + :type coord: geometry_msgs.msg.PoseStamped + :return: true if the coord are inside the rect coordinates, else false + :rtype: bool + """ + x = pose.pose.position.x + y = pose.pose.position.y + return rect[0] < x and rect[1] < y and rect[2] > x and rect[3] > y From 174a1c52f04ace67ccc42a73c31009cf55213c19 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Thu, 18 Mar 2021 23:33:29 +0100 Subject: [PATCH 08/64] That's what i call a functional PoC of position compute from vlx --- .../localisation/vlx_readjustement.py | 48 ++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index f76cee1d..b2574631 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -3,7 +3,23 @@ """Vlx readjustement for localisation""" +import numpy as np + from localisation.sensors_sim import Sensors +from localisation.utils import in_rectangle + +bottom_blue = [0.0, 0.0, 0.9, 1.0] +top_blue = [0.0, 1.0, 1.5, 2.0] + +bottom_yellow = [2.1, 0.0, 3.0, 1.0] +top_yellow = [1.5, 1.0, 3.0, 2.0] + +vlx_0x30_pos = [80.0, -130.5] +vlx_0x31_pos = [100.5, -110.0] +vlx_0x32_pos = [100.5, 110.0] +vlx_0x33_pos = [80.0, 130.5] +vlx_0x34_pos = [-100.5, 110.0] +vlx_0x35_pos = [-100.5, -110.0] class VlxReadjustement: @@ -11,10 +27,40 @@ def __init__(self, parent_node): self.parent = parent_node self.sensors = Sensors(parent_node, [30, 31, 32, 33, 34, 35]) self.parent.create_timer(1, self.testVlx) - self.vlx_readjustement = false + self.vlx_readjustement = False def testVlx(self): + values = self.sensors.get_distances() + """ self.parent.get_logger().info( f"{values[30]}, {values[31]}, {values[32]}, {values[33]}, {values[34]}, {values[35]}, {self.parent.robot_pose.pose.position.x}" ) + """ + if in_rectangle(bottom_blue, self.parent.robot_pose): + self.parent.get_logger().info("bottom_blue") + elif in_rectangle(top_blue, self.parent.robot_pose): + self.parent.get_logger().info("top_blue") + x, y, theta = self.get_pose_from_vlx( + values[31], + values[32], + values[33], + vlx_0x31_pos, + vlx_0x32_pos, + vlx_0x33_pos, + ) + self.parent.get_logger().info( + f"x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" + ) + elif in_rectangle(bottom_yellow, self.parent.robot_pose): + self.parent.get_logger().info("bottom_yellow") + elif in_rectangle(top_yellow, self.parent.robot_pose): + self.parent.get_logger().info("top_yellow") + else: + self.parent.get_logger().info("None") + + def get_pose_from_vlx(self, d1, d2, d3, d1_pos, d2_pos, d3_pos): + theta = np.arctan((d2 - d1) / (2 * np.abs(d1_pos[1]))) + x = (d3 + d3_pos[1]) * np.cos(theta) + d3_pos[0] * np.sin(theta) + y = ((d1 + d2) / 2 + d1_pos[0]) * np.cos(theta) + return (x, y, theta) From 1277a87db7a9e08fb315fad3a4498e80098f5b34 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 20 Mar 2021 02:52:24 +0100 Subject: [PATCH 09/64] a bit more accurate model --- .../localisation/localisation/vlx_readjustement.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index b2574631..e6ca7e70 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -6,7 +6,7 @@ import numpy as np from localisation.sensors_sim import Sensors -from localisation.utils import in_rectangle +from localisation.utils import in_rectangle, quaternion_to_euler bottom_blue = [0.0, 0.0, 0.9, 1.0] top_blue = [0.0, 1.0, 1.5, 2.0] @@ -61,6 +61,9 @@ def testVlx(self): def get_pose_from_vlx(self, d1, d2, d3, d1_pos, d2_pos, d3_pos): theta = np.arctan((d2 - d1) / (2 * np.abs(d1_pos[1]))) - x = (d3 + d3_pos[1]) * np.cos(theta) + d3_pos[0] * np.sin(theta) - y = ((d1 + d2) / 2 + d1_pos[0]) * np.cos(theta) + if d3_pos == vlx_0x30_pos: + x = (d3 + np.abs(d3_pos[1])) * np.cos(theta) - d3_pos[0] * np.sin(theta) + else: + x = (d3 + np.abs(d3_pos[1])) * np.cos(theta) + d3_pos[0] * np.sin(theta) + y = ((d1 + d2) / 2 + np.abs(d1_pos[0])) * np.cos(theta) return (x, y, theta) From 7cc71973dd229977722bd79f8fcc31fa0db51d8d Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 20 Mar 2021 02:53:25 +0100 Subject: [PATCH 10/64] robot pose computing from vlx now works with all orientations --- .../localisation/vlx_readjustement.py | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index e6ca7e70..08407527 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -41,14 +41,22 @@ def testVlx(self): self.parent.get_logger().info("bottom_blue") elif in_rectangle(top_blue, self.parent.robot_pose): self.parent.get_logger().info("top_blue") - x, y, theta = self.get_pose_from_vlx( - values[31], - values[32], - values[33], - vlx_0x31_pos, - vlx_0x32_pos, - vlx_0x33_pos, - ) + pose_considered = self.parent.robot_pose.pose + angle = quaternion_to_euler(pose_considered.orientation)[2] + if angle > np.pi/4 and angle < 3*np.pi/4: + d1, d2, d3 = values[31], values[32], values[33] + d1_p, d2_p, d3_p = vlx_0x31_pos, vlx_0x32_pos, vlx_0x33_pos + elif angle < -np.pi/4 and angle > -3*np.pi/4: + d1, d2, d3 = values[34], values[35], values[30] + d1_p, d2_p, d3_p = vlx_0x34_pos, vlx_0x35_pos, vlx_0x30_pos + elif angle > -np.pi/4 and angle < np.pi/4: + d1, d2, d3 = values[34], values[35], values[33] + d1_p, d2_p, d3_p = vlx_0x34_pos, vlx_0x35_pos, vlx_0x33_pos + else: + d1, d2, d3 = values[31], values[32], values[30] + d1_p, d2_p, d3_p = vlx_0x31_pos, vlx_0x32_pos, vlx_0x30_pos + + x, y, theta = self.get_pose_from_vlx(d1, d2, d3, d1_p, d2_p, d3_p) self.parent.get_logger().info( f"x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" ) From a75243fabfc59b2c25df07ff11f275a86f5a9a8e Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 20 Mar 2021 02:54:43 +0100 Subject: [PATCH 11/64] Compute vlx distance from robot position (doesn't perfeclty works) --- .../localisation/localisation/vlx_readjustement.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 08407527..b4db8471 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -75,3 +75,13 @@ def get_pose_from_vlx(self, d1, d2, d3, d1_pos, d2_pos, d3_pos): x = (d3 + np.abs(d3_pos[1])) * np.cos(theta) + d3_pos[0] * np.sin(theta) y = ((d1 + d2) / 2 + np.abs(d1_pos[0])) * np.cos(theta) return (x, y, theta) + + def get_vlx_from_pose(self, robot_pose, d1_pos, d2_pos, d3_pos): + theta = np.pi - quaternion_to_euler(robot_pose.pose.orientation)[2] + self.parent.get_logger().info( + f"y:{2000 - robot_pose.pose.position.y*1000}, theta:{theta}, tan:{np.tan(theta)}" + ) + d1 = ( 2000 - robot_pose.pose.position.y*1000 ) / np.cos(theta) - d1_pos[1] * np.tan(theta) - d1_pos[0] + d2 = ( 2000 - robot_pose.pose.position.y*1000 ) / np.cos(theta) - d2_pos[1] * np.tan(theta) - d2_pos[0] + d3 = robot_pose.pose.position.x*1000 / np.cos(theta) + d3_pos[0] * np.tan(theta) - d3_pos[1] + return (d1, d2, d3) From 22ca662d5270815c2d419ea9a83a14e0f0715c9b Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 03:01:17 +0100 Subject: [PATCH 12/64] using fixed values instead of useless coordinates --- .../localisation/localisation/vlx_readjustement.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index b4db8471..7dbad91d 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -14,13 +14,8 @@ bottom_yellow = [2.1, 0.0, 3.0, 1.0] top_yellow = [1.5, 1.0, 3.0, 2.0] -vlx_0x30_pos = [80.0, -130.5] -vlx_0x31_pos = [100.5, -110.0] -vlx_0x32_pos = [100.5, 110.0] -vlx_0x33_pos = [80.0, 130.5] -vlx_0x34_pos = [-100.5, 110.0] -vlx_0x35_pos = [-100.5, -110.0] - +vlx_lat_x, vlx_lat_y = 80.0, 130.5 +vlx_face_x, vlx_face_y = 100.5, 110.0 class VlxReadjustement: def __init__(self, parent_node): From c69145e2242d60f32f4106f0489daa6fa7239100 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 03:01:40 +0100 Subject: [PATCH 13/64] get_pose_from_vlx adjusted --- .../localisation/localisation/vlx_readjustement.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 7dbad91d..63d2eae4 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -63,12 +63,12 @@ def testVlx(self): self.parent.get_logger().info("None") def get_pose_from_vlx(self, d1, d2, d3, d1_pos, d2_pos, d3_pos): - theta = np.arctan((d2 - d1) / (2 * np.abs(d1_pos[1]))) + theta = np.arctan((d2 - d1) / (2 * vlx_face_y)) if d3_pos == vlx_0x30_pos: - x = (d3 + np.abs(d3_pos[1])) * np.cos(theta) - d3_pos[0] * np.sin(theta) + x = (d3 + vlx_lat_y) * np.cos(theta) - vlx_lat_x * np.sin(theta) else: - x = (d3 + np.abs(d3_pos[1])) * np.cos(theta) + d3_pos[0] * np.sin(theta) - y = ((d1 + d2) / 2 + np.abs(d1_pos[0])) * np.cos(theta) + x = (d3 + vlx_lat_y) * np.cos(theta) + vlx_lat_x * np.sin(theta) + y = ((d1 + d2) / 2 + vlx_face_x ) * np.cos(theta) return (x, y, theta) def get_vlx_from_pose(self, robot_pose, d1_pos, d2_pos, d3_pos): From 96a9b692ed54c2f718bcb74e5d9661f063a30b19 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 03:02:20 +0100 Subject: [PATCH 14/64] accurate get_vlx__from_pose --- .../localisation/vlx_readjustement.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 63d2eae4..00b911c2 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -71,12 +71,11 @@ def get_pose_from_vlx(self, d1, d2, d3, d1_pos, d2_pos, d3_pos): y = ((d1 + d2) / 2 + vlx_face_x ) * np.cos(theta) return (x, y, theta) - def get_vlx_from_pose(self, robot_pose, d1_pos, d2_pos, d3_pos): - theta = np.pi - quaternion_to_euler(robot_pose.pose.orientation)[2] - self.parent.get_logger().info( - f"y:{2000 - robot_pose.pose.position.y*1000}, theta:{theta}, tan:{np.tan(theta)}" - ) - d1 = ( 2000 - robot_pose.pose.position.y*1000 ) / np.cos(theta) - d1_pos[1] * np.tan(theta) - d1_pos[0] - d2 = ( 2000 - robot_pose.pose.position.y*1000 ) / np.cos(theta) - d2_pos[1] * np.tan(theta) - d2_pos[0] - d3 = robot_pose.pose.position.x*1000 / np.cos(theta) + d3_pos[0] * np.tan(theta) - d3_pos[1] + def get_vlx_from_pose(self, robot_pose_wall_relative, d1_pos, d2_pos, d3_pos, theta): + d1 = ( robot_pose_wall_relative[1] ) / np.cos(theta) + vlx_face_y * np.tan(theta) - vlx_face_x + d2 = ( robot_pose_wall_relative[1] ) / np.cos(theta) - vlx_face_y * np.tan(theta) - vlx_face_x + if d3_pos == vlx_0x33_pos: + d3 = robot_pose_wall_relative[0] / np.cos(theta) + vlx_lat_x * np.tan(theta) - vlx_lat_y + else : + d3 = robot_pose_wall_relative[0] / np.cos(theta) - vlx_lat_x * np.tan(theta) - vlx_lat_y return (d1, d2, d3) From 7a7c68fd567a0cea5bd86a1fffca5a2f3299c57c Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 03:03:45 +0100 Subject: [PATCH 15/64] wip on the "top_blue" vlx algorithm --- .../localisation/localisation/utils.py | 1 + .../localisation/vlx_readjustement.py | 20 +++++++++++++++++++ 2 files changed, 21 insertions(+) diff --git a/src/modules/localisation/localisation/utils.py b/src/modules/localisation/localisation/utils.py index 929c560b..b8b8e635 100644 --- a/src/modules/localisation/localisation/utils.py +++ b/src/modules/localisation/localisation/utils.py @@ -4,6 +4,7 @@ """Some utils functions""" import numpy as np +import math from geometry_msgs.msg import Quaternion diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 00b911c2..e270f830 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -37,24 +37,44 @@ def testVlx(self): elif in_rectangle(top_blue, self.parent.robot_pose): self.parent.get_logger().info("top_blue") pose_considered = self.parent.robot_pose.pose + x, y = pose_considered.position.x*1000, 2000 - pose_considered.position.y*1000 angle = quaternion_to_euler(pose_considered.orientation)[2] if angle > np.pi/4 and angle < 3*np.pi/4: d1, d2, d3 = values[31], values[32], values[33] d1_p, d2_p, d3_p = vlx_0x31_pos, vlx_0x32_pos, vlx_0x33_pos + robot_pose_wall_relative = [x, y] + rectif_angle = np.pi/2 - angle elif angle < -np.pi/4 and angle > -3*np.pi/4: d1, d2, d3 = values[34], values[35], values[30] d1_p, d2_p, d3_p = vlx_0x34_pos, vlx_0x35_pos, vlx_0x30_pos + robot_pose_wall_relative = [x, y] + rectif_angle = -np.pi/2 - angle elif angle > -np.pi/4 and angle < np.pi/4: d1, d2, d3 = values[34], values[35], values[33] d1_p, d2_p, d3_p = vlx_0x34_pos, vlx_0x35_pos, vlx_0x33_pos + robot_pose_wall_relative = [y, x] + rectif_angle = 0 - angle else: d1, d2, d3 = values[31], values[32], values[30] d1_p, d2_p, d3_p = vlx_0x31_pos, vlx_0x32_pos, vlx_0x30_pos + robot_pose_wall_relative = [y, x] + rectif_angle = np.pi - angle x, y, theta = self.get_pose_from_vlx(d1, d2, d3, d1_p, d2_p, d3_p) self.parent.get_logger().info( f"x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" ) + + d1_est, d2_est, d3_est = self.get_vlx_from_pose(robot_pose_wall_relative, d1_p, d2_p, d3_p, rectif_angle) + self.parent.get_logger().info( + f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" + ) + """ + self.parent.get_logger().info( + f"true vlx d1:{d1}, d2:{d2}, d3:{d3}" + ) + """ + elif in_rectangle(bottom_yellow, self.parent.robot_pose): self.parent.get_logger().info("bottom_yellow") elif in_rectangle(top_yellow, self.parent.robot_pose): From 8aac2e3bedf66868d7484a8949d181937e62e5cc Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 03:05:25 +0100 Subject: [PATCH 16/64] black vlx_adjustement --- .../localisation/vlx_readjustement.py | 52 ++++++++++++++----- 1 file changed, 38 insertions(+), 14 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index e270f830..9711db2e 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -17,6 +17,7 @@ vlx_lat_x, vlx_lat_y = 80.0, 130.5 vlx_face_x, vlx_face_y = 100.5, 110.0 + class VlxReadjustement: def __init__(self, parent_node): self.parent = parent_node @@ -37,19 +38,22 @@ def testVlx(self): elif in_rectangle(top_blue, self.parent.robot_pose): self.parent.get_logger().info("top_blue") pose_considered = self.parent.robot_pose.pose - x, y = pose_considered.position.x*1000, 2000 - pose_considered.position.y*1000 + x, y = ( + pose_considered.position.x * 1000, + 2000 - pose_considered.position.y * 1000, + ) angle = quaternion_to_euler(pose_considered.orientation)[2] - if angle > np.pi/4 and angle < 3*np.pi/4: + if angle > np.pi / 4 and angle < 3 * np.pi / 4: d1, d2, d3 = values[31], values[32], values[33] d1_p, d2_p, d3_p = vlx_0x31_pos, vlx_0x32_pos, vlx_0x33_pos robot_pose_wall_relative = [x, y] - rectif_angle = np.pi/2 - angle - elif angle < -np.pi/4 and angle > -3*np.pi/4: + rectif_angle = np.pi / 2 - angle + elif angle < -np.pi / 4 and angle > -3 * np.pi / 4: d1, d2, d3 = values[34], values[35], values[30] d1_p, d2_p, d3_p = vlx_0x34_pos, vlx_0x35_pos, vlx_0x30_pos robot_pose_wall_relative = [x, y] - rectif_angle = -np.pi/2 - angle - elif angle > -np.pi/4 and angle < np.pi/4: + rectif_angle = -np.pi / 2 - angle + elif angle > -np.pi / 4 and angle < np.pi / 4: d1, d2, d3 = values[34], values[35], values[33] d1_p, d2_p, d3_p = vlx_0x34_pos, vlx_0x35_pos, vlx_0x33_pos robot_pose_wall_relative = [y, x] @@ -65,7 +69,9 @@ def testVlx(self): f"x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" ) - d1_est, d2_est, d3_est = self.get_vlx_from_pose(robot_pose_wall_relative, d1_p, d2_p, d3_p, rectif_angle) + d1_est, d2_est, d3_est = self.get_vlx_from_pose( + robot_pose_wall_relative, d1_p, d2_p, d3_p, rectif_angle + ) self.parent.get_logger().info( f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" ) @@ -88,14 +94,32 @@ def get_pose_from_vlx(self, d1, d2, d3, d1_pos, d2_pos, d3_pos): x = (d3 + vlx_lat_y) * np.cos(theta) - vlx_lat_x * np.sin(theta) else: x = (d3 + vlx_lat_y) * np.cos(theta) + vlx_lat_x * np.sin(theta) - y = ((d1 + d2) / 2 + vlx_face_x ) * np.cos(theta) + y = ((d1 + d2) / 2 + vlx_face_x) * np.cos(theta) return (x, y, theta) - def get_vlx_from_pose(self, robot_pose_wall_relative, d1_pos, d2_pos, d3_pos, theta): - d1 = ( robot_pose_wall_relative[1] ) / np.cos(theta) + vlx_face_y * np.tan(theta) - vlx_face_x - d2 = ( robot_pose_wall_relative[1] ) / np.cos(theta) - vlx_face_y * np.tan(theta) - vlx_face_x + def get_vlx_from_pose( + self, robot_pose_wall_relative, d1_pos, d2_pos, d3_pos, theta + ): + d1 = ( + (robot_pose_wall_relative[1]) / np.cos(theta) + + vlx_face_y * np.tan(theta) + - vlx_face_x + ) + d2 = ( + (robot_pose_wall_relative[1]) / np.cos(theta) + - vlx_face_y * np.tan(theta) + - vlx_face_x + ) if d3_pos == vlx_0x33_pos: - d3 = robot_pose_wall_relative[0] / np.cos(theta) + vlx_lat_x * np.tan(theta) - vlx_lat_y - else : - d3 = robot_pose_wall_relative[0] / np.cos(theta) - vlx_lat_x * np.tan(theta) - vlx_lat_y + d3 = ( + robot_pose_wall_relative[0] / np.cos(theta) + + vlx_lat_x * np.tan(theta) + - vlx_lat_y + ) + else: + d3 = ( + robot_pose_wall_relative[0] / np.cos(theta) + - vlx_lat_x * np.tan(theta) + - vlx_lat_y + ) return (d1, d2, d3) From 67645eaac35dc5a55a41f4bc17158a9e75176030 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 03:45:06 +0100 Subject: [PATCH 17/64] estimating true robot position and true vlx_distance --- .../localisation/vlx_readjustement.py | 50 ++++++++++++------- 1 file changed, 31 insertions(+), 19 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 9711db2e..7c6686a3 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -45,41 +45,55 @@ def testVlx(self): angle = quaternion_to_euler(pose_considered.orientation)[2] if angle > np.pi / 4 and angle < 3 * np.pi / 4: d1, d2, d3 = values[31], values[32], values[33] - d1_p, d2_p, d3_p = vlx_0x31_pos, vlx_0x32_pos, vlx_0x33_pos robot_pose_wall_relative = [x, y] rectif_angle = np.pi / 2 - angle + x_est = lambda x, y: x / 1000 + y_est = lambda x, y: (2000 - y) / 1000 + theta_est = lambda t: t + np.pi / 2 elif angle < -np.pi / 4 and angle > -3 * np.pi / 4: d1, d2, d3 = values[34], values[35], values[30] - d1_p, d2_p, d3_p = vlx_0x34_pos, vlx_0x35_pos, vlx_0x30_pos robot_pose_wall_relative = [x, y] rectif_angle = -np.pi / 2 - angle + x_est = lambda x, y: x / 1000 + y_est = lambda x, y: (2000 - y) / 1000 + theta_est = lambda t: t - np.pi / 2 elif angle > -np.pi / 4 and angle < np.pi / 4: d1, d2, d3 = values[34], values[35], values[33] - d1_p, d2_p, d3_p = vlx_0x34_pos, vlx_0x35_pos, vlx_0x33_pos robot_pose_wall_relative = [y, x] rectif_angle = 0 - angle + x_est = lambda x, y: y / 1000 + y_est = lambda x, y: (2000 - x) / 1000 + theta_est = lambda t: t else: d1, d2, d3 = values[31], values[32], values[30] - d1_p, d2_p, d3_p = vlx_0x31_pos, vlx_0x32_pos, vlx_0x30_pos robot_pose_wall_relative = [y, x] rectif_angle = np.pi - angle + x_est = lambda x, y: y / 1000 + y_est = lambda x, y: (2000 - x) / 1000 + theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) - x, y, theta = self.get_pose_from_vlx(d1, d2, d3, d1_p, d2_p, d3_p) + x, y, theta = self.get_pose_from_vlx( + d1, d2, d3, True if d3 == values[30] else False + ) self.parent.get_logger().info( - f"x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" + f"algo x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" + ) + self.parent.get_logger().info( + f"computed x:{x_est(x, y)}, y:{y_est(x, y)}, theta:{theta_est(theta)}" ) - d1_est, d2_est, d3_est = self.get_vlx_from_pose( - robot_pose_wall_relative, d1_p, d2_p, d3_p, rectif_angle + robot_pose_wall_relative, + rectif_angle, + True if d3 == values[30] else False, ) self.parent.get_logger().info( f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" ) - """ self.parent.get_logger().info( - f"true vlx d1:{d1}, d2:{d2}, d3:{d3}" + f"error computed pose - true pose x:{round(x_est(x, y)-pose_considered.position.x, 4)}, \ + y:{round(y_est(x, y)-pose_considered.position.y, 4)}, \ + theta:{round(theta_est(theta)-angle, 4)}" ) - """ elif in_rectangle(bottom_yellow, self.parent.robot_pose): self.parent.get_logger().info("bottom_yellow") @@ -88,18 +102,16 @@ def testVlx(self): else: self.parent.get_logger().info("None") - def get_pose_from_vlx(self, d1, d2, d3, d1_pos, d2_pos, d3_pos): + def get_pose_from_vlx(self, d1, d2, d3, vlx_0x30): theta = np.arctan((d2 - d1) / (2 * vlx_face_y)) - if d3_pos == vlx_0x30_pos: + if vlx_0x30: x = (d3 + vlx_lat_y) * np.cos(theta) - vlx_lat_x * np.sin(theta) else: x = (d3 + vlx_lat_y) * np.cos(theta) + vlx_lat_x * np.sin(theta) y = ((d1 + d2) / 2 + vlx_face_x) * np.cos(theta) return (x, y, theta) - def get_vlx_from_pose( - self, robot_pose_wall_relative, d1_pos, d2_pos, d3_pos, theta - ): + def get_vlx_from_pose(self, robot_pose_wall_relative, theta, vlx_0x30): d1 = ( (robot_pose_wall_relative[1]) / np.cos(theta) + vlx_face_y * np.tan(theta) @@ -110,16 +122,16 @@ def get_vlx_from_pose( - vlx_face_y * np.tan(theta) - vlx_face_x ) - if d3_pos == vlx_0x33_pos: + if vlx_0x30: d3 = ( robot_pose_wall_relative[0] / np.cos(theta) - + vlx_lat_x * np.tan(theta) + - vlx_lat_x * np.tan(theta) - vlx_lat_y ) else: d3 = ( robot_pose_wall_relative[0] / np.cos(theta) - - vlx_lat_x * np.tan(theta) + + vlx_lat_x * np.tan(theta) - vlx_lat_y ) return (d1, d2, d3) From 2f0ce1bf16ab3d8b9c24eeefb92125cb736d7419 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 20:08:00 +0100 Subject: [PATCH 18/64] fixing ugly overwritting local variable --- .../localisation/localisation/vlx_readjustement.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 7c6686a3..dc10017c 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -4,6 +4,7 @@ """Vlx readjustement for localisation""" import numpy as np +import copy from localisation.sensors_sim import Sensors from localisation.utils import in_rectangle, quaternion_to_euler @@ -37,36 +38,36 @@ def testVlx(self): self.parent.get_logger().info("bottom_blue") elif in_rectangle(top_blue, self.parent.robot_pose): self.parent.get_logger().info("top_blue") - pose_considered = self.parent.robot_pose.pose - x, y = ( + pose_considered = copy.deepcopy(self.parent.robot_pose.pose) + x_wall, y_wall = ( pose_considered.position.x * 1000, 2000 - pose_considered.position.y * 1000, ) angle = quaternion_to_euler(pose_considered.orientation)[2] if angle > np.pi / 4 and angle < 3 * np.pi / 4: d1, d2, d3 = values[31], values[32], values[33] - robot_pose_wall_relative = [x, y] + robot_pose_wall_relative = [x_wall, y_wall] rectif_angle = np.pi / 2 - angle x_est = lambda x, y: x / 1000 y_est = lambda x, y: (2000 - y) / 1000 theta_est = lambda t: t + np.pi / 2 elif angle < -np.pi / 4 and angle > -3 * np.pi / 4: d1, d2, d3 = values[34], values[35], values[30] - robot_pose_wall_relative = [x, y] + robot_pose_wall_relative = [x_wall, y_wall] rectif_angle = -np.pi / 2 - angle x_est = lambda x, y: x / 1000 y_est = lambda x, y: (2000 - y) / 1000 theta_est = lambda t: t - np.pi / 2 elif angle > -np.pi / 4 and angle < np.pi / 4: d1, d2, d3 = values[34], values[35], values[33] - robot_pose_wall_relative = [y, x] + robot_pose_wall_relative = [y_wall, x_wall] rectif_angle = 0 - angle x_est = lambda x, y: y / 1000 y_est = lambda x, y: (2000 - x) / 1000 theta_est = lambda t: t else: d1, d2, d3 = values[31], values[32], values[30] - robot_pose_wall_relative = [y, x] + robot_pose_wall_relative = [y_wall, x_wall] rectif_angle = np.pi - angle x_est = lambda x, y: y / 1000 y_est = lambda x, y: (2000 - x) / 1000 From a2a402aa9852523d33793874ecd4d7a6ff1553d7 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 20:09:54 +0100 Subject: [PATCH 19/64] showing the errors ~millimeters in simulation --- src/modules/localisation/localisation/vlx_readjustement.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index dc10017c..9eed9fc2 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -76,6 +76,7 @@ def testVlx(self): x, y, theta = self.get_pose_from_vlx( d1, d2, d3, True if d3 == values[30] else False ) + new_x, new_y, new_theta = x_est(x, y), y_est(x, y), theta_est(theta) self.parent.get_logger().info( f"algo x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" ) @@ -91,9 +92,9 @@ def testVlx(self): f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" ) self.parent.get_logger().info( - f"error computed pose - true pose x:{round(x_est(x, y)-pose_considered.position.x, 4)}, \ - y:{round(y_est(x, y)-pose_considered.position.y, 4)}, \ - theta:{round(theta_est(theta)-angle, 4)}" + f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ + y:{round(new_y-pose_considered.position.y, 4)}, \ + theta:{round(new_theta-angle, 4)}" ) elif in_rectangle(bottom_yellow, self.parent.robot_pose): From 212231b33242be1b925e78b68b6f8ca50755dd13 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 20:10:26 +0100 Subject: [PATCH 20/64] Not that simple projection function --- .../localisation/vlx_readjustement.py | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 9eed9fc2..310cecef 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -137,3 +137,35 @@ def get_vlx_from_pose(self, robot_pose_wall_relative, theta, vlx_0x30): - vlx_lat_y ) return (d1, d2, d3) + + def est_proj_wall( + self, d1_est, d2_est, d3_est, rectif_angle, robot_pose_wall_relative, case + ): + if case in [1, 2]: + considered_angle = rectif_angle + considered_vlx_face_y = vlx_face_y + else: + considered_angle = -rectif_angle + considered_vlx_face_y = -vlx_face_y + + if case in [2, 3]: + considered_vlx_lat_x = -vlx_lat_x + else: + considered_vlx_lat_x = vlx_lat_x + + d1_est_proj_wall = ( + robot_pose_wall_relative[0] + + (d1_est + vlx_face_x) * np.sin(considered_angle) + + considered_vlx_face_y * np.cos(considered_angle) + ) + d2_est_proj_wall = ( + robot_pose_wall_relative[0] + + (d2_est + vlx_face_x) * np.sin(considered_angle) + - considered_vlx_face_y * np.cos(considered_angle) + ) + d3_est_proj_wall = ( + robot_pose_wall_relative[1] + - (d3_est + vlx_lat_y) * np.sin(considered_angle) + - considered_vlx_lat_x * np.cos(considered_angle) + ) + return (d1_est_proj_wall, d2_est_proj_wall, d3_est_proj_wall) From 0e9e46f0b5039446463412646afb0c359ae1bfcd Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 21 Mar 2021 20:12:00 +0100 Subject: [PATCH 21/64] Yeah, projection function is working good enough --- .../localisation/vlx_readjustement.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 310cecef..8a773b1f 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -51,6 +51,7 @@ def testVlx(self): x_est = lambda x, y: x / 1000 y_est = lambda x, y: (2000 - y) / 1000 theta_est = lambda t: t + np.pi / 2 + case = 1 elif angle < -np.pi / 4 and angle > -3 * np.pi / 4: d1, d2, d3 = values[34], values[35], values[30] robot_pose_wall_relative = [x_wall, y_wall] @@ -58,6 +59,7 @@ def testVlx(self): x_est = lambda x, y: x / 1000 y_est = lambda x, y: (2000 - y) / 1000 theta_est = lambda t: t - np.pi / 2 + case = 2 elif angle > -np.pi / 4 and angle < np.pi / 4: d1, d2, d3 = values[34], values[35], values[33] robot_pose_wall_relative = [y_wall, x_wall] @@ -65,6 +67,7 @@ def testVlx(self): x_est = lambda x, y: y / 1000 y_est = lambda x, y: (2000 - x) / 1000 theta_est = lambda t: t + case = 3 else: d1, d2, d3 = values[31], values[32], values[30] robot_pose_wall_relative = [y_wall, x_wall] @@ -72,6 +75,7 @@ def testVlx(self): x_est = lambda x, y: y / 1000 y_est = lambda x, y: (2000 - x) / 1000 theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) + case = 4 x, y, theta = self.get_pose_from_vlx( d1, d2, d3, True if d3 == values[30] else False @@ -96,6 +100,17 @@ def testVlx(self): y:{round(new_y-pose_considered.position.y, 4)}, \ theta:{round(new_theta-angle, 4)}" ) + d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( + d1_est, + d2_est, + d3_est, + rectif_angle, + robot_pose_wall_relative, + case, + ) + self.parent.get_logger().info(f"test d1:{d1_proj_est}") + self.parent.get_logger().info(f"test d2:{d2_proj_est}") + self.parent.get_logger().info(f"test d3:{d3_proj_est}") elif in_rectangle(bottom_yellow, self.parent.robot_pose): self.parent.get_logger().info("bottom_yellow") From bddd5d27246b80cebc0d83903956c70427ff0e33 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 08:37:24 +0100 Subject: [PATCH 22/64] is_simulation utils + vlx in odom_callback --- src/modules/localisation/localisation/localisation_node.py | 2 ++ src/modules/localisation/localisation/utils.py | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index c20c88e4..a1e20048 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -149,6 +149,8 @@ def odom_callback(self, msg): self.robot_pose.header.stamp = msg.header.stamp self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + self.vlx.testVlx() + def main(args=None): """Entrypoint.""" diff --git a/src/modules/localisation/localisation/utils.py b/src/modules/localisation/localisation/utils.py index b8b8e635..834d4bbd 100644 --- a/src/modules/localisation/localisation/utils.py +++ b/src/modules/localisation/localisation/utils.py @@ -7,6 +7,7 @@ import math from geometry_msgs.msg import Quaternion +from platform import machine def euler_to_quaternion(yaw, pitch=0, roll=0): @@ -61,3 +62,6 @@ def in_rectangle(rect, pose): x = pose.pose.position.x y = pose.pose.position.y return rect[0] < x and rect[1] < y and rect[2] > x and rect[3] > y + +def is_simulation(): + return True if machine() != "aarch64" else False From 70dfd8bd7cc76deaf8a7f733374257ee551499a6 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 08:38:41 +0100 Subject: [PATCH 23/64] setting y_sim_offset if simulation --- .../localisation/localisation/vlx_readjustement.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 8a773b1f..d5d46732 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -7,7 +7,7 @@ import copy from localisation.sensors_sim import Sensors -from localisation.utils import in_rectangle, quaternion_to_euler +from localisation.utils import in_rectangle, quaternion_to_euler, is_simulation bottom_blue = [0.0, 0.0, 0.9, 1.0] top_blue = [0.0, 1.0, 1.5, 2.0] @@ -23,7 +23,11 @@ class VlxReadjustement: def __init__(self, parent_node): self.parent = parent_node self.sensors = Sensors(parent_node, [30, 31, 32, 33, 34, 35]) - self.parent.create_timer(1, self.testVlx) + if (is_simulation()): + self.y_sim_offset = 0.021 + else: + self.y_sim_offset = 0.0 + #self.parent.create_timer(1, self.testVlx) self.vlx_readjustement = False def testVlx(self): @@ -34,16 +38,17 @@ def testVlx(self): f"{values[30]}, {values[31]}, {values[32]}, {values[33]}, {values[34]}, {values[35]}, {self.parent.robot_pose.pose.position.x}" ) """ + pose_considered = copy.deepcopy(self.parent.robot_pose.pose) + angle = quaternion_to_euler(pose_considered.orientation)[2] + if in_rectangle(bottom_blue, self.parent.robot_pose): self.parent.get_logger().info("bottom_blue") elif in_rectangle(top_blue, self.parent.robot_pose): self.parent.get_logger().info("top_blue") - pose_considered = copy.deepcopy(self.parent.robot_pose.pose) x_wall, y_wall = ( pose_considered.position.x * 1000, 2000 - pose_considered.position.y * 1000, ) - angle = quaternion_to_euler(pose_considered.orientation)[2] if angle > np.pi / 4 and angle < 3 * np.pi / 4: d1, d2, d3 = values[31], values[32], values[33] robot_pose_wall_relative = [x_wall, y_wall] From 74d3558461d24dc7b56d63e8e69c64e48535b7da Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 08:39:51 +0100 Subject: [PATCH 24/64] can now invert projected lat_x and angle because of some geometrical considerations --- src/modules/localisation/localisation/vlx_readjustement.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index d5d46732..1fe65529 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -159,7 +159,7 @@ def get_vlx_from_pose(self, robot_pose_wall_relative, theta, vlx_0x30): return (d1, d2, d3) def est_proj_wall( - self, d1_est, d2_est, d3_est, rectif_angle, robot_pose_wall_relative, case + self, d1_est, d2_est, d3_est, rectif_angle, robot_pose_wall_relative, case, inv_angle=False, inv_lat = False ): if case in [1, 2]: considered_angle = rectif_angle @@ -172,6 +172,10 @@ def est_proj_wall( considered_vlx_lat_x = -vlx_lat_x else: considered_vlx_lat_x = vlx_lat_x + if inv_angle: + considered_angle = -considered_angle + if inv_lat: + considered_vlx_lat_x = -considered_vlx_lat_x d1_est_proj_wall = ( robot_pose_wall_relative[0] From 5bcca7b1a9ae3765e2b03305783165ca260940e0 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 08:41:02 +0100 Subject: [PATCH 25/64] Not that simple bottom blue consideration --- .../localisation/vlx_readjustement.py | 77 +++++++++++++++++++ 1 file changed, 77 insertions(+) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 1fe65529..dd143371 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -43,6 +43,83 @@ def testVlx(self): if in_rectangle(bottom_blue, self.parent.robot_pose): self.parent.get_logger().info("bottom_blue") + x_wall, y_wall = ( + pose_considered.position.x * 1000, + (pose_considered.position.y - self.y_sim_offset) * 1000, + ) + if angle > np.pi / 4 and angle < 3 * np.pi / 4: + d1, d2, d3 = values[34], values[35], values[33] + robot_pose_wall_relative = [x_wall, y_wall] + rectif_angle = np.pi / 2 - angle + x_est = lambda x, y: x / 1000 + y_est = lambda x, y: y / 1000 - self.y_sim_offset + theta_est = lambda t: t + np.pi / 2 + case = 1 + elif angle < -np.pi / 4 and angle > -3 * np.pi / 4: + d1, d2, d3 = values[31], values[32], values[30] + robot_pose_wall_relative = [x_wall, y_wall] + rectif_angle = - np.pi / 2 - angle + x_est = lambda x, y: x / 1000 + y_est = lambda x, y: y / 1000 - self.y_sim_offset + theta_est = lambda t: t - np.pi / 2 + case = 2 + elif angle > -np.pi / 4 and angle < np.pi / 4: + d1, d2, d3 = values[34], values[35], values[30] + robot_pose_wall_relative = [y_wall, x_wall] + rectif_angle = 0 - angle + x_est = lambda x, y: y / 1000 + y_est = lambda x, y: x / 1000 - self.y_sim_offset + theta_est = lambda t: t + case = 3 + else: + d1, d2, d3 = values[31], values[32], values[33] + robot_pose_wall_relative = [y_wall, x_wall] + rectif_angle = np.pi - angle + x_est = lambda x, y: y / 1000 + y_est = lambda x, y: x / 1000 - self.y_sim_offset + theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) + case = 4 + + x, y, theta = self.get_pose_from_vlx( + d1, d2, d3, True if d3 == values[30] else False + ) + new_x, new_y, new_theta = x_est(x, y), y_est(x, y), theta_est(theta) + """ + self.parent.get_logger().info( + f"algo x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" + ) + self.parent.get_logger().info( + f"computed x:{x_est(x, y)}, y:{y_est(x, y)}, theta:{theta_est(theta)}" + ) + """ + d1_est, d2_est, d3_est = self.get_vlx_from_pose( + robot_pose_wall_relative, + rectif_angle, + True if d3 == values[30] else False, + ) + self.parent.get_logger().info( + f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" + ) + self.parent.get_logger().info( + f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ + y:{round(new_y-pose_considered.position.y, 4)}, \ + theta:{round(new_theta-angle, 4)}" + ) + d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( + d1_est, + d2_est, + d3_est, + rectif_angle, + robot_pose_wall_relative, + case, + True, + True if case in [1,2] else False, + ) + + self.parent.get_logger().info(f"test d1:{d1_proj_est}") + self.parent.get_logger().info(f"test d2:{d2_proj_est}") + self.parent.get_logger().info(f"test d3:{d3_proj_est}") + elif in_rectangle(top_blue, self.parent.robot_pose): self.parent.get_logger().info("top_blue") x_wall, y_wall = ( From 05ad8dd81d1248909883806740c5f51b671e338b Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 18:04:50 +0100 Subject: [PATCH 26/64] simulation offset isn't only on Y --- .../localisation/localisation/utils.py | 1 + .../localisation/vlx_readjustement.py | 22 +++++++++---------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/localisation/localisation/utils.py b/src/modules/localisation/localisation/utils.py index 834d4bbd..b1fc4ddb 100644 --- a/src/modules/localisation/localisation/utils.py +++ b/src/modules/localisation/localisation/utils.py @@ -63,5 +63,6 @@ def in_rectangle(rect, pose): y = pose.pose.position.y return rect[0] < x and rect[1] < y and rect[2] > x and rect[3] > y + def is_simulation(): return True if machine() != "aarch64" else False diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index dd143371..d95203d8 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -23,11 +23,11 @@ class VlxReadjustement: def __init__(self, parent_node): self.parent = parent_node self.sensors = Sensors(parent_node, [30, 31, 32, 33, 34, 35]) - if (is_simulation()): - self.y_sim_offset = 0.021 + if is_simulation(): + self.sim_offset = 0.0215 else: - self.y_sim_offset = 0.0 - #self.parent.create_timer(1, self.testVlx) + self.sim_offset = 0.0 + # self.parent.create_timer(1, self.testVlx) self.vlx_readjustement = False def testVlx(self): @@ -45,22 +45,22 @@ def testVlx(self): self.parent.get_logger().info("bottom_blue") x_wall, y_wall = ( pose_considered.position.x * 1000, - (pose_considered.position.y - self.y_sim_offset) * 1000, + (pose_considered.position.y - self.sim_offset) * 1000, ) if angle > np.pi / 4 and angle < 3 * np.pi / 4: d1, d2, d3 = values[34], values[35], values[33] robot_pose_wall_relative = [x_wall, y_wall] rectif_angle = np.pi / 2 - angle x_est = lambda x, y: x / 1000 - y_est = lambda x, y: y / 1000 - self.y_sim_offset + y_est = lambda x, y: y / 1000 - self.sim_offset theta_est = lambda t: t + np.pi / 2 case = 1 elif angle < -np.pi / 4 and angle > -3 * np.pi / 4: d1, d2, d3 = values[31], values[32], values[30] robot_pose_wall_relative = [x_wall, y_wall] - rectif_angle = - np.pi / 2 - angle + rectif_angle = -np.pi / 2 - angle x_est = lambda x, y: x / 1000 - y_est = lambda x, y: y / 1000 - self.y_sim_offset + y_est = lambda x, y: y / 1000 - self.sim_offset theta_est = lambda t: t - np.pi / 2 case = 2 elif angle > -np.pi / 4 and angle < np.pi / 4: @@ -68,7 +68,7 @@ def testVlx(self): robot_pose_wall_relative = [y_wall, x_wall] rectif_angle = 0 - angle x_est = lambda x, y: y / 1000 - y_est = lambda x, y: x / 1000 - self.y_sim_offset + y_est = lambda x, y: x / 1000 - self.sim_offset theta_est = lambda t: t case = 3 else: @@ -76,7 +76,7 @@ def testVlx(self): robot_pose_wall_relative = [y_wall, x_wall] rectif_angle = np.pi - angle x_est = lambda x, y: y / 1000 - y_est = lambda x, y: x / 1000 - self.y_sim_offset + y_est = lambda x, y: x / 1000 - self.sim_offset theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) case = 4 @@ -113,7 +113,7 @@ def testVlx(self): robot_pose_wall_relative, case, True, - True if case in [1,2] else False, + True if case in [1, 2] else False, ) self.parent.get_logger().info(f"test d1:{d1_proj_est}") From 445881666ff60dcaf628b062050fc6d625ad1326 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 18:07:16 +0100 Subject: [PATCH 27/64] Not so simple top_yellow --- .../localisation/vlx_readjustement.py | 87 ++++++++++++++++++- 1 file changed, 86 insertions(+), 1 deletion(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index d95203d8..795a2757 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -198,6 +198,83 @@ def testVlx(self): self.parent.get_logger().info("bottom_yellow") elif in_rectangle(top_yellow, self.parent.robot_pose): self.parent.get_logger().info("top_yellow") + x_wall, y_wall = ( + 3000 - (pose_considered.position.x - self.sim_offset) * 1000, + 2000 - pose_considered.position.y * 1000, + ) + if angle > np.pi / 4 and angle < 3 * np.pi / 4: + d1, d2, d3 = values[31], values[32], values[30] + robot_pose_wall_relative = [x_wall, y_wall] + rectif_angle = np.pi / 2 - angle + x_est = lambda x, y: (3000 - x) / 1000 + self.sim_offset + y_est = lambda x, y: (2000 - y) / 1000 + theta_est = lambda t: t + np.pi / 2 + case = 1 + elif angle < -np.pi / 4 and angle > -3 * np.pi / 4: + d1, d2, d3 = values[34], values[35], values[33] + robot_pose_wall_relative = [x_wall, y_wall] + rectif_angle = -np.pi / 2 - angle + x_est = lambda x, y: (3000 - x) / 1000 + self.sim_offset + y_est = lambda x, y: (2000 - y) / 1000 + theta_est = lambda t: t - np.pi / 2 + case = 2 + elif angle > -np.pi / 4 and angle < np.pi / 4: + d1, d2, d3 = values[31], values[32], values[33] + robot_pose_wall_relative = [y_wall, x_wall] + rectif_angle = 0 - angle + x_est = lambda x, y: (3000 - y) / 1000 + self.sim_offset + y_est = lambda x, y: (2000 - x) / 1000 + theta_est = lambda t: t + case = 3 + else: + d1, d2, d3 = values[34], values[35], values[30] + robot_pose_wall_relative = [y_wall, x_wall] + rectif_angle = np.pi - angle + x_est = lambda x, y: (3000 - y) / 1000 + self.sim_offset + y_est = lambda x, y: (2000 - x) / 1000 + theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) + case = 4 + + x, y, theta = self.get_pose_from_vlx( + d1, d2, d3, True if d3 == values[30] else False + ) + new_x, new_y, new_theta = x_est(x, y), y_est(x, y), theta_est(theta) + """ + self.parent.get_logger().info( + f"algo x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" + ) + self.parent.get_logger().info( + f"computed x:{x_est(x, y)}, y:{y_est(x, y)}, theta:{theta_est(theta)}" + ) + """ + d1_est, d2_est, d3_est = self.get_vlx_from_pose( + robot_pose_wall_relative, + rectif_angle, + True if d3 == values[30] else False, + ) + self.parent.get_logger().info( + f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" + ) + self.parent.get_logger().info( + f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ + y:{round(new_y-pose_considered.position.y, 4)}, \ + theta:{round(new_theta-angle, 4)}" + ) + d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( + d1_est, + d2_est, + d3_est, + rectif_angle, + robot_pose_wall_relative, + case, + True, + True if case in [3, 4] else False, + ) + + self.parent.get_logger().info(f"test d1:{d1_proj_est}") + self.parent.get_logger().info(f"test d2:{d2_proj_est}") + self.parent.get_logger().info(f"test d3:{d3_proj_est}") + else: self.parent.get_logger().info("None") @@ -236,7 +313,15 @@ def get_vlx_from_pose(self, robot_pose_wall_relative, theta, vlx_0x30): return (d1, d2, d3) def est_proj_wall( - self, d1_est, d2_est, d3_est, rectif_angle, robot_pose_wall_relative, case, inv_angle=False, inv_lat = False + self, + d1_est, + d2_est, + d3_est, + rectif_angle, + robot_pose_wall_relative, + case, + inv_angle=False, + inv_lat=False, ): if case in [1, 2]: considered_angle = rectif_angle From 4c72c442a281c629bc691448b4273c07cd297fd2 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 18:44:09 +0100 Subject: [PATCH 28/64] Not that simple bottom_yellow --- .../localisation/vlx_readjustement.py | 78 +++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 795a2757..aecc9a9e 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -196,6 +196,84 @@ def testVlx(self): elif in_rectangle(bottom_yellow, self.parent.robot_pose): self.parent.get_logger().info("bottom_yellow") + x_wall, y_wall = ( + 3000 - (pose_considered.position.x - self.sim_offset) * 1000, + (pose_considered.position.y - self.sim_offset) * 1000, + ) + if angle > np.pi / 4 and angle < 3 * np.pi / 4: + d1, d2, d3 = values[34], values[35], values[30] + robot_pose_wall_relative = [x_wall, y_wall] + rectif_angle = np.pi / 2 - angle + x_est = lambda x, y: (3000 - x) / 1000 + self.sim_offset + y_est = lambda x, y: y / 1000 - self.sim_offset + theta_est = lambda t: t + np.pi / 2 + case = 1 + elif angle < -np.pi / 4 and angle > -3 * np.pi / 4: + d1, d2, d3 = values[31], values[32], values[33] + robot_pose_wall_relative = [x_wall, y_wall] + rectif_angle = -np.pi / 2 - angle + x_est = lambda x, y: (3000 - x) / 1000 + self.sim_offset + y_est = lambda x, y: y / 1000 - self.sim_offset + theta_est = lambda t: t - np.pi / 2 + case = 2 + elif angle > -np.pi / 4 and angle < np.pi / 4: + d1, d2, d3 = values[31], values[32], values[30] + robot_pose_wall_relative = [y_wall, x_wall] + rectif_angle = 0 - angle + x_est = lambda x, y: (3000 - y) / 1000 + self.sim_offset + y_est = lambda x, y: x / 1000 - self.sim_offset + theta_est = lambda t: t + case = 3 + else: + d1, d2, d3 = values[34], values[35], values[33] + robot_pose_wall_relative = [y_wall, x_wall] + rectif_angle = np.pi - angle + x_est = lambda x, y: (3000 - y) / 1000 + self.sim_offset + y_est = lambda x, y: x / 1000 - self.sim_offset + theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) + case = 4 + + x, y, theta = self.get_pose_from_vlx( + d1, d2, d3, True if d3 == values[30] else False + ) + new_x, new_y, new_theta = x_est(x, y), y_est(x, y), theta_est(theta) + """ + self.parent.get_logger().info( + f"algo x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" + ) + self.parent.get_logger().info( + f"computed x:{x_est(x, y)}, y:{y_est(x, y)}, theta:{theta_est(theta)}" + ) + """ + d1_est, d2_est, d3_est = self.get_vlx_from_pose( + robot_pose_wall_relative, + rectif_angle, + True if d3 == values[30] else False, + ) + self.parent.get_logger().info( + f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" + ) + self.parent.get_logger().info( + f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ + y:{round(new_y-pose_considered.position.y, 4)}, \ + theta:{round(new_theta-angle, 4)}" + ) + d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( + d1_est, + d2_est, + d3_est, + rectif_angle, + robot_pose_wall_relative, + case, + False, + True, + ) + + self.parent.get_logger().info(f"test d1:{d1_proj_est}") + self.parent.get_logger().info(f"test d2:{d2_proj_est}") + self.parent.get_logger().info(f"test d3:{d3_proj_est}") + + elif in_rectangle(top_yellow, self.parent.robot_pose): self.parent.get_logger().info("top_yellow") x_wall, y_wall = ( From 040dffc0cf98be0712d33ff85d4d93abb2cfdaf2 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 22:30:49 +0100 Subject: [PATCH 29/64] vlx position on robot "yamlized" + rename area --- .../localisation/vlx_readjustement.py | 95 ++++++++----------- 1 file changed, 40 insertions(+), 55 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index aecc9a9e..66735fcc 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -9,14 +9,11 @@ from localisation.sensors_sim import Sensors from localisation.utils import in_rectangle, quaternion_to_euler, is_simulation -bottom_blue = [0.0, 0.0, 0.9, 1.0] -top_blue = [0.0, 1.0, 1.5, 2.0] +bottom_blue_area = [0.0, 0.0, 0.9, 1.0] +top_blue_area = [0.0, 1.0, 1.5, 2.0] -bottom_yellow = [2.1, 0.0, 3.0, 1.0] -top_yellow = [1.5, 1.0, 3.0, 2.0] - -vlx_lat_x, vlx_lat_y = 80.0, 130.5 -vlx_face_x, vlx_face_y = 100.5, 110.0 +bottom_yellow_area = [2.1, 0.0, 3.0, 1.0] +top_yellow_area = [1.5, 1.0, 3.0, 2.0] class VlxReadjustement: @@ -28,21 +25,24 @@ def __init__(self, parent_node): else: self.sim_offset = 0.0 # self.parent.create_timer(1, self.testVlx) + self.parent.declare_parameter("vlx_lat_x", 0.0) + self.parent.declare_parameter("vlx_lat_y", 0.0) + self.parent.declare_parameter("vlx_face_x", 0.0) + self.parent.declare_parameter("vlx_face_y", 0.0) + self.vlx_lat_x = self.parent.get_parameter("vlx_lat_x")._value + self.vlx_lat_y = self.parent.get_parameter("vlx_lat_y")._value + self.vlx_face_x = self.parent.get_parameter("vlx_face_x")._value + self.vlx_face_y = self.parent.get_parameter("vlx_face_y")._value self.vlx_readjustement = False def testVlx(self): values = self.sensors.get_distances() - """ - self.parent.get_logger().info( - f"{values[30]}, {values[31]}, {values[32]}, {values[33]}, {values[34]}, {values[35]}, {self.parent.robot_pose.pose.position.x}" - ) - """ pose_considered = copy.deepcopy(self.parent.robot_pose.pose) angle = quaternion_to_euler(pose_considered.orientation)[2] - if in_rectangle(bottom_blue, self.parent.robot_pose): - self.parent.get_logger().info("bottom_blue") + if in_rectangle(bottom_blue_area, self.parent.robot_pose): + self.parent.get_logger().info("bottom_blue_area") x_wall, y_wall = ( pose_considered.position.x * 1000, (pose_considered.position.y - self.sim_offset) * 1000, @@ -80,31 +80,16 @@ def testVlx(self): theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) case = 4 + x, y, theta = self.get_pose_from_vlx( d1, d2, d3, True if d3 == values[30] else False ) new_x, new_y, new_theta = x_est(x, y), y_est(x, y), theta_est(theta) - """ - self.parent.get_logger().info( - f"algo x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" - ) - self.parent.get_logger().info( - f"computed x:{x_est(x, y)}, y:{y_est(x, y)}, theta:{theta_est(theta)}" - ) - """ d1_est, d2_est, d3_est = self.get_vlx_from_pose( robot_pose_wall_relative, rectif_angle, True if d3 == values[30] else False, ) - self.parent.get_logger().info( - f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" - ) - self.parent.get_logger().info( - f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ - y:{round(new_y-pose_considered.position.y, 4)}, \ - theta:{round(new_theta-angle, 4)}" - ) d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( d1_est, d2_est, @@ -120,8 +105,8 @@ def testVlx(self): self.parent.get_logger().info(f"test d2:{d2_proj_est}") self.parent.get_logger().info(f"test d3:{d3_proj_est}") - elif in_rectangle(top_blue, self.parent.robot_pose): - self.parent.get_logger().info("top_blue") + elif in_rectangle(top_blue_area, self.parent.robot_pose): + self.parent.get_logger().info("top_blue_area") x_wall, y_wall = ( pose_considered.position.x * 1000, 2000 - pose_considered.position.y * 1000, @@ -194,8 +179,8 @@ def testVlx(self): self.parent.get_logger().info(f"test d2:{d2_proj_est}") self.parent.get_logger().info(f"test d3:{d3_proj_est}") - elif in_rectangle(bottom_yellow, self.parent.robot_pose): - self.parent.get_logger().info("bottom_yellow") + elif in_rectangle(bottom_yellow_area, self.parent.robot_pose): + self.parent.get_logger().info("bottom_yellow_area") x_wall, y_wall = ( 3000 - (pose_considered.position.x - self.sim_offset) * 1000, (pose_considered.position.y - self.sim_offset) * 1000, @@ -274,8 +259,8 @@ def testVlx(self): self.parent.get_logger().info(f"test d3:{d3_proj_est}") - elif in_rectangle(top_yellow, self.parent.robot_pose): - self.parent.get_logger().info("top_yellow") + elif in_rectangle(top_yellow_area, self.parent.robot_pose): + self.parent.get_logger().info("top_yellow_area") x_wall, y_wall = ( 3000 - (pose_considered.position.x - self.sim_offset) * 1000, 2000 - pose_considered.position.y * 1000, @@ -357,36 +342,36 @@ def testVlx(self): self.parent.get_logger().info("None") def get_pose_from_vlx(self, d1, d2, d3, vlx_0x30): - theta = np.arctan((d2 - d1) / (2 * vlx_face_y)) + theta = np.arctan((d2 - d1) / (2 * self.vlx_face_y)) if vlx_0x30: - x = (d3 + vlx_lat_y) * np.cos(theta) - vlx_lat_x * np.sin(theta) + x = (d3 + self.vlx_lat_y) * np.cos(theta) - self.vlx_lat_x * np.sin(theta) else: - x = (d3 + vlx_lat_y) * np.cos(theta) + vlx_lat_x * np.sin(theta) - y = ((d1 + d2) / 2 + vlx_face_x) * np.cos(theta) + x = (d3 + self.vlx_lat_y) * np.cos(theta) + self.vlx_lat_x * np.sin(theta) + y = ((d1 + d2) / 2 + self.vlx_face_x) * np.cos(theta) return (x, y, theta) def get_vlx_from_pose(self, robot_pose_wall_relative, theta, vlx_0x30): d1 = ( (robot_pose_wall_relative[1]) / np.cos(theta) - + vlx_face_y * np.tan(theta) - - vlx_face_x + + self.vlx_face_y * np.tan(theta) + - self.vlx_face_x ) d2 = ( (robot_pose_wall_relative[1]) / np.cos(theta) - - vlx_face_y * np.tan(theta) - - vlx_face_x + - self.vlx_face_y * np.tan(theta) + - self.vlx_face_x ) if vlx_0x30: d3 = ( robot_pose_wall_relative[0] / np.cos(theta) - - vlx_lat_x * np.tan(theta) - - vlx_lat_y + - self.vlx_lat_x * np.tan(theta) + - self.vlx_lat_y ) else: d3 = ( robot_pose_wall_relative[0] / np.cos(theta) - + vlx_lat_x * np.tan(theta) - - vlx_lat_y + + self.vlx_lat_x * np.tan(theta) + - self.vlx_lat_y ) return (d1, d2, d3) @@ -403,15 +388,15 @@ def est_proj_wall( ): if case in [1, 2]: considered_angle = rectif_angle - considered_vlx_face_y = vlx_face_y + considered_vlx_face_y = self.vlx_face_y else: considered_angle = -rectif_angle - considered_vlx_face_y = -vlx_face_y + considered_vlx_face_y = -self.vlx_face_y if case in [2, 3]: - considered_vlx_lat_x = -vlx_lat_x + considered_vlx_lat_x = -self.vlx_lat_x else: - considered_vlx_lat_x = vlx_lat_x + considered_vlx_lat_x = self.vlx_lat_x if inv_angle: considered_angle = -considered_angle if inv_lat: @@ -419,17 +404,17 @@ def est_proj_wall( d1_est_proj_wall = ( robot_pose_wall_relative[0] - + (d1_est + vlx_face_x) * np.sin(considered_angle) + + (d1_est + self.vlx_face_x) * np.sin(considered_angle) + considered_vlx_face_y * np.cos(considered_angle) ) d2_est_proj_wall = ( robot_pose_wall_relative[0] - + (d2_est + vlx_face_x) * np.sin(considered_angle) + + (d2_est + self.vlx_face_x) * np.sin(considered_angle) - considered_vlx_face_y * np.cos(considered_angle) ) d3_est_proj_wall = ( robot_pose_wall_relative[1] - - (d3_est + vlx_lat_y) * np.sin(considered_angle) + - (d3_est + self.vlx_lat_y) * np.sin(considered_angle) - considered_vlx_lat_x * np.cos(considered_angle) ) return (d1_est_proj_wall, d2_est_proj_wall, d3_est_proj_wall) From bec61f09f96c1f8c6d43c08fac9275c2928dbf6e Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 22:31:47 +0100 Subject: [PATCH 30/64] vlx_position in asterix.in yaml --- src/asterix/param/asterix.in.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/asterix/param/asterix.in.yml b/src/asterix/param/asterix.in.yml index 81175d37..c9f09644 100644 --- a/src/asterix/param/asterix.in.yml +++ b/src/asterix/param/asterix.in.yml @@ -49,6 +49,14 @@ asterix: strategy_mode: NORMAL + localisation_node: + ros__parameters: + vlx_lat_x: 80.0 + vlx_lat_y: 130.5 + vlx_face_x: 100.5 + vlx_face_y: 110.0 + + lcd_driver: ros__parameters: use_sim_time: !Var use_sim_time From bcc62a0ed9fc2a97b57266d292f743c47fc411c3 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 23:25:23 +0100 Subject: [PATCH 31/64] Math done, begining of the reformatting of vlx_readjustement --- .../localisation/vlx_readjustement.py | 163 +++--------------- 1 file changed, 25 insertions(+), 138 deletions(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 66735fcc..ebcefc0f 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -37,8 +37,9 @@ def __init__(self, parent_node): def testVlx(self): + def fetch_data(self, pose_considered): + values = self.sensors.get_distances() - pose_considered = copy.deepcopy(self.parent.robot_pose.pose) angle = quaternion_to_euler(pose_considered.orientation)[2] if in_rectangle(bottom_blue_area, self.parent.robot_pose): @@ -80,30 +81,8 @@ def testVlx(self): theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) case = 4 - - x, y, theta = self.get_pose_from_vlx( - d1, d2, d3, True if d3 == values[30] else False - ) - new_x, new_y, new_theta = x_est(x, y), y_est(x, y), theta_est(theta) - d1_est, d2_est, d3_est = self.get_vlx_from_pose( - robot_pose_wall_relative, - rectif_angle, - True if d3 == values[30] else False, - ) - d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( - d1_est, - d2_est, - d3_est, - rectif_angle, - robot_pose_wall_relative, - case, - True, - True if case in [1, 2] else False, - ) - - self.parent.get_logger().info(f"test d1:{d1_proj_est}") - self.parent.get_logger().info(f"test d2:{d2_proj_est}") - self.parent.get_logger().info(f"test d3:{d3_proj_est}") + inv_angle_condition = True + inv_lat_condition = True if case in [1, 2] else False elif in_rectangle(top_blue_area, self.parent.robot_pose): self.parent.get_logger().info("top_blue_area") @@ -144,40 +123,8 @@ def testVlx(self): theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) case = 4 - x, y, theta = self.get_pose_from_vlx( - d1, d2, d3, True if d3 == values[30] else False - ) - new_x, new_y, new_theta = x_est(x, y), y_est(x, y), theta_est(theta) - self.parent.get_logger().info( - f"algo x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" - ) - self.parent.get_logger().info( - f"computed x:{x_est(x, y)}, y:{y_est(x, y)}, theta:{theta_est(theta)}" - ) - d1_est, d2_est, d3_est = self.get_vlx_from_pose( - robot_pose_wall_relative, - rectif_angle, - True if d3 == values[30] else False, - ) - self.parent.get_logger().info( - f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" - ) - self.parent.get_logger().info( - f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ - y:{round(new_y-pose_considered.position.y, 4)}, \ - theta:{round(new_theta-angle, 4)}" - ) - d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( - d1_est, - d2_est, - d3_est, - rectif_angle, - robot_pose_wall_relative, - case, - ) - self.parent.get_logger().info(f"test d1:{d1_proj_est}") - self.parent.get_logger().info(f"test d2:{d2_proj_est}") - self.parent.get_logger().info(f"test d3:{d3_proj_est}") + inv_angle_condition = False + inv_lat_condition = False elif in_rectangle(bottom_yellow_area, self.parent.robot_pose): self.parent.get_logger().info("bottom_yellow_area") @@ -218,46 +165,8 @@ def testVlx(self): theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) case = 4 - x, y, theta = self.get_pose_from_vlx( - d1, d2, d3, True if d3 == values[30] else False - ) - new_x, new_y, new_theta = x_est(x, y), y_est(x, y), theta_est(theta) - """ - self.parent.get_logger().info( - f"algo x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" - ) - self.parent.get_logger().info( - f"computed x:{x_est(x, y)}, y:{y_est(x, y)}, theta:{theta_est(theta)}" - ) - """ - d1_est, d2_est, d3_est = self.get_vlx_from_pose( - robot_pose_wall_relative, - rectif_angle, - True if d3 == values[30] else False, - ) - self.parent.get_logger().info( - f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" - ) - self.parent.get_logger().info( - f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ - y:{round(new_y-pose_considered.position.y, 4)}, \ - theta:{round(new_theta-angle, 4)}" - ) - d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( - d1_est, - d2_est, - d3_est, - rectif_angle, - robot_pose_wall_relative, - case, - False, - True, - ) - - self.parent.get_logger().info(f"test d1:{d1_proj_est}") - self.parent.get_logger().info(f"test d2:{d2_proj_est}") - self.parent.get_logger().info(f"test d3:{d3_proj_est}") - + inv_angle_condition = False + inv_lat_condition = True elif in_rectangle(top_yellow_area, self.parent.robot_pose): self.parent.get_logger().info("top_yellow_area") @@ -298,48 +207,26 @@ def testVlx(self): theta_est = lambda t: (t - np.pi) if t > 0 else (t + np.pi) case = 4 - x, y, theta = self.get_pose_from_vlx( - d1, d2, d3, True if d3 == values[30] else False - ) - new_x, new_y, new_theta = x_est(x, y), y_est(x, y), theta_est(theta) - """ - self.parent.get_logger().info( - f"algo x:{x/1000}, y:{y/1000}, theta:{np.degrees(theta)}" - ) - self.parent.get_logger().info( - f"computed x:{x_est(x, y)}, y:{y_est(x, y)}, theta:{theta_est(theta)}" - ) - """ - d1_est, d2_est, d3_est = self.get_vlx_from_pose( - robot_pose_wall_relative, - rectif_angle, - True if d3 == values[30] else False, - ) - self.parent.get_logger().info( - f"error computed vlx - true vlx d1:{d1_est - d1}, d2:{d2_est- d2}, d3:{d3_est -d3}" - ) - self.parent.get_logger().info( - f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ - y:{round(new_y-pose_considered.position.y, 4)}, \ - theta:{round(new_theta-angle, 4)}" - ) - d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( - d1_est, - d2_est, - d3_est, - rectif_angle, - robot_pose_wall_relative, - case, - True, - True if case in [3, 4] else False, - ) - - self.parent.get_logger().info(f"test d1:{d1_proj_est}") - self.parent.get_logger().info(f"test d2:{d2_proj_est}") - self.parent.get_logger().info(f"test d3:{d3_proj_est}") + inv_angle_condition = True + inv_lat_condition = True if case in [3, 4] else False else: self.parent.get_logger().info("None") + return None + + return { + "d": [d1, d2, d3], + "wall_relative": [ + robot_pose_wall_relative[0], + robot_pose_wall_relative[1], + rectif_angle, + ], + "pose_est": [x_est, y_est, theta_est], + "case": case, + "vlx_0x30": True if d3 == values[30] else False, + "inv_angle": inv_angle_condition, + "inv_lat": inv_lat_condition, + } def get_pose_from_vlx(self, d1, d2, d3, vlx_0x30): theta = np.arctan((d2 - d1) / (2 * self.vlx_face_y)) From e23f70b57b5893a3e1c65383bc1e1230161305c8 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 23 Mar 2021 23:27:06 +0100 Subject: [PATCH 32/64] Hello compute_data function --- .../localisation/localisation_node.py | 2 +- .../localisation/vlx_readjustement.py | 45 ++++++++++++++++++- 2 files changed, 45 insertions(+), 2 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index a1e20048..190108f4 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -149,7 +149,7 @@ def odom_callback(self, msg): self.robot_pose.header.stamp = msg.header.stamp self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) - self.vlx.testVlx() + self.vlx.compute_data() def main(args=None): diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index ebcefc0f..3d32bddc 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -35,7 +35,49 @@ def __init__(self, parent_node): self.vlx_face_y = self.parent.get_parameter("vlx_face_y")._value self.vlx_readjustement = False - def testVlx(self): + def compute_data(self): + pose_considered = copy.deepcopy(self.parent.robot_pose.pose) + data = self.fetch_data(pose_considered) + if data == None: + self.parent.get_logger().info("data_None") + else: + self.parent.get_logger().info("we can (hopefully) compute !") + x, y, theta = self.get_pose_from_vlx( + data["d"][0], data["d"][1], data["d"][2], data["vlx_0x30"] + ) + new_x, new_y, new_theta = ( + data["pose_est"][0](x, y), + data["pose_est"][1](x, y), + data["pose_est"][2](theta), + ) + d1_est, d2_est, d3_est = self.get_vlx_from_pose( + [data["wall_relative"][0], data["wall_relative"][1]], + data["wall_relative"][2], + data["vlx_0x30"], + ) + d1_proj_est, d2_proj_est, d3_proj_est = self.est_proj_wall( + d1_est, + d2_est, + d3_est, + data["wall_relative"][2], + [data["wall_relative"][0], data["wall_relative"][1]], + data["case"], + data["inv_angle"], + data["inv_lat"], + ) + + d = data["d"] + self.parent.get_logger().info( + f"error computed vlx - true vlx d1:{d1_est - d[0]}, d2:{d2_est- d[1]}, d3:{d3_est -d[2]}" + ) + self.parent.get_logger().info( + f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ + y:{round(new_y-pose_considered.position.y, 4)}, \ + theta:{round(new_theta-quaternion_to_euler(pose_considered.orientation)[2], 4)}" + ) + self.parent.get_logger().info(f"test d1:{d1_proj_est}") + self.parent.get_logger().info(f"test d2:{d2_proj_est}") + self.parent.get_logger().info(f"test d3:{d3_proj_est}") def fetch_data(self, pose_considered): @@ -84,6 +126,7 @@ def fetch_data(self, pose_considered): inv_angle_condition = True inv_lat_condition = True if case in [1, 2] else False + elif in_rectangle(top_blue_area, self.parent.robot_pose): self.parent.get_logger().info("top_blue_area") x_wall, y_wall = ( From b177c647660ccb88d960463010734a4680103bb1 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 24 Mar 2021 13:56:32 +0100 Subject: [PATCH 33/64] setting conditions to allow the recalibration --- .../localisation/localisation/sensors_sim.py | 6 +++ .../localisation/vlx_readjustement.py | 40 +++++++++++++++++-- 2 files changed, 42 insertions(+), 4 deletions(-) diff --git a/src/modules/localisation/localisation/sensors_sim.py b/src/modules/localisation/localisation/sensors_sim.py index d439d2c0..9f48d391 100644 --- a/src/modules/localisation/localisation/sensors_sim.py +++ b/src/modules/localisation/localisation/sensors_sim.py @@ -5,6 +5,7 @@ from os import environ +from builtin_interfaces.msg._time import Time from webots_ros2_core.webots_node import WebotsNode @@ -32,6 +33,11 @@ def get_distances(self): distances.update({addr: round(vlx.getValue(), 4)}) return distances + def get_time_stamp(self): + self.node.robot.step(0) + t = self.node.robot.getTime() + return Time(sec=int(t), nanosec=int((t % int(t)) * 1e9)) + def __del__(self): """Destructor.""" for vlx in self._vlx_array: diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 3d32bddc..b91d799f 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -7,7 +7,8 @@ import copy from localisation.sensors_sim import Sensors -from localisation.utils import in_rectangle, quaternion_to_euler, is_simulation +from localisation.utils import * +from geometry_msgs.msg import TransformStamped bottom_blue_area = [0.0, 0.0, 0.9, 1.0] top_blue_area = [0.0, 1.0, 1.5, 2.0] @@ -24,7 +25,7 @@ def __init__(self, parent_node): self.sim_offset = 0.0215 else: self.sim_offset = 0.0 - # self.parent.create_timer(1, self.testVlx) + self.parent.create_timer(0.7, self.try_to_readjust_with_vlx) self.parent.declare_parameter("vlx_lat_x", 0.0) self.parent.declare_parameter("vlx_lat_y", 0.0) self.parent.declare_parameter("vlx_face_x", 0.0) @@ -33,13 +34,44 @@ def __init__(self, parent_node): self.vlx_lat_y = self.parent.get_parameter("vlx_lat_y")._value self.vlx_face_x = self.parent.get_parameter("vlx_face_x")._value self.vlx_face_y = self.parent.get_parameter("vlx_face_y")._value + self.pose_array = [self.parent.robot_pose] self.vlx_readjustement = False def compute_data(self): pose_considered = copy.deepcopy(self.parent.robot_pose.pose) - data = self.fetch_data(pose_considered) + def store_robot_pose(self, pose): + self.pose_array.insert(0, copy.deepcopy(pose)) + if len(self.pose_array) > 15: + else self.sensors.get_time_stamp() + ) + vlx_values = self.sensors.get_distances() + + for i in range(len(self.pose_array)): + if ( + abs( + float( + self.pose_array[i].header.stamp.sec + + self.pose_array[i].header.stamp.nanosec * 1e-9 + ) + - float(actual_stamp.sec + actual_stamp.nanosec * 1e-9) + ) + < 0.1 + ): + tf = self.compute_data(self.pose_array[i].pose, vlx_values) + if tf == None: + return + else: + tf.header.stamp = actual_stamp + self.parent.get_logger().info("publish") + break + + def compute_data(self, pose_considered, vlx_values): + + data = self.fetch_data(pose_considered, vlx_values) + if data == None: self.parent.get_logger().info("data_None") + return None else: self.parent.get_logger().info("we can (hopefully) compute !") x, y, theta = self.get_pose_from_vlx( @@ -80,8 +112,8 @@ def compute_data(self): self.parent.get_logger().info(f"test d3:{d3_proj_est}") def fetch_data(self, pose_considered): + def fetch_data(self, pose_considered, values): - values = self.sensors.get_distances() angle = quaternion_to_euler(pose_considered.orientation)[2] if in_rectangle(bottom_blue_area, self.parent.robot_pose): From e93f8e7a0611f3519e9c542d6ba7b8f2bc7a60da Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 24 Mar 2021 13:57:54 +0100 Subject: [PATCH 34/64] storing poses + fix language --- .../localisation/localisation_node.py | 17 +++++++++-------- .../localisation/vlx_readjustement.py | 8 ++++++-- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 190108f4..cd60d80a 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -116,15 +116,15 @@ def allies_subscription_callback(self, msg): """Identity the robot marker in assurancetourix marker_array detection publish the transformation for drive to readjust odometry compute base_link --> odom""" - for allie_marker in msg.markers: + for ally_marker in msg.markers: if ( - allie_marker.text.lower() == self.robot - and (self.get_clock().now().to_msg().sec - self.last_odom_update) > 5 + ally_marker.text.lower() == self.robot + and (self.get_clock().now().to_msg().sec - self.last_odom_update) > 0.75 ): - q = allie_marker.pose.orientation - self._tf.header.stamp = allie_marker.header.stamp - self._tf.transform.translation.x = allie_marker.pose.position.x - self._tf.transform.translation.y = allie_marker.pose.position.y + q = ally_marker.pose.orientation + self._tf.header.stamp = ally_marker.header.stamp + self._tf.transform.translation.x = ally_marker.pose.position.x + self._tf.transform.translation.y = ally_marker.pose.position.y self._tf.transform.translation.z = float(0) self._tf.transform.rotation = q self.tf_publisher_.publish(self._tf) @@ -147,9 +147,10 @@ def odom_callback(self, msg): self.robot_pose.pose.position.y = new_robot_pose_kdl.p.y() q = new_robot_pose_kdl.M.GetQuaternion() self.robot_pose.header.stamp = msg.header.stamp + self.robot_pose.header.frame_id = "map" self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) - self.vlx.compute_data() + self.vlx.store_robot_pose(self.robot_pose) def main(args=None): diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index b91d799f..ea416e48 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -37,11 +37,15 @@ def __init__(self, parent_node): self.pose_array = [self.parent.robot_pose] self.vlx_readjustement = False - def compute_data(self): - pose_considered = copy.deepcopy(self.parent.robot_pose.pose) def store_robot_pose(self, pose): self.pose_array.insert(0, copy.deepcopy(pose)) if len(self.pose_array) > 15: + self.pose_array.pop() + + def try_to_readjust_with_vlx(self): + actual_stamp = ( + self.get_clock().now() + if not is_simulation() else self.sensors.get_time_stamp() ) vlx_values = self.sensors.get_distances() From 7c7e4b45e848c89e5b6b1423c514a60e2310c8bd Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 24 Mar 2021 13:58:45 +0100 Subject: [PATCH 35/64] conditions for readjustement --- .../localisation/vlx_readjustement.py | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index ea416e48..8925dd68 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -115,7 +115,26 @@ def compute_data(self, pose_considered, vlx_values): self.parent.get_logger().info(f"test d2:{d2_proj_est}") self.parent.get_logger().info(f"test d3:{d3_proj_est}") - def fetch_data(self, pose_considered): + if ( + d1_est > 0 + and d2_est > 0 + and d3_est > 0 + and abs(new_x - pose_considered.position.x) < 0.05 + and abs(new_y - pose_considered.position.y) < 0.05 + and abs(new_theta - quaternion_to_euler(pose_considered.orientation)[2]) + < 0.1 + ): + self.parent.get_logger().warn(f"able to correct") + tf = TransformStamped() + tf.header.frame_id = "map" + tf.child_frame_id = "odom" + tf.transform.translation.x = new_x + tf.transform.translation.y = new_y + tf.transform.rotation = euler_to_quaternion(new_theta) + return tf + else: + return None + def fetch_data(self, pose_considered, values): angle = quaternion_to_euler(pose_considered.orientation)[2] From 2b6ec937313f0900596de0f66a7a377aeb529ceb Mon Sep 17 00:00:00 2001 From: PhileasL Date: Thu, 25 Mar 2021 23:46:45 +0100 Subject: [PATCH 36/64] new strategy, removing pose old one, working on vlx stamped --- .../localisation/localisation_node.py | 10 ++++- .../localisation/vlx_readjustement.py | 37 +------------------ 2 files changed, 10 insertions(+), 37 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index cd60d80a..592df132 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -129,6 +129,14 @@ def allies_subscription_callback(self, msg): self._tf.transform.rotation = q self.tf_publisher_.publish(self._tf) self.last_odom_update = self.get_clock().now().to_msg().sec + def create_and_send_tf(self, x, y, q, stamp): + self._tf.header.stamp = stamp + self._tf.transform.translation.x = x + self._tf.transform.translation.y = y + self._tf.transform.translation.z = float(0) + self._tf.transform.rotation = q + self.last_odom_update = self.get_clock().now().to_msg().sec + self.tf_publisher_.publish(self._tf) def get_initial_tf_srv_callback(self, request, response): self.get_logger().info(f"incoming request for {self.robot} odom -> map tf") @@ -150,8 +158,6 @@ def odom_callback(self, msg): self.robot_pose.header.frame_id = "map" self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) - self.vlx.store_robot_pose(self.robot_pose) - def main(args=None): """Entrypoint.""" diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 8925dd68..0e017986 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -8,7 +8,6 @@ from localisation.sensors_sim import Sensors from localisation.utils import * -from geometry_msgs.msg import TransformStamped bottom_blue_area = [0.0, 0.0, 0.9, 1.0] top_blue_area = [0.0, 1.0, 1.5, 2.0] @@ -25,7 +24,7 @@ def __init__(self, parent_node): self.sim_offset = 0.0215 else: self.sim_offset = 0.0 - self.parent.create_timer(0.7, self.try_to_readjust_with_vlx) + #self.parent.create_timer(0.7, self.try_to_readjust_with_vlx) self.parent.declare_parameter("vlx_lat_x", 0.0) self.parent.declare_parameter("vlx_lat_y", 0.0) self.parent.declare_parameter("vlx_face_x", 0.0) @@ -34,39 +33,13 @@ def __init__(self, parent_node): self.vlx_lat_y = self.parent.get_parameter("vlx_lat_y")._value self.vlx_face_x = self.parent.get_parameter("vlx_face_x")._value self.vlx_face_y = self.parent.get_parameter("vlx_face_y")._value - self.pose_array = [self.parent.robot_pose] - self.vlx_readjustement = False - - def store_robot_pose(self, pose): - self.pose_array.insert(0, copy.deepcopy(pose)) - if len(self.pose_array) > 15: - self.pose_array.pop() - - def try_to_readjust_with_vlx(self): - actual_stamp = ( - self.get_clock().now() - if not is_simulation() - else self.sensors.get_time_stamp() - ) - vlx_values = self.sensors.get_distances() - for i in range(len(self.pose_array)): if ( abs( float( - self.pose_array[i].header.stamp.sec - + self.pose_array[i].header.stamp.nanosec * 1e-9 ) - - float(actual_stamp.sec + actual_stamp.nanosec * 1e-9) ) - < 0.1 ): - tf = self.compute_data(self.pose_array[i].pose, vlx_values) - if tf == None: - return - else: - tf.header.stamp = actual_stamp - self.parent.get_logger().info("publish") break def compute_data(self, pose_considered, vlx_values): @@ -125,13 +98,7 @@ def compute_data(self, pose_considered, vlx_values): < 0.1 ): self.parent.get_logger().warn(f"able to correct") - tf = TransformStamped() - tf.header.frame_id = "map" - tf.child_frame_id = "odom" - tf.transform.translation.x = new_x - tf.transform.translation.y = new_y - tf.transform.rotation = euler_to_quaternion(new_theta) - return tf + return new_x, new_y, euler_to_quaternion(new_theta) else: return None From 06f3592f53dd2db13a06be3e17b6fa8e6b4314ed Mon Sep 17 00:00:00 2001 From: PhileasL Date: Thu, 25 Mar 2021 23:49:28 +0100 Subject: [PATCH 37/64] Hello threading, you might be usefull --- .../localisation/vlx_readjustement.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 0e017986..7458d26d 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -5,9 +5,11 @@ import numpy as np import copy +import threading from localisation.sensors_sim import Sensors from localisation.utils import * +from geometry_msgs.msg import Pose bottom_blue_area = [0.0, 0.0, 0.9, 1.0] top_blue_area = [0.0, 1.0, 1.5, 2.0] @@ -33,6 +35,16 @@ def __init__(self, parent_node): self.vlx_lat_y = self.parent.get_parameter("vlx_lat_y")._value self.vlx_face_x = self.parent.get_parameter("vlx_face_x")._value self.vlx_face_y = self.parent.get_parameter("vlx_face_y")._value + self.values_stamped_array = [ + VlxStamped( + self.sensors.get_distances(), + self.get_clock().now() + if not is_simulation() + else self.sensors.get_time_stamp(), + ) + ] + self.thread_continuous_sampling = None + self.continuous_sampling = 0 if ( abs( @@ -370,3 +382,9 @@ def est_proj_wall( - considered_vlx_lat_x * np.cos(considered_angle) ) return (d1_est_proj_wall, d2_est_proj_wall, d3_est_proj_wall) + + +class VlxStamped: + def __init__(self, values, stamp): + self.values = copy.deepcopy(values) + self.stamp = stamp From 0416a8c036e56594efa7bee487f0f9320f1e88ac Mon Sep 17 00:00:00 2001 From: PhileasL Date: Thu, 25 Mar 2021 23:50:40 +0100 Subject: [PATCH 38/64] Start, stop and function of the vlx data obtention thread --- .../localisation/vlx_readjustement.py | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 7458d26d..01ea95d3 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -46,6 +46,38 @@ def __init__(self, parent_node): self.thread_continuous_sampling = None self.continuous_sampling = 0 + def start_continuous_sampling_thread(self, sleep_time, continuous_samp): + if self.thread_continuous_sampling == None: + self.thread_continuous_sampling = threading.Thread( + target=self.continuous_vlx_sampling, args=(sleep_time, continuous_samp,) + ) + self.thread_continuous_sampling.start() + else: + self.parent.get_logger().info("vlx_thread thread is already running !") + + def stop_continuous_sampling_thread(self): + if self.thread_continuous_sampling != None: + self.continuous_sampling = 0 + self.thread_continuous_sampling.join() + self.thread_continuous_sampling = None + else: + self.parent.get_logger().info("vlx_thread thread isn't running !") + + def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): + self.continuous_sampling = continuous_samp + self.parent.get_logger().warn(f"self.continuous_sampling:{self.continuous_sampling}") + time.sleep(sleep_time_before_sampling) + while self.continuous_sampling != 0: + actual_stamp = ( + self.get_clock().now() + if not is_simulation() + else self.sensors.get_time_stamp() + ) + vlx_values = self.sensors.get_distances() + self.values_stamped_array.insert(0, VlxStamped(vlx_values, actual_stamp)) + if len(self.values_stamped_array) > 10: + self.values_stamped_array.pop() + if ( abs( float( From b99da485729d0effa9447fe0f1360325356f3432 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Thu, 25 Mar 2021 23:51:50 +0100 Subject: [PATCH 39/64] Refactoring try_to_readjust_with_vlx function --- .../localisation/vlx_readjustement.py | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 01ea95d3..3cdf24a0 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -78,13 +78,35 @@ def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): if len(self.values_stamped_array) > 10: self.values_stamped_array.pop() + def try_to_readjust_with_vlx(self, x, y, q, stamp): + send_tf = True + for i in range(len(self.values_stamped_array)): if ( abs( float( + self.values_stamped_array[i].stamp.sec + + self.values_stamped_array[i].stamp.nanosec * 1e-9 ) + - float(stamp.sec + stamp.nanosec * 1e-9) ) + < 0.06 ): + new_stamp = self.values_stamped_array[i].stamp + + pose = Pose() + pose.position.x = x + pose.position.y = y + pose.orientation = q + news = self.compute_data(pose, self.values_stamped_array[i].values) + if news != None: + send_tf = False + self.parent.get_logger().info("publish affined position") + self.parent.create_and_send_tf(news[0], news[1], news[2], new_stamp) break + if send_tf: + self.parent.create_and_send_tf(x, y, q, stamp) + if self.continuous_sampling == 1: + self.stop_continuous_sampling_thread() def compute_data(self, pose_considered, vlx_values): From 6ec3bf0b339aea9575b15f55ec4576b8e324ac78 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Thu, 25 Mar 2021 23:57:08 +0100 Subject: [PATCH 40/64] refactoring marker callback + black --- .../localisation/localisation_node.py | 43 +++++++++++++++---- .../localisation/vlx_readjustement.py | 14 ++++-- 2 files changed, 44 insertions(+), 13 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 592df132..f5d6bd11 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -6,6 +6,7 @@ import math import numpy as np +import time import copy import rclpy @@ -15,7 +16,7 @@ from tf2_ros import StaticTransformBroadcaster from transformix_msgs.srv import TransformixParametersTransformStamped from localisation.vlx_readjustement import VlxReadjustement -from localisation.utils import euler_to_quaternion +from localisation.utils import euler_to_quaternion, is_simulation from nav_msgs.msg import Odometry from tf2_kdl import * @@ -121,14 +122,38 @@ def allies_subscription_callback(self, msg): ally_marker.text.lower() == self.robot and (self.get_clock().now().to_msg().sec - self.last_odom_update) > 0.75 ): - q = ally_marker.pose.orientation - self._tf.header.stamp = ally_marker.header.stamp - self._tf.transform.translation.x = ally_marker.pose.position.x - self._tf.transform.translation.y = ally_marker.pose.position.y - self._tf.transform.translation.z = float(0) - self._tf.transform.rotation = q - self.tf_publisher_.publish(self._tf) - self.last_odom_update = self.get_clock().now().to_msg().sec + if ( + is_simulation() + ): # simulate marker delais (image analysis from assurancetourix) + time.sleep(0.15) + if False: # is_in_specific_area(ally_marker): + continuous_sampling = 2 + tempo = 0.0 + else: + continuous_sampling = 1 + tempo = 0.65 + if self.vlx.continuous_sampling == 0: + self.get_logger().info(f"initial continuous_sampling == 0") + self.create_and_send_tf( + ally_marker.pose.position.x, + ally_marker.pose.position.y, + ally_marker.pose.orientation, + ally_marker.header.stamp, + ) + self.vlx.start_continuous_sampling_thread( + tempo, continuous_sampling + ) + elif self.vlx.continuous_sampling == 1: + self.vlx.try_to_readjust_with_vlx( + ally_marker.pose.position.x, + ally_marker.pose.position.y, + ally_marker.pose.orientation, + ally_marker.header.stamp, + ) + self.vlx.start_continuous_sampling_thread( + tempo, continuous_sampling + ) + def create_and_send_tf(self, x, y, q, stamp): self._tf.header.stamp = stamp self._tf.transform.translation.x = x diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 3cdf24a0..ed0ada46 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -5,6 +5,7 @@ import numpy as np import copy +import time import threading from localisation.sensors_sim import Sensors @@ -26,7 +27,7 @@ def __init__(self, parent_node): self.sim_offset = 0.0215 else: self.sim_offset = 0.0 - #self.parent.create_timer(0.7, self.try_to_readjust_with_vlx) + # self.parent.create_timer(0.7, self.try_to_readjust_with_vlx) self.parent.declare_parameter("vlx_lat_x", 0.0) self.parent.declare_parameter("vlx_lat_y", 0.0) self.parent.declare_parameter("vlx_face_x", 0.0) @@ -49,7 +50,11 @@ def __init__(self, parent_node): def start_continuous_sampling_thread(self, sleep_time, continuous_samp): if self.thread_continuous_sampling == None: self.thread_continuous_sampling = threading.Thread( - target=self.continuous_vlx_sampling, args=(sleep_time, continuous_samp,) + target=self.continuous_vlx_sampling, + args=( + sleep_time, + continuous_samp, + ), ) self.thread_continuous_sampling.start() else: @@ -65,7 +70,9 @@ def stop_continuous_sampling_thread(self): def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): self.continuous_sampling = continuous_samp - self.parent.get_logger().warn(f"self.continuous_sampling:{self.continuous_sampling}") + self.parent.get_logger().warn( + f"self.continuous_sampling:{self.continuous_sampling}" + ) time.sleep(sleep_time_before_sampling) while self.continuous_sampling != 0: actual_stamp = ( @@ -214,7 +221,6 @@ def fetch_data(self, pose_considered, values): inv_angle_condition = True inv_lat_condition = True if case in [1, 2] else False - elif in_rectangle(top_blue_area, self.parent.robot_pose): self.parent.get_logger().info("top_blue_area") x_wall, y_wall = ( From 86c53cf01139fd4aa0436be407fa86df3d62d373 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Fri, 26 Mar 2021 15:10:00 +0100 Subject: [PATCH 41/64] Localisation thread preventing concurrency problems+clean desctructor --- src/modules/localisation/localisation/localisation_node.py | 2 ++ src/modules/localisation/localisation/vlx_readjustement.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index f5d6bd11..32a3d51e 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -192,5 +192,7 @@ def main(args=None): rclpy.spin(localisation) except KeyboardInterrupt: pass + localisation.vlx.stop_continuous_sampling_thread() + localisation.vlx.sensors.node.destroy_node() localisation.destroy_node() rclpy.shutdown() diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index ed0ada46..608b7183 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -84,6 +84,7 @@ def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): self.values_stamped_array.insert(0, VlxStamped(vlx_values, actual_stamp)) if len(self.values_stamped_array) > 10: self.values_stamped_array.pop() + time.sleep(0.02) def try_to_readjust_with_vlx(self, x, y, q, stamp): send_tf = True @@ -99,6 +100,7 @@ def try_to_readjust_with_vlx(self, x, y, q, stamp): < 0.06 ): new_stamp = self.values_stamped_array[i].stamp + self.stop_continuous_sampling_thread() pose = Pose() pose.position.x = x From 3cbc72c7ecc654f91ec7f98f62d8256f3a7d2da9 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 27 Mar 2021 19:54:10 +0100 Subject: [PATCH 42/64] near wall routine start and stop --- .../localisation/vlx_readjustement.py | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 608b7183..0fc0894a 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -11,6 +11,9 @@ from localisation.sensors_sim import Sensors from localisation.utils import * from geometry_msgs.msg import Pose +from builtin_interfaces.msg._time import Time + +from datetime import datetime bottom_blue_area = [0.0, 0.0, 0.9, 1.0] top_blue_area = [0.0, 1.0, 1.5, 2.0] @@ -27,7 +30,6 @@ def __init__(self, parent_node): self.sim_offset = 0.0215 else: self.sim_offset = 0.0 - # self.parent.create_timer(0.7, self.try_to_readjust_with_vlx) self.parent.declare_parameter("vlx_lat_x", 0.0) self.parent.declare_parameter("vlx_lat_y", 0.0) self.parent.declare_parameter("vlx_face_x", 0.0) @@ -46,6 +48,23 @@ def __init__(self, parent_node): ] self.thread_continuous_sampling = None self.continuous_sampling = 0 + self.near_walls_last_check_stamp = None + def start_near_wall_routine(self, pose_stamped): + if self.near_walls_last_check_stamp == None: + self.near_walls_last_check_stamp = Time(sec=0, nanosec=0) + if self.thread_continuous_sampling != None: + self.stop_continuous_sampling_thread() + self.start_continuous_sampling_thread(0.0, 2) + self.near_wall_routine(pose_stamped) + else: + self.parent.get_logger().info("near wall routine already running") + + def stop_near_wall_routine(self): + if self.near_walls_last_check_stamp != None: + self.near_walls_last_check_stamp = None + self.stop_continuous_sampling_thread() + else: + self.parent.get_logger().info("near wall routine already stopped") def start_continuous_sampling_thread(self, sleep_time, continuous_samp): if self.thread_continuous_sampling == None: From c01b8ac5028e781631dc5c29d219f14e36a0db0b Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 27 Mar 2021 19:54:39 +0100 Subject: [PATCH 43/64] near_wall_routine function --- .../localisation/vlx_readjustement.py | 37 ++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 0fc0894a..0f5fe610 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -49,6 +49,36 @@ def __init__(self, parent_node): self.thread_continuous_sampling = None self.continuous_sampling = 0 self.near_walls_last_check_stamp = None + + def near_wall_routine(self, pose_stamped): + actual_stamp = ( + self.get_clock().now() + if not is_simulation() + else self.sensors.get_time_stamp() + ) + if ( + abs( + float( + pose_stamped.header.stamp.sec + + pose_stamped.header.stamp.nanosec * 1e-9 + ) + - float(self.near_walls_last_check_stamp.sec + self.near_walls_last_check_stamp.nanosec * 1e-9) + ) + < 0.5 + ): + self.parent.get_logger().warn("near_wall_routine") + self.try_to_readjust_with_vlx( + pose_stamped.pose.position.x, + pose_stamped.pose.position.y, + pose_stamped.pose.orientation, + pose_stamped.header.stamp, + ) + self.near_walls_last_check_stamp = ( + self.get_clock().now() + if not is_simulation() + else self.sensors.get_time_stamp() + ) + def start_near_wall_routine(self, pose_stamped): if self.near_walls_last_check_stamp == None: self.near_walls_last_check_stamp = Time(sec=0, nanosec=0) @@ -128,13 +158,18 @@ def try_to_readjust_with_vlx(self, x, y, q, stamp): news = self.compute_data(pose, self.values_stamped_array[i].values) if news != None: send_tf = False - self.parent.get_logger().info("publish affined position") + self.parent.get_logger().warn("publish affined position") self.parent.create_and_send_tf(news[0], news[1], news[2], new_stamp) break if send_tf: self.parent.create_and_send_tf(x, y, q, stamp) if self.continuous_sampling == 1: self.stop_continuous_sampling_thread() + if self.near_walls_last_check_stamp != None: + self.start_continuous_sampling_thread(0.0, 2) + later = datetime.now() + delay = (later - now).total_seconds() + self.parent.get_logger().info(f"delais: {delay}") def compute_data(self, pose_considered, vlx_values): From 09394c71486e562d2443ff36dd6d9d0d743e48a4 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 27 Mar 2021 19:55:31 +0100 Subject: [PATCH 44/64] localisation_node now supporting full vlx_recalibration near walls --- .../localisation/localisation_node.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 32a3d51e..39c41465 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -126,12 +126,6 @@ def allies_subscription_callback(self, msg): is_simulation() ): # simulate marker delais (image analysis from assurancetourix) time.sleep(0.15) - if False: # is_in_specific_area(ally_marker): - continuous_sampling = 2 - tempo = 0.0 - else: - continuous_sampling = 1 - tempo = 0.65 if self.vlx.continuous_sampling == 0: self.get_logger().info(f"initial continuous_sampling == 0") self.create_and_send_tf( @@ -140,19 +134,18 @@ def allies_subscription_callback(self, msg): ally_marker.pose.orientation, ally_marker.header.stamp, ) - self.vlx.start_continuous_sampling_thread( - tempo, continuous_sampling - ) - elif self.vlx.continuous_sampling == 1: + else: self.vlx.try_to_readjust_with_vlx( ally_marker.pose.position.x, ally_marker.pose.position.y, ally_marker.pose.orientation, ally_marker.header.stamp, ) - self.vlx.start_continuous_sampling_thread( - tempo, continuous_sampling - ) + if self.vlx.continuous_sampling in [0, 1]: + self.vlx.start_continuous_sampling_thread(0.65, 1) + + def is_near_walls(self, pt): + return pt.x < 0.25 or pt.y < 0.25 or pt.x > 2.75 or pt.y > 1.75 def create_and_send_tf(self, x, y, q, stamp): self._tf.header.stamp = stamp @@ -182,6 +175,13 @@ def odom_callback(self, msg): self.robot_pose.header.stamp = msg.header.stamp self.robot_pose.header.frame_id = "map" self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + if self.is_near_walls(self.robot_pose.pose.position): + if self.vlx.continuous_sampling == 2: + self.vlx.near_wall_routine(self.robot_pose) + else: + self.vlx.start_near_wall_routine(self.robot_pose) + elif self.vlx.continuous_sampling == 2: + self.vlx.stop_near_wall_routine() def main(args=None): From af8a37e58cb59f1c59614fa7b2d14b765d7d88aa Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 27 Mar 2021 19:57:03 +0100 Subject: [PATCH 45/64] odom_map_relative topic: map relative pose_stamped of the robot --- src/modules/localisation/localisation/localisation_node.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 39c41465..7fe25754 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -61,6 +61,9 @@ def __init__(self): self.tf_publisher_ = self.create_publisher( TransformStamped, "adjust_odometry", 10 ) + self.odom_map_relative_publisher_ = self.create_publisher( + PoseStamped, "odom_map_relative", 10 + ) self.last_odom_update = 0 self.get_logger().info(f"Default side is {self.side.value}") self.vlx = VlxReadjustement(self) @@ -175,6 +178,7 @@ def odom_callback(self, msg): self.robot_pose.header.stamp = msg.header.stamp self.robot_pose.header.frame_id = "map" self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + self.odom_map_relative_publisher_.publish(self.robot_pose) if self.is_near_walls(self.robot_pose.pose.position): if self.vlx.continuous_sampling == 2: self.vlx.near_wall_routine(self.robot_pose) From 78d6986289adc44750c40e02db601d272bf4c7f4 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 27 Mar 2021 19:57:26 +0100 Subject: [PATCH 46/64] full debugging logs --- .../localisation/localisation/localisation_node.py | 2 ++ .../localisation/localisation/vlx_readjustement.py | 13 ++++++++++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 7fe25754..f437548e 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -19,6 +19,7 @@ from localisation.utils import euler_to_quaternion, is_simulation from nav_msgs.msg import Odometry from tf2_kdl import * +from datetime import datetime class Localisation(rclpy.node.Node): @@ -157,6 +158,7 @@ def create_and_send_tf(self, x, y, q, stamp): self._tf.transform.translation.z = float(0) self._tf.transform.rotation = q self.last_odom_update = self.get_clock().now().to_msg().sec + self.get_logger().info(f"publishing tf") self.tf_publisher_.publish(self._tf) def get_initial_tf_srv_callback(self, request, response): diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index 0f5fe610..d0df4dac 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -62,7 +62,10 @@ def near_wall_routine(self, pose_stamped): pose_stamped.header.stamp.sec + pose_stamped.header.stamp.nanosec * 1e-9 ) - - float(self.near_walls_last_check_stamp.sec + self.near_walls_last_check_stamp.nanosec * 1e-9) + - float( + self.near_walls_last_check_stamp.sec + + self.near_walls_last_check_stamp.nanosec * 1e-9 + ) ) < 0.5 ): @@ -111,6 +114,9 @@ def start_continuous_sampling_thread(self, sleep_time, continuous_samp): def stop_continuous_sampling_thread(self): if self.thread_continuous_sampling != None: + self.parent.get_logger().info( + f"stopping continuous sampling:{self.continuous_sampling}" + ) self.continuous_sampling = 0 self.thread_continuous_sampling.join() self.thread_continuous_sampling = None @@ -119,8 +125,8 @@ def stop_continuous_sampling_thread(self): def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): self.continuous_sampling = continuous_samp - self.parent.get_logger().warn( - f"self.continuous_sampling:{self.continuous_sampling}" + self.parent.get_logger().info( + f"starting continuous sampling:{self.continuous_sampling}" ) time.sleep(sleep_time_before_sampling) while self.continuous_sampling != 0: @@ -136,6 +142,7 @@ def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): time.sleep(0.02) def try_to_readjust_with_vlx(self, x, y, q, stamp): + now = datetime.now() send_tf = True for i in range(len(self.values_stamped_array)): if ( From 419686ac9a5a8cb1225599140b996c2e27b048bd Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 28 Mar 2021 20:31:52 +0200 Subject: [PATCH 47/64] add readme with some explanations --- src/modules/localisation/README.md | 167 ++++++++++++++++++ .../schematic_pictures/vlx_math_schematic.png | Bin 0 -> 62823 bytes .../schematic_pictures/vlx_proj.png | Bin 0 -> 46631 bytes .../vlx_x_y_determination.png | Bin 0 -> 41749 bytes 4 files changed, 167 insertions(+) create mode 100644 src/modules/localisation/README.md create mode 100755 src/modules/localisation/schematic_pictures/vlx_math_schematic.png create mode 100755 src/modules/localisation/schematic_pictures/vlx_proj.png create mode 100755 src/modules/localisation/schematic_pictures/vlx_x_y_determination.png diff --git a/src/modules/localisation/README.md b/src/modules/localisation/README.md new file mode 100644 index 00000000..7e9f7710 --- /dev/null +++ b/src/modules/localisation/README.md @@ -0,0 +1,167 @@ +# Localisation package + +## What does this package do ? + +- Publish the initial map->odom transformation, +- Subscribe to "odom" topic and publish on "odom_map_relative" the map relative robot pose, +- Compute the transformation to re-adjust odometry and publish it. + + +## Odometry re-adjustement method + +In order to re-adjust the odometry, 2 things are used: +- Vision from assurancetourix package, +- 6 VL53L1X around the robot. + +### Vision + +Assurancetourix is publishing aruco positions and orientation through /allies_positions_markers topic. By subscribing to the topic and identify the marker that correspond to the node's namespace, the position and orientation of the robot can be found. + +### VL53L1X + +There are 6 VL53L1X on the robot, by knowing there positions relative to the robot and by getting the distance they're returning, with a certain amount of mathematical considerations (described below), the position and orientation of the robot can re found. + +The limitation of this algorithm is that **we do have to know an estimation of the position and orientation of the robot to validate and refine the pose (position/orientation).** + +### Problem raised by a mix of vision and VLX + +We're estimating the vision localization precision of about 1 centimeter average which isn't enough in some cases (particularly near walls). + +So, in order to increase the precision, the information given by the vision needs to be refined by a vlx measure. But keep in mind that vision gives a pose STAMPED information which means the marker given by assurancetourix has a delay of a few milliseconds (~350ms average). The vlx measure takes (in simulation) about 60ms which gives a final difference between theses stamps of about 400ms. + +If the robot is moving at something like 20cm/s, we do have to allow a difference of 8cm between the vision pose and vlx computed pose. Unrealistic, goblet have a 7cm diameter. + +If you haven't understood yet what the problem is, here it is: **the difference of stamps between vlx measure and vision measure must be as small as possible.** + +### Another approach + +We do know that every 750ms we're gonna try to re-adjust odometry by allowing the node to receive a marker information. A routine can be thought that waits for 0.65s, launch a loop getting the vlx measures and stamp of the sample and storing them in a queue of something like 10 samples ( I add a small delay between each sample to avoid overloading the localisation_node thread ) . The older element of this queue is 600ms old, it's easy to retrieve the nearest element of the queue from the vision marker stamp. When retrieving this element, the vlx sample thread is stopped. + +With this nearest queue element, we have a maximum difference of stamps of 0.04ms which is 10 times better than the previous approach. We're going to consider that's enough of an optimization. + +We now just have to compare the vlx computed pose and vision pose of the robot, if those are really near ( < 5cm, <0.1rad ) and the estimated projection of vlx on the walls are effectively on the right wall, we're considering the correction as valid. The valid transformation is sent to drive for it to re-adjust odometry. If the vlx computed pose isn't considered as valid, the vision pose is sent to drive. + +After sending the correction transformation to drive, we have to restart the vlx sample thread with a starting delay of 0.65 seconds. + +### Near walls routine + +There's a specific behavior of this package near walls: in the odom callback, while detecting the robot position being near walls (<30cm from a wall), the node short circuit the behavior previously presented and adopt a new strategy. + +This strategy consider an "infinite" sampling of the vlx sampling thread with no starting delay. Each 0.5 seconds odom callback, the node ask for an odometry re-adjustment considering the last robot pose that sent drive. + +As previously, there's a comparison between stamps to get the nearest one of the queue that gives the sample to consider for calculation. When calculating the new pose from the vlx distance values, the thread is stopped avoid overloading of the localisation_node thread. + +If the computed pose is considered as valid, it's sent to drive to re-adjust odometry. Else the pose is just thrown away and nothing is sent to re-adjust odometry. + +Then, the vlx sampling thread is restarted with no delay because the node can continues trying to readjust odometry with vision callback with the strategy described in the previous paragraph. + +### Avoiding concurrency problems + +You've understood it, the localisation node contains several threads (odom callback, marker callback and vlx sample). In order to avoid concurrency problems (especially deadlocks, several thread starting, overloading etc.) several strategies are deployed: +- **Deadlocks:** + The resources shared by several threads can be writable only in one thread, in other threads, this resources is only readable. + - **Several thread starting:** +Only one vlx sampling thread can be started, when asking for the creation of this thread, there's a check of this existing thread before creating it or raising that this thread is already running. Same thing for stopping this thread, if no vlx sampling thread is existing, the node raising that this thread doesn't exist. +- **Overloading:** +Getting vlx values is a blocking call, so there's a need of a small non-blocking delay between each sample that allow the node doing its other works. + +## Mathematical considerations + +This paragraph is about to explain you how we can compute the pose (position/orientation) of the robot from the vlx measured distances. + +As all geometrical problem it's way easier to understand what's happening with a schematic of the situation: + +

+ +

+
+ +Now, time to deep dive into calculations: + ++ $f(d1,d2,d3)=(x,y,\theta)$ + + $\theta$: +Considering the triangle surrounded in purple and basic trigonometric formula: +$$\tan(\theta)=\frac{d2-d1}{2e}$$ +$$\Rightarrow\boxed{\theta=\arctan{\frac{d2-d1}{2e}}}\ (3)$$ + + + $x$ and $y$: +Now that we now $\theta$, it's fairly simple to get x and y by projecting vlx distances using $\theta$. + +

+ +

+
+ +$$\boxed{y= ( \ \frac{d2-d1}{2} + d \ ).\cos{\theta}} \ (1)$$ +$$\boxed{x=(d3+r).\cos{\theta}+L.\sin{\theta}}\ (2)$$ + ++ $f(x,y,\theta)=(d1,d2,d3)$ + + $d1$ +$$(3)\Rightarrow\tan{\theta}=\frac{d2-d1}{2e}$$ +$$\Rightarrow d2=2e\tan{\theta}+d1\ (4)$$ +$$(1)\Rightarrow y=(\frac{d1+(d1+2e\tan{\theta})}{2}+d).\cos{\theta}$$ +$$\Rightarrow y=(d1+2e\tan{\theta}).\cos{\theta}$$ +$$\Rightarrow \boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta} -d}$$ + + + $d2$ +$$(4)\Rightarrow d2=2e\tan{\theta}+\frac{y}{\cos{\theta}}-e\tan{\theta} -d$$ +$$\Rightarrow \boxed{d2=\frac{y}{\cos{\theta}}+e\tan{\theta} -d}$$ + + + $d3$ +$$(2)\Rightarrow x-L.\sin{\theta}=(d3+r).\cos{\theta}$$ +$$\Rightarrow \frac{x}{\cos{\theta}}-L.\tan{\theta}=d3+r$$ +$$\Rightarrow \boxed{d3=\frac{x}{\cos{\theta}}-L.\tan{\theta}-r}$$ + ++ $f(x,\ y,\ \theta,\ d1,\ d2,\ d3)=(d1\_proj, \ d2\_proj, \ d3\_proj)$ +

+ +

+
+All theses are just simple projection knowing $x,\ y,\ \theta$ and $f(x,y,\theta)=(d1,d2,d3)$. It constitutes an extra tool to validate the pose estimation. Indeed, if a projection is negative, it's because the vlx doesn't measure the distance to the desired wall, so the resulting pose will obviously be incorrect. +$$\boxed{d1\_proj=x+(d1+d).\sin{\theta}-e.\cos{\theta}}$$ +$$\boxed{d2\_proj=x+(d2+d).\sin{\theta}+e.\cos{\theta}}$$ +$$\boxed{d1\_proj=y-(d3+r).\sin{\theta}-L.\cos{\theta}}$$ + +### Formulas summary +$$\boxed{\begin{pmatrix} +x\\ +\\ +y\\ +\\ +\theta +\end{pmatrix} +=\begin{pmatrix} +(d3+r).\cos{\theta}+L.\sin{\theta}\\ +\\ +( \ \frac{d2-d1}{2} + d \ ).\cos{\theta}\\ +\\ +\arctan{\frac{d2-d1}{2e}} +\end{pmatrix}}$$ +$$\boxed{\begin{pmatrix} +d1\\ +\\ +d2\\ +\\ +d3 +\end{pmatrix} +=\begin{pmatrix} +\frac{y}{\cos{\theta}}-e\tan{\theta} -d\\ +\\ +\frac{y}{\cos{\theta}}+e\tan{\theta} -d\\ +\\ +\frac{x}{\cos{\theta}}-L.\tan{\theta}-r +\end{pmatrix}}$$ +$$\boxed{\begin{pmatrix} +d1\_proj\\ +\\ +d2\_proj\\ +\\ +d3\_proj +\end{pmatrix} +=\begin{pmatrix} +x+(d1+d).\sin{\theta}-e.\cos{\theta}\\ +\\ +x+(d2+d).\sin{\theta}+e.\cos{\theta}\\ +\\ +y-(d3+r).\sin{\theta}-L.\cos{\theta} +\end{pmatrix}}$$ diff --git a/src/modules/localisation/schematic_pictures/vlx_math_schematic.png b/src/modules/localisation/schematic_pictures/vlx_math_schematic.png new file mode 100755 index 0000000000000000000000000000000000000000..d2639e1ebcad4005799cc9457fe154a5ba1d9692 GIT binary patch literal 62823 zcmX_Ic_7pO|9>MCsT2{p(!qodN4Yj(N|-a{tRz>+onuJJ)k2XX=VERVVKPY~a?Bh# z=9bN|ImXx=zc+opzdurI+xzu;J@4n^`FM#oGSuba65;{?fak__Z4&_4w+;Yo9Q!%I z|M7UDf&~9!@ix)D29$K1B!e&ZIcgec0zg?D_vSrz@b!Vm*Y9`(z@f(7KP=52c@F@9 zws}Ka)9i`u{1)d2>(w-Bz|c>B%E)^gdDYUoIp+NNl?(I{B!%MTsQdG*(5FvjC{IDu z0aooJ;@TVs610y-z5I5xH9yJQW%`4+`}FzBQ>+gp;^SL3)3?)AcDl*k^;R}SenWHQ zNNGR4mpEfq)stV)QMEH31nxO3{PFC1vLCH0dd#`08cKZk;E=nj(2u&}m6%s#Ymm9LDwO*(z_9|XTYAiBGvuF;x5r^a; ze|@a0%&na<;RZ9=IUgptRQ-o@awi#oO2$lSWvd?5UKdw)(B%k|xH<1*7AI%zbz`p~ zJ19FvaophZ2aOk50N}xUJxG#y_JkgLvP`qt5As*Na;F2ilYwGHTlRM&x>cSIDa>~< z!Y43OsoRI$E7F;vH4`SrWK8D`v)P(*BRg)3P4X(K|60{CWiKO-+Jx{4+^$-4V#2{Q zh-+Ds&q6APrIeK!hBM}RWVbX^<7xP8e&+oi%W&NB5LGH@NPki}xOT+e+L?!j^DZgZ-9j%8=c!suf72(NC&i3lG*vv6x$z2Bp z91*^^(+Dn=BvqjUx!ui4+K^|%G)sOMUqD2sLF{oPVztiKv;b`yCcO;+un&8iGBdt| z$!qaukCUPK{7wo<49s~p0Unr-i%SCljLOaWoDW;4rE>MX~c`JPQpM8ls_b~8T^Fs-Y4*; zOWCXlopdgNs{HWW5t9n>RN7}u9`N#Q2z~+qO=aBiVV3}(gx&kJam=ecucmaaV~oO=Q$;gyroM=`y(kefI+V-=AR^GFX0ZglU1nfc!c zdFS}W0&SW^(W^#93ON=~s{3HgL?0Es$OdHk>`E^zd@4pA^~coA9ms)Cr-um8y7@!g zpJ&L_u>i0o^sd>n|5~^{8*>nd{*G{{3W1tYoMTEu5VS8`o@tx_&=k3=GrofcP&4Ck zTg)Fe7tAQH#dZ*+35>s1X`e5qAt3& z^xAym%53MAz%5eNv4$Z~Xwbb!6;&&KQ#f-3rk1~Gnm^OmY^G3|?EDn~ra1S`z|PMh zr@zC-6&a&}qL?;dMs1RU18pQ9OK$=|3(u~_#rs266^>rd0m8oj<iSbSzYbR)OKAhy8#MvK(hh-x!k(&wk;(H=4twrszf$2TRR}^ z0{~r*cQqc;)2r40p=2iK_5!YHCU{ zd`25fh(OqV_^ueet`WR^v)Y}6fSQlS$iYf0I&?6>qAA0c6)@8OcP1x;^EP+qCWHX5 zb|T%${t!_BJo)w*07xC*Tj5;A@wd5cEHIpo@^shDdu~PmOkz)(SF^~{H2-h0_e3v^>(Bpm2o=04^WZoV;{C{tEIJpSl*F&#ovx}kWVHIU_a)7wq ze_z~}(Kl|-Wu@=@!?t3~eoH-b#Ee#B5J6LsWPr}{pO(>dg#sOc+Nk&T-i87}LNzme zWA>ajnVK|C9yqvPucv$a)o0HA{Dg;}bgoF}QeQ}yaTl=RN2Xx8!ACCZ_!AuzzzzM2 z?Hx}0Vspvm$L(HLw=2HgnVEG+AL&~{g&sT%IPK?0y6^j(OD!iK0mR?z?Xshq+nNja zZvD{~nOq&oe8H&|Xw}TGGz?s7MUe=34`Va{*x7&XVN!$PQ9i=vZjbUsHL^+jeEqTl z%o@<2LDtP^WlGXOX4b!RJEzFa8L_~^;fpoUVA6I1ZwC;@_@|_tSPTB(lxz;*dY6<2 zQ5m8?#14Fn-884N@Aw6@$4%7mVljepcYRz@8sn}U*o~a*Fpqr3 z+qwJ29`?O=xlv{;K#%^JjMOh}@RMLAvbWo>cr%6EEEZUMHo>p<%exLI4Iu2_(}a_P zuucxL8bDrJ?gG|0m=I{TPo4#Mytnz0Y7t*W2s`lDPY91;8Wh`O?@@4zQb1 z$7fUIQY;O88_1v9Eyc!grm2s?5V05e49|$o>9N29%_udzUBi*AFy}pYE6aS1;Qqx1 zbfbOs6&;fC!GO`f%zEBQR@y+>JGO-lnC)=0?8zekxXUFEWL8X7j=A=iq#>z6qms{$ zT-$$Oxj8gYp&GUow=*tQH*-MZdC!NP$eA5=25nhj=W*c{$z$7sx&VIY$7S|AiyIoe zdtdNV2t){ce5oPvJf5LT&k^D+-kgGN{|pia-(BKn7n1;Y6az^qDa4<{d4vD%5Z%g} zda=XMH(K@#)!Q!-k&tW~DE+9t+ti1BbcZIkgJ32@>+bDrl-EA%M^^2A{rcugYUm?g z>%?tnXlMj|b0gipciDfZ14l>fd`41V?;s{}s7gPCQ!}EdCnY{EcCSMfZyXPP`ReTR z_$QI!^1q?BPu|$9tWP`;f+`TVLs4RtJL`6(w2^dB2og&u;ehPbk0zbwx2A8Kzt+j% zQcns{wjh3)>|LwNs;{D=JYKi#{A71i$r@`j+)iGRtB7?cyFnR1yPgn>(hxTlP~*pqM5SvaOz#O5~Pqej5?Rp)Jd1vb{T^P3<_VtiCRl zj`LJ={HcxkDoy03R;<{Cc+82+s#x z=b+D!pxS$tVA{%}{)9Cx`c`d-)w3ar%KDur+~Ol7FI|Vz68& z0$YotXYAgbjSesUs&)lRZGn5#Bnu@W+xz_van;+~Ej_VAvxywj{AnhtacAKovpq}o z`(;>&{+#hWRSS_r6uUUl?8h8o*DPr4c5Y@QoeU=(!c@)lONy2z=|VQcsds@H5YCZ# zCI63UgFs@i>|jb-P{|z!hbMjEyn&F#;3ivxtDz5Xx>lC`ZXLBV92ej(X^8x-mv*n5 zi46H0AL>v%!$B{EFu{_&@FO?QR~4GCge}d@+rMuD;;a3w@Y$CLM<8>(`ZWT*p)h+o zNe>HOu{_7ER#$=#G`RKExcE<@G)e#}#up;Qd%OFVvfjQEmCUW#xBVeHi5QMYz6`J@t+~Vvh>0*ysj)rK)X}L-=upIO<;!dc z{^A58Kv8qxE151r*Mgq7b^m>K7%IQr5WdhikIi6|>_8 zK3jzHrL&Z026)@}ykDh?iRKZ(zVlgmX)eb=HxTi394@y6MYnWA2BakU%pX8 zE=QnAYM8~uhovANTZy`I!{4TkaXdt)N{hO@a!m7;-D*&gs|;$Y zP!jdBPYm(ro&dnb&2FdObJnJ_!gp5JJ`rwyAKo4;Sd@yt2U+zA9gx~u zRoJy*iUk<(MMYVFA2zlbH5)PN$=ARG5J>9u2eQ6IjWqBm<;CX7Jijs;dS0IZVy?Y8 z+smb4{3|)u*+GcsPw41}%*Dbs_c|;p_rM&I%RO?WaSJ@y@7t^Qz=K^~#OE&_jMrQk zJVTMUs^mKS(a|z4CUc4)R||x#qsQ7VC}ZWZq8T1UjfhHSytCFrO6l5Vzs;%s@e_d5 zQFrWnNBk4zVtq-3mnNE~V2MDl&6=#&hy-?ikrR&@Hbt0V6@FbuSpZosUn3yt_{ZoXN6U=@z;gQ9EOP@<1m5gNwB8~*f+H~5Z!r+oHZ@e7fAv8R@L9kvqo<`)a&6 z-8IlhsihKUV@F*e@@Qs!iPd0+ZFOzq{^Jx5BkkEMM18cuviY(OKkZOmQA4tCDbGlH z@Yj%R&NdhEk#{BBz>;o5vaZm8fK6>@m3zjk&wgUvOC3=JZhvK@v@)Rw5I<+#U=c}v zXc>zDqcZN%Bb>1qd!-h2!uyzJpU+9(Ksu9iyIfAqvNrnWp&|RU{vomSJIB9f*v`6% zaF(~D;yZdT?FYhWMvG0);q+byqDE6O5kqG&X)nEEvM}Q?hnw7!va`OjH`*s2^+56xk+~jKSI6h@H?Q7NGWRQ!GXp2(rEbFedH}#C8Gcw99dl=j! zYG{k7E}2(m#6`^Q2&J=xb${gUzM?-+jPe`sjj+R0yJEj}-Qj#4*Q@-s`jpx%hBuJ% zMX1bzcRRsCe)WrL^L34VYN?*Azlq{)VSm=5`xS+t~P%)I%S({ibx) zVvSF=2&rq-HV=-fS1PE@&WI+jzMJ=Ex)wfL!HV29NFiM-G;BN3liW8!Ko;!QZ)!%1 z{F$UdB(d2dBbPAWOAtOURYbH%$rg5_r3MDNHGeiv5ZKy?hRm z{@R2e$$Wf9X5EA^CK5RCKyvP33FFh?Rlc(j<S|Nh&~?GcUDBfhW7bwWY;N@tkvh9RXH3*KwS5bWK@s>{xNb`TWD zpIK|7mE z{K0yY1^YsMU52kL0XZLn+}CaM|xnlXd(d1$`4$73&nYL=A>+IZG~3$+tG+5 z4fRY3x0Hgpbn8Ao6ksNOeZb;MYwicp1b^5g`89RZE1P z>kTrixv5`dU3GPy0LovLrerr%^HU_yrUBEBR;8J&J9JRfRq7_U-fcwg>M9QJ3%2OX znoXi}ODwPuet3h$G39+kozSxGL1ZG*xv*^ziT7pFR3hwx>n`7STxRC-D0loMu05KY z>s|E&;xD?ZORM8%!mLs~EaSSRZ+PLfLB5^BK2nitEMyI7AWSBeDQL;n@g*T{ubF1(uK!hPoV2lX9qVEH%moi2f1-@_Oettg-kJL*9lafx;VORR}=rfW2Gxg`Fuhf`-^fCU0VHSzzus#7)K8($f?JpOh~=FMnih-s&W!6^o^@Fv(| zC2|$F_itk_YT_}x$TO1G&pI&;s3(T}o5v zccyE)p!KMeh$+!=cKa9*ue$B%esw}-&H9odnpqJNHx!MyL9BjO$?VQe0HYi=-*W*+ z!Xi_Cwvlkd>qOI=Z$_Vaa=Y4A4|!IpTX|aa53#_?KtwIp++b8T+PSv2ApNqmZ#?Bt z-QciRgk8S-pNqjAPp2mHm$&T@P>W@<>IUuSol1jg-zxlq@qHhjH_wPa=k!c{RwH@c ztoH#eY2DK6*?-$t5Yy)47*1XOr&ylu*VsMvS{8PyZ#-DQ&2NlN%D=XAbf64Mj=xW- z+0!t;x+rG*qFm?jsb%bA*&63sU!!D^Ks4S>&n?o{Eo530VdYOeB3gV$r`P+8KQpCO zb-|tLjvhk)AccVLbZ5j(O#~bIppge?DegYM?vm|X%cw%*AN#lB0&k?HTM@8GJ+N3m zK}?n~eTy_&a4pP-IZpO11myPTF1EZ^aQTMRx490$ZaJsC7mc*{X_(c;tQ>jr(y5@NZ;}m| zg7->lZm)wC*H`2B7|v!;2V2a@bH;WM3TPL_(h!DGsAW(yQkELxYVf4O53=SHWFy}5 zVB&S-uedf60NlYOtOXwIlfu30cGA!x;A7l1H!St!8tZ{ivQ^Q(wgiHFXRHdl_(lP|tYPt~KxSq_3gCh~pg-?to~>m3z_&`tXP zdZW2${g-UQKKa=Vx>(i9tGHQI+56Z9ZvXMSv+SB*>S?3hPtVW)>fJOxaD0O!?B!!g zllw|K{ko=I3MmLQBj(|t`&%xYSx$vk>2?=c)HTURw^3KD5P5H*?&x&iXzaL?V^WZ1 z2TXDUM0k?>&$e64*ZfG)gKmkT@lnuGY{->DFcCvGv$N;cq`@3qTd8VTN1J}R4;nA6~ zjefYIJgf9wBMDA&{*fM}-*>aFy+!@UD$XSJC^;m-I2e?IdFO|f=G~p7%W0UAYIr&H zUSfFp3kLfZOa+M+Hq;wQze?ME5%QH(95q`CQLYjjsn8w zv&Y)_Y+gFex+e-h%{*!}$H+-+-dMw~J21n#%QH;~M4C ziFws!Rv4TV=fwV%qa;S%rUxR_VDk8jBH}cB_)r~Wa~7*r>bKI%obrwwIUxfrT6@Cq z>AbAl%GRWhPUQz+e#VOqZ>$RCh4@1*4viPY_f+`V`Lve!QG3fwgo(<&0jossbLT^# zYHvh~$0z;fOe_AT$3T_(z{1jLu%s{DOUF)g;p}^~;=0f&td@rj7qm*N6wdy)b#|e- zKwU{jm>w*Y&aU@tdn_v<7(8R^+-2q6@NTAnMG*Xnf!Boa$px@J!I(EB5-zXDm&r$j zH{HEhO7!nfb$y-T@!7&BW? zH)}I3*q~O@24~}E7zZgOJD`{9%wnzUbU8bmA;}5wiF4tsQ^J8q{2M8iLQNBX!>9#DaE67tvT@Ay5J_^s+_y zveb(TYt12<$OsP>pge2VSRypIDgEY+?iTgp&i-o836`O%AaT=xxh60@`s~X9yIJXq zN+!Hr_1$K@j3ro0alM6C%w7DnPn4<lR`-1pCbQJBI} zoj6y^xG($K#mcWrF%(b+!d_FpAKRh}X884@O`I{yhG%STxaxNthHcj3h_+>+0dBD6 zi#1!{iq{5aMxTrEwuq}!`;iV$w=O;8dM9Tu=ST#qv_y zjxT<8E)AH3&!O-6Bo4w6e|aN&A3 z?T%%%%pR(PWqwooOAxy#2m+%6S`8KlgAY^O`JU^+>N@t<2i~x$EzJe}0_=xWR+X)= zNz{@x4=B;w^a%&RmAHK&r@5GQfcKJsLXN?ijND4;tFjEgO1*uyDIJC1<;7HKtPy(X zH}Q0Xs zKunrdp6xFed{zF9Q+dgc=DWE0RKQ_MiyRluWJ0(O!)j#b!5;=;Juz+}|6g;MyPT_v zrLKc|r{CQAGk9_zU}R}F>#Uujr(ttJavKbQzS5g}VG#TQd!1hDTH(zi=dcSv59l;l z%vRiq6ebFwUZF>ZL3bru3}KP2U|UbvU#DtUinA)o21k;KN>z7}{l^KH zlBqPmO?x>syt%ArRfl3HvG67Iq>UvgEg0sD_+S1a!Rfj z@V6r~wSs?>;|?(<*(AZKR?z>}dgz1i%`=|3p`*~A8mSCJf+=EjsL}s<)rBC(AFsy# z$jX=M2Q6p-QR-V|EwcO#*KsdZsu!HRwC#3*J>w8xzel#GT2!mi}>GGs}BwH?VQv*F(sGMgJDNG51#PeTYY04IYU2kCl#ucU-SbxLnj=5&sW#n(vCW z&U5og*Hb47kTq_A$qBl}n{DeF#R`l5FI$G6%8XSs81IeJK>fX$v+|NsC}1#TU{i}Q z^+ONW^rQu_h$|_~T7~j;Pwrd3CeSUUgXZ=CKhN%WA@HFuTHB}#f(QZQY>TQvlYxJo zdNFUZHZj+YcTt1k#TLLPT1m5N-Oo)%Sjd%+vm%Q+-FkA=At7!y8$~!h=uyi~9vo9a)-Y zYhd{@7wdjZmV*W8O1y$f$48y8`7rn0Qu48&rfD!JUYHZAo7LtgjPPhR^4W_-Z!FYA zGcT?I$qV$HFEvOh{o}290NPIyETKjS z3?$>d5iM|M`{i=sIe!0LOxSV{69%O2r5h;ucovr7FC+JYlaJV}tgqup%J)1C zfag}GKyjX->7q}GrlEA{goPpYPxl@?l`${MayM zH1Jn93lI-$h@8m6i!%m{%~)sMl0x#5Q~D@3ZK@DCnPAuy;oT1?Lb_u6UVBxCo^tMT z60vcrHAZ5;S+GQP@Aii!i}sDLEJ=s^DFJRzySBoVdYwo=g?y(e-ZX0dJX@H_+kZ7b z;7pJ@qI~MK5Q?GoOuK(?1rJ?rSRaM$@hxn9-gf==_iAm)(K0PU^e-sHXqiRIVHNCZSaA0{*`1EqsC{us z5)^rjy_J!E{vuXh(rnXN+pYr4UZ9tL(2Q-GE3>6HMPa6A&b?+4!XL8{wDTm)8L5DZ zsID1xOk!^Q3D%X|5L7(4JL1fuRVPTsaKZSWKKf(qfyX0p;hj3BAtFk%fZm6pE78!3 z#FJGIGn|MTNyxxd;**C;=ay%LYcvNXWBW36`)p@0%U+1bCODG7zGvKzTDsQZSDGFxjq>zyt z<*X2Gi^%Z?gh>3}Tmp7JC0D%jXHB&URrX7#Ic_KIcN>$7L{;o9gu!)A=o8s=d) z3LB7f3feh*ZCB?ls$0E1Ap52B)h80ea&GhVr?>(C46Yq&2^jXF6*klZS{P zfgs|}IpBdwucYptKZ@T{a6y=!5mH`*4s>5dQMBsve+yqh+ErFEvt3tez$y+jYOch$ z*SegXMqlqI7Zi_qPumi5A2R9vNEP|}@+w#a`i0N<2#EST>M@>}t<~~)#0{l4PwNj` zJd6(+RXEjQ5Gwneyka?LO6MJhf&`fWx9;8CX>`GCTSs}mtEq5%xs?*XnhT~*e9#2g z5*SOg3?gAq^E&hRwpf}_-%KlBrNc?Rw+5YEDDN(g3!6)=cl&=-OL%8GKGp|#<4LDP z`fn!YhL2?R8+7)1x4o0)MrZ3wI@{FP?)QNnsq+A7H@8+v3D8{$O9_IO&lvEH9zXW^ zS}t!8_)1ZyitUK$Oh!g^Xwh6AAL^SoJs+}RjmJ1wfz$Lr8$DGn9BtZ@2gmS2GZhN1 zm)6Dxq9`&Sud%+L@go)A90z}zk0*oR&gusfx&`Bz=Rp$vCn|(MHI{h$fDt-}gH4}K zT?3>Z1|a|b1(B_=@EmRnl%ey8n%a>p=dUj*u1COr$Uu^jExF@k1-j0~1WF#Ce?lS2 z*Rii?R+1sLUT*K%OTQs~7jg#VA5=1}moU7c*s=Z|^~CfS^IfTLz9Z;KfudJEY@dij zL6Ve8Nl0i+mLTDwI797r?J?NJ!oV-7|IV-HPK(Ko9Trr3D&v#yO4sS1PQ@)hbD0(c z{V3og^f3unT7ovQj>Z@V=U0@Z4~Zm)ANCzisy)wRJLNHl_CInJR&< zd(~hI)tD_v9$KK)tMRTnqx-1ri77a`;D@*H zZ&FUk!G3jV_#Q}(p_b(UT~gz*nVGt_()Yrp7)~;8lCOBQImj#Y?~ESuWr5@v=Raf#&>;( z>WV2FGcWZC=J#?kbleiNGE@!D3Dgx&@>w1bN|+0gXUP;aQy*#<+pTvp5BW6A!|7!D zPaOe5OB7ft5H2b=W!y;4ta+&!))h31@k5>JhB}JMqc))nVbv)okyE+u) zGP_wY@&!Mx%eY&5P#>lGl7f+W72cA|!`DrcEEqMDGpz`_%gPg9%=*Ok84dqWrFon- z*Iiw7XRn_?W#03#gID8`j=LdGI&bJwn!p)@w*4eebs|*FDTTr=aC*eT`0C8atp@sX z8zv6ZeO1KY6V0+3f}eEldrl3zY_#Z*By&GI$Ih)u%rrPw*MNPrMk~|<#W{TP(YLIG zP~R;46UGU=r59vnIK`{v{KJnU1Nncv9i;5!fe5*O@CbL9US0qhSnyFDZfvf>v1Y~Bh~|V`Z~_JY5+aTxg|<=J-kOO z|K{voVW5D$&`*LYM1k_`@&2z;=i?z_dT7o2eh9lR@)tQRVN9+!rRzzemc489vCG%- zo(6ROwEj81u=hDs9Utv<+k$ul+2vtReglTB5o!9Z%&*lTHXuUke+pCs9aKnB#b%l8 z`ydfMx7WeI={m8RKufeu@lj1eMNzWna*;SZ>`ccQ9K>k{5 zek_eEZ6WmQ+jTBm2n=j?#D-T%FE1lTIgxR+t?V(5J^E;mel67{ChfJJB(ow{3|z4Y zY;kwlLn9DP5N8KDidJTis#%S4lC*VM3O1t*Szi4VvEU`O!bCv2e$2?N_Y)d8Vb6C6 z2A#O0xg)kUN(=4T>D}5{?MBL4d!B>POzMx9fZNctW1~i?Z5f;Qs|RXC-0q^^9EHM{ zSfsQ;-fK5z0#T2_=PeRiplMolIVD63a10Uy240ZU{-9?jDH^DJjP5fV%s=9(^G@cQ z+5Up_o=q7+Lpl!ctMe1&Aa{NwIm?y!;G4bAWXp7E&!C&Z!FkPJjTgcwsla*?NyXw1 z`3Y?Y9i{O~gA;j>PA&LkDna6Et7z3z_wV+WQV+d$o)k0_@`@$1%S=6cDh}-TSd6B1 zp?YddsfsdcMNRf$^)m)Ij>71!h0A@$kl zNt%gSUUhzPjI98_Q^`A1>jX?PWWd_&OZ13x;4psxeiEeYH+|H2n;lBnsx7JZ+0mu% z{H|W6o6wJV&t9ZupAnVl`_(lOu_Z~=gFK=^u^i!p|3?1HyQUbZB zVb@;wJG$?dPB8v0IjMa13_Ue}!<>=MP}@2ZSc+~Y25V4C337q8<}z@G1I)mcPF|4W z4+q&?hR$w=Te!ku-Fx?ACYz(%$;Be)}XA28+8Q@Os;XBP(6TnE7 z2z5!k+;-x}Tj3^5IS3+a_I{@BRRTZ($zOBH0ACypr;?Eh1@f<4Qd;NwRtR?Gl{m3W z*Cf}nr_<%Z%OyAVKX@)&b2GuN8XQIJgZW)VOMI!|K=*m{U=skN{r}7N51jbY;gdBy ziVD4J)5*tXXkT6%Hel`}sK@%lOV7IhKq#od2m0?qC?KtGtY|k|FWB=ydxYoo_&NK= z*uuhMGqV?&#jTKxY8xuz&_e5-8YA82#iiytiTz4RjJR%L1h{Hu73De=pAskf7xSj zF+0^sYPVTAYUH{?USm{y4m?2P%%+?N-CeRU-w_$;J{n;##OVmwP(|y3y#`1x0|22Y zsM+xkAo9%tLbM=(=!3byAUzi??3*UyZU{=v-lMJP>ly*Np6&F(iX8}ogZzX(nY zs&b8J{L;!Uw?N2xOrIf%rkvZbx7X}u1Qj)Ligy%<-uqWn>6ALO{dn`0QPN-`Ob_7; z?j_65KqO3@dlLFqTzes9I5D9g#YFnrqR2?NJjhmR)qvCa4^+wJ)}pxI|6-L6B4;EnB(YlDkO;<2 ziZnjl18m+6YashgV>R6>JR8`f>3BCIf5;gH2l(6s3A{rj)~w7p?2HFTZz zLi16zn3q;5EkC@9F8{8KgcF{uuJ-Z?7cWF|fb$_RuaRUotbK&fRi4&9nAc+}-SK@1 z2s`@8yZ2r$nKde{K&JPfNPZaw_%Bpq+OG2Zy1Nvsrd#$o^JRcUDE7?Qkv?n?PP*Rd zN^!5iR-jDE+m@)Dm-)^MIB;qRP7`;B)4wkiivUKf3OW|1zKSUzl_-05R$&iX27bJT zuQ^ZU2)EA+T}=EgFF8!o;14iXd20!j`-#QhD{9~b&1&SEP>zmPQ~Hpx05Gw8Jx%Dl za#H`oYYZM4Xo46|Hm(HA8L-Bj^9wnO;|!|+iSF+k&uZ&LgQMe#v;P$k0N;6wMIAJ` z4tMFus!4Js(*%rKK4S4NB&VYRCqX3%h$UoAMIr5a1E;uBfvEF8Q-d`S;3x6r@~hEx zBM;llUoIAd5L*N0no;xZT6u1TOFH+Goqzm(OBA!5t2ztL0|T;+g8~nI0~aTQJmP03 zcllndz=jq$+o%~6uAA{Q?!&4TTVDJwuLWi&snNZB=@2#){31cRX&TnM^~NOZRq*Ef zy1t-STWuMt9Ph7q+TM4-(Th zTfmgefhBg1Jm?>>IL;T8JbJZ72@JqOth5v+Ca5Kz1RSI&?2=FNBz z3wz#|a4Oi3ok@3}B&|9v38+WD^g^!>+Fm{XLFLX&)~G?S zSL6^ihOnO?MurPC8Ys2?EUt)6^*a!w!tYlf{AlDveH@4?x%o5(_5EGAF|Ny2t{>N6 zc}pv<-FLcA(OhBS7D&#sGJQ;(BR{4Rat)pU1KC6$MRWf~Yo&Qs@U$A&9=!c`+8YAA z?ml)35N7;t^>f@~)AdOOW6wFV?ut+{t{>EYS33~01KY{`Q#;@B^cRmpHww-q+n={= z6w`=jqbyaINY0@N&gj(p^P_f1?FmP=56%Nvq~FZvXcNlohl4QtaylK7 zwproqUyEL7S+oPnwc|allyUL z%xxD$o$48>H91^Pq>ZOX4V|8+fq!_CQ9bRg6tN=|&yJ1jv45l@NwRrrK`KpvLWtX_e3bY(ex`ZWlA>CXAdy|b7_jtT^V2R1!N7A6L zpkr_IVbqEm=)a;n7ZyTRp0UdPflX4dZkY-0ay7-jL%cmk7wy=~zY%qFFU$6+A1Zve zasx9)@EHYTk>?P}#}2%Zwk*B7;*;ksqW@N80e9j46(w{AzLL`&05+g6h4-epa%Hx) zohZOzpWTvWu=}a2m+o%5)K2{+aR_wE45QhVA(ftLZ?C}HrO)-76!nyn7 zd6$!rxTB|pz<}V=^0D&EeXZUIYo%aD{y<@AimcUf(I}(w!i)n+-(hS9>Ge(SPU=^r zh7&V}d)npPG?s98oVX`f|tzXE+fH*h-=h+b&fkh#+@o! zWT6z&@sLS!deIv5lnoJDYEBvPd2^@K_F=8x8F=t)wa3cw%=ERBYA%hrW`870kj9@w)>e+&7cWVE{#q}vit?N2Z4RxY4=;uSQ}1Q|&en2>P1`>DG{6xSoIlQc zN_iKJ2E^eCE;n5dfU|h63^RUyFjR{dH!AxzWV!5B@xqtAwTujDf=Z+5vlkjdeEmxw zNO%nq8jG8w9KM7MF~^JN#$T!a4q1Fg5O@RK94GhhG=^UP8vOVDkLyrAOfTx_uV)^( zjlSc>1ILP*=^k;1^tT_`{i)Ecd|i@NQ?c=BY|?#T>)yINTfmgxSM#gM5X%wid@1^U z-Bh={F&XNoOJK4bmOf&q1#JjDn@>EXPew#ifx^rPHsOg z%QbpJN8)hmqn-!Uthj~OR@gsh{12`j_g!t57fbbk){LvWuKxY8XvexNrE*OkdsWw^ z2Jh+7jA;xE4 zrl_zruE(-Yqpzg8&Z>N|b|wOUOUWQJF&h$V^y(M1>E1~|51 z4OELC^_&VCo5uSswWmFc>m@N>t#uVnD?M1YR1H`y{oQeOhDMi8dnqiiEpNQ|%6!Wf za`AE7o5uOqL#h!#pw@F4&v$O}eSKHd;6F9l8NR%*T&tUA{5vN#gJ^j?_OWZ|ss?_7_mvtYfq3=*IxhgwF$5W55SJd(Z?eA7R;ITQ@6(U1 zT4J2+I`piSItLrA&&5FlY}+g*kHlYJo2PL zliT1!r$y{CA(y!DTJ^;r2Y=MT5z0lcJD>wPdxd_zzi>yp+8GPpaySOoRP(?7kpNl2 z89Nr%ZT~czY-mY{i#;o>Qgcd5;#LvyuPjL@rL*EiIunaYO}6FQNrwGZBX|~URX8hq zI42vHeD%1$So_&)^6A(lZU>K1^7{*SCEP2w33Tm$Fy2pM7_K$gz1NFWAjg>m7Ih;f zR9hsQzzcujWGoQxW*&F#Ka)74FG*K$>$ka3lepf4;k1*kf~pz)m7TeiP`bU5Q5afB zXu%L3M(*4c_XZs+(#EU#?(Xt(HQrG+UN`#}(do|9KeyJr=yw_dLrZB>jwe<#q1T+> zNQ+0b);X&~!U33)xw_1M1tbiX`q;@$(ycYixi;UYXrI#T`b1&r!Z~FhuD-UFByXPf z$hkgqnfdNtzr?ngcWI{J?~kx7HEwg8eAPIp_iF|6ERHJEeE zpb44XIJGF*PgzWu{DPz=EXsmSM%4g(mo}&i{t|+M^!NXu24Jjc`2Xm7^LQxR@NImm zM~dXBND+Ek5TV7AZD_GXmQb=+_GKu`SZ0PwQiKpfD6;SCWSQ|0*~z|)nL&2L*k>%m z@48jr@AH0s@B42b-E-g9bzbLkoX2@wx&Bc1^-BJMG__Xa0udoOfmXAh&|+(sBXhP( zvK`uF>fsx8l55Ds7^L3p^Tv#j!TjMAr>lsGaNf0n+o=MaA5Uag_hIOr!#2~=p{>hN zNcUi=t{?R$fk&r@?KzDt-Vf^B96)y*_ycsoo)zhar5!rcQwwg@)AseVK+2Q-=crRkkwzx(HfNrE}{-LUJ1+nPqCt z`tbDZ;!tL`!giz5;=CP#uN*^CZt`PSp6XJo;&w9aC%q8)?eMI^0nx%G=iX#afn1gyPfH&S?v@l8$Y~ZOO3uxQ&UH3M(zIE8a-(v{Tt1ek{f1=oH})NUavbEc`2Z@r z63DL9J4TH$=G=|YO1-wSfKk1Xx?~@l*EU@vDqvK3?R&1T<0q_8GjcGG_c7xB>TNuu z@8neqeYu^$#m(f^CWFj2$g{`hIY;G?>`j^5izeC+YeZobt8(HL7TYXE5{nw)^4@2* z>`x0WnJk#P+h|veg$9@3l_%pr}`{wwk%rtaLzKxdY9r zpB65E6rf@?RU;ZYD}wPJUH(Mz#!b38Uyf=`AKE0*1(0(ukfh}R8WqdFMp;aBP+HTU z$KKVu-`_>OlkYG;9eHspRzYE?oXy-GoN0e^Udz_LdXU(-gK<7)fWI>=yc9pX8nGJ`y5jf znhiKMR~i$OInZBt3BA^Dlo!aHXr&~iKD2wUB5d%NR52Yj8Q*n2X{jhNrUc<2ng|twtW_1}ClHwW z++xv+_KUagvJ+wj!d@N`cyEFah)QgJx!y`t@rW9nsJdY zjjC0ZCrC!`FgNy9F&0DTx;3Q-1Wn8P1McnM^A_yYB-E*ucC?Ro0mK(9H5$7bhcv9} z2%=Q4CC`P&Rh(n3f+E>J5xs*6`&7S)%eM(@pQWc>?B_eXh1Dx1~c{OM=aSeVy%b{_@Q zdI*4v#O?|H&`}7eV@Xncm-A)&^v4)Z3>II@1F8*7j=g{`#Ss8 z;jxa7q~W59XYI}8t(C76Nvm7M))9eKX^FW$V!EA zmaVjXM&9je+dcGWOS()$_PiDXwSo|R(k>SlJAc2H@5WoP~QIz0=BdYtaP@s98y7%aGLCvmv z)%6l!k=_$*oG$DZTd=1+&?I)ZYhAPpC9A!r$mVkxKX;Eb(y=gGLz>JWlD&em?x;8{ zDzhqp$hZGg2Z2K)DtqWLmF=U2hx}Cu!!=WPRd&>bxE`Wrs1-T(fVT=;caELO9AqDU zc??jkOp4CU#C~UQXMwjh*v;029m*hvZrNx9khI5jJ%$l z|9t-st#aM$QPC#2>USf6v{qZn^>m}rbr4?vHuI)pdyy?u5C`u~pX}wIo z&%bbG*x%CQRB`Nmp5{tr^Bf+;m%;ph40}`JoZIBujp0_p1a`_{WQ%ANn`B|k^m5;T zgF@;+B0ulDi<{dE4;4|kc9Xa&dn{&kReD6a5;pG1hUY48$xkG8*FncOYd-N5n*Vs7 z)$%^e=TbdCx`!rOzwJ}{b_n(Ksm=m4cLD0`)#wTvC%#=&>e)4iO1hT=zSMQ0GZO7i zvb|X`GgBZ&+!|&^E|n50t@Ki_APHOHroMUq7*;$ASUp+xW8T;9y=ix`tIicu&zpiM zddZ>`dPbH&^LZY;Hqd=8>{QuA0tpE)6Ws43yw(*bOTdybAgh4A7MIpEQ09k8==b#8 z_*HhnM%9%QR48_Xxyc=XKDVRMAej2Jb;HA6rZ9(rm5{b|0om>|7WWK z_P6t(tsfX0u~h%>Pg^CZ%=ZvJT(+i>9M(d2Jj*LLN7%Ki%rxLbl>^4YR>INkUg4{> zD<8)lN{z7pEK_oA0tB1!&2jEUQoI5LP*u)>M1O=`#coE<+G;^^eLUF}e7LFW>e-?f zp~*)gK=WFEArfHNZzuLWdzD^p)UeyP0jg3Z1X+1C6-Sa|Z&{IHgn+g0CN{0PlIpv& zGCM*rH}wQ$>?Zx84V3q^oHlntQxikVdqy2KK|HuNdmImX<1(*P_xH-Li;PpBu8@w#ipl80VluMZ$au5(gWq?+^1PN)xrq> zX6XU*jrLM&?ZY5R367%`Pm?VS2wNm(oqEofu=$-!4>tu`yu<*`oO^Niy>Kz+Je7 zyUsTHt5p#1ERmo^dK!SybARgPwlZ~C6?vqnh1(X#2{-fJ7HBw6hhN;9_w~wVOZeK0WH~wbGh1iQ_=t`q*h#SBDZ9Xm^zrqHxmuK(`wkBYM~#>4r~3;QQx-g(L3BbOsJgo z8{Wax;l_6ZC}n~V>AU$93ZQCn4pW0;L$1I(AzOf7I#E~eR1TYg1ht;Yb~fOEWKDpx zRqv-a4lfEVR?Fu|Hv|bC6WoKA-#kKU7b27G7ypw|6LO$`LG_wTi>K z`Uub2;Rk)2I`e_rgrUBalhuMR<3ESeFZ8P0m5PsNSoIeqog8~J?|H%+LY5AT__@&hbcWOquh~Pl`S%jGtph5<&%tH5(}9cZ^MTqxHA<*3fCU z*D5wQ{sNJ{>!)8%*RXP_%g^lvL9rdE91^qZB|vUD5qp9M9fCUSa|*aXA^E}U22AF^ z@c;!#?ykE7Ofd~sp5^(5y)rpAY0kvtpK7w99_MI7DP7O5GWcRxXdmRxC($>Lwti4Z zsaRT@Ut2Lx9ei09H`h>~=&$Hs<~};cRlPG_SfS#nQQ_utw`hB9lX(-A1TYo@PyqaO z9yG&$i;}tPfTQ)0fph#&;;5>Fyc2IcB5GF{FO{{CzioEVvTq(E<+8+rTAEv2Ypy_@ zXRRV6o^(=B5i43HklB6DWL<$=; zHwC9<71C5x;&mI6yxf0Xn#XJ&F4BLp$DnEK%rBp+erY+%so0~*dY~1tANW*If+56_ z@iD8KQfBj)Mb@3SS5k99An}7bX>$;`!Q~}6mdTv@7r#aIi~?juQ~d=CoTss%;OgF0 zh;;wtq8;aG`OVpQtt7zwS`++eCU_NF7qYcO;VEvy1xL{(~;iY*XO z_AC5F{l=A+a@=KUyl$9DwQ1|`FWlF>+ozEXi+$mB1S z^OE*ulD<4A&UW`Vzc*SlQ>t+DzuZ5TJA(U>HgrW%eDeYq&+t?4Iw1 z3u@nrK_s-`7wLhL$~Yr-`Q(R<_uAr+p*h(@Qwpl;(oSsn?w?eCS<49PyaHr=T0cME zRNO_<_E!ICRNBs7W1|A;t;V)&5c>1s7Wey!_o~F_$&R2sS=~%z4ta&$dj*=WXEFyG zTRN0|qU}<6FR1BIt0g7`m%b=x( z*chGb%_ZE~{CSh{Em>**J^gb{3aOK4v{P5p^W(D)7ait*Rt$OhKrI1QyoEW&dQ!{o zcx8(O;?k!ZmTO6Dq@{BNyDH>sVnt;U?bJWh)>INh%6d@UVRQc0W-AWL14OkC7m3}B z?vE8^{+5V7Ju%U$;h5jC>)3FBFS38nvvNq~l_UPnarCa?6^#sNbx|_q%2CbXCbwp( zb`fl3nH$`($|6$6^?O_?rD?A~JHUKspeq66T&`)B-vi%X(Wl43YB?&vt4p;Ip@L^u zAaM7g!n6%)+%bHlA(RHxlP}Vu&JMJ#?W>qmjc|IzP#D!TO`GbKBzW8(%zx9It=ZGQ zDLzk00BDP%wD1FC?TXv`U`K~!(JWnVd%8IR1?bb1W#KS1IrIDMWD{M4gs#2PdN3h+ ztB^N%2e&xkp4E%NdHUgcP^rXjZ=FI4SPu*(WxmLLw47H%=70%lni}5$ ztf#q5R+u~Fkc4Dogz3WY0gK>?lEdVtY>+|iPx-#a8%f#=R=Oxt`){xoaN?HB-{i78 zHb0|Gr5;iqqv(;Bk%mRNjWtwo-Uk0Yn1z_cpB-^#gS|hDDE|Dyxj!ffI%{&f*4?OI z-RCs>1PHiYM2i$TdAX!ZqU>K|7aEks5|_`-Qo|lt&s4IYQV0?bOHXzz2WV*l%>8kg z$$^?46eZ3X&*mHS>`+%Ms{HUHSGL+`sS||}V$a-uSxG7j{(Dd&J4gp0Gh?H&AXg$_t`lFyeJ!15BY2R>`_Tf|4Y=7G1 zmU78zmd?)4#+@R1Sj<-UKK*gVxZsg5oo9b{1Q;L1%wB0^9RylPLj31Ak5vC31Y5IC!Vltl|Bls(_jLLCEUo^$_lpl(9XeB_H zqQMY}nBeUZ7@Sx%0GEwjci~gM)8*O@2K*Q9-G9aWdtP(Y&n%^AtEB}-QIJY1%9-TLMm!k(K39b?f&6mRe6O0 z=0WMIJfDtk!Uff=8*&LfDPQEvq;RvCt4Xd(1Dwikvq;j}Ui@ zdDL|rfz?*rJhK)2*A2#|APXG$6YD{9X#pX6@yLEy_bZS?>9}zlEr5Ezp1ak77o1G$ z;Rh$)vqhq4sGv8Fs(P-}9IJ@8R;XTPCQ@kH&`C7gj+3sQL*YW?@HJ>nn>nMI7UM;V z9S89B`=iB6?&pQFrY@A%P!qURj0gH7OWn06g3uh7pkFSo-Y+-3H=T9&>+Ec+(=y6( zq@w)68m{K-A0W1~(T*P}dkxG}fCuj?ytr*=)I98vmj1(4IWeTdl>=Rucnu7aWVD!k zC7DrWhyX%X0!Nw1uv)Wkmyzv6!+J3P0_XMT`RG z2^Rnv7LJVceJAkqU0goSTBZhy;urKbWmb8zm%k)kT~O5{_~59Vfv}-rL96$ncxRft zVhy*y75O~Yt)QL^0+Y2z0s4*i(E-fQJRdU@U^Wl|PbqygLZR2zY!)0i?;P5x zigY_0Ik%~I*h>vau(EPf8*N|$=3FfMVJFs6In_)XXaEE~>f*3`zs!_aX;Zl1usx(U z3$if8(9>&`TuPKva@NTCGN4*nct!p|Z3HlG4%6U?eK3A#w{GxyXchy2rv`gvmd3tP zv}w*gE_%QP%#y}SyA{fyoRT6`SA{f*v*2Tt1xi%i& zq?}8B^7tlDf+K#Gq(A!rSQ6pxAkl@?rJuc%>iZ$U6H{tI4I4gF)!yJwx^bFI#(7Yo zw9qA)_E8s)xn4M|HxTV@CO2EY3>~s|19P5QkC1MKxYGI3Jr>b`QW8=CU<{4pKEHc; zhv#|TskX)mvfz<(HT#s%c4G;Ew_C^jOZr}4X06Aj8lVv|c8q2NCazUZ9;CO%Ge?`X z+(=|~O8{;%>FL=OaJn^&OKmVqp2b7S!y>`?`^}M85N7_kK};-zaFzi!jh~<5?7R}= zzl63ur3)%I||3bR<&xAp?as)l_#JS?~+N3J8r#Rg; zY|SB{AJ;600<*oPUYQKym(=TT0Kt6AM}EA`avF4-ML5vs!`pkw(8pQ|IP_BUNoS7_ zV&+r^wE=%dXme$$qffB^b;hjY&Led}SdSeS5v0q6YJ@5X;7_gFuT3 zLcCIXz{l6u(8Z0+mcvXi-!E1;&a)Ok69U%=0M;@Lrr)tL)D*!F;!NAy^8`QERep9GCXz8C;E0D&YzAg>dfgY~9kwa?AX``_ln)%E-YH#@iKBc=7OX*? zfTfH1q27d(9d(o`gViMlI(n}?QbV9DY^kc zrK@INC(nSEAMin{YMP&igPf0FBv|@ROB0+hkDma`9w*``5J_jICI?BOXDpDIC1?8Y za-De-&MtJx$WQxEy#mJEL6YV*XmS%2a)YmU1N?%h*MCNNig&y224V|rD=p#dm()wj z*h__g9maODs@tgQa$9aC4h16drhiAT2SuGwD7V~vn8W{jFbL|%A51KFyaFPxS4)HI zMF8}5ba#E``VA5X%JgEbtQfe2 zBq-}=*KAb!L7nR*Uh5jB#q*sB@;XO!jkR)c7l2Uog}2${;^vKDGfVbTenj-p(0N0; z0$sg~EvC44?bJ1Hl3wJ8Z3ZY|A_PW&ZhhcBC^`DFiy)@*%B&eaxlH^_y-2O|5KD5= zKA#Abr%OOo2o#T)%r1$p&?-?>=oU9zlk~EDh9f%TMa_)u07ygf;*{e=mG}OTMJqk; z)GCKsP5`BZL5EMLvn_F;bAor^1PB5U$3*PYr0BpDuD?yWc$?JJw8hx`@??2^#=#F* zXifWnx;vYoV0^Ke8RR_gm_zz#n{4Bl4>18q5uE>MHs32VFg(czV+l152(>GRCGBoR z7BE!cSq2tb0Hz^MotegcZBzbVfa~uYCjZZFO&HXzsWjO4Hy6*uJSz*nu-6qF<6RSa z1l;f;+;&`=SNjo{ zQZPPx7db!3GZd<@xJG|?t59TX-d3*0JH*6^2bsUk^-dfhTafN9WgqO<19Pejer1DR z&I_Z%XFMn8aipM$|4Cu;xA`(Wgy^ruU85a~4`XbMy6TzF1GvO!r4&iD9t0j_uC`IU z2338cGZr;BSg^(yco>++?kFxI*4Xq5qg1v1m#aFKpA^7$Yt*%AQ_1Lu3Ovc4rhMWfZa zO=5Emke$<$&fJ{`y4wQ<7a0U4zVsozMh|e2(ZYdey3p+gLf#p(6o7+XmGe+90%?Q& z>cp*zQmbv1b~~a1l#!^anSSF&pVO0ZTX?7WvE;iMMSfs=K?(BtP}*Cu?|A>x>Shh9 z{Sci&J*G^m+VrsQGqqWpuXy8FKLHZcHM zp&B&3f&C`+U7f3-wg7TYkV47**OFUQIHSAAK0UbpE5_{HPYnaPupDFsC_IVW#TWKfm1x5YdFYOJ%m@PBRju4bKWkJY(#Y zR;lmK9%`K<6pbY``#eH`(EP{$v?6Z*|637Mgu-B{g6VL|vEY?ivf|chrAK$cO%)?u zvoLkzo@ArnO{gmw>w|5QQK;COxPzK}Cv8Q|PK@B5wEo8py>qJp8%`c5P3{Cxgmq|@ zp_2izL&1-Oi#)pa)Qk60_YvdYvpOK$$^cOAGb2=Y>=m=aqYZUFU)*zf%i zpucN)d@sJ<(fATj1FQjU={fQ%+Njba;oZ)7F_@C;yuX?b#A%^ng=cqMco4UeTcxg8 zE!0X@Gfif?6L4HDO16a&xDLWGcCsSt5E#fUurAwKv^czxj7lEY%K&b9|42I2@O} zspGkNIIhI9*DCX-@X6a$nH!z0X>sOMLt5pI#e3eD zW~!F_F(RX%b?wY?2wMqrfmTDjh!8!s#c&o!Z&4o1o;e2c!*_1r41dvavE1I8@i*mO zUs7aHM{w74`k@=i?sS>%vdon(WO}>(@B7^hz7vgFQg0tXHj0{7g12k(>Y6VoAWWdz zkAyqnjgZ$N0BQ8Swz*e|5ukRUHyQl>Uzf#8M4Ukw+Of*i1Op8r=#T65%%`~|l%s>C zU|>@2cZd9w&elA>@L_=mxz0xH3aYQ^;cg~!;KBrYkwA`(chxEZ>u0#uEwVO70GPN3 z?aq7)YV#@}YBvC)_U7h@g8bjUU>TG$`G~(8&N>H5DchI63Q?rJ3uvQP|KmSia=79* z*>^r00S+a6DF2`JrxT(?Tk8AP!Z^Yqb-W)Ci*KQE9RQ>@CW&i zJllFIH?6z0=f6Mq)Pq8wtoA3x;jBHyS&G8C&uGt50W3UXUw^>Pb{9bhZux0@fH8KE zgeT68#d{1&?~r$>(~rdq?~^;i&f=)3QhmoEJB+GmF|!%B6_@nfDf#_d>jl?GlJ~w- zbV?5XH9jdMx!XVJ>h?GG3#tHQCzSYKMPVSt7@&QZKA1XQYS>c`8gVkOsioj}1p>a0 z#}oc(`PxDis{*HM!!K$i<*8@{p<7&xWa`A;%241foeeS^nP!l6J?hz zw{*c+Y_7od71cl0v`Xu9SdKR2_EWD{(d!JVH(w$K^Bdws$DVtX5c<(^i2JG#^yw&wL)x`$p*%WJZZ^fi_cz%F3B27f=Ug zE;2g4op+yHypsZ;itE)L^oGTSyL%d{k^{8<(sLt#i83hKLvQjfm3`Y={%iiGM%4g` zwF)3}9Z6jIgC^fRP5??KF4g`3PkbFPrNVQk zi%Fdws#@*1rBN0-3!)G3ji{06ps&GF5-tE`=xB&^3^x0xj#p~nv_D7i!!^y0d-j93 zvHuJdxH>PK_+ix_4m~P#H&Q`WxR5AhdS9mBcnA>kj91po=;Ojh@}x)uR;$fRoZfyB0HJXkL`Z?gHj*!OD+~G?>WILny4h}0}_FTlu9m# zF3D8!yIB0P<)DbY!6wPETe8gfej<0oQgJtnpPNhbB%`VDE(M zq=c0wQ)0;UN0l`z3)9T6w~|*^yfRysWDPrQZu;aoX6@g7Ym51hkezO9TVNk%)vDaHrE3&!p9x$#E=_=g8ai`ko-1 z8h*1{CXKvJ-j*&RI9;73lO1=q7w+zT&6QJCufn5bm*M@g>30J6aclAOHqyM}E-9!+ z7aH9%`Uu2fR)TbX?%z^A+<*M}^y6v7?R%pXhW{zC=oxD7K zO4hKLBlAsDaYE-xRL?^DZu;86$XP&z(0zWc_fVn6q`YBH_hy=%Xij5|fe@`N{5M3c zW|HKtw5s|Sxvbb_>K%qA?W>0aydHkvQ*a~LV$xl3QC%~%G3nm7{%p)CG;*o=;R-1W zNENGdx=NIbyC;XB-_DAD2-P*Z|M{Mw;v?=z#PR1k{N9Jvhn`cHNr!A{rK+ocr8ic89^d#&4Arubx z0Z2MLkk;iHHT$tdH&a{`qn#dAcm-#0V-IMf+1DN!lHS zmcI^A?WWVc-TtgRewDjKUWhJ*P(X!!DZHKB_CYt60Z9L1{lNxSiRy~Mqd+RldbtD7 z2_Z26hS!6|qQYVEI6}Iv*%MjDQPjBfCD?X!S-e{bAOEw_|7q?DxbTLqIN$C0 zBx+ddygf@Ep5G}rcc9|Fx3jiA2vLpJO;XxyKB`b_k-A=_+~s0ysLZiq|9 z40eQB3IK6KVe^u^0U_&78kv)7$I*4wPE1gHcT%e z5|4ZOmPPBW8!m~jIM5pk_YJ9X(T#6o3(wHcwg<9pw!#_Rjziaq0{E_MD(-u1Ifv_37IIjsyDI#6mC-@#2%i6m za+giasgi~+5MQB4HoSa92&CQiz`S&b^;eYx&)0dRMNZOiFq5GT|RbmZY?dqTYL+B+oS12;B`d* zi26BcWr_IE>n_JQQ5k11ui`|;J1{T1&xhvC#9v$l%^$5zz&tod##?VMH}+wu#(JH& zyc}oJoHVHV;j$ZsAmT!}WiyEJyB8~;9%Y1vFmoZ}_qQyX(R<#%;(%x9-O_OHe(+B% zXVLuTm!(kZv448>phnuidNGqB;)Ejn;A{B#4WANaTET{enwW|Ld1LHh#azq?^dWS} zjM$)UzV$Yy>>nZe@w_1lZlPIO;zk|ThhJH@aul!aG*|-evwnsr95;I47bmRk#0>OAgXqAl%;Z)gwbci24}!8;9X-{z|;;%V~M3V zq1QbHz3y%^>XwO%1{Z2g{1~W(o~j{cX$4{?=u!0nD9w83aFLHFDF7LwO^^sxe{8+2 z4-EfPb;Y*RJ7!j&LIgp7kc}@sxco>Egg507sAl^S9*C=M2rlR{@Dkq|J<&@I^3_+r zRi*m&-*a_*1|+LYAT-%;a;N==&z?|J8QLuvmcWqpT0L`fC6`aiQR(>T)y+{Z>5b0Ff_ao)m%anhEIGl= znAqkK*ZnrJY>+g6waW;9tuq&&W%D;K*Fts@#ec&-5a^VpcmFz&_opY#XgFty4a4@j zm7&6ppcgNkXA{_=1*F?D6f`?hruGHud20@;S{!vKyts`j5?4IHo_bAcas|;=vn!+U zPQ)C&?w()vUBfihQ+!KZ5-3?S@bvQ=xVvF#VbL8&Z{=0&RqiMzPi?9e0VrC5(f0Dw z&=VQ>PsP99@O1&TWODuKS}veBcLWvP=G6&BrMBJ9u`XGVo3m$9vqDf%PD_!#WDC(w zAdC43(P9j?OXHsun{>p2zNLiojf~<^xTyKfeq{ ztf>C{G72y5D2KD0S565F8GnqpsO!42YOu)7>ou)DVCx27#R!51G1O|kI|?YXVil+_ zrd~T=YF*1Rr`&nLWwRbEPRoGa|AKa#aqXq$S>eppWr|$%w26x=7pkQ1BjgUWb{TDW z&SS_|Ais}Ti7OVx3M8!LY~S`gUAMGjg4%OpWS7<7z-ih&J=*5cp}N)anj0KL8~s2~ zcV)bZLURXCHsOL|SBn5=6VWG+zt#}sTy>-tm*ZL<4Bg48>T-lw)$JbWbu&qEe#*w! zm$D}?CUp{=TzykmoghP?70f-s!G+m}l+z}ACH$rTY*lPj%CsS#RZ$oF^+IwsIeN#I z#ofLDQrxubPK=An8g1kB0KMpp7c>h)$xSY6B9m#idqrNs>M zhzXVHd!?-GFNao4f|s)P{Tg8sv%T)ldE6@bIB+a#j8PWFUTQOD#a&x;)kLCDn;d~k zMY8+MONvU%y0nkag|zR#t&mG7pDuT-d@4IAd4IOKEU&0Hs!QOQ6HX@F$Ibk6;yrK_ zRCzAiC7=Jqkzc)Er*fw6OW7H5_t;#K?ZY%S_U^V{HppuA^ZtITIzSWxZt}}WhwWuf zAMkl`U+=!(kj;AF&hsSw(o=Kixwlz~sGPiLqtOf1%VnALw)4GXvnSARyL~G-juDUa z&K=Ks(awnh$^8gD`kRWyqbsll?l-8p4U6smNO1j6VcKqJS~SlrKdd8n3-YC6@TEeJPvaf+e(_Lp(MRta-J4KBi zeS_vTwd^`HIRj@W-Ut6~K=b+<_Pzs%_wsF2O(SS!DvInlv3Xbb!~jws@jE7We)&h$ z^jE^yc0a1&=m~PsUwE503kn)fuy00a6hIgw>_d=mlXlq*L3lj}XBN^ndbOmQz<+8e zs9s-myc^dDCd+>lSX~FP;yR}x=%fNfxq~>9XlbOH% zYTh;2hi_wN=EJ55aq@F}`|mG6tgcP6ZcCNrsF##}IIN0~>F6vLzciP3L7X!#e;#l}8lYVz`o)495R(Dt1&l)qC zued5yj_t68M7C~v>AaRN8EzX!_`eXBBNt5)6#m|#RISc;RTF?TSNIWw`y#7J(5#Xl z4Y!e-&=?xW&G}Z?#-+f=2%g;Gy%BRSW>PNi2u3#Vt`B~>mOogk=YT#i)wH_hv)PTw z_d@q5y_x#q_E=|O&cq!IO?-L)6zQ_l#_9~OX+kpP0;k+LMCrQwU`nN6a9LDH*jmie zP4i>T24*0%Nrpk$$?Vzd{KmjomlbX>F^&3?(YJxm_}D?yA~%Dh@*wH1*T$*Hr8(A0 ztQ|y3L~AmgUQ|`oh&7rAX*Y%7ak}5+CzxQ}4I@zSs+=Kzv5Pk0^bYJD>e7fr+x?p9 zwInRZ+w@0bJwQerMt`}wd4*py@7`^Xo_%_i=gklu9K;vB-?;pmBG4bo5xqH7Lwth~B zYj;QxC3baTV1;uA9<)-xnFN?yn5T*!T(Z1Ahn&WCUVnT1aZc{zrU9vebWx{>U_~}B zLBl8Opnjr~#iDMp;*0!`pxr~&ALUd<2f5Gzlde$1Y5FMq3@pWOmG*43J6~Y#--;eq zIHEWVx*BdT(6h)PuleZc9^$L&MvbVloOQBKC&=7S2~VZz(t8pXuNp8b?(8g0Y9YZq z8csPuygY}bO3e!WoabsEGoD0MzU<4N#FR*t&oGy85jPlL0Fx61z0R5tV%rIZDUx^7 zS~#0SCz#i+n8+N`aP`jp{goL&9O(M(0-$yS;@*Xpd>?b-@`~KC7v$$4uq;P;;ipQy z`Ehxh*ER;rlSH_Kv)>-i+kkU)rScegv}1Cu;?lgGZ+3?I+?QYi1$l&FrHkuAk-Yc! zS5S~+16{`biC~6PNQ}+7bI7T@V(=yN%x`C`i3M}|P8gSbT1_a2_JgXt2};J0Gav_x z7%uflg=5_U%na#`JWVw}R#`2+eVQYRabCp9r*vK!Wt;+P4ji zjde{+Ph+^Yo^uI4L8{}2R#~8#c6M3xho~$UZR*d>2f{W(4Yx6-80_6ZJEPT~S>DWv zex04mG7j8Sh?t5T(o|4wd#g_FcDZI;#+d$$1oaPO*ToR}B1DIJ7a$=%1nkUC4={MS z>kQa`zsyNR!OM~k+l87W5O29Jq$Xwj(_t$wZHg_OlFB$s)BUNAcT<_1F2&pR_+M$# z{l7p~;WHiW&qtvnHB=%^kC~UZd@B_2|oMV&c$D{uBg+d3&NwHy1fYtxRU zBvscz*!lC@LPxCiJ5!@o-{2_k9f_P2NkclzxEVL#!6nJfH4$TUbq*vY}T;&MdCRJV<+7AtfzzJW6G=`Ksdg+hAg918Bz9v!4WgVn@r2&WM0N*}=Ho{TxjVZU868C=c zggQa$O~Ik;r4xt%D6{!C$^ut*2rJXo77xO3-38^N-tA>BGEIr;G}(`?YL%m6Iq~>h zYVUjfYFeL#AY?5PB~?y$bQXArkP^Tf|A5MN_3zu?68{3=|Niz>;D}zGH(Ia@%%w`D z90ggE?+{JN+*dho^6|?Pw-uevrBY2&U0vg8Cx>Dm}+;#QuXXq;AP?IgV<6ya*=bS>p& z>tYH2HK*G;3y7iW*XNb=T)|E0mH?Z0y}NET(qy!QV#Txq>T{PRa71>VLi8Cf11T$S z20V3NpF%7=$F5sARw5_1Rn~=zu{CQ$@(}CtTIneOW3(6u?I=cks1SePmzNE=GY6ji z`qYE020%vB394~+-;)L3a?3w}MIcyAZ{*A9?d0Z8QOb&sVEeN(WQI5exs=~}>wOaA3^X{_ZJgJsa8!>I5aI#xI=8)fJQg{}v4^!uX;to`;ogdL= zXU_*`ZUyXb%gYzyn;Ekk7~vE*lCr=43h?Pk{#p3k4fv&a>~=@G8fO~xf?32gV6Zl6 zc;Mql@Y4n_oMuGW-+O1r6rQ#_YK*FDTa~ zP-olSNab~m?2R@n*_|~tUxxvwJHPgKmlltEe(XLNK8szI0|(;VRq%nnLmQZm$Ku?Y z;;T6R#ydlO=3QFEjW=%-vSqk&(Lo)d4$>5GCicHI=LKit9jI%-K*798U0bKpWyoYW zaGPd)4w`)PRNTVZnC6rbq0}fD?y#F%9TJ`0;JLw4%iy_=E0`Z{GgEG6!4E8CozHNO zryuPG8$Es1GtmYz%pY9fhxfWF4I+fh6V4xU4E8(&7MW_3Mul+ZsH~9>tL!mUXI~-GD*o*`f zdAAqjP;LZ@0&GlVSzjnat)t;`@2eo~AvZHjsL6Du1%Q4mR$OC)+G)epH-Q!_si+Ut z&IWek+8+YrmJh7%8N|TsHk*Mri#hj#2AitdWtFqZ#q)7Z@|wq1iLYu!gw1CQ` ze4Ft_{~Wj?MhmhG(CjKrB+(Nvj8-hDM4qMV)mI%d-={ML*_yJ$axVK(Il&N9fNGt6 z`R=P~N`i`|y(+2uLLZ*MbwhuC+}>k^tf*rhS_y}P#eS#G<7ivN76|{euuE*apLkgW z6#qYur(QUEBk-)G=A`JCRZdBe3daRqG%u3$Jahsc$@bT?*ka(x@()A8+(M4KcGAPPr4XLJbA)QxarguN#agaKszmX0HH--KLh9jkqp1cxYk-+ODUCez7*y2LRiU}t= zHk0*J{_ZDqjxoK-Ff8J71`TUKd@HC&^nxYK+Iz{M%L|iy0Q#1l?Sv*qwZ*@H^bMUa zdipT92OHeA^vg_AY{+p=pway6|6lm7IoYFcrVL^w;2OMUfg4=3AIuNLnP|52D!5xg z-PI~_u#&J}Fl=Ba7@K>9AYBB>Eco?|wA5DFnXB;z%4V9>v?HhuI=HH3$Ej3$z;h%1 zAQDgjLG*hTdkV~Ooxouq*R(;-4I1zkl7(9GDo%&uzP13-bivCFyde66ZIwl#yJ3_VBNQX8s~5qnAK(hZU_u$xt%NreeY6g zHt<15ZXC#v5nk2rwNo|I9KSbhK(OW&-fwD`rhCa-i^?Usn$%G?g}4e7j-;7F_x!YW z23``@J@$)W3n(AI_uCcH@heTsG`Bno9%ybK+cqoJtQP+&%XJ<@oWghie`LLRJk(qK zKRy&fC}k-ZZPHat8;a1?)h1D4WE+JPhHN8d23hKsLJ3J|qwM>PZN`=@N%m#T4B3}4 zV=#j;e9wEj_w)HYe!oBN{U>wY=e*82uh;XnJ&#k?;EnYTPc+DTGkbSHCv^a--vr$F zXNUQnj&YpmKcqBfbtGSdDCz$2Z5{cgtvX|i>@URun3ue9D)rwVJx`xGFJ9So_Z7|H3Q_lg9ijD3+2^gv`9}^dLsj-b z1aDhGg&+z%EJ+6WNRap5`+F5F9&}d}EqRi^hL`=Ha;rYyb(93Jd)e>+2% zdPKFqSvLQSeD4C)_7&!vg}*OhLbq)Tv>OrU=Q4&4yXk2I81Y*Q4Z1uz&>=)6a)HST z-)8t!yGS-iN@-KN#-J>?BwxfqMM+IBXjy^|7*zIiXm@asdklp)cpunf@YP>GK*1Ck zLAB993i?ES#t2gQKVGVx`yk7Btq(J4b^lf`ZW0T!{5q$5DhRmM=`6HUP@RD+7-Dd= z!EzHlcj5vl&wzEje<^kL7GLj1yyr(kpO%n;asYj6E%4qj^BB+dVddGb9Kt|>lHP3H zbC+Coqb++6Z1m>`AV5?d6siHJe^v*SqPUMKT3%li5CShH!pwFm1(pm#A!%Xt%CQ6TC)chfeaz@~T^-j~9G>Xj@KJr~en7-^1SJl2*oT6i#|Q2ShUN_aAuvj-sES2CO+01%o-7 z!OihxbsDdg)+g6%>)414Y>xV#LgzgI0xwK`JP?fsEJcu=_~Zt9O~Ik84^IZ6$%yo6 zF_nMw$xHM78E{ljha%KL1c95E5j2w=kpVL9&H$)Dsg!%Hp~@(0oaNH;EJwTFG)nDAeqF9NVn9r$->Zz?S3(#t`qjI!RCQ3i!grxJ<3<+lh~bvK{cW$xo@iRRz#T`on<3TM=FEEs zltXIG{fu7zSDvc7+I%VLWN>5&{bXxydC>*X!U24so_p1Q*x2b8S>H;yO5$;=IQ9>K zDrokwpVQGSg>S4y=G9>^u%#N1X}y(IuGai-@c}-Eiv3x>)$X>o!LgI2r?6AC>D`wr zbSB>V%q|3TwHXnr&>a{zm0Bti8Hg5V^b2$~wx3fCy@tKgE+mHNwz%|f2HAUx7p-5y zdmijUTOShXZmG~2kM*Z5{ho!rWe!dZ*6b6 z+^RfFK>q*=ADQ-8XaV+tk~Z6RVkc+&zFCz0m z__mjnK|pfB?R_i~?mr6KZ8(;4S-80MRIJ94@zs0UqWM1KnvVub% z=dH&HxonPoIP_I_HpAd9R1Z(pNK&T0*mU=w-(|_ad!EEDb$9>oSZ~}yTaRZdg?Idz z#RnH)?Bx}&?6?8QHj_#Mac?Cwkghfa#=QM&Fzh|6hE^+?!3hoP8@r%Ew}8g%MJ2P+ z%-k#gCC^W!279o148&o2$2XT$A?5MT9jWAEBl2v7#)(-FBe^OYL^yM-kNRhl^FjiO zW&a5bd&!{9@2!&@ZLprio2wYomp@zQ4)A%O5keQ#rk$}ssQg*^}gUiR!cU{oS< zroa7nr3&)PrZ0I+{J3NC<~5H_vxIYAIk(g*q`QnYY=YXOD+-o|Ky51E7a3ScKG?s! zD~n$JFpL^$4Xzf$X5yhw7`GMbx}au(B|!A1p{8(wohkq0=32cmpv^|eeb=b?KFiq9 zKht+qM1T3~`71Uq(ZpjlLFgM3Lm!rE6?AX5$X&3`8y0s5ed1JcTTph_Jvjf}tRkZ| zhH1a^?7_Pdpx4bgX97w$fFc)A`8d;+!HnZm1%)c^rOoQLtuLEx04}CrTxn_vL#sRe zM;6n5RvLg5h)$%2_X`sy_n7k*cHBt8Js*VYUPE~7p8`AXhHKct+ya2_la6pTDoV2S z*1pNl7~Cm94HmXw1g>n^TmNQzY`h33B1=ng3*%1#r_gUi$U)c-wu)bbDfYFwt*Q{9 z?|~M*J7f^OGLa3^E>XJcxs-B`57f5qFuZtdtQjzwfiR#HW0!!_)AMuS{h8nVE5Fwx z)-`UYYz6RX_KMGb(4q)%mIqYdUw80G5ncB%QLYi-&ZdP8*L5SD0IBeGKq zlEu^Nf|1YetWb5f`{#S?NL>uz`y;39zOBe@!y6se!oXjN?)?;V+;gg7h=lr8**Z*A z4!Lq2R1-z}Er-5{c9nkxQ~`kAg=+%d)?ibfKr|0ogB3K)JdlIe)yJfw zP;Sxw#;cDwXut~yObsY41+>(V)#Wp-ywJstmOf7^X&@Q16xpL9J(ELQsAgklR*w7;*>%#TQ4oK>_}z5SxzHZI(B=1e40ym%PnzdWZUs zcidgP0)eyayFSkWPe)<1uvrLnFV2SjaT?tLLPWQrb^*HpzHVteaV}?}x(x5sEz1xB zP_9c8YNyJ#g@w#sF95;10l=Sii!ZocC9a(U7|*8L(ESCyKy(~#wk#?(8{42puG)CV z5lq=;h;8?vmMgcvc?T;r7D`Q@#1p-(0PcJbx!bUtKFm5(_pZeycAvtI*$?T-VUdgC zo|8@-($U&LNu)zoFN$OTbBEzS#5BPeGHubI6#{hnP@pbTsPl)6HVa5EZqMwU(ZXg>R=c3?UyvN8CxTj_$&AA}h2B+! zLtqd~z|k$c79^E)efv6lQ&YtM*aAuDG@Iq?FVBc7Xk%gl$u6h|y$PBt{CkV)a%N%z z@StzZ)9dV(C&A?wJjoeO0Jy((vWgg63dE*}aA-?*rUB2qX!yqA z*%!Ty?J**GGZ?_)lsf z9S3_#MIeJ67Z7xOFZX+i!I?fdpc$YkDLUk#Q}1+6&H7G3jR9P6AZHRcSEqOZ!1{d&%#M_Iqpp=_?({2gQMdpg(jUAiD&;5}02= zPhdN|$z^VNgqS_yP6x#kJSBq|4Xp$6>z0B|Dq{NYjzYdEtZN7`T@QQ>u$fJa>YV+i z8wlB*R3%r4Lo^cMF)zC_JZAN?<3@iCqknjRV;${pSXp|gsesujNXNjY?=Gcp>IrDN z4QNj<0u_mOR>t5*4k&2j;^<`mY8xw{qEa9`MmW>lw0BP`=zSM_cOQzl!n*3-OBs~3 zpU|=bPapLIehJZUB#16UwkNy`>Rbf_yvuVLQvt%atR}vW(6_n+2RZ`uEHB0H3XhrIrFH8jpq0O~P{SmMpVjxh zt8?u22dF;a7XM5(DqFzPYzd(c>jR%D$?!0t6s&`n-^|leu zc$<-Ny*keIZ;YF+m!^i`9n$MU|ZOaokr6A{LAgoDas{V17#AmB` zKDxjryFbT{KBiPuo9YT% z!GjRlyP<#UtV*Hi0_@N}T@J5X_^Op7pvZs_s%K9tM1c>e@cXn-{d9G!EzOAZA30ls zuDYm`cdhT-)TB*M$$DuD+9dv6jL~$_4dg&}n(pk_T+ysHhkZ#l$V6G$$&-_K&qvmi zDXS4adYoQiA`zrv8nkY)94o$uR*cELtS|KMz8u<~gbs2bUdYSWO;t=h!p-%db`Dfp z0BovJq^QdY6qh$FOoVVZg%)JHd_!!HB$P3{(oP5AEMPuo07RTZJX#$>qox6Ch!)-Zq$OYBAUiHY}3GCO|8@?9*>Ac@FdG`j; zR*_#PD$GI;x}*_&0bYlwYRaXOJrDPKEW~Sue@cR~h=A;J&ssBR?1y}0zTQ5Y6Q4_V zV{El@*kDgq15*e+$dRUT!@XlxlltosXy^yt5#Ju$03OfOlQ6I2P|KjI9iXm&R5r}d zGGNjXJZe#^giRX^@;>+Z9iN4oUEKl*LcbW{Jw~p2CZ4sy-fywFpqhju1P;)yD#ES3o{cQkBWp%?3wfVnslGCKS(}K8m z%R7YjTs^6W==8N%6!^ygvc@o{uXsJ>n^j&}+4Yf*FP&*1pIOvsuk04qk}wcY$5a|g z{n-k-KuHk#s!dgbd3{s_LcDju4mJ;E;b0-pxQkyWv{3C@nD;GPx!~~|5a&@h-HL~p z6xY%eh-p%PK0jhtqOu$y4|;T#cWm0@@%wqZo!fp-fKrZP*hFt{nFX8_2U_%Hf=&J8 zJ@>2T1fXF@9-%cIB>%ly6e<;kfHgy}!tCO$y$Ys;_!GU_qKTvx<>+1y8HnjCS zfw!vaL3{w>-YBByzgupf)Ik3E_fyqXds<)M{pB43d!hg}7`nrGv+}=JGX`;3TPgo) zm>k(8S_*V*Tc^@juEvHXTT1$c`FWiK9}wwFh~c_XohdI)%Ez+eEGhvvI&AW=WR zYC`;_K-Cx8R1DMsx>bmFwL!h2GD7+ZBDB?lnk*fJ5ct|%tRw^SehIK&(V=iD+3W!n zTFj6^w3GO>O1fhTRSR}GDoQhV=OU;>wp&ms9cKz zdPRC%7R4%NR<1Qqm{*k+Rp_nXDU(Rb{|*J~Wv?C}1bjX+Z)>Id=ca!kt>x_r-tPqX z#_N1d!hWDQpRDd&Mz!I668dv74Li(T(-hq@L0W(~-)xqY+hsTbKkEjz;K_|E}zpprQqJR%s0z8c1 z9*@&b;3=XKo0LD|frI7a=?U>6DxC03vzVhW)IVOlQ{+re8FHgJel29`Zrbw$$nJ)3 z3!m-npA$fQ_dC5U23IcYA#IyKbDqb(xm>ovLarMRjPVJ8no1xg1#vq+k&F$K(6I$1 z!aj95WPq1%)3rQL8|_Z188Z&cTUtdi9@8WskwL(QbfhzV*AvyKWLL$qgx!=Lliuf@ z4GL2`vAf&lcq!9b*V?1gIDUkMIN#hOkqutTs+uJ-Zc=BMXG_Q$*2Ra;#BSql9Lh+| z)3xl}&|TuiO>SxI3O4Xt{76L-ya>j;0RdRZcPqR{DpA+rJ#VGp;jj-r3VN0(8#p3b zUOl@p8$b7Y-ZbiHa|C>Cfb|BtaL@@(a30u&Fx?3XRPvg#)Sqas=m;zC*T4#;9%g-L zv^^%H-fufK_IG62BzGD`tQye|Dh|(@8o6MMe^biwTRiqb=#72!I(O9{x6wFxj8pb^ zq)u{Y!Q{rZ>l>*gjvq2>aswDk@UY`KWintuV>8`G=(P$o@s!7aC%lNV(E;cYyx&{6 zgNI9U0fl!xodQ9f+B~h0Qb?(oF`-%mfQNn@S}p|6f$C;c$&x3J^1f!NL-j-Y=WR=m zXW|(N6(+^oV!K8(DL-TaqwJ&OkyoeZL26BtmlSn)?0Q;|J8k37C9OHfDVneCO?kb8 zYOD_`AN3_avVljL`ajWxB`BJ(r@;3qYXX#61A2AxqR>pGitN10`R-n0WYxV$d<60q z%nM=zagd|@e$(UYE+J$*Z#soq6T6H4YUc2~Yb3vnYP+0foG)p+XGW*JQKQ{4%NzZ& z>J6^t&p$;K8>{zWUAa|H4$I-51i2HFt(j@@zNFAL#RZj;Azd=4-GX_${QtrfY!J^t z{e&D5vXWj>8%O%|JbR;D2`oG1;+HNBr`r53}8t!MY1f)dxl$j9kBfD-%lEEL{y`BtpN zwK5qVs1Cf$)6ZOcD(f)kXk?J0JO0hkOIg?Y)rx-5^v3&x8K9^Q)O}9bE~45Sqk?{b z_fo`3U3qaHv8x+hjZcHlK&DTu^M#`-6og}6Y0N;6(J6pERk3Yr$&XDF11~LXDpjnG zeRl5xB$)X*tdGpB&F{7WhbLle7IjfJWAw|P^|xy>DZ@J|{=da^;1tMUNyl4cJTx*Q z;<#414nBlVZrb&gu^)#sgx>BTnxJf(Ctm7&&{zd`rsWK+pexk$12Sx~_uvxEl=t>e z;E)}X1QE`BjxZdbj(QZ{CaOEYn458gLg8a4Ec@4aBE&Q+9BV4cbHF9~%_R63p}fEx?pFBHANXpJ*qBpqntqcPw0c9YMv_ILtv$ziwQ8c*(h_EyX8Wn+iG2*j z*smmAa+aRUW26u);ulEXqM(cvVAlVt3_guTPa6Y@l0v6u1o@(Vvd7NXOS)2b`01cUX0 zWY>W%{i%+6{p>Hypl5(%WLC3gLesxKmw5u%(0;R}e^Q}bQd-{;TcYYs);&O#*4ue- z_rRZs_L(P|9BH8F=caJr>t<_c#zq!kMQ>2LYoLK8^|gZ-X{26PQ1OyNQ6HZV`KN8v z(sbOB1=lN|nB3{A4S()zjR<|@R^2SK=DW#`tF52|q0iJ~(h0C4nk**@9@+^#js$wH z3qbP3}(=>Z?6}KMOYLD-PsNj3?vq{*f2aZ_} z*>`}ccWYlg@w7OWK=WXSDIXB8^kv=SvR$y1SH`5I;`@Pb`#5!1h~bAn%dNxyr4s#> zJ$+3N%F7ua0~Tjs7Y8WKzP!09#nBBKl_G3{PFZ^BZ-_R?KQJa`V-q(ZcJ1;?VZBGa zk^X@r#+SeB898qLI_B~f!#%ZLSN=vl47dAA6>66C+_ic8F?>msx-h&@SmDKA<3_dB zO-@eX2hQ%X>AQQTiIvo&W!TVgwOW+eR`ofrUC-sJ0bGygeU-yN`1Sgx`m8Qa%}t!v zo7k}B;3m-V688)xU@$EP=SMJ>m0qFzrgF4`7IbWui=nMOYJP68tgPxuoTzE$ratgy zA*+@n;iB}Y4*iv^ssV)`cK0yL1e$?fk8bcf!6k3A{0;%-XDw91>iD7%rW>4PW88G- zMgS{vV~Q8c2%^JHNGu!bDT$w?xh$mLB?tdk@dYcZwPnu5+LcRoog>_^=rbpD10Akx z#vawc&}Nf5Nf#$M=**u-o+3PB!>hQT4{k8YwBx$h z9o(gv6t9uQsVn8xOggT}^C~hQ0akY^Ja1#Qc@)k|Gp?&RhyR{QmIG7g4v83?$)Q#( zjpnsVO2e$uh@R}$DRFith@L!VCTy@&-czm(GzL%j2W~1qj56?H)OCh1DA7Up(J|HX zl+|DUR}_w7|IaKX@3UuB9)jO+Q#TAuNlW7dQ|cmqS{9YJ>d$z+MhRXm z@Bd5CG49xMN60Xli>AFLIppmI0qqb()hbxuA$pA?JOxW9@Ro}^H0a7f+~#HuC5SOf z^0y1HpcMo!r8oPp|7-)ZmOl&x-bf{I{}cEoY(3)2!Qgqgz4V}BImMsmD`$FUskyni zGx~}>dVwNWM}lumYL#!+_+ZSH?Ic>am$m4kUWx)aL} zshx-&meZG`@*=93gLD#G2zfNIk{MByJj>8CJ);*eLJoFh!PoN$c3Zi*y!Z_l)>Gxx z{^pES?(^m{DJ-)vgpst~YNJYHJ|+ABxFWlbs=fA~fcBSetT&HhNG``$3gH-y?@0)X z|4dGih$2;4pWEN8JGVh>KI~cH&yoS>Kk^|>yNZ<)8@v&ZOg_V8GE7oVacZUtl`%6F zF42MwN4hb2m?@mvvcqOJiK6`(-j^i|gB_VnbU7GHsKOmz>D>rCzM>F3Qs$5+SVddS z>3Fl)!u7Pu=D)U?qYT#O$URE!z< zn{Fbx=&vUBOFMkojNP=*Mw-cOK9RaQII9GdY<6SWg_M**aDr{!Eou^icd*B3{*;wv zcQ=QN;ICsfBn%-RSZi;My&;hlA7binCx1|YR^7F%!vBXJ`hG@my7r)P1&4jX#YW)V z{vn0eV$0!Bh|AsZP8vZTatC7>hYlZSjKVREBI`l4k z84X(FkT3ltAC`^<8Y+u1Wh1i+As-TkwA02fp zVXz^G#L0c?MU?ZkgvWEIwF7#Sqmk}6Ws1YwyZ~^C9)3 z%kiH=%Qko_o-_ik-gj zX3UY{o3(l2_eJ|N$?n#Ig~jNb*7#P(@|GR>V=Lt?3s_vsj)o=mwgMy01J?k&?bV-KTV5>Ir?z(4ZByy2r(9@e z;PWMYl|$}_m%Vivei5M}LcyY3*PBYQ+-t*5yPqY8cwXv}VW!zt49Dcb!>7fUy+Ot` zMGu3?Xfc}HgreItDx<1D+ zyhsoYKrSdD4)qC#(p_%UAhow$S(XdHDs@U&-D=VbtfvZ#P_(pWwk~(%QXHBRn5sGE z4qX~qXEQ_7zii8czjjk5D{AMmQ^%aEc!)zp;tY^0TuxXF<%UI1*pui zg^Z`f%^?<|uiF5bHQZpw&4l0upBP+?*%|+_Dgn`L<0S<*{i_quC_|Fx1IHaK92$Kx z%VOA+JaGRIklUmNgPbc!9B?sA_o$Ykj4c@E=8`6!&9MgOFjSAb@$`W`N6Pcwhn8r4 zk38a5ho3w4J$Ay4ki;3_MwNJOP{wfJ*-KEA=XhhUt;OhA5Hbd_*DN%7^VzA6M7mn( zjrNk{X`h^Y?Np5ZMtI;c_;O3zjpr+f;l0^OU|Z4ZLo<*5qO@h(+_EfxZ;OoC*^d-L zF5stxuRfMo&Ry&)Z%b+%4MOT38k$jRshr5}?ZLCHq!gHT&0zny0BxT$>qg6bw-~b{ zOOHASWX?-d4=lFe1e9>o6Ne0LaD{?_WUArh)x_J1T5mpCn6>vd+@T22N;b`Yuz6@} zdnsloeSI8t;!{YTXK>D8(dFrtfkt$+1vtFbxtAwjzSFpMS?DO`80)%4oF&CIYvA%^ z?eSuZ_m4%_J2b6Ilv@r0rRFD$?UlU$^7-b!=@UH!6W;_53i0rkVA|v()3l08!{RW( zFMN>(kp0dT1&~W56_{gaf8;{E;&_QN+p404i{AlKK1Vlqn`pha7mU^VZtU=KsVsR+ z)JD3z%j({emDS_5GnOUDASE|8b#1Vx&D}ca{KufHx3&wCRBj@w{ppYDh?C;MY7UuZ zxwY@gm2q)*`%U9ZxLM9=?BvD-63;*SR?5deY&d@BwNGa8N*d+pT7K^I^^fo&H)Xjc ztW0+I!jce{(yPy~k_(_OSqT8OJY#MVAbMHtC%6qaQ(QAL|B)U&=9Dp%HP2(6u&qIw zu&>n=iy1`y>Ns$#Ra*ADn`2Q_2xCAX90PXw@=%vX-p>pa<^CbEszYW>hBZQ|H`jFL zk@Gcyo7UR1u8QowReKlhlaXO;#7srwpv3YA%7tdHuEH}18-EI8+c^Vrr+X$bbV$Ya zycZ;9R%Y$Vvlb-!dOj0db@-l{^*|`7JF^fjfHth&92P`PboqG6THEO^xzD_s;|vcV zUmq~5(*p?BIddC4hwOLE0`ubjeuA4#-yMsB0n1D_&;9_jGbdpm*gvu5`0j8FFmU+G zLiUCg(=V+a8qM|g1;|X+B`q#IaD|saOA}jvHcUtY2DKBM6)_H&*LLGC zy%G!i{z9s~G@q=AGwwr-EU1iT(Fm^Pc!n@A*5=jzIrl+8UH--9Euy=w>%B6AUpIOc zvX|r5vDKl(b!(Ma?+%zHGa<7od`7T}{;vs#rt{OnA>rtIx|-E{!#8G9TAz7}s9=Lm zX_hmQ)=uB^+tl;_?!dM$^NX$x4Ms>YoyHC8*#c2D%w5=5PH(fY7IHo3PCNf!@D3gM5<3hbX9HC*mzmaqM(Jj zWm$MU(G=(^aK#;#q{q_&RV{Qfddgeirr3C#QP$X-%jtZ3C55v8X@CnYAXVu!xWIgjHt(A9C5IqN!kU1cifOF0SruZX98 zJ4l(ho`bNP^qx;vOERZ9=~WkwVq+JK1*@9qZ-c>KKIZ>riAq(ulqt|xOT#N_f9#KcGQq z@l6!UL!aL#SDW$OP4!2Ukawv5$c#V^mr)h$T_>S^B?ZfB?teMlRCahGXSZ@!_#cR? zkmWvlUz9y=^7j30sY9X*9-y?gS=^o(8oBZfMWATe=w3PfxMTvq*1pg2vG++qec`FD zj2oS)VG9#&ri@fznF9?r1c3%^ittoh7!Y^Z;R4HXNyVYPS)E$GYR|%idLC^LLw!vFW-<~>FK`?-pSaC!S?^j^6!{w7( z?voQC7r!KEl=j;B+j)NJMVZc(G{Y*U*%=GAx(#2RlEOrntuE?+gpebe5gP8bwBi?5 z`cc>y+JCf#h6j1hHKXqYOmCZJF7=in5pTyaLY{mRlJ_a_^hA}8+<4^|?*M%PW${aC z)NTY)ufqSY2=ALziH+-J{;%48+lEW|347pjB{H)R&g{2#KrB7a^6%1$_bA7CD_to? zj6|guwmJQgu!C6;;{lTsU6NiKqj@YNT8xFz-Qy7v&{Xhtp~UY>iDr2k4nBWnT^B<4 z{xveK-f=ZmUysZ^HTL%Pg0scqSmix7b7ISK{AhPxkRM~l>H@a|&gB>7^wL1l(SgiLt+km){?`QT$pekz;OzCC zNBIwC>a^EyMYkPq=ytO)B)a96-5JQ?6q+&0XEch{&4cALE>CU_s?gC-1(<*p+0W7G zY1firmb|wyti(s66xJ>FP zPiE(Jk;xtdc&l}@4m6ok@2U?_>g?@mIT`90E!;azu*}0u4kv@-%l{gX|1$;ACEvyf z%JF}^me@)sP`dBR=?9rDW!iOnX2qc2x>hGRDhTh@&qM$a8qa0{;@n9~ic` zt|b<4#+8rMvIqLWo|e=JAe#eD2_Pk+^K-hf(10O_%`GrfVzL>1kii}4WE;c3Pdn%|7Z-dm3?ZeFhnmso%*_$!>$-*acmBh0` z;OfCK{pnuUgdo3A{9FC4=Oqb|^^HHyXxeOY4-5Cql1DHWr#f4vLR(&xI4)59IyV)i zji^F7{Zk)&n{%F=m@A$CzPPX53X`uWUFp2FvL-64@p_khhUl8Nk}~(SEu?AgZx&@| zuGY<#<||qr%e5D*jeC~mzf-V&Q^~m_&sN@Q@Q3^KjAm}d(4m4o@abZFtp8kyHy;fyF^?*HF-C}lku7@>3#fk z5EdU2;BDWwqUV?qHNVFHK;MN1m&}KKKA7xo#rzgD+E!11_QE_L2*|x?Rt!?1<4q5) zbgO8~VXGgwgQI@Ii!~Y%0uJyU@lzKKN#U4oXa6H3EdE9X`Zkg~Rpc6O%`|rFf-_Wm zylC#VAy6ow>M+{3r8OdXp6#%V20Qbyl&&Q}lPLvm_~&lyills?za3wnU(xs_J?k8^ z^d;T$cqvv|!wc(U+dh)U((*^f3e(~c9RjQAyn)!Q#w)d;L?A-9P@n)#qEnB@u2ZOhM z<9t2Gd7GrZOA?wzevx-U{H2ro_NIE2<=<@RwkZTw<7oS%%k^n3S7tIM6}~A)YzAke z`sz2Mn|_4M7%8i&-3a(!Z@re{XCEm@lWe!FSiQ*{&AO$?68T^PVO*H*X9>8!ntJ)zG2qg zFXy0X-$5i8s11f`US#5*nt8hX7PWPke~Y4aN`D{cDP(*5M9U_>)g#n6P;9{c&wMda z>Tw&CA<@~d#^j5OEpfk2(3)X2fpat8dWrYam1M%d3%bL(NlK-sEIZ8Z=!K&SGrg9= z0}*@TFd4tdTwAN0we1-VXcGd(kG~+ByoN#JQV{<87I!GY-V*ZrHkX=5*u&h52j7#r3#zmL4G$=kh7SnQ$k3zO0 zR~S3dmYS-bcqO-LeRZnv<0EX#)@AQA{fH=_QzfMx(nETt%KyFgM$lz4?05ZHERtJHEXX?5CI zpFFoJwmd!Gy=qwQ*cfG3Xp$*xO#h7PR{~#R{?waDmov?Xv0Adaa8wqk6E8th%|#g` zDV3$S)R*}Aj&Z^TuuMVHL@fb*bB?grCgQJ!?-s-0az$ zH@IlkbCJgIO+N{>l zZH%xVnWq1I`3AtR`A1csPF#6itegDgO#~aVrzh5vo zVN)m%ZqF8A8ys-nf4#~zNqn-c;ah27b{F_zLe*L=scNmoMSQpN>d%N6^_fDG0p!YI zIln*?5t>( zjVfJPL-39CZ=U^Z@#TR`qSmOw*=o7OyZdQwyAbGWzh`&7yCJ<6Z*mf-SQWp$uWFia zHG%o{ypdzlRrey5V$qF?MG?j);X}s%B#enF%>>Mx5|(D-MtN;lDkjHiMs(tv)+yav z779tEA(S0R@t|AdB+pA`6@G)1ZML0Z8GPmCgkZt5j|sWCk(;qH#c4|x{fg(+HduY( ze*LDIEPHmLc*}$k)~^!P?V#9rR*>}ama4483fsPQd@;EcN&@+&1;Tf@**2dt%6D(* zc05d6u5S~%CAy+EY5c|VM32U9rn+>t$~(8ZhBN*`IQqQe!86&ot%rt;Emq^U8Wiva zcfb-%?wv)f+oy~n$1TtrT`M}r(aoiSlo)#caY=hU4GX^u77rZ-8_?J)--?|1rH=Ev znPa*d0?2`VG_T*{x(jk?Fza2|GjjSFzrsw8`ID_Q{i(WqUbbEl>lOvupD2TZxP@u zSP=jKCe8F3z42PID$A^`7=F|ry1!|pV_e8Vw7qaWUjT_pb-4dxny6Ksq}up$X}{!h zX40HGTxZpbzVIX4o^~l0*p{=GdebnT!M@%zk+fTDdOaT+x^;urO1+lkKO1c#)rH@? zAiCTUfoJz3a0zqxepBS(#{s157A-K^zF7AoEIk5WUg1!_?%kFps|1UvED<-&@DTX0 zo!>;OI{`N^F@rBWCxC3AwI~-yS?5qIZDxA?Sy0Bs?D(Ah!QP9`n~vDj5-b4zTV1?o zcfbEFF?EWl+$6rCR9`4a(i*gJ8YK2e3SqlVIA2Cu=lEx6yCncShjv9Dk~?#!&G{w- zGtU6HQz9#|JRqtLWFR8s(Xy5Efj+L=7QY#*ndGtl#cwCK@Y0pmS^gE z4>WBeeKBxN#*9w&zYP0*rcXoj`3K6zEE-PmGauFM*|%+pv+(y+#VTe^GXgiUDOP)9 zO~j>5*yf$uqI8!^{$bREgdU0I>pYGM()$*ru$FjTCa^y^<#X|{{ebLd>>W+uN*oQ-cw92)wWJh8Q?`?4rV+V%2SaZeACNbmwAgEHSYkfr98iltf*BTfQD`$~ z`o9Lze>m&)ALoQ{r}1Lr(`5^uS#)Xza)uNy(V~oxI^NzuJ=_OIcY#LV?S4jLH5&jG z^9{v2wnXFu=0?SVA?_Q5{;7apbqB+4!K5#9Hg`TdS3Wsj!N?CD<;P3HoI@qXZ;k;3 zz7)^Z_r27_fd>s~s|GJl86@RINSp?M&Us*}vq5!Vh$rPVX z@wXKofe!d^JA4#?)l2(5b^XB@9}Rd9`*-(k6QF%@{_q^Tp@{w<0YTuSR(F0nQ-Do= zf*{S$8yk{n8!@@%?mww*D^w}$H#-+``PgSQ{;v{!`3TM-&F$qbU;noB#%>#PL88`B z#Z)6r+h&u(o+0v|g8G(ov;9S#Z6`r&1or*JC-8kgvT5+p%NRy4&CSwIYg7ncx{aAL zKan~XF#a($(K0VEQJ>_vSP{a$1GZYW|nqPG@3K`N@yY zG=`?qp@BB!L^(x7U?Jd5m@D5;;x6+GC zWgwgNc`p(Ykd?Uxun}aJD&QKhZtkJ-A^<-yU%SM`pqq%$^JBct2s zM_P0ow80Ui1rsVK=lWmrP$%`Kb^k<=bplKx@eiQ@&dH{kY2iQ!uF1xUC)o~-8b0>Q z0ypgI^Lw^Lqcws->Xr2=^=R9zlOF5yw4|JVq+q31-wZx`2dlLl(dC95Lt;U67U3Ul z(nxb;%^;#`?O}89O3{Xv%?Vmd&~p0}8_E49qUyQ6BK0HwYC56l$V{O`K2MsdGI*m# z`y@1Zffko;dOFz-T*z_?#_fy&3^sYWG@+sNK$i{Vcrvz?4=FV;QZ_~+fsW@z2YKVu zX}VP2Vm*p{KW}j9urjGspDb9c^(GS-qAXN0w1FXhSxcFjdCAz13QVJjz!HsUkkiRE zhaVU+R@qb+KZKHbPc;KDx;})VTsd9S-w`CrE~K~x{lXQgB((gkAQBOBzn#kHEIxL*VMrkpT0phRm#i3# zR6D|MPy1>_s-OQEu(5~kv@51qJkKJizqg+m=ICLL4Hk4q}IfVedX=UHG9lz6>&F5Ig)noGS#oSI0fl>des;piW?3Cz$MBEw68q^g&p z$V?}rs?IXs6Py;V;TOCg>y0c5z`>8MTR+tO*w<78JSlqv4BJueTv;B~4YwJ47O|cm z$EaF+KXX*;jnWeCv^oE3YXQV}EtR{NdbM%L7-1(%0Dvv!yA|?#T%yn6SGw${{%!I0 zx%hyEHje4n?f$%PkKrDiVNzCktmK{sMh6ZKdHif#z!-09GB*G{toPW-I^ z-jqS9Jd?QiI0%DZ^v=VMV5GJ(NK@QkpK=p?1dg68l73>+-k;(w;ON^oP2@ZIVeuhw z%B$l;r+DD&wg&ene2ONi%jN4geLpH9^YW5B$a=lYm_H5)(5MU+70v>`efdQ5_{sWP zE4gLn?cb9xgX~oE1*}q?X-c zBn1ABJsk_67&CM`HY@DquLM3!y(X+cvjnkdKk#;)E%C)No*XoByP5-RleWzOe20s< z_8=+V?_lqc*=Fp=xaPF!XzMbZ6{J}mm>m{>(hte$l0O}60cQZb?B-$+eCaIbh6^|Y zCxAx1lHM+iD@%5+)sv);Hj4|S9$P;DIkV?R{clg&;UqdE6i#tMfJ@g*6c2t+4IJE< zRE3K!rz|(6e($M%D!P1rW^li$X%>fdA?G7sb{}IBeF?ZfNSi%HhvM~e?fIWxh%iR|$tR5eZ_u!OMqkFC$?%|upLxZdj6`v{b$18@kz_-||6Esc}&_7@8Ouxqq zE*f(N*N)BD`RnW3ct7B;Y^U_B%235f!-*o55j=#-yyz3Zejb(O%zcMzJnL;83lo_@+$|SEJ^jxv1kN{hE&(mJo%F| zzfOr!du*<|)dZ-_Y%`#bMq;Olav4;|POz~r?|Z)RC{zFanM6D0_dNnT5LlPftL z1srom!FGPIjI0}d6suX*5Vtmb8FCPxT{gkLK&Qz_MOp(?WJsJ}{n==sz48W;Np*yQ zVFB>RraFfL!r`m@fYnEz)t<+n<<8B_rn}kdNwO64cogGIP7sfn$?1y`TfUl_qVDho z0CGVRHqTC!Kb^}nZ!Yzg2L922@-*6ZXK(g^n(*U@lIt|8Fuo@r*f$qixIICyyU8Zs@!`Z(&F*CEok=TeTGf&XnXU- z#!PBkkKnB5uI@>`pk7_CN$@Doz*-q0DB)hXUbRkk)p|7eeai-lRY?eH9Sxvyf)_oD zdcpqjl=L_m#TO({5+IL)vveD1Y`ZX7WU3S^nu^O!0EG`QZ{ ze7|+b%5_`{BM;+={NW!F5*_jOl-IAqwm)wn;_ZTYOGPT=b6{JJ+KO^4E$R~rSScKX zHGjGCVRRVLbz_)ey3R$cQJQI3xquSOJpXL<=00;Uii5(|pFFi+`S2X?6Blw+@tE*hK+?e6~T=npf z<6fKqa!y$n?T+@W@ZCiE2vsW)jxTS9$%p#-Sa-XZ8~Bft?uGO!%;x0lZOD;sdagun$X2k;PJ-)f__=PGTLISOK`uhnm+ zz}JzvDQ6hus$%Vb$no)K=MTNVE%X%k`+vW*9`L3X5+X2!l)&3~S>a!qS<>Yo7pxZh z_n59*;%4mO7D_g7QTl%gy5RAu)>NPV{zzBB$8t$=e^R75<5cNfM;oWgI#fOV+di7yLl>}nI)JTZYP7a@Us(g?9VkU+I;t( z%oi2gnK^&5jkZ~~S%F}E!24f8cBu$OwIDL8%I9V&XSFlMOYvhjHQQZ^rb0ZN^I!O*~=8%mf zg*k?lv4pxy)Lo-e4u#k-rwto(=%Cvi3&YHmW1DTVwqfYGw)^*cUa#lR=bz`F{j*)y z_4%GYhxhyax#m+cPo=3@?fL<4Mf?;ipCeVO=dQH3mJ2p|jFhNTYiR0g8rOp`RF^a_ zFojg;H6pm~EYV`F4NOuLP`vsnGjC5yhn{Gs&&Ww>!{O{WDj?%&8C?t3Bjxj}Pe?2j zL1DKq95S4sm!}dMu0vHGYMiK8{5jJ|E`12?MSOwBQhA%Vf2Q2JXax(~;8gf^y6~dT z{8_PMmACI>hS|Xy1+)vAmI%j}1WT zJPxlypm7Q)l8Z71e5E+{ZHWi7juLP_Etu6>LAAXFliD_-q)^r@ml-dMk=>6);7lB! zMj^fykjN{FG&>c+D33z4p>#y|#l%ar?<4@#*oV1Ho!+yidD$#cnX6*HlGAeX%JB5_ z%ijOqLXsPonZC+p=u$6%3#xROMm$;Ou^k%yOci+BUK|~&j7 zO*pF61?Yt<_Dm4S`~sq5vFbev?n*zzHuiRL!WWp;QGyg40-wXpR-h>ko>u>54eV1vm4>y8SUfNC()PWolc7uG8UltF2y*xOUzi;svLVkNN@Py<}9-APnyj+AG&hZqW0KuH=E z0AAUo0E^~~VDmYW2-H4T zq2b#W{0d)WtelIyYMo$wKIxV;Z{NPp<-Ujp2+O~cUw;b`pF6iPQRvt} zn^q~)ND68JdrdEE_`~N0eGCA(7OITlA?2ip`LX(pJ6P%?GsZR{tqh4>Jw>Vn8dfg* z>P$NBNQ?CpIv5oWxC8S;`!oapU7U6g!}yaqPARUI2X`UOK$--mxuUAw_66 zB{I9Py#9C5PJ)`a^Tgx2R1M+i@DF7SVat-`ws{=Q?RR|-YI#I14P-1~g7D;T=_qhm z5R=vsUym}FxXf4E91m@0y1z{vc#gk zR~=OKAljYO`90JBE-+zf_rRCCH-j~|tAJ>6!8M5G{`*?bg5&$a`p|2orUC^m|4=ik5$N4XQ zRAPd@FDZOKqcVaKo4-$=<-UP-Xl|KnhUU^t0k5DY)N0NTtphM-hdloDILieH(wAt^sq z#p!`B&1b~_(0_&ytxVH*SoJv znH~F)^kIUzU6#jG#YAt*u3p01fI+`HrQJNH&6J5bK=Vba%b)&X2to5i9lm-%T%7#p zwHk}^DdWlEJd_q?>+0X1AM1?AbWAx5e)in2ElcX}O@2Eic)8XBKnfG4y<|nt;`O6X zzUW$x7}v`yl{jR3vrA9Xf87y5K0CX|gJkt2(o{|m z_RyVt+ne=c>r?FPh1Hw(C5vbg&zgSpGh8EbQg46(2?=ajf&N&K_PpSmJf7~76Sb{* zmQ&_blfWkriTI_YrLo?gY()fgvTi2J9{%k}QD^gSvaI(?krduegN52&>untI(4u9u zAQVv0)s>?A0*38UbIvtk_b^7$;i)Nmv#2A#aLeRw*;RAygNZUSFib_wS7$*4;VmHI(7rk6y9_iKRMKYB znb4dbcS<$~juswJJw=gEEyFduSt|BeDk2Fhd6~1aJpAWasiJ$Ov-l z8x6Pln7&@_r}Uef9=t>-EB`h+~FiAgwi2 z<4VgZ`aV_*p@g3tVbU5GmHa)9c@gR4PXQsUCjOH0CaGE0A+3lGAHQqdr`)IbX^Xf) zZY7y5R0JJF^4?#>FJBu8!r3kCb49)QWwPoE^3r9NZHaEqT3wA@;L^8tx+Ohh&e}C% zQZEQ#cz!J6SilC_SUB*~&6w(PEYZ)CSx0Q7=6|$w1PEFlE}YVMy_^{+;J-iE z3>+tv8fQz03eV?RK)oNze9lwCs!OO{Xdfm`@}_@nMki-%TQKggR8H(y?^;QzSc5K6 zRumiFZcOwOHHZ^d#B7f4&n5s`n)mjECt`YaSQ4OTnD;|YCqIncrb2&W_G9#6cng3a z{>Hj_zKxN71E|1(^2$=>d$jJkDu}jcK$qs#d4OC#PSsk7Jw`|k1Yg4?D4^0VR~P}P z{8AW`ERG_mE{(1YSqcHPpJ@tt0$wcEc^YDe6`gsCc)PC%o+bhk>?o(vg^HAl@W}c~ ztSPfjBGs+$!4jze2}3muv?&fTF#qEN+4BKN%>^nMtro|9Sv8uNyk|IerYajt@O6g( ztWJo%-w4fD`ze~c4DAX%2Bd7jm^RB;X%W)L#Pai}GzfMYIE?BKA_csxa|Cf*(APZ* zdWdov#^%N?RY2)f;Q+ySuN`#a{#e9YY8d-SiRE2xZ3fnqvoy+his4Dd0HGr>uEdWO zr;u&bHZDWv-^YaU1v_Q;fey`J!U01@F(e1yfyKIc^8jz11^AL@MP9I`0%|I0m$T2! za==fQz#bPd!=cL_@V_Qp`!-cIq$UjRviiyQ7(~j0OV#DZ-QCJp;G$w9ZP1;^!r-1< z3o;TfiB5*x{>KLnV=vqt2hcDG^@-s;^L=;_=;R~erM0Lcd|~{5T#-6{Brc}<`DVto z5sPh`T7jl##=85v`Kexc^iezNS9*ElDF_h+fxSnrFFiWb?57(XVK4V$3a3R#b*=pD z@Foy-Z`(MaM2Rx7HIT)UR#35{A%tY1a1tub=-(7Z#dn2vjGV8IKPdkeJN?SiFP3o6 zu-0kk>#f63r`^chDlTGrvJ>w|+2b})C3h2$9hYk~Bd=+VD?cVjvkbBSx z0|(RtC%E~U5~)e6*Ho4!{ACW)IxEOB$}b8c7u9H$U1L<>bLw^yxBB1Jn48|JVe!aq zt=iFP5%jIVb5qP)*-x zNNn!@=lg~W1+(o(AiPGu$>8G7-ZY!}%G4WWo%au)G#EBNCIR%uz|$H^DeP%Snvcm$ zEcG_aZ5A81V{CPO-#kM(zCkIm>QZoT-ph5hS@!FO^%d?u1PUXXA5W7e0Zq5VPhk9k zcfXbmm)&3QYR|o&h{IGJ8C@`eH-YD;$uWG_E4Y@#=uWXQQMB9Psep1UOLfO+ ztPWZ#($)Y8`s-53GC+#d8Up#vP75RodQ(I3xN@?0_yrTKlGIoW_Y-OH;Q2v8Oj3*X zJ|Gehu*7xV`NBb5Dk4y(6!S;GtXLWS2nPorFsRt{1i=Ev)a8 zwdy+=7e2S})+D`_UM-ebPWt(%zLoOOwk@^Uf_PD zl9ilUERK09Q3Yf_e_>7i6bKjh8@D-5v0;+3ohe2+?ANOEl$ID7DfVqKm7BySGm8;Z zq1i|MkvgPblTpmC*a&1KtkAvvRrvM6aZ~T1jn00oj@V;FPqvwp#5$TP2sWWpZh5CY z1kZ_f37Te&g+r3&le{RN#0QW&U?@ycb2`6zUNY7%8Dq)qz#TLNC#vXs_zkG$EuH`>kIR@BnGWDxuM5}*?cL0EcCO&TNsB0Ucco7fB7Z& z&XVk*^s?$*th;UkEkMP*PG5ag%3alUjdMk+WtWAiemVZOEnAnHKMGt7J`&jZoEt{F z+31`%YM}#=jWECreK(eC!RLx4PC5cp=!1Y`NriBUwKF=`vu(7gphCw}w2Pi}%llb< zw`?Ae$0Ua`7ky@TIg1$>^oWVnVz*Q+;d(Ls`GT&!Lko{K{NLPE_Aro&l* zMhRhg5%w4+3}j_GK@eHfJQe$2H=SfL9-*nk*zp?iQ>ye&eoXuZW+mSaXu@*`hYw7F z0ABqNRB79iSsb`$dbV+IduhJ#fHS>DwJ`=&a58^QGaKx@{~*yU5u*T0d3Auh@iOa| ziuU^OD^H~0jM&>XtGQBM47olupS6I;WRCS(q6-F(kUfaS>YQjQ3xz%(;CC~FH%Y0X zMcDJAuCdwTICg0FnmE=;BAShpM2(b48~8Dd;QsWq5k&Kcw@nDZGu%zp-pY#Mf4v^4 zV)LpZ?99?5(TJy&Q+l~z*4iO9o6#H7os@fU>lSXK(USV9-7@o7zwC!jgls4}1sZ_7 zBIPIe)Q^^@ypF@H8^^ZzSe4>;m-~=i=LW*91jDb8fg~Ca`UgQIdLae+h$cIQMI@y_siwavf;8V<9AslLfQH4jA_)aNsA-z(pR8e?9l#U2%DH#e}JZ!1#fSpov+u3;&LtEaC( zRPI1eo$-Pxpsbb%M-S?ID89j~($}tRXp8$v-JurumeQn`&|7OOfREQUqgiD{Fbl5c4vRwQB!?5TpZ-B69Qkzi6r{v z?O^dfFD9z|O`*rAsl44nwGHsjtfe-|`e#2Wgqfp9ac!D%%A>Qg&8W z3xhy=_R8vLpyNxPiu}J{PVP70%>YCVQ)UbH_V2j=lQBb=7JqKI6u;-NZ_J5)*&R+h LojYFX=%4c69V=yx literal 0 HcmV?d00001 diff --git a/src/modules/localisation/schematic_pictures/vlx_proj.png b/src/modules/localisation/schematic_pictures/vlx_proj.png new file mode 100755 index 0000000000000000000000000000000000000000..d9fe76eb2992822682fb1d0210602457c3d2a580 GIT binary patch literal 46631 zcmYg&dmz)_|Npv3rBZH{8kIz}l#<+)%Up6R5^Yp+Yc9p+GKO?f3AyKfnfqmPO~y!) z+zpxA$aP!8u({uUuT}5w=jRWFmvheRoX7L=d^{h|Q*Vt8p$GR1?+1ZE2X%EcZ-YR) z=pfL}zP)>Z?|__2>w(WZ+;2l~f^h98fuDeO+uty_0Rk0=@oYTg0>0k&RL8;{1Udvf zc&L6yqifD%5U9LbSM!Fcm(}zp_s5gA$<%ejgd|Fmf>KKoHL;tj;O_Iu-e)=4CuKM; z&GCg}s$;y11K#ER)19Z)PZg+p?dBU6JAIl-TpsoTKJo4BIkkH)?{>m=&PsNApnSem z>6t5HBO89n)MZmMZM(a6ws-9|7DD95Tz3P@0f91hii0_S21KQ1?Be|8178%9^C#$b zV8kJi)*jIG3&x&sibs6&R*rX01J_ffQeJCEpEB0A;dH!F7O&+IjdAR>DrNc1etAb`L+Bh zqN$fOUp2tH&#=R+3w6g;1wyDVdlZ8t8VF~)4?72W#QSfz*>qukR6npt3|(S)vk~kq z8md2tN87c+3hxYlKnB;Qn0O37i3{QdfjSR!ru`6O1WCWESN_e?g52$FQJqoMhZ4YM z45ARnE11{{Gpy)^Z}gc@wj|b*^yTwtB3hL=OB>Ou^%O+iZ|~)7WVSJ|Ms+e}P+Brh z@yto^!CsRo+8Z}ZJ%Tp03j3=6-RE0o$PF|mlTibQ}iE97g2XtL`{v=OQHi%?x| z5a{|VV0XDX(-Gp`<&|D#)mfPtjJ*}XArXzxZmhV9)t3Y;IrUN>CJxcIOh!38(Vx0#xb^PcobUI{wxp~yM-Ujm$C`Lf>$ zM{+bfFRr5aNW^Re)5CCwU%W^jsk^BEzje&zW>BRD|4h;aoPzoeUn#nWHPQtw01On+ zRjiTn+t6wrrC4rA79R_GzIX)q_Wo4Y!*^xfbE6`xMiSXDak%GhdotyN0)1ZIOeqO? zJ9P@PiXeInGXKdXjU<-ikcN+0HmPEwYcaqM1nku0$z$+SXjX0Ax4YaivaUh8!OS{h zO$5$u=J;^m$PIt!#yvWk@R~Ey>@plqEn5B!dj@v=E?>Dv&{vZ)M85=qu*`DU5h8>% z05<=eUm}}UJUlK=I1K=+t^1q#VRlWQ-YbUdgIEr>KCENREADplHlVzbEpnKV@?{J1 z`gN(JoR`UPjUvfIF?Y(tZ6UKuZ(XZXe<$Q5%L}Wdz}pSo1qW~Tp%6F7`1!%SXPBKS z2;0-`?`pL1cRKLP^@c5|1E~9kz;33FGOJ8T&!0c}m=A{lht;{4urEHwSemh47#!1tz{pyp}nm0Y1aq^y1zGjnv1k~+>!a$H^JA| zz@ns3zXz!#-m}PeDOTV0Dz==sC^08gtl1jRuLGF3&t;#rsxJIj}Ar%LFnkJ^hVvV;kzo< zrW$$#tKoM3S{c%KM%BCPz?q3#_MQjQ`AVA}FxqZp4@|#u_9wi5m{MNAu$#+~B9%DX zcKh{?A;fy-`Gg^Di{fuJiu;+};r>;aH8a=PO_5Cnzn3wuqmY6s?Sx@h;_w>ozDbu! zbXmb2&gmowxi@?7>pL{gor2yqJFNDUI40B}bblVPRiS9dEk4aOxjTF! z;;cR)<{=S|UUy={Tq`bmMmtn(qKDTU(#Y$Mk>{v1PIB+pR4!@_&8`I`yG9V)#RS#VSVz z03XS3X1_pEXo+soSrNDjT~{dbkUt-5wmUQ;^vgWI#EYaKG!g3^z=Nh)-(!%2CU9SF zkI4s0*lt?rB)cZsQbCbVoY-ukK&~&MMWzs|I5|@N;EdkVjT^bZSrPRbS}HmD4)~Bu z7pL5wE-}=_%T_l3-!}mvaJzu>%`=vr?$Pc5&i;UHRcrkx0mA+Dd-MT4wBYYf``4^} zSr5 zb*B0b9e>_kez!QBfBF0U#>obeJClLZ zDILjB79b;eE}9>lu90VJynlUCb7WOL$nia0dL-&-&X2SZ>y~t-UE6=BDeBfJgnfStbF>otu)=Tv;5>?XCl zam-J@s;hYYk0tNYk>5-`zjo5kV=naRK9CnqH!?m#)dl9YlMf|XdK14RKryN?gJCzk zJ!9`&bN5?$_4c}}2v0j++{#}&!~^`D%m3)6git<0JRod63r&iHk_(%l{+n2iGsD%W=33iw$81U z<48!(XkXXh%%8%x9fA;+y5)=#x}=x2NPj+9FAf3;I4siTfQ6my9VolSfVoMN6@_ux zg15V{vATH~j3G~=>Si9&-dQmMpG|+WiCSKYFi<+g-7 zhyi&}P;JE(gPGLa;E?qDzl9Ar`=&4v--6Q`rcE`Fm5EZZK?+6VhX9^91^P{1V8dBw?rQgi@>#p>CDmgHrXKL0XNs)U<54rK_H`5)^ z$S$uGh50GBtg+<$;+yzqoM{DRHcUt+NPRe#4kVxR6*SlqVX7GMu_e@PaQhsVQ%$`g zB_WT#~c}q*I7$g4mn^4oi98bQ20|@pSHb}X9{Q>qj?uc_$ zqIU4-WNW515;@dd1CdA9(6|NXd3Olk?i$C^Vqkw})6K_?8_nnr3MRC)ODgF>RO4%c zc|$-elA|j8VnLH5`TGKzmx!&~#U9dN6PZBmife;Pl`*iR(s<#ckRr55NVGeIperH| z)6yp(x>>Yd&^*aU3C^_fjeF;kEGjelU3K>xzYl4@rRg}}75|=VhK0sd38N&3=AdkR zfO}*ur`P>yU!n%EqjPUU_t4RSB8Gb#>O;o^nzI{V)-pyi!&~CQn^F(OVJ%tuWnT#Q zyhEVSG8FHeZZ7@+V)fGqSZ3Y>jpIEu{o);ADteF^=Ks-TWTsi}oy)V3tg7b9%!odi z2Ux2jP#~od*K*7jT3kL`eR=)ldw=8IAgdw`yh&Fu!=W}FlmnX?_*rfBn{?@22C1;E zIURwt{Au2WEt6!t`6TEGwlK=eA)CLBrnIkc9|ERazc<1yC`BGgs~P;c)|A$@Q8V06 z^Tw(%+`!IZZUBffOH7t%8%VOU$3gf0@UprqyuIj9qW=Pm6K7wqQ6@SOAY*y(4Klgq z8A?>B>hf)~u8Kgx=y$DHMA*f}=uwWkG3#<>&K!)3#QEcM)?8v; znq6u~qP<%_{hQ&ZIEh48%#6)Fy)ldc7Su^IZvv&q?7i1iL2FxIoa+e>J zqk+AjAlT!zYHI0Q_zkh}5ZjGoKs3wPRa2FNmpf5c)~{E=a1mDI=*{|VVJxQ%=NqOAJR2CDIiiuEV3ml|v*DEZeVC(wueKqQgE7ie~4 z^MSaxU2gbif>RD@p{{7F92)9-SR}5ZZt6>;epyM&q!b-(UHFoL2^B(Z4y*p?962I0 zfABp0|C`N_w_j+)#us-f3~bV*Po# zQ#oFay)6y}7Z7Z833x zN<0UW3Iper?7s7Y58-V2y>R16dZPk4Fz;=53vJa&z!E#ca6}1?-PCL;3URsrymutG z1y2bCzu-V-*WR!ea22g*yhonmb+bn^b$l*aLjY@d-k%kY8v&ut8Y20mZpewHB0`QI^OcJQqTN z)LlFT4s2TPjAF}{8jPcu3kNT|e2<^28iH|cvTw-g{c{ zYrABXwc`7sBuuqyh|2IqMP5pK3SxGZf3t$C1r@3YiZb0tH9v#&wK4#i3JvxUHD4sf zjhHHwwN9Oou62~6D^|c=g9MRDEF7ypa!iUI&m+4}b+htUV7x@!)YZ_R{r9dl<7}0s zRKws{YIo)Iv_g&SVu$kgrF&MS0GrG~7?M(;a+ESn!9^zh*dcj5$vT$-B3v~7sv=~r zG6gUM)LN!oExJhFtS4Wy%vb*E-L9GIG{PN%TX%#@g^}l{ShnE;jEizxzt(TosADk_ zb;6eVF_(+fHeBd6`7ZPuqBy(AJXH0LVaR*y&569Zo*^gKS)2hS#D&k5tbklfn==JT zmhg4pHQz)4q~R4k_aQ&=zVNqe)FPS$R53Q`YjVZwR@<<48#43!41IB74&L8r;CaSe zRLK>?YCC~e1?jwR!}lzEg4FY_7L}((dbUZVM6iRd3BYAiIz3_qj2aK%mAq;vC7IC$Q7|;!395Ed2*xxMBn@Hic*u?a%>J z9^V-WQeazp`+itmM}RU?W;E|F*P1L�zQc*Rhs8YyAg-G$h%4zR>#6=$@zn6+Ac6xQSzP_czUw`w2ye6}0 zy)?GT>`UQ>^{|VU*M5*dc*ud`N*{0IuIJ4USfrM+pGWX~e}Iwa_>4SwEF`oVNHJI| zQsuPe05+Y~C)a9=9~?+x{#^4Z>|1jAoZ&(^zs?lRJHL!G7%u-j=r)DjB}tH--MufZ z<4V}`J?gfS-3Z$$GvS(N0NQ@j_rHvBThFkrKj-~6A0blp+GJ?}Z8#9SIk>S@m$~rM zd^B)R=Yn(WPiFaNX-X?x{ss9Tzg@K*+Q$bNXa8{9KoWen@@${;S@q!Rc6aLMY42d> zWmR(|5`3n%MZkSMLAlo+Kz)7x5YoIXwd20|cJkPcQ%BZrTwi~AK4qp2@jnP#6;(1w z4vsSQ$wx8aoPA??Qu3>l?>b|r@5GXa^VeTCxW3e1z_PlXU75m3FL0AdNew&H1s@tJ z2JHD|_EiJOM)U7NOBh*m*J=%+z2hIyN&U-3fwr3n;z=s^ftDsohV?uQ>X5GFx6o%$^2vkg4D%yG6SnOEPL~UNJIn5|)is z@$)Lt6S1vb%e|x2*L?&9%DJb=n}qosHPRS|;y)OGECD+aK6OIUS|I_h9I!_tcf+aq zjL{*s>#u=)NJ7KWZ*s9JiV-fru}5Ev#Y|M!_YdMol|Z&BMz+XecqcY-)qn!ZNQiOA z6kcG};Qo_jG(&hwX^RFU;y9M!%$@VzLDR z6fnlm*%+gd@#}2MRgzMhnht`K(gzxBP-4AEXaTy-5#h zS*tOdQV|p@t3nxoQV;zbBIL2SiZFa$w@$((S`XR^*iSs(Ws=Z4t(DW`LPQgWCw!eP z5r6(Nty+3RQb_Q5^<{OixK$;FgXdWRe zu3#BIUO5>(u~mb~p4}c;bRFVuOrRG`3TYM6=S>kT<@0n@SeF4yB-$d){mn6VCqF~MLMI_}iQg7}r0#e_z zBP&<3-Itn@w5^Y^wa$pA{4$FNLaaLAKGJpJ$!f1BB5CVESlhzVyiuSn=+{A|49RsJ zU0yxK52{O+aq0D@|6$1%*+6G^Bdm_4I}R%=@7fOp{CaS`$7||gZ|{}%UuF5p_u`ge z9v~f+%`fCE_~rCukw)sz-Rg=N>~dLm#IF+^KxVSJZbC^jk$@%P)_0-i0SDbRi)C0|4xo%6p+y1b-#7wkrU4NJP+#aU%xroFMSLl}QfGq{mC98xyv0}8l!{ZMx zDgm&FP)D#h%H@{=ScRlgKe(~zfIKd&#pB}}Hjkrur%#Fi51q%ZPwUX5{sWFwI#7F! zVSScRGZb^qa*7B5YLu5~Z`Rn+uCmRCKtHOz6M;f!GV_i``H4)V<>7yu7VQ?2J~%pj zbq!PN_x{sSofc2zMIT_Rg{Mk(sSDaPK2ukurG=ru_bSv4PFsm9bC~p$zI7tBtML&kmrdSg_pNHj0T9a6&t3w4oj4MGOam z%L2cs0lDotG78M~C$AiXb9XwNx1Qe}P|}TpW0H`C%UQesE^)^~7FKp`h~yAXPEq)F zzs~J%S@OsgJ537r2K~t0c~?1qnB*t7!&p^ucax5q6!$;K}4i8QlxapSwZ7{~cLCdhwByAu(PM*7Qr< z1Cogc;Oo<-Sg(^hc9^w$>}d`HScmivbjDW+w{x)9Ejt%0?H-o4X!gOD?ps;JLTJF| zqZP5!>Vk;rAgqzH32Ok;BtE(i6mT0`?#)pn>bkTXf{RNoR&foJc+n<`2*d|2Q-!0< z>jAKPJ9cMfWo%E|kM2__u(;zcq9XIEX0&ezEGd%|T;poJqJbt2{5i+eY2-I}O0W>v zkqq`Mk{HuQ{CGHA_Tx_9j)|TR5jgD3_8f1vRU<>1ef<@H`}_1 z`;K#fWHx>H82XJona8e~H_T*T*$2QqOznj&(O{^AMjQ|i#%V~)aF^(bc&jKpL60EF z&N9$CB;vG^H4pl*=bC!dH-v5Ye{b{1;d$8XOG9(@MSkNYOV}l_ZCGf!G+>8>#GnB* z3#NY{o!&g$qy1G*5f6hlGo*$IaqBeFDDUwo_6E2mYS+Plt`Y+Zic+#O>zyL=UVm@7BJ3DTlOxlzZP)S zLvI0TJ1Piw=QqRb)k)Nwf2@&nkFb!7)kJ@+xK25nY_g&*G2$3IyY+Xu&^>|4aOYRu}Ev6E5Q3Y%^a# zQYCZZyjN{R!{4lKj zA=&iHvrE#?LldEs!p`KY-be)ic;^9yOs=Bkc)_O^FDq6kOQnn%08QTXAJrvaOaSNUn2ibXbz}w;Urn!)ZOgz!qW)^p!J1%PUz*9q&Jjr1;&Ha!T*8& zr_;BlbTvj+`t%#z1HfP)w~d7)hnaY!wo1|`8vz@xEB{N~@qav%nnw>V3oRLZCJRyA z|4LJnS^}l!FzEAq9Wam7Z@@29$go5*b{Gp2LbLIA2(#uletSi0XL2Y#z?gm5bv_)4 zxj|N#RcsE`?2!ZP8SFI2gSOhKK89?3O1PNHQJ7CsUtC#J%r4sN|9QVo&{Cia zscmfWs)r_7l2dWY&=*qP)pmYp);rulhe`mGmQK2o+ z=XxuIJSa}Q&H+s*u7i$<64}iD`Y$?vDo7%X1i#US*Hvoh{u7 zp*al4SQuZj!p#mFyeCRr@n+b7sv3~q8nRzpAe4vu6<&%nzx_uHg7~tuDB5Kga5iPw z2aA76Tch@J^C7(1+_^+hz}mmK1!-lZ^!&~;!-Yd2z)@K}Z~T3WpA2w-4x!D4+zrsR zD_!*pTP2Biw{*u;t4$W*#CQHBFp%v7pmd@&%LREMu7YiW<|$BFlP|@BhdaqpFwwnd zD1ZFJf2tW2^){p=#cwS)9XBFoqWDB)#HLzW5s((kAY;C&`{do~^R!Tmv~N(&KQ*bo zpOz5hf&<*EBOXcP?UFv&pK`H8gII&n$|tl{AQ6DR>{7gxPK1)Pr3C&Zs}GMXhZnc6 z3PWZTcj8-&i-6h~2FUwsv2cA=(AnSP7$8k46J|3Q#)K`?G}#J~Kk*!JKxOE|mRiRA zr5lQkrqI5{52w*5XVNf2^YZdP~MZ zJM56ro6c_by)Pac4?_I#jRL(kdt>8qAF8eelMOyQMG?PJ;h8r84N=JlSypfA@4a>s zUFZhX17jsgi+5N2{Qujf$F%Lhb5>pDZw6%{Q)=rLGbLLuM5D5Es`|3O32j>N&VPU7 zgPjEW6>3n&T!6R4GuQJvw>>ul4n+fr{`pu}9Sw1+8^8nJ33K0K45<-(1+)je39VNe=+m&An0~WNM z!NEUZFcx&Ocbx|lwLA}GI|6O@Bi>2*I4YT8&=NYnKhqW_|2!<#4KjlkMTJ(iqoCM6 z$*ObzC4Z1`Q)%7nR^j7+lGhg>%vw*Bfq^ggQz^?HxH#OXJKQs*O@nNL1V6{K1Fd3d z?eiBDpO}v-S?TgaX8)K=*y;C0yOP(G@0`5~im-ig5Vy%PO>I?q&M9GcdUfb|!QC5C zmEX;};I?K!BQyGQu8%4*nK0wfJ}d7NZU_VVEl?^B@eqK<+-`ROUvty6qyIERz%pQQ zx)X6X3tQw4_7B0S6;Nt1WBKPzwdhb+RXZhom%Ii>?6FLDh)_yPGRMEDoHl#6eZ?Oj zt_eEfors44*+|)&IPupN*ijve)RkX!K4%SaaxZaU7a70*EO8UsPiCTJHgaG8`+d#r zMkH5MiPfs=J#&6gQ!$D^EYvT&QvNSEnS%Kl3}0kbA~S&~|8f>)=qYRO&%deKUYhhd z>P5^F$Gm-2GJAdyt);IB`o%z|s#{B_2w(a!bm9-q1uR>BD)3Ibh%`jQr`@9118jaL z2;VcswQSbl;s=wHu|$&7pAtw>9}NIp$V0nd<>7i$=lOFvzU#>N!}mpYye+x0E38*e zcKL5P_lrmi9fA4S$V^

vbSvyH$Q*T?iJqO=3DJy>741ne;y;<+xUI<+Na{$LkEy z*J%rY?Wex$zl<^5{RkIytDEZME8$84`VBEi69K*M(aVq$ z#PP7M?lEy-uhhQ++2Cwr?+N80d+vX^4`jNVY&ZXD`7SV?yq%s;`ftE|yukm-4O`^`9o>bFmU{1=*m$R&4wY_Zy{cr#;^#;p@q=rbJQWA#yBYj@OJfDaS{ zvXy!>{)&9iFF~>-VTNK&%`JCn)A70K6s6J(XqbA~((b-Pf^og0u^Mq?2?H+0rP-i` zTOA4uOn>^}$79unqkO3cobGt4v?6S1hA$VaGtA~JLvp_31zh!>Y`M7r$TSzFLvE#fHk?Q?&^WW=rGWH-O|#a)wuN_Km; zyQvlNClh;pTASw*)sLT@7qK!2ZtN@%$ePDrJJ|8Dy0!eTS_{a_c%`grM42k3huR}U zmBeP=0u%$L^FehKp|k^6vZ?S_C@+TrERd1fHze6V2&`f&xDH?Q~`NVb3p|bNYGC z@5d)jZf~{a^{#Gwlrb2Jac#lK{Cpp=J<;sq=F&3XzHJr~n#Wj8X0{3G<~%?}vthjj z3aGdkNIidKG_YNnPdVFlO1p(1;^q6R{munNC&NEFovK1)q#KD4x!&Q`CZ_)ZZEKQ@u*qS0#j4w~)85V6q;@I(DA} zZVY%23!Cob)EJMv|ZJ@a=m5XFFVxXJ|8xb&~>a@!P>YglVVAhVxF zCT*LqPCyN+no+!Ad;NDApIzI^W7ZNzLSNUAel!s+l*2YIHJRNA(yWHP9IMephetur|?d9ktzE)PvpUH_X0=v*FxZQoPl& z;*wM~VqQX`59>O0+vGF}+)7^zO(<2^a8^G;LuAx6PL+x$M9;wzGO0=uD(Ed$%vb(M z@|XKB^u~bV7^EKVP+X#6-%w9Or~frQY5{c=E!XVQA$o|LhL>&C^=G~an{23Iz2E~+ z3*fZsJ@&`CwpV`&+gO_#9(Cn<_r*$#)&Fg1IH(*}HE{>HV7cnQ5){u8nmiO_&VE$Q8M${DOOkVM6!X~G)C`3O2~3yl!)Jz)1Somh|#w!eJu_2GPYNKx=lrs zlD@W~wye({QHddJB`0HJf@;A!_f7|8_plJc_q$Mss=%V~S|JM+U;>U`0ZafiatA|Q zk*_qUpWWY7d~Z)951I+zE3@e11vtE^92nfraH@ZLp(+OkW=;DhxiS~x_Gta|+x;h^=o z6Q632Bv*mRaEXOB*JR7n8%q^NnC+Bp_cn5nNZ#>hK`DUI7%96l7A#{+iYL}>u+>K7 zPOT5AVjU|iwW^90@>e^5p(|#+r8nBtkM}3{l|N|I;`w|IcZ&dj)FMF4y}4j1V0Fw&61ZjK*6%+7S77C}yG@~9B-&PW z!n~G!S4VB}x(uqs>)L&iSelMEGRU2CNg`NigHH%K34|Z=PxzT0K4D8HGaZh>W!{zI z&~Ony)T6=(k+x4@AKT8)nO$rBm)%@SFZOr&joV-|IzC31IJ<(yf)HfC&1VJDq$A7z z>lJf)#`A}4x3cBV7uhjJf35lqL->JZTn6~mqz)x>&usk-kNORy2*Gfl6kFqo^q??g zh-8ChtQTNJ1tH6e;+9+d5tVoU*0%xsGo{ZHA{8)@FnkWJgAnm0ZFv7DHD}( zT^612xXPz5lm{^c7}|5&>EU);A!dR;>;8E59c7^@J0*ymJipMAeV4ZIHWWAF=i?== z?q(%Qxa!qz)rYPrb&$WNMku*|$zZp`)%7C6Wl>{>&tv4oMufhmU0a?-R*+e@CS4Zj zU8iMWPygY=Ib-Bmb#pr;)#l_FL`G&YUm=vewHcK7s=B6Nq*{ONJ?v~@@-cBWV#%GS z@NDaW8!rCn!~@CIcMZ43V$Y1M_2xF7Pk9eMF|21=Js!8MZ?GSR+V1?-H-!sKGHS0v zMQoHT2I7j#NQ~6lDMk0Nzcm_2IRnc>T4e|=2@F3V zh!vg~JmfAkcT?7J$pIBzC#?r!>6=3_&G$Qc73wgN=k zuMkE}5VfXAe7?Aumr%(N8Fr5I+dG$^fNqMl#t3fNoibWFT~^a*7QRrcck0<6fOr}5 zRJBvO$DJGH5B8Ysg=dNJ{}}L`prk%Vf@Mc6KA%XKfv+b@4XtX*qtuwyb9cY;oIg+J zr#kOTW~tR;(1l-&2GOsVU!BO61`-%94>cX^larsg|Y zUzqfdRv-Jb?KzJK5m+XdFa>E${cPU;YD{- zcL?87yVtucg@F{5h$TFpz5J)R1E~vkA3;)f58p*!F#B};;gGXYNaRSx@9_(Lx)Y`T zo8$q1rn1|kf$6!sJDU_LEGje)5k00BUre^R#_H?ZN%Zwrg6hJ%Z9#y}ZUK-;O!h zR#7AFy1jA6yO?~dax8gAf7@%61Z<|NH7ZWuqd7=I?I4p&N_+oQh9_7^1U)nB-9AR? zq#YV#qqPBQwYn0J5w&c#Q)`ryU_rluY8u)#(^n=$Ii6&nOusB~{qevF=#Y3-@hYuZ zTB~NYZNR^A&(ycl4m-R(>yt{Ce=A=li%xQDm&=}7`{bmiy%uH-Q97PhZ~!onGT!67 zAAC&oc6Q~QEX+bhp+&6~BF z_4{pj`8!mE!u*i?LhfuDe7Oh5toNSiOV?^=^xR6QFif6RG>9MSm$~1!{q(+?H;xRm zD0+hT-D-#(RoQL0%9xL=a|dq3<2NUMt1iS=alaxTID3nlZayDsaD}<^Z?Ox~xrOYg z5(ayd&sd*!@2_BDlq4?@p;H&@btl^}>l2jcyC&;nAd{?Z>bAn7{Cdp#3f+2xrKW^$ zj4nUnsn;>eGa2I{HUW*T6&jM*M3YPTAFuQ*y3IM*`UQ+-^7j7BxOJ{=L|9oZLXmA5 z^gQ@0Vh`UEk@>S)a81zAAXhpUCVpyFt2F>ljwcE_>Ef>py+WCVD{RMF>x5df1Q8C$vQ%}67%bOMk@B45s8@czZ zyvV2z&?@mAubUgjUT9X9*bEQto@g|%_7r0Jx(ydb22y{P9OUM#=JC1~iT8QF>8h2u z@zp=PkqTTt(WGop$}Os0WbrM?NojExv~L*43dD=>T4o)oIU}da|X_UU&v?qInuUjQw7@u;AsTdyZ`zdZV72&tF zNSSP9>`@H%RLx~}BGj?8pxS}^$au5p^nc|ZC`t<{fmBpXm$tS|boCm05%%Kin$t49 z-R*v>!;S5Pd4q`vu9?J^FcD=**I5d~s`t$_#nV{Z(Bd?2jq;6)gktEYGHceaOLSQ$ z$EOY#1^TIth}(M@6}3l{hpC@#O?4ucYp$QKwmbE3$*(REx$=#p%s^4Ea8C5SKFC8= zS@&26QSR#}Tm_CRnobl}eC0Qa`{dwbe9!K}J?4e72EE1*n@Q1gaKR|#=5Ix#+>!_V zcHWsxr^>aO8~tDX9tMkaedn{)krh;kz9YKbruLN?f6jwA^ zB}N$8I7dzHo^;*58{@xhZ9C+e&{EuAnv^L=|Gb}|4YWU!YKP*=f&5Okc@6XUo8KXr ziQL&z+x7+c!n{OEkiWx3<}LTG5uk9JJC;m&^s>~88kqv?hQ<%cU8=SNH5jj%?-Ted zyL99z#fh^IW&?R1Q))NJcN!E^D&JcBonH{$s?TjvTxgK5iQC+2VfsQ<31j=c@$UBC z(yVoOX8C0jX@O~2HILwe^HaQGmZc9_E_9_Q;X_Lkk_xQmc0dme-Wh+)5!%b0N~5A8 zi&EV>Yf)T-fsNDD3b_RU7Z(oTsL@+IKy(-%i1U>At8PH1fxQW9I#tf~PnP!MmEwZK zG7xbUfHP9v5O<}NtGaFnHDk8GRN@?Ss}W$knV}8$)v=Ds#qWDvyiqtlv2iE z=c-$`P5mTWAF+Sk2tR$cFeOD2Yv&Xt=9;uu^wMtCVe?47Zt%{nGK+CuqDSwd#)O1{ z(kt@*v$rZfHhVsa%$8;9{6~{lq-pWpSupF!t;yaO)dYsng`zudUVxYrK1!oD( z4!23+?rjS0vmL`7Wz)*IVuh!_KD`5QB}L4N{izc^+}%9Xy6;_Ow0YZn zhw6dYUJd92Bofp5(P^|@-g#Jl`WRO*o6#tYLi%Lz;uu1tJGc8W-{ zF-31(>UqQ;TLR=*HcKR013Tf}ct!XKc9ADAT+9!?>c*+!zE9FJy3`J z=`;OAAl%?#-hu``uGM{J0v)XNc z##HKGYtkp$>vj8Z+wrVx$uCBE!0npfiz`jMh)j9OD=9p_vM$Q)KUnIjGuDZ~Oon+L z;Ly*<&{PKEnJRQ)?%tHljz8ujDBx{~NbVf$*(~d>@hc0Jie5$hQ#+@ezY~jVWEh96SP{*SU8QB?3cDTKETvXb0MF-dfp!UYX#rXultlE?qT%kJm} z7wI;#$Nm~i6W6)+ailAA%_`QlX4OE!d#1eo>qpb?PAS&qEYgWcPU6QO;hF4R5__TH z?`WbtUJ~locxBwS56Jw#LiV3P4Gv|T6L%wps7wO&_;n!3kpLM&duO;H&Rxf?cZSve z*XjiYly%AC#HhfXPoRyyCzd_3ttvSbCtqFdUJd27*S#g>e0OnwX!NY5ws4GCP}q-e z7Kn*?!^p|y!nMcHei>&KV=H>CdW`G!!>UJ8uQAbAR@TqD(aNQ(n&2I7G`Un$S>~<( zB3K$Sp%`dYM8ayh)L2ZUY_FQ`*ya?>*3jXBk_L+dk?%x~Cauz%>{jEQyKLu_D@Vg0 z0(q=m4xZ7bA8FfKVBiL5)KPw{;-Uyqb|nGkMTT+C)bE*nTN)YJ!Uf6<_qPA%_NrT! z4_>>Vb4E7vz9Ndxu}gD;xixEgdi%S)Ax3m-qyM=5b^Qmo0a3a3vpn&t>iQBm%H)oD zvj+aw2Wv{tmHPvDzXv(hs>rtm6!cl&M^P60s2LSrPEmlCl;NDGT9xCixr!nG2?~(6 zcGC^V=j1ZIqt$ge*XB683T~Xlz}S0M7FzHblQOeW-bby^r);h?mhUI{ZT_0AN@|eF zrR^om_EXB=3rO(9y2hBVx5Xf4MWZ0oN3Sw3hSgkyXIua_rAB?0}Fhv z_~a5juHw_&#K1+$$V$Iqz879BkBEKD?xQJ6a2BC9=s6pCnm?wA*bx@Z!ZYH5cp3u2@bS!n>cbZ)r%*g$#ydHR}Q`G1XOP*+aVJ8B@fQLtW=Nt2OR zQ~heu_b#cw${fI-fcRW_uN1EqXdy|_>w#{#{ ze)yr>@sAG?dwm_|c94%+`yRKt^5N8A55{V~;$rUA|9h9RXJ9nsd=>AOoFrW3HRNeb z*&}KbaEY94$z|I-RKD;yHg}jl-X%fr;uWYw&Cxh64gMrtz!BRklR?-|A3~Z zBw=W6qVEr64vm*3BzjnS?M(l76$1ov1a6&WcT`DioVVB|f!nvg<@cgeVrD@f3H<#T zt-SfYsy%`C{waC2Yq0ju1vYcaYr5TrZp$ow+i&cHZnU=Lul5!q9LMR*?5TBJ<2qA#KS<)#yT6wrfq&L{?X3tJq7B`#jf7g;@00cbxUm(A-#Dr4 z#es~tvyx%N$WWKGBtBkx7JL@W4T*?OIP|*X^FkGUp5!(qr;^m5l+u?*UJ?PMta|Q$ zmj^&yS@(jhlvd~MZ!~qb#ljo%`;M1lR9PgIwg|KPncZJ1-8MBi!o1oJK3Qj;-Tboh z{zPAu=Xzmj~4bA(` z9tmak0E^h4cGUR4d#V6lIY>*U1dqPOl?JMKRJ}7h@7A8U9v5cyDy{+?;Z2fk| z08)`QM&28XaatqKhWIIVw*J&GoEO}0z$uj@BLM$Q+4FcuIP1Q_KAcV6Z{n!ju6>Gr z${Tws{|8;pn^b+ab*&uFCAFV1o9^=kB)p|zp4j~gX7uy%m}W}H`*3&s%$Y(a;>bi+ zBUHEcS^ne38k^BlIR$35=WPx(I4c>%vhYm5yfW*bciLSp-}K`oxyxbNyu_t?eWhmm z6nC23*;_n>SrXh+7;4e+_WzFloT4c4Sddj|y)pfA5qvVYj|BJ@rB#UIxx=mZimZ}n zRN)tiRsNrpw{d3H-dT+&*aog=R zt6%wz8GqHQt0WT*n|o2p zJ09e{*C+2>^u@s>H{L)qUi08)HB`#>w{TuiA3J4BunpS8-X`07ZsYoQjgR{su$(I2 z8Wxi=N^q~bP^Fw7#ui6o-$=?aUpAgcLgvNguYKzSD-YGzvA)Cm9?`Rza&LjiXb<1$^JFos!3*{99s4CQ(!{QT<@j167|&`~ra|DLWYuQW z6pkCYKkb^RRTH(I;Ft26*_!W8PF#^+y^}36z%yIk6b0{VNh9d)CUtfkcfEqlXa5C& zVMx_gi=mFD_?|fJrB%(ZV=th*U9d8wO92Eg&zzc=6wAUi-wNNxJ^n-WE?X*RgUpiEV3ZTpOk(aSKJ^w=W(!U*Tq(13y;py@MYp8U@QMpKbw;1AOXP(AoOzl__QOU#@5 zDl8S>(g>>P#ID5~hY5w5MYbfzDY$h(-@=avQXBCE;C5nw+xchT2P?u8i(&sm{}lh{ z3*SYUH7VvJkhH^?i6@_1@OnXsVnn}*jL2Ko)0N1@Td}tIu~5J6FWYr8>o`gkvEfUH z_js@f_8=|zpJ!92U1xb_E}=jFXZqe}vXj2sdEL1v#KpPZ>=qMsziuLfWJp*vm4fNd zERlO6PxVtYO8ZkC#%gLbH&(Y) zimg_d6^-!`okw(n_&t#{7e7H*7$PcNnV{_yJ(Aywg#)G%)=d1Es9RK3aG-4bRBZ#Qd zl4ePSHFI;t3(bJK1k~(P`qtWg)~PyjlhJgjI_tNZqxx)%_FDYf5vk}CT;xg2kFWhXdc^O=-Tmwp%3WL|1y zHYKKk#ySC)LMv3#@yw+h1ofT3#MSx8dH7MLC6b{&zWaxq2t5i>kSN`mj(n>5iQ_MP zH&~=`N(C7Sm_gIJ(7~A5{=-r8WKE3xD#!L#W;Wh(SpV}>6m_~G2iC$2*4wtb+#>(r zsj^$faG5Jzrw7zGLQo+Q@)ib-rBx07Cj$3>%bxT6vpe4x@F65!L^{(5!XU~wmX&TY z;#WVCgh~cG%=jzK${07~l+&i?Xob)zvx;)1MSkO~cU?25=kht*NXXvk_v^-|AiyiyR*H3 z?1M|Q3P53O^!8Oulf0Att!gG`oYkclBqFWzqnDF%>bV_Bk(^de&IAU}cY}W$6*~H_ zrX2j2?%5mDAkRJ;x3IX;)L8xDbktcWff_Mp16-i-QxlnnIs@Hu(l70dN-W3AXYmg8 zKA3{pM^bWvGd5-&Mo?exbu#AWfXBv-)s&E+QpY5qu<_D$@^JD$KL2^gT8}o~vG?PA z&KTsAoxhFet5s<0Cq7R-E@jiQB}L4zTj=~7t<+ays;HKqyebP0;0*kc>NiajK2)Xj zO<^p?5FEh9jTfp!9u3Q-n3`dK>5HK;F0))()5&}HNYW=v>Nr?!iox7IgK2>l<59uC zYD1e<+=be8@oM5x)bb*aTDGY!|4?!7N2;YCQ~Z4;JwJ_#$BFxYsH6<24ExLL>EJMK zEOOe?i^_D{QF0%20pgNfpjuBuIIl_~Y>RF?#G_n~HYWs@F#?H=k|LKCzmr!UV~RcR zC2ppBuIyNPrm4%$82cViaUa%e)2HFW&ONpgd`XJ=tk392?_n*7?#RYV(I&kpSM*pc z5KfVCJWVpqZj<|D_ZQ()j2+Mi`H>dal?%S191>Z4x=0Q+4BgImceX`8rdTFNJJ(LJR4%_RjUQS!{SmL{G zic_sA^6cdTUg1^W8uft`F&;ET2MV`NOu_w-^F9BFJ{+jb&MdQI2TM#3b=BX2ZA6lE zy#4V$)^Svr801qR=DpIG7-$y!O}TOB@P*mCc@?MX<3I2=sflQ3H=79PxF_-ZV~fmz zITd5TVumL+cZUJPK!-mNi9DDNb5|TL0g@R1MWlLi`o2q zSuqC>ynM4H^&^Q;k(_jprKK!?y^|&yCB}HBu6cSR2aem4{xNeD;;H{Dc)+h=n5i zr7NDk-(_vL>%W8UZ+8h|3>zze`y`bxp64#lWPb0T#Xr@5^~)fRUL}b=jkY5Md0YT| zo} z&*6RNe*=*jQg1<19t?JxyPViJ%pxMF*r)^c((WV=MnO%-iA}IqFn>8o$Z$~dyh)dc zeVbyBTfDG7yJO@i+5b3p{Y(xsXYtD)$C2Xu>4C|uAP{2?#-MT)>6Ol@`RzxsQ$J1iNV>Mp zJvjbny(W1G+6m5DHzzXOxiPYL?hO?BqFnY=bb)#a+UnV1nyB6IUQcx0K};@kr;Yx9 z%Ix{tw>4SHvJs=j?SSI=H{G9SrSeXKDi_CgRIWA4p;%b&L07b9>{(99&_l~JAO%oG ztUZ|BSt={bnu&hA`;W7j*@l|{;Q}S_r8(akhScn=$RNTte1EY$-&{-034c!ZuaN6> zxfRZ5Gg2w^S$)2eZ(N2N_cO)~^Ql~q z0LLM7vFKqx^VAhrY#+7j5x`Gy1`yr@m!xJ4hu>6g?mDkBAbkWI;i)S5nD7)(en6}c zK!1Zh5HbMX=paj#^ljnFvo6mpQc$}^r@ZLe++CTSkFP`E3C-8v6!>cPyIm34n&)5RYPYJSihFM zKHMDR;BEPD7oRgh%wi1m0vlqMV`l4AX$v2$>b5P0VfWP^TqlIKl_Nd={>}4aU^t%* z?W1`U@*ddx{6lX@rUgPupyRmnK!u?2DGC86)}P&AJ~y2rG*exjmoM9Ehi)=2QIX%+ z44%JTsHqLF%FaY_0Hc16?ze-cpo|Yxr^4GMcf)ct3>=W?`j|iG6(8pF@txLT0+?G| zPl!ww;pKB@yN&S0$n2!2gs)=4r1S2wVrDP25F(5NSGlnF`R%9l!sEo0u$zGNQtXJ( zx(=T$j#vE1bD8e1GQSd&xM?#r>6~6#D6!nY%g%Y9z$~#Kh#BBW;J;rINIDn&|F7#} zz4(XRt3O;Ar47Sh0)W%m{7OCj?@OO1y}Tm%hfjw8u^M`=EOY*1ShbIm?s7B6AoyMG zFgy#OEiR8L?-3dwUcf&d?Yi4K2Mma~cnbQe3}pC18WtsbGeOwrTmur^=Ua((<*Es> z{5Sw*8JjFvsWJQ^J9S{*e-kBPWlG6&N?PBBZ{@`2yAKLEuRikl>l_co@cB=i@ijfy zF;fjM@$&Xhpi6eHykR3X6%k{XttkO59 zvy(wTZfTd|_)?mN2--mj8?ZqwZC`%jJuUc*}qyc{ePqKT!A)`cceT z9W8eIdbQeYead=e%o>WdUvlxK$<| zd17R+2wJKWvm7W}H1i~9Y@59f_gtvlX63}-e@msTT*!t^zF&wj-F>A1nRuswB=fKU z8rsyS$Mzp>+`(5dEl8Pf5ox9$z|@poacnYct;|Xo8<#H{YrL`xRCd!jyc(`0i4zc#O)l&;M@lJggy9>y7 zR+<0#b3&t2Yb`Ry3Ojct!o0GAT2#z2PLbfuJb%Vn7p{(zv8ZcgTp56+Y>x@#GzmBr3NCYO~uL~d18fCfx{tjKn+zs&6xUKhgP z$;)3nyVnaH+l8m(oYRUQ!u9-miuLEY<^f;`=x%ic{&sm`ZXgsqVDQ~qsBg$L4yH`? znDokZ@BfNm!MITtwXd5J`JRP6YzkHMeo9c48L2I(4IckWX~or5)2e)y%(sJmmbhsd zr{$c->|$|}8_V1W2X>jwm7Vc1eETOSFShHoEe8g~kl1KakPb^R#%Hw2RY)Q>eJ+V0 zVk?AIRORoTOS<(i7c_21BkdSvS}DuZ9WCvY<~Ozy8|RG5K|YMu{ZqlQT?ytW#{rcw zzd#4$#!;pIbV~b$>C6Ja^L+R9#({EWc%^HS$UE81=^NnO6KdR_P2cz(%&8V3dqa2g zo)4%KL82>J#fVLqYJ8|@S)@v}af0MZ z)D;nJ=0Vo?$9_l3XPV0onw*NF&dRJv84d)UObXya9O(eAe{C8~Ii~QE{EFSx8{wKH z`AX2ZfX{>F1SoBRktETh7+bey;3TFu7@?nY_IjGSDn&ijW_s88N&ZvuangJg=SJ&3 z_4PIjav)f>Kc#j9r|qS!yW=w**rFtuQ*EeY=SjrA4$g0w78TXBHT!Osr=MbuL}X94 z45#M@E_87XM7ET$9E|xe$8i)3yCp)8$&EvjsxF+LIuPUJhOUf0TT1y<&Rm_%t6F8xKMw4Z|<)y8`I5RrB)asO@ck9;X#^I)b>|Uj zznSX{4EX6pJF{4j5e=Do!fx+%NV-s+I9O`HgHq8UKC%Z}^HpfPN||sWi%14h?W}{@ z2?4m%fSixYTSt1hB5qtdeC^Ux(T9!u!S`Lr$WQ`sunXUq^!Uc34`qz!)(=l_`|0Up z9{8C|6wUfSxy1QI>hnbt)RR>;rip$KpE`=VC^~RYaaqq#!Kg?yjM6~!Q4rg z>t{XyZ4%-G1Cr``)dw$Y8Z%8aFKd(3Boh#UhLZ?xc6+5oR33gwbk(zMBb4je%K`Af zw;{Q^pmaT4p@96;&}hrbV{ANxI_LtP3&RAJhh~xtIT84qEB9x|uR4IPO4Xq8x228A zLc5sCC`73}`GqmA{v9ZvWGP7^MQS@0MX2$B?CuV_ykNEe6kd8x%X-P)k@5z~DwqkW z3>T5SL_1e>xlnsEyd;xkTdt|nvBBCQ>F}s=DmyvYcL^cY4nPQ zm}A}FM9+9ZpNn^rKYTr=a1mDe&bxp`3S&!39+FEXz8xQ~_>x(Z>Y8AJU0``fMM2sFDPdOvG)EzWbjU-eQX++y2sI^GfwqCOOwi~J?Z&I z(s^|yf+>|4mz+uhrl_*SG0DY~b~!j80&BIB@INnGTw^zHL<#$AarrwGx{M2HwcX58 za7)hIW3JMTv@+Dl1 zJHuW2kdMsFc~OO>bLKp?B}WAZ`T(q0*fn}eg|`GPyw6OH&x}4)m)w z{6m-+?z7Yk!r(R9CR_+m*B?ciRfDj_`m_8uW zjU;E^bW3n)nV5M_?7|xD7*2gxF?Zjs$3i%PI_p;X+g~^$ocuR_tgxFN#_9tfR~7gx z+%cZ%^Ut+LVQbi>4d>p=8MkBvdbHnklly9dQSM&){_slt0KQ-mo$%FMyjrMEEg#gR z1fq{_g1B;ymbmFIZ(-#rC_$x2Dk1-ZS&XK#F2i zCIwb^UWVr@YSK>iDEE4m*!>%`cmQ$Eo+Bn61HaB`GXLv0l(1t}Jp}YY{-fxqd#2;rFw? zV{w8|IkAORCc@O2b42Ks1$X3TO`>bW@?@aTEBoo>1{>Noxdfnpj6R<6DSN9OSN$7E zjvq3VT#ArM;Tkn9;H?b~Da=_o?9_2s8L!r3(QX9}NrMT8WT$Zd+8CK8@00V^EUxJxOcBx#ptXV8=?_dL7Hk1E|1)0pZD)5tD=YEYVJZBqAJRvF=$$~#@IYnJ}2U_CBF*(8p!f@jum=ajI_MyG0O{ z`8(8VsW_$A%AOLMiu%{}8<0|yNooJPRem?q1>aNp_$AsM_#+qaU{L5}l7EXRSV*mZ zb?mG~T~ME2MLlgZMMBAUb$-4-y_{|=8>co&6%D>3uYo(9Qq=}VOi3dp`r20Gy&62n z)}>nJ1?!9%S%aL#;TH)3And_+0ajS-bj^3DjtZ|R)mRNZT;yF=-w-*>(($JPCu~=G znps)*g_iN0sb~8nlfA2-myb?O={uy)Sj<}6RB((7^MZj4usOAwX?zHpz>;E#-lmh8 zs|5>&Hhuy44rh8_D@r5hGvrf1Z-LmDpk#PF?=7KhEi0t2XDyGZ5QAJ#M@7%^nc=RL zI38J8nAU)zGyL}lC*iQoc@2t4yYb!}R%4*orf)8Tz(q7|S|xxmYr&-ygjw~^e}`GR z!iEEqH{#{4e#I9wwQM_e&n^vx3z&!;A_Mp4TK*Dm=OUK6HAO3ogmvnv77MWI_n;BuX_ zE@ug0G02k80Rx4Z8dmSb;(sYukLbyax(U|DJQ5mnTQ98Qco>pvmmd28wtngZ_k)=G zq>K+_?sEiraY6881l=o*U-FvxmYOMIhN;CY8U#(MhTF-@SY1LC)HP&Z0iBm*gnbs!MR5duV!(dZ!?}xL@JhDQ~*Is5l368S^TvnBzR%}j%=y4>{W{9V-#rzeSiwu}(y7>w^|dPYgP z+IdQMI|ty%9dQ%lvfb1j572Ue_LH7@gz(h*scf16G`NW`hdwiw=(9Vp{O=PbFn0g+ zb-8^4Pt&u=l9NxV(j%&G#H_{~US5szp8gxu7cA226bVEZJr{6TGmf__C6g}8ou4B6 z&OZhdwN9TXbzAbV`_t$mN5xIf$8OCw1Tu0wc5PjMlIBJ})R_vGl|!To1b6f3*mR)PoDYPx1_4&0V2$^kS`elID*8HvUc!3O)T zN|*=Bq?nr(_~aL@rS3(jZ~d|66X^Yd zgZ5s+pa=Sr8{8a1qvd;U6xUC#7(4~EYhUVO8^r$L^6^yA)fFVSuC`uJ&zbV1R)RS9 zE343GTR5d?qE7@g)(*vNf%g3Mk=R$taQllLX#_{_`%#+{o$|m=oeJuVVKc^-JmdD_ zljX5i!Nrd!Y#&YzOV}ZTo{z==0v4a;VqyuHZRdyQ3~{M33L$K8xxC{pN^0OHCUdy# zE62a?ap&I1z>UTxf!>*^s|70lEJRSRsAZBOcdAK{oZ}`}yn3o>Sr(6y0oIlGljf!u z(*0|(oJrRjL~xeoY2otH-2gCyaFBR@Ghw_IyA z4ZoYh9KUkPESI6Lnfzrb?=78P`|S!6C1Csmd8=>#x(<2@R{hDP)YO$cat2ksYI=CX zD0a#-_pMmEw)zoYU)xF7kZ(8OsU5^pzq`Sk-+S<1u6PoBKJmz2KHmRBjxaG#f7vmW z{vT~6i^`|idNX+gOIaV zKiPHq*rKAH#xV||L4vR>k0sEVe&D?e(zyWoZQql2+fGf2IfuK9ha6{v%{JMH_)^JS zA`@(!_Xu@k-`*hth*gdwT%6a-D!Ycm&n&brRhCZ4$VAGp_9tLk%1Wz?T1o&%uY1!j z!k=2Akn#bXXchNU?`2q&w!z%px01y+R79_2RMi}tp4efysb6O)%Xn_FHGu$Yh+mz2 zn-}9YQ54HxDiN@qv{ZAX_g!KP*M>KVt9QvWpVB<4WO^*O)e7J@#8j#jA8{@WB0^rx zz3W@P589=TUKJ9%l45z0DsX%Jrx?DIax>ZTDS?`199Y;H?&4DMw&uPXcJd5A*>k3& z|HsAg;Ux(v$;~w&{?={)xCJs6Yby`6WqR@_Y_?8qwzNO=Gw|}0(-O%KZT+xjWC6rR zD3rNQ3Aq2qxgVZDRLZ4ic|rnCGF)1~>J!^2u&T=KN{m`_Yhz1Bwc%9f-gOtM-{fpZ zd+76&uI{bx8#Xi8o~`ec>DIQ5hbuLu4_*!!R&aauQf(^o+cwe$VHNpqaB86&*Un$} zQ!=Q27OtZBc8{iq^KUwy)#biO5_pd4hF~fNEY*o}t-Xu}yJ3#pc~xgu*G4;Hcf+ea zJ-nA?p{PiEj8%NLg%F0Dx#mkm z5%z7xW!ayDg;y7C{BRYML2a9Dt<@ZoWQ3Du3n|bzX8a}%w~WXsNp*99W@i4S8}AD! zljM$+S*$WSaKKEidG13$OO^kx0aUJ2>JDrR%cS_uR>o885&g1Pb*Rt;^k&0*eM1Jo z*m_nhhQEbgKbcRuOzK&~roNHgP-v!+67*^L_WKBAcpN3=VG>)&oTj$Ohd`SmQeJ1* z!oMZ2Nh@tu1VPUW6`Cj(E`;(l%5mB(XiBvIMGH%Et2axv31$1AN=q|j*hT3ewA0?w zqfx}{xu6JbPo|xDd|mgtE_b5%sJwNGV+HRWDN>KkwPo4k*q)V)JA86p<|yH{vY}#m ziXU54JR_Sa_hxQdCOh%6T`&6fe4qbtG-E)@D4%b`Jmn!P<$-Xeo#P`TxzKWilk7Ur z6SiEvCIMlanXW1ze*T@ywpO)tShE;GfAjB7dLm$!d$Y&)NP&51x`qi($&>VWBDXc;?4RCa-t$ zLzquiA{gTHSHSh`ziy|&+dqa_nLzK6Yt`zH{^@eE9cyCuC|UG|Rzc5?yC?5mLm9#y zdrEr(XmbRkdPEy#+6tUux%IM=!c!rR!-u!#Z*ViFxSh+Pe4oWS%TmYETUnD@07}o2 zEx+%mY^+Li*9qWS4i}de7gkl09gih{fArI%yg~audSmKKJ40;bSP*EkX-%ryU~UA+ zoN%FOWXM~RC~Oh}TYR#omfJh^85l1qMoccQj;0RA-fwwaVBylzohTRAl|7fWdWi^F zS5Ju)4yS7FpYIZP<`j`*`T3qZ1AfFBP?C+7mk2C6+i_%~$Y1W5HM)fj+5Z|rOdy30 zgHGnDEvz-OS2O<{&;u1klkiWiOfF3hb|ASzbo7r?zd!Wi78l}NF_L@*U}`3zkpuru zGDZr6u7LaJ3>qb{FK@U=c?g1buD74jIfF;75GUq2%)7dWcx0if# zd!JOWz@6q!TxH#w&qDj2Ja@8ReJqW+9g5`k(ld<{(g#hCM^WXx zSwPIDI^5>@$@ngk(Rf|rRR5D0l4$fP;BetSk46ls3pg9|z0a-ZB0@fU|NGU`1f1Xh zK1aCPJxtu+<1Gje#Dxv;64H`{AdKIVFS+wdN>Xznj-6nU%9mVsW~+_G)D`zq&;Cf- z#+~mG+k{bp|9R3GTSgR?JvR}_B~HMA7$z2o(lz%Zi0@*+>}(d)U88M-{dSzUceQ#@1DoL0b7hke+4Wl{HPcqo`RQ%0LrBb@Vo zOEJRI@h#M6RAp|I-X`3Bpi}?^*P!GAg6nT|Xw$jbIU;|(lhf=h*n}adXeua9kC|#e z{Nl5`sB$1}yLq(E8tRl$HZwg6wE8s`%D6ANse!7^tn4YUs|)D4qS`u8HW{xotmFr{ zUT8;$u<%uVymL>}OI{|SQ&P6gqKI^SkJ~lF@=1z$3I!S$I;XB4oM=#EJ+qIV&7Ko7 zt&%)({UqF_1e$62D5_<8mXgMO>Lm;5#ssISPFz8I@n)o*2IF>2?JIemB{Io4#?{YF zi`{n6$^eY*56B{d>O%%aRh)UGVj!~&G)szVI`YO%$SA|$`>s)C*tLq%eFgGdubCu; z7qmvZ_uVaQBf4I=f=szmZJlyXEOzD~a2bE7&=K1nD*m=>t4zQqsqsfqM{?3A-6eOX zor7ZZJTaJ9{w+gCNu0R+7+;Pe6N45RmZ!0AITM#d|&TI}^ zB)kpJBFxsi&O22W+dVFR>Ug1pzqaGJ5D{GsPxvM&xNM3P(vXHKnZ@>!(jY_E`!nk& zFWp1x;tBR$Q@m%#hs}?sptw+M8F~))HMMIc zB+}wh&FGb?Tdg%FJt2UvFdza(7V&3fZ%XV9b@4eZCPXl@uVr2Jd92Pt&y8QW?vBOa zr(QXn%14F!K1$EQw2&;Qp2k5N-T)%Fw!oU-P$WkL)dtn0ziq&oeGr=81?|}n&f7~= z+Y$97#=Pf=HP3DJ2!mfQ)y=U+rDCXmw5Hc?k0^P|gNCengne@ursSlvl1E>s)(uM=z#=5{R=1FMH zY|AW)a**vFRaIUlpj_2L3Ygf#xAiYa9k5R4=moa#C&1DdjKQ$Uo$j0qyt^nqYi6`@vMZ&k(>8=@L{A|r% zGJ<#CT|sh7Agy%qu?|-c1H*IT&lD|CNEK=c#JZn9?1>yJ=q`CpEKdwgBrTaxZMLdv z2OjqYi0Fj5pr?*vc~kGufBM|GXz3B?F}jY3?u<382UHghNL{HgGY}b-&W-nF8VVAI zb^5G<)L4Jc+tV2uF1>ts{5>U%@4DJxfn5r)RNxxKInYVuNPw8g*cNw4l|4 zj6-xPcaL5$xK60-ad*?j4{nkefr}6NQ(Sy08Ct~+e90sn6i2|6XFe_fmlR|#putU; zl!xIck#^h_OR;qYm!++Mv3SN8&&m7G^V59@574v$zEKHQ3Vb8Da`9X&s8TOHxLGDw zY2tcp{XQGiBHu&)3-Lk-bgOhV|9o<>=TFtCoej;?02CKcG4md-#ak)arA$}?g{>&n z?uF(ZwkRs-+XKh--6Hbavqp;L3&D^l$sldFA^D%h|E^kcSIK&xN4JDy7lTWg6f^Kq`U(UDr9wmaMVjphe~53_alB>m;A z*c%R3O#RpO6M$)n;@PQ^)%JiYS((q`loW9?;Z~T)_EgGP#)tfv(f;xVlLV|hAp$&N z4Pc?>K!CHPvp6vze=2Qnj1(Gn(PJ&0NrG`%$DmK0I-ZF zh4UuG3V_Qr*QuN(SIcLwP?5f^gZBt%AncruC7bluh${~0< z1XH&k=$x#fa*oz{pihb2tn+t8%N|u=y5N}X;rnpjcpH)q$zMJn-6ZoH z+!o{jSQB{9`lFadEYIitW*cJhofiiVwE!(7ZrWOFOSB5OkhHd$+ldUzOFqqkEzF1# zCI9nl_RnWY4#@K`imZnCtTj7z3J)D>^3JauG4(tL)~qtuxSDr04Eg=5#PA0x^$Qf{ z->|ocjXM8a(5QmYQCAJmfJ;}8ivX}5llppcfd3)$`kU%;?$>^eGwN2hrOm)U`y1rWti87T(_m_u)}_XEm_rH_S_~Ro-L=VP+*d=AS4MJJ4?q5kRelyM|1|!6!x7 zGjqub+L(6UG@B9o_2Yf?AG0HFk=_v3ESGEY8$qRJ{}s%S#NOQCVn4TBh-_3k+eI?P zfT!R;y-q5)=+TS-ob*+NP(57bfuGv-45^m#+jqt|2H%3QH3o9;HFN=5J61jEvx|3# zCl_&Sasd1Neq(O~&eysD^jnf(@xWv*wq|hhHvmx)L36*Q+w-Sh<8hU{=nq_;WAz+6 z5}pD8A`6c7Z_AU3?A1*9W28^R4+!`PnsfoJyg=D`JfORIJ_jVPe=#G_KHN5S z))2AX+Wb1t%03?EDexP%W>mRVTL1ChS;_u$i>)-D_hJ4Uy*XIa#8ghrfzDY#U~XIm zJc1{N+4UGm^=4y<1@I?#5gYLxCP}a~@X6R@z7M5)^V1;h9doOj%yI6&VV;>^Vua2n zNxu@eJqvJ@hQJQS2pwpBlLGkG5JhebQ01%+Y&bbi2|=Xn1u+9$nqpuQEE_y@AWVDL z#NPabz(Fq&pW8}Obl<9HyX_NR$8}6D{U&nk_~G>&fuvWD}2y9d2@K# zulwdQ*iVMZGorV%p_uI+gg|SUlVf~JsD;qp=2AE5Eh~nXGxDp3C$ByZ0b%DGfH~io zCX9#XC~H)=1zWXu_P0de0ftwiMnH(g?0}(z{Bf+_f8PIgcC0R?A=j#6?3H8D_()6@ z*b|UlIUr{P5jm4!#m^3p=+F5-u7o%atBo(6>Df-IqlsJf4o!Cg6oMJwB8-mG# z+kZ9PW_pOGpTSqFw*WLG4k*FB$bWWT;J346I5h{@xea4`OvZtFm8epc3B>*T_wLkp zr&0hDtHl&*fNO`NJT)Zue;PKKfocpQi)gP?*T;<856|gjL&hNXj&B z$5w1Wka-+HRwlt(fSn8K+iE(2nf#VK3}eIp^Sg8U&tgvj*7%d=Wyy9 z(GGj}c$;+AoW&Tt-^X+Am8SVed5`8c0#7F6?=*p7D$7LyNz`iQ;z|a>8>N%NE7e!c zUg($!SFD_Vgnj@fnWyc7WVqN4$P{BGpM%GPt^|OE0T}8A$c%kD`C)7bh+{-QQeb+l zgc425=PgGa%%rzY8KTL6i_lO%#W4ns@9sdicGg*l4*)4aK#C9_2mo3g1yGDiER65+ zIm^W~{_|VEwZ#wlmTmFAi}zF37IS3QZB)8x9^U4ji%aiBt>%o0`=xm@%UuA|;PXA6 za5@x}Vb!7ab9Ez7%*gDBv;ZHQ3!1I7I-wER40hZ%p^7Bf%(Jj*h`{$W?@{uUl%&u2 zwRpPq(Up^5UloJ2JY>5aN`XZs1C(y%5`!nU?qFQWg8-L3$%jYW`CE*SRkAgiU<7sFbALM_2 z&agDF!Hoko1SGKX949GbI0ry-DT9NZDsvvzKC2J^!^vh4CY5rA z3)jRncCZ|5W?P1G04^`jrx0CWc~)l9v|HfWEdztxey&Y_7Qh_cB&+n$$NbNFR2B-{{8XEu`7)fAkjU2Ztl``Dk$1|l9+eG`g4JX4F9ap(gf9T*0YsT2& ze=4Q#rU99|e*rrU@j-rZ)h^QNORj|T*!&TjlV_e~iU49azdy$N+Uj74INicAgghP! z85Omkmc~%J7o5KV^3fM`pmRAufPpkUEw}$uyU{@=p7B;-bpmMJhU+2+X&K?lbVGIK zLW=M9bBMl@JS$=7=WjANbABU*JS_q-3fqbK?reJ$R6Rw#rW=R+U4eGWQJ%ikxK z;W(~kS6$}dr`!0#{>fjgI;hG6q?s&nrE)DJ*O)G7L0pSO-zCzpzaXBjK*A99C6L?q zBB;4UxdF5|AjZWB&1#XswMa|J7hGcPonx3lq`63HqTNqm7kEy~Qiahivo3Rj8&@m?4_uOs>|jKi_}f+iQ+WiGt2A zu=ow01SBqI87kWn_BMsr!pfvI_VQ%TyG}~Vtioj=odx(2+g~?Wfs4NI)=LwLc-#5= z0CEKJIq-$CviN%ukkT0Q>8oRcoX^5u5PKN+;(&m{AsQ*n`p-N!hgKpLAU4=a4aBg++x$4&{n4|mDD|-3EM{fxvGaAx+_3*qd?bt$(rxT~TWOV(MoIxJtJZ=S zSP~%olC|VwVFh3vg${M{Hl93RSDMUT_Xpi>TsDlxG_8_0Sq^M^obPf8P7{3aN}c9NR3YTjPhk{Q%>&n3}poDgi#-Y29Zr`s-c$1P?Ec?Jwrh%jUlQuSiIvCv&FwqF) zB@>kb!_=Fy^C|*}?)h!7FEoZ*f77?-Kl(rq>r6iLo%4XL3YeV!-gZuqHm0cG#6u~c z$(Kc&hxPUWw#(!B#DT(%=J}H-b-EX07;>AO^cMA*_QH_5_x;rTp8(pP{1D4lE>I*) zg2PQZ_%M%cGp9~SOt8r^bIS1BWcZtLZ-EXNqfQA?UH5h^DI@Fr+?~-<;HsD{;+{Z9~QYX=*dTai$JDS-hVgCu*~x zY-4*iGs?3N_hZP@b8)owi;eKbJ@oBytq=%@>3fA5^AL72JpZ=Jik%EE>6510Tn}0B z=Ck8s{^c3@JJLx;LF#nUFN3ZuB!&a@O)s*zC-Or*1(TixxvMv%ms5Cy4eTGYT~vr@ znm#1+l-U38E4wc?{1^BAdF@p0fb$*cLq>vz{qS=jvBY9xAs)g>F^4R`majJ|WU|%t zI%slOmf*&{X}79*gO6SYVmOb=s2hlF6O(ML648zT;@3c6&&giKw46Ck1V(UlXA@Rf z)PevhK%0tNo}z0@-&zd#dsFLMi_Ww?i^Ph(KoyYeMB<&5(Fs>8B3#9>^DK>jPpg3_ z{S60vCjwi=Z=L$Hv8dhdV~CTYo=JVnKd$bOOuuZm!(`Mx2mpc2@LsXv zBHPS+)-BY7tcdH>Nys z2_XKPCOLQ@h3CI-_4C>Qhj=rwdGTME{#c(i0wfLVfq{F+wN#7{wV|uWd~-#rZFE;( zU(5RAOW!iXBsn8O1K4n^E6goyZb|ulrK4kaS%$pc3LCcZM#uGgv+PDahLm(|7%sB@ z*-lGs%_%CHgJ*!q<%yX!6I>#fM#cCjkYE0|r_ig0*=B(^`dHFHDvA^o!dRoi8U4o% zrj}2oVW=m1M5u8B{*@X2K$}6wof<7rp&9+og&?JIM$O$7zFWHauV@tA95dVmYGk*e z3Qn24)D^H8puh&HNkO7#-NneVqT>;+d|mg3VESKFSfhXOW>@V$zn8-!yYxVuux}B^ z;k~zwyd;j;3vMtiPuc|WT8TY4zz#8>^6d^XoyV=$fN4DlblNhC&O|H%U8x*Y^E-}h_J(HIgmkQeCHZvA);^mHuOth_91M?NG<0;eFEu>uU zS*9IvfE>^96V#PVi~#huSTzn<=R)QDmjlC6r$QsZ^}s>H{l`F_2Ff~+%J9>P{+gwH zX6M;m0a=RG-Gz^8*=(Mit8ImLoE^7ie=9qVJSdipoUy&<&t8?nz>wBje#MjL5J1C`Ea0rmFcZ|Koh*wYHRz%B`!O)=ch1ipm8%fE82pw5u^r%6eafdWHT`BkrpG10@%CYKdH)C`!XLTiidDivKmAW#8?>Lg@jL$+f zCj*1(x56dHF`z{|4l1Nfgl_5j9eX9M75=KIvz|jG^y&eD2Dm#UDN`h~d0lNqw>b0b z=z)HV3Tzk2$HnkuK1{;phsbYowcIF3F_B#ck!RBS8 zE48f_ge>syFe*8___YfB{CCnl>S>057T2gnB9;SaqW&UV8(JpCa+?Qg7{La#HELirpS=8iX^+~<(A>olq>@NRYzIj-*i@-k zV)69gGt;Rl14kFui3gZwBo{09LTIdIY`J4XAq$h~E3>lg(;`y~>9ZH7+~Q#i5oUwF ze}O$zWHHza5g{=cC=`dU^awkzh*?+;b@3HiZJ60e@kw}=ZcvAu+(rU)zEq#znUfE1 z@A%x~+ltL&zZ&d;c=s#~F2IKNA6Wmg5T}VAzk%{jE+CwMJh_>hY59s90;c z=KU^c%2%CMB-Z0*CwcVwomw5jpt*TL^O8(RG8(t!gM8ed?V(o2vOAq{doXbK3+UzJ zU7sj#fqJzj+S|he#N1u!fqk?QkCXDAn3(~We~nB(@R1~E_iyw2)}i;)?$7Vx>sK^3 zaIR(V_&9nsYmdbGC)>}j;i1NNGx$2uFfU)Xcve3v0;f;~nuHpBVLAflJ!C)_0WQ;< zLaLC9l5^7dm5%3j3@#cx)R$f?yR586JI?w_zqMh)7iOEWO|-6>fMtGx1{K-@v> zs;R)tnQBZ{tZWa+IT8xgL2Y(Mu=M#DMD+}GAGkn%eS$|t-*xhn0T4wRES=`qRvJ2{ zULKgv$H$cdqBDc}r=XL*E`0z4^D?V?Z3a*&JLLrd%Q5O9{QBCfMG#RilDwdLucM#+ zCs*$BYX*6P!-AMY%{K7$aP(b9h#g}fkC{B{S?%jBb;Y}svYA!S5w}Tg)LwL5=^i%! z3lPS=p^IkZ*FZa99N28jU@rvkba~VM^;#is#@Ic;z2NHy)%THFcND??}rnvef)_t2#Q{%PzN>WKyV8BDw za*VW#0sQE21D()A1a_HvBSU0NZ^~&tUloy<5EC~UjgR9VD6VdWWU+Akb~kO^d~x{y zwfE#}1;6$BIww1|Kl zazqkAAOr}JT0j9oA`me^RE`kB6$pfzeZN5K_MhE9_IY;q`R)G0!}sOAX1$4YLe%sCGo2~kylgS(6k18!+NHdeW_QSIOisjjN1?)?Z5?)P49$yrhZRr)9irG69hIrHF)9$gXCm6yXZyf1GP?~WE-kqV6Vzk zQD#mBt;xN7iFceP`tg<496K%P;r%}W!)Gk~hn|u4KTosaQ}JP+q0~_PEW!?;n?iYd*?&*PGQS71h`bV}Cw17iW(^^W&Om_yiI(-yH26_>1G-zggz3?Q0C4{C z(IMj0-Gu^`g)V`TM?*NP0nN_7y#uWQ!+PrFO1tl_up<%o+kpyvB`PKk%m(a-#A+*a&}93p$N=fya5A^VPtYAa6k}SV+49@~gc8f5Z@IK`{Lkpq+vkz9df2+;cM-3ajB$gKZCUi%1+k#fs${qW0n?YunwJM4yEcw>X4U~P& z>=~TRMBuy(Nv7ll$=s5Rvt9Egh{Gy}b}iVRl`bJr7m>v8{sqlbCXjARX;Rb}0%bwZ zJ-e>JbZ$w*z1N`TzT!kH$slm4Q9#2-_0g?UEbj)qjMCUMME`#FkM)=}3HH^9qF0tf z1}5PCYQR7NV52WK7{%vk*qHDf&J1LZA`|I}bV2=go?)zVl5&hOQ`-Gp5D|SZmcP5VEGM#?w}&@nC?bnJB$J(90VGG~h_#hfh>CUtl8pU;sl}r( z8^Ik|Fxa!V527SR!$VRUy{EDM63t)SB$Z|FYc6)BHR|r zAfXy+<9P8nyJSrt0E-O3$}?PSP`jNOzQ0PV)@k|v5fq&-MVp&}*BMn1zwOP73tvbu&aanPL&AA)21gOpX)SLJ9^99|yko$^sgoE2=5aOhIs`j@m3p9fYwe0`SKZgffox@~GmH>s_+zJsmS5`lJOHK=lAEREO>0uv@?C*B#>4*O@%G-0v#A*;T%z2!Ni%7Oz-=IUG|RNNMHgW(te= z%IxrdaB+R3#yrI~enbXp)htvCD+;5!f-3hSAWAKmCZeh`Pzx+e54;5zbL!YJ0;_?R zR5vKH-I{en1&iY(oj}DT9b!8K0Cg*vN_8-+dVEq#TE-f6tC}g8qqij0`}kL7(+0`k zvbP1MIG*%5UXqRCQ80aFNFGH_jKVRBtq>ErF~7ZS%q6}8Aj0%lr2^g=fy+NF zf0g|m&^jF9`xii&QLE+HPKLYirBJWM{}l%8wsKiq$^{ z1dTKX!K5(m%@zCG8l@X!+*HrUm^=D(y5Ifj_ zaiMG3R~|lkr@$}&%?|~9;4Zi#tVzRNzLKtjCT<1^8W3*pZU?@VR!cP{ZlmH8Wl=dO ze0xaPo14pl?0$1{79@XQBFH(OM0Yq=A#!dju%V6TsuJ@?B(ce&FjH+gffz$Y6&}4} zT?u&o_2gK@^89;qtw(3To%NqGei)p^((eX9=;#JObGcNMF*EpvEwQPzO3VxXh50&; z>TOE@!yK~DVO&ttg8 zmUFp6)N=k;5`&v~woYA)|_Y|6?%*lx>>-neBTtV&ewDHZifs05Xx8J_)&R$XIWod4AYF`+z9ijIjpd6hD{$ z={B5MM7**K>I18xv`C!beT3oSw23O7`%XT3Oe~~|xULXs>z_u2g<L?z0^r zZCA{j5v|bsL5NECLp%ggsq_3{Vm!D`)2P~Jp_qlQND4`NvMZ^yGxd&JdY}#;D)nW)z6AU?ErHz)RxyYmq|?YrnR-52(ud zG}6;xl{PlY7pbqcYK2ql6#WnXC)5Kha-a!2estOYOFb-|DRH+L|7}PvoWjAG7sL>f z_?}#YI|i7laf5+}i*du9Pt>tFWf18%!sQ4YH$QSuQMDF$S$kUNt=%n+8(F$P5j#lL z%Zp=c4M}n>2ezF3BtOF(h_jH4^HNhNdf; z;Wuv4-s?YE?O07-gCq9zB1p-#jqIgIIokpL^<9j8td7-u9PD9&P1qgTjE5vqO<>(K zz=XEEysdztjbrWT!A4_N4@`p?nqo6%17*RtE3;ntg;?=yZB5&lLUz46VtU!5Q z@+mzBSKV1b%SNGPp%o)%ALFS%sj+EK5mi%g?s^PRV) zr$eoFEOG_bw{%b*t1})(56=D8`Eb-1ICSs5;05i`0gYgyyGswRRCye3gsAl~@RDGX*^eYwe)e>jOZXYemz*ybBxg<^EBSTk6@4#MW+K z5v$rjeJbheQ{=w1;Z6x5C2ZjZMv|DufaJVmcrYYMhoIda?nk)gElH#!tb0)_dIJ=Z z1{64R#L=bBXi%_xp9eeNJQ%+}#`k_yOi}3g$aOiMdUbI4M z|Ma}sf=>oS{i;{B@Jh9&#}87UH_Uoi(QiYyexxu)Hfx$(cVs#;9(K8O<0~j+>PdTL z^i8?}qp>%~vSkzcQ3aORRa!DMF&OgbSE6scvb_~=L`%Z|maSIlP?gP}Qf1#vbN#?{ z>ZJV2760nk>}@Iy1LocsoE^^ovNCPP%SJN)1z12|Y^jNohskp){u_)i>3mXvDXXD^ zYH3!rPxYv|b=5xNC-KPxGa@1~i(?c~Cf+X*2rInhDS50xKUDedaFS&HeFejUo;aGR zfDNslS+te#Ed^uoPPv}X1#$*A9o0H*y%)WXWw{RM@unL1ikvo=?wAKu_RO!3{iyZ9 zhnsBn^&;Mcm!0>lwTWQdS9d(w>(VjkF?X5tfx~MfaH&g=d>7Gv)PTz6))Sa9q(~wd zIbwcSGj{pQ^>Xtj(wdf`)G;?g2jh+Jxt8IF_Y zYG@l#HnBx7BHy&4cwS|B8}I_?88NIxi^!8E3CrF9DH%mzhc4dVi_aN+d^z*UzKeLR zM0&qNGPNLs7R)M3&&tsXSA%1KAzdO4K&KGTPK6c*42`}1#*Bf6)8;$xiD!T7hXHF$Wr??WpCThPIU{RY9_+TT?k^FX;~(1Si% zj(@zg6@5D}E%=vGzXBndq#M3ufUk%VC{?Bnl51m&9qXzI#79{&6CjJc*z8t;a|C0s zU)7RJiomcnD-#0edu4~`Dj|9gE7BJ2gixQK>B2sPV5jzXoq2Hc_3KC070ta!5gwD3 zYyTmN`)0yjQf9dg{i5JiB0ZB?uNW+?@PoUUR-G|QHcGCINKhULGp#5Xlkr_LZ!D7l z2$5mXo{!nsYJCJPHc1X(H~S^~gZHr0D>~l4_AU(p`nL+Qw}+HKSUCtZfIMnD7Nx#^aK|ka%{6k`&)e|V;Ckp1 zdOgzdT5k^Sh@wwIP_ibeSYVXp7uO^nkd~RSThp_W6P$Jn61|!2+CfbICFYAv*^kYm z%pAG^GOy(U9GMEc)9?q+&V9n_r;>pM`Zl0*mr8eKdCyI|pLbPVd)^wP8<7lp%C3*iRdztpb`wmPAGWl=Nu($9CQoi+I}`zZHgp8bf)vP^NG*FMmf}qX>P})t zVQO7tP(HpQO?~c(*e_?VNHPhUz;r>BjB)HKdWN6-v39>l2rb>>!U+vgAkH^*Y32)d z8ZSH@KH>hOCID{H5ww4*12FAz`vu(kOVoQ+!jCVR*+!ho6ack z1>ToV*|yw#HR`H9X7XUPprL`ImOWSTNu<&DrQ7lhnrtM8CK@H}!s(P-J@Z57SbbqO zQ+B^Fqor#Gt?Op>3_|mJn{o;-gNAzhwrvYr7b+Y9DV*4Ho znDS|LcG+ize}pIk7N(eZttCcHZU&&uC%*l%L{X>#{+&jYZgYn>Ls1dKrk+P%AyXj| zev=FawmD?}KRB>jJCETMv9ysVz8E$${=WX6j*qudgxWk4Kt;#qtK~&c*bR^M)+as%KMa|?#duNz7!U^f9Ez>q~I6z_RkRS3S$-#*@VZB^jVhgabDM?^RC7bal@0s zmUJICaNZ^W;O80C+7xlFfs#18f|u|x%0UC*UEq8M@CirKLHAVk9UT&DA?lE$!mA_6 zEmfYM^QGMvhbV1_eBntniVtB-%;|H(QEsMOZnm+h%pV=|a#ZEcn$p4Qp||`{Ot-E}NFD~CP1~^XsZz0jm-qCY!%|#IufSg1juEv#9FusOdo~^6@!Nd`SzWw&1}i&Vz6iHj z;p^>2dC_yUPReZ42_ z_N~*^DP!&BzGO`!6u_L`?>gal8)B5a0zu0K~K zuk%}%xz7E7wB2{P>hjdn-@;uDIcbP+S z$(HFRqxRi;vaF^eg4YMcWd+#NkTl)aiE&#csm8R|ZZMKUuLkGO>34_g zZ zd$KB{Gm91Pd6XAQrniJlTMfLKG)?xHUI%+RJF53`^43tzQo$Pm@`UXhn*RZ5^Nv&) z?yA!^^{(c}_6%}Y{qWdp;f4&#%_DSe++8q!2lrVZdu%bs?9-rhXO2&Sm=->V9ax%{ z(Mmgfod~~+8M4W0&jVxkoP|#Yf8oqEs5%t`12VOLEY~pJLTQRnyX1NcwJsmGCP~qk zwxdgLvS60x^M@11k#W1XZNt5FPx3vTYKoYZDfTZ^t!RPks#$42jzviiGgNa6WO_0g zt8`k-DpSp^%j4zscJj7XVs*ktO$w(KVUhmEUTcSxq)+~L$cTSRwaPh3uhKQIuY~R@ Sho->-VJ3z@ouC_>z4C8njRdg( literal 0 HcmV?d00001 diff --git a/src/modules/localisation/schematic_pictures/vlx_x_y_determination.png b/src/modules/localisation/schematic_pictures/vlx_x_y_determination.png new file mode 100755 index 0000000000000000000000000000000000000000..8482d270be7d68dffe204b9e7a575c3abe722c0a GIT binary patch literal 41749 zcmbrmWkAz!^fo>NDFIPJ8bL_`B?Tm`l|fa5tZTrV%Z^GqN+3{89OdzI65u`g3k@?U2t?g+_UA&k zN2whMlwbZ>Majt5>Nk-*-H6b-b<~~Sx-T8Dn4Q#|3-#D+J<846Sdu>tIMp3&yU)|* zH2&fKm)kt|UxHNV^!1D`cbd_?xT1>jV~6|YI648ZESdV3RTO!YmXE5p$_W)#t$&Su zjJ>EJ6aMWvSUk*UqI|u4eZ8Dews?pj2webH1>*e12RsJ|6slJ+eevuc$}u*SXRl-u zkwRy$oa6uZ50v|SeADxFx_l`ygjTwFL~g`(_R%)pNAjgEDD*h*Y)noG!IOZN7Wb2+ zfY-R`>2fU*(DKbr24L*6`y?g>i!8-|QX9R`KAwO6aW5Hg#T}FWN&yJdsUKQ*N#T;> z1wB__wed|+xt&tdlcE)P7xexDtp%t@?0k`<1#nhKhQ#wQeGo>nkBhg_ihZ)#cT>KzS4hHXE4!Uei(|CTd0VkT7EBxPs67g0dziLY0k{^ z@%P|Oa(RD$W9%hGYW=e(0D9%_7Zfxs*x=$AtY=rq$e|}wr$dCD>eFpUoZs7P2GRHLJc>g6 z4m7Bn3xGUC;`1{ZYQDAQO(*IVn%s4>At#cCxO-fEh>P(tydh>9#PeIr+bjx^2oXWNtpf$W^ngl!O6S`u%Rixxwq z+xtW`dSVB%TAWK*wETS+n_W#wXAq{}EGXyLxOF-zM+yRsr>Bx|UacQ@NgG{bRt>d* zlPsW6#_0Zuds2+=kXvGEZWlm3O!FH=b%p#NTz>~nNWp+dl1W9nQ4`Xi){uhD2j(o) zpa~u)`m^(a!xul?)W5r9t6KTZnX@)HsD;mBsCb6#0uiK^Tdi7qdLhy+wn$XUbO^r_ zdw3D`;>YTN=Fpc!Khm?k3BSsE)7p8ggUIPK?Po*+b>g3Y&UDRA_3tfhC#!d^?DL`~2bs21(l8v!fqe)r86hHOfa(vZurR<5u z_+w|kV^&wYNp^A8+t-J36x(< zoOB{17|gevs)QWGP1Su~Y5#ukqIeYV^5t*|*l4}W>9}--=;cI zQv1EsHBzoL^L7~{7KIfHvlTan%Hq^Ey>&@KxjqCOjC)bm8hOSYe?Cd@5B`bE%t%O^iF}CrQoHjqtjk?RM^{%yO9?1fuQNv zu+LyN1f>Aw+XA+wlHq<>6?SO$@w40ovQjSjNzqPZj~^DZm;Px=PNM3hnO>H9^q!Pd zW{Ye*^SxG^O>eLI&;Gdz3v>8&&%N5KApI|c1rLISz7~Y%R^<87*V2P;6m~T;)eep{ z&|klS!KihMu}R%N*=!OkGg=Mf#E2dezu024$9P5LxTqG)m$Ir>CbOtPeg*KEwMclZ zCZ_^*7du49=;bl^?K^o>f?xU26xZCT;%e0I*H5$ZV3I@)c!+L)Z$8vlzXeQ~&HQWL zzTi_4;Nl53X{cY{k#WJ5z;D6vgaZ;O z+?_q0l|TG$YA`$Rx%>{(e_NLTSbmgN_lSK&`@UgYdt=OCg+xCuTx-^Q*c%&|4W$J6 z8GnP%kWBVItr=6H@77j(dX<%Lr+}j~w@#IB&-x}c(x#$}J__GGA{Vf>&&+0}9+N&| zA@tGe#1F-6`2>rp?Ou7oOn1ZrZ(n)C&)PpZfSAyUO&ZZjM0?e8^Mm|4c=F!S5~PuV z>90TXk9K@`hcWIkanTVN(dBX#xqFyz3oLLqE(6l^xRzhAH=nx={GK26b0h@ZNw)FG zquN_nXK;gL2FX;L2N#6nSnuzD2oL}*r_T(;>L}x*vei3t11haAk{AbeN;0u|Puc6h z-kM4={Z!?1D;q!)2Ky(r_?x~EwtMCy+ToiTzJY9CRUIg75wc*MZ2PD3d$kC zn+)S}Uxe+%Kg#sQB?79mR%j}<-T5VTbH+pdrGnR)w8SGpt{dct=Gvy%C>V{Yziz<21UT^890Mx9kWBQ^onC>lWtJJ?N(bj>}auU*Yci~)@K&9wB+`>w%cG)jhyiJZmZD<9}F?}1U5lY=Q7XYI_a2kQU2T)-qX)gNFnhroaq z7KYL3aJX!4(6;ffmy-k5K67%sz?hv3iXT>wYEB~IP)?e* zhuMRAD2RX3gDmI; zZ+C5}^}n|ip#A}p8s*FTL+GH5j)sUM^V96*0I4gW4`6+knu#xD-d7?&h`n*3%`8@` zp%RtLivRuTlYhLuP#ZW@+TA^7RRhsD{yo2cb=9LLy}t05-os37b;t!!_-#v?F@o;o zmuq%1s*iF@qNlIK0lTXb89SLdV&MU^OVgzhOYEDoIL&+U^O1buYS`}=6`e8?#1^(F1T~?qXsb_dhMl1w?#UL# zOyg|-BOp7wy^vYfu0VqVYcJ?!c{6P|6PmzaI@@BQGmOQ!FP;`1Z*~?%%+4+^c>w|U zn(|>=$<|9&Ov#^oBY=EFD{R#^Ky_P%Jtozhw8~S31Q+*ZyynF8Y%!N^vVo%Fr{$siegD=fsg~ z>R9(<-LZZyY2qId8~kV6q1dzCx?!ZG?o3eqV(v+V-@ca!Pguub4x@{0XbFAlWqqLM_g0Lt(|coj&eF$tAZUkS z;)JN$b~OKY4Q%ISY2uL^JdsO)$>n-d#!c68u>!pAvh&9vPg{AyE+9RMcH1|m8uVWqM2&kW+&yos8eS1LwPz4 zT+zT{j6@875rQFQ`=hx!Ys87_xf`#h4ZjkZeF0E-%`DnE$zo|1K8>NADdetO7%jgu zX)fH_yD~RNUX*KcXi3xF41N;m*DJ>eQr7IRZ1bsM^1P?w9P1{Wo><&Dszqs$#IucW zSY6{QHq#YMwW+YT3Mpy-kZA*%eQ1_Jb4ff@P<-gh*&(K`Vl0{xpY7#iM37BfU!2_4 zVBbvoA;hOOy9_!ZIUDh<-9DdpgG?Q_ zO0|JdxHPqo(OFXojL{Hx^_*zwG9Aw#h?CT{puvF#0uF2d0sEFv4xAMbFL?UbVllyf zn-Hw$32rjm>H;YA>NqUPN9=HYzVNA5_U7fs?Niy@MPDr>T{}(G;jIOghYx%?Xyg>w zFipEVrO%3;ow$5&)=#(oGsc`%45x$m6xFAy3{7CRLL8f8uaLr-T->Rvg%KvEB zvn94K(bgqI&#SnUVY~nE+}bY`oN7;5bDVbXs44tb=>ovo z&APVVQh9kPp*d@8y}>eNZ_WZl_K+Aqt{w4wS@OY?uD+b&rx`RG1XNnAU0Ch?-8ZM@ zgN-9A5*ewwV&5~Ij{`)iA~Q(g8~4;4g57HI#1=U0NWEQu`{9JwpYX`}qjT53oLm*U z8ihQ7&3iw+B55|54q#X6Id;j%!6`w~wwO3o-%cu$XPBw=Lv5LXxludrL|86s2O)XnfRH`ksk#W{3o{8g~r58kQrV36Sm z)#-Op>90ae``cnL7Zq1mkKVR9s=!X3K1=C7Vb8QF;uvt;X%XX2?8OXa;>Y;QngW~B z9ve$asY(`%_;a`o9AvILl}#MfCxeZ+-#HrIo2EdJD{Dqo&0I*4l2HIj7yw2nCNedw zZ>Sa_FxEyy*xSBu)b;D5F=~0NmG95y2I=Ue#1>A~8vKih=v>8f1-+MZy zS!$($lHDfd0umsBV;Tcke)Id-@yxF{8t9qp$5%Bv9q#++zF5mL7dw@le>9nnyQ)<~ zSX7u#eyS^0@bRR(OKWJ%J`t@iiT5RPu&N|bgKFHhYgEZrfnn9{#W<$mFe%Ef#`VjG z`}M9-!b?KmK%fs4GQ37p#xSg&OnIrQcKeipBPKnQR!2_3>=j-VrvRMlg1(vILxMcc zHeUfw3)%8pNQ9_0c=^d$rs2Ccyv*9;WW+#p+pV45?61xrMI}6#xk%PySkC7AUSt^- zA7R{8o(ZqOd$w{n0)k4HlQ4*L53}9s8<`ONF=uVCZ@{5`Vb!T9RnpKEJp^`1a&xb! zr-SQT#iX{CtI3Mxk?2>;vUHkwRP*mGq#rp&BKQ&DKZKX}2EsiAYJPi3Yd#pzRo>7i zJ2x+t88rwx@16Kz>*ijT{(Q94bHX56M^ePJ1Mr#fk2dpuM#bIL657uvaKniui}0Od zYc%E)*gYvmCeyxLOVX}xT0jU<{CPcSm*J(Df5iw5sHMoH$iH1e-%6Ay%yeX&GS2Jpyd^$Jo?sq?Vt_upc!$MaJ-7U1M*T ztgN+kP2h0Q3!74p6pBoa9bIQhJgLL+B2M(piF79F(1iP^IjKqS^esRV4%AfLkt{pC zbXO)BuNgH3L-v$zY#%g4+C({x>Yfr%1tU_z#EcGd`@1s$`Dw8b>HsiU_Tp90V<7Qu zGnthyRJq3*L9d`w^E~V!nM{z*xYyHcO6o`i36gOyLl~2Uwo2=9npp}H|F<1l3 zE=_H-!r`DslB84xHd(e1%#;g??j$Rwuh}W#muw3D?9ij=r04poJU!uJ41=}M=Z$ay+l3Je^BRc} z9XOS@B8f`Y%q>oidly0FiZvwV`GN*A;AcD-_{YZ{!;4pY~8JdkJXxNHg4$}?6I4{tU6 zzU0cu2O}nS99!IJtX4KV%U%?}tVKmKp(n6~5hu~iTK4W$K2}xkRoWvY@p{HJgxFD+ z8sc_bk_ct<8@8ohTA`1~sUWmmwQw2<8&)-abjR`>c$*_bwOG2WmhKXa)I${o#X z;cBMecU2<}XwQ&o%L!2nD1c714(`I&t{;9ctxiJHkR^@KP@}^u)Udtl;jYH^#!^b` zvXjJH?``01OE-S?iV9VL!32W2x*HKDW(RFcU8uZg7&ZNiLdiis@Hw$2ahm%aXfM8f z?;~!S+sp&@&WM9c$SC9304>v!T2V2BzR^hC)$7|tOR)<|0#MmSCSO-9mKd1uBiH&z zfCSPMVpW}lE^?FQJ5eegM4>5y;wIbMW5XxjT9NjKQ1eRG3zdyLAE4-pU7wE zi*2n!j0bQ9K2?>bGWIDpEH^O{pPCNp%7jTjeCo79b({&9q4_0uOr^AtF;?m5jI+lV zLY&A6&E}KbRAgU;mQ>`RXX-UJif7inMQ7zCO~x%kwgg?G@dy#%?3ivRCH7cdL52+YWV*R8JQ2b z{3*)~%Lo;d+i-YA36q1BFc8Qo=WA80(?;1O#O#eID;dl>%v~htX6JfFSZ7<xp+vOFOZs}m1oTbth{z@SvRPUu(NLiC2>H8hB}0(bA!fNN_3nMuBU`N{+&2f> zjGjtq3GA(&D;%I6uIx7zr=~oI4jzJ^d%|NEGE+X<0os5NN^v=zt-!9fTUV%2W@lNb zxXGZZ1lNPe@!p@`{K~$N^0)jK8@Zajb#63o|Dn!&ny;m{33!$|(&N?{q;B#v4P)Ua zF$I!7BM%$hxa%DHNpxyaxE_UAU;U!#I*D_dteEPIlHlg$k%^3|EOU3S3bCqUcL*P` zpp&Sxr6b;>3Lim7Z~1wl@R?4ttY-aH((P(^NW-*CS1-gJ$przxT~=S+i+y*qlUPSwe9OR0t=o=L3fS4>sB=Yu@vGh$b# zR(p=m`A(}4&03m~A5Uvuwm&@VaysZ;l+j%K-nvziAw!`9tgM~E!@b% zyc^dpq5|mqM9*+p?=0)u7*_1qo~V7I?nn#(BURO*t?zCszT}hrgKF97g_R61JZ%g+ z-hEXy`lBIX6Lmpvk}HRG^Z@A~(#xD}Sn;k=O*UHD(;*fyXguY&&c{l8rbJ5pceol1 zNSrfyvf{&x*0a3Lhp#K4-T*|@02w55qix1g;7J-#ULmVCbQJFCzQic~Ztw6&rdKFN zt@~t3Yh{w>M)p|OkWTPTR!&>69Xc=MjJc|OS(QmX5dz3s15oGm)S|3D$pzH6QJK;H z1>zY(O3k!b#ALuLI&Uieq00QE<{n(yZ1~K>AC^9MFn9dzv<_1ocGSPcybnzi{z$p9 z{WhVE!}-BmO%mwM4?H?^H}vi`<l&JET?ae zVGhbGo^~)6=LNu0KBRe{Zhz@PSk_eG(E`}HRbk;wXWCIY@)~^ULaM#U3!P$30uBo`m3rqeuR?x* zT1q90eaM7^M$_#@`XiBbf7)_mLWN%F8Mg?%NwgmR8->{C#DN{WZg6{i0SL;QI@@p# zV(Bk$=Uqp+tZUrO5~;54RVN!og4xj+u4>q3>q$E<3`|?UKTPh27>oAJL(D!3p70GU zIvpuDz|G7It}Ni>hFXyqYg{7(58#5L!P_M}HgV{kFw;~sj}RVbu@}vE2PUoJ8}6<` zMO?b88%Aj0gqjcISE9^BMq9fIz%}Xu{~Z5pCyzV#Hk+3`j384@yq^Z(={y?UrDG`; zc=96hf=08IY&b4Etzx~sxoK4MHWZow1v?Q=AKGwVcO0@#j(wz=ZBvdyjqssx2cw#& zA!W;HRO%1X7Qx=*t(dPeewajjljkCQBCc=pc8FVpvP_In#mxOyG*o0A{uSr{MxI*f z@B<6|G>d1S&Csv&tOs=eu>C`gTVz&rTf~|@j7};%(XG}MgIh1zX!+q76FdCfgtPU& zeR>DQEyzH)%3Jx*%rbt6&*{D|L z-d`CEd)7o=^V>V~;jCZ5Zl;lNlvJ=V&O191v0~j<^~N#FuR%sgxz6*5QPg|n`Pzdf z?VK&-3NO$S(f)Fe78~v>FEOU)n~YC{4Fsh8X6cpssF>Iwi7KGf|5( z_s8EwEq-FuSllC8YYshqSvBU6LfChDKREcT{{H$F3)smko6F2^!YlS8f}69}rds73 zo)vGe!ejaC4~-X{28Is13all{fEw(}_1E?Mp5-ucQz^@FdK>5>5uS8ks zG6uyQn)C;(-4f9;rd>%WmD|J`i_6=^IXFM5|hPHqN+($)q;9Z^M*T_P zovm()s?(Zb7)$$;EUJDURS4+ka7Ywcp@d5o!b~ ziY05jRm@`5CX2(nXn~C)q`(53UB>FF-nb84qAJ+?zd*v&t7??h{Nk|;jrlJ+9rVyT zs-T`}gH<4VZudn93b)<1gO>7paHVo;iZ^?CX}C_ZzcDAChXc{PzCEmOc{>kBaPRB@yG5pfn0ke5c1O3C8*hz*Xmek;NXKzXSO(_pLvot`> z-?9K$L^LKp{kQZV)YaUkC+s26%DERIjwL^=LacdFH76iola`ia9iWI)UWwk{fN8at zV0-L05j{zCv9@p;ZVY?!Vnbz%-wq@Q+4n9gZT^l$^!r<@K>Sgo0Js#_SqP2HVE_kJ znHnn-0(Au7T)05Xt(G}HH;2N4h7+AsG_@;WRazopjOF+L)0_v;F{Nb0T+745+PmOZ zb@`1(#gpBE&)+mQsl5DU!f>d%gV*0qAL-zuuDr?SBFh?|BxjV6Ku zay)1mkL0Bi;U=wbtGJdn4D=X$S^t}Nd791_UwX|~XK_naWVUni9`~0utE%aA)Tf2j zlA`Dhhj+1wlsay8-B5B)q=&Nnvxafv3%TFc~9>}R5DY-unmhouWqgn#VR^R z%`EpwqF00^^}$rD-xTeBqv}vil67xgwxo%_1+2~vD>3(Q-&aokTE-*hoiAG7ZQ{V^ zE~B@bDIU?@V=vQ<6u@}W&Gzk50X-V3>KVIZ!Nl7sOPI;@4EQXNtrpSi-2jSnXGXz5 zF2~S1YK3~e^Ha!ON zK_j_SHdEL7%TrJ!S_>yVhuWe!B#l)>g2D-WQ|@nT;JGyERBeR;gk&H6Tb5--cu zyn9s*#k|UaF-S&6K%o>$|4~swQb-?@S7?kX}^> z*qwRgVVKfD|PF6+Vx3hAC>Ic@tlxP>0N(O@_M#jzI+V%2b zW4%M`d&%bOK0Z~t_&OQ627pu!}euty}p=KQ@eE2`+@J)YcC-H2MC}JwV^e~ zor}d6xE^WY=6y_)4TMS^ZrDJZ+6&i95rZQVol`0ylYR|I9k@Oea}&0(ib)&D=*_f) zLWy`!FL6_Vok~69YB>nby)T&c)z1-sU@8|f7BO>ar-zAUd3a<@)M1jxFONuvDsyjC z8EHK^sq(}>n$Lbjtvb&_L`uiaie*dotg~5vD!F(e4@W8!{$;#4x^uGr9Kt|A`@wC~ z#lnGAgd5FAy7{vd4E(HhS3HF2nY%CrLcikvPX85RmA`v2+**z{SG{@b&%_ zFEz^Mme{2l1A0aGvslUgu$ouza68)XjIM6-Ghc8-4%nXfbSaNfxB-k2r2GY-hSDwZ zv6yu@v#L}@d1hM0{Q}g<(#A!U=lTFiiPOUIv<40;nmzY4B=q=TMrl)m*XQr5Gn15K z9bST0*}yg1`4#|rrPF0rG4K9KWuUjI`=3xeO-y28*ZVS!&Pzg~nNb!Q`b{1DNC~-t zxJ{}9fXzQX0+LmT@Q0-{_=tDTo0u|)j2VBt{I^fydZvjJpq+s9jLr&In=x%`YsV~Vu)426lNQj?0kmkrNgPUCXxS4! z;Ud~*qz806XaTyEy|T07F$b*mP0bXqPJ9LpH9^l!aoFzYpiK7q1a6GmT18`n{hy~R zj)-+ma!}t%_VRxS^olj4kl&D7v<}y&Vk?@!y1u2s*EMX2IVKZ;9Hw%p0=9#1k}F!1 zy)bhubrmT3h6xNj=6rl%kEV;O&y`^xn$5 zu_;lYc_>rHQTpO0y^5-F=N_p-c$%qY*YH;ByQu)H@TZ$@_kfx|Ip}i{a3o2x9?ETg z`on)|Xp&v6s@|heRpDFP`2IR;X?t{qgUEm8URBz-d?ZxFYJk1uY*upcjc}ON8izOWv#r9hJZ(lsdFEGTOMQ6}xVdo@ZksbHh>jnV< zJwV&yU)xC3ERdY`FFX(4+`60A*0x~<1+g;S)V;R-itY8?v5sS3DVDv>jkV9O`nW%3 z8f84X?5r#k#2*u2{`B0HN-xZRN%n&_{I*b4(BF&u%f;&h; zrDA>lkflrx47SU^b)^isG!K+x1|TekBG*(La&NA&W7M&q*H>VLlU=g@Z~?JC_=e0Q z&i^w>&-K~W_kK$gv{piGtWWqLu*;22XG(j|=u0X}RfPv5Se>`(gIA)hf78v%=D;0^ zEG3z;nP{&n$LZuyR!le9PB_Ak_{)n@$uP+_0RAh}HR&e|JMEvgwYVsEKhU71@txzD z3x~3fh}uw({h}%OWR?PCiXm3+L?6>Ii1^gQ5U#-0vMxk-A3(v44x1gWL(_BRNChOF zY2JPqtdZzc+V7#d9FxHf?|oi6aP*PE01k^pxKAF>Z*`?nZQpk_rr4Lfd4ACGq)q&} zgO^5i)7)g6hVe}Fy=VF#&}x&UIE0qqcA#aek%MmL8>-a-ieDIMA4ewji)v}b)Nhdm zJWjhEbU~XnTcFDcZI0^!&OGp593E^qW>W$6L~la~kRD^G$D58bp%LWgz=DvFEf^i4 z&7SJYLRO9?LIfW!0G)Uu^DwWK;%N$P;VgNfoy}&h{k@V!)C7gjtA}~ z(+gI>;kR5<7#JcAI{3q9?*MrP^BK5wIa|J3LV_Q8Vo;$FWx z?^_ccEWM!6R=j5%>**-JNv1i=ZhV@oc2Wc$*;jMJOy=A0k1zBg5Tf4F7gAOKeFlEP zC`Jl7xg?$6k>6(JuzgfCu$q(X(KB)^9nwKkaS@iD`$0@L_Ll@%XBXY5Hg=lfd71J5 z!7ENhSn?Ar^%_}~n>&0k00^NFSkCWHs$qgayU<`in`}UmVYq+nQ$CRp@yEeqr9Szq z^2eGHyR0Y+$<4pxmNJ*(9&+vc_Asqf3BtsgSx*nwE4CBn$(oLOPXKyKYAjHe zp+bX)5`)-QDzxYWCaI$(rn)b2Dh#kkHzXhWX;pMpYiJ>A=VqUs^?rgB!+m|!iWVW{ z4k~vb02#;rViu{-_SWZgQoO-t)$o?R-et|sN%(Ks{k~!H{=&|q zrRB*h^EXuYx23u@PG45o9W4}WN|;re`w$ju%6-+nPZv+8S`&XsIQ?SA)(ikMzt^_H z0W-5k6LldVqoEB%bFrbgw$g;tV*-2g2Cbhq%B>JeHiOY$Etl${Tpekhk*`#tM347RE0e=pQr zbh>Kb{M|eHINzNS+XTzOifuiKe{ML{i;@lI_Wa#IA5sDn4Q>e#j2b(r7M6~iLdT^% z?RSv@Iz5h0dQUbbNQZl;Pm6f?U@F=r(jTSlE+=Hfc9>|IF$Ng@w+*Tqi@UA(mpWW6 zVe>E|0rh3ax8gd)8GYLjQ_|KpU>$mwA&mWUbr0L{;E*ccpS@nl-kn-YNaqnp96sE6RoP)w$*Jt zVy-7H@^wy0fw%^r!tp!9O8=|YPC{i8=va2*lAz<7n$)AV-;(<#4Anlis=FooK1hZM zm<=_)0RD>9WH=IWpr~CU15(Q0vOb&}N5sc_UGS$b&peMRTmzMSPP<7jvg>dn?X=>7 z>;5Iz5EhLcU~Ncdos9bA!_Bo~9Ts3{k?gdr?2hB)2O-^*)+B%KDJa>;mY~CLzV33> z`RuSGO&ehM-!{<}b2k8lW2UA5_AmG?dE($@REcv4gdeRYOzaLZ3T_$eS!06Eo10~M z|BmCpsQVn+30ABD{)tz?)d0)@>=0K6c7OHZ*Q2zoa3b~}g97~+&emGv{neC__WQ<5 zIiZWP9}d??r*_7UWTo-X-HPgqrJJ5V$}cR2Ll+N7OLip!ZIgG|Ta_iZWPUBI|GMvM zkV=ie(cEGc$1ce?EcxwZQC#WsF&L0}P7nU6@F1i9=;id)$m*)gj6Zp?lqOw17UK4s zV90%h^Aj(e$w7^_#FUj_cW_Wz{g$jbdtmTKD2qgUP=0m&n!LG#$ow70Axqens;}IH zcgyjwyoJfy%qIiSgS|IpH?5$GByO>YC#ug>u7+p*?+KTtR;{jwBLJ!$5Eii+U*5CI z4kP?|n9Gi29LvN6&Np!_o_Mcx-|x%{J|%u5xX!exUx_gnKrgTfHjZzl@-_J%o3a(n z(eEld=5-%T9DRV=y4g!)ud>d6*EvK@u|BgG`gI}nVq|OozuE~XUUJ^HeAB7V`!A=^ z=d)6n1$CZ+I2)(VY;2X0T3yB%M<6Go*HBkem~9Zp6Yg#;EyPq=M$NOEgsZ55v&dlcceDut_q8Rs{+K3DHE`rG18_U+1AJadC_a+hgSLcHfamX zC!GuS|56xG5A)W`D-tqaD+IZ3Wx$(lx9*obC9q_-C^`t{rlj>VK6>9X!=tl|J3TtB z@*SvZ;8@(-p$TznWjCL6`+g@#O*oa;JT5U;Zg>Wzdq_a-^Y-?vu`GX1cow|+$12yA z4dd7Z&BY2Y{56(4QP-x}o2pD=-+e7)DhsWo?6Hjh^|LE7) za1SHeJz1GxSkt|E9JJpnbxLSAdFT#?Rio4fC7hnMT6jsrRzGTIwyG^`JTyM}W2AsR zao!9GE`_mL2nb5Dz1Rx+Nvv4|p|*d%Iy-gu;Jd-;xll$x`DT5llwn(O+Q{0>pj2p1 zx(2xwcF$mB+qKyub2wlz98F*ErsFjiAGjYFJ5KI zL%;BwZ0%<{3%w;lzq+(Dv(UPScZqLZA+_7BPuc0~PFk+&4&aS+Cz@&&0tuyAJMrVu z>hi*HFC=?02vCt%iq+ruAarOB`kgk zl_8bq{O+8DDk?ceOe9!rWm)J{VV2f4#+TxvqHo^v+${~TdB;>Fo24uc8My)>k5*<7 zTI6E8GLRBx#hS^7@{wE@LNpB|uSGTX_lgWP$C>Q0DfpT2h)nZT`{eL9O0H)7B`Q=; znYz1+YX%bsv>^!u>}1!Ctp2q?=RyssN0X;=#24gb*XL{1bKx5g1h6)7p63z~2t-n1 zZ(>^ICQEvs-Q&bP6I$|J$+jAwdWyZ^-QqQZAp^eYssLGQ`@@2-u z(=Wa#gi@T!i0dxq=9-jyit8+t5O!&%9*`r~q&-m&cMeOxZJo@#nO|n4f{FXsjE-2y z4Ge94eUZ*#t6r(?LeV=AA@YOahN3}}p|WJc5Nfzy`8@t{4j;?rkLEU9E@{rkA1(6E zFMT{FF7`)q9EWNDee_6L-lwWzRaFkk;E_z@Ka_X`r_rQCFDlF|N*4~c&^NDqHMMT0 zf$e2K@On}#JjVlUTjDIoZ)1HJjt~)tk(cAvclGf7SW*A`es`-K+2!Z4!0p3VJ%WEK z^w!V0NT}kNv`JGE|JfnQ6lg65a_{(rm>%r3S+rqhBevQPPWD%h-_Z1xRave$ZS2oR zS|!)L)XKPe(@;RST~5utd{#nVJU7_!^;LN7lRfr_Kjr_RYF=VXMj_*#&VRdBzimyT zP*U>aiwD7bwU(QSx30aVHLv5A0wvG~>{Fiign&4WV0SFA7Z26b>Ot@ki+LCtvbUk) ztRua-y4h)c{1v>Zp;|m>&g<5!lcpfYCX$*N0H;dayTs6!=ye3s+RBplJ8TgVfG4YUJJ=x~E%hz2)^7?JZ@v`N=#_!9Z(ELK^3&PmR%cbjgB<3v+N356Vl7N%XGEV>lh&Pg?<>J;U{>d0|V;3vU)dq zo=&V|0D^~^mP2Wlz z-R`&+Ndx~F@GbaPHim&m}kO@#LrTEmS%l^;C?oHeF(%@}-dqWOo&HgBy zR=|$OyhdT44R@AihWdO;#*;jBT1>CFXFnHmF2rQC!|DRZJ&@ym&o%Ow7YHgG&htUY z0dSMyVAfF|@eLOQn1U#09!RgCQE4L(`L&i8gtILR;xt7D95*ig%mOcmN2fV+}-W4+?ya!^>nlKeVc}Ux>G~TIvE0?;S5_g8ImeM$<)p|kNM(iZ-?-U zC~(>qS0|dvY9}trkMQclIQ;$I!dPmICf#H=&k+LGI#IF{#iAsSMn6{VXrkq^)y%R z)ht1RH8T*18kqMIrDTSZ&&v9}W7v5|b)c8%)dQe&Nbi4VD2BPA3|)B!z)Q(F1+&QB zUiqNqIqtZg6%L}tlW3+SJ{7Qu7$Ai**a#uPu??%hRTF9!qv?%lW8n^7p)^K$RuRCL zx%FWz4b0rcuBv2&z52YPCVQ5}HJWQ)zNTHGF25(0>Hl&mtg@4r5fE=> zA}j8gs(Ue^snjs3?#VPq1Bs;It51^WnsYaJJ@&H%p=n#Gcxy|~Txr$M(UEIB2ZwUk z=%`q_qTFzko8&#bXV=zm?P48CA1^hknQ1*~`^ABt_*&7KHZu3sVQf;A>4A5|L;T>= z1N+vhi2Ng?*_WcQ9hn*EpSMSM`x1AiHCuSBVJG=jr@)oS%jmdZx)AsE`>6>&XP3#9 zi#I-FpXjey*7ks1DQJc2fNmD|a-cQAE6fO}yU34Ws*@U2;^{e?v#4ngv11`y`ocTBgwtIa|EE6|R zPG5j}7boxNpK>>`3qW#~&8_8!aqg94s+l1V%+%0xe54ApEAsuLA9;{m)xC`Km(8Y5 zf%~DaReTsGYHS|@7i-V913x~yv?4M@{gEN(4TjCqs~rcmEQwD2X&E<1!R-NUj}Z|hhCHR`I%@a4;wQuHzR*kKm1({YMSDK z6xQ8IDynFFoOTPpGGeNJTXWNm%D+c2S3B)7zOipxWUy)I>qQ%=F6i;uZy6vIoc-d1Yg~Xv`Em~i5#;W?CR8uu4@jgFBl9Txipu_^W4kZ4=$)6R z(guwy!z#V4xNZ3zlg62YML)W0;!P#))N;lz&JVNDJ1KZ)L)I3tQgG9j1wAq5+MjrA zHyhh3YWG#t`DEb%ituxD^jD|alN?`2^wq47VDD58pIn5Z3J{3yZ8iV1;ct_*Ls#GS`=RZ zBa(L;Q(rwF?8Qz${t3U+l;fl^$!iNXHq?a85fBh%R87^Pgr`kN`^RWczcrey=|Q~1 zN)y|t?O$c*UA4#g%j>bjU}P@Wl7q)T1pUp>WXo=e9kZRTO=7=H0!%=6KEXY=5$_VR z0ik(2A*$Pc15Q=3zg&7J^eu&2Sq(~G%rtI=MrYS(TH)@Q1y6m_&0n1iaDEG8Ycra3 zmtB^}TgosO;{s(|C@;TbFb6zA-A(`BLH{5;5nJ9<_kXH&@v+DW&|uaA_%Li`sGtp8dr zwT%{cvu4xDC^xspbt zmyN85`F&`il!suF(o$+q-Bxr89OnZ)5fRr#vhlv*MI_80#WkNwD8|;*m3}^Vsuu(+ zW?_OgyMyIV-h(IxFx&q}+EHkQ$00od8OejtZh6 zy?5!-4JAm8ibw|m2_+N@A=E@_fI#v+JL37h?>lQ|t(o~|&H2N1jC()*xyyB5_q}`8 znRJwo4;QcWps^~RIZ+J(@7^qDiTQu@@h&8ts3VR(eHBXrlC~ps_gwo=5npPUiE@>t z3t$Pi%3U8&FZ~UvgJI>RUYBnXixYfwM!>BHK%tmvn0Iw`av{@tzRZ5|IAP^v|7eG>AUg(rO>I`sIrAIyN z)vX|Zvgap30+EqqU;QmrO3yd<+B6bd(p4#f$zO=x8u(pv`cJ;Y8b(ncCOD zZfDP-%8-{%0L`6Ov=9~>oISj6QqIvb%{I+>V{t+tx1cOONhb9=%Kn=quo?pL6?`I1 z_%C_+&6A1yRCZ>SmMyz>#$>5`dap}t(M$h*n`R-69)qq=s$ple)0SM@>lyNcFTY*^ zhpq<+r_W{Hze$Olv?6^~+rx|7I#$6|fOR6`>kmUSKGl5n`R(LWx} zWw`vmsbIU{lD|8N=4 zJeQz6=^XbMC|D5Q8XVj`n_EYe7BiH-2xN5m6pqYspdC=hL2;zDMc=d7%#mYNP9drH zHSdR&i2)&(5oi3ifBdZV!;F#M5_@-{X+3OO8SW5Q37Qa{*}~ZQt4y(T-MOQw$jkr= z^j`qc3qPO5YEo9jJrIqVjm!n?y+2nhIXsEK3&cti(WV7P201EN1;ohqFUYD&tLPs$ z=|K!#v76fkC2OV^F~BkGG32WV6f|-W!$XcC>F1+43BRJ!QJ?l*Ev4B<$yu^cIV8Fp z$Mu&U>j!i689AChW>1hulwIblGIsFLwe~lGHnV8PL0HV76)gj@f{ZF%OWZFCYh7#< zysho(5?Ec>h_-*BZuDM5GhKo}#$Eeb-R(g5enaNzUFfhpm|PIjf^4Ky%+hdEZEE8Z zfC}uZ%*q!rda-AB@#`rL=jNfyx*il7zH*?yl=)$qr$nx`XCGrRj0poLkQ9+v(x6nVHdYyOG z(J|IrO;m-{T$SH@Tpk1(YPHr9L?>*!7V{gX7wf|DCHs%zCw+gldVV@cJa`arplO0= zMr#P~t6VW&-z*oxZq!N8W4d!gHs%;6%0Z!;!h2?hZnbAI-YM**Q0sgcV*8uNYWbrL z53z~cAf!^OlJ-~-3Y*mIJG2Iz%y1{~I^X@zUO)#r2-q!SMk%H!`&tidl1WBjs!ki1 z^=_?ZZKhVNl9Ho`ko9U6Ky~ebO1xH31WGSh0-1Rl9qS}1!2K6a-KiPm-5sHTM0=BIx5;b;T*GvtiiE1 ze&!)O)G$685s~pktHgfI%f7BAgR!-A;=S7TYZG2ABomvA@0%)J*YyR!Pl}Bh6a%83 zW5R%rrJlOzpHtizGZ@fBpWf>7@iVby$vP$d{YDr;G6*tW=@{yoC5nvJ>FP;Vgnil> zQ+J#FYLO!B8__uGUA(hgM1IvjPRe5D&N{)Lz>s$8eFdj%HB~pgG*a`D;CU;===`qy z>zP=PWdHV@t}a|;4$nF&lS(2T5`ZbW#oZ0e0%@K$y~cLBe3)rnqh^~1_e=G%7n`N8 zYP$xmKl1Wk{#7JsF?mkh=jUtEu>~hQB+^K3mCC4)G~XTJ4Fjh z0WQajAniK=1)l;s*!qp%$VYLbUDc`qV$VJpo`!9MdUtp#9f}@P@qmiwxXXWJ)yfyav*I%S-H5s(oU3dBT1Dn71?Da z3C?5fM+!+JwQO;2JkcGu6T2p4yR5kNhR9ooYJCHa<8It8Hp9%#dNORn^+!p?ghAnv zL2cuui!}~)94zgBdU$T2q&kr(U&`|)*h;cK^nzbN^#l} z@a7)PyLzcMz9Z;n{>mEW;dDX0DF{{0d9k;@<65vHd_0Ti(fd3*4q7jRHSD!cKe8NW zn~~5SwcUd#cRv%`WA}7OTbuC1-5uy`%Z03mFjMy`TUhqcPRsP+X-fKJv_{PF^rBU? z3dVU5+t+sjK;r)a{jdP0CH>S}3|Py3g0ODW&W04ln-2gWo-e({Z`k=*QU#xzudU?Z zgtLx4?uZAPw2xW0xaRXbd%sPaPQ9(wL|r^tz38OAe#I1*loD?|$K+)$nI=@+s_}Yw zJ(V(T-Ce5T3iwaNgwi8(moZNdOfb{AL)PT6#UZVNu8gSl%JTbXVwdbk;lk|BeP^Bpw` zKc-b(t_)h5RkqiNge~;hhI#^1FG+JvbS^W_2^aIgrlZ(t!29=Y@9Ci*z-A&{lggl7 zVDFilSFfU334tWnzOqTzuSORxi(7|yw8likzku>Ro@NCqUXf0fvzqztQ60!t<=<5R z<6D*3uq3_eJ;zxvE4?zNJSXB1;-)Obk^~(A`g^!+we$qkgztut!MURyHr2Q1C!@g{ zrotIuGsEW$W&|d9nb*_!Jj~7mu*XZPU&+`ji?1pXA1{P!b~QhA!0zL-&CFY5gJ#Fp{vVu2e8nJ6{qLD+*Ymf;eyp(4}hcbm+Yf#&4dB z=VhAi9{0Kvd)X)nbbPK7@7a*wO$n z$h;RLVu3%OAK+!b)qnqj7{`X6I60pbmdftGj~p8KlF_iIW?0{kl10|taa^6b8hWaZ zQTF_2vcq1~G+1M9%+;37X{UP#%^)FHnPLJY5e)R8!sgpW+nzjEGCg!E4hz(^3jIW= z858S-B5#5R=2SyPIajh|ZBF}`YmW$^ZA^1q6=>!ZeE6chH5|{t^3pP}>`#gV&8Gr? zN(dC_?Ra|Na2d~INDYSSi4kiu3}&Z)(kbL^Ry}Dww`KL+q=+)`PNAc0=v`7{zjuK; ztlCsFQ<~5`X56@8eD|>iheW01qc^^6M^Z=3IrmWMcXhR+54?Z&#Gn~ON#*VV&S%$bBbH<`gpy+Z+!$u zkQlc+y_D%@-M_|=5bhwc)fVV(-g;UAfx5Q`g<1e4&Di#?37pn{8nmlS*-McGmx5avSF%Om3x9;NpG$lA;;Osnc6s z=HmvB4{9$~(Es7`$tDZ?`!?=urF4hUaFOj;2Q3Q@^>_x<_sj1bTtAPf_jU(8pv|{q zRyTLs7H!b0D%Eb^X#*2@py2dCszbf|b1zTq^1S6+14&k}TC?v74Vlz!F}p+YJ1C?B;&Hjif(xUK$qO z$vF`N=i=d^`hmMI+0$A@8Pm46-R!rbAS<4`z#Y?DqQZ&1ZlsA?)y%I9{k~a}$?x%A)Y!D|`vfZoQ7isYc`#$vs@o?3EOs*pnLIZyE!C zGhn{?Q*N|nVA0>x{{WMj+uQjy=%!jJk7=-7r4jtOA7@7D;&}wMvF$!svsbQ=gBpTO5Bqs@*>xs_ar$S3m_Gdo_o;pENTd0_e4}vDcHbtO zPXC_6Y^r$3vmpBP#T_cyXm3?Zn(#Vk1zW6O*gN}QGj>*d=LOwG!D=%3}0pXT{|N`?sdrO5+s;!CpSN7sM3Lqu7zWCtn8zl z$SnZ7Xsm41TU>}>2UTHP(VopOEY%!hf2k$5&(X@n9|w^f1M*fDwo_*3`DJmxk$SiU z*Ob=>Z`#c#->(u)_l-g|T3Kiz(-Xu8TIxT^zZCn^Zj=i9HDpR}MgB;vA8sU71U)#| z^ZvrJKQAu~)#q;@i-e4e_UDpJ3v7LXblolG(GS=_(Tnih$gr{AE>FRfhiP+@sa(kG zrQS<%9V=Zv6XRY7r}sk>xeNn=|5cUUcdxoid2RucHZtJ-YPgyTka;+jMj zdb8CL){%Tibpfy#RSlOJ0$uV$`o0~3XX{G0Y z`F`{E!<+8(>fz>C14noSj~W9+(PQ$!m!8qZW*!zs$OM@-+)Yvfh|0;D^51knwKT$= zG{PU2Lk*l2x>|BbKfF|Y0_FgQt~@7fm#GVf*o^;npWP=-88FJ;vie}MaEz9foNNix zgC_|U**{hnOG>1S0An2HP@hum90`xKF6nRU+9_>Cs+4|98+1zIxIX2*iM7xnwEn<1 z?02PFRi`4d8b-O0;reg{G@2-9OGi~$}(H{HGxLCVmYZ&QcaoBm)0DhrHST{AucfS!F>a^uo+ z$5M=edJxymbxVu*HxZ2Zc_g|4Y~C@t2-Bh!JpaQzy7 z$7@ntxZU6>=4xSl#tskfe(uO;rWp2$I&qr=T8q-D&tB(77AFuZ4PX2xybettfG!j- ztlFjmT&O&8$8zHfSFba2OUj*mZ`!TF2-8dzIB+H9*{~E>NKXu9I++FHF>0y zMAa+<-}m6ag#JquQI?)(iaJ z6=j=Sft0DcOaX>gAR>C;@VM8}X}QJzKlL7HSd>AQnXGr^y4^x29k5WdPpv+5ZS;!- zX2(TfOt(uSuU^l%oCS=y_XV*8zrvp+Ua{Tm|-( zT+|DFpeu;Mm)A~P%MElSIgQ(9Z-%j8ETbOnY1cb90r*llQjV$as+Us$ zcD$neaS@cht|@|V?WG{=Xs-ckD?%tqR9|7BZv=sFI8@2EF-t8DNkcu?^w7bN zqW~M}AM^5kcLv1cZY(;zkM@PLU&n~wbhwYrNp%3&AI$mj6kqglgp&B4G0BqLF^&7*nty<;PX7uli zdej%MLEc1bd~TYk9)I!NbQZb8zfiXPGZS{twm9arT7Iq9ru}&vUrjk3@DhTvg#}TG z^tzfGv)Xl~1sA0vmzs$6gmdi)BiF9%Da`D3rlwEe5+Df(>N^MXg{k-j#T{6;mR(z+ zOksqBswAcQxv?St+W*$wQva}$N(KZR$V0$E%iA~cU##5V9=dZ9YSNAK?sA_}tNNsQ zk^(nd!YWgH5%5|kH?YDy|5;(V?VV|H+iIl$)F`B_D>GQG%y1YXhBWdeoh}9r%>@Tb z0nQ?J?J#qDhzwb~d`EzymeXjqr5_6dR4wgw|Ct7>hS|FwF63&Wb7lEKF!gakLjE)6 znYQUCTbDV=gW&4s#J1$NsYX$zCR}GJb0#ArZbRA+EDGYSQ4dD}s=jM|IwEj!8S6C)$9h{8=hcj_a8rv)qg zZmR?Ugp7FQV_~WK^td?j@bno-gj_UiSdU&+@$ZOdKr=AOIb6JwJ!H8MfxlI}9LE22 zBOw@b&tB`0Nb>;*0(mwePn8l(zrf`3PEx(XhxMcAlF0 z(TXL(5Y_WDG-qSW+|S==<3fVs0UlEf_du~a8XCd1IfOt4L#pQU86!wu>$_K(!4M~Z z{%{A^GbcXO>>@WU%J^q9&zopSr&-x8E^25ZOe-BqfYA^QZ(Wq_yfpDD-f7gYO{Ps$ zxow2)deUgKTg58V*IcU-VjBChC6d(;aeHD#j zzs?8|_jKW6i?@JrvDejXfsZM6U5lNnM?Bpp9lolT%_xi-a%?x9do=MlK44uyNg0t3 z`1jae#U@}<(nIy}gHG_8qWZS-0>&X{7_4CiY^G^?QGvTXUz-?Q8hW)d1H6Sr{v(yA zmt+Lg=%4V&KTzM; zynwjp6o%}B3-5_k7CP`=ndEZL@F74HI?);2!?)AVCxtRBeL=n+(2gNg?Wo~PoAF5B z&gYc59i*qY`2DA1q8ns?sSeD%)CX4*qwvmV?B%}V%WT754U z62v&5^LZv=N1^CwhHlDa$4Qc~)u_e-MhRlmVkqf%U9yMa+i3ty&9G zzAQw)WimzuQ9w!7DI9U0^8E(PF}mq;2Bu4wvfL2U<_i1)Gnun)u4b)*Lr|$R8k!Ys zNw~SBWQ4<@M=G@Cmc5fJ2OtV+dV)`!?kd?#lYg^8HGhNoiwvi3EnltKVOER2L8mrA zyqG5dQ;5?QbFAi_!(*wDk5V6PZR0k#(kRoJ(dks?d$ho+L83U%sxA)rmEhWyLfAG0 z6FhD#)cZ%B1P*fjS5A(7DTTzg=wxONj0V%tQP8N&Vw0qoDzJ8 zKE{&t?G&xM0N(n(+ooNjIBowQb}6&F5^H_VBnY)pSNb)cQ#FfNzlHG1L+AOo4d*ZC znRiem-lpif_RQ5UYLU?$b){yZOZ7*O!ra@Q)*rZYPzw*s=6Xl&iP88W1->wm?eK@% zD?OWi#huL`#POg*8g{du_|SiHA>G&|TjVhsXStkce75PenSMo!=R|NYm(NQve`(sR z`d)B2qNke~=Goiv$IUF=riC|N!0&Tc;@2m9OGv99BbNu!CItQ~RX>%M2SIgxN~w9q z)K=4GpK8+Z;M&Uk^s~*rix1_?Hv0etUM9=P9ML_@@vCsj?mWc^ReT)F&^5cFDo`Qg zQYS{P66~5K=t&fp$@mgNC(Wdm-jh0_>zI8gC)~_?cE4J$+Y4q~a1I)JyGsgNmwv15oeN((-XUhwwW}s5;LLWybcftM?>owwhpRJgvX8;;w zo!E~lDNG?{o3Gn81=?J2lUZBYuBxlJSoNB0wmUj*K-m3CeE^JlQl{#4UByhmY~^+p z5tzirzA%F>W60aqojH1c~BzRT{wc!+0faoqonciTRT;feB8nB<7 z?ag1S)R?@O@ZJola6pii=)*OIHorv4u-vn(aPWSUTeH32LI3i=_z5FzxX};K-(umR zs0)EYWSix|l})jR_q#^6Aq7g5kFTsbRT2n?TahcfoQWu-60}AJE}-F5#Vr|9_|ia3 ztsZyPB&f^dhpY6ujpljW6dWv~Reh0*D9z|gE6dVqHnhTm(D@16Hc@$WhFX3*48FUM zhGn=wVDd-ln?AF~en`9`6xE8PiGhQbF*S|)z|Z1Jgaq%X{e39;7zjO$0OM@@x?y-J zC`dhTz*lSk=eoI;0io>*d(K2X$Kv%D5Q&4BG|j5S;o?sP?RkcR&NB|Ux<>b1X&`d% z#G&-xZ)q`L^>fp%6+$H_XlUANpwK^v$-4mgo0qIR(VJU*Ofu?fYwWO0%2lzUg?ZqN zy<8bSM(g?31nzNI=WoaMzaq6V5>~U%CP8|VYv%eX;;Iy%Wkx#>Jqy@cFQvk z8oD;wINLe0)b|crCvC>2z4K4M`I*;}BzbTcFFfQd>nwU>f2s6RK18e^HdHI+t1mTE z6mV?Nl=~FRR)Uo1!``eCts@*elCy)9tMZzzOvE28s-ntc*SQ}Cs?*s?FO0!3Ude4F zGDicFrKGlV$;G;%RrMmk3Y+Y;b{bYuHiV)OxUC(a;QtP5=?U`he6S<0s(O!Ugp!x# z3+c`bfy4@SZD)_O4>M#+KoJvAlVFXsnLf4&B$~szAaddAb#=(FC;|osw6X~(eY;3g z$5UHz?0-a;0ef$_%QNJot#gp=p&b_4oZ1sx62{-6%*XX&kspJ*UD$8}2J8ryw_tI5 zK#>W$9bsx>FijoADd}yGa{}7st)kaI`Mwh9md)9l4kFu4nO`DvlyBx+bshs=$f)aA zAd&823n6tSP-E;YG;+dduMJm-o3f&`xbuQeE+-k7aq)myKL_@*{clcvkcna4To7>X zfH?>$Mmo7L$$F7bC0A^LDnB9!sd5!SnVF}@Ncj>Wn}ao&-UY-^rW+5d12Ywpg%DDs z{PQW}3)PlkMZ-Ujc)9n!KZohl%7m1V6=YKJ0(Th;+~q40kVAWfHSfJ{E%u)G+PXHd zHl9TSr#-0k?S)GKdV%+NLaP-V{j(==$rs=bleq@aU?iZyoa>QEz8?6bI155&jL`*=;_VS=NkmLN_ZEHnrW%aORS6W5^bR58ly)u2h(swpqHG{rB1wnoF0sa!e z6gYa#9)cqezJoKqrMHV8C?H)18)*T>;y&P70*-?`efPxYhiReH4b9w!_ch_6#m0c? ze;NX{vaSOOW4+gmcWv8=!jVj=z<#hc=OIYx_ec}viJw;THO9SGyvSqlH^TN(Xz+v~ z^@;Ph?IR$VWPGocMx$E$B!Er;KNA`b6q5Pys{{FKdeZjr-zkCk<a=6tKF$+8l@0 zrX1w;i1P>&+_NnH4hDG0TZ2Lez)2523JfNi;dE?6hNUE+bp2i8Z28cPhsmfZ+h(wU zHz0HSyTjna$Z;K1+TSG}NZ*t`cdl%70i08H$liVLAU4O**DQqI+sRdZTv6}MZdCJ5 z#NS5fK;3|<&CDBZd=Nk+cZ3VSgRl>@-ykbM6QYuQ3J4`e~04M`QGyrHI>34*+HKWZ;Ach1x z0jzmCMR4?Fr!`FJrw%&dV>R}kNCP=T;9P0_BjEY(Gfk7;1>YP-2KS~UWV6j(IKn^m zehS(~=!nM<7nFQeyJ$K79&u{$UI*1ciVS)One$hjmXPwj9P8KE1#17!dE=Gz3INKW z^=AOg6~vBhk81 z)z=5z0v@o_ES1ty3clAZ~Y zgnbigiI52W^9;n&1i8a)e<$rX;0h$lRKN(BbTW0)9-s`!bj_)_zp|k(@P--4t*C`2 z4c)e@_M_YWVA2n5OCdlH+URkzGOhiPbs{(le;%3p^)wvZYnQ2ge-B;yKm>5o)>G?cK(AL|$->wpX70+l=khk2oNkqkbF*gPEJQ%93{7p6~*(l~i*^ zAT4&LpPqY;Md;^(#il<1ErII`EmIslCSk!7Y}r6ZE6llHjEf_QvTlyuk*DqG$dtm) z&H%3CQE;||vi7xI+LdCGg4Xc7oj&z1g zJpfBl0kB9X;PqH{ZAhURSYgfQZ{5%a8N&eh@!gs)Z)Q^o-OR{h!4R`*Fcn9!#a_>wI6v}@(15|SK549RJ{xEtyKYvrRk5WV2+sTCD93Q zKGg4_VSI@c7}FKeJ^0E5R<#}tF@NWqZ;cyqIk?GZs9K9)shL0v=4Cu4KySIG0UEw+ zYO~a&zqQiMS4}N{*Ip}&Ns{ld!>(EL)Z8(4Qm$>?OaITQ-1j7=-lMblNNtxSz>rIqG_LX z{Bg>z#9qS%%W2mj3cjm(yEfdCQ2DpnArirW8$>%*d0QyEo^=&lEZ(SL^^_*NTn}tG z*cDH=?;lv}evmZ>nkjO`kjt&?8=8Oj+HAtZAphY&L0nyh|J5jz_?OYknr@EI0cU-8 zSB4V92Y_0zdS&TS3no2tLX1|5 zbEmwkKpu`puJo%M7@;*AV$)YbMoZ@fu)AxkifvpgfKq}@<1t7den6i!A_?DvLcOcJ zD%uw+!pEOHylbRky{4a3_v;il>bUwLcl`j zMbPfN8!^s5AFAwUU_zfeBI&f(=Ap-n$;v9P+Rtje-1!~Kz_uYd1yXF%yrw(^3~NBF zGXd0lXz#jG&{1~M=Xw}Ew<03CTz&>3<(Bd|gM(1sJ1~SvAO6}8N-0%Taw;OcM)y#~ z{T2)QG5n?q_$GsCW(vZ!Xo%Mw&!3#B2Qm`dSo2pttjeE-3I%M$K{*nijU1=Hf0Rcj zhy$&XrJD3{HDkC(UP-I2qL@?h-~3P(J`>KBnY627QfliYgViwQUphY^)T~yKf(v2HK*l*LY8w4$JXa2QHvahv)L!oe_nN-H z?v_YML`va+xxRbmF7YGouv&w3l{KU<6J&uzFKGN0kTZ^c6H4lQX-Nfa_9GMh+5Owf z<6pSE2FYZAM=x)-`RLi}A`}NE`OkEL zxUe#yY?lGyP=QlVsj++RVGX?)Ged}oYhWV$YPMuO2KD*o3rJCkdwN!1?HBqXIJ)T2 z21%YId3N3eC&>+C18Fcp#x1?YbsQ);HEzJE0D)#SRzpd)sMr)W&oBDiwvyM_3ue(y z;@?KqikP0YAg`((VI6JnzI!Qs__F^MId{lBmDQZPEH?2}?fI}GC@PW>;nShE#L?Ma z#FGc_19zE=HQ4G*#;NQIp>5VnInSXrTv3eT5cmEl>Wz(?gES~eylO9L$?y8URq4Ji zx)8?A8l0Dkk`ilAIrr2KvN?mE88u#vpIW%JeZ? zFdfXDu(&Oe<3>Ks2c3DkBcYL*vEY`guF|rLhn^lt7zFuJ*CVDbb2X`+>qtE*S;kWt zOlO%B$}~89Y9jke4ipik*^dKaEDuP=Y69vWd1djLJyiGWxu-I#|~A4b?J?HyA**pleZWL%c^8yOw@ z)g-+-VQ@-;YHSC0ZXHju!mms~TGkcNX~5-zY?PMCDl0&>Sx^-KUXYM?uQ9IX0aPfj zT2!_F=HL!8Oo2V+O@6*C2&^+e`Wdf3qke>|yc#d4;r!%H6*b4m zLpGkeQT?~pv4>KcpM*jzBVf()fE;R+@O|kEw+uj5prt7*dvLG}@%DsyroT%L$sy=1 zb?tL%0!S8fW|itpKHdtTi~?k|&bP%`b+75^m<4>Of2MxqsDXYt=EqK8R5EgMm!R~G z+S_%%jjan8WW2%~HSv-AijwGo!x?Fu#;2%Jun+_W(&6Z5g#a)^AcK> zt1gs6j(n?aAIOP-G^A$7P|J(kqdwJZS85DF5e9416!u_xX&@4AQK|qX&6ARQ`Yv=( zXre%dYhp+V5nL`@my{MQbh*~+?yNWiyUy*$KMS2Ata&#sNctzN%GCR{1Q1c@wV{A8 z&D&4qyKUCrgNFw4)%7`J0Nu|TBcE9<39Y>KB%P*nGNJ1#Bb ztx2sb;F-=^D>Wblq+FvQAZliEsOdXZl_#H-6Vz&Z>Xt#Ng3?(jXHV-vixPC%unOF` z3rL0e>dT3sKe0KU3a|955an~1L~CsD?I>#z?nhFnN>h^Jdy2&XQX5hlH0`-O2IcLs zAcZyHgB&2YQ7>L5UI0po2P|15gP%uP6ND@WTycT6OK5p~AY%^o6U6sqeQEt!TMA0! z3a3(7hNp%*jI>cj&IaDwOE(7s)r;WcIyR<79M|WebS)i_wndaVs;va|c;saTe*hl< z(>o-aW;!RdHq>6vOiuYl`+fCI?1mt>XIjeitH7;mU&yd|Pmy{_g&1UIou+)%x1-G2 zA|@|qfP`R?Syn#j^8zjG{BcW?U1dBw!Suc$S=E)y3=+p?$h9of;5!&hFAtS;xs7(HCQZ>mH;+b+F#AeK&;ozXd+wM0UXFOnu#)m{_Nw(( z>Jr*jR{4CDHw=7;2QTTFPh$S_R#@-fdq>7W<)N4}=Q&(FGr6Hqi2Pw%*zp6b7VW@d zNXM;#B*$^v+Yk&w!|8B1DxJ#EhnV_VG9#^A2O;i0xOwqw7wVMJ0JY$w4L&FW00Dgf z4B(_&o>G$$Wfe>SAi!%@WgtB(X--~S`v4?uQ_nx0l2SJ9uMaJh-F+6#f}IJiSHWbu zjoXUQQTOcS+X<3P(#JS7$*gFW#!8rf^I5J zvHQ<&;whDn(x9}Iak5<>NFo6!AjIa0FCTZ{1NuwJ3 zpx>)+zV=UmW92+{4S;4oT;TiWtD3*a8DQqe4>A2D%Q^-?B#MoJ7mXzbw!z4Waa;O(kWC0`$<=~V zI~Q&Q)ZAW10~y;?bH~e`OultN!XgEZsT($aq%6D&$c!pL$Jg~SegE^3Z~AZl-ERR2 z#Q*VIMr|BNS^L{7ffImh0rgGCZBINBp>EuM>y}8Zd|W$r;QLU_tJsndA~uS>^KB82 zb>HMJdcQ)LkZEYQSLOj~lC^wL3H6N78G^`g%>TjXcyuf(a%QxXjw^F^YWpcJw7&1i z2de!T^wqFt>y7)xdIcsw3qz0$yVC`{2?^icp_Wf0ou|QfyEpJ&W~B2FIhB7OFd7xp`h4SBrEYc*&1z#C>DOw~*3{@H4rei61;Q=1Ez0kD56Mv=%x)4Don*b(EsRws<(*6aojJixI^PIG(+v*ah(s{xSoNE&KV@cn9 zAHTi5b8I0%8B|?<;J!8#KXz$>HX77>OlUAOw2a6s>`HdMR2>6@T^J&)SG#vPA;X=P z#%eMShIpH-1%Pfo8PzVm14SLw=bMC~Y$3odY(!gBzRqY??Id1#mv|#~0**lpdoTJ% zn&8RSLB-ne17F|)r6EK5HcmIO`U01(cXZBr44Wo-7Ueh+JrE5KWTZzEXfSbYs*tuaNXx&^1^aqvD|32{h(@BVxqpZ?0e#d-41a$J#o>K zBv7#&X-5h~1y;iZD!P)fJ5g7_W<_EzZK#3Ig<@TL4mT{ouV6C^NopXi+-fDgW@YY2 z3p!SDKx>uQCh4p2kIr4hNxF$aM6>MtjuGr>8TYby?Ujs$BgeoeuMVMoQ@N}rUF|zF zfAHl^cFp(sc(<6KYW>(70$l3UznGxEF8n^Hv^z;6trN$bbBV%k=8cE+`UVFZYqhsr z3YRKp5G#|XXlSBg|2ke%X`EuWzm6KwHXt+^y^DM7=ENKjrBjU<;}K}Culaqyvoci? z8XJ-)brpAahJn38w(jRo*49c}!7`U+ zDoTX;I$=GXONi0wU80y}U+KuN{HUqd16T0ZPTY^Oyu{-3+IW`(EyaxKbkytbnzde+ zcy`rDye8t&wirRrr=BagL$#omjvc1BrKG>xI#5uIDNrv$w)#$NSD{9>yNb$2QU~G7 zjF@>mFp}G|GO%q{kj45QvRIp-iUvDPj|HuEbVlnRR@8irc``{58T9Vf%-ZVoS9m%P z&fjNx%v80Z6<>uO+lf~F6hXUVCO7FZ^Rt4W7U$$31hUf{9Nu#oJ7E(x9j5sD%mU{w z))j4(c;y2iZtKEb@h&$FK~4JCq$B%_vcVY~rQg~p7%T-#d(d0gDzN|Cpn8Wo@-=16 zs%xt&KY$<*(-~M#CeL?1sGPE&o%3ntH(CZx7d|- z$7{g71%wuC{>EAB>2C}=NLy6R;$5~&q6AN-xP8A28luieUkq~!QUD($Q&r0#5(@IW zDPG?OaC`wX^^L=}nnG+wDGM9t6fo-~7kNQ)*ZjJVOHw9o*J#eemr%tY=YUw3(XW3^ z@tpVYu80C2hwLp-u?w=Sxhp)=`GFM}!&wiZHuWbm1=I{&suEF_nw0+h-iPmUcalk* z`-+MJdZbz0vC>9ickiDfKIC~^V3)cXo?6GR%iH7XBQC?|KjRmHdUA2ny4bR_>{Z~w zAzxo%@)<|T9&>mC@#b%7QSuZ4)knp1TITr<=dXTc(<#K>@X7!^kUN@J01hW;fp!po z^gX)fgiY)qa{#Apnn>K)T*ZzUnkxlw?J+l3LJJOzD;-q60tDFDM)I%5mlL2CcFkw- zDdBl=#)0rYf&v&;*KEoq2ztXpnK%nRV&hXP7g6G%R+8cM60ybVb1nDX=4@pK8Q>fS zWHJF;(twW@z*NWla>E%X^(je5K<~9TG|kjU?T?lSbNY}j6{w95s0_>**B5au!KLWH z091{Gy5r=ZGO4?o@7R?O<;nmBCB6`l3L&+Vcu~`W9&=m1unt%E1++pDL>)al6}Y-f z=-QI-n7%(;?&`klv*0m(6;!eGzz^>vSmUJ*>oEFmP?25wG@u38JIkCcOIK|@aQEtQ z4aH67XJBiXtvfbuWX(FPevM6|wYRSZ)LqeQI>R0zB5@W5H$ z#61ul_`+k`OQF`jta=JV7_4BzzJV8?AgRd3DA-$VR?yYOiK ze&ZV(9k>Tb4GLT`(++`r6_6dC=TSSlf}#2<&Ry9+nvMzv&e=GotQUazNUycDl;>EJ zK8p~-*5TiU;=U}&K9C>YZce_s5=Al8Hm;~E_~{EPZ_=N}H%#ypjm-doNt|U^nO(>Q z&QoujPe=F?hbo|5g5qdXdaXz(`hpK{nQ){LWb{S;ySvIQ&kkL6Y36cW?O|XO9ks(N zG1oDwPT)BDI;Ia{riUVhrjbez+J3F#R8sGATt}=P#tL{yz0`oFz#%LC|FYoG*=IQv z6r;|0&vV5Dl4`@`9K-NqHTY*Ku>XcQfGT#Va!@D0^jQ1lR?*KOw;va980@iCN92{O z6Aj_xURmvdBp3XvQ8%cDzmQ|x?J4K5@zJ-nzTEeO=rMG@v)I!JSKrCqqu?!?oL82Y zEbHg~!9rez>^E(=e0R6jn{nU_(E#ki>1B$gS{-m_X+S*{44qK^hVA0PiZxZfa_%85Mm)$N<>+)Y=bntp6?SnY_;pH~@L zku>V8J^Jbhz71DcgQpXR#Q-p-zTQ@9afd;T3xqlCOP2Fd%k9^jnNiFwj^M}fA@A_i z8Y}DuRxFe^^h$|Ife+q65da#NQqa$XurgG%jgZ$rdE;Z9Z^=D^T(zUTNH#XM-lY*> zHT$Y=T5Ug}Z}Ep#su4UN9c5WkCq#oq>oX;3xL1 zR(&_yeO+aVOc54gu|#3r&*~p_3L3|b6%i}R-q|zT>+ZS53u4wcQrunAh(zu{BjI5j zUnrUMAhACgV;fLNblsM0iV7K6-A(ge=$phOEH-mAl%#?%*a~u0#@C2 z(-LvJD+g>tP-h^sdiGkPVSb^vS^-7$dM>K3!K9Mp+)##GAZ9tkdIL>Cce4E8+lwp? zt!VRSJAp!!mpMNKa*_M<*S6Jvs3BjY?DCD_jy{+jlz~953`WNke1bU93N<(`=F(Uz zGGqyhBW2G2f=-c@JQ%joomw{diYc6M;(I}R!OP-QdQNOpvsKr@qLH!kz7qCU^o}$E zlxs+U5YHe>c?4gO-#Lurf%mJNe2SB(G^Yz#nF(Y%SEq4aJj(q7c~HZ*ha+DQxUY#>_MYSo0t!ySP?K z>e<4mK7IHuQEJqwXSzA9O9z5eqG=O`!w#q$#KwV1YJ*A27U^{y%R0wLm@9yL{}Q-c z-*}Ekw_JT^&2@5BgdF=;{pPn#wM>KJ+8$4tU2APUFJK{2oHiI?*rrqRE}`NY0=cyx zVeo{dW?NSn_*0kKixvpJkrNzUK7FCbq+N>WRDEXb0G&b>X6pTMLdq5Wz3BUA!Ijnj zDS-6Sp+2HO2kv@f>xQN(^`^hvyXkxXc%CW<^1uC8nelH^bh*^uX<9|Uan^v|Xk-y5 zMX1dHbWMKgbCDa~3;LPPl!`gi$iAh*2Q)O$b&2wq2PDT$ZZv$i-NbM6Z49qRi zNUN7=wLdBnC=<9(qc)kkT|suaBUf?mRvYyTN z3T>{s+vOm>&S*s86rj%hr6@Rn3S}eQS3-t3bMPEGP(^ZgYKq&Y;Ip8R& zE!qvf#a*0~4~bHm8{jC=wOW>_+YeP3E1uL<*gd2-s`!Z73RP~>4Oy0id>cRI;;m=~U5UDofIYbJzVM~6)7Uq0z6sZ~9?Ozi zr3y6{X7F;*@0^iyG_A;-`wrR~hyN@;GtjG?NMO4FD8yjSWkF_&9_HqK7A9T>Fr9pB z_xO<;!@nZ8BgleGZa2b5K0$vE6tRswI?eD>Vsk5#} zz47_QtTEJOXCab5{2AtOlCO8ty7AEHjam))kI>G4f^MQV4|GLnqSe4&fG>E1|CQai z2R{zc9l?L2K$p-^M^0l!3t254yb93oZ$`i~AM|gnGC+Uh6#f4~c>O;=oToYS^YeSB z?xU#F;pRl%$s=+N9=r#Ro~76ltkSlRi5TCRD(W{b@4rF?gl2)YGF zS@^2Ay#}5+x=nZti(~cwT-rRIWR3PuFsofI37;%M4cVb`Qw-O;0P=LtHI6Ld#DZEclN9Rm~w*jO6m7?`?W`T&I)twN-Ky=OtXvMhSYqYoh+f>lpR3`vM zD8hk=b(b&Tdza9-gHETucw^0O6c#yIsDAbVx!*`AY+<4(9yP+PYt!THBM!B5&5dhjx5c=S46ytVLoG4M6zUbcHloqq5^o6m)6^s5dm7?HsEoih z5hH|1pd-Z(igwZ*2{`MTV)<~dB~7(5at}zT2tZmoidvN8-`#G}6XwMJ%JE%tm z?;YT245aWfimlKTT2g=!JR@6u2z9hh{X%6vbsbnA%ygwz&SM2Cx0_Hj3)s=?7r;Y7 zrz6?vAO2l$=->vuJgRXPe61udCl~A$_<=_Db8x{?KuK1i&JFs7#ZqoC7;<^3EknQ1 z-ORR^qvmCRzr5uCb5dEp{n^x@d(@}~2z>ofYcxmx0Qxlle?4Gjq@wa_wQ;=4Rx3YC zO-(H$3!1qVBha?P<)#fKWKH9hwQ5M~7ShvF=MrsB51URe8=3E`aUAvxn+LB8_ z*Z*qn%DG;@}S0 zC?FxAASgjuqz#ZvATf#}O$uZ;fk+S2+K`{4&R%>YO_F)O+u~TlaqV z-m33K6uIQD&sT2I^xUAq&T7jDCs=yR%Eyh!xy%*qW89>q3bs{5vByKno(K6<%*pCh7OX#L{FeYw~Rh}yuO%-lE61rea&+p-4#Md?t zM+OSAmb3a<(meMiL4J(Z0BVP`F2Osea^8pmS;dFqy$cFp$NIqO=|tF>FX0J>9M5}@ zM2Zg@4|paQGNe_CX?qEGORW)ARZG%`&XEtam#?khQvi&EdbEsoS-b*ek`Uv!U<&hy z5wFN`joT>=&*>4wQxpbq0qF$sM%O3roFh3ClUxTs8}q&{PMy^z815)+jk88H;q}q8 z#sAEXDdL9BCWTgJD>M!RthyyIPzo7f(WEA4I9Y^lC*d>Zf+2qLY*w6@>k&|qwf0Y$ zz)%wxQL_@k!uURWCNp;8slQJyDGM!Wq5rM5jo-o(l*$wiZwV)mKST^her7XGGR6Ie zO1znJlP>pCunMbg=H#XIw&Zj%k5@;=)A>^TmD?rZD(I|F;!hK8N0uqnoCu? zANMMhIrMo&@f*pJd%CnUoBfdKaKEKyNLR1K$D$_LGmxT%!*O^iUA$h1Xcl5B0oA;W z?KwnYjt}v%2cx!qIKhCgmD4?2+tkH6x`!c+DVU7h5e)Hievs1qx~KJ#CtbqY?6X2f zWJ#CAg!gqOz#C29GIPcVg$cSk9fiWCUWjuWJbL$1z(+hRj~86HR{7?Aozkkf-A3qK zqHSj9F|Da5y9=d_5LkQ?zkNs`gpAy_drCZN2EkYA#1VHxx$w=LgK$zO4B%Bvm0q9Q z1hK_{k#KCHbHrKS@j4kz`iMk>aaKmSR}0JRUo~5j)hYSqA=Ka8qjD|ZmzMHY9izS= zGj7s64s4y^ZhUqNb}Z*#VWpMGYm|6+Y*R@OJGfb1a#fbjYSRPlx&``nISR|_G%-$Z zuc0Tg*DmEk$TD2uiRxg+%yx_52<#S?3V;jlYjYfFyCeH*A`c^cMey0%v20+lr>*Y- zNb{1Z!IJw0*pw6R091Y)xRD3A&qE1y>9e%~oG0d60uCJ-Fx|%Rl^;IhkKLf#b($2g zGYa278{_W)(EFQ(c}K5Tv9-1ZBXR~$D^{)|)XeKc$jIa%p-$aFo6?V~rR)KAjKARf z!~Na8jB&a>A(-K7g=OuMRVn z_=lA%SqEI8c(@v6WjpPit;QSzu;lvv#Z)aJRp=hjbEUGdGM*pEFt=zWvZga;8loz! zP=1c7FF5AGWXb{zwiw*RHG?!@O)5yqU)3ziO9@|NFXTJ0dTNxz+Biy#gbR@#&CseS zb`(q&G+ELl6FI(qgQknF^hPpw0%6n#{g774!NL~~v#v zWPZ0Dpef<0bkYn~Fv*OWK5jAC7JCD(sqy?f8yFs%43U{u6J zrg~mv*XCslf5kpi!{J=_KiWsjb&vA8iHw$(M)i7;`=&QfZB=}N?uesfOcF(rtjUwS zF)jfMEYVKP@eKXhLFU%d$kW?)NhN1_{@@IEXV!8X_-Lr{$&)j5sYvyly~8YOL(g`O zbazRhgqX`m#<^DwW)FM@28XZXQ<+6WVrKRX)uQeo22W5oy{X7S{zD zWRldAI#fzSMmC9_8kZV491`HOyi)1(HYuH%aTW1(YpuV9j3%Xnw?I)PY79E?-}(F^ zkK)eZ+cz{@9%mVu8wNu1D>m{3_trMk`y{)TvGPPe84Ls+flU*t?wMqJWg<5nIED4# zX02)wMKwnJ$i5b;Q>|DHkChESwgcf2#DD!$4z=uazx-pp`OBJrDOa@5^$K8R&3~#R z&;l>`7&d8Jvu!vBI}4?pm>9`gC@5psC4Hh_@wL$To%DAm5s_D%>%dADIP*sJHb_(J z2hDfrDN?^8RJ3nM^E~L~nFRE0kqHZrH{?gL{k=Cb?E3zkpmJXR%1=7FUXGg_tvB2He=<~o zAHQ+n+)8s1BBzk= z%oOf~)i#vr(vmb`PV*xPowY0kK*Rljo*r5e?EIWj7x%Q+~h&}vUx zqgtRclcQuUr3!l8 zv?;1C8G4`7+kNJ2?$FjU61d)HRN3MTJE7KxvRRj}p#bNoaWcEvz%xc$&G2}+J~6gn zOg{*>S6xCzWlkte?lC9FTK`q`Wv}qyIHr2bQ7qNQePL()`oijLhWr1>32NJG8eUaW q*I}p(Xk+O()|C1mDmNjkP|k(ib>B|}DKLzW`*EM|KOeduPWcaU`b_u$ literal 0 HcmV?d00001 From e1b0e6f0af1a80b5593de7270d3df5958dfc291f Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 28 Mar 2021 20:52:00 +0200 Subject: [PATCH 48/64] test over formulas --- src/modules/localisation/README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/localisation/README.md b/src/modules/localisation/README.md index 7e9f7710..c35d8329 100644 --- a/src/modules/localisation/README.md +++ b/src/modules/localisation/README.md @@ -78,13 +78,13 @@ As all geometrical problem it's way easier to understand what's happening with a Now, time to deep dive into calculations: -+ $f(d1,d2,d3)=(x,y,\theta)$ - + $\theta$: ++ ![formula](https://render.githubusercontent.com/render/math?math=\color{blue}f(d1,d2,d3)=(x,y,\theta)) + + ![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\theta): Considering the triangle surrounded in purple and basic trigonometric formula: -$$\tan(\theta)=\frac{d2-d1}{2e}$$ -$$\Rightarrow\boxed{\theta=\arctan{\frac{d2-d1}{2e}}}\ (3)$$ +![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\tan(\theta)=\frac{d2-d1}{2e}) +![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\Rightarrow\boxed{\theta=\arctan{\frac{d2-d1}{2e}}}\ (3)) - + $x$ and $y$: + + ![formula](https://render.githubusercontent.com/render/math?math=\color{blue}x) and ![formula](https://render.githubusercontent.com/render/math?math=\color{blue}y): Now that we now $\theta$, it's fairly simple to get x and y by projecting vlx distances using $\theta$.

@@ -92,8 +92,8 @@ Now that we now $\theta$, it's fairly simple to get x and y by projecting vlx di


-$$\boxed{y= ( \ \frac{d2-d1}{2} + d \ ).\cos{\theta}} \ (1)$$ -$$\boxed{x=(d3+r).\cos{\theta}+L.\sin{\theta}}\ (2)$$ +![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\boxed{y=%20(%20\%20\frac{d2-d1}{2}%20%2B%20d%20\%20).\cos{\theta}}%20\%20(1)) +![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\boxed{x=(d3%2Br).\cos{\theta}%2BL.\sin{\theta}}\%20(2)) + $f(x,y,\theta)=(d1,d2,d3)$ + $d1$ From 53d608678cad0439e679808025017340b0e24da9 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 28 Mar 2021 21:10:37 +0200 Subject: [PATCH 49/64] another try on parsing equations --- src/modules/localisation/README.md | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/modules/localisation/README.md b/src/modules/localisation/README.md index c35d8329..8c8bfdc9 100644 --- a/src/modules/localisation/README.md +++ b/src/modules/localisation/README.md @@ -78,30 +78,30 @@ As all geometrical problem it's way easier to understand what's happening with a Now, time to deep dive into calculations: -+ ![formula](https://render.githubusercontent.com/render/math?math=\color{blue}f(d1,d2,d3)=(x,y,\theta)) - + ![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\theta): -Considering the triangle surrounded in purple and basic trigonometric formula: -![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\tan(\theta)=\frac{d2-d1}{2e}) -![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\Rightarrow\boxed{\theta=\arctan{\frac{d2-d1}{2e}}}\ (3)) ++ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(d1,d2,d3)=(x,y,\theta)) + + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta):\ +Considering the triangle surrounded in purple and basic trigonometric formula:\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\tan(\theta)=\frac{d2-d1}{2e})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{\theta=\arctan{\frac{d2-d1}{2e}}}\(3)) - + ![formula](https://render.githubusercontent.com/render/math?math=\color{blue}x) and ![formula](https://render.githubusercontent.com/render/math?math=\color{blue}y): -Now that we now $\theta$, it's fairly simple to get x and y by projecting vlx distances using $\theta$. + + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x) and ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}y):\ +Now that we now ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta), it's fairly simple to get x and y by projecting vlx distances using it.


-![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\boxed{y=%20(%20\%20\frac{d2-d1}{2}%20%2B%20d%20\%20).\cos{\theta}}%20\%20(1)) +![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\boxed{y=%20(%20\%20\frac{d2-d1}{2}%20%2B%20d%20\%20).\cos{\theta}}%20\%20(1))\ ![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\boxed{x=(d3%2Br).\cos{\theta}%2BL.\sin{\theta}}\%20(2)) -+ $f(x,y,\theta)=(d1,d2,d3)$ - + $d1$ -$$(3)\Rightarrow\tan{\theta}=\frac{d2-d1}{2e}$$ -$$\Rightarrow d2=2e\tan{\theta}+d1\ (4)$$ -$$(1)\Rightarrow y=(\frac{d1+(d1+2e\tan{\theta})}{2}+d).\cos{\theta}$$ -$$\Rightarrow y=(d1+2e\tan{\theta}).\cos{\theta}$$ -$$\Rightarrow \boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta} -d}$$ ++ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta)=(d1,d2,d3)) + + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}d1)\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(3)\Rightarrow\tan{\theta}=\frac{d2-d1}{2e})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\d2=2e\tan{\theta}%2Bd1\(4))\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(1)\Rightarrow\y=(\frac{d1%2B(d1%2B2e\tan{\theta})}{2}%2Bd).\cos{\theta})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\y=(d1%2B2e\tan{\theta}).\cos{\theta})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta}-d})\ + $d2$ $$(4)\Rightarrow d2=2e\tan{\theta}+\frac{y}{\cos{\theta}}-e\tan{\theta} -d$$ From 0bdeae7a0885965b80beb08cbae266d8d665d8ef Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 28 Mar 2021 21:40:18 +0200 Subject: [PATCH 50/64] Hopefully good enough equation visualization --- src/modules/localisation/README.md | 90 ++++++++++-------------------- 1 file changed, 31 insertions(+), 59 deletions(-) diff --git a/src/modules/localisation/README.md b/src/modules/localisation/README.md index 8c8bfdc9..e94417b6 100644 --- a/src/modules/localisation/README.md +++ b/src/modules/localisation/README.md @@ -82,7 +82,7 @@ Now, time to deep dive into calculations: + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta):\ Considering the triangle surrounded in purple and basic trigonometric formula:\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\tan(\theta)=\frac{d2-d1}{2e})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{\theta=\arctan{\frac{d2-d1}{2e}}}\(3)) +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{\theta=\arctan{\frac{d2-d1}{2e}}}%20(3)%20) + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x) and ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}y):\ Now that we now ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta), it's fairly simple to get x and y by projecting vlx distances using it. @@ -92,76 +92,48 @@ Now that we now ![formula](https://render.githubusercontent.com/render/math?math


-![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\boxed{y=%20(%20\%20\frac{d2-d1}{2}%20%2B%20d%20\%20).\cos{\theta}}%20\%20(1))\ -![formula](https://render.githubusercontent.com/render/math?math=\color{blue}\boxed{x=(d3%2Br).\cos{\theta}%2BL.\sin{\theta}}\%20(2)) +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{y=%20(%20\%20\frac{d2-d1}{2}%20%2B%20d%20\%20).\cos{\theta}}%20\%20(1))\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{x=(d3%2Br).\cos{\theta}%2BL.\sin{\theta}}\%20(2)) + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta)=(d1,d2,d3)) + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}d1)\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(3)\Rightarrow\tan{\theta}=\frac{d2-d1}{2e})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\d2=2e\tan{\theta}%2Bd1\(4))\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\d2=2e\tan{\theta}%2Bd1%20(4)%20)\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(1)\Rightarrow\y=(\frac{d1%2B(d1%2B2e\tan{\theta})}{2}%2Bd).\cos{\theta})\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\y=(d1%2B2e\tan{\theta}).\cos{\theta})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta}-d})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta}-d}) - + $d2$ -$$(4)\Rightarrow d2=2e\tan{\theta}+\frac{y}{\cos{\theta}}-e\tan{\theta} -d$$ -$$\Rightarrow \boxed{d2=\frac{y}{\cos{\theta}}+e\tan{\theta} -d}$$ + + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}d2)\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(4)\Rightarrow\d2=2e\tan{\theta}%2B\frac{y}{\cos{\theta}}-e\tan{\theta}-d)\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d2=\frac{y}{\cos{\theta}}%2Be\tan{\theta}-d}) - + $d3$ -$$(2)\Rightarrow x-L.\sin{\theta}=(d3+r).\cos{\theta}$$ -$$\Rightarrow \frac{x}{\cos{\theta}}-L.\tan{\theta}=d3+r$$ -$$\Rightarrow \boxed{d3=\frac{x}{\cos{\theta}}-L.\tan{\theta}-r}$$ + + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}d3)\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(2)\Rightarrow\x-L.\sin{\theta}=(d3%2Br).\cos{\theta})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\frac{x}{\cos{\theta}}-L.\tan{\theta}=d3%2Br)\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d3=\frac{x}{\cos{\theta}}-L.\tan{\theta}-r}) -+ $f(x,\ y,\ \theta,\ d1,\ d2,\ d3)=(d1\_proj, \ d2\_proj, \ d3\_proj)$ ++ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta,d1,d2,d3)=(d1\_proj,d2\_proj,d3\_proj))\


-All theses are just simple projection knowing $x,\ y,\ \theta$ and $f(x,y,\theta)=(d1,d2,d3)$. It constitutes an extra tool to validate the pose estimation. Indeed, if a projection is negative, it's because the vlx doesn't measure the distance to the desired wall, so the resulting pose will obviously be incorrect. -$$\boxed{d1\_proj=x+(d1+d).\sin{\theta}-e.\cos{\theta}}$$ -$$\boxed{d2\_proj=x+(d2+d).\sin{\theta}+e.\cos{\theta}}$$ -$$\boxed{d1\_proj=y-(d3+r).\sin{\theta}-L.\cos{\theta}}$$ +All theses are just simple projection knowing ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x,y,\theta) and ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta)=(d1,d2,d3)). It constitutes an extra tool to validate the pose estimation. Indeed, if a projection is negative, it's because the vlx doesn't measure the distance to the desired wall, so the resulting pose will obviously be incorrect. +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_proj=x%2B(d1%2Bd).\sin{\theta}-e.\cos{\theta}})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2\_proj=x%2B(d2%2Bd).\sin{\theta}%2Be.\cos{\theta}})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_proj=y-(d3%2Br).\sin{\theta}-L.\cos{\theta}}) ### Formulas summary -$$\boxed{\begin{pmatrix} -x\\ -\\ -y\\ -\\ -\theta -\end{pmatrix} -=\begin{pmatrix} -(d3+r).\cos{\theta}+L.\sin{\theta}\\ -\\ -( \ \frac{d2-d1}{2} + d \ ).\cos{\theta}\\ -\\ -\arctan{\frac{d2-d1}{2e}} -\end{pmatrix}}$$ -$$\boxed{\begin{pmatrix} -d1\\ -\\ -d2\\ -\\ -d3 -\end{pmatrix} -=\begin{pmatrix} -\frac{y}{\cos{\theta}}-e\tan{\theta} -d\\ -\\ -\frac{y}{\cos{\theta}}+e\tan{\theta} -d\\ -\\ -\frac{x}{\cos{\theta}}-L.\tan{\theta}-r -\end{pmatrix}}$$ -$$\boxed{\begin{pmatrix} -d1\_proj\\ -\\ -d2\_proj\\ -\\ -d3\_proj -\end{pmatrix} -=\begin{pmatrix} -x+(d1+d).\sin{\theta}-e.\cos{\theta}\\ -\\ -x+(d2+d).\sin{\theta}+e.\cos{\theta}\\ -\\ -y-(d3+r).\sin{\theta}-L.\cos{\theta} -\end{pmatrix}}$$ +\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{x=(d3%2Br).\cos{\theta}%2BL.\sin{\theta}}\%20)\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{y=%20(%20\%20\frac{d2-d1}{2}%20%2B%20d%20\%20).\cos{\theta}})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{\theta=\arctan{\frac{d2-d1}{2e}}}%20(3)%20)\ +\ +\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta}-d}) +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d2=\frac{y}{\cos{\theta}}%2Be\tan{\theta}-d}) +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d3=\frac{x}{\cos{\theta}}-L.\tan{\theta}-r}) +\ +\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_proj=x%2B(d1%2Bd).\sin{\theta}-e.\cos{\theta}})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2\_proj=x%2B(d2%2Bd).\sin{\theta}%2Be.\cos{\theta}})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_proj=y-(d3%2Br).\sin{\theta}-L.\cos{\theta}}) From 06b930a5eca0307823a3c393161eec4192f54b0c Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sun, 28 Mar 2021 21:50:58 +0200 Subject: [PATCH 51/64] Final version of equations --- src/modules/localisation/README.md | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/modules/localisation/README.md b/src/modules/localisation/README.md index e94417b6..f36be3c1 100644 --- a/src/modules/localisation/README.md +++ b/src/modules/localisation/README.md @@ -112,28 +112,30 @@ Now that we now ![formula](https://render.githubusercontent.com/render/math?math ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\frac{x}{\cos{\theta}}-L.\tan{\theta}=d3%2Br)\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d3=\frac{x}{\cos{\theta}}-L.\tan{\theta}-r}) -+ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta,d1,d2,d3)=(d1\_proj,d2\_proj,d3\_proj))\ ++ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta,d1,d2,d3)=(d1\_proj,d2\_proj,d3\_proj)) +


+ All theses are just simple projection knowing ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x,y,\theta) and ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta)=(d1,d2,d3)). It constitutes an extra tool to validate the pose estimation. Indeed, if a projection is negative, it's because the vlx doesn't measure the distance to the desired wall, so the resulting pose will obviously be incorrect. -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_proj=x%2B(d1%2Bd).\sin{\theta}-e.\cos{\theta}})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2\_proj=x%2B(d2%2Bd).\sin{\theta}%2Be.\cos{\theta}})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_proj=y-(d3%2Br).\sin{\theta}-L.\cos{\theta}}) +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_\proj=x%2B(d1%2Bd).\sin{\theta}-e.\cos{\theta}})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2\_\proj=x%2B(d2%2Bd).\sin{\theta}%2Be.\cos{\theta}})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d3\_\proj=y-(d3%2Br).\sin{\theta}-L.\cos{\theta}}) ### Formulas summary \ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{x=(d3%2Br).\cos{\theta}%2BL.\sin{\theta}}\%20)\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{y=%20(%20\%20\frac{d2-d1}{2}%20%2B%20d%20\%20).\cos{\theta}})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{\theta=\arctan{\frac{d2-d1}{2e}}}%20(3)%20)\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{\theta=\arctan{\frac{d2-d1}{2e}}})\ \ \ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta}-d}) -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d2=\frac{y}{\cos{\theta}}%2Be\tan{\theta}-d}) -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d3=\frac{x}{\cos{\theta}}-L.\tan{\theta}-r}) +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta}-d})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2=\frac{y}{\cos{\theta}}%2Be\tan{\theta}-d})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d3=\frac{x}{\cos{\theta}}-L.\tan{\theta}-r})\ \ \ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_proj=x%2B(d1%2Bd).\sin{\theta}-e.\cos{\theta}})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2\_proj=x%2B(d2%2Bd).\sin{\theta}%2Be.\cos{\theta}})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_proj=y-(d3%2Br).\sin{\theta}-L.\cos{\theta}}) +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_\proj=x%2B(d1%2Bd).\sin{\theta}-e.\cos{\theta}})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2\_\proj=x%2B(d2%2Bd).\sin{\theta}%2Be.\cos{\theta}})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d3\_\proj=y-(d3%2Br).\sin{\theta}-L.\cos{\theta}}) From 3979ffbef93102f44e5c4c3a47d43dca3fc98294 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 29 Mar 2021 11:34:44 +0200 Subject: [PATCH 52/64] Table of cases --- src/modules/localisation/README.md | 53 +++++++++++++++++++++++------- 1 file changed, 41 insertions(+), 12 deletions(-) diff --git a/src/modules/localisation/README.md b/src/modules/localisation/README.md index f36be3c1..e40a95ed 100644 --- a/src/modules/localisation/README.md +++ b/src/modules/localisation/README.md @@ -72,7 +72,7 @@ This paragraph is about to explain you how we can compute the pose (position/ori As all geometrical problem it's way easier to understand what's happening with a schematic of the situation:

- +


@@ -88,7 +88,7 @@ Considering the triangle surrounded in purple and basic trigonometric formula:\ Now that we now ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta), it's fairly simple to get x and y by projecting vlx distances using it.

- +


@@ -98,14 +98,14 @@ Now that we now ![formula](https://render.githubusercontent.com/render/math?math + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta)=(d1,d2,d3)) + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}d1)\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(3)\Rightarrow\tan{\theta}=\frac{d2-d1}{2e})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\d2=2e\tan{\theta}%2Bd1%20(4)%20)\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(1)\Rightarrow\y=(\frac{d1%2B(d1%2B2e\tan{\theta})}{2}%2Bd).\cos{\theta})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\y=(d1%2B2e\tan{\theta}).\cos{\theta})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta}-d}) +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\d2=2e.\tan{\theta}%2Bd1%20(4)%20)\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(1)\Rightarrow\y=(\frac{d1%2B(d1%2B2e.\tan{\theta})}{2}%2Bd).\cos{\theta})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\y=(d1%2B2e.\tan{\theta}).\cos{\theta})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d1=\frac{y}{\cos{\theta}}-e.\tan{\theta}-d}) + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}d2)\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(4)\Rightarrow\d2=2e\tan{\theta}%2B\frac{y}{\cos{\theta}}-e\tan{\theta}-d)\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d2=\frac{y}{\cos{\theta}}%2Be\tan{\theta}-d}) +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(4)\Rightarrow\d2=2e.\tan{\theta}%2B\frac{y}{\cos{\theta}}-e.\tan{\theta}-d)\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\Rightarrow\boxed{d2=\frac{y}{\cos{\theta}}%2Be.\tan{\theta}-d}) + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}d3)\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}(2)\Rightarrow\x-L.\sin{\theta}=(d3%2Br).\cos{\theta})\ @@ -115,11 +115,12 @@ Now that we now ![formula](https://render.githubusercontent.com/render/math?math + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta,d1,d2,d3)=(d1\_proj,d2\_proj,d3\_proj))

- +


-All theses are just simple projection knowing ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x,y,\theta) and ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta)=(d1,d2,d3)). It constitutes an extra tool to validate the pose estimation. Indeed, if a projection is negative, it's because the vlx doesn't measure the distance to the desired wall, so the resulting pose will obviously be incorrect. +All theses are just simple projection knowing ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x,y,\theta) and ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta)=(d1,d2,d3)). It constitutes an extra tool to validate the pose estimation. Indeed, if a projection is negative, it's because the vlx doesn't measure the distance to the desired wall, so the resulting pose will obviously be incorrect.\ + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_\proj=x%2B(d1%2Bd).\sin{\theta}-e.\cos{\theta}})\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2\_\proj=x%2B(d2%2Bd).\sin{\theta}%2Be.\cos{\theta}})\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d3\_\proj=y-(d3%2Br).\sin{\theta}-L.\cos{\theta}}) @@ -131,11 +132,39 @@ All theses are just simple projection knowing ![formula](https://render.githubus ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{\theta=\arctan{\frac{d2-d1}{2e}}})\ \ \ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1=\frac{y}{\cos{\theta}}-e\tan{\theta}-d})\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2=\frac{y}{\cos{\theta}}%2Be\tan{\theta}-d})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1=\frac{y}{\cos{\theta}}-e.\tan{\theta}-d})\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2=\frac{y}{\cos{\theta}}%2Be.\tan{\theta}-d})\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d3=\frac{x}{\cos{\theta}}-L.\tan{\theta}-r})\ \ \ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_\proj=x%2B(d1%2Bd).\sin{\theta}-e.\cos{\theta}})\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2\_\proj=x%2B(d2%2Bd).\sin{\theta}%2Be.\cos{\theta}})\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d3\_\proj=y-(d3%2Br).\sin{\theta}-L.\cos{\theta}}) + +## In practice + +Now that we've establish the theory, time to find all the cases. The table below summarize the different cases implemented in the algorithm, it also considers some tweaks in the algorithm (such as invert angle in some cases due to some symmetries etc.). + +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x) the distance between the robot and its sideways wall.\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}y) the distance between the robot and its facing wall.\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta) the orientation of the robot relative to the wall as described in the first schematic.\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi) the orientation of the robot relative to the map.\ +\ +| Position | Orientation | d1 | d2 | d3 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta) | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x) | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}y) | +|:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:| +| Top blue | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[\frac{\pi}{4},\frac{3\pi}{4}])| vlx_0x31 | vlx_0x32 | vlx_0x33 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\frac{\pi}{2}-\phi) | robot_pose_x . 1000 | 2000 - robot_pose_y . 1000 | +| Top blue | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[-\frac{\pi}{4},-\frac{3\pi}{4}])| vlx_0x34 | vlx_0x35 | vlx_0x30 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}-\frac{\pi}{2}-\phi) | robot_pose_x . 1000 | 2000 - robot_pose_y . 1000 | +| Top blue | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[\frac{-\pi}{4},\frac{\pi}{4}])| vlx_0x34 | vlx_0x35 | vlx_0x33 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}-\phi) | robot_pose_x . 1000 | 2000 - robot_pose_y . 1000 | +| Top blue | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[-\pi,-\frac{3\pi}{4}]\cup[\frac{3\pi}{4},\pi])| vlx_0x31 | vlx_0x32 | vlx_0x30 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\pi-\phi) | robot_pose_x . 1000 | 2000 - robot_pose_y . 1000 | +| Bottom blue | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[\frac{\pi}{4},\frac{3\pi}{4}])| vlx_0x34 | vlx_0x35 | vlx_0x33 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\frac{\pi}{2}-\phi) | robot_pose_x . 1000 | robot_pose_y . 1000 | +| Bottom blue | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[-\frac{\pi}{4},-\frac{3\pi}{4}])| vlx_0x31 | vlx_0x32 | vlx_0x30 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}-\frac{\pi}{2}-\phi) | robot_pose_x . 1000 | robot_pose_y . 1000 | +| Bottom blue | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[\frac{-\pi}{4},\frac{\pi}{4}])| vlx_0x34 | vlx_0x35 | vlx_0x30 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}-\phi) | robot_pose_x . 1000 | robot_pose_y . 1000 | +| Bottom blue | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[-\pi,-\frac{3\pi}{4}]\cup[\frac{3\pi}{4},\pi])| vlx_0x31 | vlx_0x32 | vlx_0x33 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\pi-\phi) | robot_pose_x . 1000 | robot_pose_y . 1000 | +| Top yellow | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[\frac{\pi}{4},\frac{3\pi}{4}])| vlx_0x31 | vlx_0x32 | vlx_0x30 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\frac{\pi}{2}-\phi) | 3000 - robot_pose_x . 1000 | 2000 - robot_pose_y . 1000 | +| Top yellow | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[-\frac{\pi}{4},-\frac{3\pi}{4}])| vlx_0x34 | vlx_0x35 | vlx_0x33 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}-\frac{\pi}{2}-\phi) | 3000 - robot_pose_x . 1000 | 2000 - robot_pose_y . 1000 | +| Top yellow | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[\frac{-\pi}{4},\frac{\pi}{4}])| vlx_0x31 | vlx_0x32 | vlx_0x33 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}-\phi) | 3000 - robot_pose_x . 1000 | 2000 - robot_pose_y . 1000 | +| Top yellow | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[-\pi,-\frac{3\pi}{4}]\cup[\frac{3\pi}{4},\pi])| vlx_0x34 | vlx_0x35 | vlx_0x30 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\pi-\phi) | 3000 - robot_pose_x . 1000 | 2000 - robot_pose_y . 1000 | +| Bottom yellow | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[\frac{\pi}{4},\frac{3\pi}{4}])| vlx_0x34 | vlx_0x35 | vlx_0x30 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\frac{\pi}{2}-\phi) | 3000 - robot_pose_x . 1000 | robot_pose_y . 1000 | +| Bottom yellow | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[-\frac{\pi}{4},-\frac{3\pi}{4}])| vlx_0x31 | vlx_0x32 | vlx_0x33 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}-\frac{\pi}{2}-\phi) | 3000 - robot_pose_x . 1000 | robot_pose_y . 1000 | +| Bottom yellow | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[\frac{-\pi}{4},\frac{\pi}{4}])| vlx_0x31 | vlx_0x32 | vlx_0x30 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}-\phi) | 3000 - robot_pose_x . 1000 | robot_pose_y . 1000 | +| Bottom yellow | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[-\pi,-\frac{3\pi}{4}]\cup[\frac{3\pi}{4},\pi])| vlx_0x34 | vlx_0x35 | vlx_0x33 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\pi-\phi) | 3000 - robot_pose_x . 1000 | robot_pose_y . 1000 | From 7c7d52705b7b9d27b3d86860a797f11ec6cb1632 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 29 Mar 2021 11:45:20 +0200 Subject: [PATCH 53/64] Can refine position with vision near walls but not send raw vision pose --- src/modules/localisation/README.md | 13 +++++++------ .../localisation/localisation/vlx_readjustement.py | 14 +++++++++----- 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/src/modules/localisation/README.md b/src/modules/localisation/README.md index e40a95ed..5d98f2d2 100644 --- a/src/modules/localisation/README.md +++ b/src/modules/localisation/README.md @@ -72,7 +72,7 @@ This paragraph is about to explain you how we can compute the pose (position/ori As all geometrical problem it's way easier to understand what's happening with a schematic of the situation:

- +


@@ -88,7 +88,7 @@ Considering the triangle surrounded in purple and basic trigonometric formula:\ Now that we now ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta), it's fairly simple to get x and y by projecting vlx distances using it.

- +


@@ -115,11 +115,11 @@ Now that we now ![formula](https://render.githubusercontent.com/render/math?math + ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta,d1,d2,d3)=(d1\_proj,d2\_proj,d3\_proj))

- +


-All theses are just simple projection knowing ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x,y,\theta) and ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta)=(d1,d2,d3)). It constitutes an extra tool to validate the pose estimation. Indeed, if a projection is negative, it's because the vlx doesn't measure the distance to the desired wall, so the resulting pose will obviously be incorrect.\ +All theses are just simple projection knowing ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x,y,\theta) and ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}f(x,y,\theta)=(d1,d2,d3)). It constitutes an extra tool to validate the pose estimation. Indeed, if a projection is negative, it's because the vlx doesn't measure the distance to the desired wall, so the resulting pose will obviously be incorrect. ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d1\_\proj=x%2B(d1%2Bd).\sin{\theta}-e.\cos{\theta}})\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\boxed{d2\_\proj=x%2B(d2%2Bd).\sin{\theta}%2Be.\cos{\theta}})\ @@ -148,8 +148,9 @@ Now that we've establish the theory, time to find all the cases. The table below ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x) the distance between the robot and its sideways wall.\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}y) the distance between the robot and its facing wall.\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta) the orientation of the robot relative to the wall as described in the first schematic.\ -![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi) the orientation of the robot relative to the map.\ -\ +![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi) the orientation of the robot relative to the map. + + | Position | Orientation | d1 | d2 | d3 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\theta) | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x) | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}y) | |:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:| | Top blue | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\phi\in[\frac{\pi}{4},\frac{3\pi}{4}])| vlx_0x31 | vlx_0x32 | vlx_0x33 | ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}\frac{\pi}{2}-\phi) | robot_pose_x . 1000 | 2000 - robot_pose_y . 1000 | diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustement.py index d0df4dac..29f198a7 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustement.py @@ -139,11 +139,14 @@ def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): self.values_stamped_array.insert(0, VlxStamped(vlx_values, actual_stamp)) if len(self.values_stamped_array) > 10: self.values_stamped_array.pop() - time.sleep(0.02) + time.sleep(0.03) def try_to_readjust_with_vlx(self, x, y, q, stamp): now = datetime.now() - send_tf = True + if self.continuous_sampling != 2: + send_vision_tf = True + else: + send_vision_tf = False for i in range(len(self.values_stamped_array)): if ( abs( @@ -164,11 +167,12 @@ def try_to_readjust_with_vlx(self, x, y, q, stamp): pose.orientation = q news = self.compute_data(pose, self.values_stamped_array[i].values) if news != None: - send_tf = False - self.parent.get_logger().warn("publish affined position") + send_vision_tf = False + self.parent.get_logger().warn("publish refined position") self.parent.create_and_send_tf(news[0], news[1], news[2], new_stamp) break - if send_tf: + if send_vision_tf: + self.parent.get_logger().warn("publish vision position") self.parent.create_and_send_tf(x, y, q, stamp) if self.continuous_sampling == 1: self.stop_continuous_sampling_thread() From a15eb28e8f3dd77587ddd8cbe3bc6deaaa432b34 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 29 Mar 2021 12:33:55 +0200 Subject: [PATCH 54/64] odom_map_relative subscription instead of odom and tf --- .../teb_obstacles/teb_obstacles.py | 38 ++++--------------- 1 file changed, 7 insertions(+), 31 deletions(-) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index c617c544..33f28f7e 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -11,9 +11,8 @@ from costmap_converter_msgs.msg import ObstacleArrayMsg, ObstacleMsg from visualization_msgs.msg import MarkerArray, Marker from nav_msgs.msg import Odometry -from geometry_msgs.msg import Point32 +from geometry_msgs.msg import Point32, PoseStamped from platform import machine -from transformix_msgs.srv import TransformixParametersTransformStamped class Teb_obstacles(Node): @@ -25,10 +24,11 @@ def __init__(self): "obelix" if self.get_namespace().strip("/") == "asterix" else "asterix" ) - self.get_ally_odom_transformation() - self.allies_subscription_ = self.create_subscription( - Odometry, f"/{self.ally}/odom", self.allies_subscription_callback, 10 + PoseStamped, + f"/{self.ally}/odom_map_relative", + self.allies_subscription_callback, + 10, ) self.enemies_subscription_ = self.create_subscription( MarkerArray, @@ -51,29 +51,6 @@ def __init__(self): self.get_logger().info("teb_dynamic_obstacles node is ready") - def get_ally_odom_transformation(self): - if self.simulation: - return - - get_tf_client = self.create_client( - TransformixParametersTransformStamped, f"/{self.ally}/get_odom_map_tf" - ) - - if not get_tf_client.wait_for_service(timeout_sec=15.0): - self.get_logger().info( - f"No service /{self.ally}/get_odom_map_tf availible, is there ony one robot?" - ) - return - get_tf_request = TransformixParametersTransformStamped.Request() - future = get_tf_client.call_async(get_tf_request) - rclpy.spin_until_future_complete(self, future) - try: - response = future.result() - except Exception as e: - self.get_logger().info("Service call failed %r" % (e,)) - else: - self.odom_map_tf = response.transform_stamped - def initObstaclesArray(self): """ObstacleArray index 0: ally, index 1-2: enemies""" self.obstacles = ObstacleArrayMsg() @@ -130,9 +107,8 @@ def allies_subscription_callback(self, msg): ) > 0.3 ): - pose = msg.pose.pose - x = pose.position.x + self.odom_map_tf.transform.translation.x - y = pose.position.y + self.odom_map_tf.transform.translation.y + x = msg.pose.position.x + y = msg.pose.position.y tmp_marker = Marker() tmp_marker.pose.position.x = x tmp_marker.pose.position.y = y From b0a456bfdd1ee735e561b13f3c16136a5655578d Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 29 Mar 2021 13:15:08 +0200 Subject: [PATCH 55/64] removing get_odom_map_tf service --- .../localisation/localisation/localisation_node.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index f437548e..519a89f3 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -42,11 +42,6 @@ def __init__(self): self._tf.header.frame_id = "map" self._tf.child_frame_id = "odom" self.update_transform() - self.get_initial_tf_srv = self.create_service( - TransformixParametersTransformStamped, - "get_odom_map_tf", - self.get_initial_tf_srv_callback, - ) self.subscription_ = self.create_subscription( MarkerArray, "/allies_positions_markers", @@ -161,11 +156,6 @@ def create_and_send_tf(self, x, y, q, stamp): self.get_logger().info(f"publishing tf") self.tf_publisher_.publish(self._tf) - def get_initial_tf_srv_callback(self, request, response): - self.get_logger().info(f"incoming request for {self.robot} odom -> map tf") - response.transform_stamped = self._initial_tf - return response - def odom_callback(self, msg): """Odom callback, compute the new pose of the robot relative to map""" robot_tf = TransformStamped() From 86c5379ffc11ff4818386626ad2e6bdc37631789 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 29 Mar 2021 18:01:31 +0200 Subject: [PATCH 56/64] renamming vlx_readjustment file --- .../localisation/localisation/localisation_node.py | 4 ++-- .../{vlx_readjustement.py => vlx_readjustment.py} | 10 +++++++--- 2 files changed, 9 insertions(+), 5 deletions(-) rename src/modules/localisation/localisation/{vlx_readjustement.py => vlx_readjustment.py} (98%) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 519a89f3..5472e329 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -15,7 +15,7 @@ from visualization_msgs.msg import MarkerArray from tf2_ros import StaticTransformBroadcaster from transformix_msgs.srv import TransformixParametersTransformStamped -from localisation.vlx_readjustement import VlxReadjustement +from localisation.vlx_readjustment import VlxReadjustment from localisation.utils import euler_to_quaternion, is_simulation from nav_msgs.msg import Odometry from tf2_kdl import * @@ -62,7 +62,7 @@ def __init__(self): ) self.last_odom_update = 0 self.get_logger().info(f"Default side is {self.side.value}") - self.vlx = VlxReadjustement(self) + self.vlx = VlxReadjustment(self) self.get_logger().info("Localisation node is ready") def _on_set_parameters(self, params): diff --git a/src/modules/localisation/localisation/vlx_readjustement.py b/src/modules/localisation/localisation/vlx_readjustment.py similarity index 98% rename from src/modules/localisation/localisation/vlx_readjustement.py rename to src/modules/localisation/localisation/vlx_readjustment.py index 29f198a7..318d9e92 100644 --- a/src/modules/localisation/localisation/vlx_readjustement.py +++ b/src/modules/localisation/localisation/vlx_readjustment.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Vlx readjustement for localisation""" +"""Vlx readjustment for localisation""" import numpy as np import copy @@ -22,7 +22,9 @@ top_yellow_area = [1.5, 1.0, 3.0, 2.0] -class VlxReadjustement: +class VlxReadjustment: + """ Vlx readjustment class, contains routine and algorithm relative to vlx""" + def __init__(self, parent_node): self.parent = parent_node self.sensors = Sensors(parent_node, [30, 31, 32, 33, 34, 35]) @@ -139,7 +141,7 @@ def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): self.values_stamped_array.insert(0, VlxStamped(vlx_values, actual_stamp)) if len(self.values_stamped_array) > 10: self.values_stamped_array.pop() - time.sleep(0.03) + time.sleep(0.02) def try_to_readjust_with_vlx(self, x, y, q, stamp): now = datetime.now() @@ -512,6 +514,8 @@ def est_proj_wall( class VlxStamped: + """ VlxStamped class, an instance contains stamp + vlx values """ + def __init__(self, values, stamp): self.values = copy.deepcopy(values) self.stamp = stamp From a5f3378ceb877c04c0dca582859a1ac78853a643 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 29 Mar 2021 18:24:46 +0200 Subject: [PATCH 57/64] add vlx folder containing all vlx relative things --- .../localisation/localisation/localisation_node.py | 3 +-- .../localisation/localisation/vlx/__init__.py | 0 .../localisation/localisation/{ => vlx}/sensors.py | 0 .../localisation/{ => vlx}/sensors_sim.py | 0 .../localisation/{ => vlx}/vlx_readjustment.py | 9 +++++++-- src/modules/localisation/package.xml | 4 ++-- src/modules/localisation/setup.py | 12 ++++++------ 7 files changed, 16 insertions(+), 12 deletions(-) create mode 100644 src/modules/localisation/localisation/vlx/__init__.py rename src/modules/localisation/localisation/{ => vlx}/sensors.py (100%) rename src/modules/localisation/localisation/{ => vlx}/sensors_sim.py (100%) rename src/modules/localisation/localisation/{ => vlx}/vlx_readjustment.py (98%) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 5472e329..ebb817ff 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -14,8 +14,7 @@ from rcl_interfaces.msg import SetParametersResult from visualization_msgs.msg import MarkerArray from tf2_ros import StaticTransformBroadcaster -from transformix_msgs.srv import TransformixParametersTransformStamped -from localisation.vlx_readjustment import VlxReadjustment +from .vlx.vlx_readjustment import VlxReadjustment from localisation.utils import euler_to_quaternion, is_simulation from nav_msgs.msg import Odometry from tf2_kdl import * diff --git a/src/modules/localisation/localisation/vlx/__init__.py b/src/modules/localisation/localisation/vlx/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/modules/localisation/localisation/sensors.py b/src/modules/localisation/localisation/vlx/sensors.py similarity index 100% rename from src/modules/localisation/localisation/sensors.py rename to src/modules/localisation/localisation/vlx/sensors.py diff --git a/src/modules/localisation/localisation/sensors_sim.py b/src/modules/localisation/localisation/vlx/sensors_sim.py similarity index 100% rename from src/modules/localisation/localisation/sensors_sim.py rename to src/modules/localisation/localisation/vlx/sensors_sim.py diff --git a/src/modules/localisation/localisation/vlx_readjustment.py b/src/modules/localisation/localisation/vlx/vlx_readjustment.py similarity index 98% rename from src/modules/localisation/localisation/vlx_readjustment.py rename to src/modules/localisation/localisation/vlx/vlx_readjustment.py index 318d9e92..0b77ab24 100644 --- a/src/modules/localisation/localisation/vlx_readjustment.py +++ b/src/modules/localisation/localisation/vlx/vlx_readjustment.py @@ -8,11 +8,15 @@ import time import threading -from localisation.sensors_sim import Sensors from localisation.utils import * from geometry_msgs.msg import Pose from builtin_interfaces.msg._time import Time +if is_simulation(): + from .sensors_sim import Sensors +else: + from .sensors import Sensors + from datetime import datetime bottom_blue_area = [0.0, 0.0, 0.9, 1.0] @@ -27,11 +31,12 @@ class VlxReadjustment: def __init__(self, parent_node): self.parent = parent_node - self.sensors = Sensors(parent_node, [30, 31, 32, 33, 34, 35]) if is_simulation(): self.sim_offset = 0.0215 + self.sensors = Sensors(parent_node, [30, 31, 32, 33, 34, 35]) else: self.sim_offset = 0.0 + self.sensors = Sensors(addrs=[30, 31, 32, 33, 34, 35]) self.parent.declare_parameter("vlx_lat_x", 0.0) self.parent.declare_parameter("vlx_lat_y", 0.0) self.parent.declare_parameter("vlx_face_x", 0.0) diff --git a/src/modules/localisation/package.xml b/src/modules/localisation/package.xml index fa32760c..2c4062da 100644 --- a/src/modules/localisation/package.xml +++ b/src/modules/localisation/package.xml @@ -4,8 +4,8 @@ localisation 0.8.3 Robot localisation module - Ewen BRUN - ECAM Makers :: CDFR 2020 + Philéas LAMBERT + ECAM Makers :: CDFR 2021 rclpy std_msgs diff --git a/src/modules/localisation/setup.py b/src/modules/localisation/setup.py index 010cb8f2..48c97aeb 100644 --- a/src/modules/localisation/setup.py +++ b/src/modules/localisation/setup.py @@ -13,7 +13,7 @@ setup( name=package_name, - version="0.8.3", + version="0.8.4", packages=find_packages(), data_files=[ (path.join("share", package_name), ["package.xml"]), @@ -23,11 +23,11 @@ install_requires=["setuptools"], author="Ewen BRUN", author_email="ewen.brun@ecam.fr", - maintainer="Ewen BRUN", - maintainer_email="ewen.brun@ecam.fr", - keywords=["ROS2", "", "CDFR"], - description="Code node cetautomatix.", - license="ECAM Makers :: CDFR 2020", + maintainer="Philéas LAMBERT", + maintainer_email="phileas.lambert@ecam.fr", + keywords=["ROS2", "localisation", "CDFR"], + description="Code node localisation.", + license="ECAM Makers :: CDFR 2021", entry_points={ "console_scripts": [ "localisation = localisation.localisation_node:main", From 1c83268369f09b2776c96ee88e1e7ab71c511fd2 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 29 Mar 2021 18:37:19 +0200 Subject: [PATCH 58/64] add commentaries + clean dependencies --- .../localisation/localisation_node.py | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index ebb817ff..5b2f8555 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -17,7 +17,7 @@ from .vlx.vlx_readjustment import VlxReadjustment from localisation.utils import euler_to_quaternion, is_simulation from nav_msgs.msg import Odometry -from tf2_kdl import * +from tf2_kdl import transform_to_kdl, do_transform_frame from datetime import datetime @@ -25,6 +25,7 @@ class Localisation(rclpy.node.Node): """Robot localisation node.""" def __init__(self): + """ Init Localisation node """ super().__init__("localisation_node") self.robot = self.get_namespace().strip("/") self.side = self.declare_parameter("side", "blue") @@ -65,7 +66,7 @@ def __init__(self): self.get_logger().info("Localisation node is ready") def _on_set_parameters(self, params): - """Handle Parameter events especially for side.""" + """ Handle Parameter events especially for side """ result = SetParametersResult() try: for param in params: @@ -87,8 +88,7 @@ def _on_set_parameters(self, params): return result def fetchStartPosition(self): - """Fetch start position for side and robot.""" - # TODO : Calibrate the start position using VL53L1X + """ Fetch start position for side and robot """ if self.robot == "asterix": if self.side.value == "blue": return (0.29, 1.33, 0) @@ -99,11 +99,10 @@ def fetchStartPosition(self): return (0.29, 1.33, 0) elif self.side.value == "yellow": return (3 - 0.29, 1.33, np.pi) - # Make it crash in case of undefined parameters return None def update_transform(self): - """Update and publish transform odom --> map.""" + """ Update and publish transform odom --> map """ self._tf.header.stamp = self.get_clock().now().to_msg() self._tf.transform.translation.x = self.robot_pose.pose.position.x self._tf.transform.translation.y = self.robot_pose.pose.position.y @@ -112,9 +111,9 @@ def update_transform(self): self._tf_brodcaster.sendTransform(self._tf) def allies_subscription_callback(self, msg): - """Identity the robot marker in assurancetourix marker_array detection - publish the transformation for drive to readjust odometry - compute base_link --> odom""" + """Identity the robot marker in assurancetourix marker_array detection, + send the marker to vlx_readjustment in order to try to refine the position + at stamp given by the marker""" for ally_marker in msg.markers: if ( ally_marker.text.lower() == self.robot @@ -143,9 +142,11 @@ def allies_subscription_callback(self, msg): self.vlx.start_continuous_sampling_thread(0.65, 1) def is_near_walls(self, pt): - return pt.x < 0.25 or pt.y < 0.25 or pt.x > 2.75 or pt.y > 1.75 + """ Return true if the robot if less than 30cm from the wall """ + return pt.x < 0.3 or pt.y < 0.3 or pt.x > 2.7 or pt.y > 1.7 def create_and_send_tf(self, x, y, q, stamp): + """ create and send tf to drive for it to re-adjust odometry """ self._tf.header.stamp = stamp self._tf.transform.translation.x = x self._tf.transform.translation.y = y @@ -156,7 +157,9 @@ def create_and_send_tf(self, x, y, q, stamp): self.tf_publisher_.publish(self._tf) def odom_callback(self, msg): - """Odom callback, compute the new pose of the robot relative to map""" + """Odom callback, compute the new pose of the robot relative to map, + send this new pose on odom_map_relative topic and start vlx routine + if this pose is near walls""" robot_tf = TransformStamped() robot_tf.transform.translation.x = msg.pose.pose.position.x robot_tf.transform.translation.y = msg.pose.pose.position.y From cf899ee02f808d168bf9fb000d5a16e3c8da4c0e Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 30 Mar 2021 01:25:14 +0200 Subject: [PATCH 59/64] A bunch of comments over vlx_readjustment --- .../localisation/vlx/sensors_sim.py | 3 ++- .../localisation/vlx/vlx_readjustment.py | 27 ++++++++++++++++--- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/modules/localisation/localisation/vlx/sensors_sim.py b/src/modules/localisation/localisation/vlx/sensors_sim.py index 9f48d391..c6062a8b 100644 --- a/src/modules/localisation/localisation/vlx/sensors_sim.py +++ b/src/modules/localisation/localisation/vlx/sensors_sim.py @@ -34,6 +34,7 @@ def get_distances(self): return distances def get_time_stamp(self): + """get time stamp from webots""" self.node.robot.step(0) t = self.node.robot.getTime() return Time(sec=int(t), nanosec=int((t % int(t)) * 1e9)) @@ -48,5 +49,5 @@ class VlxSupervisor(WebotsNode): """Vlx manager node.""" def __init__(self, supervisor, args=None): - """Init pharaon sim node.""" + """Init VlxSupervisor sim node.""" super().__init__(supervisor, args) diff --git a/src/modules/localisation/localisation/vlx/vlx_readjustment.py b/src/modules/localisation/localisation/vlx/vlx_readjustment.py index 0b77ab24..ae3dc68d 100644 --- a/src/modules/localisation/localisation/vlx/vlx_readjustment.py +++ b/src/modules/localisation/localisation/vlx/vlx_readjustment.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Vlx readjustment for localisation""" +"""Vlx readjustment for localisation package""" import numpy as np import copy @@ -30,6 +30,7 @@ class VlxReadjustment: """ Vlx readjustment class, contains routine and algorithm relative to vlx""" def __init__(self, parent_node): + """ Init VlxReadjustment """ self.parent = parent_node if is_simulation(): self.sim_offset = 0.0215 @@ -58,6 +59,8 @@ def __init__(self, parent_node): self.near_walls_last_check_stamp = None def near_wall_routine(self, pose_stamped): + """Near wall routine: launch try_to_readjust_with_vlx every 0.5 seconds + considering the pose_stamped given in argument""" actual_stamp = ( self.get_clock().now() if not is_simulation() @@ -90,6 +93,8 @@ def near_wall_routine(self, pose_stamped): ) def start_near_wall_routine(self, pose_stamped): + """Start the near wall routine by starting the thread_continuous_sampling + and launching a near_wall_routine immediatly after""" if self.near_walls_last_check_stamp == None: self.near_walls_last_check_stamp = Time(sec=0, nanosec=0) if self.thread_continuous_sampling != None: @@ -100,6 +105,7 @@ def start_near_wall_routine(self, pose_stamped): self.parent.get_logger().info("near wall routine already running") def stop_near_wall_routine(self): + """Stop the near wall routine by stopping the thread_continuous_sampling""" if self.near_walls_last_check_stamp != None: self.near_walls_last_check_stamp = None self.stop_continuous_sampling_thread() @@ -107,6 +113,8 @@ def stop_near_wall_routine(self): self.parent.get_logger().info("near wall routine already stopped") def start_continuous_sampling_thread(self, sleep_time, continuous_samp): + """Start the thread_continuous_sampling with an initial waiting time + and mode given by continuous_samp: 0->stopped, 1->nominal, 2->near_wall""" if self.thread_continuous_sampling == None: self.thread_continuous_sampling = threading.Thread( target=self.continuous_vlx_sampling, @@ -120,6 +128,7 @@ def start_continuous_sampling_thread(self, sleep_time, continuous_samp): self.parent.get_logger().info("vlx_thread thread is already running !") def stop_continuous_sampling_thread(self): + """Safely stop the thread_continuous_sampling""" if self.thread_continuous_sampling != None: self.parent.get_logger().info( f"stopping continuous sampling:{self.continuous_sampling}" @@ -131,6 +140,8 @@ def stop_continuous_sampling_thread(self): self.parent.get_logger().info("vlx_thread thread isn't running !") def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): + """Thread function, wait sleep_time_before_sampling seconds before + continuous sampling until continuous_sampling==0""" self.continuous_sampling = continuous_samp self.parent.get_logger().info( f"starting continuous sampling:{self.continuous_sampling}" @@ -149,6 +160,9 @@ def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): time.sleep(0.02) def try_to_readjust_with_vlx(self, x, y, q, stamp): + """Retrieve the nearest vlx values stamped from the given stamp then + send those values to compute_data function that returns the corrected + pose if not None then send this corrected pose to parent.create_and_send_tf""" now = datetime.now() if self.continuous_sampling != 2: send_vision_tf = True @@ -190,7 +204,8 @@ def try_to_readjust_with_vlx(self, x, y, q, stamp): self.parent.get_logger().info(f"delais: {delay}") def compute_data(self, pose_considered, vlx_values): - + """Fetching data for calculations detailed in README, computing new_pose + from these data, estimate if this pose is valid before returning it""" data = self.fetch_data(pose_considered, vlx_values) if data == None: @@ -250,7 +265,8 @@ def compute_data(self, pose_considered, vlx_values): return None def fetch_data(self, pose_considered, values): - + """Returning data as described in the table at the end of README + + all other data that are needed for the pose calculations""" angle = quaternion_to_euler(pose_considered.orientation)[2] if in_rectangle(bottom_blue_area, self.parent.robot_pose): @@ -440,6 +456,7 @@ def fetch_data(self, pose_considered, values): } def get_pose_from_vlx(self, d1, d2, d3, vlx_0x30): + """Computing a pose relative to the wall considering vlx measured distances""" theta = np.arctan((d2 - d1) / (2 * self.vlx_face_y)) if vlx_0x30: x = (d3 + self.vlx_lat_y) * np.cos(theta) - self.vlx_lat_x * np.sin(theta) @@ -449,6 +466,7 @@ def get_pose_from_vlx(self, d1, d2, d3, vlx_0x30): return (x, y, theta) def get_vlx_from_pose(self, robot_pose_wall_relative, theta, vlx_0x30): + """Computing an estimation of vlx distances from a given pose and orientation""" d1 = ( (robot_pose_wall_relative[1]) / np.cos(theta) + self.vlx_face_y * np.tan(theta) @@ -484,6 +502,8 @@ def est_proj_wall( inv_angle=False, inv_lat=False, ): + """Computing an estimation of the projection of vlx wall hit from a given + position relative to the wall and an estimation of the vlx distance""" if case in [1, 2]: considered_angle = rectif_angle considered_vlx_face_y = self.vlx_face_y @@ -522,5 +542,6 @@ class VlxStamped: """ VlxStamped class, an instance contains stamp + vlx values """ def __init__(self, values, stamp): + """ Init VlxStamped """ self.values = copy.deepcopy(values) self.stamp = stamp From 77d478585bd92d99fcdaf1a4019c9d8ad90aed32 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 30 Mar 2021 16:23:06 +0200 Subject: [PATCH 60/64] Removing debug logs --- .../localisation/localisation_node.py | 3 -- .../localisation/vlx/vlx_readjustment.py | 44 ++----------------- 2 files changed, 4 insertions(+), 43 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 5b2f8555..9efc4285 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -18,7 +18,6 @@ from localisation.utils import euler_to_quaternion, is_simulation from nav_msgs.msg import Odometry from tf2_kdl import transform_to_kdl, do_transform_frame -from datetime import datetime class Localisation(rclpy.node.Node): @@ -124,7 +123,6 @@ def allies_subscription_callback(self, msg): ): # simulate marker delais (image analysis from assurancetourix) time.sleep(0.15) if self.vlx.continuous_sampling == 0: - self.get_logger().info(f"initial continuous_sampling == 0") self.create_and_send_tf( ally_marker.pose.position.x, ally_marker.pose.position.y, @@ -153,7 +151,6 @@ def create_and_send_tf(self, x, y, q, stamp): self._tf.transform.translation.z = float(0) self._tf.transform.rotation = q self.last_odom_update = self.get_clock().now().to_msg().sec - self.get_logger().info(f"publishing tf") self.tf_publisher_.publish(self._tf) def odom_callback(self, msg): diff --git a/src/modules/localisation/localisation/vlx/vlx_readjustment.py b/src/modules/localisation/localisation/vlx/vlx_readjustment.py index ae3dc68d..5ef1632f 100644 --- a/src/modules/localisation/localisation/vlx/vlx_readjustment.py +++ b/src/modules/localisation/localisation/vlx/vlx_readjustment.py @@ -17,8 +17,6 @@ else: from .sensors import Sensors -from datetime import datetime - bottom_blue_area = [0.0, 0.0, 0.9, 1.0] top_blue_area = [0.0, 1.0, 1.5, 2.0] @@ -79,7 +77,6 @@ def near_wall_routine(self, pose_stamped): ) < 0.5 ): - self.parent.get_logger().warn("near_wall_routine") self.try_to_readjust_with_vlx( pose_stamped.pose.position.x, pose_stamped.pose.position.y, @@ -102,7 +99,7 @@ def start_near_wall_routine(self, pose_stamped): self.start_continuous_sampling_thread(0.0, 2) self.near_wall_routine(pose_stamped) else: - self.parent.get_logger().info("near wall routine already running") + None def stop_near_wall_routine(self): """Stop the near wall routine by stopping the thread_continuous_sampling""" @@ -110,7 +107,7 @@ def stop_near_wall_routine(self): self.near_walls_last_check_stamp = None self.stop_continuous_sampling_thread() else: - self.parent.get_logger().info("near wall routine already stopped") + None def start_continuous_sampling_thread(self, sleep_time, continuous_samp): """Start the thread_continuous_sampling with an initial waiting time @@ -125,27 +122,21 @@ def start_continuous_sampling_thread(self, sleep_time, continuous_samp): ) self.thread_continuous_sampling.start() else: - self.parent.get_logger().info("vlx_thread thread is already running !") + None def stop_continuous_sampling_thread(self): """Safely stop the thread_continuous_sampling""" if self.thread_continuous_sampling != None: - self.parent.get_logger().info( - f"stopping continuous sampling:{self.continuous_sampling}" - ) self.continuous_sampling = 0 self.thread_continuous_sampling.join() self.thread_continuous_sampling = None else: - self.parent.get_logger().info("vlx_thread thread isn't running !") + None def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): """Thread function, wait sleep_time_before_sampling seconds before continuous sampling until continuous_sampling==0""" self.continuous_sampling = continuous_samp - self.parent.get_logger().info( - f"starting continuous sampling:{self.continuous_sampling}" - ) time.sleep(sleep_time_before_sampling) while self.continuous_sampling != 0: actual_stamp = ( @@ -163,7 +154,6 @@ def try_to_readjust_with_vlx(self, x, y, q, stamp): """Retrieve the nearest vlx values stamped from the given stamp then send those values to compute_data function that returns the corrected pose if not None then send this corrected pose to parent.create_and_send_tf""" - now = datetime.now() if self.continuous_sampling != 2: send_vision_tf = True else: @@ -189,19 +179,14 @@ def try_to_readjust_with_vlx(self, x, y, q, stamp): news = self.compute_data(pose, self.values_stamped_array[i].values) if news != None: send_vision_tf = False - self.parent.get_logger().warn("publish refined position") self.parent.create_and_send_tf(news[0], news[1], news[2], new_stamp) break if send_vision_tf: - self.parent.get_logger().warn("publish vision position") self.parent.create_and_send_tf(x, y, q, stamp) if self.continuous_sampling == 1: self.stop_continuous_sampling_thread() if self.near_walls_last_check_stamp != None: self.start_continuous_sampling_thread(0.0, 2) - later = datetime.now() - delay = (later - now).total_seconds() - self.parent.get_logger().info(f"delais: {delay}") def compute_data(self, pose_considered, vlx_values): """Fetching data for calculations detailed in README, computing new_pose @@ -209,10 +194,8 @@ def compute_data(self, pose_considered, vlx_values): data = self.fetch_data(pose_considered, vlx_values) if data == None: - self.parent.get_logger().info("data_None") return None else: - self.parent.get_logger().info("we can (hopefully) compute !") x, y, theta = self.get_pose_from_vlx( data["d"][0], data["d"][1], data["d"][2], data["vlx_0x30"] ) @@ -237,19 +220,6 @@ def compute_data(self, pose_considered, vlx_values): data["inv_lat"], ) - d = data["d"] - self.parent.get_logger().info( - f"error computed vlx - true vlx d1:{d1_est - d[0]}, d2:{d2_est- d[1]}, d3:{d3_est -d[2]}" - ) - self.parent.get_logger().info( - f"error computed pose - true pose x:{round(new_x-pose_considered.position.x, 4)}, \ - y:{round(new_y-pose_considered.position.y, 4)}, \ - theta:{round(new_theta-quaternion_to_euler(pose_considered.orientation)[2], 4)}" - ) - self.parent.get_logger().info(f"test d1:{d1_proj_est}") - self.parent.get_logger().info(f"test d2:{d2_proj_est}") - self.parent.get_logger().info(f"test d3:{d3_proj_est}") - if ( d1_est > 0 and d2_est > 0 @@ -259,7 +229,6 @@ def compute_data(self, pose_considered, vlx_values): and abs(new_theta - quaternion_to_euler(pose_considered.orientation)[2]) < 0.1 ): - self.parent.get_logger().warn(f"able to correct") return new_x, new_y, euler_to_quaternion(new_theta) else: return None @@ -270,7 +239,6 @@ def fetch_data(self, pose_considered, values): angle = quaternion_to_euler(pose_considered.orientation)[2] if in_rectangle(bottom_blue_area, self.parent.robot_pose): - self.parent.get_logger().info("bottom_blue_area") x_wall, y_wall = ( pose_considered.position.x * 1000, (pose_considered.position.y - self.sim_offset) * 1000, @@ -312,7 +280,6 @@ def fetch_data(self, pose_considered, values): inv_lat_condition = True if case in [1, 2] else False elif in_rectangle(top_blue_area, self.parent.robot_pose): - self.parent.get_logger().info("top_blue_area") x_wall, y_wall = ( pose_considered.position.x * 1000, 2000 - pose_considered.position.y * 1000, @@ -354,7 +321,6 @@ def fetch_data(self, pose_considered, values): inv_lat_condition = False elif in_rectangle(bottom_yellow_area, self.parent.robot_pose): - self.parent.get_logger().info("bottom_yellow_area") x_wall, y_wall = ( 3000 - (pose_considered.position.x - self.sim_offset) * 1000, (pose_considered.position.y - self.sim_offset) * 1000, @@ -396,7 +362,6 @@ def fetch_data(self, pose_considered, values): inv_lat_condition = True elif in_rectangle(top_yellow_area, self.parent.robot_pose): - self.parent.get_logger().info("top_yellow_area") x_wall, y_wall = ( 3000 - (pose_considered.position.x - self.sim_offset) * 1000, 2000 - pose_considered.position.y * 1000, @@ -438,7 +403,6 @@ def fetch_data(self, pose_considered, values): inv_lat_condition = True if case in [3, 4] else False else: - self.parent.get_logger().info("None") return None return { From 4a6576585151c3b79f1bfc5066ebb80eac3e3d49 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 31 Mar 2021 14:33:40 +0200 Subject: [PATCH 61/64] Hotfix teb_obstacles subscriber --- src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index 33f28f7e..2f73410e 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -32,7 +32,7 @@ def __init__(self): ) self.enemies_subscription_ = self.create_subscription( MarkerArray, - "/enemies_positions_markers", + "/ennemies_positions_markers", self.enemies_subscription_callback, 10, ) From 5b56c5611a9a3b709244e956b67ca5d78a5f429d Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 31 Mar 2021 22:59:03 +0200 Subject: [PATCH 62/64] Proofreading, first time 1k commits ! --- src/modules/localisation/README.md | 20 ++++----- .../localisation/localisation_node.py | 16 +++---- .../localisation/localisation/vlx/sensors.py | 8 ++-- .../localisation/vlx/sensors_sim.py | 14 +++--- .../localisation/vlx/vlx_readjustment.py | 44 +++++++++---------- 5 files changed, 51 insertions(+), 51 deletions(-) diff --git a/src/modules/localisation/README.md b/src/modules/localisation/README.md index 5d98f2d2..5ef6f989 100644 --- a/src/modules/localisation/README.md +++ b/src/modules/localisation/README.md @@ -7,7 +7,7 @@ - Compute the transformation to re-adjust odometry and publish it. -## Odometry re-adjustement method +## Odometry re-adjustment method In order to re-adjust the odometry, 2 things are used: - Vision from assurancetourix package, @@ -19,7 +19,7 @@ Assurancetourix is publishing aruco positions and orientation through /allies_po ### VL53L1X -There are 6 VL53L1X on the robot, by knowing there positions relative to the robot and by getting the distance they're returning, with a certain amount of mathematical considerations (described below), the position and orientation of the robot can re found. +There are 6 VL53L1X on the robot, by knowing there positions relative to the robot and by getting the distance they're returning, with an certain amount of mathematical consideration (described below), the position and orientation of the robot can re found. The limitation of this algorithm is that **we do have to know an estimation of the position and orientation of the robot to validate and refine the pose (position/orientation).** @@ -27,7 +27,7 @@ The limitation of this algorithm is that **we do have to know an estimation of t We're estimating the vision localization precision of about 1 centimeter average which isn't enough in some cases (particularly near walls). -So, in order to increase the precision, the information given by the vision needs to be refined by a vlx measure. But keep in mind that vision gives a pose STAMPED information which means the marker given by assurancetourix has a delay of a few milliseconds (~350ms average). The vlx measure takes (in simulation) about 60ms which gives a final difference between theses stamps of about 400ms. +So, in order to increase the precision, the information given by the vision needs to be refined by a vlx measure. But keep in mind that vision gives a pose STAMPED information which means the marker given by assurancetourix has a delay of a few milliseconds (~350ms average). The vlx measure takes (in simulation) about 60ms which give a final difference between theses stamps of about 400ms. If the robot is moving at something like 20cm/s, we do have to allow a difference of 8cm between the vision pose and vlx computed pose. Unrealistic, goblet have a 7cm diameter. @@ -41,27 +41,27 @@ With this nearest queue element, we have a maximum difference of stamps of 0.04m We now just have to compare the vlx computed pose and vision pose of the robot, if those are really near ( < 5cm, <0.1rad ) and the estimated projection of vlx on the walls are effectively on the right wall, we're considering the correction as valid. The valid transformation is sent to drive for it to re-adjust odometry. If the vlx computed pose isn't considered as valid, the vision pose is sent to drive. -After sending the correction transformation to drive, we have to restart the vlx sample thread with a starting delay of 0.65 seconds. +After sending the correction transformation to drive, we have to restart the vlx sample thread with a starting delay of 0.65 second. ### Near walls routine There's a specific behavior of this package near walls: in the odom callback, while detecting the robot position being near walls (<30cm from a wall), the node short circuit the behavior previously presented and adopt a new strategy. -This strategy consider an "infinite" sampling of the vlx sampling thread with no starting delay. Each 0.5 seconds odom callback, the node ask for an odometry re-adjustment considering the last robot pose that sent drive. +This strategy considers an "infinite" sampling of the vlx sampling thread with no starting delay. Each 0.5 seconds odom callback, the node ask for an odometry re-adjustment considering the last robot pose that sent drive. As previously, there's a comparison between stamps to get the nearest one of the queue that gives the sample to consider for calculation. When calculating the new pose from the vlx distance values, the thread is stopped avoid overloading of the localisation_node thread. -If the computed pose is considered as valid, it's sent to drive to re-adjust odometry. Else the pose is just thrown away and nothing is sent to re-adjust odometry. +If the computed pose is considered as valid, it's sent to drive to re-adjust odometry. Else the pose is just thrown away, and nothing is sent to re-adjust odometry. -Then, the vlx sampling thread is restarted with no delay because the node can continues trying to readjust odometry with vision callback with the strategy described in the previous paragraph. +Then, the vlx sampling thread is restarted with no delay because the node can continue trying to readjust odometry with vision callback with the strategy described in the previous paragraph. ### Avoiding concurrency problems -You've understood it, the localisation node contains several threads (odom callback, marker callback and vlx sample). In order to avoid concurrency problems (especially deadlocks, several thread starting, overloading etc.) several strategies are deployed: +You've understood it, the localisation node contains several threads (odom callback, marker callback and vlx sample). In order to avoid concurrency problems (especially deadlocks, several thread starting, overloading, etc.) several strategies are deployed: - **Deadlocks:** The resources shared by several threads can be writable only in one thread, in other threads, this resources is only readable. - **Several thread starting:** -Only one vlx sampling thread can be started, when asking for the creation of this thread, there's a check of this existing thread before creating it or raising that this thread is already running. Same thing for stopping this thread, if no vlx sampling thread is existing, the node raising that this thread doesn't exist. +Only one vlx sampling thread can be started, when asking for the creation of this thread, there's a check of this existing thread before creating it or raising that this thread is already running. Same thing for stopping this thread, if no vlx sampling thread exists, the node raises that this thread doesn't exist. - **Overloading:** Getting vlx values is a blocking call, so there's a need of a small non-blocking delay between each sample that allow the node doing its other works. @@ -143,7 +143,7 @@ All theses are just simple projection knowing ![formula](https://render.githubus ## In practice -Now that we've establish the theory, time to find all the cases. The table below summarize the different cases implemented in the algorithm, it also considers some tweaks in the algorithm (such as invert angle in some cases due to some symmetries etc.). +Now that we've established the theory, time to find all the cases. The table below summarizes the different cases implemented in the algorithm, it also considers some tweaks in the algorithm (such as invert angle in some cases due to some symmetries, etc.). ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}x) the distance between the robot and its sideways wall.\ ![formula](https://render.githubusercontent.com/render/math?math=\color{brown}y) the distance between the robot and its facing wall.\ diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 9efc4285..80c8b21e 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Robot localisation node.""" +""" Robot localisation node """ import math @@ -21,7 +21,7 @@ class Localisation(rclpy.node.Node): - """Robot localisation node.""" + """ Robot localisation node """ def __init__(self): """ Init Localisation node """ @@ -101,7 +101,7 @@ def fetchStartPosition(self): return None def update_transform(self): - """ Update and publish transform odom --> map """ + """ Update and publish odom --> map transform """ self._tf.header.stamp = self.get_clock().now().to_msg() self._tf.transform.translation.x = self.robot_pose.pose.position.x self._tf.transform.translation.y = self.robot_pose.pose.position.y @@ -110,9 +110,9 @@ def update_transform(self): self._tf_brodcaster.sendTransform(self._tf) def allies_subscription_callback(self, msg): - """Identity the robot marker in assurancetourix marker_array detection, + """ Identity the robot marker in assurancetourix marker_array detection, send the marker to vlx_readjustment in order to try to refine the position - at stamp given by the marker""" + at a stamp given by the marker """ for ally_marker in msg.markers: if ( ally_marker.text.lower() == self.robot @@ -144,7 +144,7 @@ def is_near_walls(self, pt): return pt.x < 0.3 or pt.y < 0.3 or pt.x > 2.7 or pt.y > 1.7 def create_and_send_tf(self, x, y, q, stamp): - """ create and send tf to drive for it to re-adjust odometry """ + """ Create and send tf to drive for it to re-adjust odometry """ self._tf.header.stamp = stamp self._tf.transform.translation.x = x self._tf.transform.translation.y = y @@ -154,9 +154,9 @@ def create_and_send_tf(self, x, y, q, stamp): self.tf_publisher_.publish(self._tf) def odom_callback(self, msg): - """Odom callback, compute the new pose of the robot relative to map, + """ Odom callback, computes the new pose of the robot relative to map, send this new pose on odom_map_relative topic and start vlx routine - if this pose is near walls""" + if this pose is near walls """ robot_tf = TransformStamped() robot_tf.transform.translation.x = msg.pose.pose.position.x robot_tf.transform.translation.y = msg.pose.pose.position.y diff --git a/src/modules/localisation/localisation/vlx/sensors.py b/src/modules/localisation/localisation/vlx/sensors.py index 9de820e5..4c626603 100644 --- a/src/modules/localisation/localisation/vlx/sensors.py +++ b/src/modules/localisation/localisation/vlx/sensors.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Sensors definition for localisation.""" +""" Sensors definition for localisation """ from VL53L1X import VL53L1X @@ -9,7 +9,7 @@ class Sensors: def __init__(self, i2c_bus=3, addrs=[]): - """Init sensors baseclass.""" + """ Init Sensors """ self._i2c_bus = i2c_bus self._vlx_array = [] for addr in addrs: @@ -24,14 +24,14 @@ def __init__(self, i2c_bus=3, addrs=[]): ) def get_distances(self): - """Fetch distances from VL53L1Xs.""" + """ Fetch distances from VL53L1X """ distances = {} for vlx in self._vlx_array: distances.update({vlx.i2c_address: vlx.get_distance()}) return distances def __del__(self): - """Destructor.""" + """ Destructor """ for vlx in self._vlx_array: vlx.stop_ranging() vlx.close() diff --git a/src/modules/localisation/localisation/vlx/sensors_sim.py b/src/modules/localisation/localisation/vlx/sensors_sim.py index c6062a8b..e0837c77 100644 --- a/src/modules/localisation/localisation/vlx/sensors_sim.py +++ b/src/modules/localisation/localisation/vlx/sensors_sim.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Simulated sensors definition for localisation.""" +""" Simulated sensors definition for localisation """ from os import environ @@ -11,7 +11,7 @@ class Sensors: def __init__(self, node, addrs=[]): - """Init sensors baseclass.""" + """ Init Sensors """ supervisor = node.robot + "_vlx_manager" environ["WEBOTS_ROBOT_NAME"] = supervisor self.node = VlxSupervisor(supervisor, args=None) @@ -26,7 +26,7 @@ def __init__(self, node, addrs=[]): print(f"No simulated VL53L1X sensor at address {addr} !") def get_distances(self): - """Fetch distances from VL53L1Xs.""" + """ Fetch distances from VL53L1X """ distances = {} for vlx, addr in zip(self._vlx_array, self._addrs): self.node.robot.step(1) @@ -34,20 +34,20 @@ def get_distances(self): return distances def get_time_stamp(self): - """get time stamp from webots""" + """ Get time stamp from Webots """ self.node.robot.step(0) t = self.node.robot.getTime() return Time(sec=int(t), nanosec=int((t % int(t)) * 1e9)) def __del__(self): - """Destructor.""" + """ Destructor """ for vlx in self._vlx_array: vlx.disable() class VlxSupervisor(WebotsNode): - """Vlx manager node.""" + """ Vlx manager node """ def __init__(self, supervisor, args=None): - """Init VlxSupervisor sim node.""" + """ Init VlxSupervisor """ super().__init__(supervisor, args) diff --git a/src/modules/localisation/localisation/vlx/vlx_readjustment.py b/src/modules/localisation/localisation/vlx/vlx_readjustment.py index 5ef1632f..763802d2 100644 --- a/src/modules/localisation/localisation/vlx/vlx_readjustment.py +++ b/src/modules/localisation/localisation/vlx/vlx_readjustment.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Vlx readjustment for localisation package""" +""" Vlx readjustment for localisation package """ import numpy as np import copy @@ -25,7 +25,7 @@ class VlxReadjustment: - """ Vlx readjustment class, contains routine and algorithm relative to vlx""" + """ Vlx readjustment class, contains routine and algorithm relative to vlx """ def __init__(self, parent_node): """ Init VlxReadjustment """ @@ -57,8 +57,8 @@ def __init__(self, parent_node): self.near_walls_last_check_stamp = None def near_wall_routine(self, pose_stamped): - """Near wall routine: launch try_to_readjust_with_vlx every 0.5 seconds - considering the pose_stamped given in argument""" + """ Near wall routine: launch try_to_readjust_with_vlx every 0.5 seconds + considering the pose_stamped given in argument """ actual_stamp = ( self.get_clock().now() if not is_simulation() @@ -90,8 +90,8 @@ def near_wall_routine(self, pose_stamped): ) def start_near_wall_routine(self, pose_stamped): - """Start the near wall routine by starting the thread_continuous_sampling - and launching a near_wall_routine immediatly after""" + """ Start the near wall routine by starting the thread_continuous_sampling + and launching a near_wall_routine immediately after """ if self.near_walls_last_check_stamp == None: self.near_walls_last_check_stamp = Time(sec=0, nanosec=0) if self.thread_continuous_sampling != None: @@ -102,7 +102,7 @@ def start_near_wall_routine(self, pose_stamped): None def stop_near_wall_routine(self): - """Stop the near wall routine by stopping the thread_continuous_sampling""" + """ Stop the near wall routine by stopping the thread_continuous_sampling """ if self.near_walls_last_check_stamp != None: self.near_walls_last_check_stamp = None self.stop_continuous_sampling_thread() @@ -110,8 +110,8 @@ def stop_near_wall_routine(self): None def start_continuous_sampling_thread(self, sleep_time, continuous_samp): - """Start the thread_continuous_sampling with an initial waiting time - and mode given by continuous_samp: 0->stopped, 1->nominal, 2->near_wall""" + """ Start the thread_continuous_sampling with an initial waiting time + and mode given by continuous_samp: 0->stopped, 1->nominal, 2->near_wall """ if self.thread_continuous_sampling == None: self.thread_continuous_sampling = threading.Thread( target=self.continuous_vlx_sampling, @@ -125,7 +125,7 @@ def start_continuous_sampling_thread(self, sleep_time, continuous_samp): None def stop_continuous_sampling_thread(self): - """Safely stop the thread_continuous_sampling""" + """ Safely stop the thread_continuous_sampling """ if self.thread_continuous_sampling != None: self.continuous_sampling = 0 self.thread_continuous_sampling.join() @@ -134,8 +134,8 @@ def stop_continuous_sampling_thread(self): None def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): - """Thread function, wait sleep_time_before_sampling seconds before - continuous sampling until continuous_sampling==0""" + """ Thread function, wait sleep_time_before_sampling seconds before + continuous sampling until continuous_sampling==0 """ self.continuous_sampling = continuous_samp time.sleep(sleep_time_before_sampling) while self.continuous_sampling != 0: @@ -151,9 +151,9 @@ def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): time.sleep(0.02) def try_to_readjust_with_vlx(self, x, y, q, stamp): - """Retrieve the nearest vlx values stamped from the given stamp then + """ Retrieve the nearest vlx values stamped from the given stamp then send those values to compute_data function that returns the corrected - pose if not None then send this corrected pose to parent.create_and_send_tf""" + pose if not None then send this corrected pose to parent.create_and_send_tf """ if self.continuous_sampling != 2: send_vision_tf = True else: @@ -189,8 +189,8 @@ def try_to_readjust_with_vlx(self, x, y, q, stamp): self.start_continuous_sampling_thread(0.0, 2) def compute_data(self, pose_considered, vlx_values): - """Fetching data for calculations detailed in README, computing new_pose - from these data, estimate if this pose is valid before returning it""" + """ Fetching data for calculations detailed in README, computing new_pose + from these data, estimate if this pose is valid before returning it """ data = self.fetch_data(pose_considered, vlx_values) if data == None: @@ -234,8 +234,8 @@ def compute_data(self, pose_considered, vlx_values): return None def fetch_data(self, pose_considered, values): - """Returning data as described in the table at the end of README - + all other data that are needed for the pose calculations""" + """ Returning data as described in the table at the end of README + + all other data that are needed for the pose calculations """ angle = quaternion_to_euler(pose_considered.orientation)[2] if in_rectangle(bottom_blue_area, self.parent.robot_pose): @@ -420,7 +420,7 @@ def fetch_data(self, pose_considered, values): } def get_pose_from_vlx(self, d1, d2, d3, vlx_0x30): - """Computing a pose relative to the wall considering vlx measured distances""" + """ Computing a pose relative to the wall considering vlx measured distances """ theta = np.arctan((d2 - d1) / (2 * self.vlx_face_y)) if vlx_0x30: x = (d3 + self.vlx_lat_y) * np.cos(theta) - self.vlx_lat_x * np.sin(theta) @@ -430,7 +430,7 @@ def get_pose_from_vlx(self, d1, d2, d3, vlx_0x30): return (x, y, theta) def get_vlx_from_pose(self, robot_pose_wall_relative, theta, vlx_0x30): - """Computing an estimation of vlx distances from a given pose and orientation""" + """ Computing an estimation of vlx distances from a given pose and orientation """ d1 = ( (robot_pose_wall_relative[1]) / np.cos(theta) + self.vlx_face_y * np.tan(theta) @@ -466,8 +466,8 @@ def est_proj_wall( inv_angle=False, inv_lat=False, ): - """Computing an estimation of the projection of vlx wall hit from a given - position relative to the wall and an estimation of the vlx distance""" + """ Computing an estimation of the projection of vlx wall hit from a given + position relative to the wall and an estimation of the vlx distance """ if case in [1, 2]: considered_angle = rectif_angle considered_vlx_face_y = self.vlx_face_y From 702324b1b1949270aae546a4ea3c0aa831f9c18c Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 3 Apr 2021 13:59:04 +0200 Subject: [PATCH 63/64] black localisation package --- .../localisation/localisation_node.py | 8 ++--- .../localisation/vlx/sensors_sim.py | 2 ++ .../localisation/vlx/vlx_readjustment.py | 32 +++++++++---------- 3 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 80c8b21e..526a0d68 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -110,9 +110,9 @@ def update_transform(self): self._tf_brodcaster.sendTransform(self._tf) def allies_subscription_callback(self, msg): - """ Identity the robot marker in assurancetourix marker_array detection, + """Identity the robot marker in assurancetourix marker_array detection, send the marker to vlx_readjustment in order to try to refine the position - at a stamp given by the marker """ + at a stamp given by the marker""" for ally_marker in msg.markers: if ( ally_marker.text.lower() == self.robot @@ -154,9 +154,9 @@ def create_and_send_tf(self, x, y, q, stamp): self.tf_publisher_.publish(self._tf) def odom_callback(self, msg): - """ Odom callback, computes the new pose of the robot relative to map, + """Odom callback, computes the new pose of the robot relative to map, send this new pose on odom_map_relative topic and start vlx routine - if this pose is near walls """ + if this pose is near walls""" robot_tf = TransformStamped() robot_tf.transform.translation.x = msg.pose.pose.position.x robot_tf.transform.translation.y = msg.pose.pose.position.y diff --git a/src/modules/localisation/localisation/vlx/sensors_sim.py b/src/modules/localisation/localisation/vlx/sensors_sim.py index e0837c77..e01e2140 100644 --- a/src/modules/localisation/localisation/vlx/sensors_sim.py +++ b/src/modules/localisation/localisation/vlx/sensors_sim.py @@ -10,6 +10,8 @@ class Sensors: + """ Sensors class """ + def __init__(self, node, addrs=[]): """ Init Sensors """ supervisor = node.robot + "_vlx_manager" diff --git a/src/modules/localisation/localisation/vlx/vlx_readjustment.py b/src/modules/localisation/localisation/vlx/vlx_readjustment.py index 763802d2..4c4e743f 100644 --- a/src/modules/localisation/localisation/vlx/vlx_readjustment.py +++ b/src/modules/localisation/localisation/vlx/vlx_readjustment.py @@ -57,8 +57,8 @@ def __init__(self, parent_node): self.near_walls_last_check_stamp = None def near_wall_routine(self, pose_stamped): - """ Near wall routine: launch try_to_readjust_with_vlx every 0.5 seconds - considering the pose_stamped given in argument """ + """Near wall routine: launch try_to_readjust_with_vlx every 0.5 seconds + considering the pose_stamped given in argument""" actual_stamp = ( self.get_clock().now() if not is_simulation() @@ -90,8 +90,8 @@ def near_wall_routine(self, pose_stamped): ) def start_near_wall_routine(self, pose_stamped): - """ Start the near wall routine by starting the thread_continuous_sampling - and launching a near_wall_routine immediately after """ + """Start the near wall routine by starting the thread_continuous_sampling + and launching a near_wall_routine immediately after""" if self.near_walls_last_check_stamp == None: self.near_walls_last_check_stamp = Time(sec=0, nanosec=0) if self.thread_continuous_sampling != None: @@ -110,8 +110,8 @@ def stop_near_wall_routine(self): None def start_continuous_sampling_thread(self, sleep_time, continuous_samp): - """ Start the thread_continuous_sampling with an initial waiting time - and mode given by continuous_samp: 0->stopped, 1->nominal, 2->near_wall """ + """Start the thread_continuous_sampling with an initial waiting time + and mode given by continuous_samp: 0->stopped, 1->nominal, 2->near_wall""" if self.thread_continuous_sampling == None: self.thread_continuous_sampling = threading.Thread( target=self.continuous_vlx_sampling, @@ -134,8 +134,8 @@ def stop_continuous_sampling_thread(self): None def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): - """ Thread function, wait sleep_time_before_sampling seconds before - continuous sampling until continuous_sampling==0 """ + """Thread function, wait sleep_time_before_sampling seconds before + continuous sampling until continuous_sampling==0""" self.continuous_sampling = continuous_samp time.sleep(sleep_time_before_sampling) while self.continuous_sampling != 0: @@ -151,9 +151,9 @@ def continuous_vlx_sampling(self, sleep_time_before_sampling, continuous_samp): time.sleep(0.02) def try_to_readjust_with_vlx(self, x, y, q, stamp): - """ Retrieve the nearest vlx values stamped from the given stamp then + """Retrieve the nearest vlx values stamped from the given stamp then send those values to compute_data function that returns the corrected - pose if not None then send this corrected pose to parent.create_and_send_tf """ + pose if not None then send this corrected pose to parent.create_and_send_tf""" if self.continuous_sampling != 2: send_vision_tf = True else: @@ -189,8 +189,8 @@ def try_to_readjust_with_vlx(self, x, y, q, stamp): self.start_continuous_sampling_thread(0.0, 2) def compute_data(self, pose_considered, vlx_values): - """ Fetching data for calculations detailed in README, computing new_pose - from these data, estimate if this pose is valid before returning it """ + """Fetching data for calculations detailed in README, computing new_pose + from these data, estimate if this pose is valid before returning it""" data = self.fetch_data(pose_considered, vlx_values) if data == None: @@ -234,8 +234,8 @@ def compute_data(self, pose_considered, vlx_values): return None def fetch_data(self, pose_considered, values): - """ Returning data as described in the table at the end of README - + all other data that are needed for the pose calculations """ + """Returning data as described in the table at the end of README + + all other data that are needed for the pose calculations""" angle = quaternion_to_euler(pose_considered.orientation)[2] if in_rectangle(bottom_blue_area, self.parent.robot_pose): @@ -466,8 +466,8 @@ def est_proj_wall( inv_angle=False, inv_lat=False, ): - """ Computing an estimation of the projection of vlx wall hit from a given - position relative to the wall and an estimation of the vlx distance """ + """Computing an estimation of the projection of vlx wall hit from a given + position relative to the wall and an estimation of the vlx distance""" if case in [1, 2]: considered_angle = rectif_angle considered_vlx_face_y = self.vlx_face_y From ce353e55adcc7e694a37f6a9ba88fda413ee9b3b Mon Sep 17 00:00:00 2001 From: PhileasL Date: Sat, 3 Apr 2021 14:55:34 +0200 Subject: [PATCH 64/64] updating obelix.in.yml --- src/obelix/param/obelix.in.yml | 43 ++++++++++++++++++++++++++++------ 1 file changed, 36 insertions(+), 7 deletions(-) diff --git a/src/obelix/param/obelix.in.yml b/src/obelix/param/obelix.in.yml index 7798b9d1..0ffbe37a 100644 --- a/src/obelix/param/obelix.in.yml +++ b/src/obelix/param/obelix.in.yml @@ -1,5 +1,6 @@ !Defaults footprint: "[[-0.090, -0.175], [-0.090, 0.175], [0.090, 0.175], [0.090, -0.175]]" +use_sim_time: False max_steps_frequency: 10000 speedramp_resolution: 128 microsteps: 16 @@ -7,17 +8,20 @@ steps_per_turn: 400 wheels_separation: 0.282 wheels_radius: 0.061 min_turning_radius: 0.0 -max_vel_x: 0.4 -max_vel_x_backwards: 0.4 -max_vel_theta: 1.0 +max_vel_x_nominal: 2.0 +max_vel_x_accurate: 0.15 +max_vel_theta: 3.1 +max_vel_theta_accurate: 0.5 acc_lim_x: 0.5 -acc_lim_theta: 0.5 +acc_lim_theta: 2.5 robotbase_length: 0.18 robotbase_width: 0.315 --- obelix: drive_node: ros__parameters: + use_sim_time: !Var use_sim_time + wheels: separation: !Var wheels_separation radius: !Var wheels_radius @@ -45,28 +49,53 @@ obelix: strategy_mode: NORMAL + localisation_node: + ros__parameters: + vlx_lat_x: 80.0 + vlx_lat_y: 152.5 + vlx_face_x: 70.0 + vlx_face_y: 130.0 + + lcd_driver: ros__parameters: + use_sim_time: !Var use_sim_time + i2c_bus: 6 i2c_addr: 0x27 banner: "Obelix v0.8.3" + tree: + ros__parameters: + use_sim_time: !Var use_sim_time + controller_server: ros__parameters: - DynamicFollowPath: + NominalController: footprint_model.vertices: !Var footprint # Robot min_turning_radius: !Var min_turning_radius - max_vel_x: !Var max_vel_x - max_vel_x_backwards: !Var max_vel_x_backwards + max_vel_x: !Var max_vel_x_nominal + max_vel_x_backwards: !Var max_vel_x_nominal max_vel_theta: !Var max_vel_theta acc_lim_x: !Var acc_lim_x acc_lim_theta: !Var acc_lim_theta + AccurateController: + footprint_model.vertices: !Var footprint + + # Robot + min_turning_radius: !Var min_turning_radius + max_vel_x: !Var max_vel_x_accurate + max_vel_x_backwards: !Var max_vel_x_accurate + max_vel_theta: !Var max_vel_theta_accurate + acc_lim_x: !Var acc_lim_x + acc_lim_theta: !Var acc_lim_theta + local_costmap: local_costmap: