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,31 @@ 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:
+            return self.resume_handler()
+        if command.command == decision_command.DecisionCommand.CommandType.STOP:
+            return self.stop_handler()
+        return False
+
+    def resume_handler(self) -> bool:
+        """
+        Resumes the AUTO mission.
+        """
+        result = self.controller.set_flight_mode("AUTO")
+        if result:
+            print("Successfully set flight mode to AUTO.")
+        return result
+
+    def stop_handler(self) -> bool:
+        """
+        Stops the drone.
+        """
+        result = self.controller.set_flight_mode("LOITER")
+        if result:
+            print("Successfully set flight mode to LOITER.")
+        return result

modules/flight_interface/flight_interface_worker.py

@@ -3,28 +3,31 @@
 """
 
 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:
     """
     Worker process.
 
     address, timeout is initial setting.
     period is minimum period between loops.
-    output_queue is the data queue.
+    command_in_queue, odometry_out_queue are the data queues.
     controller is how the main process communicates to this worker process.
     """
+
     result, interface = flight_interface.FlightInterface.create(address, timeout)
     if not result:
         return
@@ -35,7 +38,16 @@ def flight_interface_worker(
         time.sleep(period)
 
         result, value = interface.run()
-        if not result:
+        if result:
+            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
 
-        output_queue.queue.put(value)
+        result = interface.run_decision_handler(command)
+        if result:
+            print(f"Command: {str(command.command)} received and sent to drone.")

tests/integration/test_flight_interface_worker.py

@@ -6,11 +6,12 @@
 import queue
 import time
 
+from modules import decision_command
+from modules import drone_odometry_local
+from modules.flight_interface import flight_interface_worker
 from worker import worker_controller
 from worker import queue_wrapper
 
-from modules import drone_odometry_local
-from modules.flight_interface import flight_interface_worker
 
 # Constants
 QUEUE_MAX_SIZE = 10
@@ -19,54 +20,82 @@
 FLIGHT_INTERFACE_WORKER_PERIOD = 0.1
 
 
+def simulate_decision_worker(in_queue: queue_wrapper.QueueWrapper) -> None:
+    """
+    Place example commands into the queue.
+    """
+    result, stop_command = decision_command.DecisionCommand.create_stop_mission_and_halt_command()
+    assert result
+    assert stop_command is not None
+
+    in_queue.queue.put(stop_command)
+
+    result, resume_command = decision_command.DecisionCommand.create_resume_mission_command()
+    assert result
+    assert resume_command is not None
+
+    in_queue.queue.put(resume_command)
+
+
 def main() -> int:
     """
     Main function.
     """
-
+    # Setup
     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,
         ),
     )
 
+    # Run
     worker.start()
 
+    simulate_decision_worker(command_in_queue)
+
     time.sleep(3)
 
+    # Test
     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'>"
             )
             assert input_data.local_position is not None
             assert input_data.drone_orientation is not None
 
-            print("north: " + str(input_data.local_position.north))
-            print("east: " + str(input_data.local_position.east))
-            print("down: " + str(input_data.local_position.down))
-            print("roll: " + str(input_data.drone_orientation.roll))
-            print("pitch: " + str(input_data.drone_orientation.pitch))
-            print("yaw: " + str(input_data.drone_orientation.yaw))
-            print("timestamp: " + str(input_data.timestamp))
+            print(f"north: {str(input_data.local_position.north)}")
+            print(f"east: {str(input_data.local_position.east)}")
+            print(f"down: {str(input_data.local_position.down)}")
+            print(f"roll: {str(input_data.drone_orientation.roll)}")
+            print(f"pitch: {str(input_data.drone_orientation.pitch)}")
+            print(f"yaw: {str(input_data.drone_orientation.yaw)}")
+            print(f"timestamp: {str(input_data.timestamp)}")
             print("")
 
         except queue.Empty:
             break
 
+    # Teardown
     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.
     """