Tasks 1 to 4

Author: DerekYang2

modules/bootcamp/decision_simple_waypoint.py

@@ -43,6 +43,20 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non
         # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
         # ============
 
+    def dist_sq(self, location1: location.Location, location2: location.Location) -> float:
+        """
+        Calculates the squared Euclidean distance between two locations.
+        """
+        return (location1.location_x - location2.location_x) ** 2 + (
+            location1.location_y - location2.location_y
+        ) ** 2
+
+    def clamp(self, val: float, min_val: float, max_val: float) -> float:
+        """
+        Clamps a value between a lower and upper bound.
+        """
+        return min(max_val, max(min_val, val))
+
     def run(
         self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]"
     ) -> commands.Command:
@@ -67,9 +81,16 @@ def run(
         # ============
         # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
         # ============
+        if report.status == drone_status.DroneStatus.HALTED:
+            dist_waypoint_sq = self.dist_sq(self.waypoint, report.position)
+            if dist_waypoint_sq < self.acceptance_radius**2:
+                command = commands.Command.create_land_command()
+            else:
+                rel_x = self.clamp(self.waypoint.location_x - report.position.location_x, -60, 60)
+                rel_y = self.clamp(self.waypoint.location_y - report.position.location_y, -60, 60)
+                command = commands.Command.create_set_relative_destination_command(rel_x, rel_y)
 
         # Do something based on the report and the state of this class...
-
         # ============
         # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
         # ============

modules/bootcamp/decision_waypoint_landing_pads.py

@@ -4,6 +4,7 @@
 Travel to designated waypoint and then land at a nearby landing pad.
 """
 
+import math
 from .. import commands
 from .. import drone_report
 
@@ -13,7 +14,6 @@
 from .. import location
 from ..private.decision import base_decision
 
-
 # Disable for bootcamp use
 # No enable
 # pylint: disable=duplicate-code,unused-argument
@@ -38,11 +38,26 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non
         # ============
 
         # Add your own
-
+        self.waypoint_reached = False
+        self.closest_landing_location = None
         # ============
         # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
         # ============
 
+    def dist_sq(self, location1: location.Location, location2: location.Location) -> float:
+        """
+        Calculates the squared Euclidean distance between two locations.
+        """
+        return (location1.location_x - location2.location_x) ** 2 + (
+            location1.location_y - location2.location_y
+        ) ** 2
+
+    def clamp(self, val: float, min_val: float, max_val: float) -> float:
+        """
+        Clamps a value between a lower and upper bound.
+        """
+        return min(max_val, max(min_val, val))
+
     def run(
         self, report: drone_report.DroneReport, landing_pad_locations: "list[location.Location]"
     ) -> commands.Command:
@@ -69,6 +84,44 @@ def run(
         # ============
 
         # Do something based on the report and the state of this class...
+        if not self.waypoint_reached:
+            if report.status == drone_status.DroneStatus.HALTED:
+                dist_waypoint_sq = self.dist_sq(self.waypoint, report.position)
+                if dist_waypoint_sq < self.acceptance_radius**2:
+                    self.waypoint_reached = True
+                else:
+                    rel_x = self.clamp(
+                        self.waypoint.location_x - report.position.location_x, -60, 60
+                    )
+                    rel_y = self.clamp(
+                        self.waypoint.location_y - report.position.location_y, -60, 60
+                    )
+                    command = commands.Command.create_set_relative_destination_command(rel_x, rel_y)
+        else:
+            if self.closest_landing_location is None:
+                min_dist_sq = math.inf
+                for landing_pad_location in landing_pad_locations:
+                    current_dist_sq = self.dist_sq(report.position, landing_pad_location)
+                    if current_dist_sq < min_dist_sq:
+                        min_dist_sq = current_dist_sq
+                        self.closest_landing_location = landing_pad_location
+
+            if report.status == drone_status.DroneStatus.HALTED:
+                dist_location_sq = self.dist_sq(self.closest_landing_location, report.position)
+                if dist_location_sq < self.acceptance_radius**2:
+                    command = commands.Command.create_land_command()
+                else:
+                    rel_x = self.clamp(
+                        self.closest_landing_location.location_x - report.position.location_x,
+                        -60,
+                        60,
+                    )
+                    rel_y = self.clamp(
+                        self.closest_landing_location.location_y - report.position.location_y,
+                        -60,
+                        60,
+                    )
+                    command = commands.Command.create_set_relative_destination_command(rel_x, rel_y)
 
         # ============
         # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑

modules/bootcamp/detect_landing_pad.py

@@ -98,31 +98,38 @@ 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=True
+        )
         # 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(boxes_cpu.shape[0]):
+            result, box = bounding_box.BoundingBox.create(boxes_cpu[i])
+            if not result:
+                bounding_boxes.clear()
+                break
+            bounding_boxes.append(box)
+
+        return bounding_boxes, image_annotated
         # ============
         # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
         # ============

modules/bootcamp/tests/run_decision_example.py

@@ -21,7 +21,7 @@
 # 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.05  # seconds
 
 # OpenCV ignores your display settings,
 # so if the window is too small or too large,