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