Restart worker patch

config.yaml

@@ -3,25 +3,40 @@
 queue_max_size: 10
 
 video_input:
-    camera_name: 0
     worker_period: 1.0  # seconds
-    save_prefix: "log_image"
+    camera_enum: 0  # Enum values can be found in camera_factory.py
+    width: 1920
+    height: 1200
+    # For camera_enum=0, use the OpenCV camera config. For camera_enum=1, use the PiCamera2 config
+    # OpenCV camera config (regular cameras, enum 0)
+    camera_config:
+        device_index: 0
+    # PiCamera2 camera config (PiCamera NoIR, enum 1)
+    # camera_config:
+    #     exposure_time: 250  # microseconds
+    #     analogue_gain: 64.0  # Sets ISO, 1.0 for normal, 64.0 for max, 0.0 for min
+    #     contrast: 1.0  # Contrast, 1.0 for nomral, 32.0 for max, 0.0 for min
+    #     lens_position: null  # Focal length, 1/m (0 for infinity, null for auto focus)
+    log_images: true  # Set to true to save images
+    image_name: "log_image"  # Image name when saving images
 
 detect_target:
     worker_count: 1
     option: 0  # 0 is for Ultralytics (from detect_target_factory.py)
     device: 0
-    model_path: "tests/model_example/yolov8s_ultralytics_pretrained_default.pt"  # TODO: update
+    model_path: "tests/model_example/yolov8s_ultralytics_pretrained_default.pt"  # See autonomy OneDrive for latest model
     save_prefix: "log_comp"
 
 flight_interface:
-    address: "tcp:127.0.0.1:14550"
-    timeout: 10.0  # seconds
+    # Port 5762 connects directly to the simulated auto pilot, which is more realistic
+    # than connecting to port 14550, which is the ground station
+    address: "tcp:localhost:5762"
+    timeout: 30.0  # seconds
     baud_rate: 57600  # symbol rate 
     worker_period: 0.1  # seconds
 
 data_merge:
-    timeout: 10.0  # seconds
+    timeout: 30.0  # seconds
 
 geolocation:
     resolution_x: 1920
@@ -39,3 +54,6 @@ cluster_estimation:
     min_activation_threshold: 25
     min_new_points_to_run: 5
     random_state: 0
+
+communications:
+    timeout: 30.0  # seconds

main_2024.py

@@ -12,6 +12,10 @@
 # Used in type annotation of flight interface output
 # pylint: disable-next=unused-import
 from modules import odometry_and_time
+from modules.common.modules.camera import camera_factory
+from modules.common.modules.camera import camera_opencv
+from modules.common.modules.camera import camera_picamera2
+from modules.communications import communications_worker
 from modules.detect_target import detect_target_factory
 from modules.detect_target import detect_target_worker
 from modules.flight_interface import flight_interface_worker
@@ -80,19 +84,37 @@ def main() -> int:
         # pylint: disable=invalid-name
         QUEUE_MAX_SIZE = config["queue_max_size"]
 
-        VIDEO_INPUT_CAMERA_NAME = config["video_input"]["camera_name"]
         VIDEO_INPUT_WORKER_PERIOD = config["video_input"]["worker_period"]
-        VIDEO_INPUT_SAVE_NAME_PREFIX = config["video_input"]["save_prefix"]
-        VIDEO_INPUT_SAVE_PREFIX = str(pathlib.Path(logging_path, VIDEO_INPUT_SAVE_NAME_PREFIX))
+        VIDEO_INPUT_OPTION = camera_factory.CameraOption(config["video_input"]["camera_enum"])
+        VIDEO_INPUT_WIDTH = config["video_input"]["width"]
+        VIDEO_INPUT_HEIGHT = config["video_input"]["height"]
+        match VIDEO_INPUT_OPTION:
+            case camera_factory.CameraOption.OPENCV:
+                VIDEO_INPUT_CAMERA_CONFIG = camera_opencv.ConfigOpenCV(
+                    **config["video_input"]["camera_config"]
+                )
+            case camera_factory.CameraOption.PICAM2:
+                VIDEO_INPUT_CAMERA_CONFIG = camera_picamera2.ConfigPiCamera2(
+                    **config["video_input"]["camera_config"]
+                )
+            case _:
+                main_logger.error(f"Inputted an invalid camera option: {VIDEO_INPUT_OPTION}", True)
+                return -1
+
+        VIDEO_INPUT_IMAGE_NAME = (
+            config["video_input"]["image_name"] if config["video_input"]["log_images"] else None
+        )
 
         DETECT_TARGET_WORKER_COUNT = config["detect_target"]["worker_count"]
-        detect_target_option_int = config["detect_target"]["option"]
-        DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(detect_target_option_int)
+        DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(
+            config["detect_target"]["option"]
+        )
         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_NAME_PREFIX = config["detect_target"]["save_prefix"]
-        DETECT_TARGET_SAVE_PREFIX = str(pathlib.Path(logging_path, DETECT_TARGET_SAVE_NAME_PREFIX))
+        DETECT_TARGET_SAVE_PREFIX = str(
+            pathlib.Path(logging_path, config["detect_target"]["save_prefix"])
+        )
         DETECT_TARGET_SHOW_ANNOTATED = args.show_annotated
 
         FLIGHT_INTERFACE_ADDRESS = config["flight_interface"]["address"]
@@ -117,12 +139,14 @@ def main() -> int:
         MIN_NEW_POINTS_TO_RUN = config["cluster_estimation"]["min_new_points_to_run"]
         RANDOM_STATE = config["cluster_estimation"]["random_state"]
 
+        COMMUNICATIONS_TIMEOUT = config["communications"]["timeout"]
+
         # pylint: enable=invalid-name
     except KeyError as exception:
         main_logger.error(f"Config key(s) not found: {exception}", True)
         return -1
     except ValueError as exception:
-        main_logger.error(f"Could not convert detect target option into enum: {exception}", True)
+        main_logger.error(f"{exception}", True)
         return -1
 
     # Setup
@@ -141,6 +165,10 @@ def main() -> int:
         mp_manager,
         QUEUE_MAX_SIZE,
     )
+    flight_interface_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        QUEUE_MAX_SIZE,
+    )
     data_merge_to_geolocation_queue = queue_proxy_wrapper.QueueProxyWrapper(
         mp_manager,
         QUEUE_MAX_SIZE,
@@ -153,11 +181,14 @@ def main() -> int:
         mp_manager,
         QUEUE_MAX_SIZE,
     )
-    cluster_estimation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper(
+    cluster_estimation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        QUEUE_MAX_SIZE,
+    )
+    communications_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper(
         mp_manager,
         QUEUE_MAX_SIZE,
     )
-
     result, camera_intrinsics = camera_properties.CameraIntrinsics.create(
         GEOLOCATION_RESOLUTION_X,
         GEOLOCATION_RESOLUTION_Y,
@@ -189,9 +220,12 @@ def main() -> int:
         count=1,
         target=video_input_worker.video_input_worker,
         work_arguments=(
-            VIDEO_INPUT_CAMERA_NAME,
+            VIDEO_INPUT_OPTION,
+            VIDEO_INPUT_WIDTH,
+            VIDEO_INPUT_HEIGHT,
+            VIDEO_INPUT_CAMERA_CONFIG,
+            VIDEO_INPUT_IMAGE_NAME,
             VIDEO_INPUT_WORKER_PERIOD,
-            VIDEO_INPUT_SAVE_PREFIX,
         ),
         input_queues=[],
         output_queues=[video_input_to_detect_target_queue],
@@ -238,7 +272,10 @@ def main() -> int:
             FLIGHT_INTERFACE_WORKER_PERIOD,
         ),
         input_queues=[flight_interface_decision_queue],
-        output_queues=[flight_interface_to_data_merge_queue],
+        output_queues=[
+            flight_interface_to_data_merge_queue,
+            flight_interface_to_communications_queue,
+        ],
         controller=controller,
         local_logger=main_logger,
     )
@@ -292,7 +329,7 @@ def main() -> int:
         target=cluster_estimation_worker.cluster_estimation_worker,
         work_arguments=(MIN_ACTIVATION_THRESHOLD, MIN_NEW_POINTS_TO_RUN, RANDOM_STATE),
         input_queues=[geolocation_to_cluster_estimation_queue],
-        output_queues=[cluster_estimation_to_main_queue],
+        output_queues=[cluster_estimation_to_communications_queue],
         controller=controller,
         local_logger=main_logger,
     )
@@ -303,6 +340,24 @@ def main() -> int:
     # Get Pylance to stop complaining
     assert cluster_estimation_worker_properties is not None
 
+    result, communications_worker_properties = worker_manager.WorkerProperties.create(
+        count=1,
+        target=communications_worker.communications_worker,
+        work_arguments=(COMMUNICATIONS_TIMEOUT,),
+        input_queues=[
+            flight_interface_to_communications_queue,
+            cluster_estimation_to_communications_queue,
+        ],
+        output_queues=[communications_to_main_queue],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create arguments for Communications Worker", True)
+        return -1
+
+    assert communications_worker_properties is not None
+
     # Create managers
     worker_managers = []
 
@@ -384,6 +439,19 @@ def main() -> int:
 
     worker_managers.append(cluster_estimation_manager)
 
+    result, communications_manager = worker_manager.WorkerManager.create(
+        worker_properties=communications_worker_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create manager for Communications", True)
+        return -1
+
+    # Get Pylance to stop complaining
+    assert communications_manager is not None
+
+    worker_managers.append(communications_manager)
+
     # Run
     for manager in worker_managers:
         manager.start_workers()
@@ -396,16 +464,12 @@ def main() -> int:
                 return -1
 
         try:
-            cluster_estimations = cluster_estimation_to_main_queue.queue.get_nowait()
+            cluster_estimations = communications_to_main_queue.queue.get_nowait()
         except queue.Empty:
             cluster_estimations = None
 
         if cluster_estimations is not None:
-            for cluster in cluster_estimations:
-                main_logger.debug("Cluster in world: " + True)
-                main_logger.debug("Cluster location x: " + str(cluster.location_x))
-                main_logger.debug("Cluster location y: " + str(cluster.location_y))
-                main_logger.debug("Cluster spherical variance: " + str(cluster.spherical_variance))
+            main_logger.debug(f"Clusters: {cluster_estimations}")
         if cv2.waitKey(1) == ord("q"):  # type: ignore
             main_logger.info("Exiting main loop", True)
             break
@@ -416,10 +480,12 @@ def main() -> int:
     video_input_to_detect_target_queue.fill_and_drain_queue()
     detect_target_to_data_merge_queue.fill_and_drain_queue()
     flight_interface_to_data_merge_queue.fill_and_drain_queue()
+    flight_interface_to_communications_queue.fill_and_drain_queue()
     data_merge_to_geolocation_queue.fill_and_drain_queue()
     geolocation_to_cluster_estimation_queue.fill_and_drain_queue()
+    cluster_estimation_to_communications_queue.fill_and_drain_queue()
+    communications_to_main_queue.fill_and_drain_queue()
     flight_interface_decision_queue.fill_and_drain_queue()
-    cluster_estimation_to_main_queue.fill_and_drain_queue()
 
     for manager in worker_managers:
         manager.join_workers()

modules/cluster_estimation/cluster_estimation_worker.py

@@ -5,6 +5,8 @@
 import os
 import pathlib
 
+from modules import detection_in_world
+from modules.detection_in_world import DetectionInWorld
 from utilities.workers import queue_proxy_wrapper
 from utilities.workers import worker_controller
 from . import cluster_estimation
@@ -71,6 +73,16 @@ def cluster_estimation_worker(
 
         input_data = input_queue.queue.get()
         if input_data is None:
+            local_logger.info("Recieved type None, exiting.")
+            continue
+
+        isInvalid = False
+        for input in input_data:
+            if not isinstance(input, detection_in_world.DetectionInWorld):
+                local_logger.warning(f"Skipping unexpected input: {input_data}")
+                isInvalid = True
+
+        if isInvalid:
             continue
 
         # TODO: When to override

modules/communications/communications.py

@@ -0,0 +1,90 @@
+"""
+Logs data and forwards it.
+"""
+
+import time
+
+from .. import object_in_world
+from ..common.modules import position_global
+from ..common.modules import position_local
+from ..common.modules.logger import logger
+from ..common.modules.mavlink import local_global_conversion
+
+
+class Communications:
+    """
+    Currently logs data only.
+    """
+
+    __create_key = object()
+
+    @classmethod
+    def create(
+        cls,
+        home_position: position_global.PositionGlobal,
+        local_logger: logger.Logger,
+    ) -> "tuple[True, Communications] | tuple[False, None]":
+        """
+        Logs data and forwards it.
+
+        home_position: Take-off position of drone.
+
+        Returns: Success, class object.
+        """
+
+        return True, Communications(cls.__create_key, home_position, local_logger)
+
+    def __init__(
+        self,
+        class_private_create_key: object,
+        home_position: position_global.PositionGlobal,
+        local_logger: logger.Logger,
+    ) -> None:
+        """
+        Private constructor, use create() method.
+        """
+        assert class_private_create_key is Communications.__create_key, "Use create() method"
+
+        self.__home_position = home_position
+        self.__logger = local_logger
+
+    def run(
+        self,
+        objects_in_world: list[object_in_world.ObjectInWorld],
+    ) -> tuple[True, list[object_in_world.ObjectInWorld]] | tuple[False, None]:
+
+        objects_in_world_global = []
+        for object_in_world in objects_in_world:
+            # We assume detected objects are on the ground
+            north = object_in_world.location_x
+            east = object_in_world.location_y
+            down = 0.0
+
+            result, object_position_local = position_local.PositionLocal.create(
+                north,
+                east,
+                down,
+            )
+            if not result:
+                self.__logger.warning(
+                    f"Could not convert ObjectInWorld to PositionLocal:\nobject in world: {object_in_world}"
+                )
+                return False, None
+
+            result, object_in_world_global = (
+                local_global_conversion.position_global_from_position_local(
+                    self.__home_position, object_position_local
+                )
+            )
+            if not result:
+                # Log nothing if at least one of the conversions failed
+                self.__logger.warning(
+                    f"position_global_from_position_local conversion failed:\nhome_position: {self.__home_position}\nobject_position_local: {object_position_local}"
+                )
+                return False, None
+
+            objects_in_world_global.append(object_in_world_global)
+
+        self.__logger.info(f"{time.time()}: {objects_in_world_global}")
+
+        return True, objects_in_world