diff --git a/modules/bootcamp/decision_simple_waypoint.py b/modules/bootcamp/decision_simple_waypoint.py index bdb8d5d9..9f3e4be7 100644 --- a/modules/bootcamp/decision_simple_waypoint.py +++ b/modules/bootcamp/decision_simple_waypoint.py @@ -38,6 +38,8 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float): # ============ # Add your own + self.traveled_to_waypoint = False + self.has_sent_landing_command = False # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ @@ -69,9 +71,19 @@ def run(self, # ============ # Do something based on the report and the state of this class... - - # Remove this when done - raise NotImplementedError + if report.status == drone_status.DroneStatus.HALTED: + if not self.traveled_to_waypoint: + command = commands.Command.create_set_relative_destination_command( + self.waypoint.location_x - report.position.location_x, + self.waypoint.location_y - report.position.location_y + ) + self.traveled_to_waypoint = True + elif not self.has_sent_landing_command: + command = commands.Command.create_land_command() + self.has_sent_landing_command = True + + # # Remove this when done + # raise NotImplementedError # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ diff --git a/modules/bootcamp/decision_waypoint_landing_pads.py b/modules/bootcamp/decision_waypoint_landing_pads.py index a1b4f69b..cd88aa5c 100644 --- a/modules/bootcamp/decision_waypoint_landing_pads.py +++ b/modules/bootcamp/decision_waypoint_landing_pads.py @@ -38,6 +38,9 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float): # ============ # Add your own + self.traveled_to_waypoint = False + self.has_sent_landing_command = False + self.closest_landing_pad = None # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ @@ -69,12 +72,52 @@ def run(self, # ============ # Do something based on the report and the state of this class... - - # Remove this when done - raise NotImplementedError + if report.status == drone_status.DroneStatus.HALTED: + if not self.traveled_to_waypoint: + # Fly to the waypoint + command = commands.Command.create_set_relative_destination_command( + self.waypoint.location_x - report.position.location_x, + self.waypoint.location_y - report.position.location_y + ) + self.traveled_to_waypoint = True + elif not self.closest_landing_pad: + # Calculate the closest landing pad + self.closest_landing_pad = self.find_closest_landing_pad(report.position, landing_pad_locations) + command = commands.Command.create_set_relative_destination_command( + self.closest_landing_pad.location_x - report.position.location_x, + self.closest_landing_pad.location_y - report.position.location_y + ) + elif not self.has_sent_landing_command: + # Land the drone at the closest landing pad + command = commands.Command.create_land_command() + self.has_sent_landing_command = True + + # # Remove this when done + # raise NotImplementedError # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ return command + + def find_closest_landing_pad(self, current_position: location.Location, landing_pads: "list[location.Location]") -> location.Location: + """ + Find the closest landing pad to the current position using L-2 norm. + """ + closest_pad = None + min_distance = float('inf') + + for pad in landing_pads: + distance = self.calculate_distance(current_position, pad) + if distance < min_distance: + min_distance = distance + closest_pad = pad + + return closest_pad + + def calculate_distance(self, loc1: location.Location, loc2: location.Location) -> float: + """ + Calculate the squared distance between two locations to avoid computing square roots. + """ + return (loc1.location_x - loc2.location_x) ** 2 + (loc1.location_y - loc2.location_y) ** 2 \ No newline at end of file diff --git a/modules/bootcamp/detect_landing_pad.py b/modules/bootcamp/detect_landing_pad.py index 685c9795..d3c5a7a1 100644 --- a/modules/bootcamp/detect_landing_pad.py +++ b/modules/bootcamp/detect_landing_pad.py @@ -86,22 +86,27 @@ def run(self, image: np.ndarray) -> "tuple[list[bounding_box.BoundingBox], np.nd # * conf # * device # * verbose - predictions = ... + predictions = self.__model.predict( + source=image, + conf=0.7, + device=self.__DEVICE, + verbose=False, + ) # Get the Result object - prediction = ... + prediction = predictions[0] # Plot the annotated image from the Result object # Include the confidence value - image_annotated = ... + image_annotated = prediction.plot() # Get the xyxy boxes list from the Boxes object in the Result object - boxes_xyxy = ... + boxes_xyxy = prediction.boxes.xyxy # Detach the xyxy boxes to make a copy, # move the copy into CPU space, # and convert to a numpy array - boxes_cpu = ... + boxes_cpu = boxes_xyxy.detach().cpu().numpy() # Loop over the boxes list and create a list of bounding boxes bounding_boxes = [] @@ -109,9 +114,17 @@ def run(self, image: np.ndarray) -> "tuple[list[bounding_box.BoundingBox], np.nd # for i in range(0, ...): # Create BoundingBox object and append to list # result, box = ... - - # Remove this when done - raise NotImplementedError + + for i in range(boxes_cpu.shape[0]): + results, bbox = bounding_box.BoundingBox.create(boxes_cpu[i]) + if not results: + break + else: + bounding_boxes.append(bbox) + return bounding_boxes, image_annotated + + # # Remove this when done + # raise NotImplementedError # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ diff --git a/modules/bootcamp/tests/run_decision_example.py b/modules/bootcamp/tests/run_decision_example.py index a061548b..ad3cf239 100644 --- a/modules/bootcamp/tests/run_decision_example.py +++ b/modules/bootcamp/tests/run_decision_example.py @@ -36,7 +36,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 = 1 # seconds # OpenCV ignores your display settings, # so if the window is too small or too large, diff --git a/modules/bootcamp/tests/run_decision_waypoint_landing_pads.py b/modules/bootcamp/tests/run_decision_waypoint_landing_pads.py index e83e8963..33dc98e3 100644 --- a/modules/bootcamp/tests/run_decision_waypoint_landing_pads.py +++ b/modules/bootcamp/tests/run_decision_waypoint_landing_pads.py @@ -36,7 +36,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 = 1 # seconds # OpenCV ignores your display settings, # so if the window is too small or too large, diff --git a/requirements.txt b/requirements.txt index e3c84130..a6f58d53 100644 --- a/requirements.txt +++ b/requirements.txt @@ -18,8 +18,8 @@ pytest # If you are on MacOS, remove +cu117 # Otherwise, you can keep as is, or use +cpu if you # have a CUDA capable GPU but don't want to use it -torch==1.13.1+cu117 -torchvision==0.14.1+cu117 +torch==1.13.1 +torchvision==0.14.1 # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑