diff --git a/modules/bootcamp/decision_simple_waypoint.py b/modules/bootcamp/decision_simple_waypoint.py index bdb8d5d9..84f58c90 100644 --- a/modules/bootcamp/decision_simple_waypoint.py +++ b/modules/bootcamp/decision_simple_waypoint.py @@ -38,7 +38,7 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float): # ============ # Add your own - + # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ @@ -67,14 +67,33 @@ def run(self, # ============ # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - - # Do something based on the report and the state of this class... - - # Remove this when done - raise NotImplementedError - + status = report.status + relative_x_dist = self.waypoint.location_x - report.position.location_x + relative_y_dist = self.waypoint.location_y - report.position.location_y + + if status == drone_status.DroneStatus.HALTED: + # Check if halted at initial position or at waypoint + if self.relative_distance(relative_x_dist, relative_y_dist) < self.acceptance_radius: + command = commands.Command.create_land_command() + else: + command = commands.Command.create_set_relative_destination_command( + relative_x_dist, + relative_y_dist, + ) + elif status == drone_status.DroneStatus.MOVING: + if self.relative_distance(relative_x_dist, relative_y_dist) < self.acceptance_radius: + command = commands.Command.create_halt_command() + + # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ return command + + def relative_distance( + self, + relative_x_dist: float, + relative_y_dist: float, + ) -> float: + return (relative_x_dist ** 2 + relative_y_dist ** 2) ** 0.5 \ 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 a1b4f69b..0d611478 100644 --- a/modules/bootcamp/decision_waypoint_landing_pads.py +++ b/modules/bootcamp/decision_waypoint_landing_pads.py @@ -32,13 +32,13 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float): print("Waypoint: " + str(waypoint)) self.acceptance_radius = acceptance_radius - + # ============ # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Add your own - + self.visited_waypoint = False + self.acceptance_radius_squared = self.acceptance_radius ** 2 # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ @@ -47,7 +47,7 @@ def run(self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]") -> commands.Command: """ - Make the drone fly to the waypoint and then land at the nearest landing pad. + Make the drone fly to the waypoint. You are allowed to create as many helper methods as you want, as long as you do not change the __init__() and run() signatures. @@ -67,14 +67,70 @@ def run(self, # ============ # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - - # Do something based on the report and the state of this class... - - # Remove this when done - raise NotImplementedError - + status = report.status + position = report.position + + if self.visited_waypoint: # Travel to nearest landing pad + if status == drone_status.DroneStatus.HALTED: # If halted, need to first calculate the nearest landing pad + self.nearest_landing_pad = self.get_nearest_landing_pad(position, landing_pad_locations) + + if (self.get_relative_location_distance(position, self.nearest_landing_pad) + < self.acceptance_radius_squared): + command = commands.Command.create_land_command() + else: + landing_pad_x_dist = self.nearest_landing_pad.location_x - position.location_x + landing_pad_y_dist = self.nearest_landing_pad.location_y - position.location_y + command = commands.Command.create_set_relative_destination_command( + landing_pad_x_dist, + landing_pad_y_dist, + ) + elif status == drone_status.DroneStatus.MOVING: + if (self.get_relative_location_distance(position, self.nearest_landing_pad) + < self.acceptance_radius_squared): + command = commands.Command.create_halt_command() + else: # Travel to waypoint + if status == drone_status.DroneStatus.HALTED: + if (self.get_relative_location_distance(position, self.waypoint) + > self.acceptance_radius_squared): + waypoint_x_dist = self.waypoint.location_x - position.location_x + waypoint_y_dist = self.waypoint.location_y - position.location_y + command = commands.Command.create_set_relative_destination_command( + waypoint_x_dist, + waypoint_y_dist, + ) + else: + self.visited_waypoint = True + elif status == drone_status.DroneStatus.MOVING: + if (self.get_relative_location_distance(position, self.waypoint) + < self.acceptance_radius_squared): + command = commands.Command.create_halt_command() + + + + # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ return command + + def get_nearest_landing_pad( + self, + position: location.Location, + landing_pad_locations: "list[location.Location]", + ) -> location.Location: + nearest_landing_pad = None + nearest_landing_pad_distance = float("inf") + for landing_pad in landing_pad_locations: + current_distance = self.get_relative_location_distance(position, landing_pad) + if current_distance < nearest_landing_pad_distance: + nearest_landing_pad = landing_pad + nearest_landing_pad_distance = current_distance + return nearest_landing_pad + + def get_relative_location_distance( + self, + locOne: location.Location, + locTwo: location.Location, + ) -> float: + return (locOne.location_x - locTwo.location_x) ** 2 + (locOne.location_y - locTwo.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..6c138813 100644 --- a/modules/bootcamp/detect_landing_pad.py +++ b/modules/bootcamp/detect_landing_pad.py @@ -86,25 +86,37 @@ 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(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 = [] + + # Get the dimensions of the numpy array + num_boxes, _ = boxes_cpu.shape + + for i in range(num_boxes): + result, box = bounding_box.BoundingBox.create(boxes_cpu[i]) + bounding_boxes.append(box) + + return bounding_boxes, image_annotated + # Hint: .shape gets the dimensions of the numpy array # for i in range(0, ...): # Create BoundingBox object and append to list 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 ↑