diff --git a/modules/bootcamp/decision_simple_waypoint.py b/modules/bootcamp/decision_simple_waypoint.py index 26098c2e..abbbff03 100644 --- a/modules/bootcamp/decision_simple_waypoint.py +++ b/modules/bootcamp/decision_simple_waypoint.py @@ -38,11 +38,23 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non # ============ # Add your own - + self.acceptance_radius_squared = self.acceptance_radius**2 # used for distance calculation + self.landing = False # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ + @staticmethod + def calculate_distance_squared( + from_location: location.Location, to_location_x: float, to_location_y: float + ) -> float: + """ + Calculate the non-square rooted distance between two locations + """ + return (to_location_x - from_location.location_x) ** 2 + ( + to_location_y - from_location.location_y + ) ** 2 + def run( self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]" ) -> commands.Command: @@ -70,6 +82,19 @@ def run( # Do something based on the report and the state of this class... + if report.status == drone_status.DroneStatus.HALTED: + if ( + DecisionSimpleWaypoint.calculate_distance_squared( + report.position, self.waypoint.location_x, self.waypoint.location_y + ) + <= self.acceptance_radius_squared + ): + command = commands.Command.create_land_command() + else: + command = commands.Command.create_set_relative_destination_command( + self.waypoint.location_x, self.waypoint.location_y + ) + # ============ # ↑ 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..b063563e 100644 --- a/modules/bootcamp/decision_waypoint_landing_pads.py +++ b/modules/bootcamp/decision_waypoint_landing_pads.py @@ -38,11 +38,55 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non # ============ # Add your own + self.acceptance_radius_squared = self.acceptance_radius**2 # used for distance calculation + self.finding_waypoint = True + self.nearest_landing_pad_location = location.Location(0, 0) # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ + @staticmethod + def find_nearest_pad( + reference_location: location.Location, landing_pad_locations: list[location.Location] + ) -> location.Location: + """ + Calculates returns nearest landing pad location based on a given reference location + """ + # assuming landing_pad_locations is always populated, but just in-case + closest_location = min( + landing_pad_locations, + key=lambda location: ( + (location.location_x - reference_location.location_x) ** 2 + + (location.location_y - reference_location.location_y) ** 2 + ), + default=location.Location(0, 0), + ) + return closest_location + + @staticmethod + def calculate_distance_squared( + from_location: location.Location, to_location_x: float, to_location_y: float + ) -> float: + """ + Calculate the non-square rooted distance between two locations + """ + return (to_location_x - from_location.location_x) ** 2 + ( + to_location_y - from_location.location_y + ) ** 2 + + @staticmethod + def get_relative_position( + from_location: location.Location, to_location: location.Location + ) -> tuple[float, float]: + """ + Calculates the relative position of one position to another + """ + return ( + to_location.location_x - from_location.location_x, + to_location.location_y - from_location.location_y, + ) + def run( self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]" ) -> commands.Command: @@ -70,6 +114,43 @@ def run( # Do something based on the report and the state of this class... + # only execute when "drone" is ready for another instruction + if report.status == drone_status.DroneStatus.HALTED: + if self.finding_waypoint: + if ( + DecisionWaypointLandingPads.calculate_distance_squared( + report.position, self.waypoint.location_x, self.waypoint.location_y + ) + <= self.acceptance_radius_squared + ): + self.finding_waypoint = False + self.nearest_landing_pad_location = ( + DecisionWaypointLandingPads.find_nearest_pad( + report.position, landing_pad_locations + ) + ) + else: + command = commands.Command.create_set_relative_destination_command( + self.waypoint.location_x, self.waypoint.location_y + ) + else: + if ( + DecisionWaypointLandingPads.calculate_distance_squared( + report.position, + self.nearest_landing_pad_location.location_x, + self.nearest_landing_pad_location.location_y, + ) + <= self.acceptance_radius_squared + ): + command = commands.Command.create_land_command() + else: + relative_x, relative_y = DecisionWaypointLandingPads.get_relative_position( + report.position, self.nearest_landing_pad_location + ) + command = commands.Command.create_set_relative_destination_command( + relative_x, relative_y + ) + # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ diff --git a/modules/bootcamp/detect_landing_pad.py b/modules/bootcamp/detect_landing_pad.py index f17aa677..d86b8949 100644 --- a/modules/bootcamp/detect_landing_pad.py +++ b/modules/bootcamp/detect_landing_pad.py @@ -98,31 +98,46 @@ def run(self, image: np.ndarray) -> "tuple[list[bounding_box.BoundingBox], np.nd # * conf # * device # * verbose - predictions = ... + # BC NOTE: May need to create instantiate model object with create() + predictions = self.__model.predict( + source=image, conf=0.7, device=self.__DEVICE, verbose=False + ) + # BC NOTE: See https://docs.ultralytics.com/modes/predict/#working-with-results # Get the Result object - prediction = ... + # BOOTCAMPER NOTE: may raise ValueError + prediction = predictions[0] # Plot the annotated image from the Result object # Include the confidence value - image_annotated = ... + image_annotated = prediction.plot() + # BC NOTE: conf & boxes True by default # Get the xyxy boxes list from the Boxes object in the Result object - boxes_xyxy = ... + boxes_xyxy = prediction.boxes.xyxy + # BC NOTE: Result.boxes may be None, could cause error trying to access 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 + # BC NOTE: Need for error checking from returned tuple from BoundingBox.create() bounding_boxes = [] + for rectangle in boxes_cpu: + # unwrap returned tuple + result, box = bounding_box.BoundingBox.create(rectangle) + if result: + bounding_boxes.append(box) + else: + return [], image_annotated # 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 + 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..cd3a02c0 100644 --- a/modules/bootcamp/tests/run_decision_example.py +++ b/modules/bootcamp/tests/run_decision_example.py @@ -23,6 +23,8 @@ # Larger step size is smaller FPS TIME_STEP_SIZE = 0.1 # seconds +# Alex was here + # OpenCV ignores your display settings, # so if the window is too small or too large, # change this value (between 0.0 and 1.0)