diff --git a/.gitignore b/.gitignore index bcc89311..e02621fb 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,6 @@ venv/ # Logging *log* + +# pyenv +.python-version \ No newline at end of file diff --git a/modules/bootcamp/decision_simple_waypoint.py b/modules/bootcamp/decision_simple_waypoint.py index bdb8d5d9..fd92b6d1 100644 --- a/modules/bootcamp/decision_simple_waypoint.py +++ b/modules/bootcamp/decision_simple_waypoint.py @@ -37,12 +37,37 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float): # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Add your own + self.action_dict = dict(zip(("MOVE", "HALT", "LAND"), range(3,6))) # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ + def distance_between_waypoint(self, given_location:location.Location) -> "tuple[float, float]": + """ + Returns the distance between the given location and waypoint. + """ + given_loc_from_waypoint_location_x = self.waypoint.location_x - given_location.location_x + given_loc_from_waypoint_location_y = self.waypoint.location_y - given_location.location_y + return (given_loc_from_waypoint_location_x, given_loc_from_waypoint_location_y) + + def check_if_near_waypoint(self, given_location:location.Location) -> bool: + """ + Checks if the given location is on the waypoint by an acceptance radius. + """ + absolute_acceptance_radius = abs(self.acceptance_radius) + difference_location_x, difference_location_y = self.distance_between_waypoint(given_location) + if abs(difference_location_x) < absolute_acceptance_radius and abs(difference_location_y) < absolute_acceptance_radius: + return True + return False + + def next_relative_coordinates_to_waypoint(self, given_location:location.Location) -> "tuple[float, float]": + """ + Returns the relative x and y coordinates for drone to be sent to. + """ + relative_x, relative_y = self.distance_between_waypoint(given_location) + return (relative_x, relative_y) + def run(self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]") -> commands.Command: @@ -68,10 +93,28 @@ def run(self, # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Do something based on the report and the state of this class... + action = None + report_status = report.status + report_position = report.position + + if report_status == drone_status.DroneStatus.LANDED: + action = None + elif report_status == drone_status.DroneStatus.HALTED: + if self.check_if_near_waypoint(report_position): + action = self.action_dict["LAND"] + else: + action = self.action_dict["MOVE"] + + if action is None: + pass + elif action == self.action_dict["MOVE"]: + relative_x, relative_y = self.next_relative_coordinates_to_waypoint(report_position) + command = commands.Command.create_set_relative_destination_command(relative_x, relative_y) + elif action == self.action_dict["HALT"]: + command = commands.Command.create_halt_command() + elif action == self.action_dict["LAND"]: + command = commands.Command.create_land_command() - # 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..c9778635 100644 --- a/modules/bootcamp/decision_waypoint_landing_pads.py +++ b/modules/bootcamp/decision_waypoint_landing_pads.py @@ -37,12 +37,72 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float): # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Add your own + self.action_dict = dict(zip(("MOVE", "HALT", "LAND"), range(3,6))) + self.origin = location.Location(0,0) # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ # ============ + @staticmethod + def shortest_distance(target_location:location.Location, given_location:location.Location) -> float: + """ + Finds out the shortest distance between two given locations. + """ + x_1, y_1 = given_location.location_x, given_location.location_y + x_2, y_2 = target_location.location_x, target_location.location_y + x_square = (x_2 - x_1) ** 2 + y_square = (y_2 - y_1) ** 2 + return (x_square + y_square) ** 0.5 + + def relative_coordinates_of_target(self, target_location:location.Location, given_location:location.Location) -> "tuple[float, float]": + """ + Returns the relative coordinates of target w.r.t given location. + """ + relative_location_x = target_location.location_x - given_location.location_x + relative_location_y = target_location.location_y - given_location.location_y + return (relative_location_x, relative_location_y) + + def check_if_near_target(self, target_location:location.Location, given_location:location.Location) -> bool: + """ + Checks if the given location is near the target by an acceptance radius. + """ + absolute_acceptance_radius = abs(self.acceptance_radius) + difference_location_x, difference_location_y = self.relative_coordinates_of_target(target_location, given_location) + if abs(difference_location_x) < absolute_acceptance_radius and abs(difference_location_y) < absolute_acceptance_radius: + return True + return False + + def next_relative_coordinates_to_target(self, + target_location:location.Location, + given_location:location.Location) -> "tuple[float, float]": + """ + Returns the relative x and y coordinates for drone to be sent to. + """ + relative_x, relative_y = self.relative_coordinates_of_target(target_location, given_location) + divider = 1 + if abs(relative_x) > abs(relative_y): + return (relative_x/divider, relative_y) + elif abs(relative_x) < abs(relative_y): + return (relative_x, relative_y/divider) + else: + return (relative_x/divider, relative_y/divider) + + def closest_landing_pad(self, + given_location:location.Location, + landing_pad_locations: "list[location.Location]") -> location.Location: + """ + Finds out the closest landing pad from the given location by checking out their distances. + """ + closest_location = landing_pad_locations[0] + for landing_pad in landing_pad_locations[1:]: + distance_from_given_location = DecisionWaypointLandingPads.shortest_distance(landing_pad, given_location) + distance_from_closest_location = DecisionWaypointLandingPads.shortest_distance(closest_location, given_location) + if distance_from_given_location < distance_from_closest_location: + closest_location = landing_pad + return closest_location + + def run(self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]") -> commands.Command: @@ -68,10 +128,39 @@ def run(self, # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============ - # Do something based on the report and the state of this class... + action = None + report_status = report.status + report_position = report.position + target = self.waypoint + + if report_status == drone_status.DroneStatus.HALTED: + landing_pad = self.closest_landing_pad(self.waypoint, landing_pad_locations) + action = self.action_dict["MOVE"] + if self.check_if_near_target(self.waypoint, report_position): + target = landing_pad + if (self.check_if_near_target(landing_pad, report_position) + and (DecisionWaypointLandingPads.shortest_distance(self.waypoint, report_position) - DecisionWaypointLandingPads.shortest_distance(self.waypoint, landing_pad) < 0.1) + and ( + (self.check_if_near_target(landing_pad, self.origin) and self.check_if_near_target(self.waypoint, self.origin)) + or (not self.check_if_near_target(report_position, self.origin)) + )): + """ + self.check_if_near_target(landing_pad, self.origin) - if nearest landing pad is origin + self.check_if_near_target(self.waypoint, self.origin) - if waypoint is origin + self.check_if_near_target(report_position, self.origin) - if current position is origin + """ + action = self.action_dict["LAND"] + + if action is None: + pass + elif action == self.action_dict["MOVE"]: + relative_x, relative_y = self.next_relative_coordinates_to_target(target, report_position) + command = commands.Command.create_set_relative_destination_command(relative_x, relative_y) + elif action == self.action_dict["HALT"]: + command = commands.Command.create_halt_command() + elif action == self.action_dict["LAND"]: + command = commands.Command.create_land_command() - # Remove this when done - raise NotImplementedError # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ diff --git a/modules/bootcamp/detect_landing_pad.py b/modules/bootcamp/detect_landing_pad.py index 685c9795..1e9b88e4 100644 --- a/modules/bootcamp/detect_landing_pad.py +++ b/modules/bootcamp/detect_landing_pad.py @@ -86,32 +86,36 @@ 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(pil=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 = prediction.boxes.cpu().xyxy.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, ...): + for i in range(0, boxes_cpu.shape[0]): # Create BoundingBox object and append to list - # result, box = ... + result, box = bounding_box.BoundingBox.create(boxes_cpu[i]) + if result: + bounding_boxes.append(box) - # Remove this when done - raise NotImplementedError + 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 a061548b..ce951d2d 100644 --- a/modules/bootcamp/tests/run_decision_example.py +++ b/modules/bootcamp/tests/run_decision_example.py @@ -41,7 +41,7 @@ # 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 = 0.7 # Seed for randomly generating the waypoint and landing pad # Change to a constant for reproducibility (e.g. debugging) diff --git a/modules/bootcamp/tests/run_decision_simple_waypoint.py b/modules/bootcamp/tests/run_decision_simple_waypoint.py index d76eae54..e639aeaa 100644 --- a/modules/bootcamp/tests/run_decision_simple_waypoint.py +++ b/modules/bootcamp/tests/run_decision_simple_waypoint.py @@ -34,12 +34,12 @@ # You can probably divide it by 10 or so since ML inference isn't running # Increase the step size if your computer is lagging # Larger step size is smaller FPS -TIME_STEP_SIZE = 0.01 # seconds +TIME_STEP_SIZE = 0.1 # 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 = 0.7 # Seed for randomly generating the waypoint and landing pad # Change to a constant for reproducibility (e.g. debugging) diff --git a/modules/bootcamp/tests/run_decision_waypoint_landing_pads.py b/modules/bootcamp/tests/run_decision_waypoint_landing_pads.py index e83e8963..07cc3c88 100644 --- a/modules/bootcamp/tests/run_decision_waypoint_landing_pads.py +++ b/modules/bootcamp/tests/run_decision_waypoint_landing_pads.py @@ -46,7 +46,7 @@ # Seed for randomly generating the waypoint and landing pad # Change to a constant for reproducibility (e.g. debugging) # Change back to = time.time_ns() to test robustness -SEED = time.time_ns() +SEED = time.time_ns() # 1694668265382892400 # ============ # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑