From 178b7721dff5b56033a083a9bdb6a14fa61e9f6f Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sat, 28 Dec 2024 22:23:36 -0700 Subject: [PATCH 01/29] intial commit --- .../autonomous_landing/autonomous_landing.py | 292 ++++++++++++++++++ modules/common | 2 +- 2 files changed, 293 insertions(+), 1 deletion(-) create mode 100644 modules/autonomous_landing/autonomous_landing.py diff --git a/modules/autonomous_landing/autonomous_landing.py b/modules/autonomous_landing/autonomous_landing.py new file mode 100644 index 00000000..ba138a06 --- /dev/null +++ b/modules/autonomous_landing/autonomous_landing.py @@ -0,0 +1,292 @@ +""" +Auto-landing script for AEAC 2024 competition. + +Usage: Either with contour (default) or yolo method. +python blue_only.py --method=... +""" + +import argparse +import copy +import math +import os +import pathlib +import time +import yaml + +import cv2 +import dotenv +import numpy as np +from pymavlink import mavutil + +import yolo_decision + +LOG_DIRECTORY_PATH = pathlib.Path("logs") +SAVE_PREFIX = str(pathlib.Path(LOG_DIRECTORY_PATH, "image_")) + +CAMERA = 0 +# Camera field of view +FOV_X = 62.2 +FOV_Y = 48.8 + +CONTOUR_MINIMUM = 0.8 + +CONFIG_FILE_PATH = pathlib.Path("config.yml") + + +def current_milli_time() -> int: + """ + Returns the current time in milliseconds. + """ + return round(time.time() * 1000) + + +def is_contour_circular(contour: np.ndarray) -> bool: + """ + Checks if the shape is close to circular. + + Return: True is the shape is circular, false if it is not. + """ + perimeter = cv2.arcLength(contour, True) + + # Check if the perimeter is zero + if perimeter == 0.0: + return False + + area = cv2.contourArea(contour) + circularity = 4 * np.pi * (area / (perimeter * perimeter)) + return circularity > CONTOUR_MINIMUM + + +def is_contour_large_enough(contour: np.ndarray, min_diameter: float) -> bool: + """ + Checks if the shape is larger than the provided diameter. + + Return: True if it is, false if it not. + """ + _, radius = cv2.minEnclosingCircle(contour) + diameter = radius * 2 + return diameter >= min_diameter + + +def calc_target_distance(height_agl: float, x_rad: float, y_rad: float) -> float: + """ + Get the horizontal distance. + """ + x_ground_dist_m = math.tan(x_rad) * height_agl + y_ground_dist_m = math.tan(y_rad) * height_agl + ground_hyp = math.sqrt(math.pow(x_ground_dist_m, 2) + math.pow(y_ground_dist_m, 2)) + print("Required horizontal correction (m): ", ground_hyp) + target_to_vehicle_dist = math.sqrt(math.pow(ground_hyp, 2) + math.pow(height_agl, 2)) + print("Distance from vehicle to target (m): ", target_to_vehicle_dist) + return target_to_vehicle_dist + + +# Callback expects `self` as first argument +# pylint: disable-next=unused-argument +def my_allow_unsigned_callback(self: object, message_id: int) -> bool: + """ + Specify which messages to accept. + """ + # Allow radio status messages + return message_id == mavutil.mavlink.MAVLINK_MSG_ID_RADIO_STATUS + + +def detect_landing_pads_contour(image: np.ndarray) -> "tuple[bool, tuple | None]": + """ + Detect landing pads using contours. + + image: Current image frame + """ + kernel = np.ones((2, 2), np.uint8) + + gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + threshold = 180 + im_bw = cv2.threshold(gray_image, threshold, 255, cv2.THRESH_BINARY)[1] + im_dilation = cv2.dilate(im_bw, kernel, iterations=1) + contours, hierarchy = cv2.findContours(im_dilation, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + + if len(contours) == 0: + return False, None + + contours_with_children = set(i for i, hier in enumerate(hierarchy[0]) if hier[2] != -1) + parent_circular_contours = [ + cnt + for i, cnt in enumerate(contours) + if is_contour_circular(cnt) + and is_contour_large_enough(cnt, 7) + and i in contours_with_children + ] + contour_image = copy.deepcopy(image) + cv2.drawContours(contour_image, parent_circular_contours, -1, (0, 255, 0), 2) + largest_contour = max(parent_circular_contours, key=cv2.contourArea, default=None) + + if not largest_contour: + return False, None + + return True, tuple(cv2.boundingRect(largest_contour)) + + +def detect_landing_pads_yolo(image: np.ndarray, config: dict) -> "tuple[bool, tuple | None]": + """ + Detect landing pads using YOLO model. + + image: Current image frame + """ + model_device = config["yolo_detect_target"]["device"] + detect_confidence = config["yolo_detect_target"]["confidence"] + model_path = config["yolo_detect_target"]["model_path"] + + yolo_model = yolo_decision.DetectLandingPad(model_device, detect_confidence, model_path) + + result, detections = yolo_model.get_landing_pads(image) + if not result: + return False, None + + best_landing_pad = yolo_model.find_best_pad(detections) + if not best_landing_pad: + return False, None + + x, y = best_landing_pad.x_1, best_landing_pad.y_1 + w = best_landing_pad.x_2 - best_landing_pad.x_1 + h = best_landing_pad.y_2 - best_landing_pad.y_1 + + return True, (x, y, w, h) + + +def main() -> int: + """ + Main function. + """ + try: + with CONFIG_FILE_PATH.open("r", encoding="utf8") as file: + try: + config = yaml.safe_load(file) + except yaml.YAMLError as exc: + print(f"Error parsing YAML file: {exc}") + return -1 + except FileNotFoundError: + print(f"File not found: {CONFIG_FILE_PATH}") + return -1 + except IOError as exc: + print(f"Error when opening file: {exc}") + + parser = argparse.ArgumentParser(description="AEAC 2024 Auto-landing script") + parser.add_argument( + "--method", + help="Method for selecting landing pads", + choices=["contour", "yolo"], + default="contour", + ) + args = parser.parse_args() + + dotenv.load_dotenv(".key") + secret_key = os.getenv("KEY") + + cam = cv2.VideoCapture(CAMERA) + + vehicle = mavutil.mavlink_connection("tcp:localhost:14550") + # vehicle = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600) + vehicle.wait_heartbeat() + print( + f"Heartbeat from system (system {vehicle.target_system} component {vehicle.target_component})" + ) + + pos_message = vehicle.mav.command_long_encode( + vehicle.target_system, # Target system ID + vehicle.target_component, # Target component ID + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # ID of command to send + 0, # Confirmation + 33, # param1: Message ID to be streamed + 250000, # param2: Interval in microseconds + 0, # param3 (unused) + 0, # param4 (unused) + 0, # param5 (unused) + 0, # param5 (unused) + 0, # param6 (unused) + ) + + secret_key = bytearray(secret_key, "utf-8") + vehicle.setup_signing(secret_key, True, my_allow_unsigned_callback, int(time.time()), 0) + vehicle.mav.send(pos_message) + + altitude_m = 0 + loop_counter = 0 + last_time = current_milli_time() + last_image_time = current_milli_time() + + while True: + result, image = cam.read() + if not result: + print("ERROR: Could not get image from camera") + continue + + image = cv2.flip(image, 0) + image = cv2.flip(image, 1) + im_h, im_w, _ = image.shape + print("Input image width: " + str(im_w)) + print("Input image height: " + str(im_h)) + try: + altitude_mm = vehicle.messages[ + "GLOBAL_POSITION_INT" + ].relative_alt # Note, you can access message fields as attributes! + altitude_m = max(altitude_mm / 1000, 0.0) + print("Altitude AGL: ", altitude_m) + except (KeyError, AttributeError): + print("No GLOBAL_POSITION_INT message received") + continue + + if args.method == "contour": + bounding_result, bounding_box = detect_landing_pads_contour(image) + elif args.method == "yolo": + bounding_result, bounding_box = detect_landing_pads_yolo(image, config) + + loop_counter += 1 + if current_milli_time() - last_time > 1000: + print("FPS:", loop_counter) + loop_counter = 0 + last_time = current_milli_time() + + if not bounding_result: + # Print plain image + if current_milli_time() - last_image_time > 100: + print("Plain Image Write") + cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", image) + last_image_time = current_milli_time() + continue + + x, y, w, h = bounding_box + rect_image = copy.deepcopy(image) + cv2.rectangle(rect_image, (int(x), int(y)), (int(x + w), int(y + h)), (0, 255, 0), 2) + x_center = x + w / 2 + y_center = y + h / 2 + angle_x = (x_center - im_w / 2) * (FOV_X * (math.pi / 180)) / im_w + angle_y = (y_center - im_h / 2) * (FOV_Y * (math.pi / 180)) / im_h + cv2.circle(rect_image, (int(x_center), int(y_center)), 2, (0, 0, 255), 2) + print("X Angle (rad): ", angle_x) + print("Y Angle (rad): ", angle_y) + target_dist = calc_target_distance(altitude_m, angle_x, angle_y) + vehicle.mav.landing_target_send( + 0, + 0, + mavutil.mavlink.MAV_FRAME_BODY_NED, + angle_x, + angle_y, + target_dist, + 0, + 0, + ) + + if current_milli_time() - last_image_time > 100: + print("Bounding Box Image Write") + cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", rect_image) + last_image_time = current_milli_time() + + return 0 + + +if __name__ == "__main__": + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") + + print("Done!") \ No newline at end of file diff --git a/modules/common b/modules/common index a0aac8ce..a256a497 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit a0aac8ce29273a6a1ca397a2229770add760835e +Subproject commit a256a49778d1154e03683c3b5e2fe6cb215d00e7 From 3954351cf0d8e87881394bf873ff7bd5fdeb7d41 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sun, 29 Dec 2024 00:06:23 -0700 Subject: [PATCH 02/29] added files --- modules/auto_landing/__init__.py | 0 modules/auto_landing/auto_landing.py | 14 + modules/auto_landing/auto_landing_worker.py | 292 ++++++++++++++++++++ 3 files changed, 306 insertions(+) create mode 100644 modules/auto_landing/__init__.py create mode 100644 modules/auto_landing/auto_landing.py create mode 100644 modules/auto_landing/auto_landing_worker.py diff --git a/modules/auto_landing/__init__.py b/modules/auto_landing/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py new file mode 100644 index 00000000..c518266f --- /dev/null +++ b/modules/auto_landing/auto_landing.py @@ -0,0 +1,14 @@ +""" +Auto-landing script. +""" + +class AutoLanding: + def __init__ (self): + raise NotImplementedError + + def run (self): + raise NotImplementedError + + def calculate_angles (self): + raise NotImplementedError + \ No newline at end of file diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py new file mode 100644 index 00000000..fda50ae7 --- /dev/null +++ b/modules/auto_landing/auto_landing_worker.py @@ -0,0 +1,292 @@ +""" +Auto-landing script for AEAC 2024 competition. + +Usage: Either with contour (default) or yolo method. +python blue_only.py --method=... +""" + +import argparse +import copy +import math +import os +import pathlib +import time +import yaml + +import cv2 +import dotenv +import numpy as np +from pymavlink import mavutil + +import yolo_decision # do i copy the DetectLandingPad class to replace this library? + +LOG_DIRECTORY_PATH = pathlib.Path("logs") +SAVE_PREFIX = str(pathlib.Path(LOG_DIRECTORY_PATH, "image_")) + +CAMERA = 0 +# Camera field of view +FOV_X = 62.2 +FOV_Y = 48.8 + +CONTOUR_MINIMUM = 0.8 + +CONFIG_FILE_PATH = pathlib.Path("config.yml") + + +def current_milli_time() -> int: + """ + Returns the current time in milliseconds. + """ + return round(time.time() * 1000) + + +def is_contour_circular(contour: np.ndarray) -> bool: + """ + Checks if the shape is close to circular. + + Return: True is the shape is circular, false if it is not. + """ + perimeter = cv2.arcLength(contour, True) + + # Check if the perimeter is zero + if perimeter == 0.0: + return False + + area = cv2.contourArea(contour) + circularity = 4 * np.pi * (area / (perimeter * perimeter)) + return circularity > CONTOUR_MINIMUM + + +def is_contour_large_enough(contour: np.ndarray, min_diameter: float) -> bool: + """ + Checks if the shape is larger than the provided diameter. + + Return: True if it is, false if it not. + """ + _, radius = cv2.minEnclosingCircle(contour) + diameter = radius * 2 + return diameter >= min_diameter + + +def calc_target_distance(height_agl: float, x_rad: float, y_rad: float) -> float: + """ + Get the horizontal distance. + """ + x_ground_dist_m = math.tan(x_rad) * height_agl + y_ground_dist_m = math.tan(y_rad) * height_agl + ground_hyp = math.sqrt(math.pow(x_ground_dist_m, 2) + math.pow(y_ground_dist_m, 2)) + print("Required horizontal correction (m): ", ground_hyp) + target_to_vehicle_dist = math.sqrt(math.pow(ground_hyp, 2) + math.pow(height_agl, 2)) + print("Distance from vehicle to target (m): ", target_to_vehicle_dist) + return target_to_vehicle_dist + + +# Callback expects `self` as first argument +# pylint: disable-next=unused-argument +def my_allow_unsigned_callback(self: object, message_id: int) -> bool: + """ + Specify which messages to accept. + """ + # Allow radio status messages + return message_id == mavutil.mavlink.MAVLINK_MSG_ID_RADIO_STATUS + + +def detect_landing_pads_contour(image: np.ndarray) -> "tuple[bool, tuple | None]": + """ + Detect landing pads using contours. + + image: Current image frame + """ + kernel = np.ones((2, 2), np.uint8) + + gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + threshold = 180 + im_bw = cv2.threshold(gray_image, threshold, 255, cv2.THRESH_BINARY)[1] + im_dilation = cv2.dilate(im_bw, kernel, iterations=1) + contours, hierarchy = cv2.findContours(im_dilation, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + + if len(contours) == 0: + return False, None + + contours_with_children = set(i for i, hier in enumerate(hierarchy[0]) if hier[2] != -1) + parent_circular_contours = [ + cnt + for i, cnt in enumerate(contours) + if is_contour_circular(cnt) + and is_contour_large_enough(cnt, 7) + and i in contours_with_children + ] + contour_image = copy.deepcopy(image) + cv2.drawContours(contour_image, parent_circular_contours, -1, (0, 255, 0), 2) + largest_contour = max(parent_circular_contours, key=cv2.contourArea, default=None) + + if not largest_contour: + return False, None + + return True, tuple(cv2.boundingRect(largest_contour)) + + +def detect_landing_pads_yolo(image: np.ndarray, config: dict) -> "tuple[bool, tuple | None]": + """ + Detect landing pads using YOLO model. + + image: Current image frame + """ + model_device = config["yolo_detect_target"]["device"] + detect_confidence = config["yolo_detect_target"]["confidence"] + model_path = config["yolo_detect_target"]["model_path"] + + yolo_model = yolo_decision.DetectLandingPad(model_device, detect_confidence, model_path) + + result, detections = yolo_model.get_landing_pads(image) + if not result: + return False, None + + best_landing_pad = yolo_model.find_best_pad(detections) + if not best_landing_pad: + return False, None + + x, y = best_landing_pad.x_1, best_landing_pad.y_1 + w = best_landing_pad.x_2 - best_landing_pad.x_1 + h = best_landing_pad.y_2 - best_landing_pad.y_1 + + return True, (x, y, w, h) + + +def main() -> int: + """ + Main function. + """ + try: + with CONFIG_FILE_PATH.open("r", encoding="utf8") as file: + try: + config = yaml.safe_load(file) + except yaml.YAMLError as exc: + print(f"Error parsing YAML file: {exc}") + return -1 + except FileNotFoundError: + print(f"File not found: {CONFIG_FILE_PATH}") + return -1 + except IOError as exc: + print(f"Error when opening file: {exc}") + + parser = argparse.ArgumentParser(description="AEAC 2024 Auto-landing script") + parser.add_argument( + "--method", + help="Method for selecting landing pads", + choices=["contour", "yolo"], + default="contour", + ) + args = parser.parse_args() + + dotenv.load_dotenv(".key") + secret_key = os.getenv("KEY") + + cam = cv2.VideoCapture(CAMERA) + + vehicle = mavutil.mavlink_connection("tcp:localhost:14550") + # vehicle = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600) + vehicle.wait_heartbeat() + print( + f"Heartbeat from system (system {vehicle.target_system} component {vehicle.target_component})" + ) + + pos_message = vehicle.mav.command_long_encode( + vehicle.target_system, # Target system ID + vehicle.target_component, # Target component ID + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # ID of command to send + 0, # Confirmation + 33, # param1: Message ID to be streamed + 250000, # param2: Interval in microseconds + 0, # param3 (unused) + 0, # param4 (unused) + 0, # param5 (unused) + 0, # param5 (unused) + 0, # param6 (unused) + ) + + secret_key = bytearray(secret_key, "utf-8") + vehicle.setup_signing(secret_key, True, my_allow_unsigned_callback, int(time.time()), 0) + vehicle.mav.send(pos_message) + + altitude_m = 0 + loop_counter = 0 + last_time = current_milli_time() + last_image_time = current_milli_time() + + while True: + result, image = cam.read() + if not result: + print("ERROR: Could not get image from camera") + continue + + image = cv2.flip(image, 0) + image = cv2.flip(image, 1) + im_h, im_w, _ = image.shape + print("Input image width: " + str(im_w)) + print("Input image height: " + str(im_h)) + try: + altitude_mm = vehicle.messages[ + "GLOBAL_POSITION_INT" + ].relative_alt # Note, you can access message fields as attributes! + altitude_m = max(altitude_mm / 1000, 0.0) + print("Altitude AGL: ", altitude_m) + except (KeyError, AttributeError): + print("No GLOBAL_POSITION_INT message received") + continue + + if args.method == "contour": + bounding_result, bounding_box = detect_landing_pads_contour(image) + elif args.method == "yolo": + bounding_result, bounding_box = detect_landing_pads_yolo(image, config) + + loop_counter += 1 + if current_milli_time() - last_time > 1000: + print("FPS:", loop_counter) + loop_counter = 0 + last_time = current_milli_time() + + if not bounding_result: + # Print plain image + if current_milli_time() - last_image_time > 100: + print("Plain Image Write") + cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", image) + last_image_time = current_milli_time() + continue + + x, y, w, h = bounding_box + rect_image = copy.deepcopy(image) + cv2.rectangle(rect_image, (int(x), int(y)), (int(x + w), int(y + h)), (0, 255, 0), 2) + x_center = x + w / 2 + y_center = y + h / 2 + angle_x = (x_center - im_w / 2) * (FOV_X * (math.pi / 180)) / im_w + angle_y = (y_center - im_h / 2) * (FOV_Y * (math.pi / 180)) / im_h + cv2.circle(rect_image, (int(x_center), int(y_center)), 2, (0, 0, 255), 2) + print("X Angle (rad): ", angle_x) + print("Y Angle (rad): ", angle_y) + target_dist = calc_target_distance(altitude_m, angle_x, angle_y) + vehicle.mav.landing_target_send( + 0, + 0, + mavutil.mavlink.MAV_FRAME_BODY_NED, + angle_x, + angle_y, + target_dist, + 0, + 0, + ) + + if current_milli_time() - last_image_time > 100: + print("Bounding Box Image Write") + cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", rect_image) + last_image_time = current_milli_time() + + return 0 + + +if __name__ == "__main__": + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") + + print("Done!") \ No newline at end of file From b80e66877b45283a6b21452b61b1b23497c6cbad Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sun, 29 Dec 2024 00:09:01 -0700 Subject: [PATCH 03/29] removed old module --- .../autonomous_landing/autonomous_landing.py | 292 ------------------ 1 file changed, 292 deletions(-) delete mode 100644 modules/autonomous_landing/autonomous_landing.py diff --git a/modules/autonomous_landing/autonomous_landing.py b/modules/autonomous_landing/autonomous_landing.py deleted file mode 100644 index ba138a06..00000000 --- a/modules/autonomous_landing/autonomous_landing.py +++ /dev/null @@ -1,292 +0,0 @@ -""" -Auto-landing script for AEAC 2024 competition. - -Usage: Either with contour (default) or yolo method. -python blue_only.py --method=... -""" - -import argparse -import copy -import math -import os -import pathlib -import time -import yaml - -import cv2 -import dotenv -import numpy as np -from pymavlink import mavutil - -import yolo_decision - -LOG_DIRECTORY_PATH = pathlib.Path("logs") -SAVE_PREFIX = str(pathlib.Path(LOG_DIRECTORY_PATH, "image_")) - -CAMERA = 0 -# Camera field of view -FOV_X = 62.2 -FOV_Y = 48.8 - -CONTOUR_MINIMUM = 0.8 - -CONFIG_FILE_PATH = pathlib.Path("config.yml") - - -def current_milli_time() -> int: - """ - Returns the current time in milliseconds. - """ - return round(time.time() * 1000) - - -def is_contour_circular(contour: np.ndarray) -> bool: - """ - Checks if the shape is close to circular. - - Return: True is the shape is circular, false if it is not. - """ - perimeter = cv2.arcLength(contour, True) - - # Check if the perimeter is zero - if perimeter == 0.0: - return False - - area = cv2.contourArea(contour) - circularity = 4 * np.pi * (area / (perimeter * perimeter)) - return circularity > CONTOUR_MINIMUM - - -def is_contour_large_enough(contour: np.ndarray, min_diameter: float) -> bool: - """ - Checks if the shape is larger than the provided diameter. - - Return: True if it is, false if it not. - """ - _, radius = cv2.minEnclosingCircle(contour) - diameter = radius * 2 - return diameter >= min_diameter - - -def calc_target_distance(height_agl: float, x_rad: float, y_rad: float) -> float: - """ - Get the horizontal distance. - """ - x_ground_dist_m = math.tan(x_rad) * height_agl - y_ground_dist_m = math.tan(y_rad) * height_agl - ground_hyp = math.sqrt(math.pow(x_ground_dist_m, 2) + math.pow(y_ground_dist_m, 2)) - print("Required horizontal correction (m): ", ground_hyp) - target_to_vehicle_dist = math.sqrt(math.pow(ground_hyp, 2) + math.pow(height_agl, 2)) - print("Distance from vehicle to target (m): ", target_to_vehicle_dist) - return target_to_vehicle_dist - - -# Callback expects `self` as first argument -# pylint: disable-next=unused-argument -def my_allow_unsigned_callback(self: object, message_id: int) -> bool: - """ - Specify which messages to accept. - """ - # Allow radio status messages - return message_id == mavutil.mavlink.MAVLINK_MSG_ID_RADIO_STATUS - - -def detect_landing_pads_contour(image: np.ndarray) -> "tuple[bool, tuple | None]": - """ - Detect landing pads using contours. - - image: Current image frame - """ - kernel = np.ones((2, 2), np.uint8) - - gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) - threshold = 180 - im_bw = cv2.threshold(gray_image, threshold, 255, cv2.THRESH_BINARY)[1] - im_dilation = cv2.dilate(im_bw, kernel, iterations=1) - contours, hierarchy = cv2.findContours(im_dilation, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - - if len(contours) == 0: - return False, None - - contours_with_children = set(i for i, hier in enumerate(hierarchy[0]) if hier[2] != -1) - parent_circular_contours = [ - cnt - for i, cnt in enumerate(contours) - if is_contour_circular(cnt) - and is_contour_large_enough(cnt, 7) - and i in contours_with_children - ] - contour_image = copy.deepcopy(image) - cv2.drawContours(contour_image, parent_circular_contours, -1, (0, 255, 0), 2) - largest_contour = max(parent_circular_contours, key=cv2.contourArea, default=None) - - if not largest_contour: - return False, None - - return True, tuple(cv2.boundingRect(largest_contour)) - - -def detect_landing_pads_yolo(image: np.ndarray, config: dict) -> "tuple[bool, tuple | None]": - """ - Detect landing pads using YOLO model. - - image: Current image frame - """ - model_device = config["yolo_detect_target"]["device"] - detect_confidence = config["yolo_detect_target"]["confidence"] - model_path = config["yolo_detect_target"]["model_path"] - - yolo_model = yolo_decision.DetectLandingPad(model_device, detect_confidence, model_path) - - result, detections = yolo_model.get_landing_pads(image) - if not result: - return False, None - - best_landing_pad = yolo_model.find_best_pad(detections) - if not best_landing_pad: - return False, None - - x, y = best_landing_pad.x_1, best_landing_pad.y_1 - w = best_landing_pad.x_2 - best_landing_pad.x_1 - h = best_landing_pad.y_2 - best_landing_pad.y_1 - - return True, (x, y, w, h) - - -def main() -> int: - """ - Main function. - """ - try: - with CONFIG_FILE_PATH.open("r", encoding="utf8") as file: - try: - config = yaml.safe_load(file) - except yaml.YAMLError as exc: - print(f"Error parsing YAML file: {exc}") - return -1 - except FileNotFoundError: - print(f"File not found: {CONFIG_FILE_PATH}") - return -1 - except IOError as exc: - print(f"Error when opening file: {exc}") - - parser = argparse.ArgumentParser(description="AEAC 2024 Auto-landing script") - parser.add_argument( - "--method", - help="Method for selecting landing pads", - choices=["contour", "yolo"], - default="contour", - ) - args = parser.parse_args() - - dotenv.load_dotenv(".key") - secret_key = os.getenv("KEY") - - cam = cv2.VideoCapture(CAMERA) - - vehicle = mavutil.mavlink_connection("tcp:localhost:14550") - # vehicle = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600) - vehicle.wait_heartbeat() - print( - f"Heartbeat from system (system {vehicle.target_system} component {vehicle.target_component})" - ) - - pos_message = vehicle.mav.command_long_encode( - vehicle.target_system, # Target system ID - vehicle.target_component, # Target component ID - mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # ID of command to send - 0, # Confirmation - 33, # param1: Message ID to be streamed - 250000, # param2: Interval in microseconds - 0, # param3 (unused) - 0, # param4 (unused) - 0, # param5 (unused) - 0, # param5 (unused) - 0, # param6 (unused) - ) - - secret_key = bytearray(secret_key, "utf-8") - vehicle.setup_signing(secret_key, True, my_allow_unsigned_callback, int(time.time()), 0) - vehicle.mav.send(pos_message) - - altitude_m = 0 - loop_counter = 0 - last_time = current_milli_time() - last_image_time = current_milli_time() - - while True: - result, image = cam.read() - if not result: - print("ERROR: Could not get image from camera") - continue - - image = cv2.flip(image, 0) - image = cv2.flip(image, 1) - im_h, im_w, _ = image.shape - print("Input image width: " + str(im_w)) - print("Input image height: " + str(im_h)) - try: - altitude_mm = vehicle.messages[ - "GLOBAL_POSITION_INT" - ].relative_alt # Note, you can access message fields as attributes! - altitude_m = max(altitude_mm / 1000, 0.0) - print("Altitude AGL: ", altitude_m) - except (KeyError, AttributeError): - print("No GLOBAL_POSITION_INT message received") - continue - - if args.method == "contour": - bounding_result, bounding_box = detect_landing_pads_contour(image) - elif args.method == "yolo": - bounding_result, bounding_box = detect_landing_pads_yolo(image, config) - - loop_counter += 1 - if current_milli_time() - last_time > 1000: - print("FPS:", loop_counter) - loop_counter = 0 - last_time = current_milli_time() - - if not bounding_result: - # Print plain image - if current_milli_time() - last_image_time > 100: - print("Plain Image Write") - cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", image) - last_image_time = current_milli_time() - continue - - x, y, w, h = bounding_box - rect_image = copy.deepcopy(image) - cv2.rectangle(rect_image, (int(x), int(y)), (int(x + w), int(y + h)), (0, 255, 0), 2) - x_center = x + w / 2 - y_center = y + h / 2 - angle_x = (x_center - im_w / 2) * (FOV_X * (math.pi / 180)) / im_w - angle_y = (y_center - im_h / 2) * (FOV_Y * (math.pi / 180)) / im_h - cv2.circle(rect_image, (int(x_center), int(y_center)), 2, (0, 0, 255), 2) - print("X Angle (rad): ", angle_x) - print("Y Angle (rad): ", angle_y) - target_dist = calc_target_distance(altitude_m, angle_x, angle_y) - vehicle.mav.landing_target_send( - 0, - 0, - mavutil.mavlink.MAV_FRAME_BODY_NED, - angle_x, - angle_y, - target_dist, - 0, - 0, - ) - - if current_milli_time() - last_image_time > 100: - print("Bounding Box Image Write") - cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", rect_image) - last_image_time = current_milli_time() - - return 0 - - -if __name__ == "__main__": - result_main = main() - if result_main < 0: - print(f"ERROR: Status code: {result_main}") - - print("Done!") \ No newline at end of file From 8b822e0fd6a9a1b560703c8ac0ba285290a03d8d Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sat, 11 Jan 2025 04:37:04 -0500 Subject: [PATCH 04/29] made class --- modules/auto_landing/auto_landing.py | 66 +++++++++++++++++---- modules/auto_landing/auto_landing_worker.py | 4 ++ 2 files changed, 60 insertions(+), 10 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index c518266f..669aea2f 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -1,14 +1,60 @@ -""" -Auto-landing script. -""" +import math +import numpy as np class AutoLanding: - def __init__ (self): - raise NotImplementedError + """" + Auto-landing script. + """ + # do i make a create() as well? - def run (self): - raise NotImplementedError - - def calculate_angles (self): - raise NotImplementedError + def __init__ ( + self, + image: np.ndarray, # or do i use MatLike? + bounding_box: tuple, + FOV_X: float, + FOV_Y: float, + im_h: float, + im_w: float, + ) -> None: + """" + image: The input image. + bounding_box: The bounding box defined as (x, y, w, h). + FOV_X: The horizontal camera field of view (in degrees). + FOV_Y: The vertical camera field of view (in degrees). + im_w: Width of image. + im_h: Height of image. + + """ + self.image = image + self.bounding_box = bounding_box + self.FOV_X = FOV_X + self.FOV_Y = FOV_Y + self.im_h = im_h + self.im_w = im_w + + def run (self, image, bounding_box, FOV_X, FOV_Y, im_w, im_h) -> dict: + """ + Calculates the angles (in radians) of the vertices of the bounding box. + + Return: Dictionary with vertex coordinates as keys and (angle_x, angle_y) as values. + """ + x, y, w, h = self.bounding_box + + vertices = { + "top_left": (x, y), + "top_right": (x + w, y), + "bottom_left": (x, y + h), + "bottom_right": (x + w, y + h) + } + + box_angles = {} + + for vertex, (vertex_x, vertex_y) in vertices.items(): + angle_x = (vertex_x - self.im_w / 2) * (self.FOV_X * (math.pi / 180)) / self.im_w + angle_y = (vertex_y - self.im_h / 2) * (self.FOV_Y * (math.pi / 180)) / self.im_h + box_angles[vertex] = (angle_x, angle_y) + + return box_angles + + \ No newline at end of file diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index fda50ae7..8db94d8a 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -3,6 +3,8 @@ Usage: Either with contour (default) or yolo method. python blue_only.py --method=... + +note: not sure what to name this file cause it's not a worker.. """ import argparse @@ -20,6 +22,8 @@ import yolo_decision # do i copy the DetectLandingPad class to replace this library? +import auto_landing + LOG_DIRECTORY_PATH = pathlib.Path("logs") SAVE_PREFIX = str(pathlib.Path(LOG_DIRECTORY_PATH, "image_")) From fd8241fafa370b11aa9f6347fd7e78b2cdf636ef Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sun, 12 Jan 2025 14:28:29 -0500 Subject: [PATCH 05/29] made changes --- modules/auto_landing/auto_landing.py | 59 ++++++++++++++-------------- 1 file changed, 29 insertions(+), 30 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 669aea2f..9dd2ea5f 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -5,56 +5,55 @@ class AutoLanding: """" Auto-landing script. """ - # do i make a create() as well? - + def __init__ ( self, - image: np.ndarray, # or do i use MatLike? - bounding_box: tuple, FOV_X: float, FOV_Y: float, im_h: float, im_w: float, + x_center: float, + y_center: float, + height: float, ) -> None: """" - image: The input image. - bounding_box: The bounding box defined as (x, y, w, h). - FOV_X: The horizontal camera field of view (in degrees). - FOV_Y: The vertical camera field of view (in degrees). + FOV_X: The horizontal camera field of view in degrees. + FOV_Y: The vertical camera field of view in degrees. im_w: Width of image. im_h: Height of image. + x_center: x-coordinate of center of bounding box. + y_center: y-coordinate of center of bounding box. + height: Height above ground level in meters. """ - self.image = image - self.bounding_box = bounding_box self.FOV_X = FOV_X self.FOV_Y = FOV_Y self.im_h = im_h self.im_w = im_w + self.x_center = x_center + self.y_center = y_center + self.height = height - def run (self, image, bounding_box, FOV_X, FOV_Y, im_w, im_h) -> dict: + def run (self, FOV_X, FOV_Y, im_w, im_h, x_center, y_center, height) -> "tuple[float, float, float]": """ - Calculates the angles (in radians) of the vertices of the bounding box. + Calculates the angles in radians of the bounding box based on its center. - Return: Dictionary with vertex coordinates as keys and (angle_x, angle_y) as values. + Return: Tuple of the x and y angles in radians respectively and the target distance in meters. """ - x, y, w, h = self.bounding_box - - vertices = { - "top_left": (x, y), - "top_right": (x + w, y), - "bottom_left": (x, y + h), - "bottom_right": (x + w, y + h) - } - - box_angles = {} - - for vertex, (vertex_x, vertex_y) in vertices.items(): - angle_x = (vertex_x - self.im_w / 2) * (self.FOV_X * (math.pi / 180)) / self.im_w - angle_y = (vertex_y - self.im_h / 2) * (self.FOV_Y * (math.pi / 180)) / self.im_h - box_angles[vertex] = (angle_x, angle_y) - - return box_angles + angle_x = (self.x_center - self.im_w / 2) * (self.FOV_X * (math.pi / 180)) / self.im_w + angle_y = (self.y_center - self.im_h / 2) * (self.FOV_Y * (math.pi / 180)) / self.im_h + + print("X angle (rad): ", angle_x) + print("Y angle (rad): ", angle_y) + + x_dist = math.tan(angle_x) * self.height + y_dist = math.tan(angle_y) * self.height + ground_hyp = math.sqrt(math.pow(x_dist, 2) + math.pow(y_dist, 2)) + print("Required horizontal correction (m): ", ground_hyp) + target_to_vehicle_dist = math.sqrt(math.pow(ground_hyp, 2) + math.pow(self.height, 2)) + print("Distance from vehicle to target (m): ", target_to_vehicle_dist) + + return angle_x, angle_y, target_to_vehicle_dist \ No newline at end of file From 265cae96ae30bc08c8dac9730eca09c9be7b87f4 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Mon, 13 Jan 2025 00:07:33 -0500 Subject: [PATCH 06/29] added worker --- modules/auto_landing/auto_landing.py | 1 - modules/auto_landing/auto_landing_main.py | 296 ++++++++++++++++++ modules/auto_landing/auto_landing_worker.py | 321 +++----------------- 3 files changed, 333 insertions(+), 285 deletions(-) create mode 100644 modules/auto_landing/auto_landing_main.py diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 9dd2ea5f..a29ec581 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -1,5 +1,4 @@ import math -import numpy as np class AutoLanding: """" diff --git a/modules/auto_landing/auto_landing_main.py b/modules/auto_landing/auto_landing_main.py new file mode 100644 index 00000000..cc6e40db --- /dev/null +++ b/modules/auto_landing/auto_landing_main.py @@ -0,0 +1,296 @@ +""" +Auto-landing script for AEAC 2024 competition. + +Usage: Either with contour (default) or yolo method. +python blue_only.py --method=... + +note: not sure what to name this file +""" + +import argparse +import copy +import math +import os +import pathlib +import time +import yaml + +import cv2 +import dotenv +import numpy as np +from pymavlink import mavutil + +import yolo_decision + +import auto_landing + +LOG_DIRECTORY_PATH = pathlib.Path("logs") +SAVE_PREFIX = str(pathlib.Path(LOG_DIRECTORY_PATH, "image_")) + +CAMERA = 0 +# Camera field of view +FOV_X = 62.2 +FOV_Y = 48.8 + +CONTOUR_MINIMUM = 0.8 + +CONFIG_FILE_PATH = pathlib.Path("config.yml") + + +def current_milli_time() -> int: + """ + Returns the current time in milliseconds. + """ + return round(time.time() * 1000) + + +def is_contour_circular(contour: np.ndarray) -> bool: + """ + Checks if the shape is close to circular. + + Return: True is the shape is circular, false if it is not. + """ + perimeter = cv2.arcLength(contour, True) + + # Check if the perimeter is zero + if perimeter == 0.0: + return False + + area = cv2.contourArea(contour) + circularity = 4 * np.pi * (area / (perimeter * perimeter)) + return circularity > CONTOUR_MINIMUM + + +def is_contour_large_enough(contour: np.ndarray, min_diameter: float) -> bool: + """ + Checks if the shape is larger than the provided diameter. + + Return: True if it is, false if it not. + """ + _, radius = cv2.minEnclosingCircle(contour) + diameter = radius * 2 + return diameter >= min_diameter + + +def calc_target_distance(height_agl: float, x_rad: float, y_rad: float) -> float: + """ + Get the horizontal distance. + """ + x_ground_dist_m = math.tan(x_rad) * height_agl + y_ground_dist_m = math.tan(y_rad) * height_agl + ground_hyp = math.sqrt(math.pow(x_ground_dist_m, 2) + math.pow(y_ground_dist_m, 2)) + print("Required horizontal correction (m): ", ground_hyp) + target_to_vehicle_dist = math.sqrt(math.pow(ground_hyp, 2) + math.pow(height_agl, 2)) + print("Distance from vehicle to target (m): ", target_to_vehicle_dist) + return target_to_vehicle_dist + + +# Callback expects `self` as first argument +# pylint: disable-next=unused-argument +def my_allow_unsigned_callback(self: object, message_id: int) -> bool: + """ + Specify which messages to accept. + """ + # Allow radio status messages + return message_id == mavutil.mavlink.MAVLINK_MSG_ID_RADIO_STATUS + + +def detect_landing_pads_contour(image: np.ndarray) -> "tuple[bool, tuple | None]": + """ + Detect landing pads using contours. + + image: Current image frame + """ + kernel = np.ones((2, 2), np.uint8) + + gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + threshold = 180 + im_bw = cv2.threshold(gray_image, threshold, 255, cv2.THRESH_BINARY)[1] + im_dilation = cv2.dilate(im_bw, kernel, iterations=1) + contours, hierarchy = cv2.findContours(im_dilation, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + + if len(contours) == 0: + return False, None + + contours_with_children = set(i for i, hier in enumerate(hierarchy[0]) if hier[2] != -1) + parent_circular_contours = [ + cnt + for i, cnt in enumerate(contours) + if is_contour_circular(cnt) + and is_contour_large_enough(cnt, 7) + and i in contours_with_children + ] + contour_image = copy.deepcopy(image) + cv2.drawContours(contour_image, parent_circular_contours, -1, (0, 255, 0), 2) + largest_contour = max(parent_circular_contours, key=cv2.contourArea, default=None) + + if not largest_contour: + return False, None + + return True, tuple(cv2.boundingRect(largest_contour)) + + +def detect_landing_pads_yolo(image: np.ndarray, config: dict) -> "tuple[bool, tuple | None]": + """ + Detect landing pads using YOLO model. + + image: Current image frame + """ + model_device = config["yolo_detect_target"]["device"] + detect_confidence = config["yolo_detect_target"]["confidence"] + model_path = config["yolo_detect_target"]["model_path"] + + yolo_model = yolo_decision.DetectLandingPad(model_device, detect_confidence, model_path) + + result, detections = yolo_model.get_landing_pads(image) + if not result: + return False, None + + best_landing_pad = yolo_model.find_best_pad(detections) + if not best_landing_pad: + return False, None + + x, y = best_landing_pad.x_1, best_landing_pad.y_1 + w = best_landing_pad.x_2 - best_landing_pad.x_1 + h = best_landing_pad.y_2 - best_landing_pad.y_1 + + return True, (x, y, w, h) + + +def main() -> int: + """ + Main function. + """ + try: + with CONFIG_FILE_PATH.open("r", encoding="utf8") as file: + try: + config = yaml.safe_load(file) + except yaml.YAMLError as exc: + print(f"Error parsing YAML file: {exc}") + return -1 + except FileNotFoundError: + print(f"File not found: {CONFIG_FILE_PATH}") + return -1 + except IOError as exc: + print(f"Error when opening file: {exc}") + + parser = argparse.ArgumentParser(description="AEAC 2024 Auto-landing script") + parser.add_argument( + "--method", + help="Method for selecting landing pads", + choices=["contour", "yolo"], + default="contour", + ) + args = parser.parse_args() + + dotenv.load_dotenv(".key") + secret_key = os.getenv("KEY") + + cam = cv2.VideoCapture(CAMERA) + + vehicle = mavutil.mavlink_connection("tcp:localhost:14550") + # vehicle = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600) + vehicle.wait_heartbeat() + print( + f"Heartbeat from system (system {vehicle.target_system} component {vehicle.target_component})" + ) + + pos_message = vehicle.mav.command_long_encode( + vehicle.target_system, # Target system ID + vehicle.target_component, # Target component ID + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # ID of command to send + 0, # Confirmation + 33, # param1: Message ID to be streamed + 250000, # param2: Interval in microseconds + 0, # param3 (unused) + 0, # param4 (unused) + 0, # param5 (unused) + 0, # param5 (unused) + 0, # param6 (unused) + ) + + secret_key = bytearray(secret_key, "utf-8") + vehicle.setup_signing(secret_key, True, my_allow_unsigned_callback, int(time.time()), 0) + vehicle.mav.send(pos_message) + + altitude_m = 0 + loop_counter = 0 + last_time = current_milli_time() + last_image_time = current_milli_time() + + while True: + result, image = cam.read() + if not result: + print("ERROR: Could not get image from camera") + continue + + image = cv2.flip(image, 0) + image = cv2.flip(image, 1) + im_h, im_w, _ = image.shape + print("Input image width: " + str(im_w)) + print("Input image height: " + str(im_h)) + try: + altitude_mm = vehicle.messages[ + "GLOBAL_POSITION_INT" + ].relative_alt # Note, you can access message fields as attributes! + altitude_m = max(altitude_mm / 1000, 0.0) + print("Altitude AGL: ", altitude_m) + except (KeyError, AttributeError): + print("No GLOBAL_POSITION_INT message received") + continue + + if args.method == "contour": + bounding_result, bounding_box = detect_landing_pads_contour(image) + elif args.method == "yolo": + bounding_result, bounding_box = detect_landing_pads_yolo(image, config) + + loop_counter += 1 + if current_milli_time() - last_time > 1000: + print("FPS:", loop_counter) + loop_counter = 0 + last_time = current_milli_time() + + if not bounding_result: + # Print plain image + if current_milli_time() - last_image_time > 100: + print("Plain Image Write") + cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", image) + last_image_time = current_milli_time() + continue + + x, y, w, h = bounding_box + rect_image = copy.deepcopy(image) + cv2.rectangle(rect_image, (int(x), int(y)), (int(x + w), int(y + h)), (0, 255, 0), 2) + x_center = x + w / 2 + y_center = y + h / 2 + angle_x = (x_center - im_w / 2) * (FOV_X * (math.pi / 180)) / im_w + angle_y = (y_center - im_h / 2) * (FOV_Y * (math.pi / 180)) / im_h + cv2.circle(rect_image, (int(x_center), int(y_center)), 2, (0, 0, 255), 2) + print("X Angle (rad): ", angle_x) + print("Y Angle (rad): ", angle_y) + target_dist = calc_target_distance(altitude_m, angle_x, angle_y) + vehicle.mav.landing_target_send( + 0, + 0, + mavutil.mavlink.MAV_FRAME_BODY_NED, + angle_x, + angle_y, + target_dist, + 0, + 0, + ) + + if current_milli_time() - last_image_time > 100: + print("Bounding Box Image Write") + cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", rect_image) + last_image_time = current_milli_time() + + return 0 + + +if __name__ == "__main__": + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") + + print("Done!") \ No newline at end of file diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index 8db94d8a..f0b64d42 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -1,296 +1,49 @@ """ -Auto-landing script for AEAC 2024 competition. - -Usage: Either with contour (default) or yolo method. -python blue_only.py --method=... - -note: not sure what to name this file cause it's not a worker.. +Auto-landing worker. """ +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from . import auto_landing +from . import auto_landing_main + +def auto_landing_worker( + FOV_X: float, + FOV_Y: float, + im_h: float, + im_w: float, + x_center: float, + y_center: float, + height: float, + input_queue: queue_proxy_wrapper.QueueProxyWrapper, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, + ) -> None: + + auto_lander = auto_landing.AutoLanding(FOV_X, FOV_Y, im_h, im_w, x_center, y_center, height) + """ + result, lander = cluster_estimation.ClusterEstimation.create( + min_activation_threshold, + min_new_points_to_run, + random_state, + ) -import argparse -import copy -import math -import os -import pathlib -import time -import yaml - -import cv2 -import dotenv -import numpy as np -from pymavlink import mavutil - -import yolo_decision # do i copy the DetectLandingPad class to replace this library? - -import auto_landing - -LOG_DIRECTORY_PATH = pathlib.Path("logs") -SAVE_PREFIX = str(pathlib.Path(LOG_DIRECTORY_PATH, "image_")) - -CAMERA = 0 -# Camera field of view -FOV_X = 62.2 -FOV_Y = 48.8 - -CONTOUR_MINIMUM = 0.8 - -CONFIG_FILE_PATH = pathlib.Path("config.yml") - - -def current_milli_time() -> int: - """ - Returns the current time in milliseconds. - """ - return round(time.time() * 1000) - - -def is_contour_circular(contour: np.ndarray) -> bool: - """ - Checks if the shape is close to circular. - - Return: True is the shape is circular, false if it is not. - """ - perimeter = cv2.arcLength(contour, True) - - # Check if the perimeter is zero - if perimeter == 0.0: - return False - - area = cv2.contourArea(contour) - circularity = 4 * np.pi * (area / (perimeter * perimeter)) - return circularity > CONTOUR_MINIMUM - - -def is_contour_large_enough(contour: np.ndarray, min_diameter: float) -> bool: - """ - Checks if the shape is larger than the provided diameter. - - Return: True if it is, false if it not. - """ - _, radius = cv2.minEnclosingCircle(contour) - diameter = radius * 2 - return diameter >= min_diameter - - -def calc_target_distance(height_agl: float, x_rad: float, y_rad: float) -> float: - """ - Get the horizontal distance. - """ - x_ground_dist_m = math.tan(x_rad) * height_agl - y_ground_dist_m = math.tan(y_rad) * height_agl - ground_hyp = math.sqrt(math.pow(x_ground_dist_m, 2) + math.pow(y_ground_dist_m, 2)) - print("Required horizontal correction (m): ", ground_hyp) - target_to_vehicle_dist = math.sqrt(math.pow(ground_hyp, 2) + math.pow(height_agl, 2)) - print("Distance from vehicle to target (m): ", target_to_vehicle_dist) - return target_to_vehicle_dist - - -# Callback expects `self` as first argument -# pylint: disable-next=unused-argument -def my_allow_unsigned_callback(self: object, message_id: int) -> bool: - """ - Specify which messages to accept. - """ - # Allow radio status messages - return message_id == mavutil.mavlink.MAVLINK_MSG_ID_RADIO_STATUS - - -def detect_landing_pads_contour(image: np.ndarray) -> "tuple[bool, tuple | None]": - """ - Detect landing pads using contours. - - image: Current image frame - """ - kernel = np.ones((2, 2), np.uint8) - - gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) - threshold = 180 - im_bw = cv2.threshold(gray_image, threshold, 255, cv2.THRESH_BINARY)[1] - im_dilation = cv2.dilate(im_bw, kernel, iterations=1) - contours, hierarchy = cv2.findContours(im_dilation, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - - if len(contours) == 0: - return False, None - - contours_with_children = set(i for i, hier in enumerate(hierarchy[0]) if hier[2] != -1) - parent_circular_contours = [ - cnt - for i, cnt in enumerate(contours) - if is_contour_circular(cnt) - and is_contour_large_enough(cnt, 7) - and i in contours_with_children - ] - contour_image = copy.deepcopy(image) - cv2.drawContours(contour_image, parent_circular_contours, -1, (0, 255, 0), 2) - largest_contour = max(parent_circular_contours, key=cv2.contourArea, default=None) - - if not largest_contour: - return False, None - - return True, tuple(cv2.boundingRect(largest_contour)) - - -def detect_landing_pads_yolo(image: np.ndarray, config: dict) -> "tuple[bool, tuple | None]": - """ - Detect landing pads using YOLO model. - - image: Current image frame """ - model_device = config["yolo_detect_target"]["device"] - detect_confidence = config["yolo_detect_target"]["confidence"] - model_path = config["yolo_detect_target"]["model_path"] - - yolo_model = yolo_decision.DetectLandingPad(model_device, detect_confidence, model_path) - - result, detections = yolo_model.get_landing_pads(image) if not result: - return False, None - - best_landing_pad = yolo_model.find_best_pad(detections) - if not best_landing_pad: - return False, None - - x, y = best_landing_pad.x_1, best_landing_pad.y_1 - w = best_landing_pad.x_2 - best_landing_pad.x_1 - h = best_landing_pad.y_2 - best_landing_pad.y_1 - - return True, (x, y, w, h) - - -def main() -> int: - """ - Main function. - """ - try: - with CONFIG_FILE_PATH.open("r", encoding="utf8") as file: - try: - config = yaml.safe_load(file) - except yaml.YAMLError as exc: - print(f"Error parsing YAML file: {exc}") - return -1 - except FileNotFoundError: - print(f"File not found: {CONFIG_FILE_PATH}") - return -1 - except IOError as exc: - print(f"Error when opening file: {exc}") + print("ERROR: Worker failed to create class object") + return - parser = argparse.ArgumentParser(description="AEAC 2024 Auto-landing script") - parser.add_argument( - "--method", - help="Method for selecting landing pads", - choices=["contour", "yolo"], - default="contour", - ) - args = parser.parse_args() - - dotenv.load_dotenv(".key") - secret_key = os.getenv("KEY") - - cam = cv2.VideoCapture(CAMERA) - - vehicle = mavutil.mavlink_connection("tcp:localhost:14550") - # vehicle = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600) - vehicle.wait_heartbeat() - print( - f"Heartbeat from system (system {vehicle.target_system} component {vehicle.target_component})" - ) + # Get Pylance to stop complaining + assert auto_lander is not None - pos_message = vehicle.mav.command_long_encode( - vehicle.target_system, # Target system ID - vehicle.target_component, # Target component ID - mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # ID of command to send - 0, # Confirmation - 33, # param1: Message ID to be streamed - 250000, # param2: Interval in microseconds - 0, # param3 (unused) - 0, # param4 (unused) - 0, # param5 (unused) - 0, # param5 (unused) - 0, # param6 (unused) - ) - - secret_key = bytearray(secret_key, "utf-8") - vehicle.setup_signing(secret_key, True, my_allow_unsigned_callback, int(time.time()), 0) - vehicle.mav.send(pos_message) - - altitude_m = 0 - loop_counter = 0 - last_time = current_milli_time() - last_image_time = current_milli_time() + while not controller.is_exit_requested(): + controller.check_pause() - while True: - result, image = cam.read() - if not result: - print("ERROR: Could not get image from camera") + input_data = input_queue.queue.get() + if input_data is None: continue - image = cv2.flip(image, 0) - image = cv2.flip(image, 1) - im_h, im_w, _ = image.shape - print("Input image width: " + str(im_w)) - print("Input image height: " + str(im_h)) - try: - altitude_mm = vehicle.messages[ - "GLOBAL_POSITION_INT" - ].relative_alt # Note, you can access message fields as attributes! - altitude_m = max(altitude_mm / 1000, 0.0) - print("Altitude AGL: ", altitude_m) - except (KeyError, AttributeError): - print("No GLOBAL_POSITION_INT message received") - continue - - if args.method == "contour": - bounding_result, bounding_box = detect_landing_pads_contour(image) - elif args.method == "yolo": - bounding_result, bounding_box = detect_landing_pads_yolo(image, config) - - loop_counter += 1 - if current_milli_time() - last_time > 1000: - print("FPS:", loop_counter) - loop_counter = 0 - last_time = current_milli_time() - - if not bounding_result: - # Print plain image - if current_milli_time() - last_image_time > 100: - print("Plain Image Write") - cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", image) - last_image_time = current_milli_time() + result, value = auto_lander.run(input_data, False) + if not result: continue - x, y, w, h = bounding_box - rect_image = copy.deepcopy(image) - cv2.rectangle(rect_image, (int(x), int(y)), (int(x + w), int(y + h)), (0, 255, 0), 2) - x_center = x + w / 2 - y_center = y + h / 2 - angle_x = (x_center - im_w / 2) * (FOV_X * (math.pi / 180)) / im_w - angle_y = (y_center - im_h / 2) * (FOV_Y * (math.pi / 180)) / im_h - cv2.circle(rect_image, (int(x_center), int(y_center)), 2, (0, 0, 255), 2) - print("X Angle (rad): ", angle_x) - print("Y Angle (rad): ", angle_y) - target_dist = calc_target_distance(altitude_m, angle_x, angle_y) - vehicle.mav.landing_target_send( - 0, - 0, - mavutil.mavlink.MAV_FRAME_BODY_NED, - angle_x, - angle_y, - target_dist, - 0, - 0, - ) - - if current_milli_time() - last_image_time > 100: - print("Bounding Box Image Write") - cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", rect_image) - last_image_time = current_milli_time() - - return 0 - - -if __name__ == "__main__": - result_main = main() - if result_main < 0: - print(f"ERROR: Status code: {result_main}") - - print("Done!") \ No newline at end of file + output_queue.queue.put(value) \ No newline at end of file From 6fd5f9e84273e934c634545708cea13d5bd04f62 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Tue, 14 Jan 2025 04:09:14 -0500 Subject: [PATCH 07/29] made changes --- modules/auto_landing/auto_landing.py | 34 +-- modules/auto_landing/auto_landing_main.py | 296 -------------------- modules/auto_landing/auto_landing_worker.py | 5 +- 3 files changed, 19 insertions(+), 316 deletions(-) delete mode 100644 modules/auto_landing/auto_landing_main.py diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index a29ec581..1db3f2d4 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -3,53 +3,53 @@ class AutoLanding: """" Auto-landing script. - """ + TODO: make create method + """ def __init__ ( self, FOV_X: float, FOV_Y: float, im_h: float, im_w: float, - x_center: float, - y_center: float, - height: float, ) -> None: """" FOV_X: The horizontal camera field of view in degrees. FOV_Y: The vertical camera field of view in degrees. im_w: Width of image. im_h: Height of image. - x_center: x-coordinate of center of bounding box. - y_center: y-coordinate of center of bounding box. - height: Height above ground level in meters. """ self.FOV_X = FOV_X self.FOV_Y = FOV_Y self.im_h = im_h self.im_w = im_w - self.x_center = x_center - self.y_center = y_center - self.height = height - def run (self, FOV_X, FOV_Y, im_w, im_h, x_center, y_center, height) -> "tuple[float, float, float]": + def run (self, + x_center: float, + y_center: float, + height: float + ) -> "tuple[float, float, float]": """ Calculates the angles in radians of the bounding box based on its center. + x_center: x-coordinate of center of bounding box. + y_center: y-coordinate of center of bounding box. + height: Height above ground level in meters. + Return: Tuple of the x and y angles in radians respectively and the target distance in meters. """ - angle_x = (self.x_center - self.im_w / 2) * (self.FOV_X * (math.pi / 180)) / self.im_w - angle_y = (self.y_center - self.im_h / 2) * (self.FOV_Y * (math.pi / 180)) / self.im_h + angle_x = (x_center - self.im_w / 2) * (self.FOV_X * (math.pi / 180)) / self.im_w + angle_y = (y_center - self.im_h / 2) * (self.FOV_Y * (math.pi / 180)) / self.im_h print("X angle (rad): ", angle_x) print("Y angle (rad): ", angle_y) - x_dist = math.tan(angle_x) * self.height - y_dist = math.tan(angle_y) * self.height - ground_hyp = math.sqrt(math.pow(x_dist, 2) + math.pow(y_dist, 2)) + x_dist = math.tan(angle_x) * height + y_dist = math.tan(angle_y) * height + ground_hyp = (x_dist**2 + y_dist**2)**0.5 print("Required horizontal correction (m): ", ground_hyp) - target_to_vehicle_dist = math.sqrt(math.pow(ground_hyp, 2) + math.pow(self.height, 2)) + target_to_vehicle_dist = (ground_hyp**2 + height**2)**0.5 print("Distance from vehicle to target (m): ", target_to_vehicle_dist) return angle_x, angle_y, target_to_vehicle_dist diff --git a/modules/auto_landing/auto_landing_main.py b/modules/auto_landing/auto_landing_main.py deleted file mode 100644 index cc6e40db..00000000 --- a/modules/auto_landing/auto_landing_main.py +++ /dev/null @@ -1,296 +0,0 @@ -""" -Auto-landing script for AEAC 2024 competition. - -Usage: Either with contour (default) or yolo method. -python blue_only.py --method=... - -note: not sure what to name this file -""" - -import argparse -import copy -import math -import os -import pathlib -import time -import yaml - -import cv2 -import dotenv -import numpy as np -from pymavlink import mavutil - -import yolo_decision - -import auto_landing - -LOG_DIRECTORY_PATH = pathlib.Path("logs") -SAVE_PREFIX = str(pathlib.Path(LOG_DIRECTORY_PATH, "image_")) - -CAMERA = 0 -# Camera field of view -FOV_X = 62.2 -FOV_Y = 48.8 - -CONTOUR_MINIMUM = 0.8 - -CONFIG_FILE_PATH = pathlib.Path("config.yml") - - -def current_milli_time() -> int: - """ - Returns the current time in milliseconds. - """ - return round(time.time() * 1000) - - -def is_contour_circular(contour: np.ndarray) -> bool: - """ - Checks if the shape is close to circular. - - Return: True is the shape is circular, false if it is not. - """ - perimeter = cv2.arcLength(contour, True) - - # Check if the perimeter is zero - if perimeter == 0.0: - return False - - area = cv2.contourArea(contour) - circularity = 4 * np.pi * (area / (perimeter * perimeter)) - return circularity > CONTOUR_MINIMUM - - -def is_contour_large_enough(contour: np.ndarray, min_diameter: float) -> bool: - """ - Checks if the shape is larger than the provided diameter. - - Return: True if it is, false if it not. - """ - _, radius = cv2.minEnclosingCircle(contour) - diameter = radius * 2 - return diameter >= min_diameter - - -def calc_target_distance(height_agl: float, x_rad: float, y_rad: float) -> float: - """ - Get the horizontal distance. - """ - x_ground_dist_m = math.tan(x_rad) * height_agl - y_ground_dist_m = math.tan(y_rad) * height_agl - ground_hyp = math.sqrt(math.pow(x_ground_dist_m, 2) + math.pow(y_ground_dist_m, 2)) - print("Required horizontal correction (m): ", ground_hyp) - target_to_vehicle_dist = math.sqrt(math.pow(ground_hyp, 2) + math.pow(height_agl, 2)) - print("Distance from vehicle to target (m): ", target_to_vehicle_dist) - return target_to_vehicle_dist - - -# Callback expects `self` as first argument -# pylint: disable-next=unused-argument -def my_allow_unsigned_callback(self: object, message_id: int) -> bool: - """ - Specify which messages to accept. - """ - # Allow radio status messages - return message_id == mavutil.mavlink.MAVLINK_MSG_ID_RADIO_STATUS - - -def detect_landing_pads_contour(image: np.ndarray) -> "tuple[bool, tuple | None]": - """ - Detect landing pads using contours. - - image: Current image frame - """ - kernel = np.ones((2, 2), np.uint8) - - gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) - threshold = 180 - im_bw = cv2.threshold(gray_image, threshold, 255, cv2.THRESH_BINARY)[1] - im_dilation = cv2.dilate(im_bw, kernel, iterations=1) - contours, hierarchy = cv2.findContours(im_dilation, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - - if len(contours) == 0: - return False, None - - contours_with_children = set(i for i, hier in enumerate(hierarchy[0]) if hier[2] != -1) - parent_circular_contours = [ - cnt - for i, cnt in enumerate(contours) - if is_contour_circular(cnt) - and is_contour_large_enough(cnt, 7) - and i in contours_with_children - ] - contour_image = copy.deepcopy(image) - cv2.drawContours(contour_image, parent_circular_contours, -1, (0, 255, 0), 2) - largest_contour = max(parent_circular_contours, key=cv2.contourArea, default=None) - - if not largest_contour: - return False, None - - return True, tuple(cv2.boundingRect(largest_contour)) - - -def detect_landing_pads_yolo(image: np.ndarray, config: dict) -> "tuple[bool, tuple | None]": - """ - Detect landing pads using YOLO model. - - image: Current image frame - """ - model_device = config["yolo_detect_target"]["device"] - detect_confidence = config["yolo_detect_target"]["confidence"] - model_path = config["yolo_detect_target"]["model_path"] - - yolo_model = yolo_decision.DetectLandingPad(model_device, detect_confidence, model_path) - - result, detections = yolo_model.get_landing_pads(image) - if not result: - return False, None - - best_landing_pad = yolo_model.find_best_pad(detections) - if not best_landing_pad: - return False, None - - x, y = best_landing_pad.x_1, best_landing_pad.y_1 - w = best_landing_pad.x_2 - best_landing_pad.x_1 - h = best_landing_pad.y_2 - best_landing_pad.y_1 - - return True, (x, y, w, h) - - -def main() -> int: - """ - Main function. - """ - try: - with CONFIG_FILE_PATH.open("r", encoding="utf8") as file: - try: - config = yaml.safe_load(file) - except yaml.YAMLError as exc: - print(f"Error parsing YAML file: {exc}") - return -1 - except FileNotFoundError: - print(f"File not found: {CONFIG_FILE_PATH}") - return -1 - except IOError as exc: - print(f"Error when opening file: {exc}") - - parser = argparse.ArgumentParser(description="AEAC 2024 Auto-landing script") - parser.add_argument( - "--method", - help="Method for selecting landing pads", - choices=["contour", "yolo"], - default="contour", - ) - args = parser.parse_args() - - dotenv.load_dotenv(".key") - secret_key = os.getenv("KEY") - - cam = cv2.VideoCapture(CAMERA) - - vehicle = mavutil.mavlink_connection("tcp:localhost:14550") - # vehicle = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600) - vehicle.wait_heartbeat() - print( - f"Heartbeat from system (system {vehicle.target_system} component {vehicle.target_component})" - ) - - pos_message = vehicle.mav.command_long_encode( - vehicle.target_system, # Target system ID - vehicle.target_component, # Target component ID - mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # ID of command to send - 0, # Confirmation - 33, # param1: Message ID to be streamed - 250000, # param2: Interval in microseconds - 0, # param3 (unused) - 0, # param4 (unused) - 0, # param5 (unused) - 0, # param5 (unused) - 0, # param6 (unused) - ) - - secret_key = bytearray(secret_key, "utf-8") - vehicle.setup_signing(secret_key, True, my_allow_unsigned_callback, int(time.time()), 0) - vehicle.mav.send(pos_message) - - altitude_m = 0 - loop_counter = 0 - last_time = current_milli_time() - last_image_time = current_milli_time() - - while True: - result, image = cam.read() - if not result: - print("ERROR: Could not get image from camera") - continue - - image = cv2.flip(image, 0) - image = cv2.flip(image, 1) - im_h, im_w, _ = image.shape - print("Input image width: " + str(im_w)) - print("Input image height: " + str(im_h)) - try: - altitude_mm = vehicle.messages[ - "GLOBAL_POSITION_INT" - ].relative_alt # Note, you can access message fields as attributes! - altitude_m = max(altitude_mm / 1000, 0.0) - print("Altitude AGL: ", altitude_m) - except (KeyError, AttributeError): - print("No GLOBAL_POSITION_INT message received") - continue - - if args.method == "contour": - bounding_result, bounding_box = detect_landing_pads_contour(image) - elif args.method == "yolo": - bounding_result, bounding_box = detect_landing_pads_yolo(image, config) - - loop_counter += 1 - if current_milli_time() - last_time > 1000: - print("FPS:", loop_counter) - loop_counter = 0 - last_time = current_milli_time() - - if not bounding_result: - # Print plain image - if current_milli_time() - last_image_time > 100: - print("Plain Image Write") - cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", image) - last_image_time = current_milli_time() - continue - - x, y, w, h = bounding_box - rect_image = copy.deepcopy(image) - cv2.rectangle(rect_image, (int(x), int(y)), (int(x + w), int(y + h)), (0, 255, 0), 2) - x_center = x + w / 2 - y_center = y + h / 2 - angle_x = (x_center - im_w / 2) * (FOV_X * (math.pi / 180)) / im_w - angle_y = (y_center - im_h / 2) * (FOV_Y * (math.pi / 180)) / im_h - cv2.circle(rect_image, (int(x_center), int(y_center)), 2, (0, 0, 255), 2) - print("X Angle (rad): ", angle_x) - print("Y Angle (rad): ", angle_y) - target_dist = calc_target_distance(altitude_m, angle_x, angle_y) - vehicle.mav.landing_target_send( - 0, - 0, - mavutil.mavlink.MAV_FRAME_BODY_NED, - angle_x, - angle_y, - target_dist, - 0, - 0, - ) - - if current_milli_time() - last_image_time > 100: - print("Bounding Box Image Write") - cv2.imwrite(SAVE_PREFIX + "_" + str(time.time()) + ".png", rect_image) - last_image_time = current_milli_time() - - return 0 - - -if __name__ == "__main__": - result_main = main() - if result_main < 0: - print(f"ERROR: Status code: {result_main}") - - print("Done!") \ No newline at end of file diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index f0b64d42..d821fef4 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -4,7 +4,6 @@ from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import auto_landing -from . import auto_landing_main def auto_landing_worker( FOV_X: float, @@ -19,14 +18,14 @@ def auto_landing_worker( controller: worker_controller.WorkerController, ) -> None: - auto_lander = auto_landing.AutoLanding(FOV_X, FOV_Y, im_h, im_w, x_center, y_center, height) + auto_lander = auto_landing.AutoLanding(FOV_X, FOV_Y, im_h, im_w) """ result, lander = cluster_estimation.ClusterEstimation.create( min_activation_threshold, min_new_points_to_run, random_state, ) - + currently don't have a create function """ if not result: print("ERROR: Worker failed to create class object") From 8b1c5360134c3d7784a6a48eaaa749ef197c7dff Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Tue, 14 Jan 2025 04:33:13 -0500 Subject: [PATCH 08/29] issues commiting, now fixed --- modules/auto_landing/auto_landing_worker.py | 3 --- modules/common | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index d821fef4..02258ef6 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -10,9 +10,6 @@ def auto_landing_worker( FOV_Y: float, im_h: float, im_w: float, - x_center: float, - y_center: float, - height: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, diff --git a/modules/common b/modules/common index a256a497..c7ab98a7 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit a256a49778d1154e03683c3b5e2fe6cb215d00e7 +Subproject commit c7ab98a75be0f78c2c17084d30c7fce5708897ff From 876b7146ff43342081afd463bde423e324c59f72 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Wed, 22 Jan 2025 14:31:36 -0500 Subject: [PATCH 09/29] made changes --- modules/auto_landing/auto_landing.py | 69 ++++++++++++++------- modules/auto_landing/auto_landing_worker.py | 33 ++++++---- 2 files changed, 66 insertions(+), 36 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 1db3f2d4..89710d91 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -1,56 +1,77 @@ import math +from ..common.modules.logger import logger +from .. import detections_and_time + class AutoLanding: """" Auto-landing script. - - TODO: make create method """ - def __init__ ( - self, - FOV_X: float, - FOV_Y: float, - im_h: float, - im_w: float, - ) -> None: + __create_key = object () + + @classmethod + def create ( + cls, + FOV_X: float, + FOV_Y: float, + local_logger: logger.Logger, + ) -> "tuple [bool, AutoLanding | None ]": """" FOV_X: The horizontal camera field of view in degrees. FOV_Y: The vertical camera field of view in degrees. - im_w: Width of image. - im_h: Height of image. """ + + return True, AutoLanding( + cls.__create_key, FOV_X, FOV_Y, local_logger + ) + + + def __init__ ( + self, + class_private_create_key: object, + FOV_X: float, + FOV_Y: float, + local_logger: logger.Logger + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is AutoLanding.__create_key, "Use create() method" + self.FOV_X = FOV_X self.FOV_Y = FOV_Y - self.im_h = im_h - self.im_w = im_w + self.__logger = local_logger def run (self, - x_center: float, - y_center: float, height: float - ) -> "tuple[float, float, float]": + ) -> "tuple[float, float, float]": """ Calculates the angles in radians of the bounding box based on its center. - x_center: x-coordinate of center of bounding box. - y_center: y-coordinate of center of bounding box. height: Height above ground level in meters. Return: Tuple of the x and y angles in radians respectively and the target distance in meters. """ - angle_x = (x_center - self.im_w / 2) * (self.FOV_X * (math.pi / 180)) / self.im_w - angle_y = (y_center - self.im_h / 2) * (self.FOV_Y * (math.pi / 180)) / self.im_h + x_center, y_center = detections_and_time.Detection.get_centre() + + top_left, top_right, _, bottom_right = detections_and_time.Detection.get_corners() + + im_w = abs(top_right - top_left) + im_h = abs(top_right - bottom_right) + + angle_x = (x_center - im_w / 2) * (self.FOV_X * (math.pi / 180)) / im_w + angle_y = (y_center - im_h / 2) * (self.FOV_Y * (math.pi / 180)) / im_h - print("X angle (rad): ", angle_x) - print("Y angle (rad): ", angle_y) + self.__logger.info(f"X angle (rad): {angle_x}", True) + self.__logger.info(f"Y angle (rad): {angle_y}", True) x_dist = math.tan(angle_x) * height y_dist = math.tan(angle_y) * height ground_hyp = (x_dist**2 + y_dist**2)**0.5 - print("Required horizontal correction (m): ", ground_hyp) + self.__logger.info(f"Required horizontal correction (m): {ground_hyp}", True) target_to_vehicle_dist = (ground_hyp**2 + height**2)**0.5 - print("Distance from vehicle to target (m): ", target_to_vehicle_dist) + self.__logger.info(f"Distance from vehicle to target (m): {target_to_vehicle_dist}", True) return angle_x, angle_y, target_to_vehicle_dist diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index 02258ef6..16471fa6 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -1,31 +1,40 @@ """ Auto-landing worker. """ +import pathlib +import os + from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import auto_landing +from ..common.modules.logger import logger def auto_landing_worker( FOV_X: float, FOV_Y: float, - im_h: float, - im_w: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: - auto_lander = auto_landing.AutoLanding(FOV_X, FOV_Y, im_h, im_w) - """ - result, lander = cluster_estimation.ClusterEstimation.create( - min_activation_threshold, - min_new_points_to_run, - random_state, + worker_name = pathlib.Path(__file__).stem + process_id = os.getpid() + result, local_logger = logger.Logger.create(f"{worker_name}_{process_id}", True) + if not result: + print("ERROR: Worker failed to create logger") + return + + # Get Pylance to stop complaining + assert local_logger is not None + + local_logger.info("Logger initialized", True) + + result, auto_lander = auto_landing.AutoLanding.create( + FOV_X, FOV_Y, local_logger ) - currently don't have a create function - """ + if not result: - print("ERROR: Worker failed to create class object") + local_logger.error("Worker failed to create class object", True) return # Get Pylance to stop complaining @@ -38,7 +47,7 @@ def auto_landing_worker( if input_data is None: continue - result, value = auto_lander.run(input_data, False) + result, value = auto_lander.run(input_data) if not result: continue From 0b07de101f9988225d0de7251f204c95e4889123 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Wed, 22 Jan 2025 14:38:16 -0500 Subject: [PATCH 10/29] linter issues fixed --- modules/auto_landing/auto_landing.py | 46 +++++++++------------ modules/auto_landing/auto_landing_worker.py | 14 +++---- 2 files changed, 27 insertions(+), 33 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 89710d91..fb908b91 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -3,37 +3,36 @@ from ..common.modules.logger import logger from .. import detections_and_time + class AutoLanding: - """" - Auto-landing script. + """ " + Auto-landing script. """ - __create_key = object () + + __create_key = object() @classmethod - def create ( + def create( cls, FOV_X: float, FOV_Y: float, local_logger: logger.Logger, ) -> "tuple [bool, AutoLanding | None ]": - """" + """ " FOV_X: The horizontal camera field of view in degrees. FOV_Y: The vertical camera field of view in degrees. """ - - return True, AutoLanding( - cls.__create_key, FOV_X, FOV_Y, local_logger - ) - - - def __init__ ( - self, - class_private_create_key: object, - FOV_X: float, - FOV_Y: float, - local_logger: logger.Logger - ) -> None: + + return True, AutoLanding(cls.__create_key, FOV_X, FOV_Y, local_logger) + + def __init__( + self, + class_private_create_key: object, + FOV_X: float, + FOV_Y: float, + local_logger: logger.Logger, + ) -> None: """ Private constructor, use create() method. """ @@ -43,9 +42,7 @@ def __init__ ( self.FOV_Y = FOV_Y self.__logger = local_logger - def run (self, - height: float - ) -> "tuple[float, float, float]": + def run(self, height: float) -> "tuple[float, float, float]": """ Calculates the angles in radians of the bounding box based on its center. @@ -68,12 +65,9 @@ def run (self, x_dist = math.tan(angle_x) * height y_dist = math.tan(angle_y) * height - ground_hyp = (x_dist**2 + y_dist**2)**0.5 + ground_hyp = (x_dist**2 + y_dist**2) ** 0.5 self.__logger.info(f"Required horizontal correction (m): {ground_hyp}", True) - target_to_vehicle_dist = (ground_hyp**2 + height**2)**0.5 + target_to_vehicle_dist = (ground_hyp**2 + height**2) ** 0.5 self.__logger.info(f"Distance from vehicle to target (m): {target_to_vehicle_dist}", True) return angle_x, angle_y, target_to_vehicle_dist - - - \ No newline at end of file diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index 16471fa6..68a162e8 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -1,6 +1,7 @@ """ Auto-landing worker. """ + import pathlib import os @@ -9,13 +10,14 @@ from . import auto_landing from ..common.modules.logger import logger + def auto_landing_worker( FOV_X: float, FOV_Y: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, - ) -> None: +) -> None: worker_name = pathlib.Path(__file__).stem process_id = os.getpid() @@ -28,11 +30,9 @@ def auto_landing_worker( assert local_logger is not None local_logger.info("Logger initialized", True) - - result, auto_lander = auto_landing.AutoLanding.create( - FOV_X, FOV_Y, local_logger - ) - + + result, auto_lander = auto_landing.AutoLanding.create(FOV_X, FOV_Y, local_logger) + if not result: local_logger.error("Worker failed to create class object", True) return @@ -51,4 +51,4 @@ def auto_landing_worker( if not result: continue - output_queue.queue.put(value) \ No newline at end of file + output_queue.queue.put(value) From 38aad9b0b02f29a0bde0fc1770b1dd843d7fe508 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Thu, 23 Jan 2025 13:27:32 -0500 Subject: [PATCH 11/29] fixing linter issues --- modules/auto_landing/auto_landing.py | 25 ++++++++++----------- modules/auto_landing/auto_landing_worker.py | 6 ++--- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index fb908b91..fd95d5d3 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -14,23 +14,22 @@ class AutoLanding: @classmethod def create( cls, - FOV_X: float, - FOV_Y: float, + fov_x: float, + fov_y: float, local_logger: logger.Logger, ) -> "tuple [bool, AutoLanding | None ]": - """ " - FOV_X: The horizontal camera field of view in degrees. - FOV_Y: The vertical camera field of view in degrees. - + """ + fov_x: The horizontal camera field of view in degrees. + fov_y: The vertical camera field of view in degrees. """ - return True, AutoLanding(cls.__create_key, FOV_X, FOV_Y, local_logger) + return True, AutoLanding(cls.__create_key, fov_x, fov_y, local_logger) def __init__( self, class_private_create_key: object, - FOV_X: float, - FOV_Y: float, + fov_x: float, + fov_y: float, local_logger: logger.Logger, ) -> None: """ @@ -38,8 +37,8 @@ def __init__( """ assert class_private_create_key is AutoLanding.__create_key, "Use create() method" - self.FOV_X = FOV_X - self.FOV_Y = FOV_Y + self.fov_x = fov_x + self.fov_y = fov_y self.__logger = local_logger def run(self, height: float) -> "tuple[float, float, float]": @@ -57,8 +56,8 @@ def run(self, height: float) -> "tuple[float, float, float]": im_w = abs(top_right - top_left) im_h = abs(top_right - bottom_right) - angle_x = (x_center - im_w / 2) * (self.FOV_X * (math.pi / 180)) / im_w - angle_y = (y_center - im_h / 2) * (self.FOV_Y * (math.pi / 180)) / im_h + angle_x = (x_center - im_w / 2) * (self.fov_x * (math.pi / 180)) / im_w + angle_y = (y_center - im_h / 2) * (self.fov_y * (math.pi / 180)) / im_h self.__logger.info(f"X angle (rad): {angle_x}", True) self.__logger.info(f"Y angle (rad): {angle_y}", True) diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index 68a162e8..b61f7778 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -12,8 +12,8 @@ def auto_landing_worker( - FOV_X: float, - FOV_Y: float, + fov_x: float, + fov_y: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, @@ -31,7 +31,7 @@ def auto_landing_worker( local_logger.info("Logger initialized", True) - result, auto_lander = auto_landing.AutoLanding.create(FOV_X, FOV_Y, local_logger) + result, auto_lander = auto_landing.AutoLanding.create(fov_x, fov_y, local_logger) if not result: local_logger.error("Worker failed to create class object", True) From 1c5b247210e7d825da08965367cce0187621b8c3 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Thu, 23 Jan 2025 14:49:46 -0500 Subject: [PATCH 12/29] fixing code and linter issues --- modules/auto_landing/auto_landing.py | 27 ++++++++++++--------- modules/auto_landing/auto_landing_worker.py | 10 +++++++- 2 files changed, 25 insertions(+), 12 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index fd95d5d3..f46a5edc 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -1,3 +1,7 @@ +""" +Auto-landing script. +""" + import math from ..common.modules.logger import logger @@ -5,9 +9,6 @@ class AutoLanding: - """ " - Auto-landing script. - """ __create_key = object() @@ -16,20 +17,26 @@ def create( cls, fov_x: float, fov_y: float, + im_h: float, + im_w: float, local_logger: logger.Logger, ) -> "tuple [bool, AutoLanding | None ]": """ fov_x: The horizontal camera field of view in degrees. fov_y: The vertical camera field of view in degrees. + im_w: Width of image. + im_h: Height of image. """ - return True, AutoLanding(cls.__create_key, fov_x, fov_y, local_logger) + return True, AutoLanding(cls.__create_key, fov_x, fov_y, im_w, im_h, local_logger) def __init__( self, class_private_create_key: object, fov_x: float, fov_y: float, + im_h: float, + im_w: float, local_logger: logger.Logger, ) -> None: """ @@ -39,6 +46,8 @@ def __init__( self.fov_x = fov_x self.fov_y = fov_y + self.im_h = im_h + self.im_w = im_w self.__logger = local_logger def run(self, height: float) -> "tuple[float, float, float]": @@ -49,15 +58,11 @@ def run(self, height: float) -> "tuple[float, float, float]": Return: Tuple of the x and y angles in radians respectively and the target distance in meters. """ - x_center, y_center = detections_and_time.Detection.get_centre() - top_left, top_right, _, bottom_right = detections_and_time.Detection.get_corners() - - im_w = abs(top_right - top_left) - im_h = abs(top_right - bottom_right) + x_center, y_center = detections_and_time.Detection.get_centre() - angle_x = (x_center - im_w / 2) * (self.fov_x * (math.pi / 180)) / im_w - angle_y = (y_center - im_h / 2) * (self.fov_y * (math.pi / 180)) / im_h + angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w + angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h self.__logger.info(f"X angle (rad): {angle_x}", True) self.__logger.info(f"Y angle (rad): {angle_y}", True) diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index b61f7778..ae3ec794 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -14,10 +14,18 @@ def auto_landing_worker( fov_x: float, fov_y: float, + im_h: float, + im_w: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: + """ + Worker process. + + input_queue and output_queue are data queues. + controller is how the main process communicates to this worker process. + """ worker_name = pathlib.Path(__file__).stem process_id = os.getpid() @@ -31,7 +39,7 @@ def auto_landing_worker( local_logger.info("Logger initialized", True) - result, auto_lander = auto_landing.AutoLanding.create(fov_x, fov_y, local_logger) + result, auto_lander = auto_landing.AutoLanding.create(fov_x, fov_y, im_h, im_w, local_logger) if not result: local_logger.error("Worker failed to create class object", True) From 398edaa1dd065ea844113d6fe7b810c1cf0f1c4a Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Thu, 23 Jan 2025 15:38:52 -0500 Subject: [PATCH 13/29] continuation of debugging --- modules/auto_landing/auto_landing.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index f46a5edc..a5a4643d 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -9,6 +9,9 @@ class AutoLanding: + """ + Auto-landing script. + """ __create_key = object() @@ -50,16 +53,19 @@ def __init__( self.im_w = im_w self.__logger = local_logger - def run(self, height: float) -> "tuple[float, float, float]": + def run( + self, height: float, center: detections_and_time.Detection + ) -> "tuple[float, float, float]": """ Calculates the angles in radians of the bounding box based on its center. height: Height above ground level in meters. + center: The xy coordinates of the center of the bounding box. Return: Tuple of the x and y angles in radians respectively and the target distance in meters. """ - x_center, y_center = detections_and_time.Detection.get_centre() + x_center, y_center = center.get_centre() angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h From dc81d8e0b473d35fe1fa519f283f6492b45c4f83 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Thu, 23 Jan 2025 16:31:33 -0500 Subject: [PATCH 14/29] fixing linter issues --- modules/auto_landing/auto_landing.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index a5a4643d..366dead2 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -30,7 +30,6 @@ def create( im_w: Width of image. im_h: Height of image. """ - return True, AutoLanding(cls.__create_key, fov_x, fov_y, im_w, im_h, local_logger) def __init__( @@ -40,6 +39,7 @@ def __init__( fov_y: float, im_h: float, im_w: float, + height_agl: float, local_logger: logger.Logger, ) -> None: """ @@ -51,15 +51,15 @@ def __init__( self.fov_y = fov_y self.im_h = im_h self.im_w = im_w + self.height_agl = height_agl self.__logger = local_logger def run( - self, height: float, center: detections_and_time.Detection - ) -> "tuple[float, float, float]": + self, center: detections_and_time.Detection + ) -> "tuple[bool, tuple[float, float, float]]": """ Calculates the angles in radians of the bounding box based on its center. - height: Height above ground level in meters. center: The xy coordinates of the center of the bounding box. Return: Tuple of the x and y angles in radians respectively and the target distance in meters. @@ -73,11 +73,11 @@ def run( self.__logger.info(f"X angle (rad): {angle_x}", True) self.__logger.info(f"Y angle (rad): {angle_y}", True) - x_dist = math.tan(angle_x) * height - y_dist = math.tan(angle_y) * height + x_dist = math.tan(angle_x) * self.height_agl + y_dist = math.tan(angle_y) * self.height_agl ground_hyp = (x_dist**2 + y_dist**2) ** 0.5 self.__logger.info(f"Required horizontal correction (m): {ground_hyp}", True) - target_to_vehicle_dist = (ground_hyp**2 + height**2) ** 0.5 + target_to_vehicle_dist = (ground_hyp**2 + self.height_agl**2) ** 0.5 self.__logger.info(f"Distance from vehicle to target (m): {target_to_vehicle_dist}", True) - return angle_x, angle_y, target_to_vehicle_dist + return True, (angle_x, angle_y, target_to_vehicle_dist) From af13347524a2bf8d088a3d60465ecb6a65b5f924 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Thu, 23 Jan 2025 23:34:03 -0500 Subject: [PATCH 15/29] fixing logger initialization --- modules/auto_landing/auto_landing.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 366dead2..a12fa993 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -30,6 +30,7 @@ def create( im_w: Width of image. im_h: Height of image. """ + local_logger.info("", True) return True, AutoLanding(cls.__create_key, fov_x, fov_y, im_w, im_h, local_logger) def __init__( @@ -55,17 +56,17 @@ def __init__( self.__logger = local_logger def run( - self, center: detections_and_time.Detection + self, bounding_box: detections_and_time.Detection ) -> "tuple[bool, tuple[float, float, float]]": """ Calculates the angles in radians of the bounding box based on its center. - center: The xy coordinates of the center of the bounding box. + bounding_box: A detections and time object. Return: Tuple of the x and y angles in radians respectively and the target distance in meters. """ - x_center, y_center = center.get_centre() + x_center, y_center = bounding_box.get_centre() angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h From 6f4c8296785d6b72fb3dfca5f14fffa4c74dc503 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Fri, 24 Jan 2025 03:05:47 -0500 Subject: [PATCH 16/29] fixing last linter issues --- modules/auto_landing/auto_landing.py | 21 +++++++++++++++++++-- modules/auto_landing/auto_landing_worker.py | 5 ++++- 2 files changed, 23 insertions(+), 3 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index a12fa993..46d89a6c 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -7,6 +7,8 @@ from ..common.modules.logger import logger from .. import detections_and_time +from pymavlink import mavutil + class AutoLanding: """ @@ -29,9 +31,24 @@ def create( fov_y: The vertical camera field of view in degrees. im_w: Width of image. im_h: Height of image. + height_agl: Height above ground level in meters. + + Returns an AutoLanding object. """ - local_logger.info("", True) - return True, AutoLanding(cls.__create_key, fov_x, fov_y, im_w, im_h, local_logger) + vehicle = mavutil.mavlink_connection("tcp:localhost:14550") + try: + height_agl_mm = vehicle.messages[ + "GLOBAL_POSITION_INT" + ].relative_alt # copied from blue_only.py + height_agl = max(height_agl_mm / 1000, 0.0) + local_logger.info(f"Altitude AGL: {height_agl} ", True) + except (KeyError, AttributeError): + local_logger.error("No GLOBAL_POSITION_INT message received") + return False, None + + return True, AutoLanding( + cls.__create_key, fov_x, fov_y, im_h, im_w, height_agl, local_logger + ) def __init__( self, diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index ae3ec794..70aa36f6 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -16,6 +16,7 @@ def auto_landing_worker( fov_y: float, im_h: float, im_w: float, + height_agl: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, @@ -39,7 +40,9 @@ def auto_landing_worker( local_logger.info("Logger initialized", True) - result, auto_lander = auto_landing.AutoLanding.create(fov_x, fov_y, im_h, im_w, local_logger) + result, auto_lander = auto_landing.AutoLanding.create( + fov_x, fov_y, im_h, im_w, height_agl, local_logger + ) if not result: local_logger.error("Worker failed to create class object", True) From 224a7dc57735ac2f7735dc005565607d8bfb163d Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Fri, 24 Jan 2025 03:18:54 -0500 Subject: [PATCH 17/29] last few linter issues --- modules/auto_landing/auto_landing.py | 3 +-- modules/auto_landing/auto_landing_worker.py | 4 +--- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 46d89a6c..32dd6d25 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -4,11 +4,10 @@ import math +from pymavlink import mavutil from ..common.modules.logger import logger from .. import detections_and_time -from pymavlink import mavutil - class AutoLanding: """ diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index 70aa36f6..a9093b0b 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -40,9 +40,7 @@ def auto_landing_worker( local_logger.info("Logger initialized", True) - result, auto_lander = auto_landing.AutoLanding.create( - fov_x, fov_y, im_h, im_w, height_agl, local_logger - ) + result, auto_lander = auto_landing.AutoLanding.create(fov_x, fov_y, im_h, im_w, local_logger) if not result: local_logger.error("Worker failed to create class object", True) From c0af0dc6f3b192c2b9e8aa79da6b8c90f7799e2d Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Fri, 24 Jan 2025 03:27:16 -0500 Subject: [PATCH 18/29] last few changes --- modules/auto_landing/auto_landing_worker.py | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index a9093b0b..ae3ec794 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -16,7 +16,6 @@ def auto_landing_worker( fov_y: float, im_h: float, im_w: float, - height_agl: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, From fb1e009adeffde78c5db26fd15110144e565a1c2 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sat, 25 Jan 2025 02:25:09 -0500 Subject: [PATCH 19/29] updated changes --- modules/auto_landing/auto_landing.py | 51 ++++++++++----------- modules/auto_landing/auto_landing_worker.py | 7 ++- 2 files changed, 28 insertions(+), 30 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 32dd6d25..8b02f6d8 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -1,17 +1,20 @@ """ -Auto-landing script. +Auto-landing script that calculates the necessary parameters +for use with LANDING_TARGET MAVLink command. """ import math +import time -from pymavlink import mavutil -from ..common.modules.logger import logger from .. import detections_and_time +from ..common.modules.logger import logger +from .. import merged_odometry_detections class AutoLanding: """ - Auto-landing script. + Auto-landing script that calculates the necessary parameters + for use with LANDING_TARGET MAVLink command. """ __create_key = object() @@ -23,6 +26,7 @@ def create( fov_y: float, im_h: float, im_w: float, + period: float, local_logger: logger.Logger, ) -> "tuple [bool, AutoLanding | None ]": """ @@ -30,24 +34,11 @@ def create( fov_y: The vertical camera field of view in degrees. im_w: Width of image. im_h: Height of image. - height_agl: Height above ground level in meters. Returns an AutoLanding object. """ - vehicle = mavutil.mavlink_connection("tcp:localhost:14550") - try: - height_agl_mm = vehicle.messages[ - "GLOBAL_POSITION_INT" - ].relative_alt # copied from blue_only.py - height_agl = max(height_agl_mm / 1000, 0.0) - local_logger.info(f"Altitude AGL: {height_agl} ", True) - except (KeyError, AttributeError): - local_logger.error("No GLOBAL_POSITION_INT message received") - return False, None - - return True, AutoLanding( - cls.__create_key, fov_x, fov_y, im_h, im_w, height_agl, local_logger - ) + + return True, AutoLanding(cls.__create_key, fov_x, fov_y, im_h, im_w, period, local_logger) def __init__( self, @@ -56,7 +47,7 @@ def __init__( fov_y: float, im_h: float, im_w: float, - height_agl: float, + period: float, local_logger: logger.Logger, ) -> None: """ @@ -68,7 +59,7 @@ def __init__( self.fov_y = fov_y self.im_h = im_h self.im_w = im_w - self.height_agl = height_agl + self.period = period self.__logger = local_logger def run( @@ -87,14 +78,18 @@ def run( angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h - self.__logger.info(f"X angle (rad): {angle_x}", True) - self.__logger.info(f"Y angle (rad): {angle_y}", True) + height_agl = 0 - x_dist = math.tan(angle_x) * self.height_agl - y_dist = math.tan(angle_y) * self.height_agl + x_dist = math.tan(angle_x) * height_agl + y_dist = math.tan(angle_y) * height_agl ground_hyp = (x_dist**2 + y_dist**2) ** 0.5 - self.__logger.info(f"Required horizontal correction (m): {ground_hyp}", True) - target_to_vehicle_dist = (ground_hyp**2 + self.height_agl**2) ** 0.5 - self.__logger.info(f"Distance from vehicle to target (m): {target_to_vehicle_dist}", True) + target_to_vehicle_dist = (ground_hyp**2 + height_agl**2) ** 0.5 + + self.__logger.info( + f"X angle: {angle_x} Y angle: {angle_y}\nRequired horizontal correction: {ground_hyp} Distance from vehicle to target: {target_to_vehicle_dist}", + True, + ) + + time.sleep(self.period) return True, (angle_x, angle_y, target_to_vehicle_dist) diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index ae3ec794..8f5e646f 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -2,8 +2,8 @@ Auto-landing worker. """ -import pathlib import os +import pathlib from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller @@ -16,6 +16,7 @@ def auto_landing_worker( fov_y: float, im_h: float, im_w: float, + period: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, @@ -39,7 +40,9 @@ def auto_landing_worker( local_logger.info("Logger initialized", True) - result, auto_lander = auto_landing.AutoLanding.create(fov_x, fov_y, im_h, im_w, local_logger) + result, auto_lander = auto_landing.AutoLanding.create( + fov_x, fov_y, im_h, im_w, period, local_logger + ) if not result: local_logger.error("Worker failed to create class object", True) From 44656adda4493a8a0a670bf145d794209efef058 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sat, 25 Jan 2025 02:29:17 -0500 Subject: [PATCH 20/29] fixing small linter issue --- modules/auto_landing/auto_landing.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 8b02f6d8..e01fec27 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -8,7 +8,8 @@ from .. import detections_and_time from ..common.modules.logger import logger -from .. import merged_odometry_detections + +# from .. import merged_odometry_detections class AutoLanding: From 619f8d74b4c69b606afead92ff9d2bc386d4e8c6 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Wed, 29 Jan 2025 23:24:19 -0500 Subject: [PATCH 21/29] fixed run() --- modules/auto_landing/auto_landing.py | 41 ++++++++++++++-------------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index e01fec27..12a5aeb5 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -6,10 +6,9 @@ import math import time -from .. import detections_and_time -from ..common.modules.logger import logger -# from .. import merged_odometry_detections +from ..common.modules.logger import logger +from .. import merged_odometry_detections class AutoLanding: @@ -35,6 +34,7 @@ def create( fov_y: The vertical camera field of view in degrees. im_w: Width of image. im_h: Height of image. + period: Wait time in seconds. Returns an AutoLanding object. """ @@ -64,33 +64,34 @@ def __init__( self.__logger = local_logger def run( - self, bounding_box: detections_and_time.Detection + self, input: merged_odometry_detections.MergedOdometryDetections ) -> "tuple[bool, tuple[float, float, float]]": """ - Calculates the angles in radians of the bounding box based on its center. + Calculates the x and y angles in radians of the bounding box based on its center. - bounding_box: A detections and time object. + input: A merged odometry dectections object. Return: Tuple of the x and y angles in radians respectively and the target distance in meters. """ - x_center, y_center = bounding_box.get_centre() + for bounding_box in input.detections: + x_center, y_center = bounding_box.get_centre() - angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w - angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h + angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w + angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h - height_agl = 0 + height_agl = input.odometry_local.position.down * -1 - x_dist = math.tan(angle_x) * height_agl - y_dist = math.tan(angle_y) * height_agl - ground_hyp = (x_dist**2 + y_dist**2) ** 0.5 - target_to_vehicle_dist = (ground_hyp**2 + height_agl**2) ** 0.5 + x_dist = math.tan(angle_x) * height_agl + y_dist = math.tan(angle_y) * height_agl + ground_hyp = (x_dist**2 + y_dist**2) ** 0.5 + target_to_vehicle_dist = (ground_hyp**2 + height_agl**2) ** 0.5 - self.__logger.info( - f"X angle: {angle_x} Y angle: {angle_y}\nRequired horizontal correction: {ground_hyp} Distance from vehicle to target: {target_to_vehicle_dist}", - True, - ) + self.__logger.info( + f"X angle: {angle_x} Y angle: {angle_y}\nRequired horizontal correction: {ground_hyp} Distance from vehicle to target: {target_to_vehicle_dist}", + True, + ) - time.sleep(self.period) + time.sleep(self.period) - return True, (angle_x, angle_y, target_to_vehicle_dist) + return True, (angle_x, angle_y, target_to_vehicle_dist) From fbfcc79ebd549090d20dad83fed8d3e4352f52c5 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Wed, 29 Jan 2025 23:35:56 -0500 Subject: [PATCH 22/29] pulling new commit changes in common --- modules/common | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/common b/modules/common index c7ab98a7..9acf88b4 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit c7ab98a75be0f78c2c17084d30c7fce5708897ff +Subproject commit 9acf88b42dfdb145e7eabb1b09a55df102ee00ad From 3bca007aa11ba957c4809f716cbf913308e4df08 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Wed, 29 Jan 2025 23:53:39 -0500 Subject: [PATCH 23/29] fixing linter error --- modules/auto_landing/auto_landing.py | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 12a5aeb5..56665bfa 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -64,23 +64,28 @@ def __init__( self.__logger = local_logger def run( - self, input: merged_odometry_detections.MergedOdometryDetections - ) -> "tuple[bool, tuple[float, float, float]]": + self, odometry_detections: merged_odometry_detections.MergedOdometryDetections + ) -> "tuple[bool, list[tuple[float, float, float]]]": """ Calculates the x and y angles in radians of the bounding box based on its center. - input: A merged odometry dectections object. + odometry_detections: A merged odometry dectections object. - Return: Tuple of the x and y angles in radians respectively and the target distance in meters. + Return: A list of tuples containing the x and y angles in radians + respectively, and the target distance in meters. + + ex. [(angle_x_1, angle_y_1, target_dist_1), (angle_x_2, angle_y_2, target_dist_2)] """ - for bounding_box in input.detections: + landing_commands = [] + + for bounding_box in odometry_detections.detections: x_center, y_center = bounding_box.get_centre() angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h - height_agl = input.odometry_local.position.down * -1 + height_agl = odometry_detections.odometry_local.position.down * -1 x_dist = math.tan(angle_x) * height_agl y_dist = math.tan(angle_y) * height_agl @@ -93,5 +98,6 @@ def run( ) time.sleep(self.period) + landing_commands.append((angle_x, angle_y, target_to_vehicle_dist)) - return True, (angle_x, angle_y, target_to_vehicle_dist) + return True, landing_commands From 3ff80988666687260e0e5a2ebd06e4807a34abd8 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Thu, 30 Jan 2025 21:57:24 -0500 Subject: [PATCH 24/29] using first bbox --- modules/auto_landing/auto_landing.py | 40 +++++++++++----------------- 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 56665bfa..495247e8 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -65,39 +65,31 @@ def __init__( def run( self, odometry_detections: merged_odometry_detections.MergedOdometryDetections - ) -> "tuple[bool, list[tuple[float, float, float]]]": + ) -> "tuple[bool, tuple[float, float, float]]": """ Calculates the x and y angles in radians of the bounding box based on its center. odometry_detections: A merged odometry dectections object. - Return: A list of tuples containing the x and y angles in radians - respectively, and the target distance in meters. - - ex. [(angle_x_1, angle_y_1, target_dist_1), (angle_x_2, angle_y_2, target_dist_2)] + Return: Tuple containing the x and y angles in radians respectively, and the target distance in meters. """ - landing_commands = [] - - for bounding_box in odometry_detections.detections: - x_center, y_center = bounding_box.get_centre() - - angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w - angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h + x_center, y_center = odometry_detections.detections[0].get_centre() - height_agl = odometry_detections.odometry_local.position.down * -1 + angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w + angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h - x_dist = math.tan(angle_x) * height_agl - y_dist = math.tan(angle_y) * height_agl - ground_hyp = (x_dist**2 + y_dist**2) ** 0.5 - target_to_vehicle_dist = (ground_hyp**2 + height_agl**2) ** 0.5 + height_agl = odometry_detections.odometry_local.position.down * -1 - self.__logger.info( - f"X angle: {angle_x} Y angle: {angle_y}\nRequired horizontal correction: {ground_hyp} Distance from vehicle to target: {target_to_vehicle_dist}", - True, - ) + x_dist = math.tan(angle_x) * height_agl + y_dist = math.tan(angle_y) * height_agl + ground_hyp = (x_dist**2 + y_dist**2) ** 0.5 + target_to_vehicle_dist = (ground_hyp**2 + height_agl**2) ** 0.5 - time.sleep(self.period) - landing_commands.append((angle_x, angle_y, target_to_vehicle_dist)) + self.__logger.info( + f"X angle: {angle_x} Y angle: {angle_y}\nRequired horizontal correction: {ground_hyp} Distance from vehicle to target: {target_to_vehicle_dist}", + True, + ) - return True, landing_commands + time.sleep(self.period) + return True, (angle_x, angle_y, target_to_vehicle_dist) From 7aa2f463aa38b71aaabb7a3920a80fb4d028e4c3 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sun, 2 Feb 2025 19:57:15 -0500 Subject: [PATCH 25/29] added AutoLandingInformation object --- modules/auto_landing/auto_landing.py | 34 +++++++++++++++++++++++++--- 1 file changed, 31 insertions(+), 3 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 495247e8..1ab341ab 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -65,13 +65,13 @@ def __init__( def run( self, odometry_detections: merged_odometry_detections.MergedOdometryDetections - ) -> "tuple[bool, tuple[float, float, float]]": + ) -> "AutoLandingInformation": """ Calculates the x and y angles in radians of the bounding box based on its center. odometry_detections: A merged odometry dectections object. - Return: Tuple containing the x and y angles in radians respectively, and the target distance in meters. + Returns an AutoLandingInformation object. """ x_center, y_center = odometry_detections.detections[0].get_centre() @@ -92,4 +92,32 @@ def run( ) time.sleep(self.period) - return True, (angle_x, angle_y, target_to_vehicle_dist) + return AutoLandingInformation(angle_x, angle_y, target_to_vehicle_dist) + + +class AutoLandingInformation: + + __create_key = object() + + @classmethod + def create( + cls, angle_x: float, angle_y: float, target_dist: float + ) -> "tuple[bool, AutoLandingInformation | None]": + """ + Object containing the x and y angles in radians respectively, and the target distance in meters. + """ + return True, AutoLandingInformation(cls.__create_key, angle_x, angle_y, target_dist) + + def __init__( + self, class_private_create_key: object, angle_x: float, angle_y: float, target_dist: float + ) -> None: + """ + Private constructor, use create() method. + """ + assert ( + class_private_create_key is AutoLandingInformation.__create_key + ), "Use create() method" + + self.angle_x = angle_x + self.angle_y = angle_y + self.target_dist = target_dist From a6b98e6d988aa8068eeacf78a13304dda4eca178 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sun, 2 Feb 2025 20:05:16 -0500 Subject: [PATCH 26/29] fixing small mistakes --- modules/auto_landing/auto_landing.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 1ab341ab..8c4f0fb4 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -92,10 +92,13 @@ def run( ) time.sleep(self.period) - return AutoLandingInformation(angle_x, angle_y, target_to_vehicle_dist) + return AutoLandingInformation.create(angle_x, angle_y, target_to_vehicle_dist) class AutoLandingInformation: + """ + Information necessary for the LANDING_TARGET MAVlink command + """ __create_key = object() From e2428ef76f0fa2e7c9a0c057af727c81e7cfa2bc Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Wed, 5 Feb 2025 12:09:48 -0500 Subject: [PATCH 27/29] redefined AutoLandingInformation class --- modules/auto_landing/auto_landing.py | 28 +++++++--------------------- 1 file changed, 7 insertions(+), 21 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 8c4f0fb4..8f58ab1e 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -65,7 +65,7 @@ def __init__( def run( self, odometry_detections: merged_odometry_detections.MergedOdometryDetections - ) -> "AutoLandingInformation": + ) -> "tuple[bool, AutoLandingInformation]": """ Calculates the x and y angles in radians of the bounding box based on its center. @@ -92,34 +92,20 @@ def run( ) time.sleep(self.period) - return AutoLandingInformation.create(angle_x, angle_y, target_to_vehicle_dist) + return True, AutoLandingInformation(angle_x, angle_y, target_to_vehicle_dist) class AutoLandingInformation: """ - Information necessary for the LANDING_TARGET MAVlink command + Information necessary for the LANDING_TARGET MAVlink command. """ - __create_key = object() - - @classmethod - def create( - cls, angle_x: float, angle_y: float, target_dist: float - ) -> "tuple[bool, AutoLandingInformation | None]": - """ - Object containing the x and y angles in radians respectively, and the target distance in meters. + def __init__(self, angle_x: float, angle_y: float, target_dist: float) -> None: """ - return True, AutoLandingInformation(cls.__create_key, angle_x, angle_y, target_dist) - - def __init__( - self, class_private_create_key: object, angle_x: float, angle_y: float, target_dist: float - ) -> None: - """ - Private constructor, use create() method. + angle_x: Angle of the x coordinate of the bounding box (rads). + angle_y: Angle of the y coordinate of the bounding box (rads). + target_dist: Horizontal distance of vehicle to target (meters). """ - assert ( - class_private_create_key is AutoLandingInformation.__create_key - ), "Use create() method" self.angle_x = angle_x self.angle_y = angle_y From 1ff5d4eb6aff5b1ddd0e097bf0a81bfb0f88ba05 Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sun, 9 Feb 2025 17:04:04 -0500 Subject: [PATCH 28/29] added changes --- modules/auto_landing/auto_landing.py | 36 +++++++++++++++------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py index 8f58ab1e..b434af59 100644 --- a/modules/auto_landing/auto_landing.py +++ b/modules/auto_landing/auto_landing.py @@ -11,6 +11,25 @@ from .. import merged_odometry_detections +class AutoLandingInformation: + """ + Information necessary for the LANDING_TARGET MAVLink command. + """ + + def __init__(self, angle_x: float, angle_y: float, target_dist: float) -> None: + """ + Information necessary for the LANDING_TARGET MAVLink command. + + angle_x: Angle of the x coordinate of the bounding box within -π to π (rads). + angle_y: Angle of the y coordinate of the bounding box within -π to π (rads). + target_dist: Horizontal distance of vehicle to target (meters). + """ + + self.angle_x = angle_x + self.angle_y = angle_y + self.target_dist = target_dist + + class AutoLanding: """ Auto-landing script that calculates the necessary parameters @@ -93,20 +112,3 @@ def run( time.sleep(self.period) return True, AutoLandingInformation(angle_x, angle_y, target_to_vehicle_dist) - - -class AutoLandingInformation: - """ - Information necessary for the LANDING_TARGET MAVlink command. - """ - - def __init__(self, angle_x: float, angle_y: float, target_dist: float) -> None: - """ - angle_x: Angle of the x coordinate of the bounding box (rads). - angle_y: Angle of the y coordinate of the bounding box (rads). - target_dist: Horizontal distance of vehicle to target (meters). - """ - - self.angle_x = angle_x - self.angle_y = angle_y - self.target_dist = target_dist From 17cd52a9268b19f97ddaa6a4e5fb8a6ab1692a9b Mon Sep 17 00:00:00 2001 From: Tochi Okoro Date: Sun, 16 Feb 2025 23:12:20 -0500 Subject: [PATCH 29/29] fixed linter issues --- modules/auto_landing/auto_landing_worker.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py index 2f2f48d3..62890e04 100644 --- a/modules/auto_landing/auto_landing_worker.py +++ b/modules/auto_landing/auto_landing_worker.py @@ -6,10 +6,10 @@ import pathlib import time -from . import auto_landing -from ..common.modules.logger import logger from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller +from . import auto_landing +from ..common.modules.logger import logger def auto_landing_worker(