From b29e3258838875981f98248d5cfbc9e71e406ab8 Mon Sep 17 00:00:00 2001 From: Ralf Date: Wed, 27 Nov 2024 16:17:12 +0100 Subject: [PATCH 01/37] added first draft of radar data visualization --- code/perception/src/radar_node.py | 115 +++++++++++++++++++++++++++++- 1 file changed, 113 insertions(+), 2 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 4ddf5b4d..247a2df4 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -2,10 +2,12 @@ import rospy import ros_numpy import numpy as np -from std_msgs.msg import String -from sensor_msgs.msg import PointCloud2 +from std_msgs.msg import String, Header +from sensor_msgs.msg import PointCloud2, PointField from sklearn.cluster import DBSCAN import json +from sensor_msgs import point_cloud2 +import struct class RadarNode: @@ -25,6 +27,23 @@ def callback(self, data): clustered_points_json = json.dumps(clustered_points) self.dist_array_radar_publisher.publish(clustered_points_json) + # output array [x, y, z, distance] + dataarray = pointcloud2_to_array(data) + + # input array [x, y, z, distance], max_distance, output: filtered data + # dataarray = filter_data(dataarray, 10) + + # input array [x, y, z, distance], output: dict clustered + clustered_data = cluster_data(dataarray) + + # input array [x, y, z, distance], clustered labels + cloud = create_pointcloud2(dataarray, clustered_data.labels_) + + self.visualization_radar_publisher.publish(cloud) + + cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray) + self.cluster_info_radar_publisher.publish(cluster_info) + def listener(self): """Initializes the node and its publishers.""" rospy.init_node("radar_node") @@ -35,6 +54,20 @@ def listener(self): String, queue_size=10, ) + self.visualization_radar_publisher = rospy.Publisher( + rospy.get_param( + "~image_distance_topic", "/paf/hero/Radar/Visualization" + ), + PointCloud2, + queue_size=10, + ) + self.cluster_info_radar_publisher = rospy.Publisher( + rospy.get_param( + "~image_distance_topic", "/paf/hero/Radar/ClusterInfo" + ), + String, + queue_size=10, + ) rospy.Subscriber( rospy.get_param("~source_topic", "/carla/hero/RADAR"), PointCloud2, @@ -109,6 +142,84 @@ def cluster_radar_data_from_pointcloud( return clustered_points +# filters radar data in distance, y, z direction +def filter_data(data, max_distance): + + filtered_data = data[data[:, 3] < max_distance] + filtered_data = filtered_data[ + (filtered_data[:, 1] >= -1) + & (filtered_data[:, 1] <= 1) + & (filtered_data[:, 2] <= 1.3) + & (filtered_data[:, 2] >= -0.7) + ] + return filtered_data + + +# clusters data with DBSCAN +def cluster_data(filtered_data, eps=0.2, min_samples=1): + + if len(filtered_data) == 0: + return {} + coordinates = filtered_data[:, :2] + clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) + return clustering + + +# generates random color for cluster +def generate_color_map(num_clusters): + np.random.seed(42) + colors = np.random.randint(0, 255, size=(num_clusters, 3)) + return colors + + +# creates pointcloud2 for publishing clustered radar data +def create_pointcloud2(clustered_points, cluster_labels): + header = Header() + header.stamp = rospy.Time.now() + header.frame_id = "hero/RADAR" + + points = [] + unique_labels = np.unique(cluster_labels) + colors = generate_color_map(len(unique_labels)) + + for i, point in enumerate(clustered_points): + x, y, z, _ = point + label = cluster_labels[i] + + if label == -1: + r, g, b = 128, 128, 128 + else: + r, g, b = colors[label] + + rgb = struct.unpack('f', struct.pack('I', (r << 16) | (g << 8) | b))[0] + points.append([x, y, z, rgb]) + + fields = [ + PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1), + PointField('rgb', 12, PointField.FLOAT32, 1), + ] + + return point_cloud2.create_cloud(header, fields, points) + + +# generates string with label-id and cluster size +def generate_cluster_labels_and_colors(clusters, data): + cluster_info = [] + + for label in set(clusters.labels_): + cluster_points = data[clusters.labels_ == label] + cluster_size = len(cluster_points) + if label != -1: + cluster_info.append({ + "label": int(label), + "points_count": cluster_size + }) + + return json.dumps(cluster_info) + + if __name__ == "__main__": radar_node = RadarNode() radar_node.listener() From 614869034793471e0a4c396da3eedce809cb7475 Mon Sep 17 00:00:00 2001 From: Ralf Date: Thu, 28 Nov 2024 16:15:49 +0100 Subject: [PATCH 02/37] Added velocity as clustering criteria --- code/perception/src/radar_node.py | 32 +++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 247a2df4..08dede8e 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -5,6 +5,7 @@ from std_msgs.msg import String, Header from sensor_msgs.msg import PointCloud2, PointField from sklearn.cluster import DBSCAN +from sklearn.preprocessing import StandardScaler import json from sensor_msgs import point_cloud2 import struct @@ -23,9 +24,9 @@ def callback(self, data): Args: data: Point2Cloud message containing radar data """ - clustered_points = cluster_radar_data_from_pointcloud(data, 10) - clustered_points_json = json.dumps(clustered_points) - self.dist_array_radar_publisher.publish(clustered_points_json) + # clustered_points = cluster_radar_data_from_pointcloud(data, 10) + # clustered_points_json = json.dumps(clustered_points) + # self.dist_array_radar_publisher.publish(clustered_points_json) # output array [x, y, z, distance] dataarray = pointcloud2_to_array(data) @@ -91,11 +92,14 @@ def pointcloud2_to_array(pointcloud_msg): [x, y, z, distance], where "distance" is the distance from the origin. """ cloud_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud_msg) - distances = np.sqrt( - cloud_array["x"] ** 2 + cloud_array["y"] ** 2 + cloud_array["z"] ** 2 - ) + # distances = np.sqrt( + # cloud_array["x"] ** 2 + cloud_array["y"] ** 2 + cloud_array["z"] ** 2 + # ) + # return np.column_stack( + # (cloud_array["x"], cloud_array["y"], cloud_array["z"], distances) + # ) return np.column_stack( - (cloud_array["x"], cloud_array["y"], cloud_array["z"], distances) + (cloud_array["x"], cloud_array["y"], cloud_array["z"], cloud_array["Velocity"]) ) @@ -160,8 +164,15 @@ def cluster_data(filtered_data, eps=0.2, min_samples=1): if len(filtered_data) == 0: return {} - coordinates = filtered_data[:, :2] - clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) + # coordinates = filtered_data[:, :2] + # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) + + # worse than without scaling + # scaler = StandardScaler() + # data_scaled = scaler.fit_transform(filtered_data) + # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled) + + clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(filtered_data) return clustering @@ -183,7 +194,8 @@ def create_pointcloud2(clustered_points, cluster_labels): colors = generate_color_map(len(unique_labels)) for i, point in enumerate(clustered_points): - x, y, z, _ = point + # x, y, z, _ = point + x, y, z, v = point label = cluster_labels[i] if label == -1: From f5bbe472694e29c32a5cc882cca3ac1b7c9f0c67 Mon Sep 17 00:00:00 2001 From: Ralf Date: Wed, 4 Dec 2024 16:17:28 +0100 Subject: [PATCH 03/37] started implementation of bounding boxes for detected radar clusters --- code/perception/src/radar_node.py | 176 ++++++++++++++++++++++++++++-- 1 file changed, 165 insertions(+), 11 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 08dede8e..39ed7ec0 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -8,6 +8,9 @@ from sklearn.preprocessing import StandardScaler import json from sensor_msgs import point_cloud2 +from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import Point + import struct @@ -37,14 +40,26 @@ def callback(self, data): # input array [x, y, z, distance], output: dict clustered clustered_data = cluster_data(dataarray) + # transformed_data = transform_data_to_2d(dataarray) + # input array [x, y, z, distance], clustered labels cloud = create_pointcloud2(dataarray, clustered_data.labels_) - self.visualization_radar_publisher.publish(cloud) cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray) self.cluster_info_radar_publisher.publish(cluster_info) + points_with_labels = np.hstack((dataarray, clustered_data.labels_.reshape(-1, 1))) + bounding_boxes = generate_bounding_boxes(points_with_labels) + + marker_array = MarkerArray() + for label, bbox in bounding_boxes: + if label != -1: + marker = create_bounding_box_marker(label, bbox) + marker_array.markers.append(marker) + + self.marker_visualization_radar_publisher.publish(marker_array) + def listener(self): """Initializes the node and its publishers.""" rospy.init_node("radar_node") @@ -62,6 +77,13 @@ def listener(self): PointCloud2, queue_size=10, ) + self.marker_visualization_radar_publisher = rospy.Publisher( + rospy.get_param( + "~image_distance_topic", "/paf/hero/Radar/Marker" + ), + MarkerArray, + queue_size=10, + ) self.cluster_info_radar_publisher = rospy.Publisher( rospy.get_param( "~image_distance_topic", "/paf/hero/Radar/ClusterInfo" @@ -149,18 +171,20 @@ def cluster_radar_data_from_pointcloud( # filters radar data in distance, y, z direction def filter_data(data, max_distance): - filtered_data = data[data[:, 3] < max_distance] + # filtered_data = data[data[:, 3] < max_distance] + filtered_data = data filtered_data = filtered_data[ - (filtered_data[:, 1] >= -1) - & (filtered_data[:, 1] <= 1) - & (filtered_data[:, 2] <= 1.3) - & (filtered_data[:, 2] >= -0.7) + # (filtered_data[:, 1] >= -1) + # & (filtered_data[:, 1] <= 1) + # & (filtered_data[:, 2] <= 1.3) + (filtered_data[:, 2] <= 1.3) + & (filtered_data[:, 2] >= -0.6) # -0.7 ] return filtered_data # clusters data with DBSCAN -def cluster_data(filtered_data, eps=0.2, min_samples=1): +def cluster_data(filtered_data, eps=0.8, min_samples=5): if len(filtered_data) == 0: return {} @@ -168,11 +192,11 @@ def cluster_data(filtered_data, eps=0.2, min_samples=1): # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) # worse than without scaling - # scaler = StandardScaler() - # data_scaled = scaler.fit_transform(filtered_data) - # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled) + scaler = StandardScaler() + data_scaled = scaler.fit_transform(filtered_data) + clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled) - clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(filtered_data) + # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(filtered_data) return clustering @@ -216,6 +240,136 @@ def create_pointcloud2(clustered_points, cluster_labels): return point_cloud2.create_cloud(header, fields, points) +def transform_data_to_2d(clustered_data): + + transformed_points = clustered_data + transformed_points[:, 0] = clustered_data[:, 0] + transformed_points[:, 1] = clustered_data[:, 1] + transformed_points[:, 2] = 0 + transformed_points[:, 3] = clustered_data[:, 3] + + return transformed_points + + +def calculate_aabb(cluster_points): + """_summary_ + + Args: + cluster_points (_type_): _description_ + + Returns: + _type_: _description_ + """ + + # for 2d (top-down) boxes + # x_min = np.min(cluster_points[:, 0]) + # x_max = np.max(cluster_points[:, 0]) + # y_min = np.min(cluster_points[:, 1]) + # y_max = np.max(cluster_points[:, 1]) + + # return x_min, x_max, y_min, y_max + + # for 3d boxes + x_min = np.min(cluster_points[:, 0]) + x_max = np.max(cluster_points[:, 0]) + y_min = np.min(cluster_points[:, 1]) + y_max = np.max(cluster_points[:, 1]) + z_min = np.min(cluster_points[:, 2]) + z_max = np.max(cluster_points[:, 2]) + rospy.loginfo(f"Bounding box for label: X({x_min}, {x_max}), Y({y_min}, {y_max}), Z({z_min}, {z_max})") + return x_min, x_max, y_min, y_max, z_min, z_max + + +def generate_bounding_boxes(points_with_labels): + """_summary_ + + Args: + points_with_labels (_type_): _description_ + + Returns: + _type_: _description_ + """ + bounding_boxes = [] + unique_labels = np.unique(points_with_labels[:, -1]) + for label in unique_labels: + if label == -1: + continue + cluster_points = points_with_labels[points_with_labels[:, -1] == label, :3] + bbox = calculate_aabb(cluster_points) + bounding_boxes.append((label, bbox)) + return bounding_boxes + + +def create_bounding_box_marker(label, bbox): + """_summary_ + + Returns: + _type_: _description_ + """ + # for 2d (top-down) boxes + # x_min, x_max, y_min, y_max = bbox + + # for 3d boxes + x_min, x_max, y_min, y_max, z_min, z_max = bbox + + marker = Marker() + marker.header.frame_id = "hero/RADAR" + marker.id = int(label) + # marker.type = Marker.LINE_STRIP + marker.type = Marker.LINE_LIST + marker.action = Marker.ADD + marker.scale.x = 0.1 + marker.color.r = 1.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + + # for 2d (top-down) boxes + # points = [ + # Point(x_min, y_min, 0), + # Point(x_max, y_min, 0), + # Point(x_max, y_max, 0), + # Point(x_min, y_max, 0), + # Point(x_min, y_min, 0), + # ] + # marker.points = points + + # for 3d boxes + points = [ + Point(x_min, y_min, z_min), # Ecke 0 + Point(x_max, y_min, z_min), # Ecke 1 + Point(x_max, y_max, z_min), # Ecke 2 + Point(x_min, y_max, z_min), # Ecke 3 + Point(x_min, y_min, z_max), # Ecke 4 + Point(x_max, y_min, z_max), # Ecke 5 + Point(x_max, y_max, z_max), # Ecke 6 + Point(x_min, y_max, z_max), # Ecke 7 + + # Point(x_min, y_min, z_min), # Verbinde z_min zu z_max + # Point(x_min, y_min, z_max), + + # Point(x_max, y_min, z_min), + # Point(x_max, y_min, z_max), + + # Point(x_max, y_max, z_min), + # Point(x_max, y_max, z_max), + + # Point(x_min, y_max, z_min), + # Point(x_min, y_max, z_max), + ] + # marker.points = points + lines = [ + (0, 1), (1, 2), (2, 3), (3, 0), # Boden + (4, 5), (5, 6), (6, 7), (7, 4), # Deckel + (0, 4), (1, 5), (2, 6), (3, 7), # Vertikale Kanten + ] + for start, end in lines: + marker.points.append(points[start]) + marker.points.append(points[end]) + + return marker + + # generates string with label-id and cluster size def generate_cluster_labels_and_colors(clusters, data): cluster_info = [] From 07794547ed31d4af0743270c7c1480c8d0a49642 Mon Sep 17 00:00:00 2001 From: Ralf Date: Mon, 9 Dec 2024 17:17:27 +0100 Subject: [PATCH 04/37] added deletion of old markers --- code/perception/src/radar_node.py | 93 +++++++++++++++++++++++++++++-- code/requirements.txt | 3 +- 2 files changed, 89 insertions(+), 7 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 39ed7ec0..ddff6511 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -46,20 +46,26 @@ def callback(self, data): cloud = create_pointcloud2(dataarray, clustered_data.labels_) self.visualization_radar_publisher.publish(cloud) - cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray) - self.cluster_info_radar_publisher.publish(cluster_info) - points_with_labels = np.hstack((dataarray, clustered_data.labels_.reshape(-1, 1))) bounding_boxes = generate_bounding_boxes(points_with_labels) marker_array = MarkerArray() + # marker_array = clear_old_markers(marker_array) for label, bbox in bounding_boxes: if label != -1: marker = create_bounding_box_marker(label, bbox) marker_array.markers.append(marker) + # min_marker, max_marker = create_min_max_markers(label, bbox) + # marker_array.markers.append(min_marker) + # marker_array.markers.append(max_marker) + + marker_array = clear_old_markers(marker_array, max_id=len(bounding_boxes) - 1) self.marker_visualization_radar_publisher.publish(marker_array) + cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray, marker_array, bounding_boxes) + self.cluster_info_radar_publisher.publish(cluster_info) + def listener(self): """Initializes the node and its publishers.""" rospy.init_node("radar_node") @@ -366,12 +372,85 @@ def create_bounding_box_marker(label, bbox): for start, end in lines: marker.points.append(points[start]) marker.points.append(points[end]) - + return marker +def create_min_max_markers(label, bbox, frame_id="hero/RADAR", min_color=(0.0, 1.0, 0.0, 1.0), max_color=(1.0, 0.0, 0.0, 1.0)): + """ + Erstellt RViz-Marker für die Min- und Max-Punkte einer Bounding Box. + + Args: + label (int): Die ID des Clusters (wird als Marker-ID genutzt). + bbox (tuple): Min- und Max-Werte der Bounding Box (x_min, x_max, y_min, y_max, z_min, z_max). + frame_id (str): Frame ID, in dem die Marker gezeichnet werden. + min_color (tuple): RGBA-Farbwerte für den Min-Punkt-Marker. + max_color (tuple): RGBA-Farbwerte für den Max-Punkt-Marker. + + Returns: + tuple: Ein Paar von Markern (min_marker, max_marker). + """ + x_min, x_max, y_min, y_max, z_min, z_max = bbox + + # marker = Marker() + # marker.header.frame_id = "hero/RADAR" + # marker.id = int(label) + # # marker.type = Marker.LINE_STRIP + # marker.type = Marker.LINE_LIST + # marker.action = Marker.ADD + # marker.scale.x = 0.1 + # marker.color.r = 1.0 + # marker.color.g = 1.0 + # marker.color.b = 0.0 + # marker.color.a = 1.0 + + # Min-Punkt-Marker + min_marker = Marker() + min_marker.header.frame_id = frame_id + min_marker.id = int(label * 10) # ID für Min-Punkt + min_marker.type = Marker.SPHERE + min_marker.action = Marker.ADD + min_marker.scale.x = 0.2 # Größe des Punktes + min_marker.scale.y = 0.2 + min_marker.scale.z = 0.2 + min_marker.color.r = min_color[0] + min_marker.color.g = min_color[1] + min_marker.color.b = min_color[2] + min_marker.color.a = min_color[3] + min_marker.pose.position.x = x_min + min_marker.pose.position.y = y_min + min_marker.pose.position.z = z_min + + # Max-Punkt-Marker + max_marker = Marker() + max_marker.header.frame_id = frame_id + max_marker.id = int(label * 10 + 1) # ID für Max-Punkt + max_marker.type = Marker.SPHERE + max_marker.action = Marker.ADD + max_marker.scale.x = 0.2 + max_marker.scale.y = 0.2 + max_marker.scale.z = 0.2 + max_marker.color.r = max_color[0] + max_marker.color.g = max_color[1] + max_marker.color.b = max_color[2] + max_marker.color.a = max_color[3] + max_marker.pose.position.x = x_max + max_marker.pose.position.y = y_max + max_marker.pose.position.z = z_max + + return min_marker, max_marker + + +def clear_old_markers(marker_array, max_id): + """Löscht alte Marker aus dem MarkerArray.""" + for marker in marker_array.markers: + if marker.id >= max_id: + marker.action = Marker.DELETE + return marker_array + + # generates string with label-id and cluster size -def generate_cluster_labels_and_colors(clusters, data): +def generate_cluster_labels_and_colors(clusters, data, marker_array, bounding_boxes): cluster_info = [] for label in set(clusters.labels_): @@ -380,7 +459,9 @@ def generate_cluster_labels_and_colors(clusters, data): if label != -1: cluster_info.append({ "label": int(label), - "points_count": cluster_size + "points_count": cluster_size, + "Anzahl marker": len(marker_array.markers), + "Anzahl Boundingboxen": len(bounding_boxes) }) return json.dumps(cluster_info) diff --git a/code/requirements.txt b/code/requirements.txt index 55de87f0..cbe57476 100644 --- a/code/requirements.txt +++ b/code/requirements.txt @@ -16,4 +16,5 @@ numpy==1.23.5 ultralytics==8.1.11 scikit-learn>=0.18 pandas==2.0.3 -debugpy==1.8.7 \ No newline at end of file +debugpy==1.8.7 +pyopenssl==24.3.0 \ No newline at end of file From c46c93428e9ba3988c2b5413972ce422687a4fad Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 11 Dec 2024 14:39:38 +0100 Subject: [PATCH 05/37] - Added clustering functionality for LiDAR data processing. - Refactored the existing node code by modularizing it into functions for better readability and maintainability. --- code/perception/launch/perception.launch | 2 +- code/perception/src/lidar_distance.py | 48 ++++++++++++++++++++++-- 2 files changed, 45 insertions(+), 5 deletions(-) diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 67a9c2ab..127ae9a4 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -75,7 +75,7 @@ - + diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 04394ee2..2842172c 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -3,13 +3,13 @@ import ros_numpy import numpy as np import lidar_filter_utility -from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointCloud2, Image as ImageMsg +from sklearn.cluster import DBSCAN +from cv_bridge import CvBridge +import json # from mpl_toolkits.mplot3d import Axes3D # from itertools import combinations -from sensor_msgs.msg import Image as ImageMsg -from cv_bridge import CvBridge - # from matplotlib.colors import LinearSegmentedColormap @@ -30,6 +30,22 @@ def callback(self, data): """ coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + # clustered_points = cluster_lidar_data_from_pointcloud(coordinates.copy()) + try: + filtered_coordinates = coordinates[coordinates["z"] > 0] + + # Konvertiere in JSON + clustered_points_json = json.dumps(filtered_coordinates.tolist()) + + # Veröffentliche JSON-Daten + # self.dist_array_lidar_publisher.publish(clustered_points_json) + rospy.loginfo( + f"JSON mit {len(clustered_points_json)} Punkten veröffentlicht." + ) + rospy.loginfo(clustered_points_json) + except Exception as e: + rospy.logerr(f"Fehler im Callback: {e}") + # Center reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( coordinates, @@ -146,6 +162,20 @@ def listener(self): queue_size=10, ) + """ + try: + self.dist_array_lidar_publisher = rospy.Publisher( + rospy.get_param( + "~image_distance_topic_cluster", "/paf/hero/Lidar/dist_clustered" + ), + String, + queue_size=10, + ) + rospy.loginfo("dist_array_lidar_publisher erfolgreich erstellt.") + except Exception as e: + rospy.logerr(f"Fehler beim Erstellen von dist_array_lidar_publisher: {e}") + """ + # publisher for dist_array self.dist_array_left_publisher = rospy.Publisher( rospy.get_param("~image_distance_topic", "/paf/hero/Left/dist_array"), @@ -166,6 +196,7 @@ def listener(self): self.callback, ) + rospy.loginfo("Lidar Processor Node gestartet.") rospy.spin() def reconstruct_img_from_lidar(self, coordinates_xyz, focus): @@ -246,6 +277,15 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): return dist_array +def cluster_lidar_data_from_pointcloud(coordinates, eps=1.0, min_samples=2): + + clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) + labels = clustering.labels_ + clustered_points = {label: list(labels).count(label) for label in set(labels)} + clustered_points = {int(label): count for label, count in clustered_points.items()} + return clustered_points + + if __name__ == "__main__": lidar_distance = LidarDistance() lidar_distance.listener() From 08e21757aa08fa8d3b3132d4823e673f38a8e834 Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 11 Dec 2024 14:45:44 +0100 Subject: [PATCH 06/37] Added clustering functionality for LiDAR data processing. --- code/agent/config/rviz_config.rviz | 93 +++- code/perception/src/lidar_distance.py | 482 ++++++++++++++------ code/perception/src/lidar_distance_neu.py | 516 ++++++++++++++++++++++ 3 files changed, 933 insertions(+), 158 deletions(-) mode change 100755 => 100644 code/perception/src/lidar_distance.py create mode 100755 code/perception/src/lidar_distance_neu.py diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 2e419cc2..998284c8 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -1,14 +1,12 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 70 Name: Displays Property Tree Widget: Expanded: - - /PointCloud23 - - /PointCloud24 - - /PointCloud25 + - /PointCloud2 dist_clustered1 Splitter Ratio: 0.5 - Tree Height: 308 + Tree Height: 325 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -66,26 +64,23 @@ Visualization Manager: Grid: false Imu: false Path: false - PointCloud2: false + PointCloud2: true + PointCloud2 dist_clustered: true Value: true + VisonNode Output: true Zoom Factor: 1 - Class: rviz/Image Enabled: true - Image Rendering: background and overlay Image Topic: /paf/hero/Center/segmented_image + Max Value: 1 + Median window: 5 + Min Value: 0 Name: VisonNode Output - Overlay Alpha: 0.5 + Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Visibility: - Grid: true - Imu: true - Path: true - PointCloud2: true - Value: true - Zoom Factor: 1 - Alpha: 1 Class: rviz_plugin_tutorials/Imu Color: 204; 51; 204 @@ -260,6 +255,62 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /paf/hero/dist_clustered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 dist_clustered + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /paf/hero/dist_clustered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -288,7 +339,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 34.785499572753906 + Distance: 10.540092468261719 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -304,9 +355,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.19039836525917053 + Pitch: 0.3953982889652252 Target Frame: - Yaw: 4.520427227020264 + Yaw: 3.255431652069092 Saved: ~ Window Geometry: Camera: @@ -316,7 +367,7 @@ Window Geometry: Height: 1376 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000022a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026b000002960000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002070000021e0000001600fffffffb00000020005600690073006f006e004e006f006400650020004f00750074007000750074010000042b000000d60000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -325,6 +376,8 @@ Window Geometry: collapsed: false Views: collapsed: false + VisonNode Output: + collapsed: false Width: 2488 X: 1992 - Y: 27 \ No newline at end of file + Y: 27 diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py old mode 100755 new mode 100644 index 2842172c..260cbea8 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from joblib import Parallel, delayed import rospy import ros_numpy import numpy as np @@ -6,7 +7,6 @@ from sensor_msgs.msg import PointCloud2, Image as ImageMsg from sklearn.cluster import DBSCAN from cv_bridge import CvBridge -import json # from mpl_toolkits.mplot3d import Axes3D # from itertools import combinations @@ -18,35 +18,159 @@ class LidarDistance: how to configute this node """ + cluster_buffer = [] + def callback(self, data): - """Callback function, filters a PontCloud2 message - by restrictions defined in the launchfile. + """ + Callback-Funktion, die LiDAR-Punktwolkendaten verarbeitet. + + Führt Clustering und Bildberechnungen für die Punktwolken aus. - Publishes a Depth image for the specified camera angle. - Each angle has do be delt with differently since the signs of the - coordinate system change with the view angle. + :param data: LiDAR-Punktwolken als ROS PointCloud2-Nachricht. + """ + + self.start_clustering(data) + self.start_image_calculation(data) - :param data: a PointCloud2 + def listener(self): """ + Initialisiert die ROS-Node, erstellt Publisher/Subscriber und hält sie aktiv. + """ + rospy.init_node("lidar_distance") + self.bridge = CvBridge() # OpenCV-Bridge für Bildkonvertierungen + + # Publisher für gefilterte Punktwolken + self.pub_pointcloud = rospy.Publisher( + rospy.get_param( + "~point_cloud_topic", + "/carla/hero/" + rospy.get_namespace() + "_filtered", + ), + PointCloud2, + queue_size=1, + ) + + # Publisher für Distanzbilder in verschiedene Richtungen + self.dist_array_center_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), + ImageMsg, + queue_size=10, + ) + self.dist_array_back_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Back/dist_array"), + ImageMsg, + queue_size=10, + ) + self.dist_array_lidar_publisher = rospy.Publisher( + rospy.get_param( + "~image_distance_topic_cluster", "/paf/hero/dist_clustered" + ), + PointCloud2, + queue_size=10, + ) + rospy.loginfo("dist_array_lidar_publisher erfolgreich erstellt.") + self.dist_array_left_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Left/dist_array"), + ImageMsg, + queue_size=10, + ) + self.dist_array_right_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Right/dist_array"), + ImageMsg, + queue_size=10, + ) + + # Subscriber für LiDAR-Daten (Punktwolken) + rospy.Subscriber( + rospy.get_param("~source_topic", "/carla/hero/LIDAR"), + PointCloud2, + self.callback, + ) + + rospy.loginfo("Lidar Processor Node gestartet.") + rospy.spin() + + def start_clustering(self, data): + """ + Filtert LiDAR-Punktwolken, führt Clustering durch und veröffentlicht die kombinierten Cluster. + + :param data: LiDAR-Punktwolken im ROS PointCloud2-Format. + """ + + # Punktwolken filtern, um irrelevante Daten zu entfernen coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + filtered_coordinates = coordinates[ + ~( + (-2 <= coordinates["x"]) + & (coordinates["x"] <= 2) + & (-1 <= coordinates["y"]) + & (coordinates["y"] <= 1) + ) # Ausschluss von Punkten die das eigene Auto betreffen + & ( + coordinates["z"] > -1.7 + 0.05 + ) # Mindesthöhe in z, um die Straße nicht zu clustern + ] + + # Cluster-Daten aus den gefilterten Koordinaten berechnen + clustered_points = cluster_lidar_data_from_pointcloud( + coordinates=filtered_coordinates + ) - # clustered_points = cluster_lidar_data_from_pointcloud(coordinates.copy()) - try: - filtered_coordinates = coordinates[coordinates["z"] > 0] + # Nur gültige Cluster-Daten speichern + if clustered_points: + LidarDistance.cluster_buffer.append(clustered_points) + else: + rospy.logwarn("Keine Cluster-Daten erzeugt.") - # Konvertiere in JSON - clustered_points_json = json.dumps(filtered_coordinates.tolist()) + # Cluster kombinieren + combined_clusters = combine_clusters(LidarDistance.cluster_buffer) - # Veröffentliche JSON-Daten - # self.dist_array_lidar_publisher.publish(clustered_points_json) - rospy.loginfo( - f"JSON mit {len(clustered_points_json)} Punkten veröffentlicht." - ) - rospy.loginfo(clustered_points_json) - except Exception as e: - rospy.logerr(f"Fehler im Callback: {e}") + LidarDistance.cluster_buffer = [] + + # Veröffentliche die kombinierten Cluster + self.publish_clusters(combined_clusters, data.header) + + def publish_clusters(self, combined_clusters, data_header): + """ + Veröffentlicht kombinierte Cluster als ROS PointCloud2-Nachricht. - # Center + :param combined_clusters: Kombinierte Punktwolken der Cluster als strukturiertes NumPy-Array. + :param data_header: Header-Informationen der ROS-Nachricht. + """ + # Konvertiere zu PointCloud2-Nachricht + pointcloud_msg = ros_numpy.point_cloud2.array_to_pointcloud2(combined_clusters) + pointcloud_msg.header = data_header + pointcloud_msg.header.stamp = rospy.Time.now() + # Cluster veröffentlichen + self.dist_array_lidar_publisher.publish(pointcloud_msg) + + def start_image_calculation(self, data): + """ + Berechnet Distanzbilder basierend auf LiDAR-Daten und veröffentlicht sie. + + :param data: LiDAR-Punktwolken im ROS PointCloud2-Format. + """ + coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + # Bildverarbeitung auf den Koordinaten durchführen + processed_images = { + "Center": None, + "Back": None, + "Left": None, + "Right": None, + } + processed_images["Center"] = self.calculate_image_center(coordinates) + processed_images["Back"] = self.calculate_image_back(coordinates) + processed_images["Left"] = self.calculate_image_left(coordinates) + processed_images["Right"] = self.calculate_image_right(coordinates) + + self.publish_images(processed_images, data.header) + + def calculate_image_center(self, coordinates): + """ + Berechnet ein Distanzbild für die zentrale Ansicht aus LiDAR-Koordinaten. + + :param coordinates: Gefilterte LiDAR-Koordinaten als NumPy-Array. + :return: Distanzbild als 2D-Array. + """ reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( coordinates, max_x=np.inf, @@ -62,12 +186,9 @@ def callback(self, data): dist_array_center = self.reconstruct_img_from_lidar( reconstruct_coordinates_xyz_center, focus="Center" ) - dist_array_center_msg = self.bridge.cv2_to_imgmsg( - dist_array_center, encoding="passthrough" - ) - dist_array_center_msg.header = data.header - self.dist_array_center_publisher.publish(dist_array_center_msg) + return dist_array_center + def calculate_image_back(self, coordinates): # Back reconstruct_bit_mask_back = lidar_filter_utility.bounding_box( coordinates, @@ -84,12 +205,9 @@ def callback(self, data): dist_array_back = self.reconstruct_img_from_lidar( reconstruct_coordinates_xyz_back, focus="Back" ) - dist_array_back_msg = self.bridge.cv2_to_imgmsg( - dist_array_back, encoding="passthrough" - ) - dist_array_back_msg.header = data.header - self.dist_array_back_publisher.publish(dist_array_back_msg) + return dist_array_back + def calculate_image_left(self, coordinates): # Left reconstruct_bit_mask_left = lidar_filter_utility.bounding_box( coordinates, @@ -106,12 +224,9 @@ def callback(self, data): dist_array_left = self.reconstruct_img_from_lidar( reconstruct_coordinates_xyz_left, focus="Left" ) - dist_array_left_msg = self.bridge.cv2_to_imgmsg( - dist_array_left, encoding="passthrough" - ) - dist_array_left_msg.header = data.header - self.dist_array_left_publisher.publish(dist_array_left_msg) + return dist_array_left + def calculate_image_right(self, coordinates): # Right reconstruct_bit_mask_right = lidar_filter_utility.bounding_box( coordinates, max_y=-0.0, min_y=-np.inf, min_z=-1.6 @@ -125,124 +240,78 @@ def callback(self, data): dist_array_right = self.reconstruct_img_from_lidar( reconstruct_coordinates_xyz_right, focus="Right" ) - dist_array_right_msg = self.bridge.cv2_to_imgmsg( - dist_array_right, encoding="passthrough" - ) - dist_array_right_msg.header = data.header - self.dist_array_right_publisher.publish(dist_array_right_msg) + return dist_array_right - def listener(self): + def publish_images(self, processed_images, data_header): """ - Initializes the node and it's publishers - """ - # run simultaneously. - rospy.init_node("lidar_distance") - self.bridge = CvBridge() - - self.pub_pointcloud = rospy.Publisher( - rospy.get_param( - "~point_cloud_topic", - "/carla/hero/" + rospy.get_namespace() + "_filtered", - ), - PointCloud2, - queue_size=10, - ) - - # publisher for dist_array - self.dist_array_center_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), - ImageMsg, - queue_size=10, - ) - - # publisher for dist_array - self.dist_array_back_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Back/dist_array"), - ImageMsg, - queue_size=10, - ) + Veröffentlicht Distanzbilder für verschiedene Richtungen als ROS-Bildnachrichten. + :param processed_images: Dictionary mit Richtungen ("Center", "Back", etc.) als Schlüssel und Bildarrays als Werte. + :param data_header: Header der ROS-Bildnachrichten. """ - try: - self.dist_array_lidar_publisher = rospy.Publisher( - rospy.get_param( - "~image_distance_topic_cluster", "/paf/hero/Lidar/dist_clustered" - ), - String, - queue_size=10, + # Nur gültige NumPy-Arrays weiterverarbeiten + for direction, image_array in processed_images.items(): + if not isinstance(image_array, np.ndarray): + continue + + # Konvertiere das Bild in eine ROS-Image-Nachricht + dist_array_msg = self.bridge.cv2_to_imgmsg( + image_array, encoding="passthrough" ) - rospy.loginfo("dist_array_lidar_publisher erfolgreich erstellt.") - except Exception as e: - rospy.logerr(f"Fehler beim Erstellen von dist_array_lidar_publisher: {e}") - """ + dist_array_msg.header = data_header - # publisher for dist_array - self.dist_array_left_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Left/dist_array"), - ImageMsg, - queue_size=10, - ) - - # publisher for dist_array - self.dist_array_right_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Right/dist_array"), - ImageMsg, - queue_size=10, - ) - - rospy.Subscriber( - rospy.get_param("~source_topic", "/carla/hero/LIDAR"), - PointCloud2, - self.callback, - ) - - rospy.loginfo("Lidar Processor Node gestartet.") - rospy.spin() + if direction == "Center": + self.dist_array_center_publisher.publish(dist_array_msg) + if direction == "Back": + self.dist_array_back_publisher.publish(dist_array_msg) + if direction == "Left": + self.dist_array_left_publisher.publish(dist_array_msg) + if direction == "Right": + self.dist_array_right_publisher.publish(dist_array_msg) def reconstruct_img_from_lidar(self, coordinates_xyz, focus): """ - reconstruct 3d LIDAR-Data and calculate 2D Pixel - according to Camera-World - - Args: - coordinates_xyz (np.array): filtered lidar points - focus (String): Camera Angle + Rekonstruiert ein 2D-Bild aus 3D-LiDAR-Daten für eine gegebene Kameraansicht. - Returns: - image: depth image for camera angle + :param coordinates_xyz: 3D-Koordinaten der gefilterten LiDAR-Punkte. + :param focus: Kameraansicht (z. B. "Center", "Back"). + :return: Rekonstruiertes Bild als 2D-Array. """ - # intrinsic matrix for camera: - # width -> 300, height -> 200, fov -> 100 (agent.py) + # Erstelle die intrinsische Kamera-Matrix basierend auf den Bildparametern (FOV, Auflösung) im = np.identity(3) - im[0, 2] = 1280 / 2.0 - im[1, 2] = 720 / 2.0 - im[0, 0] = im[1, 1] = 1280 / (2.0 * np.tan(100 * np.pi / 360.0)) + im[0, 2] = 1280 / 2.0 # x-Verschiebung (Bildmitte) + im[1, 2] = 720 / 2.0 # y-Verschiebung (Bildmitte) + im[0, 0] = im[1, 1] = 1280 / ( + 2.0 * np.tan(100 * np.pi / 360.0) + ) # Skalierungsfaktor basierend auf FOV - # extrinsic matrix for camera + # Erstelle die extrinsische Kamera-Matrix (Identität für keine Transformation) ex = np.zeros(shape=(3, 4)) - ex[0][0] = 1 - ex[1][1] = 1 - ex[2][2] = 1 - m = np.matmul(im, ex) + ex[0][0] = ex[1][1] = ex[2][2] = 1 + m = np.matmul(im, ex) # Kombiniere intrinsische und extrinsische Matrix - # reconstruct camera image with LIDAR-Data + # Initialisiere leere Bilder für die Rekonstruktion img = np.zeros(shape=(720, 1280), dtype=np.float32) dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32) + + # Verarbeite jeden Punkt in der Punktwolke for c in coordinates_xyz: - # center depth image if focus == "Center": point = np.array([c[1], c[2], c[0], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if x >= 0 and x <= 1280 and y >= 0 and y <= 720: - img[719 - y][1279 - x] = c[0] + pixel = np.matmul( + m, point + ) # Projiziere 3D-Punkt auf 2D-Bildkoordinaten + x, y = int(pixel[0] / pixel[2]), int( + pixel[1] / pixel[2] + ) # Normalisiere Koordinaten + if x >= 0 and x <= 1280 and y >= 0 and y <= 720: # Prüfe Bildgrenzen + img[719 - y][1279 - x] = c[0] # Setze Tiefenwert dist_array[719 - y][1279 - x] = np.array( [c[0], c[1], c[2]], dtype=np.float32 ) - # back depth image - if focus == "Back": + if focus == "Back": # Berechne Bild für die Rückansicht point = np.array([c[1], c[2], c[0], 1]) pixel = np.matmul(m, point) x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) @@ -252,8 +321,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): [-c[0], c[1], c[2]], dtype=np.float32 ) - # left depth image - if focus == "Left": + if focus == "Left": # Berechne Bild für die linke Ansicht point = np.array([c[0], c[2], c[1], 1]) pixel = np.matmul(m, point) x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) @@ -263,8 +331,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): [c[0], c[1], c[2]], dtype=np.float32 ) - # right depth image - if focus == "Right": + if focus == "Right": # Berechne Bild für die rechte Ansicht point = np.array([c[0], c[2], c[1], 1]) pixel = np.matmul(m, point) x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) @@ -277,15 +344,154 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): return dist_array -def cluster_lidar_data_from_pointcloud(coordinates, eps=1.0, min_samples=2): +def array_to_pointcloud2(points, header="hero/Lidar"): + """ + Konvertiert ein Array von Punkten in eine ROS PointCloud2-Nachricht. + + :param points: Array von Punkten mit [x, y, z]-Koordinaten. + :param header: Header-Informationen der ROS PointCloud2-Nachricht. + :return: ROS PointCloud2-Nachricht. + """ + # Sicherstellen, dass die Eingabe ein NumPy-Array ist + points_array = np.array(points) + + # Konvertiere die Punkte in ein strukturiertes Array mit Feldern "x", "y", "z" + points_structured = np.array( + [(p[0], p[1], p[2]) for p in points_array], + dtype=[("x", "f4"), ("y", "f4"), ("z", "f4")], + ) + + # Erstelle eine PointCloud2-Nachricht aus dem strukturierten Array + pointcloud_msg = ros_numpy.point_cloud2.array_to_pointcloud2(points_structured) + + # Setze den Zeitstempel und den Header der Nachricht + pointcloud_msg.header.stamp = rospy.Time.now() + pointcloud_msg.header = header + + return pointcloud_msg + + +def get_largest_cluster(clustered_points, return_structured=True): + """ + Ermittelt das größte Cluster aus gegebenen Punktwolken-Clustern. + + :param clustered_points: Dictionary mit Cluster-IDs und zugehörigen Punktwolken. + :param return_structured: Gibt ein strukturiertes NumPy-Array zurück, falls True. + :return: Größtes Cluster als NumPy-Array (roh oder strukturiert). + """ + # Prüfen, ob es Cluster gibt + if not clustered_points: + return np.array([]) + + # Identifiziere das größte Cluster basierend auf der Anzahl der Punkte + largest_cluster_id, largest_cluster = max( + clustered_points.items(), key=lambda item: len(item[1]) + ) + + # Sicherstellen, dass das größte Cluster nicht leer ist + if largest_cluster.size == 0: + return np.array([]) + + rospy.loginfo( + f"Largest cluster: {largest_cluster_id} with {largest_cluster.shape[0]} points" + ) + + # Rohdaten zurückgeben, wenn kein strukturiertes Array benötigt wird + if not return_structured: + return largest_cluster + + # Konvertiere das größte Cluster in ein strukturiertes Array + points_structured = np.empty( + largest_cluster.shape[0], dtype=[("x", "f4"), ("y", "f4"), ("z", "f4")] + ) + points_structured["x"] = largest_cluster[:, 0] + points_structured["y"] = largest_cluster[:, 1] + points_structured["z"] = largest_cluster[:, 2] + + return points_structured + - clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) +def combine_clusters(cluster_buffer): + """ + Kombiniert Cluster aus mehreren Punktwolken zu einem strukturierten NumPy-Array. + + :param cluster_buffer: Liste von Dictionaries mit Cluster-IDs und Punktwolken. + :return: Kombiniertes strukturiertes NumPy-Array mit Feldern "x", "y", "z", "cluster_id". + """ + points_list = [] + cluster_ids_list = [] + + for clustered_points in cluster_buffer: + for cluster_id, points in clustered_points.items(): + if points.size > 0: # Ignoriere leere Cluster + points_list.append(points) + # Erstelle ein Array mit der Cluster-ID für alle Punkte des Clusters + cluster_ids_list.append( + np.full(points.shape[0], cluster_id, dtype=np.float32) + ) + + if not points_list: # Falls keine Punkte vorhanden sind + return np.array( + [], dtype=[("x", "f4"), ("y", "f4"), ("z", "f4"), ("cluster_id", "f4")] + ) + + # Kombiniere alle Punkte und Cluster-IDs in zwei separate Arrays + all_points = np.vstack(points_list) + all_cluster_ids = np.concatenate(cluster_ids_list) + + # Erstelle ein strukturiertes Array für die kombinierten Daten + combined_points = np.zeros( + all_points.shape[0], + dtype=[("x", "f4"), ("y", "f4"), ("z", "f4"), ("cluster_id", "f4")], + ) + combined_points["x"] = all_points[:, 0] + combined_points["y"] = all_points[:, 1] + combined_points["z"] = all_points[:, 2] + combined_points["cluster_id"] = all_cluster_ids + + return combined_points + + +def cluster_lidar_data_from_pointcloud(coordinates, eps=0.3, min_samples=10): + """ + Führt Clustering auf LiDAR-Daten mit DBSCAN durch und gibt Cluster zurück. + + :param coordinates: LiDAR-Punktwolken als NumPy-Array mit "x", "y", "z". + :param eps: Maximaler Abstand zwischen Punkten, um sie zu einem Cluster zuzuordnen. + :param min_samples: Minimale Anzahl von Punkten, um ein Cluster zu bilden. + :return: Dictionary mit Cluster-IDs und zugehörigen Punktwolken. + """ + if coordinates.shape[0] == 0: + rospy.logerr("Das Eingabe-Array 'coordinates' ist leer.") + return {} + + # Extrahiere x, y und z aus den Koordinaten für die DBSCAN-Berechnung + xyz = np.column_stack((coordinates["x"], coordinates["y"], coordinates["z"])) + + if xyz.shape[0] == 0: + rospy.logwarn("Keine Datenpunkte für DBSCAN verfügbar. Überspringe Clustering.") + return {} + + # Wende DBSCAN an, um Cluster-Labels für die Punktwolke zu berechnen + clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(xyz) labels = clustering.labels_ - clustered_points = {label: list(labels).count(label) for label in set(labels)} - clustered_points = {int(label): count for label, count in clustered_points.items()} - return clustered_points + + # Entferne Rauschen (Cluster-ID: -1) und bestimme gültige Cluster-IDs + unique_labels = np.unique(labels) + valid_labels = unique_labels[unique_labels != -1] + + # Erstelle ein Dictionary mit Cluster-IDs und den zugehörigen Punkten + clusters = Parallel(n_jobs=-1)( + delayed(lambda l: (l, xyz[labels == l]))(label) for label in valid_labels + ) + clusters = dict(clusters) + + return clusters if __name__ == "__main__": + """ + Initialisiert die LidarDistance-Klasse und startet die Listener-Methode. + """ lidar_distance = LidarDistance() lidar_distance.listener() diff --git a/code/perception/src/lidar_distance_neu.py b/code/perception/src/lidar_distance_neu.py new file mode 100755 index 00000000..57892ed6 --- /dev/null +++ b/code/perception/src/lidar_distance_neu.py @@ -0,0 +1,516 @@ +#!/usr/bin/env python +import rospy +import ros_numpy +import numpy as np +import lidar_filter_utility +from sensor_msgs.msg import PointCloud2, Image as ImageMsg +from sklearn.cluster import DBSCAN +from cv_bridge import CvBridge +from joblib import Parallel, delayed +import queue +import threading + +# from mpl_toolkits.mplot3d import Axes3D +# from itertools import combinations +# from matplotlib.colors import LinearSegmentedColormap + + +class LidarDistance: + """See doc/perception/lidar_distance_utility.md on + how to configute this node + """ + + # Klassenattribute + cluster_queue = queue.Queue() + image_queue = queue.Queue() + workers_initialized = False + cluster_data_list = [] + + def callback(self, data): + """Callback function, filters a PontCloud2 message + by restrictions defined in the launchfile. + + Publishes a Depth image for the specified camera angle. + Each angle has do be delt with differently since the signs of the + coordinate system change with the view angle. + + :param data: a PointCloud2 + """ + + if not LidarDistance.workers_initialized: + LidarDistance.workers_initialized = True + self.start_image_worker() + self.start_cluster_worker() + + if not self.cluster_thread.is_alive: + rospy.logwarn("Cluster-Worker abgestürzt. Neustart...") + self.start_cluster_worker() + + if not self.image_thread.is_alive: + rospy.logwarn("Image-Worker abgestürzt. Neustart...") + self.start_image_worker() + + LidarDistance.cluster_queue.put(data) + if LidarDistance.cluster_queue.qsize() > 10: + rospy.logwarn( + "Cluster-Worker kommt nicht hinterher. Queue size:" + + str(LidarDistance.cluster_queue.qsize()), + ) + # Dasselbe Koordinatenset für die Bildberechnung in die Bild-Warteschlange legen + LidarDistance.image_queue.put(data) + if LidarDistance.image_queue.qsize() > 10: + rospy.logwarn( + f"Image-Worker ({self.image_thread.is_alive}) kommt nicht hinterher. Queue size: {str(LidarDistance.image_queue.qsize())}" + ) + + def listener(self): + """ + Initializes the node and it's publishers + """ + # run simultaneously. + rospy.init_node("lidar_distance") + self.bridge = CvBridge() + + self.pub_pointcloud = rospy.Publisher( + rospy.get_param( + "~point_cloud_topic", + "/carla/hero/" + rospy.get_namespace() + "_filtered", + ), + PointCloud2, + queue_size=10, + ) + + # publisher for dist_array + self.dist_array_center_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), + ImageMsg, + queue_size=10, + ) + + # publisher for dist_array + self.dist_array_back_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Back/dist_array"), + ImageMsg, + queue_size=10, + ) + + self.dist_array_lidar_publisher = rospy.Publisher( + rospy.get_param( + "~image_distance_topic_cluster", "/paf/hero/dist_clustered" + ), + PointCloud2, + queue_size=10, + ) + rospy.loginfo("dist_array_lidar_publisher erfolgreich erstellt.") + + # publisher for dist_array + self.dist_array_left_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Left/dist_array"), + ImageMsg, + queue_size=10, + ) + + # publisher for dist_array + self.dist_array_right_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Right/dist_array"), + ImageMsg, + queue_size=10, + ) + + rospy.Subscriber( + rospy.get_param("~source_topic", "/carla/hero/LIDAR"), + PointCloud2, + self.callback, + ) + + rospy.loginfo("Lidar Processor Node gestartet.") + rospy.spin() + + def cluster_worker(self): + """ + Worker für das Clustern von Punkten. + """ + while True: + try: + # Neueste Aufgabe holen + data = LidarDistance.cluster_queue.get(timeout=1) + + # Restliche Aufgaben aus der Queue entfernen + discarded_tasks = 0 + while not LidarDistance.cluster_queue.empty(): + LidarDistance.cluster_queue.get() + discarded_tasks += 1 + + # Anzahl verworfener Einträge loggen + if discarded_tasks > 0: + rospy.logwarn(f"{discarded_tasks} Einträge wurden verworfen.") + + # Stop-Signal überprüfen + if data is None: # Stop-Signal + break + + # Punktwolken-Daten verarbeiten + coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + filtered_coordinates = coordinates[(coordinates["x"] >= 2)] + clustered_points = cluster_lidar_data_from_pointcloud( + coordinates=filtered_coordinates + ) + + # Kombinierte Cluster erstellen + cluster_structured = combine_clusters(clustered_points) + + # Konvertiere zu PointCloud2-Nachricht + pointcloud_msg = ros_numpy.point_cloud2.array_to_pointcloud2( + cluster_structured + ) + pointcloud_msg.header = data.header + pointcloud_msg.header.stamp = rospy.Time.now() + + # Cluster veröffentlichen + self.dist_array_lidar_publisher.publish(pointcloud_msg) + + except queue.Empty: + rospy.loginfo("Cluster-Queue ist leer. Warte auf neue Aufgaben.") + continue # Zurück zur Queue-Warte + + except Exception as e: + rospy.logerr(f"Fehler im Cluster-Worker: {e}") + + def image_worker(self): + """ + Worker für die Berechnung von Bildern. + """ + while True: + data = LidarDistance.image_queue.get() + if data is None: # Stop-Signal + break + + coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + # Bildverarbeitung auf den Koordinaten durchführen + processed_images = { + "Center": None, + "Back": None, + "Left": None, + "Right": None, + } + processed_images["Center"] = self.calculate_image_center(coordinates) + processed_images["Back"] = self.calculate_image_back(coordinates) + processed_images["Left"] = self.calculate_image_left(coordinates) + processed_images["Right"] = self.calculate_image_right(coordinates) + + self.publish_images(processed_images, data.header) + + def start_cluster_worker(self): + self.cluster_thread = threading.Thread(target=self.cluster_worker, daemon=True) + self.cluster_thread.start() + + def start_image_worker(self): + self.image_thread = threading.Thread(target=self.image_worker, daemon=True) + self.image_thread.start() + + def calculate_image_center(self, coordinates): + reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( + coordinates, + max_x=np.inf, + min_x=0.0, + min_z=-1.6, + ) + reconstruct_coordinates_center = coordinates[reconstruct_bit_mask_center] + reconstruct_coordinates_xyz_center = np.array( + lidar_filter_utility.remove_field_name( + reconstruct_coordinates_center, "intensity" + ).tolist() + ) + dist_array_center = self.reconstruct_img_from_lidar( + reconstruct_coordinates_xyz_center, focus="Center" + ) + return dist_array_center + + def calculate_image_back(self, coordinates): + # Back + reconstruct_bit_mask_back = lidar_filter_utility.bounding_box( + coordinates, + max_x=0.0, + min_x=-np.inf, + min_z=-1.6, + ) + reconstruct_coordinates_back = coordinates[reconstruct_bit_mask_back] + reconstruct_coordinates_xyz_back = np.array( + lidar_filter_utility.remove_field_name( + reconstruct_coordinates_back, "intensity" + ).tolist() + ) + dist_array_back = self.reconstruct_img_from_lidar( + reconstruct_coordinates_xyz_back, focus="Back" + ) + return dist_array_back + + def calculate_image_left(self, coordinates): + # Left + reconstruct_bit_mask_left = lidar_filter_utility.bounding_box( + coordinates, + max_y=np.inf, + min_y=0.0, + min_z=-1.6, + ) + reconstruct_coordinates_left = coordinates[reconstruct_bit_mask_left] + reconstruct_coordinates_xyz_left = np.array( + lidar_filter_utility.remove_field_name( + reconstruct_coordinates_left, "intensity" + ).tolist() + ) + dist_array_left = self.reconstruct_img_from_lidar( + reconstruct_coordinates_xyz_left, focus="Left" + ) + return dist_array_left + + def calculate_image_right(self, coordinates): + # Right + reconstruct_bit_mask_right = lidar_filter_utility.bounding_box( + coordinates, max_y=-0.0, min_y=-np.inf, min_z=-1.6 + ) + reconstruct_coordinates_right = coordinates[reconstruct_bit_mask_right] + reconstruct_coordinates_xyz_right = np.array( + lidar_filter_utility.remove_field_name( + reconstruct_coordinates_right, "intensity" + ).tolist() + ) + dist_array_right = self.reconstruct_img_from_lidar( + reconstruct_coordinates_xyz_right, focus="Right" + ) + return dist_array_right + + def publish_images(self, processed_images, data_header): + for direction, image_array in processed_images.items(): + if not isinstance(image_array, np.ndarray): + continue + dist_array_msg = self.bridge.cv2_to_imgmsg( + image_array, encoding="passthrough" + ) + dist_array_msg.header = data_header + + if direction == "Center": + self.dist_array_center_publisher.publish(dist_array_msg) + if direction == "Back": + self.dist_array_back_publisher.publish(dist_array_msg) + if direction == "Left": + self.dist_array_left_publisher.publish(dist_array_msg) + if direction == "Right": + self.dist_array_right_publisher.publish(dist_array_msg) + + def reconstruct_img_from_lidar(self, coordinates_xyz, focus): + """ + reconstruct 3d LIDAR-Data and calculate 2D Pixel + according to Camera-World + + Args: + coordinates_xyz (np.array): filtered lidar points + focus (String): Camera Angle + + Returns: + image: depth image for camera angle + """ + + # intrinsic matrix for camera: + # width -> 300, height -> 200, fov -> 100 (agent.py) + im = np.identity(3) + im[0, 2] = 1280 / 2.0 + im[1, 2] = 720 / 2.0 + im[0, 0] = im[1, 1] = 1280 / (2.0 * np.tan(100 * np.pi / 360.0)) + + # extrinsic matrix for camera + ex = np.zeros(shape=(3, 4)) + ex[0][0] = 1 + ex[1][1] = 1 + ex[2][2] = 1 + m = np.matmul(im, ex) + + # reconstruct camera image with LIDAR-Data + img = np.zeros(shape=(720, 1280), dtype=np.float32) + dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32) + for c in coordinates_xyz: + # center depth image + if focus == "Center": + point = np.array([c[1], c[2], c[0], 1]) + pixel = np.matmul(m, point) + x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) + if x >= 0 and x <= 1280 and y >= 0 and y <= 720: + img[719 - y][1279 - x] = c[0] + dist_array[719 - y][1279 - x] = np.array( + [c[0], c[1], c[2]], dtype=np.float32 + ) + + # back depth image + if focus == "Back": + point = np.array([c[1], c[2], c[0], 1]) + pixel = np.matmul(m, point) + x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) + if x >= 0 and x <= 1280 and y >= 0 and y < 720: + img[y][1279 - x] = -c[0] + dist_array[y][1279 - x] = np.array( + [-c[0], c[1], c[2]], dtype=np.float32 + ) + + # left depth image + if focus == "Left": + point = np.array([c[0], c[2], c[1], 1]) + pixel = np.matmul(m, point) + x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) + if x >= 0 and x <= 1280 and y >= 0 and y <= 720: + img[719 - y][1279 - x] = c[1] + dist_array[y][1279 - x] = np.array( + [c[0], c[1], c[2]], dtype=np.float32 + ) + + # right depth image + if focus == "Right": + point = np.array([c[0], c[2], c[1], 1]) + pixel = np.matmul(m, point) + x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) + if x >= 0 and x < 1280 and y >= 0 and y < 720: + img[y][1279 - x] = -c[1] + dist_array[y][1279 - x] = np.array( + [c[0], c[1], c[2]], dtype=np.float32 + ) + + return dist_array + + +def array_to_pointcloud2(points, header="hero/Lidar"): + points_array = np.array(points) + + points_structured = np.array( + [(p[0], p[1], p[2]) for p in points_array], + dtype=[("x", "f4"), ("y", "f4"), ("z", "f4")], + ) + + pointcloud_msg = ros_numpy.point_cloud2.array_to_pointcloud2(points_structured) + + pointcloud_msg.header.stamp = rospy.Time.now() + pointcloud_msg.header = header + + return pointcloud_msg + + +def get_largest_cluster(clustered_points, return_structured=True): + """ + Bestimmt das größte Cluster aus den gegebenen Punkten und gibt es zurück. + + :param clustered_points: Dictionary mit Cluster-IDs als Schlüssel und Punktwolken als Werte. + :param return_structured: Gibt ein strukturiertes Array zurück, falls True. + :return: Größtes Cluster als Array (entweder raw oder strukturiert). + """ + if not clustered_points: + rospy.logerr( + "Clustered points are empty. Cannot determine the largest cluster." + ) + return np.array([]) + + # Finde das größte Cluster direkt + largest_cluster_id, largest_cluster = max( + clustered_points.items(), key=lambda item: len(item[1]) + ) + + if largest_cluster.size == 0: + rospy.logerr(f"Largest cluster (ID {largest_cluster_id}) is empty.") + return np.array([]) + + rospy.loginfo( + f"Largest cluster: {largest_cluster_id} with {largest_cluster.shape[0]} points" + ) + + # Falls kein strukturiertes Array benötigt wird, direkt zurückgeben + if not return_structured: + return largest_cluster + + # Andernfalls Punkte in ein strukturiertes Array konvertieren + points_structured = np.empty( + largest_cluster.shape[0], dtype=[("x", "f4"), ("y", "f4"), ("z", "f4")] + ) + points_structured["x"] = largest_cluster[:, 0] + points_structured["y"] = largest_cluster[:, 1] + points_structured["z"] = largest_cluster[:, 2] + + return points_structured + + +def combine_clusters(clustered_points): + """ + Kombiniert Cluster-Daten in ein strukturiertes Array. + + :param clustered_points: Dictionary mit Cluster-IDs als Schlüssel und 3D-Punktwolken als Werten. + :return: Kombiniertes strukturiertes NumPy-Array mit Feldern "x", "y", "z", "cluster_id". + """ + + # Vorab die Gesamtanzahl der Punkte berechnen + total_points = sum(points.shape[0] for points in clustered_points.values()) + + # Erstelle ein leeres strukturiertes Array + combined_points = np.empty( + total_points, + dtype=[("x", "f4"), ("y", "f4"), ("z", "f4"), ("cluster_id", "f4")], + ) + + # Fülle das Array direkt + index = 0 + for cluster_id, points in clustered_points.items(): + num_points = points.shape[0] + combined_points["x"][index : index + num_points] = points[:, 0] + combined_points["y"][index : index + num_points] = points[:, 1] + combined_points["z"][index : index + num_points] = points[:, 2] + combined_points["cluster_id"][index : index + num_points] = cluster_id + index += num_points + + return combined_points + + +def fuse_clusters(cluster_data_list): + """ + Fusioniert eine Liste von Cluster-Daten und entfernt redundante Punkte basierend auf (x, y, z). + + :param cluster_data_list: Liste von NumPy-Arrays mit Feldern (x, y, z, intensity) + :return: Fusioniertes NumPy-Array mit einzigartigen Punkten + """ + # Kombiniere alle Arrays in ein großes Array + concatenated_points = np.concatenate(cluster_data_list, axis=0) + + # Erstelle eine flache Ansicht für die Felder (x, y, z) + unique_xyz, indices = np.unique( + concatenated_points[["x", "y", "z"]].view(np.float32).reshape(-1, 3), + axis=0, + return_index=True, + ) + + # Behalte die entsprechenden `intensity`-Werte basierend auf den eindeutigen Indizes + unique_points = concatenated_points[indices] + + return unique_points + + +def cluster_lidar_data_from_pointcloud(coordinates, eps=0.1, min_samples=10): + """ + Cluster LiDAR-Daten effizient mit DBSCAN. + """ + # Erstelle Matrix direkt mit column_stack (schneller als vstack) + xyz = np.column_stack((coordinates["x"], coordinates["y"], coordinates["z"])) + + # DBSCAN-Cluster-Berechnung + clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(xyz) + labels = clustering.labels_ + + # Nur eindeutige Cluster (Rauschen entfernen) + unique_labels = np.unique(labels) + valid_labels = unique_labels[unique_labels != -1] + + # Parallelisiertes Extrahieren der Cluster + clusters = Parallel(n_jobs=-1)( + delayed(lambda l: (l, xyz[labels == l]))(label) for label in valid_labels + ) + clusters = dict(clusters) + + return clusters + + +if __name__ == "__main__": + lidar_distance = LidarDistance() + lidar_distance.listener() From 2aae9be231e3cde0e545ea4b1adf733c942598c9 Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 11 Dec 2024 14:49:52 +0100 Subject: [PATCH 07/37] Deleted file duplicate. Refactored the existing node code by modularizing it into functions for better readability and maintainability. --- code/perception/src/lidar_distance_neu.py | 516 ---------------------- 1 file changed, 516 deletions(-) delete mode 100755 code/perception/src/lidar_distance_neu.py diff --git a/code/perception/src/lidar_distance_neu.py b/code/perception/src/lidar_distance_neu.py deleted file mode 100755 index 57892ed6..00000000 --- a/code/perception/src/lidar_distance_neu.py +++ /dev/null @@ -1,516 +0,0 @@ -#!/usr/bin/env python -import rospy -import ros_numpy -import numpy as np -import lidar_filter_utility -from sensor_msgs.msg import PointCloud2, Image as ImageMsg -from sklearn.cluster import DBSCAN -from cv_bridge import CvBridge -from joblib import Parallel, delayed -import queue -import threading - -# from mpl_toolkits.mplot3d import Axes3D -# from itertools import combinations -# from matplotlib.colors import LinearSegmentedColormap - - -class LidarDistance: - """See doc/perception/lidar_distance_utility.md on - how to configute this node - """ - - # Klassenattribute - cluster_queue = queue.Queue() - image_queue = queue.Queue() - workers_initialized = False - cluster_data_list = [] - - def callback(self, data): - """Callback function, filters a PontCloud2 message - by restrictions defined in the launchfile. - - Publishes a Depth image for the specified camera angle. - Each angle has do be delt with differently since the signs of the - coordinate system change with the view angle. - - :param data: a PointCloud2 - """ - - if not LidarDistance.workers_initialized: - LidarDistance.workers_initialized = True - self.start_image_worker() - self.start_cluster_worker() - - if not self.cluster_thread.is_alive: - rospy.logwarn("Cluster-Worker abgestürzt. Neustart...") - self.start_cluster_worker() - - if not self.image_thread.is_alive: - rospy.logwarn("Image-Worker abgestürzt. Neustart...") - self.start_image_worker() - - LidarDistance.cluster_queue.put(data) - if LidarDistance.cluster_queue.qsize() > 10: - rospy.logwarn( - "Cluster-Worker kommt nicht hinterher. Queue size:" - + str(LidarDistance.cluster_queue.qsize()), - ) - # Dasselbe Koordinatenset für die Bildberechnung in die Bild-Warteschlange legen - LidarDistance.image_queue.put(data) - if LidarDistance.image_queue.qsize() > 10: - rospy.logwarn( - f"Image-Worker ({self.image_thread.is_alive}) kommt nicht hinterher. Queue size: {str(LidarDistance.image_queue.qsize())}" - ) - - def listener(self): - """ - Initializes the node and it's publishers - """ - # run simultaneously. - rospy.init_node("lidar_distance") - self.bridge = CvBridge() - - self.pub_pointcloud = rospy.Publisher( - rospy.get_param( - "~point_cloud_topic", - "/carla/hero/" + rospy.get_namespace() + "_filtered", - ), - PointCloud2, - queue_size=10, - ) - - # publisher for dist_array - self.dist_array_center_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), - ImageMsg, - queue_size=10, - ) - - # publisher for dist_array - self.dist_array_back_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Back/dist_array"), - ImageMsg, - queue_size=10, - ) - - self.dist_array_lidar_publisher = rospy.Publisher( - rospy.get_param( - "~image_distance_topic_cluster", "/paf/hero/dist_clustered" - ), - PointCloud2, - queue_size=10, - ) - rospy.loginfo("dist_array_lidar_publisher erfolgreich erstellt.") - - # publisher for dist_array - self.dist_array_left_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Left/dist_array"), - ImageMsg, - queue_size=10, - ) - - # publisher for dist_array - self.dist_array_right_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Right/dist_array"), - ImageMsg, - queue_size=10, - ) - - rospy.Subscriber( - rospy.get_param("~source_topic", "/carla/hero/LIDAR"), - PointCloud2, - self.callback, - ) - - rospy.loginfo("Lidar Processor Node gestartet.") - rospy.spin() - - def cluster_worker(self): - """ - Worker für das Clustern von Punkten. - """ - while True: - try: - # Neueste Aufgabe holen - data = LidarDistance.cluster_queue.get(timeout=1) - - # Restliche Aufgaben aus der Queue entfernen - discarded_tasks = 0 - while not LidarDistance.cluster_queue.empty(): - LidarDistance.cluster_queue.get() - discarded_tasks += 1 - - # Anzahl verworfener Einträge loggen - if discarded_tasks > 0: - rospy.logwarn(f"{discarded_tasks} Einträge wurden verworfen.") - - # Stop-Signal überprüfen - if data is None: # Stop-Signal - break - - # Punktwolken-Daten verarbeiten - coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) - filtered_coordinates = coordinates[(coordinates["x"] >= 2)] - clustered_points = cluster_lidar_data_from_pointcloud( - coordinates=filtered_coordinates - ) - - # Kombinierte Cluster erstellen - cluster_structured = combine_clusters(clustered_points) - - # Konvertiere zu PointCloud2-Nachricht - pointcloud_msg = ros_numpy.point_cloud2.array_to_pointcloud2( - cluster_structured - ) - pointcloud_msg.header = data.header - pointcloud_msg.header.stamp = rospy.Time.now() - - # Cluster veröffentlichen - self.dist_array_lidar_publisher.publish(pointcloud_msg) - - except queue.Empty: - rospy.loginfo("Cluster-Queue ist leer. Warte auf neue Aufgaben.") - continue # Zurück zur Queue-Warte - - except Exception as e: - rospy.logerr(f"Fehler im Cluster-Worker: {e}") - - def image_worker(self): - """ - Worker für die Berechnung von Bildern. - """ - while True: - data = LidarDistance.image_queue.get() - if data is None: # Stop-Signal - break - - coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) - # Bildverarbeitung auf den Koordinaten durchführen - processed_images = { - "Center": None, - "Back": None, - "Left": None, - "Right": None, - } - processed_images["Center"] = self.calculate_image_center(coordinates) - processed_images["Back"] = self.calculate_image_back(coordinates) - processed_images["Left"] = self.calculate_image_left(coordinates) - processed_images["Right"] = self.calculate_image_right(coordinates) - - self.publish_images(processed_images, data.header) - - def start_cluster_worker(self): - self.cluster_thread = threading.Thread(target=self.cluster_worker, daemon=True) - self.cluster_thread.start() - - def start_image_worker(self): - self.image_thread = threading.Thread(target=self.image_worker, daemon=True) - self.image_thread.start() - - def calculate_image_center(self, coordinates): - reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( - coordinates, - max_x=np.inf, - min_x=0.0, - min_z=-1.6, - ) - reconstruct_coordinates_center = coordinates[reconstruct_bit_mask_center] - reconstruct_coordinates_xyz_center = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates_center, "intensity" - ).tolist() - ) - dist_array_center = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_center, focus="Center" - ) - return dist_array_center - - def calculate_image_back(self, coordinates): - # Back - reconstruct_bit_mask_back = lidar_filter_utility.bounding_box( - coordinates, - max_x=0.0, - min_x=-np.inf, - min_z=-1.6, - ) - reconstruct_coordinates_back = coordinates[reconstruct_bit_mask_back] - reconstruct_coordinates_xyz_back = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates_back, "intensity" - ).tolist() - ) - dist_array_back = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_back, focus="Back" - ) - return dist_array_back - - def calculate_image_left(self, coordinates): - # Left - reconstruct_bit_mask_left = lidar_filter_utility.bounding_box( - coordinates, - max_y=np.inf, - min_y=0.0, - min_z=-1.6, - ) - reconstruct_coordinates_left = coordinates[reconstruct_bit_mask_left] - reconstruct_coordinates_xyz_left = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates_left, "intensity" - ).tolist() - ) - dist_array_left = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_left, focus="Left" - ) - return dist_array_left - - def calculate_image_right(self, coordinates): - # Right - reconstruct_bit_mask_right = lidar_filter_utility.bounding_box( - coordinates, max_y=-0.0, min_y=-np.inf, min_z=-1.6 - ) - reconstruct_coordinates_right = coordinates[reconstruct_bit_mask_right] - reconstruct_coordinates_xyz_right = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates_right, "intensity" - ).tolist() - ) - dist_array_right = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_right, focus="Right" - ) - return dist_array_right - - def publish_images(self, processed_images, data_header): - for direction, image_array in processed_images.items(): - if not isinstance(image_array, np.ndarray): - continue - dist_array_msg = self.bridge.cv2_to_imgmsg( - image_array, encoding="passthrough" - ) - dist_array_msg.header = data_header - - if direction == "Center": - self.dist_array_center_publisher.publish(dist_array_msg) - if direction == "Back": - self.dist_array_back_publisher.publish(dist_array_msg) - if direction == "Left": - self.dist_array_left_publisher.publish(dist_array_msg) - if direction == "Right": - self.dist_array_right_publisher.publish(dist_array_msg) - - def reconstruct_img_from_lidar(self, coordinates_xyz, focus): - """ - reconstruct 3d LIDAR-Data and calculate 2D Pixel - according to Camera-World - - Args: - coordinates_xyz (np.array): filtered lidar points - focus (String): Camera Angle - - Returns: - image: depth image for camera angle - """ - - # intrinsic matrix for camera: - # width -> 300, height -> 200, fov -> 100 (agent.py) - im = np.identity(3) - im[0, 2] = 1280 / 2.0 - im[1, 2] = 720 / 2.0 - im[0, 0] = im[1, 1] = 1280 / (2.0 * np.tan(100 * np.pi / 360.0)) - - # extrinsic matrix for camera - ex = np.zeros(shape=(3, 4)) - ex[0][0] = 1 - ex[1][1] = 1 - ex[2][2] = 1 - m = np.matmul(im, ex) - - # reconstruct camera image with LIDAR-Data - img = np.zeros(shape=(720, 1280), dtype=np.float32) - dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32) - for c in coordinates_xyz: - # center depth image - if focus == "Center": - point = np.array([c[1], c[2], c[0], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if x >= 0 and x <= 1280 and y >= 0 and y <= 720: - img[719 - y][1279 - x] = c[0] - dist_array[719 - y][1279 - x] = np.array( - [c[0], c[1], c[2]], dtype=np.float32 - ) - - # back depth image - if focus == "Back": - point = np.array([c[1], c[2], c[0], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if x >= 0 and x <= 1280 and y >= 0 and y < 720: - img[y][1279 - x] = -c[0] - dist_array[y][1279 - x] = np.array( - [-c[0], c[1], c[2]], dtype=np.float32 - ) - - # left depth image - if focus == "Left": - point = np.array([c[0], c[2], c[1], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if x >= 0 and x <= 1280 and y >= 0 and y <= 720: - img[719 - y][1279 - x] = c[1] - dist_array[y][1279 - x] = np.array( - [c[0], c[1], c[2]], dtype=np.float32 - ) - - # right depth image - if focus == "Right": - point = np.array([c[0], c[2], c[1], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if x >= 0 and x < 1280 and y >= 0 and y < 720: - img[y][1279 - x] = -c[1] - dist_array[y][1279 - x] = np.array( - [c[0], c[1], c[2]], dtype=np.float32 - ) - - return dist_array - - -def array_to_pointcloud2(points, header="hero/Lidar"): - points_array = np.array(points) - - points_structured = np.array( - [(p[0], p[1], p[2]) for p in points_array], - dtype=[("x", "f4"), ("y", "f4"), ("z", "f4")], - ) - - pointcloud_msg = ros_numpy.point_cloud2.array_to_pointcloud2(points_structured) - - pointcloud_msg.header.stamp = rospy.Time.now() - pointcloud_msg.header = header - - return pointcloud_msg - - -def get_largest_cluster(clustered_points, return_structured=True): - """ - Bestimmt das größte Cluster aus den gegebenen Punkten und gibt es zurück. - - :param clustered_points: Dictionary mit Cluster-IDs als Schlüssel und Punktwolken als Werte. - :param return_structured: Gibt ein strukturiertes Array zurück, falls True. - :return: Größtes Cluster als Array (entweder raw oder strukturiert). - """ - if not clustered_points: - rospy.logerr( - "Clustered points are empty. Cannot determine the largest cluster." - ) - return np.array([]) - - # Finde das größte Cluster direkt - largest_cluster_id, largest_cluster = max( - clustered_points.items(), key=lambda item: len(item[1]) - ) - - if largest_cluster.size == 0: - rospy.logerr(f"Largest cluster (ID {largest_cluster_id}) is empty.") - return np.array([]) - - rospy.loginfo( - f"Largest cluster: {largest_cluster_id} with {largest_cluster.shape[0]} points" - ) - - # Falls kein strukturiertes Array benötigt wird, direkt zurückgeben - if not return_structured: - return largest_cluster - - # Andernfalls Punkte in ein strukturiertes Array konvertieren - points_structured = np.empty( - largest_cluster.shape[0], dtype=[("x", "f4"), ("y", "f4"), ("z", "f4")] - ) - points_structured["x"] = largest_cluster[:, 0] - points_structured["y"] = largest_cluster[:, 1] - points_structured["z"] = largest_cluster[:, 2] - - return points_structured - - -def combine_clusters(clustered_points): - """ - Kombiniert Cluster-Daten in ein strukturiertes Array. - - :param clustered_points: Dictionary mit Cluster-IDs als Schlüssel und 3D-Punktwolken als Werten. - :return: Kombiniertes strukturiertes NumPy-Array mit Feldern "x", "y", "z", "cluster_id". - """ - - # Vorab die Gesamtanzahl der Punkte berechnen - total_points = sum(points.shape[0] for points in clustered_points.values()) - - # Erstelle ein leeres strukturiertes Array - combined_points = np.empty( - total_points, - dtype=[("x", "f4"), ("y", "f4"), ("z", "f4"), ("cluster_id", "f4")], - ) - - # Fülle das Array direkt - index = 0 - for cluster_id, points in clustered_points.items(): - num_points = points.shape[0] - combined_points["x"][index : index + num_points] = points[:, 0] - combined_points["y"][index : index + num_points] = points[:, 1] - combined_points["z"][index : index + num_points] = points[:, 2] - combined_points["cluster_id"][index : index + num_points] = cluster_id - index += num_points - - return combined_points - - -def fuse_clusters(cluster_data_list): - """ - Fusioniert eine Liste von Cluster-Daten und entfernt redundante Punkte basierend auf (x, y, z). - - :param cluster_data_list: Liste von NumPy-Arrays mit Feldern (x, y, z, intensity) - :return: Fusioniertes NumPy-Array mit einzigartigen Punkten - """ - # Kombiniere alle Arrays in ein großes Array - concatenated_points = np.concatenate(cluster_data_list, axis=0) - - # Erstelle eine flache Ansicht für die Felder (x, y, z) - unique_xyz, indices = np.unique( - concatenated_points[["x", "y", "z"]].view(np.float32).reshape(-1, 3), - axis=0, - return_index=True, - ) - - # Behalte die entsprechenden `intensity`-Werte basierend auf den eindeutigen Indizes - unique_points = concatenated_points[indices] - - return unique_points - - -def cluster_lidar_data_from_pointcloud(coordinates, eps=0.1, min_samples=10): - """ - Cluster LiDAR-Daten effizient mit DBSCAN. - """ - # Erstelle Matrix direkt mit column_stack (schneller als vstack) - xyz = np.column_stack((coordinates["x"], coordinates["y"], coordinates["z"])) - - # DBSCAN-Cluster-Berechnung - clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(xyz) - labels = clustering.labels_ - - # Nur eindeutige Cluster (Rauschen entfernen) - unique_labels = np.unique(labels) - valid_labels = unique_labels[unique_labels != -1] - - # Parallelisiertes Extrahieren der Cluster - clusters = Parallel(n_jobs=-1)( - delayed(lambda l: (l, xyz[labels == l]))(label) for label in valid_labels - ) - clusters = dict(clusters) - - return clusters - - -if __name__ == "__main__": - lidar_distance = LidarDistance() - lidar_distance.listener() From 279944884dd72fe2a21180768d402da77bdce794 Mon Sep 17 00:00:00 2001 From: Ralf Date: Wed, 11 Dec 2024 15:12:52 +0100 Subject: [PATCH 08/37] added code documentation --- code/perception/src/radar_node.py | 315 +++++++++++++++++------------- 1 file changed, 174 insertions(+), 141 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index ddff6511..975af2b1 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -27,34 +27,29 @@ def callback(self, data): Args: data: Point2Cloud message containing radar data """ - # clustered_points = cluster_radar_data_from_pointcloud(data, 10) - # clustered_points_json = json.dumps(clustered_points) - # self.dist_array_radar_publisher.publish(clustered_points_json) - # output array [x, y, z, distance] dataarray = pointcloud2_to_array(data) - # input array [x, y, z, distance], max_distance, output: filtered data - # dataarray = filter_data(dataarray, 10) + # radar position z=0.7 + dataarray = filter_data(dataarray, min_z=-0.6) - # input array [x, y, z, distance], output: dict clustered clustered_data = cluster_data(dataarray) # transformed_data = transform_data_to_2d(dataarray) - # input array [x, y, z, distance], clustered labels cloud = create_pointcloud2(dataarray, clustered_data.labels_) self.visualization_radar_publisher.publish(cloud) - points_with_labels = np.hstack((dataarray, clustered_data.labels_.reshape(-1, 1))) + points_with_labels = np.hstack((dataarray, + clustered_data.labels_.reshape(-1, 1))) bounding_boxes = generate_bounding_boxes(points_with_labels) marker_array = MarkerArray() - # marker_array = clear_old_markers(marker_array) for label, bbox in bounding_boxes: if label != -1: marker = create_bounding_box_marker(label, bbox) marker_array.markers.append(marker) + # can be used for extra debugging # min_marker, max_marker = create_min_max_markers(label, bbox) # marker_array.markers.append(min_marker) # marker_array.markers.append(max_marker) @@ -63,7 +58,8 @@ def callback(self, data): self.marker_visualization_radar_publisher.publish(marker_array) - cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray, marker_array, bounding_boxes) + cluster_info = generate_cluster_info(clustered_data, dataarray, marker_array, + bounding_boxes) self.cluster_info_radar_publisher.publish(cluster_info) def listener(self): @@ -117,93 +113,73 @@ def pointcloud2_to_array(pointcloud_msg): Returns: - np.ndarray A 2D array where each row corresponds to a point in the point cloud: - [x, y, z, distance], where "distance" is the distance from the origin. + [x, y, z, Velocity] """ cloud_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud_msg) - # distances = np.sqrt( - # cloud_array["x"] ** 2 + cloud_array["y"] ** 2 + cloud_array["z"] ** 2 - # ) - # return np.column_stack( - # (cloud_array["x"], cloud_array["y"], cloud_array["z"], distances) - # ) return np.column_stack( (cloud_array["x"], cloud_array["y"], cloud_array["z"], cloud_array["Velocity"]) ) -def cluster_radar_data_from_pointcloud( - pointcloud_msg, max_distance, eps=1.0, min_samples=2 -): +def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, max_z=100, + max_distance=100): """ - Filters and clusters points from a ROS PointCloud2 message based on DBSCAN - clustering. + Filters radar data based on specified spatial and distance constraints. - Parameters: - - pointcloud_msg: sensor_msgs/PointCloud2 - The ROS PointCloud2 message containing the 3D points. - - max_distance: float - Maximum distance to consider points. Points beyond this distance are - discarded. - - eps: float, optional (default: 1.0) - The maximum distance between two points for them to be considered in - the same cluster. - - min_samples: int, optional (default: 2) - The minimum number of points required to form a cluster. + This function applies multiple filtering criteria to the input radar data. + Points outside these bounds are excluded from the output. + + Args: + data (np.ndarray): A 2D numpy array containing radar data, where each row + represents a data point with the format [x, y, z, distance]. The array + shape is (N, 4), where N is the number of points. + min_x (float, optional): Minimum value for the x-coordinate. Default is -1. + max_x (float, optional): Maximum value for the x-coordinate. Default is 1. + min_y (float, optional): Minimum value for the y-coordinate. Default is 1. + max_y (float, optional): Maximum value for the y-coordinate. Default is 1. + min_z (float, optional): Minimum value for the z-coordinate. Default is -0.7. + max_z (float, optional): Maximum value for the z-coordinate. Default is 1.3. + max_distance (float, optional): Maximum allowable distance of the point from + the sensor. Default is 100. Returns: - - dict - A dictionary where the keys are cluster labels (int) and the values - are the number of points in each cluster. Returns an empty dictionary - if no points are available. + np.ndarray: A numpy array containing only the filtered data points that meet + the specified criteria. """ - data = pointcloud2_to_array(pointcloud_msg) + filtered_data = data[data[:, 3] < max_distance] filtered_data = filtered_data[ - (filtered_data[:, 1] >= -1) - & (filtered_data[:, 1] <= 1) - & (filtered_data[:, 2] <= 1.3) - & (filtered_data[:, 2] >= -0.7) + (filtered_data[:, 0] >= min_x) + & (filtered_data[:, 0] <= max_x) + & (filtered_data[:, 1] >= min_y) + & (filtered_data[:, 1] <= max_y) + & (filtered_data[:, 2] <= max_z) + & (filtered_data[:, 2] >= min_z) ] - if len(filtered_data) == 0: - return {} - coordinates = filtered_data[:, :2] - clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) - labels = clustering.labels_ - clustered_points = {label: list(labels).count(label) for label in set(labels)} - clustered_points = {int(label): count for label, count in clustered_points.items()} - return clustered_points - + return filtered_data -# filters radar data in distance, y, z direction -def filter_data(data, max_distance): - # filtered_data = data[data[:, 3] < max_distance] - filtered_data = data - filtered_data = filtered_data[ - # (filtered_data[:, 1] >= -1) - # & (filtered_data[:, 1] <= 1) - # & (filtered_data[:, 2] <= 1.3) - (filtered_data[:, 2] <= 1.3) - & (filtered_data[:, 2] >= -0.6) # -0.7 - ] - return filtered_data +def cluster_data(data, eps=0.8, min_samples=3): + """_summary_ + Args: + data (np.ndarray): data array which should be clustered + eps (float, optional): maximum distance of points. Defaults to 0.8. + min_samples (int, optional): min samples for 1 cluster. Defaults to 3. -# clusters data with DBSCAN -def cluster_data(filtered_data, eps=0.8, min_samples=5): + Returns: + dict: A dictionary where the keys are cluster labels (int) and the values + are the number of points in each cluster. Returns an empty dictionary + if no points are available. + """ - if len(filtered_data) == 0: + if len(data) == 0: return {} - # coordinates = filtered_data[:, :2] - # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) - - # worse than without scaling scaler = StandardScaler() - data_scaled = scaler.fit_transform(filtered_data) - clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled) + data_scaled = scaler.fit_transform(data) + clustered_points = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled) - # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(filtered_data) - return clustering + return clustered_points # generates random color for cluster @@ -213,8 +189,16 @@ def generate_color_map(num_clusters): return colors -# creates pointcloud2 for publishing clustered radar data def create_pointcloud2(clustered_points, cluster_labels): + """_summary_ + + Args: + clustered_points (dict): clustered points after dbscan + cluster_labels (_type_): _description_ + + Returns: + PointCloud2: pointcloud which can be published + """ header = Header() header.stamp = rospy.Time.now() header.frame_id = "hero/RADAR" @@ -224,7 +208,6 @@ def create_pointcloud2(clustered_points, cluster_labels): colors = generate_color_map(len(unique_labels)) for i, point in enumerate(clustered_points): - # x, y, z, _ = point x, y, z, v = point label = cluster_labels[i] @@ -247,6 +230,14 @@ def create_pointcloud2(clustered_points, cluster_labels): def transform_data_to_2d(clustered_data): + """_summary_ + + Args: + clustered_data (np.ndarray): clustered 3d data points + + Returns: + _np.ndarray: clustered points, every z value is set to 0 + """ transformed_points = clustered_data transformed_points[:, 0] = clustered_data[:, 0] @@ -258,13 +249,23 @@ def transform_data_to_2d(clustered_data): def calculate_aabb(cluster_points): - """_summary_ + """ + Calculates the axis-aligned bounding box (AABB) for a set of 3D points. + + This function computes the minimum and maximum values along each axis (x, y, z) + for a given set of 3D points, which defines the bounding box that contains + all points in the cluster. Args: - cluster_points (_type_): _description_ + cluster_points (numpy.ndarray): + A 2D array where each row represents a 3D point (x, y, z). + The array should have shape (N, 3) where N is the number of points. Returns: - _type_: _description_ + tuple: A tuple of the form (x_min, x_max, y_min, y_max, z_min, z_max), + which represents the axis-aligned bounding box (AABB) for the given + set of points. The values are the minimum and maximum coordinates + along the x, y, and z axes. """ # for 2d (top-down) boxes @@ -272,7 +273,7 @@ def calculate_aabb(cluster_points): # x_max = np.max(cluster_points[:, 0]) # y_min = np.min(cluster_points[:, 1]) # y_max = np.max(cluster_points[:, 1]) - + # rospy.loginfo(f"Bounding box: X({x_min}, {x_max}), Y({y_min}, {y_max})") # return x_min, x_max, y_min, y_max # for 3d boxes @@ -282,18 +283,27 @@ def calculate_aabb(cluster_points): y_max = np.max(cluster_points[:, 1]) z_min = np.min(cluster_points[:, 2]) z_max = np.max(cluster_points[:, 2]) - rospy.loginfo(f"Bounding box for label: X({x_min}, {x_max}), Y({y_min}, {y_max}), Z({z_min}, {z_max})") return x_min, x_max, y_min, y_max, z_min, z_max def generate_bounding_boxes(points_with_labels): - """_summary_ + """ + Generates bounding boxes for clustered points. + + This function processes a set of points, each associated with a cluster label, + and generates an axis-aligned bounding box (AABB) for each unique cluster label. Args: - points_with_labels (_type_): _description_ + points_with_labels (numpy.ndarray): + A 2D array of shape (N, 4) where each row contains + the coordinates (x, y, z) of a point along with its + corresponding cluster label in the last column. + The array should have the structure [x, y, z, label]. Returns: - _type_: _description_ + list: A list of tuples, where each tuple contains a cluster label and the + corresponding bounding box (bbox). The bbox is represented by a tuple + of the form (x_min, x_max, y_min, y_max, z_min, z_max). """ bounding_boxes = [] unique_labels = np.unique(points_with_labels[:, -1]) @@ -307,10 +317,23 @@ def generate_bounding_boxes(points_with_labels): def create_bounding_box_marker(label, bbox): - """_summary_ + """ + Creates an RViz Marker for visualizing a 3D bounding box. + + This function generates a Marker object for RViz to visualize a 3D bounding box + based on the provided label and bounding box dimensions. The marker is + represented as a series of lines connecting the corners of the box. + + Args: + label (int): The unique identifier for the cluster or object to which the + bounding box belongs. This label is used as the Marker ID. + bbox (tuple): A tuple containing the min and max coordinates of the bounding box + in the format (x_min, x_max, y_min, y_max, z_min, z_max). Returns: - _type_: _description_ + Marker: A Marker object that can be published to RViz to display the + 3D bounding box. The marker is of type LINE_LIST, + representing the edges of the bounding box. """ # for 2d (top-down) boxes # x_min, x_max, y_min, y_max = bbox @@ -321,8 +344,8 @@ def create_bounding_box_marker(label, bbox): marker = Marker() marker.header.frame_id = "hero/RADAR" marker.id = int(label) - # marker.type = Marker.LINE_STRIP - marker.type = Marker.LINE_LIST + # marker.type = Marker.LINE_STRIP # 2d boxes + marker.type = Marker.LINE_LIST # 3d boxes marker.action = Marker.ADD marker.scale.x = 0.1 marker.color.r = 1.0 @@ -342,32 +365,20 @@ def create_bounding_box_marker(label, bbox): # for 3d boxes points = [ - Point(x_min, y_min, z_min), # Ecke 0 - Point(x_max, y_min, z_min), # Ecke 1 - Point(x_max, y_max, z_min), # Ecke 2 - Point(x_min, y_max, z_min), # Ecke 3 - Point(x_min, y_min, z_max), # Ecke 4 - Point(x_max, y_min, z_max), # Ecke 5 - Point(x_max, y_max, z_max), # Ecke 6 - Point(x_min, y_max, z_max), # Ecke 7 - - # Point(x_min, y_min, z_min), # Verbinde z_min zu z_max - # Point(x_min, y_min, z_max), - - # Point(x_max, y_min, z_min), - # Point(x_max, y_min, z_max), - - # Point(x_max, y_max, z_min), - # Point(x_max, y_max, z_max), - - # Point(x_min, y_max, z_min), - # Point(x_min, y_max, z_max), + Point(x_min, y_min, z_min), + Point(x_max, y_min, z_min), + Point(x_max, y_max, z_min), + Point(x_min, y_max, z_min), + Point(x_min, y_min, z_max), + Point(x_max, y_min, z_max), + Point(x_max, y_max, z_max), + Point(x_min, y_max, z_max), ] - # marker.points = points + lines = [ - (0, 1), (1, 2), (2, 3), (3, 0), # Boden - (4, 5), (5, 6), (6, 7), (7, 4), # Deckel - (0, 4), (1, 5), (2, 6), (3, 7), # Vertikale Kanten + (0, 1), (1, 2), (2, 3), (3, 0), # Bottom + (4, 5), (5, 6), (6, 7), (7, 4), # Top + (0, 4), (1, 5), (2, 6), (3, 7), # Vertical Edges ] for start, end in lines: marker.points.append(points[start]) @@ -376,41 +387,33 @@ def create_bounding_box_marker(label, bbox): return marker -def create_min_max_markers(label, bbox, frame_id="hero/RADAR", min_color=(0.0, 1.0, 0.0, 1.0), max_color=(1.0, 0.0, 0.0, 1.0)): +# can be used for extra debugging +def create_min_max_markers(label, bbox, frame_id="hero/RADAR", + min_color=(0.0, 1.0, 0.0, 1.0), + max_color=(1.0, 0.0, 0.0, 1.0)): """ - Erstellt RViz-Marker für die Min- und Max-Punkte einer Bounding Box. + creates RViz-Markers for min- and max-points of a bounding box. Args: - label (int): Die ID des Clusters (wird als Marker-ID genutzt). - bbox (tuple): Min- und Max-Werte der Bounding Box (x_min, x_max, y_min, y_max, z_min, z_max). - frame_id (str): Frame ID, in dem die Marker gezeichnet werden. - min_color (tuple): RGBA-Farbwerte für den Min-Punkt-Marker. - max_color (tuple): RGBA-Farbwerte für den Max-Punkt-Marker. + label (int): cluster-id (used as marker-ID in rviz). + bbox (tuple): min- and max-values of bounding box + (x_min, x_max, y_min, y_max, z_min, z_max). + frame_id (str): frame ID for markers + min_color (tuple): RGBA-value for min-point-marker + max_color (tuple): RGBA-value for max-point-marker Returns: - tuple: Ein Paar von Markern (min_marker, max_marker). + tuple: pair of markers (min_marker, max_marker). """ x_min, x_max, y_min, y_max, z_min, z_max = bbox - # marker = Marker() - # marker.header.frame_id = "hero/RADAR" - # marker.id = int(label) - # # marker.type = Marker.LINE_STRIP - # marker.type = Marker.LINE_LIST - # marker.action = Marker.ADD - # marker.scale.x = 0.1 - # marker.color.r = 1.0 - # marker.color.g = 1.0 - # marker.color.b = 0.0 - # marker.color.a = 1.0 - - # Min-Punkt-Marker + # min-point-marker min_marker = Marker() min_marker.header.frame_id = frame_id - min_marker.id = int(label * 10) # ID für Min-Punkt + min_marker.id = int(label * 10) min_marker.type = Marker.SPHERE min_marker.action = Marker.ADD - min_marker.scale.x = 0.2 # Größe des Punktes + min_marker.scale.x = 0.2 min_marker.scale.y = 0.2 min_marker.scale.z = 0.2 min_marker.color.r = min_color[0] @@ -421,10 +424,10 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR", min_color=(0.0, 1 min_marker.pose.position.y = y_min min_marker.pose.position.z = z_min - # Max-Punkt-Marker + # max-point-marker max_marker = Marker() max_marker.header.frame_id = frame_id - max_marker.id = int(label * 10 + 1) # ID für Max-Punkt + max_marker.id = int(label * 10 + 1) max_marker.type = Marker.SPHERE max_marker.action = Marker.ADD max_marker.scale.x = 0.2 @@ -442,15 +445,45 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR", min_color=(0.0, 1 def clear_old_markers(marker_array, max_id): - """Löscht alte Marker aus dem MarkerArray.""" + """ + Removes old markers from the given MarkerArray by setting the action + to DELETE for markers with an ID greater than or equal to max_id. + + Args: + marker_array (MarkerArray): The current MarkerArray containing all markers. + max_id (int): The highest ID of the new markers. Markers with an ID + greater than or equal to this value will be marked for deletion. + + Returns: + MarkerArray: The updated MarkerArray with old markers removed. + """ for marker in marker_array.markers: if marker.id >= max_id: marker.action = Marker.DELETE return marker_array -# generates string with label-id and cluster size -def generate_cluster_labels_and_colors(clusters, data, marker_array, bounding_boxes): +# generates string with label-id and cluster size, can be used for extra debugging +def generate_cluster_info(clusters, data, marker_array, bounding_boxes): + """ + Generates information about clusters, including the label, number of points, + markers, and bounding boxes. + + Args: + clusters (DBSCAN): The clustered data, containing the labels for each point. + data (numpy.ndarray): + The point cloud data, typically with columns [x, y, z, distance]. + marker_array (MarkerArray): + The array of RViz markers associated with the clusters. + bounding_boxes (list): The list of bounding boxes for each detected object. + + Returns: + str: A JSON string containing the information about each cluster, including: + - "label": The cluster label. + - "points_count": The number of points in the cluster. + - "Anzahl marker": The number of markers in the MarkerArray. + - "Anzahl Boundingboxen": The number of bounding boxes. + """ cluster_info = [] for label in set(clusters.labels_): From 57e720d6119685ac63d3e276dcbb11c9c4a59aaa Mon Sep 17 00:00:00 2001 From: Ralf Date: Wed, 11 Dec 2024 15:23:41 +0100 Subject: [PATCH 09/37] removed trailing white spaces --- code/perception/src/radar_node.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 975af2b1..aecbeca2 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -130,8 +130,8 @@ def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, ma Points outside these bounds are excluded from the output. Args: - data (np.ndarray): A 2D numpy array containing radar data, where each row - represents a data point with the format [x, y, z, distance]. The array + data (np.ndarray): A 2D numpy array containing radar data, where each row + represents a data point with the format [x, y, z, distance]. The array shape is (N, 4), where N is the number of points. min_x (float, optional): Minimum value for the x-coordinate. Default is -1. max_x (float, optional): Maximum value for the x-coordinate. Default is 1. @@ -143,7 +143,7 @@ def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, ma the sensor. Default is 100. Returns: - np.ndarray: A numpy array containing only the filtered data points that meet + np.ndarray: A numpy array containing only the filtered data points that meet the specified criteria. """ @@ -257,7 +257,7 @@ def calculate_aabb(cluster_points): all points in the cluster. Args: - cluster_points (numpy.ndarray): + cluster_points (numpy.ndarray): A 2D array where each row represents a 3D point (x, y, z). The array should have shape (N, 3) where N is the number of points. @@ -396,7 +396,7 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR", Args: label (int): cluster-id (used as marker-ID in rviz). - bbox (tuple): min- and max-values of bounding box + bbox (tuple): min- and max-values of bounding box (x_min, x_max, y_min, y_max, z_min, z_max). frame_id (str): frame ID for markers min_color (tuple): RGBA-value for min-point-marker @@ -410,10 +410,10 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR", # min-point-marker min_marker = Marker() min_marker.header.frame_id = frame_id - min_marker.id = int(label * 10) + min_marker.id = int(label * 10) min_marker.type = Marker.SPHERE min_marker.action = Marker.ADD - min_marker.scale.x = 0.2 + min_marker.scale.x = 0.2 min_marker.scale.y = 0.2 min_marker.scale.z = 0.2 min_marker.color.r = min_color[0] @@ -446,12 +446,12 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR", def clear_old_markers(marker_array, max_id): """ - Removes old markers from the given MarkerArray by setting the action + Removes old markers from the given MarkerArray by setting the action to DELETE for markers with an ID greater than or equal to max_id. Args: marker_array (MarkerArray): The current MarkerArray containing all markers. - max_id (int): The highest ID of the new markers. Markers with an ID + max_id (int): The highest ID of the new markers. Markers with an ID greater than or equal to this value will be marked for deletion. Returns: From 78afbb6b82b7298674f15ab50584d6951c679f17 Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 11 Dec 2024 15:34:04 +0100 Subject: [PATCH 10/37] Applied some suggestions by CodeRabbit. Deleted function get_largest_cluster which was not used anywhere. --- code/perception/src/lidar_distance.py | 327 ++++++++++---------------- 1 file changed, 122 insertions(+), 205 deletions(-) diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 260cbea8..fa8ce676 100644 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -18,28 +18,28 @@ class LidarDistance: how to configute this node """ - cluster_buffer = [] + def __init__(self): + self.cluster_buffer = [] def callback(self, data): """ - Callback-Funktion, die LiDAR-Punktwolkendaten verarbeitet. + Callback function that processes LiDAR point cloud data. - Führt Clustering und Bildberechnungen für die Punktwolken aus. + Executes clustering and image calculations for the provided point cloud. - :param data: LiDAR-Punktwolken als ROS PointCloud2-Nachricht. + :param data: LiDAR point cloud as a ROS PointCloud2 message. """ - self.start_clustering(data) self.start_image_calculation(data) def listener(self): """ - Initialisiert die ROS-Node, erstellt Publisher/Subscriber und hält sie aktiv. + Initializes the ROS node, creates publishers/subscribers, and keeps it active. """ rospy.init_node("lidar_distance") - self.bridge = CvBridge() # OpenCV-Bridge für Bildkonvertierungen + self.bridge = CvBridge() # OpenCV bridge for image conversions - # Publisher für gefilterte Punktwolken + # Publisher for filtered point clouds self.pub_pointcloud = rospy.Publisher( rospy.get_param( "~point_cloud_topic", @@ -49,7 +49,7 @@ def listener(self): queue_size=1, ) - # Publisher für Distanzbilder in verschiedene Richtungen + # Publishers for distance images in various directions self.dist_array_center_publisher = rospy.Publisher( rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), ImageMsg, @@ -67,7 +67,7 @@ def listener(self): PointCloud2, queue_size=10, ) - rospy.loginfo("dist_array_lidar_publisher erfolgreich erstellt.") + rospy.loginfo("dist_array_lidar_publisher successfully created.") self.dist_array_left_publisher = rospy.Publisher( rospy.get_param("~image_distance_topic", "/paf/hero/Left/dist_array"), ImageMsg, @@ -79,168 +79,125 @@ def listener(self): queue_size=10, ) - # Subscriber für LiDAR-Daten (Punktwolken) + # Subscriber for LiDAR data (point clouds) rospy.Subscriber( rospy.get_param("~source_topic", "/carla/hero/LIDAR"), PointCloud2, self.callback, ) - rospy.loginfo("Lidar Processor Node gestartet.") + rospy.loginfo("Lidar Processor Node started.") rospy.spin() def start_clustering(self, data): """ - Filtert LiDAR-Punktwolken, führt Clustering durch und veröffentlicht die kombinierten Cluster. + Filters LiDAR point clouds, performs clustering, and publishes the combined clusters. - :param data: LiDAR-Punktwolken im ROS PointCloud2-Format. + :param data: LiDAR point clouds in ROS PointCloud2 format. """ - # Punktwolken filtern, um irrelevante Daten zu entfernen + # Filter point clouds to remove irrelevant data coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) filtered_coordinates = coordinates[ ~( - (-2 <= coordinates["x"]) + (coordinates["x"] >= -2) & (coordinates["x"] <= 2) - & (-1 <= coordinates["y"]) + & (coordinates["y"] >= -1) & (coordinates["y"] <= 1) - ) # Ausschluss von Punkten die das eigene Auto betreffen + ) # Exclude points related to the ego vehicle & ( coordinates["z"] > -1.7 + 0.05 - ) # Mindesthöhe in z, um die Straße nicht zu clustern + ) # Minimum height in z to exclude the road ] - # Cluster-Daten aus den gefilterten Koordinaten berechnen + # Compute cluster data from the filtered coordinates clustered_points = cluster_lidar_data_from_pointcloud( coordinates=filtered_coordinates ) - # Nur gültige Cluster-Daten speichern + # Only store valid cluster data if clustered_points: - LidarDistance.cluster_buffer.append(clustered_points) + self.cluster_buffer.append(clustered_points) else: - rospy.logwarn("Keine Cluster-Daten erzeugt.") + rospy.logwarn("No cluster data generated.") - # Cluster kombinieren - combined_clusters = combine_clusters(LidarDistance.cluster_buffer) + # Combine clusters + combined_clusters = combine_clusters(self.cluster_buffer) - LidarDistance.cluster_buffer = [] + self.cluster_buffer = [] - # Veröffentliche die kombinierten Cluster + # Publish the combined clusters self.publish_clusters(combined_clusters, data.header) def publish_clusters(self, combined_clusters, data_header): """ - Veröffentlicht kombinierte Cluster als ROS PointCloud2-Nachricht. + Publishes combined clusters as a ROS PointCloud2 message. - :param combined_clusters: Kombinierte Punktwolken der Cluster als strukturiertes NumPy-Array. - :param data_header: Header-Informationen der ROS-Nachricht. + :param combined_clusters: Combined point clouds of the clusters as a structured NumPy array. + :param data_header: Header information for the ROS message. """ - # Konvertiere zu PointCloud2-Nachricht + # Convert to a PointCloud2 message pointcloud_msg = ros_numpy.point_cloud2.array_to_pointcloud2(combined_clusters) pointcloud_msg.header = data_header pointcloud_msg.header.stamp = rospy.Time.now() - # Cluster veröffentlichen + # Publish the clusters self.dist_array_lidar_publisher.publish(pointcloud_msg) def start_image_calculation(self, data): """ - Berechnet Distanzbilder basierend auf LiDAR-Daten und veröffentlicht sie. + Computes distance images based on LiDAR data and publishes them. - :param data: LiDAR-Punktwolken im ROS PointCloud2-Format. + :param data: LiDAR point cloud in ROS PointCloud2 format. """ coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) - # Bildverarbeitung auf den Koordinaten durchführen + + # Directions to process + directions = ["Center", "Back", "Left", "Right"] + + # Process images for all directions processed_images = { - "Center": None, - "Back": None, - "Left": None, - "Right": None, + direction: self.calculate_image(coordinates, focus=direction) + for direction in directions } - processed_images["Center"] = self.calculate_image_center(coordinates) - processed_images["Back"] = self.calculate_image_back(coordinates) - processed_images["Left"] = self.calculate_image_left(coordinates) - processed_images["Right"] = self.calculate_image_right(coordinates) + # Publish the processed images self.publish_images(processed_images, data.header) - def calculate_image_center(self, coordinates): + def calculate_image(self, coordinates, focus): """ - Berechnet ein Distanzbild für die zentrale Ansicht aus LiDAR-Koordinaten. + Calculates a distance image for a specific focus (view direction) from LiDAR coordinates. - :param coordinates: Gefilterte LiDAR-Koordinaten als NumPy-Array. - :return: Distanzbild als 2D-Array. + :param coordinates: Filtered LiDAR coordinates as a NumPy array. + :param focus: The focus direction ("Center", "Back", "Left", "Right"). + :return: Distance image as a 2D array. """ - reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( - coordinates, - max_x=np.inf, - min_x=0.0, - min_z=-1.6, - ) - reconstruct_coordinates_center = coordinates[reconstruct_bit_mask_center] - reconstruct_coordinates_xyz_center = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates_center, "intensity" - ).tolist() - ) - dist_array_center = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_center, focus="Center" - ) - return dist_array_center - - def calculate_image_back(self, coordinates): - # Back - reconstruct_bit_mask_back = lidar_filter_utility.bounding_box( - coordinates, - max_x=0.0, - min_x=-np.inf, - min_z=-1.6, - ) - reconstruct_coordinates_back = coordinates[reconstruct_bit_mask_back] - reconstruct_coordinates_xyz_back = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates_back, "intensity" - ).tolist() - ) - dist_array_back = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_back, focus="Back" - ) - return dist_array_back - - def calculate_image_left(self, coordinates): - # Left - reconstruct_bit_mask_left = lidar_filter_utility.bounding_box( - coordinates, - max_y=np.inf, - min_y=0.0, - min_z=-1.6, - ) - reconstruct_coordinates_left = coordinates[reconstruct_bit_mask_left] - reconstruct_coordinates_xyz_left = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates_left, "intensity" - ).tolist() - ) - dist_array_left = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_left, focus="Left" - ) - return dist_array_left + # Define bounding box parameters based on focus direction + bounding_box_params = { + "Center": {"max_x": np.inf, "min_x": 0.0, "min_z": -1.6}, + "Back": {"max_x": 0.0, "min_x": -np.inf, "min_z": -1.6}, + "Left": {"max_y": np.inf, "min_y": 0.0, "min_z": -1.6}, + "Right": {"max_y": -0.0, "min_y": -np.inf, "min_z": -1.6}, + } - def calculate_image_right(self, coordinates): - # Right - reconstruct_bit_mask_right = lidar_filter_utility.bounding_box( - coordinates, max_y=-0.0, min_y=-np.inf, min_z=-1.6 - ) - reconstruct_coordinates_right = coordinates[reconstruct_bit_mask_right] - reconstruct_coordinates_xyz_right = np.array( + # Select parameters for the given focus + params = bounding_box_params.get(focus) + if not params: + rospy.logwarn(f"Unknown focus: {focus}. Skipping image calculation.") + return None + + # Apply bounding box filter + reconstruct_bit_mask = lidar_filter_utility.bounding_box(coordinates, **params) + reconstruct_coordinates = coordinates[reconstruct_bit_mask] + + # Remove the "intensity" field and convert to a NumPy array + reconstruct_coordinates_xyz = np.array( lidar_filter_utility.remove_field_name( - reconstruct_coordinates_right, "intensity" + reconstruct_coordinates, "intensity" ).tolist() ) - dist_array_right = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_right, focus="Right" - ) - return dist_array_right + + # Reconstruct the image based on the focus + return self.reconstruct_img_from_lidar(reconstruct_coordinates_xyz, focus=focus) def publish_images(self, processed_images, data_header): """ @@ -271,71 +228,71 @@ def publish_images(self, processed_images, data_header): def reconstruct_img_from_lidar(self, coordinates_xyz, focus): """ - Rekonstruiert ein 2D-Bild aus 3D-LiDAR-Daten für eine gegebene Kameraansicht. + Reconstructs a 2D image from 3D LiDAR data for a given camera perspective. - :param coordinates_xyz: 3D-Koordinaten der gefilterten LiDAR-Punkte. - :param focus: Kameraansicht (z. B. "Center", "Back"). - :return: Rekonstruiertes Bild als 2D-Array. + :param coordinates_xyz: 3D coordinates of the filtered LiDAR points. + :param focus: Camera view (e.g., "Center", "Back"). + :return: Reconstructed image as a 2D array. """ - # Erstelle die intrinsische Kamera-Matrix basierend auf den Bildparametern (FOV, Auflösung) + # Create the intrinsic camera matrix based on image parameters (FOV, resolution) im = np.identity(3) - im[0, 2] = 1280 / 2.0 # x-Verschiebung (Bildmitte) - im[1, 2] = 720 / 2.0 # y-Verschiebung (Bildmitte) + im[0, 2] = 1280 / 2.0 # x-offset (image center) + im[1, 2] = 720 / 2.0 # y-offset (image center) im[0, 0] = im[1, 1] = 1280 / ( 2.0 * np.tan(100 * np.pi / 360.0) - ) # Skalierungsfaktor basierend auf FOV + ) # Scale factor based on FOV - # Erstelle die extrinsische Kamera-Matrix (Identität für keine Transformation) + # Create the extrinsic camera matrix (identity matrix for no transformation) ex = np.zeros(shape=(3, 4)) ex[0][0] = ex[1][1] = ex[2][2] = 1 - m = np.matmul(im, ex) # Kombiniere intrinsische und extrinsische Matrix + m = np.matmul(im, ex) # Combine intrinsic and extrinsic matrices - # Initialisiere leere Bilder für die Rekonstruktion + # Initialize empty images for reconstruction img = np.zeros(shape=(720, 1280), dtype=np.float32) dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32) - # Verarbeite jeden Punkt in der Punktwolke + # Process each point in the point cloud for c in coordinates_xyz: - if focus == "Center": + if focus == "Center": # Compute image for the center view point = np.array([c[1], c[2], c[0], 1]) - pixel = np.matmul( - m, point - ) # Projiziere 3D-Punkt auf 2D-Bildkoordinaten + pixel = np.matmul(m, point) # Project 3D point to 2D image coordinates x, y = int(pixel[0] / pixel[2]), int( pixel[1] / pixel[2] - ) # Normalisiere Koordinaten - if x >= 0 and x <= 1280 and y >= 0 and y <= 720: # Prüfe Bildgrenzen - img[719 - y][1279 - x] = c[0] # Setze Tiefenwert + ) # Normalize coordinates + if ( + 0 <= x <= 1280 and 0 <= y <= 720 + ): # Check if coordinates are within image bounds + img[719 - y][1279 - x] = c[0] # Set depth value dist_array[719 - y][1279 - x] = np.array( [c[0], c[1], c[2]], dtype=np.float32 ) - if focus == "Back": # Berechne Bild für die Rückansicht + if focus == "Back": # Compute image for the rear view point = np.array([c[1], c[2], c[0], 1]) pixel = np.matmul(m, point) x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if x >= 0 and x <= 1280 and y >= 0 and y < 720: + if 0 <= x <= 1280 and 0 <= y < 720: img[y][1279 - x] = -c[0] dist_array[y][1279 - x] = np.array( [-c[0], c[1], c[2]], dtype=np.float32 ) - if focus == "Left": # Berechne Bild für die linke Ansicht + if focus == "Left": # Compute image for the left view point = np.array([c[0], c[2], c[1], 1]) pixel = np.matmul(m, point) x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if x >= 0 and x <= 1280 and y >= 0 and y <= 720: + if 0 <= x <= 1280 and 0 <= y <= 720: img[719 - y][1279 - x] = c[1] dist_array[y][1279 - x] = np.array( [c[0], c[1], c[2]], dtype=np.float32 ) - if focus == "Right": # Berechne Bild für die rechte Ansicht + if focus == "Right": # Compute image for the right view point = np.array([c[0], c[2], c[1], 1]) pixel = np.matmul(m, point) x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if x >= 0 and x < 1280 and y >= 0 and y < 720: + if 0 <= x < 1280 and 0 <= y < 720: img[y][1279 - x] = -c[1] dist_array[y][1279 - x] = np.array( [c[0], c[1], c[2]], dtype=np.float32 @@ -346,100 +303,60 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): def array_to_pointcloud2(points, header="hero/Lidar"): """ - Konvertiert ein Array von Punkten in eine ROS PointCloud2-Nachricht. + Converts an array of points into a ROS PointCloud2 message. - :param points: Array von Punkten mit [x, y, z]-Koordinaten. - :param header: Header-Informationen der ROS PointCloud2-Nachricht. - :return: ROS PointCloud2-Nachricht. + :param points: Array of points with [x, y, z] coordinates. + :param header: Header information for the ROS PointCloud2 message. + :return: ROS PointCloud2 message. """ - # Sicherstellen, dass die Eingabe ein NumPy-Array ist + # Ensure the input is a NumPy array points_array = np.array(points) - # Konvertiere die Punkte in ein strukturiertes Array mit Feldern "x", "y", "z" + # Convert the points into a structured array with fields "x", "y", "z" points_structured = np.array( [(p[0], p[1], p[2]) for p in points_array], dtype=[("x", "f4"), ("y", "f4"), ("z", "f4")], ) - # Erstelle eine PointCloud2-Nachricht aus dem strukturierten Array + # Create a PointCloud2 message from the structured array pointcloud_msg = ros_numpy.point_cloud2.array_to_pointcloud2(points_structured) - # Setze den Zeitstempel und den Header der Nachricht + # Set the timestamp and header for the message pointcloud_msg.header.stamp = rospy.Time.now() pointcloud_msg.header = header return pointcloud_msg -def get_largest_cluster(clustered_points, return_structured=True): - """ - Ermittelt das größte Cluster aus gegebenen Punktwolken-Clustern. - - :param clustered_points: Dictionary mit Cluster-IDs und zugehörigen Punktwolken. - :param return_structured: Gibt ein strukturiertes NumPy-Array zurück, falls True. - :return: Größtes Cluster als NumPy-Array (roh oder strukturiert). - """ - # Prüfen, ob es Cluster gibt - if not clustered_points: - return np.array([]) - - # Identifiziere das größte Cluster basierend auf der Anzahl der Punkte - largest_cluster_id, largest_cluster = max( - clustered_points.items(), key=lambda item: len(item[1]) - ) - - # Sicherstellen, dass das größte Cluster nicht leer ist - if largest_cluster.size == 0: - return np.array([]) - - rospy.loginfo( - f"Largest cluster: {largest_cluster_id} with {largest_cluster.shape[0]} points" - ) - - # Rohdaten zurückgeben, wenn kein strukturiertes Array benötigt wird - if not return_structured: - return largest_cluster - - # Konvertiere das größte Cluster in ein strukturiertes Array - points_structured = np.empty( - largest_cluster.shape[0], dtype=[("x", "f4"), ("y", "f4"), ("z", "f4")] - ) - points_structured["x"] = largest_cluster[:, 0] - points_structured["y"] = largest_cluster[:, 1] - points_structured["z"] = largest_cluster[:, 2] - - return points_structured - - def combine_clusters(cluster_buffer): """ - Kombiniert Cluster aus mehreren Punktwolken zu einem strukturierten NumPy-Array. + Combines clusters from multiple point clouds into a structured NumPy array. - :param cluster_buffer: Liste von Dictionaries mit Cluster-IDs und Punktwolken. - :return: Kombiniertes strukturiertes NumPy-Array mit Feldern "x", "y", "z", "cluster_id". + :param cluster_buffer: List of dictionaries containing cluster IDs and point clouds. + :return: Combined structured NumPy array with fields "x", "y", "z", "cluster_id". """ points_list = [] cluster_ids_list = [] for clustered_points in cluster_buffer: for cluster_id, points in clustered_points.items(): - if points.size > 0: # Ignoriere leere Cluster + if points.size > 0: # Ignore empty clusters points_list.append(points) - # Erstelle ein Array mit der Cluster-ID für alle Punkte des Clusters + # Create an array with the cluster ID for all points in the cluster cluster_ids_list.append( np.full(points.shape[0], cluster_id, dtype=np.float32) ) - if not points_list: # Falls keine Punkte vorhanden sind + if not points_list: # If no points are present return np.array( [], dtype=[("x", "f4"), ("y", "f4"), ("z", "f4"), ("cluster_id", "f4")] ) - # Kombiniere alle Punkte und Cluster-IDs in zwei separate Arrays + # Combine all points and cluster IDs into two separate arrays all_points = np.vstack(points_list) all_cluster_ids = np.concatenate(cluster_ids_list) - # Erstelle ein strukturiertes Array für die kombinierten Daten + # Create a structured array for the combined data combined_points = np.zeros( all_points.shape[0], dtype=[("x", "f4"), ("y", "f4"), ("z", "f4"), ("cluster_id", "f4")], @@ -454,33 +371,33 @@ def combine_clusters(cluster_buffer): def cluster_lidar_data_from_pointcloud(coordinates, eps=0.3, min_samples=10): """ - Führt Clustering auf LiDAR-Daten mit DBSCAN durch und gibt Cluster zurück. + Performs clustering on LiDAR data using DBSCAN and returns the clusters. - :param coordinates: LiDAR-Punktwolken als NumPy-Array mit "x", "y", "z". - :param eps: Maximaler Abstand zwischen Punkten, um sie zu einem Cluster zuzuordnen. - :param min_samples: Minimale Anzahl von Punkten, um ein Cluster zu bilden. - :return: Dictionary mit Cluster-IDs und zugehörigen Punktwolken. + :param coordinates: LiDAR point cloud as a NumPy array with "x", "y", "z". + :param eps: Maximum distance between points to group them into a cluster. + :param min_samples: Minimum number of points required to form a cluster. + :return: Dictionary with cluster IDs and their corresponding point clouds. """ if coordinates.shape[0] == 0: - rospy.logerr("Das Eingabe-Array 'coordinates' ist leer.") + rospy.logerr("The input array 'coordinates' is empty.") return {} - # Extrahiere x, y und z aus den Koordinaten für die DBSCAN-Berechnung + # Extract x, y, and z from the coordinates for DBSCAN clustering xyz = np.column_stack((coordinates["x"], coordinates["y"], coordinates["z"])) if xyz.shape[0] == 0: - rospy.logwarn("Keine Datenpunkte für DBSCAN verfügbar. Überspringe Clustering.") + rospy.logwarn("No data points available for DBSCAN. Skipping clustering.") return {} - # Wende DBSCAN an, um Cluster-Labels für die Punktwolke zu berechnen + # Apply DBSCAN to compute cluster labels for the point cloud clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(xyz) labels = clustering.labels_ - # Entferne Rauschen (Cluster-ID: -1) und bestimme gültige Cluster-IDs + # Remove noise (cluster ID: -1) and identify valid cluster IDs unique_labels = np.unique(labels) valid_labels = unique_labels[unique_labels != -1] - # Erstelle ein Dictionary mit Cluster-IDs und den zugehörigen Punkten + # Create a dictionary with cluster IDs and their corresponding points clusters = Parallel(n_jobs=-1)( delayed(lambda l: (l, xyz[labels == l]))(label) for label in valid_labels ) From ca171467c4eca354a36115257f89e7baa40c7557 Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 11 Dec 2024 15:38:28 +0100 Subject: [PATCH 11/37] Updated version to satisfy linter. --- code/perception/src/lidar_distance.py | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index fa8ce676..939224c5 100644 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -91,7 +91,8 @@ def listener(self): def start_clustering(self, data): """ - Filters LiDAR point clouds, performs clustering, and publishes the combined clusters. + Filters LiDAR point clouds, performs clustering, + and publishes the combined clusters. :param data: LiDAR point clouds in ROS PointCloud2 format. """ @@ -133,7 +134,8 @@ def publish_clusters(self, combined_clusters, data_header): """ Publishes combined clusters as a ROS PointCloud2 message. - :param combined_clusters: Combined point clouds of the clusters as a structured NumPy array. + :param combined_clusters: Combined point clouds of the clusters as a structured + NumPy array. :param data_header: Header information for the ROS message. """ # Convert to a PointCloud2 message @@ -165,7 +167,8 @@ def start_image_calculation(self, data): def calculate_image(self, coordinates, focus): """ - Calculates a distance image for a specific focus (view direction) from LiDAR coordinates. + Calculates a distance image for a specific focus (view direction) from + LiDAR coordinates. :param coordinates: Filtered LiDAR coordinates as a NumPy array. :param focus: The focus direction ("Center", "Back", "Left", "Right"). @@ -201,17 +204,18 @@ def calculate_image(self, coordinates, focus): def publish_images(self, processed_images, data_header): """ - Veröffentlicht Distanzbilder für verschiedene Richtungen als ROS-Bildnachrichten. + Publishes distance images for various directions as ROS image messages. - :param processed_images: Dictionary mit Richtungen ("Center", "Back", etc.) als Schlüssel und Bildarrays als Werte. - :param data_header: Header der ROS-Bildnachrichten. + :param processed_images: Dictionary with directions ("Center", "Back", etc.) + as keys and image arrays as values. + :param data_header: Header for the ROS image messages. """ - # Nur gültige NumPy-Arrays weiterverarbeiten + # Process only valid NumPy arrays for direction, image_array in processed_images.items(): if not isinstance(image_array, np.ndarray): continue - # Konvertiere das Bild in eine ROS-Image-Nachricht + # Convert the image into a ROS image message dist_array_msg = self.bridge.cv2_to_imgmsg( image_array, encoding="passthrough" ) @@ -399,8 +403,12 @@ def cluster_lidar_data_from_pointcloud(coordinates, eps=0.3, min_samples=10): # Create a dictionary with cluster IDs and their corresponding points clusters = Parallel(n_jobs=-1)( - delayed(lambda l: (l, xyz[labels == l]))(label) for label in valid_labels + delayed(lambda cluster_label: (cluster_label, xyz[labels == cluster_label]))( + label + ) + for label in valid_labels ) + clusters = dict(clusters) return clusters From 0f1a9f93b785d26cfa6945c2c7be8fb00a2b8358 Mon Sep 17 00:00:00 2001 From: Ralf Date: Wed, 11 Dec 2024 15:47:18 +0100 Subject: [PATCH 12/37] bug fixes --- code/perception/src/radar_node.py | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index aecbeca2..246e86cf 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -74,21 +74,21 @@ def listener(self): ) self.visualization_radar_publisher = rospy.Publisher( rospy.get_param( - "~image_distance_topic", "/paf/hero/Radar/Visualization" + "~visualisation_topic", "/paf/hero/Radar/Visualization" ), PointCloud2, queue_size=10, ) self.marker_visualization_radar_publisher = rospy.Publisher( rospy.get_param( - "~image_distance_topic", "/paf/hero/Radar/Marker" + "~marker_topic", "/paf/hero/Radar/Marker" ), MarkerArray, queue_size=10, ) self.cluster_info_radar_publisher = rospy.Publisher( rospy.get_param( - "~image_distance_topic", "/paf/hero/Radar/ClusterInfo" + "~clusterInfo_topic_topic", "/paf/hero/Radar/ClusterInfo" ), String, queue_size=10, @@ -133,12 +133,12 @@ def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, ma data (np.ndarray): A 2D numpy array containing radar data, where each row represents a data point with the format [x, y, z, distance]. The array shape is (N, 4), where N is the number of points. - min_x (float, optional): Minimum value for the x-coordinate. Default is -1. - max_x (float, optional): Maximum value for the x-coordinate. Default is 1. - min_y (float, optional): Minimum value for the y-coordinate. Default is 1. - max_y (float, optional): Maximum value for the y-coordinate. Default is 1. - min_z (float, optional): Minimum value for the z-coordinate. Default is -0.7. - max_z (float, optional): Maximum value for the z-coordinate. Default is 1.3. + min_x (float, optional): Minimum value for the x-coordinate. Default is -100. + max_x (float, optional): Maximum value for the x-coordinate. Default is 100. + min_y (float, optional): Minimum value for the y-coordinate. Default is -100. + max_y (float, optional): Maximum value for the y-coordinate. Default is 100. + min_z (float, optional): Minimum value for the z-coordinate. Default is -1. + max_z (float, optional): Maximum value for the z-coordinate. Default is 100. max_distance (float, optional): Maximum allowable distance of the point from the sensor. Default is 100. @@ -160,7 +160,8 @@ def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, ma def cluster_data(data, eps=0.8, min_samples=3): - """_summary_ + """ + Clusters the radar data using the DBSCAN algorithm Args: data (np.ndarray): data array which should be clustered @@ -171,6 +172,7 @@ def cluster_data(data, eps=0.8, min_samples=3): dict: A dictionary where the keys are cluster labels (int) and the values are the number of points in each cluster. Returns an empty dictionary if no points are available. + DBSCAN: A DBSCAN clustering object containing labels and core sample indices """ if len(data) == 0: @@ -458,7 +460,7 @@ def clear_old_markers(marker_array, max_id): MarkerArray: The updated MarkerArray with old markers removed. """ for marker in marker_array.markers: - if marker.id >= max_id: + if marker.id > max_id: marker.action = Marker.DELETE return marker_array @@ -493,8 +495,8 @@ def generate_cluster_info(clusters, data, marker_array, bounding_boxes): cluster_info.append({ "label": int(label), "points_count": cluster_size, - "Anzahl marker": len(marker_array.markers), - "Anzahl Boundingboxen": len(bounding_boxes) + "num_marker": len(marker_array.markers), + "num_bounding_boxes": len(bounding_boxes) }) return json.dumps(cluster_info) From 40a4595c977687321b7b3d63d5b077b218b6cc86 Mon Sep 17 00:00:00 2001 From: Ralf Date: Wed, 11 Dec 2024 15:55:29 +0100 Subject: [PATCH 13/37] added black formatter extension --- code/perception/src/radar_node.py | 83 +++++++++++++++++++------------ 1 file changed, 51 insertions(+), 32 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 246e86cf..fcb32ddd 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -40,8 +40,9 @@ def callback(self, data): cloud = create_pointcloud2(dataarray, clustered_data.labels_) self.visualization_radar_publisher.publish(cloud) - points_with_labels = np.hstack((dataarray, - clustered_data.labels_.reshape(-1, 1))) + points_with_labels = np.hstack( + (dataarray, clustered_data.labels_.reshape(-1, 1)) + ) bounding_boxes = generate_bounding_boxes(points_with_labels) marker_array = MarkerArray() @@ -58,8 +59,9 @@ def callback(self, data): self.marker_visualization_radar_publisher.publish(marker_array) - cluster_info = generate_cluster_info(clustered_data, dataarray, marker_array, - bounding_boxes) + cluster_info = generate_cluster_info( + clustered_data, dataarray, marker_array, bounding_boxes + ) self.cluster_info_radar_publisher.publish(cluster_info) def listener(self): @@ -73,23 +75,17 @@ def listener(self): queue_size=10, ) self.visualization_radar_publisher = rospy.Publisher( - rospy.get_param( - "~visualisation_topic", "/paf/hero/Radar/Visualization" - ), + rospy.get_param("~visualisation_topic", "/paf/hero/Radar/Visualization"), PointCloud2, queue_size=10, ) self.marker_visualization_radar_publisher = rospy.Publisher( - rospy.get_param( - "~marker_topic", "/paf/hero/Radar/Marker" - ), + rospy.get_param("~marker_topic", "/paf/hero/Radar/Marker"), MarkerArray, queue_size=10, ) self.cluster_info_radar_publisher = rospy.Publisher( - rospy.get_param( - "~clusterInfo_topic_topic", "/paf/hero/Radar/ClusterInfo" - ), + rospy.get_param("~clusterInfo_topic_topic", "/paf/hero/Radar/ClusterInfo"), String, queue_size=10, ) @@ -121,8 +117,16 @@ def pointcloud2_to_array(pointcloud_msg): ) -def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, max_z=100, - max_distance=100): +def filter_data( + data, + min_x=-100, + max_x=100, + min_y=-100, + max_y=100, + min_z=-1, + max_z=100, + max_distance=100, +): """ Filters radar data based on specified spatial and distance constraints. @@ -218,14 +222,14 @@ def create_pointcloud2(clustered_points, cluster_labels): else: r, g, b = colors[label] - rgb = struct.unpack('f', struct.pack('I', (r << 16) | (g << 8) | b))[0] + rgb = struct.unpack("f", struct.pack("I", (r << 16) | (g << 8) | b))[0] points.append([x, y, z, rgb]) fields = [ - PointField('x', 0, PointField.FLOAT32, 1), - PointField('y', 4, PointField.FLOAT32, 1), - PointField('z', 8, PointField.FLOAT32, 1), - PointField('rgb', 12, PointField.FLOAT32, 1), + PointField("x", 0, PointField.FLOAT32, 1), + PointField("y", 4, PointField.FLOAT32, 1), + PointField("z", 8, PointField.FLOAT32, 1), + PointField("rgb", 12, PointField.FLOAT32, 1), ] return point_cloud2.create_cloud(header, fields, points) @@ -378,9 +382,18 @@ def create_bounding_box_marker(label, bbox): ] lines = [ - (0, 1), (1, 2), (2, 3), (3, 0), # Bottom - (4, 5), (5, 6), (6, 7), (7, 4), # Top - (0, 4), (1, 5), (2, 6), (3, 7), # Vertical Edges + (0, 1), + (1, 2), + (2, 3), + (3, 0), # Bottom + (4, 5), + (5, 6), + (6, 7), + (7, 4), # Top + (0, 4), + (1, 5), + (2, 6), + (3, 7), # Vertical Edges ] for start, end in lines: marker.points.append(points[start]) @@ -390,9 +403,13 @@ def create_bounding_box_marker(label, bbox): # can be used for extra debugging -def create_min_max_markers(label, bbox, frame_id="hero/RADAR", - min_color=(0.0, 1.0, 0.0, 1.0), - max_color=(1.0, 0.0, 0.0, 1.0)): +def create_min_max_markers( + label, + bbox, + frame_id="hero/RADAR", + min_color=(0.0, 1.0, 0.0, 1.0), + max_color=(1.0, 0.0, 0.0, 1.0), +): """ creates RViz-Markers for min- and max-points of a bounding box. @@ -492,12 +509,14 @@ def generate_cluster_info(clusters, data, marker_array, bounding_boxes): cluster_points = data[clusters.labels_ == label] cluster_size = len(cluster_points) if label != -1: - cluster_info.append({ - "label": int(label), - "points_count": cluster_size, - "num_marker": len(marker_array.markers), - "num_bounding_boxes": len(bounding_boxes) - }) + cluster_info.append( + { + "label": int(label), + "points_count": cluster_size, + "num_marker": len(marker_array.markers), + "num_bounding_boxes": len(bounding_boxes), + } + ) return json.dumps(cluster_info) From fc396341a8315d1fa5eaf97fa210a19dc9ccdaaf Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 11 Dec 2024 16:04:20 +0100 Subject: [PATCH 14/37] Restored standard settings in file '/code/agent/config/rviz_config.rviz' --- code/agent/config/rviz_config.rviz | 93 +++++++----------------------- 1 file changed, 20 insertions(+), 73 deletions(-) diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 998284c8..2e419cc2 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -1,12 +1,14 @@ Panels: - Class: rviz/Displays - Help Height: 70 + Help Height: 78 Name: Displays Property Tree Widget: Expanded: - - /PointCloud2 dist_clustered1 + - /PointCloud23 + - /PointCloud24 + - /PointCloud25 Splitter Ratio: 0.5 - Tree Height: 325 + Tree Height: 308 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -64,23 +66,26 @@ Visualization Manager: Grid: false Imu: false Path: false - PointCloud2: true - PointCloud2 dist_clustered: true + PointCloud2: false Value: true - VisonNode Output: true Zoom Factor: 1 - Class: rviz/Image Enabled: true + Image Rendering: background and overlay Image Topic: /paf/hero/Center/segmented_image - Max Value: 1 - Median window: 5 - Min Value: 0 Name: VisonNode Output - Normalize Range: true + Overlay Alpha: 0.5 Queue Size: 2 Transport Hint: raw Unreliable: false Value: true + Visibility: + Grid: true + Imu: true + Path: true + PointCloud2: true + Value: true + Zoom Factor: 1 - Alpha: 1 Class: rviz_plugin_tutorials/Imu Color: 204; 51; 204 @@ -255,62 +260,6 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: "" - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /paf/hero/dist_clustered - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 dist_clustered - Position Transformer: "" - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /paf/hero/dist_clustered - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -339,7 +288,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 10.540092468261719 + Distance: 34.785499572753906 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -355,9 +304,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3953982889652252 + Pitch: 0.19039836525917053 Target Frame: - Yaw: 3.255431652069092 + Yaw: 4.520427227020264 Saved: ~ Window Geometry: Camera: @@ -367,7 +316,7 @@ Window Geometry: Height: 1376 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002070000021e0000001600fffffffb00000020005600690073006f006e004e006f006400650020004f00750074007000750074010000042b000000d60000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000022a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026b000002960000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -376,8 +325,6 @@ Window Geometry: collapsed: false Views: collapsed: false - VisonNode Output: - collapsed: false Width: 2488 X: 1992 - Y: 27 + Y: 27 \ No newline at end of file From d6615f519d366fbe11ff0437878556c5f48581b8 Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 11 Dec 2024 16:09:23 +0100 Subject: [PATCH 15/37] Removed debugger arguments. --- code/perception/launch/perception.launch | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 127ae9a4..0133a9d4 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -1,6 +1,6 @@ - + @@ -75,7 +75,7 @@ - + From 0fb8ae60b718c3bdd65d1996bea4342fc7be87e9 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 01:30:06 +0100 Subject: [PATCH 16/37] Add files via upload added new behaviour tree doc assets --- doc/assets/planning/behaviour_tree.PNG | Bin 0 -> 167731 bytes doc/assets/planning/behaviour_tree.drawio.xml | 230 ++++++++++++++++++ 2 files changed, 230 insertions(+) create mode 100644 doc/assets/planning/behaviour_tree.PNG create mode 100644 doc/assets/planning/behaviour_tree.drawio.xml diff --git a/doc/assets/planning/behaviour_tree.PNG b/doc/assets/planning/behaviour_tree.PNG new file mode 100644 index 0000000000000000000000000000000000000000..87ff5eb42a77a987b983eb6dd6bf156d9c3e6924 GIT binary patch literal 167731 zcmeFacU;qF*9YA8ZncU^sZ~S-o)(2x6r>2qeySGhz=iCkAVb-N5H?i=R91^JWEK&W zEnpZ4qaa9SD-aMe7(#$Z!VVCUyw~*$xZ7KMOP}}izV{z(LmJ5SyT-ZB`JV6jo-4x~vaSQzCW7Nq5dq2u)+A<7&_|*Cb ztsg%6C@-9E;ml{?=T%NW8K6G;Xx)AI&&Rcncg}wF5jXYV{vS@cn?7T&x@*$zA>m6I zS#z{+Hpw;qmUd)Dj>8s7H(aKa+NtuKT&bf+_IiR=36IXgH)M^|o0L;l<+;hmexW6p zaxg#YSR(p{R68r^C*u%_vtQ-pSMS!zP93c)XtLgW54^QCFq5yK$tK@LHujej#Ul|T zS#q&o*b1hsdIaA|%aL6;pi^60>mM!{jJib)5EXmYg|!7g85M0zS=H_;8+-gP_;5S; z@Nru(47DluJlP{rV0`CC4|x!TwC-SoA6+_MvPtVsDtv)ES?+hIy2fVjPLmLcQ>~HK z39Y>Uiv`0F^(^h&Bmw^()-6?)79!Es1wM%dqr7)TszR>nk4rZAOD!5^~0ZPJRs z-ZL?YL2F)vZ&W*_DudsshQF~J{OyN^>K+8koKp<3Z=8`wtNXI`6gH|*s#!US_iUST zq#Kk|9@c?h(Cp1PoB?KGZx;B?{q^9TSloowrC(P)a+BrP)q(T0zqQ6DA}e)tqzX(> z7BX>C?FQDnzb1fZy*X^Y`ygTBVSzL%)sy?h6)>KVDxIj9v(PzMn z=z?GPezl&-$pkc5#Zjgu>BIz>g$m) z%Njpl5HJDJr>qkmUDAhZ2A_X7dZ6)h@%1-H%|){^Y`~VdoEH3OtHI&!_5d)E2H=gK z*66)h24unqu34K59-8KS{UsQWP(bDmSuGtf#EGoQ8(+XNU+I0mAYgOjZav=vaLsO| zOF3$8y77g*;N)qYAK|j{c1G8Wl{YVbu?)P!l%z`gUN3{m|7-Cktv|jsw!CsK!|-r7 z*r?5y;F!S>pC7gqr}k|x_5fTnEfWvkHjP=Cxg9PmZ)ZGqd%p5t-j957#E=P}duk9} za`Q3xAs>Qvg`Z90bh(sk zRwuI1hG3!Zz=c*ohn}I{%+j|pH^0fW9olo%qCC03{PASV`1p8Ff4P22-8LWTVUHnW ztvge5x2Rx9QqfPhPzp^muQSB9tK|Yqpgc%xPFLZ#@5fekPz@N0?FP~6(Nn6sn{i>Q zYBqP}wB(sjW_8~ z4wEg)Iq|s37FC+E#8HjRiRtd-DPV54(iPOtLe*tuLLmzR!sG z4Gdcgb8v4si;xW#2Mer28?+blh=K7Su)3IbU>T*L0G^&VeEjKCiM9cNfpKHnX#)lfz1GMH_C6Z?{GGN9%7eo7XFakfvTmy(A4l$NqGGdWMFaEL$Z8QO zg%`g+aA<%NIwumAfAr|lnZ(_$YieXwzc$2>tE=~qb)M$A6h^>iCU=2vI}cyP=D_2R zMoeWCuz&0{XwAW(rVWmQ}57a(*m2OTLxb7#kBa#NMn-5AKgZZM9AA@9(dK zgX+~Mp$-zp+jk}Jkybj855gj>%fXHxx;?WuX+6d|8M}m?&4zcqGeuwGq0(mQVP){S z+l?LD2MlWSaMXyb{y6J|NkiG#TeiT)S=y$0bB}MyRX%`Q*9H9`5Z)ot7p_>6;)D-KQ#7wvJmjjCg-U9C7 z1aNA2p&l@IS%`b+3P#$Z3@n{UfRt+kn7|1`U>UuDWwe1#vf5PfE8Y#5^2M+Zdg5`o z32UBj4x-in`WpWYA^#gf@PG!F!+Gb9RzXAjWd@phITACtc%z%5}*JK2wd0T!}bvCiUe2} z3~(%Lj$9kS))geqfZHBG6m8aO)2yID1I8j&@)DeDqusH|O}g%CfP{pECkSj(_6!Ou z)1}n-Ma0DT)#<_V8OD#P5s5)f?UxQE9rF}LyrQOYdm8r8>v)WUsg$=*;Vaw3y>r$)nH{YsMw4id0XUDm?{4Ll53GuC30G1f$s*V72sZI0^Wt&k^@3L0IoYia6OX6Tu9U-CGaVf^vjq$J*uj|;7cXz z*_x#64%j?t2x2vW6osAvQTL*uvw(;mK%;ohmGWvBJEk{zZf82?ycDy^{T*9)p2apQ zI9J4K;!kpsG{*g5;=`3FI(uKrVSL_)KH{Z_>z6ionp12WYE(8JaI)juBRaM^E=giaW+6WPUj<1<^_PAk5}6GiOEatPrLl zfRDHhlZEEjC{Zk4B`~rtb7spz>}^uCwCRw8k9$gYA+4Ppi;~|UZcsmBxS_ve3B@g> zRFr@_aT)Rd%(YC%nm`l!&2C-1cCPVAj+YH_r&!YG4tsu@uTEpryH+aVwR80Qq&bz> z9BSqT4Sl#@r2li%C{187+vc_C<*7!*bUkTjD!I6hZ8O}|GgxKp*%3h{l_+MrnBmV~ z!E<|1qumG)YwOMb0GF#V_q3aK!_e2(lZN#)=njb$?b&4I4a7a0ER(!voKnL|i7ATmEbwaLo%(0pj*OMI#-WOrBQJq*jEoz8*Pnd_GHwNQqlerlkyRi@-z*pcKmB}U-~~)Hf_&#&9MCG+!^q89vn#vn zZmWqebu*?am5xW+_sM>T=?8)rxpw6QaEFrb zBk?lY_S~O+-#-Ep>ZV=kSXUer?TDt#lKbh;XnMunf)0G;v&~(v*jL(VYA$iml=bkL zt#Cicka%t4rFcL#2ti{Z1`6W-`K;T%TyS;z@Ghw(fD>+b`;j`a)Alff2=cBlE%J`p zwT?SlMws$G2hO-0J%&%P_%PkQOKssPxhI%y4=w8y$DrJGd@qJ3E=(#k$(`XCP(8D+ z{+cx)UKX@?Q-LpoIanlB`)~4Ne-o&a-`;w4EuQ#eQMTJ{(~>q5W=pY=y1Ib(nTkdq z^HyKXsDB*kJG~vw(ZuquY~t9(IrhF=%)5)}Nnr5dNm#~w;chLwnP)o*f0mfwIq)Th z9-2uEkD{+Bzno;wFq|k=p+1PD+v@FfX3CKIW=4}@OOy-NqjDB=;7d3#%(zrLJx@)3 zz@+ry`%?`l8JsQMpqq8#h#s-5a;N*qKo8%H$xw=|nHy#9kte31^Z;p^KMNCARH2HB zoWG)^_s=b^U~8Ye3b8~n%jvcmyBP1m(Jk_%of`XWUV7%CwHZoLZ6Zr$bi|Tg8mEif zW}Cz?DTqNWbT0#EM0@rgP%9o<2@@UNA`r%%r^I$E45 zAet}*xuR6tWSB3G%t8Y9m$IG1hOtP(5`vhEjxfUR;E_5Y&d=g ze(A7ai7lP-G|J9>+iaDWgNLAFCpD)WlcGY82%ul}?{FENP3)0BkjgWivH+@dKEk^| zVDj@_5X1*}Q73eF?3R^%1kqx_+HKUI2z!yt66+A5iifRcp4saRA=sVJtO1O)a2i2c zdyAiLd1$zVaYmqa8$U(^sP<@Fu1;M+m<76dn?bE&voaOLO$R&nPG;2;GQKan=UCz8 z^rV>efEiU%kYm>CU{>6i%~$$IVs{p;P|T@lr*~6=$LNH=PZfLiiZ!WRPtwZ7YZ&CF zPyogaMvWxeekJh`b*SkEEh`22x^>-HIHYmIgB_L550&i8pQEnNN3?Dd%R{7JV8smZ z@gHt5%m!4%({A7P;%gtZn=P`t6gD?+@@1BgvZ)=39tybxFiX9Zu1?$3k?}Il{W?-U z4Fkm3oL|n>heeaL`0a;${%3j{Ms-xw*Tr${$7x~-Gg-&Wvt#zK;7->f)rF959LkI# z8e*Qax7Hi4OsjdYi-#Sfp$@)O?9~$ghi!T*(NSF^)_PE)YHR?kD5#W~8yg3V2qOgEP1+suR5W zw=+{xRs3b5s9n=J+nVtx2jk reF+)A$kM{ZjKY4rhRJAL z5v+oP*FzI8iZW&jnA{B*M<>2SPo>!^cZX7S+>9kNK`a|nyQTC%hsIs9J()AnK#CI? zHjdmjZNNM@?X>~&r{2ZE{d-)SUhKx)C%sK&X985%v9+Ye$s%04(d~;em#7m{ubAR) z#cbEZ1a%FRBJ0;C9fOkz)0MZDeeOCsJbPDBdOs7VK-e!7>r-q?=}V-C5< zI~v~;)lR?V&zuMfen6r}OV)r};<{DY_H^Iey)db=Y;gF{S`mR z(Q;@d^f5=tiTkAu;DWo6QLXqi_vr)mCa z+r797mpCMfH&}}CKQnD;(n@(@DoHLFLZv$HJU&i&mzVDXIIGzOba8a*dn~JbAotG5 z?5z9PxVdxKRB21UzX{&W9{fZDJV}@XTz?+o)bL6?54n3mqB07B4r54~9| zFitJM*gnayCPrd%oDMX{F{E)m*4eCt5m44x)Re0H?N{0jc*%1lz7zud)=?T3x5 z%66R8%b;W;w8Uj{NpGpr5Py{2DFH_YJgXcR4MUvV>`hk(y2;^043${?*k?3j8~uW1 zGtP%~+e%`1dRH?pO{$%tgAm`A$(=eGKONd_>e4~RH;Je*PGD+C!o-WWP1luwln|%V zd7z{GS@|Rq;1{5l0{kO7tO~t@Cd++<>$3W92)@bb&jwXm?LEFZ02TbVmvHs!yvnbjjw zX^~Iv&U(de4)Ay2pxwOj$CF2iXc~iWnKwjhnn6;0tXp_iC%`{vR$&O!Qp8cd zq7w84!O-r`jI}?PR7@rtOj_+nci3z6I2g^wt4!hs=vn?guI+A$=$`2kLJV5$q2YT@ z>N3$sBLjw?P|e&F)$dh`pn5;SJQT4d-9JWOTmBPq@Q|YT7^^5@Hk2H$Qx)!1r6Lii zAZ10?avnOM*4XvO@8M@XZ>!8y^EYnc{(`?ZW)bTdw$IFKtn|mPyH{YIsVpe@uJwC% zI(Ndhxy?zu2FIV>g>sTeax=r(B;yBqt%-yL))6izd2u2T(i==K{yUP?DT0hmLjPLRa&Fg;iRrX=~8qms}2~-=O+A;w#~3rbps!ZQ@eXwLDWiN)q+xig7}Ii zX41;>`M}>BlC0G_o;8PTy4S!#afy9Y{VBy=Rtj4^g}?rh@(imzI-$iPVfh{0ZhKqj z;fDx14o@nd*ndy6tl3PU2%ZGT>`>w{moP`++zsXGuMhdIDCaX}y+1ZF&8I)|unj|LF8h^&EnWg5Iz zn`X)D=?4yDR+uelWwQmR@*eFX_VLY?g$7;F3huM2ZozMDA+HGVV^)n>?M-R?JWGCT zfs^oMnY>rCyjQ_3+cMjSlk_RuD3SQy1OAgHJNlUcrz-T4AwwLwT9=7hyQ^t_-ueN_ zp5M{+wpm$&4=r#5y{Sg!qRf0+TjkoH^8*!xP5rX&es_EinPN94{An^~`yYzpo)sGI z4tLm8ky(MG{=(u{m|?Yr_<~%s-f~|jH%FgYA;s~o3pdi&1&y=zF7qXQw|V}WFa}>w zH|{@*TaaaztyCzn6gFF1{yA|b%vH{u5Pmea+iE|fFOQA4grAG; zRuNyJeixThkf@QMe%HJRKUOGD2jOW+x1gO%vsnCzvpEH(A!m1vR`@Ts;Vv}L*u~Z^ zI^C{-nL&r+<#K$gNVp!QA~)8Bp5Pk$41(1qKgB9q+*)>MU`IuoubjEln&P5HBE|%&wl!Jb+s8K3fghGuIbD$1_#?MU9GNW%7;;ZjZUxWr%UPW zaDzz%5_^`fQt&zo6*ogqRUvsOY7CdV0I;;VQmA{nFY}Zi_lx)39ieCP@;3_di7*pD zg@wFp1IE+S@ZQGNAp33x%OrS-1|+Wn3N9AY@6MDqNRe5jBQH{7Leiw0=ksqTVW^bS z$3D{;mUNbeC?+#8VNLlT@o`3k4gKp^T3ZLJOxLA4katRv`d3=lH4FArG(gGJ1aten zCXlKP(tQ~x1NrkCj3hwO*0p7M@huVwn&h|4i-N(59;v?tAaiORrqH(vw#i z79PG*Or{W8>X#}T0Nwc12vStD1C%boTMXgzjT9u#v-frHp$R9FMsx6tbC{=}h2!cO zhK#{n582r<T^!T=3{`SOspzdhmw zFmbiJuE8qokyN@7KzA|HWC8E5m5cCY{duEEE<`TJV32N>|76q){r#R}-%e)PmYn36 z20)7U9Z=B#s7xR?AA%I2wYvp`ZHPmNk4V_&1j4qo93^gy#HYc%tM=9Uj7P2geav>q zJrCt0wdxh}1>|t1Lt-hym`4L9C;&7wAOR9#J1vsx0sHi+;dFyEW?00QcraRHu$=k3 zM3G&8W~Jb5RI3b~EzfjR-NP458o;Oz=sI@!*yLPAowBn@+iMv!eLOYiwOj8m*&2GB zk8F|Jt9P#@adjqOo;_hW__6GypLuGr0qCeRcw@0G-J@%#w>i7Pguk|V zuffS1)5^;ovOhIQDkG4iB}t^E_hMKl{)dN#biY6q}S-p%e4JO z&DfFN=xG3t^D>5QlFgoThT*y0!%Y<)k*pD&dLq+Lm9DF%BIG@;qUfs^aty$g2%i8w z>0mGrmFstAP`X?cf|dgYG7`&-DNJN_jT&YB zF(ODx#i);lB^Lconr&IZeAeV6hq2%D7=ST?RwC~MsoJiQjG6{TeGV=z`Jj;QId$Oi zB5&Y1#)Xv6AcS|VSD4qY-8vG0J)VeuVz{d;pDWL7>7V_drZbq>uf@k>z|o~R|J z7UjB05j12#HL@$rVK^Tpp+c(=qP^o;!b&NVl^~mR*FBWaP z;(~q0u6sERB=+zPmpGR4XLXKBm)eJ_a323i5Bn^R7)Ht!#c@(%G68!{x*6xV=HN57YhX*oJrZ`{2GXgvr^O!GSl5rV%X!5ez zl=++Z5jvJ$FDTTU9>wFM>8@1L!Y$w!Zlja88LczLKePc?E}-sI@8T7Sf&~RC0LxK^ z9h*5kj#eSfDdxJGm7Py#al&g!8P<~iGIf}AsRxqlOoMmQ+oYm#9z|I$^&J(vP}n|@ zsNofaf)vs+ujsiO3hG52CVpIHOdHQLr6Qi`UY|Ojv(v%!$0|u~2bzs~9K3D(z1*nf zH>9RK&Hu^q#hE6=xM=wHDPiW@>xo4m!lN6PiqBU09D2#x+zS�BGhE2A(I~K&pje(Qr-zTQQ8iTgCrDA*Z&{p z<5-^Q%CoKjwomId+qYmp^e-tlJfEXce^>>`grRz^icpN|1`W%2AbP<#^Zpy~!I+p1 zkJz@O>DNR-Pw%_79XV^&L0%p5gJJzFB3=P8#cWVN3R2x5{f$(MVtMtm&nqvZU@~Yp zKOx85HZr)~=Nbz@tmDyat4<_CwRY?+>7p`5B%dpiE99G-G6D$CaY7TABJFrjwZjQY( zt&C-gV(t*dn3b64sc=E=m0oN1xx*U$=kDs!GM6iO%)^b$J}I`~{w{UeKSynry&>uc zfNSnxBt27aCyK%yFE*NfcGjuj^j9Xb>ds?IMa8**N!EzBj5I3e1PHa$&atwh7Zo(R z85CoXCS0a82rSaCvFVyvPzOG%Uh$&O>WTQQYd;ND&!7JhGn{Y?pF-W~1R;AhJc<%x zzQ;akdQmQBiFmn8=|iZ15yPNlUej>a@HNi_;rk1>lz7}-{B;9KEK`=1pKKWTM<=j7 zidE#Az7H!AR8#vcJj($YD5#Xm|1w`V#+^mOIq zB6>PsxfwxELxar4^z&FfCC88|1-ky-C!<6a*#birkjOzVDY?AIK3t~E5h6T-o#tq1 zi8%3Vj&-=d&OA)~OdhP(zRD zw@-Q;Yk#xYMHi-H>`Qh3Zwp>hSw*bWRZQ-jhM2S8>}@~BOsVSEE}>n!J7YSwyRK<0 zAio)GJeNt~#fWHvb_qhAN2&_{HXrOflR@>RQ;BipsWdWUn>3%ctZeR*&zY(~t65f% z0w|w!vFqam?vL9pouBg{=>WNR%S2~DDs&e;n}6mp?^{(ja~GPKzB0}R zF;}m6WH0GyVGU$7p&HaO-urEIGjhnLu;;};C*!8gmz7-wB&S9>1C5t^XR&|dF*`PG zl(Kcsd%>!`ef##0i^N*M^y9(IC7^-{7SAs#F^Mv@e0V_n3-lg%nso)_+%zdHi>fHT zko1}g3z&sULdnS_V8%8>l^uXg#95b_4YDNB&v9#BIG^gmeXy|ZfPti>q}Q>{%7J~G z62Hh+aWpso#@+5NB4ZE?3qf8|h|*gUB>_md9q$9k}YZwiJKQPcj;1oKVuQuxy5)HkkZ~sl8vNcxBs`x z)s|8zmM63dEK6c2piRQ>lc z-%z2m*prvFa^038pUn!sYekqfx6h}5inIR|Hcp49zL#rE&EK_FfdF{vK>94h=rA~q zS^R~FL=oa=E#b9@B+LIB1XbL)cLl~#8cb9{Q>JHmK6D z{7i5PY&Dq~niNkR%p(rmFP_cH`Caw6dcH;!*9a6L$< z%u@jPHqbEzB_QAH0hcb?Z!Vf-M4pJ+M{O&qBdo%XLbo#XT!xprmLBl1c7(M<;2+ja z5GrL4uitLKSGyY`F*uZYOcqEdIEBn{4lbHk_xQ$3eKCbJrXQC{y3ennI+@Y=>~4AC zZ3tGpMQkG?_O8&lwcm`jT==>Jtw5fV1trpWvYme>-iY_B;Ls}QzLSc61xzn^0UT+t zU8)xX`7&9^P5M(bTeEk*RfFAHD)htIx9N^oBJLQ3ckwtt+JdawaFnprG8pq zzeAk(&y+{s*Yy<&kaE;?x&d@--zdd8sO2GdagrkssvG`F&ee@OVO8z4V7Q1Q0aGU^ z{#OMNK!vnasM7NPCktu2hI^tNQT43p+cNBtT)PtQHrmw#l3Y+P6DdG~W(y0nAaDaw zM#Q9rhe@A?v(Dc*o_-h#&6wl=N)YCSal@LiHJ8HHs7oj)_6;<2bx{GkrwJ>w#>XQmgugz8x#Wb8ht1R`xYtUoDB3)rGc-Qd=+DS^a^$ncd&d0n>$U;n0eCdOJFZFb4ytu zp_YB4VEz$MLVgSj+5TFXxs5KS@Xg(?FZ={U2y@fScl94{4$L#T9132sY*>q4Z^p@I z^=oh}FkR0|84g@jK5Z`>2MgE#z z^C;(x_$SlUHnk+E`V1HpvZodWdh#cEQqYJb5X2x7I{>gDG6_DM0Cb2tWRvyxb@QZ> zrJ;oMw=`^(X7%vxBvIAC++!YuZ9v+m*%K7TZdDQXjs6sbKK8i7UwVRUE2#g?fMBm~ zjzrqrF*2>(gK?t0jQYn|zvHbcY!i+ZTv^K>`RLMS{ApZI@_K*f#`f6n?maX<*YrZA z`GFIVuSrJRq6aw1CIEMN;j+q1HC^5w9gzM$0BCR;pO~`JEUqrOa2EUZJ-*x&wK-x$ zk56Izcdn-Zt@(xCedd)_aRPc8ZaGHf_681o__N+WILh7JVJH`{`-W8G3j-j`5IYHF z`CnK62LZFjy9ephH^H}s+k5Pu?K&jwm+SGYyR7l@@g3Hyh1#Z z^B#T~W)(R0U8GVW0Ri1eWRVuz4V*Uzp82Q)Ipng%VvM0c*0=@d9BEJxi?o?Ql=0us z={MaCP*k^~^>G6FSy?qTK$HG)%`4UkEBljo=eNKyhBXhf^4-j;_+0QnL` zdgd(xH>5bYiD+P#G516=bS!*>QnG-CewpN2k$uiO2x!ehgAc^knmtEg{yve+o{STiErZ~k zuKC`V#j62!u8TbWTa>SVY%L8s3RY#Ow1T*Iu*66ZlEL-sB@P9RDyAk3UV zU9gD#Vn?9l8URBXDSUJvB-d)1rn9#KV1p;Mrgdblb~hHkStIS>_u4jD(A)z$Ax<~% z2*@{ZpOKm6%#wm8<;;85@y?oK&J@rHrQ4@pm@))3AD9ht&Cjv}%m5iXPw7j~QhnR? z%22^9v==OaA=tG8*bz`;@5UW#x=dk{D5GqQ|I-S`dmI+OoGh^Xn0-}-+iE&y-5R)NxTBP>uz*WonD#7xeCN(DUhQ?Y+ zAdk!dRPxtblC;q&-4u=+GYnIj@}6M9(s2?ytv1OzZC;gunERzBC5vN;6L3DFo1+g* z*4Gufh&q)mL>~|&nI|UsUYPCl%_Up(P1bfO2>QihJB{}0C1Zj-I@qccw(@ZD-*WxD z2VN@^yJ+0M`S0U>u+>ZG4Egizu*PuHo6%t3_(E!e8zsQY?eegaeS;6`)fSz>{jxyMKwtD+U9q z1I%0>_W=?(m^IUJ0?A&%hpayaY~JccQIhAp);D4b4TWuP)Q(reQ+Wk>Vd;&s~OKg-J<;l z67U3kT~D>z&&B5D?bR{mGjhG9V)twprtRkMc~J^)^T}~{g`AtO1eN4rw%kw@KU4)f zihxqQ4>wcQg+hm}haX?1B7IDT2iA96CVJ!BSR5ofK#M$8tI8pjn5NVm?S_r2C=E9)t-dm*5 z=~`5Z-zV)>m!@vpUNW6Le@{sE-jiMUAL?6HXfBwR4Q*kLW#Ijpo3QcUwKc;o4_d}^ z2`@@Cw2s)%%W9>@t>h%EkrP>vP@bJe(*nW-yD67q&eeEqY-Aw89k7&`zgQ$+>vvCf z{HUD?dPr5wjNiLZK5j;|1cL)CYbh zhr8Yu4MW?@ilb$%^KQ-RR^2m&4%isZpJ+56O4N{8kky*?am>Dqui?e#8|r}xf7)># zYB&F<(Xy<)<|)p-U&yxR?c9s>F(3$`vSma;+?&6QEus}{QMRxh%eY0_~%|} zT-fCD(AK=BG4Mu5$;CK~Pe)MBjN; z$c6=*4f0^(S)Y&1#O}Z3dINU z*iC?y#P%#8Z&A0Q{!-&*RFjgV`x?-4VU7i5r=fT#hS-3DXfF@ou1NJjfVhJ81s ziHGf}@|ldItU=tudFU4MGDRo#mH^=Un$-aFo8eIg^1fUTMNyr1r=`>9cAEfxP#!{I zfc>-()d(n04y2So(^Hw)K{eNt5Ngg7$}_LO`Ir?C07xZA&EHPOoZWe<|KiJ700_eM zEaX111TE06rAh@J1h3m&UvpC}cqM6o-n>CkyanNj<>lzW-ehSwEF8=HX&S;g19@{; z+y?&?a92M(whQ6jZk~liiZGDok4TrcF0i;}7GBX=yRjRyImH30y!+jjvxpN`2)% z`L%e*k(%n~T{8X+1&Ei^Q5pZ3AXmXJA@LWOY5s0@G!{Q;ftG>yu_Lsq|i2 z?-}={qi1UzF^Myr)D+(yo?0*YCs6YHMO8~S?TwfZ0dO2Ot7Q#Jc4$fIW;r=})Ef~y zJ7ZW+lGp}J$ek>sa39OCAOG^`@j&YXR!|!x_`HelybozUcqyxP`ECaC=N4O0>#bi( zi^1jUno8#P`Geh+%Qkde{|Os#Cv-3kvb7CceAEciTmP|X$34m4#x#4@;<%YE{$ zViT|D=_F98N}#V7B3>YSdOSR=mhkxg1_Ck^Sc08#B6}j5$8}9PlLYTLE>rv=c`Xeo2t($|^=k z472}Qrq`IT+t|3`$zDUMhP(OL_`VHUDB?hpiywPV17(JLnsl+2U#I@kW;3an46@L# zi_#vrf$@4`4S$m1lVIP4$ew=xvYrdqE`oN#9lxHc2S~;}Ev7o!VvKW>fT~$wHbcfJ zWd6}0a|otqD=?+~A_Zij6EE||^GkJ~UpDpNgS?(dygd|xFqaWo4aX`KKm*9!az5Bh zc(1(af%S@yu^C?0pVDBX5&$$W)k^5CZ_Stv#|zHbON~y-*rS_A$wVLuzlAh6fpTDx zcb4@rlTpWk4~_;@wx}G#p{9}^JEo!^^9N=?iaON+$DZ%;xRVB2I?Z4w((-K@zj}NY zU_an0Z@i38!nN=0T5xjU^J3L8rWM5N%$4+>`EeolVE-%V_8GZP7$N_=^?l0FJRhB% z_KZBclh*MF^xzV94lq~J{^O37+8d4QlhJN;;;=(;?+PZuxvQ^O2*%G1#*z4BN?UNC z)SY)*gXDP`dRKG>0D;wMx*%Zr`|N~86c3iN-AV^M+D-62ojk4cBV4cDCN*eNpJ^TD zeKAgp2&zGrel2O?7Je|+*3-<ERMNY+pK@s4Se*ru=Fv-VY->eT> z$1y=ug{<^8?PF*^CdQ8nnCa$4fQw2V-vsGtNJY_NB6GPXCV1mc7s|q*B8f$2s$*uJ zSmRRlO5mJ9?>&TFQ=coF~7j6?c_{9Tp|aEp;->rUY2yvgHU+kr#Vh^f)0BkJG| zExC3~{Rw-vR%4y|?bL?{{>hR2Qc_%K-#wU9He-mH?G3`GgsXRvJZaf5{F z#&CPqDbRjuvxIY@`qFM9;` z2nMYh5Y-7E$4ZaI4C2niIiHiw%<4<3oQIzUh>F=6Rmgxw<8W*6LMK>RVejY_#|r0; zYlzMlRLh@lJ-uxyfeU5-AQc6WCjOmL`Wq4xCp-FTf=PgMOXZB@a_SNQvDjg5VnFVI zX*hP(!vaw%Xt@E%)mhupeAvtdL-8pxs_sUk${B{;|31j@*oc7%?`BYZL{E!zZ~LeA}8!0-1W1|(Z# zjozrOV)vAeb^L@M`U%t7d4_)H#|suKy?R$}NZi*6_$5hbMAYY%fayKFUVx4vfH~ZA zZ<5Y^v#(rPg<+I2asA;MWKfwThSD?jNbN55m~1d$x0T$}(t%Cf!%;w8@f^Sz8zzZf z4;~QDNRs8o-2Dl5n))D3`G++L`Xju~RO&nc?tNk=-~b@`WSFyggOvR_!5x@0(z)9S zgPi$XZkLHuG}qZln#-)L-a@KO5?i}XTrouY8NbSf~4P#16Dz7 zM$+=xf889=bA&zWh-X%C*>q5rg)_Qm^Qhrhrx9at*!S}^K6ismq(eSdIK=kafDi&94PyXy*@5_mn1Z-372`{7Q355*+Ic{;U})3k*0Ubj&&#J@T_Ke_!~E4YeziQf8@&`MwMqOJ;<>kUsg1 zmHqJsJB5a>O=Ezmyw-95Z}-8j87(02n{>CJqP~g~(D0m??@`la`{cxaMk#5EMAS6w z7s`Lmb!0I!aMWw@pbru!Szo-_)04?y`ZAcFaRQup_&@r)XFx0zcxW*{;3f(hIe|AH z*aqYtd%c(JVvpCwezmF1+tdI9Gsj|YUSJ^LvCB-dGrg@ZowdHti1;nJzg!D*aBnyZ zsa4`B;BIKW=SWeC|z0#(g}Qqb_$22foA zhaZ~Tzol^w_<$?Lyjy$*q|BUO@sDrF%|CKIXDqoNmO6GLx2O^@T+HhO>nNm4Zl8leelC&a(#E7$aFTJAF7$oZ@v&ScXz0d zZwgCWxxFi7Ncf_LRl1v8l?H)p&F7fW=BW8;f?S*kaG???g3IG?Mn|3_;m~w|Iikfl zV?m@Vl8Hm~3NELP+VLi)uozDvLn_Y={Vi{esR#>4SUVv%{G6kTCFRZu%?(DBd z-hvMrEXpvtYf9s8W_{?$|J@qb#-`PZdNm~Y&JWXNxDzrYKoadwbnm;;<3T`LWPm2( z(d{7F)>p($NjK%nYCR7992~z(beZ#NdJQ}<1v1K)6i=8$S6A;J>pYDpI|&-Wi*Pt<#?5&%Qhm(CY^ zYXhYxURztz!ffhd*hR{$5G&Q8{9FXg$s9WJ zqG>%89)WG_5q1@jJyxfsQ#5urds2ASVoS>F5W0%Py_y_Sn{1Gylf4+|S}{Lw2I&iT zz3RcnIP0(ps6l*or#+%!gXD<;^Z#uy7EtH;;(d$$5zHQr=C$h7ldV+&e5AZuQQB)E z#f0a*BbVC(+CYWw^FdXYK#J(?*$YiWE2*C~)q}+H+s5}{u2qG(xtV^>i!Y<*A)jmouMP z@xzl9KT~Yn+3sB0LQ)XrjO=VmqE+JUyB8iAp$2Ei(*AfhZ2-8uPYWD?G5#6`N??h2 zS|IPKF!9oFon+FHW()pH*ceC2+t=Sbvk2)GRaEMp^vkf#fAvx&ve8{5q`tx$Ne_TYzNo}Unf(|OCTw(2bZ_}RsHASZ@ zFZOtp+n4F4lQqnSpEtx+l)+zjku!H=djdaKqT?rl88k zkbMbkddQQ3?vU9>*E&`*;6=Wu=H5N)^8;=jsMI*~k}^9m+%OSD2N+~`)?`Lvu3f_M zZeLz;$kyiXln&(Uo-a_>-;%3*0JpB|fh|v9{oWjuUwJJh*8ucjV3Yb6l^W&ns4p0-?r(~fY^oLc% zvj(ru@o*g2|Ao+T73oip_w)Z-brwrZ7kEp*EQ<(qMeMr^OK3Tuk)iKwFa1tRmg&~8en5)s7EJrQ?6OrYp!vS*fKABsGxZ|VOl;?!{y54hmYZDxfNu4N_iRP=ne!30WuM82s~!T2pMD8 z++mdlk{?uMt@ZC)QPmKnBS=cv5kI;DNRZ2hJbq)q z=Qv)%BzuuoNyhhZdc`=ECT>qG9RQRhcYu|rP*aRh`MPqhTa)mcw6245F<85G+OOg9 zk(CM`gJUxM2Pf@*gHi$IVGG4BW_r4;G4TFs;WhuFt|`^-YgrK}^F;I04{qxw789ZT zx}c2MF#qfo)&oV**$`GvI{Hq=(Lu}^qx8K>l*HBtu%R`W_*7-uWsGcW*W?X>f^JI4 zrrCze>y{HPy`GJ=YTS(J4G_ff`wZ=)XPU7yyoQp4wwplt5939j2C8R*Q7sk#e%Slq zr8!#B(Uor*jGrILB-LtgoSCUIJ`YJgOp*f#y0AGXV)tHgUc7?S8$7=yFMrNp+;+4J zTTgPS--$TLNZqRgZdig@#RrrHel6BKlVK>ev~*Y>0w{vkHc}LKVSR9i&tf(_gVz^o zBlV|dJ~j1DtZl~%&?O2HP=NO(Iww?EafZBVyrkKuiQPz9&|;vz-Y9Ln$2GA!)pBoY z$NxfMAD~PLd~s}MHj}^IvwTw;W9J@ll|$ZbvE{=Jl`NXAlHMNagfh^u*PaGZ3#9by zIXSHi;MN#!uf{^qKEJ49P5aG2P|*-r*E?wlY6}@^(0Bef^6M`bNMsmyy6$05wUSn< zk9`abC$$7b)-Hp8Jp0MmP-1_6)HHT4&wI5mgDO;MkZr42@h3@imH!ygvfY)bn>}K$ z?mgx&=4!^B?;weXtA20YVXRg269E)I*X%-cAODdQdi_d`h~cYJIZoW!aPH92J-wfT zR26fY@W`p`g!Z=)SWTboy=M@s0c4q>Ue@xSPeN0I3JHdX>Z~WtI2rF=K6+ud1Imj@ z9~J3vk(USjgGF75dnSvE#iIs);B>>H&l{4)W-;*$UnZX3X%UW2T6ii+lJ78~O) z2^sF=1fQ8F^d=QXywaCpQJ|2Pf6}ZfWBKpr+G_ek zuzRafy&x=eMv7cVui96sh`n(CyeA&gD_4p=%eEd^eWR|&Eld4u50A1QpDTf@t|{jSLP5I1)`TBs7pNOB7b_sM}^x8x1viV9c0Z*z6esGDRRQ za|xsAZC$BY$FfOI?hi0k;L<8KrO-YuD!|A)Qzj%)I6+s1p>f!0A$5fz165UU_Vmdd*8z*0bJksUxm zK#c4qyNZemf}(7Jx=uyZ>rGElI9< zj^jAa<5YYTUkSb-rw~Dl3VX1z8I3E#K^OB{hA-xw9}^&tVk(cYzk+ua*8IW5HwIK9 z;ELsbTB)Pf@y>`o`K@qGCv<3>U3Tav8OBswLlbkZDWga@qu8SQ$%)QxYaxA$Y|roD z{$adK73Tk_OKww28@~9kK3X5|m6zyHr==0umNE7C@Ol4x;Ci_-k`xjoJN_tRDrU5y zC5m~0&YYWO(&U}qnANXxG7z_qw5rt2-Xo=@m8d~o9vJ6kd`MkKsO#!SX=qsg^rxpp zKB~4^AH7XEh=`m!CR^T2jSuJZM(zEPCl(L4%zK!z6}x2suIbJDOajF)Gfs0=&>awu zn)anv=gY%NH_&j|b`;9e_OIRjpF2yKNo+-L+g$`Qyfyw*GxFH|wl=e{`Hz1_R?Qi~BxoaJP{#U_^{i+7hdC3CnSNmzXA zf9}}uQDh6hE*gH67@Lrt4j0pKraZVFfm@AiVc)4yHFli}=Cv@30l(cj|9=<;*~ zZ@rtY&;Xd0wjxx3rJQ`wk40G{2_2Gz$`p@dokbePyds@iMACox39(JdOL>L5I3fFbr_-lnsGwEo=`k=Eax^Sw&d8+T+ z>t8IOwR$s_yH;cCHiYsRe(P2g7qCk z7lbCnMsM}OCNepN1qLm2LV=J*fW#b-8n|I5>ju_g;}Tt_YU-T?|J8^ zU=GN#$fTtfofg!i$&c}+3en4RjLtp>+rLbcMWP@bG3diJeg7OS0sgVVZ;MR*D(U*> zceQtwo|S{^L|;$DJ&XPEpb=Y+R0^=$7?ry$v#nUXhUxRAy-s9$!+|I^F{?(|v@$l4fFLx@D zq^@*V4Nrh0X0Z(B8S~wgCLzb+NsBdOx{G&4_A(%9gh}Twfjuk={Ec__FV6sdR5`7J zQFJ7`u429fwx~ttqGB0op7gW<=<)4g!B_+UG#1$(3yeeKQ?i%Yc+UPjxmOAacUTgF zbQ+;CgZL$W?bo2&_^l)VPsMmN*Cu)-JgdQCGybda7aJ9UPdr7u~r zJ|yh=Z14{h7WB9f%ZWoR6;ow4Q?kq49x;$)qmvRfxO(#*Q9DJ2AOT*~EgeJJ`v1(T zWg*VKr}350pVGlWff|B&tTvM6GCk&8oB$%7@Q5n_h`*d}tu-4btb9|Aq5SB>#ID%$ zd@?SUkRAgM1J(!&6PrIzbDJIsz4szf6heDYsvvrpSv3T_YD!fjkn-(dIQg$RL}S(% z?~CeHx#Wu-&VbL$bcxl#oXR+IfzR?xngYd|HGQI=7GZ2LQsB9K{DVHT;u{=9?`oor z@iBEBq@Ml}?KAq%{0Bak$!#&(mO1Uoh-x%lJcCrVSzKeOppruOGTLawaK9WT=}!f$ zCjo{|?swFZJ;@o28Je;J``4F1|7(ee!T-z-@3iwF=sCE_WkZ*jb;)n~ zd+L7VyDF#yakI4YVpIY{k36pDE%o(lm_;&lBs1ycK}6^4bm@jJ#iO0vD68=6D_2ya z>KZfIHzx?6x!yYZmEYdZ4U@r`GsuT&%aI=O9)a)rE5f%i&`}jLZiwzIJ zla1$iJ$wJz#Ry%mb+bk%GYYO~t#*u}4>IS-<5FLNXMWn4$H5AuUr0A(jvEeIe3#jy zYGoTPW${IVy0+&N7>x4n*Hxl^d#{1~Y}g15d-mvJ(^8sxgHiUoG;g-weytm;mEhO} zsj?m_;Q{%P8~$UErUo(wBOQ(;n$T9R2Rp6g9^F+^9Xj(vP@u+}Ux>?E+$%vzKNAl+ zN5%u5Gm7gdf8W}L&v}3Hz1Zt0HLE9u+FRk}RV%juS~-b!5~=d;ner?*Z0t2?B$a0+ z_a9Ajte31ev!T*ffE~XMdg(*{@r8u_%Xg9VB0|EXsI2h z5$jT5Iy?2JP$%TsKhKZB-ILHrDM@0eF~`&fz?cy~7-4JIL#8`1g{P*Dd$vh?@J*df zEC;7U)}}KI8XZju&;mkjau7Ym4V2kp%1Swd%A?_pr#3=ksX5KCnV$`COmVInCSOrt z4EwV&(~_)unY^EuFg;@jK}(YgzN(n00-FIw?X9{NqxdpMh{tYHT~<0iuujG!&DEo% zW1!~2Lz1e$TTVQAkk^$gpE0#K&3JmFCAD|7KGGqW7#vA@nKjd=YH37|7)a?FbYOFw ztDYOnTQXp1XwX-mZSr7_8XUY?*6NPTldcoe?Qno1A)6*JzCoHn7vWN^B%gI{j zfoV=NQdf|Bo`r$+t-7J+mVr=j`TkT{$Gdgey>4mrP`&!1I$pbkY~tg|AnqU-iW=DZ ztMtEy3N(AeB6isyW}Ls2*ATXVjd20Q`_ZU=u`IiLD65HmJ(I5)wE&!C_6V=mCfwQS zj$lM0bJ2l4UCcWfkGy7ef&alGh)$6->{kSXIGO48n+;mznacS&?|E78u@p!h3s#@H zP-4TnrcND0qV*m6J%yQjq0AK7%}g>?ox%L_I|fs4E~U5pg8|!x#wvHSDw~08S=Pw& zwF4K&rHN+29t{?5Q&A1#3JhOY$6+PQt{A&|A8tao#h06_$A7Rskdt2B9#ZcST$k44 zYDrb+B3k%+mRN)_Mc^@KQZ01sjjTr7A9V3JCiI8TQDSZ4-T3iJAGu^2!fqa`AOaxL zanO@VZNO?4dX4+;VG2i0n_a}~2)jgcz{V1~GV>FPGrm16z&2f6YI$2lg?f!`mI ztCgiG2MarR&gOf{j=r+aEGo}+gmC@p8=QmrXq?vD$>W6!bMU}gDc*_xv&H>xy)u@g z=ZzaKu`vrQ^k+EJT>CE$1n!bf?~(IgEd8%)AEy&4mgI_l(D&A5&-ykl&4X(fSC`tV zXE`|KU(X1hi8^$`hWTZIoEzU{f)FqR*nHpHF7+agMon82;`}r`)=^Cd_Qh{SDG_}t6rSlv$0>>{YqV$ zT_~Lz#W>m7yK*~o+Z#L9R;l@V_z)E-Yp1)S1{r`0HzG)50IExxJI{qnv}8*Y!k0 zie<&U!|5gd9%XXO_Ou2@m*tA=Q%!-bb~}c2*69VKvaP>Aw$zRuY3MezNsWR_@%UBU zfu{O3Ja{ey#a_8Gu4_)Z52Iu`&dlos%Dz84aTsm}W0zw$&9cav`fxuqkl88L$uCrx zN42}2;awXdRh@{#hw{fgRLTesJ4sZ>`*Q={(zk?*5;Cuo+nKTyXN=C@RpB5o&4v@x$FUgdTNX^tgk!0U^$KAWci!Qo|4P%6cIbU zpxd>sT#Mz2(+@>2bQiD-iUumiS!m@e*`JCKglqy@TvW?`)6Oh*rf9*l_d8HCnigothkp#(Rmxbd|L) zbZMvGrbxMw?cFd?tpE@|%_q3v(!4-8hRIn&?KmUiR zVN*(hCr|D}g*o%d8~$#&`Un>6C=3)oY?eO*TdESJwu<5AcN~33h1Qo}=TA+w!a;+Y zz96X)mcYPs7xUIvA8Ik61xLa;68dyIq(t^q^=mr^(R+MWoEN#(RXd(%MzE*xP7&)! zjOe|Tzz&tIVy5UvVZsCTI+8opyGOqo;tS&!!0>q1HT~3qN6AVt^9e(7viLxhV}h?C zuiFV(=_8&`X{sj)G_nqn)yX9&*O&TCQFtn*~Fw!LkF(T{{>R_b7$vq18&3DA$4|e{#w*U#4 zD>b8$Qx?wU%^+`>Sr*pL%~xS5j(}pY1csYllpfMad<_#3gW!C(F5eBD^dEHH!Ir|h zC%*idM%td;V{mpPvw1}WslS^MpO(Ig-R*gQUDme}BCr6v>6OYVO|&WI^*${<1}G=d zze8R58fZ_0ZDw#3f=zWX~+N-2eFhF@u?a~?4I_)x<(bT_w zm+mnazjo67t~+*FnivuN{>m5FZgN#}1cCTRStAU7I7x*Bg@4;c=zBa}Q=jB{ zYCbQ_HS#$%cZbG2zqbhAb^4gNM$iTrB0{oJti0PK zfy%qlD5l%sq8Mq*`aO2ZvfySu|K3GyykVxlAih~x>hilhG2}0W-|1>nJj9J?&jSSHkb97e)m!9N`BX%F+;9cM5it3=%Wourb zZHbpJkOkVSf+OnBBX4}h` zFHI*W{jI~7mS)hb^6Q)2JtvIq21x^htL2+y)UxEP}H)Md{+ zJu>@VMXJ{Od>`7vCIg~uBuYEixZ>;4F3Mn5-l{7nz#ZpptaGkPQTYo-7=KVQ#7gs} zCZs>hBDP#@_Nfs|?|H-Qe)*DdPX>Qz*oOJsvt^j`Fwx2Hjdydk;K9o}GvsF^np>+|I&#GHonxH19-wMxOI zD@Y)B24K_c8-yQ&0ZY3Mb@x13{Rw)T_y*sQ+x3H0O-!4~iY))7hQ+B?m5R@vC0-8a zXjyfWAtMRr=ARKeq}^xfI*Yd)tU4UPRMzIKB7350n*227Y7`svyN8K*0_mkiZL&F1 zUZ~`0qdi}L%fpls&LJX4E|;TM59e?twk4A}L?_a^milq-n=-y_+IZ)66aR+#mCF)p z6ehTj<$&)Xz&CbOvDZRB>dPjzCgr&A71WY)X1M{=liSq~UL z@BJctsW^UI`m3d26s&l#VQE3mr6>iHte_pt%T{cZ_GG{7gP(^Seu%d0_a?=l1w~3%j0A@*wF6BdO)cMm|Lw^^r33jtY<{u!u0xlqippaDRgP|M zvH-j4X}Q&9J(rUAn-taL+@W1!SR3F5<4my5wMp2^r@=(|6y?m3~Gk6?n zgUo|HICILnwzwq5T#_TK6q6f&qyAx{fcY~jUAl&iGAg{{rWMS7*U8e51hD~(R=o04 zV8)M?KE8LaQlFnSY(f((Fw^1tr%WbGlCn=0teGS@WeJ{LIWDY>*#6PDIut{}xOQRo zkSzW=P{pl4754=+^R7V%s9;T2P4>jnG_(FgaLRl4CEc)o^kUHTROZH`^n15VrT7>V z%&{tV#-8XLshVWbfia&I4OI^eSCoDhBd6}0REj;V(J;{}-UVWD5Hu-4gjlAJ~M@opPIPZj*;*m&tOi?#cxs7{J!t$!bxJAm0#VmsHf#rMA(u>7r( zC;?2v0x%T-ws$=ha1yMko$qdwAIGLdZ5*y28LeLtO9Bf?^SKoa54+}7mWpGP$4$Ox z-51^OY*;P6`sKA{W6vxz#ar7R*gbfAMtL60LV0#0(B2sZr#=<}&8#!h%wkO+0OlXb zN0i^Oyn#*+vg&UqBosqr?Yv!2NK$;dTS_ia>zHxgp>2GPzC+w!9*7bVd%e%DFFUpE zu;u7;k?l+nhQ`5wt$y4jyU5Zekl1(wtsru;I7*=jgpxlyWnNJ(dh-%do2(x7RT9DE zFF2r-E{oRXE^TKMh4b|2n zdxqLQ0Ob4~p-&4h%_p$kz=k$JCl5Rn5nh>neapSMRTo=UU1@QWSZTRwbKh%I#g-RO zR)jBq0k6H~o|OqS)dSocc6F!bt2wzwxqmZ_dYn1BS^rb1tdT;&frvr3hxGm?Mt|sG zdJd&qfjK+s%1TFL9%SdyPGFwRTg!<@zT-w@Ije>Invx#$7se+4p!GT3Z@RduHnp{G z01yLoM)8b>m3d1W-Cs8y`}c~$OG8n+#45;iM)9TGXENNgOT7l0Wwx4HU7Aw8*1A-& z#irazaTZt{%Z4mohlGAlG3*R6xtvb;5MS_+6OElY#12HT5hYSCM)(@-Lb46CqVrAW zNE|-&OVH3l*Z*e>;r;-g)daZVDNs_G&D~JhkopG(Qn)huV}*O))saG~hSP;w^;WYl zt;VEhKnqk(@%p!f{wC^j^ET+){+1F2$&$~OS=ib>oy2z&2j9FomH+{+JmrUHU_G*) z>f+c#t$#zHdx@Na`j7U?zGcIzJ| zj|h@mkyan;C#I1fSa!L?YpGJ8%28VZlr*!A^B4K?-I6ANgmY)8Hv{*+En|$H zGMY_iPsN6_ zYP3+Uzr@DwRN>miW>{TnVdxaMr${bs|s|?}e19l)gU9Og|OzqOcmz5e;6LMQDw?nb0oOB9j@A4)Z%7 zDcBp30!BPJt!FFuqHAg(*s~8#hD$NPH=4V$#xs!;e8Z=no6B`c=bWOCn-7Cp5ksf z5@<0{DW{g0An$tCV zr8ngZxq-R~x;NT=h$Z65(q$N0!CK4>`{jPbKGub2N#5CnEH9arePjwfaQ&& zamwH+`audtIUHLch5%+r!Ie=PkKzDwD7DXsOczBxw#>fadiMHsx_qyIVeL22XH(uB zUjPd6ug3uLw;btE9q8ENPio+{a;l5U%(c+5)g2T+S*0+vJgw!NTDs~eGY`sbn*oxE zp^Z$C=L?J;!;d>#QYyGeSu|lg5ewq;tmNU0fsSihytCtBUgW!4q^_v+V!5PtT-wN; zgp7+$VA?XrxG~Ub0j~(wQ&4UY5F zfEt*3t%ZuQcalJso!ajt>}2T57-(FPNp}?LZBQ$~U2}c{D8A7<#|OX1PVEbMP`i4AvY@tCaO;$2Z%{ms6tE)Ho@n{b-d z=(r^%T^VDMD8lk(KtU(1R*)a4+4hiVp~hg=(AS=li5V^Crs^6xT`IkOB)y`6nUcLa zRx2%Htk>SN=SaQ9f&DT^`o5HL+^n`Il5OS7km{9E$G;2`^LnNtb=)^}eyOy}KeN`&?p5J*r&3Oq-Cmnj2^wZ0I_c z^)SX#%vD+ze`zvajC|HOd0bERm7j)s7iFiv&yD3V=amcBUFFzHO$qr*Raj9#NhtO@ zZ8ssh#PAzM`@)xnKL)BeCnl4^f@n%g#0{Ze?Sj8~^aWvGm&-SbxnGcY@^}6iVJ9cD zo^Ne?IDB1stI^5xg|?E>8~(LrZ)Qnclslm&Y4mai`SW)RR~WJ&pstpno-@SYvZ+7)Y|HpwRHzacvH1Z{z?ny z>OOt<*qZeA@5M#Ca7n#Al#pwqWOK*3nZ&)sITLZ0_U0*VNfXA?Tlsmf7Hi~eGx;L# z@^zZ7%5?!Q__-jBR}LGsDmq2h@$EA)HhKJgbf$JKPPJ(KK0nS^<@$OqzO~1eXu;{M z<1@O5^Ec*vTuO@Mg0mVafv+>S5!?N(32Bpt?@xX!`$f4s4URiz&gge7e|}t;w{}WB zsW+4oIu~x1Y$_VWpWxPUOM{bC`ilRG#wFSiBi?i{dZVdz&$wxcZ$-(Bj#;RMa<=j6 zxQx2ZHPdn8R=vH3ec@)Stz2IB=?=Rm^*r#oCnzgb8h$1UdW7vwwzI)^3h);QM(tgqaf9Zo5}~i3AHG)6k~G|I(_Y!Q0jCN_N2r#2kgFRbnunK zE`jfE`rJ@jQ?#=^-8a^JlQme`Aw})z{)7hODtS-Sv3^&o(zE8%vgu|)aB>e!+h(moZyW$m z0(>ysrp|T84iUt6D;bf9ivF8L;YHzWh+alv%USe*-D-v;et4#mxMndWt7fWIx;U)I zP@*oV$#nIg^ED#|?(aMWVZ~=`sUo8ID5hD`Tp<3Q-hJYc&z90h84gb2l@)4|b(-}R z4hGZ3J04xdnVk&C{oZybv$)-xw-oTkSs9nPh4 zWx(Ilx@&h}=18de^$W|(+l}BHJ}-(ap@g5~+J1O@#ju5^?CY99?wmaN%$|QGnY5Ug zjweM)%NWpa?A@K5C0QO3V+{B0TMcWa(EVaxx2LaPd%qwLUN;j?Tm6@n$!Iv_w%s8pwWe6&%3O>mD;9#D8`P^l?M-xn;pF0x)2C{&><*I zsYS9aU3$CjrXsJo0(Bx;D^9zmKGOcE5Ld+vr+9I#4p*o{5}7AEqQBst6%qET=&fO^dbLCQ`C>dojvVL2s$Fv=ozYnx)4AI#igQ*e*mNEm zN}TR4?J=zrq=p8ndA^Jb_l(%6J!hLg<&C2npLe%Y3T>RJN()0iQZ=jbyK^=f!{O7> zqn-k9_3zpad8iIlNgIy=iyW7H#hT&8r6||qjhp=PFCP5TX zlO4VQs|*@j&m>Kh@Ha%M!HZQb4Ph_q8;Ih4D2i91C|+beo)^erzNLEK_VoUOm(EXd44R`s%yr1)pZ^A)C%uZ z^UE$6Ec7|PrETZUh$5@qv60ihHH=#SyInTgm15M5C^p?!X({|zoW^bZ{eT9a`t5s| z4N;)KZX|PK$mlh(C~nK9`|Wpjv4^X!$hOc_m|T)%*WupTO{44HliR8!{9We{Yx1v7 z`-`gis`O|l@K&Fa63Hw)6GC^evp(?6!1Nfmt7+(#1Ou}w6@`+%nxSJ7ybi?8!K z!I+x2jJvDZFVD27HMXR;>&TVt68|o4;*r@U8FJ}nsWnL>n(#W~;YmseckmgMwDL72 zFaOT=+uhy)do+tZFE|^W{QhNa*1)%rBh|ti1UQS`DqMHMXDb4g*p96Xk}dy<{*)r^%tFfA%-`ofd% zU}MIN<7lmV6vS!yhOqb}O13#p2JqUlqQH4O#?KxUxGSHgh=IhUc>6$xfg~2iil+iL z!sC!+H+`)v&0l}}XY{&DVvUush12;Eqh+wXzrbxN!?z2iHjnwXP8BjH0}>*KR=iWFB~fwF7BHBb}kaj zubNw_COFk_@(;e~*cm!vWNs(9i+VG_dLdMC;vkp*;Cq?VR?bi3K2&7}LW%;f zq!%JxU5LMijx$By9F{meznEyrZ};%C`sC-lcX+tCD;ig+`F7-(cMe5I$=C^mlAvv>67;G@^HhT%gjpLCgP{<$X2 zB()}W)=_+@^<>ga4JF(`WHzZLvsury>*-+V9DeYs<>1)M#+uTb7qjPl=0rko^s7Or z_Ao+;zb3u=izJUyYH{I11&b<*Jawh;Tx5;h(1pFpt{v5b()$>$Gq)AAcYC8m!nLA; zkivd&NzA~-3ZV6veAJ&yW@ zD3qD-c9Is1Xq8Z-=pzyP8XsORVpA{I@C%GxMF=S9(ne2i5Ing#D9Ku5Pww<+SNPf^ zdva=4-&3pTVqZ<-u_M(65au(oJ7i?mA45idggd$M+@(vG?t1F?i`K^J_g9sB3_W}H zlU;_SQM--OJ*@at;j@L>K!DnVek6NaWh=eo|_Zz*K-BITDHCwgqmISj| z`hk^A@G7lUlPxQbkdaREKWSL*VV!2Ytsyv>w|I}I^(zO1=(`^94;^xJNmF~l%@UJf zeW}KWr#u7Xs~mU*wYY`QpKi9*uVSCb+ty>OpP%@g7B1`gB-t>+ZcE-}0D&f&bNNn5$7$ts=y&q5pMlSMFIQZ-N?3^(-#J~BwR``MHBU6wR99N+r+(jB zYpK5u$^@Q`wfNP2x+W8=OQl_<U}pi%D)t|Nm*39PZPBT+BaiQN562@^zW$NUP5zc z3{41}r3DDy8&?cikqx0}IRTnne$wvq=5aG7jOO_iL1Xo~QMAI#vckM7N?Ln02~vC7 zUHW#095+OT73v3Zs2|j~H95$7I`eH)w&|meqw>}?E$D5fWP8y}@vIT_z1MiQg!h%5 zfJXF(v1i?7H|9FKLk|0IMhRgzw{#pgt)XF1t1A(Ly#y)7w05ZVkm+C;?_M=aHY$Z? z7GW{LUV5jrtp-H@B%^ho~;2Rzhz5mGbT?d?h9_0H!b%O|rcRKmuNR2m(`U7k8%*U>uRXt?Cc zDv?O5)dSk~VZ%qJOW!?ae?!u4xw5>x)96T?EQXqd;WR{7#p$1}DxSJK%AZQ0r)(I0 zLK^WG2$fcICT95;dYo*Wt1U6D8>!P1Z`{#D628(Fz5q?#9%UPM#aeWoi@0DL$T0iG z?y5^^2F08#^m$NHnT%8W9(6d_FKJj;?xh|O`&#qW_#Vw0(Cs)U0%+9)uQLnY3Bs>A zZ?bn|aiyi)?%iu@Doy3~?b~^E$)e(v23@R8LS0ifu~**HnnR0Ig%&JlDMgmZb6%rI zeQK0@kN2g8Z-5p@?)K3-%eHk9udEsdP8AQVom<<#R_iO`hBb9RCU6g?O{t7_=1!Cf zZff0-I#;4zD#Xy2ThDhXRYe?dlJ47{_$ZPeRh%)00IL0BMlh;gDlgiG(y9D>I*qM&q~x4>vAX51$AJR}N>h_rjc#ch>MGIi zW5G`V&K|$M(Zn&^B?8?tlf29J!&|fS%5s(HV-%@@v)|s;_#vcP?P){a_1YleH3t=B zuJW}lx#AloGjY>8mRNT5V9$-ilf}Ew{+onnv>3E=Lc2~_F`w4Lr71OplEqFknph>E zN*Rx)K)%Xy9z`;la4?{jo$*{A%pQGzn~Jpv5m(o;GxX}j!vb`(=kJrGGvoLU2+r<| z+38N?XquW6RJ2|F)p+$*>fAL>EOg&Q!C$zj(n0L=pz}&bIhz)}gd&A?i&5P|*cnnK zGy~xd(21MX>3(W6b+K&amNGU?`*b`4uf#a`udV4%zaG_D+2O-+nR-;> z@|5n9tMuzhpwo-A$Q{ z6Z_i3@nCPRdm|^?jT9da8@Zj};qkr4j{5)n6R=#94HITzO z?buGc@aU^oIlQIij!Ts?P4O>fI%KHZY}~~Eu7>skb~*PZj$k+#c0Erte38^!L)oil zMAOyv{f=|<-J5f3w-2?|NvA};_W$C`iQWsxn8Oo~etzv7&hNV9JT}WcTm^Et0r+IKYrujt-*h~bckNkB*n(&GFN|@`0C3BmjZJTzzMB? zW|MQgwbyI7OG*2o)y+3Mzw(voBCVb1^||sq=b+e~tgm(thxrL^U6XLd&?!1&=*O}^ z4yeJ5SNH4w@|pQ35~NsvokYbM1JOL^uVsjhFL#AiQKogbIR|cgTMhmbN3lCTd|boM zx^1p>ZEeo@cyw^KxnsQN)2ErXN^A0^fgTCIwPPLMo-X#@Nep&=`UOe2?R4!dU?mUF z#0hB>+~QIWk2NPn-nsy9?L0sZ2&=cz!MN)*o4BtbJ&zgzdVyYZ7MUbO<+m>$DZ6*L zWN65GEFreR1v=rUu_d~jRxAy{XQc20Dj5gR?qWKMFp{L0lbhs;HF2ITvDh(=WlN@o z&UsNPI?^Tl)F2rO?^t`4W6zN7__>YwS8ptzKp#ahU&%NJ@RY<4lPYwmwHns~V@uL^ zM&H#2xU^_I*#|+ZSOh|ebq3saN|5vEnKa9pS@?=JZ9?jO+HKk)s4Q3rEY>JMWwDb> zNAc%7ckXl>LO`zF=FXqkeD2)8x*YLJLdl%{%4_mB8Zx*8s_J=KfWaWBIxOj7)axNDm5-699`&B@9$r$lqgwNC|)-N+5t9Rd3TV9>(2i4Lyikm%Whf5vtO)6)Mw_WpcRO0PSWc55ES1+dCbysTD z_QrK_!)HRA8MX<&8@6n@_Bp49RNppkf)rkF+qWuOuH}a+xe?a>!A&!DnR_jjWcre; zCGFLdjf)C%K3Wu~5Mg1Uc_hAJK@Bt%oP#j?BwurmQK@KPel4GHn4tLN{ zkc))<1s3k8+o0`76v2oKEYaB)=0B8v3GAIjEDuq|=Z$k%vc@##qWampd zbM-q|{X~}6bf!gt24J*;#UhP+YQqlwqeo-Bk2tptF{@!P<8K=|ySh`9M>ETuk!Ly- z$gL|b4bc5mso+~2{u85gKll#iE;hFl+pZajSX4xi1I|DV1M0^#bywC)=dWdnx@{-g z@Me*gj`4HO_?NYRH=zED?mh3-g$*wbu$5G4SxM2%qAXThdAxT1QMQ`wB$Uw@7in_| zc-y1+)P4W>Vcqg|IBuN^-LP->)&jto+ho7xsdwewOr*xSwg?_QcdG4K;XlAX{OWSu z@{@FFe^Qg(>X>9Y!&Mv^s9+x{#6ED%n~fLVHWjf&b`)uEM`6Ftdw(N8D$t_f%(G#G zwWZo7_xp3Wx(b6tql@U}O##=J<_%(z!y+u-4@$e=v>}U&X z_zGd-@maIwS_z@$L89~BCqaP1D};mt7{%Cx2nHB4e4g2{%gQEIhi)1$KtV^xYEcjj zIDym=7{6#jd>+Wcmw<%efUZQ zOLnkAh|N)0S}(|rih#;8@off8Ga;jkU9Gb{6Romq(mGcI-9|G#UP>(iBED2qZS*p4 z>ACtdhAt@(8tju!G#)agJZtOh9^wg`R#rxd@b3{MT{QY->jMR$5vR>u>$RhH*7VPl z#skBPdwU*+o~!8!izv#*q&Ut2*^hZ@hU027IvtZbI;;SmbZ~3!#p>FbR?IOY@ySFxZDO-wWNEeG9zT}jq_&~jHl)CG0 zJNU2*emXo`wn)Bs@uIW4>|Uwk26sh8#ji!7I5t|B_Y@WP4p<3?;>Ub(zai8`PV?~L z)nQ&T>%+GQ4vL2xJm*~WFho+Ed?r;W^G%;ezbR@@I+snyM|dP!5jHl?JOXve>Lm9b zNz*nTt`H51M=6q>ZFK7pNHS})Gd=4mTk06sH#le;(J4~o4EF}Orym8@@mV(qR(e~! zLoq-iMnS<612GnWw?XyX4zC?OJM#-GmueNX+r=RzobqpU2;OB6dq&y3asB|)DN$LS z&ijA{mH(LVQ{R6B>{do5IBu}0A~@pB70w<%{*2VHNo<68pB^ybm0@!J0sZELF+^<} zr5SCiLtGeQZkp1m1`-)k#iDvz^(Io;_Mtj9GFJco>@((=(!poET$+KU-Ok=YimzpGZm-~yj2pbpN z*%11wBZ+Z1>TPEVlm$(so>aMJ^M;|eK%~ShD&?i+^!}C7LYY(qwdb36LF8jSv;{vTo&{w z3e{Y}_x&BVNCzA>yw;Y0!@z(EWy-ipuZ`M3-yT6VLsO?zC`Le#9qvUo3co9DC;n#m&XZ?M0uLU>sn{`Urx2 zBM48^?|>ez8Ia-FoUm@@g{+`jr#WRPRjC`@P-=g{WFwe(so0 z;zhfYhGU}Lcd}xOb0U!*ELS`1)Z(<#+@yl*UDO|R@>*t6pXTUpe73`GoFj{eIzb^>h=i& zV|iyU;IAh!u<-9ufBu>+LITWW(#%#+&0t;S-y=x-NGAd+7Yl;ND`-&>U_Yxo|=PoO1^2kK%ve7DKf(0vZQ zQ5pX_Ia0136LAW)Ld!z>&timse2=xy8C`+qHlr`M-;YK^>2@mO?v_ zx-2y!po|F{{w0p#K?{P!{oxpvgL;=>A;o8haPlGLgwt_wZ(@yhUVVUZLm8JL@K*tN z1RW0`jXqa7tUq8FxRB5jYlb$VIzWd&u&P+Rb%SH~>9m*IxIcSACQRJV$hvg_z66OZ zu_(*ZP!?9QlLoNcMMXp%ddo*FEAG#8SWm1_sJ$5rQ4c*CIiM;JC%0RK@xws|KxdWJ zY@3p!v75!)m1CL(RO=Ful-a;OT;S1%WdkZN-V#T+AVJw@wHQYYGG~N=vQ*!{3_8yj z#U1!{^*Qtz?e}G{>`(lDPdxUK^7HS14gVWuK|c5YaFo4c9OTac;UE?=io}=y2EZ+l!!Q)YF2!cm__kjOoM1NIwX!XnQH?j2x z$QK*4#9jvLR#+UO2jnKMCY4IVXGBM5I}A38?c29$$Tk^1gOGbxwD!4CFo^;tMA2~aSRlvSu>FCK0sb5#Q9e{# z&1(U%h>9sAFj59+VN+H!|CSa;y}Y^I_yNeD*l}P=TCoK~XMTj^Q}BUW-yktvGAU%Opc;~j&JS-% zu?htA{{KCy`(N|;|GmBURX+LcrSi{irz~2usI*_{o_}%_-nCXxGY+8ebTod#;tWv% zH;+DJ6}FOAPCKFnCrY-NxX|>PPV>0-iJ8f;f+d2YQPqb6(8AD5I5~+TUkCFrOWG8a zi7q9#QZFnJ!ALiRv6BcMpmzc{tuUOswsV)2vrke~*y#H+G^M5PgsqW7g^7r!# zYiT)QH`aaBOvYCp`fZMoSO7t<8tEW0W;)i$z1M^Js%ZFXD72>|rG|hZ8aB|v)RUjH z8R)oIzeGY0jD`ZC$Mj|(gtbA1GAeCSxK7-m_sy_8DmwxQwtzrd2hPg}l956MiyE=b z5CtgU{c_v-`EmqPhrKVO#D<&Sg_t5+)%_h`7cG;JkXScIJMCR&+J8DNXu=CqQO0xd z`75U*WK>T;J6*wC0UBEj=hVj#F=aZ`C>gbJ7EvJIN6;rbiAS%{zV*uXFj#_OI&q^9 zz*G>{VL0&nx_rM;Ha^8y?q;33cK~u2+CqUK5q6sijVJ8ThIr)yi{6HpgYLulKgkgh# zyk$f*$cAKsKrd;i97@EF$sB7-5!L&|3fW;C75ki}4>T6m(klni#Jl3r3l)z=mE#s+ zUoC#I8UPRC$wkMfsFjiSX~6-a2rYy!L46GiK~zR;0)WrZRBwVzhQJQ>MvJiI`D-_y z=b(;7PX;T|1!t<>ynv?d)(8-}a0Dsh_b`z6PArcgNeox^v8-4u36Iouni$5_0jq`} z?YH%Fk?ln`ZCBKM{Xvxd$>AfO4Lrvie%F5jRMx+3+VI~f``;-0cSPC0q%B5hn*{dy zAyxt75#N@XgkQ|)2$&1)X-^l@tmw2VytTu$_s!!r1(?}A{(VLa3zs{#P8rCGU+Gk% z6g;!{wp>_y8;GO#Ia`+f0&83cLfJ3s%rus8SQgPcmbRmlY5dkB%$8H;csz-|sRYl>ZPfk%lAst{~rr4>aD>JgD@? zD5C7$cNfAo)-$l;A&VelVU>_blnK74Fj4nH&_)sUo%k-e;|t24>G*-l{)o$vtoZMb zsl5>Y9h;&5QXv0NQy~98yS?}|jGJ9(7GmOl+2LABid;XZpyr;m?bsMOi?2n~aR}{& zY6?UR)QS7C$43?lw5;)XD@lL3iIAi%RWJhbuG>e24F=c2{9H*>4G5rNMdkV7sBO3` z)9>}3FQ%g^8U`tW{eIi+W6W^W+1BNwL1~$JHKzUHO6dww@9H?v4^>Me<_Q6W!7X2C zvC!z>FNpprq3VaEpcYR>Urkbe8@~^SNaoP~u>?*uMar73K{Db6AC%&z*ROvVvwFHA z(RFfQKfz!(=~#!h$(0jeVbyv1dqM{P`#xcNJf~LrZs;+}Ee;jeV{#SQP9HzdpXlZf zvl(wz5WcRoVf>6r+xN}G{tS>L=+grtz)mNRE~jG8S$}3(z58UtQL$Dt1$yZ8RK9|x zM~9Y29?r4GBtG&s!gnx7ulEhkL}AUEC(~*a?Xc{e9i>Pkfmst{BCVt&*wE6WpXyOE zTN5>>BdnDW@)Vum-it4B4SClO0zETcIztTZp*Il)*jqaJd}%x$s)qfNoD$~vxrJX=IQ!x$$ssAXox zt`EWkJ4SLAD;Z(g@(~yAP)ytGw@7>b{-E@qk^X4$tR?tOc_~bJb$?Yf(=;3IX*@t| z^V9U#_WkG`-0g|halceE(1~&zDY%HsI%)g3FtKpWi&@hBlFC~+C#ClXxosE&q%NLO zSMsE*1%7If$rw!2=lqurcr{$-4;n=MnoACN-jwnlLn`$5J^KDkbmbItZ{uzfWezmW zWehJONRy{a;)9lpA;}5L$q+(EKrV~VDx7VRZzC>=)K@BbBor(HZ(haLc*aulZ3JjD;(_;Np0TX8yj5V|wSl$Oonge$O;G zK$8lmuOn;-$X+(8sd|@a-RKR&rrh3tEbwdgnL69sOQh$nO1os8@}k@8Cf{ct+SWi! zey^kTx$*dFBJ|r%h^lVK1EJl#lgO_EyVectTUS!9zUqA;h3H}{tp5eNxC_z6AfO2& za~Zmcxg1$sRe8u~{T0OGI0gc7JwH!eIG+6BJnhcYi{Ym|1;zq;m!NDO2>4^y% z)<@%_iw)CIie`;k3d)MJOgla9hxv6;YumOoG%P92MOWLR$rloWGG~r?8o0Ko!zT4NOs8@IS zl=LS(+(-3pc9{x$NUm@Kp{)CO#YlGWECz%+Ef&k5ko;{pziYp}Ad+(FMHwvd+opB_c3? zyBy`_`14nPFJNaLE{ko5==@oK*syF!FK+8BLZwC;qiFu$$Gy8>I?LNTWvdk+(O;?2tCH{ zyUl)lxZ|xpqIU_{rM{{A$kmHzV$@VDisINB3;fkGKJoNd5 z6(HU~5q{tjzP1(qZ<2-B*Lxq9X9AZaEk>=d9MN6Eg#pj97`zCDK(B?KuXTx4IZ!@W zFvG=S6gq2E_kZFo@;*ddZkj$3iVnjn$~90Y+xgDIVX7L&Hlsrz>JhxxH*j3#kd(U0 z;V(?ez~V^lYNv2@VXzc|mx4v;7oZqjmI|T7P7m&1)i;W`xi@=Dr&aeMPgw$<^EPq? z*2<}DM{`@?o}usi(}Uvy+D~5o1Q{m`g%=hG3)RHLyr}1UESZf1R>_)N8UJUKj5GY37+V55d)4Cv8HO)y%Fo5UR!LV&P-Z1X2UgkxDR z>@}kc{&f<8C4w(;gFz68Ph)ot&I4YfI*H1t9}+xq6aaP;KD2foRNDv_Y^1DXu^g*D>usaq>qug(Fn^G@+Q0WpF^ z*NZH_dV}V_xBJ$u*vELzx6RX676kOJ9T2;_8A>Oj=3gzTA^U=}QRC{oWsNB@z<+}q zats>bc%--Rz3M*_q6r$;-&!jqlUN_PY>R4m-6emuH-Y=ex^`!Oc^l`y_ak?a^?}0w zTW%s6GLv^1djt)eAJlVt{=U8k9aD6ajdMP;vq%fd#rl7M5D{jGUd@{lK>{EoOUJ03 zY@$n!eZM#aYqKW#$(>2Iv^8Cu^|0DX5+3LKiN{x=_`^-`=4#qFAAjD~F-ew2goh37 zDPORhZ;)o&g3uAp6 zR;@b3Os|bE6Yk-=vV?u*$~|d%$_r5GCKXZ4Mglv209*ATY%6VA0Se9lZPw_4hM3aK zDQgUA{XguzcU;r=9{<}OwKytNQNaPU4p0#gkiA-~Ac{~2I|Kv?V%T9rpo)uKp|T-W z6e=T#K-eH#MnFIkA?%R=VTU9@NOC{lfURxq={Wb^KYsWAcX}Sx=KKAA#(TWp&lg`V z^wBENt%iv1wAkeJ5G|(uwBIE$s3>d_v@Z5U2xs2k)BpSP^k&|ScsdLL6G{LF*^M^p zq3{hv*Q}?mZ=@$`RE?nfELGT*eubh;QJMfG58jyu?t%Aci;{?SUIZ>Ts{R&lGV4Km zd;8JvF4@+=Uk=^O!x`?ZbuW;xxLW=YVnF`pxLwZ>91FoT1Qdiq4?-xCdt&KU4s;Zt zB)+C0?Q^*+33jwiv0OD7LUsg$+Ilqcco9!40@ZX2c>7`PPm1J1b!QL|@xn|4@E!jY z1F<#=wXSOPt=g5c7hK+UABj)wKvV!JNdFk1tpAb!??x!#TKR9HO%P^2-+hRjVjGd@ zo}}W3O_l02NZ3E?-;tZ#s1zoP<8b2w8v0dt9a{Zme0Du3oY-?H<&<5eeU!?~X@WnSL{ZfBOa=r`u0=(OfsL}M@ zYXQLrDUGpn$c|-5;m zrTx}~U~iiU{xD4!!V+>MDC6o0oo^1yh##mEaV~Vd65BB%=WAT`jDE3@K@=a3U^^+{ z>WFLx2;-J)-kTh(-}VXQa^Z*Y4OggoGiK^Ts~&O%q7e;VNmikR+EG=dc{RPH-X>px z*YLJ!WShR5c)wcy0pKO@haKA$3Xo?#6enkve|PS^>IvrUMYH&+NDrSAgu4vdL@=5C zguW-`K{O&FKpQiTyF85yT;#~&%BRtX>8yxK%>`W>|4lY-Vf=Of{_>r4lzqasI;{Cc zE}!aNKBGoIiZDil-KwefUoV4X|8ae*CSnp032J%(qCl)8WP z#S}v?rePbOXQGQ#D_lZtZxl;!R!eiQt)&{dwq(K#qef`>TwjcO?9blF8H#GMr<+C? zaSrEEIZd;(^J69S`wE=PdUlrTJ;h)~jQWZ+6tXbmB%`o%tG!imD^nueB*a_SKhMoB zRtGNdnwBZ2R?e}Cap~pObz4CjAQYsjYnQ+;M>8+xp6N+(->s)voF9)3nCwm2b13R= z5JU6%hSZ-OhxeNj8iSrOf~1NDuV1~^(i)WKSt#Z(wousXGe@Z4UiVw@XqY6`^T$!j zR+3NwAlU^VBBhyEMj8uz30UR`!f=+T$kon3dAdVTWg<(_ukBP`#=@XGm{EU&EK z5h{kN@#VZ|)XPZvE^XQ2beaAR#dIDw3S5i73~mud6KIp}DC)LYHR6361w!YlM;Tf} zFUQeQ%LYGf!i>uC%I<#2jEZRMwP6M4%v{XDPh%oeiO=H1t&5q{zTx*Sg)Oy4+o@3$ z!Daf=_TTOyTiQ*2EmWtYhJ0}1OfJJM`-p?wIUJh%WmUHe^?*$GoWi32%))2u|RGg-mW*KRaa z3bLFmxtA-!q4Qv&GsM;6#&5HU%mq8`Go+nVo3}A$11O>fkn`4 z)_woW@t1)WeyHhfFS4;fB(9cEjqynut@aA*)|F8ldb15qGl#;oTIl`BT^ z`?}tD#zx~V$j$zkL-}3_6Gl<*1o&!15*>_Pzk>UbLO%(f7C}Cn7Ah>P6ff`h#8M}i z{1jh}*6xZ(#=8i|i6>sj6Zu4Eh)69Me6md`BI(>UpJzM?T4-K zCG1a5Xo0H{e?!c?x(BZ#6f`_`YE3Zy^L2+0 z2HjWpAcBq?EKfzD9jE1cj9v=Ml9fG?mNqQqQa-8@xJLfwi#Rdk(%5y^RUef zkhOngf%fk?bUQnB_>BHnWD^j0{pM&-y!7D~Ie<^q{rtH-VNrbwTgM%hv;s4?VT3iEcD&~QW@#{<}V?^~Eb zDJVqz3T^r&4?W#|w(x3K9$*8Lp{#99^al#+5O(E5)!+Y(l6E2jswuD-D#r1)iEvNe zBS6HCd>#|Um19-=dV~iJ`I9s>D9`PPqrFz#+WC8dA@M7|{$vl>zy07=o=k6bZH3B_ zKJg(dpyI`&|ATCGZbh6@@-$L`H^%tYbBA}ww@lqElr}%&>R2cZ)#j0qqX0Cz!=r5J zf`OadW)~h9aiMO#-lG5pbRvLEseNl9N|Xtq&kZPXUt0f~f8NgA?gZKceIF^V z?K9TCLD_}G9{Shqi(^1K)X(4cky010>?Z$_-3wYGyg%~)7rQ>B^?f*<^?yXM%gR*$ zGtg}%Ng%-($^1qq-4nuAfI#uVXs#i1IVcpY@U^3b=t)hZ_^wq{v}LWw2uE5~aA4ZE zSo)+Kul*0AV*60Aty*z)4@Q&D)0(;veYM~`X+-XZ)k1$4ZLP2cRW<>YLZMRkJ>KZI zrIpNMG^Iu(<(KN}>IbIZc72w*UF}Zj3aD0QFz7!9m9GE0e;J*F{0^J~UWDDfx^i z4Z5QUX@*>sc%2*O6X8#P?VY^OY{b`U?Epg2hgvH~fnoqJ@u>jS#kJbv31ecR{p~M4 zS932gABo!j{g5R9sP@6nSO4a)T2sD%&_xGTD$rm6{SZx1$ydw~M)3+|nY^zC&N;8< z-QfRsW)3S}>?8l!97a72+D*UufUUKgSS1&)#3xz9h1LsY(5m?d*a=XO=bbqMFUWtW z$P2u{LfrwiKbGXz|A_SeUz7g;D*AueF$FxQ{@-n^K%$gope{prdn>OHfAMyZ3STpJ z*a;mxkP>j6=MtvZ0Z`BH3+T(klbbpV+U{7NNk+4JGh2SIZ>Ybmz&n46MB)B+>*o({ zF@<;je7p!9;|#y<1pEhkdiEUpt&-sd5B>cApvGIQ_G8hP3FsM|MXxtB)=<<5*Rn*- zynlb5g6Bz0a*GGC7icG@YUD(o;)gYWg8nz9PX(wZP=7w$y0*(8c_eAzDF7}1Qew}2 zW0WtWdw{AM@#qvkXj$+;6aSOKyWHxu?C^}w0G@7v%0W5hbol6bwHCL+e3?XiYz5t( z)-K;qc{IQ@u?N^^8K~w7jgF7{LHZ)6A}~z7;QXU2`%bf)1#Pg7Je6snAg01DC%1MJ zm09k{nB3z)9Rq^rL=Qof#3d?I@#MCz+JJ!)EAJRZ@*ti(oY|^`Q69Ta!wGT=eVD8P z+x~fGpnZk^(6LKmTVlnd5AW$YRe$`LJ>2&B^Wqev--#5!aAkkbr$k$eK+YoxyufT_ zmA_+?F`i|>Vcif3l{&_#fQopVK0|%4+;qN5qt3;l9s(#vk8rLFSb2savk(GPwz{mI1}|49}11 z_%b?Iv&{;c)RzW>$fN-dPGxyC8m~66tdAl9Ltc_oDZT{^0UuCxf9($-19HZ2AKSN< z)m>6oE(1**cq_kn>t?W4dHS4n%Nf}=vDNX^$OOPzNFkflQahlWXf zBx#%gerh8$U7T&~-sEO4H>SHY)7a+G5zyTI*YeBJplz{oPkweGT6o3N+O)J(br}`R za+#Gt2lFDAYi}Ex>bLyC{8)&#I^ADRDx0g`HgdKPXi+QWFr3A7yrHp$EyPsF^QBKl zm_YrMKYP|(*`aO9%z?m46s>?3+vivCz^?2X&Y`cP?Cjqd$`thzkYr0t&F76Z~Ux*!4I#$1XYItI?qskC}3o1XRC-;?+%M{ zhEgC+iTvXgeOZVC+NQ-V>n`SH7hl zRDw~Pr>!eEpFc2Rm|0`HzIVLcB)IkSJOo}&soy(QWjep}iDWlt8N==F^`&MbfCbT| zuS~0fXk~psuVFdp9}3ApygY7F%rB~bgN#qKbE;%}qFPlbabA_IKC&2hUJSoU2xb)k zwr%3G3$fY*95>yM_>M~t=y_u|RLJ_ZX)wOgHdH&oNU2nS1{MQ-DDJo1zX(S#{{Qm`v%6Gl*Iv)XO@Y$I~ST3356A1>Wp_b5ESAtD|+#k z!on}4iCt0c z3DZ2wno_vZvvYmUjHg=TrLK^+6;Rd9)4DQ^6YV?G&Qg`O3UDdhRWVwlK|lvI8qw~X z5+QR9NE#p$=Vh7}7iE`NKv1pil5WsiOuSQ|7pATh8)#u`NtU-k1RbFV_F_XZV7g}cK`m59J(d9VEJ8x z=APs%-_eIUJ)+~zM{I9LJd*s4lk@McPmyhcPT#89)PIg*(}_Bkgeu4L^Jhi;jp`}m zS?KfeUv4q}73i)U_)leAb=7yGts*s7eg@>tn~ULCXcppBdXig6@k|M|1z>`ZpYidT zYe+oBA4vvIyT9MJkFJ>&yFFW8ZVMDmo@;Q7bt&aHf0qncZQVfjJq9h%4~(Nq+ITAE zHLE#bm1AFXZ+m-JvG*r*@Mkm9qVC)*2cd{^fi@Zj7xFVENy{_J}@;(tDx zv248$2)XnJ2Rw)tycwTae~KEO)z7RL=jLvA9Jd(@l{_{9*$w?}{#>&0j<+|q70eXb zNtcX28OM1lo8F~-4GkF~8l#T1XU}&0#bZV30orSOtmjM_t#v$Jb*W_)eDr=83a@Y} z*QYH-BRgd}<7O6knd{yH|9mNST}U&`G&*anGI)~@M*luj%w_jR?7de?Wu8;-cF*JK5dmGr;&xK5y^(0^E@>cXPD?y*IfLhW{W&^$Dffh(+JPq*ixoyzHi@$1|-Z@!p@^15~O)IXo?x)8l3HUuJQwQtGo)K|kN z0fVA;n%LZd&FTUNmtEchGQ#g4sMIDrEG+KsHX&`}hsj?3KJ7v2T1!;|3D5+^@u}g% zMwd0?V5)@NKfetZ>Ixi5inPT}1D(S5ngq^_SEX8s^-{mztU_~)6p|h?7J5vo@qc=* z%>8LWd;$r8T5(q3blrwa^Hx{Y&&^hElL_O0`qD|@KRlbsCtNoTDXI4z$>XCdXHKX; z-NoF{pzFKKgFBhSIbQN|iZ#FuSp2D@uhgp$-t`!#WjIyF^OnjUq`APRade;!>!m5{y6t+nht z*MkjR{HX#JQk(t)xc}Q8)Cs|vixJMl0kp({Q)0`nb)K-^4a+Tvtq$^$mnj$}^FpWW zX%k(b*M4=&*hmZqDXDopT}aQT38~6sTtt*zUV((Iq1T^=Cudr+Gkl1p{kHm=%hyqj zM`Gi^z5Au__s^$EJRtIU_f5&u|COe7~GQ|LbZ2)`-G&!>7hW^1#IF@jy>1@o7&CJhg|1p2xk_Vsafrf8*ohJ1( z1tcL3x~3&UG->f|afKE1?Doo~<%c?=!99+aQsPAqw#CdC^cXbVAsOKRVC39e3%mae z;{9g|OF}AaXh108M`}^|PORIjM(NZ% z9HdL9UIMHYh)HsJ-d3=q8d#{lG5f2#seE2r>^GJS>cLf3;hnXkn;EjN{_|ax=Jh9_^SACYb5HIBaBCEDgk+#MP>^x$n3f zh^Nle2!RG1um}ZDqGSBS(8k>%{6+Mg6<_()?KTb*@;U;@qFRuj;xCA=lYUsFE95o~ z8Xb)r#kL7;{T!T?NxTc6qRA}&xyXM#;g>3TX1^nl=kR>TSsVw=6hfAjJR@u&(9+Eh6Ru?XzcN_yam1WkZDC(v#dsUTKJ2zm-x_W(43$;DQ>91w`FyQ=$x9_Tl`yw?=y z%Ku<+^-C=}4emBH1ytag@sw$&469D3d z*!xj(;37f0X&ig+s?!X_h!L4$X$BZ45L?g2%LL4al4ZCHGI)v>cX^yM==~)?nkA+N zNP}U4|BA9JGs!f{4spu4|D?umfYm>~#}71tKwOUKOuLv37mRY_9mfZaI^Fs*P5|-! zAANs8cJvY7pY?HS-Du?ea9z8`P3Mvcaa8?YyEw115fSo;7`Lc+vExE2UuuuJjO}wF zJ@7)o00N#4jiszP2Q_Cz6i1+kCSUr^6gT)o6Qxk`=MXrxk)=?g8*74e`|p;;jV32G zbw+ODKq#tKf!&)|#m{)EHZzjx&sfHFRw^(4hZVOinB)O6w#$M@Jlz*J2? z!W6nI*S=Pg)={YQ2n_b15e{p-N0N8-z$yql_G-+2+J=4=#GW6t?cMJWqQqk0 zVqBa{s0M7ph2CYHXb`XvO@11!FK5YJA*R}83~e3G&oAb^%N?20#VJ>1z;qns6c4&_ zyh--IGZAgE`1Riw&6upADG?M z{Ya$}b0Sl+gCv_=tha80`^z)PiOz~3>Qe+G|KTfuj~r( zS^yfR023I%^8nK7|DyIs#8+<5UKCPv1XyCY4Dxr5w7Y>MOur|#{$#x)sdpzE9LGj{FfKt%C5H|=15~#uB@%|Bp zfFW>pK=oa0>5M9B@_D+p#5sMH zu^-1Nb}2sMB@#sj;^+LJZT@GT-k`t+W#NTAHL>2DAu-NZE2rC@#lj(`bnbZhmlr_v zOBOgC24)yd%3J79^W%8rkJjQ8A@Fd2lQQAgc-^`Djx&1rE1#to5rWsDqP_bTl@!(w zKhEYx)z0HLuGtf9{=kYn;_fz9!-myaP~;!K^)!&n3>4Go729M9+i^?sP{*Xg6|)|j zuBXdEzbSry5Dq@FJ*}0+)LLned?K?j7-HRfZ%ZAb_qG7$IdBvMO{XwJ@PRmh&?9Q^kk8!~^f{Ib=_$_axKKCn&Kb1l8QTy2NQW{}{fch87!qF!ItMKDa)#sb; zl3djx#x~SyTZD+bhA|98RK0exLrT&5d;u^4d}Q5;Y61smW5aYgRG^0e{+)JYt#csB5+aw@}jG}QLx$XGx+E9?0+}gk@HcMH}bLxRprWY@| z2wdrGPY4A3lZ4XQ3R2lZlVmo0y2BcPc=8)ET8l+NqbA}AF?s>Nh7|cn-x40Ph52*c zw+n#Y(V)edAD_H0->O{W&ms*(U_)Mg^W%j-ebV!8*%qY)y+lPkba#*iF-v!r6DybZ z)7rSJZS973P?0GmC)y|{9tI@@8G!5ZX7yuU>ucvov%cemC6 zM^O2LVb>;*Xv(NgECz|KetBH=r={-?ZVcJsnJg9Z48Kt}!cc_E`6=tSJyIy?KC;G8 za-m*^oej$j9-yT#<9I=0MkZDcMABE?OEq3AK}!|C9@Iltgx~#1m>B&=wpk}qG~9`M z>mN9zr1al&?yXoF4khDAPV#mbw5Pkz5`=?Ea0aOf8GP+r)BGC`X&%7!dkrl5ajW#P zI>;-1a~_305B`H7r|Q)9gNp&rb&sqV zX^We`2~OIxFS^o60)b040h;kyXo#c)15J?U<4v2_ z=(?cj0m?ZJdJlq5%Q)4zf|^zWvy#gugU}*Jg}X{i<$IWxBe$(sdIZEXk&h(*i|sqC zwg8mg#`6|;8^dHPW=1B5p_sXSw<``~kOrhl)QCr;O%KxA#CXgH=Iu33PB}Cw%vg)j z!EdoG2F`@Ms=)8#lVimZ7b+<&@itDyV|!>_IRRZ|REt?TUP09PdOrw*5Zy0rFIDFI zHjYN|P*fR%O*Ylp*XO1+YIQZ`|5KOw^fy4SliT_EwT-m~&0iF;>n&I7EaTb&IW+PL zH761)N2);kW!j~m26m5tp&$qIHb_x&hUaT6w>*fKx6>{>-!d|}`$|m4fy@(z8iA&O z*>)Oh$3tZmTW7<|dsac0Rtdn8T-}kKkpYXT?Yj+%rr~?cbK+luwP2Suw^vv;sx9o5 z63X?N&*hkAK5z<+K}xBA%rq-$6{~P8wW_*3Z!UwL3lcj8McS#Xha;w6C~vv!UcI3M z3wM*XJSF(-d zB3Y)6h0EGPG_lKFwAdq#Sn*Nc7$~{@!g_FjTv5q(Bo_o$NDjl9t5{MkTsV}di7x3b zKpMYQioB2*?2&aC=Fv3mxuQbb2lx0y0>|2fX zOd+hVrA0$O1*Trdt^ma-H-a>1PCjj&m-kKBM=f?G${;{(!$}+ zrje-CfGAfGo@4#HL zM198}8zE2d4kKO_Gy!HCKZiHkwfk)6{U)Hv?uf0p4kbZV zWwYqnP^ay;n`>P&zhGr&$-(xGZ0wQ+uBO1`DJ0J2AcGmmWi|*Dbr_%h`a`;3)5)QY zZ@Eii+(RWD5>mCVzSS1pq0X3*ue^_5en+}B_{kRIvxEE1r5_D-2uu7N0af-vUUzLp25hb~d@yY0 z!7o2tY?sB=&-K73XagKlU?^^gaGs~f8t_Nu*H^Hoog%Z-8Ib%}Fx+K~jzudp5UZ&7cun$Ay(=wJDJbwJ{5$=#)!w(D}Mv*wAVb$1S@m{x(tmAaJW!ca2>C4 z7R8M(9e@pef7;nCo6SRO!UsX;E+51j7v35~11QjO2xk=$82Fvh3IB;t3SoD6E~C72 z`^XhIFa=H<^S!(i5I3L#GI@Z@3LIc)e$TussIPo2sex$nev>4lL@>czfFpcb@a#%V zvjjkPgC$o!_|Y~4G{+xyVc3Ectu1D+MP%fI#?=ron&sJT^Srr!39BlKuj^6q_X=|2 z=#qAc9?dDIs4<-0EOY-@)6zllJWq}SV!F4vB|#+`KPl9zl!?(IW~D3}=G|P;O6-!a zP$$c`jLI9F;x1UOYLS%!*|GQ>ZjVf)*a0@DlH-9Q9}Bc0lrLK3c@7ESOO7z(={cI# zgzPvcqZ8;V2Vs-x(Okm54Z32$4ZC-?C%sK_QNaL>FD~!1^m4ecAN`aKBT2Np-bi(M zWCMoyQLW1bptacmz+Pw3gzS?WGAW7lW{d(A?(;g!)IHKP4|KjKFmS7Yu~_a|(w3u3 z#(`s&37uN=6a;-y{t4Zi9wv{^kj+W)i7};B6I&8Y2ZYTl)8h(c;7)`J_r7-}oDF%N z^BRch4nBO3F=n4R)AH1Qf|S>S3G0AlqjHrdBv_wGMqMEd*ypWJ>6Efo7wgP>$fp2c z9>DIVqMN%auJvUiti}%H1$1Ar!4!)Lv%*#n(RxA$6p&;~kw9_^dj4Gx`kMy38ABW7 z>RTE|)r=WvGeu*}t`c&zxjoF=Toz7iU#KhlonO5`17X=OwXK)hpotR5f?FwOS$5U` zq+nz=vJoi@VssQdc?KpFH52L4SD-O*JZ=<*Eru{^(2vQgdS7znfsEu}LrrVr9stS* zkqg!uEgj_Zaw2ciQS6}wurv0yq1h~Ek{Dj(v!V$EQLJVs$F@G zjgJtNm$L~Pl7Uu??DEb7y)GD)vWZXS)t4Ulph?k)>kXac*R6I9>U{_L6lxBLu)J=} z*~?R&?{5h=PPW%vxmB3RT_JOCKoH@^)g~EkR58^rJ<&qa=_Dx@?)aGy9HCgTv^~$0 z9FORd$0#7v0^1$D=TFNQjg}&alb8f{P#%7c<3S#O_|?1#@YeVV#FHm2SW`s&h5eL` z#J658VxO|07CvZDoyB%>C=|1|sN*P5V&zT1LVPlc_m^$OcTPl=z!?qM_BQ2(n z{7IHW-o&%e7D8QYqDT(h&a=`py1SI@iK@GcIccdfjVAX6()Q4}EGCGOM~H!4tJHzz zi!>Qd9oL34@ok_;PNSOL87l2HBOOmQ7ZWotOppI|e;|TCtCBcnj%ol)i=7)sjHs)q zc1Td_&cb*w-f32z#Lc$F(@_-&MVYcDhrns+Jg-iaO_*fM>o=m#HjhNpPw3Y7hI~a# zms)=4lB>=Yy3um3KBz0J~|)lzOd?}4C( zzlnVqR7-m_TV|GPn_G-#T&1jh0TZyKEf#0*#jgpoGOU_H z*CrtTMw`?%{J_32{iz&tm+0=rj>48nioOEUF2H_@D*-+k49UP&bRFnlqU8o9*@dgj zb;Jq%?C0HZ07cy{q)N)tVTOrp-mZC*KdV$i)w}vu=c_4HOH;b0g6A90)_0$F(h&69rdJRMW`hCQx?Srr0Xqj4R&<`xbB_i6sH<*D)$ZHjle;3la!g zwIsfW(sn8nqgDr2sd}E{#3KX~sid=MfLN$;t^b`8u~;AB(@>I*&+BwLC4}(`DW=?y zRc4)IhoWT?9!?vC=k|*2e&TxDVI)*2uB)@siqN8L6Nb*{E?$u!zhZ16zn+^RI1{nJ zjR$DHoQfS}tLYE{l{a?}ZDLT^!4wy^hHow2Z&^~C6(7*^&Tgg?pSCP?*0Q!cZ}L7k zg8~&9@vOZz3?^B=uy(11y)tNn;4azCG^aj!-IT5+7)XZSZtIPVt0|2Q7dpC7pE0GJ z+iJNZ7L214?pp=O^sZh8;23bpnJa1R>FRWNWgCl@Oe*K0q?llW$G0APuoZf@^06-6t98n+TK_8w$h-cK=*EhT8|+HC`~PS%q8l@ZG!($S=tGiN() z6;pI1Ci#TmH^B~(Dyh4N6hYp$9N=7CR2#mDQ;^qJ+JLGMP0m-%r#>RQcF(xco0(7v zrvRBE#8m~jE?yEq4R61OvLgBC(Yq2;Gf8vte$=SU7LJFPPMsNXU-!llqf4bmgYrxz za7_gwrojKeV+#c0hS1bu*BxOhpuBwb9w;xrhdicbQSCtl0}a=%Y3WXU9Ghh%O5Cp_ z5RW!TXcw-SEWed_5)@R9bjlo4axCz_F=Bvc7-NHH?w1BIR1lO5zXtv&0!}*&$%*O! zqk`)zX6XLJ8*nU=UW(=G*9Jq>=n>b14xR1lp8j%E_2c`IOEEJu9s-F(f$5+9IiQmN@`Ihyu9h~$rs5-rGjd1__ht9o z@FqlClbGU)d2YjGYAGV2Jkwbzk!&Hn8;Q#)>ic}!!b16@&)qblT7=5o3tO?sn~XOA z`&T;H$>NUPWD%hxfI%gtoGLZyy-vWdWSDAEV0nJiL12SL5RU4o*-LpO@}6rB)f~C%0J5Npu6d@a4m9)@RGyFPyaL+OzAEn zBoFF~buVXimk+0nPZrz~l1NOZV)iCtc2G=Mt;gQ!!2gu(k@#?nK1##~JpfLO``|Xj z*eJFvmw*MLC|_cSLtWf9v-oE4P1qx+t7wLEQ#0I>{N}0FKbLG^v*Q6&S^@1kBd5-`B zM(iFy^3U{HeP&rM&v)>h@{A=mzHk3qePv$B8sMol5mX6l;z1iRFR2CAj+(w|ab!evyES zFFz<^tx+q?N17hMM-Aa8kORdtAquL#wNqxapG@W1p$O`*WXs%OYAgNl(mVDFPND@@ zXB=KWfbO4yp(|BX-F{YJz-F5Pz~m3?68s8W{srP;_sXOcqAL%hpcad63!6!qfMwo} z?7OeXaR3KyU3-^!>1Qce4=i94mS?-#-tpAWC97H&_e3Eqh^1pUo7N^Fkx-ZBqxJO0 z-N?o9YtYS$80*9Y&y=s!_xrR8(5rkk!!|ti5lYxj6W0-VT##XdWv&wi+QO7Q)5`aQ zC>^8K`MGq7dWjeg$=*zmcN)4Q`N=#iSYtz^U)!;BJ2TC(K;P)A1}P*jODm>XhE+Iu zX07D={J?8>6lB#NF)BpWDFDR3)(hrnV5!Q`kG42UAvn8fnXMH)O` z{uz%I_E(jmj$DRgf)k-ty~e>31g?Fw+q7@}U1r1hZjES?gDyCq)Vc3Plq338xo6cq z*spc6Q7^$gs<^#9hN@;D7p>_MR2u$n32jh<8m0i2<;=T)KwI!`ebR=|nIOCxklRnO zxPA|(2h=51<+M2vQqgW7L)4UQ!*ixDl#T5=91L3A(8mA-B!D%Pm7Nw#vL$E)rgh^+ zGseGSVGE8Hd1=1j!1Dqa-H7HWYLY-n%;;_CVSfo4{`RvvWIe1Q;bYDkCvKO4f1z=s zoBmki9mY$#vOOzcW*cJg5vCB)B&938)C#cF%Tn6v_N@V(@2HbTVR3#jDo&UEWN+62 z3P*%JNU?-_MV$wF)RL*+zR(gg@hUWxr@7~OEKkGTz@?}fQ+6JR;-Q-yfS*EdO^j3h zo(K-^CkON$=wJN_HNAyFv5>5AHqxM(=6Q|CXYUO;(zr=-zz5=Nuc6_0j`(eV3sL?5 zDj52z9vhz5GC7g1Emm(K)*)ZMq(>v`2HwEJrfxJZ){xs-??Oe05wiNL+(zKf65SBU zpggX!M@(;{#5v{!43x?GK2@-pjvBo^TgHnAHUkh=K^uKTDKS1==AvxSHW`h3lAWT8 z#+>%UH1aY0QSqF_h5LvOv1EHfxN~%+mwN?Htuogc+l}+i06Fk>`LaMXvcBipqbomn z!i%kejCTF&8sgnuSpNZ8YwQ!5Fv++t1{AW>X@5k_>?C@u(%jN;$-6Xp)kS-t59MHyagS8sK8UnCU*$U#< zyh+%~(LJ^_kLUwo^~tW-1yP zaNS6geJn4qnU-yu_=A!DHwiwEAko44TFN}1)`t{(uyu#GejGAf#j^opp++cY_3teY zWUk0NPAJ5_5WZGE(lwhv&udetVG)Dfs+zpuJ~G<*nrdlU+eAsR0Zk?JWx!AEd76AB zt2n~sthIm6E#5@be@|Bw)3S2{w0yrO?e{~dX#F4r{)#f6*#uH*rZN94oRCm%5(S$} zHZHg7kkSrwypZ&LAxWIy?SQfNJ3(5o<2*G2wM#OJWR}~bztq5v;&YcxKV;OviAs8| z83AC6+qOAssRa-_Y_7IG7Jk^4?^SbUet?;~>uv)$83^UawO`c)MaC7hVXk@R?6=W9 zCz<|bcG>Nh)*gmoH|r*KGo126UvV4dVyD}+cqcXia71wp}LZvJlFg{kZ$j~d(R;`>!%Ev{i$xh z@RR^40$Y`j>^lWA(VRGteXDV2#$u9Q7~~|oSfiUPY9x{>E7Az<^Gl{kdpC5_l8Csz zlWn=z^-6(vM0otDUIoQ{;*eT8s2*9$s~K~z`->`QN!}QJL;;(?F zixBe&Yg}~-s$MlW$Ep&eu@>IeTJ!xsv@2P(yfz zm@8;EO(bjXak!QUtA%Z!=Qom74f`HVdDLcoCO={<8{JNMy@X25+x)`~%#x3_6KO@q zQ6fGswE&eheUWRW?@qtia1Ix)g5MDsVGs~eU|F9_uuP{p0Fx5nvLKV=n`DWr{nbKF|8Vo5mzg zQKq8R!Qxcc@=)oIdHmu#CD6ms9LC^5*RMw%?vDr>;S75F1O6k4esjvTW2;sn|MyV? zUtB;-o`~pf6q!AQuFhgpvWRIu5&=4V211(o^LB`?cUA2ik({tLJFU@`g0`arX~){r zX=nUOgm)Acikv7oq0;Rv@cm1E_g)F^(3vvAH^wuKS~?iTiWcGN${dHVre#L4aXg5s zRVnXE#E5hYs+bbha_PLg8;Q<@w6OwNHy+{P*D_5lSS(N5eTc5IPajKZBvQ&N>DAsv zOCTb*WjHo-SO3Fm&EzEn>bnnhNHEky0wcmS4{6T|G!&rI!U^A$dV1!kYobU8za?>o z+kO(9RhhsvB>7sNmrIY_1jE?-6&Ts=t$UD(QEgDE)D;?gG)oucr~ZB4=QaWQ~!H#v%A+%}dpg zXGq^D#N{e6NOR0#zjH_k%tf)klP7fDF35IdUl5Xs(h|bXXLAjv4PI7wRuIFohq5#0 zdIsc&>6)|eesc7SM7z`&@XD3YFO{t{hGlTp6>if+*8a{oPE9$i?rn9%a0KAkF_+z>!ib6d4x?)a&80y#hK976! z5@s=ww&nE6sW(7ibwTlydq9t->*aTB%Wt=92BJqxnUv)>lt~UpqmspnE~ueTzxKHO zv!nUSbyNT0<38VO4lR9ORA$_Cb zt%4$z=2g1KEU72e^KBDM=`D8nH=OM^F}wXoOMtb?S*kN>3x;4OUy)6xnPdffbUIzY zev?g?*ei)2v^s9a9cJE_l^_-zZFOze&0REmx6FDc?8?1ij?`i>gKc)b5DW5iO-yq_ zHQCmC^a*ux9BKHHcreWO#4TIornHH`5QCn>&T}w-`1OnWLb|gG5*?0Hv#y<$6eN+f zj2rwNbM||2X1gV4O+6C9eVo|pg9Av)d}Hk}_JM3aPkbohOsTeh)^f5F-Ja?!Qpx9z zjZdC;z|7M{^s`AL)R#4N7`n$46}%p|5FZP#f+P4P_y#Y?SBfVX=XGyh!hlRVCGJZ{ zpw4~`8KggLG(6JzaL8i5aH4P*=I5Pi=;4gomm}y}K20A! z$-SHIwlGH9_U2qXu(`yW7I@!=Nsg6~RfCt_mnI%@ZIYYNlz%dQ#*aEE?m9T8x}~ew z!UVu}TU=qoubN$d=)!3n@_ZS2NS`&E$k8x&D#x-%P1}VZp}fEMQ0t7TBgQ35R?M!J zYI=g7_9XgTTtn@?iB;Uq`hD=S3h`w1$JZ_8s~(uhO0GN>;p$J7qWearv|srd_1#n~ zcYl6f|C@#cV`~Qlrge1ldv4Qop|Qs^!=QEzIJlQ^;YD zMgA;-VZs4~7>n+W=er31US;@<`W4BTFEU2D|Y6JVl3Fw9r z^B46OLUz1lCImBY14wr3_TLX{D&{g6$EOhorQ3<>W|1+-ZRUdwou7u(GnQ$HINmw$G=jh)FM$ ztN0X31gEL4n65_DBzS;N6J>1L6ruoyHHsvU`W~!D_Vk3u*~>4^G4qg%ae4ag;3Q<+ zAtpBpTFzHYQ9F^>8d#?@ZVbE0yayOBROL^1nYX+lq$eTI%1j;qB8*hvRJgUI=pvj{ za@1rbG<1jU0CJckC_9458)(j$e<;$XjVadQ-;P5RTU4O>>}0F4rl!aMN+Y^7CgqW@ zMSTJ$|Echf?DFRx1$0p>))~EQb}e=8j2^ROiU=jLndic06@WL|djS^I@I2lOl2G0r zBG)!WLqRUsM7uj*4>=;}c^LUrKZ>%Jl~`V<#GMfR>*4Gq%?H8!R+n>vSMvF9sv3Ox zjQ_?U4xQgaf_Da33C_wd<8wxXv2S%{h{5?011h^q9S3F{&H8hv{1DWa=8}z>CwL#t zBJ70(smGwGU?rTTOsTp|34XCitp^>JJ$Xk~iuegTVOY)i*=-G#sWhh4gRewWck;em zI88h;XaiG~n^KDMojzYm+h~wau!bi~JTQ|ks8H2ESfKCwtdiby$@`q!JI7#lex+Bw zvRpr(e@+c3Lkq-t%J9Yh@MvtOA zXOeG{dgER68R`oSdqskb$IcVwd(sSw>ZTjnUL`{BO$g`-Ak{BTy)P=($1X!+pXLcaG)bRF?K;$LH;lq%3d?f(PgVq| z5eBcyrV`f*=zlMz!)C;JX07IS&ri}@1qc=@=#vjQ-38emuZGlj*d_z~4Cd>bBJM|~ zRzEX(QqH!@L}3~k^SB$8^A$bAkz9UoPR^GE@`;G`snlfo0ukBx!^ea$uO=_-1+WiQ z3D##$-v3tiAVM|YuNagi4^Rf&m@_bDy(1BGLqp$zJ6DiR#}C}l5(9q%e#<9h<^BdB)+W$r1h@Y9p#>7DGfeie#3dV zQfFWEJsQ#5PWIq@vAS79M#{Pi%=oPnK{xn2>b%ZuqP?GM@6-6=nScm&+T zy=IZow5Z~Uu`E=!LO^w}LWgPC3*>~!0AZgx2h}Xm*Uf>q!^2*ZS2x*XSi~VZjBbn#*7vQ zbk29yJ!o<6zzq+1xy#?PzQY^r$cZPkaT50Sul$S(-;Ax~6SBC*aPr6fU#z`%Sd-b- zK0GrPL<`%p2tPAE zI8-{Q#M#g6-11pK0!Uk$x$TvfwG4+_>0XyPKLlMj`EMkf@{I;Q4rz~$WAyY2kd~E# zy!-`llPxXJ@HafviND|Wy%W$!;Wmc`=9&?d6iCF7`f zI{nzcj^fm7aIQE;_>w#TSn71~z2|=MW6syBte?}#@_0+|BKWYFp=JAs`)3y{DCTlCA*M`piY#5cr%Ib zCem?h$YZ(a%PaN=1;yVLzlh+3!jiKv;O2Zh1MpNibYj~f@^i0d@6Di>*{QJ%Wgs@iua;5DE#DjVNvhH zzy#}PxTL1+V;BCaYP{${v4(ii#&G78Ot$F7^u?<1oSRQORU+! z-~6AH-?fLP3n4XH`$-s$?rYp}OVU>qG+7M`HcY&T#a_DcJkY99nu&WI2@?%0yrG zeXiJQh2Oa5aw(ix|dQSe1$Rjp@vM2{VBpb$7^<3kUL}(8GUy7SNF^Bcz zEkr*{4{bh;>m;GKlVevE%;TIxar&i>Rm8Xm2KuxF;Tigt$lyMMkHda!-o~N=tdbio zAdW=WayS2wY^a5UmYB|v1@G!jSNKewd7PM5-h%Vox{>1q$7SN^-N|vv$(@JgdzTBA zGgV^@%I#DCby?5XF(L;XTb*<1Q|VCn=FKnlsQ8xxUfCjJkh@0Iz2Iu;u??DpDZ?Ld=x$|-S6hqW<3>PDeriOwTLcA#uNEO za-{m}TAEh89tmA!R<2g*sCP1*Pi>;^#q@AS{DG^iR8|d_>E7E2a=omRT&Uq<@QRxO zcN=4Id|ROs?-y*QhJ@0%gHB*>+*+0%sdSb13AR2Ic$3W=^tcJ9iLCU1UG_tX5~!W% zU&nx2&EzyTwpGOU>&SIiiABVkUIB>lIt9|exvKy^nCW=vO}SBJ^H?rDuWRf+%ra|j zU1T6K1@!Fx>`LnZ83`+}ou6-|?+ihU3kIpUR1l+ma(3|pI)KU86JL|l``o)5!v}T?ks67kE`n7a>`+!&o%jM9_x5vRdeCV7V zO7gHki-pS9eIzzGG?CX4{_i5TkfcYWwN+OS!UJ<}O+PNS(aSVU)W7Js{<8ul_OYzH zqWy_y7}lq@H*!6;Kx-t4@*VAgVIe=XP-jI5BLmGXTMrGKu?(HsW7l0h*j01ec9vo( zAGSEm^>-#EiS9us8nZnMayMSe%Bqel^`p7Ax^+>UvRYxeW0}Ucx~vGOTrL?URZo>r@6e5t zi_};&v~&SJ`ZA+xF|kK1${ZJ|W%{DfQ~^UjN`?$>#&;R&eRO?nReaZ8()Z z=Ej`{xhThYQ_G!x7r_{VTQfx<0wh`hv^vp8p9m0>eWWSt8g%M3tOKF}zB4qM&L9;JvsFxd%rmAL zvgV8Q`L0p4TrCXCA2L*C9k||%H&dT~fWp5MsdAK7t+&Hlpz!NC+n`ou>tOXvv=*a_%{>B6EWG!WXy=y~q)2o^BV_%mUwY5%DHN&s5;mNf8xCjGY z)Mi4@dzZTt?i2Z<1Ta8d_r6e#{mH6%#lWcRDnXa&UjSC~Ar*YSj$SL-=rb}e`>Irl zXJ7ZvD+yXC$JShDSqzC3Eo-5vDmxIe5Iet3S+k*B)vzj~!!e!JvFS6*HMWy*Ugk03 zXl2mB?u6R53Ba^QL1%rewI1EyvPj_9WY zes6U9{e1J1SkiD{ghnI7gWFW{?nCJO)*%J_fp?eK{K6JI1_4*!Jo#Z5aUV1%03ulf zfJD!lxsr}z7hNjRMn6>O4Zn|v>Rx0_zv9340hz1BF^=Er{^juJCFFZBJ)R5HyVv7d z1hgoEq(BA*2Cm({uId!yTiN2TT-!4qbF`90-m$(elGmE4z8xJu&ugRB%$*TmuE@*V zRj8O7@)(nhaprTJ_j^}FyGd~9!-$>i)6I2davnnlR%VhNw?#0c&*!6MEJX6KxP-d7 z>XD`wpjFiK8dAqDvj@M;In_JTyom}JAXz%-mOppZKlR#aS7vr6mh!Tqpk<4t(sIo< z>1&)uF`yTLo*uiERs1|G;r=~V*DSpUfc^Hd5V}xQQK#j6{{rIFpJ%P=J~%yk9Pn_x zMmB!OYSF=adZ9v(>hk>hyZPM@V9^u&r_5o39wv)Y1xs}_DU{ZmQ2iRTC}p96b6y&P z+0Crlk8$md9Y_d7C8{tT>kU3}N=wSAoIjWKHOx>6m6QEm5aD_qx6_=2u6YO(d}#S-7bd&KuH6gW;)fDZDgMMQ2T>+6EUxUR|ZslK_0 zJwR%$TfHOWi}bnnhB}ix6AK3bMFS~(|GU~YXgm1lDP#T?ZhIb3(Ta)b5=>S9Hq}{^ zzFI1I3@eg~7diCh>YzeHb!5e{0ei#RQL(iaAvS~ZFLT=Nu2!59P+dBb@!~a=SGR2> zzvOdaLrE#DuUF=Wal}X<`8F_$$vOwj*#j4k)l-&B*?Goz22=2fp^T21`Qn^aF9n|P z=;YixlL{NmBA%-%YrWNUC>Sg3X(B9hr>lwk&c)muD89XC{fh5O3py5vWaoOHQiLrE zWn#K`W*%@cxH976_%7s%0Dl2`jW$6a#*>$zbvf1ybTK4a-L=Im#g-L?7L)JRBPY#m z3VbSignjZ)d6K(o5_~7(Qq+2Em4k~ z`?z+SX^``h_9(cCV%Y_k?VLO1Rw&}g@S`0nV$b-N=LMd{-|Egv^)MK*a}nBwU@8ek zsJYXND5(rf_TQ9OUo&1r%wdQV?BZ)*0IB}D5ZOcD@}Ym_^QX}CMAENzWSLScvsL9D zOm2U7ToJX!1Y>Hf*26rshI+^y)GNg&o){}#!ZifQi1uXhoVvxi6^+DHN+ISQW=Hv3 zsWAn}(Sp<(Q1GCR^|e!7HySa6Fz}V|bR!%voBvE`0nN?9`%OMVAQRu{1}6UVDRv0l zHsq|3Zq%TmXt`UGJLtMffOau@6=eX?((tI!3?CC&E4 zI=|IG+<0*@Cl>ua*FyGu*HxgA;2ILOF zK?NAaYZI6>!LjAbu<(=)10vD2!lmG$^p3EjU7yld1b95qbtp4*AjH#KXrm2ReC8j8=BQ~u_^NPCj5DUswJJLMWzs$X!c7^T4ZsLkaX^ph) zcKh6xva6WubsPT1zzHsX{y_L~uIO8w?4-G;il=k8I1^9l+DOTT&36$)BfnPT6fCml z-AnD~t@DeSn>bkzQyQW%DGxuuM5D*5k32y)o6B3^$VRZP^$P=1gvUAbc>|loDcu>j zy===9z0K+EQ-WG5ay_52JG5UPlRtP&aJsr5KfgkC%%(^3rxU`gulp;{$scVPJ$&&b z?v!9-_*nG||NM#Y4^F%H*p)jx15_4$h}y2kWg=(mVI6sILRTwqaOg`%&Ry=opQV={ z$AO=>L&$}16d!JSqjZ5`JPv%+)?I#0eYCmgpd@;wC0=Z%x5&zAte$qvg_VM+^45DA zi?g~Ye~%COTdWL`ZZ!PUK}W~Ck9_s)!G{F?(lz(K+1|8^fecGpGry7qrkj#4j8Q+C zo7)nb>YE<_7BBqpN!aOgO?8O2!vvDXlHo1>C(a~ZVV9ht?uK4_Y@0bu0vE_>S*bk5 zsd>uk_S_t0y8{-NQx+W>mV+j7ARc}(*LR#?oV!3&qIAJHLSm|RUsijZP96X0!*(^b zxN|Aq{{VC50UC?+Hr;RX@k55UX4hLKzO6T=2(jjw8;+IB6t(aYM8ydS*B%<{Mc1rW zPmDPgE;`2{z3(YESY*lN70j!^M8NPdb)Wdw&kK5A6mSxU%38Buf2=6vIUDlnA*uR^ z+MdUJv=-J?Hp-TPrtfFAn#y8;$J6 zBFi|WBqgo35o|)+CDv8VF0$fkoZV{_>-C6F2g^qDdlV$K`gpXq7FVBo#Mx|fvoCl@ z$%yOQh`Wa+(xTW5GP|CO!!~5}Dx4mWZ}U&Wyp!U&!80LqZQT8z(>t?Kv?W`~{uDiN z5k;yp{kI2NWYq`&3JigVMBw|B#vArVg)#xEV zs>Wh^8;J>JfBen}UhZeY2j8uPP&bRPSV@1zQ5Y z)&sC0zC5gw3CuAg)-29Pg-S9hg+6p%s4KO5TSg2Yhn}reRiKLcIziJA;$=ec>BX)< zMnswjjz%-lpQaG4s>*MiEy1Gf7)3e?TdV?otcKzE@0MZdJ1UE6!`l>u;l_7Yysp=n zJ-Qhd{sBGTYou@&j#>w3$vgbHWkqSss>N~&S642HaDnW`&4mF!quNZU$8s1|ipiCo`_?>KFwz%bQM z46XM&*|WUaC?ZU7<6xil4&+ZbBf94A60_@=s0DVtP5fBPk%fftIMNH#-pQhl(j;!J z%2fk2;$*BLP!#7K$nB|MhGeI*JkMFd((FmN-BQM_kE>}n6%JKj~{leLH3hd6K_J+;3ylSv$3 zVq=fOzO2278(N#|RxEYLHM+HPNLC2NIOZ%~KApV{W9QIUA@YB|q}!8&_)NAItn)j8 zpLY}VjkYjL4We7G%tAX$O03FDB@SNj+&Of0(aG=xZSPUYGuS)aeUu*< z+2p+W`8Ye9M?igcrJO=3>gCimdO;gTR1=pWUH>D9?lMP@!we8DeO^4N_ zSYP*rQeDjd=(MwZA0|@ik7n~pNt%cHsx8i|nW*G`iLtqQ!cRFo_ znByd-Fpx0Ucs6Z47DwnCGxiS1Ip-l_@_q~keBRg zkuq84l(BOkCW%Xsk$Py6qv}6S;>w=suT?B!u5}|9Pj?UNI?X(C&F}Q>8vtC6Xx_Dn zAMdO7Mh|!ozP=?ko^FvUEnQzm7VyPZOM>@2TcxM06q=_QTk*Qy=)+TCw7JR6=AQ|u z8~0aH$~l4v==4gS4LQ=ll#0NRjM=npX*)UA*BwVy5bY4uWrwQ;}2}rk`-tN#T`ZoM%Kw7wO>y?M` z<}7~c5zRnU?XF)*Nq#ls_h@}2+IdIJY3UX{`zbfot07@x`JwdN;ry*OXaiL9Uc6?N z%+QT!dYiNezufE@<$B9X)rNo})xx_hJCj7QaHNy2-26i2UjMkpkG;g)6a^i{=cGw; zu+#3Hi|q40i6Hpiq|)Kgoa%QZl}_yGGH@hiX&ex8oa$e1w{E`a9Zj$EcnqO#B^%5f-3>P6d6d9%<{N&Nx`xy?A-y|ulSqJPUM=` z{R>sc@P9MUi|N^|qTm827hDM4digDe&Y|SkNvdfGO?_h|JdCkK@Sn1h9Y%KqJ|#ZJ zO-u>vldr=1cYaBX9ZZ{~Y7@>)S*UVqdIKEOCuD=)2^!O85YBa^h_=&Bu_NI4Yr?vh zF1^&~$8@<$0#DF$)oIMh2IctCZRKa%l1#^i=Nts>mI|lBJ!P2d`{y#{4pKdIj`4qnn{u#c!+>LN)yR8+uOI(*d>lP$jytwQ@Hv+)9~f5l<+!suD@B|>s*!%W8) zHYX#G#JH|$;*?GZ%lZ3TAJsaYW`;#!l50t&N8&6zUe)td$roV4z68FGbXu@G;AJ=S zt$avqXshj!?2n^{JSHE)P7+(ob4eiC%>IH7mb|Vj*Kl|+C9|IDarsufWd-%IdkJg? z$w@hpZY#5sv4=}N+|qScMruC@Sm5T3;xcwcmBn`7gsxS*-bh?)k1Q~u@iq@F5AX2N zH?Jwm764W@1z^f{A5Fx&$V_@@!}srTN~q;Z^B&6lrgb zCFqW>s)$cjdfG`Q-c4}iAbH(YE)&*tkA_Xj$dn_bzpYf)-K<0cBbhgYtv8M%8$QeQ zsR(uS3T-ZIAzg_<6h+?R?7j>*-CKm`6PmVWD?fN0{ADqaeLNM3D2o;ru)QdZ|U*V*8&+x5WR#nj@ z8yo&{H0!m)R!%OfUPam*HaAAN2ieR|Pm8g@LRf^1LY~sYG0)O5{2cxAQ(VL^nXke4 zm?0&n43kWcWy``*{y0kj{ARshhb|f@nA3HA^;dhhT3ys!KSa}otoy2Bmq2oa=W0!? z8{TSov7Pq)sxkNqCc)!Cw9xlpN{wcFZC!XYaKRpEH{K{vHgK zI(8tpJ@%{X%yviVt%`m*jx&APgLV`$Uev^kb_|4bEk6#9GrU>{&a<2xdRjfiu#?+9 z(wg3(t`xIno4-Jz#-&eB4?DmlDxzD@lG3cA+gN?Xa<3zK2UH$VZ1WJ3Pl)4W!`k7m zKaWjq8XuE?L$-|}rsLdAx6ZT*r^BOOkusw`9W;+1^`?zE?P|M@CX_u3JyxD5KIP44 zU+EPFTTHUplwhCt=Cp6!@knW{QeZI`ZIAqTd9`}U%kuaJNF!brNru?BdX1?gmlK|p zfj3{Vmk17Lv!ygg&dP68Ug*=_H>f00D9I_AdpDz7Lr%5f{d#$Zwgfu6MIB0_HJQ2% zZ^?8zACuqCAC)uN^I)IyFd{kw%*BYa z9AoeF%`0wBKa=0Pm18lDcZ$B9OpoOoFR z9}y=yCi+pgz9IQ#vrM=5aKoOIB&xK`y&HQMKmBP~(kX*A|rD> zi%qdWnf}u3Qk!k(sIJYXd&+a!w_WdlWp7Y^pBI#y$;!GNqg*T1y_9e|L8j8@Vq)B& z(o`spkaj(SI?8dS8&RymzlL&w@gI|S3)2V$jrz@GO>%_O5NYVG4|!4M#@_bOuG&_0 zu#T{A?co`CQ{axtP9=M^urEvS z(e2GCOIYesmhGi&XXinw#Ej1C%In#3#_(X*_hh3Y`O|~gc*&{VhoiP1R6B=$Id)F9 zW7xJO2gu80rst2~`0_1P*`OzVek!?FvCTxc`xTC_ zf(x=d|7XE%V&MP@ZgBbaKW5R(_&q~iemXP&`{(RWILBcT;x~x=XH%?L%%*4Y)TItC z2<1*8^XSwEuNkE$Rq7y-!pxB%G%?!^_!<@|8i31ef z5R}!$`p<#)hqMFQRKCtN3KgL&%0!Lf977zROpR}BCsJ?(hBj$MRO!mts=+SFCuNW9 zvtuv~{ZVeON^)0nXEcxMvF?m~oD9xjzOCh;d9P-$=`}JGBCe5c94F-3>Zh|21-{HZ zlJ&CVz8@y=nbUG9Rj1s5IEvqsm19u}%l7cvnY~-ORDGz_-DKCb?;75k7aTn_BC{c@ zc8j{t;pOMm_EHS*OauM)Z@Z+u6|GfzFtlr0b$4V>5saLs@NTv18N8j+cModH3Vtax zY%RE1H)_fIJ&Ld;Sg6GEohPbYg&cMfE3jaRJcqYkDAqtC8O>P{Ck0}M*&C3IY3|iY ztaq-c1)lc#b?0h{H(7nu_T%N0p?;@c&CNj{K{xAJ4KPfCE3@1tTIg^BEoYT>umvwj z{Mo0r&O}?p6YmS+AA=`<4?%rF20JxOhg=a1Cu16R&0li~`1^Mk*>Qb|e(PeED(}j5N(|yB9X}WKnu{eqLiq zJf_gv80^BU?7WF7kCub$S^#{A=7xUHex79E!- zhrGSFe=_e0j?X^spm!mH7i^YSO4wi)fd?8Iksetzwi3e^j1ax5~KV59y>X`(ZuMa@xQO!Y_ZL%4&I@xa}~Enw=AYdTIfzZ*>l&J#`8lt zhGqiE1%Yq(iHUF-tADw=v*=^cdAs9Qe@=Ve>C>mDBT(C^S-Hn&r^0-;O-{41mDM>9 zfB0D=49s~&4S8g_0O0_q=`YE*h0>ZP*B!rVX13?Gb1Q<4-;cl#`ouy(a38|ox0_Jh zGkri5Q9IuS!sb2E65lt$J5+}tI_58K3xu7rib_^7>o;{G&^?k6xvb{pRpF7vtKl}k z4OD|P31a8zlD>hdm;sN$c)3RD$%Ax=4CG*W>_XL^_ZFB9dCRtZsUB0zTWIYqi-Gs? z7*N_$Z?u7@63r_dMHG*P5BiLajTL&3EuG)Zf+5Ojv}~eZK7an~tDRfE9OTgqrzv8v zjUKV+QOB~0+4Os4F{=WhUACcns-v^%mIQ91qx8Su6w#}@V;Q7OxW78_@-?}h;iC`u z*Ekp%!SVSciTWSXAA3ip8(5`B4iKblVrF4>wnQQ|2tJUOmZqu;=3#++MfI!DObkYu zR&x$)?XzkY7Tg0sAxBt9D0!FGJ+?JTS_#Oeq=6BfaOulyVDh}0o?fEPZ{WodnhG(? znR~wKf7}!gDD<{5BhMMoB_GcT3|{)EepY8tyCje7FW~u!OD z$9o6g?Q`!>@*j0A8b_dAyH9P!)8sxgY1LWu?O?s~!Iz-J$$3oQ@O&kh53X53J7NlR zo_(Wp(|DlNd3NYkAoU(cF6IeKS^c4|ukRER3pHX~z|)+8xCa$*yncW2^&JGlyRH^F zua7S;@$H6f#qikjNoi2P%=F({6D6bjVd+ZI{9n~nrPC}Ifb$JUwMK{fJ=Lp{_>_!j zBRd6!HwpmHja}d8vnYESOr~>Cc7^zBZrT@>SCt-YaTfs7L0oW}{gf%YW%NFG{%s zMPOXsFmR{Fca3T;5Cke#PkK#WN8KlMjaN)ZcqKw%j-iYLv&KFC)M9N_c_hH7mESha9`nx19`^@g#%V0 z;W08q+z14~{sF|_{;COr6S}SFo$>xDY<))B-ds_C_%Jgmr*8EQ&@@u*tP4r%6HQc$ z{q~61o~ATYMATQ?6BSZcR?d{Lah>b7CtZ)n0WnZC5F1B9s>EM_z}{8orRQyMibG)} zZN!^hB@qZ1#6aPS1l}xohTDzLe8$ixlk9dj8e~;eWj0f@ilYS#aFaFX7C``ic>2^S zp#>_=V;A1JjQMgqml;ToWcH%boFdb~5}$;=fe-r?9%lpbc;tk6AMP5G5E|5Is{4C- zf(Hys^?f_&my~6YGV1(wfOD$u8NP;G(HUK$+Q*=JkRA4a&NE2P?(aKbxe{|vAv-&}Q=JX9QOnh#%nO$d2tIpN zm;V^!Sfg%)FiY1`wHnTi!lxlx#qn%rGwQ0YJkVlmn*pPEZ9^Pi_0^6p$bQ0MeveGQ zKl(oH$cO-_4D_d~rQ9O{#C3%Y)VAEV<@Q4t0;*E&$1kw%>hIM0cVVhd6|JeK?jFUZ(O6u_Sa^tbiL;4pBwqJEg zw9vh~ARGO6Bko5ZhsI~U&*~KNx>^9vvfL!F0t`{zKKn*yx!RCFV_10ah5CA z?h`KYoZyH>o|EG%N1TQ>zwC_m4CCbp13nuFFhHo`JDYrzKN^0&(|Nl%BO~M7?@J8w zZ4`*ogh~g0f`R=b5&^#Y7_*_AQ)MtG=bZ`iL^i^G42KSr#`ucFxMniS> zm^LGuYJ_RK<_5l_4y%=`uWa6$aCaf^o;TtZpeOeY`<=RP->)LpwoLd^(?lBE*zzHQ zsx`}t{Ml5z^G9laS_E3R(JuCu0z@$KTSKqz<+Xmd_IxDQl}KvUPMe|1oi4Sam>_z` zS7Ww{F#C0mM|}bR!3r$!`$Xq$mILSgwTZ~l60?0Td?f}Ha!jEz+#h*0!1wO&@oC)TPk+hr^&I5fa{+jNM%rul)$ans^qsv0v1FP2;%&!1zm#b{d*{=I=0q;>J32uq&GC zJ1mUN`<1L8nFt&z0Q4gg;28xaa{g=@B}V>YHD+dJ;ZK%uOg4urz|eP1#31gMi-%3f z5yc7cLG78za}C0r2|H9h4rvjYiSVYw|L(M_aAy8(k<%^?b^9pmKFD~#pR~UOHD6L& z@fYH7S%)&0Qaiz0E&wP0stvy&lrFA*x)_#$Z&CxVceXQomRmq-KC%U;s5f`oHe^`C zRr*c@PxMZ@DZ{^8^clFHpWkY$DwHXq!yAE|GHbj3y9&av?7 z=^C+f^xyhLjD_J1zvk?jaqTK9zmA454prbudWIFb77~1yMTm}g%-{F7cuXph+~LBL zT57`(AesL5ii7l>|8e#H)VUVXQBhe>e?oK*-{sw=j5kI!N;3z=(*$pg(1rvB1&wp9 zm@-faRW3*7W>iO5uXA|YelDW`LFF%8GE3B*E^c}<&UscWpcoaeyY<8s#H}&&uj>Vsr@SRdn+j75vBlH zH6G5Z$l1T)vsA}2W4pIz+xhEHU3wpAovj@k{fR^$AOPFm=Dl`ZWD#S*A$~>0z<|SR zhh(6|B95Nd@3Z?T>hAAuhw|RCDC!nrp8FVKuidR{J7vi=FHYQy`To04$rUxK|63`|sh9xsg>GVirrnQU1lg8Ax}LVNJoa-3<6 zItNhBRWFC2Zle-=UrurN;ZGbx6bC~P8mf@&H(!)Z>Ks!8Wl6#mNY9Y5 z*6Y{3l4d;{og*Y-4e7vR{DHJNrnPPMy(`fGvCEyU(orNXdJyO*rU@35^uo$RGt9DRRYY>)aprc$H$f9Q-{L z{fWx^=p@fGz%7jjHD6{J6#sDUU=pkbfytzK@MI#KRO3%$sguE#gwSiHX(m2-U23 zjW*f6^0;?u^9!gdbG?uf%+Zu0Qaf9DMU0ks!o$L$B>kBk%twBk=aYH}f0;yXH@E(niNV6pWazIr?Y@&@5r+H)RlNz$`7 zX2*a$uLoNi77OUds-*rg974##o{ZAEKj{J6r#(Tg`aP+3Of#wZz%$Dlx(-QqtTyrH zX2{)%Kol`ma@ZysDbmMliy}x}7&Tj;5lrqm{H!c+9vvOGAVj(KWK@hw;!$&?iYWXQ z#0e?ku}p`45Rg#71WX>g8mM>3qCIm-2>2^Noc-^9H1aAM;NhjNpKXFrGG03KylI#Z zfpxn;aL7xOGlzW%A{5r<`=a}=8SF3a7lx#akLv1f7!EZPX&^2`!(8Ulc+ej7PNaMy zpZnbi;tqPJL9;+)lhbSmcafV+(rE3Lr1mjJv0%hCIW+DUnn~dRYvUd}AnVWv9>q5WxRzf=F&K36w*vk_dW5K==$w z!T2&B0wEW(AG-hV0{-7w+=zXJi-8jJstqTXChyXpkT#`wToYJO+@?|KJq-?-BiCmhsp! zV?_3`cMyLk(K2@0^qMN$&QS*+rwb6X`mkq7Xq;?HtA6JJKyH z07V-ZaaYM$?-k%)_q&TAwjJRB;RK^5V(CKMUEUzv(eT1u!hh`W)Tmalzm5#Zqi0S& z7y}iA4%W3Fj+t?sKL*>?f zzQW;rut?LVzHY*_hD{HuBs?}Yt|`eKM>NnX96Pu-XD+K|#cM?80$1}b3K5Q;F(0uO z0)4BYDVR&^s2N8j6gKv&H{FM{k~_(Q@J(%E*jee*ry{`mtqUUo`4moRTgAH^hs*#$ z{urnza3;!%TAsiC)Kj|yKkbPC+bQ=I>h-zEvab9TCqtA!Cd=Qqm_I<@`i2^}Bh$A7 zfETvJdhILV_xAe-)_I>H#E^t8%E5W6DP!G{DP+v?1GyLWMp>%fR_&6iG-=L_)r+WL zD<~FEd>P&FSbU z7LP6nf_G5asSMia5T6I|!qis6N4r>@g+kqU5bqVbB=4H+`k}3^;*RVF@z?dd9cV9n zjA6WXFp{@iaR=x{MuF%gW?N+~Wc1}QG63&GHV0nn`b5*EbeN=M=dVpbLE2P=orQV3 zo*FJQ37d-pJs(i#nv!G_LUrm&?@^JXVuB0hD=K7kdy@2%>A9YQ;IWO}JrVg^QNc9m z9ZPDxurz9`BfUfUM8YkT>M4XR@AlLBQ8%|)4wQT3RZh6iGp=$VwUoX~3)%~t-dG9E zzsUmZj)D6GQ*V`zj|1t(AtAXlh|Ht$q)dmFA5DvCmqBEm$O3QJE$Afav*-30M|yga zhhnFZlu#tk%uawrn-37(%MklmK02!S(vR!jeZo?QjpY+iH5^M}*!)ez6Yls*q52U+ za6GRTC2YSx?t4;S)7Id;BdW!U_RJ=2xNn23TiM-!==VPrmfXi;m{aR-GxGm(Y2AR8 zKj-OhB4^$&+|J5xeCARCgsMB$6(AG@$1@XDW>TR_0%MZ)ld+d#A;EfpUmS1GZy^IB z<8O=RCKlxnXu1Va6H-PvnQSzrcUB3O`ZiM3kt*SoMF<%0t^nqpQ?}wNKfhW+SC?kd zPkc$DYdl0pc9`ApZ*ff%Q?h#!J1s-ow9-I;ayC<^V+=~(?29%2p}>F!`v4`-0_al| zNcIvyz?N$Qhr-tw^67tpzx;;&_2x}KomZ#TmWH2i#sL8yY4SoGH8?I&G^&mxk2tEb z@Af>Z;q4vTo)pW>$hfo92dal?gi&APbBo81U#u+*D1}L{v3e~>pi-w$KKgo($EME5 zYxfKmvQ*CB>h=-=#yp{J6>MU=r>8uuO4R!F2npom5I5TiRfG) z3^K;$G;DfsM3}Sm|4?D#?nA_+Gp^*xY=8S`DQDTA+7O|Tu8LAwu-C=Ll<%9QYEcVh z{kpj~(|P*fAXC>;(>FTJx1YQCBLTjD5m?O!_wwhiu=`nGQxkxlrc_M1&avpb`>fj! z8sKg#*(pD8VglyAoT{U6SoP*;plVKIiY=_$0Wibx#em{jI5-q6=Elap-Wwk|TAz4` zLDj|gF7#;C+9Adr^?4vyKT{jX0Lq+ipnijXsE_Rrh3F_=EU51W8#P4(I)uw$*5iMu zK&#9juJnhntWEtNNL&Ys$%AN`5Rd3ODj#VXXcQ*wUGvtsJXZ~g-P zI@@g;y8agF@R-uFG7TA{4^(PiM&3pU?KDu(z8Wu%{hd@Tbhu2c$PYZk+;Oh|QORN@ zMSZ@GkOb%Q;Z=ciM=lyyd>YSy|5RGG(ld%CtP2|$E|F-t_A3Do_MAwG65m#C*LXP+uH1tg+h8*2JXP zdXaRSHulhUSUty{TKm7bh)63u-J!ag-_9JK1d`ZiMRtA(4RCV)6s2UjXJkZy3<|Q} z@a;#HB_-RVhA^29ZFrb3ES?0gIwvOq!m!|?+07iN(%Q#EqzLR2r%+|$$`wrn(U$kv z_azHNakG&A!)w2MB&v1L5L}KR<{D^;A_?`zcR|4sx}@{R@)xjXwH%;-QXXgTGe5of zejn{u9-v)d_6D_Mg)tAiRL?Umn^hTXY@r^mb7Mo?YrSkpgC!JvF@Frmey2@1D6Ar) zqA(e*3>5DpJqFYUNL4o6-H$UFGml{AUh?>CL@N&aTg#6jmSr6{) zu2Yqg; zL0qccr+<^a&<9NfQL8!|?G6CU_d#2bO6xd72(o8RwtUn;P3;|;hEmHVlgz9tz|Ecf zP9Y37HaaaE6S2?RJwx*2Q;chtLb#Z#HtGc0570}!oh*^JA$fuF(i+F^B%fYsM@fqn z9X}gqPS~@e*Mn|j;9gp;UNMt(UjQiPeiTgJ6%`iFpkn%lK0O#y+oG|kEFI_$7W<=n z7ue+@I8g5Udqkgzk4^1zCWz)Ia&g zsg-`Rma&Pcpx8OyCCT#Xhv9RC&h6(+rhO`I9;a-Nj2@1%ken;U6e1~Kz|Ev*)GW!M zrKn7VO2S6en9hMObatml9U$;cooqw4-;*D4ENR<0F+0(r2x}gH_ko@mlrC5?ovHns zO(4V_FcHM^-ev#B&JXVKd)4oE+OWZY2Fb(AI|DJBkg)s;bE;@#Sa2}F9cdFOnD;UV zx|+I+ZBb0o&4>n4N6bp6tsD}dFzZ;*ymJ+c13gM9P{Wppv>;W1MV=KuzbnXe*03zwTXPw{LP)hlD?x$-)=?g&z)w8b$jxQ zA`9tr?DfVyS6y#}QEVzM66%NBM=GZA)J3?`QXSfZCRsxLg<09zYPOP4TOTM)#^fJV zyJ@&}QVRi^KfWCY4R-zs%VY~O&&Gq!c&urz7f(FB11d+`>k6hU__zg_HEMZ8No8LrcF%G zxo3Aj3LlA~`+0?N41D%QSd4?K2*ePcSd$6 zz0s#d==>hQx7o9+P``x-xl^k%UhCd*hS~wm@GW{$r6(wBq~8pnlNieneCHo8lv}YD z=h3tY+AkYbZNX+f`q8vcbLI`;LDMal>f^k#x)^Eb?`#|F5%(guwY;ZFwE!$;>1$$A zLlCEUuV~_CVG5FRcx~dlM-F;Kzf0W2ihp5Z{#%=vLW{b-zKYM(;x_vh{oaUdzwr$| z_{0h**nNR&Yh+;>oA%iV8-D(H@pg(pBn?Z?JnD$D37w*>>JHw_5F$klQT9Kkk;Po zu0GMbV?NtqvqR}Bi4O=bMlx@}oxe@S+h*NfssPTaq)$P9&c}8JMTfeFtk8!|Ym5dzj1It||s_hrm|15167EaAY@Y!zgfh$Gv-B-;X z9*RjP>ke&2>Ac#SFCMBC(R-3b02!tIiv56+o8vSn;=jY_7H7!?$!_?`z6F-q%Zm$D%KidYv0o3o9ZM=XMV72u zk<4P=0|Mh|rP>OWBY_X4uSq0ws5xs zJ0~ag9bK@`bdG7kKo`h(_+43zU|2|evAAsY5dZeex}m-*it8T#4iFsA)cF=2ucU}g zS9HLpGXQq6E)ogs7eB$m8}FR>F7aH;8znGzQB4$p#?*0-tiNAgzmK}rG%B_d6_)BvFf z2uMOedMEF_aCUj0=h^!|&-?%42M**&?zPsvuKT*q^LKV>!ej!&E^s>D@r9(w=?81M zNagMP`^VfmmbfowQ&syz^KoR+v7w+5&WE-8REW^%*fYt$VqiMdSWS;{Wd)O z<;#y?vOHixW-LzZE3UseACWx4*q<aso-@E&c6|-lFJY_~8}9`NYy~5^ zj(mm_ujb?av1T`qYZ)L3Q{dUmNd&^*th67S_J&PCH#uxl4QhdBStKPSXw!Cqd}7(1 zA1M)$tg@4>lGWR*YbYi}NE3$IwG9A=Q9IF_~8l-itL%6CHm$@WdAW*g=AC@LFESYbn=jlN+aFQ zI{LVl?L1gTIiuxgKVi8-p*|tP%TcmepIF5MHg*`L zHUhL+91(5@8(!pa7Z~7V=P=!$PVN$jPB($3Yn1y=`UFTe=WnX~D@j6W9O$ub z<70os%+K4dALB~P=uZbL6@J7gQ9@-8($^yG?Q7Q0aGWKYm;BbY5t)nSc1G=Ru8|E#ns_)KRK+QMQ_;mRK7W7?{?+=|>GXZa-s|gQ* z+^hj?a4S{eT>cdLzH<|4<)r4JGIz9oCx>KBoo9ltLunaU3 z=_5nf&HCt7Pb43+Z|+a#SIckGY|T!H*{@PgP#Sx{YWjoD)M%2+uLF#Q z^!@Uut?O=akk&HdH*)FX-!SLzbqe-Rkv2CE_)w!&J)Acw-0c-6+~4GxnT(HO34`AU z{;?xvgupvL6lcRUG-PdY?kR!8KDc^k@RWse&VR1yM7ec$K|Vq^lABYj{{#!mSxXy= zi|Pk-bj}>H6<_*x>@oidF^c>Uu(n5lvrCiY|Gl!5!*3^_@X+xxmgoTN;h3>i@27-xfjatiN&0f!?%LB0$x;LFU zVoMy}L{%2rw72`%jP)tn&R8niY&TH8><3r&?y`1nllGDsnv6*^hTx7hW6P{ZWji?F zP5>kFfMcg&f)UWeSX0?QvwyI(|NQ{{OGa9fdD6W`rKA^;OUwS^M(~-N3|lN@bbzYE zqorW0?O4xE_c!irPa^Vn)Z;h>RH|(X3*)gp%SUo;YOJ3V3!dj?=0zX$r0)fMNk1Sl zULI^SHcGxs9E1gEF~Cvm?oPW3Lj-+oKbJ90>hWPkB`S8EYL={A>7iH1I{)n8mH#qy zqA07jkecuB!-O_s&94H@pWYc}qgg*kASA^DTLucfAILi2|GHdyIN*;R?hQS4TUGqtD7!a^AAUIEA>_+x7_Bj}V2h@oJ72uTVW~6-$$YHE- zFk%n(()al*=&57YU>kbEjTnt^+X{$uVH zw42*UBhrom?xEm8Djl!(P1xz)v^KfU?Y8sfY7(Eqty*+Le_3_#K?7+!at$7`8auH~ zpHSWMn2%(94Zx^?r}yN3fXYfhFJpsM#f7$|i`#mQs`zbEMl0@PGlzR$gqeZ%@QOmg z$fz3KXV>P9pDr(aPSfQ6*D;+rZ-u!V*j!@^_)*Cw^gh14^K;hWbB+>}j}_ve5~aZ= zxgJrwe-sX>N&h@cAKK)Ax&P4k@pp|PL_+z~_koAyo=DG`96Vk?8PUA$sQXYvAd-a$pviHfPrL9u*P{jUQ#_#*`>?I@F zQ~mZ1*|j3v{l~|Xa@A~_vMB?MlCi<-H$|u2JgN7aQS6x1HyivfgGi(eTuDCjYyDvt z@D7%dk#U<#_0RzzuYL>F)nPQB*){LTm_&)-Loz4k%=v9E&yF5^D-G8~)Q*4w|V z3r3vuv+w+wN0Lu(q&)DhP1&5b1>-E*9xV{jIh$ImUv9fNB)nsg!&UU|=)eYfBi|BR zVy?oQsIb`1X9eG0&pfQe@rXH1(~OY1Bh=>84r-aoScLH{xY@9J^?0G4Tp<_ULuWVb zI<@$0&g-~ab#A;F^9K#h*Lv<0r%{#eW%HIx4@qBFGC1E)EELySRkWE%5|H^&Gr4UN z;ybLFY$lxTXP2B`jeLO#X6?m^EY5C55*ZR8ze#0V6*n7h1g1<=Hi-#9v3L7$UWP=5 zh97T>X%_)+#;1$E58uSd|BrRy#nfToYBUG2%K>N?6a@m)R6IJv*AcJ zA5V0q949LY3} zgjmwNqntAB{YjVp(ZG`H3hIlWA}*d!mQR!*K!>Uy6Cq94CGhf1^I}~8i|+Zu;k&m2 zrh%;`AElMf_1!zRK}rB2Qg~2S$Ie!TMK+?4I~5aMZtwN@%M0Qwq!3-*NxE-?+{SLy z!yVgiI9TGTv0_&!LBD`wJiJ`B38~$TUg}x3vs+=Oac#kT-Zu5W`Dz0RE$~TPs92(> zb9>(V{1tE1&c-3s*^lT|4`g9`68Q6I^U(qAsiF6`Pe&Bwb<$%^hUU8PRr;_ctj#%){1nO54 z!qvtLvIZhK^26vv6kU4Q;}5J!ogW+MhJ)?G$kZz+&1*LCGs&U-uu5cOGu0w74t8yu zLfP$M;3J!YBC)Z_ZDY%e%Z4j^Wq^e`+mC51#aVj=5+w)Vwdn%hC%9HslKD{SnO%*g z2IV=ap=>RKBZ6VJ)2r2a?&{p(n>^Z%owmajUNL{Rn**8AwICRdZfdGtI7Q_1V*K;c zq2poBCL~el$TO^4MDxQ+9xE#$un4P8U#ZXre9Pvf%P&0xQyc%egpNQboNRe_YkW;LzsfiaR zSL$*^6f1Aa1)#!nMkFq9BwvNc&tK;M?sYMZNMiA_4w!6tr&y?+VBUVk_%)s z^yD<5$1W$kliQ@h2K*Ob6X0Tf4C-Qalkmd?vdS;)WE(^fvgC}tQzocR5qjJA!k*svcwG@Q76G1_Lin!vMv%UxobRVTtEXh(V^k_gBCkoau5> zE5p)>-7#}!rBtS0S``gf9AH6RGYR9q&(X^R*V#xny9_nFv^lX=SPIS{W`C{#>pG*a zAkc~)6VSX5iri}2f4A}c+fhOr8KL63S7ZDOhoF;>p|rNg%-iM0OXTq6-0F!EJ&q@y z7PB3t+ZNWTIrYzGX+T>iD&bTgTU$yx*Xbkd+mE z&*$9pnba}IPF#dP)I40$LgmvRemvR~O4~sVS16t~U=WIEiJxPP{9(E@q6QB)Sk~@V z^#5k3ubWYrEtn`Tdm^huf@_eXB025~Sbk5!WQmQeStJr4Dkz|JalsnN>Qt4Iu5=Zx zNadYNwL8>8uV|W-l+;?Mqkl9|^G2mV>Y6EFX!2aUxjQH;AjYFM@LP108Oxk|dzbRa zN-y8YzUkOg92_xgAh18jE~rPmZzP7u!9+Q%55k&u&T!eLUcsZ3xg7r%TVtXDUf?%( zDY;Ri76Cus83-^iFhHu+^z`(69^3}UIkvkZFXq8uIU5)_wL0a}G{bBQ&kf@Z6t*SJ zO?UBS%4mB!oN}w7GsxB6{A=}EDiY1+TV>_Y)&O)^;&Cj{oX4c73fqDnyU>YCNVy{P zLe&E3e5YGZlhNCd^Y$+Zy=T<1)A=(>BDA^L>`^J3Yr{gPJr;QNnUBSbmgG#A=elGu zVDGt@&46z*ue@BU#KmuWWoGSDAZu?^7{>q!h#=e~; z%q+btETwYwcdI%Io7U1*Q(*aEn)>M`a(pfzPp{ll~NDT zlG)L%hEnaAG1RUEjIkXO-O6p3;T&VA^13Qxyf?AWF4&3N^;NuBVP;fi!IzJXJ@1rX zW--8U_Df&-fiw_IU97wuBLcd5#QODk9{k@If{vxf7d#{>LJ?N`*mof8fX(WgOAJE>hZ_vxdC3&mi$RINg3 z#9{D+(o&uR^Yg!Wxkc+4TW3q+5|uZ(c83f>Nb(FRxxT5*xdYimh#Sf0q44g>>JMw2$`|$-?8~!qs)LD$9** zm8Oz&pQawJ5N*vAi@nMc3gmEHuEHBBrJBrxIkrMeviNK~YT`$_H6;M+>i5?A8NRkI z-n|yCVVPRqj1LXJUi7{3W`}0MXzwHYyYx%Q0g;lL8xsc`l>=n4epm~k-9y}?D{L3?BV!s&Q;+JNoqb1hNU}Rx9RyEOE|K6 zL4|X7XsvvHRlgd`>}*o$3L3c`I9B@x=uY!DLXhbId1qOTQb|_ULZ3$01BE@oW$Yqo zm4I={i3WDk*>AwFzbo<%g-XCxUbt^mSsduTSCEx4+uzaHAlg?@OT||R!oT9PpoNN# zj;5~DfI<1Ki)tHCOdUFo?sRr`o^=IgsPX4Rdn0; z!*@WL)%)oB^!)|v{Iaqugc$Fd1p=?DjI3)DupSO+{ z^@0yWA5~`t3|X}i`7@Y!fQHFY2>p$-^=8%^3$>{JE&mLxhuyx%_Im?i z@9d}oxhBV(mz!lm+#50)b6kz0M1{r4M!@qv&L!{mJzcot6|%V53ZMo@;9A-PUupdt!4GrwB}A*PSa6Gu{mmn!^6&p#nr!PRa#$Z;u$( z{Q0vOCh+Q7#e?+#1cm~+p095C>4!`4Q^Pa4MLzE@zsj&=7@3cRCX1vRtNGaM+U-u| zKISmL{G7P6Y{5lKQpL}v0Ar(nDd=6em6?P0-jvA~w9*S9Zh}}2c(Ovk4@5Ju)m=g# z1meKxjq57?o+bv^qq8m+~yKlK@oA_NSI9Wk{cwD7BJl&)-)RK<3aQoor>lKsHKjDZ3; zZqJ`?1QVhTlMnw`3#O(Jn)hr3@z1rWKm@xSdVBtZ{Sq-xaupQI%MDj(4}K|lskGsN zl6jO(w^U2l)p?SBPS`paI~-y&th>{!=AzH|1Bfh3(Bxis zZ)|L&GY{$?ZEAZqKig^6hd0{Ph40}{7FLVg?dLv~a6&YXagL%~(JFK)nGhkgA|Z#L z-;Ai0kM!Vw6W+a)HD6E2S3(RHShdIXK;B;PlGO5#GCe%%1X)4qCU1+aaJg}9#&dg$ zFgL<u*Cxm zEYPV1@+Re?X@eJ49^_@*Pm}=V*S&SP;!DCwMXAb|4V_lF!4qJ;+sM=ablo=ewGcyM zQS;Vrb5}>823rNv_N0ltYwLqzP@7-!12l*X;E27IWCXUmDb;Ri0>+9|d4nSHSmI@)4^brbU1`IP~B1Xhb1R z1i5_f67}2bSM|fX{)`J`;{FdPmEPSF=tS;c(&hQvspJ;$rq}Tp(0)UR2z&A8l(gu8 zg3Tr?`>#w-uBn2&2t9%X4})K@B0|$2nMR zrFSJ zm3G}t$R=~A0Sc0ekQ0gr&3-@v1*`+V|IYxI@jNH6hg;c^Qu06DQwhj{OIL5Yc!So})sW^tA8=l^jB;#1SZurK_>WaTI1qKKj z1&a<8d}rwKu45brj5?e~zy7FY2a|F)Z$N;1K(#sC+>=}_(@&9z8mL(@R>S(Fwt+)I z^fYl1Cu($hT}s9<2CWneWIYZGWzbG5|KfOr0$HQ$@V)bQ2=TegrXl}J25~x&290UC zFl3gfU7nJ)m|c|{>dGr8WzM~p6SV6HJSj2D>~LvO0w8UN4#4FJ=Ts;Gr>-xMbuhoS z)~{Y!cEY+k9q91)>Hr^sl!O;TD_U$h5BtY^kRtq!gfL%ZTD{4!_3%eTWpOkTU$MOr zS?LI);7}V+2(J6@hEC!*bLTh9-OC*I&pIId9tp`LZnR!qov5Ie@aBMGjcfN7bRqF_ zva-F5Eq=yLmghV4Ib0zZvOLmV00yYbm?i(sYA}j+T=(-%&)9zm$7V~l*zUVlza+e) zZD9tq`z{pw;A3!L*EUba+bY_F|A-))T*cB%&p#Ihh$sBGppA+=2Tj4>R0?!pQcVWY zzSwUN=MEE~FTb=*DEtP|QaC-GSi?SfRTaL!e2u8x z26%3_Hm@5mCv3O+(%JOf4vST8i|v5ZuD%&tx`U_m!MBdMtD4tIY6GxHOV zo|e}eRMrM^dAOZj{e!%$A;PJZr?s)W$&%to!19vO+~5#PlR!YQ>x8TV+PJ*4iHDsL zdJKbUxb9pti=}~y9hfivC(1EUY&eMJ@00tk{C=C%=F}s;)bx7!hLBgivx>uFckSb} zodN<^pZ&diAN3c@vyQnZg^%FD#Z4;>b(YW+I4Te|DiCyF%?mw4}O0HW-M`A~{l zZbr(}8W<-maZ$y7Z*1Y3k=-0JVUsa2-cm5=f1z}Y5LKYnE>6@ z9zjxt9&KF|`-${xm~%sf81fPo2L56Yoz$Q5o=wjTkf&@n7sm`sD-Z+P@vz+uy55oc z_4V~SXfqzX_Ta{}iP-Gv${O3)C|dJ$fB6_(OOJ+iL581rk&dcdSjeVo6t|k*EAj0> ze`p7VS{3$YidU>y_pg8_(T{daslv`#n`|l`)ch1W&F*31kxHA7X-9!A_a}&>`9C3C z0k@e>qS8C{tFM+vpGE%=>oR$T@uThHQ_JyARrHNmms7G!jmU$e@q7{q9b+pm>^XJ`w z=I3K`I5%|c&HPN9eJ^bQnalF>^HulZKwWcR;POvqxXc0QI<-7CGZk>;^b~i4wS1~! z{&i{j(UzCr`Rlv^^~)wz`FY7)VbxJe0DkPpDVy6XbSd{-r`z7U)()l;-4VpZA4GJ6 zkU7QJr&?TZ$B=ycrkJNn@@^XC>U_FcNI-7>oGYT$ul8!!D-b2ESJ2rf1Y;z+{IqIo z$rT2KTlbrKT?}rTT$xm@h&T0CW%-eITdOZ5nc1$@-gmr7pYQEFk4+s%+Fj0wdaoAL z3TJ=%S(k}jsq4!oo<-AtcuPs#+fJapdK^mgMv!sxtHLH}H^maO@@NWOglG;hKnYoT zkEIePmgm?(9)N7=UoiuKN62W$Uk10tE0N|l!MIVdI_iJWI%}MqpCO*$U42$k(9-u%UA8b>dK$oIww%umb zzjGUu9S+#mCr9*mG{Z|7+q`alNQd30H7t3PKX`3vR?u-=vX;eP!_ijS{NzAc--WmA7U ze0%D>&#A{X50?>_I#tXA`%*cls&$!|XY0(A*fB+wOo%m4@%}w!=J^Anq_9`6!tfDh zb`0a`N4>KlF^6-bykJ@w#2l!uv#iAE_5n zO-Me2p|3{9d$riV*cx7QYCnw%Che=B?1+Y?eks0b%V&64zk8PY zrjqqN*?5SJq7A|Vq&7|RWkyoW=BWhyya#jMM4V}mjITq|t{{w&Gpm2|<`KvdqEGAp z9hjyN10OFXII%4StL*&>C~g8OCYKzruQ!g5RF~BcG4}B7GhQw4M05MU_7kaBn`e1l zS0h?~SuS6|Y0=-<87be}c=iC`RzHD5;y-SAK<_C6OYgY}Ly%)vOR%|$2f9tMfL(r> z3|<+aL;WAn;$g`^Y)|(rPw_6;;MS6Vnp$)2N*d2aU)uW*zRr_M^FIar8avzg(Pss7 zz~E^w-v3$9!&cmTmQGuJaBp)UqPNRxK7IT|U%+**^=59zkDxKmyuRb607_~f0+6t% zLDo8O47j|l>QwX=wmT@TW4)aLT>uGANssJzjHkDG1F4>bS~IG%*Y1S8y8}OyEO@%# zIzPpe>cs!cMF3k zBY(S)%%IQ99KQ0M1pJGoHoLS-7*?#Q*1uo)TTAu`^7*%6#I(gN_5Z%p#GSjZ;1dY) zKWMQy;Or~?p;F^A=R=^oJ`~wQKA49_5Gv3Km?bfnM%0ju*y4hmIFqmxwdT3cHj8i6 z)15j^5O2~Ny~NY{8CtbpmPa4Tn4CTfZ#K2Gf)X=Zrs}X*CteNMb$GeA(do^;y zZbu$2ZY6lUyhyq$>N2l{|6#gXa{ShCdD-}o_=v1u6?jkMG&Ji!)$3oWjuUln`~Iex ztsrf!Ci(7loUBZI4Z2(ZNci?#Xa1+XD}$?W+5MXNa;LO;l^Y`bV-5>!@-T*%D^OFt zg#0miO-k$52sE$hWut;@3t^yZ7Z2EO?;jj|JQ|#A=gu$}_RnrhII?#_zU1C4YKK0X zZss{k-H*fYFkN3BJF7R7u+vEM?FR2;5R#lMRRW?d=6p=Zu;AuMaCZEmx`3A7lHRH_ zLxea`5*#2WV&at2``29w6Rc*)kzmh(!~&-9{qKY4Np4<2=>fp)@?}0Z9kV*ut=L(J z$Tzqf$(O^w_lXH07pd^{ZRa2>E+ns!T(id2cMj7UM@pjQ{x-{dbKKH?GAKe>U~b~x zy=K*6Ir+MS%hlN7QO#l>@9_N}C>!ROmUJ?n;J-8U0Hpf-#$mZutiHP2Trk*0+x*;3 z3*aW%`O|~u>h8|Hx$U2`r%{EA98um}9hEEIvAdhURjg)7vJuA^7MoPZ9S+&NRzBD4 zXMGSMAtFQPfr6Uq*arrk>R>D;vDngzK3F)qE}bjHw58-xvo%mluJger;cds2G7 z*}n%r-vDzF9x!+D1E(sR07}KO$Exe6qxo~sU!jWq8sUnyXc-g>jLz8hvWzv$_wuFr zl)Ls7ApMxp3)~VLAs`K?tE+X1F%CJdJaopuG>0OthG}lTvmWDYYdincudE(7 zGovKkFK^y)#8kW}uyWz*9ldVFS$>i$LCGVIcA|5b`aOic1!A>WDv|DLvu#wW(<|sQ*1%iSO#eh96P> zww0yg6%89^RBKN*i}~K1wGq3LCclh+|xL%adr;?)0n@-x-m93dN4S@RiHh1t7nH zMvkEQ;%h1T+%{|JJ1}5Hbp#F7j8_svh{8;MuJw3UsZJqLN3l22@$@uwOCXR8};*Q)=1L&!)OWV$|(aY7{~8 zdKMOzd@?w?xBFR&#^rwrSEF9ym!7?4&&S#y*+;Z?FPkyIT+NA1Viw4`B12!$VIEEP z*E-Q6S3LXDYm}+_PE_qf!=VFi9-cRV?A)hsWE4N@4L_{Mw2#A@nwr+2_MNx4cRwl9 z-sw8;%#jMm*LIJZ@S7M8kG_S2n?+!hD#GES1X@OjOK2r<($JCgB9wjW&y9tHE{C3G z;_1^Zh4wQ%ymPPE6O1_s033XQOXLhTUug3UxxZ z^e0iWhWGwDd49Qt--y{TCk%HV!^Wi9N6hu;U3+)Z(zLN_Gy&;Qw)7p`)cN!2n8OtS z!6l?jmTr31I8|qQiA&MdsK^d8o2mpjP%blSL})d+=hNVLqu<7H7rz*t-O+!plt4qM0~gR**HcvXmaOTQVB^7Ksm zvwHKNA8NSqOCNNVax4v+_)$?8Uz9TdBuWiTK3D+{`6|Jx4#0;91(~`DZ@DXbAIzc`(2v#L%2g<~)7{A@W`)ZZ|!eYP(~i#Q~oE zy@&$r?o5o4*k6Nbip1|YcDH#KjQ_%V&x!NMNvn36LG|PE!#O?x_+X-CQ1gGg=1~cE?%es}GqKBu8mSNxLF7|Q z_;XMti{a_I)=ow<62;y%FCx0rbWzEN*WC_WceFAG^3QEg0BhfmCV@WPoNEkvOgy*d zzg%|F`iS|LUQ}R^!9X&8zi;t0D9U|_cc4qw9) zr6Wr5K^vD#lF3*+;bC7e;%ZZUKU^{EOdY>%qAdRQ!b7?Ewch8}>H{nyjue-5evDW3 zbc7=45VcnE;=_+t7i9J(yq)cV;Jm{Lorc z7Pf7kWv3_$uLp(JA8Fono_i|Wd52+cI;gl*6Ae-T&e&HTQjtH~VH;kQ`t7=}s~vp( z>NMgfpJ2SXR&ca)IxNSDkDg$l6*G8|qjz-#GiGkjIub0EE1z(u8e_XYeH*A@9qr?X z=1uA_uR|vavcOP8%*F_N;IrB(H~)l*4?-1kS2o~F9~yZ9K(l-OGofRWIZS7iW&lMd z5a|8~q6fgVsCh3Wbv$9v^==kl`c-EK(`2gb+yW`%{?KA&B#LJUa=&pb-6;8nSXQfK z^#D8~Xjm?qgSCWz^Zi;`P{*hU1Xq8s-yjT0f*xT&(1a#6hGy-xsC>h4tH??C>DQ4-;@DcF1^nQk%R9K zA{`tw{UL4}puJxupR@x|05?!g_5#X!G#nr)F!QZv3r2?2DE<$yVr3cxgD-scDH*^n zlK+MN6VY?lrEKO*8>Dx7r3h{}x1tEYz>3*c3*<1n%Chx}{)G7mIGxCns*+DcbgDep zYw8IA)e5pUEiv&dp7==^h`T0TJ9-WvXJc!$V01br9uVEU5`h$b;1BE7E@evxcB1vM zNZo-?Vit=z^t#XAcQ)Dd0892}6_|(kIh5xLXy!m-zUUYNw4J%<;{Qq_ zA5f&Q5|pFmRfmDj1~jprY*e`us1qkRTk}!=&b%iw6IutrZ z7E^%!sae5e>Ib?;e=5C%t{3)m(n!fKV{dEq#@xPK;(tqRfbFuTTNd-d=3a*rUunvI z({nu|G#%O;a-#wv=3hRH^J~$4auLRO;WDTqAN)#mz8kNAH}=>(wvm3gHeQxscC}E2 zorvoK=oUZ@M*B&E+er?NG^y+q`l~CWMNl4+)}*D0WdbLLZJuqf=hNsVK8Lid6@&1K zjr>%g1p$e0qx+SB(+Ca6HNa%ERqKrKPgvg%>%f*)*iDSPIr?HjbNQtGp&>1BMl z(B{h6eb*cdRC&@8N0={?&{1grSziGV6q#BgbXq9}`fJ{|r+)DHFqF2vIRE6letOM= zfw$SwKp~zS)q-J_qLV=NgnK7+klaap1*ojDkkkLhOTZHA1Co%N+1c5xTAgJxYPoK( zySbXZSP#2_@W~COBh91j9^EJSGN02;Di-rk;WWFOT;q#(#k;_n$a)Hx-EyGYh=t>> zN%u1_StaO@8$QSMBjzA5!2eg|325Ut6tf{ujssQSDM575@|)wDrp~WlT(x6@3q*-d z)-k~G4|8U)!TY4YOXtyA1ASClx@gW$ajJBW z>&Jx1?a!Ib#|!S0vm%O{ryPi$?hf8H68ju;-m@s+SgQh-(>WmT;ueb_cGZJpx{pP3S*Aw1wc+LswJtq)#fhN7an9AEr!+h}_hSy&zcNme$vVI@Mf z@&2N{B4>6f-Yd2^EF~9+BRJSCZXtWd_RLcd)lv_HtjdDP z_@`IqNr-&FEVL(FDW;`>XLGLD0Jtl_^8QtK1<8B+2$wxic9xvTz}oF9uMvV{!eb`a zrABO_MS}34G*Faz=mIB<^sB8}fjhZTq~#kB&S%%r#YI>2?aQDzK+v@W>JwaLWnYW( zMkyTd)vuUTuRJ0C-g2YMF8#8h-ZONJpjeX2*C8sFC$kORS#dCIQZc>$6SaWrjrv@# zIIMUE+?=s`0#+g=7^c!s*5`}W#@``_hwrkDF}?KrZP-&6|JXIzSkDTmv-&H_lOy=$ zn|cis*Z5~^oB#N_Jl#@$m>X*q4c(=@G{8SxTWJ^X>(f$Ea%{U+0#RQtvmGb`zlQLj zT%l#5+XGPluYJ7VQY0C0(dVu0Q!3s)Pe^_6Vi%e#6og&J9;&dL2z(L1Qc;hJRb_y9 zRuG)j9&2mTeT?mZn~c5+?H7MZD-4{xwJ;+w)9lxH)sgbD>#0L@Pd|PGM|%Ug?VMVq z@_@$+A{#;`dBmorMEJCLRWMv|uJ*S@vZ48iP~QQZ;BpZnf4Xgbos=hwCT27_DXn%$ zQL~Omy?3LpFjbITL)*yG`GOZK8D&6u$+?wGRkH6`&bu}i@XWj5PM*6f6Z?Kal%X;) z)tzLfep6$b%D!x0X;5Ilt9=MK5a+PyI>C<_DW+E(BY5GRQ0HWr_Ci=mQ|{+ZQih zY%N~30`HhG^A+l+FFNa=g&gu6Je8g8iygA!O#CWHsF8#T9q+f!H#9cOX|={O9E=F^ zh7M)iU%~4uY-FOl5JqWxBTwdN2})q5djGZw4A)x+?KvPnN$6)I0kjevX4-`IgAOrP zNzhgQD?=WTqcj#dKh&X0E#P@m#=Z_Jj8o|4XJMy){_Yd*{^e&%Qm@8xYnD%M1DPW( zo~2-^NzaOwK9-M(|G~(y^KW3Tub^6^?GKp-!E^U#>XKvXyB2~&3rM>t#9*F7bU$!X zU~7me0HZ53YLH7yBqU+t=#dl;Ni26&IPAw-ES00dq1x(Jt})UmX7I?VGV&&*T6NK(I?(y7n0ig8YE%rCDJ#5XT=Lth55KUUQbj?Q?vp=$ z{2hwCGEj0K3jiasLvsuA@@4NGz#UG&V9wN~+CW#&>dNsWMcJu+`uKu4+1e^N%Nfg) z7MDLn=GyPtwb*2})RzuO7LNaJH#Dx{VHe5*z1jeV|MZ;cw;XRMmgJx3D(pA&ybpcM+Lv(wEw`bE za^|KR>u4mKj%bMRjlGs{rD$@qap+b#)qi(=X6H-+r8i#;GXlm&g}yJmPYGHo8SLig zDwuCboX*>AT&`7MjfN3N4vA9v8+fYIEF5W@C`n_Js>Xkp?Y;Up`iC0aGvt$&;;mjCq__ zf_jm(;r1~NhpwQn*H-L={N~NbEUs7_P7MiI%I@7np5hqSKpKYww;^(DpagULX5E^= zYEGrMe(ljfIX8)1FLm2=?^Aehy<}s%Tux?&po?>7v}>u{H8ZuQC4JqQ**nNWD&}>a zyh$seT+2G$cicy#(4LT6GMa$!NI$8{;NL8(2mW8+q zv$2KZks%CbfwHZMQ@~3ErH7`QOC(uJNWStLhzLvaLejs2v-uDZi`1 z@)V#8tt)-Jvd##)9F;j6_A0)_A)|mr6d4+bSMuwJSDPsr3=MqD#e9AzZ7DFk=Rbo8h%9Rw+6dEemYh&Fb{QUE4o2Be7{x*D^9*i<;pXOR$mFhkmlZ!Kn?dD<#MhD1;Q z1<5Vi#{lesrL_<(-y42-^M+&V?jw+R6pWytunNrmLsufh)N}GE(R;mMXH#WcknwEX zf0qte@xY!Nb6ij18?hhy%HS<~yWXljrX7^beHK+`^vLC%F`7CR2Cpvg1wbC;zx*PE z0tG=>GZ-lJ@bCBV_XGeCK_aM?=cUrOf0mJx6WmPF1A7-tTL6(Pi^_sp(BocnygR8A z+q^qrq^&Se#JV)SnIS^o0vg{#Z(=AvXKEHWLx_^3bvP-Aq22DOYbA0lMP)>bL7VxwGg&Dm-@1)PLJ*XOnR-(FbW#`=TzY zexvOJ9tbdMl9~$FuB`~T3 zHkS$xJV8ih6ubi>a0dz*5K?Gji5c=_FDRZz$LTPRA^tyKwK*YzUqzFD`@>!RWJW#z-V^WhKlptg z0H8g(y9qHG0?~*{aGdyiZswn9HF#LJ^6o;hrP=P%UMlG(V`Jg~E)!&W=-c^lnmK@( z@mcHO)>zT|b4okk1#tApK_Dj~HjG=_48o)JN2n42_e$u~_Bf$;e4^9Qe9}cfK4f4V!O+uzbUGczYd@Q z1u7v(XUYHkt)QF`oDo0c5dFleyp=H`c&Iav;i3VsB9e=>qPp@#19S-PSDt@knbT*H zxEIq-Sd2Oz>Y5r1HoMbD<3*$Mu`19Mm46;=2tt?17Bp|!#J$tE;YU?LlG=a61N~cn z&!16hZO8fm-F2YvhP-4;^fqJ4Y>&AB#5*p^vF zjBf;qCQz>D;iljq2FF0)DY-tUg=ta(UPI*Dw-Pf1x~zQ&H02G~AWw<=&8xQ-A8tEB z(W?LRZCP<2`xZTRG+OoF-j)}KBvU1+-t-Zfbz-Wm;!8h}V1a!H^w}rRUy7538vjrT z{Zj(;nuQXn4ufU>go;8~Aq37qHo+{V4x}SxbC+={MwZi^_rX zwDL$&dj-p^#fn0_D{$WN%qg%!3X7irG0@S(to(83Q@=NOgSw4$l!-^{XY^^1h&+n} z_hc88Z@Z8L{`s?iiu+N{24LL(WE7pS1VZp?Jl+56!a~uqE2{D9bQz@W{sc>WNLeZ$ z(>ly9e9TWrpbC8#RfgP25JXiLRjc*U-$@M5?w^)C4_RY4RDqd6c997jDk}L?ggBJ> zxw*4dcFR?=9et;{oBbYkMvFy|ow2JD^)Ko>!DmA&&k;KzHoCgK2pj`h|L!ur@aI0AQX$8b&%?FX3Y%y__>pFwPfSsoJE!5?;- zoLdDU(v*GsL>B^>;@i^vo^Ax@tfy<3v~+W*fuDGZE^h@&&3yZQ2z!w|U+6O^j{w~L zX6N6MU$ZEw{g8~e3+BBj3EUS@uxUppc-;R1(gY5UuJxzmp~*J zsTM!*5jY_9&H7bww?&~-F;J!Fy5@>q-qPg8%9GjrTxU=5@~EiYL{0Y_$lPLSG67P? z2e|c!Tgi_8`7Z}m%A*Ib>A(v4(rNsA3`l6m5>j`4g1$ANJhE;o3aCDYetiZ?x2BcK z-8@&Ss6Kt0vvyc8(76ikKL9M~iV*?u-AjWw5&n-MH*@pLv607;oAYsy8lzHYIMiF% zs}RV-iSa{@*gG$_^4sAn8Zy~=ZBPG-kHH*U6d z|16QJ>>Ys(kYIpdnh7n60dV{fU6zG;Q?Fjn9_#Bjs!TU_U)LP~0G54!QTAFLFWS=Q zky|Pm$4Y$deX{sC3hN#)Iz}g2ApuU;ExMT5*gZ65Bp>A{9$ul*e z5x+#hf&^@_&j`TP1rMhf0aO|BN)?iVGys)%*N6EdYS6rXeRp_wcUJ2TZ-)7;RRuk) z?>q^5Iee>}-y;44-v`Qd_L@di?>$ktx`^d73_K>^YP$Tv9jLX$gf&y#;)`uEsNtyo z`~4bpt0Vn@LvoC}#WsK<9q!TDtG{HPL{)_K9l2{mR9R;Du=d>A{GJN{N+xGjAv8|c z%CAV_`OG8@6!kf?SOu`}zll?+5p}*{rsi{MqNffB&URRP=Pj*M@v)x|s@)EL>O8@( ziD>*;Kung~5jWgC=b^Wp9P7%F5d!cFi_GrFmQ9zTP8Tk7J+Jm9#L1;+OMdRK28 zS)Z2fRk%H!3UE`=^5eUwnwLQs%2atzBYrYYh+4v(jSV$lF9-CB{_ke7w_u}d#ss%u z;-tZfyNt{kkDhzGr+E{viWPv9hK;k7pA%hyH}TqazxCY#4WYG`(7X8j?@fOu#@Cu@ zK`4`7d*o17`~7+{HvquKH9)XOE~M`FbkE(#*L1=LCcyWnUqaKlq3%P$Snu^2yCj=A zMVX5290IAOYP)K;))+ZHkj?jtJbLY=fhH*1ctDYu0ror&s7eMI*a-5>h8f`Ly8b`r zvel!PJ0KT(M(DZ*RmdyFZTd5(T()8xlkElPD8GkW2~v?4HNQA78<22%sauYo{_);X z0o`rKsE(XDdcoZSt#gOi&ZUmhRq2n?m}B-|PxFjR)Ki#oAqcfG~gO{U1UmFTJ4c;8<-Re|bw( zedEBRM1+jnPlX3%3c1y9hO+^eO{1s%7nM_M*dRkeNzOtbT(shR!BkOkwc3;XJ9+$ zwPuY{K*t)OI94SZ>^O*`4Oj}3Q%%8}@dJi&4i0)L+8~nx(8@uL$%BA6JY$BsJK|Ux zP``*=BIDeO&fNwe{cpjGfBoDf1V<#C$p_?-TEmw{)!?gWrg*Xz2ztTB^xDmGC!NUY(E*FO;i12gcHDckLu$f+x~+GZ~ChT_kErBSzhP0JYUaeD<2+e zJDNnnCQB=ADH(14>D4P*VsQ0?ex>oGj$L8re}k@ceIV<^JS^~pP`#&3uzcQ!LAxM1 zY*kJU{EO4*6_`}uauc%ti4ICd?qp z)fI8-!PGIvG3dyo&e;#Xya4E=q`s+=Ej-W~Yo_pigMxf7Kl|!5F&AFE%|K3LdVx{- zWbiM$nyx!1YyO5I6ArB zrSfX3HWTOG>Pe^$Xb`PB->^uEE%!quzae!~gP66kB$DbC2jlvY-;(BY96xYnj1FtB z>%3H0*o2!qAYI;biRtSB^*WM#zQC4ihUV$(GZv;Iy7y*pR9E%Aa1*n9h0i zsl}sXeLC(TSvgnECzoE!FZB4<)IajfJZqfqiSQeXWOqvx@L@2yFsMRD1uE+d$k{f! z!Qo8aJ$Mu(8npft%Nb39QhWuMZ5;&3*9Peo=W{o%Qw&ov9Ixudtx2%5h;w9eaVT{% z`7B#`fUX7bphuL8()+v&Ggx1<^BkBHo-)b(I^g=kZ-H4r4A6HQAD{%AzCf-reId8U zaM{hp#l|k!@$tE^gn{?<Lg%y)Yu@(LlW0IuN_%T|gDKtGa}fJKOr8~J4rUw|ygYa~Vm;mRT-C=dle zCqo>iM>vD>b>!Ef1NND_BM%r|`Z@g0y;SpuKp1d5lEI;(e~zJR5V0}$fuW)B7Abae zl7!e`CixZ6KXL>yTv?2wVpdeyIflCeKcszrg|fD@mCB%|-ys>kl{cI$PBd zePSVFXi$@Q^uX4SKPV(}Q|%MN30?Hj3C>F9#jP*+^0ZbRQcsBw0b;@0ChbWei*)2N z-`5_dnJ@wkx6xAlR=;hJIJBc#>WE8L@I3cGaIh1}3Ma12+P?n^d$df&nnqjytzLH9 zxOeEka!s(qag1eX6-uHhP!S%O;{XmC4t07q+6*RLDGX(xRk`dgj_ki;VUQ1?s?ll} zT`@k)`Bj^H%M15ITuPtB2N0_Y(qLMdvnWq_xrlsu`rzK=o-RC&m+ZV2f3fSfJ>!VAL*nRn!#ec}I&2(DYf1yS+BTnXBJp^Hmqxvy z{c!8sHr*@73vH+Bj1xQCPXo_WQY4qeTc;tThJ4c>)O_sERgCBv$>7&8F9pxH$seH1 zpSYW=dHmR_{e*7Ac8zuj!bk4Ud#b@lfGlQ8HwoLP7*7t6>f3FEcSiTK9iR#!0iO?sd>j{GojHv-kQSp(XATr0`|5g^7k?6_K{|7{j%Ap}pVa z_IYh)PWTM0Jiv0^B%x9VU`e$O)f^bX_;vrRBhYQR^vc};B1Jj^`begHxpJD2-m}*4LG2OV ze^JmG)7Sc>OU|p!EYV_KoUrY>)azwBG*RN5cHVi@I+r-X`uQB!;bV;=SwD!o*uyWb z4@E-x&t9|0JvZ}^Rr)Nl-BlmfEE5(ie;aCk{}P0SIYO>@yYH<;xF0sEZmF+)-+kGl zx{R`kFr7-a=Orw>&3BJ%b9rLbTzi~5QV4S_bx10b!__Y9`<7dHs9#=P)5ugpyVxhQ z0)zpalabcoAFNwKy4DK?3ysk>joiH%Kkm1wNXgFFRlQv{%ChB(I;y&Dl$b(Ed9Cn709J~_1e!m6c;aZRL@XYX|BvI?=BM$t1=B`8xB68 zJmt0uQ=%r~c!Ba{^^jPW`E+%&-ALm02f#5||LdIM?PM-x+{@oJ4;~_h3epU`7uu;I zZJM}E=kott;(bpLgD&NkEc_eLn{ug&8r>j4bE2ao14wMj*!7$jaED1Dl81Fdvu^T; zx@r$uTkRQ+-IqEPYoVQz#L8G&JQZ56h$c{TFOtZDZfv$8FpM1o=SZTDIAmSosHmgY zdFEJEZz6#({VH|gv4LqZjlI{tcw>K|=eOiuyxrB2lAtP^qAynMc5n}^y5E-U@&p+n zxtw^A)S^QL`VZIr`S!`kO@y9@sC#kwVy&Z$B>vb6B`z_dkw`V*BFu}%bIkDuR#qMN<&@}d z5Y6|Rj|Ka2YX?Woy(kpdD%~(C7Mz-~7AwsI@(2)=(gn%@;6m@>YMPU>W)F6}{wPia zw0++`_iORas48JAZw}d}`ux%*DKoLT8xkAgUfgC>T3x9L)os9OG}^?xrQeXWQ7Q$d z_hoBh90nngD8ccGFOc|43sq(yaDU3bn9%^VqSm#rCId zVWm#ml4ncv3t!FAyF?S4-isFdR`o{fYu1_3w^WE_1^0?XAf)u82TLW6AZ7mCRT+!0 zHIvFl;4dNA%R?K=*5(@tb2qqgLigAc1Z*5*oi4el+USW(~ve(TQoX zr-FO)KZk$Upwu?srgt?C)P}$$<7TFrjB?;b0Awwu%H;L=Ui&O(=vQ2Znr5Adx>9)5CPB$vhNYOJ7}8TXi;kTLNSGaA1N_?IR*&F5Fv z3-^6IW7>B9aRrwkLKs&Tf?}^{v~XfegH>gHv$HTx>&yzjn0%MuRyV+^#~W)$iRoF& z5gp7oRjub;d)Vq=K%sMrChFh>rAr|^V#R!t!bq5VO@?kxKoWXARRija-0FUGhJIJv z{=_tLABZ;+5ktOFPL-VGlh>dX(2Mx-AMbmY0~revC?a79X{A`Fsf;lHgYp_nn}%04@;Cv=Q1)b9iDipxOw=lnbA{up2Q7d zn+ctlKk7zLrr;k|m;d^#cQs39b>5sQWK@BqUO|%su)2KvTT0vyWi1WdV_+y@9RFS{ z)co-6()>&l{$zYYcQcW;urvM5&NC|lK3t#S!#_E`p@xFeOmpI~zq&k{83h^>2{WR1 z(7q%)@uu!WZL>VN#$UNDSa#&w|18_R-L~BT%~Gj>99@vJZI1QCZw07itom5(2Uf2Ld3p@3y<;6Hm0~I;gK&%hmEK)iM~e zy=t3Z93V_en@yCU>g}S6(!tkrL;H#p%noO0pCsLp7aQB5CAGdps3<-0y^}N^-qV@>bT0@)k+<+dQKY zVyrbo!vz2+Ye-9?a|-B~Bt1AfKY#s$XTN=I*-nyABt^VmEVw3P?MCdB*c>lIO=>4_ zQA6?uLz|l0Akeb67(p!gWH~yRg(Zg?RRi{F=l%z2cZ#?`b*-nV{Wevuj%M3#j~cn7 z0lC*RclH%XJb4#*^@zZ5ED*q7?)vqz>fSpe1s*Yq3C&aPj@M;4zaqLAivW;Fu!U{l zt^In>L-P-tuA}e-OKFs|*WbN2880BCoIUV@BJEWpm+#sgX5y1SwX2_^$w+Yp6!abf zuj34{cxRir$?Kb9DpcK-8~|YwHO83uga7yW9gx zoLIyG+L${VN#Z!0%Hw`3->&?uQ`u^f;pOI?Le-Hzi8>q?2CvPrC5{@g*CHt+H@!9%Xr-`r9ARX zpzJL@*HVMvUR6Bq8(29YTNYA>TD7vjYAulI`#KC%uOse$;GfvbMQ(6Xs?Pa-zqXVW z@A&)Suw9uJv$Kw33;e-OMul}5s&^j_AM0={HBqetblEne%k@;L8m3+a-@fW1UFKL|OK{WoHe=S@F(}c;2`kDWO2% z8=+y`W%fYVaDuhnO4LvW#V=$l!b8|G->=`E|=d=2Tr3*`7r%;>oa7I zre36Zr0CCxwyYbEoguoFZUIGy{qxR}JcE?RT*(NFUY)tD*Q5I7tZR+-9jyW_ZtPtV z`&1`+DBu_i0J}yOnE%ho9w9+6|B6ORHq+f}q;BVl>Z6p;NM}3fa86zP!&KA|u`5s# zm6IE&IbB!l-c=twlJ6pRjqSaTKv!qr@G61Mgh)5B1EWU;-@%)V-b-cyk27c(V{b*l4KR(l-(bX{nY8d?g zX095TKBgi9PM7j+A=sDYA~i98cxwUZxIflNcl|dAClaO3aQX(z6z0364e=a!R?u&y zRg)7;dz;Nh7%j`4D7-1Q{zUG1*e2KO=D^|P+aO7cG(Gsy(6*Ygu4XE#s^BcHSv8a3 zdQ@bsy^*LPgw)>yciLhZxy9i2m3sWh!SJM@gPwUAukAME3j=e*`Wv#~w>c{K9~ zG|&&S)T>IgdTUJU$J_Sh_0J`XyWAa{Bp#7>7|Ju>O6D#0_7|B32QH#g1M^IO z%C66E4J=e4h%n`kJ9GPToDy7h;yzrfjV0pN@4K4T&#JNszt)xiOQ=kOmOyK!N>hK7 z*-S*b;y^x43|#2xL+a#(VO-9yQwAldwU1kBS}t@*77}5Ch{MK)*8?Ko9}wmew1hMOv>i|)#M^d6N-j5K z==xM0a?mG%?dnel8-8}m^{9h1@I;)7-pWfKdK$Q!24BqeMycqX|pEgg|qH^J3E@akabzp*Li_$#4W z!hVhq>JP=(&9}^*VJju!tHN_0Fuej|_!&km8teLaN6!&2`jM;arcLcA$Pz=|okB>x z%MF&eg=QYxGd(Zsn;+nqG^q}$u(HVWyGM*fe@_K^dqW4sNHW7wdH`kEok$?C^k+{* zbyW6A0HKRT9-uMXPmRs4@(Fxe>YHlyIwc`D}@?v7SIqIO*+bOvE} zHYCo?Zj1{Ieq6nief>Z9W@=}|$Au$TPJG`qc98tWGEJbaVw7_|Y4%j|I;4q;p`XBA zhWcvKsu8%p>qw=sajuW1-yQ*ZI67WW(R=EjQ zSGl77D~-_~jlwNMhAEBit)a6{19Kq`9#fR*ktWn08|Qcs%v$jm(W%;g@bZ$?=t)u; z{7wcBa?!TQ{43k0kdLUTj#1Fp#x|a!epuOI?H=(yd0o;&`43uS?t0 zx4zvPV5!P-ztB0aGt;2b+9gQ~hh;g!zU7h`GS9O%~BzAiFFinhHrJ1?hoa3_E_UiuiZ{{=o844W{ZN!m}DtS{^ zlI(Qae=DQ-bjIHwc;I0^^Qz4)L#H(%A*&R;J(W4`d-%D`z3(YOHB~2wxecv^K{su^ zNn2n#aQN}xPS>U(`P=_1e`uvDi|6c$%@nrM=G>Hyc9j`PNq(ooy~G8w=K6ER(qC~r zS@k;YEwh0xdi!6!IFJf(mIa1he>dN%);e3aqYG_-gd$V(`tcmUCx+uRvNjUW=yz(j z{=!b&0Xt!DilY-y*gF#L_ZKoqeb2vk!fmZB;JH<^V>DdTXAnFp0c_}V)6gl8@>|y9 z55-I@cTDmL8%X7yEm@2S1;C9Ewut}aP6UFEivHn$3_OEmU+sMLo-R41;R0{6a>Lnn zf-gr{4a$QRsh2tjaDRE4LlM*DBZgxzI?UXrF@HV-0hFaGQ4g6lBhf7nVoCyELhx;S zVFOdj*K?%cNwqo9LDzALO9o`-=iW2%-1Kz2htHw;I=H>=Vi7F{hH5csoi*IX)bQsd zN!ayS364NR<=~1L(ToM*sDjfd8F4bHB4K=OMX-AAgZPRsVs|X{ki%+XLNgO|zsuET2`jaTr3BVs56nbOvJ1Uhk5wy!4*TP(fR_JZCTq2*OWT`{ue zMhv_}lh3yaN_a9-4UJTF3N>FfQYBs9(cPGBG7Vj)>ro1_^Ua1N&{)uHYuzr@UTTov zB#7arfjGhoj*PR4Hk|{-s{K!W{^g-ZcspWe?kTAM#BfVlab^}ZgMtfVZ6#>XiMbn^ zuc(~Jw!@M#%jjqTnCMM~f~pju-rsuKydxW>Q6EaP%QFGI8uriCs zFNicdl>@J(4bb%3ANvgKElH0 zVC!ny2L>G}=%7^&FBU*je=mSwZ21)#D|d1X`2LKDycZ-0H=Y2m7_QAuezAjIa*;81 z*wvb%^mur(Wkdi0(12#uE`_RzIIVK@A-Y&%YHilkkzgmILP#;k86VNeN3Fm@lcGyY zyN^3GP3#Ii0T3{P^OG%g!AaZdae(jFc}0;dy++mNZJmf3M&LU|wPTRl@^{|@w6E&I zZKG)&n^;C%=22mu(NCRU2GgI^bE~(3)We;P%G<-}6iFAOr>feG-zB?IHL5OHP0u$E z`0e@8#@}YTxy6Ct&H7XX>oSD_BXpo17uFSjpEfeLw^0&JOL3d=ry>b9f>LtCjc-q^ z-zXgBlFm1Yl>H=ka)X?oB!RwB=CmOs26ux{;_@OO^6a6RkOcDQtrVWE>fg~%&OD@@ z2MH{RNZXP~;^Bmh?rdB`er6YRscXaiwgE9|lyIcowJHw0=%rf{UPY12Ft;Z_d0|+- zEJN@n)>Sa9lpSp#O@8AWYO$R$9Xtlbp#T7GK?|+fX!&ZIox=$EN-Ct`sS6wZd15nE z??JXeW#^fpsU7(6`-IfdGwIqlp2!kfBNiFbrb_c@@IWM0DGI?R&t<<}uTu$%@xk1C~_bHh!N zt*k-4!GuoY8*dk1q>lqc5@w02S{tf#4KT#wx4z<=Cn<88%~eD zYPM-*Rk&!dT=OpKHomxU)2gNA3~B)HtJ@(hDwEl434IzeU!2 zza2yIkl6n#2ncItgKs={F1XER=`JN#-9twsw;fDmkJk${yHV0JqLb>=HgSnMJzBKM z>TE@d-o?3ttNElGs;;s*(e6jJT8eO=9scF`KtN3-6>lE~b+(wGRyUwd$Cu0HYTg_WQfj~c8WaH2#cDS~ zs4jH{AS*Zow3j7!NkMNj5Lgu0TgIZx!6G z!C4>U&(n`4J@zlD-a&Jo$EO`1nh|ZAvmY)ee30F+E;Hs&>)Yc)MAH&z8iS@D;gI0&FL zz)G=t2(96P$%Xf{L8`Tm2^uSVTBpl96&5rS1e}!%&g45hGP(;+Ptva+LaPY2ea*82 zAO3k_-f#e3$S0N2f--IusQy-ban-6~DCjTOoq?nRCM8+s^;Q6xfTc2EyDpX!Ihe)o zXlf4FMEqI^bB7Pq9X39Et4h=8bRyE!JCem`^WoP$w?IQ^3CQz<_S}ttHDRS@Z5D68 z9g)?D%6i7l2$z`m&mIJ=NsY`556Qgq6ydeN%W%73j$qSAyQ|L&+%e0K38dcMH4p6p z#*3rSm3JSA4jiU52s52u!Too7!#7gHLk~y-BD>sX+S1I0uuZeI!EFtT7xD&-#x~nv zIpMC=UZ$)4wq^B>r!uto!$E5p3oW2%98V=6Z7+T>X#MG_#T;*qaUPo4zAq@j1w z(R3lert%?-wIT&btu?pI+4sCQm$=TxezJYORW75t_^i5BwUbP<=!2$`XU|>%qFKN- z)k@lBLU&9`FW&MEO`jK}F7_KQ0~7tk_AomImj`@KdvRJZR=0<6jVB}C9y^pBxjym^ zp`%eTrzDW0SM=m zPP!X<+R?1PO#(?WATYUykz!zhjpM0D2zRiQeSvk_y&hmT!)@hclynQFefLoJk>*(> zShO4iN#DHkj44sH-foydPZa`4d{?eRz2K67^CD0MRi#pNN97N~C8)lIuvCldx-thn zehoO;N0F4m>#Kz$x$JC{xXUPkUGcJY(#lnQdH5aYm1Gf5VZ-gLv(JTkKDr!iL)ge? z3T=^ct<(5JNaYARx8qzVZ?T6E(+%)@b3@#Hv<`2}>eb%bpeA=xU{h3?qfYEoecscH zs3&L|hI>$uqv>f&msqkD>puJF)x4t{w)CDTvt78zp~UPI6H`EMLp$ulY11+R)NYyuMr_66m!z-XdHijx|8 z_i#mwVsVooLYG?SzoC^0qt26D3>P{1YYaNHVFbKTV9|CDC_gC{zDWf1=_eJ~sXDha z6W=IG>qCvxCBdBAm+kXEIWlTj5(}tLTjt+UEH8|B$$ATZ_;R0AQ|f z(k^dg^ktQ`>xiEQ)SItBS$pJZ-G|*0_*^?6gF23TmVDJxhTxhFS1uJXNJq$%Y<9@o z>zo=a@aY$+zdE|UkwA>|6B*HR@sI-woJZ{OMPd6=*q=uZ9a3=d^2zAy*+Evjc75GF z`CLD0ftHG2G48-2>tBTRbJJei|8}0^3ZS7Nd-z-uW9Q@+&h2j0JaiJ(gUVxN%%Es* zHt9t6nx-F#nDGQmdC660lOoQ43^&W11PXw7+Y9@WooLsf9((ky1y?+1#6}#nZT%>d zg%@%jNn0>c+@x-CK?KUp$ySD_^?~)s?7ru4#2|l6k~vFMV$JGbK?F!0SZ9stJ5I8COK!iv3QG>MsJn(gT7Osn0Jn`YQ2E4qjk6Vor8sd8$(!jxSar} zw}brSwQOCRuYNI)0WhzJmb{+AaiZ4Qs=Jbds=8ZvLJyy~C#+}Js&#v2)MHah{jCAE z!GshVfk$IjtHA-S--SSoC|S$Zb6JDzF)bc)3&f)!;EzJ!}a`aRw@&UX~g&X z6PtuxU)d)TH_!C;CrVKh<%CsU6lt8{TzLWXE{C0S1fS%2cKHZ9Jcr(bxz}7j5L+5!roWQntd+WYWh?RK@CCh8E5S>xc};bC%r`bf z=6rTRpmJbje#nf@q2PC^kp2b+4vN352mZA-vnrGy;Ylw3w$*K?wjAx)hOdnxr#nG* zyT$FMaAd)oiXCK~5NZ(>y&$lN1GQQp+BF+efSHRre7SXmEQz9R7W7ALv=FYBOpTGA z2krSj#B`H`RN+XUaCypX*9<-$t+ylOrhO$kO9w(OC4IhI@Bx(Q$Dv-ZL)l}{aioK` z5T-1zn-R3HKIFx!Es;NQjfR{G*By;(>d4c`iS;YjmWtPo6g&H?z~qx3#Pyh+p&vzr z-9=R`2Ga0Ayrh>m-J#ohOx7!K_^t~MpQ5hBP3|q?UY9TKO)W2;FS3}pYKS*^^PQia ziaK%lCf+EW39UBi;~iX$Yu@?jxQq|#k-I(Q+Jc(>>SQ~-*MhW1pv9~k(@M3sTE4ycj z>BX(S!|hFrPtJ{+4m$yOnD0PXT1#2%_O#1pWILU^E00Ty+Fb*e<=gQ) znIi+*9;I6?!Wz0xHA;E4=Im4RfL8m&X)~v>(KppEy2^mXV$f5==v1a-sN-W}UlT?D z_ze3L{V=x_!-`!Q$(9HMdp(kY;YsPb*U!>P84JyWjy4?RUDK;KIlpy6A>_r@4{;HD z;EexqsGSfPhqGIBr$Rqt*p_hlJ-V5_<@9+E4BFiC`gGjxkXb_;QgOC$z@i&+yl~Nc zBO12d+`3TY(EU>-E?u-=qF*n6Y%w_5+E3%r&S&vrIAZ88kv?AbhQlXJsG97sL?f|w z(L|x1!y_%P*PhG&Gb+S@fAU5M_Ep-;9)lBs+UTJVGixkk0dp!qQ=@LrY5oGBZL~?L zFmlgAe@)FAgW6)4MHt#|V-kO=1j>)@lKZ=}mOT~@KY^k{F;I^LrGgw4I%{c#K`Ltd zr;2;}yh*qnY#8yVw>z+&pk{BhV3JwxcT(Z*{1aYbHiAwZFAQ4t7>2n#1M@1A;yk?A z;(upDimmtUG7-=o3ulZ4FS6}#S@gfL1xqX3`VhntzVxd8TSq?HAwUB5`(sP+K4@FE zd26w71VMyBLn~JOj*=@7&-LiuD_!K+ihhl?3SbKcLxZBxKjXvxmjElY9eQj=rZ|~ z#Ngx1K54oV;{-@G&ymD9AlG4N?i2#sx;FRx3VwgdHEg~BI%I0By3OAHVZt8uzwA~4JceBU_3}~D^FJj z7+Kh8dP^zF5RC;J04$969$~v>W(zb#CjyZUGJ_UhqnUjI2LWHN z?mfB|)yAM~u7Ael1<(X)-oJlq!(6#1WZZV}9Z>?Tvd0M`vJzPPFOdIZaPMU}eP{$m zM?L0sc|WocIk4?8vd}B2q253x1)6A)uMIa6Tt296n5r4hn2LaTgq4AaMmfqtu^8I| z*e6oQe_t$Q4&l=NFD;6H+ZnK2f7_Y=m7VD+Tuep2c`?Su0rqT(M!{;?O{8YM%sef% zhkBDb{Dt&qHeujh5RHt8zC7krTm@I$;iX2ZRoVy}vv@IBTCdupPx_-$&%fV9-tI!n z9$VrI0MZZLsa7|vXAAD8&=M~JoJSbi$z?tOO1_+h{Hy45 z45WYP$?PKeHFjfsLh1isx(-Nz*kMt}qPf&gzP>-9v%sz311Z(R5Kc4J5hVPnM;TJa zkRPJdB##r_+bYbRi3|#3h142}{(Hh#WNT?Rmh;}I3kus7|9NadC+?Td=rreR4!t7?Pk5dPs5fIWfpBOqmQ-6dr zM!+=#b`cS{ET~g*=sWU~mD=EEP=NF&by&$CtS0Xe27-=%s(7Lw^K*)20kw2)HC;Co zqbdOkqF{p;uU{j+5R6Xyl6cw!#Zv~5_k0|)3w&10jAk!3RxWvNmWIXIa{ZWUqna2N zEkIXZYPR0)xiuV);C2Iyj%O({TQ;3QRvlyD-n&R?rHDd8D^!BS!W7`DTO3r)z4;16 zy7FK6s+Yx+|3}fn-?um_BK$9I@wtVzoT7#kw|}_#Jbz(N)sE*yqMo=&0a%YY@xP{=S_YTLF{3gRfpM6jlwZrmubF~m$;oy z3^8qbzv55ullE@MyCIaPIcYd2h8*^Vmpv`6Cvz)nV!x!Fro;aqw!+t7yDm_>)c5Vb(vYCJ|; zIL5rh6y7*Mk9Rnq9OG4OP40wA5`sypf+0FFx~go+uC5d1%=#!^Kbn8_3DXck`J_B% zr;b$UyY=+7I`WP#w#2!dzJ3lePlPOW9NyyM!2VFPyGa$}PTXDAhZ)dao$vatcH3KD z-jk`FlOI0VTxBif7ZmX3&o_(1hu( z#);tqBgpdQcf7%PN0zR)WsdKMxu47I{Fe_&6FbbJQ)%hsoqZPsvt^&flQ(>9s9JGU#uiZDIn$n@(h;E`;$vH=cbKGp^=pyn=IKM}$0!=oa+CEV>1o z`K50+Yd{l^W;9Vw2%CEJzSt-h=Nx@M+HX7Ism^X`=@@b4ps2=9$5Gf?ww_5dO*!)N zw9NToX=x`*wB6ESyVauYrcWN0H^5!~3H>>ZY+wG{upmF#8|QpJ4C}Q&Z%=I3TU;i} z_6<(Yo_qltRz|mGf4!}pHr0aIVfxbr_>M|z?1js*UuqA~V54Z{z}KR4m6S4PtZ>Wg za!zHD^qIRAWhV6^9=^z5Mr!{nZtd>iqb~ZGLuY+M;>Z z$yTmhzmcM78dICI`*93iai={xim;QA4TKY;K9Qodbk6T4z!s?8mQoFiVivn-N%lB$ zbm#rgjh0zsKin~HO15P+ zSL^f-4VoSw+18;m>7rHZ6hAc;F>msszrUY;T)WHCSHI<;G&fJzZR`HpDWlZvRiN_-@%!b zTD)!!+(N6kA6D^Aisk2n71BNU`3z&G!EY$JR2LV;i^h572IGQBX!S_B9h#3_=w{rO zvXyFeKobre{8A8?X?5Th9=;Y`WDboP1-`rY-?F^taNZQ%tGi(99EXQLTa0ZfpU*I6 z8t5-O9V;7|Q@7wM`pG^k(wH(Z1Y_%@)nH#3(tYs~ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 95e7ce237e8e0b0d464f6134701a3c927b643787 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 02:20:50 +0100 Subject: [PATCH 17/37] Update Behavior_tree.md --- doc/planning/Behavior_tree.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/planning/Behavior_tree.md b/doc/planning/Behavior_tree.md index aae559df..bf78edb9 100644 --- a/doc/planning/Behavior_tree.md +++ b/doc/planning/Behavior_tree.md @@ -36,9 +36,9 @@ For visualization at runtime you might want to also install this [rqt-Plugin](ht ## Our behaviour tree The following section describes the behaviour tree we use for normal driving using all functionality provided by the agent. In the actual implementation this is part of a bigger tree, that handles things like writing topics to the blackboard, starting and finishing the decision tree. -The following tree is a simplification. +The following tree is a simplification. The draw.io xml file to update this diagram can be found inside /assets/planning/. -![Simple Tree](../assets/planning/simple_final_tree.png) +![Simple Tree](../assets/planning/behaviour_tree.PNG) ### Behavior From c4dd807dce32c4faaedbe8f62ca4a74c12149a77 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 02:22:27 +0100 Subject: [PATCH 18/37] Update README.md --- doc/planning/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/planning/README.md b/doc/planning/README.md index aff116ab..2fc268a0 100644 --- a/doc/planning/README.md +++ b/doc/planning/README.md @@ -25,7 +25,7 @@ The decision making collects most of the available information of the other comp the information. All possible traffic scenarios are covered in this component. The decision making uses a so called decision tree, which is easy to adapt and to expand. -![Simple Tree](../assets/planning/simple_final_tree.png) +![Simple Tree](../assets/planning/behaviour_tree.PNG) ### [Local Planning](./Local_Planning.md) From 0fef10c3e39296ecadd2c64c40c8712ef7a43d4c Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 02:26:05 +0100 Subject: [PATCH 19/37] Create Intersection.md --- doc/planning/behaviours/Intersection.md | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 doc/planning/behaviours/Intersection.md diff --git a/doc/planning/behaviours/Intersection.md b/doc/planning/behaviours/Intersection.md new file mode 100644 index 00000000..776928a9 --- /dev/null +++ b/doc/planning/behaviours/Intersection.md @@ -0,0 +1,6 @@ +# Intersection Behavior + +**Summary:** This file explains the Intersection behavior. + +- [Intersection Behavior](#intersection-behavior) + - [Explanation](#explanation) From e5b50194086954c9f6b715e6b0d5d148ebd59f31 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 02:26:48 +0100 Subject: [PATCH 20/37] Rename doc/planning/Unstuck_Behavior.md to doc/planning/behaviours/Unstuck_Behavior.md --- doc/planning/{ => behaviours}/Unstuck_Behavior.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename doc/planning/{ => behaviours}/Unstuck_Behavior.md (100%) diff --git a/doc/planning/Unstuck_Behavior.md b/doc/planning/behaviours/Unstuck_Behavior.md similarity index 100% rename from doc/planning/Unstuck_Behavior.md rename to doc/planning/behaviours/Unstuck_Behavior.md From 083a0b0a29aea597fdf662033a855ba2e7239c1a Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 03:59:48 +0100 Subject: [PATCH 21/37] Update Intersection.md --- doc/planning/behaviours/Intersection.md | 38 ++++++++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/doc/planning/behaviours/Intersection.md b/doc/planning/behaviours/Intersection.md index 776928a9..295b3679 100644 --- a/doc/planning/behaviours/Intersection.md +++ b/doc/planning/behaviours/Intersection.md @@ -3,4 +3,40 @@ **Summary:** This file explains the Intersection behavior. - [Intersection Behavior](#intersection-behavior) - - [Explanation](#explanation) + - [General](#general) + +## General + +The Intersection behaviour is used to control the vehicle when it encounters a intersection. It handles stop signs as well as traffic lights. +The Intersection sequence consists of the sub-behaviours "Approach", "Wait", "Enter" and "Leave". + +To enter the Intersection sequence "Intersection ahead" must firstly be successful. + +## Intersection ahead + +Successful when there is a stop line within a set distance. + +## Approach + +Handles approaching the intersection by slowing down the vehicle. Returns RUNNING while still far from the intersection, SUCCESS when the vehicle has stopped or has already entered the intersection and FAILURE when the path is faulty. + +Calculates a virtual stopline based on whether a stopline or only a stop sign has been detected and publishes a distance to it. While the vehicle is not stopped at the virtual stopline nor has entered the intersection, int_app_to_stop is published as the current behaviour. +This is used inside motion_planning to calculate a stopping velocity. + +A green light is approached with 8 m/s. + +## Wait + +Handles wating at the stop line until the vehicle is allowed to drive. + +If the light is green or when there isn't a traffic light returns SUCCESS otherwise RUNNING. + +## Enter + +Handles driving through the intersection. + +Returns SUCCESS once the next waypoint is not this intersection anymore. + +## Leave + +Signifies that the vehicle has left the intersection and simply returns FAILURE to leave the Intersection sequence. From 6efa6c2f7b8b90c54b792c3d52fba1be86d7f1de Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 04:01:18 +0100 Subject: [PATCH 22/37] Update Intersection.md --- doc/planning/behaviours/Intersection.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/doc/planning/behaviours/Intersection.md b/doc/planning/behaviours/Intersection.md index 295b3679..dd47c544 100644 --- a/doc/planning/behaviours/Intersection.md +++ b/doc/planning/behaviours/Intersection.md @@ -4,6 +4,11 @@ - [Intersection Behavior](#intersection-behavior) - [General](#general) + - [Intersection Ahead](#intersection-ahead) + - [Approach](#approach) + - [Wait](#wait) + - [Enter](#enter) + - [Leave](#leave) ## General From ecc18384a42cdcb128aa904cb3da63e1fb527017 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 15:33:13 +0100 Subject: [PATCH 23/37] Create Overtake.md --- doc/planning/behaviours/Overtake.md | 45 +++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 doc/planning/behaviours/Overtake.md diff --git a/doc/planning/behaviours/Overtake.md b/doc/planning/behaviours/Overtake.md new file mode 100644 index 00000000..a8df0dd0 --- /dev/null +++ b/doc/planning/behaviours/Overtake.md @@ -0,0 +1,45 @@ +# Overtake Behavior + +**Summary:** This file explains the Overtake behavior. + +- [Overtake Behavior](#overtake-behavior) + - [General](#general) + - [Overtake Ahead](#overtake-ahead) + - [Approach](#approach) + - [Wait](#wait) + - [Enter](#enter) + - [Leave](#leave) + +## General + +This behaviour is used to overtake an object in close proximity. This behaviour is currently not working and more like a initial prototype. + +## Overtake ahead + +Checks whether there is a object in front of the car that needs overtaking. + +Estimates whether the car would collide with the object soon. If that is the case a counter gets incremented. When that counter reaches 4 SUCCESS is returned. If the object is not blocking the trajectory, FAILURE is returned. + +## Approach + +This is running while the obstacle is still in front of the car. + +Checks whether the oncoming traffic is far away or clear, if that is the case then ot_app_free is published as the current behaviour for the motion_planner and returns SUCCESS. Otherwise ot_app_blocked is published for the car to slow down. + +If the car stops behind the obstacle SUCCESS is also returned. + +## Wait + +This handles wating for clear oncoming traffic if the car has stopped behind the obstacle. If the overtake is clear ot_wait_free gets published and SUCCESS is returned. Otherwise ot_wait_stopped gets published and the behaviour stays in RUNNING. + +If the obstacle in front is gone the behaviour is aborted with FAILURE. + +## Enter + +Handles switching the lane for overtaking. + +Waits for motion planner to finish the trajectory changes and for it to set the overtake_success flag. + +## Leave + +Runs until the overtake is fully finished and then leaves the behaviour. From 735c971baea137a2e712fe05019412112b0258e9 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 17:11:11 +0100 Subject: [PATCH 24/37] Update overtake.py Updated comments and log messages for better understanding/debugging. No logic changes. --- .../src/behavior_agent/behaviours/overtake.py | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index f90ec1ae..133e3c86 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -75,8 +75,8 @@ def update(self): - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - Gets the current distance to overtake, the current lane status and the - distance to collsion object. + Gets the current distance to overtake, the current oncoming lane status and the + distance to collsion object. Slows down while oncoming blocked until stopped or oncoming clear. :return: py_trees.common.Status.RUNNING, if too far from overtaking py_trees.common.Status.SUCCESS, if stopped behind the blocking object or entered the process. @@ -110,7 +110,7 @@ def update(self): self.curr_behavior_pub.publish(bs.ot_app_free.name) return py_trees.common.Status.SUCCESS else: - rospy.loginfo("Overtake blocked slowing down") + rospy.loginfo("Overtake Approach: oncoming blocked slowing down") self.curr_behavior_pub.publish(bs.ot_app_blocked.name) # get speed @@ -123,11 +123,11 @@ def update(self): if self.ot_distance > 20.0: # too far - rospy.loginfo("still approaching") + rospy.loginfo("Overtake Approach: still approaching obstacle, distance: {self.ot_distance}") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and self.ot_distance < TARGET_DISTANCE_TO_STOP: # stopped - rospy.loginfo("stopped") + rospy.loginfo("Overtake Approach: stopped behind obstacle") return py_trees.common.Status.SUCCESS else: # still approaching @@ -216,7 +216,7 @@ def update(self): clear_distance = 30 obstacle_msg = self.blackboard.get("/paf/hero/collision") if obstacle_msg is None: - rospy.logerr("No OBSTACLE") + rospy.logerr("No OBSTACLE in overtake wait") return py_trees.common.Status.FAILURE data = self.blackboard.get("/paf/hero/oncoming") @@ -231,14 +231,14 @@ def update(self): self.curr_behavior_pub.publish(bs.ot_wait_free.name) return py_trees.common.Status.SUCCESS else: - rospy.loginfo(f"Overtake still blocked: {distance_oncoming}") + rospy.loginfo(f"Overtake still blocked, distance to oncoming: {distance_oncoming}") self.curr_behavior_pub.publish(bs.ot_wait_stopped.name) return py_trees.common.Status.RUNNING elif obstacle_msg.data[0] == np.inf: - rospy.loginf("No OBSTACLE") + rospy.loginf("No OBSTACLE in overtake wait") return py_trees.common.Status.FAILURE else: - rospy.loginfo("No Lidar Distance") + rospy.loginfo("No Lidar Distance in overtake wait") return py_trees.common.Status.SUCCESS def terminate(self, new_status): @@ -312,7 +312,7 @@ def update(self): - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - + Waits for motion_planner to finish the new trajectory. :return: py_trees.common.Status.RUNNING, py_trees.common.Status.SUCCESS, py_trees.common.Status.FAILURE, @@ -320,17 +320,17 @@ def update(self): status = self.blackboard.get("/paf/hero/overtake_success") if status is not None: if status.data == 1: - rospy.loginfo("Overtake: Trajectory planned") + rospy.loginfo("Overtake Enter: Trajectory planned") return py_trees.common.Status.SUCCESS elif status.data == 0: self.curr_behavior_pub.publish(bs.ot_enter_slow.name) - rospy.loginfo("Overtake: Slowing down") + rospy.loginfo("Overtake Enter: Slowing down") return py_trees.common.Status.RUNNING else: - rospy.loginfo("OvertakeEnter: Abort") + rospy.loginfo("Overtake Enter: Abort") return py_trees.common.Status.FAILURE else: - rospy.loginfo("Overtake: Waiting for status update") + rospy.loginfo("Overtake Enter: Waiting for status update") return py_trees.common.Status.RUNNING def terminate(self, new_status): @@ -393,7 +393,7 @@ def initialise(self): self.curr_behavior_pub.publish(bs.ot_leave.name) data = self.blackboard.get("/paf/hero/current_pos") self.first_pos = np.array([data.pose.position.x, data.pose.position.y]) - rospy.loginfo(f"Leave Overtake: {self.first_pos}") + rospy.loginfo(f"Init Leave Overtake: {self.first_pos}") return True def update(self): @@ -412,7 +412,7 @@ def update(self): self.current_pos = np.array([data.pose.position.x, data.pose.position.y]) distance = np.linalg.norm(self.first_pos - self.current_pos) if distance > OVERTAKE_EXECUTING + NUM_WAYPOINTS: - rospy.loginfo(f"Left Overtake: {self.current_pos}") + rospy.loginfo(f"Overtake executed: {self.current_pos}") return py_trees.common.Status.FAILURE else: return py_trees.common.Status.RUNNING From 486e5bf62e4f8112c1dd6f805e89e80c6d31b04a Mon Sep 17 00:00:00 2001 From: SirMDA Date: Thu, 12 Dec 2024 17:16:24 +0100 Subject: [PATCH 25/37] fixed lidar_distance node speed by removing for loop --- code/perception/src/lidar_distance.py | 129 ++++++++++++++++---------- 1 file changed, 79 insertions(+), 50 deletions(-) mode change 100644 => 100755 code/perception/src/lidar_distance.py diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py old mode 100644 new mode 100755 index 939224c5..bea1d550 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -3,7 +3,7 @@ import rospy import ros_numpy import numpy as np -import lidar_filter_utility +from lidar_filter_utility import bounding_box, remove_field_name from sensor_msgs.msg import PointCloud2, Image as ImageMsg from sklearn.cluster import DBSCAN from cv_bridge import CvBridge @@ -189,14 +189,12 @@ def calculate_image(self, coordinates, focus): return None # Apply bounding box filter - reconstruct_bit_mask = lidar_filter_utility.bounding_box(coordinates, **params) + reconstruct_bit_mask = bounding_box(coordinates, **params) reconstruct_coordinates = coordinates[reconstruct_bit_mask] # Remove the "intensity" field and convert to a NumPy array reconstruct_coordinates_xyz = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates, "intensity" - ).tolist() + remove_field_name(reconstruct_coordinates, "intensity").tolist() ) # Reconstruct the image based on the focus @@ -256,51 +254,82 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): img = np.zeros(shape=(720, 1280), dtype=np.float32) dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32) - # Process each point in the point cloud - for c in coordinates_xyz: - if focus == "Center": # Compute image for the center view - point = np.array([c[1], c[2], c[0], 1]) - pixel = np.matmul(m, point) # Project 3D point to 2D image coordinates - x, y = int(pixel[0] / pixel[2]), int( - pixel[1] / pixel[2] - ) # Normalize coordinates - if ( - 0 <= x <= 1280 and 0 <= y <= 720 - ): # Check if coordinates are within image bounds - img[719 - y][1279 - x] = c[0] # Set depth value - dist_array[719 - y][1279 - x] = np.array( - [c[0], c[1], c[2]], dtype=np.float32 - ) - - if focus == "Back": # Compute image for the rear view - point = np.array([c[1], c[2], c[0], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if 0 <= x <= 1280 and 0 <= y < 720: - img[y][1279 - x] = -c[0] - dist_array[y][1279 - x] = np.array( - [-c[0], c[1], c[2]], dtype=np.float32 - ) - - if focus == "Left": # Compute image for the left view - point = np.array([c[0], c[2], c[1], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if 0 <= x <= 1280 and 0 <= y <= 720: - img[719 - y][1279 - x] = c[1] - dist_array[y][1279 - x] = np.array( - [c[0], c[1], c[2]], dtype=np.float32 - ) - - if focus == "Right": # Compute image for the right view - point = np.array([c[0], c[2], c[1], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if 0 <= x < 1280 and 0 <= y < 720: - img[y][1279 - x] = -c[1] - dist_array[y][1279 - x] = np.array( - [c[0], c[1], c[2]], dtype=np.float32 - ) + # Prepare points based on focus + if focus == "Center": + points = np.column_stack( + ( + coordinates_xyz[:, 1], + coordinates_xyz[:, 2], + coordinates_xyz[:, 0], + np.ones(coordinates_xyz.shape[0]), + ) + ) + elif focus == "Back": + points = np.column_stack( + ( + coordinates_xyz[:, 1], + coordinates_xyz[:, 2], + coordinates_xyz[:, 0], + np.ones(coordinates_xyz.shape[0]), + ) + ) + elif focus == "Left": + points = np.column_stack( + ( + coordinates_xyz[:, 0], + coordinates_xyz[:, 2], + coordinates_xyz[:, 1], + np.ones(coordinates_xyz.shape[0]), + ) + ) + elif focus == "Right": + points = np.column_stack( + ( + coordinates_xyz[:, 0], + coordinates_xyz[:, 2], + coordinates_xyz[:, 1], + np.ones(coordinates_xyz.shape[0]), + ) + ) + else: + rospy.logwarn(f"Unknown focus: {focus}. Skipping image calculation.") + return None + + # Project 3D points to 2D image coordinates + pixels = np.dot(m, points.T).T + x = (pixels[:, 0] / pixels[:, 2]).astype(int) + y = (pixels[:, 1] / pixels[:, 2]).astype(int) + + # Filter valid coordinates + valid_indices = (x >= 0) & (x < 1280) & (y >= 0) & (y < 720) + x = x[valid_indices] + y = y[valid_indices] + valid_coordinates = coordinates_xyz[valid_indices] + + if focus == "Center": + img[719 - y, 1279 - x] = valid_coordinates[:, 0] + dist_array[719 - y, 1279 - x] = valid_coordinates + elif focus == "Back": + img[y, 1279 - x] = -valid_coordinates[:, 0] + dist_array[y, 1279 - x] = np.column_stack( + ( + -valid_coordinates[:, 0], + valid_coordinates[:, 1], + valid_coordinates[:, 2], + ) + ) + elif focus == "Left": + img[719 - y, 1279 - x] = valid_coordinates[:, 1] + dist_array[719 - y, 1279 - x] = valid_coordinates + elif focus == "Right": + img[y, 1279 - x] = -valid_coordinates[:, 1] + dist_array[y, 1279 - x] = np.column_stack( + ( + valid_coordinates[:, 0], + -valid_coordinates[:, 1], + valid_coordinates[:, 2], + ) + ) return dist_array From eefcd29f6690efbfa765c140a77261c161386f05 Mon Sep 17 00:00:00 2001 From: SirMDA Date: Thu, 12 Dec 2024 17:33:37 +0100 Subject: [PATCH 26/37] optimized focus if clause --- code/perception/src/lidar_distance.py | 22 ++-------------------- 1 file changed, 2 insertions(+), 20 deletions(-) diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index bea1d550..5c937be0 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -255,16 +255,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32) # Prepare points based on focus - if focus == "Center": - points = np.column_stack( - ( - coordinates_xyz[:, 1], - coordinates_xyz[:, 2], - coordinates_xyz[:, 0], - np.ones(coordinates_xyz.shape[0]), - ) - ) - elif focus == "Back": + if focus in ["Center", "Back"]: points = np.column_stack( ( coordinates_xyz[:, 1], @@ -273,16 +264,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): np.ones(coordinates_xyz.shape[0]), ) ) - elif focus == "Left": - points = np.column_stack( - ( - coordinates_xyz[:, 0], - coordinates_xyz[:, 2], - coordinates_xyz[:, 1], - np.ones(coordinates_xyz.shape[0]), - ) - ) - elif focus == "Right": + elif focus in ["Left", "Right"]: points = np.column_stack( ( coordinates_xyz[:, 0], From dc053903ed2c618d2a57ff8065d240a77b783b7c Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 19:02:17 +0100 Subject: [PATCH 27/37] Create LaneChange.md --- doc/planning/behaviours/LaneChange.md | 39 +++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 doc/planning/behaviours/LaneChange.md diff --git a/doc/planning/behaviours/LaneChange.md b/doc/planning/behaviours/LaneChange.md new file mode 100644 index 00000000..f8440e9d --- /dev/null +++ b/doc/planning/behaviours/LaneChange.md @@ -0,0 +1,39 @@ +# Lane Change Behavior + +**Summary:** This file explains the Lane Change behavior. + +- [Lane Change Behavior](#lanechange-behavior) + - [General](#general) + - [Lane Change Ahead](#lanechange-ahead) + - [Approach](#approach) + - [Wait](#wait) + - [Enter](#enter) + - [Leave](#leave) + +## General + +This behaviour executes a lane change. It slows the vehicle down until the lane change point is reached and then proceeds to switch lanes. + +## Lane Change ahead + +Checks whether the next waypoint is a lane change and inititates the lane change sequence accordingly. + +## Approach + +Calculates a virtual stop line at the lane change point and publishes lc_app_blocked for motion planner to slow down while too far away. + +If the lane change is not blocked (currently not implemented) the car does not slow down (30 km/h). + +Once the car is within a set distance of the virtual stop line and not blocked it returns SUCCESS. SUCCESS is also returned when the car stops at the stop line. + +## Wait + +Waits at the lane change point until the lane change is not blocked (not implemented). + +## Enter + +Inititates the lane change with 20 km/h and continues driving on the next lane until the lane change waypoint is far enough away. + +## Leave + +Simply exits the behaviour. From 53f678c4f8c9215b3a242cb61df757abfbf9f284 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 20:20:21 +0100 Subject: [PATCH 28/37] Create LeaveParkingSpace.md --- doc/planning/behaviours/LeaveParkingSpace.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 doc/planning/behaviours/LeaveParkingSpace.md diff --git a/doc/planning/behaviours/LeaveParkingSpace.md b/doc/planning/behaviours/LeaveParkingSpace.md new file mode 100644 index 00000000..716ade84 --- /dev/null +++ b/doc/planning/behaviours/LeaveParkingSpace.md @@ -0,0 +1,13 @@ +# Leave Parking Space Behavior + +**Summary:** This file explains the Leave Parking Space behavior. + +- [Leave Parking Space Behavior](#leaveparkingspace-behavior) + - [General](#general) + +## General + +The leave parking space behaviour is only executed at the beginning of the simulation to leave the parking space. + +The behaviour calculates the euclidian distance between the starting position and the current position to determine whether the car has fully left the parking space. If that is the case a "called" +flag is set to true so that this behaviour is never executed again and FAILURE is returned to end the behaviour. Otherwise it stays in RUNNING. From ac930a289ce47dc002b1fc8a4756b9962eb594f4 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 21:11:00 +0100 Subject: [PATCH 29/37] Update lane_change.py Refined comments and log messages, this does not change any logic --- .../behavior_agent/behaviours/lane_change.py | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/code/planning/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py index 49ce0d6f..87e18cb9 100755 --- a/code/planning/src/behavior_agent/behaviours/lane_change.py +++ b/code/planning/src/behavior_agent/behaviours/lane_change.py @@ -27,7 +27,7 @@ def __init__(self, name): :param name: name of the behaviour """ super(Approach, self).__init__(name) - rospy.loginfo("Approach started") + rospy.loginfo("Lane Change Approach started") def setup(self, timeout): """ @@ -72,7 +72,8 @@ def update(self): - Triggering, checking, monitoring. Anything...but do not block! - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - Gets the current lane change distance. + Calculates a virtual stop line and slows down while approaching unless + lane change is not blocked. :return: py_trees.common.Status.RUNNING, if too far from lane change py_trees.common.Status.SUCCESS, if stopped in front of lane change or entered the lane change @@ -91,15 +92,15 @@ def update(self): if self.change_distance != np.inf and self.change_detected: self.virtual_change_distance = self.change_distance - # ADD FEATURE: Check for Traffic + # TODO: ADD FEATURE Check for Traffic distance_lidar = 20 if distance_lidar is not None and distance_lidar > 15.0: - rospy.loginfo("Change is free not slowing down!") + rospy.loginfo("Lane Change is free not slowing down!") self.curr_behavior_pub.publish(bs.lc_app_free.name) self.blocked = False else: - rospy.loginfo("Change blocked slowing down") + rospy.loginfo("Lane Change blocked slowing down") self.blocked = True # get speed @@ -110,11 +111,11 @@ def update(self): if speedometer is not None: speed = speedometer.speed else: - rospy.logwarn("no speedometer connected") + rospy.logwarn("Lane Change: no speedometer connected") return py_trees.common.Status.RUNNING if self.virtual_change_distance > target_dis and self.blocked: # too far - rospy.loginfo("still approaching") + rospy.loginfo("Lane Change: still approaching") self.curr_behavior_pub.publish(bs.lc_app_blocked.name) return py_trees.common.Status.RUNNING elif ( @@ -123,7 +124,7 @@ def update(self): and self.blocked ): # stopped - rospy.loginfo("stopped") + rospy.loginfo("Lane Change: stopped at virtual stop line") return py_trees.common.Status.SUCCESS elif ( speed > convert_to_ms(5.0) @@ -191,7 +192,7 @@ def initialise(self): Any initialisation you need before putting your behaviour to work. This just prints a state status message. """ - rospy.loginfo("Wait Change") + rospy.loginfo("Lane Change Wait") return True def update(self): @@ -213,14 +214,14 @@ def update(self): if speedometer is not None: speed = speedometer.speed else: - rospy.logwarn("no speedometer connected") + rospy.logwarn("Lane change wait: no speedometer connected") return py_trees.common.Status.RUNNING if speed > convert_to_ms(10): - rospy.loginfo("Forward to enter") + rospy.loginfo("Lane change wait: Was not blocked, proceed to drive forward") return py_trees.common.Status.SUCCESS - # ADD FEATURE: Check for Traffic + # TODO: ADD FEATURE Check for Traffic distance_lidar = 20 change_clear = False @@ -231,11 +232,11 @@ def update(self): else: change_clear = True if not change_clear: - rospy.loginfo("Change blocked") + rospy.loginfo("Lane Change Wait: blocked") self.curr_behavior_pub.publish(bs.lc_wait.name) return py_trees.common.Status.RUNNING else: - rospy.loginfo("Change clear") + rospy.loginfo("Lane Change Wait: Change clear") return py_trees.common.Status.SUCCESS def terminate(self, new_status): @@ -256,9 +257,8 @@ def terminate(self, new_status): class Enter(py_trees.behaviour.Behaviour): """ - This behavior handles the driving through an intersection, it initially - sets a speed and finishes if the ego vehicle is close to the end of the - intersection. + This behavior inititates the lane change and waits until the + lane change is finished. """ def __init__(self, name): @@ -297,7 +297,7 @@ def initialise(self): This prints a state status message and changes the driving speed for the lane change. """ - rospy.loginfo("Enter next Lane") + rospy.loginfo("Lane Change: Enter next Lane") self.curr_behavior_pub.publish(bs.lc_enter_init.name) def update(self): @@ -321,7 +321,7 @@ def update(self): if next_waypoint_msg is None: return py_trees.common.Status.FAILURE if next_waypoint_msg.distance < 5: - rospy.loginfo("Drive on the next lane!") + rospy.loginfo("Lane Change Enter: Drive on the next lane!") return py_trees.common.Status.RUNNING else: return py_trees.common.Status.SUCCESS @@ -345,7 +345,7 @@ def terminate(self, new_status): class Leave(py_trees.behaviour.Behaviour): """ This behaviour defines the leaf of this subtree, if this behavior is - reached, the vehicle left the intersection. + reached, the vehicle has finished the lane change. """ def __init__(self, name): @@ -384,7 +384,7 @@ def initialise(self): This prints a state status message and changes the driving speed to the street speed limit. """ - rospy.loginfo("Leave Change") + rospy.loginfo("Lane Change Finished") self.curr_behavior_pub.publish(bs.lc_exit.name) return True From 727f1d5dad110ae194d40ca7f3549943be9e4840 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 21:53:40 +0100 Subject: [PATCH 30/37] 529 - Implement service for planning - acting communication (#549) * implemented acting-planning service * Create RequestBehaviourChangeService.md * Update RequestBehaviourChangeService.md * Update RequestBehaviourChangeService.md * Update RequestBehaviourChangeService.py small comment fix --- code/planning/CMakeLists.txt | 7 +- code/planning/launch/planning.launch | 7 +- .../RequestBehaviourChangeService.py | 82 +++++++++++++++++++ code/planning/srv/RequestBehaviourChange.srv | 3 + doc/planning/RequestBehaviourChangeService.md | 26 ++++++ 5 files changed, 121 insertions(+), 4 deletions(-) create mode 100755 code/planning/src/behavior_agent/RequestBehaviourChangeService.py create mode 100644 code/planning/srv/RequestBehaviourChange.srv create mode 100644 doc/planning/RequestBehaviourChangeService.md diff --git a/code/planning/CMakeLists.txt b/code/planning/CMakeLists.txt index c6288e03..d8cf2bff 100755 --- a/code/planning/CMakeLists.txt +++ b/code/planning/CMakeLists.txt @@ -58,11 +58,12 @@ catkin_python_setup() ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES +add_service_files( + FILES # Service1.srv # Service2.srv -# ) + RequestBehaviourChange.srv +) ## Generate actions in the 'action' folder # add_action_files( diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 6662d9a3..d5803ddd 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -1,7 +1,7 @@ - + @@ -35,4 +35,9 @@ + + + + + diff --git a/code/planning/src/behavior_agent/RequestBehaviourChangeService.py b/code/planning/src/behavior_agent/RequestBehaviourChangeService.py new file mode 100755 index 00000000..aa058ab3 --- /dev/null +++ b/code/planning/src/behavior_agent/RequestBehaviourChangeService.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python +# import BehaviourEnum +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +from rospy import Subscriber, Publisher +import rospy +from planning.srv import RequestBehaviourChange, RequestBehaviourChangeResponse +from std_msgs.msg import String, Int8 + + +class RequestBehaviourChangeService(CompatibleNode): + def __init__(self): + super(RequestBehaviourChangeService, self).__init__( + "RequestBehaviourChangeService" + ) + self.role_name = self.get_param("role_name", "hero") + self.control_loop_rate = self.get_param("control_loop_rate", 1) + self.__curr_behavior = None + + self.service = rospy.Service( + "RequestBehaviourChange", + RequestBehaviourChange, + self.handle_request_behaviour_change, + ) + + self.behaviour_pub: Publisher = self.new_publisher( + Int8, + f"/paf/{self.role_name}/behaviour_request", + qos_profile=1, + ) + + self.curr_behavior_sub: Subscriber = self.new_subscription( + String, + f"/paf/{self.role_name}/curr_behavior", + self.__set_curr_behavior, + qos_profile=1, + ) + + self.behaviour_pub.publish(0) + rospy.spin() + + def __set_curr_behavior(self, data: String): + """ + Sets the received current behavior of the vehicle. + """ + self.__curr_behavior = data.data + + def handle_request_behaviour_change(self, req): + if ( + self.__curr_behavior == "us_unstuck" + or self.__curr_behavior == "us_stop" + or self.__curr_behavior == "us_overtake" + or self.__curr_behavior == "Cruise" + ): + self.behaviour_pub.publish(req.request) + return RequestBehaviourChangeResponse(True) + else: + return RequestBehaviourChangeResponse(False) + + def run(self): + """ + Control loop + + :return: + """ + + self.spin() + + +if __name__ == "__main__": + """ + main function starts the RequestBehaviourChangeService node + :param args: + """ + roscomp.init("RequestBehaviourChangeService") + try: + node = RequestBehaviourChangeService() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() diff --git a/code/planning/srv/RequestBehaviourChange.srv b/code/planning/srv/RequestBehaviourChange.srv new file mode 100644 index 00000000..d4da3b97 --- /dev/null +++ b/code/planning/srv/RequestBehaviourChange.srv @@ -0,0 +1,3 @@ +uint8 request +--- +bool answer \ No newline at end of file diff --git a/doc/planning/RequestBehaviourChangeService.md b/doc/planning/RequestBehaviourChangeService.md new file mode 100644 index 00000000..fe0a3751 --- /dev/null +++ b/doc/planning/RequestBehaviourChangeService.md @@ -0,0 +1,26 @@ +# Request Behaviour Change Service + +This service is hosted in the node RequestBehaviourChangeService. + +Calling it requires a behaviour ID integer (based on the corresponding enum) and returns a bool depending on whether the request is granted. + +To use it, import RequestBehaviourChange from planning.srv. + +To call it, create a callable instance with: + +```python +rospy.wait_for_service('RequestBehaviourChange') + +name = rospy.ServiceProxy('RequestBehaviourChange', RequestBehaviourChange) +``` + +Then, you can just use this instance in a try setup: + +```python +try: + response = name(input) +except rospy.ServiceException as e: + # handle exception +``` + +For communication with the behaviour tree this node publishes a granted request to a topic behaviour_request. From e1036408238054f5e103c0f42aaaca443c774d8d Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 22:15:02 +0100 Subject: [PATCH 31/37] Update Intersection.md --- doc/planning/behaviours/Intersection.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/planning/behaviours/Intersection.md b/doc/planning/behaviours/Intersection.md index dd47c544..106a4a80 100644 --- a/doc/planning/behaviours/Intersection.md +++ b/doc/planning/behaviours/Intersection.md @@ -28,7 +28,7 @@ Handles approaching the intersection by slowing down the vehicle. Returns RUNNIN Calculates a virtual stopline based on whether a stopline or only a stop sign has been detected and publishes a distance to it. While the vehicle is not stopped at the virtual stopline nor has entered the intersection, int_app_to_stop is published as the current behaviour. This is used inside motion_planning to calculate a stopping velocity. -A green light is approached with 8 m/s. +A green light is approached with 30 km/h. ## Wait @@ -38,7 +38,7 @@ If the light is green or when there isn't a traffic light returns SUCCESS otherw ## Enter -Handles driving through the intersection. +Handles driving through the intersection. Uses 50 km/h as target speed. Returns SUCCESS once the next waypoint is not this intersection anymore. From 4a8178396b24be04a493049da09a7efddfef1bd9 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 12 Dec 2024 22:57:30 +0100 Subject: [PATCH 32/37] Update intersection.py Improved comments and logging messages, no logic changes --- .../behavior_agent/behaviours/intersection.py | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/code/planning/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py index c19d3386..4ac8aad9 100755 --- a/code/planning/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/src/behavior_agent/behaviours/intersection.py @@ -97,7 +97,9 @@ def update(self): - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] Gets the current traffic light status, stop sign status - and the stop line distance + and the stop line distance. Calcualtes a virtual stop line and + publishes a distance to it. Slows down car until virtual stop line + is reached when there is a red traffic light or a stop sign. :return: py_trees.common.Status.RUNNING, if too far from intersection py_trees.common.Status.SUCCESS, if stopped in front of inter- section or entered the intersection @@ -145,7 +147,8 @@ def update(self): or (self.stop_sign_detected and not self.traffic_light_detected) ): - rospy.loginfo("slowing down!") + rospy.loginfo("Intersection Approach: slowing down! Stop sign: + {self.stop_sign_detected}, Light: {self.traffic_light_status}") self.curr_behavior_pub.publish(bs.int_app_to_stop.name) # approach slowly when traffic light is green as traffic lights are @@ -164,14 +167,14 @@ def update(self): self.traffic_light_distance > 150 ): # too far - print("still approaching") + rospy.loginfo("Intersection still approaching") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and ( (self.virtual_stopline_distance < target_distance) or (self.traffic_light_distance < 150) ): # stopped - print("stopped") + print("Intersection Approach: stopped") return py_trees.common.Status.SUCCESS elif ( speed > convert_to_ms(5.0) @@ -180,6 +183,7 @@ def update(self): ): # drive through intersection even if traffic light turns yellow + rospy.loginfo("Intersection Approach Light is green") return py_trees.common.Status.SUCCESS elif speed > convert_to_ms(5.0) and self.virtual_stopline_distance < 3.5: # running over line @@ -189,7 +193,7 @@ def update(self): self.virtual_stopline_distance < target_distance and not self.stopline_detected ): - rospy.loginfo("Leave intersection!") + rospy.loginfo("Intersection Approach: Leave intersection!") return py_trees.common.Status.SUCCESS else: return py_trees.common.Status.RUNNING @@ -276,7 +280,7 @@ def update(self): """ light_status_msg = self.blackboard.get("/paf/hero/Center/traffic_light_state") - # ADD FEATURE: Check if intersection is clear + # TODO: ADD FEATURE Check if intersection is clear lidar_data = None intersection_clear = True if lidar_data is not None: @@ -292,7 +296,7 @@ def update(self): # Wait at traffic light self.red_light_flag = True self.green_light_time = rospy.get_rostime() - rospy.loginfo(f"Light Status: {traffic_light_status}") + rospy.loginfo(f"Intersection Wait Light Status: {traffic_light_status}") self.curr_behavior_pub.publish(bs.int_wait.name) return py_trees.common.Status.RUNNING elif ( @@ -300,7 +304,7 @@ def update(self): and traffic_light_status == "green" ): # Wait approx 1s for confirmation - rospy.loginfo("Confirm green light!") + rospy.loginfo("Intersection Wait Confirm green light!") return py_trees.common.Status.RUNNING elif self.red_light_flag and traffic_light_status != "green": rospy.loginfo(f"Light Status: {traffic_light_status}" "-> prev was red") @@ -310,7 +314,7 @@ def update(self): rospy.get_rostime() - self.green_light_time > rospy.Duration(1) and traffic_light_status == "green" ): - rospy.loginfo(f"Light Status: {traffic_light_status}") + rospy.loginfo(f"Driving through Intersection Light Status: {traffic_light_status}") # Drive through intersection return py_trees.common.Status.SUCCESS else: From 0adbfc2d46ba98279045a276f8678278261fc5bd Mon Sep 17 00:00:00 2001 From: seefelke Date: Thu, 12 Dec 2024 23:43:44 +0100 Subject: [PATCH 33/37] small logging fix --- .../src/behavior_agent/behaviours/intersection.py | 12 ++++++++---- .../src/behavior_agent/behaviours/overtake.py | 12 +++++++++--- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/code/planning/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py index 4ac8aad9..68954e09 100755 --- a/code/planning/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/src/behavior_agent/behaviours/intersection.py @@ -147,8 +147,10 @@ def update(self): or (self.stop_sign_detected and not self.traffic_light_detected) ): - rospy.loginfo("Intersection Approach: slowing down! Stop sign: - {self.stop_sign_detected}, Light: {self.traffic_light_status}") + rospy.loginfo( + "Intersection Approach: slowing down! Stop sign: " + "{self.stop_sign_detected}, Light: {self.traffic_light_status}" + ) self.curr_behavior_pub.publish(bs.int_app_to_stop.name) # approach slowly when traffic light is green as traffic lights are @@ -174,7 +176,7 @@ def update(self): or (self.traffic_light_distance < 150) ): # stopped - print("Intersection Approach: stopped") + rospy.loginfo("Intersection Approach: stopped") return py_trees.common.Status.SUCCESS elif ( speed > convert_to_ms(5.0) @@ -314,7 +316,9 @@ def update(self): rospy.get_rostime() - self.green_light_time > rospy.Duration(1) and traffic_light_status == "green" ): - rospy.loginfo(f"Driving through Intersection Light Status: {traffic_light_status}") + rospy.loginfo( + f"Driving through Intersection Light Status: {traffic_light_status}" + ) # Drive through intersection return py_trees.common.Status.SUCCESS else: diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 133e3c86..447b4d79 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -76,7 +76,8 @@ def update(self): - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] Gets the current distance to overtake, the current oncoming lane status and the - distance to collsion object. Slows down while oncoming blocked until stopped or oncoming clear. + distance to collsion object. Slows down while oncoming blocked until stopped + or oncoming clear. :return: py_trees.common.Status.RUNNING, if too far from overtaking py_trees.common.Status.SUCCESS, if stopped behind the blocking object or entered the process. @@ -123,7 +124,10 @@ def update(self): if self.ot_distance > 20.0: # too far - rospy.loginfo("Overtake Approach: still approaching obstacle, distance: {self.ot_distance}") + rospy.loginfo( + "Overtake Approach: still approaching obstacle, " + "distance: {self.ot_distance}" + ) return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and self.ot_distance < TARGET_DISTANCE_TO_STOP: # stopped @@ -231,7 +235,9 @@ def update(self): self.curr_behavior_pub.publish(bs.ot_wait_free.name) return py_trees.common.Status.SUCCESS else: - rospy.loginfo(f"Overtake still blocked, distance to oncoming: {distance_oncoming}") + rospy.loginfo( + f"Overtake still blocked, distance to oncoming: {distance_oncoming}" + ) self.curr_behavior_pub.publish(bs.ot_wait_stopped.name) return py_trees.common.Status.RUNNING elif obstacle_msg.data[0] == np.inf: From 0d423a6e6e2d0e19b038f218c1c60496ee0a2f60 Mon Sep 17 00:00:00 2001 From: seefelke Date: Fri, 13 Dec 2024 00:08:06 +0100 Subject: [PATCH 34/37] some f string fixes --- .../src/behavior_agent/behaviours/intersection.py | 9 ++++++--- .../src/behavior_agent/behaviours/lane_change.py | 4 +++- code/planning/src/behavior_agent/behaviours/overtake.py | 4 ++-- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/code/planning/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py index 68954e09..6c4d4470 100755 --- a/code/planning/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/src/behavior_agent/behaviours/intersection.py @@ -148,8 +148,8 @@ def update(self): ): rospy.loginfo( - "Intersection Approach: slowing down! Stop sign: " - "{self.stop_sign_detected}, Light: {self.traffic_light_status}" + f"Intersection Approach: slowing down! Stop sign: " + f"{self.stop_sign_detected}, Light: {self.traffic_light_status}" ) self.curr_behavior_pub.publish(bs.int_app_to_stop.name) @@ -185,7 +185,10 @@ def update(self): ): # drive through intersection even if traffic light turns yellow - rospy.loginfo("Intersection Approach Light is green") + rospy.loginfo( + f"Intersection Approach Light is green, light:" + f"{self.traffic_light_status}" + ) return py_trees.common.Status.SUCCESS elif speed > convert_to_ms(5.0) and self.virtual_stopline_distance < 3.5: # running over line diff --git a/code/planning/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py index 87e18cb9..c6605308 100755 --- a/code/planning/src/behavior_agent/behaviours/lane_change.py +++ b/code/planning/src/behavior_agent/behaviours/lane_change.py @@ -115,7 +115,9 @@ def update(self): return py_trees.common.Status.RUNNING if self.virtual_change_distance > target_dis and self.blocked: # too far - rospy.loginfo("Lane Change: still approaching") + rospy.loginfo( + f"Lane Change: still approaching, distance:{self.virtual_change_distance}" + ) self.curr_behavior_pub.publish(bs.lc_app_blocked.name) return py_trees.common.Status.RUNNING elif ( diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 447b4d79..41ad24f4 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -125,8 +125,8 @@ def update(self): if self.ot_distance > 20.0: # too far rospy.loginfo( - "Overtake Approach: still approaching obstacle, " - "distance: {self.ot_distance}" + f"Overtake Approach: still approaching obstacle, " + f"distance: {self.ot_distance}" ) return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and self.ot_distance < TARGET_DISTANCE_TO_STOP: From 80aa8bb8a0382f881af87184c355c889b029e350 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Fri, 13 Dec 2024 00:30:42 +0100 Subject: [PATCH 35/37] Rename Unstuck_Behavior.md to Unstuck.md --- doc/planning/behaviours/{Unstuck_Behavior.md => Unstuck.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename doc/planning/behaviours/{Unstuck_Behavior.md => Unstuck.md} (100%) diff --git a/doc/planning/behaviours/Unstuck_Behavior.md b/doc/planning/behaviours/Unstuck.md similarity index 100% rename from doc/planning/behaviours/Unstuck_Behavior.md rename to doc/planning/behaviours/Unstuck.md From ec3b6f3fb1061850d4ed51e2bb9429468e600624 Mon Sep 17 00:00:00 2001 From: seefelke Date: Fri, 13 Dec 2024 15:24:38 +0100 Subject: [PATCH 36/37] linter --- doc/planning/behaviours/Intersection.md | 2 +- doc/planning/behaviours/LaneChange.md | 2 +- doc/planning/behaviours/LeaveParkingSpace.md | 2 +- doc/planning/behaviours/Overtake.md | 17 ++++++++--------- 4 files changed, 11 insertions(+), 12 deletions(-) diff --git a/doc/planning/behaviours/Intersection.md b/doc/planning/behaviours/Intersection.md index 106a4a80..7d17dd90 100644 --- a/doc/planning/behaviours/Intersection.md +++ b/doc/planning/behaviours/Intersection.md @@ -9,7 +9,7 @@ - [Wait](#wait) - [Enter](#enter) - [Leave](#leave) - + ## General The Intersection behaviour is used to control the vehicle when it encounters a intersection. It handles stop signs as well as traffic lights. diff --git a/doc/planning/behaviours/LaneChange.md b/doc/planning/behaviours/LaneChange.md index f8440e9d..341b40a4 100644 --- a/doc/planning/behaviours/LaneChange.md +++ b/doc/planning/behaviours/LaneChange.md @@ -9,7 +9,7 @@ - [Wait](#wait) - [Enter](#enter) - [Leave](#leave) - + ## General This behaviour executes a lane change. It slows the vehicle down until the lane change point is reached and then proceeds to switch lanes. diff --git a/doc/planning/behaviours/LeaveParkingSpace.md b/doc/planning/behaviours/LeaveParkingSpace.md index 716ade84..51192638 100644 --- a/doc/planning/behaviours/LeaveParkingSpace.md +++ b/doc/planning/behaviours/LeaveParkingSpace.md @@ -4,7 +4,7 @@ - [Leave Parking Space Behavior](#leaveparkingspace-behavior) - [General](#general) - + ## General The leave parking space behaviour is only executed at the beginning of the simulation to leave the parking space. diff --git a/doc/planning/behaviours/Overtake.md b/doc/planning/behaviours/Overtake.md index a8df0dd0..3058995a 100644 --- a/doc/planning/behaviours/Overtake.md +++ b/doc/planning/behaviours/Overtake.md @@ -2,21 +2,20 @@ **Summary:** This file explains the Overtake behavior. -- [Overtake Behavior](#overtake-behavior) - - [General](#general) - - [Overtake Ahead](#overtake-ahead) - - [Approach](#approach) - - [Wait](#wait) - - [Enter](#enter) - - [Leave](#leave) - +- [General](#general) +- [Overtake ahead](#overtake-ahead) +- [Approach](#approach) +- [Wait](#wait) +- [Enter](#enter) +- [Leave](#leave) + ## General This behaviour is used to overtake an object in close proximity. This behaviour is currently not working and more like a initial prototype. ## Overtake ahead -Checks whether there is a object in front of the car that needs overtaking. +Checks whether there is a object in front of the car that needs overtaking. Estimates whether the car would collide with the object soon. If that is the case a counter gets incremented. When that counter reaches 4 SUCCESS is returned. If the object is not blocking the trajectory, FAILURE is returned. From 1f4c75373f4cd62507615f001e2c3585792dae8d Mon Sep 17 00:00:00 2001 From: seefelke Date: Fri, 13 Dec 2024 15:27:24 +0100 Subject: [PATCH 37/37] linter --- code/planning/src/behavior_agent/behaviours/lane_change.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/code/planning/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py index c6605308..b6c67eae 100755 --- a/code/planning/src/behavior_agent/behaviours/lane_change.py +++ b/code/planning/src/behavior_agent/behaviours/lane_change.py @@ -116,7 +116,8 @@ def update(self): if self.virtual_change_distance > target_dis and self.blocked: # too far rospy.loginfo( - f"Lane Change: still approaching, distance:{self.virtual_change_distance}" + f"Lane Change: still approaching, " + f"distance:{self.virtual_change_distance}" ) self.curr_behavior_pub.publish(bs.lc_app_blocked.name) return py_trees.common.Status.RUNNING