From 6b5c530c53c106decefef7bcbe646fa93f556253 Mon Sep 17 00:00:00 2001 From: krish Date: Sat, 1 Feb 2025 15:11:01 -0500 Subject: [PATCH] Finished task 2, 3, 4 --- .DS_Store | Bin 0 -> 6148 bytes modules/bootcamp/decision_simple_waypoint.py | 29 +++++-- .../decision_waypoint_landing_pads.py | 74 +++++++++++++++++- .../bootcamp/tests/run_decision_example.py | 4 +- .../tests/run_decision_simple_waypoint.py | 4 +- requirements.txt | 2 +- 6 files changed, 99 insertions(+), 14 deletions(-) create mode 100644 .DS_Store diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..50af76e2858114c4f30818f25871330a3754ab42 GIT binary patch literal 6148 zcmeHK%}T>S5Z-NTn^J@v6nb3nTCkcbh?fxS3mDOZN^MBcV45vWY7eE5v%Zi|;`2DO zyEy~{-bCyS?0&QJvzz%K`@+2fo36EgaCm+-c}ZreeA7g7;9SYB!4lpU74{$}o>vxqQ5EwK~{^3TNEaNIfw?3{)9t>Y_JiHZ2eXqp0xtnBQz9@D^LLey>bbF0q!Gx<fmK W3ynAn+EqFrT?7;%)DZ)}z`z%a-%Kk2 literal 0 HcmV?d00001 diff --git a/modules/bootcamp/decision_simple_waypoint.py b/modules/bootcamp/decision_simple_waypoint.py index 26098c2e..dfa4b754 100644 --- a/modules/bootcamp/decision_simple_waypoint.py +++ b/modules/bootcamp/decision_simple_waypoint.py @@ -29,7 +29,6 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non Initialize all persistent variables here with self. """ self.waypoint = waypoint - print(f"Waypoint: {waypoint}") self.acceptance_radius = acceptance_radius @@ -37,8 +36,6 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Add your own - # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ @@ -68,10 +65,30 @@ def run( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Do something based on the report and the state of this class... - + status = report.status + position = report.position + + distance_from_waypoint = (self.waypoint.location_x - position.location_x) ** 2 + ( + self.waypoint.location_y - position.location_y + ) ** 2 + + # check if status is HALTED; if it is, set the relative destination + if status == drone_status.DroneStatus.HALTED: + # if the drone is in the acceptance radius, we have reached + if distance_from_waypoint < (self.acceptance_radius**2): + return commands.Command.create_land_command() + relative_destination = location.Location( + self.waypoint.location_x - position.location_x, + self.waypoint.location_y - position.location_y, + ) + return commands.Command.create_set_relative_destination_command( + relative_destination.location_x, relative_destination.location_y + ) + # if the drone is in the acceptance radius, we have reached + if distance_from_waypoint < (self.acceptance_radius**2): + return commands.Command.create_halt_command() # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ - return command + return command \ No newline at end of file diff --git a/modules/bootcamp/decision_waypoint_landing_pads.py b/modules/bootcamp/decision_waypoint_landing_pads.py index ade6f118..f6ed8195 100644 --- a/modules/bootcamp/decision_waypoint_landing_pads.py +++ b/modules/bootcamp/decision_waypoint_landing_pads.py @@ -13,12 +13,27 @@ from .. import location from ..private.decision import base_decision - # Disable for bootcamp use # No enable # pylint: disable=duplicate-code,unused-argument +def calculate_distance(position: location.Location, landing_pad: location.Location) -> float: + """ + Calculate the Euclidean distance between a given position and a landing pad. + + Args: + position (object): An object with attributes `location_x` and `location_y` representing the coordinates of the position. + landing_pad (object): An object with attributes `location_x` and `location_y` representing the coordinates of the landing pad. + + Returns: + float: The Euclidean distance between the position and the landing pad. + """ + return (landing_pad.location_x - position.location_x) ** 2 + ( + landing_pad.location_y - position.location_y + ) ** 2 + + class DecisionWaypointLandingPads(base_decision.BaseDecision): """ Travel to the designed waypoint and then land at the nearest landing pad. @@ -39,6 +54,11 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non # Add your own + # a boolean to see if the drone has reached the waypoint + self.reached = False + + self.nearest_landing_pad = None + # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ @@ -68,10 +88,58 @@ def run( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Do something based on the report and the state of this class... + status = report.status + position = report.position + + if not self.reached: + distance_from_waypoint = (self.waypoint.location_x - position.location_x) ** 2 + ( + self.waypoint.location_y - position.location_y + ) ** 2 + + # if the drone is in the acceptance radius, we have reached + if distance_from_waypoint < (self.acceptance_radius**2): + self.reached = True + self.nearest_landing_pad = min( + landing_pad_locations, + key=lambda landing_pad: calculate_distance(position, landing_pad), + ) + relative_destination = location.Location( + self.nearest_landing_pad.location_x - position.location_x, + self.nearest_landing_pad.location_y - position.location_y, + ) + return commands.Command.create_set_relative_destination_command( + relative_destination.location_x, relative_destination.location_y + ) + # return commands.Command.create_halt_command() + # otherwise, the drone has not reached in the acceptance radius + # If the drone is not at the waypoint yet + if status == drone_status.DroneStatus.HALTED: + relative_destination = location.Location( + self.waypoint.location_x - position.location_x, + self.waypoint.location_y - position.location_y, + ) + return commands.Command.create_set_relative_destination_command( + relative_destination.location_x, relative_destination.location_y + ) + return command + distance_to_pad = calculate_distance(position, self.nearest_landing_pad) + # if we are within the acceptance radius of the landing pad, we land + if distance_to_pad < (self.acceptance_radius**2): + if status == drone_status.DroneStatus.HALTED: + return commands.Command.create_land_command() + return commands.Command.create_halt_command() + # if we are not within the acceptance radius of the landing pad, we head towards it + if status == drone_status.DroneStatus.HALTED: + relative_destination = location.Location( + self.nearest_landing_pad.location_x - position.location_x, + self.nearest_landing_pad.location_y - position.location_y, + ) + return commands.Command.create_set_relative_destination_command( + relative_destination.location_x, relative_destination.location_y + ) # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ - return command + return command \ No newline at end of file diff --git a/modules/bootcamp/tests/run_decision_example.py b/modules/bootcamp/tests/run_decision_example.py index 7bf681ba..7b09cc91 100644 --- a/modules/bootcamp/tests/run_decision_example.py +++ b/modules/bootcamp/tests/run_decision_example.py @@ -21,7 +21,7 @@ # to reach the 1st command # Increase the step size if your computer is lagging # Larger step size is smaller FPS -TIME_STEP_SIZE = 0.1 # seconds +TIME_STEP_SIZE = 0.2 # seconds # OpenCV ignores your display settings, # so if the window is too small or too large, @@ -51,4 +51,4 @@ def main() -> int: if result_main != 0: print(f"ERROR: Status code: {result_main}") - print("Done!") + print("Done!") \ No newline at end of file diff --git a/modules/bootcamp/tests/run_decision_simple_waypoint.py b/modules/bootcamp/tests/run_decision_simple_waypoint.py index 29fbc5cd..4ee65942 100644 --- a/modules/bootcamp/tests/run_decision_simple_waypoint.py +++ b/modules/bootcamp/tests/run_decision_simple_waypoint.py @@ -21,7 +21,7 @@ # to reach the 1st command # Increase the step size if your computer is lagging # Larger step size is smaller FPS -TIME_STEP_SIZE = 0.1 # seconds +TIME_STEP_SIZE = 0.8 # seconds # OpenCV ignores your display settings, # so if the window is too small or too large, @@ -53,4 +53,4 @@ def main() -> int: if result_main != 0: print(f"ERROR: Status code: {result_main}") - print("Done!") + print("Done!") \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index fa1ed662..dc475c4b 100644 --- a/requirements.txt +++ b/requirements.txt @@ -17,7 +17,7 @@ pytest # If you are on MacOS, don't have a CUDA capable GPU, # or if things don't work and you just want to use the CPU, # comment out the "--extra-index-url" option ---extra-index-url https://download.pytorch.org/whl/cu124 +#--extra-index-url https://download.pytorch.org/whl/cu124 # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑