implemented local flight mode state in decision (#34)

* implemented local flight mode state in decision

* type annotations

* remove unused enum

* implemented timeout for decision module to request a command

* formatting

config.yaml

@@ -20,6 +20,7 @@ data_merge:
     delay: 0.1 # seconds
 
 decision:
-    object_proximity_limit: 10 # metres
+    object_proximity_limit: 10.0 # metres
     max_history: 80
+    command_timeout: 1.0 # seconds
 

main.py

@@ -59,6 +59,7 @@ def main() -> int:
 
         OBJECT_PROXIMITY_LIMIT = config["decision"]["object_proximity_limit"]
         MAX_HISTORY = config["decision"]["max_history"]
+        COMMAND_TIMEOUT = config["decision"]["command_timeout"]
         # pylint: enable=invalid-name
     except KeyError:
         print("Config key(s) not found.")
@@ -116,6 +117,7 @@ def main() -> int:
         args=(
             OBJECT_PROXIMITY_LIMIT,
             MAX_HISTORY,
+            COMMAND_TIMEOUT,
             merged_to_decision_queue,
             command_to_flight_interface_queue,
             controller,

modules/decision/decision.py

@@ -3,6 +3,7 @@
 """
 
 from collections import deque
+import time
 
 from .. import decision_command
 from .. import detections_and_odometry
@@ -14,33 +15,59 @@ class Decision:
     Determines best action to avoid obstacles based on LiDAR and odometry data.
     """
 
-    def __init__(self, proximity_limit: float, max_history: int) -> None:
+    def __init__(self, proximity_limit: float, max_history: int, command_timeout: float) -> None:
         """
         Initialize current drone state and its lidar detections list.
         """
         self.proximity_limit = proximity_limit
         self.detections_and_odometries = deque(maxlen=max_history)
+        self.command_timeout = command_timeout
+        self.__command_requested = False
+        self.__last_command_sent = None
 
     def run_simple_decision(
         self,
         detections_and_odometries: "deque[detections_and_odometry.DetectionsAndOdometry]",
         proximity_limit: float,
+        current_flight_mode: drone_odometry_local.FlightMode,
     ) -> "tuple[bool, decision_command.DecisionCommand | None]":
         """
         Runs simple collision avoidance where drone will stop within a set distance of an object.
         """
+        start_time = 0
         for lidar_scan_and_odometry in detections_and_odometries:
-            current_flight_mode = lidar_scan_and_odometry.odometry.flight_mode
             detections = lidar_scan_and_odometry.detections
 
+            if self.__command_requested and self.__last_command_sent == current_flight_mode:
+                self.__command_requested = False
+
+            if self.__command_requested:
+                if start_time - time.time() > self.command_timeout:
+                    if self.__last_command_sent == drone_odometry_local.FlightMode.STOPPED:
+                        return (
+                            decision_command.DecisionCommand.create_stop_mission_and_halt_command()
+                        )
+                    if self.__last_command_sent == drone_odometry_local.FlightMode.MOVING:
+                        return decision_command.DecisionCommand.create_resume_mission_command()
+                continue
+
             if current_flight_mode == drone_odometry_local.FlightMode.STOPPED:
                 for detection in detections:
                     if detection.distance < proximity_limit:
                         return False, None
+                self.__command_requested = True
+                self.__last_command_sent = drone_odometry_local.FlightMode.MOVING
+                self.detections_and_odometries.clear()
+                start_time = time.time()
                 return decision_command.DecisionCommand.create_resume_mission_command()
+
             if current_flight_mode == drone_odometry_local.FlightMode.MOVING:
                 for detection in detections:
                     if detection.distance < proximity_limit:
+                        self.__command_requested = True
+                        self.__last_command_sent = drone_odometry_local.FlightMode.STOPPED
+                        self.detections_and_odometries.clear()
+                        start_time = time.time()
                         return (
                             decision_command.DecisionCommand.create_stop_mission_and_halt_command()
                         )
@@ -52,5 +79,8 @@ def run(
         """
         Run obstacle avoidance.
         """
+        current_flight_mode = merged_data.odometry.flight_mode
         self.detections_and_odometries.append(merged_data)
-        return self.run_simple_decision(self.detections_and_odometries, self.proximity_limit)
+        return self.run_simple_decision(
+            self.detections_and_odometries, self.proximity_limit, current_flight_mode
+        )

modules/decision/decision_worker.py

@@ -11,6 +11,7 @@
 def decision_worker(
     object_proximity_limit: float,
     max_history: int,
+    command_timeout: float,
     merged_in_queue: queue_wrapper.QueueWrapper,
     command_out_queue: queue_wrapper.QueueWrapper,
     controller: worker_controller.WorkerController,
@@ -23,7 +24,7 @@ def decision_worker(
     controller is how the main process communicates to this worker process.
     """
 
-    decider = decision.Decision(object_proximity_limit, max_history)
+    decider = decision.Decision(object_proximity_limit, max_history, command_timeout)
 
     while not controller.is_exit_requested():
         controller.check_pause()

tests/integration/test_decision_worker.py

@@ -19,6 +19,7 @@
 
 OBJECT_PROXIMITY_LIMIT = 5  # metres
 MAX_HISTORY = 20  # readings
+COMMAND_TIMEOUT = 1.0  # seconds
 
 # pylint: disable=duplicate-code
 
@@ -74,6 +75,7 @@ def main() -> int:
         args=(
             OBJECT_PROXIMITY_LIMIT,
             MAX_HISTORY,
+            COMMAND_TIMEOUT,
             merged_in_queue,
             command_out_queue,
             controller,

tests/unit/test_decision.py

@@ -14,6 +14,7 @@
 
 OBJECT_PROXIMITY_LIMIT = 5.0  # metres
 MAX_HISTORY = 20  # readings
+COMMAND_TIMEOUT = 1.0  # seconds
 
 # pylint: disable=redefined-outer-name, duplicate-code
 
@@ -23,7 +24,7 @@ def decision_maker() -> decision.Decision:  # type: ignore
     """
     Construct a decision instance with predefined object proximity limit.
     """
-    decision_instance = decision.Decision(OBJECT_PROXIMITY_LIMIT, MAX_HISTORY)
+    decision_instance = decision.Decision(OBJECT_PROXIMITY_LIMIT, MAX_HISTORY, COMMAND_TIMEOUT)
     yield decision_instance
 
 
@@ -34,7 +35,7 @@ def object_within_proximity_limit_while_moving() -> detections_and_odometry.Dete
     """
     detections = []
 
-    for _ in range(0, 5):
+    for _ in range(0, 25):
         result, detection = lidar_detection.LidarDetection.create(6.0, 3.0)
         assert result
         assert detection is not None
@@ -75,7 +76,7 @@ def object_within_proximity_limit_while_stopped() -> detections_and_odometry.Det
     """
     detections = []
 
-    for _ in range(0, 5):
+    for _ in range(0, 25):
         result, detection = lidar_detection.LidarDetection.create(6.0, 3.0)
         assert result
         assert detection is not None
@@ -115,7 +116,7 @@ def object_outside_proximity_limit_while_moving() -> detections_and_odometry.Det
     Creates a DetectionsAndOdometry outside the proximity limit.
     """
     detections = []
-    for _ in range(0, 5):
+    for _ in range(0, 25):
         result, detection = lidar_detection.LidarDetection.create(6.0, 3.0)
         assert result
         assert detection is not None
@@ -150,7 +151,7 @@ def object_outside_proximity_limit_while_stopped() -> detections_and_odometry.De
     Creates a DetectionsAndOdometry outside the proximity limit.
     """
     detections = []
-    for _ in range(0, 5):
+    for _ in range(0, 25):
         result, detection = lidar_detection.LidarDetection.create(6.0, 3.0)
         assert result
         assert detection is not None
@@ -245,3 +246,87 @@ def test_decision_outside_proximity_limit_while_stopped(
         assert result
         assert command is not None
         assert command.command == expected
+
+    def test_decision_stop_then_continue(
+        self,
+        decision_maker: decision.Decision,
+        object_within_proximity_limit_while_moving: detections_and_odometry.DetectionsAndOdometry,
+        object_outside_proximity_limit_while_stopped: detections_and_odometry.DetectionsAndOdometry,
+    ) -> None:
+        """
+        Test decision module when the drone should continue after it has been stopped.
+        """
+        expected = decision_command.DecisionCommand.CommandType.RESUME_MISSION
+
+        result, _ = decision_maker.run(object_within_proximity_limit_while_moving)
+
+        assert result
+
+        result, command = decision_maker.run(object_outside_proximity_limit_while_stopped)
+
+        assert result
+        assert command is not None
+        assert command.command == expected
+
+    def test_decision_continue_then_stop(
+        self,
+        decision_maker: decision.Decision,
+        object_outside_proximity_limit_while_stopped: detections_and_odometry.DetectionsAndOdometry,
+        object_within_proximity_limit_while_moving: detections_and_odometry.DetectionsAndOdometry,
+    ) -> None:
+        """
+        Test decision module when the drone should continue after it has been stopped.
+        """
+        expected = decision_command.DecisionCommand.CommandType.STOP_MISSION_AND_HALT
+
+        result, _ = decision_maker.run(object_outside_proximity_limit_while_stopped)
+
+        assert result
+
+        result, command = decision_maker.run(object_within_proximity_limit_while_moving)
+
+        assert result
+        assert command is not None
+        assert command.command == expected
+
+    def test_decision_simulate_full_mission(
+        self,
+        decision_maker: decision.Decision,
+        object_within_proximity_limit_while_moving: detections_and_odometry.DetectionsAndOdometry,
+        object_outside_proximity_limit_while_stopped: detections_and_odometry.DetectionsAndOdometry,
+        object_outside_proximity_limit_while_moving: detections_and_odometry.DetectionsAndOdometry,
+    ) -> None:
+        """
+        Test decision module by simulating a flight test.
+        """
+        # drone is stopped since there is an object detected while moving.
+        expected = decision_command.DecisionCommand.CommandType.STOP_MISSION_AND_HALT
+        result, command = decision_maker.run(object_within_proximity_limit_while_moving)
+        assert result
+        assert command.command == expected
+
+        # drone continues since the object disappears.
+        expected = decision_command.DecisionCommand.CommandType.RESUME_MISSION
+        result, command = decision_maker.run(object_outside_proximity_limit_while_stopped)
+        assert result
+        assert command.command == expected
+
+        # after object disappears, the flight interface may not have changed the mode yet but we should still stop sending commands.
+        expected = None
+        for _ in range(10):
+            result, command = decision_maker.run(object_outside_proximity_limit_while_stopped)
+            assert not result
+            assert command == expected
+
+        # drone stops since an object appears.
+        expected = decision_command.DecisionCommand.CommandType.STOP_MISSION_AND_HALT
+        result, command = decision_maker.run(object_within_proximity_limit_while_moving)
+        assert result
+        assert command.command == expected
+
+        # after object appears, the flight interface may not have changed the mode yet but we should still stop sending commands.
+        expected = None
+        for _ in range(10):
+            result, command = decision_maker.run(object_outside_proximity_limit_while_moving)
+            assert not result
+            assert command == expected