Modified the Logger and NetworkTables

Reduced the amount of data logged to reduce file clutter. Allowed the logger to make a file named off the current date and time. Changed startNetworkComms() to allow for numbers less than four digits long to be implemented. Began testing how to triangulate positions. Signed-off-by: Alex Pereira <[email protected]>
PIE-Cubed · Jun 22, 2023 · ee01034 · ee01034
1 parent 2df3c12
commit ee01034
Show file tree

Hide file tree

Showing 11 changed files with 54 additions and 30 deletions.
diff --git a/frc_apriltags/Tag_Layouts/__init__.py b/frc_apriltags/Tag_Layouts/__init__.py
@@ -1,5 +1,7 @@
+# Import libraries
 from importlib_metadata import PackageNotFoundError, version
 
+# Generate the version
 try:
     __version__ = version("frc-apriltags")
 except PackageNotFoundError:

diff --git a/frc_apriltags/Utilities/AprilTagFieldLayout.py b/frc_apriltags/Utilities/AprilTagFieldLayout.py
@@ -7,8 +7,7 @@
 from   wpimath.geometry import *
 
 # Import Utilities
-from frc_apriltags.Utilities import Logger
-from frc_apriltags.Utilities import AprilTag
+from Utilities import Logger, AprilTag
 
 # Creates the AprilTagFieldLayout class
 class AprilTagFieldLayout:
@@ -38,7 +37,7 @@ def __init__(self, tags: Sequence[AprilTag], fieldLength: float, fieldWidth: flo
         self.fieldWidth = fieldWidth
 
         # Logs the field size
-        Logger.logInfo("Field length: {}, Field width: {}".format(self.fieldLength, self.fieldWidth), True)
+        Logger.logInfo(f"Field length: {self.fieldLength}, Field width: {self.fieldWidth}", True)
 
         # Creates the allTags array
         self.allTags = [Pose3d()] * 9
@@ -51,7 +50,7 @@ def __init__(self, tags: Sequence[AprilTag], fieldLength: float, fieldWidth: flo
             self.allTags[id] = pose
 
             # Logs the tag information
-            Logger.logInfo("Tag {}. Pose: {}".format(id, pose), True)
+            Logger.logInfo(f"Tag {id}. Pose: {pose}", True)
 
         # Variables
         self.m_origin  = None
@@ -130,7 +129,7 @@ def setOrigin(self, origin):
             raise ValueError("Unsupported enumerator value.")
 
         # Logs the origin
-        Logger.logInfo("Origin at {}".format(origin), self.logStatus)
+        Logger.logInfo(f"Origin at {origin}", self.logStatus)
 
     def getTags(self):
         """

diff --git a/frc_apriltags/Utilities/Logger.py b/frc_apriltags/Utilities/Logger.py
@@ -1,6 +1,7 @@
 # Created by Alex Pereira
 
 # Import Libraries
+import time
 import logging
 
 # Start of the Logging class
@@ -15,8 +16,11 @@ def setLogPath(dirPath: str = "/tmp/"):
 
         :param dirPath: Should be aquired by running ``Path(__file__).absolute().parent.__str__()`` in the script calling this method
         """
+        # Gets the time as a string
+        currTime = time.ctime().replace(" ", "_")[4:]
+
         # Starts a logger
-        logging.basicConfig(filename = dirPath + "/DetectionLog.log", format="%(levelname)s:%(message)s", encoding = "utf-8", level = logging.DEBUG)
+        logging.basicConfig(filename = dirPath + f"/{currTime}.log", format="%(levelname)s:%(message)s", encoding = "utf-8", level = logging.DEBUG)
 
     @staticmethod
     def logDebug(debug: str, logStatus: bool = True):

diff --git a/frc_apriltags/Utilities/__init__.py b/frc_apriltags/Utilities/__init__.py
@@ -1,5 +1,7 @@
+# Import libraries
 from importlib_metadata import PackageNotFoundError, version
 
+# Generate the version
 try:
     __version__ = version("frc-apriltags")
 except PackageNotFoundError:

diff --git a/frc_apriltags/__init__.py b/frc_apriltags/__init__.py
@@ -1,5 +1,10 @@
-from importlib_metadata import PackageNotFoundError, version
+# Import libraries
+import time
+import ntcore
+from   networktables import NetworkTablesInstance
+from   importlib_metadata import PackageNotFoundError, version
 
+# Generate the version
 try:
     __version__ = version("frc-apriltags")
 except PackageNotFoundError:
@@ -23,27 +28,31 @@
     "Streaming"
 ]
 
+# Starts the NetworkTables
 @staticmethod
 def startNetworkComms(teamNumber: int = 2199):
     """
     Starts an NT4 server and client for a specific team.
 
     :param teamNumber: Your FRC team's number.
     """
-    import time
-    import ntcore
-    from   networktables import NetworkTables
-
+    # Ensures that a team number is a least 4 character long
+    teamStr = str(teamNumber)
+    teamStr.zfill(4)
+ 
     # Waits for the Rio to start
     time.sleep(5)
 
+    # Gets the default NetworkTables instance
+    ntinst = NetworkTablesInstance.getDefault() # Get a NetworkTables Instance
+
     # Initializes the NetworkTables
-    NetworkTables.initialize(server = "roborio-" + str(teamNumber) + "-frc.local")
+    ntinst.initialize(server = "10." + teamStr[:2] + "." + teamStr[2:] + ".2")
 
     # Create the NT4 server and client
     nt = ntcore.NetworkTableInstance.getDefault()
     nt.setServerTeam(teamNumber)
     nt.startClient4(__file__)
 
     # Create the NetworkTables
-    NetworkTables.startClientTeam(teamNumber)
+    ntinst.startClientTeam(teamNumber)
diff --git a/frc_apriltags/apriltags.py b/frc_apriltags/apriltags.py
@@ -7,7 +7,7 @@
 from   frc_apriltags import NetworkCommunications
 
 # Import Utilities
-from frc_apriltags.Utilities import Logger, Units
+from .Utilities import Logger, Units
 
 # Creates the Detector Class
 class Detector:

diff --git a/frc_apriltags/calibration.py b/frc_apriltags/calibration.py
@@ -5,7 +5,7 @@
 import numpy as np
 
 # Import Utilities
-from frc_apriltags.Utilities import Logger
+from .Utilities import Logger
 
 # Defines the dimensions of the chessboard
 CHESSBOARD = (8, 5)  # Number of interior corners (width in squares - 1 x height in squares - 1)
@@ -49,7 +49,7 @@ def __init__(self, cap, camNum: int, numImages: int = 15, dirPath: str = "/home/
         self.imgPoints = []  # 2D point in image plane
 
         # Path to calibration images
-        self.PATH = dirPath + "/camera{}-{}x{}-images/".format(self.camNum, self.width, self.height)
+        self.PATH = dirPath + f"/camera{self.camNum}-{self.width}x{self.height}-images/"
 
         # File extension
         self.EXTENSION = ".png"
@@ -131,14 +131,14 @@ def calibrateCamera(self):
         if (imagesUsed < (self.calibrationImages * 1/2)):
             # Updates log
             Logger.logWarning("Calibration restarted", self.logStatus)
-            Logger.logInfo("Images found: {}".format(imagesUsed), self.logStatus)
+            Logger.logInfo(f"Images found: {imagesUsed}", self.logStatus)
 
             # Sets up for a do over
             self.doOver = True
 
             self.calibrateCamera()
         else:
-            Logger.logInfo("Images found: {}".format(imagesUsed), self.logStatus)
+            Logger.logInfo(f"Images found: {imagesUsed}", self.logStatus)
             self.doOver = False
 
         # Calibrate the camera by passing the value of known 3D points (objPoints) and corresponding pixel coordinates of the detected corners (imgPoints)
@@ -148,8 +148,7 @@ def calibrateCamera(self):
         repredictError = self.calculateRepredictionError()
 
         # Updates log
-        Logger.logInfo("Camera {} Calibrated".format(self.camNum), self.logStatus)
-        Logger.logInfo("Camera Properties: \nCamera Matrix: \n{}, \nDistortion Matrix: \n{}, \nRotation Vectors: \n{}, \nTranslation Vectors: \n{}, \nAverage Reprediction Value: {}".format(self.cameraMatrix, self.distortion, self.rVecs, self.tVecs, repredictError), self.logStatus)
+        Logger.logInfo(f"Camera {self.camNum} Calibrated", self.logStatus)
 
         # Return calibration results
         return self.ret, self.cameraMatrix, self.distortion, self.rVecs, self.tVecs
@@ -167,7 +166,7 @@ def createCalibrationImages(self):
             j = i + 1
 
             # Prints target image
-            print("Attempting to take calibration image {}".format(j))
+            print(f"Attempting to take calibration image {j}")
 
             # Resets imgSelected
             imgSelected = False
@@ -206,13 +205,13 @@ def createCalibrationImages(self):
                     tempStream = cv.cvtColor(tempStream, cv.COLOR_RGB2BGR)
 
                     # Writes the image
-                    cv.imwrite(self.PATH + "{}".format(j) + self.EXTENSION, tempStream)
+                    cv.imwrite(self.PATH + f"{j}" + self.EXTENSION, tempStream)
 
                     # Prints the status
-                    print("Calibration image {} taken".format(j))
+                    print(f"Calibration image {j} taken")
 
                     # Updates log
-                    Logger.logInfo("Calibration image {} taken".format(j), self.logStatus)
+                    Logger.logInfo(f"Calibration image {j} taken", self.logStatus)
 
                     # Breaks the while loop
                     imgSelected = True
@@ -222,8 +221,7 @@ def createCalibrationImages(self):
         cv.destroyAllWindows()
 
         # Updates log
-        Logger.logInfo("Calibration images generated", self.logStatus)
-        Logger.logInfo("Images stored at " +  self.PATH, self.logStatus)
+        Logger.logInfo("Calibration images stored at " +  self.PATH, self.logStatus)
 
     def getPathExistance(self) -> bool:
         """
@@ -239,7 +237,7 @@ def getPathExistance(self) -> bool:
         try:
             os.mkdir(self.PATH)
         except Exception as e:
-            Logger.logError("{}".format(e), self.logStatus)
+            Logger.logError(f"{e}", self.logStatus)
 
         # Attempts to read the last callibration image and updates variables accordingly
         img = cv.imread(self.PATH + str(self.calibrationImages) + self.EXTENSION)

diff --git a/frc_apriltags/camera.py b/frc_apriltags/camera.py
@@ -4,7 +4,7 @@
 from   frc_apriltags import Calibrate
 
 # Import Utilities
-from frc_apriltags.Utilities import Logger
+from .Utilities import Logger
 
 # Creates the USBCamera class
 class USBCamera:

diff --git a/frc_apriltags/communications.py b/frc_apriltags/communications.py
@@ -2,7 +2,7 @@
 from networktables import *
 
 # Import Utilities
-from frc_apriltags.Utilities import Logger
+from .Utilities import Logger
 
 # Creates the NetworkCommunications Class
 class NetworkCommunications:

diff --git a/frc_apriltags/stream.py b/frc_apriltags/stream.py
@@ -3,7 +3,7 @@
 from   cscore import CameraServer as CS
 
 # Import Utilities
-from frc_apriltags.Utilities import Logger
+from .Utilities import Logger
 
 # Creates the BasicStreaming Class
 class Streaming():

diff --git a/tests/test.py b/tests/test.py
@@ -0,0 +1,10 @@
+anchor_1 = 530
+anchor_2 = 500
+h_dist   = 30
+
+y = round(((anchor_1**2) - ((((anchor_1**2) - (anchor_2**2) + (h_dist**2)) / (2 * h_dist))**2))**0.5)
+x = round(((anchor_1**2) - (anchor_2**2) + (h_dist**2)) / (2 * h_dist))
+
+x = x - h_dist//2
+
+print(x,y)