rewrote flight interface to receive commands

modules/flight_interface/flight_interface.py

@@ -2,11 +2,11 @@
 Creates flight controller and produces local drone odometry coupled with a timestamp.
 """
 
-from . import conversions
-from .. import drone_odometry_local
-
+from modules import decision_command
+from modules import drone_odometry_local
 from ..common.mavlink.modules import drone_odometry
 from ..common.mavlink.modules import flight_controller
+from . import conversions
 
 
 class FlightInterface:
@@ -66,3 +66,27 @@ def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]":
         drone_orientation = odometry.orientation
 
         return drone_odometry_local.DroneOdometryLocal.create(local_position, drone_orientation)
+
+    def run_decision_handler(self, command: decision_command.DecisionCommand) -> bool:
+        """
+        Uploads decision commands to drone.
+        """
+        if command.command == decision_command.DecisionCommand.CommandType.RESUME:
+            self.resume_handler()
+            return True
+        if command.command == decision_command.DecisionCommand.CommandType.STOP:
+            self.stop_handler()
+            return True
+        return False
+
+    def resume_handler(self) -> None:
+        """
+        Resumes the AUTO mission.
+        """
+        self.controller.set_flight_mode("AUTO")
+
+    def stop_handler(self) -> None:
+        """
+        Stops the drone.
+        """
+        self.controller.set_flight_mode("LOITER")

modules/flight_interface/flight_interface_worker.py

@@ -3,18 +3,20 @@
 """
 
 import time
+import queue
 
+from modules import decision_command
 from worker import queue_wrapper
 from worker import worker_controller
-
 from . import flight_interface
 
 
 def flight_interface_worker(
     address: str,
     timeout: float,
     period: float,
-    output_queue: queue_wrapper.QueueWrapper,
+    command_in_queue: queue_wrapper.QueueWrapper,
+    odometry_out_queue: queue_wrapper.QueueWrapper,
     controller: worker_controller.WorkerController,
 ) -> None:
     """
@@ -25,6 +27,7 @@ def flight_interface_worker(
     output_queue is the data queue.
     controller is how the main process communicates to this worker process.
     """
+
     result, interface = flight_interface.FlightInterface.create(address, timeout)
     if not result:
         return
@@ -38,4 +41,13 @@ def flight_interface_worker(
         if not result:
             continue
 
-        output_queue.queue.put(value)
+        odometry_out_queue.queue.put(value)
+
+        try:
+            command: decision_command.DecisionCommand = command_in_queue.queue.get_nowait()
+            if command is None:
+                break
+        except queue.Empty:
+            continue
+
+        interface.run_decision_handler(command)

tests/integration/test_flight_interface_worker.py

@@ -6,11 +6,12 @@
 import queue
 import time
 
-from worker import worker_controller
-from worker import queue_wrapper
 
 from modules import drone_odometry_local
 from modules.flight_interface import flight_interface_worker
+from worker import worker_controller
+from worker import queue_wrapper
+
 
 # Constants
 QUEUE_MAX_SIZE = 10
@@ -23,19 +24,20 @@ def main() -> int:
     """
     Main function.
     """
-
     controller = worker_controller.WorkerController()
-    manager = mp.Manager()
+    mp_manager = mp.Manager()
 
-    output_queue = queue_wrapper.QueueWrapper(manager, QUEUE_MAX_SIZE)
+    command_in_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE)
+    odometry_out_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE)
 
     worker = mp.Process(
         target=flight_interface_worker.flight_interface_worker,
         args=(
             FLIGHT_INTERFACE_ADDRESS,
             FLIGHT_INTERFACE_TIMEOUT,
             FLIGHT_INTERFACE_WORKER_PERIOD,
-            output_queue,
+            command_in_queue,
+            odometry_out_queue,
             controller,
         ),
     )
@@ -46,7 +48,9 @@ def main() -> int:
 
     while True:
         try:
-            input_data: drone_odometry_local.DroneOdometryLocal = output_queue.queue.get_nowait()
+            input_data: drone_odometry_local.DroneOdometryLocal = (
+                odometry_out_queue.queue.get_nowait()
+            )
             assert (
                 str(type(input_data)) == "<class 'modules.drone_odometry_local.DroneOdometryLocal'>"
             )
@@ -67,6 +71,8 @@ def main() -> int:
 
     controller.request_exit()
 
+    command_in_queue.fill_and_drain_queue()
+
     worker.join()
 
     return 0

tests/unit/test_flight_interface.py

@@ -7,7 +7,7 @@
 
 def create_flight_interface_instance(
     address: str, timeout: float
-) -> "tuple[bool, FlightInterface | None]":
+) -> "tuple[bool, flight_interface.FlightInterface | None]":
     """
     Construct a flight interface instance.
     """