diff --git a/config.yaml b/config.yaml index 77cfd8fc..c5a441b9 100644 --- a/config.yaml +++ b/config.yaml @@ -2,18 +2,20 @@ queue_max_size: 10 +log_directory_path: "logs" + video_input: - camera_name: 0 - worker_period: 1.0 # seconds - save_prefix: "log_image" + camera_name: 0 + worker_period: 1.0 # seconds + save_prefix: "log_image" detect_target: - worker_count: 1 - device: 0 - model_path: "tests/model_example/yolov8s_ultralytics_pretrained_default.pt" # TODO: update - save_prefix: "log_comp" + worker_count: 1 + device: 0 + model_path: "tests/model_example/yolov8s_ultralytics_pretrained_default.pt" # TODO: update + save_prefix: "log_comp" flight_interface: - address: "tcp:127.0.0.1:14550" - timeout: 10.0 # seconds - worker_period: 0.1 # seconds + address: "tcp:127.0.0.1:14550" + timeout: 10.0 # seconds + worker_period: 0.1 # seconds diff --git a/main_2024.py b/main_2024.py index 0b586875..8d8a70ff 100644 --- a/main_2024.py +++ b/main_2024.py @@ -9,12 +9,14 @@ import cv2 import yaml -from utilities.workers import queue_proxy_wrapper -from utilities.workers import worker_controller -from utilities.workers import worker_manager +from modules import odometry_and_time from modules.detect_target import detect_target_worker from modules.flight_interface import flight_interface_worker from modules.video_input import video_input_worker +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from utilities.workers import worker_manager + CONFIG_FILE_PATH = pathlib.Path("config.yaml") @@ -49,15 +51,19 @@ def main() -> int: try: QUEUE_MAX_SIZE = config["queue_max_size"] + LOG_DIRECTORY_PATH = config["log_directory_path"] + VIDEO_INPUT_CAMERA_NAME = config["video_input"]["camera_name"] VIDEO_INPUT_WORKER_PERIOD = config["video_input"]["worker_period"] - VIDEO_INPUT_SAVE_PREFIX = config["video_input"]["save_prefix"] + VIDEO_INPUT_SAVE_NAME_PREFIX = config["video_input"]["save_prefix"] + VIDEO_INPUT_SAVE_PREFIX = f"{LOG_DIRECTORY_PATH}/{VIDEO_INPUT_SAVE_NAME_PREFIX}" DETECT_TARGET_WORKER_COUNT = config["detect_target"]["worker_count"] DETECT_TARGET_DEVICE = "cpu" if args.cpu else config["detect_target"]["device"] DETECT_TARGET_MODEL_PATH = config["detect_target"]["model_path"] DETECT_TARGET_OVERRIDE_FULL_PRECISION = args.full - DETECT_TARGET_SAVE_PREFIX = config["detect_target"]["save_prefix"] + DETECT_TARGET_SAVE_NAME_PREFIX = config["detect_target"]["save_prefix"] + DETECT_TARGET_SAVE_PREFIX = f"{LOG_DIRECTORY_PATH}/{DETECT_TARGET_SAVE_NAME_PREFIX}" FLIGHT_INTERFACE_ADDRESS = config["flight_interface"]["address"] FLIGHT_INTERFACE_TIMEOUT = config["flight_interface"]["timeout"] @@ -66,6 +72,8 @@ def main() -> int: print("Config key(s) not found") return -1 + pathlib.Path(LOG_DIRECTORY_PATH).mkdir(exist_ok=True) + # Setup controller = worker_controller.WorkerController() @@ -135,16 +143,21 @@ def main() -> int: except queue.Empty: image = None - odometry_and_time = flight_interface_to_main_queue.queue.get() - - if odometry_and_time is not None: - print("timestamp: " + str(odometry_and_time.timestamp)) - print("lat: " + str(odometry_and_time.odometry_data.position.latitude)) - print("lon: " + str(odometry_and_time.odometry_data.position.longitude)) - print("alt: " + str(odometry_and_time.odometry_data.position.altitude)) - print("yaw: " + str(odometry_and_time.odometry_data.orientation.yaw)) - print("roll: " + str(odometry_and_time.odometry_data.orientation.roll)) - print("pitch: " + str(odometry_and_time.odometry_data.orientation.pitch)) + odometry_and_time_info: "odometry_and_time.OdometryAndTime | None" = \ + flight_interface_to_main_queue.queue.get() + + if odometry_and_time_info is not None: + timestamp = odometry_and_time_info.timestamp + position = odometry_and_time_info.odometry_data.position + orientation = odometry_and_time_info.odometry_data.orientation.orientation + + print("timestamp: " + str(timestamp)) + print("north: " + str(position.north)) + print("east: " + str(position.east)) + print("down: " + str(position.down)) + print("yaw: " + str(orientation.yaw)) + print("roll: " + str(orientation.roll)) + print("pitch: " + str(orientation.pitch)) print("") if image is None: diff --git a/modules/common b/modules/common index 82f573ff..8c8cf6a2 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit 82f573fff15d9a1c05818e334a6dfb3d60c889b7 +Subproject commit 8c8cf6a2fb0d6337a1cb4adb0c713854f51bc651 diff --git a/modules/zp_input/__init__.py b/modules/zp_input/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/modules/zp_input/zp_input.py b/modules/zp_input/zp_input.py deleted file mode 100644 index fc1f5908..00000000 --- a/modules/zp_input/zp_input.py +++ /dev/null @@ -1,29 +0,0 @@ -""" -Combines image and timestamp together. -""" - -from ..common.comms.modules import generic_comms_device -from .. import message_and_time - - -# This is just an interface -# pylint: disable=too-few-public-methods -class ZpInput: - """ - Combines ZP message and timestamp together. - """ - - def __init__(self, port: str, baudrate: int): - self.device = generic_comms_device.GenericCommsDevice(port, baudrate) - - def run(self) -> "tuple[bool, message_and_time.MessageAndTime | None]": - """ - Returns a possible MessageAndTime with current timestamp. - """ - result, message = self.device.receive() - if not result: - return False, None - - return True, message_and_time.MessageAndTime(message) - -# pylint: enable=too-few-public-methods diff --git a/modules/zp_input/zp_input_worker.py b/modules/zp_input/zp_input_worker.py deleted file mode 100644 index d5c162d2..00000000 --- a/modules/zp_input/zp_input_worker.py +++ /dev/null @@ -1,46 +0,0 @@ -""" -Gets data from ZeroPilot. -""" - -from utilities.workers import queue_proxy_wrapper -from utilities.workers import worker_controller -from . import zp_input - - -def zp_input_worker(port: str, - baudrate: int, - telemetry_output_queue: queue_proxy_wrapper.QueueProxyWrapper, - request_output_queue: queue_proxy_wrapper.QueueProxyWrapper, - controller: worker_controller.WorkerController): - """ - Worker process. - - port is UART port. - baudrate is UART baudrate. - telemetry_output_queue is the telemetry queue. - request_output_queue is the ZP request queue. - controller is how the main process communicates to this worker process. - """ - input_device = zp_input.ZpInput(port, baudrate) - - while not controller.is_exit_requested(): - controller.check_pause() - - result, value = input_device.run() - if not result: - continue - - # Get Pylance to stop complaining - assert value is not None - assert value.message is not None - - # Decide which worker to send to next depending on message type - if value.message.header.type == 0: - # Odometry - telemetry_output_queue.queue.put(value) - elif value.message.header.type == 1: - # Request - request_output_queue.queue.put(value) - else: - # TODO: Invalid type, logging? - pass diff --git a/tests/test_zp_input.py b/tests/test_zp_input.py deleted file mode 100644 index 6ccadd90..00000000 --- a/tests/test_zp_input.py +++ /dev/null @@ -1,3 +0,0 @@ -""" -No test to run. -""" diff --git a/tests/test_zp_input_worker.py b/tests/test_zp_input_worker.py deleted file mode 100644 index 1f824927..00000000 --- a/tests/test_zp_input_worker.py +++ /dev/null @@ -1,62 +0,0 @@ -""" -Test worker process. -""" -import multiprocessing as mp -import queue -import time - -from modules.zp_input import zp_input_worker -from modules import message_and_time -from utilities.workers import queue_proxy_wrapper -from utilities.workers import worker_controller - - -PORT = "/dev/ttyS0" -BAUDRATE = 115_200 - - -if __name__ == "__main__": - # Setup - controller = worker_controller.WorkerController() - - mp_manager = mp.Manager() - telemetry_out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) - request_out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) - - worker = mp.Process( - target=zp_input_worker.zp_input_worker, - args=(PORT, BAUDRATE, telemetry_out_queue, request_out_queue, controller), - ) - - # Run - worker.start() - - time.sleep(3) - - controller.request_exit() - - # Test - while True: - try: - input_data: message_and_time.MessageAndTime = telemetry_out_queue.queue.get_nowait() - assert str(type(input_data)) == \ - "" - assert input_data.message.header.type == 0 - - except queue.Empty: - break - - while True: - try: - input_data: message_and_time.MessageAndTime = request_out_queue.queue.get_nowait() - assert str(type(input_data)) == \ - "" - assert input_data.message.header.type == 1 - - except queue.Empty: - break - - # Teardown - worker.join() - - print("Done!")