GPS and Drone Modules

library/drone.py

@@ -0,0 +1,103 @@
+"""
+Copyright MIT and Harvey Mudd College
+MIT License
+Summer 2020
+
+Defines the interface of the Drone module of the racecar_core library.
+"""
+
+import abc
+import copy
+import numpy as np
+from nptyping import NDArray
+
+
+class Drone(abc.ABC):
+    """
+    Returns the color image captured by the drone's camera.
+    """
+
+    # The dimensions of the image in pixels
+    _WIDTH: int = 640
+    _HEIGHT: int = 480
+
+
+    def get_drone_image(self) -> NDArray[(480, 640, 3), np.uint8]:
+        """
+        Returns a deep copy of the current color image captured by the drone camera.
+
+        Returns:
+            An array representing the pixels in the image, organized as follows
+                0th dimension: pixel rows, indexed from top to bottom.
+                1st dimension: pixel columns, indexed from left to right.
+                2nd dimension: pixel color channels, in the blue-green-red format.
+
+        Note:
+            Each color value ranges from 0 to 255.
+
+            This function returns a deep copy of the captured image. This means that
+            we can modify the returned image and it will not change the image returned
+            by future calls to get_drone_image().
+
+        Example::
+
+            # Initialize image with a deep copy of the most recent color image captured
+            # by the camera
+            image = rc.drone.get_drone_image()
+
+            # Store the amount of blue in the pixel on row 3, column 5
+            blue = image[3][5][0]
+        """
+        return copy.deepcopy(self.get_drone_image_no_copy())
+
+    @abc.abstractmethod
+    def get_drone_image_async(self) -> NDArray[(480, 640, 3), np.uint8]:
+        """
+        Returns the current color image without the car in "go" mode.
+
+        Returns:
+            An array representing the pixels in the image, organized as follows
+                0th dimension: pixel rows, indexed from top to bottom.
+                1st dimension: pixel columns, indexed from left to right.
+                2nd dimension: pixel color channels, in the blue-green-red format.
+
+        Note:
+            Each color value ranges from 0 to 255.
+
+        Warning:
+            This function breaks the start-update paradigm and should only be used in
+            Jupyter Notebook.
+
+        Example::
+
+            # Initialize image with the most recent color image captured by the camera
+            image = rc.drone.get_color_image_async()
+
+            # Store the amount of blue in the pixel on row 3, column 5
+            blue = image[3][5][0]
+        """
+        pass
+
+    @abc.abstractmethod
+    def get_drone_height(self) -> float:
+        """
+        Returns the drone's height.
+        """
+        pass
+
+    @abc.abstractmethod
+    def set_height(self, height: float) -> None:
+        """
+        Sets the drone's height.
+
+        Args:
+            height: The vertical height the drone should fly to.
+        """
+        pass
+
+    @abc.abstractmethod
+    def return_home(self) -> None:
+        """
+        Lands the drone back on the car
+        """
+        pass

library/physics.py

@@ -68,3 +68,29 @@ def get_angular_velocity(self) -> NDArray[3, np.float32]:
             yaw = ang_vel[1]
         """
         pass
+
+    @abc.abstractmethod
+    def get_position(self) -> NDArray[3, np.float32]:
+        """
+        Returns a 3D vector containing the car's position as x, y, and z coordinates.
+
+        Returns:
+            The position of the car along the (x, y, z) axes during
+            the last frame in m.
+
+        Note:
+            The x axis points out of the right of the car.
+            The y axis points directly up (perpendicular to the ground).
+            The z axis points out of the front of the car.
+
+        Example::
+
+            # position_vector stores the position over the previous frame
+            position_vector = rc.physics.get_position()
+
+            x_position = position_vector[0]
+            y_position = position_vector[1]
+            z_position = position_vector[2]
+        """
+        pass
+

library/racecar_core.py

@@ -16,6 +16,7 @@
 import drive
 import lidar
 import physics
+import drone
 
 import racecar_utils as rc_utils
 
@@ -33,6 +34,7 @@ def __init__(self) -> None:
         self.drive: drive.Drive
         self.lidar: lidar.Lidar
         self.physics: physics.Physics
+        self.drone: drone.Drone
 
     @abc.abstractmethod
     def go(self) -> None:

library/simulation/drone_sim.py

@@ -0,0 +1,61 @@
+import sys
+import numpy as np
+import cv2 as cv
+from nptyping import NDArray
+import struct
+
+from drone import Drone
+
+class DroneSim(Drone):
+    def __init__(self, racecar) -> None:
+        self.__racecar = racecar
+        self.__drone_image: NDArray[(480, 640, 3), np.uint8] = None
+        self.__is_drone_image_current: bool = False
+
+    def get_drone_image_no_copy(self) -> NDArray[(480, 640, 3), np.uint8]:
+        if not self.__is_drone_image_current:
+            self.__drone_image = self.__request_drone_image(False)
+            self.__is_drone_image_current = True
+
+        return self.__drone_image
+
+    def get_drone_image_async(self) -> NDArray[(480, 640, 3), np.uint8]:
+        return self.__request_drone_image(True)
+
+    def __update(self) -> None:
+        self.__is_drone_image_current = False
+
+    def __request_drone_image(self, isAsync: bool) -> NDArray[(480, 640), np.uint8]:
+        # Ask for the current color image
+        self.__racecar._RacecarSim__send_header(
+            self.__racecar.Header.drone_get_drone_image, isAsync
+        )
+
+        # Read the color image as 32 packets
+        raw_bytes = self.__racecar._RacecarSim__receive_fragmented(
+            32, self._WIDTH * self._HEIGHT * 4, isAsync
+        )
+        drone_image = np.frombuffer(raw_bytes, dtype=np.uint8)
+        drone_image = np.reshape(drone_image, (self._HEIGHT, self._WIDTH, 4), "C")
+
+        drone_image = cv.cvtColor(drone_image, cv.COLOR_RGB2BGR)
+        return drone_image
+
+    def get_drone_height(self) -> float:
+        self.__racecar._RacecarSim__send_header(
+            self.__racecar.Header.drone_get_drone_height
+        )
+        value = struct.unpack("f", self.__racecar._RacecarSim__receive_data())[0]
+        return value
+
+    def set_height(self, height: float) -> None:
+        self.__racecar._RacecarSim__send_data(
+            struct.pack(
+                "Bf", self.__racecar.Header.drone_set_height.value, height,
+            )
+        )
+
+    def return_home(self) -> None:
+        self.__racecar._RacecarSim__send_header(
+            self.__racecar.Header.drone_return_home
+        )

library/simulation/physics_sim.py

@@ -22,3 +22,10 @@ def get_angular_velocity(self) -> NDArray[3, np.float32]:
         )
         values = struct.unpack("fff", self.__racecar._RacecarSim__receive_data(12))
         return np.array(values)
+
+    def get_position(self) -> NDArray[3, np.float32]:
+        self.__racecar._RacecarSim__send_header(
+            self.__racecar.Header.physics_get_position
+        )
+        values = struct.unpack("fff", self.__racecar._RacecarSim__receive_data(12))
+        return np.array(values)

library/simulation/racecar_core_sim.py

@@ -20,6 +20,7 @@
 import drive_sim
 import lidar_sim
 import physics_sim
+import drone_sim
 
 from racecar_core import Racecar
 import racecar_utils as rc_utils
@@ -65,6 +66,11 @@ class Header(IntEnum):
         lidar_get_samples = 26
         physics_get_linear_acceleration = 27
         physics_get_angular_velocity = 28
+        physics_get_position = 29
+        drone_get_drone_image = 30
+        drone_get_drone_height = 31
+        drone_set_height = 32
+        drone_return_home = 33
 
     class Error(IntEnum):
         """
@@ -112,6 +118,7 @@ def __init__(self, isHeadless: bool = False) -> None:
         self.drive = drive_sim.DriveSim(self)
         self.physics = physics_sim.PhysicsSim(self)
         self.lidar = lidar_sim.LidarSim(self)
+        self.drone = drone_sim.DroneSim(self)
 
         self.__start: Callable[[], None]
         self.__update: Callable[[], None]
@@ -230,6 +237,7 @@ def __handle_update(self) -> None:
         self.camera._CameraSim__update()
         self.controller._ControllerSim__update()
         self.lidar._LidarSim__update()
+        self.drone._DroneSim__update()
 
     def __handle_sigint(self, signal_received: int, frame) -> None:
         # Send exit command to sync port if we are in the middle of servicing a start
@@ -263,3 +271,4 @@ def __handle_error(self, error: Error):
         rc_utils.print_error(text)
         print(">> Closing script...")
         exit(0)
+