Phase 2 - Evan Liem

.python-version

@@ -0,0 +1 @@
+3.11.9

bootcamp_main.py

@@ -30,10 +30,26 @@
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 # Set queue max sizes (<= 0 for infinity)
+HEARTBEAT_STATUS_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
+HEARTBEAT_PERIOD_S = 1
+DISCONNECT_THRESHOLD = 5
+TELEMETRY_TIMEOUT_S = 1
+HEIGHT_TOLERANCE = 0.5
+ANGLE_TOLERANCE = 5
+Z_SPEED = 1
+TURNING_SPEED = 5
+TARGET = command.Position(10, 20, 30)
+MAIN_RUN_SECONDS = 100
 
 # =================================================================================================
 #                            ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -74,43 +90,195 @@ 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_status_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        HEARTBEAT_STATUS_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, heartbeat_sender_properties = worker_manager.WorkerProperties.create(
+        count=HEARTBEAT_SENDER_WORKER_COUNT,
+        target=heartbeat_sender_worker.heartbeat_sender_worker,
+        work_arguments=(
+            connection,
+            HEARTBEAT_PERIOD_S,
+        ),
+        input_queues=[],
+        output_queues=[],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create arguments for Heartbeat Sender")
+        return -1
+
+    assert heartbeat_sender_properties is not None
 
     # Heartbeat receiver
+    result, heartbeat_receiver_properties = worker_manager.WorkerProperties.create(
+        count=HEARTBEAT_RECEIVER_WORKER_COUNT,
+        target=heartbeat_receiver_worker.heartbeat_receiver_worker,
+        work_arguments=(
+            connection,
+            HEARTBEAT_PERIOD_S,
+            DISCONNECT_THRESHOLD,
+        ),
+        input_queues=[],
+        output_queues=[heartbeat_status_queue],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create arguments for Heartbeat Receiver")
+        return -1
+
+    assert heartbeat_receiver_properties is not None
 
     # Telemetry
+    result, telemetry_properties = worker_manager.WorkerProperties.create(
+        count=TELEMETRY_WORKER_COUNT,
+        target=telemetry_worker.telemetry_worker,
+        work_arguments=(
+            connection,
+            TELEMETRY_TIMEOUT_S,
+        ),
+        input_queues=[],
+        output_queues=[telemetry_to_command_queue],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create arguments for Telemetry")
+        return -1
+
+    assert telemetry_properties is not None
 
     # Command
+    result, command_properties = worker_manager.WorkerProperties.create(
+        count=COMMAND_WORKER_COUNT,
+        target=command_worker.command_worker,
+        work_arguments=(
+            connection,
+            TARGET,
+            HEIGHT_TOLERANCE,
+            ANGLE_TOLERANCE,
+            Z_SPEED,
+            TURNING_SPEED,
+        ),
+        input_queues=[telemetry_to_command_queue],
+        output_queues=[command_to_main_queue],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create arguments for Command")
+        return -1
+
+    assert command_properties is not None
 
     # Create the workers (processes) and obtain their managers
+    worker_managers: list[worker_manager.WorkerManager] = []
+
+    result, heartbeat_sender_manager = worker_manager.WorkerManager.create(
+        worker_properties=heartbeat_sender_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Heartbeat Sender")
+        return -1
+
+    assert heartbeat_sender_manager is not None
+    worker_managers.append(heartbeat_sender_manager)
+
+    result, heartbeat_receiver_manager = worker_manager.WorkerManager.create(
+        worker_properties=heartbeat_receiver_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Heartbeat Receiver")
+        return -1
+
+    assert heartbeat_receiver_manager is not None
+    worker_managers.append(heartbeat_receiver_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)
+
+    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 < MAIN_RUN_SECONDS:
+        try:
+            heartbeat_state = heartbeat_status_queue.queue.get(timeout=0.2)
+            main_logger.info(f"Heartbeat status: {heartbeat_state}", True)
+            if heartbeat_state == "Disconnected":
+                break
+            command_output = command_to_main_queue.queue.get(timeout=0.2)
+            main_logger.info(f"Command output: {command_output}", True)
+        except queue.Empty:
+            pass
 
     # 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()
+    telemetry_to_command_queue.fill_and_drain_queue()
+    heartbeat_status_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 ↑

logs/heartbeat_sender/heartbeat_sender_drone_13220.log

@@ -0,0 +1,5 @@
+16:35:09: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 40] Logger initialized
+16:35:11: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:35:12: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:35:13: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:35:14: [ERROR] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 56] Drone: Sent incorrect message type or didn't recieve a message in time: None

logs/heartbeat_sender/heartbeat_sender_worker_22120.log

@@ -0,0 +1,11 @@
+16:35:10: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 44] Logger initialized
+16:35:10: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat
+16:35:11: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat
+16:35:12: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat
+16:35:13: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat
+16:35:14: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat
+16:35:15: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat
+16:35:16: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat
+16:35:17: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat
+16:35:18: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat
+16:35:19: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | run | 63] Sent heartbeat

logs/heartbeat_sender/main.log

@@ -0,0 +1,2 @@
+16:35:09: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+16:35:10: [INFO] [C:\Users\aclie\bootcamp_WARG_Evan_Liem\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_sender.py | main | 90] Connected!

modules/command/command.py

@@ -37,30 +37,58 @@ def create(
         cls,
         connection: mavutil.mavfile,
         target: Position,
-        args,  # Put your own arguments here
+        height_tolerance: float,
+        yaw_tolerance_deg: float,
+        z_speed: float,
+        turning_speed: float,
         local_logger: logger.Logger,
-    ):
+    ) -> "tuple[bool, Command | None]":
         """
         Falliable create (instantiation) method to create a Command object.
         """
-        pass  #  Create a Command object
+        try:
+            instance = Command(
+                cls.__private_key,
+                connection,
+                target,
+                height_tolerance,
+                yaw_tolerance_deg,
+                z_speed,
+                turning_speed,
+                local_logger,
+            )
+        except Exception as exc:  # pylint: disable=broad-exception-caught
+            local_logger.error(f"Failed to create Command: {exc}", True)
+            return False, None
+        return True, instance
 
     def __init__(
         self,
         key: object,
         connection: mavutil.mavfile,
         target: Position,
-        args,  # Put your own arguments here
+        height_tolerance: float,
+        yaw_tolerance_deg: float,
+        z_speed: float,
+        turning_speed: float,
         local_logger: logger.Logger,
     ) -> None:
         assert key is Command.__private_key, "Use create() method"
 
-        # Do any intializiation here
+        self.__connection = connection
+        self.__target = target
+        self.__height_tolerance = height_tolerance
+        self.__yaw_tolerance_deg = yaw_tolerance_deg
+        self.__z_speed = z_speed
+        self.__turning_speed = turning_speed
+        self.__logger = local_logger
+        self.__velocity_sum = [0.0, 0.0, 0.0]
+        self.__velocity_count = 0
 
     def run(
         self,
-        args,  # Put your own arguments here
-    ):
+        telemetry_data: telemetry.TelemetryData,
+    ) -> "tuple[bool, str | None]":
         """
         Make a decision based on received telemetry data.
         """
@@ -75,6 +103,87 @@ def run(
         # Adjust direction (yaw) using MAV_CMD_CONDITION_YAW (115). Must use relative angle to current state
         # String to return to main: "CHANGING_YAW: {degree you changed it by in range [-180, 180]}"
         # Positive angle is counter-clockwise as in a right handed system
+        if (
+            telemetry_data.x_velocity is None
+            or telemetry_data.y_velocity is None
+            or telemetry_data.z_velocity is None
+        ):
+            self.__logger.error("Telemetry missing velocity data", True)
+            return False, None
+
+        self.__velocity_sum[0] += telemetry_data.x_velocity
+        self.__velocity_sum[1] += telemetry_data.y_velocity
+        self.__velocity_sum[2] += telemetry_data.z_velocity
+        self.__velocity_count += 1
+
+        avg_velocity = (
+            self.__velocity_sum[0] / self.__velocity_count,
+            self.__velocity_sum[1] / self.__velocity_count,
+            self.__velocity_sum[2] / self.__velocity_count,
+        )
+        self.__logger.info(f"Average velocity: {avg_velocity}", True)
+
+        if telemetry_data.z is None:
+            self.__logger.error("Telemetry missing altitude data", True)
+            return False, None
+
+        delta_z = self.__target.z - telemetry_data.z
+        if abs(delta_z) > self.__height_tolerance:
+            try:
+                self.__connection.mav.command_long_send(
+                    1,
+                    0,
+                    mavutil.mavlink.MAV_CMD_CONDITION_CHANGE_ALT,
+                    0,
+                    self.__z_speed,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    self.__target.z,
+                )
+            except Exception as exc:  # pylint: disable=broad-exception-caught
+                self.__logger.error(f"Failed to send altitude command: {exc}", True)
+                return False, None
+
+            return True, f"CHANGE ALTITUDE: {delta_z}"
+
+        if telemetry_data.x is None or telemetry_data.y is None or telemetry_data.yaw is None:
+            self.__logger.error("Telemetry missing position or yaw data", True)
+            return False, None
+
+        desired_yaw = math.atan2(
+            self.__target.y - telemetry_data.y, self.__target.x - telemetry_data.x
+        )
+        yaw_error = math.atan2(
+            math.sin(desired_yaw - telemetry_data.yaw),
+            math.cos(desired_yaw - telemetry_data.yaw),
+        )
+        yaw_error_deg = math.degrees(yaw_error)
+
+        if abs(yaw_error_deg) > self.__yaw_tolerance_deg:
+            try:
+                self.__connection.mav.command_long_send(
+                    1,
+                    0,
+                    mavutil.mavlink.MAV_CMD_CONDITION_YAW,
+                    0,
+                    yaw_error_deg,
+                    self.__turning_speed,
+                    0,
+                    1,
+                    0,
+                    0,
+                    0,
+                )
+            except Exception as exc:  # pylint: disable=broad-exception-caught
+                self.__logger.error(f"Failed to send yaw command: {exc}", True)
+                return False, None
+
+            return True, f"CHANGE YAW: {yaw_error_deg}"
+
+        return True, None
 
 
 # =================================================================================================