diff --git a/modules/bootcamp/decision_simple_waypoint.py b/modules/bootcamp/decision_simple_waypoint.py index 26098c2e..1733087d 100644 --- a/modules/bootcamp/decision_simple_waypoint.py +++ b/modules/bootcamp/decision_simple_waypoint.py @@ -68,7 +68,18 @@ def run( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Do something based on the report and the state of this class... + dist_to_waypoint_x = self.waypoint.location_x - report.position.location_x + dist_to_waypoint_y = self.waypoint.location_y - report.position.location_y + if dist_to_waypoint_x**2 + dist_to_waypoint_y**2 > self.acceptance_radius**2: + if report.status != drone_status.DroneStatus.MOVING: + command = commands.Command.create_set_relative_destination_command( + dist_to_waypoint_x, dist_to_waypoint_y + ) + elif report.status == drone_status.DroneStatus.MOVING: + command = commands.Command.create_halt_command() + elif report.status == drone_status.DroneStatus.HALTED: + command = commands.Command.create_land_command() + # otherwise, command is already the null command, so no action required # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ diff --git a/modules/bootcamp/decision_waypoint_landing_pads.py b/modules/bootcamp/decision_waypoint_landing_pads.py index ade6f118..40f36042 100644 --- a/modules/bootcamp/decision_waypoint_landing_pads.py +++ b/modules/bootcamp/decision_waypoint_landing_pads.py @@ -37,7 +37,8 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Add your own + self.achieved_waypoint = False + self.nearest_landing_pad: None | location.Location = None # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ @@ -68,10 +69,85 @@ def run( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Do something based on the report and the state of this class... + if not self.achieved_waypoint: + dist_to_waypoint_x, dist_to_waypoint_y = self._calc_relative_dist( + report.position, self.waypoint + ) + if dist_to_waypoint_x**2 + dist_to_waypoint_y**2 > self.acceptance_radius**2: + if report.status != drone_status.DroneStatus.MOVING: + command = commands.Command.create_set_relative_destination_command( + dist_to_waypoint_x, dist_to_waypoint_y + ) + else: + self.achieved_waypoint = True + if self.achieved_waypoint: + # now we find the index of the landing pad closest to the drone + # while keeping that landing pad's distance (squared) + + nearest_landing_pad = self._find_best_landing_pad(report, landing_pad_locations) + dist_to_nearest_landing_pad_x, dist_to_nearest_landing_pad_y = self._calc_relative_dist( + report.position, nearest_landing_pad + ) + + if ( + dist_to_nearest_landing_pad_x**2 + dist_to_nearest_landing_pad_y**2 + > self.acceptance_radius**2 + ): + if report.status != drone_status.DroneStatus.MOVING: + command = commands.Command.create_set_relative_destination_command( + dist_to_nearest_landing_pad_x, dist_to_nearest_landing_pad_y + ) + elif report.status == drone_status.DroneStatus.MOVING: + command = commands.Command.create_halt_command() + elif report.status == drone_status.DroneStatus.HALTED: + command = commands.Command.create_land_command() + # otherwise, the drone is already landed at the closest landing pad + # and we are done because the command is already the null command # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ return command + + def _find_best_landing_pad( + self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]" + ) -> location.Location: + if self.nearest_landing_pad is not None: # the answer is already calculated + return self.nearest_landing_pad + + # otherwise, we must calculate it ourselves + if len(landing_pad_locations) == 0: + # if no elements in landing_pad_locations, we go back to the start + self.nearest_landing_pad = location.Location(0, 0) + else: + best_index, shortest_dist = 0, self._calc_distance_squared( + report, landing_pad_locations[0] + ) + for i in range(1, len(landing_pad_locations)): + dist = self._calc_distance_squared(report, landing_pad_locations[i]) + if dist < shortest_dist: + best_index, shortest_dist = i, dist + self.nearest_landing_pad = landing_pad_locations[best_index] + + return self.nearest_landing_pad + + def _calc_distance_squared( + self, report: drone_report.DroneReport, destination: location.Location + ) -> int: + dist_x, dist_y = self._calc_relative_dist(report.position, destination) + return dist_x**2 + dist_y**2 + + def _calc_relative_dist( + self, loc1: location.Location, loc2: location.Location + ) -> "tuple[float, float]": + """Relative location from loc1 to loc2 + + Args: + loc1 (location.Location): Location 1 + loc2 (location.Location): Location 2 + + Returns: + tuple[int, int]: horizontal distance, vertical distance + """ + return loc2.location_x - loc1.location_x, loc2.location_y - loc1.location_y diff --git a/modules/bootcamp/detect_landing_pad.py b/modules/bootcamp/detect_landing_pad.py index f17aa677..a6e66f70 100644 --- a/modules/bootcamp/detect_landing_pad.py +++ b/modules/bootcamp/detect_landing_pad.py @@ -17,9 +17,6 @@ # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ # Bootcampers remove the following lines: -# Allow linters and formatters to pass for bootcamp maintainers -# No enable -# pylint: disable=unused-argument,unused-private-member,unused-variable # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ @@ -98,22 +95,24 @@ 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.75, 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(boxes=True, conf=True) # 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 = [] @@ -121,8 +120,12 @@ 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 = ... + for i in range(0, boxes_cpu.shape[0]): + successful, box = bounding_box.BoundingBox.create(bounds=boxes_cpu[i]) + if successful: + bounding_boxes.append(box) - return [], image_annotated + return (bounding_boxes, image_annotated) # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ diff --git a/modules/bootcamp/tests/run_decision_example.py b/modules/bootcamp/tests/run_decision_example.py index 7bf681ba..e189f2e0 100644 --- a/modules/bootcamp/tests/run_decision_example.py +++ b/modules/bootcamp/tests/run_decision_example.py @@ -21,12 +21,12 @@ # 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, # change this value (between 0.0 and 1.0) -DISPLAY_SCALE = 0.8 +DISPLAY_SCALE = 1.0 # Seed for randomly generating the waypoint and landing pad # Change to a constant for reproducibility (e.g. debugging)