Alex Liu - Autonomy Bootcamp 2023

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 ↑
         # ============

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 ↑
         # ============

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 ↑
         # ============

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)