diff --git a/modules/bootcamp/decision_simple_waypoint.py b/modules/bootcamp/decision_simple_waypoint.py index 26098c2e..7dd17efd 100644 --- a/modules/bootcamp/decision_simple_waypoint.py +++ b/modules/bootcamp/decision_simple_waypoint.py @@ -37,7 +37,14 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Add your own + self.command_index = 0 + self.commands = [ + commands.Command.create_set_relative_destination_command( + waypoint.location_x, waypoint.location_y + ) + ] + self.counter = 0 + self.has_sent_landing_command = False # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ @@ -68,8 +75,24 @@ def run( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Do something based on the report and the state of this class... + if report.status == drone_status.DroneStatus.HALTED and self.command_index < len( + self.commands + ): + print(self.counter) + print(self.command_index) + print(report.status) + + command = self.commands[self.command_index] + self.command_index += 1 + + elif report.status == drone_status.DroneStatus.HALTED and not self.has_sent_landing_command: + print(self.counter) + command = commands.Command.create_land_command() + + self.has_sent_landing_command = True + + self.counter += 1 # ============ # ↑ 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..32a09625 100644 --- a/modules/bootcamp/decision_waypoint_landing_pads.py +++ b/modules/bootcamp/decision_waypoint_landing_pads.py @@ -37,12 +37,45 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Add your own + self.command_index = 0 + self.commands = [ + commands.Command.create_set_relative_destination_command( + waypoint.location_x, waypoint.location_y + ) + ] + self.has_sent_landing_command = False + self.has_reached_waypoint = False + self.has_caculated_pad = False # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ + def closest_pad( + self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]" + ) -> int: + """ " + Finds the index of the closest pad + """ + _min = -1.0 + closest_index = 0 + index = 0 + + for loc in landing_pad_locations: + print(f"landing pad locations: {landing_pad_locations}") + print(f"location being evaluated: {loc}") + x_distance = loc.location_x - report.position.location_x + y_distance = loc.location_y - report.position.location_y + if x_distance**2 + y_distance**2 < _min or _min == -1.0: + closest_index = index + _min = x_distance**2 + y_distance**2 + index += 1 + + if _min <= self.acceptance_radius: + return -1 + + return closest_index + def run( self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]" ) -> commands.Command: @@ -68,7 +101,38 @@ def run( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Do something based on the report and the state of this class... + if report.status == drone_status.DroneStatus.HALTED and self.has_reached_waypoint: + + if not self.has_caculated_pad: + closest_index = self.closest_pad(report, landing_pad_locations) + + if closest_index != -1: + self.commands.append( + commands.Command.create_set_relative_destination_command( + landing_pad_locations[closest_index].location_x + - report.position.location_x, + landing_pad_locations[closest_index].location_y + - report.position.location_y, + ) + ) + + if report.status == drone_status.DroneStatus.HALTED and self.command_index < len( + self.commands + ): + + print(self.command_index) + print(report.status) + + command = self.commands[self.command_index] + self.command_index += 1 + + if self.command_index == len(self.commands): + self.has_reached_waypoint = True + + elif report.status == drone_status.DroneStatus.HALTED and not self.has_sent_landing_command: + command = commands.Command.create_land_command() + + self.has_sent_landing_command = True # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ diff --git a/modules/bootcamp/detect_landing_pad.py b/modules/bootcamp/detect_landing_pad.py index f17aa677..c3f70906 100644 --- a/modules/bootcamp/detect_landing_pad.py +++ b/modules/bootcamp/detect_landing_pad.py @@ -16,10 +16,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 ↑ # ============ @@ -75,7 +71,7 @@ def __init__(self, class_private_create_key: object, model: ultralytics.YOLO) -> """ assert class_private_create_key is DetectLandingPad.__create_key, "Use create() method" - self.__model = model + self._model = model def run(self, image: np.ndarray) -> "tuple[list[bounding_box.BoundingBox], np.ndarray]": """ @@ -98,31 +94,41 @@ def run(self, image: np.ndarray) -> "tuple[list[bounding_box.BoundingBox], np.nd # * conf # * device # * verbose - predictions = ... + predictions = self._model.predict(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(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 = [] - # Hint: .shape gets the dimensions of the numpy array - # for i in range(0, ...): - # # Create BoundingBox object and append to list - # result, box = ... - return [], image_annotated + for i in range(0, boxes_cpu.shape[0]): + # Hint: .shape gets the dimensions of the numpy array + # for i in range(0, ...): + # # Create BoundingBox object and append to l xist + box_coordinates = np.array( + [boxes_cpu[i][0], boxes_cpu[i][1], boxes_cpu[i][2], boxes_cpu[i][3]] + ) + box = bounding_box.BoundingBox.create(box_coordinates) + + if box[1]: + bounding_boxes.append(box[1]) + # result, box = .. + + return bounding_boxes, image_annotated # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============