Ironed out many bugs with the pacakge

Signed-off-by: Alex Pereira <[email protected]>

frc_apriltags/Utilities/AprilTag.py

@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import Libraries
 from wpimath.geometry import Translation3d, Rotation3d, Pose3d
 

frc_apriltags/Utilities/AprilTagFieldLayout.py

@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import json
 import numpy as np

frc_apriltags/Utilities/Logger.py

@@ -4,7 +4,7 @@
 import logging
 
 # Starts a logger
-logging.basicConfig(filename = "DetectionLog.log", format="%(levelname)s:%(message)s", encoding = "utf-8", level = logging.DEBUG)
+logging.basicConfig(filename = "./DetectionLog.log", format="%(levelname)s:%(message)s", encoding = "utf-8", level = logging.DEBUG)
 
 # Start of the Logging class
 class Logger:

frc_apriltags/Utilities/MathUtil.py

@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import numpy as np
 

frc_apriltags/Utilities/Units.py

@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import libraries
 import math
 

frc_apriltags/apriltags.py

@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import cv2   as cv
 import numpy as np
@@ -8,17 +6,20 @@
 from   wpilib import Timer
 
 # Import Classes
-from communications import NetworkCommunications
+from frc_apriltags.communications import NetworkCommunications
 
 # Import Utilities
-from Utilities.Units import Units
-from Utilities.Logger import Logger
+from frc_apriltags.Utilities.Units import Units
+from frc_apriltags.Utilities.Logger import Logger
 
 # The size of the tag in meters
 tagSize = Units.inchesToMeters(6)
 
 # Creates the Detector Class
 class Detector:
+    """
+    Use this class to detect AprilTags from the tag16h5 family.
+    """
     def __init__(self) -> None:
         """
         Constructor for the Detector class.

frc_apriltags/calibration.py

@@ -1,13 +1,11 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import os
 import glob
 import cv2   as cv
 import numpy as np
 
 # Import Utilities
-from Utilities.Logger import Logger
+from frc_apriltags.Utilities.Logger import Logger
 
 # Defines the dimensions of the chessboard
 CHESSBOARD = (7, 7)  # Number of interior corners (width in squares - 1 x height in squares - 1)
@@ -17,6 +15,9 @@
 
 # Creates the Calibrate class
 class Calibrate:
+    """
+    Use this class to calibrate your USBCamera.
+    """
     def __init__(self, cap, camNum: int, numImages: int = 15) -> None:
         """
         Constructor for the Calibrate class.

frc_apriltags/camera.py

@@ -1,17 +1,18 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import cv2 as cv
 import numpy as np
 
 # Import Classes
-from calibration import Calibrate
+from frc_apriltags.calibration import Calibrate
 
 # Import Utilities
-from Utilities.Logger import Logger
+from frc_apriltags.Utilities.Logger import Logger
 
 # Creates the USBCamera class
 class USBCamera:
+    """
+    Use this class to create a USBCamera.
+    """
     def __init__(self, camNum: int, path: str = None, resolution: tuple = (0, 0), calibrate: bool = False) -> None:
         """
         Constructor for the USBCamera class.
@@ -23,10 +24,8 @@ def __init__(self, camNum: int, path: str = None, resolution: tuple = (0, 0), ca
         self.camNum = camNum
 
         # Init variables
-        self.width  = -1
-        self.height = -1
-        self.fps    = -1
-        self.logStatus = False
+        self.logStatus  = False
+        self.resolution = resolution
 
         # Creates a capture
         if (path is not None):
@@ -35,6 +34,13 @@ def __init__(self, camNum: int, path: str = None, resolution: tuple = (0, 0), ca
         else:
             # Path is unknown, use the camera number
             self.cap = cv.VideoCapture(self.camNum)
+
+        # Resizes the capture
+        self.resize(resolution)
+
+        # Calibrates if told to do so
+        if (calibrate == True):
+            self.calibrateCamera()
 
         # Updates log
         Logger.logInfo("USBCamera initialized for camera " + str(camNum), True)
@@ -54,26 +60,24 @@ def resize(self, cameraRes: tuple):
         self.cap.set(cv.CAP_PROP_FPS, HIGH_VALUE)
 
         # Gets the highest value they go to
-        self.width  = int(self.cap.get(cv.CAP_PROP_FRAME_WIDTH))
-        self.height = int(self.cap.get(cv.CAP_PROP_FRAME_HEIGHT))
-        self.fps    = int(self.cap.get(cv.CAP_PROP_FPS))
+        width  = int(self.cap.get(cv.CAP_PROP_FRAME_WIDTH))
+        height = int(self.cap.get(cv.CAP_PROP_FRAME_HEIGHT))
+        fps    = int(self.cap.get(cv.CAP_PROP_FPS))
 
         # Set the capture to be MJPG format
         self.cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter_fourcc(*'MJPG'))
 
         # Prealocate space for stream
-        self.stream = np.zeros(shape = (cameraRes[1], cameraRes[0], 3), dtype = np.uint8)
+        self.stream = self.prealocateSpace((width, height))
 
         # Prints telemetry
-        print("Max Resolution:", str(self.width) + "x" + str(self.height))
-        print("Max FPS:", self.fps)
+        print("Resolution:", str(width) + "x" + str(height))
+        print("Max FPS:", fps)
 
         # Updates log
         Logger.logInfo("Capture resized", self.logStatus)
 
-        return self.cap
-
-    def undistort(self, stream, cameraMatrix, distortion, resolution: tuple):
+    def undistort(self, stream):
         """
         Undistorts an image using cv.undistort()
         @param stream
@@ -83,10 +87,10 @@ def undistort(self, stream, cameraMatrix, distortion, resolution: tuple):
         @return undistortedStream
         """
         # Creates a cameraMatrix
-        newCameraMatrix, roi = cv.getOptimalNewCameraMatrix(cameraMatrix, distortion, resolution, 1, resolution)
+        newCameraMatrix, roi = cv.getOptimalNewCameraMatrix(self.camMatrix, self.camdistortion, self.resolution, 1, self.resolution)
 
         # Undistorts the image
-        undistortedStream = cv.undistort(stream, cameraMatrix, distortion, None, newCameraMatrix)
+        undistortedStream = cv.undistort(stream, self.camMatrix, self.camdistortion, None, newCameraMatrix)
 
         # Crops the image
         x, y, w, h = roi
@@ -96,25 +100,21 @@ def undistort(self, stream, cameraMatrix, distortion, resolution: tuple):
 
     def calibrateCamera(self):
         """
-        Calibrates the camera and returns the calibration parameters
-        @return calibrationSuccessful
-        @return cameraMatrix
-        @return cameraDistortion
-        @return rotationVectors
-        @return translationVectors
+        Calibrates the camera and gets the calibration parameters
         """
         # Instance creation
         self.calibrate = Calibrate(self.cap, self.camNum, 15)
 
-        # Return results
-        return self.calibrate.calibrateCamera()
-
-    def getResolution(self):
+        # Get results
+        ret, self.camMatrix, self.camdistortion, rvecs, tvecs = self.calibrate.calibrateCamera()
+    
+    def prealocateSpace(self, cameraRes):
         """
-        Gets the current capture resolution
-        @return resolution (width, height)
+        Prealocates space for the stream.
+        @param cameraResolution
+        @return blankArray
         """
-        return (self.width, self.height)
+        return np.zeros(shape = (cameraRes[1], cameraRes[0], 3), dtype = np.uint8)
 
     def getStream(self):
         """
@@ -126,6 +126,44 @@ def getStream(self):
 
         return self.stream
 
+    def getUndistortedStream(self):
+        """
+        Gets the stream from this camera's capture
+        @return stream
+        """
+        # Reads the capture
+        __, self.stream = self.cap.read()
+
+        # Undistorts the stream
+        self.stream = self.undistort(self.stream)
+
+        return self.stream
+
+    def getEnd(self):
+        """
+        Checks if the program should end
+        """
+        if (cv.waitKey(1) == ord("q")):
+            print("Process Ended by User")
+            cv.destroyAllWindows()
+            self.cap.release()
+            return True
+        else:
+            return False
+
+    def getMatrix(self):
+        """
+        Returns the intrinsic camera matrix
+        @return cameraMatrix
+        """
+        return self.camMatrix
+
+    def displayStream(self):
+        """
+        Displays a flipped version of the stream 
+        """
+        cv.imshow("Stream", cv.flip(self.stream, 1))
+
     def enableLogging(self):
         """
         Enables logging for this module

frc_apriltags/communications.py

@@ -1,18 +1,19 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import numpy as np
 from   networktables import *
 from   wpimath.geometry import *
 
 # Import Utilities
-from Utilities.Logger import Logger
+from frc_apriltags.Utilities.Logger import Logger
 
 # Variables
 firstTime = True
 
 # Creates the NetworkCommunications Class
 class NetworkCommunications:
+    """
+    Use this class to communicate with the RoboRio over NetworkTables.
+    """
     def __init__(self) -> None:
         """
         Constructor for the NetworkCommunications class.