From 50c8a1abf0b117404da378da4b9c42dd24da6f1b Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 24 Nov 2023 16:01:10 -0500 Subject: [PATCH 01/24] created decision file --- modules/decision/decision.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 modules/decision/decision.py diff --git a/modules/decision/decision.py b/modules/decision/decision.py new file mode 100644 index 00000000..ee4e5f75 --- /dev/null +++ b/modules/decision/decision.py @@ -0,0 +1,29 @@ +from .. import decision_command +from ..cluster_estimation import cluster_estimation +from ..flight_interface import flight_interface + +class Decision: + + def __init__(self): + self.__best_landing_pad = 0 + self.__res1, self.__landing_pad_states = cluster_estimation.ClusterEstimation.run() + self.__res2, self.__current_pos = flight_interface.FlightInterface.run() + self.__weighted_pads = [] + + def distance_to_pad(self, pad): + """ + finds distance to landing pad based of current position + """ + + def weight_pads(self): + """ + weights the pads based on variance and distance + """ + def __find_best_pad(self): + """ + uses list of tuple(pad, weight) to determine best pad + """ + + def run(self, + states, variance) -> decision_command.DecisionCommand: + return decision_command.DecisionCommand.CommandType.LAND_AT_CURRENT_POSITION From 30010aa4102115d4cdfb4d10a3b45ccb749452fc Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Tue, 28 Nov 2023 10:11:59 -0500 Subject: [PATCH 02/24] created decision file --- modules/decision/decision.py | 52 +++++++++++++++++++++++++++--------- 1 file changed, 40 insertions(+), 12 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index ee4e5f75..0eea2c0e 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -1,29 +1,57 @@ from .. import decision_command -from ..cluster_estimation import cluster_estimation -from ..flight_interface import flight_interface +from .. import object_in_world +from .. import odometry_and_time -class Decision: +class Decision: def __init__(self): - self.__best_landing_pad = 0 - self.__res1, self.__landing_pad_states = cluster_estimation.ClusterEstimation.run() - self.__res2, self.__current_pos = flight_interface.FlightInterface.run() + self.__best_landing_pad = None self.__weighted_pads = [] - def distance_to_pad(self, pad): + @staticmethod + def distance_to_pad(pad: object_in_world.ObjectInWorld, + current_position: odometry_and_time.OdometryAndTime): """ - finds distance to landing pad based of current position + 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): + def weight_pads(self, + pads: list[object_in_world.ObjectInWorld], + current_position: odometry_and_time.OdometryAndTime): """ weights the pads based on variance and distance """ + self.__weighted_pads = [(pad, self.distance_to_pad(pad, current_position) / pad.spherical_variance) + for pad in pads] + def __find_best_pad(self): """ - uses list of tuple(pad, weight) to determine best pad + 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, variance) -> decision_command.DecisionCommand: - return decision_command.DecisionCommand.CommandType.LAND_AT_CURRENT_POSITION + states: odometry_and_time.OdometryAndTime, + pads: list[object_in_world.ObjectInWorld]) -> decision_command.DecisionCommand: + """ + 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: + # Assume we're returning a command to move to the best pad's position + return decision_command.DecisionCommand.create_land_at_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 land at current position if no best pad is found + return decision_command.DecisionCommand.create_land_at_current_position_command() From 47617284487ff73833e7f6699d38a46a1a3be2e2 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Wed, 31 Jan 2024 21:07:46 -0500 Subject: [PATCH 03/24] updates to decision_module --- modules/decision/decision.py | 47 ++++++++++++++++++++++++------------ 1 file changed, 31 insertions(+), 16 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index 0eea2c0e..0131c0bb 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -9,23 +9,34 @@ def __init__(self): self.__weighted_pads = [] @staticmethod - def distance_to_pad(pad: object_in_world.ObjectInWorld, - current_position: odometry_and_time.OdometryAndTime): + 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 + return (dx**2 + dy**2) ** 0.5 - def weight_pads(self, - pads: list[object_in_world.ObjectInWorld], - current_position: odometry_and_time.OdometryAndTime): + def weight_pads( + self, + pads: list[object_in_world.ObjectInWorld], + current_position: odometry_and_time.OdometryAndTime, + ): """ - weights the pads based on variance and distance + Weights the pads based on variance and distance. """ - self.__weighted_pads = [(pad, self.distance_to_pad(pad, current_position) / pad.spherical_variance) - for pad in pads] + epsilon = 1e-6 # Small value to prevent division by zero + self.__weighted_pads = [ + ( + pad, + self.distance_to_pad(pad, current_position) + / (pad.spherical_variance + epsilon), + ) + for pad in pads + ] def __find_best_pad(self): """ @@ -37,21 +48,25 @@ def __find_best_pad(self): 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]) -> decision_command.DecisionCommand: + def run( + self, + states: odometry_and_time.OdometryAndTime, + pads: list[object_in_world.ObjectInWorld], + ) -> decision_command.DecisionCommand: """ 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: - # Assume we're returning a command to move to the best pad's position - return decision_command.DecisionCommand.create_land_at_absolute_position_command( + # Command to move to best location + return 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 + -states.odometry_data.position.down, # Assuming down is negative for landing ) else: # Default to land at current position if no best pad is found - return decision_command.DecisionCommand.create_land_at_current_position_command() + return ( + decision_command.DecisionCommand.create_land_at_current_position_command() + ) From 0fd497cb041ed167c5e2d2856c932a14c7676d61 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Thu, 1 Feb 2024 11:08:59 -0500 Subject: [PATCH 04/24] updated weighting of pads --- modules/decision/decision.py | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index 0131c0bb..c1f47711 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -26,16 +26,21 @@ def weight_pads( current_position: odometry_and_time.OdometryAndTime, ): """ - Weights the pads based on variance and distance. + Weights the pads based on normalized variance and distance. """ - epsilon = 1e-6 # Small value to prevent division by zero + 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, - self.distance_to_pad(pad, current_position) - / (pad.spherical_variance + epsilon), - ) - for pad in pads + (pad, distance / max_distance + variance / max_variance) + for pad, distance, variance in zip(pads, distances, variances) ] def __find_best_pad(self): From 5c1c96fe97f3516d6aefd0e6940015b78bcdb36d Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Thu, 1 Feb 2024 20:14:05 -0500 Subject: [PATCH 05/24] added land at current location decision --- modules/decision/decision.py | 30 +++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index c1f47711..5fd635e3 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -4,9 +4,10 @@ class Decision: - def __init__(self): + def __init__(self, tolerance: float): self.__best_landing_pad = None self.__weighted_pads = [] + self.__distance_tolerance = tolerance @staticmethod def distance_to_pad( @@ -64,14 +65,21 @@ def run( self.weight_pads(pads, states) best_pad = self.__find_best_pad() if best_pad: - # Command to move to best location - return 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 - ) + 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 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 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 land at current position if no best pad is found - return ( - decision_command.DecisionCommand.create_land_at_current_position_command() - ) + # Default to hover if no pad is found + return (False, None) From f065c292aaacd82bcb3d480c865543e43c54add8 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Thu, 1 Feb 2024 20:47:07 -0500 Subject: [PATCH 06/24] added tests --- tests/test_decision.py | 135 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 135 insertions(+) create mode 100644 tests/test_decision.py diff --git a/tests/test_decision.py b/tests/test_decision.py new file mode 100644 index 00000000..9aaa439b --- /dev/null +++ b/tests/test_decision.py @@ -0,0 +1,135 @@ +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(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 + command = decision_maker.run(states, total_pads) + + assert isinstance(command, decision_command.DecisionCommand) + 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 + command = decision_maker.run(states, total_pads) + + assert isinstance(command, decision_command.DecisionCommand) + 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. + """ + command = decision_maker.run(states, []) + + assert isinstance(command, decision_command.DecisionCommand) + assert ( + command.get_command_type() + == decision_command.DecisionCommand.CommandType.LAND_AT_CURRENT_POSITION + ) From ca49f99fb690d3a24ead66bd044d65d07431db8c Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 2 Feb 2024 13:35:25 -0500 Subject: [PATCH 07/24] changes to testing --- tests/test_decision.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/tests/test_decision.py b/tests/test_decision.py index 9aaa439b..3b27af02 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -128,8 +128,5 @@ def test_decision_no_pads(self, decision_maker, states): """ command = decision_maker.run(states, []) - assert isinstance(command, decision_command.DecisionCommand) - assert ( - command.get_command_type() - == decision_command.DecisionCommand.CommandType.LAND_AT_CURRENT_POSITION - ) + assert isinstance(command, None) + assert None == None From 0eddb6b232279a3f6f32a84869d13b4b3183ee97 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 2 Feb 2024 13:38:06 -0500 Subject: [PATCH 08/24] changes to test file --- tests/test_decision.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_decision.py b/tests/test_decision.py index 3b27af02..98a40a92 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -1,5 +1,5 @@ import pytest -from modules.decision import Decision +from modules.decision import decision from modules import decision_command from modules import object_in_world from modules import odometry_and_time From b317cf3ffeccd0ade2e15524da9014a8ef77e9ad Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 2 Feb 2024 13:38:52 -0500 Subject: [PATCH 09/24] updates to decision test --- tests/test_decision.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_decision.py b/tests/test_decision.py index 98a40a92..60dba39f 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -14,7 +14,7 @@ def decision_maker(): """ Construct a Decision instance with predefined tolerance. """ - decision_instance = Decision(TOLERANCE) + decision_instance = decision.Decision(TOLERANCE) yield decision_instance From 01d99cf183340e27e8b02982cb5e4e1846bad0fb Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 2 Feb 2024 13:49:07 -0500 Subject: [PATCH 10/24] type error fixed --- modules/decision/decision.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index 5fd635e3..e46389d1 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -23,7 +23,7 @@ def distance_to_pad( def weight_pads( self, - pads: list[object_in_world.ObjectInWorld], + pads: "list[object_in_world.ObjectInWorld]", current_position: odometry_and_time.OdometryAndTime, ): """ @@ -57,7 +57,7 @@ def __find_best_pad(self): def run( self, states: odometry_and_time.OdometryAndTime, - pads: list[object_in_world.ObjectInWorld], + pads: "list[object_in_world.ObjectInWorld]", ) -> decision_command.DecisionCommand: """ Determine the best landing pad and issue a command to land there. From 66f0824353116dc3dd9220dae75b98958fc2abf4 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 2 Feb 2024 13:58:45 -0500 Subject: [PATCH 11/24] fixed test cases --- modules/decision/decision.py | 26 ++++++++++++++++---------- tests/test_decision.py | 14 +++++++------- 2 files changed, 23 insertions(+), 17 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index e46389d1..b765bdba 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -58,7 +58,7 @@ def run( self, states: odometry_and_time.OdometryAndTime, pads: "list[object_in_world.ObjectInWorld]", - ) -> decision_command.DecisionCommand: + ): """ Determine the best landing pad and issue a command to land there. """ @@ -68,18 +68,24 @@ def run( 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 decision_command.DecisionCommand.create_land_at_absolute_position_command( - best_pad.position_x, - best_pad.position_y, - states.odometry_data.position.down, + 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 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 + 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) + return False, None diff --git a/tests/test_decision.py b/tests/test_decision.py index 60dba39f..f7c3aa1c 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -99,9 +99,9 @@ def test_decision_within_tolerance( Test decision making when the best pad is within tolerance. """ total_pads = [best_pad_within_tolerance] + pads - command = decision_maker.run(states, total_pads) + res, command = decision_maker.run(states, total_pads) - assert isinstance(command, decision_command.DecisionCommand) + assert res assert ( command.get_command_type() == decision_command.DecisionCommand.CommandType.LAND_AT_ABSOLUTE_POSITION @@ -114,9 +114,9 @@ def test_decision_outside_tolerance( Test decision making when the best pad is outside tolerance. """ total_pads = [best_pad_outside_tolerance] + pads - command = decision_maker.run(states, total_pads) + res, command = decision_maker.run(states, total_pads) - assert isinstance(command, decision_command.DecisionCommand) + assert res assert ( command.get_command_type() == decision_command.DecisionCommand.CommandType.MOVE_TO_ABSOLUTE_POSITION @@ -126,7 +126,7 @@ def test_decision_no_pads(self, decision_maker, states): """ Test decision making when no pads are available. """ - command = decision_maker.run(states, []) + res, command = decision_maker.run(states, []) - assert isinstance(command, None) - assert None == None + assert res == False + assert command is None # when no pads found From ec284222e9fe78367094433e6a5e6038346073b8 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 2 Feb 2024 14:03:03 -0500 Subject: [PATCH 12/24] fixed if no pads found --- modules/decision/decision.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index b765bdba..1b6decc7 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -29,6 +29,9 @@ def weight_pads( """ 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] From 50b60d99671ffcb61392fa51e1c84ceeb30e67e4 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Thu, 8 Feb 2024 17:01:38 -0500 Subject: [PATCH 13/24] updates from pr comments --- modules/decision/decision.py | 45 ++++++++++++++++++++++++------------ tests/test_decision.py | 3 +++ 2 files changed, 33 insertions(+), 15 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index 1b6decc7..b21184f4 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -1,9 +1,17 @@ +""" +Creates decision for next action based on current state and detected pads +""" + from .. import decision_command from .. import object_in_world from .. import odometry_and_time class Decision: + """ + Weighs distance to located pads and variance to choose the next action, either to land or move to the nearest pad + """ + def __init__(self, tolerance: float): self.__best_landing_pad = None self.__weighted_pads = [] @@ -21,7 +29,7 @@ def distance_to_pad( dy = pad.position_y - current_position.odometry_data.position.east return (dx**2 + dy**2) ** 0.5 - def weight_pads( + def __weight_pads( self, pads: "list[object_in_world.ObjectInWorld]", current_position: odometry_and_time.OdometryAndTime, @@ -35,12 +43,18 @@ def weight_pads( 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 + max_distance = max(distances) + + max_variance = max(variances) + + # if max distance is 0, assumes target pad is directly below, should land + if max_distance == 0: + self.__weighted_pads = [(pads[0], 0)] + return None + + # if all variance is 0, no pads are found + if max_variance == 0: + return None self.__weighted_pads = [ (pad, distance / max_distance + variance / max_variance) @@ -59,36 +73,37 @@ def __find_best_pad(self): def run( self, - states: odometry_and_time.OdometryAndTime, + curr_state: 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) + self.__weight_pads(pads, curr_state) best_pad = self.__find_best_pad() if best_pad: - distance_to_best_bad = self.distance_to_pad(best_pad, states) + distance_to_best_bad = self.distance_to_pad(best_pad, curr_state) + + # Issue a landing command if within tolerance 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, + curr_state.odometry_data.position.down, ), ) + # Move to best location if not within tolerance 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 + -curr_state.odometry_data.position.down, # Assuming down is negative for landing ), ) + # Default to do nothing if no pads are found else: - # Default to hover if no pad is found return False, None diff --git a/tests/test_decision.py b/tests/test_decision.py index f7c3aa1c..b23a4eae 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -1,10 +1,13 @@ 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 From aacb75a58ddafd3ba0ca1dfe1ca9cb16a35a8571 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Thu, 8 Feb 2024 17:02:29 -0500 Subject: [PATCH 14/24] changes --- tests/test_decision.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/tests/test_decision.py b/tests/test_decision.py index b23a4eae..0bca5b16 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -1,3 +1,7 @@ +""" +Tests the decision class +""" + import pytest @@ -8,8 +12,7 @@ from modules import drone_odometry_local -# Test parameters -TOLERANCE = 2 +TOLERANCE = 2 # Test parameters @pytest.fixture() From 2922496b0b276cd01fdd80566911f929a5387906 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 9 Feb 2024 15:06:55 -0500 Subject: [PATCH 15/24] changes to pr --- modules/decision/decision.py | 90 +++++++++++++++++------------------- tests/test_decision.py | 63 +++++++++++++------------ 2 files changed, 76 insertions(+), 77 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index b21184f4..8086a878 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -1,5 +1,5 @@ """ -Creates decision for next action based on current state and detected pads +Creates decision for next action based on current state and detected pads. """ from .. import decision_command @@ -7,9 +7,15 @@ from .. import odometry_and_time +class ScoredLandingPad: + def __init__(self, landing_pad: object_in_world.ObjectInWorld, score: float): + self.landing_pad = landing_pad + self.score = score + + class Decision: """ - Weighs distance to located pads and variance to choose the next action, either to land or move to the nearest pad + Chooses next action to take based on known landing pad information """ def __init__(self, tolerance: float): @@ -18,79 +24,70 @@ def __init__(self, tolerance: float): self.__distance_tolerance = tolerance @staticmethod - def distance_to_pad( - pad: object_in_world.ObjectInWorld, - current_position: odometry_and_time.OdometryAndTime, - ): + def __distance_to_pad(pad: object_in_world.ObjectInWorld, + current_position: odometry_and_time.OdometryAndTime) -> float: """ - Calculate Euclidean distance to landing pad based on current position. + Calculate squared 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 + return dx**2 + dy**2 - def __weight_pads( - self, - pads: "list[object_in_world.ObjectInWorld]", - current_position: odometry_and_time.OdometryAndTime, - ): + def __weight_pads(self, + pads: "list[object_in_world.ObjectInWorld]", + current_position: odometry_and_time.OdometryAndTime) -> "list[ScoredLandingPad]" | None: """ Weights the pads based on normalized variance and distance. """ - if not pads: + if len(pads) == 0: return None - distances = [self.distance_to_pad(pad, current_position) for pad in pads] + distances = [self.__distance_to_pad(pad, current_position) for pad in pads] variances = [pad.spherical_variance for pad in pads] max_distance = max(distances) max_variance = max(variances) - - # if max distance is 0, assumes target pad is directly below, should land - if max_distance == 0: - self.__weighted_pads = [(pads[0], 0)] - return None - - # if all variance is 0, no pads are found - if max_variance == 0: - return None - - self.__weighted_pads = [ - (pad, distance / max_distance + variance / max_variance) + + # if all variance is zero, assumes no significant difference amongst pads + # if max_distance is zero, assumes landing pad is directly below + if max_variance == 0 or max_distance == 0: + return [ScoredLandingPad(pad, 0) for pad in pads] + + return [ + ScoredLandingPad(pad, distance / max_distance + variance / max_variance) for pad, distance, variance in zip(pads, distances, variances) ] - def __find_best_pad(self): + @staticmethod + def __find_best_pad(weighted_pads: "list[ScoredLandingPad]") -> object_in_world.ObjectInWorld: """ Determine the best pad to land on based on the weighted scores. """ - if not self.__weighted_pads: + if len(weighted_pads) == 0: 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 + best_landing_pad = min(weighted_pads, key=lambda x: x[1])[0] + return best_landing_pad - def run( - self, - curr_state: odometry_and_time.OdometryAndTime, - pads: "list[object_in_world.ObjectInWorld]", - ): + def run(self, + curr_state: odometry_and_time.OdometryAndTime, + pads: "list[object_in_world.ObjectInWorld]") -> "tuple[bool, decision_command.DecisionCommand | None]": """ Determine the best landing pad and issue a command to land there. """ - self.__weight_pads(pads, curr_state) - best_pad = self.__find_best_pad() - if best_pad: - distance_to_best_bad = self.distance_to_pad(best_pad, curr_state) + self.__weighted_pads = self.__weight_pads(pads, curr_state) + self.__best_landing_pad = self.__find_best_pad(self.__weighted_pads) + if self.__best_landing_pad: + distance_to_best_bad = self.__distance_to_pad(self.__best_landing_pad, curr_state) # Issue a landing command if within tolerance if distance_to_best_bad <= self.__distance_tolerance: return ( True, decision_command.DecisionCommand.create_land_at_absolute_position_command( - best_pad.position_x, - best_pad.position_y, + self.__best_landing_pad.position_x, + self.__best_landing_pad.position_y, curr_state.odometry_data.position.down, ), ) @@ -99,11 +96,10 @@ def run( return ( True, decision_command.DecisionCommand.create_move_to_absolute_position_command( - best_pad.position_x, - best_pad.position_y, + self.__best_landing_pad.position_x, + self.__best_landing_pad.position_y, -curr_state.odometry_data.position.down, # Assuming down is negative for landing ), ) - # Default to do nothing if no pads are found - else: - return False, None + # Default to do nothing if no pads are found + return False, None diff --git a/tests/test_decision.py b/tests/test_decision.py index 0bca5b16..25a9bb73 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -12,7 +12,7 @@ from modules import drone_odometry_local -TOLERANCE = 2 # Test parameters +LANDING_PAD_LOCATION_TOLERANCE = 2 # Test parameters @pytest.fixture() @@ -20,11 +20,10 @@ def decision_maker(): """ Construct a Decision instance with predefined tolerance. """ - decision_instance = decision.Decision(TOLERANCE) + decision_instance = decision.Decision(LANDING_PAD_LOCATION_TOLERANCE) yield decision_instance -# Fixture for a pad within tolerance @pytest.fixture() def best_pad_within_tolerance(): """ @@ -37,26 +36,24 @@ def best_pad_within_tolerance(): position_x, position_y, spherical_variance ) assert success - return pad + yield pad -# Fixture for a pad outside tolerance @pytest.fixture() def best_pad_outside_tolerance(): """ - Create a mock ObjectInWorld instance outside tolerance. + Creates an ObjectInWorld instance outside of distance to pad tolerance. """ position_x = 100.0 position_y = 200.0 spherical_variance = 5.0 # variance outside tolerance - success, pad = object_in_world.ObjectInWorld.create( + result, pad = object_in_world.ObjectInWorld.create( position_x, position_y, spherical_variance ) - assert success - return pad + assert result + yield pad -# Fixture for a list of pads @pytest.fixture() def pads(): """ @@ -65,10 +62,9 @@ def pads(): 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] + yield [pad1, pad2, pad3] -# Fixture for odometry and time states @pytest.fixture() def states(): """ @@ -88,9 +84,9 @@ def states(): )[1] # Creating the OdometryAndTime instance with current time stamp - success, state = odometry_and_time.OdometryAndTime.create(odometry_data) - assert success - return state + result, state = odometry_and_time.OdometryAndTime.create(odometry_data) + assert result + yield state class TestDecision: @@ -98,41 +94,48 @@ class TestDecision: Tests for the Decision.run() method. """ - def test_decision_within_tolerance( - self, decision_maker, best_pad_within_tolerance, pads, states - ): + 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) + result, command = decision_maker.run(states, total_pads) - assert res + assert result assert ( - command.get_command_type() + command == decision_command.DecisionCommand.CommandType.LAND_AT_ABSOLUTE_POSITION ) - def test_decision_outside_tolerance( - self, decision_maker, best_pad_outside_tolerance, pads, states - ): + 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) + result, command = decision_maker.run(states, total_pads) - assert res + assert result assert ( - command.get_command_type() + command == decision_command.DecisionCommand.CommandType.MOVE_TO_ABSOLUTE_POSITION ) - def test_decision_no_pads(self, decision_maker, states): + def test_decision_no_pads(self, + decision_maker, + states): """ Test decision making when no pads are available. """ - res, command = decision_maker.run(states, []) + result, command = decision_maker.run(states, []) - assert res == False + assert result == False assert command is None # when no pads found + \ No newline at end of file From 1d1b1a2a99718c2985a9fe338cae5b6cffd300ff Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 9 Feb 2024 15:07:42 -0500 Subject: [PATCH 16/24] changes --- tests/test_decision.py | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/tests/test_decision.py b/tests/test_decision.py index 25a9bb73..50d254cc 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -66,7 +66,7 @@ def pads(): @pytest.fixture() -def states(): +def state(): """ Create a mock OdometryAndTime instance with the drone positioned within tolerance of the landing pad. """ @@ -91,51 +91,51 @@ def states(): class TestDecision: """ - Tests for the Decision.run() method. - """ + Tests for the Decision.run() method and weight and distance methods + """ def test_decision_within_tolerance(self, decision_maker, best_pad_within_tolerance, pads, - states): + state): """ Test decision making when the best pad is within tolerance. """ + expected = decision_command.DecisionCommand.CommandType.LAND_AT_ABSOLUTE_POSITION total_pads = [best_pad_within_tolerance] + pads - result, command = decision_maker.run(states, total_pads) + + result, actual = decision_maker.run(state, total_pads) assert result - assert ( - command - == decision_command.DecisionCommand.CommandType.LAND_AT_ABSOLUTE_POSITION - ) + assert actual == expected def test_decision_outside_tolerance(self, decision_maker, best_pad_outside_tolerance, pads, - states): + state): """ Test decision making when the best pad is outside tolerance. """ + expected = decision_command.DecisionCommand.CommandType.MOVE_TO_ABSOLUTE_POSITION total_pads = [best_pad_outside_tolerance] + pads - result, command = decision_maker.run(states, total_pads) + + result, actual = decision_maker.run(state, total_pads) assert result - assert ( - command - == decision_command.DecisionCommand.CommandType.MOVE_TO_ABSOLUTE_POSITION - ) + assert actual == expected def test_decision_no_pads(self, decision_maker, - states): + state): """ Test decision making when no pads are available. """ - result, command = decision_maker.run(states, []) + expected = None + + result, actual = decision_maker.run(state, []) assert result == False - assert command is None # when no pads found + assert actual == expected \ No newline at end of file From a6d19ec395db09a6fc540b12534ecaf01929fc91 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 9 Feb 2024 15:15:21 -0500 Subject: [PATCH 17/24] fixed type issue --- modules/decision/decision.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index 8086a878..496c968e 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -35,7 +35,7 @@ def __distance_to_pad(pad: object_in_world.ObjectInWorld, def __weight_pads(self, pads: "list[object_in_world.ObjectInWorld]", - current_position: odometry_and_time.OdometryAndTime) -> "list[ScoredLandingPad]" | None: + current_position: odometry_and_time.OdometryAndTime) -> "list[ScoredLandingPad] | None": """ Weights the pads based on normalized variance and distance. """ @@ -54,7 +54,7 @@ def __weight_pads(self, if max_variance == 0 or max_distance == 0: return [ScoredLandingPad(pad, 0) for pad in pads] - return [ + return True, [ ScoredLandingPad(pad, distance / max_distance + variance / max_variance) for pad, distance, variance in zip(pads, distances, variances) ] @@ -76,7 +76,7 @@ def run(self, """ Determine the best landing pad and issue a command to land there. """ - self.__weighted_pads = self.__weight_pads(pads, curr_state) + result, self.__weighted_pads = self.__weight_pads(pads, curr_state) self.__best_landing_pad = self.__find_best_pad(self.__weighted_pads) if self.__best_landing_pad: distance_to_best_bad = self.__distance_to_pad(self.__best_landing_pad, curr_state) From d213c31bd421a70c010e4b6a5ae0510d65cca031 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 9 Feb 2024 15:24:33 -0500 Subject: [PATCH 18/24] updated type error in the None case --- modules/decision/decision.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index 496c968e..18f43713 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -67,7 +67,7 @@ def __find_best_pad(weighted_pads: "list[ScoredLandingPad]") -> object_in_world. if len(weighted_pads) == 0: return None # Find the pad with the smallest weight as the best pad - best_landing_pad = min(weighted_pads, key=lambda x: x[1])[0] + best_landing_pad = min(weighted_pads, key=lambda pad: pad.score).landing_pad return best_landing_pad def run(self, @@ -76,7 +76,11 @@ def run(self, """ Determine the best landing pad and issue a command to land there. """ - result, self.__weighted_pads = self.__weight_pads(pads, curr_state) + self.__weighted_pads = self.__weight_pads(pads, curr_state) + + if not self.__weighted_pads: + return False, None + self.__best_landing_pad = self.__find_best_pad(self.__weighted_pads) if self.__best_landing_pad: distance_to_best_bad = self.__distance_to_pad(self.__best_landing_pad, curr_state) From 7c351b6b0c40abd111510ca45c0d8a3317f72ee6 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 9 Feb 2024 15:30:25 -0500 Subject: [PATCH 19/24] fixed bool error --- modules/decision/decision.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index 18f43713..e6a63e6e 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -54,7 +54,7 @@ def __weight_pads(self, if max_variance == 0 or max_distance == 0: return [ScoredLandingPad(pad, 0) for pad in pads] - return True, [ + return [ ScoredLandingPad(pad, distance / max_distance + variance / max_variance) for pad, distance, variance in zip(pads, distances, variances) ] From 5220d6640c048d24decdc99e7b00923c2fc0a3e7 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Fri, 9 Feb 2024 15:35:03 -0500 Subject: [PATCH 20/24] fixed command type error --- tests/test_decision.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_decision.py b/tests/test_decision.py index 50d254cc..ce5eec2c 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -108,7 +108,7 @@ def test_decision_within_tolerance(self, result, actual = decision_maker.run(state, total_pads) assert result - assert actual == expected + assert actual.get_command_type() == expected def test_decision_outside_tolerance(self, decision_maker, @@ -124,7 +124,7 @@ def test_decision_outside_tolerance(self, result, actual = decision_maker.run(state, total_pads) assert result - assert actual == expected + assert actual.get_command_type() == expected def test_decision_no_pads(self, decision_maker, From 1ef6b132d31ac0e0ebfddf0cf2b883c7e03020ae Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Sat, 10 Feb 2024 17:57:53 -0500 Subject: [PATCH 21/24] made additional formatting changes --- modules/decision/decision.py | 1 + tests/test_decision.py | 13 ++++--------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index e6a63e6e..cc7c0d71 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -107,3 +107,4 @@ def run(self, ) # Default to do nothing if no pads are found return False, None + diff --git a/tests/test_decision.py b/tests/test_decision.py index ce5eec2c..62fa6e7e 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -71,17 +71,11 @@ def state(): 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 + position = drone_odometry_local.DronePositionLocal.create(9.0, 19.0, -5.0)[1] - orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0)[ - 1 - ] + orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0)[1] - odometry_data = drone_odometry_local.DroneOdometryLocal.create( - position, orientation - )[1] + odometry_data = drone_odometry_local.DroneOdometryLocal.create(position, orientation)[1] # Creating the OdometryAndTime instance with current time stamp result, state = odometry_and_time.OdometryAndTime.create(odometry_data) @@ -138,4 +132,5 @@ def test_decision_no_pads(self, assert result == False assert actual == expected + \ No newline at end of file From 21e8486f58f404878d86958967f948ece3524c37 Mon Sep 17 00:00:00 2001 From: Jivan Kesan <117189726+jivankesan@users.noreply.github.com> Date: Sat, 10 Feb 2024 18:04:25 -0500 Subject: [PATCH 22/24] Update test_decision.py --- tests/test_decision.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/test_decision.py b/tests/test_decision.py index 62fa6e7e..fdcdbce6 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -133,4 +133,3 @@ def test_decision_no_pads(self, assert result == False assert actual == expected - \ No newline at end of file From a70fc53f5da1961634e4ebbd012ca825bb514772 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Sat, 10 Feb 2024 18:19:48 -0500 Subject: [PATCH 23/24] fixed mention of mock --- tests/test_decision.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/test_decision.py b/tests/test_decision.py index fdcdbce6..d73a08ee 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -27,7 +27,7 @@ def decision_maker(): @pytest.fixture() def best_pad_within_tolerance(): """ - Create a mock ObjectInWorld instance within tolerance. + Create an ObjectInWorld instance within distance to pad tolerance. """ position_x = 10.0 position_y = 20.0 @@ -57,7 +57,7 @@ def best_pad_outside_tolerance(): @pytest.fixture() def pads(): """ - Create a list of mock ObjectInWorld instances. + Create a list of ObjectInWorld instances for the landing pads. """ 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] @@ -68,7 +68,7 @@ def pads(): @pytest.fixture() def state(): """ - Create a mock OdometryAndTime instance with the drone positioned within tolerance of the landing pad. + Create an 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] From 10bba283d467aee9bf8f7fc8311b3a1ca50b4ad7 Mon Sep 17 00:00:00 2001 From: JivanKesan Date: Sat, 10 Feb 2024 18:22:17 -0500 Subject: [PATCH 24/24] fixed test structure --- tests/test_decision.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_decision.py b/tests/test_decision.py index d73a08ee..9e0a9833 100644 --- a/tests/test_decision.py +++ b/tests/test_decision.py @@ -32,10 +32,10 @@ def best_pad_within_tolerance(): position_x = 10.0 position_y = 20.0 spherical_variance = 1.0 - success, pad = object_in_world.ObjectInWorld.create( + result, pad = object_in_world.ObjectInWorld.create( position_x, position_y, spherical_variance ) - assert success + assert result yield pad