From c0bd3befff2a7d2fe395fe3656a05e4d2a729011 Mon Sep 17 00:00:00 2001
From: Alex Pereira <alex.pereira.6464@gmail.com>
Date: Fri, 24 Feb 2023 16:18:05 -0500
Subject: [PATCH] Ironed out many bugs with the pacakge

Signed-off-by: Alex Pereira <alex.pereira.6464@gmail.com>
---
 frc_apriltags/Utilities/AprilTag.py           |   2 -
 .../Utilities/AprilTagFieldLayout.py          |   2 -
 frc_apriltags/Utilities/Logger.py             |   2 +-
 frc_apriltags/Utilities/MathUtil.py           |   2 -
 frc_apriltags/Utilities/Units.py              |   2 -
 frc_apriltags/apriltags.py                    |  11 +-
 frc_apriltags/calibration.py                  |   7 +-
 frc_apriltags/camera.py                       | 102 ++++++++++++------
 frc_apriltags/communications.py               |   7 +-
 frc_apriltags/main.py                         |  48 ---------
 frc_apriltags/stream.py                       |  75 +++++++++++--
 pyproject.toml                                |   5 +-
 tests/test.py                                 |  49 +++------
 13 files changed, 172 insertions(+), 142 deletions(-)
 delete mode 100644 frc_apriltags/main.py

diff --git a/frc_apriltags/Utilities/AprilTag.py b/frc_apriltags/Utilities/AprilTag.py
index 0064052..a8dd878 100644
--- a/frc_apriltags/Utilities/AprilTag.py
+++ b/frc_apriltags/Utilities/AprilTag.py
@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import Libraries
 from wpimath.geometry import Translation3d, Rotation3d, Pose3d
 
diff --git a/frc_apriltags/Utilities/AprilTagFieldLayout.py b/frc_apriltags/Utilities/AprilTagFieldLayout.py
index f07b3bc..f348f4b 100644
--- a/frc_apriltags/Utilities/AprilTagFieldLayout.py
+++ b/frc_apriltags/Utilities/AprilTagFieldLayout.py
@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import json
 import numpy as np
diff --git a/frc_apriltags/Utilities/Logger.py b/frc_apriltags/Utilities/Logger.py
index 9636a4e..59d867a 100644
--- a/frc_apriltags/Utilities/Logger.py
+++ b/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:
diff --git a/frc_apriltags/Utilities/MathUtil.py b/frc_apriltags/Utilities/MathUtil.py
index cd27985..107e3e5 100644
--- a/frc_apriltags/Utilities/MathUtil.py
+++ b/frc_apriltags/Utilities/MathUtil.py
@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import numpy as np
 
diff --git a/frc_apriltags/Utilities/Units.py b/frc_apriltags/Utilities/Units.py
index 5991acf..a3b6241 100644
--- a/frc_apriltags/Utilities/Units.py
+++ b/frc_apriltags/Utilities/Units.py
@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import libraries
 import math
 
diff --git a/frc_apriltags/apriltags.py b/frc_apriltags/apriltags.py
index 085bb6c..254a19b 100644
--- a/frc_apriltags/apriltags.py
+++ b/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.
diff --git a/frc_apriltags/calibration.py b/frc_apriltags/calibration.py
index 8c761b2..89e68df 100644
--- a/frc_apriltags/calibration.py
+++ b/frc_apriltags/calibration.py
@@ -1,5 +1,3 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import os
 import glob
@@ -7,7 +5,7 @@
 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.
diff --git a/frc_apriltags/camera.py b/frc_apriltags/camera.py
index 9297b33..3db2e34 100644
--- a/frc_apriltags/camera.py
+++ b/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
diff --git a/frc_apriltags/communications.py b/frc_apriltags/communications.py
index 3723230..e786518 100644
--- a/frc_apriltags/communications.py
+++ b/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.
diff --git a/frc_apriltags/main.py b/frc_apriltags/main.py
deleted file mode 100644
index bc713bb..0000000
--- a/frc_apriltags/main.py
+++ /dev/null
@@ -1,48 +0,0 @@
-# Created by Alex Pereira
-
-# Import Libraries
-import cv2 as cv
-import numpy as np
-
-# Import Classes
-from stream    import Streaming
-from camera    import USBCamera
-from apriltags import Detector
-
-# Instance creation
-detector  = Detector()
-
-# Defines the camera resolutions (width x height)
-cameraRes = (1280, 720)
-
-# Creates a VideoCapture and calibrates it
-camera    = USBCamera(camNum = 0, path = "/dev/v4l/by-path/platform-70090000.xusb-usb-0:2.4:1.0-video-index0", resolution = cameraRes, calibrate = True)
-cap       = camera.resize(cameraRes)
-ret, camMatrix, camdistortion, rvecs, tvecs = camera.calibrateCamera()
-
-# Creates a camera for the drivers
-driverCam = Streaming(camNum = 1, path = "/dev/v4l/by-path/platform-70090000.xusb-usb-0:2.2:1.0-video-index0")
-
-# Delete unused variables
-del ret, rvecs, tvecs
-
-# Main loop
-while (cap.isOpened() == True):
-    # Reads the capture
-    sucess, stream = cap.read()
-
-    # Undistorts the image
-    stream = camera.undistort(stream, camMatrix, camdistortion, cameraRes)
-
-    # Runs April Tag detection on the undistorted image
-    results, stream = detector.detectTags(stream, camMatrix, 0)
-
-    # Displays the capture
-    # cv.imshow("Stream", stream)
-
-    # Press q to end the program
-    if ( cv.waitKey(1) == ord("q") ):
-        print("Process Ended by User")
-        cv.destroyAllWindows()
-        cap.release()
-        break
\ No newline at end of file
diff --git a/frc_apriltags/stream.py b/frc_apriltags/stream.py
index ddef4d2..5631157 100644
--- a/frc_apriltags/stream.py
+++ b/frc_apriltags/stream.py
@@ -1,23 +1,27 @@
-# Created by Alex Pereira
-
 # Import Libraries
 import ntcore
 import numpy  as np
 from   cscore import CameraServer as CS
 
+# Import Classes
+from frc_apriltags.camera import USBCamera
+
 # Import Utilities
-from Utilities.Logger import Logger
+from frc_apriltags.Utilities.Logger import Logger
 
 # Get a default network table
 nt = ntcore.NetworkTableInstance.getDefault()
 nt.setServerTeam(2199)
 nt.startClient4(__file__)
 
-# Creates the Streaming Class
-class Streaming:
+# Creates the BasicStreaming Class
+class BasicStreaming:
+    """
+    Use this class to stream unprocessed images to the driver station.
+    """
     def __init__(self, camNum: int, path: str = None) -> None:
         """
-        Constructor for the Streaming class.
+        Constructor for the BasicStreaming class.
         @param Camera Number
         @param path: It can be found on Linux by running "find /dev/v4l"
         """
@@ -39,6 +43,65 @@ def __init__(self, camNum: int, path: str = None) -> None:
         # Updates log
         Logger.logInfo("Stream initialized for camera " + str(camNum), True)
 
+    def enableLogging(self):
+        """
+        Enables logging for this module
+        """
+        self.logStatus = True
+
+# Creates the CustomStreaming Class
+class CustomStreaming:
+    """
+    Use this class to stream processed images back to the driver station.
+    """
+    def __init__(self, camNum: int, path: str = None, resolution: tuple = (0, 0)) -> None:
+        """
+        Constructor for the CustomStreaming class.
+        @param Camera Number
+        @param path: It can be found on Linux by running "find /dev/v4l"
+        """
+        # Creates a USBCamera
+        self.camera = USBCamera(camNum, path, resolution, False)
+
+        # Creates a CameraServer
+        CS.enableLogging()
+
+        # Defines the resolution
+        streamRes = (640, 480)
+
+        # Setup a CvSource. This will send images back to the Dashboard
+        self.outputStream = CS.putVideo("Camera 1", streamRes[0], streamRes[1])
+
+        # Preallocates for the incoming images
+        self.img = np.zeros(shape = (streamRes[1], streamRes[0], 3), dtype = np.uint8)
+
+        # Updates log
+        Logger.logInfo("Stream initialized for camera " + str(camNum), True)
+
+    def streamImage(self, stream = None):
+        """
+        Streams the camera back to ShuffleBoard for driver use.
+        @param stream: A processed stream
+        @return image
+        """
+        # Grab a frame from the camera and put it in the source image
+        if (stream is not None):
+            self.img = stream
+        else:
+            self.img = self.camera.getStream()
+
+        # Sends an image back to ShuffleBoard
+        self.outputStream.putFrame(self.img)
+
+        return self.img
+
+    def getUnprocessedStream(self):
+        """
+        Gets the unprocessed stream of this camera.
+        Note: OpenCV processing must happen outside of this class.
+        """
+        return self.camera.getStream()
+
     def enableLogging(self):
         """
         Enables logging for this module
diff --git a/pyproject.toml b/pyproject.toml
index 8cfcb17..ef914b6 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -17,7 +17,7 @@ name = "frc-apriltags"  # Required
 #
 # For a discussion on single-sourcing the version, see
 # https://packaging.python.org/guides/single-sourcing-package-version/
-dynamic = ["version"] # Required
+version = "0.3.0" # Required
 
 # This is a one-line description or tagline of what your project does. This
 # corresponds to the "Summary" metadata field:
@@ -158,5 +158,4 @@ dependencies = [ # Optional
 requires = ["setuptools>=60.0.0", "setuptools_scm[toml]>=6.2", "wheel"]
 build-backend = "setuptools.build_meta"
 
-[tool.setuptools_scm]
-write_to = "frc-apriltags/_version.py"
\ No newline at end of file
+[tool.setuptools_scm]
\ No newline at end of file
diff --git a/tests/test.py b/tests/test.py
index 798a1fc..1c0d8fb 100644
--- a/tests/test.py
+++ b/tests/test.py
@@ -1,52 +1,35 @@
-# Created by Alex Pereira
-
-# Import Libraries
-import cv2 as cv
-import numpy as np
-
 # Import Classes
-from frc_apriltags.stream    import Streaming
+from frc_apriltags.stream    import BasicStreaming
 from frc_apriltags.camera    import USBCamera
 from frc_apriltags.apriltags import Detector
 
-# Instance creation
-detector  = Detector()
-
 # Defines the camera resolutions (width x height)
-cameraRes = (1280, 720)
+tagCamRes    = (1280, 720)
 
-# Creates a VideoCapture and calibrates it
-camera    = USBCamera(camNum = 0, path = "/dev/v4l/by-path/platform-70090000.xusb-usb-0:2.4:1.0-video-index0")
-cap       = camera.resize(cameraRes)
-cameraRes = camera.getResolution()
-ret, camMatrix, camdistortion, rvecs, tvecs = camera.calibrateCamera()
+# Creates a USBCamera and calibrates it
+camera    = USBCamera(camNum = 0, path = "/dev/v4l/by-path/platform-70090000.xusb-usb-0:2.4:1.0-video-index0", resolution = tagCamRes, calibrate = False)
+camMatrix = camera.getMatrix()
 
 # Creates a camera for the drivers
-driverCam = Streaming(camNum = 1, path = "/dev/v4l/by-path/platform-70090000.xusb-usb-0:2.2:1.0-video-index0")
+driverCam = BasicStreaming(camNum = 1, path = "/dev/v4l/by-path/platform-70090000.xusb-usb-0:2.2:1.0-video-index0")
 
-# Delete unused variables
-del ret, rvecs, tvecs
+# Prealocate space for the detection stream
+stream = camera.prealocateSpace(tagCamRes)
 
-# Prealocate space for stream
-stream = np.zeros(shape = (cameraRes[1], cameraRes[0], 3), dtype = np.uint8)
+# Instance creation
+detector  = Detector()
 
 # Main loop
-while (cap.isOpened() == True):
-    # Reads the capture
-    sucess, stream = cap.read()
-
-    # Undistorts the image
-    stream = camera.undistort(stream, camMatrix, camdistortion, cameraRes)
+while (True):
+    # Gets the stream
+    stream = camera.getUndistortedStream()
 
     # Runs April Tag detection on the undistorted image
     results, stream = detector.detectTags(stream, camMatrix, 0)
 
-    # Displays the capture
-    # cv.imshow("Stream", stream)
+    # Displays the stream
+    camera.displayStream()
 
     # Press q to end the program
-    if ( cv.waitKey(1) == ord("q") ):
-        print("Process Ended by User")
-        cv.destroyAllWindows()
-        cap.release()
+    if ( camera.getEnd() == True ):
         break
\ No newline at end of file