diff --git a/src/__init__.py b/src/__init__.py index 888516d..9669ede 100755 --- a/src/__init__.py +++ b/src/__init__.py @@ -6,6 +6,9 @@ https://opensource.org/license/MIT. """ +""" +cv2 library provides useful functions and methods for working with images +""" import time, cv2 from config.Config import Config, LocalConfig, RemoteConfig @@ -17,6 +20,15 @@ from pipeline.Detector import FiducialDetector from pipeline.PoseEstimator import FiducialPoseEstimator +""" +Config allows us to connect to the camera and other information locally or remotely. +- LocalConfig sets up misallenous information +- RemoteConfig sets up information about the camera + +FileConfigManager() configures the path, IP, and provides other miscellaneous information + +NTConfigManager configures the NetworkTable by setting the subscribers and topics +""" config = Config(LocalConfig(), RemoteConfig()) file_config_manager = FileConfigManager() nt_config_manager = NTConfigManager() @@ -32,6 +44,9 @@ capture = DefaultCapture(publisher) def main(): + """ + Connects the vision information with other classes throughout the repository + """ publisher.sendMsg(config.local.device_name + " has started") @@ -49,15 +64,21 @@ def main(): frame = capture.getFrame(config) if frame is None: + # Handling error if frame doesn't exist publisher.sendMsg("Camera not connected") publisher.send(0, 0, None, None) capture.release() continue - + + # extracting information from the frame fiducials, tids, all_corners = detector.detect(frame) areas = [] if tids is not None and all_corners is not None: + """ + Detects the ARUCO markers from the image + https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html + """ frame = cv2.aruco.drawDetectedMarkers(frame, all_corners, tids) tids, areas = detector.orderIDs(all_corners, tids) @@ -77,5 +98,6 @@ def main(): try: main() except KeyboardInterrupt: + # resets the camera and publisher capture.release() publisher.close() diff --git a/src/output/Annotate.py b/src/output/Annotate.py index f846dc8..85890e9 100644 --- a/src/output/Annotate.py +++ b/src/output/Annotate.py @@ -6,32 +6,55 @@ https://opensource.org/license/MIT. """ +""" +cv2 library provides useful functions and methods for working with images +numpy provides a wide variety of mathematical operations on arrays +""" import cv2 import numpy from config.Config import Config class Annotate: + """ + Initial definition of the functions. + Raises exceptions if the functions are not implmented + """ + def __init__(self): raise NotImplementedError def annotate(self, image, config: Config): raise NotImplementedError - + @classmethod - def drawCube(self, frame, image_points): + def drawCube(self, frame, image_points:list): + """ + The frame parameter provides the initial image, where cv2 will perform operations on + The image_points parameter is a list containing the points in the image to perform operations on. + drawCube(...) draws contours and lines based on image_points and returns the edited image + """ + + #Reshapes the array to a 2D array where the 2nd dimension has a size of 2 image_points = numpy.int32(image_points).reshape(-1,2) + #cv2 draws contours by extracting the image points and draws ALL the contours. + #contours are outlines/boundaries of objects in an image frame = cv2.drawContours(frame, [image_points[:4]], -1, (0,255,0), 3) + #runs a nested for loop to draw the lines between every pair of image points (given 2 points, exactly one line connects them both) for i, j in zip(range(4),range(4,8)): frame = cv2.line(frame, tuple(image_points[i]), tuple(image_points[j]), (255), 3) - + frame = cv2.drawContours(frame, [image_points[4:]], -1, (0,0,255), 3) return frame class AnnotateFiducials(Annotate): + """ + Annotates the AprilTags using the class previously defined. + Returns the annotated AprilTag image + """ fiducial_size = 0.0 axis = numpy.array([]) @@ -41,13 +64,21 @@ def __init__(self): def annotate(self, image, rvecs, tvecs, fps, fpt, config: Config): if config.remote.fiducial_size != self.fiducial_size: - self.fiducial_size = config.remote.fiducial_size + self.fiducial_size = config.remote.fiducial_size #sets the correct AprilTag size based on information from Config + #Sets the 2nd dimension arrays to the maximum boundaries (the outer edges of the AprilTag) self.axis = numpy.float32([[-self.fiducial_size/2,-self.fiducial_size/2,0], [self.fiducial_size/2,-self.fiducial_size/2,0], [self.fiducial_size/2,self.fiducial_size/2,0], [-self.fiducial_size/2,self.fiducial_size/2,0], [-self.fiducial_size/2,-self.fiducial_size/2,self.fiducial_size],[self.fiducial_size/2,-self.fiducial_size/2,self.fiducial_size], [self.fiducial_size/2,self.fiducial_size/2,self.fiducial_size],[-self.fiducial_size/2,self.fiducial_size/2,self.fiducial_size] ]) + """ + Nested for loop through the rotation and translation vectors. + The rotation vetors are converted to a Rodrigues rotation vector (compact representation of a 3D rotation), which is more efficient and easier to manipulate + The 3D image points are projected into a 2D image plane + The new 2D image plane is annotated + The annotated 2D image is returned. + """ for rvec, tvec in zip(rvecs, tvecs): rvec, _ = cv2.Rodrigues(rvec) image_points, _ = cv2.projectPoints(self.axis, rvec, tvec, config.local.camera_matrix, config.local.distortion_coefficient) diff --git a/src/output/Publisher.py b/src/output/Publisher.py index cd9c333..fdea55c 100644 --- a/src/output/Publisher.py +++ b/src/output/Publisher.py @@ -6,15 +6,24 @@ https://opensource.org/license/MIT. """ +""" +ntcore passes non-driver station data to and from the robot across a network +https://github.com/robotpy/pyntcore/tree/main +""" import ntcore from typing import Union import numpy.typing import logging from wpimath.geometry import * + from config.Config import Config class Publisher: + """ + Initial definition of the functions. + Raises exceptions if the functions are not implmented + """ def __init__(self): raise NotImplementedError @@ -29,6 +38,7 @@ def close(self): raise NotImplementedError def poseToArray(pose: Pose3d): + #Converts the Pose3D into an array by mapping the XYZ translation and rotation values. return [ pose.translation().X(), pose.translation().Y(), @@ -39,21 +49,29 @@ def poseToArray(pose: Pose3d): ] class NTPublisher: - - fps_pub: ntcore.FloatPublisher - latency_pub: ntcore.DoublePublisher - tvecs_pub: ntcore.DoubleArrayPublisher - rvecs_pub: ntcore.DoubleArrayPublisher - tids_pub: ntcore.IntegerArrayPublisher + """ + Creates topics (data channels) and publishes values to it + https://docs.wpilib.org/en/stable/docs/software/networktables/publish-and-subscribe.html + """ + + fps_pub: ntcore.FloatPublisher #Frames per second + latency_pub: ntcore.DoublePublisher #Latency -- Time for data packet to transfer + tvecs_pub: ntcore.DoubleArrayPublisher #Translation vectors + rvecs_pub: ntcore.DoubleArrayPublisher #Rotation vectors + tids_pub: ntcore.IntegerArrayPublisher areas_pub: ntcore.DoubleArrayPublisher reprojection_error_pub: ntcore.DoublePublisher - + msg_pub: ntcore.StringPublisher update_counter_pub: ntcore.IntegerPublisher counter: int def __init__(self, config: Config): - + """ + The self parameter invokes the NetworkTable. + The config parameter provides information related to the camera. + """ + # Initializes an instance of a NetworkTable, gets misallenous information, and starts logging instance = ntcore.NetworkTableInstance.getDefault() instance.setServerTeam(config.local.team_number) instance.setServer(config.local.server_ip, ntcore.NetworkTableInstance.kDefaultPort4) @@ -61,6 +79,7 @@ def __init__(self, config: Config): table = instance.getTable("/" + config.local.device_name + "/output") logging.basicConfig(level=logging.DEBUG) + # Starts publishing data periodically to the NetworkTable and setting items to their default values self.fps_pub = table.getFloatTopic("fps").publish() self.fps_pub.setDefault(0) self.latency_pub = table.getDoubleTopic("latency").publish(ntcore.PubSubOptions(keepDuplicates=True, periodic=0.02)) @@ -78,11 +97,16 @@ def __init__(self, config: Config): self.counter = 0 - def send(self, fps: Union[float, None], latency: Union[float, None], tids, primary_pose, areas, reprojection_error): + def send(self, fps: Union[float, None], latency: Union[float, None], tids: Union[list, None], primary_pose: Union[list, None], areas: list, reprojection_error: Union[float, None]): + """ + Updates the data by setting their value if there's not nothing (aka theres's something). + Else, it sets values to default (empty array or 0) + """ if fps is not None: self.fps_pub.set(fps) + #ntcore._now() returns the current time (used to show when information was updated) if latency is not None and tids is not None and primary_pose is not None and len(areas) > 0: self.latency_pub.set(latency * 1000, ntcore._now()) self.tids_pub.set(tids, ntcore._now()) @@ -98,9 +122,11 @@ def send(self, fps: Union[float, None], latency: Union[float, None], tids, prima self.reprojection_error_pub.set(0) def sendMsg(self, msg: str): + #sends message (string) to the NetworkTable self.msg_pub.set(msg) def close(self): + #Stops publishing to the NetworkTable self.fps_pub.close() self.tids_pub.close() self.pose_sub.close()