Decision module

modules/decision/decision.py

@@ -0,0 +1,94 @@
+from .. import decision_command
+from .. import object_in_world
+from .. import odometry_and_time
+
+
+class Decision:
+    def __init__(self, tolerance: float):
+        self.__best_landing_pad = None
+        self.__weighted_pads = []
+        self.__distance_tolerance = tolerance
+
+    @staticmethod
+    def distance_to_pad(
+        pad: object_in_world.ObjectInWorld,
+        current_position: odometry_and_time.OdometryAndTime,
+    ):
+        """
+        Calculate Euclidean distance to landing pad based on current position.
+        """
+        dx = pad.position_x - current_position.odometry_data.position.north
+        dy = pad.position_y - current_position.odometry_data.position.east
+        return (dx**2 + dy**2) ** 0.5
+
+    def weight_pads(
+        self,
+        pads: "list[object_in_world.ObjectInWorld]",
+        current_position: odometry_and_time.OdometryAndTime,
+    ):
+        """
+        Weights the pads based on normalized variance and distance.
+        """
+        if not pads:
+            return None
+
+        distances = [self.distance_to_pad(pad, current_position) for pad in pads]
+        variances = [pad.spherical_variance for pad in pads]
+
+        max_distance = (
+            max(distances) or 1
+        )  # Avoid division by zero if all distances are zero
+        max_variance = (
+            max(variances) or 1
+        )  # Avoid division by zero if all variances are zero
+
+        self.__weighted_pads = [
+            (pad, distance / max_distance + variance / max_variance)
+            for pad, distance, variance in zip(pads, distances, variances)
+        ]
+
+    def __find_best_pad(self):
+        """
+        Determine the best pad to land on based on the weighted scores.
+        """
+        if not self.__weighted_pads:
+            return None
+        # Find the pad with the smallest weight as the best pad
+        self.__best_landing_pad = min(self.__weighted_pads, key=lambda x: x[1])[0]
+        return self.__best_landing_pad
+
+    def run(
+        self,
+        states: odometry_and_time.OdometryAndTime,
+        pads: "list[object_in_world.ObjectInWorld]",
+    ):
+        """
+        Determine the best landing pad and issue a command to land there.
+        """
+        self.weight_pads(pads, states)
+        best_pad = self.__find_best_pad()
+        if best_pad:
+            distance_to_best_bad = self.distance_to_pad(best_pad, states)
+            if distance_to_best_bad <= self.__distance_tolerance:
+                # Issue a landing command if within tolerance
+                return (
+                    True,
+                    decision_command.DecisionCommand.create_land_at_absolute_position_command(
+                        best_pad.position_x,
+                        best_pad.position_y,
+                        states.odometry_data.position.down,
+                    ),
+                )
+            else:
+                # Move to best location if not within tolerance
+                return (
+                    True,
+                    decision_command.DecisionCommand.create_move_to_absolute_position_command(
+                        best_pad.position_x,
+                        best_pad.position_y,
+                        -states.odometry_data.position.down,  # Assuming down is negative for landing
+                    ),
+                )
+        else:
+            # Default to hover if no pad is found
+            return False, None

tests/test_decision.py

@@ -0,0 +1,132 @@
+import pytest
+from modules.decision import decision
+from modules import decision_command
+from modules import object_in_world
+from modules import odometry_and_time
+from modules import drone_odometry_local
+
+# Test parameters
+TOLERANCE = 2
+
+
+@pytest.fixture()
+def decision_maker():
+    """
+    Construct a Decision instance with predefined tolerance.
+    """
+    decision_instance = decision.Decision(TOLERANCE)
+    yield decision_instance
+
+
+# Fixture for a pad within tolerance
+@pytest.fixture()
+def best_pad_within_tolerance():
+    """
+    Create a mock ObjectInWorld instance within tolerance.
+    """
+    position_x = 10.0
+    position_y = 20.0
+    spherical_variance = 1.0
+    success, pad = object_in_world.ObjectInWorld.create(
+        position_x, position_y, spherical_variance
+    )
+    assert success
+    return pad
+
+
+# Fixture for a pad outside tolerance
+@pytest.fixture()
+def best_pad_outside_tolerance():
+    """
+    Create a mock ObjectInWorld instance outside tolerance.
+    """
+    position_x = 100.0
+    position_y = 200.0
+    spherical_variance = 5.0  # variance outside tolerance
+    success, pad = object_in_world.ObjectInWorld.create(
+        position_x, position_y, spherical_variance
+    )
+    assert success
+    return pad
+
+
+# Fixture for a list of pads
+@pytest.fixture()
+def pads():
+    """
+    Create a list of mock ObjectInWorld instances.
+    """
+    pad1 = object_in_world.ObjectInWorld.create(30.0, 40.0, 2.0)[1]
+    pad2 = object_in_world.ObjectInWorld.create(50.0, 60.0, 3.0)[1]
+    pad3 = object_in_world.ObjectInWorld.create(70.0, 80.0, 4.0)[1]
+    return [pad1, pad2, pad3]
+
+
+# Fixture for odometry and time states
+@pytest.fixture()
+def states():
+    """
+    Create a mock OdometryAndTime instance with the drone positioned within tolerance of the landing pad.
+    """
+    # Creating the position within tolerance of the specified landing pad.
+    position = drone_odometry_local.DronePositionLocal.create(9.0, 19.0, -5.0)[
+        1
+    ]  # Example altitude of -5 meters
+
+    orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0)[
+        1
+    ]
+
+    odometry_data = drone_odometry_local.DroneOdometryLocal.create(
+        position, orientation
+    )[1]
+
+    # Creating the OdometryAndTime instance with current time stamp
+    success, state = odometry_and_time.OdometryAndTime.create(odometry_data)
+    assert success
+    return state
+
+
+class TestDecision:
+    """
+    Tests for the Decision.run() method.
+    """
+
+    def test_decision_within_tolerance(
+        self, decision_maker, best_pad_within_tolerance, pads, states
+    ):
+        """
+        Test decision making when the best pad is within tolerance.
+        """
+        total_pads = [best_pad_within_tolerance] + pads
+        res, command = decision_maker.run(states, total_pads)
+
+        assert res
+        assert (
+            command.get_command_type()
+            == decision_command.DecisionCommand.CommandType.LAND_AT_ABSOLUTE_POSITION
+        )
+
+    def test_decision_outside_tolerance(
+        self, decision_maker, best_pad_outside_tolerance, pads, states
+    ):
+        """
+        Test decision making when the best pad is outside tolerance.
+        """
+        total_pads = [best_pad_outside_tolerance] + pads
+        res, command = decision_maker.run(states, total_pads)
+
+        assert res
+        assert (
+            command.get_command_type()
+            == decision_command.DecisionCommand.CommandType.MOVE_TO_ABSOLUTE_POSITION
+        )
+
+    def test_decision_no_pads(self, decision_maker, states):
+        """
+        Test decision making when no pads are available.
+        """
+        res, command = decision_maker.run(states, [])
+
+        assert res == False
+        assert command is None  # when no pads found