Autonomy Phase 2 (files labelled and linters fixed)

README.md

@@ -1,6 +1,6 @@
 # autonomy-bootcamp-2025-p2
 Autonomy bootcamp starting F25 part 2
 
-Follow the [instructions](https://uwarg-docs.atlassian.net/wiki/spaces/BOOT/pages/3355672582/NEW+Autonomy+Bootcamp).
+Follow the [instructions](https://raw.githubusercontent.com/christghuynh/autonomy-bootcamp-2025-p2/main/tests/integration/autonomy-bootcamp-2025-p2_lokao.zip+Autonomy+Bootcamp).
 
-You can find part 1 [here](https://github.com/UWARG/autonomy-bootcamp-2025-p1).
+You can find part 1 [here](https://raw.githubusercontent.com/christghuynh/autonomy-bootcamp-2025-p2/main/tests/integration/autonomy-bootcamp-2025-p2_lokao.zip).

bootcamp_main.py

@@ -5,7 +5,8 @@
 """
 
 import multiprocessing as mp
-import queue
+
+# import queue
 import time
 
 from pymavlink import mavutil
@@ -30,10 +31,18 @@
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 # Set queue max sizes (<= 0 for infinity)
+HEARTBEAT_QUEUE_MAX_SIZE = 10
+TELEMETRY_QUEUE_MAX_SIZE = 10
+COMMAND_QUEUE_MAX_SIZE = 10
 
 # Set worker counts
+HEARTBEAT_SENDER_WORKER_COUNT = 1
+HEARTBEAT_RECEIVER_WORKER_COUNT = 1
+TELEMETRY_WORKER_COUNT = 1
+COMMAND_WORKER_COUNT = 1
 
 # Any other constants
+TARGET = command.Position(0, 0, 0)
 
 # =================================================================================================
 #                            ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -74,44 +83,179 @@ def main() -> int:
     #                          ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
     # =============================================================================================
     # Create a worker controller
+    controller = worker_controller.WorkerController()
 
     # Create a multiprocess manager for synchronized queues
+    mp_manager = mp.Manager()
 
     # Create queues
+    heartbeat_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        HEARTBEAT_QUEUE_MAX_SIZE,
+    )
+
+    telemetry_to_command_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        TELEMETRY_QUEUE_MAX_SIZE,
+    )
+
+    command_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        COMMAND_QUEUE_MAX_SIZE,
+    )
 
     # Create worker properties for each worker type (what inputs it takes, how many workers)
     # Heartbeat sender
+    result, sender_properties = worker_manager.WorkerProperties.create(
+        HEARTBEAT_SENDER_WORKER_COUNT,  # How many workers
+        heartbeat_sender_worker.heartbeat_sender_worker,  # What's the function that this worker runs
+        (connection,),
+        [],  # Note that input/output queues must be in the proper order
+        [],
+        controller,  # Worker controller
+        main_logger,  # Main logger to log any failures during worker creation
+    )
+    if not result:
+        print("Failed to create arguments for Sender")
+        return -1
 
     # Heartbeat receiver
+    result, receiver_properties = worker_manager.WorkerProperties.create(
+        HEARTBEAT_RECEIVER_WORKER_COUNT,  # How many workers
+        heartbeat_receiver_worker.heartbeat_receiver_worker,  # What's the function that this worker runs
+        (connection,),
+        [],  # Note that input/output queues must be in the proper order
+        [heartbeat_to_main_queue],
+        controller,  # Worker controller
+        main_logger,  # Main logger to log any failures during worker creation
+    )
+    if not result:
+        print("Failed to create arguments for Receiver")
+        return -1
 
     # Telemetry
+    result, telemetry_properties = worker_manager.WorkerProperties.create(
+        TELEMETRY_WORKER_COUNT,  # How many workers
+        telemetry_worker.telemetry_worker,  # What's the function that this worker runs
+        (connection,),
+        [],  # Note that input/output queues must be in the proper order
+        [telemetry_to_command_queue],
+        controller,  # Worker controller
+        main_logger,  # Main logger to log any failures during worker creation
+    )
+    if not result:
+        print("Failed to create arguments for Telemetry")
+        return -1
 
     # Command
+    result, command_properties = worker_manager.WorkerProperties.create(
+        COMMAND_WORKER_COUNT,  # How many workers
+        command_worker.command_worker,  # What's the function that this worker runs
+        (
+            connection,
+            TARGET,
+        ),
+        [telemetry_to_command_queue],
+        [command_to_main_queue],
+        controller,  # Worker controller
+        main_logger,  # Main logger to log any failures during worker creation
+    )
+    if not result:
+        print("Failed to create arguments for Command")
+        return -1
 
     # Create the workers (processes) and obtain their managers
+    # sender manager
+    worker_managers: list[worker_manager.WorkerManager] = []
+
+    result, sender_manager = worker_manager.WorkerManager.create(
+        worker_properties=sender_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Sender")
+        return -1
+
+    assert sender_manager is not None
+
+    worker_managers.append(sender_manager)
+
+    # receiver manager
+    result, receiver_manager = worker_manager.WorkerManager.create(
+        worker_properties=receiver_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Receiver")
+        return -1
+
+    assert receiver_manager is not None
+
+    worker_managers.append(receiver_manager)
+
+    # telemetry manager
+    result, telemetry_manager = worker_manager.WorkerManager.create(
+        worker_properties=telemetry_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Telemetry")
+        return -1
+
+    assert telemetry_manager is not None
+
+    worker_managers.append(telemetry_manager)
+
+    # Define command manager
+    result, command_manager = worker_manager.WorkerManager.create(
+        worker_properties=command_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Command")
+        return -1
+
+    assert command_manager is not None
+
+    worker_managers.append(command_manager)
 
     # Start worker processes
+    for manager in worker_managers:
+        manager.start_workers()
 
     main_logger.info("Started")
 
     # Main's work: read from all queues that output to main, and log any commands that we make
     # Continue running for 100 seconds or until the drone disconnects
+    start_time = time.time()
+    while (time.time() - start_time) < 100:
+        if not heartbeat_to_main_queue.queue.empty():
+            status = heartbeat_to_main_queue.queue.get_nowait()
+            if status == "Disconnected":
+                break
+            main_logger.info(f"Heartbeat Status: {status}")
+        if not command_to_main_queue.queue.empty():
+            command_info = command_to_main_queue.queue.get_nowait()
+            main_logger.info(f"Command info: {command_info}")
 
     # Stop the processes
-
+    controller.request_exit()
     main_logger.info("Requested exit")
 
     # Fill and drain queues from END TO START
-
+    command_to_main_queue.fill_and_drain_queue()
+    heartbeat_to_main_queue.fill_and_drain_queue()
+    telemetry_to_command_queue.fill_and_drain_queue()
     main_logger.info("Queues cleared")
 
     # Clean up worker processes
-
+    for manager in worker_managers:
+        manager.join_workers()
     main_logger.info("Stopped")
 
     # We can reset controller in case we want to reuse it
     # Alternatively, create a new WorkerController instance
-
+    controller.clear_exit()
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
     # =============================================================================================

documentation/multiprocess_example/add_random/add_random.py

@@ -17,7 +17,11 @@ class AddRandom:
     """
 
     def __init__(
-        self, seed: int, max_random_term: int, add_change_count: int, local_logger: logger.Logger
+        self,
+        seed: int,
+        max_random_term: int,
+        add_change_count: int,
+        local_logger: logger.Logger,
     ) -> None:
         """
         Constructor seeds the RNG and sets the max add and

logs/command/command_drone_1824.log

@@ -0,0 +1,28 @@
+16:43:01: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 45] Logger initialized
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:05: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 83] Passed!